深蓝学院激光slam理论与实践 -第二章（里程计标定）作业

灰太狼 2022-08-29 06:15 227阅读 0赞

# 直接法： #

### 1. 作业描述： ###

1.  本次的作业为用直接线性方法来对机器人的里程计进行校正。
2.  本次作业使用的bag数据，路径为odom\_ws/bag/odom.bag（太大了，没上传到github）。
3.  本次的作业中，需要实现三个函数，分别为：

*   main.cpp，第340行中的cal\_delta\_distance()函数，该函数的功能为给定两个里程计位姿，计算这两个位姿之间的位姿差。
 *   Odom\_Calib.cpp，第23行Add\_Data()函数，该函数的功能为构建超定方程组Ax=b，具体参考PPT。
 *   Odom\_Calib.cpp，第44行 Solve()函数，该函数的功能为对2中构建的超定方程组进行求解。

### 2. 本次程序的运行过程为： ###

1.  运行launch文件:roslaunch calib\_odom odomCalib.launch。
2.  在 1 正常的情况下，运行rviz，fix\_frame选择为odom\_frame。在Add选项卡中增加三条Path消息。一条订阅的topic为：odom\_path\_pub\_;一条订阅的topic为:scan\_path\_pub\_；最后一条为:calib\_path\_pub\_。分别选择不同的颜色。
3.  进入到odom\_ws/bag目录下，运行指令：rosbag play - -clock odom.bag。(双横线中间不空格)
4.  如果一切正常，则能看到运行矫正程序的终端会打印数据，并且rviz中可以看到两条路径。当打印的数据到达一个的数量之后，则可以开始矫正。
5.  矫正的命令为，在calib\_flag的topic下发布一个数据：rostopic pub /calib\_flag std\_msgs/Empty “\{\}”。
6.  程序矫正完毕会输出对应的矫正矩阵，并且会在rviz中显示出第三条路径，即calib\_path。可以观察里程计路径odom\_path和矫正路径\_calib\_path区别来判断此次矫正的效果。

### 3. 结果 ###

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70][]

### 4. 标定结果： ###

correct_matrix:
        0.943991     0.133506 -0.000139669
      -0.0142447      13.4325  0.000335469
     0.000152577      40.2952  0.000234346
    calibration over!!!!

### 5. 代码： ###

cal\_delta\_distence函数

//求解得到两帧数据之间的位姿差
    //即求解当前位姿　在　上一时刻　坐标系中的坐标
    Eigen::Vector3d cal_delta_distence(Eigen::Vector3d odom_pose)
    {
    
        Eigen::Vector3d d_pos; //return value
        now_pos = odom_pose;
        //TODO:
        //向量旋转的方方式，轴角
        d_pos = now_pos - last_pos;
        Eigen::AngleAxisd temp(last_pos(2),Eigen::Vector3d(0,0,1));
        Eigen::Matrix3d trans=temp.matrix().inverse();
        d_pos = trans*d_pos;
        last_pos = now_pos; 
    //方法二矩阵的方式
    //    Eigen::Matrix3d trans_temp;
    //    trans_temp<< cos(last_pos(2)), -sin(last_pos(2)), 0,
    //            sin(last_pos(2)), cos(last_pos(2)),  0,
    //            0,                           0,                       1;
    //
    //    d_pos = now_pos - last_pos;
    //    d_pos = trans_temp.transpose()*d_pos;
    //    last_pos = now_pos;
    
        //end of TODO:
        return d_pos;
    }

Add\_Data() 函数

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 1][]

构建最小二乘需要的超定方程组
    Ax = b
    
    */
    bool OdomCalib::Add_Data(Eigen::Vector3d Odom, Eigen::Vector3d scan)
    {
    
        if (now_len < INT_MAX) {
            //TODO: 构建超定方程组
    
            Eigen::Matrix<double,3,9> trans_a;
            Eigen::Vector3d vector__b;
            trans_a << Odom(0), Odom(1),Odom(2),0,0,0,0,0,0,
                    0,0,0,Odom(0), Odom(1),Odom(2),0,0,0,
                    0,0,0,0,0,0,Odom(0), Odom(1),Odom(2);
            vector__b << scan(0),scan(1),scan(2);
    
            // 给到A和B
            A.block<3,9>(3*now_len,0) = trans_a;
            b.block<3,1>(3*now_len,0) = vector__b;
            //end of TODO
            now_len++;
            return true;
        } else {
            return false;
        }
    }

Solve函数，一般用矩阵分解的方式，可以选svd或QR分解等不同方式，结果差别不大。

Eigen::Matrix3d OdomCalib::Solve()
    {
        Eigen::Matrix3d correct_matrix;
    
        //TODO: 求解线性最小二乘
        // Eigen::Matrix<double, 9, 1> params;
        // // params = (A.transpose() * A).inverse() * A.transpose() * b;
        // params = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
        // std::cout << params.transpose() << std::endl;
        // for (size_t i = 0; i < 3; i++) {
        //     correct_matrix.block<1, 3>(i, 0) = params.block<3, 1>(i * 3, 0).transpose();
        // }
        Eigen::Matrix<double,9,1> X_ = A.colPivHouseholderQr().solve(b);
        correct_matrix << X_(0),X_(1),X_(2),X_(3),X_(4),X_(5),X_(6),X_(7),X_(8);
        //end of TODO
    
        return correct_matrix;
    }

# 基于模型的方法： #

模型形式：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 2][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 3][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 4][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 5][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 6][]

### 代码： ###

#include <Eigen/Core>
    #include <Eigen/Dense>
    #include <Eigen/Geometry>
    #include <fstream>
    #include <iostream>
    #include <math.h>
    #include <string>
    
    using namespace std;
    
    string scan_match_file = "./scan_match.txt";
    string odom_file = "./odom.txt";
    
    int main(int argc, char** argv)
    {
        // 放置激光雷达的时间和匹配值 t_s s_x s_y s_th
        vector<vector<double>> s_data;
        // 放置轮速计的时间和左右轮角速度 t_r w_L w_R
        vector<vector<double>> r_data;
    
        ifstream fin_s(scan_match_file);
        ifstream fin_r(odom_file);
        if (!fin_s || !fin_r) {
            cerr << "请在有scan_match.txt和odom.txt的目录下运行此程序" << endl;
            return 1;
        }
    
        // 读取激光雷达的匹配值
        while (!fin_s.eof()) {
            double s_t, s_x, s_y, s_th;
            fin_s >> s_t >> s_x >> s_y >> s_th;
            s_data.push_back(vector<double>({ s_t, s_x, s_y, s_th }));
        }
        fin_s.close();
    
        // 读取两个轮子的角速度
        while (!fin_r.eof()) {
            double t_r, w_L, w_R;
            fin_r >> t_r >> w_L >> w_R;
            r_data.push_back(vector<double>({ t_r, w_L, w_R }));
        }
        fin_r.close();
    
        // 第一步：计算中间变量J_21和J_22
        Eigen::MatrixXd A;
        Eigen::VectorXd b;
        // 设置数据长度
        A.conservativeResize(5000, 2);
        b.conservativeResize(5000);
        A.setZero();
        b.setZero();
    
        size_t id_r = 0;
        size_t id_s = 0;
        double last_rt = r_data[0][0];
        double w_Lt = 0;
        double w_Rt = 0;
        while (id_s < 5000) {
            // 激光的匹配信息
            const double& s_t = s_data[id_s][0];
            const double& s_th = s_data[id_s][3];
            // 里程计信息
            const double& r_t = r_data[id_r][0];
            const double& w_L = r_data[id_r][1];
            const double& w_R = r_data[id_r][2];
            ++id_r;
            // 在2帧激光匹配时间内进行里程计角度积分
            if (r_t < s_t) {
                double dt = r_t - last_rt;
                w_Lt += w_L * dt;
                w_Rt += w_R * dt;
                last_rt = r_t;
            } else {
                double dt = s_t - last_rt;
                w_Lt += w_L * dt;
                w_Rt += w_R * dt;
                last_rt = s_t;
                // 填充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
                w_Lt = 0;
                w_Rt = 0;
                ++id_s;
            }
        }
        // 进行最小二乘求解
        Eigen::Vector2d J21J22;
        //TODO: (1~2 lines)
        J21J22 = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
        // J21J22 = A.colPivHouseholderQr().solve(b);
        //end of TODO
        const double& J21 = J21J22(0);
        const double& J22 = J21J22(1);
        cout << "J21: " << J21 << endl;
        cout << "J22: " << J22 << endl;
    
        // 第二步，求解轮间距b
        Eigen::VectorXd C;
        Eigen::VectorXd S;
        // 设置数据长度
        C.conservativeResize(10000);
        S.conservativeResize(10000);
        C.setZero();
        S.setZero();
    
        id_r = 0;
        id_s = 0;
        last_rt = r_data[0][0];
        double th = 0;
        double cx = 0;
        double cy = 0;
        while (id_s < 5000) {
            // 激光的匹配信息
            const double& s_t = s_data[id_s][0];
            const double& s_x = s_data[id_s][1];
            const double& s_y = s_data[id_s][2];
            // 里程计信息
            const double& r_t = r_data[id_r][0];
            const double& w_L = r_data[id_r][1];
            const double& w_R = r_data[id_r][2];
            ++id_r;
            // 在2帧激光匹配时间内进行里程计位置积分
            if (r_t < s_t) {
                double dt = r_t - last_rt;
                cx += 0.5 * (-J21 * w_L * dt + J22 * w_R * dt) * cos(th);
                cy += 0.5 * (-J21 * w_L * dt + J22 * w_R * dt) * sin(th);
                th += (J21 * w_L + J22 * w_R) * dt;
                last_rt = r_t;
            } else {
                double dt = s_t - last_rt;
                cx += 0.5 * (-J21 * w_L * dt + J22 * w_R * dt) * cos(th);
                cy += 0.5 * (-J21 * w_L * dt + J22 * w_R * dt) * sin(th);
                th += (J21 * w_L + J22 * w_R) * dt;
                last_rt = s_t;
                // 填充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
                cx = 0;
                cy = 0;
                th = 0;
                ++id_s;
            }
        }
        // 进行最小二乘求解，计算b, r_L, r_R
        double b_wheel;
        double r_L;
        double r_R;
        //TODO: (3~5 lines)
        b_wheel = C.colPivHouseholderQr().solve(S)[0];
        r_L = -J21 * b_wheel;
        r_R = J22 * b_wheel;
        //end of TODO
        cout << "b: " << b_wheel << endl;
        cout << "r_L: " << r_L << endl;
        cout << "r_R: " << r_R << endl;
    
        cout << "参考答案：轮间距b为0.6m左右，两轮半径为0.1m左右" << endl;
    
        return 0;
    }

### 运行结果 ###

J21: -0.163886
    J22: 0.170575
    b: 0.59796
    r_L: 0.0979974
    r_R: 0.101997
    参考答案：轮间距b为0.6m左右，两轮半径为0.1m左右

cmakelist 和 odom.txt 和scan\_match.txt 后续整理上传

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70]: /images/20220829/bbe5e85630f442618b45262cb3d9d510.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 1]: /images/20220829/6ef5f90eb76e4ebd92f34e8aa5560523.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 2]: /images/20220829/7dc3bdf16f3d41d794c26f09eb590a2b.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 3]: /images/20220829/412eb2f72eec482398351736d7573b40.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 4]: /images/20220829/5d915ade41524554a5a74cc9f2ce23ea.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 5]: /images/20220829/4e5210173abf4a4a98f8d29af1ed15dd.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 6]: /images/20220829/a38a23f64d62475494f09c0bfb720caf.png