267 lines
7.8 KiB
C
267 lines
7.8 KiB
C
#include "component/speed_planner.h"
|
||
#include <math.h>
|
||
#include <string.h>
|
||
|
||
/* 浮点数比较容差 */
|
||
#define FLOAT_EPSILON 1e-6f
|
||
|
||
/* 限制函数 */
|
||
static float SpeedPlanner_Limit(float value, float min, float max) {
|
||
if (value > max) return max;
|
||
if (value < min) return min;
|
||
return value;
|
||
}
|
||
|
||
/* 符号函数 */
|
||
static float SpeedPlanner_Sign(float value) {
|
||
if (value > FLOAT_EPSILON) return 1.0f;
|
||
if (value < -FLOAT_EPSILON) return -1.0f;
|
||
return 0.0f;
|
||
}
|
||
|
||
/* 获取速度符号，用于判断加速/减速方向 */
|
||
__attribute__((unused)) static float SpeedPlanner_GetVelocitySign(float velocity) {
|
||
return SpeedPlanner_Sign(velocity);
|
||
}
|
||
|
||
int8_t SpeedPlanner_Init(SpeedPlanner_t *planner, SpeedPlanner_Param_t *param, float control_freq) {
|
||
if (planner == NULL || param == NULL || control_freq <= 0.0f) {
|
||
return -1;
|
||
}
|
||
|
||
/* 参数检查 */
|
||
if (param->max_velocity <= 0.0f || param->max_acceleration <= 0.0f ||
|
||
param->max_deceleration <= 0.0f || param->position_error_limit <= 0.0f) {
|
||
return -1;
|
||
}
|
||
|
||
/* 复制参数 */
|
||
planner->param = *param;
|
||
|
||
/* 计算控制周期 */
|
||
planner->dt = 1.0f / control_freq;
|
||
|
||
/* 初始化状态 */
|
||
SpeedPlanner_Reset(planner);
|
||
|
||
return 0;
|
||
}
|
||
|
||
int8_t SpeedPlanner_Reset(SpeedPlanner_t *planner) {
|
||
if (planner == NULL) {
|
||
return -1;
|
||
}
|
||
|
||
planner->state = SPEED_PLANNER_IDLE;
|
||
planner->current_velocity = 0.0f;
|
||
planner->current_position = 0.0f;
|
||
planner->target_velocity = 0.0f;
|
||
planner->target_position = 0.0f;
|
||
planner->planned_velocity = 0.0f;
|
||
planner->planned_acceleration = 0.0f;
|
||
planner->last_target_velocity = 0.0f;
|
||
planner->position_error = 0.0f;
|
||
planner->ramp_start_velocity = 0.0f;
|
||
planner->ramp_end_velocity = 0.0f;
|
||
planner->ramp_duration = 0.0f;
|
||
planner->ramp_time = 0.0f;
|
||
|
||
return 0;
|
||
}
|
||
|
||
int8_t SpeedPlanner_UpdateState(SpeedPlanner_t *planner, float current_velocity, float current_position) {
|
||
if (planner == NULL) {
|
||
return -1;
|
||
}
|
||
|
||
planner->current_velocity = current_velocity;
|
||
planner->current_position = current_position;
|
||
planner->position_error = planner->target_position - planner->current_position;
|
||
|
||
return 0;
|
||
}
|
||
|
||
int8_t SpeedPlanner_SetTargetVelocity(SpeedPlanner_t *planner, float target_velocity) {
|
||
if (planner == NULL) {
|
||
return -1;
|
||
}
|
||
|
||
/* 限制目标速度在允许范围内 */
|
||
planner->target_velocity = SpeedPlanner_Limit(target_velocity,
|
||
-planner->param.max_velocity,
|
||
planner->param.max_velocity);
|
||
|
||
return 0;
|
||
}
|
||
|
||
int8_t SpeedPlanner_SetTargetPosition(SpeedPlanner_t *planner, float target_position) {
|
||
if (planner == NULL) {
|
||
return -1;
|
||
}
|
||
|
||
planner->target_position = target_position;
|
||
planner->position_error = planner->target_position - planner->current_position;
|
||
|
||
return 0;
|
||
}
|
||
|
||
/* 检查目标速度是否改变 */
|
||
static int8_t SpeedPlanner_IsTargetChanged(SpeedPlanner_t *planner) {
|
||
return (fabsf(planner->target_velocity - planner->last_target_velocity) > planner->param.velocity_tolerance);
|
||
}
|
||
|
||
/* 开始新的斜坡规划 */
|
||
static void SpeedPlanner_StartRamp(SpeedPlanner_t *planner) {
|
||
planner->ramp_start_velocity = planner->current_velocity;
|
||
planner->ramp_end_velocity = planner->target_velocity;
|
||
|
||
/* 计算需要的加速度和时间 */
|
||
float velocity_diff = planner->ramp_end_velocity - planner->ramp_start_velocity;
|
||
float required_acceleration = fabsf(velocity_diff);
|
||
|
||
/* 选择合适的加速度（加速或减速） */
|
||
float max_accel = (velocity_diff >= 0) ? planner->param.max_acceleration : planner->param.max_deceleration;
|
||
|
||
if (required_acceleration > max_accel) {
|
||
required_acceleration = max_accel;
|
||
}
|
||
|
||
/* 计算斜坡时间 */
|
||
if (required_acceleration > FLOAT_EPSILON) {
|
||
planner->ramp_duration = fabsf(velocity_diff) / required_acceleration;
|
||
} else {
|
||
planner->ramp_duration = 0.0f;
|
||
}
|
||
|
||
planner->ramp_time = 0.0f;
|
||
|
||
/* 设置状态 */
|
||
if (fabsf(velocity_diff) < planner->param.velocity_tolerance) {
|
||
planner->state = SPEED_PLANNER_FINISHED;
|
||
} else if (velocity_diff > 0) {
|
||
planner->state = SPEED_PLANNER_ACCELERATING;
|
||
} else {
|
||
planner->state = SPEED_PLANNER_DECELERATING;
|
||
}
|
||
}
|
||
|
||
/* 执行斜坡插值 */
|
||
static float SpeedPlanner_RampInterpolation(SpeedPlanner_t *planner) {
|
||
if (planner->ramp_duration <= FLOAT_EPSILON) {
|
||
return planner->ramp_end_velocity;
|
||
}
|
||
|
||
/* 线性插值 */
|
||
float progress = planner->ramp_time / planner->ramp_duration;
|
||
progress = SpeedPlanner_Limit(progress, 0.0f, 1.0f);
|
||
|
||
return planner->ramp_start_velocity +
|
||
(planner->ramp_end_velocity - planner->ramp_start_velocity) * progress;
|
||
}
|
||
|
||
int8_t SpeedPlanner_Update(SpeedPlanner_t *planner) {
|
||
if (planner == NULL) {
|
||
return -1;
|
||
}
|
||
|
||
/* 检查位置误差是否超限 */
|
||
if (fabsf(planner->position_error) > planner->param.position_error_limit) {
|
||
/* 紧急停止：设置目标速度为0 */
|
||
planner->target_velocity = 0.0f;
|
||
}
|
||
|
||
/* 检查目标是否改变 */
|
||
if (SpeedPlanner_IsTargetChanged(planner) || planner->state == SPEED_PLANNER_IDLE) {
|
||
SpeedPlanner_StartRamp(planner);
|
||
planner->last_target_velocity = planner->target_velocity;
|
||
}
|
||
|
||
/* 根据当前状态执行规划 */
|
||
switch (planner->state) {
|
||
case SPEED_PLANNER_IDLE:
|
||
planner->planned_velocity = planner->current_velocity;
|
||
planner->planned_acceleration = 0.0f;
|
||
break;
|
||
|
||
case SPEED_PLANNER_ACCELERATING:
|
||
case SPEED_PLANNER_DECELERATING:
|
||
/* 执行斜坡规划 */
|
||
planner->planned_velocity = SpeedPlanner_RampInterpolation(planner);
|
||
|
||
/* 计算规划加速度 */
|
||
if (planner->dt > FLOAT_EPSILON) {
|
||
planner->planned_acceleration = (planner->planned_velocity - planner->current_velocity) / planner->dt;
|
||
} else {
|
||
planner->planned_acceleration = 0.0f;
|
||
}
|
||
|
||
/* 限制加速度 */
|
||
float max_accel = (planner->state == SPEED_PLANNER_ACCELERATING) ?
|
||
planner->param.max_acceleration : planner->param.max_deceleration;
|
||
planner->planned_acceleration = SpeedPlanner_Limit(planner->planned_acceleration, -max_accel, max_accel);
|
||
|
||
/* 更新斜坡时间 */
|
||
planner->ramp_time += planner->dt;
|
||
|
||
/* 检查是否完成斜坡 */
|
||
if (planner->ramp_time >= planner->ramp_duration ||
|
||
fabsf(planner->planned_velocity - planner->ramp_end_velocity) < planner->param.velocity_tolerance) {
|
||
planner->state = SPEED_PLANNER_FINISHED;
|
||
planner->planned_velocity = planner->ramp_end_velocity;
|
||
}
|
||
break;
|
||
|
||
case SPEED_PLANNER_CONSTANT:
|
||
case SPEED_PLANNER_FINISHED:
|
||
planner->planned_velocity = planner->target_velocity;
|
||
planner->planned_acceleration = 0.0f;
|
||
break;
|
||
|
||
default:
|
||
planner->state = SPEED_PLANNER_IDLE;
|
||
break;
|
||
}
|
||
|
||
/* 最终限制输出速度 */
|
||
planner->planned_velocity = SpeedPlanner_Limit(planner->planned_velocity,
|
||
-planner->param.max_velocity,
|
||
planner->param.max_velocity);
|
||
|
||
return 0;
|
||
}
|
||
|
||
float SpeedPlanner_GetPlannedVelocity(SpeedPlanner_t *planner) {
|
||
if (planner == NULL) {
|
||
return 0.0f;
|
||
}
|
||
return planner->planned_velocity;
|
||
}
|
||
|
||
float SpeedPlanner_GetPlannedAcceleration(SpeedPlanner_t *planner) {
|
||
if (planner == NULL) {
|
||
return 0.0f;
|
||
}
|
||
return planner->planned_acceleration;
|
||
}
|
||
|
||
float SpeedPlanner_GetPositionError(SpeedPlanner_t *planner) {
|
||
if (planner == NULL) {
|
||
return 0.0f;
|
||
}
|
||
return planner->position_error;
|
||
}
|
||
|
||
SpeedPlanner_State_t SpeedPlanner_GetState(SpeedPlanner_t *planner) {
|
||
if (planner == NULL) {
|
||
return SPEED_PLANNER_IDLE;
|
||
}
|
||
return planner->state;
|
||
}
|
||
|
||
int8_t SpeedPlanner_IsEmergencyStop(SpeedPlanner_t *planner) {
|
||
if (planner == NULL) {
|
||
return 0;
|
||
}
|
||
return (fabsf(planner->position_error) > planner->param.position_error_limit) ? 1 : 0;
|
||
}
|