一架无人机要沿通道前进 4 米，从静止出发，在终点停下。速度上限为 2 米/秒，加速度大小上限为 1 米/秒²。

这次让搜索算法决定运动安排：下一秒加速、匀速还是减速？经过什么状态，才能尽快到达终点并停止？

Kinodynamic A* 将运动状态作为节点，用运动模型生成连接，再按照 A* 的规则比较候选方案。

下面构造一个以到达时间为代价的入门算例，完整算出搜索过程，再把一次扩展放到二维障碍环境中。

一、一个节点，同时记录位置和速度

1. 把任务写清楚

用沿通道的坐标 pp 表示位置，用 vv 表示向前速度，把它们组成一个状态：

X=(p,v).X=(p,v).

例如，X=(2,1)X=(2,1) 表示位于 2 米处，正以 1 米/秒向前运动。

本例采用双积分器模型：

p˙=v,v˙=u.\dot p=v,\qquad \dot v=u.

uu 是规划器选择的加速度。它先改变速度，再通过速度改变位置。

项目 本例设置
起始状态 S=(0,0)S=(0,0)：位于 0 米处，速度为 0
目标状态 G=(4,0)G=(4,0)：位于 4 米处，速度为 0
允许位置 0≤p≤40\le p\le4，整段直线通道已经通过几何检查
允许速度 0≤v≤2 m/s0\le v\le2\,\mathrm{m/s}，始终向前或静止
可选加速度 u∈{+1,0,−1} m/s2u\in\{+1,0,-1\}\,\mathrm{m/s^2}
每次动作持续时间 Δt=1\Delta t=1 秒
比较目标 从 S 到 G 的总时间最小

这里的允许位置是计入机体尺寸后的中心点范围。环境静止，运动限制在搜索期间保持不变。

三个加速度分别表示加速、保持当前速度和减速。每选一次加速度，就产生一段持续 1 秒的候选运动。

2. 同一个位置，为什么要保留不同节点？

在 2 米处，考虑两个状态：

状态 运动情况 接下来可能怎样运动
(2,0)(2,0) 静止 可以保持静止，或者重新加速
(2,2)(2,2) 以 2 米/秒向前运动 即使立即以最大减速度制动，也需要继续前进

对于第二个状态，保持加速度 −1 m/s2-1\,\mathrm{m/s^2}，需要 2 秒才能停下。这两秒的平均速度为 1 米/秒，因此还要前进 2 米。

两个状态的后续运动不同，搜索时就为它们分别保存记录。

本例用完整的 (p,v)(p,v) 区分节点。到达位置相同、速度不同的两个候选，都有各自的到达代价与前驱。

二、一条边，是一段计算出来的运动

1. 从当前状态计算下一秒

设当前状态为 (p0,v0)(p_0,v_0)，选择加速度 uu，在这 1 秒内保持不变。

用 τ\tau 表示本段开始后经过的时间，0≤τ≤10\le\tau\le1。这段运动满足：

p(τ)=p0+v0τ+12uτ2,v(τ)=v0+uτ.\boxed{ \begin{aligned} p(\tau)&=p_0+v_0\tau+\frac12u\tau^2,\\ v(\tau)&=v_0+u\tau. \end{aligned} }

到第 1 秒结束时，得到下一个节点：

Xnew=(p0+v0+12u, v0+u).\boxed{ X_{\mathrm{new}}= \left(p_0+v_0+\frac12u,\ v_0+u\right). }

最后这个数值式采用秒、米为单位，已经代入了 Δt=1\Delta t=1 秒。

节点记录一段运动的终点状态，边保存整个时间区间内的位置与速度变化。 这样的短时运动称为运动基元。

2. 从同一个状态生成三个候选

假设当前位于：

X=(0.5,1).X=(0.5,1).

选择加速度 u=+1u=+1，计算：

pnew=0.5+1×1+12×1×12=2,p_{\mathrm{new}}=0.5+1\times1+\frac12\times1\times1^2=2,

vnew=1+1×1=2.v_{\mathrm{new}}=1+1\times1=2.

得到状态 (2,2)(2,2)。

对另外两个加速度执行同样计算：

加速度 uu（米/秒²） 1 秒后的位置（米） 1 秒后的速度（米/秒） 新状态
+1 0.5+1+0.5=20.5+1+0.5=2 1+1=21+1=2 (2,2)(2,2)
0 0.5+1=1.50.5+1=1.5 1+0=11+0=1 (1.5,1)(1.5,1)
-1 0.5+1−0.5=10.5+1-0.5=1 1−1=01-1=0 (1,0)(1,0)

从位置 0.5 米、速度 1 米每秒出发，三种加速度产生不同的位置曲线与终点速度

图的横轴是本段时间，纵轴是位置。三条曲线从同一状态出发，经过 1 秒后落到不同状态。

生成候选时还要检查运动限制。本例每段速度随时间线性变化；首尾速度都在 [0,2][0,2] 内，整段速度就保持在这个区间内。速度始终非负，因此位置单调增加，可以同时用首尾位置核验通道范围。

后面加入障碍物时，还需要检查整条运动曲线与障碍的关系。

三、用时间给候选状态排序

1. 已经用了多久：g(X)g(X)

沿用 A* 的记号，用 g(X)g(X) 记录目前找到的、从 S 到状态 X 的最小累计时间。

每条运动基元持续 1 秒，因此沿一条新边到达下一状态的候选时间为：

gnew=g(X)+1.g_{\mathrm{new}}=g(X)+1.

例如，2 秒到达某个状态，再接一段运动，就得到一条耗时 3 秒的到达方案。

找到更快到达同一状态的方案时，同时更新它的累计时间、前驱和连接所用的加速度。

2. 至少还要多久：h(X)h(X)

先分别估计两项需求。

第一项是完成剩余位移。 当前位置为 pp，距离终点还有 4−p4-p 米。速度上限为 2 米/秒，因此至少需要：

hp=4−p2.h_p=\frac{4-p}{2}.

例如，位于 0.5 米处时，还剩 3.5 米，至少需要 3.5/2=1.753.5/2=1.75 秒。

第二项是把速度降到零。 当前速度为 vv，最大减速度大小为 1 米/秒²，因此至少需要：

hv=v1.h_v=\frac{v}{1}.

例如，当前速度为 2 米/秒，减速到静止至少需要 2 秒。

减速的同时还会向前移动，这两项需求可以在同一段时间内完成。任何可行的剩余运动，都至少要满足其中较长的时间要求。因此本例取：

h(X)=max⁡(4−p2,v1).\boxed{h(X)=\max\left(\frac{4-p}{2},\frac{v}{1}\right)}.

max⁡\max 表示取两个数中较大的那个。两个比值的单位都是秒，式中的分母分别来自速度上限与加速度上限。

这只是一个剩余时间下界，具体运动仍由后续搜索确定。

3. 合起来比较：f=g+hf=g+h

A* 按照：

f(X)=g(X)+h(X)f(X)=g(X)+h(X)

安排候选的处理顺序。这里的 gg、hh、ff 全部以秒计。

例如，前面从 (0.5,1)(0.5,1) 产生的三个状态，都能在总计 2 秒时到达：

候选状态 已用时间 gg 剩余时间下界 hh f=g+hf=g+h
(2,2)(2,2) 2 max⁡(1,2)=2\max(1,2)=2 4
(1.5,1)(1.5,1) 2 max⁡(1.25,1)=1.25\max(1.25,1)=1.25 3.25
(1,0)(1,0) 2 max⁡(1.5,0)=1.5\max(1.5,0)=1.5 3.5

这次先处理 (1.5,1)(1.5,1)，因为它的 f=3.25f=3.25 最小。 到达更靠前的位置与保留怎样的速度，都会影响候选的评价。

为使手算过程确定，本例规定：先选 ff 较小的；ff 相同时选 hh 较小的；两者都相同时，按进入候选清单的先后顺序处理。每次按 +1+1、0、−1-1 的顺序尝试加速度。

四、跟着搜索，算到终点

1. 第一轮：从静止起点出发

候选清单中最初只有起点 S=(0,0)S=(0,0)，g=0g=0、h=2h=2、f=2f=2。检查三个加速度：

加速度 1 秒后的状态 处理结果
+1 (0.5,1)(0.5,1) 合格，记录到达时间 1 秒
0 (0,0)(0,0) 回到同一状态，已有的 0 秒记录更好
-1 (−0.5,−1)(-0.5,-1) 超出本例的位置与速度范围

候选清单中只留下 (0.5,1)(0.5,1)。在静态问题中，多用 1 秒等待再到达同一状态，后续可选运动保持相同，所以保留原来的较早记录。

2. 第二轮：保留三种后续安排

取出 (0.5,1)(0.5,1)，它的到达时间为 1 秒。

按上一节的计算，加入 (2,2)(2,2)、(1.5,1)(1.5,1)、(1,0)(1,0) 三个候选，ff 分别为 4、3.25、3.5。它们的前驱都是 (0.5,1)(0.5,1)，对应加速度分别为 +1+1、0、−1-1。

下一轮取出 (1.5,1)(1.5,1)。另两个候选继续留在清单中。

3. 第三轮：从匀速候选继续扩展

到达 (1.5,1)(1.5,1) 已经用了 2 秒。分别尝试：

加速度 新状态 新的累计时间 新的 ff
+1 (3,2)(3,2) 3 秒 3+2=53+2=5
0 (2.5,1)(2.5,1) 3 秒 3+1=43+1=4
-1 (2,0)(2,0) 3 秒 3+1=43+1=4

例如，加速度为 −1-1 时，位置为 1.5+1−0.5=21.5+1-0.5=2，速度降到 0，得到 (2,0)(2,0)。

此时完整的候选清单按优先级排好为：

顺序 状态 gg hh ff
1 (1,0)(1,0) 2 1.5 3.5
2 (2.5,1)(2.5,1) 3 1 4
3 (2,0)(2,0) 3 1 4
4 (2,2)(2,2) 2 2 4
5 (3,2)(3,2) 3 2 5

其中，(2,0)(2,0) 和 (2,2)(2,2) 同时保留。它们都位于 2 米处，后续运动能力各不相同。

4. 第四至六轮：比较分支，记录更远的候选

先处理 (1,0)(1,0)。选择 +1+1 后，用总计 3 秒到达 (1.5,1)(1.5,1)，而这个状态已有 2 秒的记录，因此保留原记录。选择 0 会增加等待时间，选择 −1-1 则产生负速度。本轮没有新增候选。

随后取出 (2.5,1)(2.5,1)，此时 g=3g=3。三个动作产生：

加速度 新状态 到达时间 处理
+1 (4,2)(4,2) 4 秒 加入候选，位置达到终点坐标，速度仍为 2
0 (3.5,1)(3.5,1) 4 秒 加入候选
-1 (3,0)(3,0) 4 秒 加入候选

任务的目标是 (4,0)(4,0)。状态 (4,2)(4,2) 表示到达 4 米处时仍在向前运动，因此搜索继续。

第六轮取出 (2,0)(2,0)。重新加速会用总计 4 秒到达 (2.5,1)(2.5,1)，已有记录为 3 秒，其余两个动作也没有带来更优候选。

这三轮比较后，清单中轮到 (2,2)(2,2)。它一直保留着总计 2 秒的到达方案。

5. 第七轮：另一条分支带来更早到达的方案

现在从 (2,2)(2,2) 出发，已经用了 2 秒。

选择加速度 +1+1，下一秒速度会达到 3，超过上限，排除。

选择 0：

pnew=2+2=4,vnew=2.p_{\mathrm{new}}=2+2=4,\qquad v_{\mathrm{new}}=2.

到达 (4,2)(4,2) 的时间为 3 秒，优于原来的 4 秒，因此更新它的记录。

选择 −1-1：

pnew=2+2−0.5=3.5,vnew=1.p_{\mathrm{new}}=2+2-0.5=3.5,\qquad v_{\mathrm{new}}=1.

到达 (3.5,1)(3.5,1) 的时间也为 3 秒，比原来的记录更早：

方案 经过的状态 用时
旧 (0,0)→(0.5,1)→(1.5,1)→(2.5,1)→(3.5,1)(0,0)\to(0.5,1)\to(1.5,1)\to(2.5,1)\to(3.5,1) 4 秒
新 (0,0)→(0.5,1)→(2,2)→(3.5,1)(0,0)\to(0.5,1)\to(2,2)\to(3.5,1) 3 秒

于是把 (3.5,1)(3.5,1) 的累计时间从 4 秒改为 3 秒，前驱从 (2.5,1)(2.5,1) 改为 (2,2)(2,2)，最后一段加速度从 0 改为 −1 m/s2-1\,\mathrm{m/s^2}，优先级变为 f=3+1=4f=3+1=4。

更新的对象是到达同一位置、同一速度的方案。前驱改变后，最后一段运动的输入也随之改变。

6. 第八轮：用减速段接到静止终点

取出 (3.5,1)(3.5,1)，现在只用了 3 秒。

选择 +1+1 或 0，会分别到达 5 米或 4.5 米，超出本例的位置范围。

选择 −1-1：

pnew=3.5+1−0.5=4,vnew=1−1=0.p_{\mathrm{new}}=3.5+1-0.5=4, \qquad v_{\mathrm{new}}=1-1=0.

这次得到完整目标状态：

G=(4,0),g(G)=4,h(G)=0,f(G)=4.G=(4,0),\qquad g(G)=4,\qquad h(G)=0,\qquad f(G)=4.

它成为优先级最高的候选。第九轮取出 G，结束搜索。

完整的取出顺序如下：

轮次 取出状态 (p,v)(p,v) gg（秒） hh（秒） ff（秒）
1 (0,0)(0,0) 0 2 2
2 (0.5,1)(0.5,1) 1 1.75 2.75
3 (1.5,1)(1.5,1) 2 1.25 3.25
4 (1,0)(1,0) 2 1.5 3.5
5 (2.5,1)(2.5,1) 3 1 4
6 (2,0)(2,0) 3 1 4
7 (2,2)(2,2) 2 2 4
8 (3.5,1)(3.5,1) 3 1 4
9 (4,0)(4,0) 4 0 4

表中的轮次是搜索处理候选的顺序，gg 是相应运动方案的累计时间。处理不同分支时，累计时间可以从 3 回到 2；实际执行的运动顺序随后由前驱回溯确定。

五、沿前驱，取出一条带时间的运动

1. 回溯节点，也回溯每条边的输入

从 G 逐个查前驱，反向整理后得到 (0,0)→(0.5,1)→(2,2)→(3.5,1)→(4,0)(0,0)\to(0.5,1)\to(2,2)\to(3.5,1)\to(4,0)，对应的运动安排为：

时间区间（秒） 加速度（米/秒²） 起始状态 结束状态
0～1 +1 (0,0)(0,0) (0.5,1)(0.5,1)
1～2 +1 (0.5,1)(0.5,1) (2,2)(2,2)
2～3 -1 (2,2)(2,2) (3.5,1)(3.5,1)
3～4 -1 (3.5,1)(3.5,1) (4,0)(4,0)

合并相同输入的相邻段，就是先加速 2 秒，再减速 2 秒。

搜索中的候选状态与最终运动：横轴是位置，纵轴是速度，同在 2 米处的两个状态分别位于不同高度

图中的一对坐标表示位置和速度。最终运动沿曲线先到达 (2,2)(2,2)，再接到 (4,0)(4,0)。时间标记表示执行时经过各状态的先后。

2. 还原任意时刻的位置与速度

前两秒从静止加速：

p(t)=12t2,v(t)=t,0≤t≤2.p(t)=\frac12t^2,\qquad v(t)=t,\qquad 0\le t\le2.

后两秒从位置 2、速度 2 开始减速。令 τ=t−2\tau=t-2：

p(t)=2+2τ−12τ2,v(t)=2−τ,2≤t≤4.p(t)=2+2\tau-\frac12\tau^2,\qquad v(t)=2-\tau,\qquad 2\le t\le4.

例如，第 2.5 秒时，本段经过 τ=0.5\tau=0.5 秒：

p(2.5)=2+2×0.5−12×0.52=2.875 米,p(2.5)=2+2\times0.5-\frac12\times0.5^2=2.875\text{ 米},

v(2.5)=2−0.5=1.5 m/s.v(2.5)=2-0.5=1.5\,\mathrm{m/s}.

最终运动的速度曲线：前两秒从零增加到每秒两米，后两秒减速到零，总面积为四米

速度曲线下的两个三角形面积分别为 2 米，合计 4 米。整段速度在 0～2 米/秒之间，加速度大小为 1 米/秒²，起终状态也符合要求。

3. 这里的“最短时间”对应什么？

本例的节点由指定输入和 1 秒时长生成。可到达位置落在半米刻度上，速度取 0、1、2；结合通道边界，构成一个有限的离散运动图。

前面构造的 hh 是剩余时间下界，而且满足一致性。沿任意合格的 1 秒运动，最多前进 2 米，所以距离下界最多下降 1 秒；速度最多减少 1 米/秒，所以减速下界也最多下降 1 秒。取两者的较大值后，仍有：

h(X)≤1+h(Xnew).h(X)\le1+h(X_{\mathrm{new}}).

再结合 h(G)=0h(G)=0、正的边代价和正确的记录更新，目标以最小有效 ff 被取出时，得到这个离散运动图中的最短时间。

改变可选加速度、动作时长或状态保留规则，就改变了搜索能够比较的运动集合。

六、扩展到二维：连接可能是一条弯曲的运动

1. 水平面内，同时传播两个方向

在固定高度的水平面内，用：

X=(x,y,vx,vy)X=(x,y,v_x,v_y)

记录状态，输入为加速度 (ux,uy)(u_x,u_y)。本段运动满足：

x(τ)=x0+vx0τ+12uxτ2,y(τ)=y0+vy0τ+12uyτ2.\begin{aligned} x(\tau)&=x_0+v_{x0}\tau+\frac12u_x\tau^2,\\ y(\tau)&=y_0+v_{y0}\tau+\frac12u_y\tau^2. \end{aligned}

例如，当前位于 (2,2)(2,2)，速度为 (1,0)(1,0)：正在沿 +x+x 方向以 1 米/秒运动。

这一秒选择输入 (ux,uy)=(0,1)(u_x,u_y)=(0,1)，得到：

x(τ)=2+τ,y(τ)=2+12τ2.\boxed{x(\tau)=2+\tau,\qquad y(\tau)=2+\frac12\tau^2}.

横向保持匀速，侧向逐渐加速。计算几个时刻：

本段时间 τ\tau（秒） 位置 (x,y)(x,y)（米） 速度 (vx,vy)(v_x,v_y)（米/秒）
0 (2,2)(2,2) (1,0)(1,0)
0.5 (2.5,2.125)(2.5,2.125) (1,0.5)(1,0.5)
1 (3,2.5)(3,2.5) (1,1)(1,1)

速度大小为 1+τ2\sqrt{1+\tau^2}，全段不超过 2\sqrt2 米/秒；加速度大小为 1 米/秒²。因此，这段候选满足速度上限 2、加速度上限 1 的要求。

2. 用实际运动曲线检查障碍

假设地图上有一小块禁行区域，已经计入机体尺寸：

2.4≤x≤2.6,2.05≤y≤2.18.2.4\le x\le2.6,\qquad 2.05\le y\le2.18.

这段候选的两个端点都位于区域外，但在半秒时：

(x,y)=(2.5,2.125),(x,y)=(2.5,2.125),

正好落在区域内部。因此整段候选被碰撞检查拒绝。

实际运动曲线穿过禁行区域，直接连接相同端点的直线从区域上方经过

图中的端点连线是另一条几何曲线。它满足：

y=2+12(x−2).y=2+\frac12(x-2).

经过障碍的横向范围 x∈[2.4,2.6]x\in[2.4,2.6] 时，这条直线的 yy 在 2.2～2.3 之间，位于障碍上方；实际运动则在同一区域内更低，发生碰撞。

动力学搜索检查的是输入与初始状态共同产生的运动曲线。 候选边被接受后，保存的也是这段运动。

本例一个中间时刻就足以证明碰撞。判断整段无碰撞时，需要验证整个区间，例如计算曲线与几何障碍的相交关系，或者采用带保守界的曲线细分。

七、把计算整理成搜索流程

本篇的一维算例可以整理成：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
检查起终状态与运动参数

g[S] = 0
其他状态的 g = ∞
将 S 放入 OPEN，优先级为 g[S] + h(S)

只要 OPEN 非空：
    取出 f 最小的记录，得到状态 X
    如果记录中的 g 已经过期：
        跳过这条记录

    如果 X 满足目标位置和目标速度：
        沿前驱回溯，同时取出每段输入与持续时间
        返回完整运动

    依次尝试 u = +1、0、-1：
        从 X 出发，生成持续 1 秒的运动基元
        计算末端状态 Y

        检查整段运动的速度、位置范围和碰撞关系
        如果不合格：
            跳过这个候选

        new_g = g[X] + 1
        如果 new_g < g[Y]：
            更新 g[Y] = new_g
            更新 Y 的前驱为 X
            保存本段输入 u 和持续时间 1 秒
            将 Y 加入或更新到 OPEN
            优先级为 g[Y] + h(Y)

返回：当前离散运动图中没有找到符合要求的目标

每个状态的记录需要包含位置、速度、累计时间、前驱，以及接入它的运动参数。这里的前驱连接还携带加速度与时间，因此回溯时能够恢复连续运动。

本例用精确的半米位置和整数速度区分状态。更一般的搜索可以对位置、速度设置分辨率；把多个不同状态归入同一格并只保留一个代表时，代表状态的选择会影响后续可搜索的运动。

当前终点 (4,0)(4,0) 正好可以由这些运动基元到达。对于其他目标，还可以从接近终点的状态构造一段满足起终位置、速度的多项式连接，再验证整段约束。这个末端连接把多项式轨迹生成与状态空间搜索联系起来。

配套文件 kinodynamic_astar_example.py 使用 Python 标准库复算本篇的九轮搜索。运行：

1
python kinodynamic_astar_example.py

程序输出各轮状态、候选处理结果、前驱回溯和四段加速度，还会用 Dijkstra 核对最终时间。

八、搜索结果怎样交给后续轨迹规划？

本篇得到的是双积分器模型下的分段恒加速度运动。它已经包含时间、位置、速度和输入，各段的位置与速度连续。

在第 2 秒，加速度从 +1+1 切换到 −1 m/s2-1\,\mathrm{m/s^2}。把它代入等高、忽略阻力的多旋翼模型，令重力加速度大小为 g0=9.8 m/s2g_0=9.8\,\mathrm{m/s^2}，所需倾角为：

θ=arctan⁡(ug0).\theta=\arctan\left(\frac{u}{g_0}\right).

这个切换对应参考倾角从约 +5.83∘+5.83^\circ 跳到 −5.83∘-5.83^\circ。真实机体完成转动需要时间，因此后续轨迹设计还要安排姿态的连续变化，并检查角速度、推力和执行器要求。

可以把搜索结果作为初始运动，用分段多项式或样条重新安排加速度与更高阶导数。调整形状与时间后，再核验运动上限、起终条件和整段碰撞关系。

这样，搜索提供一条满足所用简化模型的运动候选，轨迹优化继续改善平滑性与障碍间距，动力学计算则检查相应的飞行需求。

小结

在这个 4 米算例中，搜索自动选出了 +1+1、+1+1、−1-1、−1-1 四段加速度，用 4 秒从静止出发并在终点停下。

状态决定当前能怎样运动，输入与时长生成候选连接，A* 比较到达这些状态的代价，前驱记录还原整段运动。

把位置扩展到二维、三维后，同一套过程可以用于绕障：生成运动曲线，检查约束，保留可行连接，再继续搜索。后续可以学习 Bézier 曲线与 B 样条，用控制点调整轨迹形状，并利用曲线的几何性质组织避障约束。