136 lines
4.0 KiB
C
136 lines
4.0 KiB
C
/**
|
|
* @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;
|
|
}
|