68 lines
2.4 KiB
Markdown
68 lines
2.4 KiB
Markdown
## LQR控制器修正建议
|
||
|
||
### 问题总结：
|
||
1. LQR增益计算方式不一致（3次多项式 vs 2次多项式）
|
||
2. 状态向量维度错误（12维 vs 10维）
|
||
3. 控制律映射不正确
|
||
4. 状态定义与MATLAB模型不匹配
|
||
|
||
### 修正建议：
|
||
|
||
#### 1. 修正LQR_K_calc函数
|
||
应该使用2次多项式而不是3次：
|
||
```cpp
|
||
float VMC::LQR_K_calc(float *coe, float l_l, float l_r) {
|
||
// 使用MATLAB中定义的2次多项式
|
||
// p(l_l,l_r) = p00 + p10*l_l + p01*l_r + p20*l_l^2 + p11*l_l*l_r + p02*l_r^2
|
||
return coe[0] + coe[1]*l_l + coe[2]*l_r + coe[3]*l_l*l_l + coe[4]*l_l*l_r + coe[5]*l_r*l_r;
|
||
}
|
||
```
|
||
|
||
#### 2. 修正增益矩阵维度
|
||
LQR增益矩阵应该是4×10，总共40个系数：
|
||
```cpp
|
||
// 为每条腿分配40个LQR系数，而不是12个
|
||
float LQR_K[40]; // 4×10矩阵，展开为一维数组
|
||
```
|
||
|
||
#### 3. 修正状态向量定义
|
||
确保状态向量与MATLAB模型一致：
|
||
```cpp
|
||
// 状态向量：[s, ds, phi, dphi, theta_ll, dtheta_ll, theta_lr, dtheta_lr, theta_b, dtheta_b]
|
||
float state_error[10] = {
|
||
move_argu_.xhat - move_argu_.target_x, // s误差
|
||
move_argu_.x_dot_hat - move_argu_.target_dot_x, // ds误差
|
||
this->yaw_ - 0.0f, // phi误差
|
||
this->gyro_.z - 0.0f, // dphi误差
|
||
leg_argu_[0].theta - 平衡角度, // theta_ll误差
|
||
leg_argu_[0].d_theta - 0.0f, // dtheta_ll误差
|
||
leg_argu_[1].theta - 平衡角度, // theta_lr误差
|
||
leg_argu_[1].d_theta - 0.0f, // dtheta_lr误差
|
||
this->pit_ - 0.0f, // theta_b误差
|
||
this->gyro_.x - 0.0f // dtheta_b误差
|
||
};
|
||
```
|
||
|
||
#### 4. 修正控制律计算
|
||
使用标准的LQR控制律：
|
||
```cpp
|
||
// 计算控制输出：u = -K * (x - x_ref)
|
||
float control[4] = {0}; // [T_wl, T_wr, T_bl, T_br]
|
||
|
||
for(int i = 0; i < 4; i++) {
|
||
for(int j = 0; j < 10; j++) {
|
||
control[i] += LQR_K[i*10 + j] * state_error[j];
|
||
}
|
||
control[i] = -control[i]; // 负反馈
|
||
}
|
||
|
||
leg_argu_[0].Tw = control[0]; // T_wl
|
||
leg_argu_[1].Tw = control[1]; // T_wr
|
||
leg_argu_[0].Tp = control[2]; // T_bl
|
||
leg_argu_[1].Tp = control[3]; // T_br
|
||
```
|
||
|
||
### 总结：
|
||
当前的LQR实现在数学原理上有偏差，需要按照标准的LQR控制理论和MATLAB模型进行修正。
|
||
主要是要确保状态向量定义、增益矩阵维度和控制律计算都与理论模型一致。
|