智能网联汽车决策规划技术算法原理与实践课件 9.RRT 算法

RRT算法目录/CONTENTS01RRT算法基础与优势02RRT经典变种算法03RRT*代码实现与可视化04综合实战：机器人路径规划与轨迹优化系统05综合实战：基于pygame的RRT*路径规划系统06总结与思考RRT算法基础与优势01RRT算法定义与核心思想RRT算法基本定义RRT算法是一种基于随机采样的树形路径规划方法，通过在自由空间中随机采样并迭代扩展树结构，快速探索高维状态空间，寻找从起点到目标点的可行路径。概率完备性RRT算法具有概率完备性，即随着采样点数量的无限增加，找到解的概率趋近于1。这一特性使其在复杂环境中表现出色。适用性RRT算法适用于复杂环境与高维空间，避免显式建模障碍物，显著降低计算复杂度，特别适合智能网联汽车的路径规划。RRT算法实现步骤拆解初始化与循环采样RRT算法的实现包括初始化将起点加入树，以及循环采样自由空间生成新点。每次迭代中，算法随机生成一个新点，并在树中找到最近的节点。扩展与碰撞检测算法沿直线向新点扩展，并进行碰撞检测。如果新点与最近节点之间没有碰撞，则将新点添加到树中，并检查是否抵达目标区域。位姿空间与运动规划基本定义位姿空间概念位姿空间（ConfigurationSpace）是机器人所有可能位姿的集合，它将机器人的运动规划问题转化为在自由空间中寻找连续路径。位姿与自由度位姿是指机器人上所有点在工作空间中的位置，而自由度则是决定机器人运动状态的独立参数的数目。障碍物空间与自由空间障碍物空间是指机器人在位姿空间中与障碍物相交的区域，而自由空间则是机器人可以自由运动的区域。路径规划问题的本质路径规划问题的本质是在自由空间中寻找一条从起始位姿到目标位姿的路径，这一过程通过C空间的抽象得以简化。RRT经典变种算法02基于概率P的RRT改进策略01概率P的引入在RRT算法的扩展阶段引入概率P，以概率P选择目标点，以1-P的概率随机探索，加速算法的收敛。02GoalBias与GoalZoom基于概率P的RRT通过GoalBias与GoalZoom两种方式实现，兼顾探索与利用，减少在空旷区域的无谓采样。03适用场景该策略特别适用于复杂城市路网下的搜索，能够显著提升搜索效率，适用于自动驾驶车辆的路径规划。RRT_Connect双向搜索机制双向搜索机制RRT_Connect算法同时从起点与目标点生长两棵随机树，每次迭代交替扩展并尝试连接对方最近节点，显著提高搜索效率。优势与应用该算法在搜索速度、避免局部极小方面表现出色，适用于自动驾驶中的快速换道、紧急避障等实时场景。RRT*渐进最优原理选父与重布线RRT*算法在原有RRT的基础上，增加了选父与重布线步骤，通过优化树结构，使路径代价趋近全局最优。渐进最优性随着采样点数量的增加，RRT*算法能够保证找到的路径代价趋近于全局最优，提供理论最优的路径规划。计算复杂度RRT*算法在优化路径的同时，保持计算复杂度可控，适用于智能网联汽车的路径规划需求。平滑路径RRT*算法生成的路径不仅代价最优，而且平滑度更高，适合自动驾驶车辆的运动控制。RRT*代码实现与可视化03RRT*节点与树结构设计Node类设计在Python实现中，Node类包含坐标、父指针、代价值，用于表示路径规划中的节点信息。RRTStar类RRTStar类维护节点列表、障碍物、步长、邻居半径等关键属性，是实现RRT*算法的核心。树结构存储采用字典以字符串键存树结构，便于快速查找父节点与回溯路径，为后续优化提供支持。采样、扩展与碰撞检测流程采样与扩展sample_point函数以一定概率返回目标点或随机点，nearest_node函数找到最近节点，extend函数沿单位步长前进。碰撞检测is_collision_free函数对障碍物矩形逐点取样判断占用，确保路径安全，为后续优化提供基础。选父与重布线优化机制路径回溯与MATLAB可视化路径回溯backtrack_path方法从目标节点沿parent链逆向收集坐标，再正向排列得最终路径，为可视化提供数据支持。MATLAB可视化plot方法利用matplotlib绘制障碍物、树边、路径及起止点，实现规划结果可视化，便于验证算法正确性。综合实战：机器人路径规划与轨迹优化系统04项目架构与模块职责项目架构本项目采用C++与MATLAB混合工程，实现从路径规划到轨迹优化的完整闭环。RRTStar模块RRTStar模块负责在三维空间内搜索避开障碍物的全局路径，输出waypoint序列。MinSnapTraj模块MinSnapTraj模块接收waypoints，生成最小加速度连续轨迹，输出位置、速度、加速度时序。可视化MATLAB脚本读取CSV绘制三维散点图，实现从规划到可视化的完整闭环。RRTStar类接口与运行流程01接口设计RRTStar类的构造函数输入空间限界、起止点、步长、迭代次数与障碍物，接口简洁，易于集成。02运行流程run方法循环采样、搜索、扩展、优化，直至找到路径，生成最优路径并保存。03适用场景适用于三维城市道路场景，为自动驾驶车辆提供高效路径规划。MinimumSnap轨迹优化原理优化原理MinimumSnap以分段多项式表示轨迹，构建约束矩阵，采用最小二乘法求解系数，生成平滑轨迹。优势该方法保证路径平滑、加速度连续，降低执行机构磨损，适用于机器人或无人车的高速平稳跟踪。主程序串联与MATLAB可视化结果

01主程序串联main.cpp先运行RRTStar得global_path，再转Eigen矩阵传入MinimumSnap，生成轨迹并保存。02MATLAB可视化MATLAB读取output_traj_optim.csv，scatter3绘制三维散点并开启rotate3d交互，直观展示结果。03应用价值端到端流程验证算法可行性，输出可直接用于实车轨迹跟踪或仿真回放，具有很高的实用价值。综合实战：基于pygame的RRT*路径规划系统05系统功能与交互设计功能介绍Python+pygame实现的交互式规划器，支持鼠标绘制障碍物、设置起止点、启动RRT*算法。交互设计用户可通过鼠标左键绘制障碍物，右键擦除；拖拽设置起止点；回车键启动RRT*，S/L保存加载地图。应用场景适用于教学演示与算法调参，降低算法理解门槛，提升用户体验。事件处理与状态机流转事件处理mainHandler基于不同状态，根据鼠标位置与事件类型切换状态，确保界面响应流畅。状态机流转rrtHandler在算法运行时捕获QUIT与H键，确保可随时中断或切换信息展示。最近邻搜索与碰撞检测实现最近邻搜索pointsContainer采用线性搜索维护节点列表，NNS方法遍历求最小距离，实现简单高效。碰撞检测collision函数对src-dst线段步进取样，查询obstaclesSurface像素颜色判断是否碰撞。性能优化实现简洁，在百节点规模下性能足够，同时为后续升级KD-Tree或GPU加速预留接口。实时绘制与路径回溯展示实时绘制addEdge将边加入缓冲池，达MAX_EDGES_POOL后批量绘制树边白线、节点橙点，降低刷新开销。路径回溯drawPath从goalPos沿parent链逆向绘制蓝色宽线并标注节点，完成路径高亮展示。总结与思考06RRT算法家族对比与适用场景原始RRT原始RRT适用于快速探索路径，但在最优性和平滑度上表现一般。基于概率P的RRT基于概率P的RRT在探索与利用间平衡，适用于复杂城市路网。RRT_ConnectRRT_Connect适合实时快速规划，适用于自动驾驶中的快速换道、紧急避障等场景。RRT*RRT*适合对最优和平滑要求高的城市场景，生成理论最优的路径。课程回顾与能力提升目标达成课程回顾回顾学习目标，包括