里程计运动模型及标定
里程计运动模型
1.两轮差速底盘的运动学模型
- 优点:结构简单,便宜(2个电机),模型简单
- 差分模型:
- 欠驱动系统:运动耦合
- 圆弧运动:左轮、右轮和底盘中心都在一条射线上,这三个的角速度是一样的。
- 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(vR−vL)
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=0∗vxV2=−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=0∗vx+1∗vy+d∗vθV2=−sin60∗vx−cos60∗vy+d∗vθV3=sin60∗vx−cos60∗vy+d∗vθ
- [ 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⎦ ⎤=⎣ ⎡0−sin60sin601−cos60−cos60ddd⎦ ⎤⎣ ⎡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θ⎦ ⎤=⎣ ⎡0323d1−33−313d133−313d1⎦ ⎤⎣ ⎡V1V2V3⎦ ⎤
- 运动分解–纯平移X:
3.航迹推算(Dead Reckoning)
-
示意图:
-
递推公式:
- ( 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,vy和vθ各自乘以 δ 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θ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 θ + ε θ ] \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θ0−sinθcosθ0001⎦ ⎤⎣ ⎡dx+εxdy+εydθ+εθ⎦ ⎤
里程计标定(系统误差标定)
1.线性最小二乘的基本原理
- 线性方程组Ax=b,A是mxn,x是nx1:
- 当m=n时,适定方程组,有唯一解;
- 当m<n时,欠定方程组,无穷多解;
- 当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*) (b−Ax∗)垂直于空间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(b−Ax∗)=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∗=X∗ui
具体代码实现有时间再附上吧