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

里程计运动模型及标定

里程计运动模型

1.两轮差速底盘的运动学模型
  • 优点:结构简单,便宜(2个电机),模型简单
  • 差分模型:
    • 欠驱动系统:运动耦合
    • 圆弧运动:左轮、右轮和底盘中心都在一条射线上,这三个的角速度是一样的。
    • 激光SLAM--(2)传感器数据处理之轮式里程计_第1张图片
    • v,w为底盘中心线加速度和角速度, v L , v R v{_L},v{_R} vL,vR为左右轮的速度,d是轮子离底盘中心的距离,r是圆弧运动的圆心到底盘中心的半径。
    • 运动学模型为:
    • v = v R + v L 2 和 ω = ( v R − v L ) 2 d \mathrm{v}=\frac{v_{R}+v_{L}}{2}和\omega=\frac{\left(v_{R}-v_{L}\right)}{2 d} v=2vR+vLω=2d(vRvL)
2.三轮全向底盘的运动学模型
  • 优点:任何方向平移,结构简单,全驱动系统,可以进行运动学分解
  • 全向模型:激光SLAM--(2)传感器数据处理之轮式里程计_第2张图片
    • 运动分解–纯平移X:
      • 则运动应为: v x ≠ 0 , v y = 0 , v θ = 0 v_{x} \neq 0, v_{y}=0, v_{\theta}=0 vx=0,vy=0,vθ=0
      • 则三个速度分别设置为: V 1 = 0 ∗ v x V 2 = − sin ⁡ 60 v x V 3 = sin ⁡ 60 v x \begin{array}{r} V_{1}=0 * v_{x} \\ V_{2}=-\sin 60 v_{x} \\ V_{3}=\sin 60 v_{x} \end{array} V1=0vxV2=sin60vxV3=sin60vx
    • 运动分解–纯平移Y:
      • 则运动应为: v x = 0 , v y ≠ 0 , v θ = 0 v_{x} = 0, v_{y}\neq0, v_{\theta}=0 vx=0,vy=0,vθ=0
      • 则三个速度分别设置为: V 1 = v y V 2 = − cos ⁡ 60 v y V 3 = − cos ⁡ 60 v y \begin{array}{r} V_{1}=v_{y} \\ V_{2}=-\cos 60 v_{y} \\ V_{3}=-\cos 60 v_{y} \end{array} V1=vyV2=cos60vyV3=cos60vy
    • 运功分解–旋转(车中心):
      • 则运动应为: v x = 0 , v y = 0 , v θ ≠ 0 v_{x} = 0, v_{y}=0, v_{\theta}\neq0 vx=0,vy=0,vθ=0
      • 则三个速度分别设置为: V 1 = v θ ∗ d V 2 = v θ ∗ d V 3 = v θ ∗ d \begin{array}{r} V_{1}=v_{\theta}*d \\ V_{2}=v_{\theta}*d \\ V_{3}=v_{\theta}*d \end{array} V1=vθdV2=vθdV3=vθd
    • 合成:(因为运动是解耦的,所以可以直接每个轮子速度各自相加)得
      • V 1 = 0 ∗ v x + 1 ∗ v y + d ∗ v θ V 2 = − sin ⁡ 60 ∗ v x − cos ⁡ 60 ∗ v y + d ∗ v θ V 3 = sin ⁡ 60 ∗ v x − cos ⁡ 60 ∗ v y + d ∗ v θ \begin{array}{l} V_{1}=0 * v_{x}+1 * v_{y}+d * v_{\theta} \\ V_{2}=-\sin 60 * v_{x}-\cos 60 * v_{y}+d * v_{\theta} \\ V_{3}=\sin 60 * v_{x}-\cos 60 * v_{y}+d * v_{\theta} \end{array} V1=0vx+1vy+dvθV2=sin60vxcos60vy+dvθV3=sin60vxcos60vy+dvθ
      • [ V 1 V 2 V 3 ] = [ 0 1 d − sin ⁡ 60 − cos ⁡ 60 d sin ⁡ 60 − cos ⁡ 60 d ] [ v x v y v θ ] \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] V1V2V3 = 0sin60sin601cos60cos60ddd vxvyvθ
      • [ v x v y v θ ] = [ 0 − 3 3 3 3 2 3 − 1 3 − 1 3 1 3 d 1 3 d 1 3 d ] [ V 1 V 2 V 3 ] \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}{3 d} & \frac{1}{3 d} & \frac{1}{3 d} \end{array}\right]\left[\begin{array}{l} V_{1} \\ V_{2} \\ V_{3} \end{array}\right] vxvyvθ = 0323d133 313d133 313d1 V1V2V3
3.航迹推算(Dead Reckoning)
  • 示意图:激光SLAM--(2)传感器数据处理之轮式里程计_第3张图片

  • 递推公式:

    • ( x , y , z ) (x, y, z) (x,y,z)为底盘当前位姿, ( d x , d y , d θ ) (dx, dy, d\theta) (dx,dy,dθ)为运动学解算增量(由上面的 v x , v y 和 v θ v_{x} , v_{y}和 v_{\theta} vx,vyvθ各自乘以 δ t \delta t δt得到),下面那个矩阵是平面二维转换矩阵
    • [ x y θ ] = [ x y θ ] + [ cos ⁡ θ − sin ⁡ θ 0 sin ⁡ θ cos ⁡ θ 0 0 0 1 ] [ d x d y d θ ] \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} d x \\ d y \\ d \theta \end{array}\right] xyθ = xyθ + cosθsinθ0sinθcosθ0001 dxdydθ
    • [ x y θ ] = [ x y θ ] + [ cos ⁡ θ − sin ⁡ θ 0 sin ⁡ θ cos ⁡ θ 0 0 0 1 ] [ d x + ε x d y + ε y d θ + ε θ ] \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} d x+\varepsilon_{x} \\ d y+\varepsilon_{y} \\ d \theta+\varepsilon_{\theta} \end{array}\right] xyθ = xyθ + cosθsinθ0sinθcosθ0001 dx+εxdy+εydθ+εθ

里程计标定(系统误差标定)

1.线性最小二乘的基本原理
  • 线性方程组Ax=b,A是mxn,x是nx1:
    • 当m=n时,适定方程组,有唯一解;
    • 当m
    • 当m>n时,超定方程组,无解;
    • 绝大多数下无解,但是可以找最靠近真实解的解(最小二乘解),通解为: x ∗ = ( A T A ) − 1 A T b x^{*}=\left(A^{T} A\right)^{-1} A^{T} b x=(ATA)1ATb A T A A^{T} A ATA条件数比较大,病态的,一般对其QR分解)
  • 最小二乘的求解–线性空间的角度
    • Ax表示A的列向量空间S,无解意味着向量b不在S中,其最近的解即为:向量b在S中的投影
    • 求解过程:设 A x ∗ Ax* Ax为向量b在空间中的投影,则显然 ( b − A x ∗ ) (b-Ax*) (bAx)垂直于空间S,即其与矩阵A的每一个列向量都垂直;
    • A = [ a 1 , a 2 , . . . a n ] A = [a{_1},a{_2},...a{_n}] A=[a1,a2,...an],则有 a i T ( b − A x ∗ ) = 0 a{_i}^T(b-Ax*)=0 aiT(bAx)=0,一系列化简可得 x ∗ = ( A T A ) − 1 A T b x*=(A^TA)^{-1}A^Tb x=(ATA)1ATb
2.最小二乘的直线拟合
3.最小二乘在里程计标定中的应用
  • 里程计标定主要方法:
    • 直接线性方法:通用性强,实现简单,精度不高,
    • 基于模型的方法:精度高、实现复杂、特异性高
  • 直接线性方程
    • 用激光雷达的scan-match数据作为真值 u i ∗ u{_i}^* ui
    • 里程计测量得到的数据作为 u i u{_i} ui
    • 假设成线性关系: u i ∗ = X ∗ u i u{_i}^*=X*u{_i} ui=Xui

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

你可能感兴趣的:(激光SLAM,slam,人工智能,机器学习,算法)