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

( 南京理工大學(xué) 機(jī)械工程學(xué)院, 南京 210094)

0 引言

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

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

1 視覺慣性里程計(jì)

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

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

(1)

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

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

相機(jī)分別從視角1與視角2觀察三維空間中同一點(diǎn)P,在P點(diǎn)未知的情況下,求解視角1到視角2相機(jī)的運(yùn)動(dòng),可以通過對(duì)極幾何約束解決,如圖1所示。

圖1 對(duì)極幾何Fig.1 Epipolar geometry

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

(2)

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

(3)

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

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

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

2 改進(jìn)RANSAC算法剔除外點(diǎn)

2.1 RANSAC算法

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

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

1) 將所有匹配點(diǎn)集S中的特征點(diǎn)進(jìn)行歸一化處理。

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

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

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

2.2 改進(jìn)的RANSAC算法

2.2.1帶優(yōu)先級(jí)的樣本點(diǎn)采集策略

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

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

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

(4)

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

(5)

(6)

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

2.2.2并行化計(jì)算

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

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

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

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

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

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

3.2 軟件總體設(shè)計(jì)

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

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

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

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

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

q=(w,x,y,z)

(7)

(8)

本方案利用不同mavros消息將無人機(jī)控制器封裝成接口形式,直接調(diào)用相應(yīng)接口即可實(shí)現(xiàn)不同控制方式,控制系統(tǒng)采用狀態(tài)模式設(shè)計(jì),控制系統(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)

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

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

4 實(shí)驗(yàn)驗(yàn)證與結(jié)果分析

4.1 改進(jìn)RANSAC算法實(shí)驗(yàn)

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

圖7 實(shí)驗(yàn)圖像Fig.7 Experimental image

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

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

圖10 運(yùn)行時(shí)間對(duì)比Fig.10 Running time comparison

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

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

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

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

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

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

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

圖13 飛行試驗(yàn)Fig.13 Flight experiment

5 結(jié)論

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

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

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

4) 算法最終部署到所搭建的無人機(jī)平臺(tái),達(dá)到了30 FPS的實(shí)時(shí)效果,滿足無人機(jī)對(duì)實(shí)時(shí)性的要求。