机器人 / 系统课时

运动模型如何预测下一时刻位姿

运动模型如何预测下一时刻位姿

给定上一时刻的二维位姿和底盘速度,可以先预测机器人下一步的位置与朝向,再估计这次运动让定位误差扩大了多少。下面以差速机器人为例,使用中点运动模型、协方差传播和一段可运行的 Python 程序,得到能够逐项核对的预测结果。

学习目标

  • 将左右轮速度转换为平面运动控制量,并计算下一时刻位姿。
  • 理解运动噪声如何经雅可比矩阵传播为位姿协方差。
  • 验证预测数值,判断时间步长、打滑与速度来源造成的误差。

从轮速得到运动输入

二维位姿记为 sk=[xk,yk,θk]T\mathbf{s}_k=[x_k,y_k,\theta_k]^\mathsf{T},其中 x,yx,y 是固定参考坐标系(例如 odom)中的位置,θ\theta 是逆时针为正的朝向。控制输入 uk=[vk,ωk]T\mathbf{u}_k=[v_k,\omega_k]^\mathsf{T} 分别表示机器人自身前进方向的线速度和绕竖直轴的角速度。

对理想差速底盘,轮半径为 rr、左右轮中心距为 bb,轮子的角速度为 ωR,ωL\omega_R,\omega_L,有 v=r(ωR+ωL)/2v=r(\omega_R+\omega_L)/2 和 ω=r(ωR−ωL)/b\omega=r(\omega_R-\omega_L)/b。例如 r=0.05 mr=0.05\,\text{m}、b=0.30 mb=0.30\,\text{m}、右轮 20.6 rad/s20.6\,\text{rad/s}、左轮 19.4 rad/s19.4\,\text{rad/s},算得 v=1.0 m/sv=1.0\,\text{m/s}、ω=0.2 rad/s\omega=0.2\,\text{rad/s}。

实际建图优先使用带时间戳的轮编码器或融合里程计速度;/cmd_vel 只是期望控制指令,不能假定机器人实际走出了完全相同的轨迹。

根据上一位姿预测下一位姿

在采样间隔 Δt\Delta t 内假设速度近似恒定,采用中点积分。先计算中间航向 θm=θk+ωkΔt/2\theta_m=\theta_k+\omega_k\Delta t/2,再更新三个状态量:

  • xk+1−=xk+vkΔtcos⁡θmx_{k+1}^-=x_k+v_k\Delta t\cos\theta_m
  • yk+1−=yk+vkΔtsin⁡θmy_{k+1}^-=y_k+v_k\Delta t\sin\theta_m
  • θk+1−=wrap⁡(θk+ωkΔt)\theta_{k+1}^-=\operatorname{wrap}(\theta_k+\omega_k\Delta t)

上标 −- 表示尚未融合激光或其他观测的预测值。wrap 将角度归一化到 [−π,π][-\pi,\pi]。起点为 (0,0,0)(0,0,0)、Δt=1 s\Delta t=1\,\text{s}、v=1 m/sv=1\,\text{m/s}、ω=0.2 rad/s\omega=0.2\,\text{rad/s} 时,预测位置约为 (0.995,0.100) m(0.995,0.100)\,\text{m},航向为 0.200 rad0.200\,\text{rad}。横向位移来自转弯,不能简单将 vΔtv\Delta t 全加到世界坐标的 xx 轴。

运动预测轨迹和位姿不确定性示意

把速度误差传播为协方差

预测的不确定性用 3×33\times3 协方差矩阵 PkP_k 表示,包含 x,y,θx,y,\theta 各自的方差和它们之间的相关性。若速度与角速度测量噪声相互独立、零均值,输入噪声协方差为 Q=diag⁡(σv2,σω2)Q=\operatorname{diag}(\sigma_v^2,\sigma_\omega^2),扩展卡尔曼滤波的预测近似为:

Pk+1−=FkPkFkT+GkQkGkTP_{k+1}^-=F_kP_kF_k^\mathsf{T}+G_kQ_kG_k^\mathsf{T}

FkF_k 是位姿更新函数对旧位姿的雅可比矩阵(3×33\times3),GkG_k 是对控制输入的雅可比矩阵(3×23\times2)。例如 F1,3=−vΔtsin⁡θmF_{1,3}=-v\Delta t\sin\theta_m,G2,2=vΔt2cos⁡θm/2G_{2,2}=v\Delta t^2\cos\theta_m/2。第一项传播上一时刻已有的定位误差;第二项增加本次轮速不确定性。这里只假设旧位姿误差与当前控制噪声不相关。

取初始三个状态的标准差均为 0.050.05(位置单位为米、角度单位为弧度),并设 σv=0.10 m/s\sigma_v=0.10\,\text{m/s}、σω=0.05 rad/s\sigma_\omega=0.05\,\text{rad/s}。注意 QQ 放的是方差而非标准差:对角线应为 0.010.01 与 0.00250.0025。不能只更新 PP 的对角线,否则会丢掉位置与航向的相关性。

用 Python 计算并核对结果

以下只依赖 Python 3 标准库。将代码保存为 motion_predict.py 并执行 python motion_predict.py;矩阵乘法函数只负责实现上面的协方差公式,不依赖 ROS 2 运行时。

python
from math import atan2, cos, sin, sqrt

def matmul(a, b):
    return [[sum(a[i][k] * b[k][j] for k in range(len(b)))
             for j in range(len(b[0]))] for i in range(len(a))]

def transpose(a):
    return [list(column) for column in zip(*a)]

def add(a, b):
    return [[u + v for u, v in zip(ra, rb)] for ra, rb in zip(a, b)]

def main():
    x, y, theta = 0.0, 0.0, 0.0
    v, omega, dt = 1.0, 0.2, 1.0
    P = [[0.0025, 0, 0], [0, 0.0025, 0], [0, 0, 0.0025]]
    Q = [[0.01, 0], [0, 0.0025]]

    # 中点法更新均值,并把航向限制在 [-pi, pi]。
    mid = theta + omega * dt / 2
    nx = x + v * dt * cos(mid)
    ny = y + v * dt * sin(mid)
    nt = atan2(sin(theta + omega * dt), cos(theta + omega * dt))

    # 分别对旧位姿和控制量求偏导,用于传播全部协方差。
    F = [[1, 0, -v * dt * sin(mid)],
         [0, 1, v * dt * cos(mid)],
         [0, 0, 1]]
    G = [[dt * cos(mid), -v * dt**2 * sin(mid) / 2],
         [dt * sin(mid), v * dt**2 * cos(mid) / 2],
         [0, dt]]
    next_P = add(matmul(matmul(F, P), transpose(F)),
                 matmul(matmul(G, Q), transpose(G)))
    std = [sqrt(next_P[i][i]) for i in range(3)]

    print(f"预测位姿: x={nx:.3f} m, y={ny:.3f} m, theta={nt:.3f} rad")
    print(f"标准差: sx={std[0]:.3f} m, sy={std[1]:.3f} m, stheta={std[2]:.3f} rad")

if __name__ == "__main__":
    main()

预期输出:

text
预测位姿: x=0.995 m, y=0.100 m, theta=0.200 rad
标准差: sx=0.111 m, sy=0.075 m, stheta=0.071 rad

初始位置标准差为 0.05 m0.05\,\text{m},运动后两个方向的标准差变得不同,因为转弯把朝向误差映射到了位置上。输出的三个标准差只描述各坐标轴的边缘不确定性;若需绘制位置误差椭圆,还须使用 PP 左上角的完整 2×22\times2 子矩阵。

什么时候需要修正运动模型

  • 时间间隔不准确:真实机器人按相邻消息时间戳计算 Δt\Delta t。例如 50 Hz 的正常采样间隔约为 0.02 s0.02\,\text{s},不可一直沿用算例里的 1 秒。
  • 转弯过快或间隔过长:中点法是近似积分。若单步航向变化很大,应缩短积分步长,或改用恒定速度圆弧的精确积分公式。
  • 轮子打滑、碰撞或地面不平:这些情况会引入有偏甚至非高斯误差,单纯增大 QQ 无法消除系统性偏差,需要 IMU、激光扫描匹配等观测进行校正。

可以做两个快速自检:设 ω=0\omega=0,起始航向为零时应沿 +x+x 直行;设 v=0v=0,应只改变朝向而不改变位置。运动模型只承担观测到来之前的预测,不能把不断积分得到的 odom 位姿直接当作已被 SLAM 全局校正的 map 位姿。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

运动模型如何预测下一时刻位姿

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通