文章目录

1 里程计运动模型
- 1.1 两轮差分底盘的运动模型
- 1.2 三轮全向底盘的运动学模型
- 1.3 航迹推算（Dead Reckoning）
2 里程计标定
- 2.1 线性最小二乘的基本原理
- 2.2 最小二乘的直线拟合
- 2.3 最小二乘在里程计标定中的应用
3 实现一个直接线性方法的里程计标定模块
- 3.1 安装
- 3.2 运行
- - 3.2.1 启动launch文件
  - 3.2.2 打开rviz
  - 3.2.3 播放bag
  - 3.2.4 矫正里程计
  - 3.2.5 结果
- 3.3 解析
- - 3.3.1 构建超定方程组Ax=b
  - 3.3.2 求解超定方程组
  - 3.3.3 计算两个位姿之间的位姿差
  - 3.3.4 获取里程计和激光雷达位姿
  - 3.3.5 小结
4 参考

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;}elsereturn 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 decompositioncorrect_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 valuenow_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 posetf::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 参考

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

里程计运动模型及标定相关推荐

《概率机器人》里程计运动模型gmapping中代码解析
里程计运动模型(odometery motion model)用距离测量代替控制.实际经验表明虽然里程计虽然仍存在误差,但通常比速度运动模型更加的精确.相比于速度运动模型运动信息utu_t由 (x¯t ...
小豆包的学习之旅：里程计运动模型
星球大战里面有2个非常出色的机器人一个是3PO,一个是R2.我的机器人呢就叫小豆包啦,代号FR2,不许侵犯它的署名哦. 常见的机器人运动模型包括:速度计运动模型,里程计运动模型和惯导运动模型.概率运动 ...
概率机器人：里程计运动模型
里程计运动模型速度运动模型采用机器人的速度来计算位姿的后验概率.里程计运动模型(Odometry motion model)则以里程计测量为基础来计算机器人随时间的运动. 通常,通过编码器可以获得机 ...
移动机器人——里程计矫正
一.问题分析机器人可以基于码盘数据和底盘运动学模型进行航迹推演,得到机器人的轨迹,但实际轨迹与推演轨迹存在误差,分析原因如下: 底盘实际尺寸与理论模型存在偏差,如轮子半径,两轮间距等: 底盘在运动过 ...
传感器数据处理Ⅰ------常用里程计模型
里程计运动模型及标定 1.常用里程计模型 1.两轮差分底盘的运动学模型优点结构简单 (越障好) 便宜(2个电机) 模型简单 2.差分模型都是依据圆弧运动:
轮式里程计与激光雷达进行标定2--里程计运动学模型方法（理论+实现代码）
轮式里程计与激光雷达进行标定2--里程计运动学模型方法(理论+实现代码) 轮式里程计定义轮式里程计与激光SLAM的关系轮式里程计标定前言轮式里程计与激光雷达标定数学建模 Code Result ...
轮式里程计+视觉紧耦合方案
基于ORB_SLAM: GitHub - izhengfan/se2lam: (ICRA 2019) Visual-Odometric On-SE(2) Localization and ...
autoware之轮式里程计计算
这部分代码主要是接收了底盘的can消息然后计算一个轮速里程计.具体的: 1.can_status_translator节点 // setup subscribersub1_ = nh_.subscr ...
双目视觉惯性里程计的在线初始化与自标定算法
标题:An Online Initialization and Self-Calibration Method for Stereo Visual-Inertial Odometry 期刊:IEEE ...

里程计运动模型及标定