在 Devin 中试用 DeepWiki 处理私有仓库
DeepWiki

commaai/openpilot

菜单

本地化系统

相关源文件

openpilot 定位系统实时确定车辆的位置、姿态和运动参数。它融合来自多个传感器的数据，以维持对车辆状态的准确估计。此信息对于规划和控制功能至关重要，为安全自动驾驶奠定了基础。

本页面重点介绍定位系统的核心组件和操作。有关使用此数据的规划和控制系统的信息，请参阅 规划与控制。

系统概览

定位系统采用基于卡尔曼滤波的方法来融合来自各种传感器的数据，估计车辆状态，并将该信息提供给系统的其他组件。核心组件是 locationd 守护进程，它处理传感器输入并生成连续更新的状态估计。

来源: selfdrive/locationd/locationd.py1-202

关键组件

LocationEstimator 类

LocationEstimator 类是定位系统的核心。它管理卡尔曼滤波器并处理传入的传感器数据，以维护最新的车辆状态估计。

主要职责

  • 初始化和维护卡尔曼滤波器
  • 验证传感器输入
  • 处理来自各种传感器的观测值
  • 检测和处理错误或无效输入
  • 为其他系统生成状态估计

来源: selfdrive/locationd/locationd.py51-202

PoseKalman 滤波器

PoseKalman 类实现了用于状态估计的扩展卡尔曼滤波器。它维护以下状态变量：

状态变量描述Slice
NED_ORIENTATION航向角、俯仰角、滚转角（弧度）0:3
DEVICE_VELOCITYNED 坐标系下的速度（米/秒）3:6
ANGULAR_VELOCITY角速度（航向角、俯仰角、滚转角速率）（弧度/秒）6:9
GYRO_BIAS陀螺仪偏差（弧度/秒）9:12
ACCELERATION设备坐标系下的加速度（米/秒²）12:15
ACCEL_BIAS加速度计偏差（米/秒²）15:18

滤波器使用反映初始状态不确定性的默认值和协方差进行初始化。

来源: selfdrive/locationd/models/pose_kf.py20-28 selfdrive/locationd/models/pose_kf.py33-45

数据流和处理

来源: selfdrive/locationd/locationd.py97-202 selfdrive/locationd/locationd.py240-253

传感器输入处理

定位系统处理来自多个传感器的数据

加速度计

  • 测量设备坐标系下的线性加速度
  • 用于估计位置变化和重力方向
  • 输入经过时间、来源和幅值验证
  • 如果幅值超过 ACCEL_SANITY_CHECK (100 m/s²) 则拒绝输入

来源: selfdrive/locationd/locationd.py98-117

陀螺仪

  • 测量设备坐标系下的角速度
  • 用于估计姿态变化
  • 输入经过时间、来源和幅值验证
  • 如果幅值超过 ROTATION_SANITY_CHECK (10 rad/s) 则拒绝输入
  • 与相机里程计的航向角速率进行交叉验证以确保一致性

来源: selfdrive/locationd/locationd.py119-143

相机里程计

  • 提供来自视觉处理的平移和旋转估计
  • 使用校准矩阵转换为设备坐标系
  • 输入经过幅值和不确定性验证
  • 如果旋转幅值超过 ROTATION_SANITY_CHECK 则拒绝旋转输入
  • 如果位移幅值超过 TRANS_SANITY_CHECK (200 m/s) 则拒绝位移输入

来源: selfdrive/locationd/locationd.py157-198

车辆数据

  • 来自 carState 的车速用于加权 posenet 估计
  • 快速的车辆移动会降低嘈杂的视觉估计的影响

来源: selfdrive/locationd/locationd.py145-146

校准数据

  • 来自 liveCalibration 的相机到设备旋转矩阵
  • 用于将相机里程计数据转换到设备坐标系

来源: selfdrive/locationd/locationd.py148-156

输入验证和错误处理

定位系统实现了鲁棒的验证以维持滤波器稳定性

系统实现了多种验证机制

  1. 时间验证:

    • 确保传感器时间戳接近日志时间
    • 拒绝时间戳差异大于 MAX_SENSOR_TIME_DIFF (0.1s) 的输入
    • 防止处理远过去的观测值

  2. 合理性检查:

    • 加速度计数据：幅值 < ACCEL_SANITY_CHECK (100 m/s²)
    • 陀螺仪数据：幅值 < ROTATION_SANITY_CHECK (10 rad/s)
    • 相机里程计：旋转 < ROTATION_SANITY_CHECK，位移 < TRANS_SANITY_CHECK

  3. 交叉验证:

    • 比较陀螺仪航向角速率与相机里程计航向角速率
    • 如果差异超过阈值，则拒绝陀螺仪数据

  4. 输入无效计数器:

    • 跟踪每个传感器的连续无效输入
    • 在有效输入后逐渐恢复对传感器的信任
    • 当无效输入过多时，将 inputsOK 标志设置为 false

  5. 滤波器稳定性:

    • 检查状态或协方差中的非有限值
    • 如果检测到不稳定的值，则重置滤波器

来源: selfdrive/locationd/locationd.py69-96 selfdrive/locationd/locationd.py241-253 selfdrive/locationd/locationd.py267-319

输出与集成

定位系统生成一个包含以下内容的 livePose 消息

  1. 姿态：航向角、俯仰角、滚转角（NED 坐标系 - 北-东-下）
  2. 速度：车辆在设备坐标系下的速度
  3. 角速度：设备坐标系下的姿态变化速率
  4. 加速度：设备坐标系下的线性加速度

每个组件包括

  • 值 (x, y, z)
  • 标准差（不确定性）
  • 有效性标志

此外，该消息还包含状态标志

  • inputsOK：传感器输入是否有效和可靠
  • posenetOK：基于视觉的里程计是否可靠
  • sensorsOK：所有必需的传感器是否正常运行

此输出由规划和控制系统用于做出驾驶决策。

来源: selfdrive/locationd/locationd.py204-238

测试与验证

定位系统包含全面的测试场景以确保鲁棒性

场景描述预期结果
基础正常运行准确的状态估计
陀螺仪关闭无陀螺仪数据零角速度估计，sensorsOK=False
陀螺仪尖峰单次无效陀螺仪读数忽略读数，状态不受影响
持续的陀螺仪尖峰多次无效陀螺仪读数inputsOK=False 直到恢复
加速度计关闭无加速度计数据零估计，sensorsOK=False
加速度计尖峰单次无效加速度计读数忽略读数，状态不受影响
时间戳尖峰带有无效时间戳的传感器忽略读数，状态不受影响

这些测试验证了系统可以在不影响状态估计的情况下处理传感器故障和无效数据。

来源: selfdrive/locationd/test/test_locationd_scenarios.py9-190

系统集成

定位系统与其他 openpilot 组件集成

该系统以高频率运行以确保及时更新，并使用消息传递基础设施与其他组件通信。

来源: selfdrive/locationd/locationd.py256-332

性能考量

定位系统设计得既鲁棒又高效

  1. 实时进程配置:

    • 使用高优先级（5）的特定 CPU 核心（0-3）
    • 确保传感器数据的及时处理

  2. 高效的传感器套接字管理:

    • 加速度计和陀螺仪的独立套接字
    • 与传感器数据源优化通信

  3. 轻量级处理:

    • 快速的输入验证
    • 高效的卡尔曼滤波器实现
    • 最小内存使用

这些优化确保定位系统能够提供准确、及时的状态估计，以实现安全的自动驾驶。

来源: selfdrive/locationd/locationd.py256-268