基于灰色模型和改進粒子濾波的無人機視覺/INS導航算法

黃衛華，何佳樂，陳 陽，章 政，郭慶瑞

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

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

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

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

1 無人機位姿估計

1.1 坐標系定義及算法設計

機體坐標系為右手正交坐標系，導航坐標系為“東北天”坐標系，無人機導航系統的坐標系對應關系如圖1所示。

圖1 無人機坐標系對應關系圖Fig.1 Correspondence diagram of UAV coordinate system

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

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

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

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

本文根據視覺/INS系統的特點，設計了一種基于改進粒子濾波算法的視覺/INS組合導航系統，系統算法框圖如圖2所示。

圖2 視覺/INS組合導航系統算法框圖Fig.2 Algorithm block diagram of vision/INS integrated navigation system

1.2 基于視覺的無人機位姿解算

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

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

(1)世界坐標系中坐標pw（xw，yw，zw）轉換為相機坐標系的坐標pc（xc，yc，zc）：

即：

其中，R、T分別為無人機運動過程中旋轉矩陣和平移矩陣。

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

化為矩陣形式：

其中，f為機載攝像頭的焦距。

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

化為矩陣形式：

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

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

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

2 基于灰色模型的視覺觀測數據分析及處理

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

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

圖3 滑動窗口獲取預測數據Fig.3 Sliding window prediction data acquisition

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

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

對Z（0）進行一次累加計算得到新的數據序列Z（1）：

其中，

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

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

其中，

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

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

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

3 基于改進粒子濾波算法的無人機位姿估計

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

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

3.1 無人機狀態方程和觀測方程

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

其中，無人機的位置為bS，速度為bS˙，加速度為a，INS數據更新周期為dt，則在k時刻無人機的狀態離散化為：

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

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

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

狀態空間模型可表示為：

其中，Xk表示在k時刻無人機的狀態，Φ表示系統的狀態轉移矩陣，Γ表示系統的隨機過程噪聲矩陣。

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

其中H表示系統的狀態觀測矩陣，Rk表示系統的隨機觀測噪聲矩陣。由于系統狀態量為全觀測，故狀態觀測矩陣H和隨機觀測噪聲矩陣Rk分別為：

矩陣Rk內參數τ1、τ2需根據實際情況整定。

3.2 基于螢火蟲算法的改進粒子濾波算法設計

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

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

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

更新過程利用最新的測量值對預測步得到的先驗概率密度進行修正，從而得到更精準的后驗概率密度。

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

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

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

本文將當前時刻即k時刻權重最大的粒子所代表的數據值作為全局最優值，與所有粒子進行信息交互來計算其他粒子的位移更新。k時刻粒子i位移更新公式如下：

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

通過最新的粒子權重和位置信息進一步得到k時刻無人機系統的位姿最優估計為：

3.3 位姿估計算法步驟

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

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

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

步驟4：結合預測值和觀測值更新粒子的權重；

步驟5：計算當前時刻所有粒子的亮度和吸引度，根據當前時刻的N個粒子的亮度更新此時全局最優值

步驟7：對步驟6得到的粒子權重進行歸一化處理，將歸一化后的粒子權重表示為；

步驟8：k時刻粒子更新完成后無人機系統狀態輸出；

步驟9：返回步驟2，將更新后的粒子信息代入系統狀態方程繼續進行下一步計算。

4 實驗及結果分析

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

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

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

圖5 無人機硬件系統結構圖Fig.5 UAV hardware system structurediagram

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

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

（1）灰色模型預測精度實驗

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

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

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

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

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

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

在無人機軌跡飛行實驗中，以合作地標中心為原點（0,0,0），通過遙控器控制無人機飛行到A（40,0,60）并處于懸停狀態，每隔5s依次控制無人機飛到B(40,40,80)、C(-40,40,60)、D(-40,40,40)、E(40,-40,50)，最終回到A點（坐標單位為cm）。用MATLAB將采集到的數據進行作圖。無人機飛行實驗三維運動軌跡如圖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的實驗結果可知，三種算法均能較好實現無人機組合導航，本文所設計的無人機組合導航算法在靜態條件和動態條件下得到的位姿估計狀態相較于常規粒子濾波算法和傳統擴展KF算法更接近理想狀態，實時數據曲線更加平滑。

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

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

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

5 結 論

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