里程计标定：直接线性方法
里程计标定：直接线性方法
通过线性最小二乘的方式标定里程计信息，使其提高精度。
航迹推算
已知里程计信息，上一帧底盘位姿和当前底盘位姿：
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
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)=Xui
功能实现
首先需要安装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区别来判断此次矫正的效果。