RRT路径规划算法在自动驾驶中的应用：车辆动力学与避障实现

做自动驾驶路径规划的人迟早会撞上同一个问题地图上明明画着一条很顺滑的曲线车就是走不了。我去年在某模拟项目里处理过一个直角弯场景最开始用A*算法给出一条折线路径结果车辆模型还没到第二个弯道就骑上了路沿。后来换成快速扩展随机树RRT并把车辆动力学约束直接织进树的扩展过程路径才真正可执行。这篇文章就围绕RRT在自动驾驶汽车路径规划中的应用展开重点讲清楚三件事算法本身怎么运转、车辆动力学怎么参与、静态障碍物怎么避同时结合一套Matlab源码15154期给落地细节。适合正在做路径规划课设、想用RRT解决实际车辆导航问题、或者刚接触采样规划算法的人参考不要求你有多深的数学底子但最好对车辆运动模型有个基本概念。1. 自动驾驶路径规划为什么盯着车能不能开过去而不是线好不好看1.1 几何路径和可行路径之间隔着车辆运动学常规路径规划教材里A*、Dijkstra这类算法在栅格地图上搜出来的是从起点到终点的一系列网格点把这些点连起来视觉上是一条路径。可是车不是扫地机器人那种能原地旋转的机构它有轴距、有前轮转角上限换句话说它受到非完整性约束。 非完整这个词听起来吓人实际意思是车辆不能朝任意方向直接平移它的瞬时速度方向始终和车身朝向保持一致。你在栅格地图上画一条横平竖直的折线车是没办法沿这条折线走的因为拐弯处的航向突变对应当的转向角早就超过物理极限了。所以路径规划的第一层问题不是找到一条安全、短的几何曲线而是找到一条满足车辆运动学约束、能够被实际跟踪执行的轨迹。几何路径是必要条件不是充分条件。考虑车辆动力学的规划器本质上在回答一个问题给定当前状态——位置、航向、速度、前轮转角——车能不能在不违反机械限制的前提下一步一步走到下一个状态。1.2 用自行车模型描述这辆车到底能怎么走工程里最常用的车辆运动学抽象是自行车模型它把四轮车简化成前后两个轮子后轮固定、前轮可以转动车辆绕瞬时旋转中心转动。模型的状态通常写成 [x, y, θ, v, δ]x、y是质心或后轴中心位置θ是航向角v是速度δ是前轮转角。运动方程是dx/dt v * cos(θ) dy/dt v * sin(θ) dθ/dt v * tan(δ) / L其中L是轴距。这个公式很多人见过但真正用起来以后会发现它直接决定了RRT扩展时从当前点能走到哪些邻居点的空间形状。δ有上限比如±30°那么单位时间内车辆只能走一定曲率的弧线而不是任意方向的直线。把这一层约束写进规划器生成的路径才有人开车的感觉。这里要补充一个容易误解的点很多教材把这个模型叫运动学模型而不是动力学模型严格说它没考虑惯性、轮胎力、加速度这些动力学量。但在路径规划语境下大家说的考虑车辆动力学通常包含两层意思一是运动学约束比如最小转弯半径和航向连续性二是最大转角、最大加速度这类执行器限制。RRT里最常接入的是运动学约束因为动力学微分约束会让扩展计算量大幅上升。15154期源码采用的就是自行车模型加转角限幅属于规划层面够用、实时性可控的折中方案。1.3 为什么RRT这类采样算法天生适合这个场景高维约束空间里图搜索算法要么需要离散化网格导致状态爆炸要么很难把复杂的微分约束融入搜索过程。RRT的思路不一样我不管整个空间长什么样就随机撒点然后一点一点把树往这些点方向长。因为树的扩展过程可以自由定义——你可以让扩展朝着满足车辆运动学的方式去生成——所以RRT能很自然地支持考虑动力学的路径规划。这也是为什么工程里经常把RRT、RRT*这类采样算法作为复杂约束下运动规划的入门选择。这里有个细节RRT不是最优算法它只能保证概率完备性意思是只要迭代次数足够多找到解的概率趋近于1。如果你需要最短路径或最平滑路径得在后面加平滑优化或改用RRT变体。但作为一套演示车辆动力学和避障逻辑的代码RRT是最好的教学载体。先把它吃透再往RRT或混合A*方向进阶会顺畅很多。2. RRT生长机制拆解随机采样、最近邻扩展、碰撞检测三件套2.1 RRT主循环到底在做什么RRT的核心是一个循环伪代码可以这样写% RRT主循环伪代码对应15154期源码的核心结构 for iter 1:maxIter x_rand sample(map, goal, goalBias); % 随机采样或者以一定概率取目标点 x_near nearest(tree, x_rand); % 在已有树里找离随机点最近的节点 x_new steer(x_near, x_rand, step, vehicle); % 朝目标方向走一步 if isCollisionFree(x_near, x_new, map, carPolygon) tree.nodes(end1) x_new; tree.edges(end1) {x_near, x_new}; if norm(x_new - x_goal) goalTol break; % 到达终点附近可以回溯路径 end end end理解这个循环的关键不是背代码而是明白四个动作各自干的事sample负责探索空间nearest负责选择离随机点最近的已有节点steer负责从near以固定步长朝rand方向生成新节点isCollisionFree负责安全检查。一旦新边和障碍物相交就丢弃这条扩展。整个树就是这样一点一点从起点向外长出去的。2.2 随机不是乱来采样策略和目标偏置如果每一步都完全均匀随机地在地图里撒点RRT会花大量时间探索无关区域尤其在窄通道环境里收敛非常慢。工程里最常见的改进是目标偏置采样以比如10%的概率直接把终点当作采样点让树被拉向终点其余90%保持随机采样来保证探索能力。可以理解成大部分时间去探路小部分时间惦记着终点。如果你把goalBias设成0树就会像无头苍蝇一样乱跑你可能要跑上万次迭代才碰巧让扩展节点落进终点附近的小圈子里。我在15154期源码里看到采样函数就是一个目标偏置函数可调参数叫goalBias默认设成0.1。实测下来把goalBias从0调到0.1收敛时间能缩短一半以上但如果调到0.5甚至更高树会频繁地朝终点直线方向怼遇到障碍物时反复撞墙反而降低探索效率。这个值建议在0.05到0.3之间调。2.3 从near到new步长和转向方式决定树的形状steer这一步是RRT的灵魂。最朴素的RRT用直线扩展从near点朝rand点方向走一个固定步长step。但考虑车辆动力学后直接连线往往不满足转向约束这个会在下一章专门展开。这里先单独说步长的作用step太大扩展粗犷可能很容易穿过窄缝或者撞障碍物step太小树长得密集迭代次数和碰撞检测次数暴增计算时间拉长。经验上step取地图短边尺寸的1/20到1/50比较稳妥具体要根据障碍物密度调。障碍物密集时步长要小一些否则一条扩展线段可能会跳过细小的障碍物间隙障碍物稀疏时步长可以适当加大减少树节点数量。另外树的扩展是基于最近邻的如果起点和终点之间存在一个狭长走廊RRT会在走廊开口处反复扩展但进不去这时候单纯调小步长不一定有用更有效的方法是加大maxIter或者调整采样分布让更多采样点落在走廊附近。碰撞检测这里先提一句每次扩展都要检查两个节点之间的连线是否穿过障碍物。在Matlab里最常见的是把地图转成occupancy grid然后沿线段密集取点查栅格值。这个函数会被调用上万次是性能瓶颈第四章会专门讲怎么优化。3. 把车辆动力学约束织进随机树状态空间扩展与控制采样3.1 树节点不只是坐标还要带着航向轮角如果你只想画一条几何曲线节点只要存[x,y]就够了。但考虑动力学之后每个节点至少要存[x,y,θ,δ]甚至还要存速度和加速度。原因很简单车辆能不能从A状态到B状态取决于这两个状态的航向差以及前轮转角。如果树节点只有坐标那么在扩展时无法判断转向是否可行。15154期源码里节点被定义成一个结构体包含x、y、theta、delta、cost、parentIndex。theta是航向角delta是前轮转角。把每个节点想象成车辆的一个瞬时状态树的边则代表从上一个状态执行什么控制输入到达这个状态。这种思路在文献里叫state space RRT本质上是把运动规划问题从几何空间提升到了状态空间。这也是为什么这套源码跑出来的路径节点序列里每个点都带有航向信息可以直接给下游的路径跟踪模块用。3.2 满足约束的扩展放弃直线段改用车辆模型前推常规steer的做法是线性插值但在动力学版本里有两种主流做法。第一种是控制空间采样在扩展时随机采样一组控制输入比如前轮转角和速度然后利用车辆模型向前积分一小段时间得到一条可达的弧线选取弧线末端作为新节点。第二种是利用Dubins曲线或Reeds-Shepp曲线在near状态和rand状态之间求解满足最小转弯半径的路径取其中一段作为扩展。15154期源码采用的是第一种因为实现相对直观也更容易理解车辆模型的作用。伪代码可以这样表达车辆前推扩展% steer函数内部示意15154期源码的简化版 function x_new steer(x_near, x_rand, step, L, deltaMax, v) desiredAngle atan2(x_rand.y - x_near.y, x_rand.x - x_near.x); % 计算期望转过的角度并限制在前轮转角范围内 delta desiredAngle - x_near.theta; delta wrapToPi(delta); delta max(min(delta, deltaMax), -deltaMax); % 用自行车模型推进一小段匀速假设 dt step / v; x_new.theta x_near.theta v * tan(delta) / L * dt; % 位置更新用均值思想减少欧拉法误差 midTheta (x_near.theta x_new.theta) / 2; x_new.x x_near.x v * cos(midTheta) * dt; x_new.y x_near.y v * sin(midTheta) * dt; x_new.delta delta; end这里有个很难从教材里直接学到的细节直接用欧拉法积分时如果dt太大车辆实际走过的圆弧和代码算出来的圆弧会有偏差导致碰撞检测线段与真实路径不一致。建议用两次近似或取中间状态来计算位置更稳妥。我实测中发现把积分步长切小、每步最多推进0.1米路径的碰撞检测才稳定车辆跟踪误差也明显下降。3.3 车辆参数对扩展结果的影响最小转弯半径、轴距、速度车辆参数不同树的生长姿态完全不同。轴距L越大车辆最小转弯半径越大扩展出的弧线越躺最大前轮转角deltaMax越小树越难以大角度转向。假如你把deltaMax设成90°那车辆运动学几乎退化成几何扩展算法会退化成接近普通RRT反过来设成10°在狭窄车库场景里树可能很难掉头迭代几万次都找不到出路。我跑过一组对比地图尺寸50x30米起点终点横跨整个地图轴距设成2.8米deltaMax从30°往下降到15°找到可行路径的迭代次数明显增加路径的转弯半径变大。如果你发现规划出来的路径曲率太大、车辆跟踪不上优先检查是不是deltaMax设得太大或者step设得太大。另外提醒一句源码里默认车速v是常数这在低速泊车场景是合理的但高速场景下速度本身也是规划变量就要考虑速度维度和加速度限制了。4. 静态障碍物场景下的避障实现地图建模和碰撞检测的工程取舍4.1 栅格地图、障碍物膨胀和车辆轮廓近似静态障碍物的避障首先要定义什么算撞上。15154期源码里地图用的是occupancy grid障碍物区域标为1可通行区域标为0。但车辆不是一个点如果只用质点去检测车虽然没碰到障碍物中心点车角却可能剐蹭到障碍物边缘。所以工程上要做两件事一是把障碍物按照车辆半径/半宽进行膨胀相当于给障碍物加胖一圈二是碰撞检测时用车辆的矩形轮廓而非单点。膨胀半径怎么取最稳妥的是取车辆外接圆半径也就是sqrt((车长/2)^2 (车宽/2)^2)。这样无论车辆朝向如何只要中心点不在膨胀后的障碍物里轮廓就不会碰到原始障碍物。缺点是会牺牲一些通道宽度但换来的是极低的风险。如果追求更精细可以按朝向做矩形障碍物检测用分离轴定理判断车辆矩形和障碍物矩形是否相交代价是计算量更大。我在另一个项目里吃过亏直接把障碍物栅格往外扩了两格就算膨胀结果车宽超过两格栅格的通道被判定为不可通行明明能过的窄路全被堵死。所以膨胀量一定要根据车辆真实尺寸和栅格分辨率换算不能瞎拍脑袋。4.2 碰撞检测的高频调用与效率优化RRT循环里碰撞检测是最热的热点。一次规划可能要迭代几千次每次检测又要在线段上采样几十个点总计算量非常可观。Matlab里如果不做优化一套50x30栅格地图的规划可能要好几分钟。我实测15154期源码发现三处优化至关重要第一把地图栅格转成逻辑矩阵用下标索引代替循环判断访问栅格值会快很多。第二碰撞检测时提前计算线段包围盒只有包围盒与障碍物区域相交才做逐点检查不相交的直接放行。第三用粗检测加细检测两段式先按车辆外接圆检测如果通过再按矩形轮廓精检。通过粗检测的大多数扩展可以直接接受或拒绝精检只在边界情况触发。除了实现层面的优化算法层面也可以通过控制扩展步长来减少检测次数。步长越大树扩展次数越少但每次检测的线段更长安全性下降。这个trade-off没有绝对答案。做性能对比实验时记得固定随机种子否则两次实验的性能差异很可能只是采样随机性带来的噪声不是参数真的变好了。5. 基于Matlab源码15154期的运行结构与改造思路5.1 源码模块结构拿到这套Matlab源码以后不要急着按F5。先看文件列表。按我的阅读习惯可以把它分成几类主脚本通常叫main_RRT.m、地图生成函数、车辆模型函数、RRT规划器函数、碰撞检测函数、可视化函数。阅读顺序建议是先从主脚本看起找到参数区熟悉起点终点和车辆参数是怎么设置的然后把RRT规划器打开对照第二章的主循环一步步理解最后再看碰撞检测和可视化的细节。主脚本的典型流程是初始化地图-设定起点终点-实例化车辆参数-调用RRT规划器-获得路径节点序列-从终点回溯parent指针得到完整路径-可视化。回溯路径这一点很容易被新手忽略树在生长过程中每个节点都有parentIndex最终从目标节点往回链到根节点才能得到从起点到终点的完整路径。我看到不少人在RRT得到终点被扩展到后就以为结束了其实还没回溯输出的只是一棵树而不是一条路径。5.2 核心参数一览我把源码里最重要的参数整理成表方便对照修改参数含义建议取值影响step扩展步长地图短边的1/20~1/50太大漏窄通道太小增加计算量goalBias目标偏置概率0.05~0.3越大收敛越快但探索能力下降maxIter最大迭代次数2000~10000过小可能无解过大耗时长vehicleLength/Width车辆外形尺寸按实际车辆设置影响膨胀半径和碰撞检测wheelBase轴距按实际车辆设置决定最小转弯半径deltaMax最大前轮转角20°~35°越小越难转向goalTol到达终点判定距离1~2米过小需要很多次迭代才能命中这些参数不是独立的。比如你把地图尺寸改大step和maxIter都要跟着动你把车型改成大巴wheelBase和vehicleLength变长膨胀半径也要同步变。最好的做法是集中放在主脚本顶部的参数区不要散落在各个函数里否则后面调参非常痛苦。5.3 想换地图、换车型怎么改很多人拿到源码后第一件事是换一张自己的地图。需要注意三个地方地图尺寸变了起点终点不能超出栅格边界障碍物密度变了步长和maxIter可能要同时调整车型变了不要只改车长车宽还要检查膨胀半径是否同步修改。我在自己的地图上测试时发现如果直接把别人地图里的step照搬过来要么树在障碍物密集区疯狂跳ping要么在空旷区浪费时间。所以建议每次换地图都做一个步长标定先粗略看一下地图里最窄通道的宽度让step小于通道宽度的三分之一这样扩展线段才不会一跨就跨过整个可通行缝隙。6. 实测后我踩过的坑树长不到终点、路径乱抖、碰撞误判6.1 树为什么迟迟不收敛最常见的现象是跑完maxIter树已经铺了很大一片终点周围也布满了节点但就是没有找到解。排查顺序先看goalBias是不是0只有纯随机采样很难精确命中终点再看step是否过大导致节点老是从终点旁边跳过去第三看碰撞检测的膨胀系数是否过大把终点附近的可行区域全膨胀掉了最后看maxIter是不是太小。有一次我把maxIter设成500在一张50x30地图上RRT怎么都到不了终点改成5000以后很快就收敛原因就是迭代量不够支撑随机探索。这个坑其实很好理解RRT是随机算法它不保证在固定的迭代次数内找到解maxIter只是一个预算预算太少就没法覆盖到终点。同理如果终点的goalTol设成1米而地图栅格分辨率是1米一格节点落点很难精确进入终点邻域可以把goalTol适当调大到2~3米。6.2 路径毛刺怎么处理RRT生成的路径往往带有随机性就算没有障碍物路径也会有一些不必要的锯齿或绕行。这不是bug而是随机采样算法的固有特性。处理办法有三种一是提高节点密度即减小步长让树的形状更细腻二是增大目标偏置让树更倾向终点三是在RRT之后接一个路径平滑步骤。我实际用的是第三种把RRT规划出的节点序列作为初始解再做一次简单的后处理把三个相邻点中间那个点向两边连线的中垂线方向挪一挪削弱尖角。如果只是想把车能开过去的路径拿来做仿真RRT的原始路径也够用毕竟车辆跟踪模块本身会做平滑。但如果你想展示的是规划结果直接可以作为控制参考那路径平滑基本绕不开。6.3 参数问题和解决方案对照根据我跑这套源码的体验把常见现象、可能原因和优先调整项整理成一张表现象可能原因优先调整长时间不收敛maxIter太小 / goalBias为0增大maxItergoalBias调到0.1路径穿障碍物膨胀半径不足 / step太大 / 检测点太稀增加膨胀系数减小step路径往返折返step太小 / 采样过散适当调大step增加goalBias运行时间太长碰撞检测优化不足 / maxIter过大粗检测精检或减小maxIter车辆跟踪失败没有考虑航向角 / delta未限幅检查状态是否包含theta和delta这些坑我在调试过程中都踩过。尤其是路径穿障碍物这个我一开始以为是碰撞检测写错了后来发现是膨胀半径用的外接圆半径但车辆在斜着走的时候矩形角仍然会插进障碍物边缘。最后的解决办法是粗检测用外接圆精检测时把车辆矩形离散成四个角点检查每个角点是否落在障碍物栅格里效果立竿见影。6.4 从静态场景向动态场景扩展的一点体会15154期源码只处理静态障碍物这符合它作为教学演示的定位。如果在实际项目里要扩展到移动障碍物一个比较自然的思路是在状态空间里加入时间维度让每个节点除了x、y、theta、delta之外还记录到达时刻t然后在碰撞检测时使用障碍物的预测位置。这种时空RRT的计算量会成倍增加但它的框架仍然是基于这篇源码里的RRT主干理解了静态版本动态版本只是在这个骨架上面加一个时间维。最后说点个人体会。我刚开始接触RRT时总觉得随机两个字不靠谱路径每次跑出来都不一样没有确定性。后来用多了才明白随机性恰恰是它面对高维约束空间时的武器你不需要把空间建模得很精确只要迭代得足够多它总能找到一条可行路径。对于自动驾驶汽车的静态场景验证这个算法足够直观也足够能说明问题。如果你手头的需求只是让一辆车在静态障碍物环境里从A走到B并且路径能被车辆动力学模型跟踪那这套RRT加车辆模型的做法完全够用别一上来就上重型优化算法。我目前还在做的一个扩展是把15200多行的大地图分块做分层RRT先在高分辨率地图上跑出一段段路径再在每段内部细化。回头有结果了再来补一篇这里先记录到这。