基于灰色模型和改進(jìn)粒子濾波的無人機(jī)視覺/INS導(dǎo)航算法

黃衛(wèi)華，何佳樂，陳 陽，章 政，郭慶瑞

（武漢科技大學(xué)信息科學(xué)與工程學(xué)院，武漢430081）

目前，無人機(jī)組合導(dǎo)航系統(tǒng)越來越多地應(yīng)用于軍事、民用、農(nóng)業(yè)等各個領(lǐng)域[1]。常用的導(dǎo)航方法主要有全球定位導(dǎo)航系統(tǒng)(Global Positioning System, GPS)、慣性導(dǎo)航系統(tǒng)(Inertial Navigation System, INS)以及基于機(jī)器視覺的導(dǎo)航技術(shù)[2-6]等。相較于前兩者而言，基于機(jī)器視覺的無人機(jī)導(dǎo)航技術(shù)通過圖像處理和位姿解算的方式獲取無人機(jī)的實時位姿參數(shù)及其相對于著陸平臺的位姿信息，具有獲取信息全面、精度高等特點。目前，將機(jī)器視覺與不依賴外部信息的INS構(gòu)成視覺/INS組合導(dǎo)航系統(tǒng)已成為無人機(jī)自主著陸導(dǎo)航的研究熱點。

在無人機(jī)實際飛行過程中，機(jī)體處于大機(jī)動狀態(tài)，易受到機(jī)身抖動、環(huán)境噪聲、氣流沖擊等因素的影響，系統(tǒng)非線性特征較為明顯。粒子濾波(Particle Filter, PF)算法具有簡單、易于實現(xiàn)等特點[7]，它為分析非線性動態(tài)系統(tǒng)提供了一種有效的解決方法。文獻(xiàn)[8]利用粒子濾波實現(xiàn)慣導(dǎo)與超寬帶無線載波通信技術(shù)(Ultra Wide Band, UWB)定位信息的融合，通過UWB的高定位精度修正慣導(dǎo)定位的累積誤差。文獻(xiàn)[9]提出基于粒子濾波的自適應(yīng)蒙特卡洛優(yōu)化定位算法，提高了機(jī)器人的定位精度。文獻(xiàn)[10]提出一種約束無跡粒子濾波算法，對狀態(tài)估計值進(jìn)行約束，并修正誤差大的估計值，由此提高了狀態(tài)量的估計精度。上述研究中，主要是直接融合視覺解算信息和INS數(shù)據(jù)，通過粒子濾波計算出位置、姿態(tài)等狀態(tài)信息。然而，基于視覺/INS無人機(jī)自主導(dǎo)航系統(tǒng)在飛行過程中，受氣候條件、光線強(qiáng)度及地面參照物可能會被遮擋等不確定因素的影響，機(jī)載攝像頭所采集的視覺信息會出現(xiàn)短暫缺失且不確定性增加，盡管可以利用INS更新位置信息，但此時機(jī)體處于大機(jī)動狀態(tài)，系統(tǒng)采集數(shù)據(jù)噪聲較大且非線性特征明顯。因此，直接融合視覺解算信息和INS數(shù)據(jù)會影響姿態(tài)解算的精度，易導(dǎo)致系統(tǒng)誤差大、狀態(tài)發(fā)散等問題。此外，粒子濾波存在粒子貧化、狀態(tài)變量維度高以及重采樣過程計算量大、耗時較長等問題[10]。

鑒于上述分析，為了提高視覺/INS組合導(dǎo)航系統(tǒng)的位姿估計精度，本文提出了一種基于灰色模型和改進(jìn)粒子濾波的視覺/INS組合導(dǎo)航方法。首先，利用灰色預(yù)測理論建立基于視覺的位姿解算數(shù)據(jù)數(shù)學(xué)模型，當(dāng)機(jī)載攝像頭受到外界因素干擾時，用灰色模型預(yù)測數(shù)據(jù)代替其原始位姿解算數(shù)據(jù)，消除解算數(shù)據(jù)的誤差以及其帶來的不利影響；其次，將螢火蟲算法引入到粒子濾波算法中，提高粒子濾波的精度并減少計算耗時；然后，采用改進(jìn)的粒子濾波算法將基于視覺的位姿解算數(shù)據(jù)和INS數(shù)據(jù)進(jìn)行融合，得到無人機(jī)實時的高精度位姿估計信息，避免直接融合視覺解算信息和INS數(shù)據(jù)產(chǎn)生額外誤差；最后，通過搭建的四旋翼無人機(jī)實驗平臺驗證了本文算法的可行性和有效性。

1 無人機(jī)位姿估計

1.1 坐標(biāo)系定義及算法設(shè)計

機(jī)體坐標(biāo)系為右手正交坐標(biāo)系，導(dǎo)航坐標(biāo)系為“東北天”坐標(biāo)系，無人機(jī)導(dǎo)航系統(tǒng)的坐標(biāo)系對應(yīng)關(guān)系如圖1所示。

圖1 無人機(jī)坐標(biāo)系對應(yīng)關(guān)系圖Fig.1 Correspondence diagram of UAV coordinate system

世界坐標(biāo)系Ow-XwYwZw：由于相機(jī)坐標(biāo)系不能給出空間點的位置信息，所以需要一個基準(zhǔn)坐標(biāo)系來描述空間點和攝像機(jī)的關(guān)系，一般稱此坐標(biāo)系為世界坐標(biāo)系。

本文中世界坐標(biāo)系與著陸坐標(biāo)系固連，原點為著陸目標(biāo)幾何中心，Xw、Yw位于著陸地標(biāo)平面內(nèi)，Zw垂直于合作地標(biāo)平面向上。

相機(jī)坐標(biāo)系Oc-XcYcZc：以相機(jī)的聚焦中心為原點Oc，以光軸為Z軸建立的三維直角坐標(biāo)系。X軸與Y軸與圖像平面的X，Y軸平行，Z軸即相機(jī)光軸與圖形平面垂直。

圖像平面坐標(biāo)系Op-XpYp：相機(jī)光軸與圖像平面的交點，即為圖像平面坐標(biāo)系的原點，圖像平面坐標(biāo)系為二維直角坐標(biāo)系。

本文根據(jù)視覺/INS系統(tǒng)的特點，設(shè)計了一種基于改進(jìn)粒子濾波算法的視覺/INS組合導(dǎo)航系統(tǒng)，系統(tǒng)算法框圖如圖2所示。

圖2 視覺/INS組合導(dǎo)航系統(tǒng)算法框圖Fig.2 Algorithm block diagram of vision/INS integrated navigation system

1.2 基于視覺的無人機(jī)位姿解算

設(shè)pw（xw，yw，zw）是三維空間內(nèi)某一點p在世界坐標(biāo)系中的坐標(biāo)，pc（xc，yc，zc）是該點在相機(jī)坐標(biāo)系中的坐標(biāo)，p（x，y）是該點在圖像平面坐標(biāo)系中的物理坐標(biāo)，p（u，v）是該點圖像平面坐標(biāo)系中的像素坐標(biāo)。

將三維空間點p在世界坐標(biāo)系中的坐標(biāo)值pw（xw，yw，zw）轉(zhuǎn)換為圖像平面坐標(biāo)系中的像素坐標(biāo)值p（u，v）的過程如下：

(1)世界坐標(biāo)系中坐標(biāo)pw（xw，yw，zw）轉(zhuǎn)換為相機(jī)坐標(biāo)系的坐標(biāo)pc（xc，yc，zc）：

即：

其中，R、T分別為無人機(jī)運動過程中旋轉(zhuǎn)矩陣和平移矩陣。

(2)相機(jī)坐標(biāo)系中坐標(biāo)pc（xc，yc，zc）在針孔模型進(jìn)行投影后得到圖像平面坐標(biāo)系中物理坐標(biāo)p（x，y）。根據(jù)三角形相似原理：

化為矩陣形式：

其中，f為機(jī)載攝像頭的焦距。

(3)圖像平面坐標(biāo)系中物理坐標(biāo)p（x，y）轉(zhuǎn)換為圖像平面坐標(biāo)系中的像素坐標(biāo)p（u，v）：

化為矩陣形式：

其中，將圖像中心點（u0，v0）作為基準(zhǔn)點，dx為機(jī)載攝像頭所采集圖像在水平方向(x方向)上相鄰兩像素之間的有效距離，dy為機(jī)載攝像頭所采集圖像在垂直方向(y方向)上相鄰兩像素之間的有效距離。

綜合式(1)-(6)可得到式(7)，將世界坐標(biāo)系中特征點轉(zhuǎn)換到圖像平面坐標(biāo)系中：

其中，相機(jī)的內(nèi)參焦距f和外參旋轉(zhuǎn)R、平移矩陣T均可通過張正友標(biāo)定法獲取。根據(jù)式(7)，由世界坐標(biāo)系三維空間內(nèi)的合作地標(biāo)的坐標(biāo)pw（xw，yw，zw）可以推算出機(jī)載攝像頭采集圖像的像素坐標(biāo)p（u，v），從而獲取無人機(jī)相對地標(biāo)的位置與速度信息。

2 基于灰色模型的視覺觀測數(shù)據(jù)分析及處理

在無人機(jī)的實際飛行過程中，由于受光線強(qiáng)度不均勻、障礙物遮擋等外部環(huán)境因素的干擾，當(dāng)機(jī)載攝像頭采集視覺數(shù)據(jù)受到嚴(yán)重干擾時，基于視覺的無人機(jī)位姿解算數(shù)據(jù)會出現(xiàn)突變、抖動現(xiàn)象，即相鄰時刻的數(shù)據(jù)突變過大，造成無人機(jī)飛行姿態(tài)不穩(wěn)定的問題。考慮到灰色模型具有無需大量數(shù)據(jù)樣本，運算過程簡單等優(yōu)點，采用灰色預(yù)測理論對基于視覺的位姿解算數(shù)據(jù)建立數(shù)學(xué)模型，預(yù)測下一時刻的數(shù)據(jù)并在原始數(shù)據(jù)突變時進(jìn)行替換，從而引導(dǎo)無人機(jī)繼續(xù)穩(wěn)定飛行。

以無人機(jī)在飛行過程中的X軸方向上位移為例，滑動窗口獲取預(yù)測數(shù)據(jù)過程如圖3所示，設(shè)定滑動窗口大小為n,根據(jù)窗口內(nèi)的n個歷史數(shù)據(jù)，預(yù)測下一時刻的視覺解算數(shù)據(jù)。

圖3 滑動窗口獲取預(yù)測數(shù)據(jù)Fig.3 Sliding window prediction data acquisition

設(shè)無人機(jī)飛行過程中的視覺解算數(shù)據(jù)序列{Zk-n-1,Zk-n,...Zk-1,Zk}分別為k-n-1，k-n...k-1，k時刻的無人機(jī)X軸方向上位移的視覺解算數(shù)據(jù)。

假設(shè)k時刻滑動窗口內(nèi)n個視覺解算數(shù)據(jù)組成數(shù)據(jù)樣本序列Z（0）：

對Z（0）進(jìn)行一次累加計算得到新的數(shù)據(jù)序列Z（1）：

其中，

并建立GM(1,1)灰色模型一階微分方程，如式(11)所示：

設(shè)u為待求參數(shù)矩陣，利用最小二乘法求取參數(shù)a和b：

其中，

對式(11)微分方程求解得到建立模型的時間響應(yīng)函數(shù)如式(14)所示：

在初始條件Z（1）（1） =Z（0）（1）下，對式(14)離散化后，可得到一次累加后k+1時刻的預(yù)測值：

采用灰色模型預(yù)測X軸方向上基于視覺解算的位移量，如果k+1時刻機(jī)載攝像頭采集數(shù)據(jù)受到嚴(yán)重外部干擾，將預(yù)測量（k+ 1）替換原始數(shù)據(jù)Zk+1，從而反映無人機(jī)真實位置信息和飛行姿態(tài)。同理，可以得到另外兩個軸的位置預(yù)測數(shù)據(jù)和速度預(yù)測數(shù)據(jù)。

3 基于改進(jìn)粒子濾波算法的無人機(jī)位姿估計

在基于視覺/INS的無人機(jī)系統(tǒng)中，原始數(shù)據(jù)存在零偏及其時間累積誤差變大的問題，同時，由于粒子濾波算法中存在著重采樣過程去除了權(quán)重較少的粒子造成粒子貧化導(dǎo)致濾波精度低、粒子重采樣過程耗時較長的問題，影響了無人機(jī)位姿估計的實時性和精確性。

為了提高粒子濾波的精度并減少算法計算耗時，本文引入螢火蟲算法對粒子濾波的重采樣過程進(jìn)行改進(jìn)，螢火蟲算法是一種尋優(yōu)算法，利用螢火蟲算法對粒子濾波重采樣過程進(jìn)行改進(jìn)，增加了粒子迭代更新的速率，減少了濾波計算耗時。最終采用改進(jìn)的粒子濾波算法將基于視覺的位姿解算數(shù)據(jù)和INS數(shù)據(jù)進(jìn)行融合，保證了無人機(jī)飛行位姿的精確性和實時性。

3.1 無人機(jī)狀態(tài)方程和觀測方程

無人機(jī)飛行過程中需要獲取Xw、Yw、Zw三個方向上分別對應(yīng)的位置和速度。本文選取無人機(jī)三軸的位移和速度作為狀態(tài)變量：

其中，無人機(jī)的位置為bS，速度為bS˙，加速度為a，INS數(shù)據(jù)更新周期為dt，則在k時刻無人機(jī)的狀態(tài)離散化為：

其中，wb、wa分別為無人機(jī)位置和速度的噪聲，用于描述未知因素帶來的影響，加速度a（k）是由機(jī)動加速度u（k）和隨機(jī)加速度w（k）兩部分合成的。即

式(20)中，u（k）是由加速度計測量的無人機(jī)機(jī)體的機(jī)動加速度，而w（k）由空氣阻力等外部隨機(jī)因素決定。

由式(18)-(20)可得飛行過程中無人機(jī)系統(tǒng)的狀態(tài)空間模型為：

狀態(tài)空間模型可表示為：

其中，Xk表示在k時刻無人機(jī)的狀態(tài)，Φ表示系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣，Γ表示系統(tǒng)的隨機(jī)過程噪聲矩陣。

取基于視覺得到的位置、速度值作為無人機(jī)系統(tǒng)的觀測量Zk，系統(tǒng)觀測方程如式(25)所示：

其中H表示系統(tǒng)的狀態(tài)觀測矩陣，Rk表示系統(tǒng)的隨機(jī)觀測噪聲矩陣。由于系統(tǒng)狀態(tài)量為全觀測，故狀態(tài)觀測矩陣H和隨機(jī)觀測噪聲矩陣Rk分別為：

矩陣Rk內(nèi)參數(shù)τ1、τ2需根據(jù)實際情況整定。

3.2 基于螢火蟲算法的改進(jìn)粒子濾波算法設(shè)計

基于粒子濾波的無人機(jī)組合導(dǎo)航系統(tǒng)位姿估計就是根據(jù)k時刻以前的一系列視覺解算數(shù)據(jù)Z1:k，求得位姿的后驗概率密度函數(shù)p（Xk|Z1:k），通過最小均方誤差估計準(zhǔn)則計算得到k時刻Xw、wY、Zw三軸方向上的位姿最優(yōu)估計。

無人機(jī)位姿的后驗概率密度函數(shù)p（Xk|Z1:k）的計算過程分為預(yù)測和更新兩步：

預(yù)測過程利用無人機(jī)系統(tǒng)的狀態(tài)方程（即式(22)）計算出下一時刻的先驗概率密度：

更新過程利用最新的測量值對預(yù)測步得到的先驗概率密度進(jìn)行修正，從而得到更精準(zhǔn)的后驗概率密度。

對于四旋翼無人機(jī)這種非線性的系統(tǒng)模型，式(28)-(30)中的積分運算困難，因此引入蒙特卡洛方法進(jìn)行采樣計算，并通過重要性概率密度生成采樣粒子i，利用粒子的加權(quán)和來逼近后驗概率密度，k時刻粒子i的權(quán)重用來表示：

在粒子濾波算法中粒子的權(quán)重越大，越接近實際值；在螢火蟲算法中，越接近最優(yōu)值時，螢火蟲的亮度I越高。結(jié)合兩者的特點，用個體粒子的權(quán)重表示螢火蟲算法中的個體亮度：

將螢火蟲算法的吸引度函數(shù)引入到粒子濾波重采樣過程中，設(shè)計了粒子的吸引度函數(shù)如式(33)所示。粒子間距離遠(yuǎn)時低權(quán)重粒子快速向高權(quán)重粒子快速移動，距離近時高權(quán)重粒子吸引度降低，粒子間的移動速度逐漸減小，避免出現(xiàn)局部極值導(dǎo)致精度下降的問題：

本文將當(dāng)前時刻即k時刻權(quán)重最大的粒子所代表的數(shù)據(jù)值作為全局最優(yōu)值，與所有粒子進(jìn)行信息交互來計算其他粒子的位移更新。k時刻粒子i位移更新公式如下：

本文改進(jìn)的粒子濾波算法通過不斷更新粒子權(quán)重和位置來逼近無人機(jī)系統(tǒng)的后驗概率分布。位移更新后粒子位置、權(quán)重信息均發(fā)生了改變，對粒子的權(quán)重進(jìn)行更新和歸一化處理得到：

通過最新的粒子權(quán)重和位置信息進(jìn)一步得到k時刻無人機(jī)系統(tǒng)的位姿最優(yōu)估計為：

3.3 位姿估計算法步驟

本文將螢火蟲算法引入到粒子濾波算法的重采樣過程中，歸納出本文所設(shè)計的位姿估計算法實現(xiàn)步驟如下：

步驟2：將所有粒子代入系統(tǒng)狀態(tài)方程（即式(22)）進(jìn)行計算，預(yù)測k時刻系統(tǒng)狀態(tài)的先驗概率密度p（Xk|Z1:k-1）；

步驟3：通過系統(tǒng)觀測方程（即式(25)）得到k時刻觀測數(shù)據(jù)Zk修正步驟2計算的系統(tǒng)狀態(tài)的先驗概率密度，得到系統(tǒng)的后驗概率密度p（Xk|Z1:k-1）；

步驟4：結(jié)合預(yù)測值和觀測值更新粒子的權(quán)重；

步驟5：計算當(dāng)前時刻所有粒子的亮度和吸引度，根據(jù)當(dāng)前時刻的N個粒子的亮度更新此時全局最優(yōu)值

步驟7：對步驟6得到的粒子權(quán)重進(jìn)行歸一化處理，將歸一化后的粒子權(quán)重表示為；

步驟8：k時刻粒子更新完成后無人機(jī)系統(tǒng)狀態(tài)輸出；

步驟9：返回步驟2，將更新后的粒子信息代入系統(tǒng)狀態(tài)方程繼續(xù)進(jìn)行下一步計算。

4 實驗及結(jié)果分析

本文自主搭建了如圖4所示的四旋翼無人機(jī)實驗平臺，包括飛控、2208無刷電機(jī)、螺旋槳葉、3S航模電池、電子調(diào)速器、機(jī)架、PPM接收機(jī)、遙控器、攝像頭和視覺解算模塊，四旋翼無人機(jī)整體機(jī)身重量為0.9kg，機(jī)架對角線軸距為330mm，機(jī)身高度為185mm。

圖4 四旋翼無人機(jī)實驗平臺Fig.4 Quadrotor UAV experimental platform

四旋翼無人機(jī)的硬件系統(tǒng)設(shè)計圖如圖5所示，其中主控制器采用32位微處理器STM32F103RCT6，電子調(diào)速器采用好盈樂天20A電調(diào)，姿態(tài)傳感器為慣性測量單元MPU6050，磁力計采用IST8310，氣壓計采用SPL06-001，視覺解算模塊由OV5647攝像頭模組和安裝了Raspbian操作系統(tǒng)、OpenCV3.4.1開發(fā)庫的“樹莓派”微型卡片式計算機(jī)組成。

圖5 無人機(jī)硬件系統(tǒng)結(jié)構(gòu)圖Fig.5 UAV hardware system structurediagram

為了測試本文設(shè)計算法的效果，根據(jù)實際調(diào)試設(shè)置觀測噪聲矩陣Rk中的參數(shù)τ1、τ2分別為0.075和0.6，灰色預(yù)測過程滑動窗口大小n設(shè)為10，粒子濾波過程中取粒子數(shù)N為50，粒子距離閾值h取0.37，螢火蟲算法中步長因子α取0.5。

本文共設(shè)置了兩組對比實驗：灰色模型預(yù)測精度實驗以及三維軌跡飛行對比實驗。

（1）灰色模型預(yù)測精度實驗

控制無人機(jī)在合作地標(biāo)上空保持懸停狀態(tài)，高度保持在120cm，在同一實驗條件下，通過人工遮擋合作地標(biāo)給無人機(jī)視覺識別造成干擾，以此來實現(xiàn)外部環(huán)境變化這一條件。同時采集灰色預(yù)測的位置信息以及通過PX4Flow高精度光流傳感器得到的實際位置信息，從而進(jìn)行對比實驗，驗證灰色預(yù)測算法抗外界環(huán)境干擾的有效性。圖6給出了X軸方向上預(yù)測位置和實際位置的對比效果。

圖6 預(yù)測位置與實際位置對比Fig.6 Estimated position compared with actual position

圖7及表1對比了外部環(huán)境變化后X軸方向上位置誤差。從圖中可以看出，有灰色預(yù)測作為補(bǔ)充的數(shù)據(jù)融合算法可以讓位置誤差穩(wěn)定在1.2cm以內(nèi)，沒有灰色預(yù)測作為補(bǔ)充的數(shù)據(jù)融合算法得到的位置誤差較大，誤差極大值達(dá)到了3.3cm，因此有灰色預(yù)測作為補(bǔ)充的數(shù)據(jù)融合算法具有更小的平均誤差和誤差極值，對于位置誤差的控制效果提升了70%。

圖7 位置誤差對比圖Fig.7 Chart ofposition error comparison

表1 位置誤差對比Tab.1Positionerror comparison

（2）三維軌跡飛行對比實驗

在無人機(jī)軌跡飛行實驗中，以合作地標(biāo)中心為原點（0,0,0），通過遙控器控制無人機(jī)飛行到A（40,0,60）并處于懸停狀態(tài)，每隔5s依次控制無人機(jī)飛到B(40,40,80)、C(-40,40,60)、D(-40,40,40)、E(40,-40,50)，最終回到A點（坐標(biāo)單位為cm）。用MATLAB將采集到的數(shù)據(jù)進(jìn)行作圖。無人機(jī)飛行實驗三維運動軌跡如圖8所示。

圖8 三維運動軌跡圖Fig.8 Three-dimensional motion trajectory diagram

X、Y、Z三個方向的實際運動軌跡分別如圖9、圖10和圖11所示。

圖9 X軸位移軌跡圖Fig.9 X-axisdisplacement trajectory di agram

圖10 Y軸位移軌跡圖Fig.10 Y-axisdisplacement trajectory diagram

圖11 Z軸位移軌跡圖Fig.11 Z-axisdisplacement trajectory diagram

由圖9-11的實驗結(jié)果可知，三種算法均能較好實現(xiàn)無人機(jī)組合導(dǎo)航，本文所設(shè)計的無人機(jī)組合導(dǎo)航算法在靜態(tài)條件和動態(tài)條件下得到的位姿估計狀態(tài)相較于常規(guī)粒子濾波算法和傳統(tǒng)擴(kuò)展KF算法更接近理想狀態(tài)，實時數(shù)據(jù)曲線更加平滑。

由圖12、表2的無人機(jī)飛行實驗誤差結(jié)果可知，本文算法在X、Y、Z三個方向上的相對誤差在靜態(tài)時可以保持在0.5 cm內(nèi)，動態(tài)時也能控制在2.3 cm之內(nèi)，相較于傳統(tǒng)粒子濾波算法，算法平均計算耗時減少了0.06 s，相較于傳統(tǒng)擴(kuò)展KF濾波算法，誤差減小了39%，保證了無人機(jī)飛行過程中位姿估計的精確性和飛行狀態(tài)的穩(wěn)定性。

表2 飛行軌跡融合實驗數(shù)據(jù)分析Tab.2 Data analysis of flight trajectory fusion experiment

圖12 XYZ三軸相對誤差圖Fig.12 XYZ three-axis relative error diagram

5 結(jié) 論

對于視覺/INS旋翼無人機(jī)而言，由于機(jī)載攝像頭采集的視覺信息易受外界環(huán)境因素干擾，影響了視覺/INS組合導(dǎo)航系統(tǒng)的位姿估計精度。鑒于此，首先本文利用灰色預(yù)測理論建立基于視覺的位姿解算數(shù)據(jù)的灰色模型，當(dāng)視覺信息受到外界因素干擾時，用灰色模型預(yù)測數(shù)據(jù)代替其原始位姿解算數(shù)據(jù)，實驗證明了采用灰色預(yù)測作為補(bǔ)充的數(shù)據(jù)融合算法具有著更小的平均誤差和誤差極值；然后引入螢火蟲算法對粒子濾波的重采樣過程進(jìn)行改進(jìn)，采用改進(jìn)粒子濾波算法將基于視覺的位姿解算數(shù)據(jù)和INS數(shù)據(jù)進(jìn)行融合，三維軌跡飛行對比實驗結(jié)果表明本文設(shè)計的組合導(dǎo)航算法將誤差控制在2 cm以內(nèi)，算法平均計算耗時減少了0.06 s，誤差減小了39%，更能夠保證視覺/INS無人機(jī)位姿估計的精確性。