机器人 / 系统课时
运动模型如何预测下一时刻位姿
运动模型如何预测下一时刻位姿
给定上一时刻的二维位姿和底盘速度,可以先预测机器人下一步的位置与朝向,再估计这次运动让定位误差扩大了多少。下面以差速机器人为例,使用中点运动模型、协方差传播和一段可运行的 Python 程序,得到能够逐项核对的预测结果。
学习目标
- 将左右轮速度转换为平面运动控制量,并计算下一时刻位姿。
- 理解运动噪声如何经雅可比矩阵传播为位姿协方差。
- 验证预测数值,判断时间步长、打滑与速度来源造成的误差。
从轮速得到运动输入
二维位姿记为 ,其中 是固定参考坐标系(例如 odom)中的位置, 是逆时针为正的朝向。控制输入 分别表示机器人自身前进方向的线速度和绕竖直轴的角速度。
对理想差速底盘,轮半径为 、左右轮中心距为 ,轮子的角速度为 ,有 和 。例如 、、右轮 、左轮 ,算得 、。
实际建图优先使用带时间戳的轮编码器或融合里程计速度;/cmd_vel 只是期望控制指令,不能假定机器人实际走出了完全相同的轨迹。
根据上一位姿预测下一位姿
在采样间隔 内假设速度近似恒定,采用中点积分。先计算中间航向 ,再更新三个状态量:
上标 表示尚未融合激光或其他观测的预测值。wrap 将角度归一化到 。起点为 、、、 时,预测位置约为 ,航向为 。横向位移来自转弯,不能简单将 全加到世界坐标的 轴。

把速度误差传播为协方差
预测的不确定性用 协方差矩阵 表示,包含 各自的方差和它们之间的相关性。若速度与角速度测量噪声相互独立、零均值,输入噪声协方差为 ,扩展卡尔曼滤波的预测近似为:
是位姿更新函数对旧位姿的雅可比矩阵(), 是对控制输入的雅可比矩阵()。例如 ,。第一项传播上一时刻已有的定位误差;第二项增加本次轮速不确定性。这里只假设旧位姿误差与当前控制噪声不相关。
取初始三个状态的标准差均为 (位置单位为米、角度单位为弧度),并设 、。注意 放的是方差而非标准差:对角线应为 与 。不能只更新 的对角线,否则会丢掉位置与航向的相关性。
用 Python 计算并核对结果
以下只依赖 Python 3 标准库。将代码保存为 motion_predict.py 并执行 python motion_predict.py;矩阵乘法函数只负责实现上面的协方差公式,不依赖 ROS 2 运行时。
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()预期输出:
预测位姿: x=0.995 m, y=0.100 m, theta=0.200 rad
标准差: sx=0.111 m, sy=0.075 m, stheta=0.071 rad初始位置标准差为 ,运动后两个方向的标准差变得不同,因为转弯把朝向误差映射到了位置上。输出的三个标准差只描述各坐标轴的边缘不确定性;若需绘制位置误差椭圆,还须使用 左上角的完整 子矩阵。
什么时候需要修正运动模型
- 时间间隔不准确:真实机器人按相邻消息时间戳计算 。例如 50 Hz 的正常采样间隔约为 ,不可一直沿用算例里的 1 秒。
- 转弯过快或间隔过长:中点法是近似积分。若单步航向变化很大,应缩短积分步长,或改用恒定速度圆弧的精确积分公式。
- 轮子打滑、碰撞或地面不平:这些情况会引入有偏甚至非高斯误差,单纯增大 无法消除系统性偏差,需要 IMU、激光扫描匹配等观测进行校正。
可以做两个快速自检:设 ,起始航向为零时应沿 直行;设 ,应只改变朝向而不改变位置。运动模型只承担观测到来之前的预测,不能把不断积分得到的 odom 位姿直接当作已被 SLAM 全局校正的 map 位姿。