一種自適應(yīng)混合濾波鐵塔姿態(tài)解算方法

岑海倫，熊 鳴，王麗婕，李廣元

(1.北京信息科技大學(xué) 自動化學(xué)院，北京 100192；2.北京信息科技大學(xué) 高動態(tài)導(dǎo)航技術(shù)北京市重點(diǎn)實(shí)驗(yàn)室，北京 100192)

0 引言

受酸雨、臺風(fēng)、地震等惡劣環(huán)境的影響，輸電鐵塔結(jié)構(gòu)會出現(xiàn)不同程度的形變問題[1]，為了保障電力系統(tǒng)安全穩(wěn)定運(yùn)行，需要對鐵塔形變進(jìn)行監(jiān)測。目前主要有3種監(jiān)測方式：①基于機(jī)器視覺方法，通過卷積神經(jīng)網(wǎng)絡(luò)(convolutional neural networks，CNN)算法[2]對無人機(jī)航拍到的鐵塔圖像進(jìn)行鋼架主軸信息提取，并依據(jù)地面法線方向與軸之間的夾角來判斷鐵塔形變情況。這種方法屬于事后檢測，無法實(shí)時監(jiān)測鐵塔狀態(tài)也不能及時預(yù)警。②基于全球?qū)Ш蕉ㄎ幌到y(tǒng)[3-4](global navigation satellite system，GNSS)技術(shù)，在輸電鐵塔主要節(jié)點(diǎn)上裝設(shè)接收機(jī)，根據(jù)節(jié)點(diǎn)三維坐標(biāo)的變化計算出鋼材的形變。然而在強(qiáng)電磁場或者非視距的環(huán)境中，接收機(jī)常常會出現(xiàn)信號丟失情況，這使得GNSS解算出來的信號不可靠。③基于慣性導(dǎo)航系統(tǒng)[5](inertial navigation system，INS)技術(shù)，由慣性傳感器通過導(dǎo)航姿態(tài)解算方法解算當(dāng)前時刻偏離初始時刻的角度，可判斷鐵塔當(dāng)前形變情況。這種慣性傳感元件幾乎不受外界電磁環(huán)境干擾，具有很強(qiáng)的抗干擾能力[6]。

近年來，眾多學(xué)者圍繞導(dǎo)航姿態(tài)算法展開了大量的研究，涉及的姿態(tài)解算方法有高斯牛頓算法、互補(bǔ)濾波算法、卡爾曼濾波算法及其擴(kuò)展形式。于中亞[7]提出了一種加速度反饋修正的高斯牛頓算法，將其應(yīng)用于鐵塔形變監(jiān)測。楊金顯等[8]提出了一種桿塔傾斜狀態(tài)監(jiān)測方法，在濾波前先將傳感器采集到的數(shù)據(jù)進(jìn)行降噪處理，然后由四元數(shù)解算出當(dāng)前時刻傾斜角度。陳光武等[9]將非重力加速度作為信賴指標(biāo)調(diào)整互補(bǔ)濾波控制器參數(shù)，但存在所有的姿態(tài)角共用一個控制參數(shù)的問題，并且該算法在高動態(tài)環(huán)境中濾波效果不理想。以上方法只是針對加速度計或陀螺儀進(jìn)行補(bǔ)償，并且動態(tài)性能和靜態(tài)性能難以同時保證。陳仕毅等[10]提出了一種簡化卡爾曼濾波模型，其解算精度優(yōu)于傳統(tǒng)互補(bǔ)濾波算法。王見等[11]通過擴(kuò)展卡爾曼濾波矯正互補(bǔ)濾波解算的姿態(tài)角，從而抑制了數(shù)據(jù)波動。但是無論卡爾曼濾波還是擴(kuò)展卡爾曼濾波算法對系統(tǒng)模型要求都比較苛刻，且魯棒性較差。Sage-Husa自適應(yīng)卡爾曼作為極大后驗(yàn)估計，可對量測噪聲實(shí)時估計[12]，但存在計算量大、應(yīng)對突變情況的能力較差等問題。

針對以上問題，本文提出一種基于自適應(yīng)互補(bǔ)濾波與強(qiáng)跟蹤卡爾曼濾波(strong tracking Kalman filter，STK)的混合算法。設(shè)計自適應(yīng)互補(bǔ)濾波器融合陀螺儀和加速度信息，由加速度與重力向量之間的偏差修正載體角速度，再將濾波后角速度作為強(qiáng)跟蹤卡爾曼濾波的輸入矩陣。這種強(qiáng)跟蹤濾波器無需對噪聲進(jìn)行實(shí)時估計，而是由比例因子直接調(diào)整當(dāng)前量測噪聲，不僅大大簡化了計算量，而且還避免了在Sage-Husa自適應(yīng)卡爾曼濾波算法中因量測噪聲方差矩陣Rk非正定而導(dǎo)致的濾波發(fā)散問題。

1 慣性導(dǎo)航姿態(tài)解算

常用的姿態(tài)角解算方法包括歐拉角法和四元數(shù)法，其中四元數(shù)法因其具有計算量小、全姿態(tài)解算等優(yōu)點(diǎn)得到了廣泛的應(yīng)用。為描述鐵塔姿態(tài)變化，需定義兩個坐標(biāo)系：以地理坐標(biāo)系(n系)作為姿態(tài)解算導(dǎo)航坐標(biāo)系(R系)；以鐵塔固聯(lián)的坐標(biāo)系作為載體坐標(biāo)系(b系)，其中將載體坐標(biāo)系xb軸定義為鐵塔橫軸方向，yb軸定義為鐵塔縱軸方向，zb軸定義為鐵塔豎軸方向。導(dǎo)航坐標(biāo)系和載體坐標(biāo)系之間的轉(zhuǎn)換關(guān)系為

(1)

(2)

由式(2)可分別得到橫滾角γ和俯仰角θ：

(3)

2 傳統(tǒng)互補(bǔ)濾波理論

陀螺儀測量載體三軸角速度，經(jīng)積分可求解載體三軸角度信息；加速度計測量載體三軸加速度信息。兩種傳感器在解算過程中各有優(yōu)缺點(diǎn)，其中陀螺儀高頻特性好，但在姿態(tài)解算過程中存在隨時間遞增的積分誤差，因此不適合長時間單獨(dú)工作[13]。加速度傳感器靜態(tài)不存在累計誤差，但在動態(tài)環(huán)境中無法跟蹤姿態(tài)變化。根據(jù)兩種測量元件在頻域特性上呈現(xiàn)的互補(bǔ)關(guān)系，利用互補(bǔ)濾波融合兩種傳感器的數(shù)據(jù)，其具體步驟如下：

(4)

2)對加速度計輸出值進(jìn)行歸一化：

(5)

3)載體加速度與重力矢量之間的誤差向量為

(6)

4)誤差向量e經(jīng)PI控制器輸出，可得到陀螺儀角速度的修正值δω，修正后的陀螺儀角速度為ω′=ω+δω，利用修正后的角速度參與四元數(shù)更新：

(7)

(8)

式中，Ts為采樣周期

5)將更新后的四元數(shù)代入式(3)可求解出橫滾角和俯仰角。

3 混合濾波算法

由于自然風(fēng)具有隨機(jī)性、突發(fā)性等特點(diǎn)，鐵塔振動姿態(tài)與風(fēng)速和脈振頻率息息相關(guān)。面對復(fù)雜情況，傳統(tǒng)互補(bǔ)濾波算法無法準(zhǔn)確地估算出載體的狀態(tài)。本文所提出的混合濾波算法是在傳統(tǒng)互補(bǔ)濾波算法的基礎(chǔ)上，根據(jù)載體狀態(tài)自適應(yīng)調(diào)整濾波器狀態(tài)參數(shù)，并將濾波后角速度向量作為強(qiáng)跟蹤卡爾曼濾波算法的輸入矩陣，通過比例因子自適應(yīng)調(diào)整量測方差，混合濾波算法如圖1所示。

圖1 混合濾波算法

3.1 自適應(yīng)互補(bǔ)濾波

根據(jù)控制器參數(shù)對濾波效果的影響，積分常數(shù)KI主要是為了消除陀螺儀穩(wěn)態(tài)誤差，而KP調(diào)節(jié)低通濾波器和高通濾波器之間的截止頻率[14]。本文將載體的運(yùn)動狀態(tài)分為3種：靜止?fàn)顟B(tài)、低加速度狀態(tài)和高加速度狀態(tài)，根據(jù)載體狀態(tài)自適應(yīng)調(diào)整控制器參數(shù)KP。選取非重力加速度λ作為鐵塔狀態(tài)辨識量，用以判斷當(dāng)前時刻鐵塔狀態(tài)。

非重力加速度為

(9)

靜止?fàn)顟B(tài)下加速度計量測值存在漂移現(xiàn)象，需要考慮加速度白噪聲帶來的影響。將慣性元件水平放置，讀取加速度計測量的n組數(shù)據(jù)：

(10)

(11)

自適應(yīng)互補(bǔ)濾波算法規(guī)則如下：

1)當(dāng)λ≤ξ時，表明鐵塔主部件處在靜止?fàn)顟B(tài)，此時控制參數(shù)Kp，0=KP，其中Kp為比例系數(shù)初始值。

3)當(dāng)λ>h時，表明鐵塔主部件處于高加速度狀態(tài)，此時加速度計測量結(jié)果不可靠，控制參數(shù)為KP,2=0。

自適應(yīng)互補(bǔ)濾波算法如圖2所示。

圖2 自適應(yīng)互補(bǔ)濾波

首先根據(jù)加速度值λ判別載體當(dāng)前狀態(tài)，調(diào)整控制參數(shù)；然后再根據(jù)互補(bǔ)濾波算法進(jìn)行傳感器數(shù)據(jù)融合，用以補(bǔ)償陀螺儀角速度；最后由四元數(shù)矩陣Ω(ω′)更新當(dāng)前姿態(tài)。

3.2 強(qiáng)跟蹤卡爾曼濾波

強(qiáng)跟蹤卡爾曼濾波以卡爾曼濾波為依托，當(dāng)檢測到量測異常時可由比例系數(shù)調(diào)整量測方差[15]，實(shí)現(xiàn)量測噪聲方差自適應(yīng)濾波。算法實(shí)現(xiàn)過程如式(12)～(20)所示。

1)預(yù)測過程:

(12)

(13)

2)量測方差估計過程：

(14)

(15)

(16)

(17)

3)更新過程：

(18)

(19)

(20)

4 實(shí)驗(yàn)驗(yàn)證與分析

為了驗(yàn)證本文所提濾波算法的有效性，搭建實(shí)驗(yàn)平臺采集數(shù)據(jù)，并通過Matlab仿真軟件進(jìn)行算法驗(yàn)證。實(shí)驗(yàn)設(shè)備包括三維轉(zhuǎn)臺與數(shù)據(jù)采集模塊，如圖3所示。設(shè)置單片機(jī)STM32F407ZGT6采樣頻率為100 Hz，用于采集MPU6050數(shù)據(jù)，并將數(shù)據(jù)通過串口的方式發(fā)送給上位機(jī)，實(shí)驗(yàn)原理如圖4所示。

圖3 三維轉(zhuǎn)臺

圖4 實(shí)驗(yàn)原理

實(shí)驗(yàn)分為靜態(tài)實(shí)驗(yàn)和搖擺實(shí)驗(yàn)兩部分。本文將傳統(tǒng)互補(bǔ)濾波算法、歐拉角卡爾曼濾波算法、自適應(yīng)互補(bǔ)濾波算法、混合濾波算法進(jìn)行對比。設(shè)定PI濾波器控制參數(shù)KP為1.2，KI為0.55。并以均方根誤差(root mean square error，RMSE)來評價各種濾波的性能。均方根誤差為

(21)

式中：xk為k時刻濾波解算出來的姿態(tài)角；x為真實(shí)姿態(tài)角；n為數(shù)據(jù)的長度。

4.1 靜態(tài)實(shí)驗(yàn)

首先讓轉(zhuǎn)臺處于水平位置，理想狀態(tài)下俯仰角和橫滾角均為0°。考慮到三軸加速度波動幅度較小，傳統(tǒng)互補(bǔ)濾波算法與自適應(yīng)互補(bǔ)濾波算法姿態(tài)解算結(jié)果幾乎一致，所以靜態(tài)實(shí)驗(yàn)只使用互補(bǔ)濾波、歐拉角卡爾曼濾波和混合濾波3種算法分別對橫滾角和俯仰角進(jìn)行解算。圖5為橫滾角解算結(jié)果，圖6為俯仰角解算結(jié)果。可以看出，混合濾波算法解算結(jié)果均優(yōu)于其他兩種方法，其橫滾角和俯仰角誤差均小于0.05°。

圖5 靜態(tài)橫滾角解算結(jié)果

圖6 靜態(tài)俯仰角解算結(jié)果

表1給出了3種濾波算法姿態(tài)角均方根誤差，其中傳統(tǒng)互補(bǔ)濾波算法的性能最差，歐拉角卡爾曼濾波算法優(yōu)于傳統(tǒng)互補(bǔ)濾波算法，其整體RMSE約為傳統(tǒng)互補(bǔ)濾波算法的72.00%。混合濾波算法抑制噪聲效果最好，精度提升較大，其整體RMSE下降為歐拉角卡爾曼濾波算法的51.04%左右。

表1 靜態(tài)實(shí)驗(yàn)姿態(tài)角均方根誤差

4.2 搖擺實(shí)驗(yàn)

設(shè)計轉(zhuǎn)臺搖擺實(shí)驗(yàn)來模擬鐵塔擺動，將慣性傳感器固定在轉(zhuǎn)臺上，令轉(zhuǎn)臺分別繞橫滾軸和俯仰軸方向做周期搖擺運(yùn)動，設(shè)置搖擺幅值為3°，搖擺頻率為2 Hz。分別采用歐拉角卡爾曼濾波、傳統(tǒng)互補(bǔ)濾波、自適應(yīng)互補(bǔ)濾波、混合濾波4種算法對載體姿態(tài)進(jìn)行解算，其橫滾角和俯仰角姿態(tài)解算結(jié)果如圖7～8所示，局部放大圖如圖9～10所示。

圖7 轉(zhuǎn)臺搖擺橫滾角估計

圖8 轉(zhuǎn)臺搖擺俯仰角估計

圖9 橫滾角局部放大圖

圖10 俯仰角局部放大圖

從圖7～10中可以看出，歐拉角卡爾曼濾波和傳統(tǒng)互補(bǔ)濾波算法解算的姿態(tài)角存在較大超調(diào)，自適應(yīng)互補(bǔ)濾波算法根據(jù)載體當(dāng)前非重力加速度自適應(yīng)調(diào)整PI控制器，在一定程度上抑制了超調(diào)，然而在高動態(tài)環(huán)境中無法對當(dāng)前噪聲進(jìn)行準(zhǔn)確評估?；旌蠟V波算法在自適應(yīng)互補(bǔ)濾波作為狀態(tài)值的基礎(chǔ)上，通過對當(dāng)前量測噪聲進(jìn)行估計，進(jìn)一步提高了濾波效果。表2為4種濾波算法的均方根誤差。

表2 搖擺實(shí)驗(yàn)姿態(tài)角均方根誤差

從表2可以看出，在動態(tài)實(shí)驗(yàn)中傳統(tǒng)互補(bǔ)濾波算法精度優(yōu)于歐拉角卡爾曼濾波算法，自適應(yīng)互補(bǔ)濾波算法整體精度相比于傳統(tǒng)互補(bǔ)濾波有較大提升，整體RMSE僅為傳統(tǒng)濾波算法的37.33%?；旌蠟V波算法動態(tài)跟蹤能力最好，整體精度最高，其整體RMSE下降到約為自適應(yīng)互補(bǔ)濾波算法的54.23%。

5 結(jié)束語

針對傳統(tǒng)的濾波算法存在姿態(tài)解算精度不高的問題，本文提出一種基于自適應(yīng)互補(bǔ)濾波與強(qiáng)跟蹤卡爾曼濾波的混合算法。在轉(zhuǎn)臺上設(shè)計靜態(tài)和搖擺兩組實(shí)驗(yàn)，對比了傳統(tǒng)互補(bǔ)濾波、歐拉角卡爾曼濾波、自適應(yīng)互補(bǔ)濾波以及混合濾波4種算法解算的精確度。實(shí)驗(yàn)結(jié)果表明，混合濾波算法無論在靜態(tài)實(shí)驗(yàn)還是動態(tài)實(shí)驗(yàn)中均優(yōu)于其他算法，其在靜態(tài)環(huán)境中能夠抑制姿態(tài)角波動，在動態(tài)環(huán)境中能夠及時跟蹤姿態(tài)變化，可滿足對鐵塔實(shí)時監(jiān)測的任務(wù)需求。