洪 亮,章 政,2,3*,李亞貴,李宇峰,張艦棟

(1.武漢科技大學(xué)信息科學(xué)與工程學(xué)院,武漢 430081;2.武漢科技大學(xué)機器人與智能系統(tǒng)研究院,武漢 430081;3.武漢科技大學(xué)冶金自動化與檢測技術(shù)教育部工程研究中心,武漢 430081)

自主著陸是無人機自主飛行系統(tǒng)中的重要環(huán)節(jié),通過機載導(dǎo)航設(shè)備獲取實時、精確的位姿信息是實現(xiàn)無人機安全、有效著陸的前提和基礎(chǔ)[1]。相比于GPS導(dǎo)航方式[2],基于視覺的無人機著陸導(dǎo)航方式可全覆蓋、獨立獲取著陸點周圍高精度的三維空間反饋信息,對環(huán)境要求較低,具有抗干擾能力強、精度高和功耗低等特點[3]。目前,慣性導(dǎo)航系統(tǒng)INS(Inertial Navigation System)與視覺傳感器構(gòu)成INS/視覺組合導(dǎo)航系統(tǒng)已成為無人機自主著陸導(dǎo)航系統(tǒng)設(shè)計的主要趨勢[4-5]。

對于基于INS/視覺的無人機自主著陸導(dǎo)航系統(tǒng),常采用卡爾曼濾波KF(Kalman Filter)實現(xiàn)INS與視覺傳感器的數(shù)據(jù)融合。文獻[6]由視覺位姿估計系統(tǒng)得出飛行器的位姿信息,通過卡爾曼濾波實現(xiàn)位置、姿態(tài)匹配;文獻[7]構(gòu)建了一種非線性濾波器實現(xiàn)了慣導(dǎo)誤差估計,在提高算法效率的同時有效地避免了無跡卡爾曼濾波中由于矩陣開方運算導(dǎo)致的濾波失效問題;文獻[8]提出了一種基于圖像輔助慣導(dǎo)的擴展卡爾曼濾波方法,使得基于視覺輔助慣導(dǎo)的組合導(dǎo)航系統(tǒng)逐漸成為一種新的導(dǎo)航模式;文獻[9]通過多估計中心的加權(quán)并將擴展卡爾曼濾波與單目視覺位姿估計相結(jié)合,提高了無人機的位姿估計精度。上述研究主要是利用視覺傳感器的觀測數(shù)據(jù)修正INS的估計誤差,再結(jié)合卡爾曼濾波實現(xiàn)無人機的位姿估計,由此避免INS定位誤差隨時間累積的缺陷。

然而,在無人機實際著陸過程中,機載攝像頭獲取著陸點附近圖像信息是一個動態(tài)測量過程,其視覺觀測數(shù)據(jù)會隨著時間和空間而變化且包含了飛行環(huán)境中的各種干擾信息。因此,通過透視投影模型解算出的視覺觀測數(shù)據(jù)存在觀測噪聲與解算誤差[10],直接融合INS和視覺的數(shù)據(jù)易導(dǎo)致系統(tǒng)額外誤差[11]、位姿狀態(tài)估計不收斂[12-13]等問題。此外,由于受到機載傳感器低主頻、無硬件浮點運算等限制,視覺觀測數(shù)據(jù)的更新周期長,從而影響無人機位姿估計的實時性。

基于上述分析,考慮到模糊邏輯[14]可有效處理機載攝像頭獲取觀測數(shù)據(jù)的時變性和不確定性,本文設(shè)計了一種基于模糊預(yù)測的INS/視覺無人機自主著陸導(dǎo)航算法。首先,采用模糊數(shù)描述機載攝像頭解算所得的觀測數(shù)據(jù),在INS數(shù)據(jù)更新的同時,構(gòu)造基于歷史觀測數(shù)據(jù)的模糊預(yù)測模型,由此預(yù)測機載攝像頭當前時刻的觀測數(shù)據(jù);然后,設(shè)計基于狀態(tài)量同步更新的卡爾曼濾波器,對INS產(chǎn)生的誤差進行修正,得到無人機的位姿估計,從而避免直接融合INS與視覺數(shù)據(jù)導(dǎo)致系統(tǒng)額外誤差的問題,提高了無人機的位姿估計精度;最后,通過基于四旋翼無人機平臺的實測實驗驗證了本文算法的有效性和實時性。

1 基于INS/視覺的無人機自主著陸導(dǎo)航系統(tǒng)設(shè)計

1.1 自主著陸導(dǎo)航系統(tǒng)的坐標系定義

本文所設(shè)計的INS/視覺無人機自主著陸導(dǎo)航系統(tǒng)主要包括:四旋翼無人機、機載攝像頭和著陸地標,其位姿關(guān)系如圖1所示。

圖1 坐標系對應(yīng)關(guān)系

Oc-XcYcZc為四旋翼無人機和機載攝像頭的坐標系,坐標系原點Oc為無人機的機體重心。X軸和Y軸位于無人機機體平面內(nèi),Y軸指向無人機前進方向,X軸指向無人機左側(cè)方向,Z軸指向無人機上升方向。

Op-XpYp為圖像坐標系,平行于無人機機體平面,Y軸指向無人機前進方向,X軸指向無人機左側(cè)方向。

Ow-XwYwZw為著陸地標坐標系,坐標系原點Ow為著陸地標的幾何中心。X軸和Y軸位于著陸地標平面內(nèi),Y軸指向地理正北方向,X軸指向地理正西方向,Z軸垂直于著陸地標平面向上。

1.2 基于INS/視覺的無人機自主著陸導(dǎo)航算法

針對INS/視覺的無人機自主著陸系統(tǒng)的特點,將模糊預(yù)測用于導(dǎo)航算法中,算法結(jié)構(gòu)圖如圖2所示。

圖2 基于INS/視覺的無人機自主著陸導(dǎo)航算法

首先,對圖像中的著陸地標進行特征點提取,通過視覺解算得到無人機在地標坐標系下的位置觀測數(shù)據(jù)。針對視覺觀測數(shù)據(jù)存在不確定性問題,采用三角模糊處理視覺解算得到的觀測數(shù)據(jù)。然后,針對無人機非線性、強耦合和靜態(tài)不穩(wěn)定的特性,考慮到機載計算機的處理能力有限,在慣導(dǎo)數(shù)據(jù)更新的同時,利用模糊預(yù)測的方法預(yù)測視覺傳感器的位置觀測數(shù)據(jù)。最后,在得到視覺觀測數(shù)據(jù)的一步預(yù)測值后,通過卡爾曼濾波器修正慣導(dǎo)的加速度零偏、速度誤差和位置累計誤差,得到用于無人機位置反饋控制的最優(yōu)估計值,達到無人機自主著陸中對位姿估計實時性的要求。

2 基于模糊預(yù)測的INS/視覺無人機位姿估計算法

2.1 視覺觀測數(shù)據(jù)解算

對機載攝像頭拍攝的圖像進行Harris特征點檢測,提取著陸地標的特征點。根據(jù)透視投影模型和剛體旋轉(zhuǎn)平移的坐標變換原理,結(jié)合圖1所示的坐標系對應(yīng)關(guān)系,著陸坐標系中的特征點(xw,yw,zw)與其對應(yīng)圖像坐標中像素點(u,v)的關(guān)系如下:

(1)

式(1)中zc為特征點(xw,yw,zw)在相機坐標系的坐標,K為相機參數(shù),R,T為無人機相對于地標的旋轉(zhuǎn)矩陣和偏移矩陣,分別為:

R=

(2)

(3)

式(2)中,θ,φ,ψ分別為俯仰角、橫滾角和偏航角,式(3)中,Tz為無人機相對于地標的高度,Tx和Ty為無人機相對地標的水平偏移量。

由于zc和攝像頭參數(shù)K為已知量,因此根據(jù)若干組地標中特征點(xw,yw,zw)在圖像中的像素(u,v),就可以求得旋轉(zhuǎn)矩陣R和偏移矩陣T,獲取無人機相對于地標的姿態(tài)和位置信息。

圖3 觀測量預(yù)測及狀態(tài)同步更新

2.2 基于模糊預(yù)測的視覺觀測數(shù)據(jù)分析及處理

以X軸位置的觀測數(shù)據(jù)為例,考慮到二次函數(shù)可以表征大部分目標的運動軌跡,在短時間內(nèi)攝像頭的位置觀測數(shù)據(jù)Tx(t)采用式(4)所示的模糊序列模型描述:

Tx(t)=r0+r1t+r2t2+ε

(4)

(5)

(6)

取k+1時刻以前的L個時刻的攝像頭歷史位置觀測數(shù)據(jù)Tx1,Tx2,…,TxL,令mt=max{Txt-1,Txt,Txt+1},nt=min{Txt-1,Txt,Txt+1}(t=2,3,…,L-1),m1=max{Tx1,Tx2},n1=min{Tx1,Tx2},mL=max{TxL-1,TxL},nL=max{TxL-1,TxL}。將這L個數(shù)據(jù)進行模糊處理,構(gòu)成如式(7)所示的一組三角模糊數(shù)st(x)。

(7)

(8)

(9)

式(9)中,權(quán)重ω={ω0,ω1,ω2}由普通線性回歸給出:

(10)

(11)

2.3 基于卡爾曼濾波器的狀態(tài)量同步更新

以O(shè)w-XwYwZw著陸地標坐標系為導(dǎo)航坐標系,在INS更新時,利用攝像頭觀測數(shù)據(jù)對無人機INS估計出的位置狀態(tài)進行誤差修正。

無人機在降落階段需要獲取OwXw、OwYw、OwZw方向上的3個位置與3個速度狀態(tài)量,選取無人機的狀態(tài)量為:

(12)

(13)

由勻加速直線運動方程,得到系統(tǒng)狀態(tài)方程在k時刻離散化為:

X(k)=AX(k-1)+BU(k)+Γω

(14)

式(14)中系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣A、驅(qū)動矩陣B、過程噪聲分布矩陣為Γ為:

(15)

(16)

(17)

其中DT為攝像頭觀測數(shù)據(jù)的更新周期。

Zk=HXk+Vkν

(18)

式中的系統(tǒng)狀態(tài)量為全觀測,故狀態(tài)觀測矩陣H=I6×6。

系統(tǒng)過程噪聲協(xié)方差矩陣Qk與靜態(tài)觀測噪聲矩陣Rk需要根據(jù)實際使用來整定:

(19)

(20)

先驗協(xié)方差矩陣與卡爾曼濾波增益更新的更新表達式為:

P(k+1|k)=AP(k|k)AT+ΓQΓT

(21)

K(k+1)=P(k+1|k)HT[HP(k+1|k)HT+R]-1

(22)

(23)

(24)

式(24)中,ΔT為當前INS的更新時刻k+1與最近一次觀測傳感器的更新時刻ko的時間間隔。ΔT和ko均來自計算機系統(tǒng)的測量時間。

后驗協(xié)方差矩陣更新如下:

P(k+1|k+1)=[I6×6-K(k+1)H]P(k+1|k)

(25)

根據(jù)上述分析,本文設(shè)計的基于模糊預(yù)測的INS/視覺無人機位姿估計算法框圖如圖4所示。

在無人機INS/視覺組合導(dǎo)航降落系統(tǒng)中,針對攝像頭觀測傳感器數(shù)據(jù)滯后特性引起的狀態(tài)估計誤差甚至發(fā)散問題,本文利用模糊預(yù)測的方法,同步預(yù)測滯后的觀測傳感器數(shù)據(jù)?？紤]到低成本微處理器的實際處理能力有限,利用卡爾曼濾波對INS的累計誤差進行修正,減小了計算量,保證了融合算法的實時性。

3 組合導(dǎo)航系統(tǒng)設(shè)計與實驗

本文自主設(shè)計了無人機自主著陸導(dǎo)航實驗平臺并設(shè)計了三組實驗,驗證了本文算法的實時性、有效性和實用性。

3.1 實驗配置介紹

無人機自主著陸導(dǎo)航實驗平臺包括四旋翼無人機和著陸地標,如圖5所示。四旋翼無人機由飛行控制器、電子調(diào)速器、無刷電機、螺旋槳、機架、電池、低壓報警器、遙控器和視覺模塊等部分組成。四旋翼無人機的總重量0.865 kg,機體對角線軸距為33 cm,高12 cm。

四旋翼無人機的系統(tǒng)硬件結(jié)構(gòu)如圖6所示,飛行控制器采用32位微處理器STM32F103RCT6,主頻為72 MHz。姿態(tài)傳感器采用六軸陀螺儀加速度計傳感器MPU6050,磁力計采用IST8310,氣壓計為SPL06-001。視覺模塊由OV5647攝像頭模組和“樹莓派”微型計算機組成。微型計算機的CPU采用Broadcom BCM2837,內(nèi)存1G,安裝了Raspbian操作系統(tǒng)和OpenCV 3.4.1視覺開發(fā)庫。

圖6 飛控系統(tǒng)結(jié)構(gòu)示意圖

本文在每幀圖像中選取8個特征點,求解無人機的偏移矩陣T,然后通過串口實時發(fā)送給飛行控制器。飛行控制器中INS更新頻率為200 Hz,根據(jù)實際調(diào)試,選取過程噪聲矩陣Qk與觀測噪聲矩陣Rk中參數(shù)a、b、c、d分別為2.0×10-6、0.015、0.075、2.5,貼近度h0取0.7。

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

①實時性對比實驗

在實時性對比實驗中,以高度方向位置融合為例,大約1 s左右將無人機快速拉至降落引導(dǎo)地標正上方約90 cm高度位置,得到同一實驗條件下本文組合導(dǎo)航算法和常規(guī)卡爾曼算法的實驗數(shù)據(jù)如圖7所示,算法的收斂時間和均方根誤差如表1所示。

表1 實時性對比實驗的數(shù)據(jù)分析

從圖7(a)和表1的實驗數(shù)據(jù)可以看出,本文設(shè)計的無人機組合導(dǎo)航算法得到的高度位置估計值相比于常規(guī)卡爾曼濾波算法,能一次性快速地跟蹤視覺傳感器解算出的高度變化過程,并且迅速收斂。結(jié)合圖7(b)的實驗數(shù)據(jù),在動態(tài)過程中,由于觀測傳感器更新周期長,短時間內(nèi)原始觀測高度更新不及時,而經(jīng)模糊預(yù)測得到的觀測數(shù)據(jù)能夠較為準確地描述觀測數(shù)據(jù)的變化。使得圖7(a)中組合導(dǎo)航得到的高度估計值相位超前于視覺傳感器觀測的高度,提高了位置估計的實時性。

圖7 實時性對比實驗

②無人機實際飛行實驗

在無人機實際飛行實驗中,無人機初始狀態(tài)為定點懸停狀態(tài),位于(10,-10,40),3 s后利用遙控器依次飛行至(10,10,40)、(-10,10,40)、(0,0,40)。懸停3 s后,控制遙控器油門動作位,讓無人機在高度方向做快速機動動作,最后,懸停在(0,0,22)處。無人機實際飛行實驗結(jié)果如圖8所示,X、Y、Z三方向的均方根誤差和誤差最大值如表2所示。

圖8 實際飛行融合實驗數(shù)據(jù)

方向誤差最大值/cm常規(guī)算法本文算法均方根誤差常規(guī)算法本文算法XYZ4.75.96.24.65.34.10.160.190.150.110.130.09

表2的數(shù)據(jù)表明本文組合導(dǎo)航算法相比于常規(guī)算法融合得到的位置估計精度更高,由圖8的位置融合實驗結(jié)果可知,靜態(tài)時慣導(dǎo)估計狀態(tài)和攝像頭的觀測狀態(tài)基本吻合,經(jīng)過融合得到的估計狀態(tài)數(shù)據(jù)會更加平滑,受觀測傳感器自身噪聲影響,融合得到的估計狀態(tài)數(shù)據(jù)會有一定的波動,其波動范圍遠小于觀測狀態(tài)數(shù)據(jù)自身的波動。觀察由靜止到運動的過程,攝像頭得到的觀測狀態(tài)存在一定的滯后現(xiàn)象,動態(tài)過程中融合得到的估計狀態(tài)相位始終超前于攝像頭的觀測狀態(tài)。觀察動態(tài)到靜止的運動過程可知,融合得到的位置估計會迅速收斂到位置觀測量。本文設(shè)計的組合導(dǎo)航算法提高了位姿估計的實時性,同時位姿估計數(shù)據(jù)能保證長時間收斂。

③自主著陸實驗

在自主降落實驗中,降落地標安裝在移動平臺上,利用遙控器控制無人機飛行在平臺上方1.5 m后,將自主降落對應(yīng)通道打到高位,無人機自動完成著陸控制。經(jīng)過多次實驗,著陸后無人機與地標中心的平均偏移誤差如表3所示。

表3 不同速度下降落誤差對比

表3的實驗數(shù)據(jù)表明了本文INS/視覺組合導(dǎo)航算法相比于常規(guī)算法能夠為無人機的自主降落控制提供更為實時、準確的反饋數(shù)據(jù),滿足四旋翼無人機的自主著陸要求。

4 結(jié)束語

在INS/視覺導(dǎo)航無人機自主著陸導(dǎo)航系統(tǒng)中,由于視覺觀測數(shù)據(jù)存在不確定性噪聲、更新慢等問題,導(dǎo)致組合導(dǎo)航算法的精度和實時性受到限制。鑒于此,本文設(shè)計了一種基于模糊預(yù)測的INS/視覺無人機自主著陸導(dǎo)航算法。考慮到機載處理器的硬件限制,本文從卡爾曼濾波模型出發(fā),采用三角模糊數(shù)處理視覺觀測數(shù)據(jù),減小了觀測噪聲和解算誤差帶來的濾波估計誤差。在INS數(shù)據(jù)更新時,利用模糊預(yù)測的方法同步預(yù)測滯后的視覺觀測傳感器數(shù)據(jù),相比于常規(guī)卡爾曼濾波算法,提高了無人機位姿估計的實時性。實測實驗結(jié)果表明利用本文設(shè)計的組合導(dǎo)航算法能實時、準確地估計無人機相對于地標的位姿信息,滿足無人機自主著陸所需數(shù)據(jù)的實時性與動態(tài)跟蹤精度要求,為基于INS/視覺的無人機自主著陸系統(tǒng)提供了一種簡單可靠的組合導(dǎo)航算法。