里程计运动模型及标定【激光slam（一）】

文章目录

1、里程计模型
- [1.1 两轮差分底盘的运动学模型](#1.1 两轮差分底盘的运动学模型)
- [1.2 三轮全向底盘的运动学模型](#1.2 三轮全向底盘的运动学模型)
- [1.3 航迹推算(Dead Reckoning)](#1.3 航迹推算(Dead Reckoning))
2、里程计标定
- [2.1 线性最小二乘的基本原理](#2.1 线性最小二乘的基本原理)
- [2.2 最小二乘的直线拟合](#2.2 最小二乘的直线拟合)
- [2.3 最小二乘在里程计标定中的应用](#2.3 最小二乘在里程计标定中的应用)

1、里程计模型

1.1 两轮差分底盘的运动学模型

应用实例：

优点：

结构简单
便宜(2个电机)
模型简单

差分模型：

运动解算：

v v v和 ω \omega ω为底盘中心线速度和角速度
v L v_L vL， v R v_R vR为左右两轮的速度
d d d为轮子离底盘中心的距离

v = v R + v L 2 v = \frac{v_R + v_L}{2} v=2vR+vL

ω = v R − v L 2 d \omega = \frac{v_R - v_L}{2d} ω=2dvR−vL

求解过程：

v L r − d = v R r + d v L ( r + d ) = v R ( r − d ) ( v R − v L ) r = ( v R + v L ) d r = ( v R + v L ) d ( v R − v L ) \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−dvLvL(r+d)(vR−vL)rr=r+dvR=vR(r−d)=(vR+vL)d=(vR−vL)(vR+vL)d

ω = v R r + d r + d = ( v R + v L ) d ( v R − v L ) + ( v R − v L ) d ( v R − v L ) = 2 v R d ( v R − v L ) ω = ( v R − v L ) 2 d \begin{align*} \omega &= \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_R d}{(v_R - v_L)} \\ \omega &= \frac{(v_R - v_L)}{2d} \end{align*} ωr+dω=r+dvR=(vR−vL)(vR+vL)d+(vR−vL)(vR−vL)d=2(vR−vL)vRd=2d(vR−vL)

v = ω ⋅ r = ( v R − v L ) ( v R + v L ) d 2 d = v R + v L 2 v = v R + v L 2 \begin{align*} v &= \omega \cdot r = \frac{(v_R - v_L)(v_R + v_L)d}{2d} = \frac{v_R + v_L}{2} \\ v &= \frac{v_R + v_L}{2} \end{align*} vv=ω⋅r=2d(vR−vL)(vR+vL)d=2vR+vL=2vR+vL

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

应用实例：

优点：

任何方向平移
全驱动系统
可以进行运动学分解
结构简单

全向模型：

运动分解：

平移 x x x：

v x ≠ 0 , v y = 0 , v θ = 0 V 1 = 0 ⋅ v x V 2 = − sin ⁡ 60 v x V 3 = sin ⁡ 60 v x \begin{align*} v_x &\neq 0, \quad v_y = 0, \quad v_\theta = 0 \\ V_1 &= 0 \cdot v_x \\ V_2 &= -\sin 60 \, v_x \\ V_3 &= \sin 60 \, v_x \end{align*} vxV1V2V3=0,vy=0,vθ=0=0⋅vx=−sin60vx=sin60vx

平移 y y y：

v x = 0 , v y ≠ 0 , v θ = 0 V 1 = v y V 2 = − cos ⁡ 60 v y V 3 = − cos ⁡ 60 v y \begin{align*} v_x &= 0, \quad v_y \neq 0, \quad v_\theta = 0 \\ V_1 &= v_y \\ V_2 &= -\cos 60 \, v_y \\ V_3 &= -\cos 60 \, v_y \end{align*} vxV1V2V3=0,vy=0,vθ=0=vy=−cos60vy=−cos60vy

旋转：

v x = 0 , v y = 0 , v θ ≠ 0 V 1 = v θ d V 2 = v θ d V 3 = v θ d \begin{align*} v_x &= 0, \quad v_y = 0, \quad v_\theta \neq 0 \\ V_1 &= v_\theta d \\ V_2 &= v_\theta d \\ V_3 &= v_\theta d \end{align*} vxV1V2V3=0,vy=0,vθ=0=vθd=vθd=vθd

V 1 = 0 ⋅ v x V 2 = − sin ⁡ 60 ∘ v x V 3 = sin ⁡ 60 ∘ v x v x ≠ 0 , v y = 0 , v θ = 0 V 1 = v y V 2 = − cos ⁡ 60 ∘ v y V 3 = − cos ⁡ 60 ∘ v y v x = 0 , v y ≠ 0 , v θ = 0 V 1 = v θ d V 2 = v θ d V 3 = v θ d v x = 0 , v y = 0 , v θ ≠ 0 \begin{array}{l|l} \begin{aligned} V_1 &= 0 \cdot v_x \\ V_2 &= -\sin 60^\circ\, v_x \\ V_3 &= \sin 60^\circ\, v_x \end{aligned} & v_x \neq 0,\ v_y = 0,\ v_\theta = 0 \\ \hline \begin{aligned} V_1 &= v_y \\ V_2 &= -\cos 60^\circ\, v_y \\ V_3 &= -\cos 60^\circ\, v_y \end{aligned} & v_x = 0,\ v_y \neq 0,\ v_\theta = 0 \\ \hline \begin{aligned} V_1 &= v_\theta d \\ V_2 &= v_\theta d \\ V_3 &= v_\theta d \end{aligned} & v_x = 0,\ v_y = 0,\ v_\theta \neq 0 \end{array} V1V2V3=0⋅vx=−sin60∘vx=sin60∘vxV1V2V3=vy=−cos60∘vy=−cos60∘vyV1V2V3=vθd=vθd=vθdvx=0, vy=0, vθ=0vx=0, vy=0, vθ=0vx=0, vy=0, vθ=0

👇

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 θ $V 1 V 2 V 3$ = $0 1 d - sin 60 \circ - cos 60 \circ d sin 60 \circ - cos 60 \circ d$ $v x v y v θ$ $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$ \begin{aligned} V_1 &= 0 * v_x + 1 * v_y + d * v_\theta \\ V_2 &= -\sin 60^\circ * v_x - \cos 60^\circ * v_y + d * v_\theta \\ V_3 &= \sin 60^\circ * v_x - \cos 60^\circ * v_y + d * v_\theta \\ $3ex$ \begin{bmatrix} V_1 \\ V_2 \\ V_3 \end{bmatrix} &= \begin{bmatrix} 0 & 1 & d \\ -\sin 60^\circ & -\cos 60^\circ & d \\ \sin 60^\circ & -\cos 60^\circ & d \end{bmatrix} \begin{bmatrix} v_x \\ v_y \\ v_\theta \end{bmatrix} \\ $3ex$ \begin{bmatrix} v_x \\ v_y \\ v_\theta \end{bmatrix} &= \begin{bmatrix} 0 & -\dfrac{\sqrt{3}}{3} & \dfrac{\sqrt{3}}{3} \\ $2ex$ \dfrac{2}{3} & -\dfrac{1}{3} & -\dfrac{1}{3} \\ $2ex$ \dfrac{1}{3d} & \dfrac{1}{3d} & \dfrac{1}{3d} \end{bmatrix} \begin{bmatrix} V_1 \\ V_2 \\ V_3 \end{bmatrix} \end{aligned} V1V2V3 V1V2V3 vxvyvθ =0∗vx+1∗vy+d∗vθ=−sin60∘∗vx−cos60∘∗vy+d∗vθ=sin60∘∗vx−cos60∘∗vy+d∗vθ= 0−sin60∘sin60∘1−cos60∘−cos60∘ddd vxvyvθ = 0323d1−33 −313d133 −313d1 V1V2V3

1.3 航迹推算(Dead Reckoning)

示意图：

递推公式：

( 𝑥 , 𝑦 , 𝜃 ) (𝑥, 𝑦, 𝜃) (x,y,𝜃) 为底盘当前位姿

( 𝑑 𝑥 , 𝑑 𝑦 , 𝑑 𝜃 ) (𝑑𝑥, 𝑑𝑦, 𝑑𝜃) (dx,dy,d𝜃) 为运动学解算增量

机器人坐标系转为世界坐标系的转换矩阵是：

T = $cos θ - sin θ x sin θ cos θ y 0 0 1$ T=\begin{bmatrix} \cos\theta & -\sin\theta & x \cr \sin\theta & \cos\theta & y \cr 0 & 0 & 1 \end{bmatrix} T= cosθsinθ0−sinθcosθ0xy1

$x y θ$ = $x y θ$ + $cos θ - sin θ 0 sin θ cos θ 0 0 0 1$ $d x d y d θ$ \begin{bmatrix} x \cr y \cr \theta \end{bmatrix} = \begin{bmatrix} x \cr y \cr \theta \end{bmatrix} + \begin{bmatrix} \cos\theta & -\sin\theta & 0 \cr \sin\theta & \cos\theta & 0 \cr 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} dx \cr dy \cr d\theta \end{bmatrix} xyθ = xyθ + cosθsinθ0−sinθcosθ0001 dxdydθ

$x y θ$ = $x y θ$ + $cos θ - sin θ 0 sin θ cos θ 0 0 0 1$ $d x + ε x d y + ε y d θ + ε θ$ \begin{bmatrix} x \cr y \cr \theta \end{bmatrix} = \begin{bmatrix} x \cr y \cr \theta \end{bmatrix} + \begin{bmatrix} \cos\theta & -\sin\theta & 0 \cr \sin\theta & \cos\theta & 0 \cr 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} dx + \varepsilon_x \cr dy + \varepsilon_y \cr d\theta + \varepsilon_\theta \end{bmatrix} xyθ = xyθ + cosθsinθ0−sinθcosθ0001 dx+εxdy+εydθ+εθ

2、里程计标定

2.1 线性最小二乘的基本原理

线性方程组 A x = b Ax=b Ax=b

A A A为 m × n m × n m×n的矩阵。

x x x为 n × 1 n × 1 n×1的向量

当 m = n m=n m=n时，适定方程组，方程组有唯一解

当 m < n m<n m<n时，欠定方程组，方程组有无穷多解

当 m > n m>n m>n时，超定方程组，方程组有通常无解

最小二乘解：

绝大多数情况为 m > n m>n m>n，超定方程组

无解，可以寻找最靠近真实解的解

无解但是有最小二乘解

通解： x ∗ = ( A T A ) − 1 A T b x^* = (A^T A)^{-1} A^T b x∗=(ATA)−1ATb

最小二乘的求解：线性空间的角度

𝐴 𝑥 𝐴𝑥 Ax表示 𝐴 𝐴 A的列向量空间 𝑆 𝑆 S， 𝐴 𝑥 𝐴𝑥 Ax也表示 𝐴 𝐴 A列向量的线性组合，

无解意味着向量 𝑏 𝑏 b不在 𝑆 𝑆 S中

最近的解即为：向量 𝑏 𝑏 b在 𝑆 𝑆 S中的投影

设 𝐴 𝑥 ∗ 𝐴𝑥^∗ Ax∗为向量 b b b在空间 𝑆 𝑆 S中的投影，显然 𝑏 − 𝐴 𝑥 ∗ 𝑏 − 𝐴𝑥^* b−Ax∗垂直于空间 𝑆 𝑆 S。

𝑏 − 𝐴 𝑥 ∗ 𝑏 − 𝐴𝑥^∗ b−Ax∗ 跟矩阵 𝐴 𝐴 A的每一个列向量都垂直

设：

A = $a 1 , a 2 , \dots , a n$ \begin{equation*} A = $a_1, a_2, \\cdots, a_n$ \end{equation*} A= $a1,a2,\dots,an$

𝑎 𝑖 𝑎_𝑖 ai表示矩阵 A A A的第 i i i个列向量

则：

a i T ( b − A x ∗ ) = 0 \begin{equation*} a_i^T (b - A x^*) = 0 \end{equation*} aiT(b−Ax∗)=0

可得：

A T ( b − A x ∗ ) = 0 A T b = A T A x ∗ x ∗ = ( A T A ) − 1 A T b \begin{align*} A^T (b - A x^*) &= 0 \\ A^T b &= A^T A x^* \\ x^* &= (A^T A)^{-1} A^T b \end{align*} AT(b−Ax∗)ATbx∗=0=ATAx∗=(ATA)−1ATb