先说个结论自动驾驶里的“无地图环境路径探索”本质上不是让车蒙着眼睛乱跑而是让车在只有局部感知、没有先验全局地图的前提下把“规划路径”这件事从一次性求解变成时刻都在进行的增量式重算。D* Lite 就是这套逻辑里非常经典、也非常适合工程落地的一颗核心算法。如果你接触过路径规划大概率听过 A* 和 D*。A* 适合静态环境地图变了就得重新跑一遍D* 解决了部分动态问题但实现复杂、调试成本高。D* Lite 可以理解为“继承了 D* 的增量重规划能力但实现上又比 D* 干净利落得多”的改进版。我在模拟项目X里用 D* Lite 做过园区低速自动驾驶的局部路径重规划实测下来在传感器发现新障碍物、前方道路变化的时候它的响应速度和路径稳定性都比“全局 A* 每次全量重跑”好一个量级。这篇文章会围绕“无地图环境探索”这个场景把 D* Lite 的算法思路、状态量含义、可落地的实现步骤、还有我实际调参与排查问题时的经验一次性拆开讲清楚。适合正在做自动驾驶或移动机器人路径规划的同学哪怕你之前只写过 A*也完全能跟上。1. 无地图环境路径探索我们到底在解决什么问题1.1 什么是“无地图环境”先别被“无地图”三个字吓住。这里的无地图指的是没有预先构建好的全局高精度地图或者全局地图已经过期、不可信。车辆能依赖的只有实时传感器激光雷达、毫米波雷达、相机这些传感器在当前车周围构建出一个“局部环境认知”。举个例子在园区接驳、矿区运输、地下车库这类场景里无人车往往没有完整的全局地图甚至在 GPS 信号遮挡区域连定位都困难。车辆开到某个路口发现前方被临时堆放物堵住这时候规划层必须立刻绕行不能在几十毫秒里慢慢重算一张全局地图。无地图环境探索的核心矛盾是信息是逐步暴露的路径必须按当前已知信息滚动更新。它和“从 A 点到 B 点的全局路径规划”不是同一个问题更接近“一边往前走、一边确认前方可通行性、一边修正路径”的连续决策过程。1.2 传统先规划后执行方案为什么不够用假设环境完全已知A* 就能给出不错的路径。问题是真实运行环境里地图的“已知”是低频快照而障碍物的出现是高频事件。如果前方突然多了一辆违停车辆全局路径规划器必须重新读图、重新构建栅格、重新全量搜索整个链路可能在几百毫秒甚至数秒才给出新路径。低速车辆还能勉强接受一两百毫秒的延迟高速工况下就非常危险了。而且全量重规划还有一个隐患新路径可能与原路径偏差过大导致车辆反复左右横摆体感很差底盘控制压力也大。这是无地图环境下最典型的痛点不是能不能重新规划而是能不能在局部变化后只更新受影响区域快速得到一条与上一帧路径平滑衔接的新路径。D* Lite 的增量式思想恰好切中了这个问题。1.3 无地图路径探索需要同时满足的三项指标结合我在项目里的落地经验评价一个路径规划方案是否适合无地图环境至少看三项指标重规划时延从传感器探测到代价地图变化到规划器输出新路径的时间。D* Lite 这类增量算法希望把成效集中在受影响节点而不是全图重扫。路径连续性与平滑性新路径不应该和上一帧路径剧烈跳变否则跟踪控制失效、车辆会有明显顿挫。可解释性和调试友好度当车突然绕了一个大弯你需要知道是哪个代价栅格发生了变化、哪条边被更新。D* Lite 的“局部更新”特征让我在复盘时可以直接定位到障碍物的影响范围这一点在工程上是巨大的隐性价值。2. D* Lite 算法思路与核心状态量拆解2.1 从 A*、D* 到 D* Lite增量与方向的选择A* 是基于启发式的静态路径规划算法它从起点向外扩展用“已走代价 到目标启发代价”决定搜索顺序。地图不变时A* 很好运行一次就够。但地图变了A* 没有记忆得从头再来。D* 则是在 A* 基础上加入了“弧代价变化后的增量修复”能力。它维护了一个从目标点向起点扩展的搜索树当环境变化时只需要处理受影响节点的代价更新。D* 在火星车这类场景里很有名但它的回溯指针维护逻辑比较绕容易把人绕晕。D* Lite 的思路和 D* 一脉相承但实现上采用了一个更统一的状态量框架每个节点有 g 值、rhs 值、优先级 key。它没有 D* 的复杂指针结构重规划逻辑在数学上更简洁代码写起来也少一大截。D* Lite 的另一个特点是“反向搜索”。它不是从车当前位置向目标点扩展而是从目标点反向往当前位置扩展。搜索树里的 g 值含义是“这个节点到目标点的当前估计代价”。反向搜索的好处是车辆往前走一步起点变了但目标点没变那么大部分搜索树的代价信息还能继续复用。这正好匹配无地图环境下“目标不变、起点前移、障碍变化”的节奏。2.2 g 值、rhs 值、key 值读懂 D* Lite 的三个概念刚开始看论文时我也被这三个值搞得头疼。实际上把它们的关系理清整个算法就通了一半。g 值节点的已知代价表示这个节点到目标点的当前最短代价估计。当搜索稳定时所有节点的 g 值就是最终代价。rhs 值节点的“一步看齐值”它表示“通过所有相邻节点经过一条边到达当前节点所得到的最小代价”。也就是说rhs 是利用邻居节点的 g 值算出来的“邻居到目标的代价 从邻居到当前节点的代价”取最小。如果没有路径变化理论上 g 永远等于 rhs。如果 rhs 比 g 小说明发现了更优路径需要降低节点代价如果 rhs 比 g 大说明原来依赖的最优边被障碍堵住需要把当前节点代价拉高甚至拉成无穷大。key 值优先级队列里的排序依据。D* Lite 从目标点反向往当前位置搜索但如何决定先处理哪个节点答案就是给每个节点算一个 key然后每次都从优先级队列里弹出 key 最小的节点。key 是一个二元组key(s) [ min(g(s), rhs(s)) h(s), min(g(s), rhs(s)) ]其中 h(s) 是启发式代价估计当前节点到目标点或到起点根据实际实现方向的距离。第一维相当于 A* 里的 f g h第二维用来打破平局保证搜索的优先顺序稳定。这里最反直觉的地方是不是所有节点都会进入队列只有 g 值和 rhs 值不一致的节点才会被插入优先级队列。队列里存的不是“待扩展的候选”而是“代价需要修复的异常节点”。这叫法看起来有点绕却是增量规划的关键环境变化后我们不需要重新计算全图只需要把受影响节点重新扔进队列让修复过程沿着影响范围传播。2.3 搜索树一致性与路径提取方式当每个节点的 g 值都等于 rhs 值时搜索树就是一致的此时从起点沿“下一个最优邻居”走过去就能得到最短路径。具体提取路径的方法是从当前位置开始依次选择使目标代价最小的邻居next argmin_{s_nei in pred(s)} ( g(s_nei) cost(s, s_nei) )这里 g(s_nei) 是邻居到目标点的代价加上从当前节点走到邻居的边代价选最小的一个。沿着这个方向一路走到目标点就是完整路径。我在实际项目中把这段路径提取逻辑单独封装成一个函数没有直接访问 D* Lite 的内部队列这样调试起来很方便即使算法内部震荡至少可以先用旧搜索树输出一条可行路径保证车辆不傻停。3. 核心细节解析无地图环境下的探索与重规划机制3.1 局部感知到代价地图的映射D* Lite 本身不碰传感器数据它吃的是“节点图”或“栅格代价图”。所以路径规划之前必须先把激光雷达或视觉感知结果转成规划器认识的代价地图。栅格地图有一个重要概念代价不是单纯 0/1 可通行/不可通行而是连续或分级的安全代价。离障碍物近的栅格代价高离得远的代价低。规划器计算路径时不仅会把“撞上去”判为无穷大还会天然避开障碍物附近的膨胀区域。我在项目里的做法是激光点云先做地面滤波、聚类、获得障碍物外接框再用代价扩散函数做膨胀处理。膨胀半径一般设成车辆外接圆半径加上安全距离再额外加上一个和速度相关的裕量。速度越快膨胀半径越大。这样 D* Lite 规划出来的路径天然给横向控制留了余量。有一点需要特别注意代价地图千万不要拿原始点云直接做栅格占用。点云稀疏反射区域会出现空洞物体边缘会破碎代价地图会变得忽宽忽窄导致 D* Lite 反复重规划。应该先对障碍物做聚类和轮廓拟合再用轮廓栅格做膨胀地图性能会稳定非常多。3.2 障碍变化后 D* Lite 的局部更新链路无地图环境里最频繁的触发事件是传感器突然发现前方有个新障碍物挡住了当前路径。这个障碍物在栅格地图上表现为一部分栅格的代价变为无穷大。D* Lite 并不直接“清空所有路径重新算”而是从受影响的边开始标记哪些节点的 rhs 值可能发生变化然后把这些节点放入优先级队列。比如车辆预计要走的边被堵了那么该边起点以及所有从其他邻居能到达它的节点都会在 update_vertex 过程中重新计算 rhs 值。新的 rhs 值如果和 g 值不一致节点又会被重新放入优先级队列。这个过程会像水波一样逐层向外传播直到路径重新一致。关键好处在于如果新障碍物只影响了一小块区域那么传播过程也会集中在那一块区域不会像 A那样把整张地图重新扫一遍*。障碍物出现的位置距离当前路径越远重规划耗时越短这是增量的天然优势。3.3 重规划触发阈值与路径稳定性取舍在项目里我发现路径规划代码跑得快不快只是问题的一小部分更大的坑在于“规划器被过频触发”。假设车辆前方 10 米有个行人正在横穿传感器每一帧都发现行人的位置变化代价地图每帧都在变D* Lite 每帧都在重规划。结果就是路径一直在左右摇摆车辆控制层为了追路径忙个不停平顺性很差。解决思路是给重规划加“变化量门槛”。具体做法记录上一帧代价地图中被判定为“障碍状态翻转”的栅格数量只有当翻转数量超过阈值或者新障碍物与当前路径的横向距离小于安全裕量时才触发 D* Lite 重规划。否则沿用上一帧路径。另一个经验是输出限幅即使 D* Lite 给出了一条和上帧差别很大的路径也要对路径点做平滑和限幅让车辆逐步靠近新路径而不是瞬间切过去。这在低速自动驾驶里是必要的工程保护不然算法再快底盘也跟不上。3.4 动态车辆与离散时间步下的处理无地图环境下除了静态障碍物还有行人、其他车辆等动态物。D* Lite 处理动态物有两种策略一种是把动态物当作持续移动的障碍物每一帧更新其占用栅格这会带来大量重规划另一种是预测动态物未来位置把“未来某段时间可能占用的栅格”提前标记为高代价从而降低后续重规划频次。我建议在低速场景下双轨并行针对低速动态物行人、慢车使用“未来占用标记”策略把预测轨迹投影到代价地图上投影的时间窗通常取 2 到 4 秒针对突然出现的不明障碍物才触发即时重规划。这种分层处理带来的好处很明显D* Lite 不需要每帧处理所有动态物只需要关注那些真正侵入规划“安全走廊”的障碍物。4. 实操过程从感知输入到路径输出的一整套实现4.1 系统结构与数据流一个完整可跑通的 D* Lite 路径规划节点通常包含三层感知与建图层接收点云、图像、毫米波数据输出局部代价栅格。路径规划层维护 D* Lite 搜索树响应代价地图变化输出局部路径。控制层跟踪路径并输出速度与转向指令。D* Lite 规划节点的接口可以设计得非常简单输入是“栅格地图变化事件”和“当前车辆位姿”输出是“路径点数组”。内部维护全局搜索树对象这样当目标点变化时才全量初始化否则一直复用。4.2 核心伪代码初始化、更新与重规划我尽量展示一个工程化的伪代码因为太多文章只讲公式不给可落地的流程。这里用类 Python 伪码表示class DStarLite: def __init__(self, grid, start, goal): self.grid grid self.start start self.goal goal self.g {s: INF for s in grid.keys()} self.rhs {s: INF for s in grid.keys()} self.rhs[goal] 0 self.queue PriorityQueue() self.queue.insert(goal, self.compute_key(goal)) def compute_key(self, s): h self.heuristic(s, self.goal) k min(self.g[s], self.rhs[s]) return (round(k h, 6), round(k, 6)) def update_vertex(self, u): if u ! self.goal: neighbors self.get_neighbors(u) self.rhs[u] min( self.g[v] self.cost(v, u) for v in neighbors if self.grid.passable(v) ) self.queue.remove_if_exists(u) if self.g[u] ! self.rhs[u]: self.queue.insert(u, self.compute_key(u)) def cost(self, from_node, to_node): # 如果 from-to 这条边被障碍物影响返回 INF if self.grid.is_blocked(from_node, to_node): return INF return self.grid.get_cost(from_node, to_node) def compute_shortest_path(self): while (self.queue.top_key() self.compute_key(self.start) or self.rhs[self.start] ! self.g[self.start]): u self.queue.pop() if self.g[u] self.rhs[u]: self.g[u] self.rhs[u] for v in self.get_predecessors(u): self.update_vertex(v) else: self.g[u] INF self.update_vertex(u) for v in self.get_predecessors(u): self.update_vertex(v)重规划主循环则更简单几乎可以套一个模板def replan_loop(): # 当前传感器发现障碍变化后更新对应边的代价 changed_edges detect_map_changes() for (u, v) in changed_edges: update_vertex(u) update_vertex(v) compute_shortest_path() path extract_path() speed_command follow_path(path)这段代码的精髓在于不是把所有栅格重新丢进队列而是把与变化边有关联的节点做一次局部修复。队列弹出的节点数量理论上只取决于变化范围而非地图大小。4.3 与速度规划和控制模块的接口衔接D* Lite 输出路径后还需要一个下层的轨迹跟踪模块。我在项目里常采用的做法是先对原始路径做三次样条平滑再根据前方路径曲率限制速度然后交给横向 PID 或纯跟踪控制。这里有个必须强调的细节Dont 直接把 DLite 输出的折线路径 RAW 给底盘控制*。栅格路径往往是锯齿状的直接跟踪会导致方向控制抖动。正确做法是把栅格路径经过矢量化、抽稀、平滑再生成可供跟踪的 trajectory。这个步骤可以在路径规划模块内部完成也可以放在独立的轨迹生成模块中。速度限制也要结合“未来路径代价”来定。例如路径前方 5 米处栅格代价突然升高即使当前路径还远速度也要提前降下来否则车辆会有迟滞感。4.4 关键参数与调参经验我整理了一份最小参数表适合做低速自动驾驶的场景参数推荐初值调参方向栅格分辨率0.2 m越小精度越高但规划耗时上升障碍物膨胀半径车辆半宽 0.3 m大一点更安全但路径可能绕远重规划触发阈值8~15 个栅格变化阈值小响应快但抖动多最大路径提取次数1000 次防止意外死循环平滑窗口5~7 个路径点窗口过大动态障碍响应慢从这些参数的经验来看最影响手感的是膨胀半径和重规划触发阈值的配合。膨胀半径设太大车辆会绕远路阈值设太小路径抖得像电风扇。我的调参顺序是先固定膨胀半径再逐步调阈值直到车辆能稳定通过一条窄通道。5. 常见问题与排查技巧实录5.1 路径反复横跳我最早调试 D* Lite 时遇到最哭笑不得的路况是车走到窄道口路径每隔几帧就左右横跳一次车上的人能感觉到明显的“画龙”。排查后确认根因出在代价地图无规律抖动。某个障碍物的聚类边缘在一帧里有一格下一帧少一格导致代价地图在边缘处出现“呼吸效应”。D* Lite 会忠实地把这种微小变化放大了——每次变化都触发重规划路径自然不稳定。解决办法有两个层面感知层面给障碍物聚类加时间一致性滤波连续 N 帧都出现才认为稳定规划层面把重规划触发阈值调高忽略少量栅格翻转。5.2 重规划超时理论上 D* Lite 是增量式的但如果障碍物变化范围很大或环境处在“障碍物大范围移动”的状态搜索传播范围也会变大规划耗时可能从 10ms 涨到 100ms 以上。遇到这种情况首先要确认不是栅格分辨率过细导致搜索节点爆炸。我的经验是全局规划用 0.5m 分辨率局部精细调整用 0.2m 分辨率两者分层而不是让 D* Lite 单层处理全尺度的栅格地图。另外要在规划循环里加时间预算。如果 D* Lite 在 30ms 内没有完成收敛直接输出当前搜索树能找到的可行路径而不是卡死在 compute_shortest_path 里。这个“先保住安全、再追求最优”的思路在自动驾驶领域太重要了。5.3 队列陷入“假死”还有一种诡异的现象D* Lite 主循环退出来了但提取路径时却一直找不到终点。这不是队列死循环而是 start 节点被障碍物包围了所有邻居的 g 值都是无穷大路径提取当然失败。这时候车辆应该立刻停车等待感知层把代价地图更新出一块可行区域再重新触发重规划。千万不要让车辆继续沿着旧路径盲开。同时要区分是“真无路可走”还是“代价地图被临时误标”。我习惯在界面上叠加显示当前 start 所在栅格的占用值和邻居状态一眼就能判断。5.4 优先级队列 key 值精度抖动D* Lite 对 key 值非常敏感。如果启发式和边代价都用浮点数直接比较可能出现两个节点的 key 值因为精度抖动而频繁交换顺序导致路径在两条近似代价相等的路线之间反复切换。解决办法是给 key 值做保留几位小数再进队列同时如果两条路径代价差小于一个小阈值比如 0.05m就优先沿用上一帧路径不因为微小代价差异改变路径选择。这在复杂园区道路里非常管用。5.5 排查工具建议分享一个很实用的排查流程。当路径行为不对时不要第一反应去看路径结果而要按这个顺序看数据先看代价地图是否稳定再看 D* Lite 队列弹出的节点分布最后才看输出路径。绝大多数路径规划问题都不是规划算法本身的 bug而是喂给它上游数据出了问题。我每次都会把代价地图变化事件和路径输出打点成日志回放时可以直接对比“哪个栅格变了、算法在哪一步做出了什么样的绕行”这个习惯帮我省了大量时间。6. 写在最后一点个人实验体会无地图环境路径探索本身就是个“信息不完整条件下的决策问题”D* Lite 提供了很好的增量计算框架但真正让方案跑得好还要结合传感器质量、代价地图稳定性和控制执行能力一起来看。我在模拟项目X里一开始只关注 D* Lite 本身速度有多快结果发现重规划很快、路径却很乱。后来把精力放在代价地图的稳定性和变化触发过滤上整体表现才有了质的提升。调参过程中我最大的体会是增量算法解决的是算力问题而工程落地解决的是输入稳定性问题两者缺一不可。如果你正在做类似项目我建议先搭一个简单的最小验证环境一张局部栅格地图一个会随时间变化的障碍物再加上 D* Lite 核心逻辑和一段可视化日志输出。先把这个闭环跑顺再逐渐接入真实感知数据。等你能把障碍物变化、队列更新、路径输出三者严格对应起来你就真正掌握了这套系统。下一篇我打算继续拆 D* Lite 在不同场景下的变体和优化策略包括多层栅格融合、动态障碍物预测等后面内容随时可以往下展开。