本文開(kāi)始，我們將從介紹其中的一個(gè)關(guān)鍵性模塊，即時(shí)定位與建圖技術(shù)（Simultaneous Localization and Mapping，以下簡(jiǎn)稱SLAM）入手，逐步為讀者揭開(kāi)關(guān)于機(jī)器人自主移動(dòng)的神秘面紗。

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

SLAM問(wèn)題的提出

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

數(shù)學(xué)上的狀態(tài)估計(jì)

通常在數(shù)學(xué)上，可以將SLAM建模成一個(gè)狀態(tài)估計(jì)問(wèn)題，即：

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

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

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

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

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

問(wèn)題求解方法

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

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

圖2.a.TOF測(cè)距激光雷達(dá)的工作原理

圖2.b.三角測(cè)距激光雷達(dá)的工作原理

2D與3D激光傳感器

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

圖3.2D激光傳感器表示

圖4.3D激光傳感器表示

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

通過(guò)求最大條件后驗(yàn)概率(MAP)，計(jì)算出相應(yīng)的pose和地圖點(diǎn)坐標(biāo)，即

延伸出的兩類SLAM算法

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

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

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

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

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

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

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

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

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

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

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

圖中機(jī)器人從i點(diǎn)運(yùn)動(dòng)到j(luò)點(diǎn)，根據(jù)前端我們計(jì)算出了機(jī)器人的在j點(diǎn)的pose，即xj，則xj在xi為坐標(biāo)系的坐標(biāo)是z_hat_ij，此時(shí)我們又獲得一個(gè)激光的觀測(cè)，通過(guò)激光幀匹配得到xj在xi坐標(biāo)為z_ij。假設(shè)觀測(cè)和實(shí)際運(yùn)動(dòng)是完美的，則z_ij=z_hat_ij。

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

我們?cè)倥e一個(gè)閉環(huán)檢測(cè)的例子，如圖7所示：

圖7.機(jī)器人運(yùn)動(dòng)軌跡

機(jī)器人從1點(diǎn)運(yùn)動(dòng)到4點(diǎn)，假設(shè)機(jī)器人運(yùn)動(dòng)到4點(diǎn)時(shí)，觀測(cè)到了1點(diǎn)，此時(shí)，通過(guò)前端匹配計(jì)算出4點(diǎn)在1點(diǎn)的pose，即z14，同時(shí)因?yàn)闄C(jī)器人從1->2->3->4，通過(guò)pose間變換，估計(jì)出了4點(diǎn)在1點(diǎn)的pose，hat_z_14

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

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

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

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

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

A.如何有效的對(duì)激光噪點(diǎn)進(jìn)行去噪？

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

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

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

E.如何克服重復(fù)掃圖導(dǎo)致的地圖分辨率下降？

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

G....

最后筆者認(rèn)為，作為一名高仙的SLAM算法工程師，不僅需要深耕理論，同時(shí)對(duì)于工程問(wèn)題也應(yīng)該有更多的思考和改進(jìn)。

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

圖9.室外百萬(wàn)平米建圖-龍東大道

圖10.高仙SLAM-Mapping the World

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