[發(fā)明專(zhuān)利]一種基于失準(zhǔn)角的卡爾曼濾波姿態(tài)估計(jì)方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201810380263.8
• 申請(qǐng)日: 2018-04-25
• 公開(kāi)（公告）號(hào): CN108592917B
• 公開(kāi)（公告）日: 2021-02-23
• 發(fā)明（設(shè)計(jì)）人: 劉兵;劉恒;呂元宙;莊秋彬
• 申請(qǐng)（專(zhuān)利權(quán)）人: 珠海全志科技股份有限公司
• 主分類(lèi)號(hào): G01C21/20
• 分類(lèi)號(hào): G01C21/20;G01C21/16
• 代理公司: 深圳市君之泉知識(shí)產(chǎn)權(quán)代理有限公司 44366
• 代理人: 張丕陽(yáng)

本發(fā)明實(shí)施例公開(kāi)了一種基于失準(zhǔn)角的卡爾曼濾波姿態(tài)估計(jì)方法。所述方法包括：構(gòu)建載體坐標(biāo)系，并確定載體坐標(biāo)系到NED導(dǎo)航坐標(biāo)系的轉(zhuǎn)換矩陣；采用東向水平失準(zhǔn)角、北向水平失準(zhǔn)角和陀螺漂移作為系統(tǒng)狀態(tài)，以水平加速度作為量測(cè)狀態(tài)；根據(jù)失準(zhǔn)角方程構(gòu)建卡爾曼濾波系統(tǒng)的狀態(tài)方程；基于卡爾曼濾波實(shí)現(xiàn)載體姿態(tài)的估計(jì)。另外，綜合考慮載體運(yùn)動(dòng)狀態(tài)，確定是否對(duì)載體姿態(tài)進(jìn)行修正。本發(fā)明實(shí)施例的技術(shù)方案根據(jù)失準(zhǔn)角方程構(gòu)建4維狀態(tài)量的卡爾曼濾波，在保證姿態(tài)估計(jì)精確度的同時(shí)，降低了系統(tǒng)運(yùn)算復(fù)雜度，并且基于載體運(yùn)動(dòng)狀態(tài)，判斷是否進(jìn)行姿態(tài)修正，能夠進(jìn)一步提高運(yùn)算效率，提高系統(tǒng)的魯棒性。

技術(shù)領(lǐng)域

本發(fā)明實(shí)施例涉及導(dǎo)航領(lǐng)域，尤其涉及一種基于失準(zhǔn)角的卡爾曼濾波姿態(tài)估計(jì)方法。

背景技術(shù)

慣性傳感器IMU中通常內(nèi)置有卡爾曼濾波系統(tǒng)，能夠采用卡爾曼濾波進(jìn)行姿態(tài)的估計(jì)和修正，卡爾曼濾波的效率在載體姿態(tài)估計(jì)上起到了重要的作用。目前，為了保證姿態(tài)估計(jì)的準(zhǔn)確性，往往設(shè)計(jì)較為復(fù)雜的狀態(tài)方程，導(dǎo)致解算復(fù)雜，容易受到噪聲的干擾。并且，當(dāng)應(yīng)用于無(wú)人機(jī)或小型飛行器時(shí)，受到載體體積和設(shè)備成本的制約，置于載體上的慣性傳感器IMU通常不具備較強(qiáng)的運(yùn)算能力，無(wú)法執(zhí)行復(fù)雜的算法。另外，基于卡爾曼濾波進(jìn)行姿態(tài)修正的過(guò)程中，還面對(duì)何時(shí)啟動(dòng)姿態(tài)修正的難題。現(xiàn)有技術(shù)中，通常首先計(jì)算慣性傳感器中三軸加速度計(jì)的模值，然后判斷模值是否在預(yù)設(shè)范圍的方式，確定載體是否處于無(wú)運(yùn)動(dòng)加速度狀態(tài)，如果是，則進(jìn)行卡爾曼濾波的量測(cè)修正。然而，由于所述方法僅考慮模值這一單一因素，往往會(huì)出現(xiàn)漏判情況，導(dǎo)致錯(cuò)誤的量測(cè)修正，造成較大的誤差影響。

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

有鑒于此，本發(fā)明實(shí)施例提供了一種基于失準(zhǔn)角的卡爾曼濾波姿態(tài)估計(jì)方法，以解決現(xiàn)有技術(shù)中運(yùn)算量大、精度低，量測(cè)修正不及時(shí)、不準(zhǔn)確的問(wèn)題。

本發(fā)明實(shí)施例提供了一種基于失準(zhǔn)角的卡爾曼濾波姿態(tài)估計(jì)方法，所述方法包含以下步驟：

步驟一：設(shè)置載體坐標(biāo)系，所述載體坐標(biāo)系三軸表示為記作xyz；

步驟二：通過(guò)慣性傳感器確定當(dāng)前時(shí)刻的載體姿態(tài)，記作att＝[φ θ ψ]，其中φ代表滾轉(zhuǎn)角，θ代表俯仰角，ψ代表航向角；

步驟三：確定載體坐標(biāo)系向?qū)Ш阶鴺?biāo)系的轉(zhuǎn)換矩陣，記作

簡(jiǎn)化轉(zhuǎn)換矩陣為以下形式

步驟四：采用東向水平失準(zhǔn)角北向水平失準(zhǔn)角和陀螺漂移ω_bx,ω_by作為系統(tǒng)狀態(tài)，記作

采用水平加速度f(wàn)_x,f_y作為量測(cè)狀態(tài)，記作Z＝[f_y f_x]^T；

步驟五：構(gòu)建基于失準(zhǔn)角的卡爾曼濾波；

步驟六：根據(jù)所述基于失準(zhǔn)角的卡爾曼濾波進(jìn)行載體姿態(tài)的估計(jì)。

優(yōu)選地，所述載體坐標(biāo)系與載體固聯(lián)，坐標(biāo)系符合右手法則，原點(diǎn)在載體重心處，x軸指向載體前進(jìn)方向，y軸由原點(diǎn)指向載體右側(cè)，z軸方向根據(jù)xy軸由右手法則確定。

優(yōu)選地，設(shè)卡爾曼濾波系統(tǒng)的狀態(tài)方程為：

其中，為系統(tǒng)狀態(tài)的導(dǎo)數(shù)，f(x)為系統(tǒng)狀態(tài)的函數(shù)，w為系統(tǒng)噪聲，Z為系統(tǒng)的量測(cè)狀態(tài)，h(x)為系統(tǒng)的量測(cè)狀態(tài)函數(shù)，v為量測(cè)噪聲；

記失準(zhǔn)角為陀螺漂移為ω_b＝[ω_bx ω_by 0]^T，根據(jù)失準(zhǔn)角方程，上式可表示為