运动规划之搜索算法：前端规划、后端轨迹生成到状态求解( 九 )

2026-09-13 算法

在每次对角线步骤之前，我们首先递归直线。只有当两次直线递归都未能识别出跳点时，我们才再次对角线步进。

节点 w，x 的强制邻居，正常扩展。（也推入开放列表，优先队列）。
记住:你只能直线跳跃或对角线跳跃;不能分段跳跃。:::success

维护一个优先队列来存储所有待扩展的节点

对所有节点预先定义启发函数h(n)

用起始状态XS初始化优先队列

设置g(XS)=0,对图中的其他节点设置g(n)=无穷

循环:

如果队列为空,返回FALSE并退出循环

从队列中取出f(n)=g(n)+h(n)最小的节点“n”

将节点“n”标记为已扩展

如果节点“n”是目标状态,返回TRUE并退出循环

对节点“n”的所有未扩展邻居节点“m”:

如果g(m)=无穷

g(m)= g(n) + Cnm

将节点“m”加入队列

如果g(m)>g(n)+Cnm

g(m)= g(n) + Cnm

结束对邻居节点的循环

结束主循环 ::: openlist查找具体流程如下²：

初始化起点节点 start ，将起点周围四个角落的空闲节点相对于起点的相对位置加入起点节点的 forced_neighbor_list 。

创建一个 openlist ，将 start 加入 openlist 。

while openlist is not empty:

node ← openlist.Pop ()

从 node 开始跳跃，首先进行直线跳跃，再进行对角线跳跃。

用 parent 表示从 node 进行对角线跳跃得到的节点，用 current 表示从 parent 进行直线跳跃得到的节点。

如果 current 是跳点，而 parent 与 node 是同一个节点，则将 current 加入 openlist ，同时将 current 的父节点指向 node；

如果 current 是跳点，而 parent 与 node 不是同一个节点，则将 parent 和 current 加入 openlist，同时将 current 的父节点指向 parent，将 parent 的父节点指向 node；

如果 current 是障碍物或者边界，则进行对角线跳跃；

如果 parent 是障碍物或者边界，则进入下一轮循环。

例子：

运动规划之搜索算法：前端规划、后端轨迹生成到状态求解

文章插图

扩展—>对角线移动
最终找到一个关键节点,将其加入开放列表。
从开放列表中弹出它(唯一的节点) 。
垂直扩展,在障碍物处结束。

运动规划之搜索算法：前端规划、后端轨迹生成到状态求解

文章插图

水平扩展,遇到一个具有强制邻居的节点。
将其添加到开放列表。

运动规划之搜索算法：前端规划、后端轨迹生成到状态求解

文章插图

对角线扩展,扩展后没有发现任何新的节点。
完成当前节点的扩展。

运动规划之搜索算法：前端规划、后端轨迹生成到状态求解

文章插图

对角线移动。
先沿垂直和水平方向扩展。

运动规划之搜索算法：前端规划、后端轨迹生成到状态求解

文章插图

没有发现任何新的节点。
对角线移动。

更详细跳点搜索可以参考下面文章：
https://blog.csdn.net/LIQIANGEASTSUN/article/details/118766080
小结：本文介绍了motion plan学院派的框架：

上一篇：TensorRT是如何做到架构更快的？

下一篇：Java网络代理：探索代理服务器与虚拟网络技术