激光SLAM学习——机器人的里程计标定(直接线性方法)
1 实践操作
1.1 本次的作业中,需要实现三个函数,分别为:
- Main.cpp ,第 358 行中的 cal_delta_distance()函数,该函数的功能为给定两个里程计位姿,计
算这两个位姿之间的位姿差。(路径:/odom_ws/src/calib_odom/src/main.cpp)
//求解得到两帧数据之间的位姿差
//即求解当前位姿在上一时刻坐标系中的坐标
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 = 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;
//end of TODO:
last_pos = now_pos;
return d_pos;
}
- Odom_Calib.cpp ,第 28 行 Add_Data()函数,该函数的功能为构建超定方程组
Ax=b ,具体参考 PPT。(路径:/odom_ws/src/calib_odom/src/Odom_Calib.cpp)
构建最小二乘需要的超定方程组
Ax = b
*/
bool OdomCalib::Add_Data(Eigen::Vector3d Odom,Eigen::Vector3d scan)
{
if(now_len<INT_MAX)
{
//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;
}
}
- Odom_Calib.cpp ,第 48 行 Solve() 函数,该函数的功能为对 2 中构建的超定方程组进行求解。(路径:/odom_ws/src/calib_odom/src/Odom_Calib.cpp)
* 求解线性最小二乘Ax=b
* 返回得到的矫正矩阵
*/
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;
}
1.2 本次程序的运行过程为:
- 实现上述的三个函数,在 odom_ws 下用 catkin_make 进行编译。
(如果编译过程中提示缺少 csm 库,可以用命令 sudo apt-get install ros-melodic-csm 进行安装。
如果你用的 ROS 版本为其它版本,则将 CMakeLists.txt 中所有 melodic 内容改为你的 ROS 版本。其它库安装方式也类似,比如缺少 nav_core,则用 sudo apt-get install ros-melodic-nav-core 安装。)
2. 在 odom_ws 下,进行 source,具体命令为:source devel/setup.bash。
(每次不用再source的操作:1.ctrl+h后打开 .bashrc ;2.在 # Set ROS melodic 下 加入一行 source ~/jg_slam(你自己在主目录下放的文件名)/devel/setup.bash )
-
运行 launch 文件:roslaunch calib_odom odomCalib.launch。执行本条指令的时候,必须保
证没有任何 ros 节点在运行,roscore 也要关闭。
-
在 3 正常的情况下,ctrl+alt+T 打开新终端,运行 rviz,fix_frame 选择为 odom 。在 Add 选项卡中增加三条 Path 消息。一条订阅的 topic 为:odom_path_pub_ ;一条订阅的 topic 为:scan_path_pub_;最后一条为:calib_path_pub_。分别选择不同的颜色。
-
进入到 odom_ws/bag 目录下,运行指令: rosbag play --clock odom.bag。如果一切正常, 则能看到 运行矫正程序的终端会打印数据,并且 rviz 中可以看到 两条路径。当打印的数据到达一个的数量之后,则可以开始矫正。
-
进行7前,还需检查在此路径中 /jg_slam/src/basicTransformStudy/basic_transform_study.cpp 中机器人的坐标转换代码是否完成;
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace std;
int main(int argc, char** argv)
{
// 机器人B在坐标系O中的坐标:
Eigen::Vector3d B(3, 4, M_PI);
// 坐标系B到坐标O的转换矩阵:
Eigen::Matrix3d TOB;
TOB << cos(B(2)), -sin(B(2)), B(0),
sin(B(2)), cos(B(2)), B(1),
0, 0, 1;
// 坐标系O到坐标B的转换矩阵:
Eigen::Matrix3d TBO = TOB.inverse();
// 机器人A在坐标系O中的坐标:
Eigen::Vector3d A(1, 3, -M_PI / 2);
// 求机器人A在机器人B中的坐标:
Eigen::Matrix3d TOA;
// TODO 参照第一课PPT
TOA << cos(A(2)), -sin(A(2)), A(0),
sin(A(2)), cos(A(2)), A(1),
0, 0, 1;
// start your code here (5~10 lines)
Eigen::Matrix3d TBA;
TBA=TBO*TOA;
Eigen::Vector3d BA(TBA(0,2),TBA(1,2),atan2(TBA(1,0),TBA(0,0)));
// end your code here
cout << "The right answer is BA: 2 1 1.5708" << endl;
cout << "Your answer is BA: " << TBA.transpose() << endl;
return 0;
}
-
矫正的命令为,在 calib_flag 的 topic 下发布一个数据:rostopic pub /calib_flag std_msgs/
Empty “{}” 。(此处命令不要复制,自己手打)
-
程序矫正完毕会输出对应的矫正矩阵,并且 会在 rviz 中显示出第三条路径,即 calib_path。可
以观察里程计路径 odom_path 和矫正路径 calib_path 的区别来判断此次矫正的效果。
2 参考结果
可以看到,校正后的轨迹跟激光的轨迹接近了很多。