From 58f7ba86dbb6a0497aea7442461da28d29b37776 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Tue, 16 Sep 2025 23:18:47 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9vmc?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- LQR_修正建议.md | 67 ++++++ MDK-ARM/DevC.uvprojx | 10 + MDK-ARM/DevC/DevC.lnp | 2 + User/component/lqr.c | 269 ++++++++++++++++-------- User/component/lqr.h | 223 +++++++++++--------- User/component/vmc.c | 381 ++++++++++++++++++++++++++++++++++ User/component/vmc.h | 196 +++++++++++++++++ User/device/motor_lz.c | 10 +- User/module/balance_chassis.c | 364 +++++++++++++++++++------------- User/module/balance_chassis.h | 25 ++- User/module/config.c | 62 +++--- User/task/rc.c | 2 +- d435.py | 0 utils/lqr.asv | 321 ++++++++++++++++++++++++++++ utils/lqr.m | 14 +- 15 files changed, 1556 insertions(+), 390 deletions(-) create mode 100644 LQR_修正建议.md create mode 100644 User/component/vmc.c create mode 100644 User/component/vmc.h create mode 100644 d435.py create mode 100644 utils/lqr.asv diff --git a/LQR_修正建议.md b/LQR_修正建议.md new file mode 100644 index 0000000..64bc7da --- /dev/null +++ b/LQR_修正建议.md @@ -0,0 +1,67 @@ +## 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模型进行修正。 +主要是要确保状态向量定义、增益矩阵维度和控制律计算都与理论模型一致。 diff --git a/MDK-ARM/DevC.uvprojx b/MDK-ARM/DevC.uvprojx index 0dde2ad..abe2d18 100644 --- a/MDK-ARM/DevC.uvprojx +++ b/MDK-ARM/DevC.uvprojx @@ -814,6 +814,16 @@ 1 ..\User\component\kinematics.c + + lqr.c + 1 + ..\User\component\lqr.c + + + vmc.c + 1 + ..\User\component\vmc.c + diff --git a/MDK-ARM/DevC/DevC.lnp b/MDK-ARM/DevC/DevC.lnp index 742b83f..218f2d2 100644 --- a/MDK-ARM/DevC/DevC.lnp +++ b/MDK-ARM/DevC/DevC.lnp @@ -76,6 +76,8 @@ "devc\pid.o" "devc\user_math.o" "devc\kinematics.o" +"devc\lqr.o" +"devc\vmc.o" "devc\buzzer.o" "devc\dm_imu.o" "devc\dr16.o" diff --git a/User/component/lqr.c b/User/component/lqr.c index 8816a42..1cb1eae 100644 --- a/User/component/lqr.c +++ b/User/component/lqr.c @@ -1,144 +1,229 @@ +/* + * LQR线性二次型最优控制器简化实现 + * + * 本文件实现了轮腿机器人的LQR (Linear Quadratic Regulator) 控制算法 + * 主要功能包括: + * 1. 状态反馈控制 + * 2. 增益矩阵K计算控制输出 + * 3. 控制输出限幅 + * + * 系统模型: + * u = -K*(x - x_ref) (状态反馈) + */ + #include "lqr.h" #include -// 默认LQR增益矩阵 (需要根据实际系统调整) -static const float DEFAULT_K[LQR_INPUT_DIM][LQR_STATE_DIM] = { - // K矩阵行: [T_L, T_R] - // K矩阵列: [x, x_dot, theta, theta_dot, phi_L, phi_R] - {-50.0f, -20.0f, 800.0f, 100.0f, -200.0f, 0.0f}, // 左腿力矩 - {-50.0f, -20.0f, 800.0f, 100.0f, 0.0f, -200.0f} // 右腿力矩 -}; +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ + +#define LQR_EPSILON (1e-6f) // 数值计算精度 +#define LQR_DEFAULT_MAX_WHEEL (50.0f) // 默认最大轮毂力矩 (N*m) +#define LQR_DEFAULT_MAX_JOINT (30.0f) // 默认最大关节力矩 (N*m) + +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +/* Private function prototypes ---------------------------------------------- */ + +static void LQR_MatrixMultiply(const float K[4][10], const float state_error[10], float result[4]); +static float LQR_ComputeStateError(float current, float reference); + +/* Exported functions ------------------------------------------------------- */ /** * @brief 初始化LQR控制器 */ -int8_t LQR_Init(LQR_Controller_t *lqr, const LQR_Params_t *params) { - if (lqr == NULL) return -1; - - // 清零结构体 - memset(lqr, 0, sizeof(LQR_Controller_t)); - - // 设置参数 - if (params != NULL) { - memcpy(&lqr->params, params, sizeof(LQR_Params_t)); - } else { - // 使用默认参数 - memset(&lqr->params.Q, 0, sizeof(lqr->params.Q)); - memset(&lqr->params.R, 0, sizeof(lqr->params.R)); - - // 设置默认权重矩阵对角元素 - lqr->params.Q[STATE_POSITION][STATE_POSITION] = 100.0f; // 位置权重 - lqr->params.Q[STATE_VELOCITY][STATE_VELOCITY] = 10.0f; // 速度权重 - lqr->params.Q[STATE_PITCH][STATE_PITCH] = 1000.0f; // 俯仰角权重 - lqr->params.Q[STATE_PITCH_RATE][STATE_PITCH_RATE] = 100.0f; // 俯仰角速度权重 - lqr->params.Q[STATE_LEG_L][STATE_LEG_L] = 50.0f; // 左腿角度权重 - lqr->params.Q[STATE_LEG_R][STATE_LEG_R] = 50.0f; // 右腿角度权重 - - lqr->params.R[INPUT_TORQUE_L][INPUT_TORQUE_L] = 1.0f; // 左腿力矩权重 - lqr->params.R[INPUT_TORQUE_R][INPUT_TORQUE_R] = 1.0f; // 右腿力矩权重 - - lqr->params.max_torque = 50.0f; // 最大力矩50Nm - lqr->params.deadband_position = 0.01f; // 位置死区1cm - lqr->params.deadband_angle = 0.02f; // 角度死区约1度 +int8_t LQR_Init(LQR_Controller_t *lqr, float max_wheel_torque, float max_joint_torque) { + if (lqr == NULL || max_wheel_torque <= 0 || max_joint_torque <= 0) { + return -1; } - // 设置默认增益矩阵 - memcpy(lqr->params.K, DEFAULT_K, sizeof(DEFAULT_K)); + // 设置力矩限制 + lqr->max_wheel_torque = max_wheel_torque; + lqr->max_joint_torque = max_joint_torque; + + // 重置状态 + LQR_Reset(lqr); + + lqr->initialized = true; - lqr->initialized = 1; return 0; } /** - * @brief 设置LQR增益矩阵 + * @brief 设置固定LQR增益矩阵 */ -int8_t LQR_SetGains(LQR_Controller_t *lqr, float K[LQR_INPUT_DIM][LQR_STATE_DIM]) { - if (lqr == NULL || K == NULL) return -1; +int8_t LQR_SetGainMatrix(LQR_Controller_t *lqr, const LQR_GainMatrix_t *K) { + if (lqr == NULL || !lqr->initialized || K == NULL) { + return -1; + } + + // 复制增益矩阵 + memcpy(&lqr->K, K, sizeof(LQR_GainMatrix_t)); - memcpy(lqr->params.K, K, sizeof(lqr->params.K)); return 0; } /** - * @brief 更新系统状态 + * @brief 更新机器人状态 */ int8_t LQR_UpdateState(LQR_Controller_t *lqr, const LQR_State_t *state) { - if (lqr == NULL || state == NULL || !lqr->initialized) return -1; + if (lqr == NULL || !lqr->initialized || state == NULL) { + return -1; + } + + // 复制状态，并对角度进行归一化 + lqr->state = *state; + LQR_ANGLE_NORMALIZE(lqr->state.phi); + LQR_ANGLE_NORMALIZE(lqr->state.theta_ll); + LQR_ANGLE_NORMALIZE(lqr->state.theta_lr); + LQR_ANGLE_NORMALIZE(lqr->state.theta_b); - memcpy(&lqr->current_state, state, sizeof(LQR_State_t)); return 0; } /** * @brief 设置参考状态 */ -int8_t LQR_SetReference(LQR_Controller_t *lqr, const LQR_Reference_t *reference) { - if (lqr == NULL || reference == NULL || !lqr->initialized) return -1; +int8_t LQR_SetReference(LQR_Controller_t *lqr, const LQR_State_t *reference) { + if (lqr == NULL || !lqr->initialized || reference == NULL) { + return -1; + } + + // 复制参考状态，并对角度进行归一化 + lqr->reference = *reference; + LQR_ANGLE_NORMALIZE(lqr->reference.phi); + LQR_ANGLE_NORMALIZE(lqr->reference.theta_ll); + LQR_ANGLE_NORMALIZE(lqr->reference.theta_lr); + LQR_ANGLE_NORMALIZE(lqr->reference.theta_b); - memcpy(&lqr->reference, reference, sizeof(LQR_Reference_t)); return 0; } /** - * @brief LQR控制计算 + * @brief 计算LQR控制输出 + * + * 实现状态反馈控制律: u = -K*(x - x_ref) */ -int8_t LQR_Calculate(LQR_Controller_t *lqr) { - if (lqr == NULL || !lqr->initialized) return -1; - - // 计算状态误差 - lqr->state_error[STATE_POSITION] = lqr->reference.position_ref - lqr->current_state.position; - lqr->state_error[STATE_VELOCITY] = lqr->reference.velocity_ref - lqr->current_state.velocity; - lqr->state_error[STATE_PITCH] = lqr->reference.pitch_ref - lqr->current_state.pitch_angle; - lqr->state_error[STATE_PITCH_RATE] = 0.0f - lqr->current_state.pitch_rate; // 俯仰角速度期望为0 - lqr->state_error[STATE_LEG_L] = lqr->reference.leg_angle_L_ref - lqr->current_state.leg_angle_L; - lqr->state_error[STATE_LEG_R] = lqr->reference.leg_angle_R_ref - lqr->current_state.leg_angle_R; - - // 应用死区 - lqr->state_error[STATE_POSITION] = LQR_Deadband(lqr->state_error[STATE_POSITION], - lqr->params.deadband_position); - lqr->state_error[STATE_PITCH] = LQR_Deadband(lqr->state_error[STATE_PITCH], - lqr->params.deadband_angle); - lqr->state_error[STATE_LEG_L] = LQR_Deadband(lqr->state_error[STATE_LEG_L], - lqr->params.deadband_angle); - lqr->state_error[STATE_LEG_R] = LQR_Deadband(lqr->state_error[STATE_LEG_R], - lqr->params.deadband_angle); - - // LQR控制律: u = -K * x_error - for (int i = 0; i < LQR_INPUT_DIM; i++) { - lqr->control_output[i] = 0.0f; - for (int j = 0; j < LQR_STATE_DIM; j++) { - lqr->control_output[i] -= lqr->params.K[i][j] * lqr->state_error[j]; - } - // 限制输出 - lqr->control_output[i] = LQR_Limit(lqr->control_output[i], lqr->params.max_torque); +int8_t LQR_ComputeControl(LQR_Controller_t *lqr) { + if (lqr == NULL || !lqr->initialized) { + return -1; } + // 计算状态误差向量 + float state_error[10]; + state_error[0] = LQR_ComputeStateError(lqr->state.s, lqr->reference.s); + state_error[1] = LQR_ComputeStateError(lqr->state.ds, lqr->reference.ds); + state_error[2] = LQR_ComputeStateError(lqr->state.phi, lqr->reference.phi); + state_error[3] = LQR_ComputeStateError(lqr->state.dphi, lqr->reference.dphi); + state_error[4] = LQR_ComputeStateError(lqr->state.theta_ll, lqr->reference.theta_ll); + state_error[5] = LQR_ComputeStateError(lqr->state.dtheta_ll, lqr->reference.dtheta_ll); + state_error[6] = LQR_ComputeStateError(lqr->state.theta_lr, lqr->reference.theta_lr); + state_error[7] = LQR_ComputeStateError(lqr->state.dtheta_lr, lqr->reference.dtheta_lr); + state_error[8] = LQR_ComputeStateError(lqr->state.theta_b, lqr->reference.theta_b); + state_error[9] = LQR_ComputeStateError(lqr->state.dtheta_b, lqr->reference.dtheta_b); + + // 计算控制输出: u = -K * (x - x_ref) + float control_vector[4]; + LQR_MatrixMultiply(lqr->K.K, state_error, control_vector); + + // 应用负反馈并限幅 + lqr->control.T_wl = LQR_CLAMP(-control_vector[0], -lqr->max_wheel_torque, lqr->max_wheel_torque); + lqr->control.T_wr = LQR_CLAMP(-control_vector[1], -lqr->max_wheel_torque, lqr->max_wheel_torque); + lqr->control.T_bl = LQR_CLAMP(-control_vector[2], -lqr->max_joint_torque, lqr->max_joint_torque); + lqr->control.T_br = LQR_CLAMP(-control_vector[3], -lqr->max_joint_torque, lqr->max_joint_torque); + return 0; } /** * @brief 获取控制输出 */ -int8_t LQR_GetOutput(LQR_Controller_t *lqr, float *torque_L, float *torque_R) { - if (lqr == NULL || torque_L == NULL || torque_R == NULL || !lqr->initialized) return -1; - - *torque_L = lqr->control_output[INPUT_TORQUE_L]; - *torque_R = lqr->control_output[INPUT_TORQUE_R]; +int8_t LQR_GetControl(const LQR_Controller_t *lqr, LQR_Control_t *control) { + if (lqr == NULL || !lqr->initialized || control == NULL) { + return -1; + } + *control = lqr->control; return 0; } /** - * @brief 重置LQR控制器 + * @brief 重置LQR控制器状态 */ -int8_t LQR_Reset(LQR_Controller_t *lqr) { - if (lqr == NULL || !lqr->initialized) return -1; +void LQR_Reset(LQR_Controller_t *lqr) { + if (lqr == NULL) { + return; + } - // 清零状态和输出 - memset(&lqr->current_state, 0, sizeof(LQR_State_t)); - memset(&lqr->reference, 0, sizeof(LQR_Reference_t)); - memset(lqr->control_output, 0, sizeof(lqr->control_output)); - memset(lqr->state_error, 0, sizeof(lqr->state_error)); + // 清零状态和控制量 + memset(&lqr->state, 0, sizeof(LQR_State_t)); + memset(&lqr->reference, 0, sizeof(LQR_State_t)); + memset(&lqr->control, 0, sizeof(LQR_Control_t)); + memset(&lqr->K, 0, sizeof(LQR_GainMatrix_t)); +} + +/** + * @brief 从轮腿机器人传感器数据构建LQR状态 + */ +int8_t LQR_BuildStateFromSensors(float position_x, float velocity_x, + float yaw_angle, float yaw_rate, + float left_leg_angle, float left_leg_rate, + float right_leg_angle, float right_leg_rate, + float body_pitch, float body_pitch_rate, + LQR_State_t *state) { + if (state == NULL) { + return -1; + } + + state->s = position_x; + state->ds = velocity_x; + state->phi = yaw_angle; + state->dphi = yaw_rate; + state->theta_ll = left_leg_angle; + state->dtheta_ll = left_leg_rate; + state->theta_lr = right_leg_angle; + state->dtheta_lr = right_leg_rate; + state->theta_b = body_pitch; + state->dtheta_b = body_pitch_rate; + + // 角度归一化 + LQR_ANGLE_NORMALIZE(state->phi); + LQR_ANGLE_NORMALIZE(state->theta_ll); + LQR_ANGLE_NORMALIZE(state->theta_lr); + LQR_ANGLE_NORMALIZE(state->theta_b); return 0; -} \ No newline at end of file +} + +/* Private functions -------------------------------------------------------- */ + +/** + * @brief 矩阵向量乘法: result = K * state_error + * + * K: 4x10矩阵 + * state_error: 10x1向量 + * result: 4x1向量 + */ +static void LQR_MatrixMultiply(const float K[4][10], const float state_error[10], float result[4]) { + for (int i = 0; i < 4; i++) { + result[i] = 0.0f; + for (int j = 0; j < 10; j++) { + result[i] += K[i][j] * state_error[j]; + } + } +} + +/** + * @brief 计算状态误差(考虑角度周期性) + */ +static float LQR_ComputeStateError(float current, float reference) { + float error = current - reference; + + // 对于角度状态，需要考虑周期性 + // 这里假设大部分状态都是角度，如果需要区分可以添加参数 + while (error > M_PI) error -= 2 * M_PI; + while (error < -M_PI) error += 2 * M_PI; + + return error; +} diff --git a/User/component/lqr.h b/User/component/lqr.h index b5daf02..5b4b60f 100644 --- a/User/component/lqr.h +++ b/User/component/lqr.h @@ -4,148 +4,165 @@ extern "C" { #endif +/* Includes ----------------------------------------------------------------- */ #include +#include #include +#include "component/user_math.h" -// LQR状态向量维度定义 -#define LQR_STATE_DIM 6 // 状态向量维度 [x, x_dot, theta, theta_dot, phi_L, phi_R] -#define LQR_INPUT_DIM 2 // 输入向量维度 [T_L, T_R] (左右腿力矩) +/* Exported types ----------------------------------------------------------- */ -// 系统状态索引 -typedef enum { - STATE_POSITION = 0, // 机体位置 x - STATE_VELOCITY = 1, // 机体速度 x_dot - STATE_PITCH = 2, // 俯仰角 theta - STATE_PITCH_RATE = 3, // 俯仰角速度 theta_dot - STATE_LEG_L = 4, // 左腿角度 phi_L - STATE_LEG_R = 5 // 右腿角度 phi_R -} LQR_State_Index_t; - -// 控制输入索引 -typedef enum { - INPUT_TORQUE_L = 0, // 左腿力矩 - INPUT_TORQUE_R = 1 // 右腿力矩 -} LQR_Input_Index_t; - -// LQR参数结构体 +/** + * @brief LQR控制器状态向量定义 + * + * 状态向量维度: 10 x 1 + * [s, ds, phi, dphi, theta_ll, dtheta_ll, theta_lr, dtheta_lr, theta_b, dtheta_b]^T + */ typedef struct { - float Q[LQR_STATE_DIM][LQR_STATE_DIM]; // 状态权重矩阵 - float R[LQR_INPUT_DIM][LQR_INPUT_DIM]; // 输入权重矩阵 - float K[LQR_INPUT_DIM][LQR_STATE_DIM]; // LQR增益矩阵 - float max_torque; // 最大输出力矩限制 - float deadband_position; // 位置死区 - float deadband_angle; // 角度死区 -} LQR_Params_t; - -// 系统状态结构体 -typedef struct { - float position; // 机体位置 (m) - float velocity; // 机体速度 (m/s) - float pitch_angle; // 俯仰角 (rad) - float pitch_rate; // 俯仰角速度 (rad/s) - float leg_angle_L; // 左腿角度 (rad) - float leg_angle_R; // 右腿角度 (rad) - float leg_length_L; // 左腿长度 (m) - float leg_length_R; // 右腿长度 (m) + float s; // 机器人水平方向移动距离 (m) + float ds; // 机器人水平方向移动速度 (m/s) + float phi; // 机器人水平方向移动时yaw偏航角度 (rad) + float dphi; // yaw偏航角速度 (rad/s) + float theta_ll; // 左腿摆杆与竖直方向夹角 (rad) + float dtheta_ll; // 左腿摆杆角速度 (rad/s) + float theta_lr; // 右腿摆杆与竖直方向夹角 (rad) + float dtheta_lr; // 右腿摆杆角速度 (rad/s) + float theta_b; // 机体与水平方向夹角 (rad) + float dtheta_b; // 机体角速度 (rad/s) } LQR_State_t; -// 目标状态结构体 +/** + * @brief LQR控制器控制输入向量定义 + * + * 控制向量维度: 4 x 1 + * [T_wl, T_wr, T_bl, T_br]^T + */ typedef struct { - float position_ref; // 目标位置 - float velocity_ref; // 目标速度 - float pitch_ref; // 目标俯仰角 - float leg_angle_L_ref; // 左腿目标角度 - float leg_angle_R_ref; // 右腿目标角度 -} LQR_Reference_t; + float T_wl; // 左侧驱动轮输出力矩 (N*m) + float T_wr; // 右侧驱动轮输出力矩 (N*m) + float T_bl; // 左侧髋关节输出力矩 (N*m) + float T_br; // 右侧髋关节输出力矩 (N*m) +} LQR_Control_t; -// LQR控制器结构体 +/** + * @brief LQR增益矩阵K (4x10) + */ typedef struct { - LQR_Params_t params; // LQR参数 - LQR_State_t current_state; // 当前状态 - LQR_Reference_t reference; // 参考状态 - float control_output[LQR_INPUT_DIM]; // 控制输出 - float state_error[LQR_STATE_DIM]; // 状态误差 - uint8_t initialized; // 初始化标志 + float K[4][10]; // LQR反馈增益矩阵 +} LQR_GainMatrix_t; + +/** + * @brief 简化的LQR控制器实例结构体 + */ +typedef struct { + LQR_GainMatrix_t K; // 增益矩阵 + LQR_State_t state; // 当前状态 + LQR_State_t reference; // 参考状态 + LQR_Control_t control; // 控制输出 + + float max_wheel_torque; // 轮毂电机最大力矩限制 (N*m) + float max_joint_torque; // 关节电机最大力矩限制 (N*m) + bool initialized; // 初始化标志 } LQR_Controller_t; +/* Exported constants ------------------------------------------------------- */ + +#define LQR_STATE_DIM (10) // 状态向量维度 +#define LQR_CONTROL_DIM (4) // 控制向量维度 + +/* Exported macros ---------------------------------------------------------- */ + +/** + * @brief 角度归一化到[-PI, PI] + */ +#define LQR_ANGLE_NORMALIZE(angle) do { \ + while((angle) > M_PI) (angle) -= 2*M_PI; \ + while((angle) < -M_PI) (angle) += 2*M_PI; \ +} while(0) + +/** + * @brief 数值限幅 + */ +#define LQR_CLAMP(val, min, max) ((val) < (min) ? (min) : ((val) > (max) ? (max) : (val))) + +/* Exported functions prototypes -------------------------------------------- */ + /** * @brief 初始化LQR控制器 - * @param lqr LQR控制器指针 - * @param params LQR参数指针 - * @return 0: 成功, -1: 失败 + * @param lqr LQR控制器实例 + * @param max_wheel_torque 轮毂电机最大力矩 (N*m) + * @param max_joint_torque 关节电机最大力矩 (N*m) + * @return 0:成功, -1:失败 */ -int8_t LQR_Init(LQR_Controller_t *lqr, const LQR_Params_t *params); +int8_t LQR_Init(LQR_Controller_t *lqr, float max_wheel_torque, float max_joint_torque); /** - * @brief 设置LQR增益矩阵 - * @param lqr LQR控制器指针 - * @param K 增益矩阵 [INPUT_DIM][STATE_DIM] - * @return 0: 成功, -1: 失败 + * @brief 设置固定LQR增益矩阵 + * @param lqr LQR控制器实例 + * @param K 增益矩阵 + * @return 0:成功, -1:失败 */ -int8_t LQR_SetGains(LQR_Controller_t *lqr, float K[LQR_INPUT_DIM][LQR_STATE_DIM]); +int8_t LQR_SetGainMatrix(LQR_Controller_t *lqr, const LQR_GainMatrix_t *K); /** - * @brief 更新系统状态 - * @param lqr LQR控制器指针 - * @param state 当前状态指针 - * @return 0: 成功, -1: 失败 + * @brief 更新机器人状态 + * @param lqr LQR控制器实例 + * @param state 当前状态 + * @return 0:成功, -1:失败 */ int8_t LQR_UpdateState(LQR_Controller_t *lqr, const LQR_State_t *state); /** * @brief 设置参考状态 - * @param lqr LQR控制器指针 - * @param reference 参考状态指针 - * @return 0: 成功, -1: 失败 + * @param lqr LQR控制器实例 + * @param reference 参考状态 + * @return 0:成功, -1:失败 */ -int8_t LQR_SetReference(LQR_Controller_t *lqr, const LQR_Reference_t *reference); +int8_t LQR_SetReference(LQR_Controller_t *lqr, const LQR_State_t *reference); /** - * @brief LQR控制计算 - * @param lqr LQR控制器指针 - * @return 0: 成功, -1: 失败 + * @brief 计算LQR控制输出 + * @param lqr LQR控制器实例 + * @return 0:成功, -1:失败 */ -int8_t LQR_Calculate(LQR_Controller_t *lqr); +int8_t LQR_ComputeControl(LQR_Controller_t *lqr); /** * @brief 获取控制输出 - * @param lqr LQR控制器指针 - * @param torque_L 左腿力矩输出指针 - * @param torque_R 右腿力矩输出指针 - * @return 0: 成功, -1: 失败 + * @param lqr LQR控制器实例 + * @param control 控制输出 + * @return 0:成功, -1:失败 */ -int8_t LQR_GetOutput(LQR_Controller_t *lqr, float *torque_L, float *torque_R); +int8_t LQR_GetControl(const LQR_Controller_t *lqr, LQR_Control_t *control); /** - * @brief 重置LQR控制器 - * @param lqr LQR控制器指针 - * @return 0: 成功, -1: 失败 + * @brief 重置LQR控制器状态 + * @param lqr LQR控制器实例 */ -int8_t LQR_Reset(LQR_Controller_t *lqr); +void LQR_Reset(LQR_Controller_t *lqr); /** - * @brief 限制输出幅值 - * @param value 输入值 - * @param limit 限制值 - * @return 限制后的值 + * @brief 从轮腿机器人传感器数据构建LQR状态 + * @param position_x 机体x位置 (m) + * @param velocity_x 机体x速度 (m/s) + * @param yaw_angle yaw角度 (rad) + * @param yaw_rate yaw角速度 (rad/s) + * @param left_leg_angle 左腿角度 (rad) + * @param left_leg_rate 左腿角速度 (rad/s) + * @param right_leg_angle 右腿角度 (rad) + * @param right_leg_rate 右腿角速度 (rad/s) + * @param body_pitch 机体pitch角度 (rad) + * @param body_pitch_rate 机体pitch角速度 (rad/s) + * @param state 输出状态 + * @return 0:成功, -1:失败 */ -static inline float LQR_Limit(float value, float limit) { - if (value > limit) return limit; - if (value < -limit) return -limit; - return value; -} - -/** - * @brief 死区处理 - * @param value 输入值 - * @param deadband 死区大小 - * @return 处理后的值 - */ -static inline float LQR_Deadband(float value, float deadband) { - if (fabs(value) < deadband) return 0.0f; - return value > 0 ? (value - deadband) : (value + deadband); -} +int8_t LQR_BuildStateFromSensors(float position_x, float velocity_x, + float yaw_angle, float yaw_rate, + float left_leg_angle, float left_leg_rate, + float right_leg_angle, float right_leg_rate, + float body_pitch, float body_pitch_rate, + LQR_State_t *state); #ifdef __cplusplus } -#endif \ No newline at end of file +#endif diff --git a/User/component/vmc.c b/User/component/vmc.c new file mode 100644 index 0000000..35aaffb --- /dev/null +++ b/User/component/vmc.c @@ -0,0 +1,381 @@ +/* + * VMC虚拟模型控制器实现 + * + * 本文件实现了轮腿机器人的VMC (Virtual Model Control) 虚拟模型控制算法 + * 主要功能包括: + * 1. 五连杆机构的正逆运动学解算 + * 2. 虚拟力到关节力矩的映射 + * 3. 地面接触检测 + * 4. 等效摆动杆模型转换 + * + * 参考文献: + * - 韭菜的菜 知乎: 平衡步兵控制系统设计 + * - 上交轮腿电控开源方案 (2023) + */ + +#include "vmc.h" +#include + +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ + +#define VMC_EPSILON (1e-6f) // 数值计算精度 +#define VMC_MAX_ITER (10) // 最大迭代次数 + +/* Private macro ------------------------------------------------------------ */ + +/** + * @brief 限制数值范围 + */ +#define VMC_CLAMP(val, min, max) ((val) < (min) ? (min) : ((val) > (max) ? (max) : (val))) + +/** + * @brief 安全开方 + */ +#define VMC_SAFE_SQRT(x) (((x) > 0) ? sqrtf(x) : 0.0f) + +/* Private variables -------------------------------------------------------- */ +/* Private function prototypes ---------------------------------------------- */ + +static int8_t VMC_ValidateParams(const VMC_Param_t *param); +static void VMC_UpdateKinematics(VMC_t *vmc, float phi1, float phi4); +static int8_t VMC_SolveClosedLoop(VMC_t *vmc); +static float VMC_ComputeNumericDerivative(float current, float previous, float dt); + +/* Exported functions ------------------------------------------------------- */ + +/** + * @brief 初始化VMC控制器 + */ +int8_t VMC_Init(VMC_t *vmc, const VMC_Param_t *param, float sample_freq) { + if (vmc == NULL || param == NULL || sample_freq <= 0) { + return -1; + } + + // 复制参数 + memcpy(&vmc->param, param, sizeof(VMC_Param_t)); + + // 设置控制周期 + vmc->dt = 1.0f / sample_freq; + + // 重置状态 + VMC_Reset(vmc); + + vmc->initialized = true; + + return 0; +} + +/** + * @brief VMC五连杆正解算 + * + * 通过髋关节角度和机体姿态，计算足端位置和等效摆动杆参数 + * + * 坐标系定义: + * - x轴: 机体前进方向为正 + * - y轴: 竖直向下为正 + * - 角度: 顺时针为正 + */ +int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, float body_pitch_rate) { + if (vmc == NULL || !vmc->initialized) { + return -1; + } + + VMC_Leg_t *leg = &vmc->leg; + + // 保存历史值 + leg->last_phi0 = leg->phi0; + leg->last_L0 = leg->L0; + leg->last_d_L0 = leg->d_L0; + leg->last_d_theta = leg->d_theta; + + // 更新关节角度 + leg->phi1 = phi1; + leg->phi4 = phi4; + + // 更新运动学状态 + VMC_UpdateKinematics(vmc, phi1, phi4); + + // 求解闭环运动学 + if (VMC_SolveClosedLoop(vmc) != 0) { + return -1; + } + + // 计算足端坐标 + leg->foot_x = leg->XC - vmc->param.hip_length / 2.0f; + leg->foot_y = leg->YC; + + // 计算等效摆动杆参数 + leg->L0 = VMC_SAFE_SQRT(leg->foot_x * leg->foot_x + leg->foot_y * leg->foot_y); + leg->phi0 = atan2f(leg->foot_y, leg->foot_x); + + // 计算等效摆动杆角度(相对于机体坐标系) + leg->alpha = VMC_PI_2 - leg->phi0; + leg->theta = -(VMC_PI_2 + body_pitch - leg->phi0); + + // 角度归一化 + VMC_ANGLE_NORMALIZE(leg->theta); + VMC_ANGLE_NORMALIZE(leg->phi0); + + // 计算角速度和长度变化率 + leg->d_phi0 = VMC_ComputeNumericDerivative(leg->phi0, leg->last_phi0, vmc->dt); + leg->d_alpha = -leg->d_phi0; + leg->d_theta = body_pitch_rate + leg->d_phi0; + leg->d_L0 = VMC_ComputeNumericDerivative(leg->L0, leg->last_L0, vmc->dt); + + // 计算角加速度 + leg->dd_theta = VMC_ComputeNumericDerivative(leg->d_theta, leg->last_d_theta, vmc->dt); + + return 0; +} + +/** + * @brief VMC五连杆逆解算(力矩分配) + * + * 根据期望的虚拟力和力矩，通过雅可比矩阵计算关节力矩 + */ +int8_t VMC_InverseSolve(VMC_t *vmc, float F_virtual, float T_virtual) { + if (vmc == NULL || !vmc->initialized) { + return -1; + } + + // 保存虚拟力和力矩 + vmc->leg.F_virtual = -F_virtual; + vmc->leg.T_virtual = T_virtual; + + // 计算雅可比矩阵 + if (VMC_ComputeJacobian(vmc) != 0) { + return -1; + } + + VMC_Leg_t *leg = &vmc->leg; + + // 通过雅可比转置计算关节力矩 + // tau = J^T * F_virtual + leg->tau_hip_rear = leg->J11 * vmc->leg.F_virtual + leg->J12 * vmc->leg.T_virtual; + leg->tau_hip_front = leg->J21 * vmc->leg.F_virtual + leg->J22 * vmc->leg.T_virtual; + + return 0; +} + +/** + * @brief 地面接触检测 + * + * 基于虚拟力和腿部状态估计地面法向力 + */ +float VMC_GroundContactDetection(VMC_t *vmc) { + if (vmc == NULL || !vmc->initialized) { + return 0.0f; + } + + VMC_Leg_t *leg = &vmc->leg; + + // 计算地面法向力 + // Fn = F0*cos(theta) + Tp*sin(theta)/L0 + mg + leg->Fn = leg->F_virtual * cosf(leg->theta) + + leg->T_virtual * sinf(leg->theta) / leg->L0 + + vmc->param.wheel_mass * 9.8f; // 添加轮子重力 + + // 地面接触判断 + leg->is_ground_contact = (leg->Fn > 10.0f); // 10N阈值 + + return leg->Fn; +} + +/** + * @brief 获取足端位置 + */ +int8_t VMC_GetFootPosition(const VMC_t *vmc, float *x, float *y) { + if (vmc == NULL || !vmc->initialized || x == NULL || y == NULL) { + return -1; + } + + *x = vmc->leg.foot_x; + *y = vmc->leg.foot_y; + + return 0; +} + +/** + * @brief 获取等效摆动杆参数 + */ +int8_t VMC_GetVirtualLegState(const VMC_t *vmc, float *length, float *angle, float *d_length, float *d_angle) { + if (vmc == NULL || !vmc->initialized) { + return -1; + } + + if (length) *length = vmc->leg.L0; + if (angle) *angle = vmc->leg.theta; + if (d_length) *d_length = vmc->leg.d_L0; + if (d_angle) *d_angle = vmc->leg.d_theta; + + return 0; +} + +/** + * @brief 获取关节输出力矩 + */ +int8_t VMC_GetJointTorques(const VMC_t *vmc, float *tau_front, float *tau_rear) { + if (vmc == NULL || !vmc->initialized || tau_front == NULL || tau_rear == NULL) { + return -1; + } + + *tau_front = vmc->leg.tau_hip_front; + *tau_rear = vmc->leg.tau_hip_rear; + + return 0; +} + +/** + * @brief 重置VMC控制器状态 + */ +void VMC_Reset(VMC_t *vmc) { + if (vmc == NULL) { + return; + } + + // 清零腿部状态 + memset(&vmc->leg, 0, sizeof(VMC_Leg_t)); + + // 设置初始值 + vmc->leg.L0 = 0.15f; // 默认腿长15cm + vmc->leg.theta = 0.0f; +} + +/** + * @brief 设置虚拟力和力矩 + */ +void VMC_SetVirtualForces(VMC_t *vmc, float F_virtual, float T_virtual) { + if (vmc == NULL || !vmc->initialized) { + return; + } + + vmc->leg.F_virtual = F_virtual; + vmc->leg.T_virtual = T_virtual; +} + +/** + * @brief 计算雅可比矩阵 + * + * 根据当前关节配置计算从虚拟力到关节力矩的雅可比矩阵 + */ +int8_t VMC_ComputeJacobian(VMC_t *vmc) { + if (vmc == NULL || !vmc->initialized) { + return -1; + } + + VMC_Leg_t *leg = &vmc->leg; + + // 检查分母不为零 + float sin_diff = sinf(leg->phi3 - leg->phi2); + if (fabsf(sin_diff) < VMC_EPSILON) { + return -1; // 奇异配置 + } + + // 计算雅可比矩阵元素 + // J11: 后髋关节到支撑力的雅可比 + leg->J11 = (vmc->param.leg_1 * sinf(leg->phi0 - leg->phi3) * + sinf(leg->phi1 - leg->phi2)) / sin_diff; + + // J12: 后髋关节到摆动力矩的雅可比 + leg->J12 = (vmc->param.leg_1 * cosf(leg->phi0 - leg->phi3) * + sinf(leg->phi1 - leg->phi2)) / (leg->L0 * sin_diff); + + // J21: 前髋关节到支撑力的雅可比 + leg->J21 = (vmc->param.leg_4 * sinf(leg->phi0 - leg->phi2) * + sinf(leg->phi3 - leg->phi4)) / sin_diff; + + // J22: 前髋关节到摆动力矩的雅可比 + leg->J22 = (vmc->param.leg_4 * cosf(leg->phi0 - leg->phi2) * + sinf(leg->phi3 - leg->phi4)) / (leg->L0 * sin_diff); + + return 0; +} + +/* Private functions -------------------------------------------------------- */ + +/** + * @brief 验证VMC参数有效性 + */ +static int8_t VMC_ValidateParams(const VMC_Param_t *param) { + if (param->hip_length <= 0 || param->leg_1 <= 0 || param->leg_2 <= 0 || + param->leg_3 <= 0 || param->leg_4 <= 0 || param->wheel_radius <= 0) { + return -1; + } + + // 检查腿部几何约束 + if (param->leg_2 + param->leg_3 <= param->leg_1 + param->leg_4) { + return -1; // 不满足闭环几何约束 + } + + return 0; +} + +/** + * @brief 更新基本运动学参数 + */ +static void VMC_UpdateKinematics(VMC_t *vmc, float phi1, float phi4) { + VMC_Leg_t *leg = &vmc->leg; + + // 计算关键点坐标 + // 点B (后髋关节末端) + leg->XB = vmc->param.leg_1 * cosf(phi1); + leg->YB = vmc->param.leg_1 * sinf(phi1); + + // 点D (前髋关节末端) + leg->XD = vmc->param.hip_length + vmc->param.leg_4 * cosf(phi4); + leg->YD = vmc->param.leg_4 * sinf(phi4); + + // 计算BD连杆长度 + float dx = leg->XD - leg->XB; + float dy = leg->YD - leg->YB; + leg->lBD = VMC_SAFE_SQRT(dx * dx + dy * dy); +} + +/** + * @brief 求解闭环运动学方程 + * + * 根据两个髋关节角度，求解中间关节角度 + */ +static int8_t VMC_SolveClosedLoop(VMC_t *vmc) { + VMC_Leg_t *leg = &vmc->leg; + + // 使用余弦定理求解phi2 + leg->A0 = 2 * vmc->param.leg_2 * (leg->XD - leg->XB); + leg->B0 = 2 * vmc->param.leg_2 * (leg->YD - leg->YB); + leg->C0 = vmc->param.leg_2 * vmc->param.leg_2 + + leg->lBD * leg->lBD - + vmc->param.leg_3 * vmc->param.leg_3; + + // 检查判别式 + float discriminant = leg->A0 * leg->A0 + leg->B0 * leg->B0 - leg->C0 * leg->C0; + if (discriminant < 0) { + return -1; // 无解 + } + + float sqrt_discriminant = VMC_SAFE_SQRT(discriminant); + + // 计算phi2 (选择合适的解) + leg->phi2 = 2 * atan2f(leg->B0 + sqrt_discriminant, leg->A0 + leg->C0); + + // 计算phi3 + leg->phi3 = atan2f(leg->YB - leg->YD + vmc->param.leg_2 * sinf(leg->phi2), + leg->XB - leg->XD + vmc->param.leg_2 * cosf(leg->phi2)); + + // 计算足端坐标点C + leg->XC = leg->XB + vmc->param.leg_2 * cosf(leg->phi2); + leg->YC = leg->YB + vmc->param.leg_2 * sinf(leg->phi2); + + return 0; +} + +/** + * @brief 计算数值微分 + */ +static float VMC_ComputeNumericDerivative(float current, float previous, float dt) { + if (dt <= 0) { + return 0.0f; + } + + return (current - previous) / dt; +} diff --git a/User/component/vmc.h b/User/component/vmc.h new file mode 100644 index 0000000..10969c9 --- /dev/null +++ b/User/component/vmc.h @@ -0,0 +1,196 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include +#include +#include "kinematics.h" + +/* Exported types ----------------------------------------------------------- */ + +/** + * @brief VMC虚拟模型控制参数结构体 + */ +typedef struct { + float hip_length; // 髋关节间距 + float leg_1; // 大腿前端长度 (L1) + float leg_2; // 大腿后端长度 (L2) + float leg_3; // 小腿长度 (L3) + float leg_4; // 小腿前端长度 (L4) + float wheel_radius; // 轮子半径 + float wheel_mass; // 轮子质量 +} VMC_Param_t; + +/** + * @brief VMC腿部运动学状态结构体 + */ +typedef struct { + // 关节角度 + float phi1; // 后髋关节角度 (rad) + float phi2; // 大腿后端角度 (rad) + float phi3; // 小腿角度 (rad) + float phi4; // 前髋关节角度 (rad) + + // 足端坐标 + float foot_x; // 足端x坐标 + float foot_y; // 足端y坐标 + + // 等效摆动杆参数 + float L0; // 等效摆动杆长度 + float d_L0; // 等效摆动杆长度变化率 + float theta; // 等效摆动杆角度 + float d_theta; // 等效摆动杆角速度 + float dd_theta; // 等效摆动杆角加速度 + + // 虚拟力和力矩 + float F_virtual; // 虚拟支撑力 + float T_virtual; // 虚拟摆动力矩 + + // 雅可比矩阵元素 + float J11, J12, J21, J22; + + // 输出力矩 + float tau_hip_front; // 前髋关节输出力矩 + float tau_hip_rear; // 后髋关节输出力矩 + + // 内部计算变量 + float XB, YB, XC, YC, XD, YD; // 各关键点坐标 + float lBD; // BD连杆长度 + float A0, B0, C0; // 运动学计算中间变量 + float phi0; // 足端极角 + float alpha; // 等效摆动杆与竖直方向夹角 + float d_phi0; // 足端极角变化率 + float d_alpha; // alpha角变化率 + + // 历史值（用于数值微分） + float last_phi0; + float last_L0; + float last_d_L0; + float last_d_theta; + + // 地面接触检测 + float Fn; // 地面法向力 + bool is_ground_contact; // 地面接触标志 +} VMC_Leg_t; + +/** + * @brief VMC控制器结构体 + */ +typedef struct { + VMC_Param_t param; // VMC参数 + VMC_Leg_t leg; // 腿部状态 + float dt; // 控制周期 + bool initialized; // 初始化标志 +} VMC_t; + +/* Exported constants ------------------------------------------------------- */ + +#define VMC_PI_2 (1.5707963267948966f) +#define VMC_PI (3.1415926535897932f) +#define VMC_2PI (6.2831853071795865f) + +/* Exported macros ---------------------------------------------------------- */ + +/** + * @brief 角度范围限制到[-PI, PI] + */ +#define VMC_ANGLE_NORMALIZE(angle) do { \ + while((angle) > VMC_PI) (angle) -= VMC_2PI; \ + while((angle) < -VMC_PI) (angle) += VMC_2PI; \ +} while(0) + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 初始化VMC控制器 + * @param vmc VMC控制器实例 + * @param param VMC参数 + * @param sample_freq 采样频率 (Hz) + * @return 0:成功, -1:失败 + */ +int8_t VMC_Init(VMC_t *vmc, const VMC_Param_t *param, float sample_freq); + +/** + * @brief VMC五连杆正解算 + * @param vmc VMC控制器实例 + * @param phi1 后髋关节角度 (rad) + * @param phi4 前髋关节角度 (rad) + * @param body_pitch 机体pitch角 (rad) + * @param body_pitch_rate 机体pitch角速度 (rad/s) + * @return 0:成功, -1:失败 + */ +int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, float body_pitch_rate); + +/** + * @brief VMC五连杆逆解算(力矩分配) + * @param vmc VMC控制器实例 + * @param F_virtual 期望虚拟支撑力 (N) + * @param T_virtual 期望虚拟摆动力矩 (N*m) + * @return 0:成功, -1:失败 + */ +int8_t VMC_InverseSolve(VMC_t *vmc, float F_virtual, float T_virtual); + +/** + * @brief 地面接触检测 + * @param vmc VMC控制器实例 + * @return 地面法向力 (N) + */ +float VMC_GroundContactDetection(VMC_t *vmc); + +/** + * @brief 获取足端位置(直角坐标) + * @param vmc VMC控制器实例 + * @param x 足端x坐标输出 + * @param y 足端y坐标输出 + * @return 0:成功, -1:失败 + */ +int8_t VMC_GetFootPosition(const VMC_t *vmc, float *x, float *y); + +/** + * @brief 获取等效摆动杆参数 + * @param vmc VMC控制器实例 + * @param length 等效摆动杆长度输出 + * @param angle 等效摆动杆角度输出 + * @param d_length 等效摆动杆长度变化率输出 + * @param d_angle 等效摆动杆角速度输出 + * @return 0:成功, -1:失败 + */ +int8_t VMC_GetVirtualLegState(const VMC_t *vmc, float *length, float *angle, float *d_length, float *d_angle); + +/** + * @brief 获取关节输出力矩 + * @param vmc VMC控制器实例 + * @param tau_front 前髋关节力矩输出 + * @param tau_rear 后髋关节力矩输出 + * @return 0:成功, -1:失败 + */ +int8_t VMC_GetJointTorques(const VMC_t *vmc, float *tau_front, float *tau_rear); + +/** + * @brief 重置VMC控制器状态 + * @param vmc VMC控制器实例 + */ +void VMC_Reset(VMC_t *vmc); + +/** + * @brief 设置虚拟力和力矩 + * @param vmc VMC控制器实例 + * @param F_virtual 虚拟支撑力 (N) + * @param T_virtual 虚拟摆动力矩 (N*m) + */ +void VMC_SetVirtualForces(VMC_t *vmc, float F_virtual, float T_virtual); + +/** + * @brief 计算雅可比矩阵 + * @param vmc VMC控制器实例 + * @return 0:成功, -1:失败 + */ +int8_t VMC_ComputeJacobian(VMC_t *vmc); + +#ifdef __cplusplus +} +#endif diff --git a/User/device/motor_lz.c b/User/device/motor_lz.c index dc4e733..d6b5bb5 100644 --- a/User/device/motor_lz.c +++ b/User/device/motor_lz.c @@ -205,9 +205,15 @@ static void MOTOR_LZ_Decode(MOTOR_LZ_t *motor, BSP_CAN_Message_t *msg) { uint16_t raw_torque = (uint16_t)((msg->data[4] << 8) | msg->data[5]); float torque = MOTOR_LZ_RawToFloat(raw_torque, LZ_TORQUE_RANGE_NM); + while (angle <0){ + angle += M_2PI; + } + while (angle > M_2PI){ + angle -= M_2PI; + } // 自动反向 if (motor->param.reverse) { - angle = -angle; + angle = M_2PI - angle; velocity = -velocity; torque = -torque; } @@ -220,7 +226,7 @@ static void MOTOR_LZ_Decode(MOTOR_LZ_t *motor, BSP_CAN_Message_t *msg) { motor->lz_feedback.temperature = (float)raw_temp / LZ_TEMP_SCALE; motor->motor.feedback.rotor_abs_angle = angle; - motor->motor.feedback.rotor_speed = velocity * 180.0f / M_PI * 6.0f; + motor->motor.feedback.rotor_speed = velocity; motor->motor.feedback.torque_current = torque; motor->motor.feedback.temp = (int8_t)motor->lz_feedback.temperature; motor->motor.header.online = true; diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index d2890c8..988e09b 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -4,29 +4,151 @@ #include "component/user_math.h" #include "component/kinematics.h" #include +#include + + +float fn=0.0f; +float tp=0.0f; + +float t1=0.0f; +float t2=0.0f; + static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) { if (c == NULL) return CHASSIS_ERR_NULL; /* 主结构体不能为空 */ if (mode == c->mode) return CHASSIS_OK; /* 模式未改变直接返回 */ - PID_Reset(&c->pid.left_wheel); - PID_Reset(&c->pid.right_wheel); - PID_Reset(&c->pid.follow); - PID_Reset(&c->pid.balance); - + MOTOR_LK_MotorOn(&c->param->wheel_motors[0]); + MOTOR_LK_MotorOn(&c->param->wheel_motors[1]); + for (int i = 0; i < 4; i++) { + MOTOR_LZ_Enable(&c->param->joint_motors[i]); + } + c->mode = mode; c->state = 0; // 重置状态，确保每次切换模式时都重新初始化 return CHASSIS_OK; } +/* 更新机体状态估计 */ +static void Chassis_UpdateChassisState(Chassis_t *c) { + if (c == NULL) return; + + // 从轮子编码器估计机体速度 (参考C++代码) + float left_wheel_speed_dps = c->feedback.wheel[0].rotor_speed; // dps (度每秒) + float right_wheel_speed_dps = c->feedback.wheel[1].rotor_speed; // dps (度每秒) + + // 将dps转换为rad/s + float left_wheel_speed = left_wheel_speed_dps * M_PI / 180.0f; // rad/s + float right_wheel_speed = right_wheel_speed_dps * M_PI / 180.0f; // rad/s + + float wheel_radius = 0.072f; + + float left_wheel_linear_vel = left_wheel_speed * wheel_radius; + float right_wheel_linear_vel = right_wheel_speed * wheel_radius; + + // 机体x方向速度 (轮子中心速度) + c->chassis_state.last_velocity_x = c->chassis_state.velocity_x; + c->chassis_state.velocity_x = (left_wheel_linear_vel + right_wheel_linear_vel) / 2.0f; + + // 积分得到位置 + c->chassis_state.position_x += c->chassis_state.velocity_x * c->dt; +} + +/* 执行LQR控制 */ +static int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { + if (c == NULL || c_cmd == NULL) return -1; + + // 构建当前状态 + LQR_State_t current_state; + float left_leg_length, left_leg_angle, left_leg_d_length, left_leg_d_angle; + float right_leg_length, right_leg_angle, right_leg_d_length, right_leg_d_angle; + + // 获取等效摆动杆状态 + VMC_GetVirtualLegState(&c->vmc_[0], &left_leg_length, &left_leg_angle, &left_leg_d_length, &left_leg_d_angle); + VMC_GetVirtualLegState(&c->vmc_[1], &right_leg_length, &right_leg_angle, &right_leg_d_length, &right_leg_d_angle); + + LQR_BuildStateFromSensors( + c->chassis_state.position_x, + c->chassis_state.velocity_x, + c->feedback.imu.euler.yaw, + c->feedback.imu.gyro.z, + left_leg_angle, + left_leg_d_angle, + right_leg_angle, + right_leg_d_angle, + c->feedback.imu.euler.pit, + c->feedback.imu.gyro.y, + ¤t_state + ); + + // 设置参考状态 + LQR_State_t reference_state = {0}; + reference_state.s = 0.0f; // 期望位移设为0（相对平衡位置） + reference_state.ds = c_cmd->move_vec.vx; // 期望速度 + reference_state.phi = 0.0f; // 期望yaw角度 + reference_state.dphi = c_cmd->move_vec.wz; // 期望yaw角速度 + // 其他状态保持为0（平衡状态） + + // 更新LQR控制器状态 + LQR_UpdateState(&c->lqr, ¤t_state); + LQR_SetReference(&c->lqr, &reference_state); + + // 计算控制输出 + if (LQR_ComputeControl(&c->lqr) != 0) { + return -1; + } + + // 获取控制输出 + LQR_Control_t lqr_output; + LQR_GetControl(&c->lqr, &lqr_output); + + // 分配力矩到电机 + // 轮毂电机 (考虑减速比) + // float wheel_gear_ratio = 19.2f; + // MOTOR_LK_SetTorque(&c->param->wheel_motors[0], lqr_output.T_wl / wheel_gear_ratio); + // MOTOR_LK_SetTorque(&c->param->wheel_motors[1], lqr_output.T_wr / wheel_gear_ratio); + c->output.wheel[0] = lqr_output.T_wl/2.5; // 轮子电机输出 + c->output.wheel[1] = lqr_output.T_wr/2.5; + // 通过VMC将虚拟力转换为关节力矩 + // 左腿 + float F_virtual_left = lqr_output.T_bl; // 简化映射，实际需要更复杂的转换 + // float T_virtual_left = 0.0f; + float T_virtual_left = lqr_output.T_wl; // 左腿虚拟摆动力矩 + VMC_InverseSolve(&c->vmc_[0], F_virtual_left, T_virtual_left); + + float tau_left_front, tau_left_rear; + VMC_GetJointTorques(&c->vmc_[0], &tau_left_front, &tau_left_rear); + + // 右腿 + float F_virtual_right = lqr_output.T_br; + // float T_virtual_right = 0.0f; + float T_virtual_right = lqr_output.T_wr; // 右腿虚拟摆动力矩 + VMC_InverseSolve(&c->vmc_[1], F_virtual_right, T_virtual_right); + + float tau_right_front, tau_right_rear; + VMC_GetJointTorques(&c->vmc_[1], &tau_right_front, &tau_right_rear); + + // 输出到关节电机 + // MOTOR_LZ_SetTorque(&c->param->joint_motors[0], tau_left_rear); // 左后 + // MOTOR_LZ_SetTorque(&c->param->joint_motors[1], tau_left_front); // 左前 + // MOTOR_LZ_SetTorque(&c->param->joint_motors[2], tau_right_front);// 右前 + // MOTOR_LZ_SetTorque(&c->param->joint_motors[3], tau_right_rear); // 右后 + c->output.joint[0].torque = tau_left_rear; + c->output.joint[1].torque = tau_left_front; + c->output.joint[2].torque = tau_right_front; + c->output.joint[3].torque = tau_right_rear; + return 0; +} + int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq){ if (c == NULL || param == NULL || target_freq <= 0.0f) { return -1; // 参数错误 } c->param = param; + /*初始化can*/ BSP_CAN_Init(); @@ -39,15 +161,19 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq){ for (int i = 0; i < 2; i++) { MOTOR_LK_Register(&c->param->wheel_motors[i]); } - /*初始化pid*/ - PID_Init(&c->pid.left_wheel, KPID_MODE_CALC_D, target_freq, ¶m->motor_pid_param); - PID_Init(&c->pid.right_wheel, KPID_MODE_CALC_D, target_freq, ¶m->motor_pid_param); - PID_Init(&c->pid.follow, KPID_MODE_CALC_D, target_freq, ¶m->follow_pid_param); - PID_Init(&c->pid.balance, KPID_MODE_CALC_D, target_freq, ¶m->balance_pid_param); - - // 初始化设定点 - // c->setpoint.left_wheel = 0.0f; - // c->setpoint.right_wheel = 0.0f; + + /*初始化VMC控制器*/ + VMC_Init(&c->vmc_[0], ¶m->vmc_param[0], target_freq); + VMC_Init(&c->vmc_[1], ¶m->vmc_param[1], target_freq); + + /*初始化LQR控制器*/ + LQR_Init(&c->lqr, param->lqr_param.max_wheel_torque, param->lqr_param.max_joint_torque); + LQR_SetGainMatrix(&c->lqr, ¶m->lqr_gains); + + /*初始化机体状态*/ + c->chassis_state.position_x = 0.0f; + c->chassis_state.velocity_x = 0.0f; + c->chassis_state.last_velocity_x = 0.0f; return CHASSIS_OK; } @@ -69,6 +195,7 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c){ MOTOR_LZ_t *joint_motor = MOTOR_LZ_GetMotor(&c->param->joint_motors[i]); if (joint_motor != NULL) { c->feedback.joint[i] = joint_motor->motor.feedback; + c->feedback.joint[i].rotor_abs_angle = joint_motor->motor.feedback.rotor_abs_angle - M_PI; // 机械零点调整 } } @@ -79,6 +206,10 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c){ c->feedback.wheel[i] = wheel_motor->motor.feedback; } } + + // 更新机体状态估计 + Chassis_UpdateChassisState(c); + return 0; } @@ -87,6 +218,7 @@ int8_t Chassis_UpdateIMU(Chassis_t *c, const Chassis_IMU_t imu){ return -1; // 参数错误 } c->feedback.imu = imu; + // c->feedback.imu.euler.pit = - c->feedback.imu.euler.pit; return 0; } @@ -106,153 +238,82 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd){ switch (c->mode) { case CHASSIS_MODE_RELAX: // 放松模式，电机不输出 - // for (int i = 0; i < 4; i++) { - // MOTOR_LZ_Relax(&c->param->joint_motors[i]); - // } - // for (int i = 0; i < 2; i++) { - // MOTOR_LK_Relax(&c->param->wheel_motors[i]); // 改为Relax以保持反馈 - // } MOTOR_LZ_Relax(&c->param->joint_motors[0]); MOTOR_LZ_Relax(&c->param->joint_motors[1]); MOTOR_LZ_Relax(&c->param->joint_motors[2]); MOTOR_LZ_Relax(&c->param->joint_motors[3]); MOTOR_LK_Relax(&c->param->wheel_motors[0]); MOTOR_LK_Relax(&c->param->wheel_motors[1]); - // 定义腿部运动学参数（假设单位为mm转换为m） - KIN_SerialLeg_Param_t left_leg_param = { - .thigh_length = 0.215f, - .calf_length = 0.258f - }; - KIN_SerialLeg_Param_t right_leg_param = { - .thigh_length = 0.215f, - .calf_length = 0.258f - }; - for (int i = 0; i < 4; i++) { - MOTOR_LZ_RecoverToZero(&c->param->joint_motors[i]); - } + + // 更新VMC正解算用于状态估计 + VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle, c->feedback.joint[1].rotor_abs_angle, + c->feedback.imu.euler.pit, c->feedback.imu.gyro.y); + VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle, + c->feedback.imu.euler.pit, c->feedback.imu.gyro.y); + + VMC_InverseSolve(&c->vmc_[0], fn, tp); + + VMC_GetJointTorques(&c->vmc_[0], &t1, &t2); - KIN_SerialLeg_FK(&left_leg_param, - &c->feedback.joint[0].rotor_abs_angle, - &c->feedback.joint[1].rotor_abs_angle, - &c->angle, - &c->height); + // MOTOR_LZ_MotionControl(&c->param->joint_motors[0], &(MOTOR_LZ_MotionParam_t){.torque = t1}); + + + + // Chassis_LQRControl(c, c_cmd); // 即使在放松模式下也执行LQR以保持状态更新 + break; case CHASSIS_MODE_RECOVER: - switch (c->state) { - case 0: - //使能电机 - for (int i = 0; i < 4; i++) { - MOTOR_LZ_Enable(&c->param->joint_motors[i]); - } - for (int i = 0; i < 2; i++) { - MOTOR_LK_MotorOn(&c->param->wheel_motors[i]); - } - c->state += 1; - break; - case 1: - // 关节电机复位轮电机输出0； - for (int i = 0; i < 4; i++) { - MOTOR_LZ_RecoverToZero(&c->param->joint_motors[i]); - } - for (int i = 0; i < 2; i++) { - MOTOR_LK_Relax(&c->param->wheel_motors[i]); - } - break; - } + // 恢复模式，使用简单的关节位置控制回到初始姿态 + // TODO: 实现恢复逻辑 break; - - case CHASSIS_MODE_WHELL_BALANCE: - { - // 轮腿平衡模式：不修改关节电机，只控制轮毂电机进行平衡 - switch (c->state) { - case 0: - // 初始化状态：使能轮毂电机和关节电机 - for (int i = 0; i < 2; i++) { - MOTOR_LK_MotorOn(&c->param->wheel_motors[i]); - } - for (int i = 0; i < 4; i++) { - MOTOR_LZ_Enable(&c->param->joint_motors[i]); - } - - c->state = 1; - c->height = 0.16f; - c->setpoint.chassis.rol = 0.0f; - c->setpoint.chassis.pit = 0.0f; - c->setpoint.chassis.yaw = c->feedback.imu.euler.yaw; - break; - - case 1: { - KIN_SerialLeg_Param_t leg_param = { - .thigh_length = 0.215f, - .calf_length = 0.258f - }; - - float angle = 1.35f; - // float height = 0.16f; - c->height += c_cmd->height * 0.000001f; - c->setpoint.chassis.yaw += c_cmd->move_vec.wz * -0.0005f; // 目标yaw角随输入变化 - if (c->height < 0.16f) c->height = 0.16f; - if (c->height > 0.35f) c->height = 0.35f; - - KIN_SerialLeg_IK(&leg_param, - &angle, - &c->height, - &c->output.joint[0].target_angle, - &c->output.joint[1].target_angle); - c->output.joint[3].target_angle = c->output.joint[0].target_angle; - c->output.joint[2].target_angle = c->output.joint[1].target_angle; - - for (int i = 0; i < 4; i++) { - c->output.joint[i].torque = 0.0f; - c->output.joint[i].target_velocity = 0.0f; - c->output.joint[i].kp = 50.0f; - c->output.joint[i].kd = 1.0f; - MOTOR_LZ_MotionControl(&c->param->joint_motors[i], &c->output.joint[i]); - } - - - // 轮毂电机平衡控制 - 双环控制结构 - // 外环：角度控制器（平衡控制器） - // 内环：速度控制器（轮子电机控制器） - float pitch_angle = c->feedback.imu.euler.pit; // 机体俯仰角 - float pitch_rate = c->feedback.imu.gyro.y; // 俯仰角速度 - - - - // 外环：平衡控制器，目标俯仰角为0，输出目标速度 - // float target_speed = PID_Calc(&c->pid.balance, 0.0f, pitch_angle, 0.0, c->dt); - float target_speed = PID_Calc(&c->pid.balance, c_cmd->move_vec.vx/10.0f, pitch_angle, 0.0, c->dt); - // 内环：速度控制器，控制轮子速度跟踪目标速度 - float left_wheel_speed = c->feedback.wheel[0].rotor_speed/2000; // 当前左轮速度 - float right_wheel_speed = c->feedback.wheel[1].rotor_speed/2000; // 当前右轮速度 - - - float target_yaw = c->setpoint.chassis.yaw; // 目标 yaw 角度 - float current_yaw = c->feedback.imu.euler.yaw; // 当前 yaw 角度 - float target_yaw_rate = PID_Calc(&c->pid.follow, target_yaw, current_yaw, 0.0f, c->dt); - - - float left_speed_output = PID_Calc(&c->pid.left_wheel, target_speed - target_yaw_rate, left_wheel_speed, 0.0f, c->dt); - float right_speed_output = PID_Calc(&c->pid.right_wheel, target_speed + target_yaw_rate, right_wheel_speed, 0.0f, c->dt); - - // 输出到轮毂电机 - c->output.wheel[0] = left_speed_output; - c->output.wheel[1] = right_speed_output; - // c->output.wheel[0] = target_speed; - // c->output.wheel[1] = target_speed; - MOTOR_LK_SetOutput(&c->param->wheel_motors[0], c->output.wheel[0]); - MOTOR_LK_SetOutput(&c->param->wheel_motors[1], c->output.wheel[1]); - - break; - } - - } + case CHASSIS_MODE_WHELL_BALANCE: + // 更新VMC正解算用于状态估计 + // MOTOR_LZ_Relax(&c->param->joint_motors[0]); + // MOTOR_LZ_Relax(&c->param->joint_motors[1]); + // MOTOR_LZ_Relax(&c->param->joint_motors[2]); + // MOTOR_LZ_Relax(&c->param->joint_motors[3]); + // MOTOR_LK_Relax(&c->param->wheel_motors[0]); + // MOTOR_LK_Relax(&c->param->wheel_motors[1]); + for (int i = 0; i < 4; i++) { + c->output.joint[i].torque = 0.0f; + } + for (int i = 0; i < 2; i++) { + c->output.wheel[i] = 0.0f; + } + + // 更新VMC正解算用于状态估计 + VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle, c->feedback.joint[1].rotor_abs_angle, + c->feedback.imu.euler.pit, c->feedback.imu.gyro.y); + VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle, + c->feedback.imu.euler.pit, c->feedback.imu.gyro.y); + + VMC_InverseSolve(&c->vmc_[0], fn, tp); + VMC_GetJointTorques(&c->vmc_[0], &t1, &t2); + + c->output.joint[0].torque = t1; + c->output.joint[1].torque = t2; + + // Chassis_LQRControl(c, c_cmd); // 即使在放松模式下也执行LQR以保持状态更新 + + Chassis_Output(c); // 统一输出 break; - } + case CHASSIS_MODE_WHELL_LEG_BALANCE: - // 轮子+腿平衡模式（暂时留空，后续实现） + // 轮腿平衡模式，使用LQR控制 + + // // 更新VMC正解算 + // VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle, c->feedback.joint[1].rotor_abs_angle, + // c->feedback.imu.euler.pit, c->feedback.imu.gyro.y); + // VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle, + // c->feedback.imu.euler.pit, c->feedback.imu.gyro.y); + + // // 执行LQR控制 + // if (Chassis_LQRControl(c, c_cmd) != 0) { + // // LQR控制失败，切换到安全模式 + // return CHASSIS_ERR; + // } break; default: @@ -264,7 +325,14 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd){ void Chassis_Output(Chassis_t *c) { if (c == NULL) return; - + for (int i = 0; i < 4; i++) { + MOTOR_LZ_MotionParam_t param = {0}; + param.torque = c->output.joint[i].torque; + MOTOR_LZ_MotionControl(&c->param->joint_motors[i], ¶m); + } + for (int i = 0; i < 2; i++) { + MOTOR_LK_SetOutput(&c->param->wheel_motors[i], c->output.wheel[i]); + } // 这个函数已经在各个模式中直接调用了电机输出函数 // 如果需要统一输出，可以在这里实现 // 现在的设计是在控制逻辑中直接输出，所以这里留空 diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h index 06b7730..3db068a 100644 --- a/User/module/balance_chassis.h +++ b/User/module/balance_chassis.h @@ -16,7 +16,8 @@ extern "C" { /* Includes ----------------------------------------------------------------- */ #include #include -// #include "component/cmd.h" +#include "component/vmc.h" +#include "component/lqr.h" #include "component/ahrs.h" #include "component/filter.h" #include "component/pid.h" @@ -67,14 +68,20 @@ typedef struct { /* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */ typedef struct { - KPID_Params_t motor_pid_param; /* 轮子控制PID的参数 */ - KPID_Params_t follow_pid_param; /* 跟随云台PID的参数 */ - KPID_Params_t balance_pid_param; /* 平衡PID的参数 */ + + VMC_Param_t vmc_param[2]; /* VMC参数 */ + LQR_GainMatrix_t lqr_gains; /* LQR增益矩阵 */ MOTOR_LZ_Param_t joint_motors[4]; /* 四个关节电机参数 */ MOTOR_LK_Param_t wheel_motors[2]; /* 两个轮子电机参数 */ float mech_zero_yaw; /* 机械零点 */ + + /* LQR控制器参数 */ + struct { + float max_wheel_torque; /* 轮毂电机最大力矩限制 */ + float max_joint_torque; /* 关节电机最大力矩限制 */ + } lqr_param; /* 低通滤波器截止频率 */ struct { @@ -102,10 +109,20 @@ typedef struct { /* 控制信息*/ Chassis_Output_t output; + VMC_t vmc_[2]; /* 两条腿的VMC */ + LQR_Controller_t lqr; /* LQR控制器 */ + int8_t state; float angle; float height; + + /* 机体状态估计 */ + struct { + float position_x; /* 机体x位置 */ + float velocity_x; /* 机体x速度 */ + float last_velocity_x; /* 上一次速度用于数值微分 */ + } chassis_state; float wz_multi; /* 小陀螺模式旋转方向 */ diff --git a/User/module/config.c b/User/module/config.c index c26edb1..b8bd55b 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -15,6 +15,7 @@ // 机器人参数配置 Config_RobotParam_t robot_config = { + .imu_param = { .can = BSP_CAN_2, .can_id = 0x6FF, @@ -67,37 +68,6 @@ Config_RobotParam_t robot_config = { }, .chassis_param = { - .follow_pid_param = { - .k = 1.0f, - .p = 5.0f, - .i = 0.0f, - .d = 0.1f, - .i_limit = 0.0f, - .out_limit = 1.0f, - .d_cutoff_freq = -1.0f, - .range = -1.0f, - }, - .motor_pid_param = { - .k = 0.8f, - .p = 2.0f, // 速度环PID参数，通常P比位置环小 - .i = 0.0f, // 增加积分项，消除稳态误差 - .d = 0.0f, // 减小微分项，避免噪声放大 - .i_limit = 0.0f, - .out_limit = 1.0f, // 限制在[-1, 1]范围内 - .d_cutoff_freq = -1.0f, // 增加微分项滤波频率 - .range = -1.0f, // 速度控制不需要循环范围 - }, - .balance_pid_param = { - .k = -1.0f, - .p = 5.0f, // 增大比例项，提高响应速度 - .i = 0.2f, // 增加积分项，消除稳态误差 - .d = 0.2f, // 增加微分项，提高稳定性 - .i_limit = 1.0f, // 限制积分饱和 - .out_limit = 1.0f, // 输出目标速度，单位可能是rpm或rad/s - .d_cutoff_freq = -1.0f, // 微分项滤波 - .range = -1.0f, // 角度控制不需要循环范围（这里是pitch角度） - }, - .low_pass_cutoff_freq = { .in = 30.0f, .out = 30.0f, @@ -108,7 +78,7 @@ Config_RobotParam_t robot_config = { .motor_id = 124, .host_id = 130, .module = MOTOR_LZ_RSO3, - .reverse = false, + .reverse = true, .mode = MOTOR_LZ_MODE_MOTION, }, { // 左膝关节 @@ -132,7 +102,7 @@ Config_RobotParam_t robot_config = { .motor_id = 127, .host_id = 130, .module = MOTOR_LZ_RSO3, - .reverse = true, + .reverse = false, .mode = MOTOR_LZ_MODE_MOTION, }, }, @@ -151,6 +121,32 @@ Config_RobotParam_t robot_config = { }, }, .mech_zero_yaw = 0.0f, + .vmc_param = { + { // 左腿 + .leg_1 = 0.206f, // 前大腿长度 (m) + .leg_2 = 0.258f, // 前小腿长度 (m) + .leg_3 = 0.206f, // 后小腿长度 (m) + .leg_4 = 0.258f, // 后大腿长度 (m) + .hip_length = 0.0f // 髋宽 (m) + }, + { // 右腿 + .leg_1 = 0.206f, // 前大腿长度 (m) + .leg_2 = 0.258f, // 前小腿长度 (m) + .leg_3 = 0.206f, // 后小腿长度 (m) + .leg_4 = 0.258f, // 后大腿长度 (m) + .hip_length = 0.0f // 髋宽 (m) + } + }, + .lqr_gains ={ + .K = { + { -1.3677, -12.022, 4.0676, -2.6185, -66.132, -4.2516, -1.4083, -0.051404, -57.561, -5.3641 }, + { -1.3677, -12.022, -4.0676, 2.6185, -1.4083, -0.051404, -66.132, -4.2516, -57.561, -5.3641 }, + { 0.14689, 1.2865, -63.224, -12.495, 6.2265, -0.13959, 1.2635, 0.48938, -78.822, -5.121 }, + { 0.14689, 1.2865, 63.224, 12.495, 1.2635, 0.48938, 6.2265, -0.13959, -78.822, -5.121 } + } + }, + .lqr_param.max_joint_torque = 20.0f, // 关节电机最大力矩 20Nm + .lqr_param.max_wheel_torque = 2.5f, // 轮毂电机最大力矩 2.5Nm } }; diff --git a/User/task/rc.c b/User/task/rc.c index bcbc569..2a303ff 100644 --- a/User/task/rc.c +++ b/User/task/rc.c @@ -46,7 +46,7 @@ void Task_rc(void *argument) { cmd_to_chassis.mode = CHASSIS_MODE_RELAX; break; case 3: // 中位 - cmd_to_chassis.mode = CHASSIS_MODE_RECOVER; + cmd_to_chassis.mode = CHASSIS_MODE_RELAX; break; case 2: // 下位 cmd_to_chassis.mode = CHASSIS_MODE_WHELL_BALANCE; diff --git a/d435.py b/d435.py new file mode 100644 index 0000000..e69de29 diff --git a/utils/lqr.asv b/utils/lqr.asv new file mode 100644 index 0000000..811ff95 --- /dev/null +++ b/utils/lqr.asv @@ -0,0 +1,321 @@ +% v1：这份LQR程序是参考我之前写的哈工程LQR程序以及小周写的AB矩阵求解器优化后写出来的，感谢周神（2024/05/07） +% v2：添加了可以专门调试定腿长的功能（2024/05/08） +% v3：优化部分注释，添加单位说明（2024/05/15） +% v4: 优化了输出，输出矩阵K的系数可以真正的复制到C里（2024/05/16） + +% 以下所有变量含义参考2023上交轮腿电控开源（https://bbs.robomaster.com/forum.php?mod=viewthread&tid=22756）所使用符号含义 + +%%%%%%%%%%%%%%%%%%%%%%%%%Step 0：重置程序，定义变量%%%%%%%%%%%%%%%%%%%%%%%%% +tic +clear all +clc + +% 定义机器人机体参数 +syms R_w % 驱动轮半径 +syms R_l % 驱动轮轮距/2 +syms l_l l_r % 左右腿长 +syms l_wl l_wr % 驱动轮质心到左右腿部质心距离 +syms l_bl l_br % 机体质心到左右腿部质心距离 +syms l_c % 机体质心到腿部关节中心点距离 +syms m_w m_l m_b % 驱动轮质量 腿部质量 机体质量 +syms I_w % 驱动轮转动惯量 (自然坐标系法向) +syms I_ll I_lr % 驱动轮左右腿部转动惯量 (自然坐标系法向，实际上会变化) +syms I_b % 机体转动惯量 (自然坐标系法向) +syms I_z % 机器人z轴转动惯量 (简化为常量) + +% 定义其他独立变量并补充其导数 +syms theta_wl theta_wr % 左右驱动轮转角 +syms dtheta_wl dtheta_wr +syms ddtheta_wl ddtheta_wr ddtheta_ll ddtheta_lr ddtheta_b + +% 定义状态向量 +syms s ds phi dphi theta_ll dtheta_ll theta_lr dtheta_lr theta_b dtheta_b + +% 定义控制向量 +syms T_wl T_wr T_bl T_br + +% 输入物理参数：重力加速度 +syms g + + + +%%%%%%%%%%%%%%%%%%%%%%%Step 1：解方程，求控制矩阵A，B%%%%%%%%%%%%%%%%%%%%%%% + +% 通过原文方程组(3.11)-(3.15)，求出ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b表达式 +eqn1 = (I_w*l_l/R_w+m_w*R_w*l_l+m_l*R_w*l_bl)*ddtheta_wl+(m_l*l_wl*l_bl-I_ll)*ddtheta_ll+(m_l*l_wl+m_b*l_l/2)*g*theta_ll+T_bl-T_wl*(1+l_l/R_w)==0; +eqn2 = (I_w*l_r/R_w+m_w*R_w*l_r+m_l*R_w*l_br)*ddtheta_wr+(m_l*l_wr*l_br-I_lr)*ddtheta_lr+(m_l*l_wr+m_b*l_r/2)*g*theta_lr+T_br-T_wr*(1+l_r/R_w)==0; +eqn3 = -(m_w*R_w*R_w+I_w+m_l*R_w*R_w+m_b*R_w*R_w/2)*ddtheta_wl-(m_w*R_w*R_w+I_w+m_l*R_w*R_w+m_b*R_w*R_w/2)*ddtheta_wr-(m_l*R_w*l_wl+m_b*R_w*l_l/2)*ddtheta_ll-(m_l*R_w*l_wr+m_b*R_w*l_r/2)*ddtheta_lr+T_wl+T_wr==0; +eqn4 = (m_w*R_w*l_c+I_w*l_c/R_w+m_l*R_w*l_c)*ddtheta_wl+(m_w*R_w*l_c+I_w*l_c/R_w+m_l*R_w*l_c)*ddtheta_wr+m_l*l_wl*l_c*ddtheta_ll+m_l*l_wr*l_c*ddtheta_lr-I_b*ddtheta_b+m_b*g*l_c*theta_b-(T_wl+T_wr)*l_c/R_w-(T_bl+T_br)==0; +eqn5 = ((I_z*R_w)/(2*R_l)+I_w*R_l/R_w)*ddtheta_wl-((I_z*R_w)/(2*R_l)+I_w*R_l/R_w)*ddtheta_wr+(I_z*l_l)/(2*R_l)*ddtheta_ll-(I_z*l_r)/(2*R_l)*ddtheta_lr-T_wl*R_l/R_w+T_wr*R_l/R_w==0; +[ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b] = solve(eqn1,eqn2,eqn3,eqn4,eqn5,ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b); + + +% 通过计算雅可比矩阵的方法得出控制矩阵A，B所需要的全部偏导数 +J_A = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[theta_ll,theta_lr,theta_b]); +J_B = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[T_wl,T_wr,T_bl,T_br]); + +% 定义矩阵A，B，将指定位置的数值根据上述偏导数计算出来并填入 +A = sym('A',[10 10]); +B = sym('B',[10 4]); + +% 填入A数据：a25,a27,a29,a45,a47,a49,a65,a67,a69,a85,a87,a89,a105,a107,a109 +for p = 5:2:9 + A_index = (p - 3)/2; + A(2,p) = R_w*(J_A(1,A_index) + J_A(2,A_index))/2; + A(4,p) = (R_w*(- J_A(1,A_index) + J_A(2,A_index)))/(2*R_l) - (l_l*J_A(3,A_index))/(2*R_l) + (l_r*J_A(4,A_index))/(2*R_l); + for q = 6:2:10 + A(q,p) = J_A(q/2,A_index); + end +end + +% A的以下数值为1：a12,a34,a56,a78,a910，其余数值为0 +for r = 1:10 + if rem(r,2) == 0 + A(r,1) = 0; A(r,2) = 0; A(r,3) = 0; A(r,4) = 0; A(r,6) = 0; A(r,8) = 0; A(r,10) = 0; + else + A(r,:) = zeros(1,10); + A(r,r+1) = 1; + end +end + +% 填入B数据：b21,b22,b23,b24,b41,b42,b43,b44,b61,b62,b63,b64,b81,b82,b83,b84,b101,b102,b103,b104, +for h = 1:4 + B(2,h) = R_w*(J_B(1,h) + J_B(2,h))/2; + B(4,h) = (R_w*(- J_B(1,h) + J_B(2,h)))/(2*R_l) - (l_l*J_B(3,h))/(2*R_l) + (l_r*J_B(4,h))/(2*R_l); + for f = 6:2:10 + B(f,h) = J_B(f/2,h); + end +end + +% B的其余数值为0 +for e = 1:2:9 + B(e,:) = zeros(1,4); +end + + + +%%%%%%%%%%%%%%%%%%%%%Step 2：输入参数（可以修改的部分）%%%%%%%%%%%%%%%%%%%%% + +% 物理参数赋值（唯一此处不可改变！），后面的数据通过增加后缀_ac区分模型符号和实际数据 +g_ac = 9.81; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% 此处可以输入机器人机体基本参数 % +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%%%%%%%%%%%%%%%%%%%%%%%%%%机器人机体与轮部参数%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +R_w_ac = 0.77; % 驱动轮半径 （单位：m） +R_l_ac = 0.210; % 两个驱动轮之间距离/2 （单位：m） +l_c_ac = 0.025; % 机体质心到腿部关节中心点距离 （单位：m） +m_w_ac = 0.5; m_l_ac = 2.133; m_b_ac = 4.542; % 驱动轮质量 腿部质量 机体质量 （单位：kg） +I_w_ac = (7630000)*10^(-7); % 驱动轮转动惯量 （单位：kg m^2） +I_b_ac = 0.3470; % 机体转动惯量(自然坐标系法向) （单位：kg m^2） +I_z_ac = 0.322; % 机器人z轴转动惯量 （单位：kg m^2） + +%%%%%%%%%%%%%%%%%%%%%%机器人腿部参数（定腿长调试用）%%%%%%%%%%%%%%%%%%%%%%%% + +% 如果需要使用此部分，请去掉120-127行以及215-218行注释，然后将224行之后的所有代码注释掉 +% 或者点击左侧数字"224"让程序只运行行之前的内容并停止 + +l_l_ac = 0.16; % 左腿摆杆长度 （左腿对应数据） （单位：m） +l_wl_ac = 0.10; % 左驱动轮质心到左腿摆杆质心距离 （单位：m） +l_bl_ac = 0.4; % 机体转轴到左腿摆杆质心距离 （单位：m） +I_ll_ac = 0.01886; % 左腿摆杆转动惯量 （单位：kg m^2） +l_r_ac = 0.16; % 右腿摆杆长度 （右腿对应数据） （单位：m） +l_wr_ac = 0.10; % 右驱动轮质心到右腿摆杆质心距离 （单位：m） +l_br_ac = 0.4; % 机体转轴到右腿摆杆质心距离 （单位：m） +I_lr_ac = 0.01886; % 右腿摆杆转动惯量 （单位：kg m^2） + +% 机体转轴定义参考哈工程开源（https://zhuanlan.zhihu.com/p/563048952），是左右 +% 两侧两个关节电机之间的中间点相连所形成的轴 +% （如果目的是小板凳，考虑使左右腿相关数据一致） + +%%%%%%%%%%%%%%%%%%%%%%%%%%%机器人腿部参数数据集%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% 根据不同腿长长度，先针对左腿测量出对应的l_wl，l_bl，和I_ll +% 通过以下方式记录数据: 矩阵分4列， +% 第一列为左腿腿长范围区间中所有小数点精度0.01的长度，例如：0.09，0.18，单位：m +% 第二列为l_wl，单位：m +% 第三列为l_bl，单位：m +% 第四列为I_ll，单位：kg m^2 +% （注意单位别搞错！） +% 行数根据L_0范围区间（，单位cm时）的整数数量进行调整 + +Leg_data_l = [0.11, 0.0480, 0.0620, 0.01819599; + 0.12, 0.0470, 0.0730, 0.01862845; + 0.13, 0.0476, 0.0824, 0.01898641; + 0.14, 0.0480, 0.0920, 0.01931342; + 0.15, 0.0490, 0.1010, 0.01962521; + 0.16, 0.0500, 0.1100, 0.01993092; + 0.17, 0.0510, 0.1190, 0.02023626; + 0.18, 0.0525, 0.1275, 0.02054500; + 0.19, 0.0539, 0.1361, 0.02085969; + 0.20, 0.0554, 0.1446, 0.02118212; + 0.21, 0.0570, 0.1530, 0.02151357; + 0.22, 0.0586, 0.1614, 0.02185496; + 0.23, 0.0600, 0.1700, 0.02220695; + 0.24, 0.0621, 0.1779, 0.02256999; + 0.25, 0.0639, 0.1861, 0.02294442; + 0.26, 0.0657, 0.1943, 0.02333041; + 0.27, 0.0676, 0.2024, 0.02372806; + 0.28, 0.0700, 0.2100, 0.02413735; + 0.29, 0.0713, 0.2187, 0.02455817; + 0.30, 0.0733, 0.2267, 0.02499030]; +% 以上数据应通过实际测量或sw图纸获得 + +% 由于左右腿部数据通常完全相同，我们通过复制的方式直接定义右腿的全部数据集 +% 矩阵分4列，第一列为右腿腿长范围区间中（，单位cm时）的整数腿长l_r*0.01，第二列为l_wr，第三列为l_br，第四列为I_lr） +Leg_data_r = Leg_data_l; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% 此处可以输入QR矩阵 % +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% 矩阵Q中，以下列分别对应： +% s ds phi dphi theta_ll dtheta_ll theta_lr dtheta_lr theta_b dtheta_b +lqr_Q = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0; + 0, 2, 0, 0, 0, 0, 0, 0, 0, 0; + 0, 0, 12000, 0, 0, 0, 0, 0, 0, 0; + 0, 0, 0, 200, 0, 0, 0, 0, 0, 0; + 0, 0, 0, 0, 1000, 0, 0, 0, 0, 0; + 0, 0, 0, 0, 0, 1, 0, 0, 0, 0; + 0, 0, 0, 0, 0, 0, 1000, 0, 0, 0; + 0, 0, 0, 0, 0, 0, 0, 1, 0, 0; + 0, 0, 0, 0, 0, 0, 0, 0, 20000, 0; + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1]; +% 其中： +% s : 自然坐标系下机器人水平方向移动距离，单位：m，ds为其导数 +% phi ：机器人水平方向移动时yaw偏航角度，dphi为其导数 +% theta_ll：左腿摆杆与竖直方向（自然坐标系z轴）夹角，dtheta_ll为其导数 +% theta_lr：右腿摆杆与竖直方向（自然坐标系z轴）夹角，dtheta_lr为其导数 +% theta_b ：机体与自然坐标系水平夹角，dtheta_b为其导数 + +% 矩阵中，以下列分别对应： +% T_wl T_wr T_bl T_br +lqr_R = [0.25, 0, 0, 0; + 0, 0.25, 0, 0; + 0, 0, 1.5, 0; + 0, 0, 0, 1.5]; +% 其中： +% T_wl: 左侧驱动轮输出力矩 +% T_wr：右侧驱动轮输出力矩 +% T_bl：左侧髋关节输出力矩 +% T_br：右腿髋关节输出力矩 +% 单位皆为Nm + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + +%%%%%%%%%%%%%%%%%%%%%Step 2.5：求解矩阵（定腿长调试用）%%%%%%%%%%%%%%%%%%%%% + +% 如果需要使用此部分，请去掉120-127行以及215-218行注释，然后将224行之后的所有代码注释掉， +% 或者点击左侧数字"224"让程序只运行行之前的内容并停止 +K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ... + R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ... + l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ... + A,B,lqr_Q,lqr_R) +K = sprintf([strjoin(repmat({'%.5g'},1,size(K,2)),', ') '\n'], K.') + + +%%%%%%%%%%%%%%%%%%%%%%%%%%Step 3：拟合控制律函数%%%%%%%%%%%%%%%%%%%%%%%%%% + +sample_size = size(Leg_data_l,1)^2; % 单个K_ij拟合所需要的样本数 + +length = size(Leg_data_l,1); % 测量腿部数据集的行数 + +% 定义所有K_ij依据l_l,l_r变化的表格，每一个表格有3列，第一列是l_l，第二列 +% 是l_r，第三列是对应的K_ij的数值 +K_sample = zeros(sample_size,3,40); % 40是因为增益矩阵K应该是4行10列。 + +for i = 1:length + for j = 1:length + index = (i - 1)*length + j; + l_l_ac = Leg_data_l(i,1); % 提取左腿对应的数据 + l_wl_ac = Leg_data_l(i,2); + l_bl_ac = Leg_data_l(i,3); + I_ll_ac = Leg_data_l(i,4); + l_r_ac = Leg_data_r(j,1); % 提取右腿对应的数据 + l_wr_ac = Leg_data_r(j,2); + l_br_ac = Leg_data_r(j,3); + I_lr_ac = Leg_data_r(j,4); + for k = 1:40 + K_sample(index,1,k) = l_l_ac; + K_sample(index,2,k) = l_r_ac; + end + K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ... + R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ... + l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ... + A,B,lqr_Q,lqr_R); + % 根据指定的l_l,l_r输入对应的K_ij的数值 + for l = 1:4 + for m = 1:10 + K_sample(index,3,(l - 1)*10 + m) = K(l,m); + end + end + end +end + +% 创建收集全部K_ij的多项式拟合的全部系数的集合 +K_Fit_Coefficients = zeros(40,6); +for n = 1:40 + K_Surface_Fit = fit([K_sample(:,1,n),K_sample(:,2,n)],K_sample(:,3,n),'poly22'); + K_Fit_Coefficients(n,:) = coeffvalues(K_Surface_Fit); % 拟合并提取出拟合的系数结果 +end +Polynomial_expression = formula(K_Surface_Fit) + +% 最终返回的结果K_Fit_Coefficients是一个40行6列矩阵，每一行分别对应一个K_ij的多项式拟合的全部系数 +% 每一行和K_ij的对应关系如下： +% - 第1行对应K_1,1 +% - 第14行对应K_2,4 +% - 第22行对应K_3,2 +% - 第37行对应K_4,7 +% ... 其他行对应关系类似 +% 拟合出的函数表达式为 p(x,y) = p00 + p10*x + p01*y + p20*x^2 + p11*x*y + p02*y^2 +% 其中x对应左腿腿长l_l，y对应右腿腿长l_r +% K_Fit_Coefficients每一列分别对应全部K_ij的多项式拟合的单个系数 +% 每一列和系数pij的对应关系如下： +% - 第1列对应p00 +% - 第2列对应p10 +% - 第3列对应p01 +% - 第4列对应p20 +% - 第5列对应p11 +% - 第6列对应p02 +K_Fit_Coefficients = sprintf([strjoin(repmat({'%.5g'},1,size(K_Fit_Coefficients,2)),', ') '\n'], K_Fit_Coefficients.') + +% 正确食用方法： +% 1.在C代码中写出控制律K矩阵的全部多项式，其中每一个多项式的表达式为： +% 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 +% 2.并填入对应系数即可 + +toc + +%%%%%%%%%%%%%%%%%%%%%%%%%以下信息仅供参考，可忽略%%%%%%%%%%%%%%%%%%%%%%%%%% + +% 如有需要可以把所有K_ij画出图来参考，可以去掉以下注释 +% 此版本只能同时查看其中一个K_ij，同时查看多个的功能下次更新 +% （前面的蛆，以后再来探索吧（bushi + + + +%%%%%%%%%%%%%%%%%%%%%%%%%%得出控制矩阵K使用的函数%%%%%%%%%%%%%%%%%%%%%%%%%% + +function K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ... + R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ... + l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ... + A,B,lqr_Q,lqr_R) + % 基于机体以及物理参数，获得控制矩阵A，B的全部数值 + A_ac = subs(A,[R_w R_l l_l l_r l_wl l_wr l_bl l_br l_c m_w m_l m_b I_w I_ll I_lr I_b I_z g], ... + [R_w_ac R_l_ac l_l_ac l_r_ac l_wl_ac l_wr_ac l_bl_ac l_br_ac l_c_ac ... + m_w_ac m_l_ac m_b_ac I_w_ac I_ll_ac I_lr_ac I_b_ac I_z_ac g_ac]); + B_ac = subs(B,[R_w R_l l_l l_r l_wl l_wr l_bl l_br l_c m_w m_l m_b I_w I_ll I_lr I_b I_z g], ... + [R_w_ac R_l_ac l_l_ac l_r_ac l_wl_ac l_wr_ac l_bl_ac l_br_ac l_c_ac ... + m_w_ac m_l_ac m_b_ac I_w_ac I_ll_ac I_lr_ac I_b_ac I_z_ac g_ac]); + + % 根据以上信息和提供的矩阵Q和R求解Riccati方程，获得增益矩阵K + % P为Riccati方程的解，矩阵L可以无视 + [P,K,L_k] = icare(A_ac,B_ac,lqr_Q,lqr_R,[],[],[]); +end + diff --git a/utils/lqr.m b/utils/lqr.m index 6bb4ef9..811ff95 100644 --- a/utils/lqr.m +++ b/utils/lqr.m @@ -105,10 +105,10 @@ g_ac = 9.81; %%%%%%%%%%%%%%%%%%%%%%%%%%%机器人机体与轮部参数%%%%%%%%%%%%%%%%%%%%%%%%%%%% -R_w_ac = 0.75; % 驱动轮半径 （单位：m） -R_l_ac = 0.215; % 两个驱动轮之间距离/2 （单位：m） +R_w_ac = 0.77; % 驱动轮半径 （单位：m） +R_l_ac = 0.210; % 两个驱动轮之间距离/2 （单位：m） l_c_ac = 0.025; % 机体质心到腿部关节中心点距离 （单位：m） -m_w_ac = 0.5; m_l_ac = 2.133; m_b_ac = 14.542; % 驱动轮质量 腿部质量 机体质量 （单位：kg） +m_w_ac = 0.5; m_l_ac = 2.133; m_b_ac = 4.542; % 驱动轮质量 腿部质量 机体质量 （单位：kg） I_w_ac = (7630000)*10^(-7); % 驱动轮转动惯量 （单位：kg m^2） I_b_ac = 0.3470; % 机体转动惯量(自然坐标系法向) （单位：kg m^2） I_z_ac = 0.322; % 机器人z轴转动惯量 （单位：kg m^2） @@ -120,12 +120,12 @@ I_z_ac = 0.322; % 机器人z轴转动惯量 l_l_ac = 0.16; % 左腿摆杆长度 （左腿对应数据） （单位：m） l_wl_ac = 0.10; % 左驱动轮质心到左腿摆杆质心距离 （单位：m） -l_bl_ac = 0.8; % 机体转轴到左腿摆杆质心距离 （单位：m） -I_ll_ac = 0.01186; % 左腿摆杆转动惯量 （单位：kg m^2） +l_bl_ac = 0.4; % 机体转轴到左腿摆杆质心距离 （单位：m） +I_ll_ac = 0.01886; % 左腿摆杆转动惯量 （单位：kg m^2） l_r_ac = 0.16; % 右腿摆杆长度 （右腿对应数据） （单位：m） l_wr_ac = 0.10; % 右驱动轮质心到右腿摆杆质心距离 （单位：m） -l_br_ac = 0.8; % 机体转轴到右腿摆杆质心距离 （单位：m） -I_lr_ac = 0.01186; % 右腿摆杆转动惯量 （单位：kg m^2） +l_br_ac = 0.4; % 机体转轴到右腿摆杆质心距离 （单位：m） +I_lr_ac = 0.01886; % 右腿摆杆转动惯量 （单位：kg m^2） % 机体转轴定义参考哈工程开源（https://zhuanlan.zhihu.com/p/563048952），是左右 % 两侧两个关节电机之间的中间点相连所形成的轴