# LQR控制器设计文档

本文档描述了为平衡步兵轮腿机器人设计的LQR（线性二次调节器）控制器。

## 1. 系统建模

### 1.1 状态变量定义

系统状态向量为6维：
```
x = [theta, d_theta, x, d_x, phi, d_phi]^T
```

其中：
- `theta`: 等效摆杆与竖直方向夹角 (rad)
- `d_theta`: 等效摆杆角速度 (rad/s)
- `x`: 驱动轮位移 (m)
- `d_x`: 驱动轮速度 (m/s)  
- `phi`: 机体与水平夹角（俯仰角） (rad)
- `d_phi`: 机体俯仰角速度 (rad/s)

### 1.2 控制输入

控制输入为2维：
```
u = [T, Tp]^T
```

其中：
- `T`: 驱动轮输出力矩 (N·m)
- `Tp`: 髋关节输出力矩 (N·m)

### 1.3 系统动力学方程

基于拉格朗日方程建立的系统动力学方程组：

1. **驱动轮动力学**：
   ```
   d²x/dt² = (T - N*R)/(Iw/R + mw*R)
   ```

2. **摆杆动力学**：
   ```
   Ip*d²theta/dt² = (P*L + PM*LM)*sin(theta) - (N*L + NM*LM)*cos(theta) - T + Tp
   ```

3. **机体动力学**：
   ```
   IM*d²phi/dt² = Tp + NM*l*cos(phi) + PM*l*sin(phi)
   ```

### 1.4 线性化

在平衡点附近进行线性化，得到状态空间模型：
```
dx/dt = A*x + B*u
```

其中A和B矩阵通过雅可比矩阵计算得到。

## 2. LQR控制器设计

### 2.1 代价函数

LQR控制器最小化以下二次代价函数：
```
J = ∫[x^T*Q*x + u^T*R*u]dt
```

权重矩阵选择：
- `Q = diag([100, 1, 500, 100, 5000, 1])` (状态权重)
- `R = diag([240, 25])` (控制权重)

### 2.2 增益矩阵计算

由于系统参数随腿长变化，采用多项式拟合方法：

1. **离线计算**：对不同腿长（0.1-0.4m），使用MATLAB计算最优LQR增益
2. **多项式拟合**：对每个增益元素进行3阶多项式拟合
3. **在线计算**：根据当前腿长实时计算增益矩阵

### 2.3 控制律

```
u = -K(L) * (x - x_ref)
```

其中：
- `K(L)`: 与腿长L相关的增益矩阵
- `x_ref`: 参考状态

## 3. 实现架构

### 3.1 文件结构

```
User/component/
├── lqr.h                    # LQR控制器头文件
├── lqr.c                    # LQR控制器实现
└── lqr_config_example.c     # 配置示例
```

### 3.2 主要功能模块

1. **LQR_Init()**: 初始化控制器
2. **LQR_UpdateState()**: 更新系统状态
3. **LQR_CalculateGainMatrix()**: 计算增益矩阵
4. **LQR_Control()**: 执行控制计算
5. **LQR_GetOutput()**: 获取控制输出

### 3.3 与VMC的集成

LQR控制器输出虚拟力，通过VMC转换为关节力矩：

```c
// LQR计算虚拟力和力矩
LQR_Control(&chassis->lqr);
LQR_GetOutput(&chassis->lqr, &lqr_output);

// VMC将虚拟力矩转换为关节力矩
VMC_InverseSolve(&chassis->vmc[0], virtual_force, lqr_output.Tp);
VMC_GetJointTorques(&chassis->vmc[0], &tau_front, &tau_rear);
```

## 4. 调试和优化

### 4.1 参数调优

1. **Q矩阵调优**：
   - 增大theta权重提高平衡性能
   - 增大phi权重减少机体摆动
   - 增大x,d_x权重提高位置跟踪精度

2. **R矩阵调优**：
   - 增大R减少控制输出，提高稳定性
   - 减小R提高响应速度

### 4.2 常见问题

1. **增益过大**：导致系统震荡，需减小Q或增大R
2. **响应迟缓**：增大Q权重或减小R权重
3. **静态误差**：检查目标状态设置是否合理

### 4.3 监控指标

建议监控以下指标：
- 状态误差大小
- 控制输出饱和情况
- 系统稳定性指标

## 5. 使用示例

```c
// 1. 配置LQR参数
Chassis_Params_t chassis_params = {
    .lqr_param = {
        .max_wheel_torque = 10.0f,
        .max_joint_torque = 5.0f,
    },
    .lqr_gains = example_lqr_gains,
};

// 2. 初始化底盘（自动初始化LQR）
Chassis_Init(&chassis, &chassis_params, 1000.0f);

// 3. 控制循环
while(1) {
    // 更新传感器数据
    Chassis_UpdateFeedback(&chassis);
    Chassis_UpdateIMU(&chassis, imu_data);
    
    // 执行LQR控制
    Chassis_Control(&chassis, &cmd);
    
    // 延时
    osDelay(1);
}
```

## 6. 参考资料

1. MATLAB仿真代码：`utils/Simulation-master/balance/series_legs/`
2. C++参考实现：`User/module/mod_wheelleg_chassis.cpp`
3. VMC控制器：`User/component/vmc.c`
4. 理论参考：Modern Control Engineering, Ogata