4.1 KiB
4.1 KiB
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 系统动力学方程
基于拉格朗日方程建立的系统动力学方程组：
-
驱动轮动力学：
d²x/dt² = (T - N*R)/(Iw/R + mw*R) -
摆杆动力学：
Ip*d²theta/dt² = (P*L + PM*LM)*sin(theta) - (N*L + NM*LM)*cos(theta) - T + Tp -
机体动力学：
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 增益矩阵计算
由于系统参数随腿长变化，采用多项式拟合方法：
- 离线计算：对不同腿长（0.1-0.4m），使用MATLAB计算最优LQR增益
- 多项式拟合：对每个增益元素进行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 主要功能模块
- LQR_Init(): 初始化控制器
- LQR_UpdateState(): 更新系统状态
- LQR_CalculateGainMatrix(): 计算增益矩阵
- LQR_Control(): 执行控制计算
- LQR_GetOutput(): 获取控制输出
3.3 与VMC的集成
LQR控制器输出虚拟力，通过VMC转换为关节力矩：
// 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 参数调优
-
Q矩阵调优：
- 增大theta权重提高平衡性能
- 增大phi权重减少机体摆动
- 增大x,d_x权重提高位置跟踪精度
-
R矩阵调优：
- 增大R减少控制输出，提高稳定性
- 减小R提高响应速度
4.2 常见问题
- 增益过大：导致系统震荡，需减小Q或增大R
- 响应迟缓：增大Q权重或减小R权重
- 静态误差：检查目标状态设置是否合理
4.3 监控指标
建议监控以下指标：
- 状态误差大小
- 控制输出饱和情况
- 系统稳定性指标
5. 使用示例
// 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. 参考资料
- MATLAB仿真代码：
utils/Simulation-master/balance/series_legs/ - C++参考实现：
User/module/mod_wheelleg_chassis.cpp - VMC控制器：
User/component/vmc.c - 理论参考：Modern Control Engineering, Ogata