第一次把RRT跑到非完整系统上的时候我犯过一个特别蠢的错误明明车位几何上“完全能停进去”算法却怎么也搜不出一条可执行的路径。后来我才想明白问题不在搜索本身而在搜索时根本没考虑车辆只能“往前开、往后倒、按最小转弯半径拐弯”这个物理事实。今天这篇就来聊聊如何在地图级信息已知的前提下用基于信息的RRT方法informed RRT*解决带有非完整约束的车辆泊车辅助问题并给出可以直接跑起来的MATLAB代码骨架。这篇文章不是教科书式的算法介绍。我把重点放在三个地方一是非完整系统到底改变了路径规划的哪些假设二是informed RRT相比标准RRT在泊车场景里改了什么、为什么这么改三是把整个算法拆成模块后每个核心函数在MATLAB里该怎么写、调试时又会踩哪些坑。适合对机器人/自动驾驶路径规划有一定了解、想动手做车辆泊车demo的工程师和学生。1. 泊车为什么难非完整系统到底“别扭”在哪1.1 阿克曼转向模型与车辆的运动边界先看一辆普通乘用车。按阿克曼转向模型简化车辆是一个矩形加上前轮转角δ、后轮速度v轴距为L的运动体。忽略轮胎侧偏和纵向滑移时运动学方程写作x_dot v · cos θ y_dot v · sin θ θ_dot v / L · tan δ其中(x, y)是后轴中心位置θ是车头朝向。这组方程看起来平平无奇但它描述的本质是车辆任何时刻只能沿着当前车头方向移动不能横向平移也不能原地转向。两个控制量v和δ要驱动一个三维状态空间(x, y, θ)所以系统是欠驱动的。这类约束还有个更硬核的称呼——非完整约束。用大白话说无人机、AGV那类“想往哪走就往哪走”的规划思维在汽车上完全不成立。你让几何规划器生成一段漂亮的圆弧或直线组合车辆未必真的能以固定的前轮转角跟得上反过来说规划器如果只是把坐标点序列“画”出来而没有考虑曲率连续、最小转弯半径这些限制泊车控制器根本没法跟踪。1.2 几何可达不等于运动可达很多初学者会先做个几何路径比如从车位外任意一点到车位内部某点画一条光滑曲线检查曲线不撞墙就算成功。这么做在小车上可能还能对付但在狭小车位场景里很快暴露问题。举个真实感受过的例子一个车位长5.8米车长4.7米理论上纵向空间足够。但你要“一把倒进去”车从直行到转向的过渡必须满足最小转弯半径约束。如果起点角度不对或者路径中间有一段曲率半径小于5米那这段路径在运动学上就不可行。几何可达只回答了“空间上能不能从A点到B点”而泊车要回答的是“在这台车的最小转弯半径、最大前轮转角、挡位切换次数限制下能不能从A点开到B点”。1.3 泊车场景对路径规划的额外要求泊车辅助和高速巡航这类任务还有一个不同终点精度要求极高。车最终要停在车位正中、车身摆正横向和纵向误差通常要控制在0.1米左右航向误差控制在5度以内。这对规划器提出了两个要求一是路径的末端必须满足精确位姿不能“差不多到了就停”。二是路径要足够平滑。目标点附近的微小角度误差会被控制器放大导致实际停车姿态偏斜。除此之外泊车路径往往是低速、带多段倒车/前进切换的规划算法必须显式支持倒挡不能只搜索正向运动——这点在后面的Reeds-Shepp曲线连接里会反复出现。2. RRT到Informed RRT为什么泊车要用“带信息”的随机树2.1 标准RRT的工作方式与局限标准RRT快速探索随机树在高维、非凸障碍空间里表现不错因为它不需要对障碍物做精确建模只需要一个碰撞检测函数。它的核心循环很简单从起点生成一棵树每次随机采样一个目标状态从已有节点里找一个“最近邻居”沿采样方向扩展固定步长生成新节点如果新节点无碰撞就加入树。但标准RRT有个很明显的毛病它在整个可行空间里均匀地到处采样。对于泊车这种“起点和终点都很明确”的问题均匀采样意味着大量节点消耗在路径根本不可能经过的区域。比如车辆在车道上车位在右侧算法却把大量节点扔在左侧最终树的规模很大规划的时长不可控。更麻烦的是非完整系统的“最近邻居”不能直接用欧氏距离衡量。因为车辆从一个位姿到另一个位姿的运动代价取决于RS曲线长度、挡位切换次数和曲率限制而标准RRT在扩展阶段没有利用这些信息导致很多扩展尝试都因为不满足运动学约束而失败。2.2 Informed RRT*改良了什么Informed RRT*也就是“基于信息的RRT”的核心改良点非常聚焦——把采样区域收缩到以起点为焦点、终点为另一个焦点的椭圆内。椭圆的几何依据是如果当前已知一条最优路径长度是c_best那么比它更优的路径其长度必然小于c_best。而一条路径如果连接起点和终点其总长度至少是两点之间的直线距离c_min。根据椭圆的性质路径长度不超过c_best的所有可行点都必须位于这个以起点/终点为焦点、长轴为c_best的椭圆内。换句话说一旦找到一条初始可行路径后续采样就只在这个椭圆内进行而不是在整个地图上乱撒点。这样树的扩展会越来越集中于“可能改进当前解”的区域收敛速度明显加快。RRT*本身还有重连机制保证路径的渐近最优性但在泊车场景里最直接的收益其实是“更快找到第一条能开进去的路径”。2.3 为什么这个改进对泊车是刚需泊车天然是个目标明确的优化问题不是探索型问题。你要的不是“看看地图上有什么可能路径”而是“从已知位姿进入已知车位的某条好路径”。Informed RRT*把目标信息注入采样过程这在泊车场景里价值巨大。我在跑仿真时做过对比同样的车位、同样的车辆参数标准RRT需要大概2万到3万个节点才能稳定找到一条可行路径Informed RRT*在找到第一条可行路径后往往再跑一两千个节点就能把路径长度从30米优化到20米以内。差距不在于树本身的扩展逻辑而在于“在哪里采样”这个根本环节。3. 核心建模与MATLAB模块拆分3.1 状态空间、控制输入与运动学离散化在MATLAB里实现前先把状态和控制定义清楚。我用的状态是状态向量[x; y; theta]分别为后轴中心的横坐标、纵坐标、航向角控制输入[v; delta]分别为车速前进为正倒车为负和前轮转角车辆参数轴距L、车长l_w、车宽l_h、最小转弯半径rho低速泊车场景可以进一步假设车速恒定只关注转向。树中的每条边本质上是“从父节点位姿出发按固定控制输入积分一小段时间后得到的新位姿”。离散化时不要把dt定得太小以致计算量爆炸也不要把dt定得太大导致轨迹失真。我一般取dt0.05秒扩展步长限制在0.2到0.3米之间。3.2 椭圆域采样是怎么写出来的先给出informed RRT*采样域的MATLAB实现。这个函数会在主循环里被调用返回值是一个随机位姿。function q sampleInformedEllipse(pose0, poseG, bestCost, bounds, params) if isinf(bestCost) % 还没有找到过可行路径此时做全地图均匀采样 x rand * (bounds(1,2) - bounds(1,1)) bounds(1,1); y rand * (bounds(2,2) - bounds(2,1)) bounds(2,1); theta (rand * 2 - 1) * pi; q [x; y; theta]; return; end % 椭圆参数 c_min norm(pose0(1:2) - poseG(1:2)) / 2; % 半焦距 c_cur bestCost / 2; % 半长轴 b sqrt(c_cur^2 - c_min^2); % 半短轴 a c_cur; % 单位圆内均匀取点 r2 rand * 2 - 1; r1 rand * 2 - 1; while r1^2 r2^2 1 r1 rand * 2 - 1; r2 rand * 2 - 1; end x_unit r1; y_unit r2; % 映射到椭圆 x_e a * x_unit; y_e b * y_unit; % 旋转到起点-终点连线方向再平移到中点 psi atan2(poseG(2) - pose0(2), poseG(1) - pose0(1)); R [cos(psi), -sin(psi); sin(psi), cos(psi)]; center (pose0(1:2) poseG(1:2)) / 2; xy R * [x_e; y_e] center; % 角度仍随机因为车位入口方向对航向非常敏感 theta (rand * 2 - 1) * pi; q [xy(1); xy(2); theta]; end这个函数一定要放在主循环之前单独测试一遍。很多人在泊车demo里看到的树“只有一条直线长过去”就是因为椭圆的旋转矩阵写错或者au单位圆采样方法不对。单位圆均匀采样用拒绝采样即可效率足够没必要引入更复杂的极坐标变换那反而会把分布搞偏。3.3 最近节点求取与RS扩展策略非完整系统的最近邻居不能直接按欧氏距离选。更稳妥的做法是取欧氏距离最近的K个候选节点分别尝试用Reeds-Shepp曲线做Steer挑出代价最小且不碰撞的作为扩展结果。K一般取10到20。这个“投机”做法很适合工程调试你不需要在一开始就造一个完美的非完整度量只要候选池够大RRT依然能稳定生长。Steer的实现核心是RS曲线连接函数。简单说RS曲线就是从当前位姿到目标位姿、满足最小转弯半径和前进/倒车切换的最短路径形态结果是由直线段、左转圆弧段、右转圆弧段拼接而成。在MATLAB里可以封装一个函数function [seg, cost] steerRS(qNear, qRand, rho) % 基于Reeds-Shepp曲线返回从qNear到qRand的轨迹段 % 若转角超过限制则返回空 [pathData, len] rsPath(qNear, qRand, rho); if len params.maxStep % 只截取前maxStep长度的轨迹作为新节点 seg trimPath(pathData, params.maxStep); cost params.maxStep; qNew seg.end; else seg pathData; cost len; end end这里有个极其重要的细节RRT扩展步长的截断必须是“沿RS曲线截取”而不是把整条RS曲线偷偷放进树里。如果直接把整段RS曲线当作树的边树会失去逐步扩展的优势障碍物之间的大型跳跃会变得难以处理。截取后的终点位姿才应当成为树中的新节点。3.4 碰撞检测模型的车辆矩形与车位表达泊车场景的碰撞检测要比普通点状机器人复杂。车辆是有尺寸的必须在每个轨迹点上生成车辆矩形再去检查矩形是否与车位边界、障碍物发生交叠。我的做法是将车辆后轴中心离散到若干轨迹采样点每个采样点处按当前航向角生成四个顶点坐标。假设车辆长l_w、宽l_h后轴中心在车辆几何中心靠后0.9倍轴距处四角坐标可以写成function corners vehicleCorners(x, y, theta, lw, lh) c cos(theta); s sin(theta); % 车身中心的偏移 off_x 0.1 * lw; % 根据实际情况调整 center_x x off_x * c; center_y y off_x * s; hw lh / 2; hl lw / 2; corners [center_x - hl*c hw*s, center_x hl*c hw*s, ... center_x hl*c - hw*s, center_x - hl*c - hw*s; ... center_y - hl*s - hw*c, center_y hl*s - hw*c, ... center_y hl*s hw*c, center_y - hl*s hw*c]; end然后对车位矩形做多边形相交判定。MATLAB里可以用polyshape对象加intersect函数很方便但注意程序性能每个节点最多有几十个轨迹采样点每个采样点都要做一次polyshape相交跑几千个节点会有点慢。优化思路是先用AABB粗过滤再去精度判定或提前把车位/障碍物表示成顶点列表用线段相交判断不放polyshape。碰撞检测里最容易影响结果的是“膨胀距离”。车辆和车位之间留多少安全余量我一般设0.05到0.1米。留小了看着吓人但确实能塞进去留大了算法会认为车永远进不了车位。4. 主循环、路径回溯与平滑后处理的代码骨架4.1 Informed RRT*主循环代码结构把上面各个模块组装起来主循环可以写得非常清晰function [path, tree] informedRRTStarParking(pose0, poseG, bounds, obstacles, params) tree(1).q pose0; tree(1).parent 0; tree(1).cost 0; bestCost inf; bestIdx -1; for iter 1:params.maxIter % 1. 采样 qRand sampleInformedEllipse(pose0, poseG, bestCost, bounds, params); % 2. 找K个近邻候选 idxCand knnCandidates(tree, qRand, params.knn); % 3. 对候选节点做RS扩展 bestNew []; bestParent -1; for i 1:length(idxCand) qNear tree(idxCand(i)).q; [seq, cost] steerRS(qNear, qRand, params); if isempty(seq), continue; end if ~isCollisionFree(seq, obstacles, params), continue; end if tree(idxCand(i)).cost cost bestCost ... tree(idxCand(i)).cost cost tree(idxCand(i)).cost cost % 基础版本 bestNew seq; bestParent idxCand(i); break; end end if isempty(bestNew), continue; end % 4. 加入树 idxNew length(tree) 1; tree(idxNew).q bestNew.end; tree(idxNew).parent bestParent; tree(idxNew).cost tree(bestParent).cost bestNew.cost; % 5. 检查是否进入目标区域 if norm(tree(idxNew).q(1:2) - poseG(1:2)) params.tolXY ... abs(angleDiff(tree(idxNew).q(3), poseG(3))) params.tolTheta if tree(idxNew).cost bestCost bestCost tree(idxNew).cost; bestIdx idxNew; end end end if bestIdx 0 path backtrack(tree, bestIdx); else path []; end end上面代码里我故意保留了一行注释“基础版本”因为真正完整的RRT*还需要对邻居节点做“重连”操作——检查新节点能否降低某些邻居的累计代价如果可以就改父节点。在泊车这样终点明确的场景重连不是最紧急的优化但没有它路径长度不会自愈。建议在拿到基础版本后补上重连逻辑。4.2 路径回溯从树节点还原完整轨迹树里每个节点存了父节点索引回溯就是从目标节点一路顺着parent指针走到起点。function path backtrack(tree, idx) path []; while idx 0 path [tree(idx).q, path]; idx tree(idx).parent; end end但注意这里回溯出来的只是“节点位姿序列”不是完整连续轨迹。我们之所以还能得到连续轨迹是因为树中每条边本身就是一条RS路径段。更合理的做法是把每一条边的路径段都存下来回溯时拼接。我自己的实现里树节点结构保存了edgeSegments回溯时直接把这些segment拼接成一条大路径这样后处理阶段才能对整个路径做平滑和长度统计。4.3 用Shortcutting把锯齿路径修成能开的路RRT生成的路径即使满足曲率约束节点切换处也常有冗余的S形路径长度也不是最优的。常用的后处理是shortcutting随机选路径上两个相距较远的点尝试用RS曲线直接连接如果新连接无碰撞且总长度更短就替换掉中间那段。function path smoothPath(pathRaw, obstacles, params) improved true; while improved improved false; n length(pathRaw); for i 1:n-2 for j min(i2, n):min(n, i10) segTmp rsPath(pathRaw(i).q, pathRaw(j).q, params.rho); if isempty(segTmp), continue; end if ~isCollisionFree(segTmp, obstacles, params), continue; end currentCost sumPathCost(pathRaw, i, j); if segTmp.cost currentCost - 0.05 pathRaw [pathRaw(1:i), segTmp, pathRaw(j:end)]; improved true; break; end end if improved, break; end end end path pathRaw; end这个平滑过程我一般循环10到20轮就收敛。另外提醒一句如果每一步都重新生成RS路径计算量会比较大落地时可以限制最多尝试路径中不超过15%的点对效果依然很好。5. 调试泊车Demo时最值得记录的四个坑5.1 目标偏置太大树直接朝着车位墙猛长为了让算法快速接近车位很多人的第一个想法是把目标位姿作为采样结果的概率调高比如0.5甚至0.9。结果就是RRT每次扩展都努力往终点方向冲但中间很多步都撞墙树长不出来收敛反而更慢。原因在于目标偏置破坏了RRT的探索性。泊车问题的关键是“先探索到能绕入车位的那条通道”而不是“一开始就盯着车位中心”。我实测下来目标偏置概率设在0.02到0.1之间比较合适剩下的交给椭圆采样去引导。如果发现必须靠大偏置才能找到解说明椭圆采样或候选邻居选择逻辑有问题不要用偏置概率硬凑。5.2 膨胀距离太保守车位直接“消失”了我最初把车辆安全余量设为0.3米结果算法永远找不到能塞进窄车位的路径。仔细一算车位宽2.4米车宽1.9米左右各留0.3米余量之后安全宽度只剩1.3米几乎比车窄了1/3这物理上可能吗当然不可能。安全余量要根据真实工程需求定。系统里如果有高精度摄像头/雷达0.05到0.1米足够如果是纯GPS定位0.3米都不够。这里真正要调的是“感知不确定性预算”不是单纯为了避开障碍。仿真demo里给个0.05到0.1米就能很好跑。5.3 步长太大非完整约束被“离散”没了有一版我把扩展步长设成1米想着减少节点数、跑快点。结果路径在车位前的折线特别生硬有些地方甚至出现“车头方向没转够、车直接横着挪”的错觉。仔细排查后发现步长不变、dt固定时车辆在1米内能完成的转角非常有限树的自由度反而下降。步长0.2到0.3米是个比较稳的经验区间。如果你要处理大型车辆比如卡车步长可以适当加大到0.5米左右但前提是转弯半径足够大。步长太小时树节点数暴涨所有时间都花在碰撞检测上同样不建议。5.4 一组可复现的调试参数速查表我调通这套泊车辅助demo时使用的参数放在下面供参考参数典型值调参说明车辆轴距 L2.8 m车辆模型的基本尺寸最小转弯半径 rho5.0 m由前轮最大转角决定扩展步长 maxStep0.25 m每步距离控制树的分辨率目标偏置概率0.05过大则探索不足最近邻候选数 K10取多个候选做RS扩展安全余量0.08 m根据定位精度调整终点横纵向误差0.1 m车辆停车精度目标终点航向误差5 deg角度约东最大迭代次数5000超过则返回失败调试顺序建议先固定车辆参数和安全余量再调步长和迭代次数最后微调候选邻居数。不要一口气改多个参数否则很难判断是哪一环导致失败。6. 这套方案还能往哪里延伸从辅助泊车到自动泊车系统6.1 动态场景与增量式重规划这篇里用的Informed RRT*是静态规划算法但泊车场景里行人、其他车辆可能移动。实际做自动泊车系统时通常的做法是让规划器每隔100到200毫秒感知一次环境把车辆当前位置作为新的起点、目标点保持不变重新调用这套规划流程。由于椭圆采样区域只在当前最优解附近收缩重规划速度很快能够跟上低速场景的动态变化。6.2 规划结果如何交给控制层规划器输出的是由多段RS曲线拼接的路径但直接执行会有问题每条边的起始点处曲率可能跳变。低速泊车控制器一般用轨迹跟踪例如Stanley或纯跟踪对曲率跳变的容忍度有限。更稳妥的做法是在路径后方接一个局部轨迹平滑器或者用更精细的非线性MPC对路径做跟踪与修正。规划层只要保证“可行且无碰撞”控制层再在跟踪过程中做二次优化。6.3 怎么衡量算法好坏跑通demo之后建议用三个指标衡量整体效果成功率100次随机起点里有多少次能找到可行路径并安全入库、平均规划时间、路径总长度/挡位切换次数。我看到很多文章只展示一条漂亮轨迹却没报成功率。实际上泊车场景起点稍微偏一点算法可能就从90%成功率掉到60%。如果需要稳健性就必须在你的参数表里把起点采样范围、障碍物布置、车位宽度都纳入测试集。这套基于信息的RRT方法加上RS曲线连接和非完整运动学分枝是我觉得最适合入门车辆泊车规划的路径之一。它比纯几何规划器更能应对非规则车位又比昂贵的优化方法更容易调试和理解。如果你照着代码骨架跑通了第一个demo记得把车位宽度从2.4米慢慢往窄调比如调到2.15米你会直观感受到非完整约束和碰撞余量对成功率的影响有多大——那种“明明肉眼看着能停算法却怎么都绕不进去”的感觉恰恰是理解这套算法的最好课堂。