里程计标定：直接线性方法
通过线性最小二乘的方式标定里程计信息，使其提高精度。
航迹推算
已知里程计信息，上一帧底盘位姿和当前底盘位姿：
l
a
s
t
_
p
o
s
e
=
[
x
l
a
s
t
y
l
a
s
t
θ
l
a
s
t
]
n
o
w
_
p
o
s
e
=
[
x
n
o
w
y
n
o
w
θ
n
o
w
]
last\_pose=\begin{bmatrix} x_{last}\\y_{last}\\\theta_{last} \end{bmatrix}\quad\quad now\_pose=\begin{bmatrix} x_{now}\\y_{now}\\\theta_{now} \end{bmatrix}
last_pose=⎣⎡xlastylastθlast⎦⎤now_pose=⎣⎡xnowynowθnow⎦⎤
则两帧之间具有如下等式成立：
n
o
w
_
p
o
s
e
=
l
a
s
t
_
p
o
s
e
+
R
⋅
[
d
x
d
y
d
θ
]
now\_pose=last\_pose+R\cdot\begin{bmatrix}dx\\dy\\d\theta\end{bmatrix}
now_pose=last_pose+R⋅⎣⎡dxdydθ⎦⎤
其中，旋转矩阵R计算公式如下：
R
=
[
cos
θ
n
o
w
−
sin
θ
n
o
w
0
sin
θ
n
o
w
cos
θ
n
o
w
0
0
0
1
]
R = \begin{bmatrix} \cos\theta_{now}&-\sin\theta_{now}&0\\ \sin\theta_{now}&\cos\theta_{now}&0\\ 0&0&1 \end{bmatrix}
R=⎣⎡cosθnowsinθnow0−sinθnowcosθnow0001⎦⎤
有时，由于里程计存在噪音，会对运动学解算增量加上噪声估计
ϵ
\epsilon
ϵ
[
x
n
o
w
y
n
o
w
θ
n
o
w
]
=
[
x
l
a
s
t
y
l
a
s
t
θ
l
a
s
t
]
+
[
cos
θ
n
o
w
−
sin
θ
n
o
w
0
sin
θ
n
o
w
cos
θ
n
o
w
0
0
0
1
]
⋅
[
d
x
+
ϵ
x
d
y
+
ϵ
y
d
θ
+
ϵ
θ
]
\begin{bmatrix} x_{now}\\y_{now}\\\theta_{now} \end{bmatrix}= \begin{bmatrix} x_{last}\\y_{last}\\\theta_{last} \end{bmatrix}+ \begin{bmatrix} \cos\theta_{now}&-\sin\theta_{now}&0\\ \sin\theta_{now}&\cos\theta_{now}&0\\ 0&0&1 \end{bmatrix}\cdot \begin{bmatrix}dx+\epsilon_x\\dy+\epsilon_y\\d\theta+\epsilon_\theta\end{bmatrix}
⎣⎡xnowynowθnow⎦⎤=⎣⎡xlastylastθlast⎦⎤+⎣⎡cosθnowsinθnow0−sinθnowcosθnow0001⎦⎤⋅⎣⎡dx+ϵxdy+ϵydθ+ϵθ⎦⎤
线性最小二乘
1.代数意义
对于一个线性方程组 A x = b Ax=b Ax=b，其中 A A A为 m × n m\times n m×n的矩阵， x x x为 n × 1 n\times 1 n×1的向量：
- 当 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.几何意义
从线性空间角度看， A x Ax Ax表示 A A A的列向量空间 S S S。
方程组无解即表面向量 b b b不在空间 S S S中，最小二乘解就表示向量 b b b在空间 S S S中的投影。
设向量
b
b
b在空间
S
S
S中的投影为
A
x
∗
Ax^*
Ax∗，则：
(
b
−
A
x
∗
)
⊥
S
(b-Ax^*)\bot S
(b−Ax∗)⊥S
即，向量
(
b
−
A
x
∗
)
(b-Ax^*)
(b−Ax∗)垂直于矩阵
A
A
A的每一个列向量。
设矩阵
A
=
[
a
1
a
2
…
a
n
]
A=\begin{bmatrix}a_1 & a_2 &…&a_n\end{bmatrix}
A=[a1a2…an]，其中
a
i
a_i
ai表示矩阵第
i
i
i个列向量，则：
a
i
T
(
b
−
A
x
∗
)
=
0
即
A
T
(
b
−
A
x
∗
)
=
0
A
T
b
=
A
T
A
x
∗
a^T_i(b-Ax^*)=0\\ 即A^T(b-Ax^*)=0\\ A^Tb=A^TAx^*\\
aiT(b−Ax∗)=0即AT(b−Ax∗)=0ATb=ATAx∗
对上式化简得到最小二乘通解：
x
∗
=
(
A
T
A
)
−
1
A
T
b
x^*=(A^TA)^{-1}A^Tb
x∗=(ATA)−1ATb
里程计标定
通过激光雷达scan-match信息，获取到观测值 u i ∗ u^*_i ui∗
通过里程计测量，获得预测值 u i u_i ui
假设两者成线性关系：
u
i
∗
=
X
⋅
u
i
u_i^*=X\cdot u_i
ui∗=X⋅ui，即：
[
u
i
x
∗
u
i
y
∗
u
i
θ
∗
]
=
[
x
11
x
12
x
13
x
21
x
22
x
23
x
31
x
32
x
33
]
[
u
i
x
u
i
y
u
i
θ
]
\begin{bmatrix}u_{ix}^*\\u_{iy}^*\\u_{i\theta}^*\end{bmatrix}= \begin{bmatrix} x_{11} & x_{12} & x_{13}\\ x_{21} & x_{22} & x_{23}\\ x_{31} & x_{32} & x_{33} \end{bmatrix} \begin{bmatrix}u_{ix}\\u_{iy}\\u_{i\theta}\end{bmatrix}
⎣⎡uix∗uiy∗uiθ∗⎦⎤=⎣⎡x11x21x31x12x22x32x13x23x33⎦⎤⎣⎡uixuiyuiθ⎦⎤
将矩阵
X
X
X由
3
×
3
3\times3
3×3转为
9
×
1
9\times1
9×1的向量，得到如下方程组：
[
u
i
x
u
i
y
u
i
θ
0
0
0
0
0
0
0
0
0
u
i
x
u
i
y
u
i
θ
0
0
0
0
0
0
0
0
0
u
i
x
u
i
y
u
i
θ
]
[
x
11
⋅
⋅
⋅
x
33
]
=
[
u
i
x
∗
u
i
y
∗
u
i
θ
∗
]
\begin{bmatrix} u_{ix}&u_{iy}&u_{i\theta}&0&0&0&0&0&0\\ 0&0&0&u_{ix}&u_{iy}&u_{i\theta}&0&0&0\\ 0&0&0&0&0&0&u_{ix}&u_{iy}&u_{i\theta}\\ \end{bmatrix} \begin{bmatrix}x_{11}\\\cdot\\\cdot\\\cdot\\x_{33}\end{bmatrix}= \begin{bmatrix}u_{ix}^*\\u_{iy}^*\\u_{i\theta}^*\end{bmatrix}
⎣⎡uix00uiy00uiθ000uix00uiy00uiθ000uix00uiy00uiθ⎦⎤⎣⎢⎢⎢⎢⎡x11⋅⋅⋅x33⎦⎥⎥⎥⎥⎤=⎣⎡uix∗uiy∗uiθ∗⎦⎤
即对每一帧的观测数据，可以得到一个方程组：
A
i
X
→
=
b
i
其
中
A
i
=
[
u
i
x
u
i
y
u
i
θ
0
0
0
0
0
0
0
0
0
u
i
x
u
i
y
u
i
θ
0
0
0
0
0
0
0
0
0
u
i
x
u
i
y
u
i
θ
]
X
→
=
[
x
11
⋅
⋅
⋅
x
33
]
b
i
=
[
u
i
x
∗
u
i
y
∗
u
i
θ
∗
]
A_i\overrightarrow{X}=b_i\\ 其中A_i=\begin{bmatrix} u_{ix}&u_{iy}&u_{i\theta}&0&0&0&0&0&0\\ 0&0&0&u_{ix}&u_{iy}&u_{i\theta}&0&0&0\\ 0&0&0&0&0&0&u_{ix}&u_{iy}&u_{i\theta}\\ \end{bmatrix}\\ \overrightarrow{X}=\begin{bmatrix}x_{11}\\\cdot\\\cdot\\\cdot\\x_{33}\end{bmatrix}\\ b_i=\begin{bmatrix}u_{ix}^*\\u_{iy}^*\\u_{i\theta}^*\end{bmatrix}
AiX
=bi其中Ai=⎣⎡uix00uiy00uiθ000uix00uiy00uiθ000uix00uiy00uiθ⎦⎤X
=⎣⎢⎢⎢⎢⎡x11⋅⋅⋅x33⎦⎥⎥⎥⎥⎤bi=⎣⎡uix∗uiy∗uiθ∗⎦⎤
对一段时间内的数据，可以得到：
A
=
A
1
⋅
⋅
⋅
A
n
b
=
b
1
⋅
⋅
⋅
b
n
A=\begin{matrix}A_1\\\cdot\\\cdot\\\cdot\\A_n\end{matrix}\quad\quad\quad b=\begin{matrix}b_1\\\cdot\\\cdot\\\cdot\\b_n\end{matrix}
A=A1⋅⋅⋅Anb=b1⋅⋅⋅bn
由此，可以计算出最小二乘解：
X
→
=
(
A
T
A
)
−
1
A
T
b
\overrightarrow{X}=(A^TA)^{-1}A^Tb
X
=(ATA)−1ATb
在获得到最小二乘解后，用其对里程计进行矫正，可以得到矫正后的数据：
u
i
(
c
o
r
r
)
=
X
→
u
i
u_i^{(corr)}=\overrightarrow{X}u_i
ui(corr)=X
ui
功能实现
首先需要安装PL-ICP的环境
sudo apt install ros-melodic-csm
1.构建超定方程组
改函数用于获取里程计信息及观测值，并构建超定方程组。
函数位于Odom_Calib.cpp中Add_Data()函数
/*
* 输入:里程计和激光数据
* 构建最小二乘需要的超定方程组
* Ax = b
* */
bool OdomCalib::Add_Data(Eigen::Vector3d Odom,Eigen::Vector3d scan)
{
if(now_len<INT_MAX)
{
/*
* 三行九列浮点型矩阵
* u_ix u_iy u_iθ 0 0 0 0 0 0
* 0 0 0 u_ix u_iy u_iθ 0 0 0
* 0 0 0 0 0 0 u_ix u_iy u_iθ
* */
Eigen::Matrix<double, 3, 9> A_i;
/*
* 三行一列浮点型矩阵，接收观测值scan
* u_ix^*
* u_iy^*
* u_iθ^*
* */
Eigen::Matrix<double, 3, 1> b_i;
// 初始化置零
A_i.setZero();
b_i.setZero();
// 对b_i赋值，获取观测数据
b_i = scan;
// A_i赋值，获取里程计数据
for(int i = 0; i < 3; i++)
{
// 一行九列矩阵，初始置零
Eigen::Matrix<double, 1, 9> tmp;
tmp.setZero();
/*
* 从第0行第i*3列开始，定义一个1行3列的块，用于存储里程计信息
* 第0行取第 0 1 2 个元素
* 第1行取第 3 4 5 个元素
* 第2行取第 6 7 8 个元素
* */
tmp.block<1, 3>(0, i*3) = Odom.transpose();
// 按行赋值
A_i.block<1, 9>(i, 0) = tmp;
}
// 将A_i、b_i置于A、b中
A.block<3, 9>(now_len * 3.0) = A_i;
b.block<3, 1>(now_len * 3.0) = b_i;
now_len++;
return true;
}
else
{
return false;
}
}
2.求解最小二乘解
该函数用于求解超定方程组的最小二乘解并返回矫正矩阵 X X X
函数位于Odom_Calib.cpp中的Solve()
/*
* 求解线性最小二乘Ax=b
* 线性最小二乘通解：x^*=(A^T A)^{-1} A^T b
* A_tmp * x = b_tmp
* 返回得到的矫正矩阵
*/
Eigen::Matrix3d OdomCalib::Solve()
{
// 矫正矩阵
Eigen::Matrix3d correct_matrix;
// 存储通解
Eigen::Matrix<double, 9, 1> x;
// 存储 A^T A
Eigen::MatrixXd A_tmp;
// 存储 A^T b
Eigen::MatrixXd b_tmp;
// 计算
A_tmp = A.transpose() * A;
b_tmp = A.transpose() * b;
// LDLT分解
x = A_Tmp.ldlt().solve;
// 赋值，返回
correct_matrix << x(0), x(1), x(2),
x(3), x(4), x(5),
x(6), x(7), x(8);
return correct_matrix;
}
3.位姿变换
改函数用于求解当前时刻机器人位姿在上一帧中坐标系下的坐标。即用于得到两帧数据之间的位姿差
函数位于main.cpp中 cal_delta_distance()
/*
* 求解得到两帧数据之间的位姿差
* 即求解当前位姿 在 上一时刻 坐标系中的坐标
* now_pos = last_pos + R * d_pos
* */
Eigen::Vector3d cal_delta_distance(Eigen::Vector3d odom_pose)
{
/*
* 两帧时刻 机器人的位姿 三行一列向量
* X
* Y
* θ
* */
static Eigen::Vector3d now_pos,last_pos;
/*
* 返回值，位姿差 三行一列向量
* dx
* dy
* dθ
* */
Eigen::Vector3d d_pos;
// 获取当前位姿
now_pos = odom_pose;
// 航迹推算公式
Eigen::Matrix3d R;
R << cos(now_pos(2)), -sin(now_pos(2)), 0,
sin(now_pos(2)), cos(now_pos(2)), 0,
0 , 0 , 1;
// 解算
d_pos = R.inverse() * (now_pos - last_pos);
// 更新数据
last_pos = now_pos;
return d_pos;
}
4.程序运行方式
本程序参考深蓝学院课程进行编写，源码下载地址如下：
下载程序并编译成功后，在odom_ws下进行source
source devel/setup.bash
随后运行launch文件
roslaunch calib_odom odomCalib.launch
开启rviz并添加话题：
- Fix_frame选择为odom_frame
- Add选项卡订阅Path信息：
- 原始里程计topic：odom_path_pub_
- 激光雷达topic：scan_path_pub_
- 标定里程计topic：calib_path_pub_
- 分别选择不同的颜色展示即可
然后，进入到odom_ws/bag目录下播放里程计信息
rosbag play –clock odom.bag
如果一切正常，则能看到运行矫正程序的终端会打印数据，并且rviz中可以看到两条路径。当打印的数据到达一个的数量之后，则可以开始矫正。矫正的命令如下：
rostopic pub /calib_flag std_msgs/Empty "{}"
程序矫正完毕会输出对应的矫正矩阵，并且会在rviz中显示出第三条路径，即calib_path。可以观察里程计路径odom_path和矫正路径calib_path区别来判断此次矫正的效果。