基于MEMS振動陀螺的旋翼無人機組合導航算法設計

劉 昕,秦 超,2

(1. 中國電子科技集團公司 第五十四研究所,河北 石家莊 050081;2. 哈爾濱工程大學 智能科學與工程學院,黑龍江 哈爾濱 150001)

0 引言

近年來國內四旋翼飛行器產業的發展迅猛,與傳統的直升機和固定翼相比,旋翼無人機的易用性(運動相互解耦)、可靠性(機械磨損小)和維護性(結構簡單、模塊化)等剛性體驗讓人們的選擇成為必然[1]。旋翼無人機實現精準姿態控制與位置控制的前提是導航模塊提供穩定可靠的姿態、位置數據,目前多采取組合導航技術,將多種傳感器對姿態、位置等數據進行信息融合[2]。文獻[3]介紹了過去10年中提出的解決方案。文獻[4-5]在旋翼無人機控制中應用了傳統的導航算法。針對如何確保無人機控制平穩性的問題,本文使用微機電系統(MEMS)振動陀螺提出了一種基于擴展卡爾曼濾波(EKF)的導航算法,融合全球導航衛星系統(GNSS)觀測和自身加速度計輸出觀測,克服GNSS數據失效時的姿態穩定問題。同時引入自適應平滑采樣方法,降低了角速度輸出噪聲。

1 系統模型

組合導航算法基于擴展卡爾曼濾波模型,以全球導航衛星系統/慣性導航系統(GNSS/INS)組合導航的框架為基礎,結合了無地磁傳感器的航姿參考系統算法[6]。算法以加速度計的輸出值和GNSS測量的位置Pm、速度Vm為觀測量,估計出四元數偏差和陀螺零偏bω。組合導航系統框架如圖1所示,圖中ωm、am分別為三軸MEMS陀螺儀和加速度的輸出,P、V和q分別為慣性解算得到的位置、速度和四元數,φ、θ和γ為組合導航系統輸出的航向、俯仰和橫滾。

圖1 組合導航系統框架

1.1 慣性測量單元(IMU)誤差模型

IMU的誤差包括系統誤差和隨機誤差。系統誤差可通過器件常規標定方法進行消除或降低,包括零偏、刻度系數誤差及非正交性誤差等。本文選用的高解耦微型陀螺如圖2所示。微型陀螺敏感芯片采用多質量塊解耦設計方案,驅動與檢測音叉分離,通過撓性支撐結構連接,避免了各模態的機械耦合誤差,達到解耦效果,隔離框的設計可有效地提升敏感芯片的環境適應性[7]。

圖2 高解耦微型陀螺總體結構示意圖

隨機誤差包括高斯白噪聲及隨機游走噪聲,其中白噪聲在概率上服從高斯分布,其功率譜密度為均勻分布。偏置隨機游走被認為是離散化的維納過程,由于IMU每次更新位置時都會疊加一個新的白噪聲,故將其偏置噪聲建模為隨機游走噪聲[8]。IMU誤差模型為

(1)

1.2 航姿表示形式

(2)

航向角φ、俯仰角θ、橫滾角γ為

(3)

2 基于EKF的組合導航算法設計

2.1 狀態方程

(4)

式中:bω為陀螺零偏矢量;wω為噪聲矢量;Ω為簡化的四元數乘法矩陣,且:

(5)

2.2 量測方程

GNSS系統是輔助INS進行姿態保持的良好互補系統,此處以其輸出位置、速度為觀測量建立輸出方程1:

(6)

由于無人機的運動形式有別于地面車輛,當GNSS系統失效時,無法引入車輛的運動學約束來抑制導航系統的誤差[9],因此,參照常規的航姿參考系統算法,即以陀螺儀和加速度計輸出、航姿變化量等參數為判斷條件,當判定無人機處于靜態、準靜態和低動態條件下,以加速度計輸出為觀測量建立輸出方程2:

(7)

2.3 濾波算法

依照EKF的工作流程,將連續時間模型進行離散化,得到:

(8)

式中:wk為過程噪聲矢量;vk為量測噪聲。假定wk,vk均為白噪聲,各自的協方差矩陣分別為Q、R。

離散化計算采用一階近似:

(9)

式中:T為濾波周期。

(10)

中間過程矩陣計算如下:

(11)

FVq0=2(q0amx-q3amy+q2amz)

(12)

FVq1=2(q1amx+q2amy+q3amz)

(13)

FVq2=2(-q2amx+q1amy+q0amz)

(14)

FVq3=2(-q3amx-q0amy+q1amz)

(15)

Fqq=

(16)

量測矩陣H為

(17)

(18)

以GNSS系統的位置、速度為觀測量時,使用此矩陣的前6行;以航姿參考系統算法中的加速度計輸出為觀測量時,使用此矩陣的后3行。

3 陀螺數據的自適應平滑采樣

旋翼無人機控制要求導航系統輸出的姿態和角速度信息平穩,數據噪聲會影響控制的穩定性。在盡量降低數據處理復雜度的前提下,將組合導航算法劃分為機械編排、卡爾曼濾波預測過程和卡爾曼濾波更新過程3個主要耗時模塊。分別在機械編排前、機械編排與卡爾曼濾波預測過程之間、卡爾曼濾波預測與更新之間進行采樣,并依次將陀螺輸出分別記為ω1、ω2和ω3,在下次機械編排前進行陀螺數據的自適應平滑,算法流程圖如圖3所示。

圖3 基于自適應平滑采樣的濾波流程

自適應平滑采樣器基于Sigmoid函數[10],考慮無人機動態性和陀螺的噪聲水平,依據無人機控制角速度輸入ωc及其變化率δωc構造權重函數:

(19)

則下一周期機械編排時角速度輸入可表示為

(20)

4 實驗結果與分析

為了驗證該算法的正確性,在某型搭載了SBG Ellipse-N型導航模塊的旋翼無人機上進行了對比測試,該模塊的姿態精度可達0.05°,航向精度可達0.2°。分別在導航計算機中使用EKF和本文方法進行了導航解算,并與安裝在無人機上的法國SBG導航模塊的輸出進行對比,陀螺和姿態數據對比如圖4～8和表 1、2所示。結果表明,最大角速度誤差和角誤差標準差有明顯提升。

表1 EKF與本文方法最大角速度誤差對比

表2 EKF與本文方法角誤差標準差對比

圖4 SBG、EKF以及本文方法陀螺x軸輸出對比

圖5 SBG、EKF以及本文方法陀螺y軸輸出對比

圖6 SBG、EKF及本文方法陀螺z軸輸出對比

圖7 SBG、EKF及本文方法橫滾角對比

圖8 SBG、EKF及本文方法俯仰角對比

圖9為在旋翼無人機平臺上采用本文方法后的無人機飛行軌跡圖。由圖可看出軌跡重復性高,控制平穩。

圖9 旋翼無人機飛行軌跡

5 結束語

本文提出了一種面向旋翼無人機的組合導航算法。算法基于擴展卡爾曼濾波,結合了GNSS/INS組合導航算法和無地磁傳感器的航姿參考系統算法,在一定程度上解決了GNSS失效情況下的姿態穩定問題。針對旋翼無人機對導航系統輸出信息平穩性的要求,采用自適應陀螺采樣平滑方法,在少量增加數據處理復雜度的前提下,降低了航姿和角速度信息的噪聲,在無人機時實現了良好的飛行軌跡重復性和控制平穩性。