仅供参考。 建议看时有一定A*基础 主要示例 根据坐标推一次A*算法常见层级核心思想优点缺点适合场景A*全局规划Dijkstra 启发函数快、常用、路径可靠栅格路径可能不平滑室内 AMR 全局路径核心公式f(n) g(n) h(n)g(n)从起点走到当前点 n已经花费的真实代价h(n)从当前点 n 到终点的预估代价f(n)总代价评分越小越优先探索g已经走了多远h估计还要走多远f这条路线看起来总共要多远如果h 0A* 就退化成 Dijkstra。曼哈顿距离h |当前x - 目标x| |当前y - 目标y|h |3 - 3| |2 - 7| 5open、closed、parentopenopen是待探索的候选点集合。可以理解为所有已经发现、但还没正式展开的边界点。A* 每一轮都会从open里选f最小的点作为当前点。closedclosed是已经探索过的点集合。一个点进入closed后表示它已经被展开过通常不再重复处理。parentparent用来记录路径。算法流程1. 把起点 S 加入 open2. 从 open 中选出 f 最小的点作为 current3. 如果 current 是终点 G结束搜索4. 把 current 从 open 移到 closed5. 检查 current 的上下左右邻居6. 如果邻居是障碍物跳过7. 如果邻居已经在 closed 中通常跳过8. 计算邻居的新 g、h、f9. 如果邻居不在 open 中加入 open并记录 parent10. 如果邻居已经在 open 中但这次路线更短更新 g、f、parent11. 重复第 2 步12. 找到终点后根据 parent 反向追踪路径open {S} closed {} g[S] 0 parent[S] none while open 不为空: current open 中 f 最小的点 if current G: return 根据 parent 反向追踪出的路径 从 open 移除 current 把 current 加入 closed for neighbor in current 的邻居: if neighbor 是障碍物: continue if neighbor 在 closed 中: continue new_g g[current] 移动代价 if neighbor 不在 open 中: parent[neighbor] current g[neighbor] new_g h[neighbor] 曼哈顿距离(neighbor, G) f[neighbor] g[neighbor] h[neighbor] 把 neighbor 加入 open else if new_g g[neighbor]: parent[neighbor] current g[neighbor] new_g f[neighbor] g[neighbor] h[neighbor]推导示例地图y0 . . . S . . . .y1 . . . . . . . .y2 . . # . . # . .y3 . . # . . # . .y4 . . # . . # . .y5 . . # # # # . .y6 . . . . . . . .y7 . . . G . . . .其中S是起点 G是终点 #是障碍物每一轮都从 open 里选 f (代价)最小的点(3,0) - (3,1) - (3,2) - (3,3) - (3,4) 不会回头 从open里找最小的点Jump Point Search对一次jump来说确实可以理解为给定起始点 跳跃方向然后沿这个方向一直检测直到遇到终点、障碍/边界、强制邻居。从 open 里取 f 最小的点根据来时方向剪枝邻居方向对保留下来的方向分别 jumpjump 找到跳点后加入 open跳点跳点可以理解为必须停下来做决策的关键格子。常见跳点来源1. 当前点是终点 2. 当前点有强制邻居 3. 斜向跳跃时横向或纵向子方向能找到跳点或终点(空旷环境下没有障碍物 咳咳)如果沿某个方向遇到障碍或边界通常表示这个方向失败返回nulljump(当前点, 方向): 沿方向走一步 如果越界或遇到障碍: 这里注意啦 方向上有障碍物的不算喔 return null 如果当前点是终点: return 当前点 如果当前点有强制邻居: return 当前点 如果当前方向是斜向: 如果横向 jump 能找到跳点: return 当前点 如果纵向 jump 能找到跳点: return 当前点 继续沿同方向 jumpHybrid AHybrid A A搜索框架 车辆运动学模型 姿态离散化 碰撞检测 启发函数加速。**普通 A* 的状态通常是二维位置它假设机器人可以从一个格子直接走到相邻格子比如上、下、左、右、斜向。但汽车不行。汽车有几个限制不能横向平移不能原地旋转有最小转弯半径车头朝向会影响下一步能去哪所以对于汽车来说只知道(x, y)不够还必须知道朝向Hybrid A* 的“混合”主要体现在连续运动 离散搜索普通 A* 扩展邻居上、下、左、右、左上、右上...Hybrid A* 扩展的是车辆可执行动作例如前进左转前进直行前进右转后退左转后退直行后退右转Hybrid A* 的核心是f g hHybrid A* 的g往往不只是路径长度。它通常会惩罚一些“不舒服”或者“不合理”的驾驶行为倒车惩罚频繁换挡惩罚大转角惩罚方向盘快速变化惩罚靠近障碍物惩罚障碍物检测普通 A* 通常检查某个格子是不是障碍物。Hybrid A* 不能只检查车辆中心点因为车有长宽。它要检查整个车辆矩形在当前姿态下是否和障碍物重叠。也就是说每个状态 (x, y, theta)都对应一个旋转后的车辆轮廓。路径上一小段运动也要检查中间过程是否碰撞。所以 Hybrid A* 的碰撞检测通常比搜索本身还耗时。流程1. 从起点姿态开始2. 根据车辆模型生成若干可行动作3. 对每个动作形成一个新姿态4. 检查新姿态和中间轨迹是否碰撞5. 计算 g、h、f6. 选择 f 最小的节点继续扩展7. 接近目标时尝试 Reeds-Shepp 直接连接8. 找到路径后回溯节点得到轨迹9. 再进行平滑优化所以 Hybrid A* 特别适合自动泊车无人车低速规划仓储 AMR 带拖挂或类车体约束狭窄空间掉头非完整约束机器人路径规划但它不适合追求全局大范围高速规划。