上一篇概述了运动规划的三大分类。今天开始深入图搜索——这是最经典的全局规划方法也是A*、Dijkstra等算法的基础。图搜索的核心思想就一句话把连续的空间切成格子把规划问题变成在格子上找路的问题。就像你手机上的导航地图——地球表面是连续的但导航系统把它离散化成路网然后在这个图上搜索最短路径。机器人规划也是同样的道理。一、为什么要离散化机器人的运动空间是连续的——关节角度可以取任意实数值末端位置也可以在三维空间里任意移动。连续空间理论上包含无穷多个点计算机没法处理无穷多个元素。离散化就是把连续空间切成有限个格子或者叫节点。每个格子代表一个离散状态格子之间的连接代表状态转移。# 2D网格离散化示例 # 空间范围 [0, 10] x [0, 10] # 分辨率 1.0 → 10x10 100个格子 resolution 1.0 grid_size (10, 10) # 每个格子用 (i, j) 索引 # 格子 (3, 5) 对应物理坐标 (3.5, 5.5)离散化之后原本在连续空间找一条无碰撞曲线的问题变成了在图上找一条从起点节点到终点节点的路径。后者是经典的图论问题有成熟的算法可以解决。二、图的构建方式离散化有几种常见方式1. 规则网格Grid最直观的方式。把空间切成均匀的正方形2D或立方体3D格子。# 2D网格的邻居连接 # 4连接上下左右 neighbors_4 [(-1,0), (1,0), (0,-1), (0,1)] # 8连接加上对角线 neighbors_8 [(-1,-1), (-1,0), (-1,1), (0,-1), (0,1), (1,-1), (1,0), (1,1)]优点简单、规则、容易实现。 缺点维度灾难。3D空间分辨率0.1m10m×10m×3m的房间就有300万个格子。6D构型空间想都别想。2. 可见图Visibility Graph只在障碍物的顶点之间连线。只有两个顶点之间看得见连线不穿过障碍物时才建立连接。优点节点数少路径贴着障碍物走通常是最短路径。 缺点只适合2D多边形障碍物3D场景很难用。路径贴障碍物太近实际执行不安全。3. Voronoi图把空间按离障碍物最远的原则划分。Voronoi图的边是离最近障碍物等距的点的集合。优点路径离障碍物最远安全裕度最大。在移动机器人导航中特别实用。 缺点路径通常不是最短的绕来绕去。计算Voronoi图在3D场景下比较慢高维空间几乎不可行。4. 路标图Waypoint Graph人工或者自动在空间中放置一些路标点路标之间建立连接。优点灵活可以针对具体场景优化。工厂里的固定路线导航常用这种方式。 缺点需要人工设计覆盖率不保证。环境变化后需要重新设计路标。三、图搜索的基本框架不管用哪种离散化方式图搜索的基本框架是一样的def graph_search(graph, start, goal): open_set {start} # 待探索的节点 closed_set set() # 已探索的节点 came_from {} # 路径回溯 while open_set: # 从open_set中选一个最好的节点 current select_best(open_set) if current goal: return reconstruct_path(came_from, current) open_set.remove(current) closed_set.add(current) for neighbor in graph.neighbors(current): if neighbor in closed_set: continue if neighbor in open_set: # 检查是否找到了更好的路径 update_if_better(came_from, neighbor, current) else: open_set.add(neighbor) came_from[neighbor] current return None # 无解这个框架的关键在于select_best——从open_set里选哪个节点来扩展。不同的选择策略就是不同的算法选g(n)最小的 → Dijkstra算法选f(n) g(n) h(n)最小的 → A*算法选h(n)最小的 → 贪心最佳优先搜索四、代价函数和启发式图搜索有两个核心概念g(n)从起点到节点n的实际代价。Dijkstra算法只关心这个值——选g(n)最小的节点扩展保证找到最短路径。h(n)从节点n到终点的估计代价启发式函数。A*算法用h(n)来引导搜索方向——优先探索看起来离终点近的节点。# 启发式函数示例2D网格 def manhattan_distance(a, b): 曼哈顿距离只能走上下左右 return abs(a[0]-b[0]) abs(a[1]-b[1]) def euclidean_distance(a, b): 欧几里得距离可以走对角线 return ((a[0]-b[0])**2 (a[1]-b[1])**2) ** 0.5启发式函数的选择直接影响算法行为。h(n)0时A*退化成Dijkstra。h(n)越接近真实代价搜索越快。但h(n)不能超过真实代价可采纳性否则可能找不到最优解。五、离散化的代价离散化不是免费的有几个坑需要注意分辨率 vs 计算量分辨率越高路径质量越好但节点数指数增长。工程上需要在路径质量和计算时间之间权衡。离散化误差离散化后的路径只能沿网格方向走会出现锯齿形路径。解决方法后处理平滑或者用更高阶的连接方式比如8连接比4连接好。拓扑信息丢失离散化可能丢失空间的拓扑结构。比如两个障碍物之间的狭窄通道如果分辨率太粗通道可能被填满导致找不到本来存在的路径。这就是所谓的狭窄通道问题——采样规划也面临同样的挑战。维度灾难这是离散化最大的问题。构型空间每增加一维节点数就乘以分辨率的倒数。6轴机械臂每轴分辨率1度就有360^6 ≈ 2万亿个节点。根本不可能用网格搜索。这也是为什么高维规划必须用采样方法——RRT、PRM这些算法不依赖网格在高维空间依然可行。六、面试实战Q图搜索算法的时间复杂度是多少A最坏情况O(VE)V是节点数E是边数。因为每个节点最多被访问一次每条边最多被检查一次。A*在最坏情况下也是O(VE)但好的启发式函数可以大幅减少实际访问的节点数。Q4连接和8连接有什么区别A4连接只能走上下左右路径是之字形的路径长度偏大。8连接多了对角线路径更平滑更接近真实最短路径。但8连接的对角线代价是sqrt(2)而不是1计算时注意不要搞错。工程上移动机器人常用8连接机械臂关节空间规划有时用更多连接比如16连接。Q为什么机械臂规划很少用网格搜索A因为维度太高。6轴机械臂的构型空间是6D的用网格离散化节点数爆炸。所以机械臂通常用采样规划RRT/PRM不用图搜索。图搜索适合2D或3D的低维空间。小结图搜索的核心把连续空间离散化成图然后用图搜索算法找路径。离散化方式有网格、可见图、Voronoi图、路标图等。不同的选择策略对应不同的算法——Dijkstra、A*、贪心搜索。启发式函数h(n)引导搜索方向决定了算法的效率。离散化最大的问题是维度灾难——高维空间用不了。这也是为什么高维规划要用采样方法RRT、PRM等。下一篇讲Dijkstra算法——最短路径的经典解法。如果这篇文章对你有帮助欢迎点赞、在看、转发三连。 你的支持是我持续更新的最大动力。「机器人软件开发面试·从入门到精通」连载系列上一篇第206篇 运动规划概述——全局/局部/反应式规划的分类下一篇预告第208篇 Dijkstra算法——最短路径的经典解法有任何问题欢迎评论区留言我会尽量回复。
第207篇 图搜索基础——状态空间离散化的思路
上一篇概述了运动规划的三大分类。今天开始深入图搜索——这是最经典的全局规划方法也是A*、Dijkstra等算法的基础。图搜索的核心思想就一句话把连续的空间切成格子把规划问题变成在格子上找路的问题。就像你手机上的导航地图——地球表面是连续的但导航系统把它离散化成路网然后在这个图上搜索最短路径。机器人规划也是同样的道理。一、为什么要离散化机器人的运动空间是连续的——关节角度可以取任意实数值末端位置也可以在三维空间里任意移动。连续空间理论上包含无穷多个点计算机没法处理无穷多个元素。离散化就是把连续空间切成有限个格子或者叫节点。每个格子代表一个离散状态格子之间的连接代表状态转移。# 2D网格离散化示例 # 空间范围 [0, 10] x [0, 10] # 分辨率 1.0 → 10x10 100个格子 resolution 1.0 grid_size (10, 10) # 每个格子用 (i, j) 索引 # 格子 (3, 5) 对应物理坐标 (3.5, 5.5)离散化之后原本在连续空间找一条无碰撞曲线的问题变成了在图上找一条从起点节点到终点节点的路径。后者是经典的图论问题有成熟的算法可以解决。二、图的构建方式离散化有几种常见方式1. 规则网格Grid最直观的方式。把空间切成均匀的正方形2D或立方体3D格子。# 2D网格的邻居连接 # 4连接上下左右 neighbors_4 [(-1,0), (1,0), (0,-1), (0,1)] # 8连接加上对角线 neighbors_8 [(-1,-1), (-1,0), (-1,1), (0,-1), (0,1), (1,-1), (1,0), (1,1)]优点简单、规则、容易实现。 缺点维度灾难。3D空间分辨率0.1m10m×10m×3m的房间就有300万个格子。6D构型空间想都别想。2. 可见图Visibility Graph只在障碍物的顶点之间连线。只有两个顶点之间看得见连线不穿过障碍物时才建立连接。优点节点数少路径贴着障碍物走通常是最短路径。 缺点只适合2D多边形障碍物3D场景很难用。路径贴障碍物太近实际执行不安全。3. Voronoi图把空间按离障碍物最远的原则划分。Voronoi图的边是离最近障碍物等距的点的集合。优点路径离障碍物最远安全裕度最大。在移动机器人导航中特别实用。 缺点路径通常不是最短的绕来绕去。计算Voronoi图在3D场景下比较慢高维空间几乎不可行。4. 路标图Waypoint Graph人工或者自动在空间中放置一些路标点路标之间建立连接。优点灵活可以针对具体场景优化。工厂里的固定路线导航常用这种方式。 缺点需要人工设计覆盖率不保证。环境变化后需要重新设计路标。三、图搜索的基本框架不管用哪种离散化方式图搜索的基本框架是一样的def graph_search(graph, start, goal): open_set {start} # 待探索的节点 closed_set set() # 已探索的节点 came_from {} # 路径回溯 while open_set: # 从open_set中选一个最好的节点 current select_best(open_set) if current goal: return reconstruct_path(came_from, current) open_set.remove(current) closed_set.add(current) for neighbor in graph.neighbors(current): if neighbor in closed_set: continue if neighbor in open_set: # 检查是否找到了更好的路径 update_if_better(came_from, neighbor, current) else: open_set.add(neighbor) came_from[neighbor] current return None # 无解这个框架的关键在于select_best——从open_set里选哪个节点来扩展。不同的选择策略就是不同的算法选g(n)最小的 → Dijkstra算法选f(n) g(n) h(n)最小的 → A*算法选h(n)最小的 → 贪心最佳优先搜索四、代价函数和启发式图搜索有两个核心概念g(n)从起点到节点n的实际代价。Dijkstra算法只关心这个值——选g(n)最小的节点扩展保证找到最短路径。h(n)从节点n到终点的估计代价启发式函数。A*算法用h(n)来引导搜索方向——优先探索看起来离终点近的节点。# 启发式函数示例2D网格 def manhattan_distance(a, b): 曼哈顿距离只能走上下左右 return abs(a[0]-b[0]) abs(a[1]-b[1]) def euclidean_distance(a, b): 欧几里得距离可以走对角线 return ((a[0]-b[0])**2 (a[1]-b[1])**2) ** 0.5启发式函数的选择直接影响算法行为。h(n)0时A*退化成Dijkstra。h(n)越接近真实代价搜索越快。但h(n)不能超过真实代价可采纳性否则可能找不到最优解。五、离散化的代价离散化不是免费的有几个坑需要注意分辨率 vs 计算量分辨率越高路径质量越好但节点数指数增长。工程上需要在路径质量和计算时间之间权衡。离散化误差离散化后的路径只能沿网格方向走会出现锯齿形路径。解决方法后处理平滑或者用更高阶的连接方式比如8连接比4连接好。拓扑信息丢失离散化可能丢失空间的拓扑结构。比如两个障碍物之间的狭窄通道如果分辨率太粗通道可能被填满导致找不到本来存在的路径。这就是所谓的狭窄通道问题——采样规划也面临同样的挑战。维度灾难这是离散化最大的问题。构型空间每增加一维节点数就乘以分辨率的倒数。6轴机械臂每轴分辨率1度就有360^6 ≈ 2万亿个节点。根本不可能用网格搜索。这也是为什么高维规划必须用采样方法——RRT、PRM这些算法不依赖网格在高维空间依然可行。六、面试实战Q图搜索算法的时间复杂度是多少A最坏情况O(VE)V是节点数E是边数。因为每个节点最多被访问一次每条边最多被检查一次。A*在最坏情况下也是O(VE)但好的启发式函数可以大幅减少实际访问的节点数。Q4连接和8连接有什么区别A4连接只能走上下左右路径是之字形的路径长度偏大。8连接多了对角线路径更平滑更接近真实最短路径。但8连接的对角线代价是sqrt(2)而不是1计算时注意不要搞错。工程上移动机器人常用8连接机械臂关节空间规划有时用更多连接比如16连接。Q为什么机械臂规划很少用网格搜索A因为维度太高。6轴机械臂的构型空间是6D的用网格离散化节点数爆炸。所以机械臂通常用采样规划RRT/PRM不用图搜索。图搜索适合2D或3D的低维空间。小结图搜索的核心把连续空间离散化成图然后用图搜索算法找路径。离散化方式有网格、可见图、Voronoi图、路标图等。不同的选择策略对应不同的算法——Dijkstra、A*、贪心搜索。启发式函数h(n)引导搜索方向决定了算法的效率。离散化最大的问题是维度灾难——高维空间用不了。这也是为什么高维规划要用采样方法RRT、PRM等。下一篇讲Dijkstra算法——最短路径的经典解法。如果这篇文章对你有帮助欢迎点赞、在看、转发三连。 你的支持是我持续更新的最大动力。「机器人软件开发面试·从入门到精通」连载系列上一篇第206篇 运动规划概述——全局/局部/反应式规划的分类下一篇预告第208篇 Dijkstra算法——最短路径的经典解法有任何问题欢迎评论区留言我会尽量回复。
