简单的梯形规划算法实践

简单的梯形规划控制实践，实际使用时测试有一些问题，为后续优化迭代留个思路。

typedef struct {
  uint8_t plan_flag; //是否需要执行梯形规划的标志位
  uint8_t driect_flag; //不规划直接下发控制的标志位
  uint8_t complete_flag; //规划执行完成的标志位；
  
  float delta_angle; //需要执行规划的角度差
  float start_angle; //起始规划的角度
  float v_start; //起始规划的速速
  float v_end; //结束规划时的速度
  float v_max; //最大速度
  float acceleration; //加速度的値
  float accelerate_until; //需要执行加速的角度
  float decelerate_after; //加速和匀速的角度
  float accelerate_rate; //加速的速率
  float plan_angle; //当前规划的角度值
}trapezoid_parameter_t;

trapezoid_parameter_t tp_lf;

/*
使用前，需要提前初始化好如下参数。
*/
tp_lf.acceleration = 0.1f;
tp_lf.v_max = 3;
tp_lf.v_start = 1;
tp_lf.v_end = 1;


/* 
下面为周期性调用的代码：
theta_lf 为下发的目标值
last_theta_lf 为上次记录的下发的目标值
-steer_current_position[0] 为当前的实际位置
lf 为下发的目标角度
 */
float theta_lf_1 = theta_lf; //= SlipAveFilterCalculate(&saf_theta_lf, theta_lf);
      if(fabs(theta_lf_1 - last_theta_lf) > tp_lf.v_start + tp_lf.v_end) {//需要重新规划计算
      //if(fabs(theta_lf_1 + steer_current_position[0]) > tp_lf.v_start + tp_lf.v_end) {
        tp_lf.plan_flag = 1;
        tp_lf.complete_flag = 0;
        tp_lf.accelerate_rate = 0;
        tp_lf.plan_angle = 0;//tp_lf.v_start;
        tp_lf.start_angle = - steer_current_position[0];
        tp_lf.delta_angle = theta_lf_1 - tp_lf.start_angle;
        float acc_angle = (tp_lf.v_max * tp_lf.v_max) / (2.0f * tp_lf.acceleration);
        float dec_angle = (tp_lf.v_max * tp_lf.v_max) / (2.0f * tp_lf.acceleration);
        float plateau_angle = fabs(tp_lf.delta_angle) - acc_angle - dec_angle;
        if(plateau_angle < 0) {
          acc_angle = fabs(tp_lf.delta_angle) / 2;
          plateau_angle = 0;
        }
        tp_lf.accelerate_until = acc_angle;
        tp_lf.decelerate_after = acc_angle + plateau_angle;
      } else {
        if(tp_lf.plan_flag != 1) { //非规划过程并且运动间隔比较小时，直接下发控制
          lf = theta_lf_1;
        } else {
          if(tp_lf.complete_flag == 1) { //规划过程执行完成
            lf = theta_lf_1;
            tp_lf.plan_angle = 0;
            tp_lf.accelerate_rate = 0;
          } else { //规划过程
            if(tp_lf.plan_angle < tp_lf.accelerate_until) { //加速过程
              tp_lf.accelerate_rate += tp_lf.acceleration;
              if(tp_lf.accelerate_rate > tp_lf.v_max) {
                tp_lf.accelerate_rate = tp_lf.v_max;
              }
            } else if(tp_lf.plan_angle > tp_lf.decelerate_after) { // 减速过程
              tp_lf.accelerate_rate -= tp_lf.acceleration;
              if(tp_lf.accelerate_rate < 0) {
                tp_lf.accelerate_rate = 0;
                tp_lf.complete_flag = 1;
              }
            } else { //匀速过程
            
            }
            
            tp_lf.plan_angle += tp_lf.accelerate_rate;
            if(tp_lf.delta_angle > 0) {
              //tp_lf.plan_angle += (tp_lf.accelerate_rate + tp_lf.v_start);
              lf = tp_lf.plan_angle + tp_lf.start_angle;
              if(theta_lf_1 - lf < tp_lf.v_end) {
                lf = theta_lf_1;
                tp_lf.complete_flag = 1;
                
              }
            } else {
              //tp_lf.plan_angle += tp_lf.accelerate_rate;
              lf = tp_lf.start_angle - tp_lf.plan_angle;
              if(theta_lf_1 - lf > -tp_lf.v_end) {
                lf = theta_lf_1;
                tp_lf.complete_flag = 1;
              }
            }
          }
        }
      }
      last_theta_lf = theta_lf_1;