轮式里程计原理与状态传播数学推导

轮速计原理

• 磁电式轮速传感器是由永磁性磁芯和线圈组成。磁力线从磁芯的一极出来，穿过齿圈和空气，返回到磁芯的另一极。由于传感器的线圈圈绕在磁芯上，因此，这些磁力线也会穿过线圈。当车轮旋转时，与车轮同步的齿圈（转子）随之旋转，齿圈上的齿和间隙依次快速经过传感器的磁场，其结果是改变了磁路的磁阻，从而导致线圈中感应电势发生变化，产生一定幅值、频率的电势脉冲。
• 光电式轮速传感器：轮子转一圈打4096个光电脉冲，通过光电脉冲的计数来获得当前转动的弧度（\(\frac{n}{4096}\)个圆周）。

可以发现其中磁电式轮速计测量得到的值是连续的，其原始测量可以通过一个常数系数转成轮子旋转的角速度，光电式测量的值不是连续的，其本身更像是里程计。

优点

• 结构简单
• 便宜（2个电机就可以）
• 模型简单

两轮差速底盘的运动学模型

• 模型

传感器测量值为两个轮子的旋转角速度\(w_R,w_L\)（此处角速度是轮子旋转的角速度，绕小圆） \[ \begin{align} w_{ml}= w_L + n_{wl} \\ w_{mr}= w_R + n_{wr} \end{align} \] 假设\(v,w\)分别为车体的线速度和角速度（此处角速度是yaw姿态变化的速度），\(v_R,v_L\)为两个轮子的线速度，\(d\)为轮子距离底盘中心的距离，\(b\)为两轮之间的距离（\(b=2d\)），\(r_R，r_L\)分别为两个轮子的半径。则 \[ \begin{align} v&=\frac{v_R+v_L}{2}=\frac{w_R.r_R+w_L.r_L}{2} \\ w&=\frac{v_R-v_L}{b}=\frac{w_R.r_R-w_L.r_L}{b} \end{align} \] 矩阵表示如下： \[ \left[\begin{array}{cc} v \\ w \end{array}\right]= \left[\begin{array}{cc} \frac{r_L}{2} & \frac{r_R}{2} \\ -\frac{r_L}{b} & \frac{r_R}{b} \end{array}\right] \left[\begin{array}{cc} w_L \\ w_R \end{array}\right] = J\left[\begin{array}{cc} w_L \\ w_R \end{array}\right] \]

• 原理

假设当前小车绕半径为r的圆弧运动，则有(此处假设左轮更靠近圆心) \[ \begin{align} \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} \end{align} \] 继续推导将r带入可以得到运动的角速度w \[ \begin{align} w&=\frac{v_R}{r+d} \\ r+d &=\frac{(v_R+v_L)d}{(v_R-v_L)}+\frac{(v_R-v_L)d}{(v_R-v_L)}=2\frac{v_Rd}{(v_R-v_L)} \\ w&=\frac{(v_R-v_L)}{2d} \end{align} \] 同样可以求得线速度如下 \[ \begin{align} v=w*r &=\frac{(v_R-v_L)}{2d}\frac{(v_R+v_L)d}{(v_R-v_L)} \\ &= \frac{v_R+v_L}{2} \end{align} \] 结论如下： \[ \begin{align} v&=\frac{v_R+v_L}{2}=\frac{w_R.r_R+w_L.r_L}{2} \\ w&=\frac{v_R-v_L}{2d}=\frac{w_R.r_R-w_L.r_L}{2d} \end{align} \]

虽然此处主要讨论了速度的相关公式，但是其实实际应用中我们不关心速度，上述公式乘以\(dt\)就变成了距离与姿态变换并且依旧成立，后面的推导基于此。

位姿递推公式

• \((x,y,\theta)\)为世界坐标系下的当前位姿；
• \((dx,dy,d\theta)\)为车体坐标系下的运动增量；

所以叠加为（首先将增量从车体转换到世界坐标系，然后叠加）： \[ \left[\begin{array}{cc} x \\ y \\ \theta \end{array}\right] = \left[\begin{array}{cc} x \\ y \\ \theta \end{array}\right] + \left[\begin{array}{cc} cos\theta & -sin\theta & 0 \\ sin\theta & cos\theta & 0 \\ 0 & 0 & 1 \end{array}\right] \left[\begin{array}{cc} dx \\ dy \\ d\theta \end{array}\right] \]

可以发现此处的位姿是se2的，为了更加直观，下面会以kaist数据集为例，讲述拓展到se3上基于轮速计测量的状态传播过程。

以kaist数据集为例说明轮速里程计(状态传播过程)

测量数据说明

• 轮速计标定参数：
  • 分辨率：\(resolution=4096\)
  • 左轮直径：\(R_l=0.623479\)
  • 右轮直径：\(R_r=0.622806\)
  • 两轮子之间距离：\(b=1.52439\)
• 轮速计测量：
  • 时间戳
  • 左轮目前计数\(m_l\)
  • 右轮目前计数\(m_r\)

均值传播

首先计算左轮\(k_l\)与右轮\(k_r\)（计数与距离的乘法系数） \[ \begin{align} k_l = R_l * \pi /resolution = 0.00047820240382508 \\ k_r = R_r * \pi /resolution = 0.00047768621928995 \end{align} \] 假设上一时刻左轮计数是\(b_{ml}\)，右轮计数是\(b_{mr}\)，当前时刻左轮计数是\(e_{ml}\)，右轮计数是\(e_{mr}\)。则可以计算得到左轮的移动距离\(d_{l}\)与右轮的移动距离\(d_r\) \[ \begin{align} d_l = (e_{ml}-b_{ml})*k_l \\ d_r = (e_{mr}-b_{mr})*k_r \end{align} \] 然后得到得到车体（两轮中点）前进的距离与旋转的角度(yaw角)：本文重点公式 \[ \begin{align} dp = 0.5(d_l+d_r) \\ \theta = (d_r-d_l)/b \end{align} \] 传播位姿：然后更新系统中车体的位姿\(R_O^G,p_O^G\) \[ \begin{align} p_O^G &= p_O^G + R_O^G * \left[\begin{array}{cc} dp \\ 0 \\ 0 \end{array}\right] = p_O^G + R_O^G * dx \\ R_O^G &= R_O^G * exp_{so3}(\left[\begin{array}{cc} 0 \\ 0 \\ \theta \end{array}\right] ) = R_O^G * \left[\begin{array}{cc} cos\theta & -sin\theta & 0 \\ sin\theta & cos\theta & 0 \\ 0 & 0 & 1 \end{array}\right] = R_O^G * dR \end{align} \]

此处可以发现相对于上一时刻的位移直接用\([dp,0,0]^T\)表示了，这其实是忽略了\(\theta\)的影响的（小车车头朝向自身坐标系的x轴方向，可以认为时间很短的情况下，\(\theta\)很小，\(cos\theta\)接近1，\(sin\theta\)接近1），会有一定的误差。

协方差传播

对于协方差传播原理详见之前系统误差状态维护的博客。 \[ \tilde{x}_{k+1} = F_c \tilde{x_k} + G_c n \] 其中，\(F_c\)是k+1时刻系统状态相对于k时刻系统状态的雅克比，\(G_c\)是k+1时刻系统状态相对于输入噪声（此处输入噪声协方差就是dR与dx的噪声协方差矩阵）的雅克比。由上面的状态转移方程，可以得到（哈密顿） \[ F_c = \begin{bmatrix}{cc} dR^T & 0 \\ -R_O^G [dx]_{\times} & I \end{bmatrix} \]

\[ G_c= \left[\begin{array}{cc} I_3 & 0 \\ 0 & R_O^G \end{array}\right] \] 车体位姿的协方差矩阵（\(6\times6\)）转移为 \[ Cov_{new} = F_c . Cov_{old}. F_c^T + G_c . Cov_n . G_c^T \] 其中\(Cov_n\)是自己设置的一般设置为如下对角矩阵 \[ \left[\begin{array}{cc} 10^{-6} & 0 & 0 & 0 & 0 & 0 \\ 0 & 10^{-6} & 0 & 0 & 0 & 0 \\ 0 & 0 & {\theta}^2*noise^2 & 0 & 0 & 0 \\ 0 & 0 & 0 & dp^2*noise^2 & 0 & 0 \\ 0 & 0 & 0 & 0 & dp^2*noise^2 & 0 \\ 0 & 0 & 0 & 0 & 0 & 10^{-4} \end{array}\right] \] 可以发现其中roll和pitch角误差还有z轴移动噪声其实我们是没有的，都设置了一个比较小的值，而yaw和x，y的噪声是有迹可循的，其中noise可以通过轮速计的分辨率获得，不过一般都会设置的偏大一些。

可以发现此处对odo的建模其实是把其当做里程计来使用，这是不够准确的。更精确的轮速计的建模后续会整理上来。

更精确的轮速计建模

哈密顿

均值传播

首先是轮速计测量 \[ \begin{align} w_{ml} = w_l + n_{wl} \\ w_{mr} = w_r + n_{wr} \end{align} \] 其中\(w\)为角速度，一般磁电轮速计测量的raw数据可以看成是角速度。其中\(n\)是零均值高斯白噪。结合轮速计的内参可以得到车体的角速度与线速度 \[ \begin{align} o_w=(w_rr_r-w_lr_l)/b \\ o_v=(w_rr_r+w_lr_l)/2 \end{align} \] 其中\([r_l,r_r,b]^T\)是轮速度的内参（分别是左右轮半径、左右轮距离）。据此，假设经过了一个很小的时间间隔\(\Delta t\)，此时小车在二维平面上运动了 \[ \begin{align} d\theta = o_w * \Delta t = (w_rr_r-w_lr_l)\Delta t/b = (d_r-d_l)/b \\ dp = o_v * \Delta t = (w_rr_r+w_lr_l)\Delta t/2 = (d_r+d_l)/2 \end{align} \] 其中\(d\theta\)是小车在2维平面上逆时针（从右向左拐弯）旋转的角度，dp是小车沿着\(d\theta\)角度直线运动的距离，因此将dp分解称为dx和dy后，小车的运动可以概括为 \[ \begin{align} d\theta = o_w * \Delta t = (w_rr_r-w_lr_l)\Delta t/b = (d_r-d_l)/b \\ dx = dp*cos(d\theta) = (d_r+d_l)cos(d\theta)/2 \\ dy = dp*sin(d\theta) = (d_r+d_l)sin(d\theta)/2 \end{align} \] 将上面转成三维情况，即相对初始时刻旋转向量（轴角表示）为\(d\theta=\left[\begin{array}{cc} 0 \\ 0 \\ d\theta \end{array}\right]\)，平移为\(\left[\begin{array}{cc} dx \\ dy \\ 0 \end{array}\right]\)。所以 \[ \begin{align} p_O^G &= p_O^G + R_O^G * \left[\begin{array}{cc} dx \\ dy \\ 0 \end{array}\right] \\ R_O^G &= R_O^G * exp_{so3}(\left[\begin{array}{cc} 0 \\ 0 \\ d\theta \end{array}\right] ) = R_O^G * \left[\begin{array}{cc} cos(d\theta) & -sin(d\theta) & 0 \\ sin(d\theta) & cos(d\theta) & 0 \\ 0 & 0 & 1 \end{array}\right] = R_O^G * dR \end{align} \] 协方差传播

对于协方差传播原理详见之前系统误差状态维护的博客。 \[ \tilde{x}_{k+1} = F_c \tilde{x_k} + G_c n \] 其中，\(F_c\)是k+1时刻系统状态相对于k时刻系统状态的雅克比，\(G_c\)是k+1时刻系统状态相对于输入噪声（此处输入噪声协方差就是\(d_r\)与\(d_l\)的噪l声协方差矩阵）的雅克比。由上面的状态转移方程，可以得到（哈密顿） \[ F_c = \left[\begin{array}{cc} dR^T & 0 \\ -R_O^G \left[\begin{array}{cc} dx \\ dy \\ 0 \end{array}\right]_{\times} & I \end{array}\right] \]

\[ G_c= \left[\begin{array}{cc} R_O^G\ dR \left[\begin{array}{cc} 0 \\ 0 \\ 1/b \end{array}\right] & R_O^G\ dR \left[\begin{array}{cc} 0 \\ 0 \\ -1/b \end{array}\right] \\ R_O^G \left[\begin{array}{cc} \frac{cos(d\theta)}{2} - \frac{(d_r+d_l)}{2b}sin(d\theta) \\ \frac{sin(d\theta)}{2} + \frac{(d_r+d_l)}{2b}cos(d\theta) \\ 0 \end{array}\right] & R_O^G \left[\begin{array}{cc} \frac{cos(d\theta)}{2} + \frac{(d_r+d_l)}{2b}sin(d\theta) \\ \frac{sin(d\theta)}{2} - \frac{(d_r+d_l)}{2b}cos(d\theta) \\0 \end{array}\right] \end{array}\right] \] 其中 \[ \begin{align} \frac{d\theta}{d_r}&=\frac{1}{b} \\ \frac{d\theta}{d_l}&=-\frac{1}{b} \\ \frac{dx}{d_r}&=\frac{cos(d\theta)}{2} - \frac{(d_r+d_l)}{2b}sin(d\theta) \\ \frac{dx}{d_l}&=\frac{cos(d\theta)}{2} + \frac{(d_r+d_l)}{2b}sin(d\theta) \\ \frac{dy}{d_r}&=\frac{sin(d\theta)}{2} + \frac{(d_r+d_l)}{2b}cos(d\theta) \\ \frac{dy}{d_l}&=\frac{sin(d\theta)}{2} - \frac{(d_r+d_l)}{2b}cos(d\theta) \\ \end{align} \]

车体位姿的协方差矩阵（\(6\times6\)）转移为 \[ Cov_{new} = F_c . Cov_{old}. F_c^T + G_c . Cov_n . G_c^T \] 其中\(Cov_n\)是自己设置的一般设置为如下对角矩阵 \[ Cov_n = \left[\begin{array}{cc} k_r*k_r & 0 \\ 0 & k_l*k_l \end{array}\right] \] 对角线是误差的平方，其中\(k_r\)与\(k_l\)前面有计算，表示的是编码器前后差1时轮子移动的距离。（为了系统稳定可靠，可以将协方差放大一点）

轮式里程计标定方法

内参标定（与其他传感器比如激光、视觉一同标定）

从上述可知，轮式里程计需要标定的是\(k_l,k_r,b\)。(也有标定左右轮半径+b的，此处就是)，外参需要标定的是相机与odo之间的外参。

• 旷视开源了一个相机odo标定的代码

• 作者：旷视科技 链接：https://zhuanlan.zhihu.com/p/101727151 来源：知乎 著作权归作者所有。商业转载请联系作者获得授权，非商业转载请注明出处。

  我们已将Cam_Odo的标定代码开源：https://github.com/MegviiRobot/CamOdomCalibraTool。代码使用方案二，因为要同时标定码盘内参（左右轮半径和轮距）和相机码盘外参（旋转外参3个角度，平移外参的前两维，第三维在平面运动下不可观）。标定流程分为【采集并筛选数据】，【解析计算标定参数】，【优化refine标定结果】这三个步骤。接下来我们概要说明一下各个步骤和相关注意点。

  采集并筛选数据

  • 采集数据时尽量走S路线以保证机体有足够的平移和旋转（充足的激励）。
  • 注意修改wheel_callback函数，确保odoDatas里存的是odo数据的元素是左右轮转速（rad/s）。我们的代码里由于odo_msg里是用出厂时带的码盘内参算出来的线速度和角速度，所以先用出厂码盘参数还原了一下初始转速，才push进了odoDatas。
  • 采集图像时，采用的apriltag2来解算的cam位姿（相关代码在calc_cam_pose文件夹内，所需apriltag选用kalibr里的50cm×50cm），计算帧间位姿变换后，把旋转变成轴角表示，计算帧间平移等存入cam_data并push到camDatas中。
  • 筛选数据，【nan数据】、【运动平移太小的】、【相机帧间旋转的旋转轴偏离平面法向量的】（我们相机是x右y下z前所以旋转轴应该在[0 -1 0]附近，偏离多的说明那一次的帧间运动可能存在地不平的情况，舍之）、【运动角度（/转速）太小的】、【cam和odo转的角度方向不同的】（多是延时导致，在运动发生变化时延时会导致方向不一致）等情况下的数据均被剔除。
  • 对odo数据进行插值（根据图像时间戳），并求得两个cam帧之间多个odo数据的线速度和角速度均值保存起来。

  解析计算标定参数

  这一部分主要包括三个函数，estimateRyx、correctCamera和calib。

轮式内参已知标定其与相机外参

问题定义：已知相机帧间运动\(R_{c_i}^{c_{i+1}}，p_{c_i}^{c_{i+1}}\)，与odo的帧间运动\(R_{o_i}^{o_{i+1}}，p_{o_i}^{o_{i+1}}\)，还已知了odo的内参（左右轮半径和距离），要标定的是cam和odo之间的\(R_{c}^{o}，p_{c}^{o}\)

首先根据经典的手眼标定公式\(AX=XB\)可得 \[ R_{o_i}^{o_{i+1}} R_{c}^{o} = R_{c}^{o}R_{c_i}^{c_{i+1}} \\ (R_{o_i}^{o_{i+1}} - I_3) p_c^o = R_c^o p_{c_i}^{c_{i+1}} - p_{o_i}^{o_{i+1}} \]

其中第一个式子是通过两种形式求得\(R_{c_i}^{o_{i+1}}\)，第二个式子是

可以发现只要帧间运动的测量够多，这个方程就可以解出来。