4. 梯形速度规划：数学描述、三段设计与代码实现
好，咱们进入速度规划里最经典、也最实用的一个模型——梯形速度规划。
为什么叫梯形？你想想看，把速度随时间的变化画出来，加速段、匀速段、减速段，连起来就像一个梯形。说白了，这是最简单的“先加速、再匀速、最后减速”的运动模式。
我在项目里最早接触的就是这个模型。那时候做AEB的跟车策略，发现梯形规划虽然简单，但胜在稳定、可预测。你只要把参数算对，车就能老老实实按你画的曲线走。
4.1 梯形速度曲线的数学描述
先看数学表达。梯形速度曲线由三段组成：
- 加速段：速度从 v₀ 线性增加到 v_max
- 匀速段：速度保持 v_max 不变
- 减速段：速度从 v_max 线性减小到 v_end
加速度 a 在加速段为正，减速段为负，匀速段为0。嗯，这里要注意：减速段的加速度通常是负值，但有些教材里用绝对值表示，我个人习惯用带符号的 a，这样代码里好处理。
数学公式长这样：
加速段 (0 ≤ t ≤ t₁):
v(t) = v₀ + a * t
s(t) = v₀ * t + 0.5 * a * t²
匀速段 (t₁ < t ≤ t₂):
v(t) = v_max
s(t) = s₁ + v_max * (t - t₁)
减速段 (t₂ < t ≤ t₃):
v(t) = v_max - a * (t - t₂)
s(t) = s₂ + v_max * (t - t₂) - 0.5 * a * (t - t₂)²
其中 s₁ 是加速段结束时的位移，s₂ 是匀速段结束时的位移。
关键点：梯形规划的核心是三个时间点 t₁、t₂、t₃。只要算出这三个时间，整个曲线就定了。
4.2 三段设计：加速/匀速/减速
实际设计时，三段不是随便定的。我一般按这个顺序来：
加速段设计
加速段主要受两个约束：最大加速度和乘坐舒适性。
我曾经在某个项目中，为了追求快速响应，把加速度设到 3 m/s²。结果测试时乘客直接往前栽，投诉说“这车跟新手司机似的”。后来我学乖了，乘用车一般控制在 1.5~2.0 m/s²，商用车更低。
加速段的时间 t₁ 由下式决定：
t₁ = (v_max - v₀) / a_acc其中 a_acc 是你设定的加速度值。
匀速段设计
匀速段说白了就是“巡航”。这段的长短取决于总路程和加减速段占用的距离。
如果总路程很短，可能根本没有匀速段——这就是所谓的“三角形速度规划”。我在做泊车路径规划时就经常遇到这种情况，车位就那么短，刚加速就得减速。
匀速段时间：
t₂ - t₁ = (s_total - s_acc - s_dec) / v_max减速段设计
减速段比加速段更敏感。为什么？因为减速涉及安全。
我建议减速段的加速度绝对值不要超过加速段的1.2倍。你想想看，急刹车虽然能停住，但后面的车可能追尾。在跟车策略里，减速段的舒适性往往比响应速度更重要。
减速段时间：
t₃ - t₂ = (v_max - v_end) / a_dec个人经验：如果 v_end 不为0（比如跟车场景下目标车速不是0），减速段会短一些。但要注意，减速段结束时速度必须精确匹配目标值，否则会有冲击。
4.3 参数计算：从需求到数值
好，现在咱们来算具体的参数。假设一个场景：
- 起始速度 v₀ = 0 m/s
- 目标速度 v_max = 10 m/s (36 km/h)
- 终点速度 v_end = 0 m/s
- 总路程 s_total = 100 m
- 加速度 a = 2 m/s²
- 减速度 a_dec = 2 m/s²
第一步，算加速段和减速段的时间和距离：
t₁ = 10 / 2 = 5 s
s_acc = 0 * 5 + 0.5 * 2 * 25 = 25 m
t_dec = 10 / 2 = 5 s
s_dec = 10 * 5 - 0.5 * 2 * 25 = 25 m
第二步，看匀速段：
s_cruise = 100 - 25 - 25 = 50 m
t_cruise = 50 / 10 = 5 s
总时间 t₃ = 5 + 5 + 5 = 15 s。
你看，这个梯形很对称。但实际项目中很少这么完美。比如总路程只有60米：
s_cruise = 60 - 25 - 25 = 10 m
t_cruise = 10 / 10 = 1 s
如果总路程只有40米呢？那就没有匀速段了，变成三角形规划。这时候需要重新算最大能达到的速度：
s_total = v_max² / (2*a) + v_max² / (2*a_dec)
40 = v_max² / 4 + v_max² / 4 = v_max² / 2
v_max = sqrt(80) ≈ 8.94 m/s
注意：当总路程不足时，必须降低目标速度。我曾经在测试中忘了这个判断，结果规划出的曲线在终点前速度还没降到0，直接冲出边界了。嗯，这个坑我踩过。
4.4 代码实现：C语言版
最后上代码。我习惯用C语言写底层规划，因为实时性好。这里给一个完整的梯形规划函数：
#include <stdio.h>
#include <math.h>
typedef struct {
float t1; // 加速结束时间
float t2; // 匀速结束时间
float t3; // 减速结束时间
float v_max; // 实际最大速度
float s_acc; // 加速段距离
float s_dec; // 减速段距离
} TrapezoidProfile;
TrapezoidProfile calc_trapezoid_profile(float v0, float v_end, float v_target,
float a_acc, float a_dec, float s_total) {
TrapezoidProfile profile = {0};
// 先假设能达到目标速度
float v_max = v_target;
// 计算加减速段距离
float s_acc = (v_max*v_max - v0*v0) / (2 * a_acc);
float s_dec = (v_max*v_max - v_end*v_end) / (2 * a_dec);
// 判断是否有匀速段
if (s_acc + s_dec <= s_total) {
// 有匀速段
float s_cruise = s_total - s_acc - s_dec;
profile.t1 = (v_max - v0) / a_acc;
profile.t2 = profile.t1 + s_cruise / v_max;
profile.t3 = profile.t2 + (v_max - v_end) / a_dec;
} else {
// 无匀速段，需要降低最大速度
// 解方程: v_max^2/(2a_acc) + v_max^2/(2a_dec) = s_total
float coeff = 1/(2*a_acc) + 1/(2*a_dec);
v_max = sqrt(s_total / coeff);
s_acc = (v_max*v_max - v0*v0) / (2 * a_acc);
s_dec = (v_max*v_max - v_end*v_end) / (2 * a_dec);
profile.t1 = (v_max - v0) / a_acc;
profile.t2 = profile.t1; // 无匀速段
profile.t3 = profile.t1 + (v_max - v_end) / a_dec;
}
profile.v_max = v_max;
profile.s_acc = s_acc;
profile.s_dec = s_dec;
return profile;
}
// 根据时间t获取当前速度和位置
void get_state_at_time(TrapezoidProfile profile, float t,
float v0, float v_end, float a_acc, float a_dec,
float *v, float *s) {
if (t <= profile.t1) {
// 加速段
*v = v0 + a_acc * t;
*s = v0 * t + 0.5 * a_acc * t * t;
} else if (t <= profile.t2) {
// 匀速段
float t_cruise = t - profile.t1;
*v = profile.v_max;
*s = profile.s_acc + profile.v_max * t_cruise;
} else if (t <= profile.t3) {
// 减速段
float t_dec = t - profile.t2;
*v = profile.v_max - a_dec * t_dec;
*s = profile.s_acc + (profile.t2 - profile.t1) * profile.v_max
+ profile.v_max * t_dec - 0.5 * a_dec * t_dec * t_dec;
} else {
// 已结束
*v = v_end;
*s = profile.s_acc + (profile.t2 - profile.t1) * profile.v_max + profile.s_dec;
}
}
使用建议：实际工程中，我一般把 calc_trapezoid_profile 放在初始化阶段调用一次，然后在控制循环里反复调用 get_state_at_time。这样计算量小，实时性好。
代码里有个细节：当总路程不足时，我重新算了 v_max。这个逻辑一定要有。我记得有一次在实车上，因为忘记处理这个边界情况，规划器直接输出了负的匀速段时间，导致控制模块报错。从那以后，我所有规划代码的第一件事就是检查“能不能跑完”。
好了，梯形速度规划的核心内容就这些。下一章咱们会在这个基础上，加入跟车策略中的前车运动预测，让规划更智能。