自动驾驶全局路径精简计算：从栅格冗余到配送主干路径

做校内机器人配送的同学，多半都有过这种感觉：地图看着不大，全局路径却动不动输出成百上千个坐标点，跑起来小车主控喘得不行，稍微有点动态障碍，路径一重算，整条轨迹又开始抖动。这个项目要聊的（6-4）自动驾驶中的全局路径精简计算，简单说，就是在保证路径安全、可通行的前提下，把全局路径上的冗余点大幅压缩，让机器人拿到的不是一条“密密麻麻的坐标锁链”，而是一条精练、平滑、便于局部规划器跟踪的主干路线。这个内容适合正在做园区配送、楼宇送物、校园无人小车的同学，也适合想搞清楚全局规划和局部规划到底怎么衔接的工程师。

先给结论：全局路径精简计算不是一个单独的算法，而是一整套“地图抽象 + 路网重构 + 路径点压缩 + 轨迹平滑”的组合拳。它不是把精度降低，而是把表达方式切换成更高层、更省算力的形式。接下来我把这套东西从为什么要做、怎么拆解、参数怎么选、现场踩过哪些坑，一个一个讲清楚。

1. 从整片栅格到主干路网：为什么非精简不可

1.1 全局路径在自动驾驶里的真实地位

一套典型的自动驾驶/机器人导航系统，从上到下大概分四层：感知、定位、全局规划、局部规划与控制。全局规划负责“我要去哪”，局部规划负责“眼下这个坑怎么绕” ，控制负责“轮子怎么转”。

全局规划的输出，就是一条从起点到终点的路径，通常表达为一系列坐标点。这些点送入局部规划器后，局部规划器会以此为参考，结合实时障碍物信息生成短距离轨迹。

这里有个很微妙的矛盾：全局路径越精细，理论上参考价值越高，但它承担的是“战略层”职责，不需要细致到每一个像素。相反，全局路径如果过于稠密，反而会干扰局部规划的灵活性——局部规划器想绕一个临时出现的锥桶，结果全局路径密密麻麻地把所有空间都占满了，局部规划器怎么算都算不出一个不碰线的解。

所以在整套系统里，全局路径应该被“精简”到这样程度：足够描述道路走向，保留路口、拐弯等关键拓扑特征，让局部规划器知道大方向，又不至于被细碎点绑死。

1.2 校内配送场景的三大现实压力

第一，地图尺寸不大，但栅格化之后点数量很大。一个普通的校园园区，大概300m × 400m，如果分辨率用5cm一格，栅格总量是6000 × 8000，也就是4800万个格子。就算用优化过的A*处理，在这上面做全局搜索的耗时也是毫秒级到几十毫秒级，而配送小车的主控往往是Jetson Nano级别，资源有限。

第二，道路结构相对规整，但动态对象多。学生上下课、电动车穿越、临时施工，都会让“可通行区域”发生局部变化。全局路径需要重新规划，如果每次重算都面对几千万个栅格，实时性完全跟不上。

第三，末端执行场景精细，容错率低。配送机器人要在教学楼门口、宿舍楼下一点一点挪进指定位置，全局路径如果粗糙到只剩几个大拐点，最后几米根本没法用，必须结合前端精细坐标系。

所以精简计算要同时满足三件事：搜索快、主干稳、末端准。

1.3 “精简计算”到底减的是什么

很多朋友一听“精简”就以为是降精度、减道路信息，这是最大的误解。全局路径精简计算削减的对象，是这几类东西：

• 栅格地图产生的锯齿状折线段，比如对角线会变成“阶梯状”的走法；
• 长直道中大量共线点，这些点不提供方向变化信息，纯粹撑数量；
• 小幅度曲率波动点，还没到需要绕障的程度，属于噪声；
• 局部规划器本可以自行处理的冗余细节，例如一个0.2m的凸起，全局完全不必体现。

保留的，则是道路拓扑、路口、转弯、目标点附近的结构关系。

一个直观类比：你开车出远门，导航软件给你规划的“路线”是几条主干道、几个匝道口、几个红绿灯路口，而不会告诉你每分钟要保持在车道内的哪几厘米位置。后者是控制层自己解决的事。全局路径的“精简”就是做前者，把车的宏观意图从数千个坐标点压缩成几十个关键点。

2. 地图抽象与拓扑路网重构

2.1 栅格地图为什么不适合直接做全局规划

栅格地图做全局规划有几个天然毛病。

第一是锯齿问题。A*这类算法本质上在网格里搜索，从左上到右下，路径经常是“先横走几步、再竖走几步”，呈阶梯状。即使同一方向，也因为网格粒度，产生大量微小折线。

第二是计算量问题。搜索算法和栅格数量强相关。拿刚才算的校园环境，几千万个栅格，即便用分层搜索或预处理，也扛不住频繁重算。

第三是“整片可走”的语义缺失。栅格地图告诉你每个格子的通行状态，但不告诉你这条路是“主干道”还是“楼间窄缝”。配送机器人走到楼与楼之间的狭窄通道，和走在大路上，全局路径的余量设计要求完全不同。

2.2 拓扑路网的基本构建步骤

把栅格地图转成拓扑路网的思路，简单讲，就是“降维”：把二维栅格地图提炼成一张图（Graph），图里的节点是道路上的关键位置，图里的边是两节点之间可通行的道路段。搜索算法在这张图上跑，节点数量可能只有几十到一两百个，计算量可以忽略不计。

构建流程，我在模拟项目X里是这样做的：

1. 栅格地图预处理：先做膨胀处理，把障碍物边界向外扩一个机器人半径，确保拓扑图中的边有足够的物理通行宽度。
2. 可通行区域提取：用连通域标记法把隔离出来的区域找出来，筛掉面积过小的噪声区域。
3. 骨架提取：基于形态学细化或距离变换，得到可通行区域的中心线。这一步相当于把“面”变成“线”，为后续路网打基础。
4. 关键点标注：在骨架图上找端点、交叉点、转折点这些关键结构位置。这一步通常用邻域分析完成。
5. 路网生成：以这些关键点为节点，以骨架线为连接关系建图，同时给每条边附上属性：长度、平均宽度、是否在室内等。

最终得到的就是一张节点少、边清晰的路网，可以直接拿来做路径搜索。

2.3 关键节点怎么选：我们试过的几种标准

选点是整个路网构建里最依赖经验的一环。选点太少，路网无法表达实际道路结构；选点太多，又失去精简的意义。

我常用的节点类型有这四种：

• 端点：道路末端的尽头或衔接点，例如仓库大门、实验室出入口。
• 交叉点：两条以上道路的交汇处，通常是一个十字路口或三岔口。
• 转折点：道路方向发生显著变化的位置，用角度阈值判断，一般转角大于15°到20°就落一个点。
• 特征点：电梯口、充电桩、固定取货点这类任务相关的固定点位。

实际测试下来，节点选点的核心法则是“以交叉点和端点为骨架，以转折点为辅助，以任务点为冗余兜底”。好路网的精简程度反而高。

2.4 路网对于全局路径的意义

路网一旦建好，全局路径搜索就从“二维平面搜索”变成了“图搜索”。搜索范围大大缩减，原来动辄几千万个栅格，现在只剩100个节点、150条边，在图上做Dijkstra或A*几乎瞬时完成。

更重要的是，图搜索天然避开了栅格锯齿问题。路径在一段边内部就是一条直线段，到了不同边的交界处才可能出现转弯，输出的原始路径天然比栅格A*路径干净好几个数量级。

还有一个好处：路网承载了语义信息。比如“某条边是主干道，宽度3米”“某条边是楼间连廊，宽度1.2米”。全局搜索可以据此自动避开太窄的通道，或者优先选择更宽的主路。

3. 路径点压缩：道格拉斯-普克与其同行者

路网搜索完成后，我们会得到一条由节点组成的折线路径。这还不够——折线的每个拐点都要保留吗？长直道中间的点要多少合适？这就要进入路径点压缩环节。

3.1 三种常见的路径点压缩思路

我实际对比过三种做法，各有适用场景：

• 固定间隔抽稀：每隔N个点保留一个。实现最简单，但缺点是直道上可能保留多余点，拐弯处又可能因为间隔太大丢掉关键转弯特征。
• 道格拉斯-普克算法（DP算法）：基于“最大距离偏差”实现压缩。给定一个距离阈值，它不断把折线拆成子线段，丢掉偏离候选线段小于阈值的点。适合坐标点串的抽稀，也是这个项目里效果最好的。
• 角度阈值法：逐点计算转角，只保留转角大于阈值的点。问题在于直道上可能积累缓慢漂移，转角很小但累计方向变化很大，在全路段上体现为大偏移。

综合来看，推荐方案是：先用DP做一次几何层抽稀，再叠加角度和累加距离约束，防止关键特征点被误删。单一算法都不够稳。

3.2 DP算法在配送路径上的落地细节

DP算法流程不复杂，但落地时有几个细节容易被忽略。

流程是这样的：

1. 输入：一串有序路径点 P0、P1、…、Pn，距离阈值 epsilon。
2. 以首尾两点画一条直线段。
3. 遍历中间所有点到这条线段的垂直距离，找到最大距离 dmax。
4. 如果 dmax > epsilon，则把该点作为“断点”，将路径分成两段，分别递归执行。
5. 如果 dmax ≤ epsilon，则中间所有点都丢弃，只保留首尾两点。

在实际配送场景里，epsilon怎么取？我推荐取栅格分辨率的2~3倍作为下限。栅格5cm一个点，epsilon取0.3m比较合理；如果直接取0.5m，长直道上的点会被大量压缩，但小弯路可能连“弯”都被抹掉。对于校园主干道，0.3~0.5m是安全区间。

这里有一个特别容易踩的坑：DP算法可能把“大弯”简化成“割线抄近路”的效果。比如一段U型弯道，DP可能在弯曲内侧拉一条直线，把原本贴着路走的路径直接穿透到路外。规避办法是：折叠条件里增加一个“累计包络距离”检查，当某段原始路径总长度显著大于候选直线段长度时，强行走原来的曲线，只做稀疏化采样，不做整体拉直。

3.3 转角点保护与任务点锚定

就算DP算法给出一个看似漂亮的结果，也不能直接相信它。因为有些点具有拓扑语义，删掉了会出大事。

我总结了一套“必保点清单”：

• 起点和终点：这个不用解释，动了起点，车还没出发就错了。
• 转弯点：转弯半径本身由底盘约束决定，全局路径一旦把转弯点删掉，局部规划器会以为这里“直行即可”，结果高速冲到路口才发现要转。
• 道路交汇点：也就是十字路口的中心参考点，一般和拓扑图节点重合，是重规划时的锚定目标。
• 停靠/取货点前的最后一个点：这个点是局部规划器用来做精细对准的参考，必须保留。

实际操作中，我会先把这些必保点标记出来，DP压缩完成后再把这些点重新插入，并重算相邻段落。注意插入时不要简单塞回原坐标就完事，还要把插入点前后的点重新检查一遍共线性，否则可能把一个原本该弯的地方搞成反向折角。

3.4 角度约束：压缩后的路径不能“折手折脚”

还有一个维度是压缩后的转弯角度。很多压缩算法只看几何距离，忽略折线夹角的合理性。比如一条路径转弯点前后各保留了一个点，本来角度变化是均匀的，压缩后变成“两个点连过来猛拐90°再接直道”，物理上跑不出来。

处理办法：在DP之后加一个角度过滤。计算每个保留点的转角，如果某个转角超过底盘最大允许转角（由车辆物理模型决定，比如AGV一般不超过45°），就要在转角两侧插入若干线性插值点，把一次大转弯分解成多个小段，保证曲率连续。

这一步在做全局路径精简时尤其重要，因为精简后的点少，意味着每段的权重变大。你删掉的点，最后都要靠局部规划器去背，如果路径本身拐得太急，局部规划器往往来不及做平滑就被迫急停。

4. 实操环节：一个配送任务从地图到精简路径的完整流程

前面讲了不少原理和概念，不看实操总归是悬的。这里我用一个模拟项目X的配送场景，把完整流程串起来。场景是这样：一台校园配送车从A栋地下车库出发，前往B栋教学楼门口，全程大约450m，中间要过一个十字路口、一片减速带区域和一个楼前小广场。

4.1 环境的栅格化

第一步，把预先采集的激光点云/高精度地图栅格化。我这边的地图范围是400m×300m，栅格分辨率5cm，最终得到一张8000×6000的栅格图。

栅格图长这样：0表示可通行，100表示障碍物，255表示未知。对地图先做一次“障碍物膨胀”，膨胀半径等于机器人半径0.5m加上安全余量0.1m，即0.6m。

为什么要膨胀？因为后面生成的路径是“中心线”，如果中心线距离障碍物过近，实际车体通过时就会刮蹭。膨胀就是在物理层面先让出安全空间。

4.2 骨架提取与路网建图

对通行区域做距离变换，提取骨架线。骨架线会自然地落在道路中央位置。对骨架线做细化、去毛刺，然后找端点、交叉点。

这一步会得到一批候选节点。我们手动作微调，把A栋停车库出口设为一个端点节点，取名为node_garage_A；B栋教学楼门口设为一个端点节点，取名node_bdoor。过了减速带区域之后的道路交叉点，自动检测为交叉节点。

最终整个园区路网约120个节点，230条边。相比8000×6000的栅格，计算压力已经不是一个量级。

4.3 全局搜索：路网上算出初始全局路径

接下来在路网上用A*搜索。搜索的cost来自两部分的加权：一部分是边长度的欧氏距离，另一部分是道路宽度的加权系数，窄路的cost会更高，这样搜索更倾向于选择主干道。搜索代价函数可以简单这样设计：

cost = length + k × width_factor

其中width_factor在宽路为0，在窄路为2.0，k取0.8。实测下来，配送给机器人更愿意拐远路走大路，而不是挤小路，后续局部避障的难度大大降低。

A*在这张120个节点的图上搜索，几乎瞬时返回结果。初始路径为从node_garage_A到node_bdoor的一条节点序列，总计经过17个路网节点。展开成几何路径后，得到一条有412个坐标点的折线。

4.4 路径点的DP压缩

现在开始关键一步：点压缩。输入412个坐标点，epsilon取0.4m。DP算法跑完，点从412个降到38个。

此时人工核对一下这38个保留下来的点，重点检查之前提到的必保点有没有被误删。检查后发现有大约6个点分布在转弯处，属于需要重新插入的“关键点”。把这些点重新插入后，点数变为44个。

44个点，相对于原来412个点，压缩比接近90%。但这些点仍有几个拐角过陡的段落，需要进入平滑环节。

4.5 平滑处理：让局部规划器舒服地接手

平滑处理我用了一种实用组合：先用三次样条插值生成一条连续曲线，再在曲线上按车体轨迹要求重新采样。

具体地，对44个关键点做三次样条插值，得到一条平滑曲线，然后以0.2m等间距采样，又得到约230个点。这里解释一下：为什么压缩完又增加点？因为44个关键点是“骨架表达层”，适合人读、适合存储、适合传给局部规划器做语义理解；但实际轨迹跟踪时，控制器需要相对密集且平滑的插值点。230个点相比原先412个点，仍然少了44%，但最重要的是，这230个点的曲率连续性好，没有锯齿，没有突兀折角，局部规划器几乎不需要做额外平滑就能跟踪。

平滑处理之前，路径折角平均在22°左右；平滑之后，全路径最大转角不超过9°。这个质量对配送体验的提升是肉眼可见的。

4.6 下发给局部规划器时的参数

精简后的路径会作为“nav_path”的参考轨迹下发给局部规划器。局部规划器会结合实时激光数据做绕障规划。

下发的路径信息包含几个核心字段：路径点数、总长度、路径通道宽度（用于约束避障方向）、每段的最大允许速度。速度这一项建议直接从全局路径的“语义”里算出来。比如在直道段，允许1.5m/s；在拐弯段，按曲率限制在0.5m/s；在末端精细停靠段，限速0.2m/s。这些信息可以存成路径点属性，方便局部规划器直接读取。

这条链路整个走完后，全局路径规划耗时从原来栅格A*的大约12ms，降到了路网搜索+压缩和平滑的1.5ms左右。注意，这里的1.5ms是全流程，包含路网搜索和点压缩。而且由于路网固定，即使动态障碍导致重规划，也只是在局部路网范围内做小规模修正，远远比整图重算高效。

5. 现场实跑效果与观察记录

空谈理论没用，我把这套精简路径方案接到了实车平台上，在模拟项目X的校园环境里跑了几十趟。这里记录几个典型的现场表现。

第一次试跑，路径从仓库到东门，原始栅格A*规划出的路径有超过300个折点，车在转弯时会出现明显的先减速、再调整、再加速的“顿挫感”。换成路网+精简计算后，同样的起终点，路径只有25个关键点和约100个平滑采样点，每个转弯点相对均匀，车几乎是平滑顺畅地过弯。

第二次试跑，我们模拟了一个动态障碍场景，在教学楼门口临时放了一个锥桶。局部规划器在跟踪全局路径时需要局部绕障。原来的稠密全局路径完全没有给绕障留空间，局部规划器甚至算不出可行局部轨迹，车直接停在原地。精简路径给了足够的缓冲空间，局部规划器很快在旁边算出一条侧向的小绕行轨迹，车以0.6m/s依次绕过。

第三次是末端停靠。目标点距离教学楼门口1m，原来的全局路径在最后一米有大量冗余折点，车停靠前反复修正。精简路径保留了末端锚定点，局部规划器能稳定执行直线对准，停靠精度从原来的±12cm提升到±5cm以内。

这些数据说明一个问题：全局路径精简不是简单地“把点变少”，而是在“计算效率”和“控制能力”之间找到最优配合点。点太多，计算和控制都被拖死；点太少，关键信息不够，控制完不成。精简化是一个系统性的调优过程。

6. 常见问题与排查技巧实录

做这类项目，坑一定比理论多。我把实操中遇到过的典型问题整理成一份排查速查表，希望对你有直接用处。

6.1 路网断裂：最容易被忽略的拓扑问题

有一次换了一版新采集的高精度地图，路网重新提取后从150个节点变成170个。当时没细查，直接跑任务，结果发现有一块区域全局路径绕了大圈才到达，明明有一条直通的路。

排查后发现：两条道路的骨架线端点没有精确落在同一个点，相交位置差了几十厘米，于是这两段骨架在拓扑图中被判定为不连通。处理办法是做“局部去重”：如果两个端点距离小于1m，合并成一个节点，并连接相邻的边。之后路网恢复连通，路径又变得正常了。

这类问题一定要在路网构建后做一次连通性自检，否则全系统最终都会表现成“莫名其妙绕路”。

6.2 DP压缩的“割线抄近路”事故

第三版算法上线后，有一次路径明显不对——原本应该沿着L形走的路，直接变成“斜跨绿地到达”。头更疼的是，这条斜线在空中看距离还挺近，单纯对比路径长度居然比原来的路径还短一点，如果只在总长度上做验证根本发现不了。

这起事故的根本原因就是DP算法把L形内侧的两个中间点给压掉了，直接用直线连接了转弯前和转弯后的点。最后我们补了一个判断：“如果DP抽稀后的线段长短相比，偏离某条内部点密集折线超过60%，就回到原始折线形态仅做共线点删除，不做几何抽稀”，再未出现过这类问题。

6.3 新地图增加了施工区，动态失效

某天园区临时围蔽了一侧主路，地图没有变更，但实际道路不可通行。全局路径仍然规划了走那条路的方案，车靠近后才通过局部实时避障发现自己过不去，只能原地反复尝试。

后来在全局规划的输入中加入一块“临时障碍地图”叠加层，通过运维更新封锁区域，全局路径重新规划时会避开封锁路段。这个是纯栅格方案做起来很自然的事，但路网方案必须额外实现“动态边禁用”功能。把这部分加到拓扑边上，让禁用的边有两种状态：不可用或用低优先级cost代替。

做完这个才发现，全局路径精简计算的前置能力其实比单纯算法复杂，要在建图、路网、搜索、压缩四个环节都做好工程化，才能真正稳。

7. 几个值得写下来的优化心得

第一个心得，路网的“稀疏”不代表“粗”。一个好路网，节点不多，但每条边都带有精确的物理属性（宽度、长度、限速、地面材质）。精简后的路径质量，七成取决于路网建得好不好，只有三成取决于压缩算法本身。

第二个心得，DP算法在机器人路径压缩里是主力，但永远要加护栏。真正的工程实现，是在DP外面包了两层：一层保护必保点，二层保护大弯几何形态。不加护栏的DP，迟早会在现场给你演一出“穿墙抄近路”。

第三个心得，全局路径精简之后，调试难度其实会增加。因为路径点变少了，控制层一旦表现异常，你要判断到底是压缩算法删错了点、平滑算法把轨迹给弄糊了，还是局部规划器没跟好。所以我在每个精简后的路径点上都加了“信息来源标记”：这个点是原始路网节点、DP保留点、或者是重建插入点。排查问题时能顺着标记一层层追回去，比摸黑试参数快得多。

第四个心得，多推荐一种“语义压缩”的思路。对于配送机器人这种相对规则的任务场景，最高效的路径存储方式不是一堆点坐标，而是结构化的“道路段+动作指令”。比如路径可以表示为：从node_garage_A出发，沿road_ID_17行驶120m，在node_junction_3左转，进入road_ID_22行驶80m，终点node_bdoor。每个road_ID对应路网上的一段边，车到节点处按语义动作执行。这种表达几乎不占存储，也天然具有可解释性，对后续的多车调度、人机交互非常有价值。

我在项目后期尝试做过这种语义路径。全局规划器输出的是“路网+动作”序列，而不是一排坐标点。局部规划器需要坐标时，再动态展开road_ID对应的几何中心线。实测这个方向有两个明显优势：一是路径长度和点坐标在重规划时可复用，二是故障排查时可以直接读路径语义，迅速知道车在哪个路段的哪个节点遇到了障碍。

当然，这个方案的精简能力更强，但对路网的准确性要求也极高，只要路网节点坐标有一丁点累计误差，展开出来的路径就会有明显偏差。适合路网质量已经调试得足够稳定的场景使用。

8. 最后分享一点长期实践的经验

整体跑通这套全局路径精简计算，回过头来最深的体会是：路径精简不该被当作一个“可选优化项”，而应该是配送机器人的默认基本功。很多团队一上来就沉迷于把局部规划器调得天花乱坠，忽略了全局路径层面的冗余才是底层拖累。

只要全局路径这一层的表达足够精简、语义足够清晰，局部规划器才能把算力花在真正需要动态决策的地方。否则，就像让一个短跑运动员在起跑前还要扛着一麻袋沙子在原地反复踩点，又慢又乱。

如果你正要做一个类似的校内配送项目，我的建议是，不要把重点放在寻找某个“神奇压缩算法”上，当初我们也是绕了一大圈，最后发现最稳的组合还是：高质量路网建图、DP压缩加必保点保护、三次样条平滑。这几样做好，路径点数降一个数量级的同时，路径质量也许比以前更高。

在校内这种道路规则、建筑固定的场景里，稳定性和可解释性才是第一位的。路径精简计算的所有工作，最终都是为了这两个目标服务的。做完这一层之后，后续加高精度停靠、多车调度，都会顺畅很多。