★ FEATURED ARTICLE 自动驾驶赛车这两年一直是我投入精力最多的方向。相比普通道路场景赛车讲究的是极限工况下的快速决策——车速高、障碍物动态性强、赛道边界约束紧任何路径规划算法放进这个环境跑一遍基本就能试出它的工程上限。这次做的项目主题很明确基于快速探索随机树RRT的路径规划附带完整的MATLAB代码和Simulink仿真模型。说白了就是让一辆具备基本动力学特性的赛车模型在带障碍物的赛道上自动规划出可行路径再跟踪这条路径完成跑圈整个过程在仿真环境里闭环验证。这个项目适合两类人参考一类是做路径规划或无人驾驶仿真方向的工程师想弄明白RRT从数学原理到MATLAB代码、再到Simulink闭环仿真到底怎么落地另一类是刚接触移动机器人路径规划的同学想找一份结构清楚、能直接运行的RRT实现。文章里我会把算法原理、代码结构、联调步骤以及个人踩过的坑都讲透你可以照着搭一套自己的环境再往赛车方向做进一步扩展。1. 项目核心思路为什么自动驾驶赛车需要RRT1.1 赛车场景到底是一个什么样的规划问题先把这句话说透自动驾驶赛车的路径规划本质上是在约束密集的连续空间里搜索可行路径的问题。普通公路场景中车道线和交规本身就是很强的约束很多时候可以把规划退化成沿参考线修正偏移。但赛道不一样边界就是硬几何约束障碍物出现的位置和时机更激进车速又高留给规划算法的时间窗口通常只有几十毫秒。这种问题如果强行用栅格搜索去解马上就会撞上分辨率这个坎——栅格粗了找不到路栅格细了计算量爆炸。RRT的思路则完全绕开了这个问题。它不做全局枚举而是靠随机采样引导探索。核心思想特别朴素在状态空间里随机撒一个点在已经生长的树上找到离这个随机点最近的节点从该节点向随机点方向伸出一小段距离检查这一段是否安全安全就把新节点挂到树上。反复执行树就像藤蔓一样在自由空间里蔓延直到某个节点落进目标区域。为什么赛车场景适合用RRT两个原因。第一它是采样方法计算量与状态维度解耦得比较好你可以把(xy航向角速度)都放进状态空间里一起规划而不至于像栅格那样维度一涨就爆第二它天然擅长探索性搜索在通道狭窄、障碍较多的赛道上比势场法那种局部优化方法更容易找到全局可行路径。换句话讲你面对的环境越复杂、越缺少先验结构RRT这种“无模型”探索方法的优势就越明显。1.2 赛车工况对RRT提出的额外要求但基础RRT只保证概率完备意思是只要时间足够长一定能找到一条几何可行路径。赛车要的不是“能走”而是“走得快、走得稳”。这就在几个方面给基础算法提出了升级要求。第一是路径光滑性。基础RRT输出的是由线段连成的折线路径曲率在节点处突变。直接丢给横向跟踪控制器车辆会在每个拐点急打方向轻则路径跟踪失败重则车辆失稳冲出赛道。所以必须加一个后处理步骤用样条插值或者轨迹平滑把折线转换成曲率连续的路径。第二是运动学可行性。赛车前轮转角有物理极限最小转弯半径下不来。如果扩展节点时只按“最近节点加直线延伸”来做得到的路径可能几何上完整但车辆实际根本走不出来。改进方向有两个一是在RRT扩展时直接限制方向变化量相当于把车辆转向能力内建到每一步二是在平滑阶段对输出路径做曲率上限约束。我在项目里选择的是后者因为这样RRT本身可以保持通用平滑模块承担车辆适配职责更清晰。第三是速度规划必须和路径规划联动。赛车过弯的最优策略通常是“慢进快出”——进弯前刹车把载荷前移增强前轮转向能力过了弯心再全力加速出弯。RRT本身只给路径不给速度所以项目里我额外做了一个速度剖面生成模块根据路径曲率反推每个点允许的最大速度再交给跟踪层。这一步做完整个系统才算从“RRT演示代码”变成“能在赛道上跑圈、能观察车辆动态响应的仿真项目”。2. RRT算法原理与代码实现细节2.1 基本流程五步循环加三个判断标准RRT的主循环可以压缩成五步随机采样、最近邻搜索、扩展生成、碰撞检测、终点判断。看起来简单但每一步都有值得抠的细节。随机采样决定探索的方向偏好。纯均匀采样是基础版实测收敛偏慢因为树会花大量时间去探索没什么希望的区域。通用改进是目标偏置每次迭代以5%到10%的概率直接采样目标点树会明显更快地靠近终点。这个偏置比例需要根据地图大小调整地图大可以稍微低一点地图小、通道窄则调高一些更好。最近邻搜索是性能瓶颈所在。树节点一多线性扫描的复杂度就是O(n)几千个节点之后在MATLAB里就能感觉到明显卡顿。正规做法是建KD树MATLAB自带的KDTreeSearcher就可以直接用近邻查询复杂度降到O(log n)实测在5000节点规模下速度能提升一个数量级以上。很多RRT示例代码没有这一步跑小地图没事换大赛道立刻露馅。扩展生成的核心是步长σ。步长太小树长得慢迭代次数爆炸步长太大容易跳过狭窄通道或者压到障碍物边界。针对赛道的经验值是取赛道宽度的1/10到1/5这个量级能兼顾探索效率和通道通过性。基础RRT的扩展方向是沿“最近节点到随机点”的直线方向完全不管车辆运动学所以后续平滑和约束处理的负担会相对重一些但换来的是实现简单和调试直观。碰撞检测最容易出错。关键是要对整段扩展线段做碰撞检查而不是只查新节点。因为随机采样点可能恰好落在障碍物外侧附近线段中间反而穿过了障碍区域。工程做法是把线段离散成若干个点逐个查占用栅格值点间距要与栅格分辨率匹配——太密浪费算力太疏容易漏检。终点判断也有讲究。不要用“新节点坐标严格等于目标点”这种条件几乎永远触发不了。标准做法是设定目标区域半径比如1.5米新节点落进这个圆就算成功。配合目标偏置采样终点命中率会有肉眼可见的提升。2.2 MATLAB核心代码逐段拆解下面这段是项目中RRT主循环的核心代码我按实际使用习惯做了精简保留主干逻辑。function [path, tree] rrt_plan(start, goal, map, params) % start, goal: [x; y; theta] % map: binary occupancy map (1 occupied) % params: struct with step_size, max_iter, goal_tol, goal_bias tree.nodes start; tree.parent 0; kdtree KDTreeSearcher(tree.nodes(1:2, :)); for iter 1 : params.max_iter % 1. 采样带目标偏置 if rand params.goal_bias sample goal; else sample [rand * params.map_x; rand * params.map_y; 0]; end % 2. 最近邻搜索KD树加速 idx knnsearch(kdtree, sample(1:2)); nearest tree.nodes(:, idx); dist norm(sample(1:2) - nearest(1:2)); % 3. 扩展生成新节点 if dist params.step_size new_node sample; else theta_dir atan2(sample(2) - nearest(2), sample(1) - nearest(1)); new_node nearest; new_node(1:2) new_node(1:2) params.step_size * [cos(theta_dir); sin(theta_dir)]; end % 4. 碰撞检测 if is_collision_line(nearest, new_node, map, params) continue; % 有碰撞就跳过本次扩展 end % 5. 挂载节点并更新树和KD树 tree.nodes [tree.nodes, new_node]; tree.parent [tree.parent, idx]; kdtree KDTreeSearcher(tree.nodes(1:2, :)); % 终点判断落入目标圆域 if norm(new_node(1:2) - goal(1:2)) params.goal_tol path extract_path(tree, size(tree.nodes, 2)); return; end end error(超过最大迭代次数未找到路径); end几个实现细节说明。KDTreeSearcher每次加节点后都重建其实不是最优写法节点到几千之后重建开销也会上来但实现简单、逻辑直观用来做验证和demo完全足够。想要更快可以维护一个增量式KD树工程量会明显增加一般教学和算法验证场景不值得投入。start和goal用三状态向量[xytheta]但基础RRT扩展时只用xy平面theta是留给平滑和控制器用的。原因很简单如果真把theta和车辆转向约束都放进RRT采样空间里那就成了运动学RRTKinodynamic RRT扩展逻辑会复杂一个档次。项目拆分上我选择先用几何RRT快速出一条参考路径再由平滑模块和控制器处理运动学可行性每块职责清晰出了问题也好定位。2.3 路径平滑从折线到可跟踪的光滑曲线RRT出来的路径要直接给车用必须先过平滑这一关。我在项目里试过两种方案效果差异不小。第一种是B样条平滑。把RRT输出路径的顶点作为控制点生成一条B样条曲率连续性天然有保障。优点是简单稳定缺点是控制点太多时曲线会向原始折线回归不够顺滑控制点太少又可能严重偏离原始路径甚至把路径拉到障碍物里。B样条的阶数和节点矢量需要反复试验对新手来说调试成本偏高。第二种是路径粗化加三次样条插值的组合方案。先把RRT路径上共线的中间点删掉只保留关键拐点再用带约束的插值方法生成光滑路径。这样既保留了原始路径的拓扑结构又让曲率连续。实测下来第二种方案对跟踪效果更友好尤其配合后面的曲率约束检查车辆能稳稳吃到赛线。不管用哪种平滑完成后都必须做一件事逐点计算路径曲率检查是否超过车辆最大允许曲率即κ ≤ tan(δ_max)/L其中δ_max是最大前轮转角L是轴距。发现超限可以局部加大转弯半径重新插值或者在速度规划阶段把那个点的允许速度压得非常低。后面Simulink联调时我会专门盯着这个量看它直接决定了跟踪控制器会不会在某个弯道突然饱和。2.4 速度剖面让路径真正“开得起来”路径平滑之后我加了一个速度规划模块核心逻辑是根据曲率反推限速再结合赛车加减速能力做平滑处理。标准做法是每个路径点的最大允许速度由向心加速度约束给出即v_max sqrt(a_cent_max / |κ|)其中a_cent_max是轮胎抓地力决定的横向加速度上限。弯道曲率大允许速度就低直道曲率小限速就可以逼近全局最高速度。算完限速曲线后还要做速度平滑。不能直接从高速一步刹到低速必须按车辆最大制动减速度约束在时间轴上做规划。这一步用动态规划或者向前向后两遍扫描都能实现向前扫描确保速度不超过限速包络向后扫描确保每个点都满足制动距离约束。这套速度规划做下来赛车会在进入大曲率弯道之前提前减速出弯之后加速回到直道最高速度整体行驶时间比匀速巡航快非常多。这也是自动驾驶赛车区别于普通移动机器人巡航的核心差异之一——路径规划不是终点速度分配才是真正拉开圈速差距的地方。3. Simulink仿真环境搭建与联调3.1 车辆动力学模型怎么选RRT本身在MATLAB脚本里就能跑但要让“赛车自动跑圈”形成完整闭环必须在Simulink里搭一个车辆动力学模型接收油门、刹车、转向指令输出车辆位姿和速度。我用的模型是单车模型也叫Bicycle Model。它假设车辆左右轮合并等效为一个前轮和一个后轮状态量是纵向速度、横向速度、横摆角速度和航向角。公式上是以质心处横向力平衡和横摆力矩平衡为核心的常微分方程组。为什么用单车模型而不是更复杂的七自由度模型因为路径规划验证阶段我们最关心的是车辆能否跟踪几何路径、横向动力学趋势是否正确而不是轮胎滑移率、悬架载荷转移这些细节。单车模型参数少、仿真快、物理意义直观用来验证RRT算法效果刚刚好。模型搭建上可以用Simulink自带的车辆模块库也可以自己写S-Function。个人建议是写一个MATLAB Function块把单车模型的微分方程直接放进去再用积分器闭环。好处是参数一目了然之后想扩展成四轮模型时替换起来也容易。控制量是前轮转角δ和纵向加速度a_x输出是x、y、航向角θ、纵向速度v_x、横向速度v_y和横摆角速度r。如果后续想更贴近真实赛车响应再引入魔术公式轮胎模型、悬架载荷转移这些但那是进阶手感调校的范畴前期验证路径规划完全用不上。3.2 路径跟踪控制器纯跟踪还是Stanley规划出来的路径最终要靠底层横向控制器来跟踪。Simulink里两种最常见选择是纯跟踪Pure Pursuit和Stanley控制器。纯跟踪的思路是找预瞄距离内的目标点计算车头方向与该点方向的角度差换算成转向角指令。它的特点是有前瞻性对路径平滑程度不敏感调参简单新手友好。缺点是预瞄距离需要随车速动态调整太短容易震荡太长又会“抄近路”切弯。Stanley控制器是另一种经典前轮控制方法直接控制前轮转角对准路径切线方向同时引入横向误差反馈项。它的稳态横向误差收敛特性比纯跟踪好在小曲率路径上跟踪误差很小但在曲率突变的地方响应更激进容易出现超调。我在这个项目里最终用了带动态预瞄距离的纯跟踪预瞄距离取L_d k*v L_min即车速越快预瞄越远。这个式子符合真实驾驶直觉高速下要看远低速下可以看近。实测下来配合平滑后的RRT路径横向误差能稳定控制在半个车宽以内对赛车这个场景来说完全够用。控制器在Simulink里就是一个子系统输入当前车辆位姿和参考路径点列输出前轮转角。内部逻辑用MATLAB Function块实现也就二三十行代码调试修改都很方便。3.3 RRT模块与Simulink的集成方式这是项目里联调最容易卡壳的地方。RRT规划是在MATLAB脚本里做的Simulink仿真在另一个环境里跑两边数据怎么接上决定着整个闭环能不能转起来。我采取的分层思路是“离线规划加在线跟踪”。主线流程是这样的第一层先用MATLAB脚本跑RRT得到全局参考路径保存成结构体或者timeseries加载到Simulink工作区。Simulink里的路径跟踪控制器直接从这个变量取点作为跟踪目标。第二层如果模拟比赛中的动态障碍物需要在线重规划可以在Simulink里加一个MATLAB Function块把RRT主函数包成仿真步内调用的接口。这个方案的代价是每个重规划周期都要等待RRT计算仿真实时性受影响较大一般只在低速场景或者预留足够时间时才可行。第三层是我个人推荐的前瞻避障方案不把RRT放在控制回路里实时跑而是用“障碍物感知加候选路径切换”的结构提前离线规划好几条典型赛线比如内线、中线、外线在线根据障碍物位置选择切换。这个方案兼顾实时性和稳定性但要求提前定义好候选路径集对赛道设计工作量有一定要求。很多同学在第一步就会卡住——代码算出了路径但不知道Simulink里怎么用。这里给一个调试经验先在MATLAB里用plot把路径显示出来确认数据形态是N×2矩阵还是N×3带航向再在Simulink里用From Workspace模块读取中间用Scope监视点列是否连续。数据形态一旦对不上控制器拿到的就是空矩阵整个仿真会直接跑飞。3.4 仿真条件设置与调参顺序Simulink仿真步长建议用固定步长取0.01秒即100Hz比较合适。路径跟踪控制器的输出刷新频率最好不低于10HzRRT重规划周期则要显著长于单次RRT计算时间否则仿真里会出现“规划还没完成车已经撞上去”的尴尬情况。整个项目的调参顺序有讲究。先调RRT的步长和目标偏置概率确保离线能稳定生成路径再调平滑参数看曲率曲线是否连续然后调速度规划限速保证最大侧向加速度在车辆极限内最后才调路径跟踪器的预瞄系数。顺序一乱问题混在一起很难定位是规划的问题还是跟踪的问题。每一层调到什么程度算合格我的经验是RRT每十次运行至少九次能在限定迭代次数内找到路径平滑后最大曲率不超过车辆极限速度曲线上没有任何一个点出现加速度突变最后在阶跃障碍物测试中横向误差不超过半个车宽。这几条都过了整个仿真系统才算真正“立住了”。4. 常见问题与排查技巧实录4.1 路径抖动与终点不可达这是RRT项目里最高频的问题。路径抖动通常来自最近邻搜索的随机性——同一障碍环境跑十次出来的路径可能十次都不同这是采样方法的特性不是bug。但如果抖动大到跟踪控制器无法收敛就要检查是不是步长设置太小导致路径毛刺过多。终点不可达有三个最常见原因。第一是目标偏置概率太低树迟迟不朝终点方向生长第二是最大迭代次数设置过小复杂地图里探索不够第三是碰撞检测的离散点间距太大导致终点附近区域一直被误判为碰撞。排查方法很简单——把RRT每一次扩展画出来一眼就能看出树卡在哪里、为什么过不去。我个人还会做一个压力测试把障碍物密度调高两倍验证当前参数是否仍然稳定。很多参数在简单地图上跑得好好的一换复杂环境就露馅提前做压力测试能省去大量事后排查时间。4.2 碰撞检测的边界误判碰撞检测最坑的边界问题是“刚好擦着障碍物边缘”。离散点间距设为栅格分辨率的1到1.5倍最稳妥太密计算量大到仿真卡顿太疏几米长的步长里可能穿过的障碍物被漏检。另外一定要给障碍物做膨胀处理把障碍物边界向外扩展一个安全距离通常取车辆半宽加一点余量。不膨胀的话规划出来的路径可能距离障碍物只有几厘米实际车辆稍微一贴边就撞上了。4.3 Simulink联调时的常见崩溃点联调中最常见的坑有三个。第一个是工作区变量名不一致RRT脚本里用的变量名和Simulink模块读的变量名对不上仿真一开始就报错。第二个是维度不匹配路径数组是N×2矩阵但控制器接口期望的是带航向的N×3矩阵导致矩阵运算维度错误。第三个是模型初始化顺序问题Simulink在仿真初始步就尝试取路径变量但此时工作区里还没有这个变量——解决办法是在仿真前把工作区变量全部加载好或者用Initialize Function回调处理。这三个坑每个我都踩过不止一次。调试方法无非是分段验证先把RRT路径画出来确认没问题再单独让控制器跟踪一条手工画的光滑曲线来确认控制器正常最后才把两者合到一起。跳过中间这一步直接做全闭环出问题后根本分不清责任在规划还是在跟踪。下面这张速查表是我在实际运行中最常翻的排障清单遇到问题可以先对照检查一遍问题现象常见原因处理优先级路径抖动明显步长太小 / 平滑参数不合适先调平滑再调步长终点长时间不可达目标偏置太低 / 迭代上限过小增大偏置概率车辆穿越障碍物障碍物未膨胀 / 碰撞检测间距过大立即修复碰撞配置Simulink报维度错误路径数组列数不匹配统一N×2或N×3格式跟踪横向误差大预瞄系数不合理按车速动态调整预瞄距离仿真卡顿严重KD树未使用 / 步长过小优先升级最近邻搜索4.4 从调试到验收的一个小方法这里分享一个我个人的验收习惯。在仿真里跑通之后我会故意把目标点放在一块极端困难的区域比如U型弯道最内侧检查RRT是否还能稳定规划出路径。如果这个场景过了再换一个几乎封闭的通道考察算法在“死路”附近的反应。这两个场景比普通绕障测试更能暴露参数问题也更容易看出你是否真正理解了RRT的行为逻辑——它会持续探索死路区域直到随机采样终于把它引向出口这个过程在图上会非常直观。5. 项目扩展方向与经验总结5.1 从基础RRT到RRT*和双向RRT如果想让规划质量再上一个台阶下一步扩展方向是RRT*。它在基础RRT基础上增加了“重新选择父节点”和“重布线路”两步代价是每次新节点加入后要检查邻域内的已有节点计算量会上升但路径代价长度、时间能显著下降。对赛车这种对时间敏感的工况RRT*在长直道上生成的路径会明显更直过弯时也更容易贴近最优赛线。双向RRT是另一个值得做的变体从起点和终点同时生长两棵树交替扩展谁离目标近谁先长。窄通道环境下单棵RRT需要很长时间才能钻进通道双向生长相当于两边同时向中间挤压收敛速度提升非常明显。代价是实现复杂度高一些要处理两棵树之间的连接判定和方向一致性问题。如果你手头已经有基础RRT的代码改造成双向版本是个不错的进阶练习。5.2 真车迁移要注意什么如果要把这套方案往真车上迁移有几个点需要提前做好准备。RRT的计算时延在真车上必须严格可控需要把算法从MATLAB原型移植到C和实时计算平台并对完整时序做分析。障碍物感知要靠激光雷达或视觉而不是仿真里理想的占用栅格。车辆模型参数需要标定轮胎、转向、刹车的响应滞后都要建进去否则离线调好的参数上车必出偏差。我做这个项目最大的体会是路径规划算法本身只是“上半场”平滑、速度规划、跟踪控制、仿真联调组成的“下半场”才是决定一辆自动赛车能不能跑完一圈的关键。纯算法演示和完整系统之间隔着的就是这一整套工程化细节任何一个环节掉链子整车表现都会很难看。5.3 给入门者的一条嘱咐最后说句实在话。刚接触RRT的话不要一上来就追RRT*、双向RRT、运动学RRT这些高级变体。先把最基础的五步循环用MATLAB跑通把碰撞检测、最近邻搜索这些最琐碎的环节彻底理解再逐步往里面加东西。我见过不少同学一上来就套用复杂框架调试时代码量巨大出了bug完全不知道从哪查起最后消耗掉的是最宝贵的学习耐心。这个项目后续能扩展的方向还有很多比如用模型预测控制做局部轨迹优化、引入动态障碍物轨迹预测、做多车交互博弈等等但前提都是把基础RRT这条主线走扎实。按我自己的经验把平滑、速度规划、Simulink闭环这几块都亲手跑通一遍你对路径规划整个链条的理解会比看十篇论文都有用。希望这篇文章能帮你少走一些弯路。
阅读完成 · 觉得有帮助？
