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

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

SLAM問題的提出

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

數(shù)學上的狀態(tài)估計

通常在數(shù)學上，可以將SLAM建模成一個狀態(tài)估計問題，即：

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

動態(tài)貝葉斯網(wǎng)絡描述

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

圖1.動態(tài)貝葉斯網(wǎng)絡

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

問題求解方法

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

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

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

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

2D與3D激光傳感器

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

圖3.2D激光傳感器表示

圖4.3D激光傳感器表示

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

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

延伸出的兩類SLAM算法

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

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

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

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

B.基于圖優(yōu)化的方法

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

圖5.基于圖優(yōu)化的激光SLAM

前端負責從里程計和激光數(shù)據(jù)中求得pose，后端采用回環(huán)檢測，構(gòu)造閉環(huán)約束，通過最小化觀測和估計殘差求得優(yōu)化后的pose。

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

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

圖6.圖優(yōu)化約束構(gòu)建

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

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

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

圖7.機器人運動軌跡

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

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

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

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

圖8.閉環(huán)優(yōu)化效果

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

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

B.如何克服動態(tài)環(huán)境下的定位漂移問題？

C.幾何結(jié)構(gòu)相似的環(huán)境如何避免閉環(huán)錯誤？

D.里程計初始值異常跳變?nèi)绾螜z測？如何處理？

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

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

G....

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

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

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

圖10.高仙SLAM-Mapping the World

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