梯形速度规划的原理：梯形速度规划算法
对应的代码如下：

#pragma once
#include <cmath>
#include <iostream>
#include <vector>
struct SpeedPoint {SpeedPoint() {s = 0;speed = 0;t = 0;}double s;      // mdouble speed;  // m/sdouble t;      // s
};inline std::tuple<double, double, double> trapezoidalSpeedPlanningGetS(const double& max_speed, const double& init_speed,const double& aim_distance, const double& aim_speed,const double& aim_acc) {// 分类讨论，初始车速&最高速度的大小double valid_aim_speed = aim_speed;if (aim_speed > max_speed) {valid_aim_speed = max_speed;}double s1 =(std::pow(max_speed, 2) - std::pow(init_speed, 2)) / (2 * aim_acc);double s3 =(std::pow(max_speed, 2) - std::pow(valid_aim_speed, 2)) / (2 * aim_acc);double s2 = aim_distance - fabs(s1) - s3;if (s2 < 0) {s2 = 0;s1 = (2 * aim_acc * aim_distance - std::pow(init_speed, 2) +std::pow(valid_aim_speed, 2)) /(4 * aim_acc);if (fabs(s1) > aim_distance) {s1 = std::copysign(aim_distance, s1);s2 = s3 = 0;} elses3 = aim_distance - fabs(s1);}return std::tuple<double, double, double>(s1, s2, s3);
}/*** @brief trapezoidalSpeedPlanning: 梯形速度规划* @param max_speed* @param init_speed* @param aim_distance* @param aim_speed* @param aim_acc* s1: 加速段* s2: 匀速段* s3: 减速段* @return*/
inline std::vector<SpeedPoint> trapezoidalSpeedPlanning(const double& max_speed, const double& init_speed,const double& aim_distance, const double& aim_speed,const double& aim_acc) {std::vector<SpeedPoint> results;if (max_speed <= 0) {// to zero.return results;}double s1, s2, sk, s3;std::tuple<double, double, double> result_ss = trapezoidalSpeedPlanningGetS(max_speed, init_speed, aim_distance, aim_speed, aim_acc);s1 = std::get<0>(result_ss);s2 = std::get<1>(result_ss);s3 = std::get<2>(result_ss);sk = fabs(s1) + s2;const double delta_t = 0.1;double acculate_s = 0;double acculate_v = init_speed;double acculate_t = 0;SpeedPoint speed_point;double s1_sign = std::copysign(1, s1);double speed_m = std::sqrt(2 * aim_acc * s1 + init_speed * init_speed);for (; acculate_s < aim_distance;) {speed_point.s = acculate_s;speed_point.t = acculate_t;speed_point.speed = acculate_v;acculate_t += delta_t;results.push_back(speed_point);if (acculate_s <= fabs(s1)) {acculate_s += acculate_v * delta_t;acculate_v += aim_acc * delta_t * s1_sign;} else if (acculate_s < sk) {acculate_s += acculate_v * delta_t;} else {// 减速段acculate_s += acculate_v * delta_t;acculate_v -= aim_acc * delta_t;if (acculate_v <= 0) break;}}return results;
}inline double trapezoidalSpeedPlanningTime(const double& max_speed,const double& init_speed,const double& aim_distance,const double& aim_speed,const double& aim_acc) {if (max_speed <= 0) {// to zero.return INFINITY;}double s1, s2, s3;std::tuple<double, double, double> result_ss = trapezoidalSpeedPlanningGetS(max_speed, init_speed, aim_distance, aim_speed, aim_acc);double valid_aim_speed = aim_speed;if (aim_speed > max_speed) {valid_aim_speed = max_speed;}s1 = std::get<0>(result_ss);s2 = std::get<1>(result_ss);s3 = std::get<2>(result_ss);//  std::cout << "s1: " << s1 << ", s2: " << s2 << ", s3: " << s3 <<//  std::endl;double speed_m = std::sqrt(2 * aim_acc * s1 + init_speed * init_speed);double speed_final = std::sqrt(speed_m * speed_m + 0.2 - 2 * aim_acc * s3);if (fabs(speed_final - valid_aim_speed) > 1) return INFINITY;double time_1 = fabs(speed_m - init_speed) / aim_acc, time_2 = s2 / speed_m,time_3 = (speed_m - speed_final) / aim_acc;return time_1 + time_2 + time_3;
}
inline double GetSpeedPlanningTimeByReultS(const std::tuple<double, double, double>& result_ss,const double& max_speed, const double& init_speed,const double& aim_distance, const double& aim_speed,const double& aim_acc) {if (max_speed <= 0) {// to zero.return INFINITY;}double s1, s2, s3;double valid_aim_speed = aim_speed;if (aim_speed > max_speed) {valid_aim_speed = max_speed;}s1 = std::get<0>(result_ss);s2 = std::get<1>(result_ss);s3 = std::get<2>(result_ss);//  std::cout << "s1: " << s1 << ", s2: " << s2 << ", s3: " << s3 <<//  std::endl;double speed_m = std::sqrt(2 * aim_acc * s1 + init_speed * init_speed);double speed_final = std::sqrt(speed_m * speed_m + 0.2 - 2 * aim_acc * s3);if (fabs(speed_final - valid_aim_speed) > 1) return INFINITY;double time_1 = fabs(speed_m - init_speed) / aim_acc, time_2 = s2 / speed_m,time_3 = (speed_m - speed_final) / aim_acc;return time_1 + time_2 + time_3;
}inline SpeedPoint trapezoidalSpeedPlanningPointByS(const double& s1, const double& s2, const double& s3, const double& s,const double& init_speed, const double& aim_acc) {SpeedPoint speed_point;double speed_m = std::sqrt(2 * aim_acc * s1 + init_speed * init_speed);speed_point.s = s;double s1_sign = std::copysign(1, s1);if (s <= fabs(s1)) {double vt = sqrt(2 * aim_acc * s1_sign * s + init_speed * init_speed);speed_point.speed = vt;speed_point.t = (vt - init_speed) / (aim_acc * s1_sign);} else if (s < fabs(s1) + s2) {double v1_t = speed_m;speed_point.speed = v1_t;// s1_t + (s - fabs(s1)) / v1_t;speed_point.t =(v1_t - init_speed) / (aim_acc * s1_sign) + (s - fabs(s1)) / v1_t;} else {// 减速段double v1_t = speed_m;double t1 = fabs(speed_m - init_speed) / aim_acc;double t2 = s2 / v1_t;double end_speed_2 = v1_t * v1_t - 2 * aim_acc * (s - s2 - fabs(s1));if (end_speed_2 < 1e-2)speed_point.speed = 0;elsespeed_point.speed = sqrt(end_speed_2);speed_point.t = t1 + t2 + (v1_t - speed_point.speed) / aim_acc;}return speed_point;
}