PythonRobotics 采样路径规划全解析从基础 RRT 到 Informed RRT*、BIT* 与闭环运动规划【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics本篇文章以开源仓库 PythonRoboticsPython sample codes and textbook for robotics algorithms中 RRT 路径规划文档 为主线系统讲解该仓库PathPlanning目录下从基础 RRT、RRT*到面向车辆运动学的 Dubins/Reeds-Shepp 变体再到 Informed RRT*、Batch Informed RRT*BIT*、Closed Loop RRT* 与 LQR-RRT* 的完整算法家族。读完本文你将掌握每一类算法在仓库中的源码结构、核心参数语义、运行方式与适用场景并能直接复用这些示例搭建自己的采样运动规划实验。算法家族总览rrt_main.rst与其包含的rrt_star.rst共同构成 PythonRobotics 文档体系中基于随机采样的路径规划核心章节。文档按基础算法 → 最优变体 → 运动学约束变体 → 启发式加速变体 → 闭环可执行变体的脉络组织对应的源码与测试如下表算法源码文档章节基础 RRTrrt.pyBasic RRTRRT*rrt_star.pyRRT*rrt_star.rstRRT Dubins 路径rrt_dubins.pyRRT with dubins pathRRT* Dubins 路径rrt_star_dubins.pyRRT* with dubins pathRRT* Reeds-Shepp 路径rrt_star_reeds_shepp.pyRRT* with reeds-sheep pathInformed RRT*informed_rrt_star.pyInformed RRT*Batch Informed RRT*BIT*batch_informed_rrt_star.pyBatch Informed RRT*Closed Loop RRT*closed_loop_rrt_star_car.pyClosed Loop RRT*LQR-RRT*lqr_rrt_star.pyLQR-RRT*全部示例共用同一套可视化约定黑色圆形表示障碍物绿色线条表示已搜索的随机树红色叉号表示起点与终点最终红色路径为规划结果。所有算法均继承自RRT基类PathPlanning/RRT/rrt.py形成了清晰的核心搜索框架 差异化 steer/cost继承体系。基础 RRT随机树如何快速搜索到路径Basic RRT 是文档的第一个主题也是整个家族的地基。其核心思想非常简单在状态空间中不断随机采样把树向随机点生长一个固定步长直到某节点足够接近目标点。构造函数与核心参数基础 RRT 实现 的构造函数定义了全部关键参数RRT(start, goal, obstacle_list, rand_area, expand_dis3.0, # 每次扩展的最大步长 path_resolution0.5, # 路径离散化分辨率 goal_sample_rate5, # 直接采样目标点的概率% max_iter500, # 最大迭代次数 play_areaNone, # 可选的活动区域约束 [xmin,xmax,ymin,ymax] robot_radius0.0) # 机器人半径圆形建模用于膨胀障碍各参数的实际影响如下expand_dis树每次生长的最大距离。过小会导致收敛慢过大容易跳过狭窄通道。path_resolution扩展段内部的离散采样步长直接影响碰撞检测精度与路径点数。goal_sample_rate以百分比表示的目标偏置。每次随机采样时以该概率直接把目标点作为采样点显著加速收敛。play_area若指定则新节点必须落在该矩形区域内通过check_if_outside_play_area判定见 rrt.py。robot_radius将机器人建模为给定半径的圆碰撞检测时对障碍半径做膨胀处理。主循环采样—找最近—生长—碰撞检测planning()方法rrt.py是算法主循环每次迭代执行get_random_node()按goal_sample_rate概率采样目标点否则在rand_area内均匀采样get_nearest_node_index()在当前node_list中找离随机点最近的节点欧氏距离平方最小steer()从最近节点向随机点生长expand_dis步长并按path_resolution逐点记录路径见 steer 实现check_collision()检查新节点路径上的每个离散点与所有障碍半径膨胀robot_radius的距离见 碰撞检测当树中最新节点距目标小于expand_dis时尝试直接连接到目标成功则通过generate_final_course()沿parent指针回溯输出完整路径。文档给出的运行示例rrt.py 的 main在起点[0,0]、目标[6,10]、采样区域[-2,15]、机器人半径0.8的配置下运行障碍列表为[(5,5,1),(3,6,2),(3,8,2),(3,10,2),(7,5,2),(9,5,2),(8,10,1)]格式为[x, y, 半径]。若max_iter内未找到路径返回None并打印Cannot find path否则打印found path!!并绘制最终路径。运行方式在仓库根目录下python PathPlanning/RRT/rrt.py对应测试 tests/test_rrt.py 关闭动画后调用main(gx1.0, gy1.0)验证短距离场景可正常搜索。RRT*从找得到到找得好基础 RRT 只能保证找到一条路径不保证最优。rrt_star.rst文档指出 RRT* 在采样框架之上增加了**选择父节点choose_parent与重布线rewire**两步操作使路径代价随迭代渐近收敛到最优。关键差异代价、近邻球与重连RRTStar 实现 继承RRT并在三个层面做了增强节点带代价Node增加cost字段rrt_star.py路径代价为沿树累计的欧氏距离。近邻搜索find_near_nodes()以r connect_circle_dist * sqrt(log(n) / n)的半径并受expand_dis约束圈定新节点的候选邻域见 近邻球实现。choose_parent rewirechoose_parent()在所有无碰撞的近邻中挑选代价最小的父节点rrt_star.pyrewire()则检查经由新节点到达近邻是否更便宜若是则改写近邻父节点并递归传播新代价rrt_star.py。RRT* 构造函数新增两个参数RRTStar(start, goal, obstacle_list, rand_area, expand_dis30.0, # 注意默认步长比基础 RRT 更大 path_resolution1.0, goal_sample_rate20, max_iter300, connect_circle_dist50.0, # 近邻搜索球半径系数 search_until_max_iterFalse, robot_radius0.0)其中connect_circle_dist控制重连搜索范围search_until_max_iterTrue时算法会跑满全部迭代以充分优化路径代价更高False时找到可达目标即提前返回。RRT* 对应的论文文献文档 Ref 一节为Sampling-based Algorithms for Optimal Motion Planning与Incremental Sampling-based Algorithms for Optimal Motion Planning二者奠定了渐近最优采样规划的理论基础。运行方式python PathPlanning/RRTStar/rrt_star.py运动学约束变体Dubins 与 Reeds-Shepp对汽车类机器人路径不仅要避开障碍还要满足最小转弯半径与朝向约束。文档用三个小节分别介绍了 RRT/RRT* 与曲线路径规划器的组合。RRT with Dubins 路径RRTDubins 针对只能前进的汽车机器人设计。与基础 RRT 的最大区别节点状态从(x, y)扩展为(x, y, yaw)采样时 yaw 在[-π, π]内随机steer()不再走直线段而是调用 dubins_path_planner 的plan_dubins_path生成满足曲率约束curvature1.0的圆弧-直线组合路径rrt_dubins.py代价按 Dubins 路径各航段长度绝对值之和累计到达判定同时要求位置误差goal_xy_th0.5与朝向误差goal_yaw_th1°均满足search_best_goal_node。示例配置起点[0,0,0°]、目标[10,10,0°]max_iter200、goal_sample_rate10python PathPlanning/RRTDubins/rrt_dubins.pyRRT* with Dubins 路径RRTStarDubins 在 RRT* 框架choose_parentrewire之上替换 Dubins 扩展器是最优采样 前进车辆运动学的完整组合。其planning()流程与RRTStar一致但steer、calc_new_cost均基于 Dubins 路径长度计算示例起点/目标与 RRTDubins 相同。RRT* with Reeds-Shepp 路径Reeds-Shepp 曲线允许前进/倒车能生成比 Dubins 更短的可达路径可原地切换方向。RRTStarReedsShepp 在 RRT* 基础上steer()调用 reeds_shepp_path_planning 生成满足曲率约束的 RS 路径step_size0.2控制离散采样提供try_goal_path()每加入一个新节点就尝试直接连到目标提高到达概率rrt_star_reeds_shepp.py最终路径点格式为[x, y, yaw]三元组。运行python PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py启发式加速Informed RRT* 与 Batch Informed RRT*BIT*当已找到一条可行路径后若继续在全状态空间均匀采样大量样本会浪费在不可能改善当前解的区域。Informed RRT* 与 BIT* 正是为解决这一瓶颈而设计。Informed RRT*把采样域收缩到椭球文档对 Informed RRT* 的核心描述是The cyan ellipse is the heuristic sampling domain of Informed RRT*.青色椭圆为 Informed RRT* 的启发式采样域。其原理见 informed_rrt_star.py 与参考论文Informed RRT: Optimal Sampling-based Path Planning Focused via Direct Sampling of an Admissible Ellipsoidal Heuristic*以当前最优路径长度c_best为椭球长轴、起点到目标直线距离c_min为焦距定义 admissible 椭球启发式域只有位于该椭球内的样本才有可能改善当前解因此informed_sample()informed_rrt_star.py在找到首条路径后只在椭球内采样每发现更短路径c_best收缩、椭球随之缩小采样越来越聚焦加速收敛到最优椭球通过 SVD 计算旋转矩阵C后由sample_unit_ball()均匀采样再仿射变换得到与plot_ellipse绘制逻辑一致。核心调用为informed_rrt_star_search(animationTrue)构造函数参数与基础 RRT 相似InformedRRTStar(start, goal, obstacle_list, rand_area, expand_dis0.5, goal_sample_rate10, max_iter200)python PathPlanning/InformedRRTStar/informed_rrt_star.pyBatch Informed RRT*BIT*批采样 启发式图搜索Batch Informed RRT*源码类名BITStarbatch_informed_rrt_star.py将采样规划与 A* 式的增量图搜索结合参考论文为Batch Informed Trees (BIT): Sampling-based Optimal Planning via the Heuristically Guided Search of Implicit Random Geometric Graphs*。文档将其定位为更快收敛于 RRT* 与 Informed RRT* 的变体。从源码结构看其核心机制包括显式随机几何图RGGRTree类batch_informed_rrt_star.py把连续空间按resolution0.01网格离散化并用real_world_to_node_id把坐标映射为唯一整数节点 ID构建显式图结构顶点/边双队列vertex_queue与edge_queue分别存放待扩展顶点与候选边best_vertex_queue_value()/best_edge_queue_value()用g h的 A* 启发式评估优先级批式采样setup_sample()每批生成 100/200 个样本找到目标后改用椭球式 informed 采样并重用历史信息old_vertices保留已扩展顶点惰性连接plan()中先基于 f/g 得分剪枝不可行边f1/f2/f3三个条件再调用connect()做碰撞检测与真实连接见 plan 主循环路径回溯通过find_final_path()沿nodes父子关系完成。BITStar(start, goal, obstacleList, randArea, eta2.0, # 可调参数控制采样密度相关尺度 maxIter80) # 批迭代次数python PathPlanning/BatchInformedRRTStar/batch_informed_rrt_star.py面向真实车辆Closed Loop RRT* 与 LQR-RRT*前序算法规划出的几何路径未必能被真实车辆动力学跟踪。文档最后两个主题针对这一问题在规划阶段就引入闭环控制器与车辆模型保证输出路径可执行。Closed Loop RRT*pure-pursuit 转向 PID 速度控制的闭环预测文档对 Closed Loop RRT* 的说明非常明确pure-pursuit algorithm is used for steering control, PID is used for speed control.ClosedLoopRRTStar 继承自RRTStarReedsShepp规划分两步先用 Reeds-Shepp 变体做几何路径搜索复用super().planning()再对每个可达目标节点生成的候选路径做闭环可行性检验check_tracking_path_is_feasible()用单轮车模型unicycle_model.py轴距L0.9m、最大转向角40°、最大加速度5.0m/s²、时间步dt0.05s做前向仿真pure_pursuit_control()负责转向、PIDControl()负责速度跟踪见 pure_pursuit.py前视距离Lf0.5m、速度增益Kp2.0search_best_feasible_path()在满足约束的路径中挑选跟踪时间最短者输出完整的x, y, yaw, v, t, a, d时序数据位置、航向、速度、时间、加速度、转向角。关键约束参数target_speed10/3.6 m/s、yaw_th3°、xy_th0.5、invalid_travel_ratio5.0实际行驶距离与几何路径长度之比超过该值判为不可行。main 中还会绘制 yaw、速度km/h、加速度、转向角的时序图。参考论文为Motion Planning in Complex Environments using Closed-loop Prediction、Real-time Motion Planning with Applications to Autonomous Urban Driving等。python PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.pyLQR-RRT*双积分器模型 LQR 局部规划器文档对 LQR-RRT* 的定位是A double integrator motion model is used for LQR local planner.使用双积分器运动模型作为 LQR 局部规划器。LQRRRTStar 继承RRTStar不同点在于每次steer()/calc_new_cost()不再走直线或曲线而是调用 LQRPlanner.lqr_planning 用 LQR 最优控制生成从当前节点到目标点的轨迹双积分器模型再按step_size0.2重采样为路径点sample_path()见 lqr_rrt_star.py代价以 LQR 轨迹长度累计goal_xy_th0.5用于到达判定。参考论文为LQR-RRT: Optimal Sampling-Based Motion Planning with Automatically Derived Extension Heuristics*。python PathPlanning/LQRRRTStar/lqr_rrt_star.py参数速查与选型建议参数所在类默认值含义expand_disRRT / RRTStar3.0 / 30.0每次生长的最大步长path_resolutionRRT / RRTStar0.5 / 1.0路径离散化分辨率goal_sample_rateRRT / RRTStar5 / 20目标偏置采样概率%max_iterRRT / RRTStar500 / 300最大迭代次数robot_radius所有类0.0机器人圆形半径play_areaRRTNone活动区域约束connect_circle_distRRTStar 家族50.0近邻重连球半径系数search_until_max_iterRRTStarFalse是否跑满迭代优化curvatureDubins/RS 变体1.0最小转弯曲率goal_xy_th/goal_yaw_th运动学变体0.5 / 1°位置/朝向到达阈值step_sizeRS/LQR 变体0.2局部路径重采样步长eta/maxIterBITStar2.0 / 80采样尺度系数 / 批次数target_speed/yaw_th/invalid_travel_ratioClosedLoopRRTStar10/3.6 m/s / 3° / 5.0闭环跟踪约束选型建议基于仓库文档与源码结构的推断无障碍动力学约束的简单场景选基础 RRT 即可需要更优路径用 RRT*汽车机器人且只能前进用 RRT/RRT*Dubins允许倒车入库用 Reeds-Shepp 变体追求更快最优收敛用 Informed RRT*找到初解后椭球聚焦或 BIT*批采样图搜索最终需要可被车辆动力学执行的轨迹则选 Closed Loop RRT*需要考虑动力学最优扩展则参考 LQR-RRT*。所有示例的依赖见 requirements/requirements.txtnumpy、scipy、matplotlib 等文档源码位于 rrt_main.rst 与 rrt_star.rst相关测试覆盖可参考 tests/test_rrt.py 等测试文件。【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考 PythonRobotics 采样路径规划全解析：从基础 RRT 到 Informed RRT*、BIT* 与闭环运动规划
PythonRobotics 采样路径规划全解析从基础 RRT 到 Informed RRT*、BIT* 与闭环运动规划【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics本篇文章以开源仓库 PythonRoboticsPython sample codes and textbook for robotics algorithms中 RRT 路径规划文档 为主线系统讲解该仓库PathPlanning目录下从基础 RRT、RRT*到面向车辆运动学的 Dubins/Reeds-Shepp 变体再到 Informed RRT*、Batch Informed RRT*BIT*、Closed Loop RRT* 与 LQR-RRT* 的完整算法家族。读完本文你将掌握每一类算法在仓库中的源码结构、核心参数语义、运行方式与适用场景并能直接复用这些示例搭建自己的采样运动规划实验。算法家族总览rrt_main.rst与其包含的rrt_star.rst共同构成 PythonRobotics 文档体系中基于随机采样的路径规划核心章节。文档按基础算法 → 最优变体 → 运动学约束变体 → 启发式加速变体 → 闭环可执行变体的脉络组织对应的源码与测试如下表算法源码文档章节基础 RRTrrt.pyBasic RRTRRT*rrt_star.pyRRT*rrt_star.rstRRT Dubins 路径rrt_dubins.pyRRT with dubins pathRRT* Dubins 路径rrt_star_dubins.pyRRT* with dubins pathRRT* Reeds-Shepp 路径rrt_star_reeds_shepp.pyRRT* with reeds-sheep pathInformed RRT*informed_rrt_star.pyInformed RRT*Batch Informed RRT*BIT*batch_informed_rrt_star.pyBatch Informed RRT*Closed Loop RRT*closed_loop_rrt_star_car.pyClosed Loop RRT*LQR-RRT*lqr_rrt_star.pyLQR-RRT*全部示例共用同一套可视化约定黑色圆形表示障碍物绿色线条表示已搜索的随机树红色叉号表示起点与终点最终红色路径为规划结果。所有算法均继承自RRT基类PathPlanning/RRT/rrt.py形成了清晰的核心搜索框架 差异化 steer/cost继承体系。基础 RRT随机树如何快速搜索到路径Basic RRT 是文档的第一个主题也是整个家族的地基。其核心思想非常简单在状态空间中不断随机采样把树向随机点生长一个固定步长直到某节点足够接近目标点。构造函数与核心参数基础 RRT 实现 的构造函数定义了全部关键参数RRT(start, goal, obstacle_list, rand_area, expand_dis3.0, # 每次扩展的最大步长 path_resolution0.5, # 路径离散化分辨率 goal_sample_rate5, # 直接采样目标点的概率% max_iter500, # 最大迭代次数 play_areaNone, # 可选的活动区域约束 [xmin,xmax,ymin,ymax] robot_radius0.0) # 机器人半径圆形建模用于膨胀障碍各参数的实际影响如下expand_dis树每次生长的最大距离。过小会导致收敛慢过大容易跳过狭窄通道。path_resolution扩展段内部的离散采样步长直接影响碰撞检测精度与路径点数。goal_sample_rate以百分比表示的目标偏置。每次随机采样时以该概率直接把目标点作为采样点显著加速收敛。play_area若指定则新节点必须落在该矩形区域内通过check_if_outside_play_area判定见 rrt.py。robot_radius将机器人建模为给定半径的圆碰撞检测时对障碍半径做膨胀处理。主循环采样—找最近—生长—碰撞检测planning()方法rrt.py是算法主循环每次迭代执行get_random_node()按goal_sample_rate概率采样目标点否则在rand_area内均匀采样get_nearest_node_index()在当前node_list中找离随机点最近的节点欧氏距离平方最小steer()从最近节点向随机点生长expand_dis步长并按path_resolution逐点记录路径见 steer 实现check_collision()检查新节点路径上的每个离散点与所有障碍半径膨胀robot_radius的距离见 碰撞检测当树中最新节点距目标小于expand_dis时尝试直接连接到目标成功则通过generate_final_course()沿parent指针回溯输出完整路径。文档给出的运行示例rrt.py 的 main在起点[0,0]、目标[6,10]、采样区域[-2,15]、机器人半径0.8的配置下运行障碍列表为[(5,5,1),(3,6,2),(3,8,2),(3,10,2),(7,5,2),(9,5,2),(8,10,1)]格式为[x, y, 半径]。若max_iter内未找到路径返回None并打印Cannot find path否则打印found path!!并绘制最终路径。运行方式在仓库根目录下python PathPlanning/RRT/rrt.py对应测试 tests/test_rrt.py 关闭动画后调用main(gx1.0, gy1.0)验证短距离场景可正常搜索。RRT*从找得到到找得好基础 RRT 只能保证找到一条路径不保证最优。rrt_star.rst文档指出 RRT* 在采样框架之上增加了**选择父节点choose_parent与重布线rewire**两步操作使路径代价随迭代渐近收敛到最优。关键差异代价、近邻球与重连RRTStar 实现 继承RRT并在三个层面做了增强节点带代价Node增加cost字段rrt_star.py路径代价为沿树累计的欧氏距离。近邻搜索find_near_nodes()以r connect_circle_dist * sqrt(log(n) / n)的半径并受expand_dis约束圈定新节点的候选邻域见 近邻球实现。choose_parent rewirechoose_parent()在所有无碰撞的近邻中挑选代价最小的父节点rrt_star.pyrewire()则检查经由新节点到达近邻是否更便宜若是则改写近邻父节点并递归传播新代价rrt_star.py。RRT* 构造函数新增两个参数RRTStar(start, goal, obstacle_list, rand_area, expand_dis30.0, # 注意默认步长比基础 RRT 更大 path_resolution1.0, goal_sample_rate20, max_iter300, connect_circle_dist50.0, # 近邻搜索球半径系数 search_until_max_iterFalse, robot_radius0.0)其中connect_circle_dist控制重连搜索范围search_until_max_iterTrue时算法会跑满全部迭代以充分优化路径代价更高False时找到可达目标即提前返回。RRT* 对应的论文文献文档 Ref 一节为Sampling-based Algorithms for Optimal Motion Planning与Incremental Sampling-based Algorithms for Optimal Motion Planning二者奠定了渐近最优采样规划的理论基础。运行方式python PathPlanning/RRTStar/rrt_star.py运动学约束变体Dubins 与 Reeds-Shepp对汽车类机器人路径不仅要避开障碍还要满足最小转弯半径与朝向约束。文档用三个小节分别介绍了 RRT/RRT* 与曲线路径规划器的组合。RRT with Dubins 路径RRTDubins 针对只能前进的汽车机器人设计。与基础 RRT 的最大区别节点状态从(x, y)扩展为(x, y, yaw)采样时 yaw 在[-π, π]内随机steer()不再走直线段而是调用 dubins_path_planner 的plan_dubins_path生成满足曲率约束curvature1.0的圆弧-直线组合路径rrt_dubins.py代价按 Dubins 路径各航段长度绝对值之和累计到达判定同时要求位置误差goal_xy_th0.5与朝向误差goal_yaw_th1°均满足search_best_goal_node。示例配置起点[0,0,0°]、目标[10,10,0°]max_iter200、goal_sample_rate10python PathPlanning/RRTDubins/rrt_dubins.pyRRT* with Dubins 路径RRTStarDubins 在 RRT* 框架choose_parentrewire之上替换 Dubins 扩展器是最优采样 前进车辆运动学的完整组合。其planning()流程与RRTStar一致但steer、calc_new_cost均基于 Dubins 路径长度计算示例起点/目标与 RRTDubins 相同。RRT* with Reeds-Shepp 路径Reeds-Shepp 曲线允许前进/倒车能生成比 Dubins 更短的可达路径可原地切换方向。RRTStarReedsShepp 在 RRT* 基础上steer()调用 reeds_shepp_path_planning 生成满足曲率约束的 RS 路径step_size0.2控制离散采样提供try_goal_path()每加入一个新节点就尝试直接连到目标提高到达概率rrt_star_reeds_shepp.py最终路径点格式为[x, y, yaw]三元组。运行python PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py启发式加速Informed RRT* 与 Batch Informed RRT*BIT*当已找到一条可行路径后若继续在全状态空间均匀采样大量样本会浪费在不可能改善当前解的区域。Informed RRT* 与 BIT* 正是为解决这一瓶颈而设计。Informed RRT*把采样域收缩到椭球文档对 Informed RRT* 的核心描述是The cyan ellipse is the heuristic sampling domain of Informed RRT*.青色椭圆为 Informed RRT* 的启发式采样域。其原理见 informed_rrt_star.py 与参考论文Informed RRT: Optimal Sampling-based Path Planning Focused via Direct Sampling of an Admissible Ellipsoidal Heuristic*以当前最优路径长度c_best为椭球长轴、起点到目标直线距离c_min为焦距定义 admissible 椭球启发式域只有位于该椭球内的样本才有可能改善当前解因此informed_sample()informed_rrt_star.py在找到首条路径后只在椭球内采样每发现更短路径c_best收缩、椭球随之缩小采样越来越聚焦加速收敛到最优椭球通过 SVD 计算旋转矩阵C后由sample_unit_ball()均匀采样再仿射变换得到与plot_ellipse绘制逻辑一致。核心调用为informed_rrt_star_search(animationTrue)构造函数参数与基础 RRT 相似InformedRRTStar(start, goal, obstacle_list, rand_area, expand_dis0.5, goal_sample_rate10, max_iter200)python PathPlanning/InformedRRTStar/informed_rrt_star.pyBatch Informed RRT*BIT*批采样 启发式图搜索Batch Informed RRT*源码类名BITStarbatch_informed_rrt_star.py将采样规划与 A* 式的增量图搜索结合参考论文为Batch Informed Trees (BIT): Sampling-based Optimal Planning via the Heuristically Guided Search of Implicit Random Geometric Graphs*。文档将其定位为更快收敛于 RRT* 与 Informed RRT* 的变体。从源码结构看其核心机制包括显式随机几何图RGGRTree类batch_informed_rrt_star.py把连续空间按resolution0.01网格离散化并用real_world_to_node_id把坐标映射为唯一整数节点 ID构建显式图结构顶点/边双队列vertex_queue与edge_queue分别存放待扩展顶点与候选边best_vertex_queue_value()/best_edge_queue_value()用g h的 A* 启发式评估优先级批式采样setup_sample()每批生成 100/200 个样本找到目标后改用椭球式 informed 采样并重用历史信息old_vertices保留已扩展顶点惰性连接plan()中先基于 f/g 得分剪枝不可行边f1/f2/f3三个条件再调用connect()做碰撞检测与真实连接见 plan 主循环路径回溯通过find_final_path()沿nodes父子关系完成。BITStar(start, goal, obstacleList, randArea, eta2.0, # 可调参数控制采样密度相关尺度 maxIter80) # 批迭代次数python PathPlanning/BatchInformedRRTStar/batch_informed_rrt_star.py面向真实车辆Closed Loop RRT* 与 LQR-RRT*前序算法规划出的几何路径未必能被真实车辆动力学跟踪。文档最后两个主题针对这一问题在规划阶段就引入闭环控制器与车辆模型保证输出路径可执行。Closed Loop RRT*pure-pursuit 转向 PID 速度控制的闭环预测文档对 Closed Loop RRT* 的说明非常明确pure-pursuit algorithm is used for steering control, PID is used for speed control.ClosedLoopRRTStar 继承自RRTStarReedsShepp规划分两步先用 Reeds-Shepp 变体做几何路径搜索复用super().planning()再对每个可达目标节点生成的候选路径做闭环可行性检验check_tracking_path_is_feasible()用单轮车模型unicycle_model.py轴距L0.9m、最大转向角40°、最大加速度5.0m/s²、时间步dt0.05s做前向仿真pure_pursuit_control()负责转向、PIDControl()负责速度跟踪见 pure_pursuit.py前视距离Lf0.5m、速度增益Kp2.0search_best_feasible_path()在满足约束的路径中挑选跟踪时间最短者输出完整的x, y, yaw, v, t, a, d时序数据位置、航向、速度、时间、加速度、转向角。关键约束参数target_speed10/3.6 m/s、yaw_th3°、xy_th0.5、invalid_travel_ratio5.0实际行驶距离与几何路径长度之比超过该值判为不可行。main 中还会绘制 yaw、速度km/h、加速度、转向角的时序图。参考论文为Motion Planning in Complex Environments using Closed-loop Prediction、Real-time Motion Planning with Applications to Autonomous Urban Driving等。python PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.pyLQR-RRT*双积分器模型 LQR 局部规划器文档对 LQR-RRT* 的定位是A double integrator motion model is used for LQR local planner.使用双积分器运动模型作为 LQR 局部规划器。LQRRRTStar 继承RRTStar不同点在于每次steer()/calc_new_cost()不再走直线或曲线而是调用 LQRPlanner.lqr_planning 用 LQR 最优控制生成从当前节点到目标点的轨迹双积分器模型再按step_size0.2重采样为路径点sample_path()见 lqr_rrt_star.py代价以 LQR 轨迹长度累计goal_xy_th0.5用于到达判定。参考论文为LQR-RRT: Optimal Sampling-Based Motion Planning with Automatically Derived Extension Heuristics*。python PathPlanning/LQRRRTStar/lqr_rrt_star.py参数速查与选型建议参数所在类默认值含义expand_disRRT / RRTStar3.0 / 30.0每次生长的最大步长path_resolutionRRT / RRTStar0.5 / 1.0路径离散化分辨率goal_sample_rateRRT / RRTStar5 / 20目标偏置采样概率%max_iterRRT / RRTStar500 / 300最大迭代次数robot_radius所有类0.0机器人圆形半径play_areaRRTNone活动区域约束connect_circle_distRRTStar 家族50.0近邻重连球半径系数search_until_max_iterRRTStarFalse是否跑满迭代优化curvatureDubins/RS 变体1.0最小转弯曲率goal_xy_th/goal_yaw_th运动学变体0.5 / 1°位置/朝向到达阈值step_sizeRS/LQR 变体0.2局部路径重采样步长eta/maxIterBITStar2.0 / 80采样尺度系数 / 批次数target_speed/yaw_th/invalid_travel_ratioClosedLoopRRTStar10/3.6 m/s / 3° / 5.0闭环跟踪约束选型建议基于仓库文档与源码结构的推断无障碍动力学约束的简单场景选基础 RRT 即可需要更优路径用 RRT*汽车机器人且只能前进用 RRT/RRT*Dubins允许倒车入库用 Reeds-Shepp 变体追求更快最优收敛用 Informed RRT*找到初解后椭球聚焦或 BIT*批采样图搜索最终需要可被车辆动力学执行的轨迹则选 Closed Loop RRT*需要考虑动力学最优扩展则参考 LQR-RRT*。所有示例的依赖见 requirements/requirements.txtnumpy、scipy、matplotlib 等文档源码位于 rrt_main.rst 与 rrt_star.rst相关测试覆盖可参考 tests/test_rrt.py 等测试文件。【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考