蔡安江,劉凱峰,郭師虹,舒 展

(1.西安建筑科技大學(xué)機(jī)電工程學(xué)院,陜西西安 710055;2.西安建筑科技大學(xué)土木工程學(xué)院,陜西西安 710055)

1 引言

多旋翼無人機(jī)姿態(tài)估計(jì)是多旋翼無人機(jī)姿態(tài)控制、導(dǎo)航定位的基礎(chǔ),姿態(tài)估計(jì)的精度直接影響著姿態(tài)控制和導(dǎo)航定位的精度.考慮到尺寸、功耗以及成本等原因,多旋翼無人機(jī)通常不會(huì)使用傳統(tǒng)的體積和重量比較大的高精度的傳感器,而是使用價(jià)格相對(duì)低廉的微機(jī)電系統(tǒng)(micro-electro-mechanical system,MEMS)慣性傳感器來進(jìn)行姿態(tài)估計(jì).但是目前的MEMS慣性傳感器相較于軍用級(jí)別的高精度慣性傳感器來說精度低、穩(wěn)定性較差并且其系統(tǒng)誤差隨時(shí)間積累較快,其測量值無法滿足多旋翼無人機(jī)姿態(tài)控制的要求.同時(shí)多旋翼無人機(jī)常使用低成本的單片機(jī)或者微控制器作為控制單元,整個(gè)無人機(jī)控制系統(tǒng)需具有較強(qiáng)的實(shí)時(shí)性.因此,必須采用合理的姿態(tài)估計(jì)算法來處理傳感器的數(shù)據(jù),所以設(shè)計(jì)精度高、迭代速度快且性能穩(wěn)定的姿態(tài)估計(jì)算法一直是多旋翼無人機(jī)領(lǐng)域的研究難點(diǎn)和熱點(diǎn)之一.

低成本慣性陀螺存在較大的零點(diǎn)漂移,直接對(duì)陀螺儀數(shù)據(jù)進(jìn)行積分會(huì)使得姿態(tài)角發(fā)散,因此通常使用加速度計(jì)數(shù)據(jù)作為參考去估計(jì)橫滾角和俯仰角,使用磁力計(jì)數(shù)據(jù)作為參考去估計(jì)航向角.低成本陀螺儀、加速度計(jì)和磁力計(jì)測量值都包含較大的噪聲,現(xiàn)有研究人員對(duì)于如何使用存在噪聲干擾的陀螺儀、加速度計(jì)和磁力計(jì)數(shù)據(jù)獲得精確的姿態(tài)角的問題進(jìn)行了大量的研究[1–2].由于四元數(shù)能進(jìn)行全姿解算并具有較小的計(jì)算量,卡爾曼濾波器能有效的降低噪聲,均被廣泛用于無人機(jī)姿態(tài)解算中[3–5].由于姿態(tài)估計(jì)模型具有非線性的特征,通常使用非線性卡爾曼濾波器來處理[6–8].

劉興川等[9]提出一種基于四元數(shù)的擴(kuò)展卡爾曼濾波(extended Kalman filter,EKF)算法來降低陀螺儀、加速度計(jì)和磁力計(jì)的測量噪聲,但并未考慮加速度計(jì)數(shù)據(jù)會(huì)對(duì)航向角的估計(jì)造成干擾以及磁力計(jì)數(shù)據(jù)會(huì)對(duì)橫滾角和俯仰角的估計(jì)造成干擾的問題.針對(duì)這個(gè)問題,Suh等[10–11]在基于四元數(shù)的擴(kuò)展卡爾曼濾波器基礎(chǔ)上,將加速度計(jì)和磁力計(jì)數(shù)據(jù)分為兩個(gè)階段進(jìn)行處理,提出一種二階段EKF濾波算法,以降低磁力計(jì)數(shù)據(jù)對(duì)橫滾角和俯仰角估計(jì)的干擾,但是該算法需要改變卡爾曼濾波增益系數(shù),這樣會(huì)使得卡爾曼濾波器失去最優(yōu)性.Valenti等[12]在二階段EKF濾波算法的基礎(chǔ)上提出一種代數(shù)四元數(shù)算法,將狀態(tài)四元數(shù)分為兩個(gè)輔助四元數(shù)以降低磁力計(jì)數(shù)據(jù)對(duì)橫滾角和俯仰角估計(jì)的干擾.但在算法推導(dǎo)過程中,存在一些簡化過程,影響算法的可靠性;且在算法實(shí)現(xiàn)過程中加入了一些外部計(jì)算和協(xié)方差估計(jì)過程,增加了整個(gè)算法的計(jì)算量.同時(shí),目前大多數(shù)姿態(tài)估計(jì)算法均是在EKF基礎(chǔ)上設(shè)計(jì)的姿態(tài)估計(jì)算法.EKF是針對(duì)非線性系統(tǒng)的一種最廣泛的狀態(tài)估計(jì)方法,但EKF僅僅是將非線性方程使用泰勒公式展開的一階近似,在系統(tǒng)存在較強(qiáng)的非線性時(shí),EKF算法存在較大的截?cái)嗾`差.無跡卡爾曼濾波(unscented Kalman filter,UKF)也是卡爾曼濾波的延伸,它降低了擴(kuò)展卡爾曼濾波線性化帶來的誤差,與EKF相比,UKF帶來了巨大的改進(jìn),Garcia,Cao等[13–14]就曾將UKF應(yīng)用于小型衛(wèi)星和航天器的姿態(tài)估計(jì)中.雖然UKF減小了線性化誤差,擁有較高的狀態(tài)估計(jì)精度,但UKF 計(jì)算過程中的無跡變換(unscented transformation,UT)會(huì)帶來大量的計(jì)算,這給使用低成本處理器的多旋翼無人機(jī)實(shí)時(shí)解算帶來很大的困難.

本文從四元數(shù)本質(zhì)考慮,推測四元數(shù)各元素分別包含著不同的姿態(tài)角信息,通過仿真實(shí)驗(yàn)證明了該推測,由此設(shè)計(jì)了一種可以分離干擾的二段式算法,且針對(duì)傳統(tǒng)UKF算法計(jì)算量大的問題,參考了一種相對(duì)計(jì)算量較小的衍生無跡卡爾曼濾波算法(derivative unscented Kalman filter,DUKF)[15–16],提出了一種基于四元數(shù)DUKF的二段式多旋翼姿態(tài)估計(jì)算法.最后本文通過提取靜止?fàn)顟B(tài)PIXHAWK飛控中的慣性傳感器和磁力計(jì)數(shù)據(jù),進(jìn)行仿真實(shí)驗(yàn),驗(yàn)證了本文所提算法的有效性.

2 基于四元數(shù)的姿態(tài)估計(jì)系統(tǒng)模型

2.1 姿態(tài)估計(jì)系統(tǒng)的狀態(tài)方程

則可以建立連續(xù)時(shí)間的狀態(tài)方程為[17]

其中

為了方便迭代計(jì)算,將連續(xù)時(shí)間的狀態(tài)方程轉(zhuǎn)換為離散時(shí)間狀態(tài)方程,令A(yù)T=T為陀螺儀數(shù)據(jù)采樣間隔,對(duì)連續(xù)時(shí)間方程離散化:

令A(yù)k=(I+ATT),則有離散時(shí)間狀態(tài)方程為

其中:I為四階單位矩陣;為了描述方便,本文后續(xù)內(nèi)容中符號(hào)將用q代替.

2.2 姿態(tài)估計(jì)系統(tǒng)的量測方程

本文采用三軸加速度計(jì)和三軸磁力計(jì)數(shù)據(jù)來對(duì)四元數(shù)狀態(tài)向量進(jìn)行校正,校正過程分兩個(gè)階段進(jìn)行,須建立兩個(gè)量測方程.

2.2.1 基于三軸加速度計(jì)數(shù)據(jù)的量測方程的建立

三軸加速度計(jì)可測得重力場在機(jī)體坐標(biāo)系下的3個(gè)分量,重力場在地球坐標(biāo)系的3個(gè)分量為[0 0|g|]T,其中|g|為重力加速度的絕對(duì)值,則有基于三軸加速度計(jì)數(shù)據(jù)的量測方程為[18]

2.2.2 基于三軸磁力計(jì)數(shù)據(jù)的量測方程的建立

三軸磁力計(jì)可測得地磁場在機(jī)體坐標(biāo)系下的3個(gè)分量,假設(shè)當(dāng)?shù)卮艌鲈诘厍蜃鴺?biāo)系的3 個(gè)分量為[bxbybz]T,則有基于三軸磁力計(jì)數(shù)據(jù)的量測方程為

3 基于四元數(shù)DUKF的二段式姿態(tài)估計(jì)算法

3.1 衍生無跡卡爾曼濾波算法(DUKF)

假設(shè)非線性系統(tǒng)狀態(tài)方程和量測方程為

針對(duì)上述狀態(tài)空間模型的濾波問題,由于其狀態(tài)方程具有線性特征,文獻(xiàn)[15]提出一種衍生無跡卡爾曼濾波算法(DUKF).與標(biāo)準(zhǔn)的無跡卡爾曼濾波算法(UKF)相比,DUKF在時(shí)間更新的過程中采用與卡爾曼濾波(Kalman filter,KF)相同的形式,這樣可以簡化計(jì)算過程,從而避免標(biāo)準(zhǔn)UKF因無跡變換(UT)帶來的額外計(jì)算量;而在量測更新的過程中則使用無跡變換,以獲得與UKF算法相同的非線性濾波精度.因此與UKF算法相比,DUKF算法在保持精度的同時(shí),很好的縮短了算法每個(gè)周期的迭代時(shí)間.DUKF算法的具體步驟可參考文獻(xiàn)[15–16].

3.2 二段式四元數(shù)狀態(tài)向量校正策略

無人機(jī)姿態(tài)估計(jì)中,加速度計(jì)數(shù)據(jù)用來校正由陀螺儀漂移造成的姿態(tài)角累積誤差,但由于加速度計(jì)的數(shù)據(jù)不能用于測量航向角,通常引入磁力計(jì)來校正航向角的估計(jì).但這樣導(dǎo)致傳感器配置冗余,磁力計(jì)的數(shù)據(jù)不僅可以測得航向角,對(duì)橫滾角、俯仰角也會(huì)有影響,同時(shí),錯(cuò)誤的加速度計(jì)數(shù)據(jù)也會(huì)影響航向角的估計(jì)[12].橫滾角和俯仰角的估計(jì)的精確度是無人機(jī)穩(wěn)定飛行的基礎(chǔ),在無人機(jī)作業(yè)過程中可能會(huì)出現(xiàn)一些電磁干擾污染磁力計(jì)數(shù)據(jù),以至于嚴(yán)重影響橫滾角、俯仰角的估計(jì),造成無人機(jī)失穩(wěn)甚至墜機(jī).因此,消除磁力計(jì)數(shù)據(jù)對(duì)于橫滾角、俯仰角估計(jì)的影響尤為重要,本文通過對(duì)衍生無跡卡爾曼濾波過程中的四元數(shù)校正向量進(jìn)行處理來解決這一問題.

如式(15)所述,衍生無跡卡爾曼濾波使用殘差乘以卡爾曼濾波增益系數(shù)得到相應(yīng)的校正向量來校正先驗(yàn)估計(jì)值,

其中:a代表旋轉(zhuǎn)角度,e=[exeyez]代表旋轉(zhuǎn)軸.使用四元數(shù)表示姿態(tài)角時(shí),四元數(shù)中有一個(gè)元素是冗余的,從式(17)可以看出,p1,p2,p3已經(jīng)代表了旋轉(zhuǎn)軸和旋轉(zhuǎn)角度,元素p0是冗余的,p0的存在是為了保證四元數(shù)p為單位四元數(shù),即

p1,p2,p3三個(gè)元素分別與繞x軸、y軸和z軸的旋轉(zhuǎn)有關(guān),由此可設(shè)想p1主要包含橫滾角信息,p2主要包含俯仰角信息,p3主要包含航向角信息,這樣可以在量測更新階段中求得校正向量?x后,保留需要的信息,去掉不需要的信息以消除干擾.現(xiàn)有姿態(tài)估計(jì)算法常將三軸加速度計(jì)數(shù)據(jù)和三軸磁力計(jì)數(shù)據(jù)作為一個(gè)六維量測向量進(jìn)行姿態(tài)解算,這樣計(jì)算出的校正向量耦合了加速度計(jì)和磁力計(jì)的數(shù)據(jù),使得橫滾角、俯仰角和航向角的估計(jì)均受到這兩種傳感器數(shù)據(jù)的影響,無法分離相互間的干擾.為消除相互干擾,本文將加速度計(jì)和磁力計(jì)數(shù)據(jù)分為兩個(gè)量測更新階段處理.第1階段中,由加速度計(jì)數(shù)據(jù)計(jì)算出的四元數(shù)校正向量乘以相應(yīng)的校正系數(shù)得到修改后的校正向量,以去除原來校正向量中關(guān)于校正航向角的信息,消除加速度數(shù)據(jù)對(duì)航向角的影響,修改后的校正向量被代入式(15)中計(jì)算得到經(jīng)過加速度計(jì)數(shù)據(jù)校正的第1階段后驗(yàn)狀態(tài)估計(jì)值,完成第1階段量測校正;第2階段中,由磁力計(jì)數(shù)據(jù)計(jì)算出的四元數(shù)校正向量經(jīng)過乘以相應(yīng)的校正系數(shù)的得到修改后的校正向量,以去除校正橫滾角、俯仰角的信息,消除對(duì)橫滾角和俯仰角的影響,修改后的校正向量和第1階段后驗(yàn)狀態(tài)估計(jì)值也被代入式(15)中計(jì)算得到經(jīng)過磁力計(jì)數(shù)據(jù)校正的第2階段后驗(yàn)狀態(tài)估計(jì)值,完成第2階段量測校正.這樣使得加速度計(jì)數(shù)據(jù)只用以校正橫滾角、俯仰角,磁力計(jì)數(shù)據(jù)只用以校正航向角,達(dá)到分離相互干擾的目的.由此,提出一種基于四元數(shù)的DUKF二段式姿態(tài)估計(jì)算法,算法具體實(shí)施過程如第2.3節(jié)所示.

3.3 基于四元數(shù)DUKF 的二段式姿態(tài)估計(jì)算法(two-step DUKF,TDUKF)

結(jié)合前面的系統(tǒng)狀態(tài)方程和量測方程,以姿態(tài)四元數(shù)為狀態(tài)向量的非線性系統(tǒng)為

其中:qk為k時(shí)刻系統(tǒng)的四元數(shù)狀態(tài)向量;z1k和z2k分別為k時(shí)刻三軸加速度計(jì)數(shù)據(jù)和三軸磁力計(jì)數(shù)據(jù)組成的量測向量;Ak?1為式(7)中的狀態(tài)轉(zhuǎn)移矩陣;wk,v1k和v2k為互不相關(guān)的零均值高斯白噪聲過程,其方差分別為

則基于四元數(shù)DUKF的二段式姿態(tài)估計(jì)算法流程為

a)系統(tǒng)初始化.

設(shè)置初始狀態(tài)四元數(shù)為q0=[1 0 0 0]T,初始狀態(tài)誤差協(xié)方差陣為

計(jì)算系統(tǒng)先驗(yàn)估計(jì).

1)讀取三軸陀螺儀數(shù)據(jù),獲得wx,wy和wz,將其代入式(4)中計(jì)算得到?.

2)計(jì)算離散時(shí)間狀態(tài)轉(zhuǎn)移矩陣.

3)時(shí)間更新.

由于系統(tǒng)狀態(tài)方程具有線性特征,系統(tǒng)先驗(yàn)狀態(tài)估計(jì)值及其誤差協(xié)方差陣的計(jì)算與線性卡爾曼濾波相同,則k ?1時(shí)刻系統(tǒng)先驗(yàn)狀態(tài)估計(jì)值為

k ?1時(shí)刻系統(tǒng)先驗(yàn)狀態(tài)估計(jì)誤差協(xié)方差為

4)Sigma點(diǎn)選取.

根據(jù)先驗(yàn)狀態(tài)估計(jì)值及其誤差協(xié)方差選擇一組加權(quán)Sigma點(diǎn),選取的Sigma點(diǎn)為

b)開始第1階段校正.

1)第1階段量測更新.

經(jīng)過非線性量測方程hg(·)變換后的Sigma點(diǎn)為

2)計(jì)算量測預(yù)測值及其協(xié)方差

計(jì)算先驗(yàn)狀態(tài)估計(jì)值與量測預(yù)測值間的互協(xié)方差

3)確定卡爾曼增益矩陣.

4)讀取加速度計(jì)數(shù)據(jù)zk1=[axayaz]T.

5)計(jì)算第1階段校正向量

其中:qa1為與?q1維度相同的校正系數(shù),⊙表示向量間對(duì)應(yīng)元素相乘的運(yùn)算符.

c)開始第2階段校正.

1)第2階段量測更新.

經(jīng)過非線性量測方程hm(·)變換后的Sigma點(diǎn)為

2)計(jì)算量測預(yù)測值及其協(xié)方差

計(jì)算先驗(yàn)狀態(tài)估計(jì)值與量測預(yù)測值間的互協(xié)方差

其中:

3)確定卡爾曼增益矩陣

4)讀取磁力計(jì)數(shù)據(jù)zk2=[mxmymz]T.

5)計(jì)算第2階段校正向量

其中:qa2為與?q2維度相同的校正系數(shù),⊙表示向量間對(duì)應(yīng)元素相乘的運(yùn)算符.

d)進(jìn)入下一個(gè)迭代周期,重復(fù)步驟2)–4)的過程.

圖1 TDUKF算法結(jié)構(gòu)框圖Fig.1 TDUKF algorithm block diagram

3.4 狀態(tài)四元數(shù)校正系數(shù)的選取

為證明第2.2節(jié)中四元數(shù)狀態(tài)向量校正策略的可行性,本文將qa1和qa2按不同取值分為表1中的7種情況,通過從水平放置的PIXHAWK飛控中提取陀螺儀、加速度計(jì)及磁力計(jì)數(shù)據(jù),利用MATLAB軟件進(jìn)行仿真實(shí)驗(yàn),仿真實(shí)驗(yàn)結(jié)果如圖2所示.如圖2(a)所示,只使用陀螺儀數(shù)據(jù)更新姿態(tài)角時(shí),由于陀螺儀漂移的存在,更新的姿態(tài)角會(huì)隨著時(shí)間發(fā)散;圖2(b)–2(c)與圖2(a)出現(xiàn)了相同的現(xiàn)象,表明?q1和?q2的第1個(gè)元素并未影響姿態(tài)四元數(shù),即其不包含姿態(tài)信息;圖2(d)中俯仰角和航向角發(fā)散,但橫滾角收斂;圖2(e)中橫滾角和航向角發(fā)散,但俯仰角收斂;圖2(f)中航向角發(fā)散,但橫滾角和俯仰角收斂,表明?q1中第2和第3個(gè)元素分別起了校正了橫滾角和俯仰角的作用,即表明四元數(shù)的第2,3個(gè)元素分別主要包含著橫滾角和俯仰角的信息;圖2(g)中橫滾角和俯仰角發(fā)散;圖2(h)中航向角收斂,圖2(i)中所有姿態(tài)角都收斂,表明?q2的第4個(gè)元素對(duì)校正航向角產(chǎn)生作用,即表明四元數(shù)第4個(gè)元素主要包含著航向角信息;綜上可知本文之前設(shè)想正確.本文所提姿態(tài)解算算法中,加速度計(jì)數(shù)據(jù)只用來校正橫滾角和俯仰角,所以在加速度校正階段,對(duì)于校正參數(shù)?q1,置其第4個(gè)元素為零,以消除加速度計(jì)數(shù)據(jù)對(duì)航向角估計(jì)的干擾;同理在磁力計(jì)校正階段,為消除磁力計(jì)數(shù)據(jù)對(duì)橫滾角和俯仰角估計(jì)的干擾,將校正參數(shù)?q2的第2和第3個(gè)元素置為零,即qa1和qa2按Case7取值.

表1 qa1,qa2取值表Table 1 value table of qa1,qa2

圖2 不同情況下的的姿態(tài)角Fig.2 Attitude angle under different conditions

4 仿真實(shí)驗(yàn)及結(jié)果分析

為了驗(yàn)證本文所提算法在計(jì)算精度和解算時(shí)間方面的有效性,本文利用二段式EKF算法(two-step EKF,TEKF)和二段式UKF算法(two-step UKF,TUKF)與本文所提算法進(jìn)行仿真實(shí)驗(yàn)對(duì)比,仿真結(jié)果見圖3 和表2–3.為了驗(yàn)證本文所提算法的抗干擾性能,使用僅包含加速度計(jì)數(shù)據(jù)的DUKF校正的算法(acceleration-DUKF,A–DUKF)以及傳統(tǒng)的將加速度數(shù)據(jù)和磁力計(jì)數(shù)據(jù)的六維量測向量一起處理的DUKF算法(normal-DUKF,N–DUKF)與本文所提算法進(jìn)行仿真實(shí)驗(yàn)對(duì)比,仿真結(jié)果見圖4(a)–(b);室內(nèi)外環(huán)境中經(jīng)常會(huì)存在各種物體造成的電磁場干擾,如高壓電塔,各種電器等,這些干擾多為0.1 Gs級(jí)別[20],為了驗(yàn)證本文所提算法在環(huán)境出現(xiàn)磁場異常時(shí)的分離相互干擾的能力,人為的在三軸磁力計(jì)40～80 s 的數(shù)據(jù)上均加入一個(gè)約為0.1 Gs的常值偏移以模擬環(huán)境中出現(xiàn)的磁異常,使用傳統(tǒng)的將加速度數(shù)據(jù)和磁力計(jì)數(shù)據(jù)的六維量測向量一起處理的DUKF算法(N–DUKF)與本文所提算法進(jìn)行仿真實(shí)驗(yàn)對(duì)比,仿真結(jié)果見圖4(c)–(e);在執(zhí)行一般任務(wù)時(shí),多旋翼無人機(jī)加速比較緩慢,為了驗(yàn)證本文所提算法在存在載體加速度時(shí)的分離相互干擾的能力,人為的在三軸加速度計(jì)40～80 s的數(shù)據(jù)上均加入一個(gè)約為0.05 g的常值偏移以模擬存在的外部加速度干擾,使用傳統(tǒng)的將加速度數(shù)據(jù)和磁力計(jì)數(shù)據(jù)的六維量測向量一起處理的DUKF算法(N–DUKF)與本文所提算法進(jìn)行仿真實(shí)驗(yàn)對(duì)比,仿真結(jié)果見圖4(f)–(h).

由圖3(a)–(b)可以看出,本文所提算法(TDUKF)解算精度明顯高于TEKF算法,與TUKF算法的解算精度相當(dāng),由圖3(c)可以看出,這3種算法估計(jì)航向角時(shí),均能較快的收斂.

圖3 不同算法姿態(tài)角對(duì)比Fig.3 Attitude angle comparison of different algorithms

由表2 的數(shù)據(jù)可以看出TEKF 算法的解算精度最低,TUKF算法的解算精度高于TEKF算法,本文所提的TDUKF算法的解算精度略微高于TUKF算法.

表2 姿態(tài)誤差(?)Table 2 Attitude error(?)

表3 的數(shù)據(jù)是在裝有MATLAB 的PC 機(jī)上經(jīng)過100次蒙特卡洛仿真計(jì)算出的平均值,PC配置如下:處理器Intel Core i5--2450 M 2.5 GHz,內(nèi)存4 GB.由表3可知,與TUKF算法相比,本文所提算法(TDUKF)的運(yùn)算時(shí)間約縮短了40%左右,這為姿態(tài)解算實(shí)時(shí)性帶來很大的提高.

表3 平均每個(gè)算法周期解算時(shí)間(ms)Table 3 Average calculation time per algorithm cycle(ms)

由圖4(a)–(b)可以看出本文所提算法(TDUKF)的橫滾角和俯仰角的估計(jì)和只使用加速度計(jì)數(shù)據(jù)的DUKF算法(A–DUKF)的相當(dāng),而將加速度數(shù)據(jù)和磁力計(jì)數(shù)據(jù)一起處理的DUKF算法(N–DUKF)由于磁力計(jì)數(shù)據(jù)的影響,橫滾角和俯仰角的估計(jì)都出現(xiàn)了一定的偏差;由圖4(c)–(e)可以看出,加入人為磁干擾后,傳統(tǒng)N–DUKF算法和本文所提TDUKF 算法的航向角估計(jì)都受到影響,與圖4(a)–(b)相比,由于加入了人為磁干擾,N–DUKF算法的橫滾角和俯仰角估計(jì)均受到影響,但TDUKF算法的橫滾角和俯仰角估計(jì)并未受到加入的磁干擾的影響;由圖4(f)–(h)可以看出,加入人為加速度干擾后,傳統(tǒng)N–DUKF算法的橫滾角、俯仰角和航向角估計(jì)都受到影響,但本文所提TDUKF算法只有橫滾角和俯仰角的估計(jì)受到影響,航向角估計(jì)并未受到影響.說明本文所提算法能很好的消除磁力計(jì)數(shù)據(jù)對(duì)俯仰角和橫滾角估計(jì)的影響以及加速度計(jì)數(shù)據(jù)對(duì)航向角估計(jì)的影響,分離了相互之間的干擾,具有良好的抗干擾能力.

綜上所述,本文所提算法(TDUKF)具有良好的抗干擾能力,且在解算時(shí)間上又明顯少于傳統(tǒng)UKF算法,略高于傳統(tǒng)EKF算法,但解算精度高于傳統(tǒng)EKF算法.

圖4 不同算法姿態(tài)角對(duì)比Fig.4 Attitude angle comparison of different algorithms

5 結(jié)論

為了提高多旋翼無人機(jī)姿態(tài)估計(jì)的精度、系統(tǒng)實(shí)時(shí)性以及抗干擾能力,本文從四元數(shù)本質(zhì)出發(fā),推測四元數(shù)的第2、第3和第4個(gè)元素分別主要包含著橫滾角、俯仰角和姿態(tài)角的信息,并通過仿真實(shí)驗(yàn)證明該推測.根據(jù)四元數(shù)的這種性質(zhì),本文結(jié)合一種計(jì)算量較常規(guī)UKF算法小的DUKF算法,將加速度數(shù)據(jù)和磁力計(jì)數(shù)據(jù)分別在兩個(gè)校正過程中進(jìn)行處理,并將校正過程中的校正四元數(shù)乘以相應(yīng)的系數(shù)來設(shè)置所需要的姿態(tài)角信息,以消除加速度計(jì)數(shù)據(jù)對(duì)航向角估計(jì)的干擾以及磁力計(jì)數(shù)據(jù)對(duì)橫滾角和俯仰角估計(jì)的干擾,由此提出一種基于四元數(shù)DUKF的二段式姿態(tài)估計(jì)算法(TDUKF).并利用從水平放置的PIXHAWK飛控中提取的傳感器數(shù)據(jù),將本文所提算法(TDUKF)與二段式EKF 算法(TEKF)以及二段式UKF 算法(TUKF)進(jìn)行仿真對(duì)比實(shí)驗(yàn),從解算精度、解算時(shí)間方面證明了本文所提算法的有效性;將本文算法所提算法與使用僅包含加速度計(jì)數(shù)據(jù)的DUKF 校正的算法(A–DUKF)以及常規(guī)的將加速度數(shù)據(jù)和磁力計(jì)數(shù)據(jù)的六維量測向量一起處理的DUKF算法(N–DUKF)進(jìn)行仿真實(shí)驗(yàn)對(duì)比,從抗干擾能力方面證明了本文所提算法的有效性,對(duì)工程實(shí)踐具有一定的參考意義.