跳转到内容
HITSZ-WTRobot-Packages

ChassisLocalization::EKF

cpkg add ChassisLocalization::EKF

文档

来源

LocEKF

LocEKF 是当前仓库中最完整的融合定位后端。它继承 IChassisLoc，对外继续提供统一的位姿、速度和坐标变换接口；但如果要正确接入它，必须理解这里的时间基准、观测回放和状态语义。

融合对象

当前实现融合三类输入：

  • Motion 层反馈出来的车体速度 forwardGetVelocity()
  • HWT101CT 提供的航向角 / 角速度
  • 外部定位观测 updateLidar(const Posture& pos, uint32_t ticks)，当前注释中典型来源是 10Hz 的雷达或 SLAM 位姿

这意味着 LocEKF 只负责“把这些输入融合成一个统一的平面位姿”，并不负责创建或驱动这些底层设备。

状态定义

内部 EKF 状态是四维：

  • x
  • y
  • yaw
  • yaw_offset

当前源码里的语义是：

  • x、y 单位为 m
  • yaw、yaw_offset 单位为 deg
  • 角速度 wz 单位为 deg/s

对外暴露的 postureInWorld().yaw 实际是 yaw + yaw_offset。这是内部滤波状态，不是额外对外暴露的新坐标系。

还需要特别理解 yaw 的“连续角”语义：

  • 当前 Loc 对外保存的 yaw 希望是连续的，工程上可理解为落在 -inf ~ +inf，而不是被强行截断到 [-180, 180]。
  • 这样做的主要原因不是“定位要知道到底转了几圈”，而是为了让控制层在跨过 +180 / -180 时不要看到角度突跳。
  • 但雷达一类外部观测返回的角度通常只有 [-180, 180] 内的等效角。
  • 因此 LocEKF::lidarUpdate() 里会先把雷达 yaw 残差连续化到最近等效角，再用于融合。
  • 这一步不保留整圈圈数，理论上存在丢圈风险；但当前定位并不关心“累计转了几圈”，只关心当前朝向等效角，所以这里没有实际影响。

构造参数

LocEKF<StateBufferCapacity, InputBufferCapacity>::Config 对应内部 PositionEKF::Config，包括：

  • x_init：初始状态
  • covP：初始协方差
  • noiseQ：过程噪声
  • noiseR.gyro：陀螺仪观测噪声
  • noiseR.lidar：外部位姿观测噪声

如果你只想把它当“更稳的定位后端”接入，可以先保持这几组参数和外部传感器实际精度同量级，再结合整机表现细调。

这里还要补一条真实接入里很常见的做法：

  • x_init 不一定在系统上电瞬间就已知。
  • 对很多 EKF 接法来说，初始观测往往由“雷达首次返回的世界系位姿 + 陀螺仪首次数据”共同定义。
  • 因此工程里常见顺序是：先让 Motion 开始工作，等待第一帧雷达位姿和第一帧陀螺仪数据到来，用它们准备 x_init 等初始条件，然后再构造 LocEKF，最后再构造依赖它的 Controller。

这也是主 README 里反复强调“Motion 往往要先于 Loc 构造”的一个典型案例。

时间基准与调用要求

这里最重要的约束不是公式，而是时间：

  • update() 每调用一次，就会采样一次 HAL_GetTick()、一次底盘速度、一次陀螺仪 yaw。
  • 构造函数里的 delta_ticks 表示滤波更新周期对应多少个 tick。
  • dt() 直接用 delta_ticks * 0.001f 计算秒数，默认假设 tick 基准是 1ms。
  • updateLidar(pos, ticks) 里的 ticks 必须来自同一套时基，否则延迟补偿和状态回放都会失真。

这里要特别强调边界：LocEKF 只消费“已经对齐好的时间戳”，不负责上下位机对时，也不负责把另一块板子、另一颗 MCU、另一条通信链路上的时间自动映射到本地时基。

也就是说，如果外部观测来自另一套时钟，调用方必须先在接入工程里完成时间换算，再把换算后的 ticks 传给 updateLidar()；不能把不同时间基准直接塞进来，也不要把“对时逻辑”理解成这个驱动库内部待补的功能。

周期更新模型

推荐的接入方式是：

  1. 若初始状态依赖首次外部观测，先只运行 Motion，等待第一帧雷达位姿和第一帧陀螺仪数据
  2. 用这两类首帧数据准备 x_init 等初始条件后构造 LocEKF
  3. 固定周期调用 update()
  4. 在后续外部定位观测到达时调用 updateLidar(pos, ticks)

update() 做了三件事：

  • 采样当前 Motion 速度和陀螺仪 yaw
  • 按固定 dt 推进 EKF 预测和 gyro 更新
  • 刷新对外可读的 posture_ 和 velocity_

updateLidar() 做了两类处理：

  • 如果观测时间不早于当前最新状态，就直接融合
  • 如果观测是一个“晚到的旧观测”，就回溯到历史状态、插入该观测、再重放后续状态

这里所谓“旧观测”或“晚到观测”的判断，前提仍然是 ticks 已经和 update() 内部使用的时间戳在同一基准上。若时间本身没先对齐，回放机制的结果没有意义。

这也是为什么本后端内部维护了状态历史缓冲区。

历史缓冲和晚到观测

当前实现里有两组缓冲：

  • state_buffer_：保存历史状态点，容量由模板参数 StateBufferCapacity 决定
  • input_buffer_：暂存还没推进进 EKF 的输入，容量由模板参数 InputBufferCapacity 决定，默认 4

当 updateLidar() 收到一个旧时间戳观测时：

  1. 查找对应历史状态
  2. 把 EKF 回退到该状态
  3. 融合这条观测
  4. 对回退点之后的历史输入重新执行一次 odom + gyro 更新

如果外部观测太早，早到超出 state_buffer_ 可回溯范围，当前实现会直接丢弃该观测。

如果不同工程的回放窗口需求和 RAM 预算不同，可以在接入侧改用不同容量实例，例如：

using MyLocEKF = chassis::loc::LocEKF<128>;

这会把历史状态缓冲缩小到 128 个点。当前项目里若继续保持现有行为，可以显式实例化 LocEKF<512>。

对外输出语义

虽然内部用了 EKF，外部接口仍然保持 IChassisLoc 统一语义：

  • velocityInBody()：当前实现用 Motion 的 vx、vy 和陀螺仪 wz
  • velocityInWorld()：把 body 速度按当前估计 yaw 旋转到 world
  • postureInWorld()：输出融合后的世界坐标系位姿

这意味着即便更换为 LocEKF，上层控制器也无需改接口调用方式。

使用边界

  • 当前实现是二维平面定位，不提供三维姿态。
  • 当前实现只显式使用单轴 yaw / wz，不处理完整 IMU 六轴或九轴状态。
  • 角度在当前实现中统一按 deg 处理，接入外部观测时不要混入 rad。
  • 外部定位观测默认应提供 Posture 语义，即 x/y 单位 m、yaw 单位 deg。

与主 README 的边界

主 README 负责说明 Loc 层统一给上层什么能力。本文件负责说明 LocEKF 的专项要求：同一时间基准、固定周期推进、晚到观测回放，以及当前只支持平面位姿融合。