flykhan
2536c937e3
翻译自英文原版 maths-cs-ai-compendium，共 20 章全部完成。 第01章 向量 | 第02章 矩阵 | 第03章 微积分 第04章 统计学 | 第05章 概率论 | 第06章 机器学习 第07章 计算语言学 | 第08章 计算机视觉 | 第09章 音频与语音 第10章 多模态学习 | 第11章 自主系统 | 第12章 图神经网络 第13章 计算与操作系统 | 第14章 数据结构与算法 第15章 生产级软件工程 | 第16章 SIMD与GPU编程 第17章 AI推理 | 第18章 ML系统设计 第19章 应用人工智能 | 第20章 前沿人工智能 翻译说明： - 所有数学公式 $...$ / $$...$$、代码块、图片引用完整保留 - mkdocs.yml 配置中文导航 + language: zh - README.md 已翻译为中文（兼 docs/index.md） - docs/ 目录包含指向各章文件的 symlink - 约 29,000 行中文内容，排除 .cache/ 构建缓存
313 lines
23 KiB
Markdown
313 lines
23 KiB
Markdown
# 自动驾驶汽车
|
||
|
||
*自动驾驶汽车是商业上最先进的自主系统，将感知、预测、规划和控制集成到单个车辆中。本章涵盖自动驾驶堆栈、高精地图、运动预测、规划、端到端驾驶、仿真、安全标准和自主等级。*
|
||
|
||
- 自动驾驶汽车可以说是正在大规模尝试的最困难的机器人问题。与在受控环境中运行的工厂机器人不同，自动驾驶汽车必须处理一个开放世界：不可预测的人类驾驶员、乱穿马路的行人、一夜之间出现的施工区域以及每分钟都在变化的天气。
|
||
|
||
- 其风险也异常之高。自动驾驶汽车在高速公路上行驶，周围是脆弱的道路使用者。对于安全关键的故障，误差容限几乎为零。
|
||
|
||
## 自动驾驶堆栈
|
||
|
||
- 经典的自动驾驶架构是一个**模块化流水线**，包含四个阶段，每个阶段作为下一个阶段的输入：
|
||
|
||
$$\\text{感知} \\to \\text{预测} \\to \\text{规划} \\to \\text{控制}$$
|
||
|
||
|
||
|
||
- **感知**（本章文件1中已介绍）将原始传感器数据处理为结构化的场景表示：带有3D位置、速度和类别标签的检测物体；车道标线；交通信号灯；可行驶表面边界。
|
||
|
||
- **预测**预测其他交通参与者（车辆、行人、骑行者）未来将如何移动。给定场景的当前状态，预测模块为每个交通参与者输出未来一段时间（通常3-8秒）的轨迹。
|
||
|
||
- **规划**决定主车应该做什么：走哪条路径、何时变道、何时让行、何时加速或刹车。它接收预测的场景，为主车生成一条安全、舒适且向目的地前进的轨迹。
|
||
|
||
- **控制**将规划的轨迹转化为执行器命令：转向角、油门和刹车。这是最底层，将抽象轨迹转化为物理运动。
|
||
|
||
- 模块化设计有明确的工程优势：每个模块可以独立开发、测试和改进。但它也有弱点：误差向下游传播（漏检对规划器是不可见的），并且信息在每个接口处丢失（规划器看到的是边界框，而不是产生它们的丰富传感器数据）。
|
||
|
||
## 高精地图
|
||
|
||
- **高精（HD）地图**是详细、厘米级精度的数字地图，编码道路结构：车道边界、车道连通性（哪个车道在交叉口连接到哪个）、交通标志位置、限速、人行横道位置和路面高程。
|
||
|
||
- 高精地图为驾驶任务提供了强有力的先验。感知模块不需要每帧从头发现车道边界；它只需要将车辆在地图中进行定位，并验证现实是否与存储的结构匹配。这极大地简化了规划。
|
||
|
||
- 构建高精地图需要配备高端LiDAR、相机和RTK-GPS的专业测量车辆。地图必须随着道路变化而维护和更新。这很昂贵，且不容易扩展到地球上的每条道路。
|
||
|
||
- **无图驾驶**（也称为"在线地图构建"）旨在消除对预建高精地图的依赖。车辆从传感器实时构建局部地图。像**MapTR**和**MapTRv2**这样的模型使用transformer架构直接从相机图像预测矢量化地图元素（车道中心线、道路边界、人行横道），将多段线输出为有序点序列。
|
||
|
||
- 无图方法用地图精度换取可扩展性：任何汽车能行驶的道路，它都能建图。但它要求感知系统足够鲁棒，能够实时检测所有相关的道路结构，包括在复杂交叉口、高速公路匝道和施工区域中。
|
||
|
||
- 在实践中，许多系统采用混合方法：轻量级地图包含粗略的道路拓扑（来自现有地图提供商），并通过车辆的传感器实时丰富。
|
||
|
||
## 运动预测
|
||
|
||
- 预测其他道路使用者将去哪里是自动驾驶中最困难的子问题之一。人类不可预测，意图是隐藏的，未来可能性的空间迅速分叉。
|
||
|
||
- 预测模型的输入是**场景上下文**：所有检测到的参与者在近期过去（通常1-2秒的历史）的位置和速度，加上静态上下文（车道几何、交通信号、道路边界）。
|
||
|
||
- 输出是每个参与者的一组**预测轨迹**，通常覆盖未来3-8秒。由于未来是不确定的，好的预测模型输出多条可能的轨迹及其相关概率，而不是单一的点估计。
|
||
|
||
- **轨迹预测**作为一个回归问题：预测每个参与者在离散未来时间步的$(x, y)$坐标。损失通常是$K$条预测轨迹上的最小平均位移误差（minADE）：
|
||
|
||
$$\\text{minADE}_K = \\min_{k \\in \\{1, \\ldots, K\\}} \\frac{1}{T} \\sum_{t=1}^{T} \\| \\hat{\\mathbf{p}}_t^{(k)} - \\mathbf{p}_t \\|_2$$
|
||
|
||
- 这是一个"最佳$K$个"指标：如果模型的$K$个预测中有一个接近真实值，模型就得分。这鼓励多样化的多模态预测。
|
||
|
||
- **社会力模型**将行人行为建模为动力系统，其中每个人受到吸引力（朝向目标）和排斥力（远离其他行人和障碍物）。行人$i$的加速度为：
|
||
|
||
$$\\mathbf{a}_i = \\frac{\\mathbf{v}_i^{\\text{期望}} - \\mathbf{v}_i}{\\tau} + \\sum_{j \\neq i} \\mathbf{f}_{ij}^{\\text{排斥}} + \\sum_{\\text{墙壁}} \\mathbf{f}_{\\text{墙壁}}$$
|
||
|
||
- 这是一个与本章文件2中的机器人动力学方程类似的微分方程组。该模型优雅但依赖于手工调谐的力参数，并且在复杂多智能体交互中表现不佳。
|
||
|
||
- **图神经网络（GNN）**用于预测时将场景建模为图：每个参与者是一个节点，边表示空间关系（邻近度、共享车道）。节点之间的消息传递捕获交互："这辆车正在给那个行人让行"或"这两辆车正在汇入同一条车道。"
|
||
|
||
- 现代预测架构（例如**MTR**、**QCNet**）使用基于transformer的模型，联合推理参与者历史、地图上下文和参与者之间的交互。参与者通过交叉注意力关注相关的地图特征（当前车道、即将到来的交叉口）和其他参与者（前车、人行横道上的行人）。输出是一组通过自回归生成或混合模型产生的轨迹假设。
|
||
|
||
- **目标条件预测**首先预测参与者可能去哪里（一组候选目标点，如车道端点或交叉口出口），然后预测到达每个目标的轨迹。这将问题分解为"去哪里"（离散的、可管理的）和"怎么去"（给定目标的连续路径），使多模态预测问题更加可解。
|
||
|
||
## 规划
|
||
|
||
- 给定预测的场景，规划器必须为主车生成一条轨迹。这是一个约束优化问题：找到一条安全、舒适、高效且合法的轨迹。
|
||
|
||
- **基于规则的规划器**将驾驶行为编码为一组if-then规则："如果行人在人行横道上，让行"、"如果与前车距离小于2秒，不变道"、"如果接近红灯，减速停在停止线处。"这些规则是可解释和可审计的，但对于复杂场景（数千条规则、许多边缘情况、规则间的交互），它们变得难以管理。
|
||
|
||
- **基于优化的规划器**将驾驶形式化为轨迹优化。主车轨迹被参数化（例如，作为未来时间步的$(x, y, \\theta, v)$状态序列），并最小化一个目标函数：
|
||
|
||
$$\\min_{\\boldsymbol{\\xi}} \\underbrace{w_1 \\cdot J_{\\text{进度}}(\\boldsymbol{\\xi})}_{\\text{到达目的地}} + \\underbrace{w_2 \\cdot J_{\\text{舒适}}(\\boldsymbol{\\xi})}_{\\text{平稳行驶}} + \\underbrace{w_3 \\cdot J_{\\text{安全}}(\\boldsymbol{\\xi})}_{\\text{避免碰撞}}$$
|
||
|
||
$$\\text{约束条件：运动学约束、限速、车道边界}$$
|
||
|
||
- 进度项惩罚偏离期望路线。舒适项惩罚高横向加速度、加加速度（加速度的导数）和突然转向，因为乘客能感受到这些。安全项惩罚与其他交通参与者的接近程度，使用预测轨迹评估碰撞风险。
|
||
|
||
- 这是约束优化（第3章）：在不等式约束下最小化代价函数。权重$w_1, w_2, w_3$权衡竞争目标（激进驾驶更快但更不舒适且更不安全）。
|
||
|
||
- **基于学习的规划器**使用在人类驾驶数据上训练的神经网络生成轨迹。模型观察场景并直接输出规划的轨迹，从专家人类驾驶示例中隐式学习复杂的权衡。
|
||
|
||
- 优势在于人类驾驶行为被整体捕获，包括那些微妙且难以形式化的方面：何时激进地合流、何时在交叉口前微微前移、给骑行者留出多少空间。缺点是来自模仿学习（文件2）的相同分布偏移问题：模型在训练数据中未充分代表的情况下可能表现不可预测。
|
||
|
||
## 端到端驾驶
|
||
|
||
- **端到端驾驶**完全消除了模块边界。单个神经网络接收原始传感器输入（相机图像、LiDAR点云）并直接输出驾驶命令（转向、油门、刹车）或规划轨迹。没有独立的感知、预测或规划模块。
|
||
|
||
- 其吸引力在于整个系统针对最终任务（安全驾驶）进行联合优化，因此没有信息在模块边界丢失。感知模块学习精确提取规划器所需的特征，而不是通用的目标检测结果，后者可能不捕获任务相关的细节。
|
||
|
||
- **UniAD**（统一自动驾驶）是一个里程碑式的端到端架构。它通过BEV编码器处理多相机图像，然后应用一系列基于transformer的模块：跟踪、在线建图、运动预测、占据预测和规划。虽然它有内部模块，但它们都是可微的，并端到端联合训练，规划损失通过整个网络反向传播。
|
||
|
||
- UniAD中的规划模块通过关注预测的BEV特征、预测的参与者轨迹和预测的占据来生成未来主车路径点。这就是多元链式法则（第3章）的实际应用：梯度从规划损失一直流回图像编码器，告诉感知特征如何对规划更有用。
|
||
|
||
- 更近期端到端方法使用VLA风格的架构（本章文件3）。像**DriveVLM**这样的模型接收相机图像和导航指令（或路线），并使用VLM主干网络产生驾驶动作。这带来了大规模预训练（视觉理解、推理）的好处，直接融入驾驶堆栈。
|
||
|
||
- 端到端驾驶中的张力是**可解释性**。模块化系统可以报告"我检测到行人在(x,y)处，预测他们会横穿"——故障模式是可诊断的。端到端系统是一个产生转向角的黑盒。当它失败时，诊断原因很困难，这对安全认证是一个严重问题。
|
||
|
||
## 驾驶世界模型
|
||
|
||
- **世界模型**学习在给定当前状态和主车动作的情况下预测驾驶场景的未来状态：$p(s_{t+1} \\mid s_t, a_t)$（如第10章所述）。在驾驶中，这意味着生成逼真的未来帧或BEV布局："如果我加速并左转，3秒后的场景会是这样。"
|
||
|
||
- 世界模型为自动驾驶提供了两种强大能力：
|
||
|
||
- **基于想象的规划**：规划器不是先执行一个动作再看结果，而是可以通过世界模型"想象"多条候选轨迹，评估每条的安全性和舒适性，然后选择最佳的一条。这是基于模型的RL（本章文件2中介绍）应用于驾驶。
|
||
|
||
- **学习型仿真**：在真实驾驶数据上训练的世界模型实际上是一个数据驱动的仿真器。它生成逼真的场景（包括罕见的边缘情况），无需手工构建仿真器的工作。关键是，它捕获了真实驾驶的统计模式：其他驾驶员实际如何表现、光照如何变化、雨水如何影响可见度。
|
||
|
||
- **GAIA-1**（Wayve）是一个用于驾驶的生成式世界模型。给定过去相机帧和主车动作的序列，它自回归地生成未来视频帧。它使用以动作为条件的视频扩散架构。模型学习生成合理的未来：遵守交通规则的车辆、在人行道上行走的行人以及正确变化的交通信号灯——都从训练数据中涌现，而非编程规则。
|
||
|
||
- **DriveDreamer**和**GenAD**采取类似方法，但在BEV空间而非像素空间中操作。预测未来BEV布局比生成完整视频帧更紧凑（类似于机器人学中的DreamerV3在潜在空间而非像素空间中进行预测，如文件2所述）。BEV世界模型预测所有参与者的位置、道路结构的样子以及自由空间的位置，规划器直接使用这些信息。
|
||
|
||
- **神经闭环仿真**使用世界模型替代手工构建的仿真器进行测试。给定真实驾驶日志作为起点，世界模型生成如果主车采取了不同动作会发生什么。这使得反事实评估成为可能："如果我刹车晚了0.5秒会怎样？"而无需实际重现场景。
|
||
|
||
- 与**JEPA**框架（第10章）的联系在这里很自然。驾驶世界模型不需要预测像素级完美的未来（每个像素的精确RGB值）。它们需要预测对规划重要的方面：参与者在哪、移动速度多快、自由空间在哪。嵌入空间预测（JEPA风格）捕获这些语义上有意义的属性，而无需浪费容量在无关的视觉细节上，如确切的云纹理。
|
||
|
||
- 主要挑战是**长时程保真度**。世界模型随时间累积误差：第2帧的一个小错误会偏移所有后续帧。对于驾驶，3秒的预测时域对战术决策有用（我应该现在合流吗？），但30秒的时域（用于路线规划等战略决策所需）仍然不可靠。当前工作通过重新锚定（定期用真实观测重置模型）和不确定性估计（在预测变得不可靠时标记）来缓解这一问题。
|
||
|
||
## 仿真
|
||
|
||
- 通过在真实道路上驾驶来测试自动驾驶汽车是必要的，但还不够。危险场景（近碰撞、边缘情况）很少见，因此通过行驶里程来测试效率低下。一辆车需要行驶数亿英里才能以统计学方式证明安全性，这是不可行的。
|
||
|
||
- **仿真**提供了无限、可控且安全的测试。在现实世界中罕见的场景（一个孩子跑上马路、轮胎爆胎、突然的障碍物）可以在仿真中测试数百万次。
|
||
|
||
- **CARLA**是一个基于Unreal Engine构建的开源驾驶仿真器。它提供逼真的城市环境、动态天气、交通参与者以及传感器仿真（相机、LiDAR、雷达）。研究人员使用CARLA训练基于RL的驾驶智能体并评估感知算法。
|
||
|
||
- **nuPlan**（Motional）是一个闭环规划基准测试。与开环评估（重放记录数据，比较规划器的输出与人类驾驶员的实际轨迹）不同，闭环评估允许规划器的决策影响仿真：如果规划器决定变道，仿真会相应地演变。这测试了反应性行为，而不仅仅是轨迹相似性。
|
||
|
||
|
||
|
||
- **开环**和**闭环**评估之间的区别至关重要：
|
||
|
||
- 开环：重放记录的场景，计算模型输出与人类驾驶员动作的相似度。这容易设置但具有误导性：一个总是预测"直行"的模型在高速公路上误差可能很低，但在第一个转弯处就会撞车。
|
||
|
||
- 闭环：模型的动作改变仿真状态，仿真相应地演变。这测试了模型从自身错误中恢复和响应动态情况的能力。它昂贵得多，但更有意义。
|
||
|
||
- **场景生成**创建对系统进行压力测试的测试用例。对抗性场景（车辆突然刹车、行人隐藏在停放的汽车后面）通过优化使自动驾驶系统表现最差的情况来生成。这与ML中的对抗训练（第6章）有关：寻找最大化损失的输入。
|
||
|
||
## 安全性
|
||
|
||
- 自动驾驶中的安全性由工程标准而非仅ML指标来管理。
|
||
|
||
- **ISO 26262**（功能安全）是安全关键电子系统的汽车标准。它根据潜在危害的严重性、暴露度和可控性定义了**汽车安全完整性等级（ASIL）**，从A（最低）到D（最高）。自动驾驶系统的感知和规划组件通常为ASIL-D，即最高等级，需要广泛的验证、冗余和故障安全设计。
|
||
|
||
- **SOTIF**（预期功能安全，ISO 21448）处理另一类危害：不是硬件故障（ISO 26262覆盖的），而是系统按设计工作但仍产生不安全结果的情况。一个将白色卡车误分类为天空的感知模型（真实事件）是SOTIF问题：硬件工作正常，但算法的局限性导致了危害。
|
||
|
||
- **运行设计域（ODD）**定义了自动驾驶系统设计用于运行的条件：特定的地理区域、道路类型（仅高速、城市道路、两者兼有）、天气条件（无大雪）、速度范围和时间段。不允许在ODD之外运行：如果系统不能处理雪，就不能在雪中驾驶。
|
||
|
||
- **故障安全** vs **故障可操作**设计：
|
||
- 故障安全：检测到故障时，系统过渡到安全状态（例如，靠边停车）。这是最低要求。
|
||
- 故障可操作：系统在故障情况下仍能安全运行，使用冗余组件。具有冗余转向、制动和计算的自动驾驶汽车可以在单个组件故障后存活并仍然行驶到安全位置。
|
||
|
||
- **冗余**是基础。关键感知传感器被复制：多个相机覆盖重叠视场、LiDAR和雷达同时提供独立的深度测量、双计算平台运行相同的软件。如果任何单个组件发生故障，其他组件提供足够的信息来安全驾驶。
|
||
|
||
## 自动驾驶等级
|
||
|
||
|
||
|
||
- **SAE J3016**标准定义了六个驾驶自动化等级，从0（无自动化）到5（完全自动化）：
|
||
|
||
- **等级0（无自动化）**：人类做所有事情。系统可能提供警告（车道偏离警报）但不控制车辆。
|
||
|
||
- **等级1（驾驶辅助）**：系统控制转向或速度，但不能同时控制两者。自适应巡航控制（保持速度和跟车距离）或车道保持辅助（使车辆保持在车道中央）属于等级1。
|
||
|
||
- **等级2（部分自动化）**：系统同时控制转向和速度，但人类必须时刻监控并准备接管。特斯拉Autopilot、GM Super Cruise和大多数当前的"自动驾驶"功能属于等级2。人类仍然是负责的驾驶员。
|
||
|
||
- **等级3（条件自动化）**：系统驾驶并监控环境，但仅在特定条件下（ODD内）。人类可以脱离关注，但必须准备好在系统请求时接管（有时间缓冲，通常10秒以上）。Mercedes Drive Pilot（特定高速公路上，低于60公里/小时）是第一个经认证的等级3系统。
|
||
|
||
- **等级4（高度自动化）**：系统在ODD内驾驶并处理所有情况，无需人类干预。如果遇到ODD之外的情况，它可以安全地自己停车。Waymo的机器人出租车服务在特定地理区域内以等级4运行。
|
||
|
||
- **等级5（完全自动化）**：系统能在人类能去的一切地方、一切条件下驾驶。无需方向盘或踏板。这目前还不存在。
|
||
|
||
- 关键区别在于**谁对安全负责**。在等级0-2，人类负责。在等级3-5，系统负责（在其ODD内）。这具有深远的法律、保险和伦理影响。
|
||
|
||
- 当前行业状态是等级2（广泛部署）、等级3（开始部署）和等级4（有限地理部署）的混合。等级5仍然是一个长期研究目标。
|
||
|
||
## 编程任务（使用CoLab或notebook）
|
||
|
||
1. 实现一个简单的轨迹优化规划器。给定起始位置、目标和障碍物，使用梯度下降找到最平滑的无碰撞路径。
|
||
```python
|
||
import jax
|
||
import jax.numpy as jnp
|
||
import matplotlib.pyplot as plt
|
||
|
||
# 轨迹：N个路径点，每个(x, y)
|
||
N = 20
|
||
start = jnp.array([0.0, 0.0])
|
||
goal = jnp.array([10.0, 0.0])
|
||
obstacle = jnp.array([5.0, 0.0])
|
||
obs_radius = 1.5
|
||
|
||
# 初始化：从起点到终点的直线
|
||
waypoints_init = jnp.linspace(start, goal, N)
|
||
|
||
def cost(waypoints):
|
||
wp = jnp.concatenate([start[None], waypoints, goal[None]], axis=0)
|
||
|
||
# 平滑度：惩罚加速度（二阶差分）
|
||
accel = wp[2:] - 2 * wp[1:-1] + wp[:-2]
|
||
smooth_cost = jnp.sum(accel ** 2)
|
||
|
||
# 避障：惩罚接近度
|
||
dists = jnp.linalg.norm(wp - obstacle, axis=1)
|
||
collision_cost = jnp.sum(jnp.maximum(0, obs_radius + 0.5 - dists) ** 2)
|
||
|
||
return 10 * smooth_cost + 100 * collision_cost
|
||
|
||
grad_cost = jax.grad(cost)
|
||
|
||
# 优化内部路径点
|
||
waypoints = waypoints_init[1:-1]
|
||
lr = 0.01
|
||
for _ in range(500):
|
||
g = grad_cost(waypoints)
|
||
waypoints = waypoints - lr * g
|
||
|
||
# 绘图
|
||
full_path = jnp.concatenate([start[None], waypoints, goal[None]], axis=0)
|
||
theta = jnp.linspace(0, 2 * jnp.pi, 100)
|
||
|
||
plt.figure(figsize=(10, 4))
|
||
plt.plot(full_path[:, 0], full_path[:, 1], "b.-", label="优化后路径")
|
||
plt.plot(waypoints_init[:, 0], waypoints_init[:, 1], "r--", alpha=0.5, label="初始（直线）")
|
||
plt.fill(obstacle[0] + obs_radius * jnp.cos(theta),
|
||
obstacle[1] + obs_radius * jnp.sin(theta), alpha=0.3, color="red", label="障碍物")
|
||
plt.plot(*start, "go", markersize=10); plt.plot(*goal, "g*", markersize=15)
|
||
plt.legend(); plt.axis("equal"); plt.grid(True)
|
||
plt.title("轨迹优化：平滑无碰撞路径")
|
||
plt.show()
|
||
```
|
||
|
||
2. 模拟一个匀速运动预测模型，并与转弯车辆的真实值比较。
|
||
```python
|
||
import jax.numpy as jnp
|
||
import matplotlib.pyplot as plt
|
||
|
||
# 真实值：车辆右转
|
||
dt = 0.1
|
||
T = 40 # 4秒
|
||
v = 10.0 # 米/秒
|
||
omega = 0.3 # 弧度/秒（转弯速率）
|
||
|
||
# 真实轨迹（恒定转弯速率）
|
||
t = jnp.arange(T) * dt
|
||
theta = omega * t
|
||
gt_x = (v / omega) * jnp.sin(theta)
|
||
gt_y = (v / omega) * (1 - jnp.cos(theta))
|
||
|
||
# 从t=0开始的匀速预测
|
||
# 假设车辆沿当前航向继续直行
|
||
obs_steps = 10 # 观察前1秒
|
||
vx0 = v * jnp.cos(theta[obs_steps - 1])
|
||
vy0 = v * jnp.sin(theta[obs_steps - 1])
|
||
pred_t = jnp.arange(T - obs_steps) * dt
|
||
pred_x = gt_x[obs_steps - 1] + vx0 * pred_t
|
||
pred_y = gt_y[obs_steps - 1] + vy0 * pred_t
|
||
|
||
plt.figure(figsize=(8, 6))
|
||
plt.plot(gt_x[:obs_steps], gt_y[:obs_steps], "ko-", label="已观测")
|
||
plt.plot(gt_x[obs_steps:], gt_y[obs_steps:], "g-", linewidth=2, label="真实未来")
|
||
plt.plot(pred_x, pred_y, "r--", linewidth=2, label="匀速预测")
|
||
plt.legend(); plt.axis("equal"); plt.grid(True)
|
||
plt.xlabel("x (米)"); plt.ylabel("y (米)")
|
||
plt.title("匀速预测 vs 转弯车辆")
|
||
plt.show()
|
||
```
|
||
|
||
3. 实现一个简单的基于规则的规划器，根据检测到的障碍物决定保持车道还是停车。
|
||
```python
|
||
import jax.numpy as jnp
|
||
|
||
def rule_based_planner(ego_speed, obstacles, speed_limit=13.9):
|
||
"""
|
||
简单的基于规则的规划器。
|
||
ego_speed: 当前速度（米/秒）
|
||
obstacles: 前方车辆的（距离，速度）元组列表
|
||
speed_limit: 最高允许速度（米/秒），默认约50公里/小时
|
||
|
||
返回：(目标速度，动作标签)
|
||
"""
|
||
min_following_distance = 2.0 * ego_speed # 2秒规则
|
||
emergency_distance = 5.0 # 米
|
||
|
||
if not obstacles:
|
||
return speed_limit, "巡航"
|
||
|
||
# 找到最近的前方障碍物
|
||
closest_dist, closest_speed = min(obstacles, key=lambda o: o[0])
|
||
|
||
if closest_dist < emergency_distance:
|
||
return 0.0, "紧急停车"
|
||
elif closest_dist < min_following_distance:
|
||
# 匹配前车速度
|
||
target = min(closest_speed, speed_limit)
|
||
return target, "跟随"
|
||
else:
|
||
return speed_limit, "巡航"
|
||
|
||
# 测试场景
|
||
scenarios = [
|
||
(13.9, [], "空旷道路"),
|
||
(13.9, [(30.0, 10.0)], "前方有较慢车辆"),
|
||
(13.9, [(3.0, 0.0)], "前方有停靠车辆，距离极近"),
|
||
(13.9, [(50.0, 13.9)], "前方车辆同速行驶"),
|
||
]
|
||
|
||
for speed, obs, desc in scenarios:
|
||
target, action = rule_based_planner(speed, obs)
|
||
print(f"{desc:30s} → {action:15s} 目标速度={target:.1f} 米/秒 ({target*3.6:.0f} 公里/小时)")
|
||
```
|