PythonRobotics 中的 RRT* 路径规划：RRTStar 最优采样规划的源码级解析

PythonRobotics 中的 RRT* 路径规划RRTStar 最优采样规划的源码级解析【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRoboticsRRT*Rapidly-exploring Random Tree Star是在基本 RRT 基础上引入渐进最优性asymptotic optimality的采样路径规划算法通过在扩展新节点时执行选择父节点choose_parent与重布线rewire两步优化使搜索树随迭代次数增加而收敛到接近最优的路径。本篇文章以 PythonRobotics 项目 PathPlanning 模块中的 RRTStar 实现为主体结合其文档 rrt_star.rst、源码与测试用例讲解 RRT* 的算法原理、完整参数语义、核心方法调用链以及可复现的运行示例读完即可独立配置并运行一次 RRT* 路径规划仿真。RRT* 在 PythonRobotics 中的位置与文档定位在 PythonRobotics 的文档体系中RRT* 位于 rrt_main.rst 所组织的Rapidly-Exploring Random Trees (RRT)大节之下与基本 RRT、RRT with dubins path、RRT* with dubins path、RRT* with reeds-shepp path、Informed RRT*、Batch Informed RRT*、Closed Loop RRT*、LQR-RRT* 共同构成完整的 RRT 家族算法示例原始文档通过.. include:: rrt_star.rst将其嵌入 RRT 主题页。该文档对 RRT* 示例的定位非常简洁明确This is a path planning code with RRT*即一个基于 RRT* 的路径规划示例代码。图中黑色圆形为障碍物Black circles are obstacles绿色线为搜索树green line is a searched tree红色十字为起点和终点red crosses are start and goal positions。仓库中对应的可执行实现位于 rrt_star.py文档通过.. autoclass:: PathPlanning.RRTStar.rrt_star.RRTStar自动挂载该类的 docstring因此类文档即接口文档。RRT* 算法原理从 RRT 到渐进最优基本 RRT 每次只向随机采样点扩展一个固定步长expand_dis的新节点并直接将其挂到最近邻节点之下因此生成的路径可行但不保证最优。RRT* 在 rrt_star.py 中通过引入路径代价cost概念把随机扩展升级为带优化的扩展核心改进体现在两个步骤Choose Parent选择父节点新节点诞生后不再无条件挂到最近邻而是在其邻域一个以新节点为圆心的球内所有已有节点中逐一尝试将邻域节点作为候选父节点进行碰撞检测与代价计算最终选择代价最小的无碰撞节点作为父节点见choose_parent方法rrt_star.py。若邻域内所有候选路径都碰撞则该节点被丢弃打印There is no good path.(min_cost is inf)。Rewire重布线新节点加入树后反过来检查邻域内已有节点——如果经由新节点到达它们的路径比其原有路径更便宜就重新指定新节点为这些节点的父节点并递归更新子树代价见rewire与propagate_cost_to_leaves方法rrt_star.py、rrt_star.py。正是这两步操作使得树的结构随采样增多而持续被修剪优化最终路径代价随迭代收敛从而获得渐近最优性质。该算法依据的两篇奠基性论文在原始文档 References 中列出《Sampling-based Algorithms for Optimal Motion Planning》与《Incremental Sampling-based Algorithms for Optimal Motion Planning》前者即 RRT* 的原始出处。RRTStar 类源码解析完整参数语义从源码结构看RRTStar直接继承自基本 RRT 类 rrt.py 中的RRT并重载了内部节点类Node在基类的x、y、path_x、path_y、parent基础上新增cost字段从而复用基类的采样、转向steer、碰撞检测、绘图等基础设施只重写与最优性相关的逻辑。构造函数签名rrt_star.py完整定义了以下参数这也是文档autoclass挂载的接口内容参数默认值含义start必填起点位置[x, y]goal必填目标点位置[x, y]obstacle_list必填障碍物列表[[x, y, size], ...]size为圆形障碍半径rand_area必填随机采样区域[min, max]x、y 共用该范围expand_dis30.0每次扩展的最大步长path_resolution1.0转向路径的插值分辨率用于碰撞检测的逐点采样goal_sample_rate20采样时直接以目标点为随机点的概率百分比max_iter300最大迭代次数connect_circle_dist50.0邻域搜索圆的基准半径search_until_max_iterFalse是否一直搜索到最大迭代而非找到可行解即返回robot_radius0.0机器人半径用于障碍物膨胀机器人被建模为圆形其中expand_dis与connect_circle_dist对算法行为影响最显著expand_dis决定树的生长步长与目标连接阈值connect_circle_dist则通过邻域半径公式r connect_circle_dist * sqrt(log(n) / n)n为当前节点数决定 choose_parent 与 rewire 的候选范围源码中还会将r进一步限制为不超过expand_dis见find_near_nodesrrt_star.py——该半径随节点数增大而收缩是 RRT* 渐进最优性的理论关键。核心方法调用链一次完整的主循环planning(animationTrue)rrt_star.py是算法入口其单次迭代的调用链可概括为get_random_node()按goal_sample_rate概率返回目标点或rand_area内均匀随机点复用基类实现rrt.pyget_nearest_node_index()找到树中距随机点最近的节点steer()从最近邻节点向随机点扩展expand_dis距离得到带逐点路径的新节点并累加计算其初始代价check_collision()利用path_x/path_y逐点判断是否与obstacle_list叠加robot_radius碰撞find_near_nodes()→choose_parent()在邻域内挑选代价最小的无碰撞父节点rewire()以新节点为枢纽重连邻域内可被改善的节点并propagate_cost_to_leaves()递归更新后代代价检查收敛若search_until_max_iter为False则调用search_best_goal_node()rrt_star.py在所有与目标距离不超过expand_dis且无碰撞的节点中挑选节点代价 到目标距离最小者通过generate_final_course()回溯父指针生成最终路径否则一直迭代到max_iter后再做同样处理。值得注意的是planning每轮迭代都会打印Iter: i, number of nodes: ...日志便于观察树的生长规模若全部迭代后仍无可行解则返回None调用方需自行处理demo 中打印Cannot find path。运行仿真可复现的演示示例文档的 Simulation 小节给出了仿真实例图对应的可运行示例位于 rrt_star.py 的main()函数。其标准配置如下起点start[0, 0]目标goal[6, 10]采样区域rand_area[-2, 15]8 个圆形障碍物(5,5,1)、(3,6,2)、(3,8,2)、(3,10,2)、(7,5,2)、(9,5,2)、(8,10,1)、(6,12,1)格式[x, y, 半径]expand_dis1、robot_radius0.8其余参数使用默认值。从仓库根目录直接运行即可看到带动画的规划过程与最终路径python PathPlanning/RRTStar/rrt_star.py运行结果与文档中的仿真图一致绿色折线为搜索过程中不断生长的树红色十字标记起点与目标黑色圆为障碍物算法收敛后以红色虚线绘制出最终规划路径。此外演示代码在最终绘图时调用了基类的draw_graph()rrt.py它支持按esc键随时退出动画、以-r红色圆环绘制机器人轮廓、绘制可选的play_area边界并保持坐标轴等比plt.axis(equal)。测试验证无障碍与机器人半径场景仓库通过 test_rrt_star.py 对 RRTStar 进行了自动化验证测试用例可作为二次开发与参数调优的回归基线test1关闭动画后完整运行main()演示覆盖默认障碍场景test_no_obstacle在无障碍列表下规划断言path is not Noneassert path is not Nonetest_no_obstacle_and_robot_radius无障碍且robot_radius0.8时规划同样断言成功找到路径。运行测试只需在仓库根目录执行仓库使用 pytest路径注入由 conftest.py 完成python -m pytest tests/test_rrt_star.py这些用例从侧面验证了 RRTStar 即使在没有障碍、或引入机器人半径膨胀的简单场景下也能稳定产出可行路径可作为验证自定义参数是否合理的快速手段。小结本文以 PythonRobotics 文档 rrt_star.rst 为骨架结合 rrt_star.py 源码与其基类 rrt.py、测试 test_rrt_star.py完整覆盖了 RRT* 的算法动机choose_parent 与 rewire 带来的渐进最优性、全量参数语义expand_dis、connect_circle_dist、goal_sample_rate、robot_radius等、主循环调用链以及可直接运行的演示与测试命令。若需进一步探索 RRT* 在运动学约束场景下的应用可继续阅读同目录文档体系中的 RRT* with dubins pathRRTStarDubins、RRT* with reeds-shepp pathRRTStarReedsShepp与 Informed RRT*InformedRRTStar等变体实现。【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考