朱道俊，徐湛楠，馬婷婷，曹平國，宋全軍*

(1.中國科學(xué)院合肥物質(zhì)科學(xué)研究院，安徽 合肥 230031；2.中國科學(xué)技術(shù)大學(xué)研究生院科學(xué)島分院，安徽 合肥 230026)

隨著機(jī)器人技術(shù)的發(fā)展和計算資源能力的提高，輪式機(jī)器人的應(yīng)用逐漸得到普及。定位技術(shù)是實(shí)現(xiàn)輪式機(jī)器人自主導(dǎo)航的重要技術(shù)環(huán)節(jié)。對于室內(nèi)外小場景范圍內(nèi)的輪式機(jī)器人應(yīng)用，如巡邏機(jī)器人，低成本、高精度、高魯棒性、易用性成為了定位系統(tǒng)的研究目標(biāo)。

同時建圖與定位(SLAM)技術(shù)[1]是研究機(jī)器人在未知環(huán)境下定位的主流技術(shù)方案。其中基于視覺傳感器的SLAM(V-SLAM)技術(shù)由于其低成本、測量信息豐富得到了廣泛研究。經(jīng)典的VSLAM工作有PTAM[2]、ORB-SLAM[3]、ORB-SLAM2[4]等，Mur-Artal R等提出的ORB-SLAM第一次將SLAM分為三個運(yùn)行線程進(jìn)行處理，實(shí)現(xiàn)了V-SLAM大場景下的實(shí)時運(yùn)行能力[3]。然而實(shí)際環(huán)境中由于場景特征的單一(如走廊環(huán)境)和傳感器的快速運(yùn)動等，很容易導(dǎo)致V-SLAM定位失敗，魯棒性較差。

視覺-慣性融合SLAM(VI-SLAM)是通過融合視覺和IMU傳感器的SLAM方案，由于IMU具有很好的動態(tài)特性，VI-SLAM能有效處理機(jī)體的快速運(yùn)動和場景單一的情況。經(jīng)典工作有MSCKF[5]、SMSCKF[6]、OKVIS[7]、VINS-Mono[8]等。香港科技大學(xué)秦通等提出的VINS-Mono因其快速精確初始化流程、視覺-慣性緊耦合非線性滑動窗口優(yōu)化估計器、四自由度位姿圖優(yōu)化等特點(diǎn)成為當(dāng)前VI-SLAM中定位效果最好的算法之一，實(shí)現(xiàn)了在無人航空飛行器上的精準(zhǔn)位姿軌跡估計，然而其在輪式移動機(jī)器人上的應(yīng)用卻會出現(xiàn)很多退化問題，導(dǎo)致定位精度下降甚至定位失敗。為了實(shí)現(xiàn)VINS-Mono精確有效的初始化，需要保證初始化過程中機(jī)體有充分的三軸運(yùn)動激勵，而輪式機(jī)器人由于平面運(yùn)動的限制，很難做到這點(diǎn)。Wu等通過數(shù)學(xué)和實(shí)驗(yàn)證明了當(dāng)輪式機(jī)器人在某些特定運(yùn)動場景下，VINS會出現(xiàn)額外的不可觀自由度，會帶來很大的估計偏差[9]，比如純直線運(yùn)動、常量局部加速度運(yùn)動等，而這些運(yùn)動情形在輪式機(jī)器人上是很常見的。

針對輪式機(jī)器人，輪速計傳感器可以很好的替代IMU，輪速計也能夠提供高頻率的機(jī)體狀態(tài)量測量，同時對于特殊運(yùn)動場景，輪速計不會帶來退化問題。Wu等也證明了通過融合輪速計信息能夠避免尺度不可觀的問題，提高定位精度[9]。Zhang等[10]、Liu等[11]、Lee等[12]通過融合視覺-IMU-輪速計信息來實(shí)現(xiàn)輪式機(jī)器人定位，結(jié)果相比于VINS都有了很大的提高。Zheng等提出了一種基于視覺-輪速計緊耦合的定位系統(tǒng)，它基于ORB-SLAM框架進(jìn)行改進(jìn)實(shí)現(xiàn)，提出了一種新的視覺重投影觀測誤差形式[13]。

為了同時追求系統(tǒng)的運(yùn)行效率和定位精度，本文發(fā)現(xiàn)在室內(nèi)外小范圍平面運(yùn)動場景下，視覺-IMU-輪速計定位系統(tǒng)相比于視覺-輪速計定位系統(tǒng)，會引入大量的額外系統(tǒng)負(fù)擔(dān)，比如，IMU的融合大大增加了系統(tǒng)的狀態(tài)維度、增加了系統(tǒng)初始化的難度[8]，然而定位精度不會有很大的提高，甚至對于低成本IMU，會帶來更大的定位誤差。因此，本文中提出緊耦合視覺和輪速計定位方案，提高系統(tǒng)定位精度的同時提升系統(tǒng)運(yùn)行效率，主要貢獻(xiàn)如下:①提出了一種基于非線性滑動窗口優(yōu)化的視覺-輪速計定位系統(tǒng)估計器；②提出改進(jìn)了一種聯(lián)合視覺-輪速計的快速精確系統(tǒng)初始化方案。③輪速計-相機(jī)坐標(biāo)系變換外參在線標(biāo)定。

1 數(shù)學(xué)記號

在本文中，我們使用大寫字母表示向量/矩陣，使用小寫字母表示標(biāo)量，∈SO(3)表示坐標(biāo)系B到坐標(biāo)系A(chǔ)的旋轉(zhuǎn)變換，為其單位四元數(shù)表示，這里使用Hamilton表達(dá)形式[14]；ApB∈R表示坐標(biāo)系B原點(diǎn)在坐標(biāo)系A(chǔ)下的坐標(biāo)；A(·)表示坐標(biāo)系A(chǔ)下的某量，表示狀態(tài)估計值或者測量值，δ(·)表示狀態(tài)的誤差形式，本文使用右乘擾動來定義旋轉(zhuǎn)誤差，其形式為:

?表示四元數(shù)乘法，(·)img表示四元數(shù)的虛數(shù)部分；表示向量對應(yīng)的反對稱矩陣，比如向量a＝[a1a2a3]T對應(yīng)的反對稱矩陣為:

ei表示R3×3單位矩陣的第i列，。SO(3)上的李代數(shù)表示為so(3)，log(·)獲得SO(3)對應(yīng)的so(3)，反之，exp(·)將so(3)轉(zhuǎn)換為SO(3)。向量v的第i個元素表示為v(i)。

本文涉及到三個不同的坐標(biāo)系，分別是世界坐標(biāo)系、輪速計坐標(biāo)系(機(jī)體坐標(biāo)系)和相機(jī)坐標(biāo)系，我們將它們依次表示為w、b和c。另外bk、ck表示時間順序上第k個機(jī)體坐標(biāo)系和對應(yīng)的相機(jī)坐標(biāo)系。

2 系統(tǒng)描述

如圖1所示。系統(tǒng)輸入為相機(jī)和輪速計觀測信息，對于新的圖像幀，先使用KLT光流法跟蹤上一幀的特征點(diǎn)，再提取新的Shi-Tomasi角點(diǎn)。對于新幀和上一幀之間的輪速計測量值，進(jìn)行預(yù)積分處理，詳見4.4節(jié)。

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

若系統(tǒng)未完成初始化，則進(jìn)行相機(jī)輪速計聯(lián)合初始化，詳見3節(jié)。若初始化成功，則進(jìn)行視覺-輪速計緊耦合非線性滑動窗口優(yōu)化，詳見4節(jié)，若窗口中幀數(shù)達(dá)到最大，則判斷次新幀是否為關(guān)鍵幀，若是，則選擇窗口中的最老關(guān)鍵幀進(jìn)行邊緣化，否則邊緣化次新幀[8]。

為消除長時間累積誤差，對新關(guān)鍵幀進(jìn)行回環(huán)檢測，在歷史關(guān)鍵幀數(shù)據(jù)庫中查找當(dāng)前關(guān)鍵幀的回環(huán)幀?；丨h(huán)檢測基于視覺詞袋庫DBoW2[15]進(jìn)行實(shí)現(xiàn)。若檢測到回環(huán)，則進(jìn)行六自由度位姿圖優(yōu)化，保證長時間位姿軌跡估計的一致性[8]。

3 視覺-輪速計聯(lián)合初始化

與VINS-Mono不同的是，本文提出使用兩幀快速初始化，將第一幀F(xiàn)0設(shè)置為世界坐標(biāo)系原點(diǎn)，為保證比較好的初始化，初始化兩幀之間需要有足夠的視差，設(shè)置初始化視差閾值為Tp，當(dāng)前幀F(xiàn)c與F0的視差小于Tp時，則丟棄該幀，反之，設(shè)置當(dāng)前幀為初始化第二幀F(xiàn)1。由于輪速計能夠提供比較精確的短時間位姿估計，因此可通過輪速計積分測量來預(yù)測F1的位姿，如圖2所示，對F0和F1跟蹤匹配的特征點(diǎn)進(jìn)行三角化，最后進(jìn)行一次集束調(diào)整(BA)優(yōu)化，所需最小化的誤差函數(shù)為:

為求解式(2)，我們采用迭代求解的方式，每次迭代通過求解增量方程Hδx＝b得到更新增量δx，然后更新到當(dāng)前狀態(tài)估計值上:，對于平移狀態(tài)，，對于旋轉(zhuǎn)狀態(tài)，。本文中使用Ceres Solver庫[16]來求解上訴迭代優(yōu)化問題。

4 視覺-輪速計緊耦合非線性滑動窗口優(yōu)化估計器

非線性滑動窗口優(yōu)化估計器中的狀態(tài)及測量約束關(guān)系可用因子圖進(jìn)行表示，如圖3所示，其中圓形和橢圓形表示系統(tǒng)優(yōu)化的狀態(tài)，方塊表示測量誤差因子，與方塊相連接的連接線表示該測量因子所關(guān)聯(lián)到的狀態(tài)。優(yōu)化的狀態(tài)包括關(guān)鍵幀位姿、輪速計相機(jī)外參和特征點(diǎn)位置，詳見4.1節(jié)。傳感器測量約束因子包括特征點(diǎn)重投影誤差因子和輪速計預(yù)積分測量誤差因子，詳見4.2和4.3節(jié)。為了處理低頻視覺信息和高頻輪速計信息的融合，本文進(jìn)行了相鄰關(guān)鍵幀間輪速計測量預(yù)積分和誤差傳播，詳見4.4節(jié)。

圖3 非線性滑動窗口優(yōu)化估計器因子圖

4.1 系統(tǒng)狀態(tài)

滑動窗口中估計狀態(tài)為:

式中:N為滑動窗口大小，m為滑動窗口中所能觀測到的路標(biāo)點(diǎn)數(shù)量表示第i幀位姿狀態(tài)，λl，l＝1，…，m表示第l個路標(biāo)點(diǎn)逆深度狀態(tài)，參數(shù)化表示在錨坐標(biāo)系(觀測首幀相機(jī)坐標(biāo)系)下[8]，定義如下:

式中:ba/ca分別表示觀測首幀機(jī)體/相機(jī)坐標(biāo)系，表示路標(biāo)點(diǎn)l在ca幀下的像素觀測坐標(biāo)值，將其通過相機(jī)內(nèi)參和畸變參數(shù)轉(zhuǎn)換到歸一化平面坐標(biāo)，這個轉(zhuǎn)換過程即為。表示輪速計坐標(biāo)系和相機(jī)坐標(biāo)系之間的外參變換，這里將其作為系統(tǒng)狀態(tài)進(jìn)行在線估計，可以減小不精確的外參標(biāo)定結(jié)果對定位精度的影響。如果有比較精確的外參線下標(biāo)定值，可以將其從系統(tǒng)狀態(tài)估計中移除。

4.2 視覺觀測誤差因子

如圖4所示，路標(biāo)點(diǎn)l在cj幀的重投影誤差定義為觀測坐標(biāo)和重投影坐標(biāo)之間的坐標(biāo)差值，重投影坐標(biāo)計算方法為首先將路標(biāo)點(diǎn)l通過其逆深度和觀測首幀位姿估計轉(zhuǎn)換到世界坐標(biāo)系下，再通過cj幀位姿估計投影到cj幀圖像平面上，如下所示:

圖4 特征點(diǎn)重投影觀測誤差

其中式(5.1)表示重投影誤差，為路標(biāo)點(diǎn)l在cj幀上預(yù)測歸一化平面坐標(biāo)，為觀測歸一化平面坐標(biāo)，式(5.2)將相機(jī)坐標(biāo)系下坐標(biāo)點(diǎn)投影到相機(jī)歸一化平面上，為路標(biāo)點(diǎn)l在相機(jī)坐標(biāo)系cj下的坐標(biāo)，(2)表示向量的第三個元素，式(5.3)將路標(biāo)點(diǎn)l的世界坐標(biāo)轉(zhuǎn)換到cj幀相機(jī)坐標(biāo)系下，通過式(4)計算獲得。

由式(4)、式(5)可以看出，路標(biāo)點(diǎn)l一次視覺重投影誤差分別關(guān)聯(lián)到錨機(jī)體坐標(biāo)系位姿狀態(tài)、當(dāng)前機(jī)體坐標(biāo)系位姿狀態(tài)和路標(biāo)點(diǎn)的逆深度狀態(tài)，因此需要分別對三個狀態(tài)求誤差雅可比矩陣，分別設(shè)為。

4.3 輪速計誤差因子

兩個相鄰關(guān)鍵幀F(xiàn)k和Fk＋1之間的輪速計積分測量為:

式中:rd表示輪速計平移測量誤差，rθ表示輪速計航向角測量誤差。

4.4 輪速計預(yù)積分

由于相鄰關(guān)鍵幀之間有多次輪速計測量，為了獲得相鄰關(guān)鍵幀之間輪速計測量的噪聲協(xié)方差矩陣，需要將輪速計測量進(jìn)行積分。如圖5所示，用i、j表示圖像測量時間索引，用k表示輪速計測量時間索引，我們將圖像測量時刻ti和tj之間的輪速計測量進(jìn)行積分和誤差遞推。

圖5 輪速計預(yù)積分

輪速計k時刻的測量為:

根據(jù)SSε(2)上的遞推公式，有:

式中:Φ(·)表示對應(yīng)的二維旋轉(zhuǎn)矩陣，ivk＋1的噪聲協(xié)方差由ivk噪聲協(xié)方差和nvk測量噪聲協(xié)方差遞推得到，因此需要求得式(15)對ivk和nvk的雅可比矩陣，設(shè)分別為Jvk、Jnk，解析表達(dá)式為:

以此遞推，可以得到ti和tj時刻圖像幀之間的輪速計積分測量ivj和噪聲協(xié)方差矩陣iΣj。

5 實(shí)驗(yàn)及分析

5.1 實(shí)驗(yàn)配置

本部分對本文所提出算法進(jìn)行了實(shí)驗(yàn)驗(yàn)證，實(shí)驗(yàn)平臺如圖6所示，四輪車上搭載了一個PC和一個相機(jī)。PC用于讀取相機(jī)和輪速計傳感器測量信息和進(jìn)行算法處理，機(jī)器人操作系統(tǒng)(ROS)用來進(jìn)行四輪車、相機(jī)和PC的信息通信傳輸，使用操作系統(tǒng)為Ubuntu18.0。

圖6 實(shí)驗(yàn)平臺配置

實(shí)驗(yàn)設(shè)備的具體型號配置如表1所示。本實(shí)驗(yàn)代碼基于開源VINS-Mono[17]代碼框架修改。

表1 實(shí)驗(yàn)設(shè)備具體配置信息

為了驗(yàn)證所提算法的精確度，另外進(jìn)行了兩組對比實(shí)驗(yàn)，其一是基于視覺-IMU融合的VINS-Mono方案；其二是僅使用輪速計的定位方案。為了量化評估軌跡誤差，實(shí)驗(yàn)數(shù)據(jù)采集時確保了起始位置和終止位置相同，通過計算終點(diǎn)、起始點(diǎn)位置估計距離差和軌跡總長度的比例來定義估計誤差，如式(18)所示，其中為終止點(diǎn)估計位置，為起始點(diǎn)位姿估計，＾S為軌跡總長度估計。因?yàn)槠鹗键c(diǎn)設(shè)置為世界坐標(biāo)系原點(diǎn)，所以＝0。

傳感器之前的外參數(shù)通過線下進(jìn)行標(biāo)定，相機(jī)內(nèi)參和相機(jī)-IMU外參數(shù)通過Kalibr[18]進(jìn)行標(biāo)定。相機(jī)-輪速計外參數(shù)通過CamOdomCalibraTool[19]進(jìn)行標(biāo)定，該標(biāo)定參數(shù)為粗略標(biāo)定，會對該參數(shù)進(jìn)行在線估計優(yōu)化。

5.2 軌跡誤差分析

實(shí)驗(yàn)中錄制了三種不同場景的數(shù)據(jù)集進(jìn)行測試，分別是實(shí)驗(yàn)室，會議室和走廊環(huán)境，如圖7所示。對每個實(shí)驗(yàn)場景采集的數(shù)據(jù)集，分別對本文所提算法和比較算法運(yùn)行了3次，并取結(jié)果平均值作為誤差結(jié)果。如圖8所示，分別為本文所提算法、VINSMono和輪速計在三個實(shí)驗(yàn)場景下其中一次運(yùn)行結(jié)果所估計的運(yùn)動軌跡。從圖中可以看出，VINSMono和輪速計都產(chǎn)生了很大的軌跡誤差，VINSMono甚至在實(shí)驗(yàn)室場景中產(chǎn)生了發(fā)散，原因在于以下幾個方面，一是本實(shí)驗(yàn)采用了低價的IMU傳感器，有比較大的誤差，二是VINS-Mono對相機(jī)和IMU之前的外參標(biāo)定和時間同步參數(shù)有很高的精度要求，而這對低價的傳感器配置來說是很難實(shí)現(xiàn)，三是本實(shí)驗(yàn)的場景主要是平面運(yùn)動，且大部分是直線運(yùn)動和勻加速運(yùn)動，這給VINS-Mono的初始化以及后面的估計帶來了很大的不可觀問題[9]。另外，僅用輪速計的軌跡估計隨著時間增長誤差將不斷增大，原因是輪速計是通過積分得到位姿估計的，因?yàn)闇y量帶有誤差，所以長時間的輪速計積分將不斷積累誤差。而本文所提算法通過融合相機(jī)和輪速計信息獲得了很精確的定位表現(xiàn)，通過視覺特征點(diǎn)、觀測測量來矯正輪速計積分誤差，同時輪速計積分測量也對相鄰幀間的位姿起到較精確的約束作用。相比IMU，視覺-輪速計融合能夠獲得更魯棒的表現(xiàn)，機(jī)體終點(diǎn)位置估計能很好的回到初始點(diǎn)，其終點(diǎn)位置估計誤差如表2所示。如圖9所示為所提算法和其他兩種對比算法分別在三種實(shí)驗(yàn)場景下的估計軌跡誤差，其中水平橫軸表示不同的估計算法，水平縱軸表示不同的實(shí)驗(yàn)場景，垂直軸表示算法估計誤差?？梢钥闯鏊崴惴ㄏ啾扔赩INS-Mono和純輪速計估計都有很大程度的精度提高。

圖7 實(shí)驗(yàn)數(shù)據(jù)采集環(huán)境，從左至右分別為實(shí)驗(yàn)室、會議室和走廊

圖8 所提算法和其他兩種對比算法分別在三種實(shí)驗(yàn)場景下的估計軌跡對比圖

圖9 所提算法和對比算法分別在三種場景下估計軌跡誤差對比

表2 所提算法在三個實(shí)驗(yàn)場景下的估計誤差

5.3 輪速計-相機(jī)外參在線標(biāo)定實(shí)驗(yàn)分析

實(shí)驗(yàn)中對輪速計-相機(jī)外參進(jìn)行了粗略線下標(biāo)定，為了進(jìn)一步優(yōu)化該參數(shù)，可對其進(jìn)行在線估計。輪速計-相機(jī)外參數(shù)有6個自由度，分別是3自由度平移和3自由度旋轉(zhuǎn)，然而由于平面運(yùn)動，只有5個自由度可觀，z軸平移不可觀[20]，因此只能估計出輪速計-相機(jī)外參數(shù)中的x、y軸平移和roll、pitch、yaw旋轉(zhuǎn)角。如圖10和圖11所示，分別為輪速計-相機(jī)平移外參和旋轉(zhuǎn)外參估計值，可以看出，參數(shù)估計在經(jīng)歷初始階段的稍微震蕩后，最后都趨于收斂穩(wěn)定。

圖10 輪速計-相機(jī)平移外參在線估計結(jié)果

圖11 輪速計-相機(jī)旋轉(zhuǎn)外參在線估計結(jié)果

5.4 系統(tǒng)運(yùn)行效率實(shí)驗(yàn)分析

本部分實(shí)驗(yàn)對比了所提算法與VINS-Mono的運(yùn)行效率。保持其他條件參數(shù)相同，如滑動窗口大小(實(shí)驗(yàn)中設(shè)置為10幀)，優(yōu)化完成的最長時間(實(shí)驗(yàn)中設(shè)置為0.04 s)和最大迭代次數(shù)(實(shí)驗(yàn)中設(shè)置為8)。以會議室場景為例，如圖12所示，為兩個系統(tǒng)在初始化完成后的30 s內(nèi)每次迭代優(yōu)化所需的時

圖12 本文所提算法與VINS-Mono運(yùn)行效率對比

間，可以看出本文所提算法一次優(yōu)化耗時大約是VINS-Mono的1/10，效率得到了很大提高。原因在于輪速計信息的融合并沒有引入額外的系統(tǒng)狀態(tài)，每幀的狀態(tài)維度為6維，相反，VINS-Mono系統(tǒng)中，每幀需要估計的狀態(tài)維度為15維，因此大大增加了優(yōu)化迭代時需要的時間。

6 結(jié)論

本文提出了一個基于視覺-輪速計緊耦合的輪式移動機(jī)器人定位算法。采用了基于滑動窗口的非線性優(yōu)化框架，最小化特征點(diǎn)觀測重投影誤差和幀間輪速計積分測量誤差，來保證系統(tǒng)的實(shí)時性和精確度。此外，本文提出了基于視覺-輪速計聯(lián)合的系統(tǒng)初始化流程，能夠快速精確的完成初始化。本文所提系統(tǒng)中對相機(jī)-輪速計坐標(biāo)系變換外參進(jìn)行了在線標(biāo)定，在粗略的線下標(biāo)定外參數(shù)下，對其進(jìn)行在線估計優(yōu)化，減少了對傳感器線下高精度標(biāo)定的要求。通過實(shí)驗(yàn)表明，在低成本實(shí)驗(yàn)傳感器配置的情況下，本文所提算法在小范圍室內(nèi)外輪式機(jī)器人應(yīng)用場景下相比于VINS-Mono獲得了更魯棒更精確的定位表現(xiàn)，同時運(yùn)行效率也有比較大的提高。本文實(shí)驗(yàn)場景為室內(nèi)外規(guī)則平面環(huán)境，未考慮輪速計打滑造成的影響，下一步將加入對輪速計打滑情況的處理。