一種基于二階量測更新的低成本姿態(tài)估計(jì)方法
【專利摘要】本發(fā)明提供的是一種基于二階量測更新的低成本姿態(tài)估計(jì)方法。本發(fā)明通過對(duì)三軸微機(jī)械陀螺、三軸微機(jī)械加速度計(jì)和三軸磁強(qiáng)計(jì)的輸出數(shù)據(jù)，進(jìn)行濾波處理得到載體的姿態(tài)信息。針對(duì)在室內(nèi)或磁干擾較強(qiáng)場所，磁強(qiáng)計(jì)輸出會(huì)使橫搖和縱搖誤差變大，傳統(tǒng)方法難以解決的問題。本方法在濾波的量測更新階段，創(chuàng)新性地采用二階量測更新，即先進(jìn)行加速度計(jì)量測更新再進(jìn)行磁強(qiáng)計(jì)量測更新。以此修正標(biāo)準(zhǔn)量測更新算法從而使磁強(qiáng)計(jì)更新只影響方位角。利用本方法可以使用低成本的微慣性測量單元和磁強(qiáng)計(jì)進(jìn)行姿態(tài)估計(jì)，并且估計(jì)精度高、實(shí)時(shí)性好、適應(yīng)強(qiáng)磁干擾環(huán)境。本方法適用于車輛、無人機(jī)和船艦等載體的姿態(tài)估計(jì)。
【專利說明】一種基于二階量測更新的低成本姿態(tài)估計(jì)方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及的是一種適用于利用低成本的傳感器，對(duì)車輛、船舶、無人機(jī)載體等進(jìn)行姿態(tài)估計(jì)地方法。具體地說是一種基于二階量測更新的低成本姿態(tài)估計(jì)方法。
【背景技術(shù)】
[0002]在很多工程應(yīng)用領(lǐng)域中都需要進(jìn)行姿態(tài)估計(jì)，例如，機(jī)動(dòng)追蹤、無人機(jī)和生物應(yīng)用等方面。如果給定載體初始狀態(tài)，則可以通過積分陀螺儀計(jì)算出估計(jì)姿態(tài)值。它的估計(jì)誤差主要由陀螺漂移和算法誤差組成。此外，也可以通過加速度計(jì)和磁力計(jì)的輸出計(jì)算出姿態(tài)值。在這種情況下，由于外部加速度和磁干擾，估計(jì)誤差可能會(huì)變大。因此每種姿態(tài)估計(jì)算法的基本問題就是如何將陀螺儀、加速度計(jì)和磁力計(jì)結(jié)合起來使用。因此，用微慣性傳感器和磁傳感器來估計(jì)姿態(tài)是近年來的流行方法之一。
[0003]目前，大多數(shù)的姿態(tài)估計(jì)中觀測量都會(huì)選取地球磁場，利用磁強(qiáng)計(jì)或羅經(jīng)等設(shè)備輸出磁力信息。其中磁強(qiáng)計(jì)的輸出不僅能提供航向信息同時(shí)也包括縱搖和橫搖的一些信息。但它有一個(gè)嚴(yán)重的缺點(diǎn)就是縱搖角和橫搖角受磁干擾影響，由于室內(nèi)存在較強(qiáng)的磁干擾，因此在室內(nèi)應(yīng)用該種算法時(shí)縱搖誤差和橫搖誤差都有較大的變化。所以較為明智的選擇就是利用磁強(qiáng)計(jì)輸出數(shù)據(jù)只計(jì)算航向角，因此這不適用于傳統(tǒng)的四元數(shù)間接卡爾曼濾波。

【發(fā)明內(nèi)容】

[0004]本發(fā)明的目的在于提供一種通過測量微慣性測量單元和磁強(qiáng)計(jì)輸出的載體三個(gè)軸的角速率、線加速度和磁力數(shù)據(jù)，利用明改進(jìn)的四元數(shù)卡爾曼濾波處理，得到載體的精確姿態(tài)信息的基于二階量測更新的低成本姿態(tài)估計(jì)方法。
[0005]本發(fā)明的目的是這樣實(shí)現(xiàn)的:
[0006](I)在k=0時(shí)刻，即初始時(shí)刻由初始對(duì)準(zhǔn)得到初始姿態(tài)所述初始姿態(tài)包括橫搖、縱搖和航向；
[0007](2)在k > I時(shí)刻，利用微機(jī)械陀螺輸出的載體三軸的角速率數(shù)據(jù)，計(jì)算出姿態(tài)角的變化值Λ Φ，由式計(jì)算出濾波開始前的粗略姿態(tài)角；
[0008](3)采集微機(jī)械加速度計(jì)和磁力計(jì)輸出的載體三軸線的加速度和磁力數(shù)據(jù)；
[0009](4)設(shè)濾波的狀態(tài)變量為x=[qe bg bJT e R9xi ;式中:?表示四元數(shù)誤差么矢量部
分，bg表示陀螺漂移，ba表示加速度計(jì)零偏；假設(shè)四元數(shù)誤差^/ =[1 H J，根據(jù)四元數(shù)相關(guān)性質(zhì)建立狀態(tài)方程:
【權(quán)利要求】
1.一種基于二階量測更新的低成本姿態(tài)估計(jì)方法，其特征是: (1)在k=0時(shí)刻，即初始時(shí)刻由初始對(duì)準(zhǔn)得到初始姿態(tài)Φ(!,所述初始姿態(tài)包括橫搖、縱搖和航向； (2)在k≥1時(shí)刻，利用微機(jī)械陀螺輸出的載體三軸的角速率數(shù)據(jù)，計(jì)算出姿態(tài)角的變化值Λ Φ，由式
【文檔編號(hào)】G01C21/20GK103822633SQ201410047878
【公開日】2014年5月28日 申請(qǐng)日期:2014年2月11日 優(yōu)先權(quán)日:2014年2月11日
【發(fā)明者】周廣濤, 孫艷濤, 姜鑫, 梁宏, 時(shí)貴敏, 趙博, 郝勤順, 李佳璇, 林萌萌, 張麗麗 申請(qǐng)人:哈爾濱工程大學(xué)