From b4fe3ca2c3f80ea6b43da3589d33d60f1570b345 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Tue, 7 Oct 2025 13:56:53 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A5=BD=E6=BB=A4=E6=B3=A2?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/component/speed_planner.c | 135 +++++++++++++++++++++++++++++++++ User/component/speed_planner.h | 76 +++++++++++++++++++ User/module/balance_chassis.c | 67 ++++++++++------ User/module/balance_chassis.h | 4 +- User/module/config.c | 10 +-- User/module/gimbal.c | 2 +- User/task/rc.c | 45 +++++------ 7 files changed, 282 insertions(+), 57 deletions(-) create mode 100644 User/component/speed_planner.c create mode 100644 User/component/speed_planner.h diff --git a/User/component/speed_planner.c b/User/component/speed_planner.c new file mode 100644 index 0000000..c9e7a72 --- /dev/null +++ b/User/component/speed_planner.c @@ -0,0 +1,135 @@ +/** + * @file speed_planner.c + * @brief 斜坡速度规划器实现 + */ + +#include "component/speed_planner.h" +#include +#include + +/** + * @brief 限制函数 + */ +static inline float clamp(float value, float min, float max) { + if (value < min) return min; + if (value > max) return max; + return value; +} + +/** + * @brief 初始化速度规划器 + */ +int8_t SpeedPlanner_Init(SpeedPlanner_t *planner, const SpeedPlanner_Params_t *params) { + if (planner == NULL || params == NULL) { + return -1; + } + + memset(planner, 0, sizeof(SpeedPlanner_t)); + planner->param = *params; + + return 0; +} + +/** + * @brief 重置速度规划器 + */ +void SpeedPlanner_Reset(SpeedPlanner_t *planner, float current_position, float current_velocity) { + if (planner == NULL) { + return; + } + + planner->current_position = current_position; + planner->current_velocity = current_velocity; + planner->target_position = current_position; + planner->planned_position = current_position; + planner->planned_velocity = current_velocity; + planner->target_velocity = current_velocity; +} + +/** + * @brief 更新速度规划器 + */ +float SpeedPlanner_Update(SpeedPlanner_t *planner, + float target_velocity, + float current_position, + float current_velocity, + float dt) { + if (planner == NULL || dt <= 0.0f) { + return 0.0f; + } + + /* 更新当前状态 */ + planner->current_position = current_position; + planner->current_velocity = current_velocity; + planner->target_velocity = target_velocity; + + /* 限制目标速度到最大速度范围 */ + float limited_target_velocity = clamp(target_velocity, + -planner->param.max_velocity, + planner->param.max_velocity); + + /* 计算速度变化 */ + float velocity_error = limited_target_velocity - planner->planned_velocity; + + /* 使用最大加速度限制速度变化率 */ + float max_velocity_change = planner->param.max_acceleration * dt; + float velocity_change = clamp(velocity_error, -max_velocity_change, max_velocity_change); + + /* 更新规划速度 */ + planner->planned_velocity += velocity_change; + + /* 限制规划速度 */ + planner->planned_velocity = clamp(planner->planned_velocity, + -planner->param.max_velocity, + planner->param.max_velocity); + + /* 更新规划位置 */ + planner->planned_position += planner->planned_velocity * dt; + + /* 防止位移起飞：限制规划位置与当前位置的误差 */ + float position_error = planner->planned_position - current_position; + + if (fabsf(position_error) > planner->param.max_position_error) { + /* 位置误差过大，重新规划 */ + if (position_error > 0.0f) { + planner->planned_position = current_position + planner->param.max_position_error; + } else { + planner->planned_position = current_position - planner->param.max_position_error; + } + + /* 根据位置误差调整速度，使位置逐渐收敛 */ + /* 使用简单的比例控制，系数可以调整 */ + float position_correction_gain = 2.0f; // 位置收敛增益 + planner->planned_velocity = (planner->planned_position - current_position) * position_correction_gain; + + /* 再次限制速度 */ + planner->planned_velocity = clamp(planner->planned_velocity, + -planner->param.max_velocity, + planner->param.max_velocity); + } + + /* 更新目标位置（用于外部参考） */ + planner->target_position = planner->planned_position; + + return planner->planned_velocity; +} + +/** + * @brief 获取规划后的目标位置 + */ +float SpeedPlanner_GetPlannedPosition(const SpeedPlanner_t *planner) { + if (planner == NULL) { + return 0.0f; + } + return planner->planned_position; +} + +/** + * @brief 获取规划后的速度 + */ +float SpeedPlanner_GetPlannedVelocity(const SpeedPlanner_t *planner) { + if (planner == NULL) { + return 0.0f; + } + return planner->planned_velocity; +} diff --git a/User/component/speed_planner.h b/User/component/speed_planner.h new file mode 100644 index 0000000..53f57ff --- /dev/null +++ b/User/component/speed_planner.h @@ -0,0 +1,76 @@ +/** + * @file speed_planner.h + * @brief 斜坡速度规划器 + * @details 提供最大加速度和最大速度限制,防止目标位移起飞 + */ + +#ifndef SPEED_PLANNER_H +#define SPEED_PLANNER_H + +#include + +/* 速度规划器参数结构体 */ +typedef struct { + float max_velocity; /* 最大速度 (m/s) */ + float max_acceleration; /* 最大加速度 (m/s²) */ + float max_position_error; /* 最大位置误差 (m), 防止位移起飞 */ +} SpeedPlanner_Params_t; + +/* 速度规划器状态结构体 */ +typedef struct { + float current_velocity; /* 当前速度 (m/s) */ + float target_velocity; /* 目标速度 (m/s) */ + float planned_velocity; /* 规划后的速度 (m/s) */ + float current_position; /* 当前位置 (m) */ + float target_position; /* 目标位置 (m) */ + float planned_position; /* 规划后的位置 (m) */ + + SpeedPlanner_Params_t param; /* 参数 */ +} SpeedPlanner_t; + +/** + * @brief 初始化速度规划器 + * @param planner 规划器结构体指针 + * @param params 参数结构体指针 + * @return 0:成功, -1:失败 + */ +int8_t SpeedPlanner_Init(SpeedPlanner_t *planner, const SpeedPlanner_Params_t *params); + +/** + * @brief 重置速度规划器 + * @param planner 规划器结构体指针 + * @param current_position 当前位置 + * @param current_velocity 当前速度 + */ +void SpeedPlanner_Reset(SpeedPlanner_t *planner, float current_position, float current_velocity); + +/** + * @brief 更新速度规划器 + * @param planner 规划器结构体指针 + * @param target_velocity 目标速度 (m/s) + * @param current_position 当前位置 (m) + * @param current_velocity 当前速度 (m/s) + * @param dt 时间间隔 (s) + * @return 规划后的速度 (m/s) + */ +float SpeedPlanner_Update(SpeedPlanner_t *planner, + float target_velocity, + float current_position, + float current_velocity, + float dt); + +/** + * @brief 获取规划后的目标位置 + * @param planner 规划器结构体指针 + * @return 规划后的目标位置 (m) + */ +float SpeedPlanner_GetPlannedPosition(const SpeedPlanner_t *planner); + +/** + * @brief 获取规划后的速度 + * @param planner 规划器结构体指针 + * @return 规划后的速度 (m/s) + */ +float SpeedPlanner_GetPlannedVelocity(const SpeedPlanner_t *planner); + +#endif // SPEED_PLANNER_H diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index cd1d833..4da5ecb 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -1,6 +1,7 @@ #include "module/balance_chassis.h" #include "bsp/can.h" #include "bsp/time.h" +#include "component/filter.h" #include "component/kinematics.h" #include "component/limiter.h" #include "component/user_math.h" @@ -60,6 +61,15 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) { LQR_Reset(&c->lqr[0]); LQR_Reset(&c->lqr[1]); + + LowPassFilter2p_Reset(&c->filter.joint_out[0], 0.0f); + LowPassFilter2p_Reset(&c->filter.joint_out[1], 0.0f); + LowPassFilter2p_Reset(&c->filter.joint_out[2], 0.0f); + LowPassFilter2p_Reset(&c->filter.joint_out[3], 0.0f); + LowPassFilter2p_Reset(&c->filter.wheel_out[0], 0.0f); + LowPassFilter2p_Reset(&c->filter.wheel_out[1], 0.0f); + + c->mode = mode; c->state = 0; // 重置状态，确保每次切换模式时都重新初始化 @@ -129,6 +139,19 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) { LQR_Init(&c->lqr[0], ¶m->lqr_gains); LQR_Init(&c->lqr[1], ¶m->lqr_gains); + LowPassFilter2p_Init(&c->filter.joint_out[0], target_freq, + param->low_pass_cutoff_freq.out); + LowPassFilter2p_Init(&c->filter.joint_out[1], target_freq, + param->low_pass_cutoff_freq.out); + LowPassFilter2p_Init(&c->filter.joint_out[2], target_freq, + param->low_pass_cutoff_freq.out); + LowPassFilter2p_Init(&c->filter.joint_out[3], target_freq, + param->low_pass_cutoff_freq.out); + LowPassFilter2p_Init(&c->filter.wheel_out[0], target_freq, + param->low_pass_cutoff_freq.out); + LowPassFilter2p_Init(&c->filter.wheel_out[1], target_freq, + param->low_pass_cutoff_freq.out); + /*初始化机体状态*/ c->chassis_state.position_x = 0.0f; c->chassis_state.velocity_x = 0.0f; @@ -219,7 +242,7 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) { VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]); VMC_InverseSolve(&c->vmc_[1], fn, tp); VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]); - // Chassis_MotorEnable(c); + c->output.wheel[0] = c_cmd->move_vec.vx * 0.2f; c->output.wheel[1] = c_cmd->move_vec.vx * 0.2f; @@ -244,27 +267,23 @@ 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]; - // // MOTOR_LZ_MotionControl(&c->param->joint_motors[i], ¶m); - // } - // BSP_TIME_Delay_us(400); // 等待CAN总线空闲，确保前面的命令发送完成 - // for (int i = 0; i < 2; i++) { - // MOTOR_LK_SetOutput(&c->param->wheel_motors[i], c->output.wheel[i]); - // // MOTOR_LK_SetOutput(&c->param->wheel_motors[i], 0.0f); - // } + c->output.joint[0] = + LowPassFilter2p_Apply(&c->filter.joint_out[0], c->output.joint[0]); + c->output.joint[1] = + LowPassFilter2p_Apply(&c->filter.joint_out[1], c->output.joint[1]); + c->output.joint[2] = + LowPassFilter2p_Apply(&c->filter.joint_out[2], c->output.joint[2]); + c->output.joint[3] = + LowPassFilter2p_Apply(&c->filter.joint_out[3], c->output.joint[3]); float out[4] = {c->output.joint[0], c->output.joint[1], c->output.joint[2], c->output.joint[3]}; - // out[0] = 0.0f; - // out[1] = 0.0f; - // out[2] = 0.0f; - // out[3] = 0.0f; Virtual_Chassis_SendJointTorque(&virtual_chassis, out); + c->output.wheel[0] = + LowPassFilter2p_Apply(&c->filter.wheel_out[0], c->output.wheel[0]); + c->output.wheel[1] = + LowPassFilter2p_Apply(&c->filter.wheel_out[1], c->output.wheel[1]); Virtual_Chassis_SendWheelCommand(&virtual_chassis, c->output.wheel[0], c->output.wheel[1]); - // Virtual_Chassis_SendWheelCommand(&virtual_chassis, 0.0f, 0.0f); - // for (int i = 0; i < 2; i++) { } int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { @@ -333,8 +352,6 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { c->lqr[0].control_output.T / 4.5f + c->yaw_control.yaw_force; c->output.wheel[1] = c->lqr[1].control_output.T / 4.5f - c->yaw_control.yaw_force; - // c->output.wheel[0] = c->lqr[0].control_output.T / 4.5f; - // c->output.wheel[1] = c->lqr[1].control_output.T / 4.5f; /* 腿长控制和VMC逆解算（使用PID控制） */ float virtual_force[2]; float target_L0[2]; @@ -353,8 +370,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { roll_compensation_force = -20.0f; // 目标腿长设定（移除横滚角补偿） - target_L0[0] = 0.12f + c_cmd->height * 0.2f; // 左腿：基础腿长 + 高度调节 - target_L0[1] = 0.12f + c_cmd->height * 0.2f; // 右腿：基础腿长 + 高度调节 + target_L0[0] = 0.15f + c_cmd->height * 0.2f; // 左腿：基础腿长 + 高度调节 + target_L0[1] = 0.15f + c_cmd->height * 0.2f; // 右腿：基础腿长 + 高度调节 // 获取腿长变化率 VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &leg_d_length[0], NULL); @@ -380,7 +397,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { // 横滚角补偿力（左腿减少支撑力） virtual_force[0] = (c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) + - pid_output + 40.0f; + pid_output + 50.0f + roll_compensation_force; // VMC逆解算（包含摆角补偿） if (VMC_InverseSolve(&c->vmc_[0], virtual_force[0], @@ -404,7 +421,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { // 横滚角补偿力（右腿增加支撑力） virtual_force[1] = (c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) + - pid_output + 40.0f; + pid_output + 55.0f - roll_compensation_force; // VMC逆解算（包含摆角补偿） if (VMC_InverseSolve(&c->vmc_[1], virtual_force[1], @@ -420,8 +437,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { /* 安全限制 */ for (int i = 0; i < 2; i++) { - if (fabsf(c->output.wheel[i]) > 0.8f) { - c->output.wheel[i] = (c->output.wheel[i] > 0) ? 0.8f : -0.8f; + if (fabsf(c->output.wheel[i]) > 1.0f) { + c->output.wheel[i] = (c->output.wheel[i] > 0) ? 1.0f : -1.0f; } } diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h index 63c67d0..1160288 100644 --- a/User/module/balance_chassis.h +++ b/User/module/balance_chassis.h @@ -150,8 +150,8 @@ typedef struct { /* 滤波器 */ struct { - LowPassFilter2p_t *in; /* 反馈值滤波器 */ - LowPassFilter2p_t *out; /* 输出值滤波器 */ + LowPassFilter2p_t joint_out[4]; /* 关节输出滤波器 */ + LowPassFilter2p_t wheel_out[2]; /* 轮子输出滤波器 */ } filter; } Chassis_t; diff --git a/User/module/config.c b/User/module/config.c index aa0d6ad..c88a709 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -168,9 +168,9 @@ Config_RobotParam_t robot_config = { .chassis_param = { .yaw={ - .k=1.0f, + .k=0.8f, .p=1.2f, - .i=0.0f, + .i=0.01f, .d=0.05f, .i_limit=0.0f, .out_limit=1.0f, @@ -179,7 +179,7 @@ Config_RobotParam_t robot_config = { }, .roll={ - .k=10.0f, + .k=20.0f, .p=5.0f, /* 横滚角比例系数 */ .i=0.0f, /* 横滚角积分系数 */ .d=0.2f, /* 横滚角微分系数 */ @@ -190,8 +190,8 @@ Config_RobotParam_t robot_config = { }, .leg_length={ - .k = 20.0f, - .p = 5.0f, + .k = 50.0f, + .p = 10.0f, .i = 0.0f, .d = 1.0f, .i_limit = 0.0f, diff --git a/User/module/gimbal.c b/User/module/gimbal.c index bce9c48..48f8f63 100644 --- a/User/module/gimbal.c +++ b/User/module/gimbal.c @@ -240,7 +240,7 @@ void Gimbal_Output(Gimbal_t *g){ MOTOR_RM_SetOutput(&g->param->pit_motor, g->out.pit); MOTOR_MIT_Output_t output = {0}; output.torque = g->out.yaw * 5.0f; // 乘以减速比 - output.kd = 0.5f; + output.kd = 0.3f; MOTOR_RM_Ctrl(&g->param->pit_motor); MOTOR_DM_MITCtrl(&g->param->yaw_motor, &output); } diff --git a/User/task/rc.c b/User/task/rc.c index 6989659..76d28fc 100644 --- a/User/task/rc.c +++ b/User/task/rc.c @@ -1,14 +1,14 @@ /* rc Task - + */ /* Includes ----------------------------------------------------------------- */ #include "task/user_task.h" /* USER INCLUDE BEGIN */ #include "device/dr16.h" -#include "module/config.h" #include "module/balance_chassis.h" +#include "module/config.h" #include "module/gimbal.h" #include "module/shoot.h" #include @@ -34,7 +34,6 @@ Gimbal_CMD_t cmd_for_gimbal; void Task_rc(void *argument) { (void)argument; /* 未使用argument，消除警告 */ - /* 计算任务运行到指定频率需要等待的tick数 */ const uint32_t delay_tick = osKernelGetTickFreq() / RC_FREQ; @@ -44,7 +43,7 @@ void Task_rc(void *argument) { /* USER CODE INIT BEGIN */ DR16_Init(&dr16); /* USER CODE INIT END */ - + while (1) { tick += delay_tick; /* 计算下一个唤醒时刻 */ /* USER CODE BEGIN */ @@ -56,31 +55,31 @@ void Task_rc(void *argument) { DR16_Offline(&dr16); } - switch (dr16.data.sw_l) { case DR16_SW_UP: - cmd_for_chassis.mode = CHASSIS_MODE_RELAX; - break; + cmd_for_chassis.mode = CHASSIS_MODE_RELAX; + break; case DR16_SW_MID: - cmd_for_chassis.mode = CHASSIS_MODE_RECOVER; - break; + cmd_for_chassis.mode = CHASSIS_MODE_RECOVER; + break; case DR16_SW_DOWN: - cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE; - break; + cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE; + break; default: - cmd_for_chassis.mode = CHASSIS_MODE_RELAX; - break; + cmd_for_chassis.mode = CHASSIS_MODE_RELAX; + break; } cmd_for_chassis.move_vec.vx = dr16.data.ch_l_y; cmd_for_chassis.move_vec.vy = dr16.data.ch_l_x; cmd_for_chassis.move_vec.wz = dr16.data.ch_r_x; - cmd_for_chassis.height = dr16.data.res; + cmd_for_chassis.height = dr16.data.ch_res; - osMessageQueueReset(task_runtime.msgq.Chassis_cmd); // 重置消息队列，防止阻塞 - osMessageQueuePut(task_runtime.msgq.Chassis_cmd, &cmd_for_chassis, 0, 0); // 非阻塞发送底盘控制命令 - + osMessageQueueReset( + task_runtime.msgq.Chassis_cmd); // 重置消息队列，防止阻塞 + osMessageQueuePut(task_runtime.msgq.Chassis_cmd, &cmd_for_chassis, 0, + 0); // 非阻塞发送底盘控制命令 - switch (dr16.data.sw_l) { + switch (dr16.data.sw_l) { case DR16_SW_UP: cmd_for_gimbal.mode = GIMBAL_MODE_RELAX; cmd_for_gimbal.delta_yaw = 0.0f; @@ -88,13 +87,13 @@ void Task_rc(void *argument) { break; case DR16_SW_MID: cmd_for_gimbal.mode = GIMBAL_MODE_ABSOLUTE; - cmd_for_gimbal.delta_yaw = -dr16.data.ch_r_x*5.0f; - cmd_for_gimbal.delta_pit = dr16.data.ch_r_y*5.0f; + cmd_for_gimbal.delta_yaw = -dr16.data.ch_r_x * 5.0f; + cmd_for_gimbal.delta_pit = dr16.data.ch_r_y * 5.0f; break; case DR16_SW_DOWN: cmd_for_gimbal.mode = GIMBAL_MODE_ABSOLUTE; - cmd_for_gimbal.delta_yaw = -dr16.data.ch_r_x* 5.0f; - cmd_for_gimbal.delta_pit = dr16.data.ch_r_y*5.0f; + cmd_for_gimbal.delta_yaw = -dr16.data.ch_r_x * 5.0f; + cmd_for_gimbal.delta_pit = dr16.data.ch_r_y * 5.0f; break; default: cmd_for_gimbal.mode = GIMBAL_MODE_RELAX; @@ -106,7 +105,6 @@ void Task_rc(void *argument) { osMessageQueueReset(task_runtime.msgq.gimbal.cmd); osMessageQueuePut(task_runtime.msgq.gimbal.cmd, &cmd_for_gimbal, 0, 0); - for_shoot.online = dr16.header.online; switch (dr16.data.sw_r) { case DR16_SW_UP: @@ -132,5 +130,4 @@ void Task_rc(void *argument) { /* USER CODE END */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ } - } \ No newline at end of file