孫希君 王秋瀅 王水根 吳應(yīng)為

摘 ?要：精確魯棒的定位系統(tǒng)是保證室內(nèi)巡檢機(jī)器人正常工作的重要基礎(chǔ)。文章基于機(jī)器人操作系統(tǒng)（ROS），針對(duì)現(xiàn)有公開視覺算法ORB-SLAM2在低性能計(jì)算平臺(tái)上因計(jì)算能力不足導(dǎo)致的特征跟蹤丟失問題，提出一種將ORB-SLAM2與慣性導(dǎo)航系統(tǒng)（Inertial Navigation System， INS）解算誤差進(jìn)行卡爾曼濾波融合的方法。經(jīng)公開數(shù)據(jù)集驗(yàn)證表明，該方法能夠完整地估計(jì)出視覺失效時(shí)丟失的位姿信息，與ORB-SLAM2相比，定位系統(tǒng)的精度與魯棒性有效提高。

關(guān)鍵詞：ROS;定位;卡爾曼濾波;ORB-SLAM2

中圖分類號(hào)：TP242 ? ?文獻(xiàn)標(biāo)識(shí)碼：A文章編號(hào)：2096-4706（2021）13-0139-06

Visual Inertial Fusion Positioning Method Based on ROS Indoor Inspection Robot

SUN Xijun1， WANG Qiuying2，3， WANG Shuigen4， WU Yingwei1

（1.College of Information and Communication Engineering， Harbin Engineering University， Harbin ?150001， China; 2.College of Underwater Acoustic Engineering， Harbin Engineering University， Harbin ?150001， China; 3.Key Laboratory of Underwater Acoustic Technology， Harbin Engineering University， Harbin ?150001， China; 4.Yantai Iray Technology Co.， Ltd.， Yantai ?264006， China）

Abstract： Accurate and robust positioning system is an important basis to ensure the normal operation of indoor inspection robot. Based on the robot operating system （ROS）， aiming at the loss of feature tracking caused by the insufficient computing power of the existing public vision algorithm ORB-SLAM2 on the low-performance computing platform， this paper proposes a method for Kalman filter fusion in the solution errors of ORB-SLAM2 and inertial navigation system （INS）. The verification of public data sets shows that this method can completely estimate the pose information lost when visual failure. Compared with ORB-SLAM2， the accuracy and robustness of the positioning system are effectively improved.

Keywords： ROS; positioning; Kalman filter; ORB-SLAM2

0 ?引 ?言

在化學(xué)工業(yè)工廠、電力設(shè)備廠房、電力隧道、物流倉儲(chǔ)庫房等室內(nèi)高危作業(yè)環(huán)境下，配備24小時(shí)持續(xù)巡檢機(jī)器人可有效提升作業(yè)人員的工作效率，并在極大程度上保證員工安全問題。確保巡檢機(jī)器人正常工作的關(guān)鍵前提是機(jī)器人自身可以實(shí)現(xiàn)高精度、高可靠性的實(shí)時(shí)自主定位。

常見的用于單一定位系統(tǒng)的傳感器主要有衛(wèi)星接收機(jī)、激光雷達(dá)、視覺相機(jī)、慣性測量單元（Inertial Measurement Unit， IMU）由于建筑物遮擋等影響室內(nèi)機(jī)器人無法通過衛(wèi)星信號(hào)進(jìn)行精準(zhǔn)定位，激光雷達(dá)的價(jià)格相對(duì)昂貴，而視覺相機(jī)與IMU的成本相對(duì)低廉、且無須外界提供任何信息便可進(jìn)行獨(dú)立自主定位，因此利用視覺相機(jī)與IMU進(jìn)行運(yùn)載體的定位逐漸引起學(xué)術(shù)研究者的廣泛關(guān)注[1]。

現(xiàn)有的純視覺算法都存在著易受光照、相機(jī)快速運(yùn)動(dòng)等問題的影響，基于直接法的DSO（Direct Sparse Odometry）算法[2]在計(jì)算速度上相對(duì)較快，但是對(duì)光照尤其敏感，在實(shí)際場景中很難保持穩(wěn)定的曝光時(shí)間以至于系統(tǒng)經(jīng)常無法正常工作;基于特征點(diǎn)法的Orb-slam算法[3]在相機(jī)平穩(wěn)運(yùn)動(dòng)時(shí)定位精度較高，但在相機(jī)快移動(dòng)或光照突然變化等復(fù)雜動(dòng)態(tài)場景下會(huì)出現(xiàn)跟蹤丟失[4]無法檢測特征點(diǎn)的問題。本文基于ROS（Robot Operating System）開發(fā)平臺(tái)，驗(yàn)證分析了ORB-SLAM2算法的實(shí)際效果，并針對(duì)算法出現(xiàn)的問題提出一種與IMU誤差狀態(tài)進(jìn)行卡爾曼濾波融合的方法來對(duì)視覺信息進(jìn)行補(bǔ)償修正，解決了復(fù)雜場景下純視覺算法跟蹤丟失的問題，提高了系統(tǒng)的定位精度與魯棒性。

1 ?ORB-SLAM2視覺算法

ORB-SLAM2算法是目前非常完善的主流視覺SLAM算法之一，它通過提取并匹配圖像中的ORB特征來逐幀估計(jì)相機(jī)的位置與姿態(tài)。整個(gè)算法由三個(gè)線程分別進(jìn)行，跟蹤線程負(fù)責(zé)ORB特征的提取與相機(jī)位姿估計(jì)并確定是否需要引入關(guān)鍵幀，局部建圖線程負(fù)責(zé)處理新的關(guān)鍵幀并完成局部地圖的創(chuàng)建，采用局部束調(diào)整的方法來篩選去除多余的關(guān)鍵幀，對(duì)局部空間內(nèi)的特征點(diǎn)與相機(jī)位姿進(jìn)行求解，閉環(huán)檢測部分則對(duì)全局的地圖與關(guān)鍵幀進(jìn)行檢測與校正以消除累積誤差。其最大的優(yōu)點(diǎn)是在于它在原有的視覺里程計(jì)基礎(chǔ)上增加了閉環(huán)檢測線程，保證了相機(jī)軌跡與地圖的全局一致性，一定程度上避免了累積誤差對(duì)系統(tǒng)帶來的影響。ORB-SLAM2算法的整體流程圖如圖1所示。

但ORB-SLAM2算法采用的三線程結(jié)構(gòu)也對(duì)計(jì)算機(jī)的CPU負(fù)荷能力有更高的要求，當(dāng)相機(jī)快速運(yùn)動(dòng)引起圖像模糊或者光照強(qiáng)度突變時(shí)，在性能較低的計(jì)算機(jī)或嵌入式系統(tǒng)上將無法正常工作，會(huì)出現(xiàn)特征點(diǎn)跟蹤丟失的問題。因此，本文在原有的ORB-SLAM2算法基礎(chǔ)上做了改進(jìn)，將其與慣性導(dǎo)航誤差狀態(tài)做卡爾曼濾波融合，用慣性導(dǎo)航解算的誤差修正ORB-SLAM2算法在特征跟蹤丟失時(shí)的位姿誤差，并將誤差反饋到子系統(tǒng)中進(jìn)行迭代修正，使其可以在低性能計(jì)算平臺(tái)上更穩(wěn)定地自主工作。

2 ?濾波融合定位算法

多傳感器融合主要包括緊耦合和松耦合2種基本框架。松耦合是指各子系統(tǒng)分別解算后再融合，緊耦合則是在系統(tǒng)前端將各子系統(tǒng)的測量參數(shù)均加入模型中，狀態(tài)量維度的提高使得緊耦合算法的計(jì)算量也更大，為不再額外增加平臺(tái)的計(jì)算負(fù)擔(dān)且便于后續(xù)研究中視覺算法的輕量化改進(jìn)，本文采用松耦合框架來構(gòu)建模型實(shí)現(xiàn)視覺慣性的數(shù)據(jù)融合。

2.1 ?構(gòu)建IMU誤差狀態(tài)模型

本文利用IMU自身的傳感器誤差及解算得到的導(dǎo)航信息的誤差作為狀態(tài)量進(jìn)行建模，微小量誤差相對(duì)于直接將導(dǎo)航信息作為狀態(tài)量的方法在線性化過程[5]中得到的結(jié)果更為精確。

IMU測量獲取到的原始數(shù)據(jù)的計(jì)算公式為：

ai=Riw（i）（aw-gw）+ab+an ? ? ? ? ? ? ? ? ? ? ?   ? ?（1）

ωi=ωw+ωb+ωn ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? （2）

其中，ai、aw、ab、an分別表示加速度計(jì)在傳感器坐標(biāo)系下測得的帶誤差的實(shí)際加速度、世界坐標(biāo)系下不帶噪聲的真實(shí)加速度、加速度計(jì)自身零偏、測量過程中加速度計(jì)產(chǎn)生的隨機(jī)噪聲，gw表示重力向量，Riw（i）表示在慣性導(dǎo)航模塊中定義的世界坐標(biāo)系到IMU坐標(biāo)系的旋轉(zhuǎn)矩陣，本文中直接將IMU坐標(biāo)系作為世界坐標(biāo)系，ωi、ωw、ωb、ωn分別表示陀螺儀在傳感器坐標(biāo)系下測得的帶誤差的實(shí)際角速度、世界坐標(biāo)系下不帶誤差的真實(shí)角速度、陀螺儀自身零偏以及測量過程中陀螺儀產(chǎn)生的隨機(jī)噪聲。

慣性導(dǎo)航解算中對(duì)加速度進(jìn)行一次積分獲取速度信息，再進(jìn)行二次積分獲取位置信息，本文為避免歐拉角萬向鎖的影響，采用漢密爾頓表示法的四元數(shù)來描述姿態(tài)信息[6]。實(shí)際場景中自主運(yùn)動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型會(huì)受隨機(jī)誤差及IMU內(nèi)部噪聲的影響，本文假設(shè)兩種噪聲均為高斯白噪聲。結(jié)合上述IMU測量的計(jì)算公式可推導(dǎo)出真實(shí)狀態(tài)下的運(yùn)動(dòng)學(xué)微分方程為：

Pw（i）=vw（i）

vw（i）=aw=Riw（i）T（ai-ab-an）+gw

qw（i）=qw（i）ωw=qw（i）（ωi-ωb-ωn）

ab=aε

ωb=ωε ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?（3）

其中pw（i）、vw（i）、aw、ωw分別表示機(jī)器人在世界坐標(biāo)系下的位置、速度、加速度與角速度信息，qw（i）表示機(jī)器人在三維空間中的方向信息，ab、ωb分別表示慣性測量單元加速度計(jì)與陀螺儀的零偏誤差，aε、ωε分別表示加速度計(jì)與陀螺儀的隨機(jī)游走噪聲，該世界坐標(biāo)系在本文中即為IMU坐標(biāo)系。

理想狀態(tài)下的運(yùn)動(dòng)學(xué)模型是在假設(shè)不存在外界隨機(jī)誤差及傳感器內(nèi)部偏差的情況下建立的，忽略掉噪聲影響，理想的標(biāo)稱運(yùn)動(dòng)學(xué)微分方程為：

Ps=vs

vs=as=Riw（i）T（ai-ab）+gs

qs=qsωs=qs（ωi-ωb）

abs=0

ωbs=0 ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? （4）

其中ps、vs、qs分別表示機(jī)器人在世界坐標(biāo)系下的理想位置、速度、四元數(shù)姿態(tài)信息，as、ωs分別表示不考慮零偏誤差的加速度、角速度信息，gs表示理想重力加速度，abs、ωbs分別表示理想情況下的加速度計(jì)與陀螺儀零偏誤差。

將真實(shí)狀態(tài)運(yùn)動(dòng)學(xué)方程與理想狀態(tài)運(yùn)動(dòng)學(xué)方程聯(lián)立作差，便可推算出IMU解算信息誤差以及傳感器內(nèi)部隨機(jī)游走誤差的運(yùn)動(dòng)學(xué)模型：

δp=δv

δv=-Riw（i）T[ai-ab]×δθ-Riw（i）Tδab+δg-Riw（i）Tan

δθ=-[ωi-ωb]×δθ-δωb-ωn

δab=aε

δωb=ωε ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?（5）

其中，δp=pw（i）-ps表示位置誤差，δv=vw（i）-vs表示速度誤差，δθ表示旋轉(zhuǎn)的角度矢量，由qw（i）=qseδθ/2計(jì)算得到，δab=ab-abs表示加速度計(jì)誤差，δωb=ωb-ωbs表示陀螺儀誤差，δg=gw-gs表示重力加速度誤差，本文將其忽略不計(jì)，即取δg=0，[·]×表示反對(duì)稱矩陣。

2.2 ?創(chuàng)建觀測模型

ORB-SLAM2利用采集到的圖像數(shù)據(jù)可逐幀估計(jì)出相機(jī)的位姿信息，在ORB-SLAM2中位姿信息由跟蹤線程中的函數(shù)返回，在此基礎(chǔ)上，本文用4×4的剛體變換矩陣Tcw（c）來描述ORB-SLAM2算法返回的相機(jī)姿態(tài)，其公式為：

（6）

其中3×3矩陣Rcw（c）表示相機(jī)在世界坐標(biāo)系中（相機(jī)的初始幀坐標(biāo)系記為相機(jī)當(dāng)前對(duì)應(yīng)的世界坐標(biāo)系）由初始幀到當(dāng)前幀姿態(tài)的旋轉(zhuǎn)矩陣，三維向量tcw（c）是用來描述相機(jī)在該世界坐標(biāo)系下的位置的平移向量，該矩陣描述的是相機(jī)坐標(biāo)系到世界坐標(biāo)系的變換關(guān)系。對(duì)該矩陣求逆即可得到相機(jī)的外參矩陣，由于旋轉(zhuǎn)矩陣為正交陣，故可推導(dǎo)出轉(zhuǎn)換公式為：

（7）

由旋轉(zhuǎn)矩陣和平移向量可以推導(dǎo)出相機(jī)的三維位置和姿態(tài)四元數(shù)信息，位置計(jì)算公式為：

pcx=tcw（c）（1，1）

pcy=tcw（c）（2，1） ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?（8）

pcz=tcw（c）（3，1）

其中pc=[pcx，pcy，pcz]T表示相機(jī)測量到的相機(jī)坐標(biāo)系下的位置信息。

測量誤差為觀測值與理想預(yù)測值的殘差，本文中預(yù)測值通過IMU解算得出，因此需要將相機(jī)坐標(biāo)系下的位姿信息轉(zhuǎn)換到IMU坐標(biāo)系下進(jìn)行空間同步，相機(jī)坐標(biāo)系到IMU坐標(biāo)系的轉(zhuǎn)換關(guān)系可通過標(biāo)定得出，記旋轉(zhuǎn)關(guān)系為Ric，平移關(guān)系為pic，可推導(dǎo)出相機(jī)在IMU坐標(biāo)系下的位置：

（9）

其中pi=[pix，piy，piz]T表示相機(jī)測量到的在IMU坐標(biāo)系下的位置信息，三個(gè)坐標(biāo)系的空間變換過程為：

Riw（c）=RicRcw（c） ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? （10）

其中Riw（c）表示相機(jī)從相機(jī)的世界坐標(biāo)系到IMU坐標(biāo)系的旋轉(zhuǎn)矩陣，有qi=[qiw，qix，qiy，qiz]T，qiw為實(shí)部，則相機(jī)由世界坐標(biāo)系到IMU坐標(biāo)系下的姿態(tài)四元數(shù)信息轉(zhuǎn)換公式為：

（11）

由上述兩組公式可以構(gòu)建出3維位置觀測誤差模型：

δpi=pi-ps ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?（12）

通過以下公式可以推導(dǎo)出4維旋轉(zhuǎn)矢量角模型：

（13）

2.3 ?濾波融合定位算法

在數(shù)據(jù)融合過程中將所有需要估計(jì)的信息列入狀態(tài)量中，可用來進(jìn)行輔助修正的信息列為觀測量，本文中將IMU的解算誤差及傳感器內(nèi)部隨機(jī)游走誤差作為狀態(tài)量，記X=[δp ?δv δθ ?δab ?δωb]T，觀測量為已與IMU進(jìn)行空間同步的視覺模塊觀測到的位姿誤差，記Z=[δpi ?δθi]T。融合定位算法的總體流程如圖2所示。

以慣導(dǎo)系統(tǒng)的預(yù)測誤差作為狀態(tài)量，視覺解算的誤差作為觀測量，進(jìn)行卡爾曼濾波融合過程，當(dāng)ORB-SLAM2算法可以正常工作時(shí)相信觀測模型，在視覺特征跟蹤丟失時(shí)相信預(yù)測模型，為使IMU的預(yù)測精度更高，在每一次迭代后將更新得到的估計(jì)誤差控制量反饋到慣導(dǎo)系統(tǒng)內(nèi)部對(duì)狀態(tài)量進(jìn)行校正，記更新后的估計(jì)誤差為δx=[δp ? ?δv ? ? δθ ? ? δab ? ? ? ?δωb] T，通過以下公式可計(jì)算得到經(jīng)過校正的信息：

（14）

其中p為定位系統(tǒng)估計(jì)的新位置信息，q為新姿態(tài)四元數(shù)信息。

3 ?驗(yàn)證及分析

該融合算法完全基于kinetic版本的ROS開發(fā)測試，實(shí)驗(yàn)環(huán)境為Ubuntu16.04系統(tǒng)，CPU為英特爾i5-8300H。為驗(yàn)證分析本文融合定位算法的有效性，本文采用蘇黎世聯(lián)邦理工學(xué)院ETH公開的室內(nèi)視覺慣性數(shù)據(jù)集對(duì)ORB-SLAM2算法和改進(jìn)后的融合算法性能進(jìn)行對(duì)比測試。該數(shù)據(jù)集的信息由搭載了雙目VIO相機(jī)和運(yùn)動(dòng)捕捉系統(tǒng)的微型飛行器來采集，數(shù)據(jù)集中包含雙目圖像、已進(jìn)行時(shí)間同步的IMU測量值以及精確的地面真值，其傳感器的裝配坐標(biāo)系如圖3所示。

由于IMU安裝在飛行器的正中心，IMU坐標(biāo)系與飛行器的本體坐標(biāo)系重合，因此，本文將視覺估計(jì)信息轉(zhuǎn)換到IMU坐標(biāo)系下進(jìn)行解算得到的位姿即機(jī)器人的實(shí)際位姿。

對(duì)ORB-SLAM2算法進(jìn)行驗(yàn)證，在飛行器運(yùn)動(dòng)過程中，相機(jī)在正常工作狀態(tài)下ORB特征點(diǎn)跟蹤情況如圖4所示，當(dāng)相機(jī)受到光照變化等環(huán)境因素影響時(shí)，跟蹤線程無法正常工作，如圖5所示部分出現(xiàn)特征點(diǎn)跟蹤丟失的問題。

ORB-SLAM2算法的三線程使其對(duì)運(yùn)行平臺(tái)的計(jì)算能力要求較高，因此就出現(xiàn)了上述跟蹤丟失的狀況。將其解算得到的位姿信息、本文的融合位姿信息與運(yùn)動(dòng)捕捉系統(tǒng)采集到的基準(zhǔn)值作對(duì)比，二維位置信息對(duì)比如圖6所示，橫滾、俯仰、偏航三個(gè)方向的姿態(tài)角信息對(duì)比如圖7所示。

由圖6與圖7可以看出視覺算法定位過程中出現(xiàn)兩次特征跟蹤丟失的狀況，ORB-SLAM2在本實(shí)驗(yàn)平臺(tái)上的魯棒性較弱，當(dāng)CPU負(fù)擔(dān)較大時(shí)，跟蹤線程無法正常工作，就會(huì)出現(xiàn)如圖中的丟失狀況，定位精度大大降低。而本文的算法解決了視覺跟蹤丟失的問題，在跟蹤丟失時(shí)以IMU的估計(jì)值去校正結(jié)果，經(jīng)過校正的估值更趨近于基準(zhǔn)值。

圖8與圖9分別將ORB-SLAM2算法和本文融合算法估計(jì)的二維軌跡與真實(shí)軌跡進(jìn)行對(duì)比，兩圖中灰色虛線均表示地面真實(shí)參考值，彩色線分別表示ORBSLAM2的軌跡與本文融合算法的軌跡，由藍(lán)到紅的顏色過渡條表示軌跡絕對(duì)誤差的大小。由圖可以更直觀地看出跟蹤丟失路段的估值已得到較好地修正，本文算法估計(jì)的整體軌跡更靠近真實(shí)軌跡。

采用計(jì)算絕對(duì)位姿誤差的方式來驗(yàn)證軌跡的全局一致性，對(duì)兩種算法的定位誤差進(jìn)行評(píng)定及分析，定位誤差如圖10、圖11所示，ORB-SLAM2算法的定位誤差在跟蹤丟失時(shí)最大誤差達(dá)3.22 m，整個(gè)運(yùn)動(dòng)過程中最小誤差0.13 m，均值0.78 m，均方根誤差1.0 m，利用本文的方法進(jìn)行融合校正之后跟蹤丟失問題得到有效解決，定位誤差最大為0.71 m，最小誤差0.003 m，均值0.08 m，均方根誤差0.11 m，各項(xiàng)誤差參數(shù)均表明，本文提出的算法定位精度有效提升，方法具有可行性。

4 ?結(jié) ?論

本文提出了一種基于ROS開發(fā)的視覺慣性融合定位算法，解決了ORB-SLAM2算法在低性能計(jì)算平臺(tái)上因計(jì)算能力不足出現(xiàn)的ORB特征跟蹤丟失問題。本文首先建立了IMU誤差模型與視覺觀測誤差模型，并將視覺信息與IMU信息進(jìn)行空間同步，最后利用卡爾曼濾波估計(jì)出位姿誤差，將誤差反饋到系統(tǒng)中進(jìn)行位姿更新。經(jīng)數(shù)據(jù)集驗(yàn)證分析表明，本文提出的融合算法具有可行性，在定位精度和魯棒性上均有效提高。未來的工作包括：輕量化視覺部分，減輕CPU計(jì)算負(fù)擔(dān)，從根本上提高系統(tǒng)的可靠性。

參考文獻(xiàn)：

[1] 楊觀賜，王霄遠(yuǎn)，蔣亞汶，等.視覺與慣性傳感器融合的SLAM技術(shù)綜述 [J].貴州大學(xué)學(xué)報(bào)（自然科學(xué)版），2020，37（6）：1-12.

[2] ENGEL J，KOLTUN V，CREMERS D. Direct Sparse Odometry [J].IEEE Transactions on Pattern Analysis and Machine Intelligence，2018，40（3）：611-625.

[3] MUR-ARTAL R，TARDóS J D. ORB-SLAM2：AnOpen-Source SLAM System for Monocular，Stereo，and RGB-D Cameras [J].IEEE Transactions on Robotics，2017，33（5）：1255-1262.

[4] 黃劍雄，劉小雄，章衛(wèi)國，等.基于視覺/慣導(dǎo)的無人機(jī)組合導(dǎo)航算法研究 [J].計(jì)算機(jī)測量與控制，2021，29（2）：137-143+149.

[5] 杜小菁，陳洪，王欣，等.基于SINS解算的行人導(dǎo)航技術(shù)發(fā)展綜述 [J].戰(zhàn)術(shù)導(dǎo)彈技術(shù)，2020（1）：113-120.

[6] 孟春見，李開龍，張夢得.捷聯(lián)慣性基組合導(dǎo)航濾波算法比較研究 [J].電光與控制，2020，27（2）：18-21.

作者簡介：孫希君（1997—），女，漢族，山東武城人，碩士研究生在讀，研究方向：傳感器融合定位、組合導(dǎo)航。