激光SLAM--(2)传感器数据处理之轮式里程计

里程计运动模型及标定

里程计运动模型

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

• 优点：结构简单，便宜（2个电机），模型简单
• 差分模型：
  • 欠驱动系统：运动耦合
  • 圆弧运动：左轮、右轮和底盘中心都在一条射线上，这三个的角速度是一样的。
  • 
  • v，w为底盘中心线加速度和角速度，$v{}_L,v{}_R$为左右轮的速度，d是轮子离底盘中心的距离，r是圆弧运动的圆心到底盘中心的半径。
  • 运动学模型为：
  • $$\mathrm{v}=\frac{v_R+v_L}{2}和\omega =\frac{\left(v_R-v_L\right)}{2d}$$

2.三轮全向底盘的运动学模型

• 优点：任何方向平移，结构简单，全驱动系统，可以进行运动学分解
• 全向模型：
  • 运动分解–纯平移X：
    • 则运动应为：$v_x\ne 0,v_y=0,v_{\theta }=0$
    • 则三个速度分别设置为：$\begin{array}{r}{V}_1=0\ast v_x\\ V_2=-\sin 60v_x\\ V_3=\sin 60v_x\end{array}$
  • 运动分解–纯平移Y：
    • 则运动应为：$v_x=0,v_y\ne 0,v_{\theta }=0$
    • 则三个速度分别设置为：$\begin{array}{r}{V}_1=v_y\\ V_2=-\cos 60v_y\\ V_3=-\cos 60v_y\end{array}$
  • 运功分解–旋转（车中心）：
    • 则运动应为：$v_x=0,v_y=0,v_{\theta }\ne 0$
    • 则三个速度分别设置为：$\begin{array}{r}{V}_1=v_{\theta }\ast d\\ V_2=v_{\theta }\ast d\\ V_3=v_{\theta }\ast d\end{array}$
  • 合成：（因为运动是解耦的，所以可以直接每个轮子速度各自相加）得
    • $$\begin{array}{l}{V}_1=0\ast v_x+1\ast v_y+d\ast v_{\theta }\\ V_2=-\sin 60\ast v_x-\cos 60\ast v_y+d\ast v_{\theta }\\ V_3=\sin 60\ast v_x-\cos 60\ast v_y+d\ast v_{\theta }\end{array}$$
    • $$\left[\begin{array}{l}{V}_1\\ V_2\\ V_3\end{array}\right]=\left[\begin{array}{ccc}0& 1& d\\ -\sin 60& -\cos 60& d\\ \sin 60& -\cos 60& d\end{array}\right]\left[\begin{array}{l}{v}_x\\ v_y\\ v_{\theta }\end{array}\right]$$
    • $$\left[\begin{array}{l}{v}_x\\ v_y\\ v_{\theta }\end{array}\right]=\left[\begin{array}{ccc}0& -\frac{\sqrt{3}}{3}& \frac{\sqrt{3}}{3}\\ \frac{2}{3}& -\frac{1}{3}& -\frac{1}{3}\\ \frac{1}{3d}& \frac{1}{3d}& \frac{1}{3d}\end{array}\right]\left[\begin{array}{l}{V}_1\\ V_2\\ V_3\end{array}\right]$$

3.航迹推算（Dead Reckoning）

• 示意图：

• 递推公式：

  • $(x,y,z)$为底盘当前位姿，$(dx,dy,d\theta )$为运动学解算增量(由上面的$v_x,v_y和v_{\theta }$各自乘以$\delta t$得到），下面那个矩阵是平面二维转换矩阵
  • $$\left[\begin{array}{l}x\\ y\\ \theta \end{array}\right]=\left[\begin{array}{l}x\\ y\\ \theta \end{array}\right]+\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{l}dx\\ dy\\ d\theta \end{array}\right]$$
  • $$\left[\begin{array}{l}x\\ y\\ \theta \end{array}\right]=\left[\begin{array}{l}x\\ y\\ \theta \end{array}\right]+\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{l}dx+{\epsilon }_x\\ dy+{\epsilon }_y\\ d\theta +{\epsilon }_{\theta }\end{array}\right]$$

里程计标定（系统误差标定）

1.线性最小二乘的基本原理

• 线性方程组Ax=b，A是mxn，x是nx1：
  • 当m=n时，适定方程组，有唯一解；
  • 当m
  • 当m>n时，超定方程组，无解；
  • 绝大多数下无解，但是可以找最靠近真实解的解（最小二乘解），通解为：$x^{\ast }={\left(A^{T}A\right)}^{-1}{A}^{T}b$（$A^{T}A$条件数比较大，病态的，一般对其QR分解）
• 最小二乘的求解–线性空间的角度
  • Ax表示A的列向量空间S，无解意味着向量b不在S中，其最近的解即为：向量b在S中的投影。
  • 求解过程：设$Ax\ast$为向量b在空间中的投影，则显然$(b-Ax\ast )$垂直于空间S，即其与矩阵A的每一个列向量都垂直；
  • 设$A=[a{}_1,a{}_2,...a{}_n]$，则有$a{{}_i}^T(b-Ax\ast )=0$，一系列化简可得$x\ast =(A^{T}A{)}^{-1}{A}^{T}b$

2.最小二乘的直线拟合

3.最小二乘在里程计标定中的应用

• 里程计标定主要方法：
  • 直接线性方法：通用性强，实现简单，精度不高，
  • 基于模型的方法：精度高、实现复杂、特异性高
• 直接线性方程
  • 用激光雷达的scan-match数据作为真值$u{{}_i}^{\ast }$
  • 里程计测量得到的数据作为$u{}_i$
  • 假设成线性关系：$u{{}_i}^{\ast }=X\ast u{}_i$

具体代码实现有时间再附上吧