第二章 里程计运动模型及标定
作业要求
- 本次的作业为用直接线性方法来对机器人的里程计进行校正。
- 给出的文件中包含有本次作业使用的bag数据,路径为odom_ws/bag/odom.bag。
- 本次的作业中,需要实现三个函数,分别为:
- Main.cpp,第340行中的cal_delta_distance()函数,该函数的功能为给定两个里程计位姿,计算这两个位姿之间的位姿差。
- Odom_Calib.cpp,第23行Add_Data()函数,该函数的功能为构建超定方程组Ax=b,具体参考PPT。
- Odom_Calib.cpp,第44行 Solve()函数,该函数的功能为对2中构建的超定方程组进行求解。
本次程序的运行过程为:
7. 实现上述的三个函数,并且进行编译。
8. 在odom_ws下,进行source:source devel/setup.bash
9. 运行launch文件:roslaunch calib_odom odomCalib.launch。执行本条指令的时候,必须保证没有任何ros节点在运行,roscore也要关闭。
10. 在3正常的情况下,运行rviz,fix_frame选择为odom_frame。在Add选项卡中增加三条Path消息。一条订阅的topic为:odom_path_pub_;一条订阅的topic为:scan_path_pub_;最后一条为:calib_path_pub_。分别选择不同的颜色。
11. 进入到odom_ws/bag目录下,运行指令:rosbag play –clock odom.bag。
12. 如果一切正常,则能看到运行矫正程序的终端会打印数据,并且rviz中可以看到两条路径。当打印的数据到达一个的数量之后,则可以开始矫正。
13. 矫正的命令为,在calib_flag的topic下发布一个数据:rostopic pub /calib_flag std_msgs/Empty 。
14. 程序矫正完毕会输出对应的矫正矩阵,并且会在rviz中显示出第三条路径,即calib_path。可以观察里程计路径odom_path和矫正路径_calib_path区别来判断此次矫正的效果。
15.
注意事项
(1)
建议自己新建一个工作空间,然后初始化,比如这里我就新建了一个工作空间lidar_space。然后在~/.bashrc中加入
source /home/mjy/dev/lidar_space/devel/setup.bash
建后将功能包calib_odom直接拷贝到此工作空间src之下。
(2)
由于里程代码是跑在indigo上面的,但是我的是ros-kinetic,因此需要改一个地方:
/home/mjy/dev/lidar_space/src/calib_odom/CMakeLists.txt
文件中搜索到indigo的地方,均换为kinetic。
# 第126行
/opt/ros/kinetic/include/csm
# 第150行
/opt/ros/kinetic/lib/libcsm.so
如果不修改,则编译的时候会报错:No rule to make target '/opt/ros/indigo/lib/libcsm.so'
(3)
pcl包是依赖Eigen的,而我是新装的eigen,路径在usr/local/include之下,因此pcl是找不到eigen的。
当然如果你是默认eigen,则无须理会。
所以,我这种特殊情况要修改pcl的源代码:
cd /opt/ros/kinetic/share/pcl_conversions/cmake
sudo gedit pcl_conversionsConfig.cmake
cd /opt/ros/kinetic/share/pcl_ros/cmake
sudo gedit pcl_rosConfig.cmake
将上述两个文件中的eigen搜寻路径改为自己的。
答案
答案分享给大家:
cal_delta_distance()函数
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;
//TODO:
// -------------------------------------------------------------------------------------------------
// d_pos = last_pos.inverse() * now_pos;
d_pos = now_pos - last_pos;
//end of TODO:
last_pos = now_pos;
return d_pos;
}
一开始以为要求逆运算,但实际上这是个二维平面的运动,所以之间位姿相减就可以
Add_Data()函数
bool OdomCalib::Add_Data(Eigen::Vector3d Odom,Eigen::Vector3d scan)
{
if(now_len<INT_MAX)
// if(now_len<data_len)
{
//TODO: 构建超定方程组
Eigen::Matrix<double,3,9> temp_A;
Eigen::Vector3d temp_b;
temp_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);
temp_b << scan(0),scan(1),scan(2);
// 给到A和B
A.block<3,9>(3*now_len,0) = temp_A;
b.block<3,1>(3*now_len,0) = temp_b;
//end of TODO
now_len++;
return true;
}
else
{
return false;
}
}
Eigen块赋值操作 学习:https://www.cnblogs.com/wangxiaoyong/p/8906213.html
或我的blog:https://blog.csdn.net/weixin_44684139/article/details/105514563
Solve()函数
Eigen::Matrix3d OdomCalib::Solve()
{
Eigen::Matrix3d correct_matrix;
//TODO:求解线性最小二乘
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;
}
因为是超定方程,并且直接套公式的话容易放大误差,因此用到了QR分解求线性方程组。colPivHouseholderQr()
然后编译—roslaunch–rviz–rostopic pub这些都按照作业要求走即可。
rviz画面
标定结果:
correct_matrix:
0.946384 -1.64479 0.0124617
-0.0709081 -1.04267 0.00173608
-0.00674715 0.0415879 0.0619791
calibration over!!!!