基于單目相機(jī)與里程計(jì)的室內(nèi)機(jī)器人定位研究

溫倩雯，蘇毅輝，李 庚

（福州大學(xué) 物理與信息工程學(xué)院，福建 福州 350108）

0 引 言

隨著機(jī)器人技術(shù)的發(fā)展，越來越多需要人力完成的重復(fù)煩瑣工作可利用機(jī)器人完成。對(duì)于自主移動(dòng)機(jī)器人，尤其是室內(nèi)動(dòng)態(tài)環(huán)境中的服務(wù)型移動(dòng)機(jī)器人，準(zhǔn)確的自我定位技術(shù)尤為重要。傳統(tǒng)方法常采用在室內(nèi)放置如紅外和超聲波發(fā)射裝置等主動(dòng)信號(hào)源［1］，或布置一些已知信息的路標(biāo)實(shí)現(xiàn)定位任務(wù)［2］。但是，這些方法需要安裝額外的裝置或?qū)Νh(huán)境進(jìn)行特殊布置，無法做到普遍適用。

Smith等提出的同時(shí)定位與建圖（Simultaneous Localization And Mapping，SLAM）技術(shù)在不依賴外部設(shè)備或特殊標(biāo)志的情況下，利用傳感器信息計(jì)算機(jī)器人位姿，并且構(gòu)建環(huán)境地圖，逐漸成為解決室內(nèi)服務(wù)機(jī)器人定位問題的熱門技術(shù)之一［3］。在室內(nèi)動(dòng)態(tài)環(huán)境、天花板信息基本穩(wěn)定不變的情況下，選擇其作為視覺信息將減少動(dòng)態(tài)因素改變所帶來的誤差。此外，學(xué)術(shù)研究［4-5］和市場(chǎng)應(yīng)用方面也逐漸出現(xiàn)了采用攝像頭朝向天花板方向的SLAM定位方案實(shí)例，如科沃斯和石頭等掃地機(jī)器人。

針對(duì)室內(nèi)場(chǎng)景下的機(jī)器人應(yīng)用，本文提出兩點(diǎn)改進(jìn)。一是選擇體積小、成本低且適合在嵌入式系統(tǒng)上實(shí)現(xiàn)的單目相機(jī)作為視覺傳感器，通過固定攝像頭方向使其朝向天花板，以克服室內(nèi)動(dòng)態(tài)場(chǎng)景下視覺信息變化多端不可靠的問題，同時(shí)優(yōu)化特征匹配算法，以應(yīng)對(duì)紋理重復(fù)度高或紋理稀疏的天花板場(chǎng)景。二是融合輪式里程計(jì)，結(jié)合里程計(jì)預(yù)積分信息，為視覺里程計(jì)提供初值，加速非線性優(yōu)化收斂速度，以保證視覺里程計(jì)在稀疏紋理場(chǎng)景中不丟失。

1 系統(tǒng)框架

該定位系統(tǒng)主要分為視覺跟蹤、局部建圖及全局優(yōu)化3個(gè)模塊，如圖1所示。視覺跟蹤模塊通過追蹤特征點(diǎn)建立幾何關(guān)系以篩選出關(guān)鍵幀。局部建圖模塊求解機(jī)器人位姿。全局優(yōu)化模塊通過回環(huán)檢測(cè)消除累積誤差，提高機(jī)器人定位的精度。此外，重定位功能可以在定位丟失或機(jī)器人被人為移動(dòng)后重新進(jìn)行定位，極大地提升了機(jī)器人定位的魯棒性。

圖1 定位系統(tǒng)流程圖

2 系統(tǒng)實(shí)現(xiàn)

系統(tǒng)參考ORB-SLAM3的結(jié)構(gòu)實(shí)現(xiàn)。

2.1 視覺里程計(jì)

對(duì)于空間中一個(gè)特征點(diǎn)WP=W(X,Y,Z)，在相機(jī)坐標(biāo)系下記為CP=C(X,Y,Z)，通過針孔模型投影到相機(jī)平面為p(u,v)。將這樣的投影關(guān)系描述為：

式中，K為相機(jī)的內(nèi)參數(shù)矩陣。這里先不考慮相機(jī)的畸變，于是有：

式中，WφC表示世界坐標(biāo)到相機(jī)坐標(biāo)的旋轉(zhuǎn)，是一個(gè)3×3的正交矩陣；WΔC表示世界坐標(biāo)到相機(jī)坐標(biāo)的1×3平移向量；符號(hào)d(·)表示齊次坐標(biāo)變換。

在圖像上提取特征點(diǎn)時(shí)，需要特別關(guān)注特征點(diǎn)旋轉(zhuǎn)和光照不變性。室內(nèi)環(huán)境存在大量人工光源，光源距離不同會(huì)導(dǎo)致較大的明暗差異。對(duì)同一場(chǎng)景下的天花板圖片分別用 ORB［6］、SURF［7］、GFTT［8］、FAST［9］等常用特征點(diǎn)提取特征，結(jié)果如圖2所示，從左上至右下的特征點(diǎn)分別為SIFT、ORB、BRISK、KAZE、AKAZE、FAST、AGAST、GFTT和SURF。由實(shí)驗(yàn)結(jié)果可知，SURF特征點(diǎn)在不同光照條件下均有較好的表現(xiàn)。相比于其他特征點(diǎn)，無論場(chǎng)景中是否存在人造光源，其數(shù)量及分布都表現(xiàn)得更為魯棒。

圖2 不同特征點(diǎn)提取算法的效果對(duì)比圖

對(duì)于視覺里程計(jì)而言，準(zhǔn)確的圖像配準(zhǔn)結(jié)果是后續(xù)所有工作的基礎(chǔ)，需先保證配準(zhǔn)結(jié)果的準(zhǔn)確度。在特征匹配方面，考慮到掃地機(jī)器人移動(dòng)速度較低，前后幀相機(jī)位姿變換量較小，故采用基于鄰域的特征點(diǎn)搜索算法替代傳統(tǒng)的全局搜索，以顯著降低計(jì)算量。由于天花板視圖具有紋理重復(fù)的特性，在圖像配準(zhǔn)過程中更容易得到誤匹配，如圖3（a）所示，其中綠色直線為正確匹配點(diǎn)對(duì)，藍(lán)色直線為誤匹配點(diǎn)對(duì)。為了改善特征點(diǎn)匹配的精度，依據(jù)里程計(jì)變化值預(yù)估出圖像的仿射變換矩陣，并對(duì)參考幀的特征點(diǎn)做仿射投影，以限制鄰域搜索范圍。仿射變換是線性變換與平移變換的疊加，具有共線性、平行性及共線比例不變性的性質(zhì)。進(jìn)行變換后，圖像結(jié)構(gòu)信息不會(huì)發(fā)生變化。

通過圖3的匹配結(jié)果和圖4的統(tǒng)計(jì)數(shù)據(jù)可知，在一定范圍內(nèi)，兩幀間隔越大，本方法的改進(jìn)效果越明顯。特別地，在重復(fù)紋理下，當(dāng)兩幀視圖存在較大旋轉(zhuǎn)時(shí)，普通方法下的錯(cuò)誤匹配極高，內(nèi)點(diǎn)率僅達(dá)30%，而本文所改進(jìn)方法在該情況下內(nèi)點(diǎn)率仍可達(dá)到85%，如圖3（b）所示?；诶锍逃?jì)信息的特征點(diǎn)鄰域搜索算法可顯著降低誤匹配率，提升匹配精度，同時(shí)降低計(jì)算量。

圖3 紋理重復(fù)的場(chǎng)景下傳統(tǒng)特征點(diǎn)匹配算法與本文改進(jìn)算法匹配結(jié)果對(duì)比圖

對(duì)于單目相機(jī)，僅憑單張圖像無法獲得所求點(diǎn)的深度信息，需通過三角測(cè)量的方法估計(jì)地圖點(diǎn)的深度。三角測(cè)量由相機(jī)平移得到，測(cè)量結(jié)果存在不確定性，如圖5所示。紅色部分為測(cè)量的不確定度，當(dāng)平移較小時(shí)，深度估計(jì)上的不確定性較大，三角化精度過低。但是，增大平移可能導(dǎo)致圖像觀測(cè)發(fā)生過大變化而無法保證足夠多的匹配數(shù)量，因此存在三角測(cè)量的矛盾。本文采用深度濾波［10］思想，通過融合多幀多次三角化測(cè)量結(jié)果以克服偶然誤差。

圖4 傳統(tǒng)特征點(diǎn)匹配算法及本文改進(jìn)算法匹配結(jié)果統(tǒng)計(jì)數(shù)據(jù)對(duì)比

圖5 三角化誤差示意圖

2.2 數(shù)據(jù)融合

輪式里程計(jì)通過航跡推算的方式獲得機(jī)器人軌跡絕對(duì)值，可以幫助單目視覺確定絕對(duì)尺度。但是，由于輪胎打滑等因素的影響，里程計(jì)信息會(huì)造成累積漂移誤差。將視覺信息與輪式里程計(jì)信息相結(jié)合可優(yōu)劣互補(bǔ)，但在實(shí)際應(yīng)用中，即使按照固定頻率采集，圖像與輪式里程計(jì)數(shù)據(jù)仍會(huì)在硬件和軟件等相關(guān)因素的影響下無法得到時(shí)間戳嚴(yán)格對(duì)齊的數(shù)據(jù)。因此，在兩種數(shù)據(jù)結(jié)合使用前應(yīng)先對(duì)其進(jìn)行時(shí)間戳對(duì)齊。一般情況下，里程計(jì)的采樣頻率高于圖像采樣頻率，且更容易實(shí)現(xiàn)插值，故以相機(jī)作為核心傳感器，以采集圖像數(shù)據(jù)的時(shí)刻作為要插入里程計(jì)數(shù)據(jù)的時(shí)間點(diǎn)，通過插值的方法獲取這一時(shí)刻的里程計(jì)數(shù)據(jù)，且需要索引這一時(shí)間點(diǎn)前后的兩個(gè)數(shù)據(jù)，并利用前后數(shù)據(jù)插值計(jì)算得到對(duì)應(yīng)時(shí)刻的里程計(jì)信息。

將對(duì)齊后的里程計(jì)信息通過預(yù)積分計(jì)算出相鄰兩幀圖像時(shí)刻之間的變換信息作為視覺里程計(jì)的一個(gè)初值估計(jì)，以提高非線性優(yōu)化的迭代收斂速度和精度。k和k+1時(shí)刻之間里程計(jì)的測(cè)量為：

式中，ηok為噪聲項(xiàng)，包含平移分量ηvk以及旋轉(zhuǎn)分量ηφk。在室內(nèi)環(huán)境下時(shí)，機(jī)器人的運(yùn)動(dòng)可以近似在一個(gè)平面上，忽略Z方向的運(yùn)動(dòng)，只考慮X、Y分方向上的運(yùn)動(dòng)。所以，V=［vx,vy］T表示平移的X、Y分量，φ為沿軸的旋轉(zhuǎn)。在k和k+1時(shí)刻間的狀態(tài)傳播為：

式中，Vk+1依賴φk。對(duì)式（5）左右各乘一個(gè)Φ(-φi)，得到：

于是，關(guān)鍵幀i到關(guān)鍵幀j之間的旋轉(zhuǎn)預(yù)積分測(cè)量表示為：

噪聲項(xiàng)為：

積分的噪聲項(xiàng)傳播迭代過程為：

通過預(yù)積分得到兩圖像之間的相對(duì)位姿作為視覺里程計(jì)的一個(gè)初值估計(jì)，以提高非線性優(yōu)化的迭代收斂速度和精度。

3 實(shí)驗(yàn)結(jié)果

為了驗(yàn)證該系統(tǒng)的有效性，在常見的室內(nèi)辦公環(huán)境進(jìn)行實(shí)驗(yàn)。實(shí)驗(yàn)設(shè)備如圖6（a）所示。設(shè)備搭載的相機(jī)型號(hào)為樂視深度相機(jī)LeTMC-520，采集圖像分辨率為640×480，圖像采集頻率為10 Hz，輪式里程計(jì)輸出頻率為20 Hz。需要特別說明的是，實(shí)驗(yàn)設(shè)備搭載的相機(jī)雖然為深度相機(jī)，但在實(shí)驗(yàn)中僅使用其單目圖像信息，未使用其深度計(jì)算功能。

實(shí)驗(yàn)測(cè)試了ORB-SLAM3以及改進(jìn)方法在室內(nèi)動(dòng)態(tài)環(huán)境的建圖與定位情況。實(shí)驗(yàn)環(huán)境如圖6（b）所示，行走軌跡大致如圖6（b）中紅色線條所示。該場(chǎng)景為典型的室內(nèi)辦公場(chǎng)景，有行走的人、易移動(dòng)的椅子等。由于紋理重復(fù)、信息單一且存在動(dòng)態(tài)障礙物，ORB-SLAM3在實(shí)驗(yàn)中存在初始化困難、多次丟失位姿、無法成功重定位以及建圖結(jié)果混亂無法識(shí)別輪廓等問題，而本文所提方法基本不存在上述問題。

圖6 實(shí)驗(yàn)設(shè)備及環(huán)境

該實(shí)驗(yàn)兩種方法的建圖效果對(duì)比如圖7所示，圖中矩形框?yàn)殛P(guān)鍵幀的位姿，紅色矩形框?yàn)槠鹗紟蛔耍G色矩形框?yàn)楫?dāng)前幀位姿，藍(lán)色矩形框?yàn)橐话阆鄼C(jī)位姿。此外，若存在不同顏色矩形框，則說明機(jī)器人在運(yùn)動(dòng)過程中曾丟失位姿，構(gòu)建了一個(gè)新的子地圖，其中黑點(diǎn)為全局地圖點(diǎn)，紅點(diǎn)為局部地圖點(diǎn)。從圖7（a）與圖7（b）可以看出，所提方法構(gòu)建的地圖輪廓清晰，關(guān)鍵幀間隔合理。從圖7（c）與圖7（d）可以看出，ORB-SLAM3多次丟失位姿，計(jì)算出多條軌跡，如圖7（c）中多種顏色關(guān)鍵幀所示，無法與實(shí)際情況對(duì)應(yīng)。本文所提方法可從地圖點(diǎn)分辨出天花板紋理，絕大多數(shù)地圖點(diǎn)的深度能保持在同一平面，且關(guān)鍵幀分布可顯示出機(jī)器人的實(shí)際行動(dòng)軌跡。

圖7 建圖結(jié)果對(duì)比圖

實(shí)驗(yàn)場(chǎng)景2如圖8所示，圖中紅色線條為行走軌跡。機(jī)器人在行走過程中有直行和旋轉(zhuǎn)等運(yùn)動(dòng)狀態(tài)。圖9為兩種方法的軌跡對(duì)比圖，其中Trajectory_OURS為所提方法軌跡，Trajectory_GT為軌跡真值，Trajectory_ORB為ORB-SLAM3軌跡。實(shí)驗(yàn)時(shí)，ORB-SLAM3成功初始化耗時(shí)久，且短暫運(yùn)行后即丟失位置，故其軌跡不全。所提方法的軌跡與軌跡真值的平均誤差為0.155 m。

圖8 實(shí)驗(yàn)場(chǎng)景2

圖9 實(shí)驗(yàn)場(chǎng)景2下的軌跡結(jié)果對(duì)比圖

4 結(jié) 語

基于單目相機(jī)和輪式里程計(jì)，本文提出了一種室內(nèi)動(dòng)態(tài)環(huán)境下的機(jī)器人定位方法。該方法固定單目相機(jī)朝向天花板，以減少室內(nèi)動(dòng)態(tài)環(huán)境下信息多變帶來的誤差。為適應(yīng)天花板紋理弱且重復(fù)性較高的特點(diǎn)，通過改變特征匹配策略提升紋理重復(fù)度高場(chǎng)景下的匹配精度，以獲得更加準(zhǔn)確的位姿求解；結(jié)合里程計(jì)預(yù)積分為視覺里程計(jì)提供初值，并為單目相機(jī)固定尺度信息。實(shí)驗(yàn)證明，在室內(nèi)動(dòng)態(tài)場(chǎng)景和重復(fù)紋理場(chǎng)景下，所提方法能有效避免動(dòng)態(tài)信息的影響，能在常見的室內(nèi)場(chǎng)景得到可靠的機(jī)器人行進(jìn)軌跡并保證其定位精度。該方法在室內(nèi)服務(wù)機(jī)器人的自主運(yùn)動(dòng)方面意義重大，具有較好的應(yīng)用前景。