揭開關(guān)于機(jī)器人自主移動的神秘面紗

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

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

SLAM問題的提出

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

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

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

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

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

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

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

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

問題求解方法

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

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

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

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

2D與3D激光傳感器

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

圖3.2D激光傳感器表示

圖4.3D激光傳感器表示

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

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

延伸出的兩類SLAM算法

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

G....

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

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

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

圖10.高仙SLAM-Mapping the World

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