3. 机器人系统建模

在机器人状态估计中，建模是第一步，也是决定后续滤波效果上限的关键。EKF 并不是“凭空估计”机器人位姿，它依赖两个基本模型：描述机器人如何随时间演化的运动模型，以及描述传感器如何观测状态的观测模型。本节围绕状态空间表示，把连续时间系统与离散时间系统串联起来，为后续 EKF 的预测与更新建立统一框架。

一、状态空间表示：把机器人“压缩”成状态向量

状态空间表示的核心思想是：用一组有限维变量描述系统当前的全部内部信息，使未来演化只依赖当前状态和输入，而不需要更早的历史。对移动机器人而言，最常见的状态向量包括二维平面位姿与速度：

x = [px, py, theta, v, omega]^T

其中 px、py 为位置，theta 为航向角，v 为线速度，omega 为角速度。若使用 IMU 估计姿态，还可加入陀螺仪零偏；若使用视觉惯性系统，状态维度会进一步扩展。状态空间表示的价值在于：它把“机器人现在处于什么情况”变成一个向量，把“机器人如何变化”变成函数，把“传感器看到什么”变成另一个函数。

状态估计问题可统一写成：x_k = f(x_{k-1}, u_{k-1}) + w_{k-1}，z_k = h(x_k) + v_k。前者是运动模型，后者是观测模型，w 与 v 分别表示过程噪声和观测噪声。

二、运动模型：从连续微分方程到离散递推

运动模型描述状态随时间的演化。以差速机器人为例，连续时间运动学可写为：

px_dot = v * cos(theta)
py_dot = v * sin(theta)
theta_dot = omega

这是典型的非线性连续时间系统。EKF 在计算机中运行，必须把它离散化。若控制周期为 dt，并假设 v、omega 在周期内近似恒定，则离散运动模型为：

px_k   = px_{k-1} + v * dt * cos(theta_{k-1})
py_k   = py_{k-1} + v * dt * sin(theta_{k-1})
theta_k = theta_{k-1} + omega * dt

该模型直接用于 EKF 预测步。需要注意的是，真实机器人还存在轮子打滑、地面不平、标定误差等，因此过程噪声 Q 必须反映这些不确定性。若把 Q 设得过小，滤波器会过度自信，导致观测被忽略；设得过大，则估计会抖动。

三、观测模型：把状态映射到传感器读数

观测模型 h(x) 描述“如果机器人处于状态 x，传感器应该看到什么”。不同传感器对应不同观测模型。例如，激光雷达观测到某个路标距离与方位角时，观测方程可写为：

r = sqrt((mx - px)^2 + (my - py)^2)
phi = atan2(my - py, mx - px) - theta

其中 (mx, my) 是已知路标坐标。若使用 GPS，则观测模型可能直接观测 px、py；若使用轮式里程计，则观测模型可能观测 v、omega。观测模型越接近真实物理关系，EKF 的更新步就越可靠。

机器人状态空间建模与 EKF 预测-更新链路 状态向量 x [px, py, θ, v, ω]ᵀ 描述机器人当前全部信息 控制输入 u v_cmd, ω_cmd 来自规划器或遥控 运动模型 f(x,u) 连续：ẋ = f(x,u) 离散：x_k = f(x_{k-1},u) + 过程噪声 w ~ N(0,Q) 观测模型 h(x) z = h(x) + v 激光：r, φ + 观测噪声 v ~ N(0,R) EKF 预测 x̂_k⁻ = f(x̂_{k-1},u) P_k⁻ = F P_{k-1} Fᵀ + Q F = ∂f/∂x 雅可比 EKF 更新 K = P⁻Hᵀ(HP⁻Hᵀ+R)⁻¹ x̂_k = x̂_k⁻ + K(z - h(x̂_k⁻)) H = ∂h/∂x 雅可比 连续时间系统 ẋ = f(x,u) + w，z = h(x) + v，适合物理建模与理论推导 离散时间系统 x_k = f(x_{k-1},u) + w，z_k = h(x_k) + v，适合数字计算机实现

四、连续与离散时间系统：为什么必须离散化

连续时间模型描述的是物理世界的真实演化，但机器人控制器和 EKF 都在数字计算机上运行，只能处理离散时刻的数据。因此，连续模型必须通过积分或近似离散化。常见方法包括欧拉法、中点法、龙格-库塔法。对于小周期、低速机器人，欧拉法通常足够；对于高速或高动态场景，应使用更高阶方法或直接采用离散运动学模型。

模型类型 数学形式 典型用途 注意事项
连续运动模型 ẋ = f(x,u) + w 物理推导、仿真建模 需离散化后才能用于 EKF
离散运动模型 x_k = f(x_{k-1},u) + w EKF 预测步 dt 必须与传感器周期匹配
连续观测模型 z = h(x) + v 传感器物理关系描述 注意观测维度与状态维度关系
离散观测模型 z_k = h(x_k) + v EKF 更新步 需计算雅可比 H

五、案例：差速机器人 EKF 建模流程

假设机器人以 20 Hz 运行 EKF，激光雷达以 10 Hz 提供路标观测。建模流程如下：

  1. 定义状态向量 x = [px, py, theta, v, omega]^T。
  2. 根据差速运动学写出连续运动模型，并用 dt = 0.05 s 离散化。
  3. 根据激光路标观测写出观测模型 h(x)，并计算雅可比 H。
  4. 设置过程噪声 Q 与观测噪声 R，Q 反映打滑与模型误差，R 反映测距与测角误差。
  5. 在每个 EKF 周期执行预测；当激光数据到达时执行更新。
dt = 0.05
x_pred = [px + v*dt*cos(theta),
          py + v*dt*sin(theta),
          theta + omega*dt,
          v,
          omega]
F = [[1, 0, -v*dt*sin(theta), dt*cos(theta), 0],
     [0, 1,  v*dt*cos(theta), dt*sin(theta), 0],
     [0, 0, 1, 0, dt],
     [0, 0, 0, 1, 0],
     [0, 0, 0, 0, 1]]
P_pred = F @ P @ F.T + Q
工程建议：先保证运动模型与观测模型的物理意义正确，再调 Q 与 R。很多“EKF 不收敛”问题，根源不是滤波器参数，而是模型符号、坐标系或角度归一化写错。

六、常见误区与复盘

  • 误区一：把连续模型直接塞进离散 EKF。 必须明确 dt 和离散化方法，否则预测步会系统性偏移。
  • 误区二：忽略角度周期性。 theta 的差值应归一化到 [-π, π]，否则航向更新会出现跳变。
  • 误区三：Q 和 R 随意设置。 Q 过小导致滤波器“自信过头”，R 过小导致观测噪声被放大。
  • 误区四：观测模型坐标系不一致。 激光观测通常位于机器人坐标系，而状态位于世界坐标系，必须统一。

复盘时可以用一句话检查建模是否完整：状态向量是否包含所有影响观测的量？运动模型是否描述了状态如何随控制输入变化？观测模型是否把状态正确映射到传感器读数？连续模型是否已正确离散化？只要这四个问题都能回答清楚，EKF 的预测与更新就有了可靠基础。

← 上一章 📖 返回目录 下一章 →