从路径到轨迹：一步一步安排无人机的运动

一架无人机要沿通道前进 10 米，起点和终点都要静止，途中速度不超过 2 米/秒，加速度大小不超过 1 米/秒²。

路径规划只回答了“经过哪里”：一条从 0 米到 10 米的直线。真正交给控制器执行时，还需要回答“什么时候到哪里”：什么时候开始加速，加速时走了多远，在哪里开始减速，什么时候到达终点。这份带时间的运动安排就是轨迹。本文用这个例子，从头算出一条完整的轨迹。

一、把运动要求写清楚

通道中的直线路径已经通过碰撞检查。用沿通道的坐标 $p$ 表示无人机中心的位置，起点为 0 米，终点为 10 米，朝终点为正方向。飞行高度不变，只计算沿通道方向的运动。

模型采用最简单的一种：规划器指定加速度，由加速度推出速度和位置。用到的三个量是：

量	含义	例子
位置 $p$	无人机中心沿通道走到了哪里	$p=4$：位于起点前方 4 米
速度 $v$	位置每秒怎样变化	$v=2$：正以 2 米/秒向终点运动
加速度 $a$	速度每秒怎样变化	$a=1$：速度每秒增加 1 米/秒

这里算的是规划器生成的参考运动，实际飞行时由控制器根据反馈去跟踪它。

任务条件如下：

条件	数值
起始位置 / 速度	0 米 / 0 米/秒
目标位置 / 到达时速度	10 米 / 0 米/秒
途中速度	$0\le v\le2\,\mathrm{m/s}$
加速度大小	$\lvert a\rvert\le1\,\mathrm{m/s^2}$

起终位置和速度规定了运动两端要满足的条件，称为边界条件；速度、加速度的范围则约束整个运动过程。

最直接的安排是三个阶段：从静止加速，保持速度前进，再减速到静止。下面依次算出每个阶段的时间和距离。

二、加速段：2 秒走 2 米

取 $a=1\,\mathrm{m/s^2}$，速度每秒增加 1 米/秒，第 1 秒末达到 1 米/秒，第 2 秒末达到上限 2 米/秒。

加速度恒定时，速度均匀变化，一段时间内的平均速度就是首尾速度的平均值：

• 第 1 秒：速度从 0 到 1，平均 0.5 米/秒，前进 0.5 米；
• 第 2 秒：速度从 1 到 2，平均 1.5 米/秒，前进 1.5 米。

所以加速段用时 2 秒，走了 2 米，结束时位于 2 米处、速度 2 米/秒。后面的运动就从这个状态接着算。

三、减速段：从终点倒推

1. 在哪里开始减速

取 $a=-1\,\mathrm{m/s^2}$，速度从 2 降到 0 需要 2 秒。这期间速度仍然为正，无人机继续向前，负号只表示向前的速度在减小。

减速段平均速度为 1 米/秒，2 秒前进 2 米。要在 10 米处停下，就必须从 $10-2=8$ 米处开始减速。

终点要求静止，决定了前面必须留出一段减速距离。

2. 减速距离和速度的平方成正比

一般地，设开始减速时速度为 $v_0$，最大减速度为 $b>0$。减速到静止需要 $t_{\mathrm{stop}}=v_0/b$，平均速度为 $v_0/2$，所以减速距离为：

$$d_{\mathrm{stop}} =\frac{v_0}{2}\times\frac{v_0}{b} =\frac{v_0^2}{2b}.$$

代入 $v_0=2$、$b=1$，得到 2 米，与上面一致。

这个式子说明：减速能力不变时，速度翻倍，刹车距离变成四倍。 高速飞行时必须更早开始减速，就是这个原因。

四、中间补上匀速段

加速段占 0～2 米，减速段占 8～10 米，中间还剩 6 米。以 2 米/秒匀速通过，需要 3 秒。完整安排如下：

阶段	时间	加速度	速度	位置
加速	第 0～2 秒	$+1\,\mathrm{m/s^2}$	$0\rightarrow2\,\mathrm{m/s}$	$0\rightarrow2$ 米
匀速	第 2～5 秒	0	保持 $2\,\mathrm{m/s}$	$2\rightarrow8$ 米
减速	第 5～7 秒	$-1\,\mathrm{m/s^2}$	$2\rightarrow0\,\mathrm{m/s}$	$8\rightarrow10$ 米

总距离 $2+6+2=10$ 米，总时间 $2+3+2=7$ 秒。

把每一秒的状态列出来：

时刻 $t$（秒）	位置 $p$（米）	速度 $v$（米/秒）	这一秒前进
0	0	0	—
1	0.5	1	0.5 米
2	2	2	1.5 米
3	4	2	2 米
4	6	2	2 米
5	8	2	2 米
6	9.5	1	1.5 米
7	10	0	0.5 米

最后一列能直接看出运动的快慢：加速时每秒走得越来越多，匀速时不变，减速时越来越少。

五、三条曲线

1. 速度：梯形

速度前两秒从 0 升到 2，中间三秒保持，最后两秒降回 0，形状是一个梯形，称为梯形速度曲线。

曲线下方的面积就是走过的距离：左右两个三角形各 $2\times2\div2=2$ 米，中间长方形 $3\times2=6$ 米，合计 10 米。速度在一段时间内的累积，就是这段时间的位移。

2. 位置：斜率就是速度

位置曲线从 0 升到 10 米：加速时越来越陡，匀速时是直线，减速时逐渐变平。例如第 3 秒到第 4 秒，位置从 4 米到 6 米，斜率为 2，正是当时的速度。时间间隔取得越来越短，得到的瞬时变化率就是速度：

$$v(t)=\frac{dp(t)}{dt}.$$

3. 加速度：三段常数

运动前后静止，加速度为 0；三个阶段分别是 $+1$、0、$-1\,\mathrm{m/s^2}$。在速度平滑变化的区间内：

$$a(t)=\frac{dv(t)}{dt}.$$

三条曲线的关系可以概括为：加速度决定速度怎样变，速度决定位置怎样变。

六、任意时刻的位置与速度

1. 状态传播公式

设一段运动开始时位置为 $p_0$、速度为 $v_0$，保持加速度 $a$ 经过时间 $\Delta t$。速度增加了 $a\Delta t$：

$$v_{\mathrm{new}}=v_0+a\Delta t.$$

平均速度是首尾平均 $v_0+\frac12a\Delta t$，乘以时间就是位移：

$$p_{\mathrm{new}} =p_0+v_0\Delta t+\frac12a\Delta t^2.$$

三项分别是起始位置、按起始速度走过的距离、加速度额外带来的距离。

给定当前位置和速度，再给定加速度和持续时间，就能算出之后的位置和速度，这个过程称为状态传播。

2. 例：第 5.5 秒在哪里

第 5 秒开始减速，此时 $p_0=8$ 米、$v_0=2$ 米/秒、$a=-1$ 米/秒²。到第 5.5 秒，减速段持续了 $\Delta t=0.5$ 秒：

$$v=2-1\times0.5=1.5\,\mathrm{m/s}, \qquad p=8+2\times0.5-\frac12\times0.5^2=8.875\text{ 米}.$$

用平均速度检查：这半秒速度从 2 降到 1.5，平均 1.75 米/秒，前进 0.875 米，结果一致。

注意公式里的 $\Delta t$ 是这一段开始后经过的时间（0.5 秒），而不是总时间 5.5 秒。

3. 整条轨迹的表达式

令 $t$ 为从出发开始的总时间，三个阶段写在一起：

$$p(t)= \begin{cases} \dfrac12t^2, & 0\le t\le2,\\[4pt] 2+2(t-2), & 2<t\le5,\\[4pt] 8+2(t-5)-\dfrac12(t-5)^2, & 5<t\le7. \end{cases}$$

每一行都是状态传播公式代入该段的初始条件：

阶段	段初位置 $p_0$	段初速度 $v_0$	加速度 $a$	本段经过的时间
加速	0	0	1	$t$
匀速	2	2	0	$t-2$
减速	8	2	-1	$t-5$

有了 $p(t)$，同一条直线路径就有了明确的时间安排：任意时刻的位置都能查到，再求导就得到速度和加速度。

七、为什么要把速度也算进状态

同样在 8 米处，下面两种情况的后续运动完全不同：

情况	位置	速度	接下来能做什么
A	8 米	0 米/秒	可以继续静止，也可以重新加速
B	8 米	2 米/秒	按本例的减速能力，至少还要前进 2 米才能停下

只看位置无法区分它们，所以描述运动时要把位置和速度放在一起，写成状态 $x=(p,v)$。本例减速开始时的状态就是 $x=(8,2)$。

位置、速度、加速度之间的关系写成变化率：

$$\dot p=v,\qquad \dot v=a.$$

加速度作为输入，积分一次得到速度，再积分一次得到位置，所以这个模型叫双积分器模型。前面第 5 到 5.5 秒的计算，就是它的一次状态传播：

初始状态 (8 米, 2 米/秒)  +  输入 -1 米/秒²，持续 0.5 秒  →  新状态 (8.875 米, 1.5 米/秒)

几何路径只规定经过哪些位置；状态和输入决定两个时刻之间怎样连接。后面在状态空间中搜索运动（比如 Kinodynamic A*）时，用的就是这个计算。

八、如果只走 3 米

把终点改成 3 米，其他条件不变。加速到 2 米/秒要 2 米，再减速停下又要 2 米，合计 4 米，已经超过 3 米，所以根本达不到速度上限。

这时的安排是前半段加速、后半段减速，两段对称，各走 1.5 米。由 $\frac12t_a^2=1.5$ 得到加速时间：

$$t_a=\sqrt3\approx1.73\text{ 秒}.$$

此时的最高速度为 $v_{\mathrm{peak}}=\sqrt3\approx1.73\,\mathrm{m/s}$，再用相同时间减速，总时间 $T=2\sqrt3\approx3.46$ 秒。

路程	加速时间	匀速时间	减速时间	最高速度	总时间
10 米	2 秒	3 秒	2 秒	2 米/秒	7 秒
3 米	约 1.73 秒	0	约 1.73 秒	约 1.73 米/秒	约 3.46 秒

3 米的速度曲线只有上升和下降，形成三角形速度曲线。最高速度由可用距离和加减速能力共同决定，不一定能达到速度上限。

九、衔接处还不够平滑

回到 10 米的轨迹。第 2 秒和第 5 秒两个交界处，前后两段的位置和速度都能接上（第 2 秒都是 2 米、2 米/秒；第 5 秒都是 8 米、2 米/秒），所以位置和速度是连续的。

加速度却在交界处突变：第 2 秒从 $+1$ 跳到 0，第 5 秒从 0 跳到 $-1\,\mathrm{m/s^2}$，起步和停止时也是从 0 直接跳到 $+1$、从 $-1$ 直接跳回 0。真实无人机的推力和姿态无法瞬间改变，这种突变会让跟踪变差。

改进的办法是让加速度也逐渐过渡。比如用 0.5 秒把加速度从 0 增加到 $1\,\mathrm{m/s^2}$，这段时间加速度的平均变化率为：

$$\frac{1-0}{0.5}=2\,\mathrm{m/s^3}.$$

加速度对时间的变化率称为 Jerk（加加速度）。一旦要求加速度也平滑过渡，各段的速度、距离和时间都要重新计算，同时还要满足总路程和终点静止。手工逐段计算会越来越繁琐，这正是多项式轨迹要解决的问题。

小结

• 路径规定经过哪里，轨迹进一步规定什么时候到哪里；
• 一段运动由初始位置、初始速度、加速度和持续时间决定，用状态传播公式计算；
• 边界条件和速度、加速度约束，限定了哪些运动安排是允许的；
• 梯形速度曲线的位置和速度连续，但加速度会突变。

下一篇《多项式轨迹》换一种思路：直接给定起终点的位置、速度和加速度，求出一条在整个时间区间内连续变化的曲线。