第三节：速度规划问题建模
好，咱们接着聊。上一节我们把路径规划讲透了，但说实话，光有路径远远不够。你想想看，一条弯道，你是60码过还是20码过？这直接决定了乘客会不会晕车，车子会不会侧滑。这就是速度规划要干的事。
我个人习惯把速度规划看作「给路径赋予时间属性」。路径是几何的，速度是时间的。两者结合，才是一个完整的轨迹。
3.1 从路径规划到速度规划：为什么需要这一步？
路径规划输出的是什么？是一串点：(x₁, y₁), (x₂, y₂), ..., (xₙ, yₙ)。每个点只有位置信息，没有时间信息。
但实际控制需要什么？是「什么时间到达什么位置」。比如：
- 第0秒，在起点
- 第2秒，到达弯道入口，速度降到30km/h
- 第4秒，出弯，加速到50km/h
这就是速度规划要解决的问题——把路径上的每个点，映射到时间轴上。
核心观点：路径规划告诉你「走哪条路」，速度规划告诉你「怎么走这条路」。
我在项目中遇到过不少新手，路径规划做得漂漂亮亮，结果一上速度规划就崩了。为什么？因为忽略了动力学约束。你规划了一条完美的曲线，但车子根本跑不出那个速度曲线，那规划就是废纸一张。
3.2 速度-时间（S-T）图：速度规划的「作战地图」
说到速度规划，就绕不开S-T图。这玩意儿说白了就是一个二维坐标系：
- 横轴：时间 t（秒）
- 纵轴：路径里程 s（米）
你想想看，路径上的每个点都有一个里程值 s。比如起点 s=0，终点 s=100。那么速度规划就是在找一条 s(t) 曲线——描述每个时刻车子走到了哪里。
为什么要用S-T图？因为它能直观地表达「时间-空间」关系。举个例子：
- 如果 s(t) 是一条直线，说明车子匀速运动
- 如果 s(t) 是上凸曲线，说明车子在减速
- 如果 s(t) 是下凹曲线，说明车子在加速
嗯，这里要注意：S-T图的斜率就是速度。斜率越大，速度越快。所以看S-T图，一眼就能看出速度变化趋势。
我的经验：调试速度规划时，我第一件事就是画S-T图。如果曲线不平滑，或者斜率突变，那肯定有问题。别急着调参数，先看图。
下面我画了一张S-T图的示意图，帮你理解这个核心概念：
这张图里，红色曲线就是一条典型的S-T曲线。前段加速（斜率逐渐变大），中段匀速（斜率不变），后段减速（斜率逐渐变小）。中间那个红色虚线框代表障碍物——S-T曲线必须绕开它，这就是速度规划的约束。
3.3 速度规划的目标函数：我们到底要优化什么？
好，现在有了S-T图这个工具，接下来要回答一个问题：什么样的速度曲线是「好」的？
这就涉及到目标函数了。我个人习惯把速度规划的目标拆成三个维度：
3.3.1 效率目标：尽快到达
说白了就是时间最短。数学上可以写成：
J₁ = ∫₀ᵀ 1 dt = T其中 T 是总时间。这个目标很简单——时间越短越好。但光追求效率，乘客会骂娘的。
3.3.2 舒适性目标：加速度要小
加速度太大，乘客会前仰后合。所以我们要惩罚大的加速度：
J₂ = ∫₀ᵀ a(t)² dt这里 a(t) 是加速度。平方是为了让大的加速度受到更大的惩罚。嗯，这里要注意：不仅仅是纵向加速度，横向加速度（转弯时）也要考虑。
我曾经踩过的坑：只优化纵向加速度，忽略了横向加速度。结果规划出来的速度曲线在弯道处纵向减速很平缓，但横向加速度爆表。乘客直接贴到车门上去了。从那以后，我都是把横向和纵向加速度一起优化。
3.3.3 平滑性目标：加加速度要小
加加速度（Jerk）是加速度的变化率。加加速度太大，会让人感觉「一顿一顿」的：
J₃ = ∫₀ᵀ j(t)² dt综合起来，目标函数就是这三个的加权和：
J = w₁·J₁ + w₂·J₂ + w₃·J₃权重 w₁、w₂、w₃ 怎么调？我一般先设成 1:10:100。为什么？因为三个量的量纲不同，需要平衡。具体数值得根据实车调试来定。
3.4 速度规划的约束条件：不能越过的红线
目标函数告诉你「往哪个方向走」，约束条件告诉你「不能走到哪里」。速度规划的约束主要有这几类：
| 约束类型 | 数学表达 | 说明 |
|---|---|---|
| 速度上限 | v(t) ≤ v_max(s) | 不同路段限速不同，弯道限速更低 |
| 加速度上限 | |a(t)| ≤ a_max | 受轮胎抓地力、电机能力限制 |
| 加加速度上限 | |j(t)| ≤ j_max | 保证乘坐舒适性 |
| 避障约束 | s(t) ∉ 障碍物区域 | S-T图中避开障碍物占据的区域 |
| 边界条件 | s(0)=0, v(0)=v₀, v(T)=v_T | 起点终点速度已知 |
你想想看，这些约束就像一道道红线。速度曲线必须全部满足，否则就是不可行的规划。
我的调试技巧：如果规划出来的速度曲线总是违反约束，别急着改权重。先检查约束本身是否合理。比如限速是不是设得太低了？加速度上限是不是太保守了？有时候问题出在约束上，而不是优化算法上。
3.5 完整的优化问题表述
把上面这些串起来，速度规划就变成了一个带约束的优化问题：
minimize J = w₁·T + w₂·∫a²dt + w₃·∫j²dt
subject to:
s(0) = 0, v(0) = v₀, v(T) = v_T
0 ≤ v(t) ≤ v_max(s(t))
|a(t)| ≤ a_max
|j(t)| ≤ j_max
s(t) ∉ 障碍物区域
t ∈ [0, T]这个问题的解，就是一条最优的S-T曲线。有了它，控制器就知道每个时刻该以什么速度行驶了。
说实话，这个优化问题看起来简单，但实际求解并不容易。因为约束条件里包含了状态变量 s(t) 和 v(t) 的耦合关系。常用的求解方法有：
- 数值优化法：把时间离散化，变成非线性规划问题
- 动态规划法：在S-T网格上搜索最优路径
- 凸优化法：把问题近似成凸优化，求解速度快
每种方法都有优缺点。我个人在量产项目中用得最多的是数值优化法——虽然计算量大一点，但约束处理最灵活。
总结一下：速度规划的核心就是「在S-T图上找一条最优曲线」。目标函数平衡效率、舒适和平滑，约束条件保证安全可行。搞懂了这些，速度规划就算入门了。
嗯，这一节内容不少。你先把S-T图的概念吃透，目标函数和约束条件理解清楚。下一节我们讲具体的求解算法，到时候会用到今天这些知识。