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

提交

Browse Source
This commit is contained in:
Robofish 2025-09-08 20:05:22 +08:00
parent b7876f7eab
commit e7aaf1b98f
17 changed files with 3184 additions and 219 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1358
Chassis_Task.c Normal file
View File

File diff suppressed because it is too large Load Diff

299
Chassis_Task.h Normal file
View File

@ -0,0 +1,299 @@
#ifndef _CHASSIS_TASK
#define _CHASSIS_TASK
#include "main.h"
#include "struct_typedef.h"
#include "pid.h"
#include "bsp_can.h"
// ------------- Limit info -------------
#define MAX_ACCL 13000.0f
#define MAX_ACCL_JOINT 15.0f
#define MAX_FOOT_OUTPUT 2048
// ------------- Target value info -------------
#define SIT_MODE_HEIGHT_SET 0.18f
#define NORMAL_MODE_HEIGHT_SET 0.10f
#define HIGH_MODE_HEIGHT_SET 0.27f
#define EXTREMELY_HIGH_MODE_HEIGHT_SET 0.30f
// ------------- Mech info -------------
#define L1 0.15f
#define L2 0.25f
#define L3 0.25f
#define L4 0.15f
#define L5 0.1f
#define WHEEL_PERIMETER 0.56547
#define WHEEL_RADIUS 0.09f
#define LEG_OFFSET 0.3790855135f // 水平位置到上限位的夹角
#define LOWER_SUPPORT_FORCE_FOR_JUMP 5.0f
#define LOWER_SUPPORT_FORCE 0.0f
#define MOVE_LOWER_BOUND 0.3f
#define EXIT_PITCH_ANGLE 0.1f
#define DANGER_PITCH_ANGLE 0.25f
#define FEED_f 7.5f
#define FEED_f_1 3.5f
#define NORMAL_MODE_WEIGHT_DISTANCE_OFFSET -0.0f
#define MOTOR_POS_UPPER_BOUND 0.05f
#define MOTOR_POS_LOWER_BOUND 1.4f
#define LIMITED_TORQUE 0.5f
#define UNLIMITED_TORQUE 200.0f
// ------------- Time info -------------
#define CHASSIS_TASK_INIT_TIME 500
#define TASK_RUN_TIME 0.002f
// ------------- Transfer info -------------
#define MOTOR_ECD_TO_RAD 0.00019174779 // 2*PI / 32767
#define HALF_ECD_RANGE 14383
#define HALF_POSITION_RANGE 3.0f
// #define CC 0.00512f
// #define CC 1/494.0f
#define TORQ_K 494.483818182
// ------------- Math info -------------
#define PI2 6.28318530717959f
#define PI 3.14159265358979f
#define PI_2 1.57079632679489f
#define PI_4 0.78539816339744f
typedef enum
{
ENABLE_CHASSIS = 0,
DISABLE_CHASSIS,
} chassis_mode_e;
typedef enum
{
NO_FORCE,
FOOT_LAUNCHING,
JOINT_LAUNCHING,
BALANCING_READY,
JOINT_REDUCING,
} chassis_balancing_mode_e;
typedef enum
{
NONE,
NORMAL_MOVING_MODE,
ABNORMAL_MOVING_MODE,
JUMPING_MODE,
CAP_MODE,
FLY_MODE,
TK_MODE,
} sport_mode_e;
typedef enum
{
READY_TO_JUMP,
PREPARING_LANDING,
PREPARING_STAND_JUMPING,
CONSTACTING_LEGS,
EXTENDING_LEGS,
CONSTACTING_LEGS_2,
FINISHED,
} jumping_stage_e;
typedef enum
{
NOT_DEFINE,
STANDING_JUMP,
MOVING_JUMP,
} jumping_mode_e;
typedef enum
{
SIT_MODE = 0,
NORMAL_MODE,
HIGH_MODE,
EXTREMELY_HIGH_MODE,
CHANGING_HIGH,
} chassis_high_mode_e;
typedef enum
{
MOTOR_NO_FORCE = 0,
MOTOR_FORCE,
} chassis_motor_mode_e;
typedef enum
{
ON_GROUND = 0,
OFF_GROUND = 1,
} suspend_flag_e;
typedef struct
{
chassis_mode_e chassis_mode, last_chassis_mode;
chassis_balancing_mode_e chassis_balancing_mode, last_chassis_balancing_mode;
sport_mode_e sport_mode, last_sport_mode;
jumping_mode_e jumping_mode, last_jumping_mode;
jumping_stage_e jumping_stage, last_jumping_stage;
chassis_high_mode_e chassis_high_mode, last_chassis_high_mode;
} mode_t;
typedef struct
{
const fp32 *chassis_INS_angle_point;
const fp32 *chassis_INS_gyro_point;
const fp32 *chassis_INS_accel_point;
fp32 yaw_angle, pitch_angle, roll_angle;
fp32 yaw_gyro, pitch_gyro, roll_gyro;
fp32 yaw_accel, pitch_accel, roll_accel;
fp32 yaw_angle_sett, pitch_angle_set, roll_angle_set;
fp32 yaw_gyro_set, pitch_gyro_set, roll_gyro_set;
fp32 ideal_high;
fp32 leg_length_L, last_leg_length_L, leg_length_L_set;
fp32 leg_length_R, last_leg_length_R, leg_length_R_set;
fp32 leg_dlength_L;
fp32 leg_dlength_R;
fp32 foot_roll_angle;
fp32 leg_angle_L, last_leg_angle_L, leg_angle_L_set;
fp32 leg_angle_R, last_leg_angle_R, leg_angle_R_set;
fp32 leg_gyro_L, leg_gyro_R;
fp32 foot_distance, foot_distance_K, foot_distance_set;
fp32 foot_speed, foot_speed_K, foot_speed_set;
fp32 supportive_force_L, supportive_force_R;
} chassis_posture_info_t;
typedef struct
{
// -------- horizontal force --------
fp32 joint_balancing_torque_L, joint_balancing_torque_R;
fp32 foot_balancing_torque_L, foot_balancing_torque_R;
fp32 foot_moving_torque_L, foot_moving_torque_R;
fp32 joint_moving_torque_L, joint_moving_torque_R;
fp32 joint_prevent_splits_torque_L, joint_prevent_splits_torque_R;
fp32 joint_horizontal_torque_L, joint_horizontal_torque_R;
fp32 foot_horizontal_torque_L, foot_horizontal_torque_R;
fp32 joint_horizontal_torque_temp1_R, joint_horizontal_torque_temp2_R;
fp32 joint_horizontal_torque_temp1_L, joint_horizontal_torque_temp2_L;
fp32 yaw_torque;
// -------- vertical force --------
fp32 joint_roll_torque_L, joint_roll_torque_R;
fp32 joint_stand_torque_L, joint_stand_torque_R;
fp32 joint_vertical_torque_L, joint_vertical_torque_R;
fp32 joint_real_vertical_torque_L, joint_real_vertical_torque_R;
fp32 joint_vertical_torque_temp1_R, joint_vertical_torque_temp2_R;
fp32 joint_vertical_torque_temp1_L, joint_vertical_torque_temp2_L;
} torque_info_t;
typedef struct
{
fp32 J1_L,J2_L;
fp32 J3_L,J4_L;
fp32 J1_R,J2_R;
fp32 J3_R,J4_R;
fp32 invJ1_L,invJ2_L;
fp32 invJ3_L,invJ4_L;
fp32 invJ1_R,invJ2_R;
fp32 invJ3_R,invJ4_R;
} mapping_info_t;
typedef struct
{
const HT_motor_measure_t *motor_measure;
chassis_motor_mode_e motor_mode, last_motor_mode;
bool_t offline_flag;
fp32 position;
fp32 init_position;
fp32 position_offset;
fp32 velocity;
fp32 torque_out, torque_get;
fp32 max_torque, min_torque;
} joint_motor_t;
typedef struct
{
motor_measure_t motor_measure;
chassis_motor_mode_e motor_mode, last_motor_mode;
bool_t offline_flag;
fp32 distance, distance_offset, last_position, position, turns;
fp32 speed;
fp32 torque_out, torque_get;
} foot_motor_t;
typedef struct
{
bool_t init_flag;
suspend_flag_e suspend_flag_L, last_suspend_flag_L;
suspend_flag_e suspend_flag_R, last_suspend_flag_R;
bool_t Ignore_Off_Ground;
bool_t abnormal_flag;
bool_t static_flag, last_static_flag;
bool_t moving_flag, last_moving_flag;
bool_t rotation_flag;
bool_t controlling_flag;
bool_t set_pos_after_moving;
bool_t overpower_warning_flag;
bool_t last_overpower_warning_flag;
bool_t stablize_high_flag;
bool_t last_stablize_high_flag;
} flag_info_t;
typedef struct
{
pid_type_def buffer_control_pid;
pid_type_def cap_pid;
pid_type_def scale_down_pid;
} pid_info_t;
typedef struct
{
mode_t mode;
chassis_posture_info_t chassis_posture_info;
torque_info_t torque_info;
mapping_info_t mapping_info;
flag_info_t flag_info;
pid_info_t pid_info;
const Gimbal_ctrl_t *chassis_rc_ctrl;
joint_motor_t joint_motor_1, joint_motor_2, joint_motor_3, joint_motor_4;
foot_motor_t foot_motor_L, foot_motor_R;
}chassis_control_t;
enum Chassis_Mode
{
Chassis_No_Force = 0,
Follow_Gimbal,
Follow_Gimbal_90Deg,
No_Follow,
Rotate,
// TK_MODE,
};
extern enum Chassis_Mode chassis_mode;
extern chassis_control_t chassis_control;
extern fp32 roll_PD[2];
#endif

144
User/component/lqr.c Normal file
View File

@ -0,0 +1,144 @@
#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} // 右腿力矩
};
/**
* @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度
}
// 设置默认增益矩阵
memcpy(lqr->params.K, DEFAULT_K, sizeof(DEFAULT_K));
lqr->initialized = 1;
return 0;
}
/**
* @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;
memcpy(lqr->params.K, K, sizeof(lqr->params.K));
return 0;
}
/**
* @brief 更新系统状态
*/
int8_t LQR_UpdateState(LQR_Controller_t *lqr, const LQR_State_t *state) {
if (lqr == NULL || state == NULL || !lqr->initialized) return -1;
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;
memcpy(&lqr->reference, reference, sizeof(LQR_Reference_t));
return 0;
}
/**
* @brief LQR控制计算
*/
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);
}
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];
return 0;
}
/**
* @brief 重置LQR控制器
*/
int8_t LQR_Reset(LQR_Controller_t *lqr) {
if (lqr == NULL || !lqr->initialized) return -1;
// 清零状态和输出
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));
return 0;
}

151
User/component/lqr.h Normal file
View File

@ -0,0 +1,151 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <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] (左右腿力矩)
// 系统状态索引
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参数结构体
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)
} LQR_State_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;
// LQR控制器结构体
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; // 初始化标志
} LQR_Controller_t;
/**
* @brief 初始化LQR控制器
* @param lqr LQR控制器指针
* @param params LQR参数指针
* @return 0: 成功, -1: 失败
*/
int8_t LQR_Init(LQR_Controller_t *lqr, const LQR_Params_t *params);
/**
* @brief 设置LQR增益矩阵
* @param lqr LQR控制器指针
* @param K 增益矩阵 [INPUT_DIM][STATE_DIM]
* @return 0: 成功, -1: 失败
*/
int8_t LQR_SetGains(LQR_Controller_t *lqr, float K[LQR_INPUT_DIM][LQR_STATE_DIM]);
/**
* @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: 失败
*/
int8_t LQR_SetReference(LQR_Controller_t *lqr, const LQR_Reference_t *reference);
/**
* @brief LQR控制计算
* @param lqr LQR控制器指针
* @return 0: 成功, -1: 失败
*/
int8_t LQR_Calculate(LQR_Controller_t *lqr);
/**
* @brief 获取控制输出
* @param lqr LQR控制器指针
* @param torque_L 左腿力矩输出指针
* @param torque_R 右腿力矩输出指针
* @return 0: 成功, -1: 失败
*/
int8_t LQR_GetOutput(LQR_Controller_t *lqr, float *torque_L, float *torque_R);
/**
* @brief 重置LQR控制器
* @param lqr LQR控制器指针
* @return 0: 成功, -1: 失败
*/
int8_t LQR_Reset(LQR_Controller_t *lqr);
/**
* @brief 限制输出幅值
* @param value 输入值
* @param limit 限制值
* @return 限制后的值
*/
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);
}
#ifdef __cplusplus
}
#endif

134
User/module/balance_chassis.c
View File

@ -10,12 +10,6 @@ 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参数的原始range设置
if (c->mode == CHASSIS_MODE_WHELL_BALANCE) {
((KPID_Params_t*)c->pid.left_wheel.param)->range = 0.0f;
((KPID_Params_t*)c->pid.right_wheel.param)->range = 0.0f;
}
PID_Reset(&c->pid.left_wheel);
PID_Reset(&c->pid.right_wheel);
PID_Reset(&c->pid.follow);
@ -49,11 +43,11 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq){
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->motor_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;
// c->setpoint.left_wheel = 0.0f;
// c->setpoint.right_wheel = 0.0f;
return CHASSIS_OK;
}
@ -170,59 +164,91 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd){
case CHASSIS_MODE_WHELL_BALANCE:
{
// 定义腿部运动学参数（假设单位为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
};
// 轮腿平衡模式：不修改关节电机，只控制轮毂电机进行平衡
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.2f;
float height = 0.16f;
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;
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;
}
KIN_SerialLeg_IK(&left_leg_param, &angle, &height,
KIN_SerialLeg_IK(&leg_param,
&angle,
&c->height,
&c->output.joint[0].target_angle,
&c->output.joint[1].target_angle);
c->output.joint[2].target_angle = c->output.joint[1].target_angle;
c->output.joint[3].target_angle = c->output.joint[0].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++) {
MOTOR_LZ_MotionControl(&c->param->joint_motors[i], &c->output.joint[i]);
}
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;
}
// 轮子控制：保持位置或进行平衡控制
float left_wheel_angle = c->feedback.wheel[0].rotor_abs_angle;
float right_wheel_angle = c->feedback.wheel[1].rotor_abs_angle;
// 使用setpoint作为目标位置
if (c->state == 0) {
// 首次进入模式时，将当前位置设为目标位置
c->setpoint.left_wheel = left_wheel_angle;
c->setpoint.right_wheel = right_wheel_angle;
c->state = 1;
}
float left_wheel_output = PID_Calc(&c->pid.left_wheel, c->setpoint.left_wheel, left_wheel_angle, 0.0f, c->dt);
float right_wheel_output = PID_Calc(&c->pid.right_wheel, c->setpoint.right_wheel, right_wheel_angle, 0.0f, c->dt);
// 限制输出范围
left_wheel_output = AbsClip(left_wheel_output, 1.0f);
right_wheel_output = AbsClip(right_wheel_output, 1.0f);
// 输出到轮毂电机
MOTOR_LK_SetOutput(&c->param->wheel_motors[0], left_wheel_output);
MOTOR_LK_SetOutput(&c->param->wheel_motors[1], right_wheel_output);
break;
}
case CHASSIS_MODE_WHELL_LEG_BALANCE:

4
User/module/balance_chassis.h
View File

@ -69,6 +69,7 @@ typedef struct {
typedef struct {
KPID_Params_t motor_pid_param; /* 轮子控制PID的参数 */
KPID_Params_t follow_pid_param; /* 跟随云台PID的参数 */
KPID_Params_t balance_pid_param; /* 平衡PID的参数 */
MOTOR_LZ_Param_t joint_motors[4]; /* 四个关节电机参数 */
MOTOR_LK_Param_t wheel_motors[2]; /* 两个轮子电机参数 */
@ -110,8 +111,7 @@ typedef struct {
/* PID计算的目标值 */
struct {
float left_wheel;
float right_wheel;
AHRS_Eulr_t chassis;
} setpoint;
/* 反馈控制用的PID */

37
User/module/config.c
View File

@ -69,24 +69,35 @@ Config_RobotParam_t robot_config = {
.chassis_param = {
.follow_pid_param = {
.k = 1.0f,
.p = 20.0f,
.p = 5.0f,
.i = 0.0f,
.d = 0.0f,
.d = 0.1f,
.i_limit = 0.0f,
.out_limit = 500.0f,
.d_cutoff_freq = 30.0f,
.range = 0.0f,
.out_limit = 1.0f,
.d_cutoff_freq = -1.0f,
.range = -1.0f,
},
.motor_pid_param = {
.k = 0.1f,
.p = 10.0f,
.i = 0.2f,
.d = 0.2f,
.i_limit = 0.3f,
.out_limit = -1.0f,
.d_cutoff_freq = 0.0f,
.range = M_2PI,
.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,

662
User/module/mod_wheelleg_chassis.cpp Normal file
View File

@ -0,0 +1,662 @@
#include "mod_wheelleg_chassis.hpp"
#include <cmath>
#include <tuple>
using namespace Module;
Wheelleg::Wheelleg(Param& param)
: param_(param),
roll_pid_(param.roll_pid_param, 333.0f),
yaw_pid_(param.yaw_pid_param, 333.0f),
yaccl_pid_(param.yaccl_pid_param, 333.0f),
lowfilter_(333.0f, 50.0f),
acclfilter_(333.0f, 30.0f),
manfilter_(param.manfilter_param),
ctrl_lock_(true) {
memset(&(this->cmd_), 0, sizeof(this->cmd_));
this->hip_motor_.at(0) =
new Device::MitMotor(param.hip_motor_param.at(0), "hip_left_front");
this->hip_motor_.at(1) =
new Device::MitMotor(param.hip_motor_param.at(1), "hip_left_back");
this->hip_motor_.at(2) =
new Device::MitMotor(param.hip_motor_param.at(2), "hip_right_front");
this->hip_motor_.at(3) =
new Device::MitMotor(param.hip_motor_param.at(3), "hip_left_back");
this->wheel_motor_.at(0) =
new Device::RMMotor(param.wheel_motor_param.at(0), "wheel_left");
this->wheel_motor_.at(1) =
new Device::RMMotor(param.wheel_motor_param.at(1), "wheel_right");
this->leglength_pid_.at(0) =
new Component::PID(param.leglength_pid_param.at(0), 333.0f);
this->leglength_pid_.at(1) =
new Component::PID(param.leglength_pid_param.at(1), 333.0f);
this->theta_pid_.at(0) =
new Component::PID(param.theta_pid_param.at(0), 333.0f);
this->theta_pid_.at(1) =
new Component::PID(param.theta_pid_param.at(1), 333.0f);
this->tp_pid_.at(0) = new Component::PID(param.Tp_pid_param.at(0), 333.0);
this->tp_pid_.at(1) = new Component::PID(param.Tp_pid_param.at(1), 333.0);
this->leg_.at(0) = new Component::VMC(param_.leg_param.at(0), 333.0f);
this->leg_.at(1) = new Component::VMC(param_.leg_param.at(1), 333.0f);
auto event_callback = [](Wheelleg::ChassisEvent event, Wheelleg* chassis) {
chassis->ctrl_lock_.Wait(UINT32_MAX);
switch (event) {
case SET_MODE_RELAX:
chassis->SetMode(RELAX);
break;
case SET_MODE_STAND:
chassis->SetMode(STAND);
break;
case SET_MODE_ROTOR:
chassis->SetMode(ROTOR);
break;
case SET_MODE_RESET:
chassis->SetMode(RESET);
break;
default:
break;
}
chassis->ctrl_lock_.Post();
};
Component::CMD::RegisterEvent<Wheelleg*, ChassisEvent>(
event_callback, this, this->param_.EVENT_MAP);
auto chassis_thread = [](Wheelleg* chassis) {
auto cmd_sub =
Message::Subscriber<Component::CMD::ChassisCMD>("cmd_chassis");
auto eulr_sub =
Message::Subscriber<Component::Type::Eulr>("chassis_imu_eulr");
auto gyro_sub =
Message::Subscriber<Component::Type::Vector3>("chassis_gyro");
auto yaw_sub = Message::Subscriber<float>("chassis_yaw");
auto accl_sub =
Message::Subscriber<Component::Type::Vector3>("chassis_imu_accl_abs");
// auto yaw_sub = Message::Subscriber<float>("chassis_yaw");
uint32_t last_online_time = bsp_time_get_ms();
while (1) {
eulr_sub.DumpData(chassis->eulr_); /* imu */
gyro_sub.DumpData(chassis->gyro_); /* imu */
accl_sub.DumpData(chassis->accl_);
yaw_sub.DumpData(chassis->yaw_);
cmd_sub.DumpData(chassis->cmd_);
// yaw_sub.DumpData(chassis->yaw_);
chassis->ctrl_lock_.Wait(UINT32_MAX);
chassis->MotorSetAble();
chassis->UpdateFeedback();
chassis->Calculate();
chassis->Control();
chassis->ctrl_lock_.Post();
/* 运行结束，等待下一次唤醒 */
chassis->thread_.SleepUntil(3, last_online_time);
}
};
this->thread_.Create(chassis_thread, this, "chassis_thread", 1536,
System::Thread::MEDIUM);
}
void Wheelleg::MotorSetAble() {
if (this->hip_motor_flag_ == 0) {
this->hip_motor_[0]->Relax();
this->hip_motor_[1]->Relax();
this->hip_motor_[2]->Relax();
this->hip_motor_[3]->Relax();
this->dm_motor_flag_ = 0;
}
else {
if (this->dm_motor_flag_ == 0) {
for (int i = 0; i < 5; i++) {
this->hip_motor_[0]->Enable();
}
for (int i = 0; i < 5; i++) {
this->hip_motor_[1]->Enable();
}
for (int i = 0; i < 5; i++) {
this->hip_motor_[2]->Enable();
}
for (int i = 0; i < 5; i++) {
this->hip_motor_[3]->Enable();
}
this->dm_motor_flag_ = 1;
}
};
}
void Wheelleg::UpdateFeedback() {
this->now_ = bsp_time_get();
this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_);
this->last_wakeup_ = this->now_;
this->wheel_motor_[0]->Update();
this->wheel_motor_[1]->Update();
this->leg_argu_[0].phi4_ =
this->hip_motor_[0]->GetAngle() -
this->param_.wheel_param.mech_zero[0]; // 前关节角度
this->leg_argu_[0].phi1_ =
this->hip_motor_[1]->GetAngle() -
this->param_.wheel_param.mech_zero[1]; // 后关节角度
this->leg_argu_[1].phi4_ =
(-this->hip_motor_[2]->GetAngle() +
this->param_.wheel_param.mech_zero[3]); // 前关节角度
if (leg_argu_[1].phi4_ > M_PI) {
this->leg_argu_[1].phi4_ = this->leg_argu_[1].phi4_ - 2 * M_PI;
}
this->leg_argu_[1].phi1_ =
(-this->hip_motor_[3]->GetAngle() +
this->param_.wheel_param.mech_zero[2]); // 后关节角度
if (leg_argu_[1].phi1_ > M_PI) {
this->leg_argu_[1].phi1_ = this->leg_argu_[1].phi1_ - 2 * M_PI;
}
this->pit_ = this->eulr_.pit;
if (this->pit_ > M_PI) {
this->pit_ = this->eulr_.pit - 2 * M_PI;
}
/* 注意极性 */
std::tuple<float, float, float, float> result0 =
this->leg_[0]->VMCsolve(-leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
this->pit_, -this->gyro_.x, this->dt_);
this->leg_argu_[0].L0 = std::get<0>(result0);
this->leg_argu_[0].d_L0 = std::get<1>(result0);
this->leg_argu_[0].theta = -std::get<2>(result0);
this->leg_argu_[0].d_theta = -std::get<3>(result0);
if (leg_argu_[0].theta > M_PI) {
leg_argu_[0].theta = leg_argu_[0].theta - 2 * M_PI;
}
if (leg_argu_[0].theta < -M_PI) {
leg_argu_[0].theta = leg_argu_[0].theta + 2 * M_PI;
}
std::tuple<float, float, float, float> result1 =
this->leg_[1]->VMCsolve(-leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
this->pit_, -this->gyro_.x, this->dt_);
this->leg_argu_[1].L0 = std::get<0>(result1);
this->leg_argu_[1].d_L0 = std::get<1>(result1);
this->leg_argu_[1].theta = -std::get<2>(result1);
this->leg_argu_[1].d_theta = -std::get<3>(result1);
if (leg_argu_[1].theta > M_PI) {
leg_argu_[1].theta = leg_argu_[1].theta - 2 * M_PI;
}
if (leg_argu_[1].theta < -M_PI) {
leg_argu_[1].theta = leg_argu_[1].theta + 2 * M_PI;
}
/* 轮子转速近似于编码器速度 由此推测机体速度 */
this->leg_argu_[0].single_x_dot =
-wheel_motor_[0]->GetSpeed() * 2 * M_PI *
(this->param_.wheel_param.wheel_radius) / 60.f / 15.765 +
leg_argu_[0].L0 * leg_argu_[0].d_theta * cosf(leg_argu_[0].theta) +
leg_argu_[0].d_L0 * sinf(leg_argu_[0].theta);
this->leg_argu_[1].single_x_dot =
wheel_motor_[1]->GetSpeed() * 2 * M_PI *
(this->param_.wheel_param.wheel_radius) / 60.f / 15.765 +
leg_argu_[1].L0 * leg_argu_[1].d_theta * cosf(leg_argu_[1].theta) +
leg_argu_[1].d_L0 * sinf(leg_argu_[1].theta);
this->move_argu_.last_x_dot = this->move_argu_.x_dot;
this->move_argu_.x_dot =
(this->leg_argu_[0].single_x_dot + this->leg_argu_[1].single_x_dot) / 2;
this->move_argu_.x_dot =
(-wheel_motor_[0]->GetSpeed() + wheel_motor_[1]->GetSpeed()) * M_PI *
this->param_.wheel_param.wheel_radius / 60.f / 15.765;
this->move_argu_.x = this->move_argu_.x_dot * dt_ + move_argu_.x;
this->move_argu_.delta_speed =
lowfilter_.Apply((move_argu_.x_dot - move_argu_.last_x_dot) / dt_);
this->move_argu_.accl = acclfilter_.Apply(this->accl_.y * 9.8f);
if (now_ > 1000000) {
this->move_argu_.x_dot_hat =
manfilter_.comp_filter(move_argu_.x_dot, move_argu_.delta_speed,
this->move_argu_.last_x_dot, accl_.y * 9.8f,
dt_) -
((leg_argu_[0].L0 * leg_argu_[0].d_theta * cosf(leg_argu_[0].theta) +
leg_argu_[0].d_L0 * sinf(leg_argu_[0].theta)) +
(leg_argu_[1].L0 * leg_argu_[1].d_theta * cosf(leg_argu_[1].theta) +
leg_argu_[1].d_L0 * sinf(leg_argu_[1].theta))) /
2;
this->move_argu_.xhat = dt_ * move_argu_.x_dot_hat + move_argu_.xhat;
}
}
void Wheelleg::Calculate() {
switch (this->mode_) {
case Wheelleg::RELAX:
// if (fabs(move_argu_.target_dot_x - cmd_.y * 1.5f) > 0.005) {
// if (move_argu_.target_dot_x > cmd_.y * 1.5f) {
// move_argu_.target_dot_x -= 0.004;
// }
// if (move_argu_.target_dot_x < cmd_.y * 1.5f) {
// move_argu_.target_dot_x += 0.004;
// }
// } else {
// move_argu_.target_dot_x = cmd_.y * 1.5f;
// }
// move_argu_.target_x = move_argu_.target_dot_x * dt_ +
// move_argu_.target_x;
move_argu_.target_x = 0;
move_argu_.target_dot_x = 0;
break;
case Wheelleg::STAND:
/* 0.002为最大加速度 即500hz*0.002 梯度式递增减 */
if (fabs(move_argu_.target_dot_x -
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) > 0.005) {
if (move_argu_.target_dot_x >
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
move_argu_.target_dot_x -= 0.004;
}
if (move_argu_.target_dot_x <
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
move_argu_.target_dot_x += 0.004;
}
} else {
move_argu_.target_dot_x =
cosf(yaw_) * cmd_.y * param_.wheel_param.max_speed;
}
move_argu_.target_x = move_argu_.target_dot_x * dt_ + move_argu_.target_x;
this->move_argu_.target_yaw = 0.0f;
/*双零点*/
if (this->yaw_ > M_PI_2) {
this->move_argu_.target_yaw = 3.14158f;
}
if (this->yaw_ < -M_PI_2) {
this->move_argu_.target_yaw = 3.14158f;
}
break;
case Wheelleg::ROTOR:
if (fabs(move_argu_.target_dot_x -
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) > 0.005) {
if (move_argu_.target_dot_x >
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
move_argu_.target_dot_x -= 0.004;
}
if (move_argu_.target_dot_x <
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
move_argu_.target_dot_x += 0.004;
}
} else {
move_argu_.target_dot_x =
cosf(yaw_) * cmd_.y * param_.wheel_param.max_speed;
}
move_argu_.target_x = move_argu_.target_dot_x * dt_ + move_argu_.target_x;
this->move_argu_.target_yaw = this->yaw_ + 0.01;
break;
// move_argu_.target_dot_x = cmd_.x;
// move_argu_.target_x =
// move_argu_.target_dot_x * dt_ + move_argu_.target_dot_x;
// // move_argu_.target_dot_x = sqrtf(cmd_.x * cmd_.x + cmd_.y *
// cmd_.y);
// // move_argu_.x_dot =
// // move_argu_.target_dot_x * dt_ + move_argu_.target_dot_x;
// // move_argu_.target_yaw = atan2(cmd_.x, cmd_.y);
// break;
case Wheelleg::RESET:
this->move_argu_.target_dot_x = 0;
move_argu_.target_x = 0;
this->move_argu_.target_yaw = this->eulr_.yaw;
this->move_argu_.xhat = 0;
// move_argu_.target_yaw - atan2(cmd_.x, cmd_.y);
// if ((fabs(move_argu_.target_yaw) - fabs(eulr_.yaw)) > M_PI / 2) {
// this->move_argu_.target_dot_x = -this->move_argu_.target_dot_x;
// }
break;
default:
XB_ASSERT(false);
return;
}
leg_argu_[0].Fn = leg_[0]->GndDetector(leg_argu_[0].Tp, leg_argu_[0].F0);
onground0_flag_ = false;
if (leg_argu_[0].Fn > 30) {
onground0_flag_ = true;
}
leg_argu_[1].Fn = leg_[1]->GndDetector(leg_argu_[1].Tp, leg_argu_[1].F0);
onground1_flag_ = false;
if (leg_argu_[1].Fn > 30) {
onground1_flag_ = true;
}
std::tuple<float, float> result3;
std::tuple<float, float> result4;
switch (this->mode_) {
case Wheelleg::RELAX:
case Wheelleg::ROTOR:
case Wheelleg::STAND:
for (int i = 0; i < 12; i++) {
leg_argu_[0].LQR_K[i] = this->leg_[0]->LQR_K_calc(
&this->param_.wheel_param.K_Poly_Coefficient_L[i][0],
leg_argu_[0].L0);
}
for (int i = 0; i < 12; i++) {
leg_argu_[1].LQR_K[i] = this->leg_[1]->LQR_K_calc(
&this->param_.wheel_param.K_Poly_Coefficient_R[i][0],
leg_argu_[1].L0);
}
if (now_ > 1000000)
if (fabs(move_argu_.target_dot_x) > 0.5 ||
fabs(move_argu_.target_dot_x - move_argu_.x_dot_hat) > 0.7 ||
((!onground0_flag_) & (!onground1_flag_))) {
leg_argu_[0].LQR_K[2] = 0;
leg_argu_[1].LQR_K[2] = 0;
this->move_argu_.xhat = 0;
move_argu_.target_x = 0;
}
if (onground0_flag_) {
leg_argu_[0].Tw =
(leg_argu_[0].LQR_K[0] *
(-0.8845 * leg_argu_[0].L0 + 0.40663 - leg_argu_[0].theta) +
leg_argu_[0].LQR_K[1] * (-leg_argu_[0].d_theta) +
leg_argu_[0].LQR_K[2] *
(-move_argu_.xhat + move_argu_.target_x - 0.56) +
leg_argu_[0].LQR_K[3] *
(-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
leg_argu_[0].LQR_K[4] * (-this->pit_ + 0.16) +
leg_argu_[0].LQR_K[5] * (-this->gyro_.x + 0.0f));
leg_argu_[0].Tp =
(leg_argu_[0].LQR_K[6] *
(-0.8845 * leg_argu_[0].L0 + 0.40663 - leg_argu_[0].theta) +
leg_argu_[0].LQR_K[7] * (-leg_argu_[0].d_theta) +
leg_argu_[0].LQR_K[8] *
(-move_argu_.xhat + move_argu_.target_x - 0.56) +
leg_argu_[0].LQR_K[9] *
(-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
leg_argu_[0].LQR_K[10] * (-this->pit_ + 0.16) +
leg_argu_[0].LQR_K[11] * (-this->gyro_.x + 0.0f));
} else {
leg_argu_[0].Tw = 0;
leg_argu_[0].Tp =
(leg_argu_[0].LQR_K[6] * (-leg_argu_[0].theta + 0.0f) +
leg_argu_[0].LQR_K[7] * (-leg_argu_[0].d_theta + 0.0f));
}
if (onground1_flag_) {
leg_argu_[1].Tw =
(leg_argu_[1].LQR_K[0] *
(-0.8845 * leg_argu_[1].L0 + 0.40663 - leg_argu_[1].theta) +
leg_argu_[1].LQR_K[1] * (-leg_argu_[1].d_theta + 0.0f) +
leg_argu_[1].LQR_K[2] *
(-move_argu_.xhat + move_argu_.target_x - 0.56) +
leg_argu_[1].LQR_K[3] *
(-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
leg_argu_[1].LQR_K[4] * (-this->pit_ + 0.16) +
leg_argu_[1].LQR_K[5] * (-this->gyro_.x + 0.0f));
leg_argu_[1].Tp =
(leg_argu_[1].LQR_K[6] *
(-0.8845 * leg_argu_[1].L0 + 0.40663 - leg_argu_[1].theta) +
leg_argu_[1].LQR_K[7] * (-leg_argu_[1].d_theta + 0.0f) +
leg_argu_[1].LQR_K[8] *
(-move_argu_.xhat + move_argu_.target_x - 0.56) +
leg_argu_[1].LQR_K[9] *
(-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
leg_argu_[1].LQR_K[10] * (-this->pit_ + 0.16) +
leg_argu_[1].LQR_K[11] * (-this->gyro_.x + 0.0f));
} else {
leg_argu_[1].Tw = 0;
leg_argu_[1].Tp =
(leg_argu_[1].LQR_K[6] * (-leg_argu_[1].theta + 0.0f) +
leg_argu_[1].LQR_K[7] * (-leg_argu_[1].d_theta + 0.0f));
}
this->leg_argu_[0].Delat_L0 = fabs(0.35 * cmd_.z) - fabs(gyro_.z) / 100 +
this->param_.wheel_param.static_L0[0] +
+roll_pid_.Calculate(0, eulr_.rol, dt_);
this->leg_argu_[1].Delat_L0 = fabs(0.35 * cmd_.z) - fabs(gyro_.z) / 100 +
this->param_.wheel_param.static_L0[1] -
roll_pid_.Calculate(0, eulr_.rol, dt_);
leg_argu_[0].F0 =
leg_argu_[0].Tp * sinf(leg_argu_[0].theta) +
this->param_.wheel_param.static_F0[0] +
leglength_pid_.at(0)->Calculate(this->leg_argu_[0].Delat_L0,
this->leg_argu_[0].L0, this->dt_);
leg_argu_[1].F0 =
leg_argu_[1].Tp * sinf(leg_argu_[1].theta) +
this->param_.wheel_param.static_F0[1] +
leglength_pid_.at(1)->Calculate(this->leg_argu_[1].Delat_L0,
this->leg_argu_[1].L0, this->dt_);
this->leg_argu_[0].Delta_Tp =
leg_argu_[0].L0 * 10.0f *
tp_pid_.at(0)->Calculate(this->leg_argu_[1].theta,
this->leg_argu_[0].theta, this->dt_);
this->leg_argu_[1].Delta_Tp =
-leg_argu_[1].L0 * 10.0f *
tp_pid_.at(0)->Calculate(this->leg_argu_[1].theta,
this->leg_argu_[0].theta, this->dt_);
result3 = leg_[0]->VMCinserve(
-leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
-leg_argu_[0].Tp - this->leg_argu_[0].Delta_Tp, leg_argu_[0].F0);
this->leg_argu_[0].T1 = std::get<0>(result3);
this->leg_argu_[0].T2 = std::get<1>(result3);
result4 = leg_[1]->VMCinserve(
-leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
-leg_argu_[1].Tp - this->leg_argu_[1].Delta_Tp, leg_argu_[1].F0);
this->leg_argu_[1].T1 = -std::get<0>(result4);
this->leg_argu_[1].T2 = -std::get<1>(result4);
if (onground0_flag_ & onground1_flag_) {
move_argu_.yaw_force =
-this->yaw_pid_.Calculate(move_argu_.target_yaw, this->yaw_, dt_);
} else {
move_argu_.yaw_force = 0;
}
/*3508不带减速箱是Tw*3.2f*/
/*2006带减速是Tw/1.8f*/
/* 3508带15.7减速箱是Tw/4.9256 */
this->wheel_motor_out_[0] =
this->leg_argu_[0].Tw / 4.9256f - move_argu_.yaw_force;
this->wheel_motor_out_[1] =
this->leg_argu_[1].Tw / 4.9256f + move_argu_.yaw_force;
this->hip_motor_out_[0] = this->leg_argu_[0].T1;
this->hip_motor_out_[1] = this->leg_argu_[0].T2;
this->hip_motor_out_[2] = this->leg_argu_[1].T1;
this->hip_motor_out_[3] = this->leg_argu_[1].T2;
this->hip_motor_flag_ = 1;
break;
case Wheelleg::RESET:
if (fabs(pit_) > M_PI / 2 || fabs(leg_argu_[0].theta) > 1.57 ||
fabs(leg_argu_[1].theta) > 1.57) {
leg_argu_[0].target_theta = leg_argu_[0].theta + cmd_.y / 1000;
if (fabs(pit_) > M_PI / 2) {
if ((leg_argu_[0].theta - leg_argu_[1].theta) > 0.03) {
leg_argu_[1].target_theta = leg_argu_[1].theta + 0.001;
}
if ((leg_argu_[0].theta - leg_argu_[1].theta) < -0.03) {
leg_argu_[1].target_theta = leg_argu_[1].theta - 0.001;
}
leg_argu_[0].F0 = 10 * leglength_pid_.at(0)->Calculate(
0.65f, this->leg_argu_[0].L0, this->dt_);
leg_argu_[1].F0 = 10 * leglength_pid_.at(1)->Calculate(
0.65f, this->leg_argu_[1].L0, this->dt_);
}
if (fabs(leg_argu_[0].theta) < 1.57) {
leg_argu_[1].target_theta = leg_argu_[1].theta + cmd_.y / 1000;
leg_argu_[0].target_theta = leg_argu_[0].theta;
}
if (fabs(leg_argu_[1].theta) < 1.57) {
leg_argu_[0].target_theta = leg_argu_[0].theta + cmd_.y / 1000;
leg_argu_[1].target_theta = leg_argu_[1].theta;
}
if (leg_argu_[0].target_theta > M_PI) {
leg_argu_[0].target_theta -= 2 * M_PI;
}
if (leg_argu_[0].target_theta < -M_PI) {
leg_argu_[0].target_theta += 2 * M_PI;
}
if (leg_argu_[1].target_theta > M_PI) {
leg_argu_[1].target_theta -= 2 * M_PI;
}
if (leg_argu_[1].target_theta < -M_PI) {
leg_argu_[1].target_theta += 2 * M_PI;
}
leg_argu_[0].Tp =
500 * this->theta_pid_[0]->Calculate(leg_argu_[0].target_theta,
leg_argu_[0].theta, dt_);
leg_argu_[1].Tp =
500 * this->theta_pid_[1]->Calculate(leg_argu_[1].target_theta,
leg_argu_[1].theta, dt_);
} else {
leg_argu_[0].F0 = 3 * leglength_pid_.at(0)->Calculate(
0.10f, this->leg_argu_[0].L0, this->dt_);
leg_argu_[1].F0 = 3 * leglength_pid_.at(1)->Calculate(
0.10f, this->leg_argu_[1].L0, this->dt_);
if ((this->leg_argu_[0].L0 < 0.20) && (this->leg_argu_[1].L0 < 0.20)) {
leg_argu_[0].Tp = 5.5 * this->theta_pid_[0]->Calculate(
0.1, leg_argu_[0].theta, dt_);
leg_argu_[1].Tp = 5.5 * this->theta_pid_[1]->Calculate(
0.1, leg_argu_[1].theta, dt_);
clampf(&leg_argu_[0].Tp, -10, 10);
clampf(&leg_argu_[1].Tp, -10, 10);
} else {
leg_argu_[0].Tp = 0;
leg_argu_[1].Tp = 0;
}
}
result3 = leg_[0]->VMCinserve(-leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
-leg_argu_[0].Tp, leg_argu_[0].F0);
this->leg_argu_[0].T1 = std::get<0>(result3);
this->leg_argu_[0].T2 = std::get<1>(result3);
result4 = leg_[1]->VMCinserve(-leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
-leg_argu_[1].Tp, leg_argu_[1].F0);
this->leg_argu_[1].T1 = -std::get<0>(result4);
this->leg_argu_[1].T2 = -std::get<1>(result4);
this->hip_motor_out_[0] = this->leg_argu_[0].T1;
this->hip_motor_out_[1] = this->leg_argu_[0].T2;
this->hip_motor_out_[2] = this->leg_argu_[1].T1;
this->hip_motor_out_[3] = this->leg_argu_[1].T2;
this->hip_motor_flag_ = 1;
break;
}
}
void Wheelleg::Control() {
clampf(&wheel_motor_out_[0], -0.8f, 0.8f);
clampf(&wheel_motor_out_[1], -0.8f, 0.8f);
clampf(&hip_motor_out_[0], -20.0f, 20.0f);
clampf(&hip_motor_out_[1], -20.0f, 20.0f);
clampf(&hip_motor_out_[2], -20.0f, 20.0f);
clampf(&hip_motor_out_[3], -20.0f, 20.0f);
// if (fabs(wheel_motor_[0]->GetSpeed()) > 5000 ||
// fabs(wheel_motor_[1]->GetSpeed()) > 5000) {
// wheel_motor_out_[0] = 0;
// wheel_motor_out_[1] = 0;
// if (fabs(this->pit_) > 0.5f) {
// this->hip_motor_flag_ = 0;
// }
// }
switch (this->mode_) {
case Wheelleg::RELAX:
this->wheel_motor_[0]->Relax();
this->wheel_motor_[1]->Relax();
hip_motor_flag_ = 0;
hip_motor_[0]->SetMit(0.0f);
hip_motor_[1]->SetMit(0.0f);
hip_motor_[2]->SetMit(0.0f);
hip_motor_[3]->SetMit(0.0f);
break;
case Wheelleg::STAND:
case Wheelleg::ROTOR:
// this->wheel_motor_[0]->Relax();
// this->wheel_motor_[1]->Relax();
this->wheel_motor_[0]->Control(-wheel_motor_out_[0]);
this->wheel_motor_[1]->Control(wheel_motor_out_[1]);
hip_motor_[0]->SetMit(this->hip_motor_out_[0]);
hip_motor_[1]->SetMit(this->hip_motor_out_[1]);
hip_motor_[2]->SetMit(this->hip_motor_out_[2]);
hip_motor_[3]->SetMit(this->hip_motor_out_[3]);
// hip_motor_[0]->SetMit(0.0f);
// hip_motor_[1]->SetMit(0.0f);
// hip_motor_[2]->SetMit(0.0f);
// hip_motor_[3]->SetMit(0.0f);
break;
case Wheelleg::RESET:
this->wheel_motor_[0]->Relax();
this->wheel_motor_[1]->Relax();
hip_motor_[0]->SetMit(this->hip_motor_out_[0]);
hip_motor_[1]->SetMit(this->hip_motor_out_[1]);
hip_motor_[2]->SetMit(this->hip_motor_out_[2]);
hip_motor_[3]->SetMit(this->hip_motor_out_[3]);
break;
}
}
void Wheelleg::SetMode(Wheelleg::Mode mode) {
if (mode == this->mode_) {
return;
}
this->leg_[0]->Reset();
this->leg_[1]->Reset();
move_argu_.x = 0;
move_argu_.x_dot = 0;
move_argu_.last_x_dot = 0;
move_argu_.target_x = move_argu_.xhat;
move_argu_.target_yaw = this->eulr_.yaw;
move_argu_.target_dot_x = 0;
move_argu_.xhat = 0;
move_argu_.x_dot_hat = 0;
this->manfilter_.reset_comp();
this->mode_ = mode;
}

4
User/task/rc.c
View File

@ -60,7 +60,7 @@ void Task_rc(void *argument) {
// 将遥控器通道值从[364, 1684]映射到[-1.0, 1.0]
float ch_l_y_norm = (float)(dr16.data.ch_l_y - 1024) / 660.0f; // 前后
float ch_l_x_norm = (float)(dr16.data.ch_l_x - 1024) / 660.0f; // 左右
float ch_r_x_norm = (float)(dr16.data.ch_r_x - 1024) / 660.0f; // 旋转
float ch_r_x_norm = (float)(dr16.data.ch_r_y - 1024) / 660.0f; // 旋转
// 设置运动向量（根据需要调整增益）
cmd_to_chassis.move_vec.vx = ch_l_y_norm * 2.0f; // 前后运动，增益可调
@ -68,7 +68,7 @@ void Task_rc(void *argument) {
cmd_to_chassis.move_vec.wz = ch_r_x_norm * 3.0f; // 旋转运动，增益可调
// 设置目标高度（可根据右拨杆或其他输入调整）
cmd_to_chassis.height = 0.0f;
cmd_to_chassis.height = dr16.data.res-1024; // 目标高度，范围[-1024, 1024]，可根据需要调整比例
// 发送命令到底盘控制任务
osMessageQueuePut(task_runtime.msgq.Chassis_cmd, &cmd_to_chassis, 0, 0);

133
some_functions.c
View File

@ -20,81 +20,10 @@
(ptr)->real_torque = uint_to_float((uint16_t)(((data[4] & 0x0f) << 8) | (data)[5]), -18, +18, 12); \
}
#define get_BM_motor_measure(ptr, data) \
{ \
(ptr)->last_ecd = (ptr)->ecd; \
(ptr)->ecd = (uint16_t)((data)[4] << 8 | (data)[5]); \
(ptr)->speed_rpm = (int16_t)((data)[0] << 8 | (data)[1]); \
(ptr)->given_current = (int16_t)((data)[2] << 8 | (data)[3]); \
(ptr)->error = (data)[6]; \
(ptr)->mode = (data)[7]; \
}
void CAN_BM_ENABLE_CMD(void)
{
CAN_TxHeaderTypeDef bm_tx_measure;
uint8_t bm_can_send_data[8];
uint32_t send_mail_box;
bm_tx_measure.StdId = 0x105; // 120
bm_tx_measure.IDE = CAN_ID_STD;
bm_tx_measure.RTR = CAN_RTR_DATA;
bm_tx_measure.DLC = 0x08;
bm_can_send_data[0] = 0x0A;
bm_can_send_data[1] = 0x0A;
bm_can_send_data[2] = 0x00;
bm_can_send_data[3] = 0x00;
bm_can_send_data[4] = 0x00;
bm_can_send_data[5] = 0x00;
bm_can_send_data[6] = 0x00;
bm_can_send_data[7] = 0x00;
HAL_CAN_AddTxMessage(&BM_CAN, &bm_tx_measure, bm_can_send_data, &send_mail_box);
}
void CAN_BM_MODE_SET_CMD(void)
{
CAN_TxHeaderTypeDef bm_tx_measure;
uint8_t bm_can_send_data[8];
uint32_t send_mail_box;
bm_tx_measure.StdId = 0x106; // 120
bm_tx_measure.IDE = CAN_ID_STD;
bm_tx_measure.RTR = CAN_RTR_DATA;
bm_tx_measure.DLC = 0x08;
bm_can_send_data[0] = 0x01;
bm_can_send_data[1] = 0x01;
bm_can_send_data[2] = 0x00;
bm_can_send_data[3] = 0x00;
bm_can_send_data[4] = 0x00;
bm_can_send_data[5] = 0x00;
bm_can_send_data[6] = 0x00;
bm_can_send_data[7] = 0x00;
HAL_CAN_AddTxMessage(&BM_CAN, &bm_tx_measure, bm_can_send_data, &send_mail_box);
}
void CAN_BM_CONTROL_CMD( int16_t motor1, int16_t motor2 )
{
CAN_TxHeaderTypeDef bm_tx_measure;
uint8_t bm_can_send_data[8];
uint32_t send_mail_box;
bm_tx_measure.StdId = 0x32; // 120
bm_tx_measure.IDE = CAN_ID_STD;
bm_tx_measure.RTR = CAN_RTR_DATA;
bm_tx_measure.DLC = 0x08;
bm_can_send_data[0] = motor1 >> 8;
bm_can_send_data[1] = motor1;
bm_can_send_data[2] = motor2 >> 8;
bm_can_send_data[3] = motor2;
bm_can_send_data[4] = 0x00;
bm_can_send_data[5] = 0x00;
bm_can_send_data[6] = 0x00;
bm_can_send_data[7] = 0x00;
HAL_CAN_AddTxMessage(&BM_CAN, &bm_tx_measure, bm_can_send_data, &send_mail_box);
}
#define LIMIT_MIN_MAX(x,min,max) (x) = (((x)<=(min))?(min):(((x)>=(max))?(max):(x)))
#define P_MIN -95.5f// Radians
@ -108,68 +37,6 @@ void CAN_BM_CONTROL_CMD( int16_t motor1, int16_t motor2 )
#define T_MIN -18.0f
#define T_MAX 18.0f
void CAN_HT_CMD( uint8_t id, fp32 f_t )
{
uint32_t canTxMailbox = CAN_TX_MAILBOX0;
fp32 f_p = 0.0f, f_v = 0.0f, f_kp = 0.0f, f_kd = 0.0f;
uint16_t p, v, kp, kd, t;
uint8_t buf[8];
LIMIT_MIN_MAX(f_p, P_MIN, P_MAX);
LIMIT_MIN_MAX(f_v, V_MIN, V_MAX);
LIMIT_MIN_MAX(f_kp, KP_MIN, KP_MAX);
LIMIT_MIN_MAX(f_kd, KD_MIN, KD_MAX);
LIMIT_MIN_MAX(f_t, T_MIN, T_MAX);
p = float_to_uint(f_p, P_MIN, P_MAX, 16);
v = float_to_uint(f_v, V_MIN, V_MAX, 12);
kp = float_to_uint(f_kp, KP_MIN, KP_MAX, 12);
kd = float_to_uint(f_kd, KD_MIN, KD_MAX, 12);
t = float_to_uint(f_t, T_MIN, T_MAX, 12);
buf[0] = p>>8;
buf[1] = p&0xFF;
buf[2] = v>>4;
buf[3] = ((v&0xF)<<4)|(kp>>8);
buf[4] = kp&0xFF;
buf[5] = kd>>4;
buf[6] = ((kd&0xF)<<4)|(t>>8);
buf[7] = t&0xff;
chassis_tx_message.StdId = id;
chassis_tx_message.IDE = CAN_ID_STD;
chassis_tx_message.RTR = CAN_RTR_DATA;
chassis_tx_message.DLC = 0x08;
// while (HAL_CAN_GetTxMailboxesFreeLevel(&hcan1) == 0);
if ((hcan1.Instance->TSR & CAN_TSR_TME0) != RESET)
{canTxMailbox = CAN_TX_MAILBOX0;}
else if ((hcan1.Instance->TSR & CAN_TSR_TME1) != RESET)
{canTxMailbox = CAN_TX_MAILBOX1;}
else if ((hcan1.Instance->TSR & CAN_TSR_TME2) != RESET)
{canTxMailbox = CAN_TX_MAILBOX2;}
if(HAL_CAN_AddTxMessage(&hcan1, &chassis_tx_message, buf, (uint32_t *)&canTxMailbox)==HAL_OK){};
}
void CAN_CMD_HT_Enable(uint8_t id, uint8_t unterleib_motor_send_data[8] )
{
uint32_t canTxMailbox= CAN_TX_MAILBOX0;
chassis_tx_message.StdId = id;
chassis_tx_message.IDE = CAN_ID_STD;
chassis_tx_message.RTR = CAN_RTR_DATA;
chassis_tx_message.DLC = 0x08;
// while (HAL_CAN_GetTxMailboxesFreeLevel(&hcan1) == 0);
if ((hcan1.Instance->TSR & CAN_TSR_TME0) != RESET)
{canTxMailbox = CAN_TX_MAILBOX0;}
else if ((hcan1.Instance->TSR & CAN_TSR_TME1) != RESET)
{canTxMailbox = CAN_TX_MAILBOX1;}
else if ((hcan1.Instance->TSR & CAN_TSR_TME2) != RESET)
{canTxMailbox = CAN_TX_MAILBOX2;}
if(HAL_CAN_AddTxMessage(&hcan1, &chassis_tx_message, unterleib_motor_send_data, (uint32_t *)&canTxMailbox)==HAL_OK){};
}
void Forward_kinematic_solution(chassis_control_t *feedback_update,
fp32 Q1,fp32 S1,fp32 Q4,fp32 S4, uint8_t ce)

30
utils/lqr.m
View File

@ -105,27 +105,27 @@ g_ac = 9.81;
%%%%%%%%%%%%%%%%%%%%%%%%%%%机器人机体与轮部参数%%%%%%%%%%%%%%%%%%%%%%%%%%%%
R_w_ac = 0.9; % 驱动轮半径 （单位：m）
R_l_ac = 0.25; % 两个驱动轮之间距离/2 （单位：m）
l_c_ac = 0.037; % 机体质心到腿部关节中心点距离 （单位：m）
m_w_ac = 0.8; m_l_ac = 1.6183599; m_b_ac = 11.542; % 驱动轮质量 腿部质量 机体质量 （单位：kg）
I_w_ac = (3510000)*10^(-7); % 驱动轮转动惯量 （单位：kg m^2）
I_b_ac = 0.260; % 机体转动惯量(自然坐标系法向) （单位：kg m^2）
I_z_ac = 0.226; % 机器人z轴转动惯量 （单位：kg m^2）
R_w_ac = 0.75; % 驱动轮半径 （单位：m）
R_l_ac = 0.215; % 两个驱动轮之间距离/2 （单位：m）
l_c_ac = 0.025; % 机体质心到腿部关节中心点距离 （单位：m）
m_w_ac = 0.5; m_l_ac = 2.133; m_b_ac = 14.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.18; % 左腿摆杆长度 （左腿对应数据） （单位：m）
l_wl_ac = 0.05; % 左驱动轮质心到左腿摆杆质心距离 （单位：m）
l_bl_ac = 0.13; % 机体转轴到左腿摆杆质心距离 （单位：m）
I_ll_ac = 0.02054500; % 左腿摆杆转动惯量 （单位：kg m^2）
l_r_ac = 0.18; % 右腿摆杆长度 （右腿对应数据） （单位：m）
l_wr_ac = 0.05; % 右驱动轮质心到右腿摆杆质心距离 （单位：m）
l_br_ac = 0.13; % 机体转轴到右腿摆杆质心距离 （单位：m）
I_lr_ac = 0.02054500; % 右腿摆杆转动惯量 （单位：kg m^2）
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_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）
% 机体转轴定义参考哈工程开源（https://zhuanlan.zhihu.com/p/563048952），是左右
% 两侧两个关节电机之间的中间点相连所形成的轴

86
utils/matlab/LQR_calc.m Normal file
View File

@ -0,0 +1,86 @@
clear;
clc;
syms x xd xdd T Tp thetadd thetad theta phidd phid phi P N PM NM L LM;
%% 参数设定
% 均为标准单位制
g = 9.81;
% 驱动轮
R = 0.075; %轮子半径
mw = 0.58; %轮子质量
Iw = 0.00823; %轮子转动惯量
% 大腿
l_active_leg = 0.14;
m_active_leg = 0.174;
% 小腿
l_slave_leg = 0.24;
m_slave_leg = 0.180;
% 关节间距
joint_distance = 0.11;
% 摆杆
mp = (m_active_leg + m_slave_leg)*2 + 0.728; % 需要加上定子质量
Ip = mp*L^2/3; %摆杆转动惯量
% 机体
M = 12; %机体重量
l = -0.014; %机体质心到关节电机转轴的距离
IM = 0.124;
%% 经典力学方程
fu1=N-NM==mp*(xdd+L*(thetadd*cos(theta)-thetad*thetad*sin(theta)));
fu2=P-PM-mp*g==mp*L*(-thetadd*sin(theta)-thetad*thetad*cos(theta));
fu3=NM==M*(xdd+(L+LM)*(thetadd*cos(theta)-thetad*thetad*sin(theta))-l*(phidd*cos(phi)-phid*phid*sin(phi)));
fu4=PM-M*g==M*((L+LM)*(-thetadd*sin(theta)-thetad*thetad*cos(theta))+l*(-phidd*sin(phi)-phid*phid*cos(phi)));
%% 不同部件之间的力求解
[N,NM,P,PM]=solve(fu1,fu2,fu3,fu4,N,NM,P,PM);
f1=xdd==(T-N*R)/(Iw/R+mw*R);
f2=Ip*thetadd==(P*L+PM*LM)*sin(theta)-(N*L+NM*LM)*cos(theta)-T+Tp;
f3=IM*phidd==Tp+NM*l*cos(phi)+PM*l*sin(phi);
[xdd,thetadd,phidd]=solve(f1,f2,f3,xdd,thetadd,phidd);
%% 计算雅可比矩阵A and B
func=[thetad,thetadd,xd,xdd,phid,phidd];
A_lin_model=jacobian(func,[theta,thetad,x,xd,phi,phid]);
temp_A=subs(A_lin_model,[theta,thetad,xd,phi,phid],zeros(1,5));
final_A=simplify(temp_A);
B_lin_model=jacobian(func,[T Tp]);
temp_B=subs(B_lin_model,[theta,thetad,xd,phi,phid],zeros(1,5));
final_B=simplify(temp_B);
%% 计算不同腿长下LQR增益K
L_var = 0.1; % 腿质心到机体转轴距离
Q_mat = diag([1,1,500,100,5000,1]);
R_mat = diag([1,0.25]);
K = zeros(20,12);
leg_len = zeros(20,1);
for i=1:20
L_var = L_var + 0.005;
leg_len(i) = L_var*2;
A = double(subs(final_A, [L LM], [L_var L_var]));
B = double(subs(final_B, [L LM], [L_var L_var]));
KK = lqrd(A, B, Q_mat, R_mat, 0.001);
KK_t=KK.';
K(i,:)=KK_t(:);
end
%% 不同腿长下二项式拟合K
K_cons=zeros(12,3);
for i=1:12
res=fit(leg_len,K(:,i),"poly2");
K_cons(i,:)=[res.p1, res.p2, res.p3];
end
for j=1:12
for i=1:3
fprintf("%f,",K_cons(j,i));
end
fprintf("\n");
end

103
utils/matlab/UseBodyVel.m Normal file
View File

@ -0,0 +1,103 @@
clear;
clc;
syms theta phi L x x_b N N_f T T_p M N_M P_M L_M
syms theta_dot x_dot phi_dot theta_ddot x_ddot phi_ddot
syms x_b_dot x_b_ddot
%% 参数设定
% 均为标准单位制
g = 9.81; %重力加速度
% 驱动轮
mw = 0.58; %轮子质量
R = 0.075; %轮子半径
Iw = 0.00823; %轮子转动惯量
% 大腿
l_active_leg = 0.14;
m_active_leg = 0.174;
% 小腿
l_slave_leg = 0.24;
m_slave_leg = 0.180;
% 关节间距
joint_distance = 0.11;
% 摆杆
mp = (m_active_leg + m_slave_leg)*2 + 0.728;
Ip = mp*L^2/3; % 摆杆转动惯量
% 机体
M = 12; %机体重量
IM = 0.124; %机体惯量,绕质心
l = -0.014; %机体质心到电机转轴的距离
% QR设置为相同的权重
Q_cost = diag([1,1,500,100,5000,1]);
R_cost = diag([1,0.25]);
useBodyVelocity = 1;
%% 经典力学方程
if useBodyVelocity
x_ddot = x_b_ddot - (L+L_M)*cos(theta)*theta_ddot+ (L+L_M)*sin(theta)*theta_dot^2;
end
N_M = M*(x_ddot+(L+L_M)*theta_ddot*cos(theta)-(L+L_M)*theta_dot^2*sin(theta)-l*phi_ddot*cos(phi)+l*phi_dot^2*sin(phi));
P_M = M*(g-(L+L_M)*theta_ddot*sin(theta)-(L+L_M)*theta_dot^2*cos(theta)-l*phi_ddot*sin(phi)-l*phi_dot^2*cos(phi));
N = mp*(x_ddot+L*theta_ddot*cos(theta)-L*theta_dot^2*sin(theta))+N_M;
P = mp*(g-L*theta_dot^2*cos(theta)-L*theta_ddot*sin(theta))+P_M;
eqA = x_ddot == (T-N*R)/(Iw/R+mw*R);
eqB = Ip*theta_ddot == (P*L+P_M*L_M)*sin(theta)-(N*L+N_M*L_M)*cos(theta) - T + T_p;
eqC = IM*phi_ddot == T_p + N_M*l*cos(phi) + P_M*l*sin(phi);
%% 计算雅可比矩阵
U = [T T_p].';
if useBodyVelocity
model_sol = solve([eqA eqB eqC],[theta_ddot,x_b_ddot,phi_ddot]);
X = [theta,theta_dot,x_b,x_b_dot,phi,phi_dot].';
dX = [theta_dot,simplify(model_sol.theta_ddot),...
x_b_dot,simplify(model_sol.x_b_ddot),...
phi_dot,simplify(model_sol.phi_ddot)].';
A_sym = subs(jacobian(dX,X),[theta theta_dot x_b_dot phi phi_dot],zeros(1,5));
B_sym = subs(jacobian(dX,U),[theta theta_dot x_b_dot phi phi_dot],zeros(1,5));
else
model_sol = solve([eqA eqB eqC],[theta_ddot,x_ddot,phi_ddot]);
X = [theta,theta_dot,x,x_dot,phi,phi_dot].';
dX = [theta_dot,simplify(model_sol.theta_ddot),...
x_dot,simplify(model_sol.x_ddot),...
phi_dot,simplify(model_sol.phi_ddot)].';
A_sym = subs(jacobian(dX,X),[theta theta_dot x_dot phi phi_dot],zeros(1,5));
B_sym = subs(jacobian(dX,U),[theta theta_dot x_dot phi phi_dot],zeros(1,5));
end
%% 计算变长度LQR
L_var = 0.1; % 腿质心到机体转轴距离
K=zeros(20,12);
leglen=zeros(20,1);
for i=1:20
L_var=L_var+0.005; % 10mm线性化一次
leglen(i)=L_var*2;
trans_A=double(subs(A_sym,[L L_M],[L_var L_var]));
trans_B=double(subs(B_sym,[L L_M],[L_var L_var]));
KK=lqrd(trans_A,trans_B,Q_cost,R_cost,0.001);
KK_t=KK.';
K(i,:)=KK_t(:);
end
%% 用二项式拟合K,一共12个参数
K_cons=zeros(12,3);
for i=1:12
res=fit(leglen,K(:,i),"poly2");
K_cons(i,:)=[res.p1,res.p2,res.p3];
end
for j=1:12
for i=1:3
fprintf("%f,",K_cons(j,i));
end
fprintf("\n");
end

39
utils/matlab/vmc.m Normal file
View File

@ -0,0 +1,39 @@
clear;
clc;
syms phi1(t) phi2(t) phi3(t) phi4(t) l5 phi0 L0 T_Leg F_Leg
syms phi_dot_1 phi_dot_4
syms l1 l2 l3 l4
%% 腿长解算
x_B = l1*cos(phi1);
y_B = l1*sin(phi1);
x_C = x_B+l2*cos(phi2);
y_C = y_B+l2*sin(phi2);
x_D = l5+l4*cos(phi4);
y_D = l4*sin(phi4);
x_dot_B = diff(x_B,t);
y_dot_B = diff(y_B,t);
x_dot_C = diff(x_C,t);
y_dot_C = diff(y_C,t);
x_dot_D = diff(x_D,t);
y_dot_D = diff(y_D,t);
%% 速度导数求解
phi_dot_2 = ((x_dot_D-x_dot_B)*cos(phi3)+(y_dot_D-y_dot_B)*sin(phi3))/l2/sin(phi3-phi2);
x_dot_C = subs(x_dot_C,diff(phi2,t),phi_dot_2);
x_dot_C = subs(x_dot_C,[diff(phi1,t),diff(phi4,t)],[phi_dot_1,phi_dot_4]);
y_dot_C = subs(y_dot_C,diff(phi2,t),phi_dot_2);
y_dot_C = subs(y_dot_C,[diff(phi1,t),diff(phi4,t)],[phi_dot_1,phi_dot_4]);
%% 运动映射(虚功原理)+极坐标转换
x_dot = [x_dot_C; y_dot_C];
q_dot = [phi_dot_1; phi_dot_4];
x_dot = collect(simplify(collect(x_dot,q_dot)),q_dot);
J = simplify(jacobian(x_dot,q_dot));
rotate = [cos(phi0-pi/2) -sin(phi0-pi/2);
sin(phi0-pi/2) cos(phi0-pi/2)];
map = [0 -1/L0;
1 0];
Trans_Jacobian = simplify(J.'*rotate*map);

BIN
utils/matlab/wheel_leg.slx Normal file
View File

Binary file not shown.

BIN
utils/matlab/wheel_leg.slxc Normal file
View File

Binary file not shown.

219
utils/vmc.m Normal file
View File

@ -0,0 +1,219 @@
function [tau_hip, tau_knee] = vmc_control(L1, L2, q1, q2, F_virtual, theta_virtual)
% VMC (Virtual Model Control) for serial leg mechanism
% 基于虚拟模型控制的串联腿机构力矩计算
%
% 输入参数:
% L1: 大腿长度 (m)
% L2: 小腿长度 (m)
% q1: 髋关节角度 (rad)
% q2: 膝关节角度 (rad)
% F_virtual: 虚拟力大小 (N)
% theta_virtual: 虚拟力方向角度 (rad)
%
% 输出:
% tau_hip: 髋关节所需力矩 (N*m)
% tau_knee: 膝关节所需力矩 (N*m)
% 计算雅可比矩阵
J = compute_jacobian(L1, L2, q1, q2);
% 虚拟力向量 (笛卡尔空间)
F_cartesian = [F_virtual * cos(theta_virtual);
F_virtual * sin(theta_virtual)];
% 通过雅可比转置将笛卡尔力转换为关节力矩
tau = J' * F_cartesian;
tau_hip = tau(1);
tau_knee = tau(2);
end
function J = compute_jacobian(L1, L2, q1, q2)
% 计算串联腿机构的雅可比矩阵
% 从关节空间到足端笛卡尔空间的映射
%
% 输入参数:
% L1: 大腿长度 (m)
% L2: 小腿长度 (m)
% q1: 髋关节角度 (rad)
% q2: 膝关节角度 (rad)
%
% 输出:
% J: 2x2雅可比矩阵
% 足端位置 (相对于髋关节)
% x = L1*cos(q1) + L2*cos(q1+q2)
% y = L1*sin(q1) + L2*sin(q1+q2)
% 雅可比矩阵元素
J11 = -L1*sin(q1) - L2*sin(q1+q2); % dx/dq1
J12 = -L2*sin(q1+q2); % dx/dq2
J21 = L1*cos(q1) + L2*cos(q1+q2); % dy/dq1
J22 = L2*cos(q1+q2); % dy/dq2
J = [J11, J12;
J21, J22];
end
function [x, y] = forward_kinematics(L1, L2, q1, q2)
% 串联腿正运动学 - 足端位置
%
% 输入参数:
% L1: 大腿长度 (m)
% L2: 小腿长度 (m)
% q1: 髋关节角度 (rad)
% q2: 膝关节角度 (rad)
%
% 输出:
% x: 足端x坐标 (m)
% y: 足端y坐标 (m)
x = L1*cos(q1) + L2*cos(q1+q2);
y = L1*sin(q1) + L2*sin(q1+q2);
end
function [q1, q2] = inverse_kinematics(L1, L2, x, y)
% 串联腿逆运动学 - 关节角度
%
% 输入参数:
% L1: 大腿长度 (m)
% L2: 小腿长度 (m)
% x: 足端目标x坐标 (m)
% y: 足端目标y坐标 (m)
%
% 输出:
% q1: 髋关节角度 (rad)
% q2: 膝关节角度 (rad)
% 计算到足端的距离
r = sqrt(x^2 + y^2);
% 检查工作空间
if r > (L1 + L2) || r < abs(L1 - L2)
error('目标位置超出工作空间范围');
end
% 使用余弦定理计算膝关节角度
cos_q2 = (r^2 - L1^2 - L2^2) / (2*L1*L2);
q2 = acos(cos_q2); % 肘部向上的解
% 计算髋关节角度
alpha = atan2(y, x);
beta = atan2(L2*sin(q2), L1 + L2*cos(q2));
q1 = alpha - beta;
end
function [angle_equiv, length_equiv] = serial_to_virtual_leg(L1, L2, q1, q2)
% 将串联腿转换为等效摆动杆表示 (对应C代码中的正运动学)
%
% 输入参数:
% L1: 大腿长度 (m)
% L2: 小腿长度 (m)
% q1: 髋关节角度 (rad)
% q2: 膝关节角度 (rad)
%
% 输出:
% angle_equiv: 等效摆动杆角度 (rad)
% length_equiv: 等效摆动杆长度 (m)
% 对应C代码中的计算
q4 = (pi - q1 - q2) / 2;
% 使用余弦定理求解等效摆动杆长度
a = 1.0;
b = -2.0 * L1 * cos(q4);
c = L1^2 - L2^2;
discriminant = b^2 - 4*a*c;
if discriminant < 0
error('无实数解，配置不可达');
end
sqrt_discriminant = sqrt(discriminant);
L3_1 = (-b + sqrt_discriminant) / (2*a);
L3_2 = (-b - sqrt_discriminant) / (2*a);
% 选择正的解
if L3_1 > 0 && L3_2 > 0
L3 = min(L3_1, L3_2); % 选择较小的解
elseif L3_1 > 0
L3 = L3_1;
elseif L3_2 > 0
L3 = L3_2;
else
error('无正解');
end
angle_equiv = q1 + q4;
length_equiv = L3;
end
function [q1, q2] = virtual_to_serial_leg(L1, L2, angle_equiv, length_equiv)
% 将等效摆动杆转换为串联腿表示 (对应C代码中的逆运动学)
%
% 输入参数:
% L1: 大腿长度 (m)
% L2: 小腿长度 (m)
% angle_equiv: 等效摆动杆角度 (rad)
% length_equiv: 等效摆动杆长度 (m)
%
% 输出:
% q1: 髋关节角度 (rad)
% q2: 膝关节角度 (rad)
h = length_equiv;
q = angle_equiv;
% 计算cos(q4)
cos_q4 = (L1^2 + h^2 - L2^2) / (2.0 * L1 * h);
% 检查范围
if cos_q4 < -1.0 || cos_q4 > 1.0
error('不可达配置');
end
q4 = acos(cos_q4);
% 计算关节角度
q1 = q - q4;
q2 = pi - 2.0 * q4 - q1;
end
% 示例使用函数
function demo_vmc_control()
% VMC控制示例
% 腿部参数
L1 = 0.15; % 大腿长度 15cm
L2 = 0.15; % 小腿长度 15cm
% 当前关节角度
q1 = pi/6; % 30度
q2 = pi/3; % 60度
% 期望的虚拟力
F_virtual = 50; % 50N
theta_virtual = -pi/2; % 向下的力
% 计算所需关节力矩
[tau_hip, tau_knee] = vmc_control(L1, L2, q1, q2, F_virtual, theta_virtual);
fprintf('髋关节所需力矩: %.3f N*m\n', tau_hip);
fprintf('膝关节所需力矩: %.3f N*m\n', tau_knee);
% 显示雅可比矩阵
J = compute_jacobian(L1, L2, q1, q2);
fprintf('雅可比矩阵:\n');
disp(J);
% 测试等效摆动杆转换
[angle_eq, length_eq] = serial_to_virtual_leg(L1, L2, q1, q2);
fprintf('等效摆动杆角度: %.3f rad (%.1f deg)\n', angle_eq, rad2deg(angle_eq));
fprintf('等效摆动杆长度: %.3f m\n', length_eq);
% 验证逆变换
[q1_back, q2_back] = virtual_to_serial_leg(L1, L2, angle_eq, length_eq);
fprintf('验证 - 原始角度: q1=%.3f, q2=%.3f\n', q1, q2);
fprintf('验证 - 恢复角度: q1=%.3f, q2=%.3f\n', q1_back, q2_back);
end