王 瑞,王茂森,戴勁松,管紅根

( 南京理工大學 機械工程學院, 南京 210094)

0 引言

以多旋翼無人機為主的小型空中機器人技術(shù)取得了長足發(fā)展,廣泛應(yīng)用于軍事偵察、武裝打擊、人員搜救,場景重建等領(lǐng)域。上述應(yīng)用場景中,無人機主要依靠工作人員遙控操作,只有少部分可以根據(jù)電腦終端設(shè)定的軌跡進行小范圍、低復雜度的飛行。小型多旋翼無人機的定位是實現(xiàn)其在復雜環(huán)境下自主移動的基本問題,在軍事、民用等多種領(lǐng)域下有著廣闊的應(yīng)用前景。對于實現(xiàn)無人機的智能化與自主化而言,最重要的前提是無人機能夠利用機載傳感器完成對環(huán)境的感知和自身位姿的精確估計。常見機載傳感器有GPS、北斗、慣性測量單元、視覺傳感器和激光傳感器等。GPS、北斗等衛(wèi)星導航定位系統(tǒng)具有全天候工作、覆蓋范圍廣等特點,將其與慣性測量單元融合,可以獲得較高的位姿估計精度,在軍用和民用無人機上有較多應(yīng)用,但當無人機在城市或叢林等遮擋嚴重的環(huán)境中時,衛(wèi)星導航系統(tǒng)的定位數(shù)據(jù)不可靠,甚至無法使用[1]。文獻[2]利用激光雷達產(chǎn)生的點云數(shù)據(jù)實現(xiàn)無人機在缺乏GPS信號情況下進行自身定位,但激光雷達體積大、且價格昂貴,不適用于小型旋翼無人機。文獻[3]使用單目相機估計無人機位姿并基于Apriltag2進行融合以獲取深度信息,該方法可以彌補單目相機無法獲取環(huán)境尺度的缺陷,尺度信息依賴于Apriltag2,在室內(nèi)場景下有較好效果,但不適用于室外環(huán)境。文獻[4]提出了使用RTK技術(shù)輔助定位的方案,在室外可以獲得較高的定位精度,但無人機搭載的相機僅用于拍攝與后期數(shù)據(jù)處理,沒有介紹在RTK信號覆蓋不到場景下的解決方案。文獻[5]則進一步將RTK定位數(shù)據(jù)融合到基于視覺的定位方案中,通過擴展卡爾曼濾波算法將不同信息進行有效融合,有利于解決由于累積誤差造成的漂移問題,但該方式需要事先在環(huán)境中架設(shè)RTK基站,不具有普適性。文獻[6]使用視覺與高度傳感器進行無人機在室內(nèi)環(huán)境下的定位,但只適用于低速運動情況,且在室外光線變化大時效果不理想?；趦?yōu)化的視覺慣性里程計(visual-inertial-odometry,VIO)采用視覺和慣性測量單元緊耦合的方式來估計本體運動狀態(tài)[7],充分結(jié)合了2種傳感器的特性,在GPS信號缺失的場景下具有定位精度高、成本低等優(yōu)點,因此該方式逐漸成為當前主流發(fā)展趨勢,受到了普遍關(guān)注。

針對小型多旋翼無人機在無GNSS信號的復雜場景下對自身位姿的估計問題,在基于優(yōu)化的視覺慣性里程計基礎(chǔ)上,提出改進的里程計并構(gòu)建一套無人機系統(tǒng)進行驗證。通過改進后的RANSAC算法剔除外點,設(shè)計飛控通信接口與控制模塊并搭建了gazebo仿真環(huán)境。實驗驗證,改進后算法具有良好的定位精度與實時性,對無人機在復雜環(huán)境下的自主運動具有實際工程意義。

1 視覺慣性里程計

1.1 視覺SLAM數(shù)學模型

經(jīng)典的視覺SLAM數(shù)學模型如下:

(1)

式(1)表示運動方程和觀測方程,xk與xk-1分別表示k,k-1時刻的位姿,uk表示k時刻的運動輸入量,wk表示對應(yīng)的噪聲。zk, j表示k時刻,在xk位姿下對yj路標的觀測值,vk, j表示k時刻的觀測噪聲。在視覺慣性里程計中,f(xk-1,uk)由慣性測量單元的數(shù)學模型確定,h(yj,xk)由相機投影模型確定。慣性測量單元通過預(yù)積分模型構(gòu)造殘差項,而視覺部分通過重投影誤差構(gòu)造殘差項,最后利用非線性優(yōu)化算法求解代價函數(shù)即可得到融合后的最優(yōu)狀態(tài)量。

1.2 對極約束與基礎(chǔ)矩陣

相機分別從視角1與視角2觀察三維空間中同一點P,在P點未知的情況下,求解視角1到視角2相機的運動,可以通過對極幾何約束解決,如圖1所示。

圖1 對極幾何Fig.1 Epipolar geometry

對極約束關(guān)系可表示為公式(2):

(2)

式中:p1與p2為真實物理世界中某點P分別在視角1與視角2下的投影,F為3×3的基礎(chǔ)矩陣。p1與p2使用齊次坐標,在相差一個常系數(shù)情況下式(2)仍然成立,因此F矩陣的自由度為8,一般使用8點法[8]計算。假設(shè)p1=[x1,y1,1]T,p2=[x2,y2,1]T,代入式(2),得到:

(3)

將基礎(chǔ)矩陣F各元素展開為一個向量處理,可得到8點法求解基礎(chǔ)矩陣的一般式。

文獻[9]認為,原始圖像坐標點的齊次值各分量的數(shù)量級相差太大,導致8點法求解基礎(chǔ)矩陣的結(jié)果不穩(wěn)定,提出一種預(yù)處理方式,先對像素坐標進行歸一化處理,再使用8點法求解基礎(chǔ)矩陣,最后通過基礎(chǔ)矩陣分解得到相機運動的旋轉(zhuǎn)矩陣和平移向量。

在已知三維空間點和像素點的情況下,可以通過n點透視法(Perspective-n-Point,PnP)求解相機位姿。無論是通過對極約束還是PnP方法求解相機位姿,求解精度與特征點的匹配精度都有很大關(guān)系,因此,對前端算法進行改進,通過剔除錯誤匹配點,達到提高里程計精度與魯棒性的目的。

2 改進RANSAC算法剔除外點

2.1 RANSAC算法

根據(jù)前端光流追蹤或特征匹配算法,不同視角下兩幅圖像的匹配點通常遠遠大于8對,為充分利用更多的點計算基礎(chǔ)矩陣,可以采用最小二乘算法。但在實踐中考慮匹配存在誤差,由于噪聲的存在,匹配點對之間并不能嚴格遵循式(2),最小二乘算法中錯誤的匹配數(shù)據(jù)可能會導致結(jié)果偏離真實值。隨機采樣一致性算法(random sample consensus,RANSAC)是一種從包含錯誤值的數(shù)據(jù)中迭代估計模型參數(shù)的算法[10]。它具有異常檢測的特性,在視覺慣性里程計中,將錯誤匹配點稱為外點,正確的匹配點稱為內(nèi)點,借助該算法可以實現(xiàn)2個作用:在缺失3D點時計算基礎(chǔ)矩陣,從而進一步分解出相機的運動姿態(tài);向里程計后端發(fā)送匹配點對之前,通過對極約束計算基礎(chǔ)矩陣的方式,盡可能剔除外點,為后端優(yōu)化創(chuàng)造更好的條件。

傳統(tǒng)的RANSAC算法包含以下幾個步驟:

1) 將所有匹配點集S中的特征點進行歸一化處理。

2) 在匹配點集S中隨機采樣抽取8對點,采用8點法計算基礎(chǔ)矩陣F。

3) 在基礎(chǔ)矩陣F下,計算所有匹配點的重投影誤差e,并與預(yù)設(shè)閾值T進行比較,當e≤T時,記該點為內(nèi)點,否則將該點標記為外點,通過內(nèi)點占總匹配點的比例計算該模型得分score。

4)重復步驟2)、3),直到達到最大迭代次數(shù)或滿足內(nèi)點比例超過預(yù)設(shè)值,獲取得分最高的結(jié)果作為基礎(chǔ)矩陣F,并從匹配點集S中剔除該估計下的所有外點。

2.2 改進的RANSAC算法

2.2.1帶優(yōu)先級的樣本點采集策略

實際環(huán)境中由于存在動態(tài)目標的干擾,如果某次采樣的匹配點位于動態(tài)目標區(qū)域,基礎(chǔ)矩陣的估計可能會失效,盡管RANSAC算法能夠在一定程度上避免這種情況,但傳統(tǒng)RANSAC算法的樣本點采集完全隨機,如果多次采樣都存在樣本點位于動態(tài)目標區(qū)域的情況,基礎(chǔ)矩陣估計的精度和魯棒性會降低[11]。

因此本文中提出一種帶優(yōu)先級的樣本點采集策略:首先通過目標檢測算法對潛在的動態(tài)目標進行檢測,獲取目標二維包圍框集合B,假設(shè)b為檢測結(jié)果集合B中某一目標包圍框,其滿足:

b=[cx,cy,h,w]T

(4)

式中:cx、cy表示包圍框b的中心點像素坐標,h和w分別表示包圍框b的高和寬。其次,構(gòu)造特征容器與哈希表,特征容器的元素保存特征點的二維像素坐標,哈希表的鍵碼k保存特征點在容器中的索引,對應(yīng)的值記為mp[k],它的值由特征點坐標值與集合B中各包圍框的關(guān)系確定,計算公式如下:

(5)

(6)

式(5)中,m為大于0的正整數(shù),px與py表示鍵碼k所對應(yīng)特征點的像素坐標,改進后帶優(yōu)先級的樣本點采集策略為:在一次迭代中,對于某特征點p,先在哈希表中查詢其對應(yīng)值是否為0,如果為0,則將其作為一個樣本點,如果是大于0的正整數(shù),則將其值減1,重新選擇樣本點。通過優(yōu)先級的設(shè)置,在采集樣本點時將優(yōu)先采集潛在的動態(tài)目標區(qū)域之外的特征點,從而在一定程度上提高基礎(chǔ)矩陣的估計精度。本文中并沒有將潛在的動態(tài)目標包圍框內(nèi)部的特征點直接剔除,因為僅依靠目標檢測算法獲取的包圍框內(nèi)的目標并不一定處于運動狀態(tài),如果目標所占圖像比例較大且處于靜止狀態(tài),直接剔除無疑會浪費較多的有效特征點,因此,在不借助其他更加復雜算法如目標跟蹤算法的前提下,通過對預(yù)設(shè)值m的自減,既提高了包圍框外部特征點優(yōu)先被采集的概率,又不會完全剔除該部分特征點,一定程度提高了算法的魯棒性。

2.2.2并行化計算

RANSAC算法雖然能夠?qū)⑵ヅ潼c劃分為內(nèi)點與外點,但如果迭代次數(shù)過少,無法保證得到的基礎(chǔ)矩陣精度與外點的剔除效果,迭代次數(shù)過多,又會影響系統(tǒng)的實時性。針對該問題的一般做法是先設(shè)置一個最大迭代次數(shù),當某次迭代計算得到一個得分最高的F矩陣時,更新迭代次數(shù),如果達到最大迭代次數(shù)或當前得分最高的解對應(yīng)的內(nèi)點比例超過設(shè)定值,則終止算法,具有一定通用性。內(nèi)點比例的設(shè)定是一個經(jīng)驗值,雖然可以將迭代次數(shù)限制在較低水平,在一定程度上保證較高的內(nèi)點比例,但由于迭代次數(shù)的快速下降,最終可能導致所估計的基礎(chǔ)矩陣F不是全局最優(yōu)解。文獻[12]通過加入預(yù)檢驗過程,過濾掉偏差較大的模型參數(shù),一定程度上提高了算法的效率,但計算過程過于復雜。文獻[13]通過對樣本點集合預(yù)先設(shè)置,使初始樣本集合中內(nèi)點比例增加,從而減少迭代次數(shù)。

針對該問題,采用多線程技術(shù),對基礎(chǔ)矩陣F估計進行并行化計算的改進,達到保證計算效率的同時,提高內(nèi)點比例的效果。具體地,首先獲取系統(tǒng)當前可用進程數(shù)N,開辟n個線程執(zhí)行改進后的RANSAC算法,其中n≤N。所有線程各自維護一個該線程的最優(yōu)基礎(chǔ)矩陣。最后,從所有線程中尋找得分最高的基礎(chǔ)矩陣,并將該基礎(chǔ)矩陣對應(yīng)的所有外點從匹配點集合中剔除。

3 無人機系統(tǒng)軟硬件設(shè)計

3.1 硬件系統(tǒng)搭建

系統(tǒng)硬件組成如圖2所示,板載計算機使用Intel NUC11TNKi5,主控芯片為11代酷睿i5系列,具有四核八線程,同時具有8GB DDR4主存。飛控使用Holybro Pixhawk4 mini,其主控為ARM Cortex M7,并內(nèi)置了BMI055慣性測量元件,輸出頻率可達200 Hz。視覺傳感器使用Intel d455實感相機,分辨率為640×480,幀率設(shè)置為30FPS,RGB彩色相機水平視角86°±3°,垂直視角57°±3°。

圖2 系統(tǒng)硬件組成Fig.2 System hardware composition

3.2 軟件總體設(shè)計

VINS-Fusion[14]是目前先進的基于優(yōu)化的多傳感器融合里程計通用框架,將本文改進后的RANSAC算法應(yīng)用于該系統(tǒng)并作為無人機位姿來源。軟件系統(tǒng)基于ubuntu和ROS框架實現(xiàn),主要包括部分:改進的視覺慣性里程計、飛控通信接口和控制器,軟件系統(tǒng)工作框圖如圖3所示。

圖3 軟件系統(tǒng)工作框圖Fig.3 Software system block diagram

3.3 飛控通信接口設(shè)計

硬件平臺所使用的Holybro Pixhawk4 mini飛控采用mavlink通信協(xié)議,在ROS框架下,需要借助mavros功能包與板載計算機進行通信,通過mavros進行飛控狀態(tài)查詢、控制量輸出等操作變得異常復雜,為了提高調(diào)試與開發(fā)的便捷性,設(shè)計了專門的通信接口,對mavros話題進行了封裝。

無人機狀態(tài)量state包含:連接狀態(tài)connected、加解鎖狀態(tài)arm、模式mode、系統(tǒng)啟動時間start_time、當前位置pos、當前速度vel、當前姿態(tài)q、歐拉角rpy、三軸角速度angular_v和三軸加速度linear_a。其中當前姿態(tài)q與歐拉角rpy分別滿足式(7)與式(8):

q=(w,x,y,z)

(7)

(8)

本方案利用不同mavros消息將無人機控制器封裝成接口形式,直接調(diào)用相應(yīng)接口即可實現(xiàn)不同控制方式,控制系統(tǒng)采用狀態(tài)模式設(shè)計,控制系統(tǒng)狀態(tài)轉(zhuǎn)移圖如圖4所示。

圖4 控制系統(tǒng)狀態(tài)轉(zhuǎn)移圖Fig.4 Control system state transition diagram

3.4 仿真系統(tǒng)

為高效便捷的驗證算法有效性,選取gazebo物理仿真平臺作為本方案的仿真軟件,該平臺能夠提供高保真度的物理模擬。將雙目相機模型搭載于四旋翼無人機,該相機具有90°水平視角,使用開源協(xié)議PX4作為飛控軟件,其內(nèi)部的慣性元件數(shù)據(jù)及無人機搭載的相機圖像,可作為視覺慣性里程計的輸入數(shù)據(jù),仿真框架示意圖如圖5所示。

為模擬現(xiàn)實世界的復雜環(huán)境,通過樹木、車輛等模型構(gòu)建了仿真環(huán)境如圖6所示。

4 實驗驗證與結(jié)果分析

4.1 改進RANSAC算法實驗

為驗證改進RANSAC算法在實際場景中的效果,本文進行如下實驗,其中所涉及硬件使用圖2所示平臺。如圖7為南京理工大學校園內(nèi)采集的6組實驗圖像,其中每組圖片由兩幅相機在不同視角下對同一場景拍攝的圖像組成。實驗時直接使用無人機搭載的d455相機拍攝,每幅圖像的分辨率為640×480。特征點使用opencv庫的ORB[15]提取器進行提取,描述子分別使用256位的BRIEF描述子[16]和512位的BAD描述子[17],6組圖像使用傳統(tǒng)RANSAC與改進后算法進行內(nèi)點比例的比較,置信度設(shè)置為0.99,其他參數(shù)均一致,結(jié)果如圖8、圖9所示。為驗證改進后算法的時間效率,對6組圖像在256位的BRIEF描述子下2種算法分別進行100次測試,并計算平均值,得到結(jié)果如圖10所示。

圖7 實驗圖像Fig.7 Experimental image

圖8 采用BRIEF描述子的內(nèi)點比例Fig.8 Inlier scale using the BRIEF descriptor

圖9 采用BAD描述子的內(nèi)點比例Fig.9 Inlier scale using the BAD descriptor

圖10 運行時間對比Fig.10 Running time comparison

分析實驗結(jié)果可知,改進的RANSAC算法在運行時間方面與傳統(tǒng)RANSAC算法基本一致,滿足無人機應(yīng)用中實時性的要求。算法理論上可以獲取更多組解,從而提高基礎(chǔ)矩陣的估計精度,從實驗上看,改進后的算法在不同特征點數(shù)目上的內(nèi)點比例均高于傳統(tǒng)算法,因此能夠更好剔除外點。

4.2 數(shù)據(jù)集里程計測試

本文使用EuRoC MAV真實場景的視覺慣性數(shù)據(jù)集[18]評估改進后的算法,數(shù)據(jù)集使用一個小型無人機采集,其中包括雙目圖像,頻率為200 Hz同步的慣性測量數(shù)據(jù),真實狀態(tài)使用vicon運動捕捉系統(tǒng)和Leica MS50全站儀提供。在本實驗中,使用MH_01和MH_02兩個數(shù)據(jù)集評估算法對VINS-Fusion的改進效果,通過evo[19]評估工具對程序運行結(jié)果進行分析,軌跡誤差圖如圖11、圖12所示。與未改進的VINS-Fusion在絕對誤差估計下得到的均方根誤差(root-mean-square-error,RMSE)進行對比如表1所示。

圖11 MH_01軌跡誤差Fig.11 MH_01 trajectory error

圖12 MH_02軌跡誤差Fig.12 MH_02 trajectory error

表1 RMSE軌跡估計誤差參數(shù)Table 1 RMSE trajectory estimation error parameters

最后,使用無人機進行飛行實驗如圖13所示,實驗時Intel d455相機雙目圖像幀率設(shè)置為30 FPS,滿足實時性要求。

圖13 飛行試驗Fig.13 Flight experiment

5 結(jié)論

1) 改進的RANSAC算法,在計算基礎(chǔ)矩陣時平均內(nèi)點比例提高1.8%。

2) 將改進的算法嵌入VINS-Fusion,在MH_01序列上的實驗,絕對軌跡誤差的均方根誤差降低了0.001 m,在MH_02序列上的實驗,絕對軌跡誤差的均方根誤差降低了0.019 m。

3) 基于Linux和ROS系統(tǒng)編寫無人機通信接口,簡化了無人機飛控與板載計算機的通信方式,實現(xiàn)上層規(guī)劃模塊、控制模塊和底層飛控的解耦。

4) 算法最終部署到所搭建的無人機平臺,達到了30 FPS的實時效果,滿足無人機對實時性的要求。