轮式里程计模型及标定
内容基于深蓝学院课件,如有错误,烦请斧正,不胜感激。
1.模型
1.1 两轮差分底盘的运动学模型(以下简称运动模型)
1.1.1 目的
通过已知量推出未知量。
已知量
两轮角速度:
w
L
,
w
R
w_L,w_R
wL,wR
两轮线速度:
v
L
,
v
R
v_L,v_R
vL,vR
轮子离底盘中心的距离:
d
d
d
两轮之间的距离:
b
=
2
d
b=2d
b=2d
未知量
底盘中心的线速度:
v
v
v
底盘中心的角速度:
w
w
w
底盘中心圆弧运动的半径:
r
r
r
图
1.1.2 推导
首先明确,左右轮子的角速度是相同的,即
w
L
=
w
R
w
=
w
l
=
w
r
w_L=w_R\\ w=w_l=w_r
wL=wRw=wl=wr
其次明确,线速度是沟通大圆周(整个运动模型转圈)和小圆周(轮子转圈)的桥梁,即
v
L
=
w
l
∗
(
r
−
d
)
=
w
L
∗
r
L
v
R
=
w
r
∗
(
r
+
d
)
=
w
R
∗
r
R
v_L=w_l*(r-d)=w_L*r_L\\ v_R=w_r*(r+d)=w_R*r_R
vL=wl∗(r−d)=wL∗rLvR=wr∗(r+d)=wR∗rR
r r r
w w w
v v v
向量表示
1.2 航迹推算
递推公式
图
2.标定
2.1 线性最小二乘的基本原理
2.1.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
m
m表示约束个数,
n
n
n表示自变量个数。
- 当 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^TA)^{-1}A^Tb
x∗=(ATA)−1ATb
下面会推导通解是怎么来的。
2.1.2 最小二乘求解
从线性空间的角度看最小二乘求解。
S
S
S表示
A
A
A的列向量张成的线性空间。
- 无解:表示 A x = b Ax=b Ax=b对于任意的 x x x均不成立,即 b b b不在 S S S中
- 最小二乘解:线性空间 S S S中,离 b b b最近的向量
设:
A
x
∗
Ax^*
Ax∗为向量
b
b
b在空间
S
S
S中的投影,显然
(
b
−
A
x
∗
)
(b-Ax^*)
(b−Ax∗)垂直于空间
S
S
S。
则:
(
b
−
A
x
∗
)
(b-Ax^*)
(b−Ax∗)跟矩阵A中的每一个列向量都垂直。
令
A
=
[
a
1
,
a
2
,
…
…
,
a
n
]
,
a
i
A=[a_1,a_2,……,a_n],a_i
A=[a1,a2,……,an],ai表示矩阵
A
A
A的第
i
i
i个列向量,
可得
2.2 线性最小二乘的直线拟合
直线拟合
2.3 线性最小二乘在里程计标定中的应用
2.3.1 直接线性方法(通用、黑盒)
2.3.2 基于模型的方法(定制、白盒)
根据运动模型有
里程计的积分如下
假设
假设在标定的
Δ
t
Δt
Δt时间内匀速运动,则
w
(
t
)
=
w
=
J
21
w
L
+
J
22
w
R
v
(
t
)
=
v
=
J
11
w
L
+
J
12
w
R
w(t)=w=J_{21}w_L+J_{22}w_R\\ v(t)=v=J_{11}w_L+J_{12}w_R
w(t)=w=J21wL+J22wRv(t)=v=J11wL+J12wR
因为
J
11
=
−
b
2
∗
J
21
,
J
12
=
b
2
∗
J
22
J_{11}=-\frac{b}{2}*J_{21},J_{12}=\frac{b}{2}*J_{22}
J11=−2b∗J21,J12=2b∗J22,
所以
v
(
t
)
=
v
=
b
2
(
−
J
21
w
L
+
J
22
w
R
)
v(t)=v=\frac{b}{2}(-J_{21}w_L+J_{22}w_R)
v(t)=v=2b(−J21wL+J22wR)。
因此,已知量
w
L
,
w
R
w_L,w_R
wL,wR,未知量
b
,
r
L
,
r
R
,
v
b,r_L,r_R,v
b,rL,rR,v。
未知量求解顺序
b
b
b→
r
L
,
r
R
r_L,r_R
rL,rR→
v
v
v。
假设
- 激光雷达位于车体的正中心
- 激光雷达的匹配值作为观测值
- 里程计的积分值作为预测值
通过最小化预测值和观测值的差,即可得到里程计的参数。
符号及推导
符号
- 里程计的积分值: r x , r y , r θ r_x,r_y,r_\theta rx,ry,rθ
- 激光雷达的匹配值: S x , S y , S θ S_x,S_y,S_\theta Sx,Sy,Sθ
推导
(
θ
(
t
)
\theta(t)
θ(t)为运动模型在时刻
t
t
t与世界坐标系
x
x
x轴的夹角)
总结
收集 n n n段数据,每段数据包含两个轮子的角速度 w L w_L wL和 w R w_R wR,该段数据持续的时间为 Δ t Δt Δt以及激光雷达的匹配值为 S x , S y , S θ S_x,S_y,S_\theta Sx,Sy,Sθ。
- 按照公式1,计算中间变量 J 21 J_{21} J21和 J 22 J_{22} J22
- 按照公式2,计算轮间距 b b b
- 按照公式3,计算两个轮子的半径 r L , r R r_L,r_R rL,rR