王立玲，李森，馬東

(1.河北大學(xué)電子信息工程學(xué)院，河北保定 071002；2.河北省數(shù)字醫(yī)療工程重點(diǎn)實(shí)驗(yàn)室，河北保定 071002)

0 前言

移動(dòng)機(jī)器人在運(yùn)動(dòng)中自主導(dǎo)航完成設(shè)定任務(wù)，需要定位系統(tǒng)計(jì)算自身所在環(huán)境中的位姿。SLAM是同步定位與地圖構(gòu)建(Simultaneous Localization and Mapping)，主要用于解決移動(dòng)機(jī)器人在未知環(huán)境中運(yùn)行時(shí)定位導(dǎo)航與地圖構(gòu)建的問題。為獲取先驗(yàn)信息確定移動(dòng)機(jī)器人的位姿，在設(shè)計(jì)移動(dòng)機(jī)器人時(shí)配備了各種傳感器，如編碼器、IMU(慣性測量單元)傳感器、相機(jī)、激光雷達(dá)等。移動(dòng)機(jī)器人從搭載的傳感器上獲取到觀測數(shù)據(jù)，進(jìn)行移動(dòng)機(jī)器人相對(duì)于未知環(huán)境的位姿估計(jì)，完成定位。

移動(dòng)機(jī)器人自主定位導(dǎo)航算法研究過程中，基于MEMS-IMU的捷聯(lián)式慣性導(dǎo)航系統(tǒng)以其成本低、體積小、功耗低等成為研究的熱點(diǎn)[1]。尤其，基于MEMS-IMU/GPS 的組合導(dǎo)航系統(tǒng)[2]被廣泛用于各種領(lǐng)域，在移動(dòng)機(jī)器人定位領(lǐng)域產(chǎn)生深遠(yuǎn)影響。針對(duì)GPS無法提供服務(wù)的地方，提出一種基于高精度慣導(dǎo)和里程計(jì)的車載 DR 誤差補(bǔ)償算法。但是，此系統(tǒng)定位誤差會(huì)隨時(shí)間迅速增長,無法滿足實(shí)際使用要求[3]。

為了提高移動(dòng)機(jī)器人姿態(tài)估計(jì)的精確性，采用多個(gè)傳感器進(jìn)行數(shù)據(jù)融合，完成移動(dòng)機(jī)器人定位位姿計(jì)算。傳感器融合的優(yōu)點(diǎn)是能夠提供比單個(gè)傳感器更好的測量精度和更精準(zhǔn)的推斷。

文獻(xiàn)[4]中提出了一個(gè)基于事件的卡爾曼濾波器的傳感器融合框架，將全局傳感器和慣性測量單元的測量結(jié)合，在基于事件的時(shí)間表上，改進(jìn)了計(jì)算資源有限的移動(dòng)機(jī)器人的定位系統(tǒng)。文獻(xiàn)[5]中提出了一種融合GPS和IMU測量的自適應(yīng)神經(jīng)元模糊推理系統(tǒng)。

根據(jù)載體搭載的傳感器不同，可構(gòu)建不同的SLAM系統(tǒng)。當(dāng)機(jī)器人搭載的傳感器是相機(jī)，則視為視覺SLAM。其中，視覺SLAM分為單目視覺、RGB-D SLAM、雙目SLAM。由于單目相機(jī)質(zhì)量輕、功耗小、成本低，能夠廣泛應(yīng)用在各個(gè)領(lǐng)域。MUR-ARTAL 等[6]于2015年提出了ORB-SLAM算法，運(yùn)用單目相機(jī)在計(jì)算機(jī)的CPU上實(shí)時(shí)建圖與定位。該算法的關(guān)鍵點(diǎn)提取和描述子的計(jì)算花費(fèi)近20 ms；但是，會(huì)忽略除特征點(diǎn)以外的大部分信息，在特征缺失地方采集不到足夠的匹配點(diǎn)進(jìn)行計(jì)算，從而可能丟棄有用的視覺信息。FORSTER等[7]于2014年提出了一種半直接法的視覺里程計(jì)SVO(Semi-direct Monocular Visual Odometry)，前期只跟蹤稀疏的關(guān)鍵點(diǎn)(不計(jì)算描述子)，再使用直接法估計(jì)相機(jī)的位姿，計(jì)算速度快于使用特征點(diǎn)法的ORB-SLAM2，此算法存在累計(jì)誤差，在特征點(diǎn)丟失后便不能進(jìn)行重定位如果相機(jī)自動(dòng)曝光，當(dāng)調(diào)整曝光參數(shù)時(shí)，會(huì)使得整體變亮或變暗造成直接法無法計(jì)算。

為解決視覺SLAM系統(tǒng)在定位過程中遇到的問題，如單目SLAM系統(tǒng)無法恢復(fù)尺度信息等，香港科技大學(xué)QIN等[8]提出VINS-Mono(Monocular Visual-Inertial State Estimator)算法，通過單目視覺與IMU結(jié)合恢復(fù)單目相機(jī)的尺度因子，實(shí)現(xiàn)在四旋翼飛機(jī)載體中的定位與導(dǎo)航。但是，算法整體對(duì)單目相機(jī)和 IMU的性能以及CPU的數(shù)據(jù)處理要求較高[9-10]。隨后，提出支持多種傳感器組合的視覺慣性SLAM算法框架VINS-Fusion[11]。MUR-ARTAL、TARDS[12]提出結(jié)合ORB-SLAM 緊耦合的單目視覺慣性 SLAM，使單目相機(jī)的狀態(tài)變量與IMU 傳感器的狀態(tài)變量進(jìn)行數(shù)據(jù)融合，采用緊耦合方式共同構(gòu)建出系統(tǒng)的運(yùn)動(dòng)方程和觀測方程，再計(jì)算系統(tǒng)的狀態(tài)。其算法中IMU傳感器的狀態(tài)變量的初始化依賴單目相機(jī)狀態(tài)的初始化。

綜合上述研究，本文作者在無視覺信息輸入以及單目相機(jī)移動(dòng)過快視覺跟蹤丟失時(shí)，通過擴(kuò)展卡爾曼濾波融合里程計(jì)和IMU輸入觀測數(shù)據(jù)進(jìn)行移動(dòng)機(jī)器人定位，提供短時(shí)間位姿估計(jì)，保證在缺失相機(jī)位姿時(shí)移動(dòng)機(jī)器人能精確定位；視覺前端相機(jī)位姿計(jì)算采用半直接法，在移動(dòng)端側(cè)(嵌入式端)上實(shí)時(shí)運(yùn)行；移動(dòng)過程中完成單目相機(jī)尺度信息的計(jì)算，單目輸出位置信息通過編碼器輸出的速度信息和IMU信息進(jìn)行擴(kuò)展卡爾曼融合輸出位姿，改善了編碼器和慣性測量單元單獨(dú)定位精度低、累計(jì)誤差大的缺點(diǎn)；邊緣側(cè)混合式多傳感器融合免去當(dāng)前移動(dòng)機(jī)器人移動(dòng)過程必須有線連接便攜式計(jì)算機(jī)或者將便攜式計(jì)算機(jī)放置在移動(dòng)機(jī)器人頂部進(jìn)行SLAM算法計(jì)算的方式，減小了移動(dòng)機(jī)器人的體積，具有廣泛的擴(kuò)展性。

1 單目位姿輸出

MUR-ARTAL等提出的ORB-SLAM系統(tǒng)與其他SLAM系統(tǒng)相比較具有較高的精度和運(yùn)行效率。但是，嵌入式領(lǐng)域不具備實(shí)時(shí)運(yùn)行ORB-SLAM系統(tǒng)的能力。半直接法(SVO)視覺SLAM算法能夠在移動(dòng)側(cè)運(yùn)行完成相機(jī)位姿估計(jì)，但是半直接法(SVO)視覺里程計(jì)只能估計(jì)當(dāng)前時(shí)間的相機(jī)位姿，不具備后端優(yōu)化和閉環(huán)檢測的能力。

作者采用特征點(diǎn)與直接法相結(jié)合的方法進(jìn)行優(yōu)化相機(jī)位姿，將半直接法(SVO)前端位姿計(jì)算方法融合到ORB-SLAM2中，充分利用直接法對(duì)光照變化、稀疏紋理的魯棒性，在使用單目相機(jī)之前首先進(jìn)行相機(jī)標(biāo)定，獲取相機(jī)的內(nèi)參以及畸變參數(shù)。融合后的半直接法的初始化位姿流程如圖1所示。

圖1 Tracking線程流程

最小化殘差損失函數(shù)：

(1)

通過式(1)計(jì)算幀間匹配得到的當(dāng)前幀相機(jī)位姿具有累計(jì)誤差，從而導(dǎo)致相機(jī)位姿漂移，因此需要通過特征對(duì)齊找到地圖中特征塊在新一幀中真正的位置?；诠舛炔蛔冃缘募僭O(shè)，重新構(gòu)造殘差對(duì)特征點(diǎn)的預(yù)測位置進(jìn)行再次的優(yōu)化：

(2)

式(2)優(yōu)化后比式(1)預(yù)測的相機(jī)位姿位置更精準(zhǔn)，但是式(2)優(yōu)化后和式(1)預(yù)測的特征點(diǎn)的位置也會(huì)存在誤差，因此需要構(gòu)造新的目標(biāo)函數(shù)進(jìn)行優(yōu)化。由式(3)進(jìn)行優(yōu)化計(jì)算，得出更準(zhǔn)確的相機(jī)位姿：

(3)

2 多傳感器融合算法流程

張福斌、劉宗偉[13]提出改進(jìn)ORB-SLAM2算法，將單目相機(jī)、三軸陀螺儀、里程計(jì)3種傳感器進(jìn)行緊組合，通過非線性優(yōu)化實(shí)現(xiàn)精度較高的定位。而文獻(xiàn)[14]中創(chuàng)新性是提出穩(wěn)定自治系統(tǒng)的光照魯棒直接視覺里程計(jì)，空中機(jī)器人在不可預(yù)知的光照條件下飛行。因此，為提高移動(dòng)機(jī)器人的定位精度采用多傳感器融合方法。

根據(jù)傳感器融合方法的不同，多傳感器融合方式分為松耦合和緊耦合兩種方式。其中，松耦合是各個(gè)傳感器以各自的傳感器數(shù)據(jù)進(jìn)行運(yùn)算后再以融合算法將其數(shù)據(jù)融合后計(jì)算出位姿。緊耦合是考慮各個(gè)傳感器之間的耦合性，通過視覺和慣性傳感器進(jìn)行數(shù)據(jù)耦合計(jì)算，但是對(duì)計(jì)算載體的計(jì)算算力具備一定要求。多傳感器緊耦合框架面對(duì)某個(gè)傳感器失效時(shí)，會(huì)出現(xiàn)迅速漂移等問題。松耦合面對(duì)單個(gè)傳感器故障而導(dǎo)致失效時(shí)，也能夠精確定位。因此，本文作者采用基于擴(kuò)展卡爾曼濾波的松耦合融合算法進(jìn)行定位研究。

2.1 算法思路

本文作者通過采集單目攝像頭數(shù)據(jù)流、IMU和里程計(jì)數(shù)據(jù)，通過擴(kuò)展卡爾曼濾波融合算法進(jìn)行位姿定位?；舅悸啡鐖D2所示。

圖2 算法思路

2.2 單目視覺位姿估計(jì)模型：

針對(duì)單目相機(jī)，選用針孔相機(jī)模型。

Puv=KPw

(4)

(5)

移動(dòng)機(jī)器人移動(dòng)的距離xb通過編碼器和IMU的擴(kuò)展卡爾曼融合數(shù)據(jù)獲取，解決單獨(dú)使用編碼器計(jì)算移動(dòng)距離導(dǎo)致的誤差累計(jì)這一問題。與此同時(shí)獲取固定時(shí)間間隔內(nèi)的單目視覺里程計(jì)估算的移動(dòng)距離xc。在實(shí)驗(yàn)過程中，實(shí)時(shí)估計(jì)計(jì)算出單目視覺的尺度因子，采用如式(6)進(jìn)行計(jì)算，完成單目視覺的尺度恢復(fù)。

λ≈xb/xc

(6)

2.3 里程計(jì)模型

2.4 IMU運(yùn)動(dòng)模型

IMU的觀測模型描述如式(7)—式(8)所示，上標(biāo)I表示IMU坐標(biāo)系，上標(biāo)G表示參考坐標(biāo)系。

(7)

(8)

3 基于EKF的多傳感器融合算法實(shí)現(xiàn)

以擴(kuò)展卡爾曼濾波法作為多傳感器融合框架，是以松耦合方式融合編碼器里程計(jì)速度、單目視覺位姿、慣性傳感器信息進(jìn)行定位。設(shè)計(jì)的SLAM系統(tǒng)算法框架如圖3所示。

圖3 多信息融合算法

由于移動(dòng)機(jī)器人工作在平面中，它是非完整約束機(jī)器人，需要將3D變量歸零，包括Z軸、俯仰、橫滾以及各自的速度和Z軸加速度。但是需要將Y軸速度融合進(jìn)去，因?yàn)橹挥羞@樣才能夠證明Y軸沒有移動(dòng)。即表示為7維列向量：

系統(tǒng)方程及測量方程：

xk=f(xk-1)+ωk

(9)

zk=h(xk)+vk

(10)

其中：變量xk、zk分別表示為在時(shí)間k時(shí)刻的狀態(tài)向量和測量向量。協(xié)方差為Q的過程噪聲由變量ωk-1表示，協(xié)方差為R的觀測噪聲由vk表示。過程噪聲和觀測噪聲為互不相關(guān)的零均值高斯白噪聲。

(11)

(12)

(1)系統(tǒng)狀態(tài)初始化

多傳感器融合系統(tǒng)在接收到傳感器發(fā)出的第一個(gè)測量數(shù)據(jù)時(shí)，對(duì)系統(tǒng)狀態(tài)變量和狀態(tài)協(xié)方差進(jìn)行初始化，得出初始狀態(tài)量和初始狀態(tài)協(xié)方差。

(2)預(yù)測過程

(13)

(14)

(3)更新過程

(15)

(16)

(17)

上述傳感器的輸入數(shù)據(jù)Rk受到誤差Q和方差Pk-1的影響。

4 算法的驗(yàn)證及分析

4.1 實(shí)驗(yàn)平臺(tái)

為完成上述實(shí)驗(yàn)算法的驗(yàn)證，采用自主設(shè)計(jì)實(shí)驗(yàn)平臺(tái)，驗(yàn)證平臺(tái)采用Ackerman模型的四輪智能小車移動(dòng)機(jī)器人，按圖4所示的硬件框圖自主搭建實(shí)驗(yàn)平臺(tái)。設(shè)計(jì)的移動(dòng)機(jī)器人采用Teensy單片機(jī)讀取直流減速電機(jī)和MPU9250數(shù)據(jù)，視覺里程計(jì)傳感器采用樹莓派自帶單目相機(jī)。多傳感器融合的決策部分采用樹莓派完成移動(dòng)機(jī)器人位姿估計(jì)計(jì)算，PC端實(shí)時(shí)顯示當(dāng)前位姿，通過路由進(jìn)行無線鏈接進(jìn)行數(shù)據(jù)傳輸。搭建完成的實(shí)物平臺(tái)如圖5所示。

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

圖5 實(shí)物平臺(tái)

硬件平臺(tái)參數(shù)：

(1)相機(jī)為Pi Camera V2：分辨率2 592像素×1 944 像素(500萬像素)，感光芯片OV5647,角度65°；

(2)慣性傳感器采用MPU9250，其中陀螺儀采用±250(°)/s，陀螺儀的RMS噪聲0.1(°)/s(RMS),噪聲密度0.01(°)/(s·Hz1/2)；加速度為±2g；

(3)采用減速比30、編碼器精度為390的直流減速電機(jī)；

(4)采用四核64位處理器的3B+樹莓派；

(5)計(jì)算機(jī)：RedmiBook16,CPU為AMD-R7-4700U；

軟件平臺(tái)：

開發(fā)平臺(tái)使用安裝Linux系統(tǒng)虛擬機(jī)的RedmiBook16便攜式計(jì)算機(jī)。Linux系統(tǒng)版本是Ubuntu16.04，并在此系統(tǒng)下安裝ROS機(jī)器人操作系統(tǒng)。樹莓派使用Ubuntu16.04系統(tǒng)并搭建ROS系統(tǒng)。運(yùn)用ROS分布式的系統(tǒng)優(yōu)勢，進(jìn)行數(shù)據(jù)傳輸完成算法的軟件開發(fā)。

4.2 實(shí)驗(yàn)設(shè)計(jì)

采用Nature-Point公司的高速動(dòng)作捕捉系統(tǒng)Motive，完整記錄移動(dòng)機(jī)器人頂部mark點(diǎn)的移動(dòng)軌跡，設(shè)定動(dòng)作捕捉系統(tǒng)記錄的移動(dòng)軌跡為真值。設(shè)定小車以約0.01 m/s的速度恒速運(yùn)行，前輪舵機(jī)以約0.012 rad/s速度進(jìn)行轉(zhuǎn)彎。其中，由于移動(dòng)機(jī)器人運(yùn)動(dòng)在平面上，只有繞Z軸轉(zhuǎn)動(dòng)，因此繪制XY軸上的軌跡。圖形以EKF融合后的數(shù)據(jù)以及動(dòng)作捕捉獲取的移動(dòng)小車的真實(shí)位置數(shù)據(jù)繪制。

為驗(yàn)證光照變化對(duì)單目視覺以及融合算法的影響，根據(jù)不同時(shí)間光照射入室內(nèi)，在室內(nèi)形成的光影變化不同，設(shè)定光照較為明亮的14：00-16：30時(shí)間段為時(shí)間段1和光照變化明顯的17：00-19：30時(shí)間段為時(shí)間段2，在兩個(gè)時(shí)間段進(jìn)行驗(yàn)證實(shí)驗(yàn)數(shù)據(jù)采集。

設(shè)定兩種運(yùn)行軌跡運(yùn)行方式：

(1)圓弧運(yùn)行：(1)以設(shè)定轉(zhuǎn)向角度前進(jìn)運(yùn)動(dòng)軌跡成半圓?。?2)先以設(shè)定的舵機(jī)轉(zhuǎn)向角度前進(jìn)，然后再以原點(diǎn)為前進(jìn)方向運(yùn)行，后形成橢圓形圓弧。

(2)直線加圓弧運(yùn)行。進(jìn)行直線運(yùn)行，完成兩次直角轉(zhuǎn)彎，最后采用圓弧轉(zhuǎn)彎回歸到起始位置。

上述兩種運(yùn)行軌跡能夠完成室內(nèi)移動(dòng)機(jī)器人室內(nèi)移動(dòng)、避障、返回充電等全部運(yùn)動(dòng)的原始基本操作。

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

圖6是在時(shí)間段1移動(dòng)小車以設(shè)定轉(zhuǎn)彎角度在軸方向運(yùn)行約1 m左右后，再以原點(diǎn)作為運(yùn)行終點(diǎn)，記運(yùn)行的為軌跡1。

圖6 軌跡1實(shí)驗(yàn)結(jié)果對(duì)比

圖7是在時(shí)間段2先沿直線運(yùn)行，在第二個(gè)直角轉(zhuǎn)彎后，以設(shè)定轉(zhuǎn)動(dòng)角度使移動(dòng)小車返回到起點(diǎn)，記為軌跡2。

圖7 軌跡2實(shí)驗(yàn)結(jié)果對(duì)比

表1 不同傳感器及融合方案均方根誤差比較 單位：m

在光照強(qiáng)度變化的場景下，單目視覺里程計(jì)誤差明顯減小，單目稀疏法的視覺里程計(jì)能夠準(zhǔn)確估計(jì)相機(jī)位姿。其中，擴(kuò)展卡爾曼濾波融合各個(gè)傳感器數(shù)據(jù)后得到的均方根誤差相較于單個(gè)傳感器均方根誤差明顯減小。

以上實(shí)驗(yàn)證明在光照強(qiáng)度變化的環(huán)境中采用稀疏直接法(半直接法)的視覺里程計(jì)計(jì)算出的相機(jī)位姿具備有效性和準(zhǔn)確性，并且根據(jù)視覺得出的位姿，EKF融合算法抑制了編碼器的誤差累計(jì)，有效提升整體移動(dòng)機(jī)器人的定位精度。

5 結(jié)論

以稀疏直接法作為單目視覺的前端估計(jì)相機(jī)位姿，結(jié)合慣性傳感器和編碼器進(jìn)行擴(kuò)展卡爾曼濾波多傳感器融合定位。通過在光照明亮不變和光照變化的時(shí)間段內(nèi)進(jìn)行實(shí)驗(yàn),驗(yàn)證此算法的準(zhǔn)確性，證明該算法能夠改善在光照變化的場景中的定位精度，提高單個(gè)傳感器定位的定位精度，降低編碼器的累計(jì)誤差，擴(kuò)展單目視覺的應(yīng)用場景，減小了當(dāng)前自主定位移動(dòng)機(jī)器人的體積。