diff --git a/User/component/limiter.h b/User/component/limiter.h
index d0aa92a..dde71f3 100644
--- a/User/component/limiter.h
+++ b/User/component/limiter.h
@@ -61,3 +61,4 @@ float PowerLimit_TargetPower(float power_limit, float power_buffer);
  */
 float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate,
                           float heat_increase, bool is_big);
+
diff --git a/User/component/speed_planner.c b/User/component/speed_planner.c
index c9e7a72..1714b1b 100644
--- a/User/component/speed_planner.c
+++ b/User/component/speed_planner.c
@@ -1,135 +1,266 @@
-/**
- * @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;
+/* 浮点数比较容差 */
+#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;
 }
 
-/**
- * @brief 初始化速度规划器
- */
-int8_t SpeedPlanner_Init(SpeedPlanner_t *planner, const SpeedPlanner_Params_t *params) {
-  if (planner == NULL || params == NULL) {
+/* 符号函数 */
+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;
   }
   
-  memset(planner, 0, sizeof(SpeedPlanner_t));
-  planner->param = *params;
+  /* 参数检查 */
+  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;
 }
 
-/**
- * @brief 重置速度规划器
- */
-void SpeedPlanner_Reset(SpeedPlanner_t *planner, float current_position, float current_velocity) {
+int8_t SpeedPlanner_Reset(SpeedPlanner_t *planner) {
   if (planner == NULL) {
-    return;
+    return -1;
   }
   
-  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;
+  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;
 }
 
-/**
- * @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) {
+int8_t SpeedPlanner_UpdateState(SpeedPlanner_t *planner, float current_velocity, float current_position) {
   if (planner == NULL) {
-    return 0.0f;
+    return -1;
   }
-  return planner->planned_position;
+  
+  planner->current_velocity = current_velocity;
+  planner->current_position = current_position;
+  planner->position_error = planner->target_position - planner->current_position;
+  
+  return 0;
 }
 
-/**
- * @brief 获取规划后的速度
- */
-float SpeedPlanner_GetPlannedVelocity(const SpeedPlanner_t *planner) {
+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;
+}
diff --git a/User/component/speed_planner.h b/User/component/speed_planner.h
index 53f57ff..8881d0c 100644
--- a/User/component/speed_planner.h
+++ b/User/component/speed_planner.h
@@ -1,76 +1,143 @@
-/**
- * @file speed_planner.h
- * @brief 斜坡速度规划器
- * @details 提供最大加速度和最大速度限制,防止目标位移起飞
- */
-
 #ifndef SPEED_PLANNER_H
 #define SPEED_PLANNER_H
 
 #include <stdint.h>
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* 速度规划器状态枚举 */
+typedef enum {
+  SPEED_PLANNER_IDLE = 0,       /* 空闲状态 */
+  SPEED_PLANNER_ACCELERATING,   /* 加速阶段 */
+  SPEED_PLANNER_CONSTANT,       /* 匀速阶段 */
+  SPEED_PLANNER_DECELERATING,   /* 减速阶段 */
+  SPEED_PLANNER_FINISHED        /* 完成状态 */
+} SpeedPlanner_State_t;
+
 /* 速度规划器参数结构体 */
 typedef struct {
-  float max_velocity;     /* 最大速度 (m/s) */
-  float max_acceleration; /* 最大加速度 (m/s²) */
-  float max_position_error; /* 最大位置误差 (m), 防止位移起飞 */
-} SpeedPlanner_Params_t;
+  float max_velocity;           /* 最大速度 (m/s) */
+  float max_acceleration;       /* 最大加速度 (m/s²) */
+  float max_deceleration;       /* 最大减速度 (m/s²) */
+  float position_error_limit;   /* 位置误差限制 (m) */
+  float velocity_tolerance;     /* 速度容差，用于判断是否到达目标速度 (m/s) */
+} SpeedPlanner_Param_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_Param_t param;
   
-  SpeedPlanner_Params_t param; /* 参数 */
+  /* 状态变量 */
+  SpeedPlanner_State_t state;
+  float current_velocity;       /* 当前速度 (m/s) */
+  float current_position;       /* 当前位置 (m) */
+  float target_velocity;        /* 目标速度 (m/s) */
+  float target_position;        /* 目标位置 (m) */
+  
+  /* 规划变量 */
+  float planned_velocity;       /* 规划输出速度 (m/s) */
+  float planned_acceleration;   /* 规划输出加速度 (m/s²) */
+  
+  /* 内部状态 */
+  float last_target_velocity;   /* 上次目标速度，用于检测目标变化 */
+  float position_error;         /* 位置误差 (目标位置 - 当前位置) */
+  float dt;                     /* 控制周期 (s) */
+  
+  /* 斜坡规划状态 */
+  float ramp_start_velocity;    /* 斜坡起始速度 */
+  float ramp_end_velocity;      /* 斜坡结束速度 */
+  float ramp_duration;          /* 斜坡持续时间 */
+  float ramp_time;              /* 斜坡当前时间 */
 } SpeedPlanner_t;
 
 /**
  * @brief 初始化速度规划器
- * @param planner 规划器结构体指针
- * @param params 参数结构体指针
- * @return 0:成功, -1:失败
+ * @param planner 速度规划器指针
+ * @param param 参数结构体指针
+ * @param control_freq 控制频率 (Hz)
+ * @return 0: 成功, -1: 失败
  */
-int8_t SpeedPlanner_Init(SpeedPlanner_t *planner, const SpeedPlanner_Params_t *params);
+int8_t SpeedPlanner_Init(SpeedPlanner_t *planner, SpeedPlanner_Param_t *param, float control_freq);
+
+/**
+ * @brief 更新当前状态
+ * @param planner 速度规划器指针
+ * @param current_velocity 当前速度 (m/s)
+ * @param current_position 当前位置 (m)
+ * @return 0: 成功, -1: 失败
+ */
+int8_t SpeedPlanner_UpdateState(SpeedPlanner_t *planner, float current_velocity, float current_position);
+
+/**
+ * @brief 设置目标速度
+ * @param planner 速度规划器指针
+ * @param target_velocity 目标速度 (m/s)
+ * @return 0: 成功, -1: 失败
+ */
+int8_t SpeedPlanner_SetTargetVelocity(SpeedPlanner_t *planner, float target_velocity);
+
+/**
+ * @brief 设置目标位置
+ * @param planner 速度规划器指针
+ * @param target_position 目标位置 (m)
+ * @return 0: 成功, -1: 失败
+ */
+int8_t SpeedPlanner_SetTargetPosition(SpeedPlanner_t *planner, float target_position);
+
+/**
+ * @brief 执行速度规划计算
+ * @param planner 速度规划器指针
+ * @return 0: 成功, -1: 失败
+ */
+int8_t SpeedPlanner_Update(SpeedPlanner_t *planner);
+
+/**
+ * @brief 获取规划的速度输出
+ * @param planner 速度规划器指针
+ * @return 规划的速度 (m/s)
+ */
+float SpeedPlanner_GetPlannedVelocity(SpeedPlanner_t *planner);
+
+/**
+ * @brief 获取规划的加速度输出
+ * @param planner 速度规划器指针
+ * @return 规划的加速度 (m/s²)
+ */
+float SpeedPlanner_GetPlannedAcceleration(SpeedPlanner_t *planner);
+
+/**
+ * @brief 获取位置误差
+ * @param planner 速度规划器指针
+ * @return 位置误差 (m)
+ */
+float SpeedPlanner_GetPositionError(SpeedPlanner_t *planner);
+
+/**
+ * @brief 获取当前状态
+ * @param planner 速度规划器指针
+ * @return 当前状态
+ */
+SpeedPlanner_State_t SpeedPlanner_GetState(SpeedPlanner_t *planner);
 
 /**
  * @brief 重置速度规划器
- * @param planner 规划器结构体指针
- * @param current_position 当前位置
- * @param current_velocity 当前速度
+ * @param planner 速度规划器指针
+ * @return 0: 成功, -1: 失败
  */
-void SpeedPlanner_Reset(SpeedPlanner_t *planner, float current_position, float current_velocity);
+int8_t SpeedPlanner_Reset(SpeedPlanner_t *planner);
 
 /**
- * @brief 更新速度规划器
- * @param planner 规划器结构体指针
- * @param target_velocity 目标速度 (m/s)
- * @param current_position 当前位置 (m)
- * @param current_velocity 当前速度 (m/s)
- * @param dt 时间间隔 (s)
- * @return 规划后的速度 (m/s)
+ * @brief 检查是否需要紧急停止（位置误差过大）
+ * @param planner 速度规划器指针
+ * @return 1: 需要紧急停止, 0: 正常
  */
-float SpeedPlanner_Update(SpeedPlanner_t *planner, 
-                          float target_velocity,
-                          float current_position,
-                          float current_velocity,
-                          float dt);
+int8_t SpeedPlanner_IsEmergencyStop(SpeedPlanner_t *planner);
 
-/**
- * @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);
+#ifdef __cplusplus
+}
+#endif
 
 #endif // SPEED_PLANNER_H