186 lines
4.1 KiB
Markdown
186 lines
4.1 KiB
Markdown
# 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
|