## 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模型进行修正。
主要是要确保状态向量定义、增益矩阵维度和控制律计算都与理论模型一致。