基于單目視覺與里程計(jì)的組合室內(nèi)定位研究*

方章云，袁 亮，侯愛萍，吳金強(qiáng)

(新疆大學(xué) a.機(jī)械工程學(xué)院；b.紡織與服裝學(xué)院,烏魯木齊 830047)

0 引言

伴隨著移動(dòng)機(jī)器人定位技術(shù)的快速發(fā)展，基于傳感器的同時(shí)定位與地圖構(gòu)建[1](Simultaneous Localization and Mapping, SLAM)技術(shù)成為研究的熱點(diǎn)。用于SLAM技術(shù)的常見傳感器有相機(jī)、激光雷達(dá)、IMU(慣性測(cè)量單元)、里程計(jì)等。而VSLAM[2](Visual SALM)技術(shù)主要優(yōu)點(diǎn)是采用相機(jī)作為單一的外部感知傳感器。J Engle等[3]于2015年提出的LSD-SLAM是利用基于像素的直接法進(jìn)行SLAM工作，但是光照變化對(duì)其影響大，且計(jì)算成本高。而2017年Mur-Artal等基于原有的ORB-SLAM[4]提出了ORB-SLAM2[5]，該方法以單目視覺為主，整個(gè)過程均采用ORB特征點(diǎn)，極大的縮短了計(jì)算時(shí)間，然而單目ORB-SLAM2無法提供尺度信息，只能提供帶有比例縮放的軌跡，因此許多學(xué)者將目光轉(zhuǎn)向多傳感器融合，其中Mur-Artal等使用單目ORB-SLAM與IMU的結(jié)合十分突出[6]，將IMU提供的慣性數(shù)據(jù)用于緊耦合融合，插入預(yù)積分作為約束，進(jìn)而實(shí)現(xiàn)對(duì)單目ORB-SLAM跟蹤的改進(jìn)，但是IMU的初始化依賴于SLAM。周紹磊等[7]利用INS與ORB-SLAM結(jié)合實(shí)現(xiàn)了室外的慣性導(dǎo)航，利用精度為5cm的差分GPS估計(jì)尺度因子，有效的提高了定位精度，該方法成本較高，且GPS不適合在室內(nèi)實(shí)現(xiàn)移動(dòng)機(jī)器人的定位。王澤華等[8]添加磁力計(jì)改進(jìn)IMU的姿態(tài)解算實(shí)現(xiàn)與SLAM的松耦合，該方法通過IMU位姿信息估計(jì)相機(jī)的尺度因子，而IMU的位姿信息是通過加速度的二重積分計(jì)算得到，因此加速度的微小測(cè)量噪聲會(huì)被放大，從而影響到移動(dòng)機(jī)器人的定位。

綜上所述，單目相機(jī)作為SLAM所用的傳感器，成本較低，使用方便，缺點(diǎn)是無法提供真實(shí)深度信息。本文采用ORB-SLAM2算法，利用全向移動(dòng)機(jī)器人里程計(jì)估算尺度因子，并將ORB-SLAM2輸出的經(jīng)尺度變換后的位姿信息和里程計(jì)位姿進(jìn)行卡爾曼濾波，最終根據(jù)誤差比重加權(quán)實(shí)現(xiàn)對(duì)移動(dòng)機(jī)器人的定位。

1 移動(dòng)機(jī)器人算法模型描述

1.1 全向移動(dòng)機(jī)器人航跡推算算法

本文采用的是三輪全向移動(dòng)機(jī)器人[9],而傳統(tǒng)的室內(nèi)移動(dòng)機(jī)器人采取的是兩輪差速的小車底盤，機(jī)械結(jié)構(gòu)具有很強(qiáng)的約束性，很難實(shí)現(xiàn)在室內(nèi)擁擠環(huán)境下的小半徑轉(zhuǎn)彎，因此全向移動(dòng)機(jī)器人在室內(nèi)狹窄環(huán)境下沿任意方向移動(dòng)更有優(yōu)勢(shì)。

對(duì)于全向移動(dòng)機(jī)器人，假設(shè)機(jī)器人坐標(biāo)系和世界坐標(biāo)系的初始夾角為θ，x軸方向上的速度為vx，y軸方向上的速度為vy，自轉(zhuǎn)的速度為ω，各輪子中心到機(jī)器人中心的距離為L(zhǎng)=0.43m且是中心對(duì)稱分布，由此可以得到全向移動(dòng)機(jī)器人3個(gè)輪子的速度為：

(1)

其中，V1、V2、V3分別為3個(gè)輪的轉(zhuǎn)速。

為方便計(jì)算取θ=0，即求解出vx、vy、ω：

(2)

本文采用的全向移動(dòng)機(jī)器人DR算法是從二輪差速底盤改進(jìn)而來，根據(jù)原有的二輪差速DR算法[10]，選擇固定其中一輪方向與機(jī)器人坐標(biāo)系(y-o-x)y軸重合，假設(shè)全向移動(dòng)機(jī)器人的偏航角范圍為-180°～180°(順時(shí)針為正)，如圖1所示移動(dòng)機(jī)器人從tn時(shí)刻到tn+1時(shí)刻，全向輪底盤的偏航角變化為Δθ=θ2-θ1以及在tn時(shí)刻機(jī)器人坐標(biāo)系下y軸方向的速度為vy，x軸方向的速度為vx。

圖1 三輪全向移動(dòng)機(jī)器人DR算法模型

當(dāng)編碼器采樣時(shí)間足夠短時(shí)，此時(shí)x、y軸上的速度變化不明顯，因此可以將vx、vy分別正交分解到世界坐標(biāo)系Y-O-X可得：

將vy、vy分別分解到X，Y軸上：

(3)

其中，vyY、vyX為速度vy分別在世界坐標(biāo)系Y、X上的投影，vxY、vxX為速度vx分別在世界坐標(biāo)系Y、X上的投影。

因此可以得到全向輪底盤里程遞推公式為：

(4)

其中，Y、X分別為全向移動(dòng)機(jī)器人在世界坐標(biāo)系X、Y方向上的坐標(biāo)，θ為航向角，下標(biāo)n為任意時(shí)刻。

1.2 單目視覺ORB-SLAM2算法

單目ORB-SLAM2[5]算法是基于旋轉(zhuǎn)不變性的ORB特征實(shí)現(xiàn)，算法創(chuàng)新式地使用了三個(gè)線程并行完成SLAM工作：首先利用Tracking線程實(shí)時(shí)跟蹤特征點(diǎn)，并用局部Bundle Adjustment最小化重投影誤差，進(jìn)而進(jìn)行定位構(gòu)圖，在算法運(yùn)行過程中利用視覺字典進(jìn)行回環(huán)檢測(cè)，在檢測(cè)到回環(huán)之后會(huì)對(duì)相機(jī)位姿進(jìn)行全局BA，從而大大提高定位精度。

在使用ORB-SLAM2之前需要對(duì)單目相機(jī)進(jìn)行標(biāo)定，得到相機(jī)內(nèi)參參數(shù)fx、fy、cx、cy以及畸變系數(shù)，填寫相應(yīng)的參數(shù)文件。然而，單目視覺SLAM算法在通過SVD分解本質(zhì)矩陣求解旋轉(zhuǎn)矩陣R和平移矩陣t時(shí)，會(huì)存在尺度等價(jià)性問題，而R∈SO(3)自身具有約束，但t和本質(zhì)矩陣分解的奇異值有關(guān)，所以不同的t決定了空間三維點(diǎn)的深度，換言之，單目ORB-SLAM2中，對(duì)軌跡和地圖同時(shí)放縮任意倍數(shù)，得到的軌跡形狀依然是一樣的。因此，本文利用全向移動(dòng)機(jī)器人里程計(jì)來實(shí)現(xiàn)單目視覺尺度因子的估計(jì)。

2 DR/ORB-SLAM2融合算法

2.1 尺度因子估計(jì)

融合算法流程如圖2所示，本文通過卡爾曼濾波對(duì)底盤位姿和ORB-SLAM2經(jīng)尺度轉(zhuǎn)換后的位姿進(jìn)行過濾噪聲，通過誤差比重進(jìn)行松耦合加權(quán)實(shí)現(xiàn)對(duì)機(jī)器人的定位。其中全向移動(dòng)機(jī)器人的里程計(jì)算經(jīng)過碼盤的平均采樣和PID調(diào)速較傳統(tǒng)里程計(jì)累計(jì)誤差小，因此能利用全向移動(dòng)機(jī)器人的里程位姿作為估計(jì)單目ORB-SLAM2尺度因子的信息。

圖2 融合算法流程圖

ei=bi-sR·ai-b0(i=1,2,…,n)

(5)

其中，ei為第i次的測(cè)量誤差，R和b0分別為相機(jī)坐標(biāo)系到機(jī)器人坐標(biāo)系的旋轉(zhuǎn)矩陣和平移向量，s為單目ORB-SLAM2的尺度因子。

(6)

函數(shù)Q對(duì)s求偏導(dǎo)并令其為0得：

(7)

在單目初始化后，全向移動(dòng)機(jī)器人移動(dòng)的短距離內(nèi)分別取n個(gè)時(shí)刻相機(jī)位置和對(duì)應(yīng)的n個(gè)里程計(jì)位置。而函數(shù)的極值點(diǎn)即是所求的尺度因子s。如圖3所示，分別求取X、Y方向的尺度值，隨著點(diǎn)數(shù)的增加最終同時(shí)趨向于s=3.4182。

圖3 X、Y方向尺度因子估計(jì)

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

由于DR和單目ORB-SLAM2輸出的經(jīng)尺度變換的位姿軌跡存在噪聲，軌跡不光滑，因此利用卡爾曼濾波分別對(duì)兩組軌跡進(jìn)行濾波得到位姿Xa、Xb。

構(gòu)建目標(biāo)數(shù)學(xué)模型：

運(yùn)動(dòng)模型：Xk=AXk-1+εk

(8)

觀測(cè)模型：Zk=CXk+δk

(9)

在獲得較為平穩(wěn)的數(shù)據(jù)后，再通過位置均方根誤差權(quán)值的比重進(jìn)行加權(quán)融合。見式(10)：

(10)

其中,w1、w2分別為ORB-SLAM2經(jīng)尺度轉(zhuǎn)換后的位置和DR的位置均方根誤差，X為融合后位姿。

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

如圖4所示，實(shí)驗(yàn)的硬件采用的是三輪全向移動(dòng)機(jī)器人底盤，控制層采用的是STM32F103系列開發(fā)板，同時(shí)采用增量式PID算法實(shí)現(xiàn)對(duì)底盤三個(gè)電機(jī)的精準(zhǔn)控制；上層采用聯(lián)想Y410P系列的PC機(jī)作為機(jī)載計(jì)算機(jī)，通過ROS機(jī)器人操作系統(tǒng)實(shí)現(xiàn)對(duì)多個(gè)傳感器的話題信息提取，并利用rosbag包記錄話題信息。視覺傳感器采用型號(hào)為XGY300，像素為300萬的單目相機(jī)，激光雷達(dá)采用測(cè)距分辨率為0.5mm的Rplidar A2。具體實(shí)驗(yàn)平臺(tái)如圖5所示。

圖4 硬件系統(tǒng)圖

圖5 全向移動(dòng)機(jī)器人實(shí)物圖

實(shí)驗(yàn)在6m×5m的室內(nèi)范圍內(nèi)進(jìn)行，地面為普通的光滑平面，全向移動(dòng)機(jī)器人只在平面上移動(dòng)，相當(dāng)于只在XY平面上移動(dòng)，在Z軸上只有轉(zhuǎn)動(dòng)，故取全向移動(dòng)機(jī)器人的運(yùn)動(dòng)軌跡以及航向角作為實(shí)驗(yàn)對(duì)比數(shù)據(jù)，同時(shí)利用rosbag記錄的時(shí)間，將里程計(jì)、激光雷達(dá)、ORB-SLAM2算法輸出的數(shù)據(jù)按照每秒的間隔求和平均對(duì)齊，保證數(shù)據(jù)的一致性。由于實(shí)驗(yàn)場(chǎng)景較大，人工量取移動(dòng)機(jī)器人真實(shí)軌跡較困難，因此采用Rplidar A2實(shí)現(xiàn)的Hector SLAM[11]算法繪制的軌跡作為真實(shí)軌跡進(jìn)行對(duì)比，如圖6所示。

圖6 實(shí)驗(yàn)場(chǎng)景地圖

圖7為不同算法對(duì)機(jī)器人定位的軌跡點(diǎn),在實(shí)驗(yàn)中，移動(dòng)機(jī)器人由于抖動(dòng)會(huì)使軌跡不光滑，以及單目相機(jī)、激光雷達(dá)、里程計(jì)坐標(biāo)起點(diǎn)沒有實(shí)現(xiàn)完全對(duì)齊等都會(huì)使軌跡數(shù)據(jù)產(chǎn)生偏差，在本文使用的算法中，ORB-SLAM2是帶有回環(huán)檢測(cè)的，因此融合ORB-SLAM2后的軌跡圖也會(huì)帶有回環(huán)的特性，能大大減小定位誤差。

圖7 軌跡對(duì)比圖

機(jī)器人的制造誤差以及單目相機(jī)的標(biāo)定誤差等均會(huì)造成不同程度的定位誤差，因此在X、Y方向上的定位誤差都會(huì)略有不同，如圖8、圖9所示，由于移動(dòng)機(jī)器人在X、Y方向上的運(yùn)動(dòng)距離的不同，運(yùn)動(dòng)距離較短的Y方向的誤差較小，同時(shí)可以發(fā)現(xiàn)，雖然移動(dòng)機(jī)器人的定位誤差處于波動(dòng)狀態(tài)，但大多數(shù)位置的誤差均能保持在0.1m以內(nèi)。其中本文方法在X、Y方向上的誤差均方根為0.0761m、0.0293m，均小于原算法。

圖8 X方向誤差

圖9 Y方向誤差

在移動(dòng)機(jī)器人定位中，往往還需考慮機(jī)器人的朝向問題，即航向角，如圖10所示，可以觀察到航向角的變化是符合運(yùn)動(dòng)航向變化的，4條曲線基本保持一致，本文方法的航向角誤差大小始終保持在7°以內(nèi)。

圖10 航向角對(duì)比圖

本文的方法最終數(shù)據(jù)對(duì)比結(jié)果如表1所示。

表1 各項(xiàng)數(shù)據(jù)均方根誤差對(duì)比

由表中的實(shí)驗(yàn)數(shù)據(jù)可以看出,融合后的軌跡位置相對(duì)原來的DR和ORB-SLAM*s的位置結(jié)果分別提高了27.85%、36.60%，而航向角上的誤差比ORB-SLAM*s大，但相對(duì)于DR提高了30.83%。

4 結(jié)束語

本文采用DR算法與單目ORB-SLAM2結(jié)合，為單目ORB-SLAM2算法提供了尺度因子估計(jì)方法，以及通過卡爾曼濾波與比重加權(quán)實(shí)現(xiàn)對(duì)里程計(jì)位姿以及經(jīng)尺度變換后輸出的ORB-SLAM2位姿融合。實(shí)驗(yàn)結(jié)果表明，本文方法使移動(dòng)機(jī)器人在定位結(jié)果上相對(duì)于原來的里程計(jì)和單目視覺定位結(jié)果分別提高了27.85%、36.60%。然而，算法仍有不足，在單目尺度因子估計(jì)上，由于運(yùn)動(dòng)時(shí)間過長(zhǎng)后，里程計(jì)的DR誤差會(huì)產(chǎn)生累計(jì)，不能很好的實(shí)現(xiàn)對(duì)里程計(jì)數(shù)據(jù)點(diǎn)的選取。下一步可以考慮將里程計(jì)與單目視覺進(jìn)行緊耦合融合，保證尺度因子估計(jì)的穩(wěn)定性。

DOI:10.1109/TRO.2016.2624754.