做无人地面车辆UGV路径规划很多人第一个跑通的是A或者RRT。但真把车放到场景里跑一圈你会发现一个很尴尬的问题全局路径算得再好地图里多出一个箱子、一辆临时停靠的车或者墙边有一片一开始没扫出来的障碍原来那条“最优路径”就立刻变成了不可行路径。我这次分享的方案就是基于DLite做全局路径重规划、叠加横向避障算法处理局部突发障碍的一套Matlab实现框架。核心思路说白了就一句话全局D* Lite保证“能走到并且尽量短”局部横向避障保证“不会一头撞上去”两者合起来才是真正能让无人地面车辆跑起来的最优路径。这篇内容适合正在做移动机器人、无人地面车课题的研究生也适合想在课程设计里把路径规划做得更系统一点的本科生。我会把这套方案里最关键的算法原理、Matlab实现细节、参数调试方法还有我踩过的坑一次说清楚。1. 这个方案到底在解决什么问题1.1 单一全局路径规划的先天短板先说说为什么不能只靠A这类静态全局规划。A在已知静态地图上确实能找到最短路径但它有一个隐含假设地图从规划开始到执行结束始终保持不变。而UGV实际跑起来环境几乎不可能不变。激光扫描到一片之前没有的障碍区地图更新之后原来算好的路径可能正好穿墙而过。有人会想那简单障碍更新一次我就重新跑一次A*。这在理论上可行但实际代价很大。假设地图是150×150的栅格A*每次搜索都会访问上万个节点如果车辆每秒运动一步、每一步都可能触发重规划运算开销会急速膨胀。更重要的是每次重规划都是“从零开始”完全浪费了上一次搜索结果中仍然有效的信息。1.2 全局路径与局部避障的分工逻辑这里就需要理解“全局路径生成最优路径”这个标题的含义。它不是说只做一次全局规划而是让全局和局部形成一套配合机制全局层维护一张最新地图负责从当前位置到目标点的拓扑最优路径一旦地图变化影响到了当前路径就触发增量式重规划。局部层车辆的感知范围毕竟是有限的。当障碍出现在前方几米内、但还没被确认为地图上的永久障碍时局部横向避障先出手在路径的横向方向找绕行点让车辆安全通过。回归层绕过障碍后车辆重新回到全局参考路径上继续执行D* Lite给出的最优路线。说白了D* Lite解决的是“路怎么走”的问题横向避障解决的是“眼前这块怎么绕”的问题。两者不是二选一而是上下承接的关系。这也是我认为这套标题背后真正想表达的东西不是单独调一个算法而是把全局最优增量重规划和局部实时避障做成一个闭环。2. D* Lite在全局规划层到底强在哪里2.1 反向搜索与rhs/g值传播的原理D* Lite是Koenig和Likhachev在2002年前后提出的增量搜索算法可以说是经典D*的简化改版。它最核心的几个设计我拆开讲。首先它的搜索方向是反的。A从起点搜向目标DLite从目标点搜向起点。为什么要反着搜因为车辆是不断前进的当前位置在变目标点通常不变。如果从目标向起点做反向搜索那么车辆每向前挪一步上一轮搜索得到的绝大部分“启发信息”都还能复用真正需要更新的只是局部区域。其次它维护两个代价值g值表示从目标点到当前节点的已知最短路径代价rhs值表示基于当前后继节点信息计算出来的“一步前瞻最短代价”。两者之间的数学关系是rhs(s) 0如果s是目标节点 rhs(s) min( c(s, s) g(s) )s是s的所有后继节点当一个节点的g值等于rhs值时这个节点处于一致状态可以认为它的代价信息已经收敛。如果g值大于rhs值说明存在一条更短的路径还没被吸收需要更新如果g值小于rhs值说明原来算出来的代价已经被环境变化破坏了同样需要修正。D* Lite的优先队列里保存的正是这些“不一致”的节点队列排序用的不是单一数值而是一个两维Key。我在Matlab里实现时Key的计算是function key calcKey(map, s, startNode) k1 min(map.g(s), map.rhs(s)) map.h(s, startNode); k2 min(map.g(s), map.rhs(s)); key [k1, k2]; endk1是路径代价加启发式剩余距离k2是当前已知代价。优先队列先比较k1再比较k2。这样设计的意义在于队列顶部永远是“最需要被处理”的节点搜索每次弹出并修复它直到起点节点也达到一致状态。2.2 为什么它比“A*全图重跑”更适合动态环境我用一张对比表来说清楚A*、经典D*、D* Lite在动态环境下的差异算法搜索方向环境变化后的处理方式历史信息利用A*静态正向不处理路径失效无A*全图重规划正向删掉旧结果从零重算几乎无D*反向从受影响的节点开始传播更新大量复用D* Lite反向局部更新只修复不一致区域大量复用A全图重跑的问题在于它没有区分“路径受影响的区域”和“没受影响的区域”。DLite的做法聪明在当地图变化时只把受影响节点周围的一小片区域重新加入优先队列那些离得远、没受影响的区域直接保留旧结果。实践下来在栅格地图上如果每次环境变化只影响一小块区域D* Lite单次重规划的开销通常只有A*全图重跑的几十分之一。2.3 在Matlab里如何组织D* Lite的数据结构我用自己的模拟项目X做过一版Matlab实现数据结构这么组织地图层用一个逻辑矩阵obsMap表示障碍物1为障碍0为可通行一个代价矩阵costMap预先算好障碍膨胀区内代价设为无穷。节点状态层维护gMatrix和rhsMatrix大小与栅格地图一致初始g均为无穷大。优先队列层因为Matlab没有内置可更新键的优先队列我用结构体数组加排序索引来实现每个队列元素包含节点编号和对应的Key值。初始化时从目标节点开始调用更新函数把目标节点的rhs设成0并加入队列。然后反复执行ComputeShortestPath循环直到起点一致。这里有一个容易搞错的点因为D* Lite是反向搜索代码里说的“后继节点”在直观上实际是实体车辆朝目标方向走时的“前方节点”而“前驱节点”才是车辆当前可以走回去的位置。方向一旦搞反整个搜索就乱套。3. 横向避障策略的绕行逻辑与代价设计3.1 在路径横截面上的采样绕行思路横向避障这个设计直观理解就是在车辆当前位置的路线上展开一个“横截面”在这个横截面上找绕开障碍的最优横向偏移点。先做投影把车辆当前位置投影到全局参考路径上得到一个参考点和参考方向。参考方向用切线方向表示法向量就是垂直方向。然后以车辆位置为中心沿法向量向左右两侧按固定步长采样出一系列候选点。每个候选点做一个可行性与代价评估。我用Matlab实现的核心逻辑框架是这样function bestPoint lateralAvoidance(center, heading, obsMap, params) n params.nSamples; % 横向采样点数 dMax params.dMaxLateral; % 最大横向偏移量 nx -sin(heading); % 法向量x分量 ny cos(heading); % 法向量y分量 ds linspace(-dMax, dMax, n); bestCost inf; bestPoint center; for i 1:n cand center ds(i) * [nx, ny]; if ~isInsideMap(cand, params) || isObstacle(cand, obsMap, params) continue; end cost lateralCost(cand, center, heading, obsMap, params); if cost bestCost bestCost cost; bestPoint cand; end end end沿法向量均匀采样这个操作本质上就是在问我往左偏多少、往右偏多少才既能躲开障碍又不至于离全局路径太远。采样点数不能太少我一般取21到31个点太少的话转弯不够细腻太多的话计算量上去了但收益有限。3.2 代价函数的设计四个维度的取舍横向避障的候选点选哪个完全取决于代价函数怎么设计。我用的代价函数长这样function cost lateralCost(cand, center, heading, obsMap, params) dObs nearestObstacleDistance(cand, obsMap, params); offset abs(localOffset(cand, center)); angleDiff headingDifference(heading, cand - center); curv localCurvature(cand, params); cost params.w1 * exp(-dObs / params.r0) ... params.w2 * offset ... params.w3 * angleDiff ... params.w4 * curv; end四项的含义和调参原则障碍距离惩罚项候选点离障碍越近代价指数上升得越猛。r0决定了影响半径一般取0.5到1个车辆半径。横向偏移惩罚项候选点偏离原路径越多代价越大这样车辆不会为了绕一个小障碍而长时间远离全局路径。航向差惩罚项候选点相对当前车头方向的转角越大代价越高防止车辆原地猛打方向。曲率平滑项候选点附近路径的弯曲程度用来过滤掉那种蛇形走位。权重怎么配我自己常用的起步值是w11.0、w21.2、w30.8、w40.3。注意w2要稍微压得住w1否则车辆看到障碍就大幅横移绕完以后很难收回来但w2如果太大车辆又会顶着障碍硬走把安全距离全部吃掉。3.3 绕完障碍之后如何回归全局路径横向避障不是独立跑完整段路它只是“局部解围”。因此必须设计回归机制。我的做法是记录当前车辆位置到全局参考路径的横向投影距离。连续检测如果横向距离连续若干步都小于某个阈值比如小于0.5个栅格就认定车辆已经绕开障碍。将控制目标从“横向避障候选点”切换回“全局路径上的前瞻点”。这里有小技巧切换不要太急。我习惯加一个滞回区间比如偏离超过1.2米时强制避障回到0.6米以内才允许切回全局路径。这样避免了车辆在临界点上反复横跳。此外绕障结束后车辆往往会与全局路径存在一个角度偏差直接硬切会让方向盘猛打一下最好用一个线性插值窗口让参考点逐渐回归原始路径这样跟踪曲线会平滑很多。4. Matlab实现主循环架构与核心代码4.1 整体仿真循环的结构一套完整的仿真程序我通常分成四个文件环境初始化、D* Lite全局规划、横向避障模块、主循环。主循环的执行流程没有用复杂建模是标准的分层控制框架%% 主循环globalPlan - localAvoidance - vehicleUpdate - replan for step 1:maxSteps % 1. 获取当前位姿与全局参考点 curPos vehicleState(1:2); [refPoint, refHeading] getReferencePoint(globalPath, curPos); % 2. 局部感知检查前方是否出现新障碍 sensorMap updateLocalMap(obsMap, curPos, sensingRange); [hasRisk, obstaclePos] checkRisk(refPoint, sensorMap, params); % 3. 生成控制参考点 if hasRisk refPos lateralAvoidance(curPos, refHeading, sensorMap, params); else refPos purePursuitRef(refPoint, curPos, params.lookAhead); end % 4. 更新车辆位姿简化运动学模型 [vehicleState] updateVehicle(vehicleState, refPos, params); % 5. 维护全局地图并触发D* Lite增量重规划 if mapChanged [map] updateGlobalMap(obsMap, newObstacle); [globalPath] dStarLiteReplan(map, robotStart, goal); end % 6. 到达终点判断 if norm(curPos - goal) params.goalTolerance break; end end这个循环里有几个容易被忽略的细节。局部感知模块里我只把车辆前方一定角度和距离内的障碍标记为“风险障碍”而不是全图障碍都触发避障否则后方一个八竿子打不着的障碍也可能让车辆乱绕。全局重规划不是每步都触发必须检测到新障碍确实落在当前路径的安全走廊内才下手重算。4.2 D* Lite两个容易写错的核心函数D* Lite的核心在UpdateVertex和ComputeShortestPath这两个函数上我给出一版可以跑通逻辑的Matlab骨架。UpdateVertex负责更新单个节点的rhs值并根据节点状态决定是否加入优先队列function map updateVertex(map, s, targetNode) % D* Lite反向搜索rhs取后继节点的最小值 if s ~ targetNode minRhs inf; succList getSuccessors(map, s); for i 1:length(succList) s2 succList(i); tmp map.gMatrix(s2) edgeCost(map, s, s2); if tmp minRhs minRhs tmp; end end map.rhsMatrix(s) minRhs; end % 从优先队列中移除旧条目若不一致则重新插入 map.U queueRemove(map.U, s); if map.gMatrix(s) ~ map.rhsMatrix(s) key calcKey(map, s, map.startNode); map.U queueInsert(map.U, s, key); end endComputeShortestPath则是在优先队列上反复修复不一致节点直到起点一致或者队列为空function map computeShortestPath(map) while ~queueIsEmpty(map.U) [sMin, keyOld] queueTop(map.U); keyNew calcKey(map, sMin, map.startNode); % 如果Key变小需要更新条目重新排队 if keyNew(1) keyOld(1) || ... (keyNew(1) keyOld(1) keyNew(2) keyOld(2)) map.U queueUpdateKey(map.U, sMin, keyNew); continue; end % g大于rhs说明有更优代价吸收它并传播 if map.gMatrix(sMin) map.rhsMatrix(sMin) map.gMatrix(sMin) map.rhsMatrix(sMin); map.U queuePop(map.U); predList getPredecessors(map, sMin); for i 1:length(predList) map updateVertex(map, predList(i), map.targetNode); end else % 否则g设置为无穷重新计算rhs map.gMatrix(sMin) inf; map updateVertex(map, sMin, map.targetNode); predList getPredecessors(map, sMin); for i 1:length(predList) map updateVertex(map, predList(i), map.targetNode); end end end end这里最值得注意的就是方向。我在第一版实现时把getSuccessors写成了“从当前节点往前走”的邻居结果导致重规划完成后路径总是绕远。D* Lite的反向搜索对节点s来说后继节点其实是“目标方向上游”的节点在机器人坐标系里是它前方能够走向目标的邻居。写代码之前先在纸上把格子的方向画出来比直接抄代码可靠得多。4.3 车辆运动学近似与控制量输出虽然标题重点是路径规划但如果后面要接仿真演示还是要有一个最少参数的车辆模型。我用二轮自行车模型做近似状态量是[x, y, yaw, v]控制量是前轮转角delta和油门加速度a。每步更新function stateNew updateVehicle(state, refPos, dt, params) x state(1); y state(2); yaw state(3); v state(4); dx refPos(1) - x; dy refPos(2) - y; desiredYaw atan2(dy, dx); yawError wrapToPi(desiredYaw - yaw); % 简化纯跟踪转向控制 delta params.kpYaw * yawError; delta max(-params.maxDelta, min(params.maxDelta, delta)); % 纵向速度控制 a params.kpSpeed * (params.desiredSpeed - v); v max(0, min(params.maxSpeed, v a * dt)); % 更新位姿 x x v * cos(yaw) * dt; y y v * sin(yaw) * dt; yaw yaw v / params.wheelbase * tan(delta) * dt; yaw wrapToPi(yaw); stateNew [x, y, yaw, v]; end有了这个模型路径规划的优劣会直接体现在车辆轨迹的平滑度和能否贴边通过上。我建议不要把路径规划和跟踪控制分开看很多同学把规划轨迹做得很好一接上车辆模型就跑偏原因就是没考虑车辆最小转弯半径。路径规划阶段就要加一条约束路径上任意连续三个点的夹角变化不能超过车辆最大转向角对应的弧线。4.4 仿真参数参考表我在模拟项目X里用的一套参数可以直接作为起步参考参数数值说明栅格大小0.5 m每格边长地图尺寸30 × 30 栅格约15m × 15m车辆半径0.4 m碰撞半径安全膨胀2 栅格1m比车体大一些横向采样数21法向左右各10点最大横向偏移2.0 m4个栅格障碍距离影响半径r00.6 m代价函数参数代价权重w1/w2/w3/w41.0 / 1.2 / 0.8 / 0.3见代价函数全局重规划触发距离5 m前方5m内有新障碍才触发最大转向角35°前轮极限期望速度1.0 m/s取决于仿真步长这套参数不是最优解但能保证大多数场景下车辆不会乱抖、不会贴障碍太近。等基本跑通之后再按自己的地图尺寸调整。5. 三种典型场景下的仿真效果与对比5.1 静态地图D* Lite与A*的基线对比第一个场景我拿一张固定障碍地图做基线测试。地图里随机撒了十几个矩形障碍起点在左下角终点在右上角。先用A算出一条全局最短路径再用DLite跑一遍两者路径长度几乎一致差异只来自相邻栅格的对角线处理方式。但D* Lite在这里显示出一个重要特点它维护的gMatrix在车辆从起点出发的过程中持续有效不需要重新搜索整张地图。如果此时车辆只是沿着原路径往前走、没有环境变化D* Lite的计算开销基本为零。这一点是A做不到的A在每次出发前都要重新搜索才能给出路径。5.2 动态障碍重规划次数与绕行成功率的权衡第二个场景模拟真实作业中常见的情况车辆行至半途前方突然出现一辆临时停放的障碍车。我对比了两套方案方案A只使用D* Lite全局重规划。车辆发现新障碍后更新全局地图重算全局路径然后绕行。方案BD* Lite全局重规划加横向避障。车辆先用横向避障绕开眼前障碍再触发D* Lite更新后续路径。从仿真表现看方案B的路径稍微长一点因为横向避障先做了局部绕行之后D* Lite可能在后续路径中又做了一次调整但方案B的重规划触发次数明显更少车辆轨迹也平滑很多。方案A的问题是每发现一个新障碍就全局重算一次如果车辆连续发现多个障碍路径会因为频繁重规划产生明显的不连续点。所以我的建议是在未知动态环境里尽量让局部避障处理“短时突发问题”让全局规划处理“地图模型的系统性更新”。它们处理的障碍应该分属不同时间尺度。5.3 U型陷阱验证反向搜索的增量修复能力第三个场景是路径规划最经典的U型陷阱。障碍物围成一个U形车辆的起点在U形的开口一侧目标点恰好卡在U形底部。A*在正向搜索时很容易陷入先扎进U形、发现无路可走再退回来的情况如果地图在行驶过程中发生变化正向重规划的代价更高。D* Lite在这里的优势非常明显。因为它是反向搜索的一开始就能感知到目标点被U形包围的事实提前规划出绕行方案。我在仿真里故意在车辆接近U形底部时通过地图更新把U型入口堵死D* Lite只需要更新受影响的一段节点就能重新规划出一条从当前位置绕到另一侧进入的路径整个过程里的重规划耗时只有A*全图重算的一个零头。这种场景特别适合拿来验证“全局路径生成最优路径”这个命题最优路径不是一次性算出来的而是随着环境模型不断修正在“增量搜索局部避障”的配合下逼近当前条件下的最优解。6. 踩坑实录我在这套方案里遇到过的六个问题6.1 优先队列实现不当D* Lite直接卡死第一次实现的时候我用Matlab普通数组模拟优先队列每次插入或者更新Key都直接排序整个数组。30×30地图勉强能跑但一旦地图扩到150×150单次ComputeShortestPath就慢得让人怀疑人生。后来我改成了“索引数组加稀疏映射”的方式队列里只存节点编号和Key两个字段每次堆操作只调整一小部分元素而不是全数组排序。Matlab的sort在中小地图上其实够用但不要每次插入都排序而是先攒一批更新统一重排。实际跑下来重规划耗时降低了一个数量级以上。耗时大头还有一个隐蔽地方计算启发式h时如果每次都调用一个多维数组索引重算欧几里得距离也会拖慢速度。我选择预先算好一张“到起点距离表”初始化时用一次快速扫描生成之后查表即可。6.2 Key比较方向没搞清楚路径总是绕远这个坑很隐蔽。D* Lite的Key里k1包含启发式h(node, start)。由于是反向搜索这个h是当前节点到起点的启发式估计。我一开始想当然地写成了当前节点到目标的距离结果D* Lite每次更新都倾向于先修复靠近目标端的节点起点附近的路径迟迟不收敛算出来的路径明显要绕一大圈。验证方法很简单静态地图上把D* Lite的结果和A*的结果对比如果路径长度差距超过几个栅格大概率不是算法问题而是启发式方向或者后继节点方向定义错了。6.3 横向避障的左右振荡问题参数调得不好的时候车辆会像喝醉一样左右摇摆。原因是当障碍出现在正前方时左右两侧候选点的代价几乎对称车辆会这步偏左、下步偏右宏观上看就是来回横移。解决思路有两个。一是加滞回比如车辆已经选择了左边绕行在连续三步之内禁止切换到右边绕行除非右边代价比左边低非常大的程度。二是在代价函数里再补一个“历史选择项”把上一次选择的横向偏移符号作为弱先验给同一个方向加一点额外权重。我也试过直接把w2调大确实能减少振荡但副作用是绕障幅度变小遇到宽度比较大的障碍时容易绕不过去。做这类方案参数一定要协同调整不能只动一个。6.4 全局重规划与局部避障“互相打架”这个问题在方案A和方案B对比时特别明显。局部避障先把车引向左边绕障此时全局重规划发现地图变化又规划出一条从右边绕的“更短路径”车辆随即收到新的全局路径又开始右转。左右两边反复横跳车辆几乎原地打转。我的解决方案是给全局重规划加“最低触发门槛”只有当新障碍确实落在当前待行驶路径节点上时才触发重规划如果局部避障已经能从旁边绕开就暂时不更新全局路径等车辆离开障碍影响范围之后再统一更新地图模型。这样全局和局部各管一段不会互相干扰。6.5 膨胀半径不够规划出来的路径看似能走实际穿模仿真里经常出现这样一种情况路径规划结果在栅格图上看起来很顺滑但接到车辆模型上一跑车角直接刮到障碍。原因就是只按车辆中心点做了碰撞检测没有考虑车体是一个有面积的刚体。我给地图做膨胀时膨胀半径要比车辆半径多出一个安全余量。具体做法是把车辆包络近似成半径为0.4m的圆栅格边长0.5m那么至少膨胀2个栅格才比较安全如果车辆速度较快或者转向角较大我建议膨胀到3个栅格宁可路径看起来“保守一点”也绝不拿碰撞换路径长度。6.6 调参顺序先调几何再调权重最后调速度最后说一个整体调参经验。很多人上来就调代价函数的权重或者纠结某个阈值其实顺序错了。我的顺序是先固定地图和车辆模型把膨胀半径、横向最大偏移量这些几何参数调好。它们决定了路径的可行性边界。再调代价权重让车辆在“绕障灵活性”和“回归平滑性”之间取得平衡。最后才调速度控制器和巡航速度。因为速度参数会放大路径跟踪误差如果路径本身还在抖动调速度只会雪上加霜。还有一个实用技巧仿真过程中把每一步的“横向偏移量、障碍距离、当前代价”打点记录下来曲线画出来之后振荡和过近问题一目了然。用数据说话比凭感觉调参快得多。这套方案从搭建框架到调通我陆续花了两周时间。最大的体会是D* Lite和横向避障单独拎出来都不算特别难难的是把它们拼成一个闭环时各个模块之间的触发条件、时间尺度、参数耦合必须想清楚。如果你也打算在自己的项目里复现这套流程建议从30×30的小地图开始先跑通静态场景再加动态障碍最后再加多障碍连续场景。一步步来别指望一次仿真就完美。