Skip to content

位姿递推方法#

在时间步长 \(dt\) 内，假设 \(v, \omega\) 恒定，将位姿从 \((x, y, \theta)\) 更新到 \((x', y', \theta')\)。

方法一：精确圆弧模型（推荐，适合任意 \(\omega\)）#

运动学方程#

dt_theta = omega * dt
if abs(omega) > 1e-6:  # 小角度阈值
    R = v / omega  # 转弯半径
    dx = R * (sin(theta + dt_theta) - sin(theta))
    dy = R * (cos(theta) - cos(theta + dt_theta))  # Y 轴正向
else:
    # 直线运动近似（避免除零）
    dx = v * dt * cos(theta)
    dy = v * dt * sin(theta)

小角度处理#

当 \(|\omega|\) 极小时，圆弧半径趋于无穷大，必须切换到直线近似。一般阈值设为 \(10^{-6}\) 或更小，确保在纯直线运动时数值稳定。

矩阵形式#

\[\begin{bmatrix} \Delta x \\ \Delta y \end{bmatrix} = \begin{cases} \frac{v}{\omega} \begin{bmatrix} \sin(\theta + \omega\Delta t) - \sin\theta \\ \cos\theta - \cos(\theta + \omega\Delta t) \end{bmatrix} & , |\omega| > \varepsilon \\ v\Delta t \begin{bmatrix} \cos\theta \\ \sin\theta \end{bmatrix} & , |\omega| \leq \varepsilon \end{cases}\]

方法二：中点欧拉（比简单欧拉精度高，且自动避免除零）#

如果我们不计算 \(R\)，直接用速度和角度增量递推，也可以使用二阶中点法：

dt_theta = omega * dt
dx = v * dt * cos(theta + dt_theta / 2)
dy = v * dt * sin(theta + dt_theta / 2)

原理#

"机器人以当前速度直线走过半步长的距离，方向是初始角与终点角的中间值"

  • 对大多数情况精度足够
  • 自然避开 \(\omega \to 0\) 时的奇异性
  • 在实际嵌入式里程计中常被采用

矩阵形式#

\[\begin{bmatrix} \Delta x \\ \Delta y \end{bmatrix} = v\Delta t \begin{bmatrix} \cos\left(\theta + \frac{\omega\Delta t}{2}\right) \\ \sin\left(\theta + \frac{\omega\Delta t}{2}\right) \end{bmatrix}\]

两种方法对比#

特性 精确圆弧模型 中点欧拉
精度 高（适合任意 \(\omega\)） 中等
计算量 需计算 \(R\) 和三角函数 只需三角函数
奇异性处理 需手动切换直线近似 自动避免
适用场景 精密仿真 嵌入式里程计

推荐#

  • 精密仿真：精确圆弧模型
  • 实时里程计：中点欧拉（计算简单，精度足够）