里程计运动模型及标定

1 里程计运动模型

1.1 两轮差分底盘的运动模型

1.2 三轮全向底盘的运动学模型

1.3 航迹推算（Dead Reckoning）

2 里程计标定

2.1 线性最小二乘的基本原理

2.2 最小二乘的直线拟合

2.3 最小二乘在里程计标定中的应用

3 实现一个直接线性方法的里程计标定模块

3.1 安装

mkdir -p ~/calib_odom_ws/src
cd ~/calib_odom_ws/src
git clone https://github.com/KOTOKORURU/odometry_calibration
cd ..
catkin_make

错误1：

odom_calib/src/odometry_calibration/src/main.cpp:5:40: fatal error: nav_core/recovery_behavior.h: 没有那个文件或目录

解决办法：打开odom_calib/src/odometry_calibration/src/main.cpp，注释#nav_core/recovery_behavior.h。

错误2：

odom_calib/src/odometry_calibration/src/main.cpp:28:25: fatal error: csm/csm_all.h: 没有那个文件或目录

解决办法：安装ros-kinetic-csm。

sudo apt install ros-kinetic-csm

3.2 运行

3.2.1 启动launch文件

cd ~/calib_odom_ws/
source /devel/setup.bash
roslaunch calib_odom odomCalib.launch

注意：保证没有任何ROS节点在运行，roscore也要关闭。

3.2.2 打开rviz

1.打开rviz

rosrun rviz rviz

2.配置rviz

fix_frame选择为odom。在Add选项卡中增加三条Path消息，订阅的topic分别为：

• odom_path_pub_
• scan_path_pub_
• calib_path_pub_

分别选择不同的颜色，用来区分路径。

可以将此rviz的配置保存为odom_calib.rviz文件，然后在odomCalib.launch文件中添加：

<node pkg="rviz" type="rviz" name="rviz" args="-d $(find calib_odom)/rviz_cfg/odom_calib.rviz" />

这样，在启动launch文件时，就自动打开了配置好的rviz。

3.2.3 播放bag

首先，解压odom.bag.tar.gz：

cd ~/calib_odom_ws/src/odometry_calibration/bag
tar zxvf odom.bag.tar.gz

然后，播放bag：

rosbag play –clock odom.bag

如果一切正常，则能看到运行矫正程序的终端会打印数据，并且rviz中可以看到两条路径。

3.2.4 矫正里程计

当打印的数据到达一定的数量后，则可以开始矫正。这里，需要给calib_flag话题发布一个数据。

rostopic pub /calib_flag std_msgs/Empty "{}"

3.2.5 结果

程序矫正完毕会输出对应的矫正矩阵，并且会在rviz中显示出第三条路径，即calib_path。可以观察里程计路径odom_path和矫正路径calib_path区别来判断此次矫正的效果。

如下图所示，激光雷达的pose好像有点问题。。

3.3 解析

3.3.1 构建超定方程组Ax=b

/*
 * 输入:里程计和激光数据
 * 构建最小二乘需要的超定方程组
 * Ax = b
*/
typedef Eigen::Matrix<double, 1, 9> Vector9d_T;
typedef Eigen::Matrix<double, 9, 1> Vector9d;

bool OdomCalib::Add_Data(Eigen::Vector3d Odom, Eigen::Vector3d scan)
{
    if(now_len < INT_MAX)
    {
		Eigen::Matrix<double, 3, 9> A_tmp;
		Eigen::Matrix<double, 3, 1> b_tmp;
		A_tmp.setZero();
		b_tmp.setZero();
	        
        b_tmp = scan;

		for(int i = 0; i < 3; i++){
			Vector9d_T tmp;
			tmp.setZero();
			tmp.block<1, 3>(0, i * 3) = Odom.transpose();
			A_tmp.block<1, 9>(i, 0) = tmp;
		}

		A.block<3, 9>(now_len * 3, 0) =  A_tmp;
		b.block<3, 1>(now_len * 3, 0) =  b_tmp;

        now_len++;
        return true;
    }
    else
        return false;
}

3.3.2 求解超定方程组

/*
 * 求解线性最小二乘Ax=b(A^TAx^* = A^Tb)
 * 返回得到的矫正矩阵
*/
Eigen::Matrix3d OdomCalib::Solve()
{
    Eigen::Matrix3d correct_matrix;
    Vector9d x;
    Eigen::MatrixXd A_tmp;
    Eigen::VectorXd b_tmp;
    
    A_tmp = A.transpose() * A;
    b_tmp = A.transpose() * b;
    x = A_tmp.ldlt().solve(b_tmp);  // Cholesky decomposition
    
    correct_matrix << x(0), x(1), x(2),
                      x(3), x(4), x(5),
                      x(6), x(7), x(8);
    
    return correct_matrix;
}

3.3.3 计算两个位姿之间的位姿差

Eigen::Vector3d cal_delta_distance(Eigen::Vector3d odom_pose)
{
    static Eigen::Vector3d now_pos, last_pos;
    Eigen::Vector3d d_pos;  // return value
    now_pos = odom_pose;

    Eigen::Matrix3d R_last;
    R_last << cos(now_pos(2)), -sin(now_pos(2)), 0,
              sin(now_pos(2)), cos(now_pos(2)),  0,
              0,                             0,  1;
    d_pos = R_last.transpose() * (now_pos - last_pos);
    // d_pos = R_last.inverse() * (now_pos - last_pos); // 根据航迹推算公式，应该是逆啊
    
    last_pos = now_pos;

    return d_pos;
}

3.3.4 获取里程计和激光雷达位姿

输出bag文件的信息：

其中，/odom话题为里程计的pose，/sick_scan为2D激光雷达的数据，同时还发布了TF。激光雷达的pose是通过PI-ICP算法计算的，而里程计是直接从bag发布的。

获取里程计pose：

bool Scan2::getOdomPose(Eigen::Vector3d& pose, const ros::Time& t)
{
    // Get the robot's pose
    tf::Stamped<tf::Pose> ident(tf::Transform(tf::createQuaternionFromRPY(0,0,0),
                                               tf::Vector3(0,0,0)), t, base_frame_);
    tf::Stamped<tf::Transform> odom_pose;

    try
    {
        tf_.transformPose(odom_frame_, ident, odom_pose);
    }
    catch(tf::TransformException e)
    {
        ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
        return false;
    }

    double yaw = tf::getYaw(odom_pose.getRotation());

    pose << odom_pose.getOrigin().x(),
            odom_pose.getOrigin().y(),
            yaw;
    //pub_msg(pose, path_odom, odom_path_pub_);
    return true;
}

3.3.5 小结

理论上，相邻的两个时刻，激光雷达和里程计的位姿变换是相同的，而实际的估计值和真实值呈线性关系，因此此可以构建基于直接线性方法的最小二乘问题。

目前此项目，只是演示如何使用激光雷达纠正里程计的误差，属于后处理，并不适用于实际应用。

我觉得，真正意义上得里程计标定可以参考：https://github.com/MegviiRobot/OdomLaserCalibraTool 。

4 参考

1. 激光SLAM理论与实践
2. https://github.com/KOTOKORURU/odometry_calibration