总览

本篇文档介绍了如何将四叉树（QuadTree）空间表示法与**模型预测控制（MPC）**集成，用于复杂环境下的机器人路径规划。

该架构的优势在于：

1. 四叉树 提供了适应环境复杂度的空间划分与凸包区域
2. MPC 可在动态、带约束的前瞻性框架下生成最优控制

核心思想是：将四叉树节点生成的凸包区域转化为线性不等式约束，供 MPC 在轨迹优化中使用，完成避障与路径限制。

架构图

   
环境地图 ────► 四叉树分解 ────►凸包生成 

                                            │
                                            ▼

机器人控制 ◄──── MPC优化器 ◄────线性几何约束

四叉树地图表示

四叉树将环境划分为不同分辨率的空间单元：

• 开放区域使用较大的节点
• 障碍物附近使用更小的细节节点
• 仅保留表示自由区域的叶子节点

优势

• 自适应分辨率：按需细化，避免资源浪费
• 内存友好：比统一网格更节省存储
• 支持多分辨率路径规划

凸包区域构建

四叉树分解完成后：

1. 将每个自由叶子节点看作不规则区域
2. 为每个叶子节点生成一个凸包（Convex Hull）
3. 这些凸包表示机器人可以安全通行的区域
4. 邻接凸包间连接形成路径图

MPC 所需的约束生成方式

对于路径经过的每个凸包区域：

1. 凸包每条边转化为半空间线性约束 ax + by + c ≤ 0
2. 约束的法向量需朝向凸包外部
3. 保证 MPC 优化轨迹始终在凸包（安全区）内

数学上，对于边 (x₁,y₁) → (x₂,y₂)：

a = -(y₂ - y₁)
b =  (x₂ - x₁)
c = -ax₁ - by₁

约束形式为：ax + by + c ≤ 0

在这个混合路径规划系统中，DRL 策略负责接管那些局部空间极端受限、传统 MPC 难以生效的区域，特别是“狭窄通道（narrow passage）”问题。为此，我们构建了一个专门应对该类场景的 DRL 策略，并通过 TD3 或 DDPG 算法进行训练。

1. 使用的算法与损失函数

我们使用 TD3 或 DDPG 算法，分别训练策略网络（Actor）和价值网络（Critic）。

DDPG

DDPG 有时能够实现出色的性能，但它在超参数和其他类型的调优方面往往很脆弱。DDPG 的一个常见故障模式是，学习到的 Q 函数开始大幅高估 Q 值，从而导致策略破坏，因为它利用了 Q 函数中的误差。

TD3

td3 在DDPG基础上做到了三个技巧的更新，解决DDPG Q值过高的问题

• 技巧1： 裁剪双Q学习 TD3学习两个Q函数 而不是一个（因此称为twin），并使用两个Q值比较小的一个作为bellman误差损失函数中的目标

• 技巧2： “延迟”策略更新 TD3 更新策略和目标网络的频率低于Q函数，本文建议没更新两次Q函数就进行依次策略更新

• 技巧2： 目标策略平滑 TD3为目标动作添加了噪声，通过平滑动作中的Q的变化，使策略更难利用Q函数误差 这三个技巧可以显著提高baseline DDPG 的性能

• TD3 是一种off-policy algorithm

• TD3 只能用于具有连续动作空间的环境

• TD3 的Spinning up实现不支持并行化

关键方程式： TD3通过 均方bellman误差最小化的同时 学习两个Q函数Q_phi_1和Q_phi_2， 其方式于DDPG学习单个Q函数的方式几乎相同， 为了准确展示TD3的实现方式， 以及它与普通DDPG的区别，我们将从损失函数的最内层向外进行讲解。

• 第一：目标策略平滑 用于构成Q学习目标的动作 基于目标策略/mu_theta_targ , 但在动作的每个维度上添加了截断噪声。添加阶段噪声后，目标动作将被阶段， 使其位于有效动作范围内（所有有效动作，都满足alpha_low <= alpha <= alpha_high）。因此，目标动作如下： 目标策略平滑本质上充当了算法的正则化器（正则化是一组用于减少机器学习模型中过拟合的方法。正则化会用训练准确性的边际下降来换取泛化性的提高。 正则化包含一系列用于纠正机器学习模型过拟合问题的方法。） 它解决了DDPG中可能出现的一种特殊故障模式：如果Q函数逼近器针对某些动作产生了错误的尖峰，策略就会迅速利用改封至，从而导致脆弱或错误的行为。 这种情况可以通过平滑类似动作的Q函数来避免， 而这正是目标策略平滑的设计初衷。

背景介绍：路径规划 + 控制的解耦 vs 联合

在移动机器人导航中，路径规划（如基于地图构建的搜索）与轨迹跟踪（如模型预测控制 MPC）通常被拆解为两个阶段：

• 规划模块：找到一条从起点到目标的无碰路径
• 控制模块：跟随这条路径，使机器人平稳、可控地到达目标

但在复杂环境中，尤其是动态障碍物、曲折通道等场景中，如果不能做好两者的耦合与接口设计，系统效果会受到很大限制。

本项目采用 四叉树（QuadTree）划分安全区域 + MPC 控制器跟踪轨迹 的结构，成功实现了一个典型的路径→控制联合流程。

第一步：通过 QuadTree 提取可行路径

我们使用 connective_quadtree 构建了一个基于图的四叉树分区系统，它可以：

• 在任意障碍物地图上生成 QuadTree 结构
• 快速查询从 start_pos 到 goal_pos 的路径节点
• 每个叶子节点有 center()，表示其几何中心

path_nodes = self.quadtree.find_path(start_pos[:2], goal_pos[:2])
path_points = [node.center() for node_id in path_nodes]

这里输出的是一个 [(x0, y0), (x1, y1), …] 的路径点序列，尚不能直接用于控制器输入。

第二步：MPC 控制器的输入格式要求

为了让 MPC 能正常运行，需要如下格式：

initial_state = [x, y, yaw, v]  # 当前状态
reference_trajectory = np.array([[x0, y0], [x1, y1], ...])

控制器内部会基于参考轨迹进行插值、预测、代价优化。

第三步：桥接逻辑实现（完整整合流程）

我们在 trajectory_generator.py 中实现了完整的桥接类 TrajectoryGenerator，封装逻辑如下：

路径转为 MPC 输入轨迹

ref_path = PathNodeList([PathNode(p[0], p[1]) for p in path_points])
self.set_ref_trajectory(ref_path)

这一过程自动插值生成 TrajectoryNodeList（x, y, theta），最终用于 MPC 滑窗轨迹。

在自主移动机器人中，快速判断哪些区域是可通行的、哪些区域需要避障，是路径规划系统的关键。这里我采用 四叉树结构（QuadTree） 对占用图进行递归划分，再对空闲区域构建 凸包（Convex Hull），生成**连接图（Connectivity Graph）**从而生成可用于路径规划的安全走廊（Safe Corridor）。

一、构建思路概览

参考了项目中 connective_quadtree.py 和 corridor_algorithm.py 中的实现，思路如下：

1. 输入原始占用图（Occupancy Grid）
2. 使用 QuadTreeNode 递归划分空间, 根据RGB颜色判断：
    - 若区域为空（全白[255,255,255]或者接近全白）：标记为“自由区域”
    - 若包含障碍（RGB值大于白色）：继续细分（最多分到 min_size）
    - 黑色（[0,0,0]）表示障碍物
3. 收集所有自由叶子节点作为 Safe Area 候选
4. 为每个叶子节点生成其凸包边界
5. 输出凸包用于路径规划和约束构建

二、QuadTree 类详解（connective_quadtree.py）

Quadtree:

Quadtree 是一种树形数据结构，可以用于将二维空间递归划分成更小的矩形区域（占用和空闲区域），
从整张地图作为根节点开始分裂出四个子区域，每个子区域根据RGB值判断是否有障碍物。

基于原始图像的RGB value来划分

树结构

1. QuadTree 类

   • 主要负责管理整个四叉树结构：
   • root: 根节点
   • leaf_nodes: 所有自由（无障碍物）的叶节点列表
   • max_depth: 树的最大深度

2. QuadTreeNode 类

   • 表示四叉树中的单个节点：
   • values: 节点对应的栅格值(RGB值比如[0,0,0])矩阵
   • _ru_indices: 节点在原始矩阵中的右上角索引 (i, j)
   • id_sequence: 节点的ID序列，表示从根到该节点的路径
   • _neighbors: 节点的邻居 (左, 右, 上, 下)
   • _children: 子节点列表
   • state: 节点状态 (FREE, MIX, FULL)

3. 节点状态：

在复杂的动态环境中，单一的路径规划或控制策略常常难以兼顾全局最优性与局部可行性。本篇博客介绍一个结合了 四叉树（QuadTree）+ 模型预测控制（MPC）+ 深度强化学习（DRL） 的混合轨迹规划框架，该方案的主模块集中在 helper_main_continous.py 文件中，具备良好的模块化与通用性。

一、模块概览

1. 地图生成与表示

• generate_map()：支持多场景自动生成地图，包括边界、静态/动态障碍物与目标点。
• get_geometric_map()：将地图对象转换为可用于膨胀和路径推理的几何结构。
• Inflator 类：对障碍物轮廓进行 buffer 膨胀，提升安全冗余。

2. 四叉树路径规划模块

• 由外部函数 create_quadtree_from_occupancy_map() 从栅格占用地图创建四叉树划分空间
• quadtree_to_mpc_constraints()（外部实现）：提取四叉树路径节点所在区域的凸包，生成可传递给 MPC 的约束区域。

3. MPC 控制器接口

• InterfaceMpc（外部模块）：支持静态/动态障碍物约束注入与轨迹生成。
• integrate_quadtree_with_mpc()：将自由空间区域和动态障碍一起传入 MPC，生成优化轨迹。

4. DRL 策略控制

• 控制逻辑中支持调用训练好的 DDPG / TD3 策略模型预测动作。
• 动作 smoothness 与可行性在 Metrics 中有单独评估指标。

5. 智能切换模块

• HintSwitcher：在 MPC 不再可行或目标区域过于复杂时，切换至 DRL 策略接管。
• 切换机制基于当前轨迹与障碍物距离，具备滞回机制避免频繁切换。

二、系统整体工作流程

以下代码片段概括了系统的主要运行步骤：

# Step 1: 创建地图对象
map_data = generate_map()
mpc_controller = InterfaceMpc(map_data)
config = Configurator()

# Step 2: 四叉树路径规划
quadtree = create_quadtree_from_occupancy_map(occupancy_map)
constraints = quadtree_to_mpc_constraints(quadtree, current_state, goal_state)

# Step 3: MPC 轨迹生成
mpc_controller.update_static_constraints(constraints)
trajectory = mpc_controller.generate_trajectory(current_state, goal_state)

# Step 4: 智能控制切换（DRL or MPC）
switcher = HintSwitcher(max_switch_distance=5.0, min_detach_distance=8.0)
if switcher.switch(current_position, original_traj, new_traj, obstacle_list):
    action = drl_model.predict(observation)
else:
    action = mpc_controller.get_control_action()

三、关键函数解读

quadtree_to_mpc_constraints

将路径所经过的四叉树节点提取对应的多边形凸包作为可通行区域，转换为 MPC 优化器所需的约束形式。

背景：为什么要合并 Safe Area？

在使用 QuadTree 构建环境分区时，每一个 FREE 类型的叶子节点都会生成一个凸包（Convex Hull）作为局部的 Safe Area。但随着环境分辨率提高或障碍物分布稠密，Safe Area 数量可能激增，带来以下问题：

• 路径图节点数量过多，导致 A* 搜索复杂度提升
• MPC 控制器的参考轨迹中断多，规划不平滑
• 可视化和维护困难

因此，将临近的 Safe Area 合并为更大的安全区域 是简化规划图结构、提高控制性能的重要手段。

合并判据设计

在 connective_quadtree.py 和 utils_geo.py 中，我们可以基于以下原则实现 Safe Area 合并：

1. 邻接性：两个凸包边界是否接近或重合
2. 几何相容性：合并后的点集是否仍然可以构成一个有效凸包
3. 面积提升评估：合并后凸包面积未显著膨胀，避免引入死角区域

利用 shapely 和 scipy.spatial.ConvexHull，可以进行合并模拟与几何判断。

合并过程实现思路

合并逻辑可以封装为 merge_safe_areas() 函数，并添加到 QuadTree 类中：

def merge_safe_areas(self, distance_threshold=2.0):
    merged = []
    used = set()
    keys = list(self.convex_hulls.keys())

    for i in range(len(keys)):
        id1 = keys[i]
        if id1 in used: continue
        hull1 = self.convex_hulls[id1]
        points1 = hull1.points[hull1.vertices]

        for j in range(i+1, len(keys)):
            id2 = keys[j]
            if id2 in used: continue
            hull2 = self.convex_hulls[id2]
            points2 = hull2.points[hull2.vertices]

            # 判断是否可合并（如边界相近、联合后仍凸）
            if can_merge(points1, points2, distance_threshold):
                combined = np.vstack((points1, points2))
                new_hull = ConvexHull(combined)
                merged.append(new_hull)
                used.update([id1, id2])
                break

    self.convex_hulls = {f"merged_{i}": hull for i, hull in enumerate(merged)}

其中 can_merge() 可以使用 shapely.Polygon 判断是否相交或几何接近。

项目背景

本项目旨在探索如何使用卷积神经网络（ResNet）提取图像空间特征，并结合LSTM对时序依赖进行建模，从而预测车辆在自动驾驶中的转向角度。

此任务属于端到端自动驾驶建模的一部分，重点在于将连续图像帧映射为连续控制输出（方向盘角度）。

模型结构概览

整体结构采用两阶段：

1. ResNet 特征提取：对每帧输入图像进行空间编码,提取每一帧的空间语义信息，如车道线、前车位置等。
2. LSTM 序列建模：将序列化的图像特征送入LSTM，实现对短期历史状态的记忆和对当前状态的连续预测,预测未来1~N帧的角度

模型结构图

数据与预处理

数据来源为模拟驾驶场景中的中心摄像头图像，标签为方向盘角度。

• 输入帧尺寸统一为 224x224
• 标签为每帧角度（可选包含扭矩、车速）
• 图像预处理：resize → normalize → batch
• 标签标准化：使用均值/方差归一化角度值
• 损失函数：MSE 均方误差损失
• 优化器：Adam, 学习率 = 1e-4
• 序列长度：T = 5（即每个样本为连续5帧）

实验结果

在测试集上，MSE 误差为 0.06，对应角度误差约为 7°。
模型在直线段表现稳定，在连续转弯段有一定预测延迟

关键代码片段：

class ResNetLSTMModel(nn.Module):
    def __init__(self):
        super().__init__()
        self.resnet = resnet18(pretrained=True)
        self.lstm = nn.LSTM(input_size=512, hidden_size=128, batch_first=True)
        self.fc = nn.Linear(128, 1)

    def forward(self, x_seq):
        batch, seq_len, C, H, W = x_seq.shape
        feats = [self.resnet(x_seq[:, i]) for i in range(seq_len)]
        feats = torch.stack(feats, dim=1)  # [B, T, D]
        out, _ = self.lstm(feats)
        return self.fc(out[:, -1])

模型选择与对比分析

为什么选择 ResNet 而不是简单 CNN：

在移动机器人路径规划项目中，地图的表达形式对规划算法的效率与可行性起着决定性作用。本文将介绍两种核心地图结构 —— GeometricMap 与 OccupancyMap，并结合代码逻辑说明它们与四叉树（QuadTree）在路径规划中的协作方式。

一、OccupancyMap 占用地图

OccupancyMap 是通过图像（通常是.png地图或实时图像）生成的像素级地图，用于表示每个区域是否可通行。

占用网格图将空间离散化为任意分辨率的正方形,为每个正方形分配一个二进制值0/1标志被占用或者空闲

一旦将空间表示为图，就有经典的最短路径图算法可以保证在给定无限的计算时间和资源的情况下找到最短路径。

功能概述

• 接收一张地图图像 map_image，支持 RGB 或灰度图；
• 将图像转为灰度图后，基于 occupancy_threshold 生成二值图（黑=障碍，白=可通行）；

• 可返回背景图、灰度图或二值图；
• 支持从图像提取边界与障碍的几何信息（返回 boundary_coords 和 obstacle_list）。

典型代码片段

occupancy_map = OccupancyMap(map_image)
boundary_coords, obstacle_list = occupancy_map.get_geometric_map()

在项目中的作用

• 提供最原始的环境表达方式
• 可以作为输入转为 GeometricMap
• 支持动态更新：如结合摄像头识别障碍，实时构建地图

二、GeometricMap 几何地图

GeometricMap 用于几何表达方式表示边界和障碍，以便生成高精度的导航地图（用于后续凸包、安全走廊、参考路径的构建）。

GeometricMap 是“几何表达形式的地图”，用于：

它不是图像地图，而是 多边形表达的结构地图

例如：

boundary = [(0,0), (10,0), (10,10), (0,10)]
obstacle_list = [[(2,2), (3,2), (3,3), (2,3)]]

map = GeometricMap(boundary, obstacle_list)

这里就表达了一个 10x10 的区域，里面有一个 1x1 的方形障碍物。

深度强化学习（Deep RL）发展出多个主流算法流派，包括基于值函数的 DQN、基于策略梯度的 REINFORCE/PPO，以及融合策略和值函数的 Actor-Critic 框架（如 A2C、DDPG、SAC）。每种方法适用于不同场景，选择合适算法将显著影响模型性能与训练效率。

三大算法流派对比

一、DQN（Deep Q-Network）

类型：Value-based

损失函数（均方 TD 误差）：

L(θ) = 𝔼ₜ [(rₜ + γ · maxₐ′ Qθ⁻(sₜ₊₁, a′) − Qθ(sₜ, aₜ))²]

其中：

• θ 是 Q 网络参数
• θ⁻ 是目标网络参数（定期同步）
• 使用贪婪策略选择 a′ = argmax Q(s′, a′)

优化器：Adam 或 SGD，通过反向传播最小化 TD 误差更新 θ。

在深度强化学习（Deep Reinforcement Learning）中，我们的目标是：训练一个智能体（Agent），使其能够在复杂环境中通过交互、试错和学习，掌握解决任务的策略。

整个训练流程不是一行 .fit() 就能完成的，它涉及数据采集、策略评估、价值估计、梯度优化等多个协同模块。本文将逐步介绍训练所需的关键模块和完整流程。

DRL 训练流程的核心结构

一个标准的 DRL 系统至少包括以下几个部分：

1. 环境（Environment）

• 提供 reset() 和 step(action) 接口
• 返回状态、奖励、终止信号和调试信息
• 通常使用 Gymnasium 编写，也可以是仿真器（如 PyBullet、AirSim）或实际系统接口

2. 策略网络（Policy Network）

• 输入当前状态，输出一个动作（或动作分布）
• 对于离散动作空间，常见输出为 softmax 分布；连续动作空间则直接输出浮点数
• 通常是 MLP（结构化输入）或 CNN（图像输入）网络

3. 值函数网络（Critic, 可选）

• 用于评估当前策略下某状态的“好坏”，即状态值 V(s) 或动作值 Q(s,a)
• 在 Actor-Critic 架构中，Actor 提出动作，Critic 提供反馈

4. 回放缓存（Replay Buffer）

• 保存经验 (state, action, reward, next_state, done)
• 支持随机采样，避免训练中数据高度相关
• 对于 off-policy 算法（如 DDPG、TD3）是必要组件

5. 优化器与更新规则（Optimizer）

• 根据策略梯度、TD 误差、KL 散度等损失函数对网络参数进行更新
• 通常使用 Adam 优化器

强化学习算法的三种主流架构

• Policy-based 方法训练稳定性强、适合高维动作空间，但样本效率较低。
• Value-based 方法样本效率较高，适用于离散动作任务，但连续控制较困难。
• Actor-Critic 综合两者优势，是当前主流算法的主干框架。

一个完整训练循环的结构

以下是一个 off-policy 强化学习算法（如 DDPG）的大致流程：

Gymnasium 是 Python 中一个标准化的强化学习环境库（它是原始 OpenAI Gym 的升级版）。它的作用是：

把复杂问题（如机器人走路、自驾车避障）包装成统一的“游戏接口”，让强化学习模型可以跟它反复互动、学会做决策。

换句话说，它是一个统一标准，让算法和环境能“说上话”。只要环境符合 Gymnasium 接口，你就能直接套用主流算法如 PPO、DDPG、DQN 去训练。

如何安装 Gymnasium？

在终端输入以下命令安装（建议使用虚拟环境）：

pip install gymnasium[all]

如果你只用基本环境，不包括 Atari、Box2D 等，可以简化为：

pip install gymnasium

在代码中如何导入并使用

import gymnasium as gym

env = gym.make("CartPole-v1")
obs, info = env.reset()

for _ in range(100):
    action = env.action_space.sample()  # 随机动作（一般训练中由模型决定）
    obs, reward, terminated, truncated, info = env.step(action)
    if terminated or truncated:
        obs, info = env.reset()

这段代码展示了标准的强化学习交互流程：

1. 初始化环境（reset()）
2. 持续循环：选择动作 → 执行动作 → 接收反馈

环境的输入与输出：Env 接口结构

输入：action

你给环境的输入是一个“动作”，比如：

通过本案例可以掌握：

• 如何使用 C++ 编写一个 ROS 设备驱动节点；
• 如何调试 ROS 发布频率、时间戳和消息内容；
• 如何结合 rosbag 与 rqt_plot 分析传感器数据的稳定性；
• 为后续多传感器时间同步与融合（如 EKF）打下基础。

多传感器融合中的 ROS 驱动开发实战案例

在自动驾驶系统中，IMU、GNSS 和雷达等传感器是实现环境感知和车辆定位的关键。本文以 ROS 框架为基础，讲解如何编写一个简单的传感器驱动节点，实现数据采集 → 解析 → 发布 → 可视化，并附带常见调试命令。

1. 项目结构

my_sensor_driver/
├── launch/
│   └── sensor_driver.launch
├── src/
│   └── imu_driver_node.cpp
├── include/
│   └── imu_driver.hpp
├── config/
│   └── imu_config.yaml
├── CMakeLists.txt
├── package.xml

• my_sensor_driver： ROS package 根目录，所有内容都放在这里

• launch/sensor_driver.launch：

  • ROS 的启动脚本，用 XML 格式描述：
    • 启动哪个节点（node）
    • 使用哪些参数文件（如 config/imu_config.yaml）
    • 是否输出日志到屏幕等
    • 用法：roslaunch my_sensor_driver sensor_driver.launch
    • 示例：

    <launch>
    <!-- 加载 IMU 参数配置 -->
    <rosparam file="$(find my_sensor_driver)/config/imu_config.yaml" command="load"/>
    
    <!-- 启动 IMU 驱动节点 -->
    <node name="imu_driver" pkg="my_sensor_driver" type="imu_driver_node" output="screen"/>
    </launch>
    

• src/imu_driver_node.cpp

在强化学习中，状态空间（Observation Space）和动作空间（Action Space）是两个基础概念。理解这两个空间，就等于弄清楚“模型看到了什么”和“模型可以做什么”。

在本项目中，我们使用的是基于图像和传感器输入的强化学习模型，用于端到端的控制任务，如预测方向、速度或操作行为。下面我们从通用概念讲起，最后具体说明本项目的定义。

状态空间（Observation Space）

状态空间描述了智能体每一步能从环境中获取到的“状态信息”。这些信息构成了模型的输入，可以是一个向量，也可以是图像、组合信号等。

常见类型：

本项目中的状态空间定义：

在本项目中，我们的状态空间通常包括：

• 图像帧：从机器人前置摄像头获取的 RGB 图像（如 (224, 224, 3)）
• 附加状态向量（可选）：如上一帧速度、转角、航向角等传感器读数

状态空间结构可能是：

Box(low=0, high=255, shape=(224, 224, 3), dtype=np.uint8)

或

Dict({
    "image": Box(...),
    "speed": Box(...),
    "yaw": Box(...)
})

动作空间（Action Space）

动作空间描述了模型每一步可以采取的动作范围，也就是输出的结构。

常见类型：

本项目中的动作空间定义：

我们通常使用连续动作空间来控制机器人，例如：

数据结构与算法（自动驾驶应用方向）

在自动驾驶中，无论是路径规划、障碍物识别，还是实时控制，都离不开对数据的高效组织和计算方法的选择。

2. 常见数据结构（及自动驾驶应用）

3. 算法在自动驾驶中的典型应用

3.1 搜索与路径规划算法

3.2 数学优化类算法

3.3 搜索结构加速

🧪 4. 示例：A* 路径规划（伪代码）

def A_star(start, goal, map):
    open_list = PriorityQueue()
    open_list.put(start)
    while not open_list.empty():
        current = open_list.get()
        if current == goal:
            return reconstruct_path()
        for neighbor in get_neighbors(current):
            if not visited(neighbor):
                cost = calculate_cost(current, neighbor)
                open_list.put(neighbor, cost)

📘 推荐学习资料

• 《算法图解》：通俗易懂的入门书
• 《数据结构与算法分析》：理论+实现
• LeetCode 专题：图、堆、树、搜索优化
• ROS Navigation Stack 源码分析

理想、小鹏、蔚来的

城市NOA技术战局（上）：从BEV到VLA，拆解头部玩家技术路线

主要结论：

继2023–2024年BEV+Transformer+Occupancy Network架构成为智能驾驶的感知主流后，2025年起，行业正加速向更高级的**VLM（Vision-Language Model）与VLA（Vision-Language-Action）**模型演进。相较于传统BEV结构仅关注空间几何关系，VLM/VLA引入语言推理模块，可对标志、规则、指令进行语义理解，结合多模态输入（如摄像头图像、语音交互、交通文本信息），生成更泛化、更类人的行为决策。

芯片与算力能力成为支撑城市NOA架构升级的关键基础。2020年前后主流平台算力集中于144 TOPS，如今已普遍跃升至500 TOPS以上，高端平台如蔚来NX9031、理想Thor-U、小鹏图灵AI芯片等更达到千级算力，支撑端到端推理、大模型部署与多模态处理。自研芯片、自建智算中心、自主模型训练正成为具备战略自主能力车企的核心壁垒。

在感知侧，“视觉为主+激光雷达补盲”已成为当前主流配置。视觉方案具备数据规模与算力适配优势，激光雷达则增强空间精度与安全冗余，尤其在无高精地图场景中具备更强稳定性。不同厂商依据策略选择单感知或多模态融合方案，构建安全容错体系。

1. 产品背景

近年来，随着特斯拉AutoPilot不断进阶至FSD，辅助驾驶已从L0级定速巡航，逐步拓展至高速NOA，再迈入城市NOA阶段，覆盖更复杂多变的交通环境。智能驾驶正在从“技术亮点”转向“核心卖点”，显著影响消费者购车决策。

以小鹏和问界为例：小鹏G6智驾版配置占比超过70%，问界新M7的大定用户中也有60%以上选择搭载高阶智能驾驶方案。这一趋势标志着，中国智能驾驶市场已全面进入L2+/NOA功能的规模化普及阶段。

L2+辅助驾驶能力包括自适应巡航（ACC）、车道居中控制（LCC）、自动变道（ALC）、高速NOA等，城市NOA则在此基础上，扩展至非结构化路况中的红绿灯通行、环岛绕行、路口博弈等复杂决策工况，对感知精度、决策智能与系统泛化能力提出更高要求。

在城市NOA推进过程中，高精地图鲜度不足、覆盖受限、成本高昂等问题，成为核心瓶颈。为突破此限制，主流技术路线正在从依赖地图的规则驱动方案，全面转向端到端模型驱动 + 自主环境建图 + 多模态策略融合的新范式。

技术架构上，感知模块经历了从前向2D视角 → BEV建图 → Occupancy Network建模的迭代，决策控制模块也正由传统rule-based状态机，向基于神经网络的端到端行为预测发展。2025年起，VLM（视觉语言模型）和VLA（Vision-Language-Action）架构逐步上车，赋予系统语义理解与策略推理能力，成为当前算法演进的前沿方向。

本报告将围绕特斯拉、小鹏、理想、蔚来等具备自研能力的头部品牌，梳理其城市NOA技术路线演进路径，结合软硬件系统方案与市场落地节奏，探讨中国品牌如何在“从BEV到VLA”的产业跃迁中建立领先优势。

2. 目标用户与使用场景

L2/L2+辅助驾驶系统主要面向具备一定驾驶经验、希望在日常通勤或长途驾驶中降低疲劳、提升安全性的新能源车主及购车潜在用户。

这类用户普遍关注两个关键问题：

各品牌城市NOA在不同城市、路况下的表现差异；

智驾能力与整车价格之间的性价比。

智能驾驶的核心价值在于：在用户监管下，系统能够预判潜在风险、辅助完成复杂驾驶动作，从而减轻认知负担、降低事故风险。尤其在城市NOA场景中，系统需完成红绿灯识别、路口博弈、避让行人、自主换道等行为，对系统泛化能力、策略鲁棒性和人机交互体验提出更高要求。

因此，城市NOA不仅是“功能拓展”，更代表了整车系统智能化水平的“天花板”。

3. 竞品分析

本篇章节主要对于特斯拉，以蔚来理想小鹏为代表的头部企业在“城市导航辅助驾驶功能”的进展上进行对比分析，除此之外还包括，硬件系统方案，软件算法，功能配置，交互逻辑，运营方案，亮点场景等对比。

3.1 调研洞察摘要

城市NOA的落地，标志着行业正从规则驱动向数据驱动、从模块化架构向端到端大模型架构转变。以下趋势尤为明显：

技术架构趋同： 特斯拉、小鹏、蔚来等主流厂商均采用BEV+Transformer+Occupancy Network感知结构，逐步引入端到端神经网络控制器替代传统状态机，提升整体智驾稳定性。

算力平台升级： 主流平台从原先144TOPS提升至500–1000TOPS以上，大算力平台成为支持端到端大模型部署的基础设施。

数据闭环能力强化： 具备自动采集、筛选、标注、仿真重建与在线训练能力的“数据-模型-闭环”体系，成为城市NOA性能提升的核心引擎。

用户体验导向增强： 城市NOA正加速从试点区域向全国多城落地，主机厂纷纷围绕“全国都能开、用户真愿用”目标，推动功能迭代与大模型泛化能力提升。

结论：在城市NOA成为新一代智能驾驶标配的背景下，能否构建“低成本、高泛化、大闭环”的自研体系，将成为厂商能否突围的核心分水岭。

3.2 技术方案演进与“去高精地图化”

高速NOA因场景结构稳定，长期采用规则驱动+高精地图方式实现；而城市NOA因存在海量动态变量与非结构化场景，传统Rule-based方案难以覆盖，技术路线普遍演进为以下三个阶段：

基于高精地图的三段式结构： 感知–规划–控制，依赖静态地图与有限状态机；

BEV+Transformer感知融合结构： 引入Occupancy Network构建3D空间占据图，提升遮挡处理与障碍识别能力；

端到端 + 多模态语义推理架构（VLA）： 引入视觉语言模型、扩散策略网络、博弈推理模块等，实现感知–理解–决策一体化。

“去高精地图化”已成为城市NOA发展的行业共识，各家方案通过Occupancy Network、弱地图辅助、分布式建图等方式逐步替代静态地图依赖，以提升系统泛化能力与落地效率。

3.2.1 特斯拉技术演进：

自2014年发布AutoPilot 1.0以来，特斯拉持续引领辅助驾驶从规则驱动走向数据驱动、再走向端到端神经控制的范式演进。

HW1.0 + Mobileye平台（2014–2016）： 初代AutoPilot基于Mobileye方案，支持基本的巡航与车道保持功能，但受限于单摄像头配置与规则逻辑，系统泛化能力有限。

汽车行业智能化下半场：人工智能时代的自研趋势以及产品力跃迁

本报告基于对当前中国主流智能车企技术路径的梳理，重点聚焦AI时代自研趋势、技术栈重构与用户体验跃迁路径，提出自研芯片+操作系统在产品竞争力打造中的关键价值。

一、引言：智能驾驶竞争进入“底层能力为王”阶段

随着智能驾驶算法从传统的三段式架构逐步演化为统一大模型，底层算力的压力正在指数级上升。过去由规则驱动的架构，正逐步让位于数据驱动与模型推理为主导的新范式。小鹏、理想都曾表明目标是成为面向全球的AI汽车公司（小鹏想成为面向全球的AI汽车公司，李想“隐退”九个月后高调发声，要将理想汽车打造成AI公司）， 在这种趋势下，AI三要素中的数据和算力，不再是支持角色，而正在成为影响智驾系统架构、决策能力和推理效率的“底层决定性资源”。

以智能驾驶主控芯片为例，从2019年Mobileye EyeQ4的24 TOPS算力，到2023年NVIDIA Orin-X单颗256 TOPS，算力提升10倍以上，芯片价格和适配难度也随之飙升。公开数据显示，Orin-X 零售单价接近 500 美元，在算法开发完成后仍需4~6个月进行底层适配与算子调优。芯片不再是“模块化采购”的一环，而是演化为算法性能释放的“主控变量”。

回顾历史，从智能手机厂商（如苹果、三星以及华为）到云厂商（如亚马逊、谷歌），都经历了通过自研芯片实现性能边界控制、系统主导权和成本结构优化，自研芯片已成为“技术驱动产品体验”的底层共识。

面对这一演进趋势，蔚小理纷纷加速芯片自研布局，希望打通“自研算法+定制芯片”的协同链条。在这条路径上，在国内厂商中，华为无疑走在更前一步——其MDC系列芯片已在极狐、阿维塔等平台落地，与ADS系统形成“芯片即平台”的整合模式。

自研（智驾/座舱）芯片不仅提高了算力上限，降低了整车成本，同时让企业掌握底层算力与接口设计的主动权，从而决定了智驾域和座舱域从芯片架构、操作系统、通信协议到电子电器架构等多个层级的自主可控程度。这些系统共同决定了智能驾驶系统的处理能力、响应速度、功能复用率和可持续升级能力。

这一趋势正是推动智驾系统从“功能运营型”向“技术驱动型”转变的关键分水岭——决定其是跟随迭代，还是主导演进。

但这场底层变革之战的真正价值，或许并不止于技术领先本身，而在于它能否带来一种“自我定义产品体验、构建自驱闭环”的能力边界重塑。自研芯片+自研架构+OS 作为企业软硬结合的锚点，其作用机制、投入路径与演进节奏，也因此成为观察智能车产业未来格局演化的关键切口。

本文将聚焦华为与小鹏两家典型代表，从技术栈演化、系统能力建构、用户体验反馈、品牌市场影响等多个维度展开分析，尝试还原自研芯片与操作系统背后，技术战略与产品能力之间的真实因果链条。

二、以华为为例，解读OS+MDC+集中式架构带来的体验跃迁

MDC计算平台作为算力支撑，鸿蒙OS构建生态加强交互体验，集中式架构降低系统时延提升智驾功能表现。

2.2 华为 MDC 智能驾驶计算平台—自2018年起持续演进

规划与产品矩阵完善（2018–2021）

2018年3月，华为推出首款MDC智能驾驶计算平台，最初基于昇腾310芯片，迄今已发展出包括MDC210、300F、610、810、Pro 610等产品，覆盖从L2到L4的不同自动驾驶级别。2024年，搭载MDC610的车型可升级到ADS 3.0，为高阶城市NOA提供了算力基础。

更高算力平台到位（2021–2023）

随后推出的MDC810以稠密算力峰值超400 TOPS，支持更复杂的多传感器融合与高精度感知，为L3前装落地铺设平台基础。

最新演进方向：MDC Pro 610系列

截至2025年，华为MDC产品线已新增MDC Pro 610等型号，搭载更高算力的昇腾610芯片，并采用12位LPDDR5+液冷散热方案，实现500–800+ TOPS算力与更高能效密度。华为 MDC Pro 610 智能驾驶控制器主要用于执行融合、识别、分类任务，同时处理地图定位，以进行路径规划和驾驶决策，从而实现车辆的精确控制和自动驾驶。

2.3 麒麟芯片追赶高通地位，搭配鸿蒙OS，提升智能座舱交互体验

在智能汽车逐步迈入“全栈自研”的时代，操作系统作为底层能力承上启下，是实现软件解耦、整车协同和体验跃迁的核心基座。华为率先部署三层操作系统体系——HarmonyOS OS（座舱）+ AOS（车控）+ VOS（智驾），不仅实现了智舱、智驾、车控的能力分离与协同调度，更通过操作系统的持续演进，显著提升整车的流畅性、智能性与安全性体验。

HarmonyOS：从“操作系统”到“生态系统”，提升人车交互体验

作为车载鸿蒙的重要落地形态，HarmonyOS 不仅承担座舱控制任务，更整合手机、平板、手表、家居IoT等跨终端生态能力，打造“万物互联”的空间体验：

分布式架构打造跨终端无缝协同体验，支持多设备间无缝流转（导航、通话、视频会议、音乐等）；

语音助手基于端云协同大模型能力升级（如方舟语音大模型），具备更高意图理解力和情境感知；

界面响应与流畅性极大提升：借助统一内核与多线程渲染机制，复杂场景下UI响应延迟降低40%、用户交互成功率提升25%以上。

如在问界M9、小鹏G6等车型上，鸿蒙座舱系统实现“智慧语音 + 视觉感知 + 空间音效 + 多设备协同”的沉浸式体验，显著领先传统车机。

AOS：高实时性车控操作系统，提升系统响应稳定性

AOS是专为汽车底层控制场景设计的轻量级实时操作系统，以调度精度、冗余安全与定制能力为核心特性：

精确控制如转向、电机、车身等关键子系统

对车规芯片如MCU/SoC适配性强，支持多线程任务管理与高优先级中断

与自研芯片联合优化，可实现端到端时延<10ms、异常恢复时间<100ms的性能表现

全域领航辅助NOP+

1. 产品概述

全域领航辅助NOP+是一种舒适性功能，它是能让车辆根据导航规划好的路线进行行驶的辅助驾驶功能，能够完成如路口通行、导航变道、超车变道、绕向车辆等驾驶任务。在面对施工区域、通用障碍物等场景时，车辆具有安全能力，能够提供联通地面道路与城市快速路、高速公路的辅助驾驶体验。

2. 车型配置

本文档中提出的 NOP+ 全域领航辅助功能，配置在 ES8车型上。

3. 参考文档

4. 名词解释

5. 子功能介绍

自动辅助驾驶Pilot包括高速自动辅助驾驶（highway pilot）、拥堵自动辅助驾驶（traffic jam pilot)两种功能, 主要适用于车道线清晰及有进入限制的封闭道路上使用，譬如高速和高架的主路及拥堵路段，且在本车道内行驶，不会换道变道行驶。

激光雷达（Lidar）是自动驾驶中感知环境的核心传感器之一，它输出的是一帧帧点云数据（point cloud），表示空间中障碍物的位置和形状。

1. 什么是点云？

点云是由一系列点（x, y, z, intensity）组成的数据集，每个点表示空间中某个反射物的坐标和强度信息。

2. 点云数据结构

3. 常见处理流程

3.1 点云预处理

• 体素滤波（VoxelGrid）：降低点云密度，加快处理速度
• 裁剪（CropBox）：去除远处或地面点，聚焦感兴趣区域
• 时间同步：与IMU/GNSS同步，统一坐标系

3.2 点云配准与融合

• ICP算法（Iterative Closest Point）：配准不同帧点云，构建地图
• NDT算法：用于鲁棒配准，适应点云局部稀疏问题
• 多帧融合：提升障碍检测的完整性

3.3 点云感知

• 地面分割（Ground Segmentation）：识别并移除地面点
• 聚类（Euclidean Clustering）：提取独立目标（车辆、行人）
• 目标跟踪与识别：结合深度学习模型进行物体分类

4. 使用工具与库

5. 示例：PCL聚类伪代码

pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance(0.5); // 50cm内为一类
ec.setMinClusterSize(30);
ec.setMaxClusterSize(2500);
ec.setSearchMethod(tree);
ec.setInputCloud(cloud_filtered);
ec.extract(cluster_indices);

推荐学习资料

• PCL官方文档
• Open3D教程
• Apollo / Autoware 点云处理模块源码分析
• Udacity Sensor Fusion Nanodegree