城市领航（Urban Navigate on Autopilot, U-NOA）与高快领航（Highway Navigate on Autopilot, H-NOA）是智能驾驶系统落地的核心功能，其实现依赖于高精地图（SD/HQ/HD）与多源定位（GNSS、视觉、IMU、融合感知）。
高快领航（HNOA）典型场景是高速公路、快速路、匝道。需求是进行车道级精度（≤0.2m），低频地图更新，连续定位能力。城市领航（CNOA）的典型场景主要关心城区主干道、交叉口、红绿灯、掉头等。需求是针对高密度语义信息，强鲁棒性定位，快速更新。高精度的地图数据能够为车辆提供详细的道路信息，包括车道线、交通标志、路况等，帮助车辆更好地进行路径规划和决策。而准确的定位方式则能够确保车辆实时获取自身的位置和姿态信息，为 NOA 技术的精确执行提供保障。然而，目前不同类型的地图数据结构和定位方式繁多，各自具有优缺点，如何基于合适的地图和定位数据实现道路路径计算，以满足高快领航和城市领航中 NOA 技术的需求，成为了亟待解决的问题。
本文系统基于 NOA 所使用的地图数据结构与类型、典型定位方式，提出城市领航中非常典型的导航算路逻辑，且在算路过程中以弱地图和学习驱动的中间态发展路径实现常规场景下的城市领航算路逻辑。
一、高精定位方式与融合架构
高精定位技术是实现自动驾驶高快领航和城市领航的关键支撑，其核心原理是通过多种技术手段，对车辆在三维空间中的位置进行精确测定，确保定位精度达到厘米级甚至更高，以满足自动驾驶系统对车辆位置信息的高精度需求。
基于卫星的高精定位技术是目前应用较为广泛的一类定位技术，主要依托全球导航卫星系统（GNSS），如美国的 GPS、中国的北斗卫星导航系统、俄罗斯的 GLONASS 以及欧洲的 Galileo 等。基于地基的高精定位技术则是利用地面基础设施来实现车辆的高精度定位。这类技术包括基于蜂窝网络的定位、超宽带（UWB）定位、激光雷达定位以及惯性导航系统（INS）等。
其高精定位误差可分解为：
其中，eGNSS表示绝对坐标误差（高楼遮挡时增大），eIMU表示积分漂移误差，随时间累积，ePerception则是基于标线/地标的匹配误差。
INS 则是基于加速度计和陀螺仪等惯性传感器，测量车辆的加速度和角速度，通过积分运算推算出车辆的位置、速度和姿态信息，INS 具有自主性强、不受外界环境干扰等优点，但随着时间的推移，误差会逐渐累积，因此通常需要与其他定位技术进行融合，以提高定位的准确性和可靠性 。
如下表示了融合定位可用加权滤波或 EKF 进行优化的典型模型：
其中，f(⋅)是运动模型（IMU 提供），h(⋅)是观测模型（GNSS/Camera/Lidar），wt,vt为噪声项。可以看到，随着时间的推移，定位值xt误差会随着噪声wt而增大，但是将xt加入到h观测模型进行矫正后的输出zt将不断修正这个误差值。这样，这种多模态融合保证了高速场景的连续性和城市场景的鲁棒性。
高精定位技术+INS的航迹推算是助力城市领航算路的基础。
二、解析城市领航中如何通过地图信息算路
简单回顾完地图及定位的基本信息后，我们将重点放在了本文的另一个重要的话题。那就是如何结合地图信息进行算路，特别是这种感知地图结合的算路模型可以很大程度上影响城市领航的效能。
本文将基于此，详细介绍“偏工程实现”的城市领航（CNOA）基于地图算路。重点放在地图怎样抽象成图、怎么分阶段求解、代价函数怎么设计、动态信息如何注入、以及特殊机动（变道/掉头）的约束建模与落地。
2.1、算路目标
我们对城市领航算路的目标是从当前车辆姿态 x0=(p,θ)到目标区域 G 生成合法、可行、代价最小的车道级路线与机动计划（含变道/转向时机），并输出给下游的行为决策与轨迹规划。
其中，输入包括HD/HQD地图要素、环境静态状态与约束、环境动态信息三大板块：
1）HD/HQ 地图（Lanelet2/OpenDRIVE 等）：几何（车道中心线/边界）、拓扑（successor/left/right/merge/split）、规章（单行、禁转、车道属性、优先级、信号灯关联、限速）、道路要素（路缘、护栏、斑马线、停车线）。
2）状态与约束：当前车道占用与车道索引、车辆尺寸/最小转弯半径、车道允许车辆类型、目的地或“目标道路/目标车道集合”。
3）动态信息（可选但强烈建议）:施工/封控、临时禁行、V2X 的 MAP/SPaT、在线交通与历史拥堵、感知到的封闭/断头（感知→地图实时修正）。
基于上述说法，如何在现有导航（SD/HQ/HD）基础上，利用路网 Topology（Topo）+ 导航信息做“二次算路（secondary routing）”，支撑城市领航（CNOA）量产。
2.2、算路前提：多层车道图抽象（Graph Building）
算路前提是需要把多层车道从“全局地图抽象到路径图“。其实就是把地图文件里的几何 + 属性 + 拓扑，转化成算法可以用的图结构（nodes + edges）。
设想城市领航中的一个典型场景：“十字路口 + 四个入口方向”。
东西向：4 车道（双向各 2 车道）。 南北向：4 车道（双向各 2 车道）。 右转车道有专用引导线，左转车道需要在口内掉头。 地图格式：假设用 Lanelet2（很多 NOA 使用的 HD Map 格式）。 |
那么，地图数据原始形态（HD Map）可以定义为车道中心线：一条 polyline，存储了几何坐标。车道属性：方向、限速、是否公交专用、能否掉头。连接关系：successor（直行继续的下游车道）、left/right（邻接的左/右车道，其是否可变道由属性控制）、lanelet relation（路口内的 lane-link，描述 “从 A 车道可以转向到 B 车道”）。
那么，如何将如上一系列地图原始要素抽象成算法可用的图呢？其实质性的工作就是构建<节点，边>，并为这些节点和边附上应有的属性信息。
这里，我们定义“节点= 一个车道段（lanelet segment）”。例如，东进口内侧直行车道 → 是一个节点。东进口外侧右转车道 → 另一个节点。为了更灵活，还可以把一条长车道切分成多个段（每段 50m），这样便于动态赋权（例如前半段畅通，后半段堵塞）。
当车道的几何形状被提取出来后，获取交叉口信息就相对容易了，而详细呈现交叉口的情况至关重要。车道级道路网络中的交叉口不仅包含交叉区域，还涉及与交通规则相关的拓扑关系。虚拟车道在交叉口的自动驾驶导航中被广泛应用。行驶路线可用于抽象地表示虚拟车道，其中既包含路径信息，又体现常规的转向交通情况。有几种用于对行驶路线进行建模的数学函数，包括圆弧曲线、脊线和折线。
如下图表示系统计算的某一个交叉口中某一转向的虚拟车道与行驶路线示例。
2.2、系统分层算路策略（二次算路）
二次算路需要在已有导航路径（APP/IVI 导航或地图 SDK 提供的 road-level 路径）之上，结合 HD/HQ 车道级 topo、动态交通/信号/感知闭塞，实现车道级可行、舒适、稳定的 NOA 路由走廊和机动（直/左/右/掉头/变道就位）。边界条件设计是不替代全局导航，只在其约束内做“走廊化 + 细化 + 动态重规划”。重点覆盖体系架构、算法流程、触发策略、稳定性与安全冗余，以及落地化的参数与验收指标。
算路包括两阶段方法：道路级骨架约束（Fast）和车道级细化（Accurate）。这种分层算路的策略可以简单的总结描述如下：
1）用导航骨架作为大方向。
2）在 HD/HQ/SD 图里只保留导航一致或容差内的路段，避免搜索过大。
3）用 CH/ALT/A* 等高效算法在道路级先校验“可走性”。
4）用静态和时间依赖代价模型修正路径，使结果既符合导航目标，也能应对实际路况。
2.2.1、阶段 A：道路级骨架约束（Fast）
在道路级阶段（road-level）我们既要快速得到“走哪条路（road IDs 序列）”，又要提前剔除或打分那些从工程/法规/动态角度不适合被当作 NOA 骨架的道路。道路级附加约束就是在 road-graph 的边/节点上加入的一系列静态或动态规则、阈值与代价修正，用于快速过滤或重新排序候选骨架。
按性质分为静态约束（基于地图与法规）与动态/时序约束（基于实时数据/预测）。
对于静态约束的工程实现属于硬剪枝（edge.allowed=false），在 road-level 预处理时删除，不纳入候选。而软约束/优先级调整过程中，会把这些映射为每条边的 base_cost 或 dynamic_penalty，用于 A*/CH 等算法的 cost 函数。对于时序/事件驱动约束的工程实现中，这些通过 overlay（动态权重层）注入 road_graph，必须支持快速更新和回滚。
那么，如何将约束融入道路级算路（算法设计）呢？这个过程主要包括两个重要的策略：硬剪枝优先（Pre-filtering）+带权重/多目标优化。即把所有硬约束直接从 graph 中剪掉（forbidden=true），减少搜索空间。这样做的优点是最简单、最快。缺点是若数据错误或 OTA 不一致可能把可行路段误剪掉 → 需要 safe-fallback。把软约束变为 cost 项，使用 A*/CH/ALT 等带启发的算法，或采用多目标/分层搜索（主目标最小时间，次目标最小风险）。其优点是更柔性，允许权衡。缺点是计算复杂度上升，需要合理的权重标定。
在 Road-Graph 上的建模，即相应的数据结构设计如下：
如上图所示为整个道路图层的所有属性信息。通过id作为唯一标识，用来区分每个 lane segment。车道中心线由一系列 GPS 点或相对坐标点构成，决定了车道的实际空间形态。车道段的长度，用于算路径代价（cost function）。车道类型（直行、左转、右转、掉头、应急车道、匝道入口/出口等），直接影响规划逻辑。限速信息，通常来自 HD Map / SD Map，控制规划轨迹的速度上限。车道线类型（包括实线、虚线、双黄、可跨/不可跨），影响是否允许换道。附加语义信息则表征如车道属性（主干道/辅路）、车道优先级（高速主线 vs 匝道）、是否靠近路口。而设置边的代价，则用于最优路径搜索（A*、Dijkstra、强化学习）。可根据距离、交通规则、红绿灯等待时间等动态调整。
而对于算路过程中涉及到的边来说，则通过构建起点和终点车道段 ID来构建拓扑关系类型（包括前序关系、转向关系及换道关系），构建约束条件，比如时间段有效（潮汐车道）、天气条件（大雨禁入某隧道）。
推荐算路流程（高性能 + 可控）如下：
如上图中的代价函数表示为 cost_edge(t0) = travel_time_estimation(edge, t0) + soft_penalty(edge)。其中的两个参数分别表示为运行估计时间估计和软道路约束。当然，其关键是做到路网层的快速但保守筛选：不要把所有软约束当硬剪枝，否则可能丢掉唯一可行路径；但对硬约束必须零容忍。
2.2.2、阶段B：车道级细化（Accurate）
在道路级骨架中，全局导航（高德/百度/Here 等提供的道路级路径）会输出一串 Road IDs 或 Link IDs（例如“G107 国道 → XX 匝道 → XX 高速”）。它是整条路径的“骨架”，决定车辆总体要走哪些道路。实际上就是生成受导航约束的 K 条道路级候选骨架（K=1–3），作为车道级细化的“走廊上界”。其精度较低（几十米级），不包含具体哪条车道。
当系统拿到导航骨架后，不会盲目用 HD map 的所有 lane graph，而是只保留和导航一致的道路/车道子图。
这样能限制搜索空间，提升算路效率，避免出现偏离导航太远的“奇怪”路径。在导航给的骨架路段不可行（比如施工、临时封闭）时，可以选择同功能/同向的绕行替代路段，但限制条件是偏航 ≤ 200–500 m（取决于策略），即保证替代路径不会让车“乱跑”。
1、车道级行为层估算
在车道级细化中，需要输入骨架道路 → 映射到对应的车道簇（lane cluster）。输出：车道段序列 + 变道计划 + 路口机动序列（适合行为层执行），并保留次优走廊做降级/切换缓冲。过程中需要构建Lane-level 有向图：节点=车道段/segment；边=successor / lane-change / lane-link。对即将到来的路口，强制就位约束：在距离阈值前必须进入允许机动的车道集合。
在大规模路网中，如果每次都在车道级 HD 图上做算路，计算量太大。于是先在道路级骨架图上用快速路径算法做一次可达性校验。比如使用CH（Contraction Hierarchies）算法预处理压缩图层，支持超快最短路径查询。或者使用A*这一经典启发式搜索，用在骨架图上可快速验证“目标路段是否可达”。这一步的意义在于可以先用轻量级的算法快速确认骨架是否连通（可行），如果骨架断裂（比如路网中断/导航骨架有误），就能提前发现并修正，而不用浪费时间在 HD 车道图里深度搜索。
2、车道级路径细化估算约束
在车道级路径细化时，边的代价不仅是几何距离，还会引入更真实的约束。其中，静态代价包括路段长度（m）、限速/期望速度、车道数、转向限制。时间依赖代价包括历史速度曲线（某些路段在高峰期更拥堵，代价应调高）、路口延误模型（红绿灯、排队时延统计）、可用性（夜间封路、施工期禁行）。
以上规划路网行驶可行性时需要通过制定一定的规则或代价函数：比如变道边需满足最小连续变道距离Llc,min、线型合法、动态安全窗口（由行为层/感知提供）来对预估的路径进行有效的定义和筛选。
上面的代价模型（量产可调）可以参照如下进行：
其中，如上公式中，de表示距离项（meters）通过polyline 点列逐段累加，定义该边的几何长度，鼓励不绕路。^e(t) 表示预测通行时间。该时间主要是从进入该边的时刻 t 起，预计通过该边所需的时间。可加法分解该时间：
其中，^e(t)：时间依赖速度（历史+实时融合），常用指数平滑或卡尔曼融合。dsignal(t)为信号延迟（路口边/含停止线的段）。通过绿窗预测：已知相位与队列，按预计到达时刻取下一次放行剩余时间 + 排队清空时间。dmerge(t)为匝道合流/瓶颈的排队延时（可用简单容量—需求模型或历史均值）。
Φκ(e)表示曲率/舒适度项用来抑制高弯曲、频繁转折带来的不舒适与操控负担。避免把“必要的法定转弯”过度惩罚（左/右/U 转单独由 turn 项控制），曲率项更多约束路径内部的弯弯绕绕。对分段敏感 → 用积分/累计角，而不是“每段固定罚”。
lc(e) 表示换道指示（0/1，二值：换则 1，不换 0）。推荐动态放大，即结合路口剩余距离和交通密度系数综合判定换道指示，防止临门一脚换道。工程实践中，需要注意实线/导流区/禁变道直接不可行（∞），不要仅靠大权重软惩罚。避免分段导致多次计数：把一次换道的完整轨迹抽象为一条逻辑边来记 1 次。
penturn(e) 为转向惩罚，表示法定机动（直/左/右/U）在操控难度、冲突/信号延迟等方面的先验惩罚。若该路口有保护左转（protected left）且绿窗长，适当降低 pL，若无保护左转/需让行对向直行，增大 pL。
riske为风险指数，指示该边的安全风险先验或在线评估，越高越不建议。当 risk 由法规/封闭触发时直接 ∞ 禁止；当为环境与舒适性考量，用权重惩罚即可。可以对不同的环境因素设置不同的风险影响权重。
其中，workzone为施工/临时改道（1 则强风险）。occlusion_score为路侧遮挡、急弯、视距短。accident_heat为历史事故密度（按路段标准化）。sensor_degrade为隧道、强反光、恶劣天气时段等。当 risk 由法规/封闭触发时直接将惩罚权重设置为∞ 来禁止这样的行为；当为环境与舒适性考量，用权重惩罚即可。当然必须注意：risk 的时变性要有 TTL 与衰减，避免短时噪声导致频繁改道。