考慮運(yùn)動加速度干擾的無人機(jī)姿態(tài)估計(jì)算法

劉旭航，劉小雄，章衛(wèi)國，楊 躍，郭一聰

(西北工業(yè)大學(xué) 自動化學(xué)院，西安 710072)

低成本無人機(jī)導(dǎo)航系統(tǒng)主要由基于微機(jī)電系統(tǒng)(Micro-electromechanical system,MEMS)的傳感器組成，局限于成本和功耗，噪聲誤差和偏移誤差導(dǎo)致陀螺儀的精度下降[1-2]，僅利用陀螺儀不能實(shí)現(xiàn)準(zhǔn)確的姿態(tài)估計(jì)，互補(bǔ)濾波被用來融合陀螺儀、加速度計(jì)和磁力計(jì)信息，獲得穩(wěn)定的姿態(tài)。近年來隨著傳感器技術(shù)的飛速發(fā)展，基于互補(bǔ)濾波的姿態(tài)估計(jì)算法在無人系統(tǒng)中得到了廣泛的應(yīng)用[3-5]。

傳統(tǒng)的互補(bǔ)濾波模型采用加速度計(jì)和陀螺儀作為輔助傳感器，利用陀螺儀和輔助傳感器的噪聲頻段互補(bǔ)的特性，減小噪聲信號對姿態(tài)估計(jì)的干擾，但是在動態(tài)環(huán)境下，載體具有較大的運(yùn)動加速度，運(yùn)動加速度對姿態(tài)估計(jì)的結(jié)果產(chǎn)生嚴(yán)重干擾，針對上述問題，國內(nèi)外研究人員提出了不同的姿態(tài)估計(jì)算法。李瑞涵等[6]提出一種加速梯度姿態(tài)融合方法，利用閾值切換的方法來抑制運(yùn)動加速度的干擾，該算法抑制運(yùn)動加速度干擾的能力優(yōu)于傳統(tǒng)的互補(bǔ)濾波算法，但并沒有給出運(yùn)動加速度準(zhǔn)確值。Euston等[7]計(jì)算向心加速度作為運(yùn)動加速度，排除運(yùn)動加速度對姿態(tài)估計(jì)的干擾，但是該方法未考慮切向加速度，對運(yùn)動加速度的估計(jì)仍不夠精確?？锩赳Y[8]設(shè)計(jì)了一種新型的垂直/水平歐拉角姿態(tài)解算方法，利用慣性器件、磁力計(jì)、全球定位系統(tǒng)(Global positioning system, GPS)和空速計(jì)的不同特性，獲得準(zhǔn)確的姿態(tài)信息。但是該算法中運(yùn)動加速度由更新頻率較低的GPS和空速計(jì)求得，系統(tǒng)誤差不能及時(shí)得到修正，姿態(tài)估計(jì)的精度不高。王鼎杰等[9]采用慣性導(dǎo)航系統(tǒng)(Inertial navigation system, INS)和GPS構(gòu)建組合導(dǎo)航系統(tǒng)，通過擴(kuò)展卡爾曼濾波(Extended Kalman filter, EKF)算法獲得實(shí)時(shí)的姿態(tài)信息和位置信息。但是該算法需要高精度且穩(wěn)定的量測信息，通常采用定位精度達(dá)到厘米級的差分GPS，但差分GPS體積大，且價(jià)格昂貴，不適合在低成本無人機(jī)導(dǎo)航系統(tǒng)中使用，單點(diǎn)GPS成本低，但精度較低，更新頻率較慢，在使用單點(diǎn)GPS的導(dǎo)航系統(tǒng)中，采用經(jīng)典的GPS/INS組合導(dǎo)航算法獲得的姿態(tài)角依然存在較大誤差。Lee等[10]設(shè)計(jì)了一種將姿態(tài)角和運(yùn)動加速度聯(lián)合估計(jì)的姿態(tài)估計(jì)算法，運(yùn)動加速度被建模成一階低通白噪聲過程，能夠在短加速周期內(nèi)精確估計(jì)運(yùn)動加速度，但是涉及數(shù)值較大的外部加速度時(shí)，該算法不能實(shí)現(xiàn)對運(yùn)動加速度的準(zhǔn)確估計(jì)。

現(xiàn)有的無人機(jī)姿態(tài)估計(jì)算法存在精度低，無法精確估計(jì)運(yùn)動加速度的缺點(diǎn)，為解決動態(tài)環(huán)境下低成本無人機(jī)姿態(tài)估計(jì)易受傳感器噪聲和運(yùn)動加速度干擾的問題，本文設(shè)計(jì)了一種考慮運(yùn)動加速度干擾的姿態(tài)估計(jì)算法。首先，建立運(yùn)動加速度估計(jì)模型，該模型分別從加速度計(jì)和GPS速度中求取運(yùn)動加速度，利用獲得的運(yùn)動加速度對加速度計(jì)原始值進(jìn)行修正，從而排除運(yùn)動加速度對姿態(tài)估計(jì)的干擾。然后，建立基于互補(bǔ)濾波的姿態(tài)估計(jì)模型，利用修正后的加速度信息和磁力計(jì)信息作為參考向量，構(gòu)建誤差量作為互補(bǔ)濾波器的輸入，互補(bǔ)濾波器的輸出為修正后的陀螺儀角速度，通過基于互補(bǔ)濾波的姿態(tài)估計(jì)模型抑制傳感器噪聲干擾，提升姿態(tài)估計(jì)的精度。最后，利用無人機(jī)試飛過程中采集的傳感器數(shù)據(jù)，設(shè)計(jì)對比實(shí)驗(yàn)驗(yàn)證本文所設(shè)計(jì)算法的有效性。

1 運(yùn)動加速度估計(jì)模型

1.1 模型框架

圖1 運(yùn)動加速度估計(jì)模型框架Fig.1 Framework of motion acceleration estimation model

由于卡爾曼濾波器只能在短時(shí)間精確估計(jì)運(yùn)動加速度ae，誤差的累積導(dǎo)致ae的精度逐漸下降。因此，引入外部運(yùn)動加速度ag，ag由GPS速度信息差分獲得，ag的數(shù)值較為穩(wěn)定，但是由于外部傳感器GPS的更新頻率遠(yuǎn)低于導(dǎo)航系統(tǒng)姿態(tài)更新的頻率，單獨(dú)使用外部傳感器估計(jì)運(yùn)動加速度會導(dǎo)致姿態(tài)估計(jì)的精度下降。因此，在運(yùn)動加速度估計(jì)模型中，同時(shí)利用ae和ag對加速度計(jì)原始值進(jìn)行修正，提升姿態(tài)估計(jì)的精度和抗干擾能力。

1.2 誤差運(yùn)動加速度

Xk=Fk,k-1Xk-1+wk

(1)

式中：Fk,k-1為狀態(tài)轉(zhuǎn)移矩陣，wk為噪聲分量。

(2)

(3)

(4)

式中，ΔR( )為旋轉(zhuǎn)角度為θε,k旋轉(zhuǎn)矩陣。

由式(4)，系統(tǒng)量測方程為

(5)

由于濾波后得到運(yùn)動加速度誤差aε和姿態(tài)角誤差θε分別對運(yùn)動加速度和姿態(tài)角進(jìn)行了修正，修正后的誤差量為零，因此，相鄰時(shí)刻的狀態(tài)變量不相關(guān)，更新后的狀態(tài)一步預(yù)測等于零，狀態(tài)一步預(yù)測方程簡化為

(6)

狀態(tài)估計(jì)方程簡化為

(7)

濾波增益為

(8)

一步預(yù)測均方誤差為

Pk/k-1=Φk,k-1Pk-1Φk,k-1+Qk=Qk

(9)

估計(jì)均方誤差為

Pk=(I-KkHk)Pk/k-1

(10)

(11)

1.3 外部運(yùn)動加速度

在低成本無人機(jī)導(dǎo)航系統(tǒng)中，GPS更新周期慢于加速度計(jì)更新周期，設(shè)加速度計(jì)更新周期為T1,GPS更新周期為T2，T1和T2通常存在倍數(shù)關(guān)系，即T2=nT1，其中，n為正整數(shù)。

(12)

(13)

式中，dt為加速度更新周期T1。

(14)

(15)

2 基于互補(bǔ)濾波的姿態(tài)估計(jì)

由于陀螺儀噪聲主要存在于低頻段，加速度計(jì)和磁力計(jì)噪聲主要在高頻段，因此，設(shè)計(jì)互補(bǔ)濾波器濾除噪聲的干擾。設(shè)陀螺儀輸出值為ωg，由修正加速度和磁力計(jì)得到的角速度修正值為ωc，載體角速度真實(shí)值為ω，則有如下關(guān)系:

(16)

式中：ng為陀螺儀低頻噪聲，nc為加速度計(jì)和磁力計(jì)的高頻噪聲。在互補(bǔ)濾波器中，對ωg應(yīng)用高通濾波器H，對ωc應(yīng)用低通濾波器L，即

ω=L×ωc+H×ωg=
(L+H)×ω+L×nc+H×ng

(17)

由低通濾波器和高通濾波器性質(zhì)，有如下關(guān)系:

(18)

濾波后的角速度為

ω=(L+H)×ω

(19)

低通濾波和高通濾波的傳遞函數(shù)如下所示[13]：

(20)

(21)

L(s)與H(s)的和為1，說明經(jīng)過濾波后的角速度與真實(shí)角速度相同，且不包含傳感器噪聲。

由于角速度修正值ωc來自修正加速度和磁力計(jì)，含有高頻噪聲，因此控制器一般不應(yīng)含有微分項(xiàng)，通常采用如下形式比例積分(Proportional-integral, PI)控制器：

(22)

式中，KP、KI分別為比例和微分系數(shù)，其選值受傳感器參數(shù)和載體運(yùn)動狀況的影響[14]。

基于互補(bǔ)濾波的姿態(tài)估計(jì)算法采用來自修正加速度和磁力計(jì)的信息構(gòu)建誤差量，并將該誤差量作為PI控制器的輸入，PI控制器的輸出為角速度修正值ωc，ωc被用來消除由陀螺儀漂移和偏差帶來的累積誤差，姿態(tài)角由修正后的角速度通過方向余弦法解算?；诨パa(bǔ)濾波的姿態(tài)估計(jì)算法的框架如圖2所示。

圖2 姿態(tài)估計(jì)算法框架圖Fig.2 Block diagram of attitude estimation algorithm

2.1 方向余弦法

方向余弦矩陣的微分方程為

(23)

其中

由式(23)，當(dāng)時(shí)間間隔dt趨向于無窮小時(shí)，方向余弦矩陣更新可以表達(dá)為如下形式:

(24)

式(24)利用陀螺信號實(shí)現(xiàn)了方向余弦矩陣的實(shí)時(shí)更新，利用方向余弦矩陣可以求出姿態(tài)角，姿態(tài)角與方向余弦矩陣的關(guān)系如下所示:

(25)

2.2 誤差量的計(jì)算

采用參考向量和觀測向量做外積的方式計(jì)算誤差量，誤差量由修正俯仰角和滾轉(zhuǎn)角的誤差量e1和修正偏航角的誤差量e2組成。

(26)

單位化的修正加速度g2為與參考向量g1相同大小和方向的另一向量，利用運(yùn)動加速度估計(jì)模型計(jì)算修正加速度gb，在GPS信號未更新的時(shí)刻，gb為由誤差運(yùn)動加速度修正得到，GPS信號更新的時(shí)刻，gb由外部運(yùn)動加速度修正得到，即

(27)

對修正加速度單位化處理:

(28)

g2與參考向量g1做叉乘，求出誤差向量e1。

e1=g1×g2

(29)

(30)

在n系的XOZ平面上和Z軸上，磁力計(jì)測量值應(yīng)為同一組模值[15]，即

(31)

(32)

誤差量e2由參考向量m1和磁力計(jì)輸出值m2叉乘獲得:

e2=m1×m2

(33)

總的誤差量為e1與e2的和:

e=e1+e2

(34)

2.3 PI控制器

誤差量e為PI控制器的輸入，PI控制器的輸出量存在ωP和ωI兩個(gè)分量，分別為：

ωP=KPe

(35)

ωI=ωI+KIe·dt

(36)

式中dt為方向余弦矩陣更新的周期。

綜上所述，由修正加速度和磁力計(jì)得到的角速度修正值ωc為

ωc=ωP+ωI

(37)

3 實(shí) 驗(yàn)

3.1 傳感器數(shù)據(jù)采集

進(jìn)行無人機(jī)飛行試驗(yàn)，利用搭載于固定翼無人機(jī)的Ellipse2-N系統(tǒng)采集所需要的傳感器信息，Ellipse2-N系統(tǒng)(如圖3所示)包含了基于MEMS的慣性測量單元和GPS接收機(jī)，慣性測量單元中集成了陀螺儀、加速度計(jì)和磁力計(jì)。表1為Ellipse2-N系統(tǒng)的參數(shù)特性。

圖3 Ellipse2-N慣性導(dǎo)航系統(tǒng)Fig.3 Ellipse2-N inertial navigation system

表1 Ellipse2-N參數(shù)特性Tab.1 Parameter characteristics of Ellipse2-N

利用Ellipse2-N系統(tǒng)采集陀螺儀、加速度計(jì)、磁力計(jì)和GPS速度信息。參考姿態(tài)為Ellipse2-N系統(tǒng)輸出的姿態(tài)，其滾轉(zhuǎn)角和俯仰角的誤差均方根均小于0.1°，偏航角的誤差均方根小于0.5°。GPS接收機(jī)的速度更新頻率為10 Hz，精度為0.1 m/s。采集時(shí)長為80 s的實(shí)驗(yàn)數(shù)據(jù)，初始滾轉(zhuǎn)角為-23.60°，俯仰角為0.8°，偏航角為-80.6°。陀螺儀、加速度計(jì)和磁力計(jì)的采集頻率均為50 Hz，GPS速度的采集頻率為10 Hz，無人機(jī)在該段時(shí)間內(nèi)盤旋飛行。

3.2 運(yùn)動加速度估計(jì)模型的驗(yàn)證

首先利用時(shí)長為3 s的傳感器數(shù)據(jù)進(jìn)行仿真實(shí)驗(yàn)，驗(yàn)證運(yùn)動加速度估計(jì)模型的估計(jì)效果。ga為加速度計(jì)輸出值的模值，go是僅使用外部運(yùn)動加速度修正后的得到的加速度的模值，ge為僅使用誤差運(yùn)動加速度修正后得到的加速度的模值，gp為使用運(yùn)動加速度估計(jì)模型獲得的加速度的模值。gl為當(dāng)?shù)刂亓铀俣鹊哪Ｖ?。圖4為加速度模值的對比圖，表2為由不同算法得到的加速度模值的統(tǒng)計(jì)特性。

圖4 加速度模值對比圖Fig.4 Comparison of acceleration modulus

表2 加速度模值的統(tǒng)計(jì)特性對比Tab.2 Comparison of characteristics of acceleration modulus(m·s-2)

觀察圖4，加速度計(jì)輸出值的模值ga變化區(qū)間為[10.2,11.9]，大于當(dāng)?shù)刂亓铀俣萭l，說明載體在該段時(shí)間內(nèi)具有運(yùn)動加速度。由于GPS更新周期較長，go數(shù)值跳變劇烈且更新頻率較慢，ge在前1 s內(nèi)數(shù)值接近gp，隨后ge偏離gl的程度迅速加大，這是由于誤差運(yùn)動加速度僅能在短時(shí)間內(nèi)準(zhǔn)確估計(jì)運(yùn)動加速度，gp數(shù)值偏離gl的程度最小。

采用平均絕對誤差(Mean absolute error, MAE)和均方根誤差(Root mean squared error, RMSE)來衡量觀測值同真值之間的偏差，觀察表2，gp所對應(yīng)的的MAE值和RMSE值最小。

綜上所述，利用運(yùn)動加速度估計(jì)模型對加速度計(jì)原始值修正后得到的修正加速度最接近真實(shí)的重力加速度，運(yùn)動加速度估計(jì)模型對載體運(yùn)動加速度估計(jì)的精度最高，可以有效抑制動態(tài)環(huán)境下運(yùn)動加速度對加速度計(jì)測量值的干擾。

3.3 考慮運(yùn)動加速度的姿態(tài)估計(jì)算法驗(yàn)證

利用時(shí)長為80 s的傳感器數(shù)據(jù)對所設(shè)計(jì)的考慮運(yùn)動加速度干擾的姿態(tài)估計(jì)算法進(jìn)行驗(yàn)證。參考姿態(tài)角來源于Ellipse2-N慣性導(dǎo)航系統(tǒng)，記為RE，本文所設(shè)計(jì)算法得到的姿態(tài)角記為PR。圖5為加速度計(jì)輸出模值，圖6為姿態(tài)角對比圖。

圖5 加速度計(jì)輸出模值Fig.5 Output modulus of accelerometer

圖6 姿態(tài)角對比圖Fig.6 Comparison of attitude angle

圖5中加速度計(jì)輸出模值變化區(qū)間在[9.3, 12.9]，表明載體具有運(yùn)動加速度。由圖6可知，本文所設(shè)計(jì)的算法估計(jì)得到的滾轉(zhuǎn)角和俯仰角與參考值之間的誤差均小于1°，偏航角與參考值之間的誤差小于2°。因此，在具有運(yùn)動加速度干擾的高動態(tài)環(huán)境下，本文所設(shè)計(jì)的姿態(tài)估計(jì)算法能夠給出穩(wěn)定可靠的姿態(tài)。

3.4 不同姿態(tài)估計(jì)算法的性能比較

分別采用不考慮運(yùn)動加速度的互補(bǔ)濾波姿態(tài)估計(jì)算法和基于EKF的GPS/INS組合導(dǎo)航算法，以及本文所設(shè)計(jì)的算法，同時(shí)處理時(shí)長為50 s的傳感器數(shù)據(jù)，將不考慮運(yùn)動加速度的互補(bǔ)濾波姿態(tài)估計(jì)算法的估計(jì)值記為CF，GPS/INS組合導(dǎo)航算法的估計(jì)值記為EKF，考慮運(yùn)動加速度的姿態(tài)估計(jì)算法的估計(jì)值記為PR。圖7為加速度計(jì)輸出模值，圖8為姿態(tài)角誤差對比圖所示，表3為3種算法的誤差統(tǒng)計(jì)特性。

圖7 加速度計(jì)輸出模值Fig.7 Output modulus of accelerometer

圖8 姿態(tài)角誤差對比圖Fig.8 Comparison of attitude angle error

圖7中加速度計(jì)輸出模值變化區(qū)間為[9.3，12.2]，即載體在該段時(shí)間內(nèi)具有運(yùn)動加速度。由圖8可知，在存在運(yùn)動加速度干擾的情況下，考慮運(yùn)動加速度的姿態(tài)估計(jì)算法的俯仰角和滾轉(zhuǎn)角的誤差值均小于1.2°，偏航角誤差值小于1.3°。原始的互補(bǔ)濾波算法由于運(yùn)動加速度的干擾，俯仰角誤差和滾轉(zhuǎn)角誤差分別達(dá)到7.6°和4.8°。偏航角誤差達(dá)2.1°，且呈發(fā)散趨勢。基于EKF的組合導(dǎo)航算法受限于單點(diǎn)GPS速度的較差的性能，其俯仰角誤差值和滾轉(zhuǎn)角誤差值與本文所提出算法的對應(yīng)的誤差值類似，但偏航角誤差值大于1.6°，遠(yuǎn)大于本文所提出算法的偏航角誤差值。

進(jìn)一步觀察表3中不同算法的姿態(tài)角誤差的統(tǒng)計(jì)特性，本文所設(shè)計(jì)算法姿態(tài)估計(jì)的平方絕對誤差和均方根誤差均小原始互補(bǔ)濾波算法和基于EKF組合導(dǎo)航算法所對應(yīng)的平方絕對誤差和均方根誤差，即本文所設(shè)計(jì)算法姿態(tài)估計(jì)的性能優(yōu)于原始互補(bǔ)濾波算法和基于EKF的組合導(dǎo)航算法，該算法能夠有效提升無人機(jī)導(dǎo)航系統(tǒng)在動態(tài)環(huán)境下姿態(tài)估計(jì)的精度。

表3 姿態(tài)角誤差的統(tǒng)計(jì)特性對比Tab.3 Comparison of characteristics of attitude angle error (°)

4 結(jié) 論

1)動態(tài)環(huán)境下，無人機(jī)機(jī)動所產(chǎn)生的運(yùn)動加速度和低成本傳感器固有的噪聲對導(dǎo)航系統(tǒng)姿態(tài)估計(jì)存在嚴(yán)重的干擾，針對這一問題提出了一種考慮運(yùn)動加速度干擾的姿態(tài)估計(jì)算法。

2)該算法首先利用運(yùn)動加速度估計(jì)模型精確估計(jì)運(yùn)動加速度，避免運(yùn)動加速度對姿態(tài)估計(jì)的干擾。然后構(gòu)建基于互補(bǔ)濾波的姿態(tài)估計(jì)模型，進(jìn)一步減小陀螺儀的噪聲信號對姿態(tài)估計(jì)的干擾，實(shí)現(xiàn)準(zhǔn)確的姿態(tài)輸出。

3)實(shí)驗(yàn)結(jié)果表明，所提出算法能夠準(zhǔn)確估計(jì)無人機(jī)運(yùn)動加速度，減弱運(yùn)動加速度和傳感器噪聲對姿態(tài)估計(jì)的干擾，該算法在提高低成本無人機(jī)導(dǎo)航系統(tǒng)姿態(tài)估計(jì)的精度和抗干擾能力方面具有較高的實(shí)用價(jià)值。