基于巴特沃斯低通濾波的自適應(yīng)步長梯度下降姿態(tài)解算方法

王柏森,盧艷軍,2,張曉東,2,劉 冬

(1.沈陽航空航天大學(xué)自動化學(xué)院,遼寧 沈陽 110135;2.遼寧通用航空研究院,遼寧 沈陽 110136)

0 引言

近年來多旋翼無人機(jī)在復(fù)雜環(huán)境探索、低空城市搜救等場景廣泛應(yīng)用,飛行控制逐步由平緩小姿態(tài)向快響應(yīng)大角度方向發(fā)展[1-2]。目前姿態(tài)測量系統(tǒng)受限于多旋翼無人機(jī)的體積與成本,多數(shù)仍依賴于陀螺儀、加速度計(jì)、磁強(qiáng)計(jì)構(gòu)成的微型慣性測量單元(miniature inertial measurement unit,MIMU)[3],單一傳感器一般采用高頻采樣(1～2 kHz)、數(shù)字濾波處理、低頻輸出(100～200 Hz)方案,在濾波預(yù)處理階段,由于濾波器具有相位遲延效應(yīng)致使原始數(shù)據(jù)輸出存在一定時滯,導(dǎo)致無人機(jī)大角度姿態(tài)變換速率無法進(jìn)一步提高[4]。針對以上問題,文獻(xiàn)[5]提出基于巴特沃斯濾波的自適應(yīng)卡爾曼姿態(tài)解算方法,有效剔除傳感器原始數(shù)據(jù)噪聲減緩姿態(tài)解算時滯,但該算法較依賴高性能處理器平臺。文獻(xiàn)[6]針對低成本處理器提出動態(tài)步長梯度下降姿態(tài)解算方法,有效抑制運(yùn)動加速度對姿態(tài)解算的干擾,但在角速率變化快時將出現(xiàn)姿態(tài)解算滯后現(xiàn)象。

本文針對旋翼機(jī)使用低成本MIMU而造成的姿態(tài)原始數(shù)據(jù)預(yù)處理后具有滯后的問題進(jìn)行研究,建立傳感器誤差模型后進(jìn)行現(xiàn)場標(biāo)定[7],根據(jù)零相位偏差原理修正巴特沃斯濾波時滯,設(shè)計(jì)自適應(yīng)步長梯度下降姿態(tài)解算算法,并在低成本微處理器中驗(yàn)證了算法可行性和提高姿態(tài)信息獲取的實(shí)時準(zhǔn)確性。

1 MEMS傳感器姿態(tài)解算原理

MEMS姿態(tài)解算中通常定義相對運(yùn)動的導(dǎo)航坐標(biāo)系(n系)和機(jī)體坐標(biāo)系(b系)用以表達(dá)姿態(tài)變換關(guān)系,根據(jù)三軸坐標(biāo)系正向不同指向可產(chǎn)生多個坐標(biāo)系定義方式[8]。本文導(dǎo)航坐標(biāo)系使用東北天系,機(jī)體坐標(biāo)系使用右前上系進(jìn)行姿態(tài)描述,初始時兩坐標(biāo)系的原點(diǎn)相互重合,并定義機(jī)體坐標(biāo)系下繞X軸旋轉(zhuǎn)的姿態(tài)角為傾仰角(θ),繞Y軸旋轉(zhuǎn)的姿態(tài)角為橫滾角(γ),繞Z軸旋轉(zhuǎn)的姿態(tài)角為偏航角(ψ)。兩個坐標(biāo)系之間一般可用四元數(shù)法、歐拉角法、方向余弦法進(jìn)行轉(zhuǎn)換,其中歐拉角法因萬向節(jié)死鎖問題無法描述全姿態(tài)變化,方向余弦法涉及較多三角函數(shù)運(yùn)算不適用低成本處理器中,因此本文使用四元數(shù)法更新姿態(tài)角變化,根據(jù)zyx順規(guī)三軸姿態(tài)角有如下四元數(shù)表達(dá)方式[9]：

(1)

2 基于LPB的自適應(yīng)梯度下降姿態(tài)解算

2.1 LPB參數(shù)初始化

傳統(tǒng)低通濾波器處理姿態(tài)原始數(shù)據(jù)時,當(dāng)載體作大角度變換時易受加速度高頻噪聲干擾,姿態(tài)測量與真實(shí)值存在一定時延,巴特沃斯低通濾波器(low-pass Butterworth filter,LPB)在處理線性相位與斜率衰減方面具有平坦過渡的特點(diǎn)[10],可有效剔除高頻振動干擾,其增益平方幅度響應(yīng)公式為

(2)

式(2)中,N為LPB濾波階數(shù),ω為輸入角速度,ωc為LPB截止頻率。由公式可得平方幅度響應(yīng)是輸入角速度的單調(diào)遞減函數(shù),濾波器階數(shù)越高幅頻特性效果越好,低頻測量信號失真度越低。由阻帶頻率ωs確定阻帶衰減As：

As=-20lg|H(jωs)|。

(3)

將幅頻特性帶入得

(4)

將上述數(shù)字角頻率推導(dǎo)轉(zhuǎn)換為模擬角頻率后,通過查表確定傳遞函數(shù)G(s),再根據(jù)雙線性變換將S域轉(zhuǎn)換為Z域,通過求取G(z)系數(shù)確定LPB的參數(shù),與傳統(tǒng)濾波器特性相似的LPB也存在因相位偏移導(dǎo)致的時延問題。

2.2 數(shù)字濾波器相位時延的改善

根據(jù)零相位數(shù)字濾波器原理[11]進(jìn)行預(yù)修正處理,將一定長度的姿態(tài)原始數(shù)據(jù)正向時間序列和反向時間序列通過LPB,根據(jù)正反時延差大小消除相位偏移,進(jìn)而實(shí)現(xiàn)巴特沃斯濾波的零相位偏移。

假設(shè)x[n]是一組長度為N+1的單邊序列,通過沖擊響應(yīng)為h[n]的LPB濾波后得到單邊序列y1[n],對y1[n]進(jìn)行時間序列反轉(zhuǎn)處理并平移得到單邊序列y2[n],再次通過沖擊響應(yīng)為h[n]的LPB濾波后得到單邊序列y3[n],對y3[n]再次進(jìn)行時間序列反轉(zhuǎn)處理并平移得到y(tǒng)[n],即

(5)

將以上變換轉(zhuǎn)為頻域描述并整理后得到零相位濾波輸入輸出關(guān)系如下：

Y(ejω)=X(ejω)|H(ejw)|2。

(6)

從式(6)可知,輸出Y(ejω)與輸入X(ejω)之間理論不存在相位偏差,在實(shí)際應(yīng)用時受限于低成本微處理器算力影響,當(dāng)待濾波數(shù)據(jù)輸入速率大于濾波器處理速度時仍將出現(xiàn)相位偏移現(xiàn)象,此時可通過在存儲器中添加FIFO機(jī)制適當(dāng)緩存待處理數(shù)據(jù)。該種方案將占用大量存儲空間,因此需根據(jù)實(shí)際濾波效果適當(dāng)選擇濾波器階數(shù)。

2.3 梯度下降算法姿態(tài)解算

梯度下降算法是一種求解函數(shù)極值的數(shù)學(xué)模型,其一般迭代公式為

(7)

式(7)中,?J(a(k-1))為目標(biāo)函數(shù)J(a(k-1))在某點(diǎn)a(k-1)的正梯度向量,μ為每次迭代的固定步長。

將n系中的重力矢量單位歸一化處理,并將其三軸分解至b系得

(8)

將重力矢量在b系下的分量ab與加速度傳感器測量值作差得到誤差函數(shù)并將其用四元數(shù)法表達(dá)后可得

(9)

(10)

2.4 自適應(yīng)步長梯度下降算法

上述梯度下降姿態(tài)解算中迭代步長μ為定值,在實(shí)際飛行中姿態(tài)變化平緩時解算跟隨效果較好,當(dāng)姿態(tài)變化迅速時定步長梯度下降迭代更新速率小于姿態(tài)變化速率,導(dǎo)致姿態(tài)更新遲滯。梯度下降迭代更新過程中迭代步長與機(jī)體運(yùn)動時的角速度以及系統(tǒng)的采樣時間成正相關(guān)關(guān)系[12]。

為避免弱動態(tài)飛行時頻繁計(jì)算得到過小步長,消耗算力增加解算時延,引入機(jī)身運(yùn)動狀態(tài)估計(jì)因子α：

(11)

當(dāng)狀態(tài)估計(jì)因子α大于閾值α0時,開啟自適應(yīng)計(jì)算步長,否則使用定步長μ0解算姿態(tài)。

(12)

式(12)中,Ts為系統(tǒng)采樣時間,ω為載體運(yùn)動角速度。

綜上,自適應(yīng)梯度下降姿態(tài)解算迭代公式為

(13)

3 實(shí)驗(yàn)與結(jié)果

3.1 實(shí)驗(yàn)測試平臺

以項(xiàng)目組自研飛控為平臺,微處理器采用低成本ARM Cortex-M3芯片,MEMS傳感器為ICM20602,定高傳感器為氣壓計(jì)與超聲波,水平定位傳感器為光流,整機(jī)如圖1所示。在無轉(zhuǎn)臺設(shè)備情況下,為驗(yàn)證本系統(tǒng)姿態(tài)解算效果使用高精度慣導(dǎo)系統(tǒng)輸出的數(shù)據(jù)作為對比參考,如圖2所示型號為440 Series Inertial Systems系列的AHRS440,該慣導(dǎo)可動態(tài)輸出三軸姿態(tài)角度以及相關(guān)姿態(tài)原始數(shù)據(jù),高精度慣導(dǎo)AHRS440參數(shù)如表1所示。將本系統(tǒng)與高精度慣性測量系統(tǒng)測量的數(shù)據(jù)在弱動態(tài)、強(qiáng)動態(tài)狀態(tài)下作對比驗(yàn)證。

圖1 多旋翼實(shí)驗(yàn)平臺Fig.1 Multi rotor experimental platform

3.2 巴特沃斯濾波分析

首先通過快速傅里葉變換分析靜止?fàn)顟B(tài)下傳感器數(shù)據(jù)頻譜圖,提取噪聲截止頻率并初始化濾波器參數(shù)。圖3為加速度原始信號(Raw)在低通濾波(LPF)以及不同階巴特沃斯濾波(LPB-x)下響應(yīng)曲線,由圖中可知LPB濾波后數(shù)據(jù)更加平緩,但存在一定時滯。圖4為LPF處理后加速度原始信號與未時延修正巴特沃斯濾波(LPB)和時延修正巴特沃斯濾波(ZLPB)后對比圖,由圖中可知零相位修正后時延得到顯著改善,高頻噪聲部分得到剔除,同時當(dāng)LPB階數(shù)越高數(shù)據(jù)過度越平坦,時滯越大。圖5為LPF與各階ZLPB濾波對比,加入零相位修正后中高階LPB仍存在較大時滯,因此采用二階LPB。

圖3 各階LPB濾波效果Fig.3 LPB filtering effect of each order

圖4 零相位修正對比Fig.4 Comparison of zero phase correction

圖5 各階零相位修正LPB對比Fig.5 Comparison of zero phase correction LPB of each order

濾波器階數(shù)選擇與時延修正完成后開始處理姿態(tài)原始數(shù)據(jù)。為測量單次姿態(tài)解算時長,首先在無操作系統(tǒng)的處理器中設(shè)計(jì)足夠長時間的定時器中斷,并記錄單次最長解算時間,測量發(fā)現(xiàn)在運(yùn)行速度為72 MHz的處理器中單次采集MEMS姿態(tài)傳感器9軸原始數(shù)據(jù)最高消耗1 ms,姿態(tài)解算最高消耗2.5 ms,為預(yù)留充足系統(tǒng)調(diào)度以及其他緊急任務(wù)時間,系統(tǒng)最小中斷周期設(shè)計(jì)為5 ms,即傳感器采樣頻率為200 Hz。

實(shí)際測試時磁強(qiáng)計(jì)作為航向修正觀測量,無需高頻濾波處理,只需測量加速度與角速度濾波效果。將MEMS姿態(tài)傳感器低通濾波和時延修正巴特沃斯濾波后加速度數(shù)據(jù)與高精度慣導(dǎo)輸出的加速度數(shù)據(jù)對比如圖6所示。

圖6 加速度數(shù)據(jù)處理對比Fig.6 Comparison of acceleration processing

由圖6可知高精度慣導(dǎo)輸出的加速度原始數(shù)據(jù)更加敏感,通過與LPF和AHRS440系統(tǒng)輸出的數(shù)據(jù)對比可知,設(shè)計(jì)的ZLPB有效濾除了高頻噪聲使信號過度平坦,經(jīng)過零相位修正后有效減小時延,誤差對比如表2所示。

表2 時延修正濾波后優(yōu)化結(jié)果Tab.2 Optimization results after time delay correction filtering

將MEMS姿態(tài)傳感器低通濾波后角速度數(shù)據(jù)和時延修正后巴特沃斯濾波角速度數(shù)據(jù)與高精度慣導(dǎo)輸出的角速度數(shù)據(jù)對比如圖7所示,由圖中可知ZLPB處理后的角速度數(shù)據(jù)時延更小、噪聲低。

圖7 角速度數(shù)據(jù)處理對比Fig.7 Comparison of angular velocity data processing

3.3 自適應(yīng)步長梯度下降姿態(tài)解算分析

將兩個相同器件飛控現(xiàn)場在線標(biāo)定后,對比定步長梯度下降姿態(tài)解算(N-GD)與自適應(yīng)步長梯度下降姿態(tài)解算(M-GD)分別在弱動態(tài)、強(qiáng)動態(tài)下姿態(tài)解析精度。圖8為弱動態(tài)下姿態(tài)解析對比,由圖中可知在弱動態(tài)條件下二者解算近似,自適應(yīng)步長梯度下降姿態(tài)解算與高精度慣導(dǎo)數(shù)據(jù)誤差對比,PIT軸為2.12%,ROL軸為1.95%。

圖8 弱動態(tài)下姿態(tài)解析對比Fig.8 Analytical comparison of attitude under weak dynamics

圖9為強(qiáng)動態(tài)下姿態(tài)解析對比,由于傳統(tǒng)梯度下降姿態(tài)解算每次迭代步長固定,當(dāng)機(jī)身運(yùn)動角速度大于30 (°)/s的強(qiáng)動態(tài)條件時姿態(tài)變化較快,迭代不能及時跟隨響應(yīng),導(dǎo)致姿態(tài)解算存在一定時延,且在端點(diǎn)處不能反饋真實(shí)姿態(tài)信息。定步長梯度下降姿態(tài)解算與高精度慣導(dǎo)數(shù)據(jù)誤差對比,PIT軸誤差為9.11%,ROL軸誤差為8.68%;自適應(yīng)步長梯度下降姿態(tài)解算與高精度慣導(dǎo)數(shù)據(jù)誤差對比,PIT軸誤差為2.31%,ROL軸誤差為2.14%。

圖9 強(qiáng)動態(tài)下姿態(tài)解析對比Fig.9 Analytical comparison of attitude under strong dynamics

4 結(jié)論

本文以旋翼無人機(jī)強(qiáng)動態(tài)高速運(yùn)動背景下提高低成本MEMS傳感器姿態(tài)解析精度、降低時延為核心思想,分析并校正傳感器原始數(shù)據(jù)誤差,在強(qiáng)動態(tài)運(yùn)動條件下確定傳感器量程范圍后設(shè)計(jì)二階巴特沃斯低通濾波器,并用零相位時延修正方法削弱因?yàn)V波器相位偏移導(dǎo)致的時滯,在72 MHz處理器中設(shè)計(jì)自適應(yīng)步長梯度下降姿態(tài)解算方法,根據(jù)運(yùn)動狀態(tài)估計(jì)因子大小自適應(yīng)調(diào)整迭代步長。實(shí)驗(yàn)結(jié)果表明該解算方法在強(qiáng)動態(tài)條件下姿態(tài)解算精度更高、時延更小,可為無人機(jī)提供準(zhǔn)確姿態(tài)信息,具有一定的實(shí)際工程應(yīng)用價值。