sensor odometry 上

---

1 概述

  里程计(Odometry)是机器人定位与导航中的基础模块，用于通过传感器数据估计机器人相对于初始位置的相对运动。
  里程计分为轮式里程计(Wheel Odometry)、视觉里程计(Visual Odometry，VO)、惯性里程计(IMU Odometry)、雷达里程计(Laser Odometry)等。本文所说的odometry是指轮式里程计。

2 基本原理

  轮式里程计是一种基于车轮转动信息来估算移动机器人或车辆位置与姿态变化的技术。它通过处理车轮旋转产生的数据，推算出车辆或机器人在平面上的运动轨迹。
  其核心思想是：通过编码器或其他传感器测量车轮的旋转量，然后根据车轮的几何关系，将旋转量转换为线性位移和角度变化，最后通过积分计算，估计机器人的位置与姿态。

3 运动学模型

  轮式里程计的运动学模型分为单轮模型、差分运动模型(两轮)、全向轮模型(三轮或四轮全向轮)，本文主要描述差分运动模型。

3.1 差分运动模型 概念

  差分运动模型(Differential Drive Model)是描述两轮差分驱动机器人运动的一种数学模型。这种模型广泛应用于移动机器人、AGV和轮式机器人中。它通常由两个驱动轮(左右各一个)和一个或多个支撑轮(如万向轮)组成。
  差分运动模型的核心思想是控制两个驱动轮的速度差来实现机器人的运动。

3.2 差分运动模型 运动求解

  差分运动模型的机器人始终做的是以$r$为半径的圆弧运动。

已知：两轮旋转的角速度$w_R,w_L$、两轮的线速度$v_R,v_L$、两轮的半径$r_R,r_L$、轮子离底盘中心的距离$d$、两轮之间的距离$D=2d$
求解：底盘中心的线速度$v$、底盘中心的角速度$w$、两轮相对于车体旋转中心$O$转动的角速度$w_r,w_l$、底盘中心圆弧运动的半径$r$
注意：直线运动可看作圆周运动的特殊情况，即$r\to +\propto$，直线运动时，$v_R=v_L$

求解运动半径$r$:
底盘中心相对于车体旋转中心$O$转动的角速度与两轮相对于车体旋转中心$O$转动的角速度是相等的。
$$\begin{array}{l}w=w_l=w_r\\ v=wr\end{array}\}⟹\frac{v_L}{r-d}=\frac{v_R}{r+d}⟹v_L(r+d)=v_R(r-d)⟹(v_R-v_L)r=(v_R+v_L)d⟹r=\frac{(v_R+v_L)d}{(v_R-v_L)}$$
求解角速度$w$:
$$w=\frac{v_R}{r+d}⟹\frac{v_R}{\frac{(v_R+v_L)d}{(v_R-v_L)}+d}⟹w=\frac{v_R-v_L}{2d}$$
求解线速度$v$:
$$v=wr⟹v=\frac{v_R-v_L}{2d}\frac{(v_R+v_L)d}{(v_R-v_L)}⟹v=\frac{v_R+v_L}{2}$$

3.3 差分运动模型 航迹推演

  航迹推演的核心思想是基于物体的运动状态进行位置的递推计算。
  它假定物体在运动过程中，其速度和方向是可以被测量或估计的。从一个已知的起始位置开始，根据物体在每个时间段内的运动速度和方向，逐步推算出后续时刻的位置。
  由于机器人在相邻时刻$\Delta t$内，运动距离非常短，因此可认为机器人是匀速运动，两相邻点之间的运动轨迹可看成直线。
  已知$t-1$时刻机器人位姿$(x,y,\theta )$、线速度$v_x$、角速度$w$、与$t$时刻的间隔时间$\Delta t$，求解$t$时刻机器人位姿$(x^{\prime },y^{\prime },{\theta }^{\prime })$
  $t$时刻沿机器人坐标系$X_R$轴移动了$v_x\ast \Delta t$，只需要将该段距离分别投影在世界坐标系$X_w,Y_w$轴上，就可以得到$t$时刻相对于$t-1$时刻机器人在世界坐标系中坐标移动的位移$\Delta x和\Delta y$。
$$\begin{gathered} \Delta x=v_x\ast \Delta t\ast cos({\theta }_{t-1}) \\ \Delta y=v_x\ast \Delta t\ast sin({\theta }_{t-1}) \end{gathered}$$
  则$t$时刻机器人的位姿$(x^{\prime },y^{\prime },{\theta }^{\prime })$
$$\begin{gathered} x^{\prime }=x+v_x\ast \Delta t\ast cos({\theta }_{t-1}) \\ {y}^{\prime }=y+v_x\ast \Delta t\ast sin({\theta }_{t-1}) \\ {\theta }^{\prime }={\theta }^{\prime }+w\Delta t \end{gathered}$$
  附加功能，如果机器人是全向运动即有了$y$轴的速度时，那轨迹如何推演呢？
  已知$t-1$时刻机器人位姿$(x,y,\theta )$、线速度$v_x,v_y$、角速度$w$、与$t$时刻的间隔时间$\Delta t$，求解$t$时刻机器人位姿$(x^{\prime },y^{\prime },{\theta }^{\prime })$
  $t$时刻沿机器人坐标系$X_R$轴移动了$v_x\ast \Delta t$，沿机器人坐标系$Y_R$轴移动了$v_y\ast \Delta t$，只需要将每段距离分别投影在世界坐标系$X_w,Y_w$轴上并相加，就可以得到$t$时刻相对于$t-1$时刻机器人在世界坐标系中坐标移动的位移$\Delta x和\Delta y$。
$$\begin{gathered} \Delta x=v_x\ast \Delta t\ast cos({\theta }_{t-1})-v_y\ast \Delta t\ast sin({\theta }_{t-1}) \\ \Delta y=v_x\ast \Delta t\ast sin({\theta }_{t-1})+v_y\ast \Delta t\ast cos({\theta }_{t-1}) \end{gathered}$$
  则$t$时刻机器人的位姿$(x^{\prime },y^{\prime },{\theta }^{\prime })$
$$\begin{gathered} x^{\prime }=x+v_x\ast \Delta t\ast cos({\theta }_{t-1})-v_y\ast \Delta t\ast sin({\theta }_{t-1}) \\ {y}^{\prime }=y+v_x\ast \Delta t\ast sin({\theta }_{t-1})+v_y\ast \Delta t\ast cos({\theta }_{t-1}) \\ {\theta }^{\prime }={\theta }^{\prime }+w\Delta t \end{gathered}$$
  从以上得知，全向运动的计算是包含不是全向的，只需要令线速度$v_y=0$即可。

4 总结

  本博文讲解了轮式里程计的基本原理和运动学模型的推导。

---