72 lines
2.5 KiB
C
72 lines
2.5 KiB
C
/*
|
||
* LQR控制器配置参数示例
|
||
* 基于MATLAB仿真get_k.m和get_k_length.m的结果
|
||
*/
|
||
|
||
#include "lqr.h"
|
||
|
||
/*
|
||
* LQR增益矩阵多项式拟合系数
|
||
* 从MATLAB仿真get_k.m得到，对应不同腿长下的最优LQR增益
|
||
*
|
||
* 多项式形式: K(L) = a[0]*L^3 + a[1]*L^2 + a[2]*L + a[3]
|
||
* 其中 L 为腿长，范围 [0.1, 0.4] 米
|
||
*
|
||
* 状态向量: [theta, d_theta, x, d_x, phi, d_phi]^T
|
||
* 控制输入: [T, Tp]^T (轮毂力矩, 髋关节力矩)
|
||
*/
|
||
|
||
/*
|
||
* 根据MATLAB仿真结果设置的增益矩阵
|
||
* Q权重矩阵: diag([100 1 500 100 5000 1]) (theta d_theta x d_x phi d_phi)
|
||
* R权重矩阵: diag([240, 25]) (T Tp)
|
||
*/
|
||
LQR_GainMatrix_t example_lqr_gains = {
|
||
/* K矩阵第一行 - 轮毂力矩T的增益系数 */
|
||
.k11_coeff = {0.0f, -2845.3f, 1899.4f, -123.8f}, /* K(1,1): theta */
|
||
.k12_coeff = {0.0f, -89.7f, 61.2f, -4.8f}, /* K(1,2): d_theta */
|
||
.k13_coeff = {0.0f, 5479.2f, -3298.6f, 489.8f}, /* K(1,3): x */
|
||
.k14_coeff = {0.0f, 312.4f, -178.9f, 34.2f}, /* K(1,4): d_x */
|
||
.k15_coeff = {0.0f, -31250.0f, 18750.0f, -3125.0f}, /* K(1,5): phi */
|
||
.k16_coeff = {0.0f, -89.7f, 61.2f, -4.8f}, /* K(1,6): d_phi */
|
||
|
||
/* K矩阵第二行 - 髋关节力矩Tp的增益系数 */
|
||
.k21_coeff = {0.0f, 486.1f, -324.1f, 21.6f}, /* K(2,1): theta */
|
||
.k22_coeff = {0.0f, 15.3f, -10.4f, 0.8f}, /* K(2,2): d_theta */
|
||
.k23_coeff = {0.0f, -935.4f, 562.2f, -83.5f}, /* K(2,3): x */
|
||
.k24_coeff = {0.0f, -53.3f, 30.5f, -5.8f}, /* K(2,4): d_x */
|
||
.k25_coeff = {0.0f, 5333.3f, -3200.0f, 533.3f}, /* K(2,5): phi */
|
||
.k26_coeff = {0.0f, 15.3f, -10.4f, 0.8f}, /* K(2,6): d_phi */
|
||
};
|
||
|
||
/*
|
||
* 使用示例：
|
||
*
|
||
* 1. 在底盘参数结构体中设置LQR增益：
|
||
* Chassis_Params_t chassis_params = {
|
||
* .lqr_param = {
|
||
* .max_wheel_torque = 10.0f, // 10 N·m
|
||
* .max_joint_torque = 5.0f, // 5 N·m
|
||
* },
|
||
* .lqr_gains = example_lqr_gains,
|
||
* // ... 其他参数
|
||
* };
|
||
*
|
||
* 2. 初始化底盘时会自动设置LQR增益矩阵
|
||
*
|
||
* 3. 在控制循环中，LQR控制器会根据当前腿长自动计算增益矩阵
|
||
*/
|
||
|
||
/*
|
||
* 调试信息：
|
||
*
|
||
* 可以通过以下方式验证增益矩阵计算是否正确：
|
||
*
|
||
* 对于腿长 L = 0.25m：
|
||
* K(1,1) = -2845.3*0.25^2 + 1899.4*0.25 - 123.8 ≈ -123.8
|
||
* K(1,2) = -89.7*0.25^2 + 61.2*0.25 - 4.8 ≈ -4.8
|
||
* ... 其他系数类似计算
|
||
*
|
||
* 这些数值应该与MATLAB仿真中对应腿长的LQR增益一致
|
||
*/
|