基于視覺(jué)和輪速計(jì)緊耦合的輪式機(jī)器人定位算法研究*

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

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

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

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

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

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

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

1 數(shù)學(xué)記號(hào)

在本文中，我們使用大寫(xiě)字母表示向量/矩陣，使用小寫(xiě)字母表示標(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)估計(jì)值或者測(cè)量值，δ(·)表示狀態(tài)的誤差形式，本文使用右乘擾動(dòng)來(lái)定義旋轉(zhuǎn)誤差，其形式為:

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

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

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

2 系統(tǒng)描述

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

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

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

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

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

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

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

4 視覺(jué)-輪速計(jì)緊耦合非線(xiàn)性滑動(dòng)窗口優(yōu)化估計(jì)器

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

圖3 非線(xiàn)性滑動(dòng)窗口優(yōu)化估計(jì)器因子圖

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

滑動(dòng)窗口中估計(jì)狀態(tài)為:

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

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

4.2 視覺(jué)觀(guān)測(cè)誤差因子

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

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

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

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

4.3 輪速計(jì)誤差因子

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

式中:rd表示輪速計(jì)平移測(cè)量誤差，rθ表示輪速計(jì)航向角測(cè)量誤差。

4.4 輪速計(jì)預(yù)積分

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

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

輪速計(jì)k時(shí)刻的測(cè)量為:

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

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

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

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

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

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

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

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

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

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

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

5.2 軌跡誤差分析

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

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

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

圖9 所提算法和對(duì)比算法分別在三種場(chǎng)景下估計(jì)軌跡誤差對(duì)比

表2 所提算法在三個(gè)實(shí)驗(yàn)場(chǎng)景下的估計(jì)誤差

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

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

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

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

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

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

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

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

6 結(jié)論

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