robofish/rm_balance
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

修改vmc

Browse Source
This commit is contained in:
Robofish 2025-09-16 23:18:47 +08:00
parent e7aaf1b98f
commit 58f7ba86db
15 changed files with 1556 additions and 390 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
LQR_修正建议.md Normal file
View File

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

10
MDK-ARM/DevC.uvprojx
View File

@ -814,6 +814,16 @@
<FileType>1</FileType>
<FilePath>..\User\component\kinematics.c</FilePath>
</File>
<File>
<FileName>lqr.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\component\lqr.c</FilePath>
</File>
<File>
<FileName>vmc.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\component\vmc.c</FilePath>
</File>
</Files>
</Group>
<Group>

2
MDK-ARM/DevC/DevC.lnp
View File

@ -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"

269
User/component/lqr.c
View File

@ -1,144 +1,229 @@
/*
* LQR线性二次型最优控制器简化实现
*
* 本文件实现了轮腿机器人的LQR (Linear Quadratic Regulator) 控制算法
* 主要功能包括:
* 1. 状态反馈控制
* 2. 增益矩阵K计算控制输出
* 3. 控制输出限幅
*
* 系统模型:
* u = -K*(x - x_ref) (状态反馈)
*/
#include "lqr.h"
#include <string.h>
// 默认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;
}
}
/* 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;
}

223
User/component/lqr.h
View File

@ -4,148 +4,165 @@
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include <stdbool.h>
#include <math.h>
#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
#endif

381
User/component/vmc.c Normal file
View File

@ -0,0 +1,381 @@
/*
* VMC虚拟模型控制器实现
*
* 本文件实现了轮腿机器人的VMC (Virtual Model Control) 虚拟模型控制算法
* 主要功能包括:
* 1. 五连杆机构的正逆运动学解算
* 2. 虚拟力到关节力矩的映射
* 3. 地面接触检测
* 4. 等效摆动杆模型转换
*
* 参考文献:
* - 韭菜的菜 知乎: 平衡步兵控制系统设计
* - 上交轮腿电控开源方案 (2023)
*/
#include "vmc.h"
#include <string.h>
/* 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;
}

196
User/component/vmc.h Normal file
View File

@ -0,0 +1,196 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include <stdbool.h>
#include <math.h>
#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

10
User/device/motor_lz.c
View File

@ -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;

364
User/module/balance_chassis.c
View File

@ -4,29 +4,151 @@
#include "component/user_math.h"
#include "component/kinematics.h"
#include <math.h>
#include <string.h>
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,
&current_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, &current_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, &param->motor_pid_param);
PID_Init(&c->pid.right_wheel, KPID_MODE_CALC_D, target_freq, &param->motor_pid_param);
PID_Init(&c->pid.follow, KPID_MODE_CALC_D, target_freq, &param->follow_pid_param);
PID_Init(&c->pid.balance, KPID_MODE_CALC_D, target_freq, &param->balance_pid_param);
// 初始化设定点
// c->setpoint.left_wheel = 0.0f;
// c->setpoint.right_wheel = 0.0f;
/*初始化VMC控制器*/
VMC_Init(&c->vmc_[0], &param->vmc_param[0], target_freq);
VMC_Init(&c->vmc_[1], &param->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, &param->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], &param);
}
for (int i = 0; i < 2; i++) {
MOTOR_LK_SetOutput(&c->param->wheel_motors[i], c->output.wheel[i]);
}
// 这个函数已经在各个模式中直接调用了电机输出函数
// 如果需要统一输出，可以在这里实现
// 现在的设计是在控制逻辑中直接输出，所以这里留空

25
User/module/balance_chassis.h
View File

@ -16,7 +16,8 @@ extern "C" {
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include <stdbool.h>
// #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; /* 小陀螺模式旋转方向 */

62
User/module/config.c
View File

@ -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
}
};

2
User/task/rc.c
View File

@ -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;

0
d435.py Normal file
View File

321
utils/lqr.asv Normal file
View File

@ -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

14
utils/lqr.m
View File

@ -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），是左右
% 两侧两个关节电机之间的中间点相连所形成的轴