第二次作業

一．本次作業練習目標

鞏固課程所學知識，理解基于直接線性法和基于模型的標定方法，并對下一節課激光雷達模型的內容做一些鋪墊學習。

二．作業計分原則

滿分為12分，10分為優秀优信彩票|官网登录，8分為良好，6分為及格。

三．作業提交說明

需提供完整的PDF報告及代碼优信彩票|官网登录。公式推導題可以手寫照相粘進PDF优信彩票|官网登录，也可以直接寫進PDF。

四．作業題目說明

1. 補充直接線性方法的里程計標定模塊代碼。(6分)

2. 補充輪速計+角速度計標定模塊代碼。（2分）

3. 對于線性方程 Ax = b的求解优信彩票|官网登录，Eigen庫[1]提供了多種求解方法：

具體可參見

http://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] http://eigen.tuxfamily.org/index.php?title=Main_Page