最近打工接觸了快一年的2d slam工作，因為先前毫無這方面的知識儲備，一直都理解不了為什么cartographer在開啟IMU和輪速計數(shù)據(jù)輸輸入后，依舊無法在退化環(huán)境中工作，按理來說有了IMU和輪速計，完全可以用慣導的思路做位姿推算，但cartographer就是不行，遇到長走廊就呆在原地不動??。

正好最近也在準備解決這個問題，就看源碼完整過了一遍cartographer 2d 建圖的工作流程，初步猜測這個問題的原因是cartographer根本就沒有充分利用IMU和輪速計的數(shù)據(jù)做位姿推算，甚至后端都沒有任何關于這兩個傳感器的跡象，簡直無大語??。

下面是我在cartographer中看到涉及到和位姿推算的流程，核心代碼主要集中在local_trajectory_builder_2d里，同時涉及到global_trajectory_builder和pose_extrapolator：

IMU和里程計數(shù)據(jù)更新，一路往下，存入extrapolator_里，并按一定策略對數(shù)據(jù)進行丟棄；
點云數(shù)據(jù)更新，數(shù)據(jù)先進入到global_trajectory_builder中，然后再從AddSensorData到AddAccumulatedRangeData；
AddSensorData中先對點云做各種濾波處理，AddAccumulatedRangeData將處理后的點云傳遞到前端匹配部分；
前端匹配先使用當前時間作為參數(shù)調(diào)用extrapolator_.ExtrapolatePose預估當前位姿；
將extrapolator_預估的三維空間pose投影到二維平面；
執(zhí)行點云自適應體素濾波，并將點云轉換到PointCloud類型；
使用預估的二維pose和點云在已有的地圖上做匹配，成功后將匹配的二維pose轉換回三維空間的pose后傳給extrapolator_.AddPose作為下一次推算的先驗；
使用匹配的二維pose對點云做坐標變換，并轉換成內(nèi)部的PointCloud類型；
最后將PointCloud和轉換的三維pose傳入InsertIntoSubmap，更新子圖，并返回匹配關系和子圖到global_trajectory_builder中更新pose_graph_；

從上面這流程里可以看出，主要就是一個 "預估位姿 -> 點云和地圖匹配新位姿 -> 更新先驗位姿" 大循環(huán)。

預估位姿：基于先驗位姿，使用輪速計估算位移，使用IMU估算旋轉；

點云和地圖匹配：在預估位姿附近做匹配，得到新位姿；

更新先驗：將新位姿作為下一次預估的先驗值；

問題也就出在點云和地圖匹配到更新先驗位姿這一步，點云匹配出來的新位姿沒有任何處理就直接作為下一次推算的先驗值，如果是退化環(huán)境或者是別的什么原因?qū)е缕ヅ涞慕Y果是錯誤的，后續(xù)全部的位姿也都是錯的了。這也是為什么一進到細長走廊cartographer的定位就停在原地的原因，直到雷達能掃描到走廊的另一頭才重新恢復移動，但這時候一切都已經(jīng)錯了，還沒有任何糾正補救的措施。

想要解決這個問題，切入點我想應該就是匹配到更新先驗這里，需要用卡爾曼濾波之類的東西對預估位姿和匹配位姿做一下融合再作為新位姿更新先驗值。

不過到底行不行還要等后面做實驗驗證。