Tao's Blog
0%

lidarSLAM-第二次作业

Posted on Edited on In Comment Counts:
5.7k 5 mins.

本文是我在学习深蓝学院的激光slam课程第二次作业的笔记。本次作业前半部分是通过代码实践熟悉里程计不同标定方法和看下实现的效果；第三题是关于线性方程组不同解法的总结；第四题是扩展题，考虑一下怎么设计里程计和激光雷达外参标定方法。

直接线性方法的里程计标定的模块

这一题老师提供了一个 ROS 的工作空间以及一个 odom_calib 的 package，大致代码框架已经写好，只需要补充一下几个关键函数。

代码结构

首先大致了解一下下代码框架，src文件夹里面有一个main.cpp用来初始化节点以及做各个topic之间的数据传递用，odom_calib.cpp和odom_calib.hpp是用来校正里程计数据的类。首先通过main.cpp来了解大致涉及到的 node 、 topic以及大致的工作流程。

工作流程

可以看到文件一开始一个初始化了一个odom_calib对象用来后面校正用，main函数里定义了一个scan2，大部分的工作由这个对象完成。scan2的构造函数里面声明了一些关于 PI-ICP 的变量和函数，这些这次作业先不用处理，后续地学习会涉及到，这次主要两个 subscriber 和 3 个 publisher。

  • scan_filter_sub_ ： 订阅激光 scan的主题并和里程计数据通过message_filters做了同步，所以后面不需要考虑时间问题，每当接收到一个激光扫描信息时，计算前后两帧激光数据的位姿差（这一部分本次作业不涉及）。同时获取最近时间的两个里程计数据点，通过里程计数据也计算一个位姿差（通过cal_delta_distence()计算）。这样就可以产生两条路径分别通过两个publisher发布到相应的topic，同时每次更新数据的时候会把相应的激光和里程计数据存进odom_calib对象里。

  • calib_flag_sub_ : 通过订阅一个校正的 topic 通过odom_calib启动校正，校正完成后将校正之后的数据发布到相应的publisher上。

  • odom_path_pub_,scan_path_pub_,calib_path_pub_：分别发布里程计，激光数据，校正之后的位姿数据。

OdomCalib类

OdomCalib 类内部比较简单：

  • set_data_len()设置数据的最大长度
  • set_data_zero()将存放数据的数组清零
  • is_full()判断数据有没有存满
  • add_data()添加新的数据到A和b
  • solve()对存好的数据进行校正，最后返回一个 3x3 的校正矩阵。

这次作业主要是补充一下main.cpp里面的cal_delta_distance()函数以及OdomCalib类里面的add_data()函数以及solve()函数。

代码补充

  • Eigen::Vector3d cal_delta_distance(Eigen::Vector3d odom_pose)

这里输入参数是当前的位姿，要计算的是两帧之间的位姿差，所以要求的是当前位姿在上一时间点的位姿，主要涉及坐标转换，具体代码部分如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
//求解得到两帧数据之间的位姿差
//即求解当前位姿 在 上一时刻 坐标系中的坐标
Eigen::Vector3d  cal_delta_distence(Eigen::Vector3d odom_pose)
{

    //TODO:
    Eigen::Vector3d d_pos;  //return value
    now_pos = odom_pose;

    // 上一时刻的位姿矩阵(在世界坐标系下)
    Eigen::Matrix3d T_world_last;
    T_world_last << cos(last_pos(2)), -sin(last_pos(2)), last_pos(0),
                    sin(last_pos(2)),  cos(last_pos(2)), last_pos(1),
                    0,  0,  1;

    // 这一时刻的位姿矩阵(在世界坐标系下)
    Eigen::Matrix3d T_world_now;
    T_world_now << cos(now_pos(2)), -sin(now_pos(2)), now_pos(0),
                    sin(now_pos(2)),  cos(now_pos(2)), now_pos(1),
                    0,  0,  1;

    // T_AB = (T_OA)^(-1)*T_(OB)
    Eigen::Matrix3d T_last_world = T_world_last.inverse();
    Eigen::Matrix3d T_last_now = T_last_world * T_world_now;

    d_pos << T_last_now(0, 2), T_last_now(1, 2),
            atan2(T_last_now(1, 0), T_last_now(0, 0));

    return d_pos;
}

通过计算位姿差之后可以分别输出用前后两帧 odom 和 scan 产生的路径，如下图所示，可以发现两者有比较的偏移：

  • bool Add_Data(Eigen::Vector3d Odom,Eigen::Vector3d scan)

添加校正数据比较简单，基本按照课上的讲解将 odom 数据放入A矩阵，scan 数据放入b矩阵即可，具体代码如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
/*
 * 输入:里程计和激光数据
 * 构建最小二乘需要的超定方程组
 * Ax = b
 */
bool OdomCalib::Add_Data(Eigen::Vector3d Odom,Eigen::Vector3d scan)
{

    if(now_len<INT_MAX)
    {
        //TODO: 构建超定方程组
        A(now_len * 3, 0) = Odom(0);
        A(now_len * 3, 1) = Odom(1);
        A(now_len * 3, 2) = Odom(2);
        A(now_len * 3 + 1, 3) = Odom(0);
        A(now_len * 3 + 1, 4) = Odom(1);
        A(now_len * 3 + 1, 5) = Odom(2);
        A(now_len * 3 + 2, 6) = Odom(0);
        A(now_len * 3 + 2, 7) = Odom(1);
        A(now_len * 3 + 2, 8) = Odom(2);

        b(now_len * 3, 0) = scan(0);
        b(now_len * 3 + 1, 0) = scan(1);
        b(now_len * 3 + 2, 0) = scan(2); 
        //end of TODO
        now_len++;
        return true;
    }
    else
    {
        return false;
    }
}
  • Eigen::Matrix3d Solve()

这一部分主要就是通过求解 Ax=b 求出对 odom 数据的校正矩阵，因为是直接线性方法所以不需要考虑模型，ROS 中有很多求解工具，这里用的是 QR 分解的一种，具体各种求解方法放在第三题统一讨论，具体代码如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
/*
 * 求解线性最小二乘Ax=b
 * 返回得到的矫正矩阵
 */
Eigen::Matrix3d OdomCalib::Solve()
{
    Eigen::Matrix3d correct_matrix;

    Eigen::VectorXd correct_vector(9);
    correct_vector = A.colPivHouseholderQr().solve(b);
    correct_matrix << correct_vector(0),correct_vector(1),correct_vector(2),
                        correct_vector(3),correct_vector(4),correct_vector(5),
                        correct_vector(6),correct_vector(7),correct_vector(8);

    return correct_matrix;
}

校正之后的数据如下：

1
2
3
4
5
6
#输出
correct_matrix:
  0.936194    1.81675 -0.0432266
 -0.012844    7.20126   0.142539
0.00144064    23.4053   0.378674
calibration over!!!!

可以发现，将 scan 的结果作为真实值的话，经过用直接线性方法校正之后的 odom 结果明显准确很多。

基于模型方法的里程计设计模块

这一部分老师也给出了大致的代码框架，只需要按照课程上的推导结果填入系数矩阵求解即可，推导过程见这里，代码结构也比较简单，大致是读取预先存好的 scan 数据和 odom 数据，然后通过积分每两帧 scan 数据之间的轮速计数据来求角速度，接下来就是利用这些数据求机器人的$J_{21}$, $J_{22}$, $b$, $r_L$, $r_R$ 参数，中间补充的代码如下：

1
2
3
4
5
6
// 填充A, b矩阵
//TODO: (3~5 lines)
A(id_s, 0) = w_Lt;
A(id_s, 1) = w_Rt;
b(id_s) = s_th;
//end of TODO
1
2
3
4
// 进行最小二乘求解
Eigen::Vector2d J21J22;
J21J22 = (A.transpose() * A).inverse() * A.transpose() * b;
//TODO: (1~2 lines)
1
2
3
4
5
6
7
// 填充C, S矩阵
//TODO: (4~5 lines)
C(id_s * 2) = cx;
C(id_s * 2 + 1) = cy;
S(id_s * 2) = s_x;
S(id_s * 2 + 1) = s_y;
//end of TODO
1
2
3
4
5
//TODO: (3~5 lines)
b_wheel = (C.transpose() * C).inverse() * C.transpose() * S;
r_L = -J21 * b_wheel;
r_R = J22 * b_wheel;
//end of TODO

结果如下：

1
2
3
4
5
6
7
./odom_calib/odom_calib
J21: -0.163886
J22: 0.170575
b: 0.59796
r_L: 0.0979974
r_R: 0.101997
参考答案：轮间距b为0.6m左右，两轮半径为0.1m左右

线性方程组 $Ax = b$ 的不同解法

这一部分网上已经有很多关于推导以及优缺点的资料，这里主要是做一个总结整理。

从高斯消元法到 LU 分解 和 Cholesky 分解

高斯消元法是我们比较熟悉的解线性方程组的方法，而 LU 分解则是把列主元的高斯消元法用矩阵变换的思路表示出来，具体推导过程看这里，具体方法是利用以下变换：

其中 P 是置换矩阵， L 是单位下三角矩阵， U 是上三角矩阵，通过这个方法求解 $Ax = b$ 可以转换为 $P(Ax) = P(b) \rightarrow LUx = Pb$， 令$y = Ux$，简单可以求出 $Ly = Pb$ 的解，再进而求出 $Ux = y$ 的解。

对于正定矩阵，还能通过 Cholesky 分解减少一点计算量，具体分解公式如下：

还能进一步分解为以下形式，其中 $\tilde{L}$ 为单位下三角阵， $\tilde{D}$ 为主对角线为正值的对角阵，这个分解也被称为矩阵 A 的 LDL 分解：

其中 A 要求是 正定矩阵， L 是主对角元素都是正值的小三角矩阵，利用分解公式求解线性方程组和上述方法类似。

从正交化到 QR 分解

正交化意味着将矩阵变换成一个正交矩阵和三角矩阵的乘积，具体形式如下：

其中， Q 为正交矩阵， R 为上三角阵，通过这种方法也可以求解线性方程组，具体方式同上，区别在于求解 $Qy = b$ 的过程中，由于 Q 是正交阵，所以可以通过转置求出他的逆进而求解方程组，QR 分解主要有基于 Gram-Schmidt 和 Householder 两种正交化方法，具体推导过程和原理同样可以参考这篇文章。

SVD 分解

除了 QR 分解以外，还可以将矩阵 A 分解成两个正交矩阵和一个对角矩阵的乘积，具体形式如下：

设 $A \in R^{m\times n}$, $rank(A) = r$, $(r > 0)$，则 $A$ 可以分解成以下形式：

其中， $U$ 和 $V$ 分别为 m 和 n 阶的正交矩阵，$\Sigma$ 为对角矩阵，其中前 r 个元素为A 的奇异值，后面对角线元素为0，这称为矩阵 A 的 SVD 分解。通过这种方法也能求解线性方程组的解，具体解的个数和 A 的秩有关，具体推导过程见这篇博客。

性能比较

我们在实际过程中大部分只在乎求解的速度、稳定性和准确性，下面对 Eigen 里面的各种求法做一个归纳，具体参考自 Eigen 官方文档。

设计里程计和激光雷达外参标定方法（思路）

我们一般把传感器内自身要调节的参数称为内参,比如前面作业中里程计模型的两轮间距与两个轮子的半径。把传感器之间的信息称为外参,比如里程计与激光雷达之间的时间延迟,位姿变换等。

这里的主要思路找到一些想要标定的外参，整理出一个时间点的等式，这样通过列举多个时间点的等式就可以构建超定方程组从而进行标定，比如我们如果想要标定传感器之间的位姿关系，可以有以下思路。

首先假设里程计和激光雷达的内参都已经标定好，忽略时间同步的问题，在每一时刻，我们可以通过里程计和激光雷达分别计算出一个位姿。以激光雷达求出的位姿为标定观测值，里程计求出的位姿为预测值。同时假设激光雷达和里程计之间是刚性连接（位姿关系不变），那么每一个时间段可以有以下等式：

通过这个等式以及后续的变换可以构建超定方程组求解，具体的关于标定方面的知识可以参阅这篇论文 Simultaneous Calibration of Odometry and SensorParameters for Mobile Robots。

参考