mcl_3dl閱讀整理

算法簡介

（這部分是github上ReadME搬運過來的）mcl_3dl節點是基於三維激光雷達的定位算法，核心是蒙特卡洛定位（MCL），也稱爲粒子濾波定位，通過用粒子的密度和權重來表示待估計位姿的概率分佈。

觀測：

1 LIDAR測量模型：

通常波束模型與似然域模型相比，可以減少地圖中被測點的false positive匹配，但更復雜。mcl_3dl節點同時使用這兩個模型來以良好的計算能力實現好的匹配結果。計算粒子的likelihood的時候，將用大量隨機採樣點做似然域模型得到的結果，與用少量隨機採樣點做波束模型預測的結果相乘，計算不復雜但是又減少了誤匹配。
具體做法是：
在似然域模型中，likelihood是每個觀測點到地圖中最近點的距離之和，最近點的搜索使用分塊Kd樹，觀測點進行體素濾波並隨機採樣以降低計算複雜度。具體實現在lidar_measurement_model_likelihood.cpp
在波束模型中，likelihood通過ray casting計算，N是觀測點的射線數量，n是穿過地圖中障礙物的射線數量，alpha是拒絕權重，計算公式是alpha ^（n / N）。具體實現在lidar_measurement_model_beam.cpp

2 軸相關距離權重

在牆壁稀疏只有地板的一些環境中，由於可以修正估計Z位置的點的數量相對較大，在Z方向上的距離就更少關注。距離計算的權重係數可以由參數更改，在上述環境下權重設置爲：（（dist_weight_x，dist_weight_y，dist_weight_z）=（1.0，1.0，5.0））。這個在代碼裏面沒有找到體現

3 加速度測量模型 imu產生的觀測

通過獲取IMU數據中的加速度矢量與粒子姿態之間的差來計算姿態的likelihood。

預測

根據帶有噪聲參數的里程計數據預測粒子的姿態，建議使用由IMU補償的里程計數據。

重採樣：

mcl_3dl節點通過複製較大概率的粒子來對粒子重新採樣，複製的粒子會因高斯噪聲而擴散。

擴展重置Expansion resetting

mcl_3dl臨時實現了擴展重置方法。如果所有觀測點中，具有相關聯地圖點的觀測點的比例小於閾值，則通過給定的高斯噪聲擴展粒子分佈。一定的程度上可以解決局部極值問題。
代碼裏的實現就是在之前的基礎上給粒子加噪聲，然後等待重新收斂

執行

roslaunch mcl_3dl test.launch use_pointcloud_map:=false use_cad_map:=false \ use_bag_file:=true bag_file:=${HOME}/Downloads/short_test.bag

有幾個參數：
use_neonavigation = true（默認false） 會執行下面兩個其他地方的包：

trajectory_tracker trajectory_recorder path_recorder
dist_interval = 0.1
ang_interval = 0.2
trajectory_tracker trajectory_recorder poses_ref
dist_interval = 0.5
ang_interval = 100.0

use_cad_map = true默認! use_pointcloud_map 會執行下面這個包：

obj_to_pointcloud obj_to_pointcloud obj_to_pointcloud
    <param name="objs" value="$(arg map_objs)" />
    <param name="offset_x" value="$(arg map_offset_x)" />
    <param name="offset_y" value="$(arg map_offset_y)" />
    <param name="scale" value="$(arg map_scale)" />
    <remap from="~/cloud" to="mapcloud" />

use_pointcloud_map = true（默認true）會執行下面這個包：

<node pkg="pcl_ros" type="pcd_to_pointcloud" name="pcd_to_pointcloud"
      args="$(arg map_pcd)" if="$(arg use_pointcloud_map)">
    <remap from="cloud_pcd" to="mapcloud" />
  </node>

上面三個參數額外執行的包都不是mcl_3dl提供的

主要結構

這是mcl_3dl節點執行後的graph

Topic：
接收

• /cloud(雷達發佈)
• /odom(碼盤)
• /imu/data(imu)
• /tf(一系列的tf變換)
• /mapcloud(點雲地圖，插入到chunk kdtree，會隨時檢測地圖更新)
• /initialpose(發佈這個話題，粒子就在提供的位置處疊加噪聲，如果不發佈這個話題，粒子就在默認位置處疊加噪聲（默認位置可以通過ros參數修改和讀取）)

發佈

• /tf(odom到map 還有一個floor到map)
• /amcl_pose(map座標系下的位置)

服務：

• /resize_mcl_particle 可以實時調整粒子數（調整粒子數的操作和重採樣很相似，都是大概率的粒子多複製）
• /global_localization 實時啓動全局定位
• /expansion_resetting 實時進行擴展重置

在test/src/ test_global_localization.cpp文件中給出了全局定位的使用過程：

• 發佈cloud_map；
• 發佈生成的cloud imu odom數據 並等待初始化（status變成normal；
• 然後發了5次odom數據 用來那邊做預測
• 調用重定位服務並等待狀態變成GLOBAL_LOCALIZATION （下面的過程會一直髮/cloud /odom /imu/data數據）
• 等待全局定位結束
• 等了40個循環提高位姿精度

部分代碼

• chunked_kdtree.h 塊kd樹 在pcl kdtree基礎上增加了分塊 在每個分塊內進行pcl的kdtree搜索：kdtree_->radiusSearch(p, radius, id, dist_sq, num) 點，半徑，緊鄰索引值，近鄰中心距，

• raycast.h 定義了迭代器，與塊kd樹相結合的射線遍歷 用在雷達波束模型中。這裏有一個點：頭文件裏面重載了*操作符，因爲for的範圍循環在循環前，會對每一個對象進行一次" * "，再賦值 這個重載真牛逼；然後遍歷射線 對射線的每個點在kdtree上搜索最近點。裏面進行了兩次搜索，沒有看太懂 最後用兩次的結果計算了sin_ang 做進一步判斷

• filter.h 提供了兩種濾波方式 LPF和HPF 低通濾波和高通濾波

• nd.h 正態分佈 一維和多維的

• point_types.h 定義了pointcloud類型然後註冊到點雲庫中

• point_conversion.h 定義了ros msg到pcl pointcloud轉換

• point_cloud_random_sampler.h 隨機生成一坨點雲

• vec3.h 自己定義了位姿

• quat.h 自己定義了旋轉 以四元數形式存儲 中間又RPY和軸角的轉換

• pf.h 定義了ParticleWeightedMean ParticleFilter（這個裏面實現了重採樣） Particle ParticleBase

• state_6dof.h 擴展到6自由度位姿

• parameters.h 參數集合

• imu_measurement_model.h 重力加速度計觀測 用觀測與predict的第三分量，求cos 然後作爲方差 返回對應的高斯分佈的概率；之前對IMU做觀測瞭解的不多，第一次遇到這種做法。

• motion_prediction_model.h 差分輪式預測模型 設置前與當前的pose與時間間隔 預測下一時刻的pose 運動模型更新部分

• noise_generator.h 生成噪聲 正態分佈 只在對角線生成和全矩陣生成兩種

• lidar_measurement_model.h 雷達觀測模型 likelihood和beam模型

• compatibility.h ros subscribe和advertise等兼容性相關的文件

• voxel_grid.h 對點雲體素化 然後進行濾波（取每個體素中所有三維點中間一點的點）輸出

• lidar_measurement_model_beam.cpp 波束模型的具體實現

• lidar_measurement_model_likelihood.cpp 似然域模型的具體實現

• mcl_3dl.cpp 整個的ros節點，各種topic的回調函數，粒子濾波的過程在/cloud對應的回調函數裏面，這個文件比較大 做了一部分註釋貼上來

理解

1 mcl_3dl節點的全局定位實現就是和正常定位一樣，在整個地圖撒粒子，這樣開始的時候計算效率肯定不高，但是隨着收斂迭代，會不斷減小粒子數，這一點想法很好。因爲沒法實際測試，所以不知道具體的效果怎麼樣，收斂效率怎麼樣。
2 全局定位似乎不一定能解決局部收斂問題，比如集中在某幾個地方會存在大量的粒子；
3 在定位的過程中也會構建新的地圖，但是這個地圖沒有與傳入的地圖融合

（最後就是發一些感慨，最近疫情期間，在家學習效率太低了……希望借這篇博客給自己提提神，接下來抓緊時間多學習啊啊啊啊 最後的最後 武漢加油！！！