本文開始，我們將從介紹其中的一個關鍵性模塊，即時定位與建圖技術（Simultaneous Localization and Mapping，以下簡稱SLAM）入手，逐步為讀者揭開關于機器人自主移動的神秘面紗。

高仙是全球較早從事自主移動技術研發與應用探索的機器人公司之一。成立至今，高仙不斷打磨并完善了機器人全場景移動技術，并通過全球超過50+機型、1萬+機器人終端在5000+行業的落地積累，實現了從室內到室外等不同環境、從清潔到安防等不同業務領域的廣泛應用。高仙機器人全場景移動技術給越來越多的機器人賦予了自主移動必需的“軀殼”、“大腦”和“靈魂”。

SLAM問題的提出

試想人如何判斷自己在周圍環境中的位置？沒錯，眼睛！不過，雙腳的移動，大腦對運動的感知等也能給你提供相對運動的信息。事實上，人就是通過綜合這些感官信息來判斷位置和移動的，這也是SLAM技術的靈感來源。SLAM技術的出現徹底解決了機器人領域中“我在哪兒”的問題，使機器人在未知環境中的自主移動成為可能。SLAM通過輸入的多種外部和內部的傳感器數據，使用算法求解出一個準確的機器人位姿（即位置和姿態，可以理解為坐標和朝向，以下簡稱pose），同時，將每個pose處得到的傳感器數據拼接起來形成完整的地圖，又可作為計算位姿的依據。

數學上的狀態估計

通常在數學上，可以將SLAM建模成一個狀態估計問題，即：

其中，公式(1)為運動方程，表示在k時刻，機器人的pose(x(k))由k-1時刻的pose(x(k-1))和k時刻的運動輸入(u(k))所決定，由于實際物理環境總會引入誤差，所以添加一個噪聲量(v(k))對狀態變化形成一定約束。公式(2)為觀測方程，表示k時刻的機器人傳感器觀測(z(k))，由當前時刻的機器人pose所決定。同理，因為物理環境的影響，會帶入一定的觀測誤差，即w(k)。

動態貝葉斯網絡描述

上述過程也可以使用動態貝葉斯網絡(Dynamic Bayes Network,DBN)來描述，如圖1所示：

圖1.動態貝葉斯網絡

整個網絡就是一個概率圖模型，箭頭的指向表示變量的依賴關系，如上圖中的子圖x0->x1，和u1->x1，可以用條件概率描述。

問題求解方法

由于目前激光SLAM作為成熟的算法已經在無人駕駛和機器人中得到廣泛應用，因此本文主要介紹激光SLAM方案。

顧名思義，激光SLAM是以激光數據作為傳感器輸入的SLAM方案。圖2.a展示了TOF激光雷達的測距原理，通過電機帶動旋轉，將激光脈沖不斷投射到障礙物同時接收反射回的激光脈沖，將光速與飛行時間差相乘，求得雷達到相應障礙物的距離（TOF測距原理測距范圍廣。此外還有基于三角測距的激光雷達，主要針對室內的中近距離測距，圖2.b展示了三角測距激光雷達的工作原理。）。

圖2.a.TOF測距激光雷達的工作原理

圖2.b.三角測距激光雷達的工作原理

2D與3D激光傳感器

圖3為2D激光傳感器數據的表示。左圖為工作區間在270度激光數據，可以看到激光從+135度掃描到-135度，獲取了水平面上每隔一個角分辨率的測距信息。右圖為ros中激光的數據顯示。圖4為3D激光的數據表示，相對于2D激光雷達，3D激光在垂直方向會同時發射多組激光脈沖，獲得了三維空間中對障礙物的測距信息。

圖3.2D激光傳感器表示

圖4.3D激光傳感器表示

由于SLAM本質是個系統狀態估計問題，即在給定系統輸入的條件下，估計出機器人的pose和地圖點的坐標，針對兩維空間的SLAM，式(1)具體可以表示為：

通過求最大條件后驗概率(MAP)，計算出相應的pose和地圖點坐標，即

延伸出的兩類SLAM算法

A.基于貝葉斯濾波器的方法

基于貝葉斯濾波器的方法包括Kalman filter[1]、Extended Kalman fiter、Particle filter等，是貝葉斯迭代狀態估計理論，即先對機器人運動進行建模，構造出貼合物理場景的運動方程和觀測方程，如輪式機器人常使用基于速度的運動模型。之后，套用卡爾曼濾波的五條公式，進行狀態預測和測量更新。狀態預測依靠運動方程，從當前狀態估計出下一時刻的機器人pose。而測量更新，則是在機器人觀測到新的點時，對之前的預測值進行修正。可以看到，該過程是一個遞歸估計過程，從k時刻到k+1時刻的估計。

由于卡爾曼濾波算法是針對線性系統且高斯分布的最優無偏估計，而實際場景中，機器人的運動并不滿足線性特性，且噪聲項不滿足高斯分布，因此使用卡爾曼濾波不能精確的計算出結果。擴展卡爾曼濾波可以將線性系統約束擴展到非線性系統，獲得更好的結果。然而其依舊不能逃出高斯分布的限制，因而實際中使用粒子濾波代替上述方案。粒子濾波類SLAM不依賴參數化的運動方程，使用大規模粒子點去模擬無參數化的分布，理論上可以近似各種分布。如早年的業界流行的GMapping，即采用了該方案。

濾波類算法最大的問題，是無法處理大尺度場景的建圖。由于濾波類算法是基于遞歸計算，下一時刻的估計值依賴于上一時刻的估計，因而在大尺度場景下，由于系統參數和傳感器觀測的不確定性，會造成誤差的逐漸累積，一旦當前時刻的估計出現偏差，之后是無法修正該誤差的，其結果是最后無法獲得一致性的地圖。

B.基于圖優化的方法

基于圖優化的SLAM出現解決了一致性建圖的問題。如果說濾波類SLAM是屬于序慣估計的話，基于圖優化則是屬于批處理。圖優化SLAM是目前主流的SLAM方案。其主要分為兩個模塊，前端和后端。如圖5所示。

圖5.基于圖優化的激光SLAM

前端負責從里程計和激光數據中求得pose，后端采用回環檢測，構造閉環約束，通過最小化觀測和估計殘差求得優化后的pose。

前端方面，先從里程計獲得初始的pose，之后通過激光數據連續幀匹配，求得激光約束后的pose(注意這里使用了一次觀測信息)。在連續幀匹配上，有ICP[2]、NDT[3]、暴力匹配[4]等算法。如目前主流的激光SLAM-cartographer采用暴力匹配作為前端方法，即使用激光掃描匹配，在機器人運動中，通過匹配前后兩幀的激光變化，求得機器人的相對運動。而其中的一個關鍵模塊是三維窗口遍歷尋優。這是一種窮舉搜索算法，針對平面運動的機器人，可以將pose分解為三個維度，即x軸，y軸，角度軸。通過構建三層for循環，分別對x方向、y方向、旋轉角度方向進行遍歷搜索，尋找到最優的pose匹配作為估計結果。

需要強調的是，前端屬于局部估計，即只能估計當前時刻機器人相對上一個時刻的pose變化。事實上，由于估計所引入的誤差，隨著運動范圍的擴大，該誤差會逐漸累積，導致最后錯誤的結果(這個問題和濾波器類SLAM一樣)。解決該問題，可以使用閉環檢測(Loop Closure)[5]的圖優化后端。通過判斷機器人是否回到歷史中的某一點，從而構建閉環約束，通過非線性優化將運動過程中的誤差分散到參與優化的每個pose中，從而消除誤差累積。圖優化的核心是構造誤差函數，我們先舉一個簡單的例子，如圖6所示

圖6.圖優化約束構建

圖中機器人從i點運動到j點，根據前端我們計算出了機器人的在j點的pose，即xj，則xj在xi為坐標系的坐標是z_hat_ij，此時我們又獲得一個激光的觀測，通過激光幀匹配得到xj在xi坐標為z_ij。假設觀測和實際運動是完美的，則z_ij=z_hat_ij。

實際中，準確的xi和xj應該滿足上述誤差最小，該過程構造了無約束的非線性優化問題，之后就是借助常用的優化方法做梯度下降了。

我們再舉一個閉環檢測的例子，如圖7所示：

圖7.機器人運動軌跡

機器人從1點運動到4點，假設機器人運動到4點時，觀測到了1點，此時，通過前端匹配計算出4點在1點的pose，即z14，同時因為機器人從1->2->3->4，通過pose間變換，估計出了4點在1點的pose，hat_z_14

實際中，都是以pose間的變換矩陣作為變量參與優化。

通過對比圖優化和濾波器SLAM，兩者的本質是統一的，都是通過輸入觀測，修正預測量，求解最大后驗概率，圖優化的方式只是將最大后驗通過計算負對數的形式，轉換成最小化帶有誤差項的二次型。而兩者區別是，圖優化技術采用了批處理方式，引入了更強的約束（閉環約束），修正了濾波器基于一階馬爾科夫假設的狀態遞歸估計錯誤累積。

圖8顯示了閉環的效果，左圖中，未使用閉環檢測糾正pose，可以看到機器人行駛一周之后，在應該回到原點的位置，pose出現較大偏差，導致地圖沒有正確閉合。右圖中，由于使用了閉環檢測，糾正了pose的累積誤差，使得建圖與實際場景一致，取得了較好的效果。

圖8.閉環優化效果

最后需要補充的是，由于激光SLAM算法框架較為固定，大同小異，那么決定一個SLAM算法的好壞就落在了對具體工程問題的處理上，如筆者在日常開發中，處理并解決了以下的問題：

A.如何有效的對激光噪點進行去噪？

B.如何克服動態環境下的定位漂移問題？

C.幾何結構相似的環境如何避免閉環錯誤？

D.里程計初始值異常跳變如何檢測？如何處理？

E.如何克服重復掃圖導致的地圖分辨率下降？

F.碼盤里程計與激光數據時間戳雖然一致，但實際上存在延遲，如何解決？

G....

最后筆者認為，作為一名高仙的SLAM算法工程師，不僅需要深耕理論，同時對于工程問題也應該有更多的思考和改進。

正是這樣的心態和追求，才奠定了高仙在機器人自主移動領域的領先地位。目前高仙擁有百萬平米的建圖能力，超出行業平均水平20倍。

圖9.室外百萬平米建圖-龍東大道

圖10.高仙SLAM-Mapping the World

至此，我們對SLAM技術要解決的問題，以及在機器人、無人駕駛、無人機等領域內比較成熟的激光SLAM算法有了一定的認識。SLAM對于機器人的行動和交互起到至關重要的作用，它是使智體知道自己在那里、周圍環境如何以及下一步該如何行動的關鍵性基礎。