一．本次作业练习目标

巩固课程所学知识，理解基于直接线性法和基于模型的标定方法，并对下一节课激光雷达模型的内容做一些铺垫学习。

二．作业计分原则

满分为12分，10分为优秀，8分为良好，6分为及格。

三．作业提交说明

需提供完整的PDF报告及代码。公式推导题可以手写照相粘进PDF，也可以直接写进PDF。

四．作业题目说明

1. 补充直接线性方法的里程计标定模块代码。(6分)

2. 补充轮速计+角速度计标定模块代码。（2分）

3. 对于线性方程 Ax = b的求解，Eigen库[1]提供了多种求解方法：

具体可参见

https://eigen.tuxfamily.org/dox/group__TopicLinearAlgebraDecompositions.html

https://www.cnblogs.com/wangxiaoyong/p/8977343.html

通过网络等各种渠道学习总结，对于最小二乘问题，都有哪几种求解方法，各优缺点有哪些，分别在什么条件下较常使用？（2分）

4. 阅读论文Precise indoor localization for mobile laserscanner前两章，回答问题。（2分）

（1）根据第二章内容，简述激光雷达测距原理；

（2）简要介绍一下右图的含义。 

第一题代码说明：

1. 本次的作业为用直接线性方法来对机器人的里程计进行校正。

2. 给出的文件中包含有本次作业使用的 bag 数据，路径为odom_ws/bag/odom.bag。

3. 本次的作业中，需要实现三个函数，分别为：

1. Main.cpp，第 340 行中的 cal_delta_distance()函数,该函数的功能为给定两个里程计位姿,计算这两个位姿之间的位姿差。

2. Odom_Calib.cpp，第 23 行 Add_Data()函数，该函数的功能为构建超定方程组

Ax=b，具体参考 PPT。

3. Odom_Calib.cpp，第 44 行 Solve()函数，该函数的功能为对 2 中构建的超定方程组进行求解。

本次程序的运行过程为：

Step1: 实现上述的三个函数,并进行编译。

Step2: 在odom_ws下,进行 source,具体命令为: source devel/setup.bash

Step3: 运行launch文件:roslaunch calib_odom odomCalib.launch。执行本条指令的时候,必须保证没有任何 ros 节点在运行,roscore也要关闭。

Step4: 在 3 正常的情况下,运行 rviz, fix_frame 选择为odom。在 Add 选项卡中增加三条Path 消息。一条订阅的topic 为: odom_path_pub_;一条订阅的 topic 为:scan_path_pub_;最后一条为:calib_path_pub_。分别选择不同的颜色。

Step5: 进入到 odom_ws/bag 目录下,运行指令: rosbag play -–clock odom.bag。

Step6: 如果一切正常,则能看到运行矫正程序的终端会打印数据,并且 rviz 中可以看到两条路径。当打印的数据到达一个的数量之后,则可以开始矫正。

Step7: 矫正的命令为,在 calib_flag 的 topic 下发布一个数据:rostopic pub /calib_flag std_msgs/Empty "{}" 。

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

参考结果：绿色为odom、黄色为激光、红色为矫正后的轨迹，可以看到校正后的轨迹跟激光的轨迹接近了很多。

说明：以上结果为数据包开始到179s的2500个数据计算出来的，采集不同量数据得到的结果会有所不同，也并非采集时间越长结果越好，标定结果数据仅供参考，效果直接看轨迹图。

第二题代码说明：

在实际标定中，直接线性方法通用性强，但精度不如基于模型的方法。

在课程中我们得知，差速轮运动模型中，可以通过轮速差得到机器人底盘中心的线速度和角速度。但有时由于地面打滑或其它原因导致角速度计算不准。这里我们用角速度计来替代轮速计计算机器人的角速度w，而线速度v依旧通过轮速计计算。

机器人由于载重不同或轮胎气压大小不同，真实的速度v_real与v相差一个比例v_ratio, v_real = v * v_ratio + noise. 角速度计得到的角速度w与真实角速度w_real相差一个缓慢变化的bias, w_real = w + bias + noise. noise一般为0均值的高斯白噪声，bias在此次标定作业中假设变化不大。

本次作业中你需要通过一小段groundtruth.txt标定出v_ratio和noise的大小，之后生成标定后的航迹推算轨迹，利用DrawTrajectory.py比较标定前后的轨迹不同。

代码编译说明：

cd 你放置作业代码的目录/wheel_imu/

mkdir build

cd build

cmake ..

make

cd ..

./wheel_imu

之后可用命令python DrawTrajectory.py raw_poses.txt 和python DrawTrajectory.py corrected_poses.txt 查看标定前生成的轨迹和标定后生成的轨迹。

最后标定成功的轨迹大致为：

[1] https://eigen.tuxfamily.org/index.php?title=Main_Page