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-10-07 13:56:53 +08:00
parent 51fe29f605
commit b4fe3ca2c3
7 changed files with 282 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

135
User/component/speed_planner.c Normal file
View File

@ -0,0 +1,135 @@
/**
* @file speed_planner.c
* @brief 斜坡速度规划器实现
*/
#include "component/speed_planner.h"
#include <math.h>
#include <string.h>
/**
* @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;
}

76
User/component/speed_planner.h Normal file
View File

@ -0,0 +1,76 @@
/**
* @file speed_planner.h
* @brief 斜坡速度规划器
* @details 提供最大加速度和最大速度限制,防止目标位移起飞
*/
#ifndef SPEED_PLANNER_H
#define SPEED_PLANNER_H
#include <stdint.h>
/* 速度规划器参数结构体 */
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

67
User/module/balance_chassis.c
View File

@ -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], &param->lqr_gains);
LQR_Init(&c->lqr[1], &param->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], &param);
// }
// 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;
}
}

4
User/module/balance_chassis.h
View File

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

10
User/module/config.c
View File

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

2
User/module/gimbal.c
View File

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

45
User/task/rc.c
View File

@ -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 <stdbool.h>
@ -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); /* 运行结束，等待下一次唤醒 */
}
}