張富春，鄭武略，汪 豪，吳陽陽，丁紅濤，崔家山

(1.中國南方電網(wǎng)超高壓輸電公司廣州局， 廣州 510405; 2.西安電子科技大學(xué)空間科學(xué)與技術(shù)學(xué)院， 西安 710126)

高空檢修作業(yè)是電網(wǎng)穩(wěn)定運行的必要手段。近年來，新建線路均采用高跨設(shè)計，而且大部分線路地處偏遠、環(huán)境惡劣、交通不便，這對線路高空維護檢修及其安全性帶來了巨大挑戰(zhàn)。目前，國內(nèi)外科研人員針對高空作業(yè)安全與防墜進行了深入研究與探索，并取得了一系列成果[1-3]，但絕大多數(shù)方案是基于機械原理實現(xiàn)的，即通過結(jié)構(gòu)設(shè)計，增加系統(tǒng)的摩檫力。

在其他行業(yè)出現(xiàn)了通過MES(Microelectro Mechanical Systems)傳感器檢測裝置姿態(tài)的方式進行安全性研究[4-6]。在導(dǎo)航與制導(dǎo)領(lǐng)域，出現(xiàn)了井噴的現(xiàn)象。文獻[7]提出了基于互補濾波器的姿態(tài)信息融合方法。文獻[8]設(shè)計了基于梯度下降法的互補濾波器進行四元數(shù)誤差估計，并應(yīng)用于開源無人機項目。國內(nèi)外學(xué)者對基于慣導(dǎo)系統(tǒng)加上磁力計的互補濾波算法進行了大量研究[9-10]，后來Premerlani W和Bizaed P對Mahony的互補濾波算法進行了驗證，并形成了開源飛控APM和PX4的姿態(tài)解算互補濾波算法[11]。擴展卡爾曼濾波算法作為非線性系統(tǒng)下的高效算法也被大量的應(yīng)用于飛行器的姿態(tài)解算中[12-14]。

基于已有檢測方法，針對輸電線路高空作業(yè)特點，研究了基于改進擴展卡爾曼濾波的柔性繩索自動升降裝置姿態(tài)估計方法。

1 姿態(tài)表示方法

姿態(tài)用于描述剛體的剛體坐標系和參考坐標系之間的角度位置關(guān)系，如圖1所示。

圖1 姿態(tài)表示的角度位置關(guān)系

常見的表示方法有方向余弦矩陣、歐拉角和四元數(shù)。柔性繩索自動升降裝置的姿態(tài)信號指的是柔性繩索自動升降裝置在導(dǎo)航坐標系下的空間位置信息表示，即機體坐標系在導(dǎo)航坐標系中的方位，考慮到處理的方便性，采用四元數(shù)表示[15]。定義四元數(shù)q為：

(1)

為了簡化描述，四元數(shù)q可以寫為：

(2)

式(2)中，q0,…,q3稱作旋轉(zhuǎn)四元數(shù)。

定義υB和υN分別表示機體坐標系和導(dǎo)航坐標系中的矢量，則有：

(3)

(4)

為了書寫簡便，旋轉(zhuǎn)矩陣可以簡寫為：

(5)

與此同時，姿態(tài)四元數(shù)也可以由旋轉(zhuǎn)矩陣表示為：

(6)

四元數(shù)q的時間變化率與柔性繩索自動升降裝置的運動角速度ω之間的關(guān)系可由如下的動力學(xué)方程表示：

(7)

式(7)中：ω表示陀螺儀的輸出

(8)

Ω(ω)和Ξ(q)分別由角速度和四元數(shù)組成

(9)

(10)

式(9)、式(10)中：I表示單位陣；反對稱矩陣運算符?qv×」定義為

(11)

姿態(tài)檢測傳感器中，一般將陀螺儀和加速度計作為核心器件，而磁力計僅對陀螺儀在偏航角上的積分進行長時矯正[16]。

加速度計測量的是重力分量，當檢測變速運動的加速度信號時，會導(dǎo)致姿態(tài)角的解算不準確。陀螺儀測量升降裝置角速度時，隨著積分時間的增加，其誤差增加。磁力計測量地磁場強度在升降裝置傾斜時，其計算結(jié)果也需要根據(jù)傾斜角進行補償。因此，需對多種傳感器進行數(shù)據(jù)融合，解算出更準確的姿態(tài)。

2 基于四元數(shù)的擴展卡爾曼濾波

擴展卡爾曼濾波 (extended kalman filter， EKF)是一種應(yīng)用最廣泛的非線性濾波方法。EKF采用圍繞上一步估計值Xk將非線性函數(shù)f(·)和h(·)展開成泰勒級數(shù)，并取一階近似來進行線性化，再利用濾波遞推方程進行系統(tǒng)的狀態(tài)估計。當系統(tǒng)具有較強的非線性或初始誤差較大時，濾波精度明顯下降，甚至造成濾波發(fā)散[17]。

EKF用于處理非線性離散時間過程，其同樣使用兩步法：預(yù)測-校正進行狀態(tài)估計。預(yù)測過程根據(jù)當前時刻的系統(tǒng)狀態(tài)獲得下一時刻的先驗估計；校正過程將觀測和先驗估計相結(jié)合獲得改進的系統(tǒng)后驗估計。

2.1 離散化

與其他典型非線性系統(tǒng)相同，柔性繩索自動升降裝置的系統(tǒng)狀態(tài)方程也可以表示為：

(12)

式(12)中:f(·)為多維向量函數(shù)，并且相對于其獨立變量都是非線性的；h(·)是一個非線性向量函數(shù)；W(t)和V(t)分別表示過程噪聲和觀測噪聲。

EKF算法中一個關(guān)鍵的部分就是狀態(tài)方程與觀測方程從非線性轉(zhuǎn)化為線性，最終變成一個離散的系統(tǒng)。為了使狀態(tài)變量成為線性的，可以將X(t+Δt)在X(t)處進行泰勒展開，考慮到實際情況，這里僅取二階近似值，有：

(13)

式(13)中：

忽略高階項，系統(tǒng)狀態(tài)方程的離散形式可以表示成如下的標準狀態(tài)空間形式，即：

(14)

這里假定W(k)和V(k)都是均值為0的高斯白噪聲，方差分別記作Qk、Rk。A(X(k))表述為：

(15)

2.2 建立系統(tǒng)過程方程

由于MEMS陀螺儀存在靜態(tài)漂移，故將姿態(tài)四元數(shù)和陀螺儀偏差作為EKF的狀態(tài)變量，記作：

(16)

柔性繩索自動升降裝置通常是由三軸陀螺儀來測量旋轉(zhuǎn)角速度，眾所周知的是，陀螺儀非常容易受到角速度噪聲以及溫度等的影響。本文采用如下的陀螺儀模型(這是Farrenkopf 在1974年提出的既簡單又實用陀螺模型[18])：

(17)

(18)

陀螺儀的角速度偏移也并非靜態(tài)常量，其表現(xiàn)為二階高斯白噪聲，表示為：

(19)

式(19)中：

根據(jù)式(7)和式(19)，可以得到柔性繩索自動升降裝置狀態(tài)方程的微分形式為：

(20)

在預(yù)測過程中，使用式(20)的簡化形式：

(21)

由于采樣時間非常短，在積分過程中認為陀螺儀偏移量為常值，可以得到：

(22)

并且

(23)

最終建立系統(tǒng)的狀態(tài)方程為：

(24)

2.3 建立離散測量方程

為了使狀態(tài)向量的測量更直觀，即使測量矩陣更簡單，本文選擇姿態(tài)四元數(shù)作為測量變量。因此測量向量為：

(25)

使用高斯牛頓算法[19]、或者求解Wahaba問題[20]，可以直接根據(jù)加速度計與磁力計的輸出計算出機體的四元數(shù)。但是由于地磁常量難以確定并且極易受到周圍鐵磁體的干擾，本文提出了一種向量正交化的方法，使用加速度計與磁力計的的測量值得到單位正交的旋轉(zhuǎn)矩陣，進而計算出機體的單位四元數(shù)。旋轉(zhuǎn)矩陣由三維向量i,j,k組成，分別代表機體的X,Y,Z三個體軸向量。因此有：

(26)

(27)

加速度計的輸出用于構(gòu)造機體坐標系的Z軸，將X軸強制與Z軸垂直，可以計算出i為：

(28)

由于選擇矩陣為單位正交矩陣，可以直接將k和i叉乘得到向量j為：

(29)

(30)

由此可得四元數(shù)的測量值為：

(31)

2.4 EKF的實現(xiàn)

EKF濾波算法的工作原理如圖2所示。

圖2 擴展卡爾曼濾波算法的工作原理框圖

根據(jù)式(24)和式(31)推導(dǎo)出的狀態(tài)向量和測量向量表達式以及EKF工作原理，可得基于四元數(shù)的擴展卡爾曼濾波器的五步法：

步驟1：估計先驗狀態(tài)。

(32)

步驟2：計算先驗誤差協(xié)方差矩陣。

P(k+1|k)=Φ(k)P(k|k)ΦT(k)+Qk

(33)

式(33)中：

Q(k)=E[W(k),W(k)T]

(34)

步驟3：計算卡爾曼增益。

K(k+1)=P(k+1|k)HT(k+1)×

(35)

式(35)中：

(36)

步驟4：估計后驗狀態(tài)。

{Z(k+1)-h[X(k+1|k)]}

(37)

步驟5：計算后驗誤差協(xié)方差矩陣。

P(k+1|k+1)=[I-K(k+1)H(k+1)]P(k+1|k)

(38)

3 仿真實驗

為了驗證上述算法的有效性，分別對升降裝置在靜態(tài)和動態(tài)下的傳感器原始數(shù)據(jù)進行了實驗。數(shù)據(jù)采樣頻率50 Hz。為了獲得升降裝置的參考位姿，使用二維轉(zhuǎn)臺的數(shù)據(jù)作為參考，其旋轉(zhuǎn)精度可達到0.1°。二維轉(zhuǎn)臺和升降裝置姿態(tài)檢測傳感器模塊如圖3所示。

圖3 二維轉(zhuǎn)臺及升降裝置姿態(tài)檢測傳感器

3.1 靜態(tài)測試

首先，將升降裝置姿態(tài)檢測傳感器模塊靜止放置在二維轉(zhuǎn)臺上，設(shè)置系統(tǒng)的采集頻率為100 Hz，采樣個數(shù)5 000點。加速度計和陀螺儀原始數(shù)據(jù)分別如圖4、圖5所示。

圖4 靜態(tài)加速度計測量數(shù)據(jù)曲線

圖5 靜態(tài)陀螺儀測量數(shù)據(jù)曲線

對數(shù)據(jù)進行分析后，得到橫滾角的樣本方差為0.003 24，俯仰角的樣本方差為0.000 991 3，偏航角的方差為0.241 37。

對數(shù)據(jù)進行分析后，得到橫滾角的樣本方差為3.068 26×10-7，俯仰角的樣本方差為2.535 02×10-7，偏航角的方差為9.759 27×10-7。

然后，將升降裝置載重250 kg懸置在半空中進行了懸停測試，懸置方向以普通的指南針為參考，升降裝置懸停測試如圖6所示。

圖6 升降裝置懸停測試

傳感器原始數(shù)據(jù)和姿態(tài)輸出信息分別如圖7～圖10所示。

圖7 加速度計數(shù)據(jù)曲線

圖8 磁力計數(shù)據(jù)曲線

圖9 陀螺儀數(shù)據(jù)曲線

圖10 姿態(tài)角輸出數(shù)據(jù)曲線

由圖7-圖9可以看出：傳感器在懸停狀態(tài)下的輸出與靜止狀態(tài)基本一致，但數(shù)據(jù)的動態(tài)范圍要大于靜止狀態(tài)，其原因在于，懸停狀態(tài)的升降裝置受吊繩垂直與水平方向的彈性形變影響，存在微振動現(xiàn)象。經(jīng)過濾波之后，由圖10可以看出：橫滾角、俯仰角和偏航角的偏差基本上控制在0.2°之內(nèi)，較為真實準確的反應(yīng)了升降裝置在懸停狀態(tài)下的姿態(tài)信息。同時采用了陀螺儀誤差的四元數(shù)狀態(tài)能夠通過擴展卡爾曼濾波有效的修正陀螺儀零點漂移帶來的累計誤差。

3.2 動態(tài)測試

設(shè)置轉(zhuǎn)臺分別在X軸和Y軸進行±2°和0～25°勻速旋轉(zhuǎn)，傳感器數(shù)據(jù)進行采集，并分別使用本文所提方法與梯度下降法、高斯牛頓算法GDA和未改進EKF以及兩軸轉(zhuǎn)臺所測得的數(shù)據(jù)進行對比，結(jié)果如圖11所示。

從圖11中顯示的效果看來，這3種算法都能夠有效地進行姿態(tài)估計。但本文所提算法在保持平滑前提下收斂速度也快于其他的EKF算法。

圖11 動態(tài)測試姿態(tài)角輸出數(shù)據(jù)

4 結(jié)論

提出了一種柔性繩索自動升降裝置姿態(tài)估計算法，使用姿態(tài)四元數(shù)表示柔性繩索自動升降裝置的姿態(tài)，并使用擴展卡爾曼濾波器(EKF)進行數(shù)據(jù)融合。靜態(tài)實驗和動態(tài)實驗驗證了所提方法的有效性，靜態(tài)偏差控制在0.2°之內(nèi)，且在保持平滑的前提下收斂速度快于傳統(tǒng)EKF算法。為研制基于姿態(tài)估計的柔性繩索自動升降裝置安全防墜系統(tǒng)提供了數(shù)據(jù)支撐。