溫倩雯，蘇毅輝，李 庚

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

0 引 言

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

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

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

1 系統(tǒng)框架

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

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

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

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

2.1 視覺里程計

對于空間中一個特征點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)，是一個3×3的正交矩陣；WΔC表示世界坐標(biāo)到相機(jī)坐標(biāo)的1×3平移向量；符號d(·)表示齊次坐標(biāo)變換。

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

圖2 不同特征點提取算法的效果對比圖

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

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

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

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

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

圖5 三角化誤差示意圖

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

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

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

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

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

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

噪聲項為：

積分的噪聲項傳播迭代過程為：

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

3 實驗結(jié)果

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

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

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

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

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

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

圖8 實驗場景2

圖9 實驗場景2下的軌跡結(jié)果對比圖

4 結(jié) 語

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