激光追蹤演算法

A. SLAM與VSLAM有什麼區別

一、成熟度不同

激光 SLAM 比 VSLAM 起步早，在理論、技術和產品落地上都相對成熟。基於視覺的 SLAM 方案目前主要有兩種實現路徑，一種是基於 RGBD 的深度攝像機，比如 Kinect；還有一種就是基於單目、雙目或者魚眼攝像頭的。

VSLAM 目前尚處於進一步研發和應用場景拓展、產品逐漸落地階段。

二、應用場景

從應用場景來說，VSLAM 的應用場景要豐富很多。VSLAM 在室內外環境下均能開展工作，但是對光的依賴程度高，在暗處或者一些無紋理區域是無法進行工作的。而激光 SLAM 目前主要被應用在室內，用來進行地圖構建和導航工作。

三、地圖精度不同

激光 SLAM 在構建地圖的時候，精度較高；

VSLAM，比如常見的，用的非常多的深度攝像機 Kinect，（測距范圍在 3-12m 之間），地圖構建精度約 3cm；所以激光 SLAM 構建的地圖精度一般來說比 VSLAM 高，且能直接用於定位導航。

四、易用性不同

激光 SLAM 和基於深度相機的 VSLAM 均是通過直接獲取環境中的點雲數據，根據生成的點雲數據，測算哪裡有障礙物以及障礙物的距離。

但是基於單目、雙目、魚眼攝像機的 VSLAM 方案，則不能直接獲得環境中的點雲，而是形成灰色或彩色圖像，需要通過不斷移動自身的位置，通過提取、匹配特徵點，利用三角測距的方法測算出障礙物的距離。

B. 基於激光雷達的SLAM和路徑規劃演算法研究與實現

本文僅供學習使用，並非商業用途，全文是針對哈爾濱工業大學劉文之的論文《移動機器人的路徑規劃與定位技術研究》進行提煉與學習。論文來源中國知網，引用格式如下：
[1]劉文之. 基於激光雷達的SLAM和路徑規劃演算法研究與實現[D].哈爾濱工業大學,2018.

相關坐標系轉換原理已經在前一篇文章寫完了，直接上轉換方程。

這里他的運動模型選擇的是基於里程計的運動模型，還有一種基於速度的運動模型，其實都差不多，整體思想都一樣。里程計是通過計算一定時間內光電編碼器輸出脈沖數來估計機器人運動位移的裝置，主要是使用光電碼盤。根據光電碼盤計算出此時輪子的速度，然後通過已知的輪子半徑來獲得單位時間 每個輪子 的位移增量。

高等數學可知單位時間位移增量就是速度，對速度在一定時間上進行積分就得到這一段時間所走過的路程。

根據上圖，我們可以求出來機器人航向角角速度、圓弧運動半徑和機器人角度變化量，由此可以解的機器人在當前時刻的位姿。

實際上也是有誤差，所以單獨依靠里程計會與實際結果產生較大誤差，所以必須引入其他的外部感測器對外部環境的觀測來修正這些誤差，從而提高定位精度。

首先肯定需要將激光雷達所測得的端點坐標從極坐標、機器人坐標中轉換到世界坐標中。

這張略過，暫時不需要看這個

路徑規劃演算法介紹：

因為該演算法會產生大量的無用臨時途徑，簡單說就是很慢，所以有了其他演算法。

了解兩種代價之後，對於每一個方塊我們採用預估代價與當前路徑代價相加的方法，這樣可以表示每一個路徑點距離終點的距離。在BFS搜索過程的基礎上，優先挑選總代價最低的那個路徑進行搜索，就可以少走不少彎路。（演算法講解 https://www.bilibili.com/video/BV1bv411y79P?from=search&seid=3623681329596549549 ）

在局部路徑規劃演算法之中，我們選用DWA演算法（dynamic window approach），又叫動態窗口法。動態窗口法主要是在速度(v, w)空間中采樣多組速度，並模擬機器人在這些速度下一定時間內的軌跡。在得到多組軌跡後，對這些軌跡進行評價，選取最優的軌跡所對應的速度來驅動機器人運動。
state sampling就是按照之前給出的全局路徑規劃，無論是Dijkstra還是A* 都可以方便的得到state sampling，DWA演算法所需要提前建立的action sampling有兩種：

但是無論是什麼情況，上述所做的工作就是把機器人的位移轉化到世界坐標中來，而不是機器人坐標系。速度采樣結束之後，只需要對小車的軌跡進行評判，就可以得到最優解了。下面介紹速度采樣的辦法。

對速度進行采樣一般有以下三個限制：

當確定了速度范圍之後，就需要根據速度解析度來對小車速度離散化，在每一時刻將小車在不同直線速度角速度組合下所即將要行駛的距離都可視化出來。

其中每一條軌跡都是很多小直線連接起來的。

需要用評價函數來對上述軌跡進行選擇，選擇最適合的軌跡

最後為了讓三個參數在評價函數里所發揮的作用均等，我們使用歸一化處理來計算權重。

演算法流程整體如下：

C. 激光雷達SLAM包括以下幾種方案:

激光雷達SLAM包括以下幾種方案:

直接法, 直接法的典型代表是ICP和NDT系列，ICP可以直接計算出激光的位姿，多幀可以構成聯合優化，這種方案比較簡單但是比較有效，常用於激光雷達的多程對齊。

基於特徵的匹配(LO)，該方案的典型代表是LOAM和後續的改進方案A-LOAM/F-LOAM,該方案通過尋找線面特徵，並通過特徵匹配來計算幀間的位姿，多個位姿可以做BA優化。

多感測器融合的方案。該方案的典型代表是LIO-Mapping,LINS和LIO-SAM.LIO-Mapping演算法借鑒VINS-Mono的預積分和後端優化，前端視覺里程計改成激光里程計。

基於柵格的，該方案的代表是谷歌開源的cartography，這種方案在室內的機器人定位比較有優勢。

基於面元，該方案的典型代表是suma。

基於語義信息，該方案的典型代表是segmap和suma++。

高精地圖的生產包括語義信息的提取(通常來自視覺，部分也可來自激光),單程LIO(gnss+imu+dmi+lidar/visual odometry),多程對齊。說一下每一部分的難點吧:

圖像語義信息的提取。圖像的語義信息包括車道線，桿狀物體，牌子，地面車信等；在高速場景，物體遮擋較少，物體檢測的准確率是可以做到95%以上的；在城市道路，由於樹木的遮擋(對牌子和桿狀物體的遮擋), 車輛遮擋(對車道線和地面車信的遮擋),目前的檢測是很難做到90%以上的。

點雲的語義信息提取。點雲對於特殊材料反射率較大(如車道線),基本上二分就可以解決很多問題，對於高速場景，車道線磨損並不嚴重，語義信息提取較容易；對於城市道路車道線磨損嚴重，且存在老舊的車道線的干擾，語義信息的提取是很難做到90%以上的。

LIO. 高精採集車一般配備激光雷達，相機，imu, dmi和RTK等設備，對單程軌跡來說，是可以採用多感測器融合的。對於高速場景，建築物遮擋較少，RTK信號較好，後結算的RTK的精度是可以做到30cm以內的(在山區和隧道場景例外)，自動化比較難做，這個和場景有關系。

多程融合。主要看人眼去分辨點雲是否對齊，當然是可以建立小規模數據集進行評測，自動化率非常低。

總的來說，目前高精地圖的生產是很難做到自動化的，主要原因是場景復雜,corner case太多，絕對精度和相對精度都很難滿足要求。

總結一下，目前激光SLAM的直接法比較簡單，可以用於激光里程計的多程對齊或回環檢測; 純LO的演算法目前工業圈很少使用，一般採用多感測器融合的方案，畢竟激光雷達都用了，也不差IMU這樣的感測器。多感測器融合的方案主要用於高精地圖的製作，自動駕駛領域一般採用高精地圖作為先驗來定位，不會同時定位和建圖。基於柵格的可以用於移動機器人，在室內環境柵格假設大多數還是有效的，在室外場景的話，一般會採用NDT Map來存儲地圖。基於語義信息和面元的話，個人不太了解，在工業圈的使用也不多。

D. 激光雷達SLAM演算法

機器人研究的問題包含許許多多的領域，我們常見的幾個研究的問題包括：建圖(Mapping)、定位(Localization)和路徑規劃（Path Planning），如果機器人帶有機械臂，那麼運動規劃（Motion Planning）也是重要的一個環節，SLAM需要機器人在未知的環境中逐步建立起地圖，然後根據地區確定自身位置，從而進一步定位。

ROS系統通常由大量節點組成，其中任何一個節點均可以通過發布/訂閱的方式與其他節點進行通信。舉例來說，機器人上的一個位置感測器如雷達單元就可以作為ROS的一個節點，雷達單元可以以信息流的方式發布雷達獲得的信息，發布的信息可以被其他節點如導航單元、路徑規劃單元獲得。

ROS的通信機制：

ROS（機器人操作系統）中SLAM的一些功能包，也就是一些常用的SLAM演算法，例如Gmapping、Karto、Hector、Cartographer等演算法。我們不會去關注演算法背後的數學原理，而是更注重工程實現上的方法，告訴你SLAM演算法包是如何工作的，怎樣快速的搭建起SLAM演算法。

地圖 ： ROS中的地圖很好理解，就是一張普通的灰度圖像，通常為pgm格式。這張圖像上的黑色像素表示障礙物，白色像素表示可行區域，灰色是未探索的區域

地圖在ROS中是以Topic的形式維護和呈現的，這個Topic名稱就叫做 /map ，由於 /map 中實際上存儲的是一張圖片，為了減少不必要的開銷，這個Topic往往採用鎖存（latched）的方式來發布。地圖如果沒有更新，就維持著上次發布的內容不變，此時如果有新的訂閱者訂閱消息，這時只會收到一個 /map 的消息，也就是上次發布的消息；只有地圖更新了（比如SLAM又建出來新的地圖），這時 /map 才會發布新的內容。 這種方式非常適合變動較慢、相對固定的數據（例如地圖），然後只發布一次，相比於同樣的消息不定的發布，鎖存的方式既可以減少通信中對帶寬的佔用，也可以減少消息資源維護的開銷。

Gmapping ，Gmapping演算法是目前基於激光雷達和里程計方案裡面比較可靠和成熟的一個演算法，它基於粒子濾波，採用RBPF的方法效果穩定，許多基於ROS的機器人都跑的是gmapping_slam。

gmapping的作用是根據激光雷達和里程計（Odometry）的信息，對環境地圖進行構建，並且對自身狀態進行估計。因此它得輸入應當包括激光雷達和里程計的數據，而輸出應當有自身位置和地圖。

論文支撐：R-LINS: A Robocentric Lidar-Inertial State Estimator for Robust and Efficient Navigation

6軸 IMU：高頻，聚焦自身運動，不採集外界環境數據
3D LiDAR：低頻，聚焦車體運動，採集外界環境數據

R-LINS使用以上兩種感測器來估計機器人的運動姿態， 對於任一感測器而言，單獨的依靠自己的數據是很難實現地圖構建的， 比如純雷達模型使用的感測器是激光雷達，可以很好的探測到外界的環境信息。但是，同樣的，也會受到這些信息的干擾，再長時間的運算中會產生一定的累計誤差。為了防止這種誤差干擾到後續的地圖構建中，需要使用另一種感測器來矯正機器人自身的位姿信息， 即IMU感測器，IMU感測器由於是自身運動估計的感測器，所以，採集的都是自身運動的姿態信息。可以很好的矯正激光雷達里程計的位姿信息。所以，通常使用激光雷達和慣導來進行數據融合，實現姿態信息的矯正。

一共分為三大塊：