專利名稱：機(jī)動(dòng)加速度輔助的擴(kuò)展卡爾曼濾波航姿系統(tǒng)姿態(tài)估計(jì)方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種捷聯(lián)慣性航姿系統(tǒng)航姿估計(jì)技術(shù)，尤其涉及一種利用載體機(jī)動(dòng)加速度輔助的九態(tài)擴(kuò)展卡爾曼濾波航姿系統(tǒng)姿態(tài)估計(jì)方法。

背景技術(shù)：
由于開(kāi)環(huán)式IFOG航姿系統(tǒng)的陀螺精度有限，初始化后僅依靠傳統(tǒng)的捷聯(lián)算法，姿態(tài)將很快發(fā)散，需要利用加速度計(jì)和磁羅盤(pán)的信息來(lái)進(jìn)行修正?，F(xiàn)有方法在載體機(jī)動(dòng)小時(shí)比較有效，而當(dāng)應(yīng)用于無(wú)人機(jī)、直升機(jī)等載體時(shí)，由于其機(jī)動(dòng)時(shí)間較長(zhǎng)，且要求姿態(tài)仍能保持較高的精度，那么載體本身的機(jī)動(dòng)加速度的影響無(wú)法消除，這樣根據(jù)加速度計(jì)信息不能得到較準(zhǔn)確的俯仰角和橫滾角估計(jì)，同時(shí)也不能根據(jù)磁羅盤(pán)信息對(duì)航向角信息進(jìn)行準(zhǔn)確的估計(jì)。所以當(dāng)載體長(zhǎng)時(shí)間處于大機(jī)動(dòng)狀態(tài)時(shí)(如飛機(jī)長(zhǎng)時(shí)間協(xié)調(diào)轉(zhuǎn)彎)，由于向心加速度的存在，導(dǎo)致一段時(shí)間后加速度計(jì)和磁羅盤(pán)不能有效的對(duì)姿態(tài)誤差進(jìn)行修正，相應(yīng)陀螺的零偏也越來(lái)越大，這是開(kāi)環(huán)式IFOG航姿系統(tǒng)在機(jī)動(dòng)載體應(yīng)用時(shí)存在的關(guān)鍵性障礙。


發(fā)明內(nèi)容
現(xiàn)有的開(kāi)環(huán)式IFOG航姿系統(tǒng)三軸加速度計(jì)和三軸磁傳感器信息不僅包含了對(duì)橫滾角、俯仰角和航向角的觀測(cè)，同時(shí)也能對(duì)載體自身機(jī)動(dòng)加速度進(jìn)行觀測(cè)。為解決載體長(zhǎng)時(shí)機(jī)動(dòng)情形下的姿態(tài)估計(jì)問(wèn)題，本發(fā)明利用加速度和磁場(chǎng)觀測(cè)冗余信息，將載體機(jī)動(dòng)加速度描述為一種非零均值時(shí)間相關(guān)模型，提出了一種九狀態(tài)擴(kuò)展卡爾曼濾波方法，即取三個(gè)姿態(tài)角誤差、三軸陀螺的零偏誤差和載體系的三軸載體機(jī)動(dòng)加速度誤差為狀態(tài)量，觀測(cè)量為三軸加速度誤差和三軸地磁場(chǎng)誤差的九狀態(tài)擴(kuò)展卡爾曼濾波，并與捷聯(lián)姿態(tài)算法進(jìn)行數(shù)據(jù)融合，能夠?qū)⒉煌瑱C(jī)動(dòng)情形下姿態(tài)角均方差控制在2°以內(nèi)。
本發(fā)明提供一種能夠有效解決載體長(zhǎng)時(shí)機(jī)動(dòng)情形下的姿態(tài)精度保持的捷聯(lián)慣性航姿系統(tǒng)航姿估計(jì)方法，主要包括下列步驟 步驟一、慣性傳感器標(biāo)定，包括光纖陀螺儀、加速度計(jì)和磁傳感器的標(biāo)定，分別取標(biāo)定后載體坐標(biāo)系下的三軸角速度、三軸加速度及三軸地磁場(chǎng)。
步驟二、羅差校正，利用陀螺和地磁場(chǎng)的轉(zhuǎn)動(dòng)信息對(duì)三軸軟鐵和硬鐵效應(yīng)進(jìn)行校正。
步驟三、捷聯(lián)姿態(tài)解算采用四元數(shù)表示方法，進(jìn)行捷聯(lián)姿態(tài)解算。
步驟四、載體機(jī)動(dòng)加速度模型 當(dāng)載體進(jìn)行直線加減速或長(zhǎng)時(shí)間轉(zhuǎn)彎?rùn)C(jī)動(dòng)時(shí)，將機(jī)動(dòng)加速度描述為一種非零均值時(shí)間相關(guān)模型，即當(dāng)載體以某一加速度機(jī)動(dòng)時(shí)，下一瞬時(shí)的加速度只能在當(dāng)前加速度的鄰域內(nèi)，如下式 其中a為機(jī)動(dòng)加速度，

為下一時(shí)刻的機(jī)動(dòng)加速度，a為機(jī)動(dòng)加速度均值，取前一時(shí)刻的機(jī)動(dòng)加速度，且在每一采樣周期內(nèi)為常數(shù)，δa為零均值有色加速度噪聲，即為相鄰時(shí)刻的機(jī)動(dòng)加速度誤差，

為下一相鄰時(shí)刻的機(jī)動(dòng)加速度誤差，α為機(jī)動(dòng)頻率，wa為白噪聲，方差σa2為機(jī)動(dòng)加速度方差，當(dāng)機(jī)動(dòng)行為不太劇烈時(shí)可取為常值。
步驟五、建立機(jī)動(dòng)加速度輔助的九態(tài)擴(kuò)展卡爾曼濾波模型，狀態(tài)量取為三個(gè)姿態(tài)角誤差、三軸陀螺的零偏誤差和載體系的三軸載體機(jī)動(dòng)加速度誤差，觀測(cè)量為三軸加速度誤差和三軸地磁場(chǎng)誤差。
i.狀態(tài)方程 九態(tài)擴(kuò)展卡爾曼濾波器的誤差狀態(tài)量選取如下 其中δρ＝[∈x，∈y，∈z]T，是姿態(tài)角誤差向量；δb＝[bx，by，bz]T，是陀螺零偏誤差向量；δa＝[δax，δay，δaz]T，是載體系中的機(jī)動(dòng)加速度誤差向量，模型如式(2)所示。姿態(tài)角誤差方程忽略地球表觀運(yùn)動(dòng)角速度，以式(3)為狀態(tài)量的狀態(tài)方程為 其中 δb建模為一個(gè)隨機(jī)游走過(guò)程，αx，αy，αz分別為載體系內(nèi)三軸機(jī)動(dòng)加速度的機(jī)動(dòng)頻率； ii.量測(cè)方程 量測(cè)方程中用到了兩種量測(cè)信息，分別是 ①加速度誤差量測(cè)方程 取向量gn＝
T，為重力向量在導(dǎo)航坐標(biāo)系的投影。載體系中加速度計(jì)測(cè)得加速度為

為載體系中重力向量gb、載體加速度a、量測(cè)噪聲vf之和，其中加速度的常值零偏可歸結(jié)到載體加速度a中。

為加速度估計(jì)值，

為導(dǎo)航坐標(biāo)系到載體坐標(biāo)系變換矩陣的估計(jì)陣，取加速度計(jì)在載體系測(cè)得的加速度

與估計(jì)加速度

之差δfb為觀測(cè)量 ②地磁場(chǎng)誤差量測(cè)方程 向量mn為地磁場(chǎng)向量在導(dǎo)航坐標(biāo)系的投影，mb為地磁場(chǎng)向量在載體坐標(biāo)系分量，航姿系統(tǒng)工作前在現(xiàn)場(chǎng)要首先進(jìn)行軟硬鐵補(bǔ)償，然后根據(jù)靜態(tài)初始化時(shí)的初始姿態(tài)矩陣和三軸磁傳感器測(cè)量值可得到當(dāng)?shù)氐膍n。取磁傳感器在載體系內(nèi)測(cè)得的三軸地磁場(chǎng)

與估計(jì)地磁場(chǎng)

之差δmb為觀測(cè)量 其中vm為載體系中磁場(chǎng)的量測(cè)噪聲，[mb×]是由向量mb構(gòu)成的反對(duì)稱陣 綜合式(5)定義的載體系中加速度誤差和式(6)中定義的載體系中地磁場(chǎng)誤差，得到量測(cè)方程 Z(t)＝H(t)δx+v(t) (7) 其中 步驟六、將機(jī)動(dòng)加速度輔助的九態(tài)擴(kuò)展卡爾曼濾波和捷聯(lián)姿態(tài)算法進(jìn)行數(shù)據(jù)融合，得到航姿系統(tǒng)姿態(tài)估計(jì)。
采用四階龍格庫(kù)塔法進(jìn)行捷聯(lián)姿態(tài)解算，解算得到的四元數(shù)

實(shí)時(shí)地由誤差四元數(shù)

進(jìn)行修正得到濾波修正后的輸出四元數(shù)

其中誤差四元數(shù)

正是由九態(tài)擴(kuò)展卡爾曼濾波估計(jì)的姿態(tài)角誤差向量

經(jīng)轉(zhuǎn)換后得到的。與此同時(shí)，由九態(tài)擴(kuò)展卡爾曼濾波估計(jì)的陀螺零偏誤差向量δb+和機(jī)動(dòng)加速度誤差向量

分別用來(lái)實(shí)時(shí)修正陀螺零偏向量

和機(jī)動(dòng)加速度向量

九態(tài)擴(kuò)展卡爾曼濾波觀測(cè)量取為δfb和δmb，狀態(tài)量為姿態(tài)角誤差向量δρ、陀螺零偏誤差向量δb和載體系中的機(jī)動(dòng)加速度誤差向量δa。
步驟七、將三軸角速度、加速度、橫滾、俯仰、航向角等數(shù)據(jù)打包輸出，即得到航姿估計(jì)值。本發(fā)明具有以下優(yōu)點(diǎn) a、能夠有效解決載體長(zhǎng)時(shí)間機(jī)動(dòng)情形下的姿態(tài)精度保持問(wèn)題； b、實(shí)時(shí)修正機(jī)動(dòng)加速度估計(jì)值和陀螺零偏； c、不改變硬件結(jié)構(gòu)和安裝方式。



圖1九態(tài)擴(kuò)展卡爾曼濾波姿態(tài)數(shù)據(jù)融合算法流程圖。
圖2(a)9態(tài)EKF三軸載體機(jī)動(dòng)加速度曲線。
圖2(b)三軸陀螺零偏估計(jì)曲線。
圖2(c)橫滾、俯仰和航向角估計(jì)曲線。
圖3(a)9態(tài)EKF三軸載體機(jī)動(dòng)加速度曲線。
圖3(b)6態(tài)EKF三軸陀螺零偏曲線。
圖3(c)9態(tài)EKF三軸陀螺零偏曲線。
圖3(d)捷聯(lián)算法橫滾、俯仰和航向角曲線。
圖3(e)6態(tài)EKF橫滾、俯仰和航向角曲線。
圖3(f)9態(tài)EKF橫滾、俯仰和航向角曲線。
圖4(a)9態(tài)EKF三軸載體機(jī)動(dòng)加速度曲線。
圖4(b)6態(tài)EKF三軸陀螺零偏曲線。
圖4(c)9態(tài)EKF三軸陀螺零偏曲線。
圖4(d)捷聯(lián)算法橫滾、俯仰和航向角曲線。
圖4(e)6態(tài)EKF橫滾、俯仰和航向角曲線。
圖4(f)9態(tài)EKF橫滾、俯仰和航向角曲線。
圖5(a)9態(tài)EKF三軸載體機(jī)動(dòng)加速度曲線。
圖5(b)6態(tài)EKF三軸陀螺零偏曲線。
圖5(c)9態(tài)EKF三軸陀螺零偏曲線。
圖5(d)6態(tài)EKF橫滾、俯仰和航向角曲線。
圖5(e)9態(tài)EKF橫滾、俯仰和航向角曲線。
圖69態(tài)EKF橫滾角和俯仰角曲線。
圖7POS與AHRS橫滾、俯仰和航向角對(duì)比曲線。

具體實(shí)施例方式 下面結(jié)合附圖和實(shí)施例對(duì)本發(fā)明進(jìn)行詳細(xì)說(shuō)明。
本發(fā)明提供的機(jī)動(dòng)加速度輔助的擴(kuò)展卡爾曼濾波航姿系統(tǒng)姿態(tài)估計(jì)方法，通過(guò)如下步驟實(shí)現(xiàn) 步驟一、慣性傳感器標(biāo)定，包括光纖陀螺儀、加速度計(jì)和磁傳感器的標(biāo)定，分別取標(biāo)定后載體坐標(biāo)系下的三軸角速度、三軸加速度及三軸地磁場(chǎng)，如圖1。
步驟二、羅差校正，利用陀螺和地磁場(chǎng)的轉(zhuǎn)動(dòng)信息對(duì)三軸軟鐵和硬鐵效應(yīng)進(jìn)行校正。
步驟三、捷聯(lián)姿態(tài)解算采用四元數(shù)表示方法，進(jìn)行捷聯(lián)姿態(tài)解算。
步驟四、載體機(jī)動(dòng)加速度模型建立 當(dāng)載體進(jìn)行直線加減速或長(zhǎng)時(shí)間轉(zhuǎn)彎?rùn)C(jī)動(dòng)時(shí)，將機(jī)動(dòng)加速度描述為一種非零均值時(shí)間相關(guān)模型，即當(dāng)載體以某一加速度機(jī)動(dòng)時(shí)，下一瞬時(shí)的加速度只能在當(dāng)前加速度的鄰域內(nèi)，如下式 其中a為機(jī)動(dòng)加速度，

為下一時(shí)刻的機(jī)動(dòng)加速度，a為機(jī)動(dòng)加速度均值，取前一時(shí)刻的機(jī)動(dòng)加速度，且在每一采樣周期內(nèi)為常數(shù)，δa為零均值有色加速度噪聲，即為相鄰時(shí)刻的機(jī)動(dòng)加速度誤差，

為下一相鄰時(shí)刻的機(jī)動(dòng)加速度誤差，α為機(jī)動(dòng)頻率，wa為白噪聲，方差σa2為機(jī)動(dòng)加速度方差，當(dāng)機(jī)動(dòng)行為不太劇烈時(shí)可取為常值。
步驟五、機(jī)動(dòng)加速度輔助的九態(tài)擴(kuò)展卡爾曼濾波算法 i.狀態(tài)方程 九態(tài)擴(kuò)展卡爾曼濾波器的誤差狀態(tài)量選取如下 其中δρ＝[∈x，∈y，∈z]T，是姿態(tài)角誤差向量；δb＝[bx，by，bz]T，是陀螺零偏誤差向量，δa＝[δax，δay，δaz]T，是載體系中的機(jī)動(dòng)加速度誤差向量，模型如式(2)所示。姿態(tài)角誤差方程忽略地球表觀運(yùn)動(dòng)角速度，以式(3)為狀態(tài)量的狀態(tài)方程為 其中 δb建模為一個(gè)隨機(jī)游走過(guò)程，αx，αy，αz分別為載體系內(nèi)三軸機(jī)動(dòng)加速度的機(jī)動(dòng)頻率。
ii.量測(cè)方程 量測(cè)方程中用到了兩種量測(cè)信息，分別是 ①加速度誤差量測(cè)方程 取向量gn＝
T，為重力向量在導(dǎo)航坐標(biāo)系的投影。載體系中加速度計(jì)測(cè)得加速度為

為載體系中重力向量gb、載體加速度a、量測(cè)噪聲vf之和，其中加速度的常值零偏可歸結(jié)到載體加速度a中。

為加速度估計(jì)值，

為導(dǎo)航坐標(biāo)系到載體坐標(biāo)系變換矩陣的估計(jì)陣，取加速度計(jì)在載體系測(cè)得的加速度

與估計(jì)加速度

之差

為觀測(cè)量 ②地磁場(chǎng)誤差量測(cè)方程 向量mn為地磁場(chǎng)向量在導(dǎo)航坐標(biāo)系的投影，mb為地磁場(chǎng)向量在載體坐標(biāo)系分量，航姿系統(tǒng)工作前在現(xiàn)場(chǎng)要首先進(jìn)行軟硬鐵補(bǔ)償，然后根據(jù)靜態(tài)初始化時(shí)的初始姿態(tài)矩陣和三軸磁傳感器測(cè)量值可得到當(dāng)?shù)氐膍n。取磁傳感器在載體系內(nèi)測(cè)得的三軸地磁場(chǎng)

與估計(jì)地磁場(chǎng)

之差δmb為觀測(cè)量 其中vm為載體系中磁場(chǎng)的量測(cè)噪聲，[mb×]是由向量mb構(gòu)成的反對(duì)稱陣 綜合式(5)定義的載體系中加速度誤差和式(6)中定義的載體系中地磁場(chǎng)誤差，得到量測(cè)方程 Z(t)＝H(t)δx+v(t)(7) 其中 iii.離散化和遞推方程 卡爾曼濾波器在離散時(shí)間系統(tǒng)中的實(shí)現(xiàn)需要一個(gè)離散時(shí)間狀態(tài)轉(zhuǎn)移矩陣Ф，和一個(gè)離散時(shí)間過(guò)程噪聲協(xié)方差陣Qd。
AHRS在一個(gè)濾波周期ΔT內(nèi)能得到10個(gè)采樣值，一步狀態(tài)轉(zhuǎn)移陣和等效離散噪聲方差陣可簡(jiǎn)化計(jì)算如下 每個(gè)采樣周期內(nèi)，根據(jù)采用四元數(shù)對(duì)姿態(tài)進(jìn)行更新；而每個(gè)濾波周期內(nèi)，誤差狀態(tài)及其協(xié)方差的時(shí)間更新如下 當(dāng)獲得有效的測(cè)量值時(shí)，濾波器增益計(jì)算如下 誤差狀態(tài)協(xié)方差陣和誤差狀態(tài)觀測(cè)更新的估計(jì)如下 通過(guò)反饋δxk+1+可對(duì)姿態(tài)角和陀螺零偏進(jìn)行校正，而在下個(gè)濾波周期誤差狀態(tài)預(yù)測(cè)值δxk+1-應(yīng)該被設(shè)為零。
通過(guò)分析加速度計(jì)和磁傳感器測(cè)量噪聲的頻譜密度，可以確定R。通過(guò)分析陀螺的過(guò)程噪聲及與其相關(guān)的漂移，可以確定Qd。當(dāng)系統(tǒng)處于靜態(tài)模式時(shí)，利用已確定的R和Qd驅(qū)使卡爾曼濾波器產(chǎn)生最佳的增益，從而得到最好的狀態(tài)估計(jì)。
步驟六、將機(jī)動(dòng)加速度輔助的九態(tài)擴(kuò)展卡爾曼濾波和捷聯(lián)姿態(tài)算法進(jìn)行數(shù)據(jù)融合，得到航姿系統(tǒng)姿態(tài)估計(jì)。
結(jié)合捷聯(lián)姿態(tài)算法和機(jī)動(dòng)加速度輔助的九態(tài)擴(kuò)展卡爾曼濾波算法，得到的航姿系統(tǒng)姿態(tài)算法流程圖如圖1所示，其中b0是航姿系統(tǒng)靜態(tài)初始上電一段時(shí)間得到的零偏，

是捷聯(lián)算法得到的四元數(shù)，

是根據(jù)卡爾曼濾波估計(jì)的姿態(tài)角誤差向量δρ+轉(zhuǎn)換后得到的誤差四元數(shù)，

是濾波修正后四元數(shù)，加速度測(cè)量值增加了機(jī)動(dòng)加速度a，而在測(cè)量方程中也引入了機(jī)動(dòng)加速度誤差，其中的三個(gè)虛線部分表示卡爾曼濾波估計(jì)后實(shí)時(shí)修正了機(jī)動(dòng)加速度、捷聯(lián)矩陣和陀螺零偏。
步驟七、將三軸角速度、加速度、橫滾、俯仰、航向角等數(shù)據(jù)打包輸出，即得到航姿估計(jì)值。
實(shí)施例 本實(shí)施例中以純捷聯(lián)算法、六態(tài)擴(kuò)展卡爾曼濾波姿態(tài)算法(狀態(tài)量為三個(gè)姿態(tài)角誤差、三軸陀螺的零偏誤差，加速度誤差量測(cè)方程未考慮載體機(jī)動(dòng)加速度，或者說(shuō)將載體機(jī)動(dòng)加速度設(shè)為量測(cè)白噪聲)、九態(tài)擴(kuò)展卡爾曼濾波姿態(tài)算法及高精度組合導(dǎo)航系統(tǒng)POS(Positionand Orientation System)進(jìn)行試驗(yàn)，得出有益的結(jié)論。
(一)單軸轉(zhuǎn)臺(tái)試驗(yàn) (1)靜態(tài)試驗(yàn) 將航姿系統(tǒng)緩慢轉(zhuǎn)動(dòng)兩周，進(jìn)行軟鐵和硬鐵補(bǔ)償，再將航姿系統(tǒng)傾斜大概-60°，在其靜止時(shí)采集一段時(shí)間數(shù)據(jù)；濾波參數(shù)設(shè)置三軸機(jī)動(dòng)頻率均選為1，機(jī)動(dòng)加速度均方差為0.001g，姿態(tài)角誤差噪聲均方差為0.01°，陀螺零偏誤差噪聲均方差為0.001°/s，加速度計(jì)測(cè)量噪聲均方差為0.01g，磁傳感器測(cè)量噪聲均方差為0.006gauss，結(jié)果見(jiàn)圖2a、2b、2c。
(2)轉(zhuǎn)臺(tái)向心加速度試驗(yàn) 將航姿系統(tǒng)轉(zhuǎn)動(dòng)兩圈進(jìn)行羅差補(bǔ)償，然后將長(zhǎng)鋁棒垂直固定在單軸速率轉(zhuǎn)臺(tái)上方，通過(guò)在鋁棒頂端固定并引出水平木棒，將航姿系統(tǒng)接近水平固定在水平木棒遠(yuǎn)端，這樣航姿系統(tǒng)距離轉(zhuǎn)臺(tái)中心水平距離約為0.8米。航姿系統(tǒng)從靜止開(kāi)始工作一段時(shí)間，接著繞垂直軸以-150°/s勻速轉(zhuǎn)動(dòng)70秒，然后停止轉(zhuǎn)動(dòng)，靜止一段時(shí)間。濾波參數(shù)設(shè)置三軸機(jī)動(dòng)頻率選為1，由于轉(zhuǎn)臺(tái)突起突停變化非?？?，機(jī)動(dòng)加速度均方差放大，選為0.004g，由于150°/s不在一體化標(biāo)定的測(cè)試轉(zhuǎn)速內(nèi)，非線性補(bǔ)償效果相對(duì)較差，因此姿態(tài)角誤差噪聲均方差放大，選為0.1°/s，陀螺零偏誤差噪聲均方差為0.001°/s，加速度計(jì)測(cè)量噪聲均方差為0.01g，磁傳感器測(cè)量噪聲均方差為0.006gauss，結(jié)果見(jiàn)圖3a～3f。
(二)車載動(dòng)態(tài)試驗(yàn) (1)快速機(jī)動(dòng)試驗(yàn) 實(shí)驗(yàn)設(shè)計(jì)測(cè)試車在豐臺(tái)科技園總部基地沿一矩形區(qū)域行駛四圈，直線段不時(shí)做突然加減速運(yùn)動(dòng)，每個(gè)拐彎處盡量快速轉(zhuǎn)彎，最后慢慢停下來(lái)。在這個(gè)測(cè)試中，沒(méi)有準(zhǔn)確的地面實(shí)況，不能準(zhǔn)確判定系航姿統(tǒng)真正的性能。然而，地面實(shí)況的傾角變化不應(yīng)超過(guò)3°，而且每圈的數(shù)據(jù)可看做具有重復(fù)性。濾波參數(shù)選擇測(cè)試車加減速均較快，三軸機(jī)動(dòng)頻率選為1。安裝在測(cè)試車頂部的AHRS受周圍軟鐵和硬鐵影響較大，在進(jìn)行軟硬鐵補(bǔ)償時(shí)，由于找不到傾角變化較大的場(chǎng)地做14參數(shù)羅差補(bǔ)償，因此補(bǔ)償后的磁場(chǎng)精度較差，這里降低磁場(chǎng)觀測(cè)精度，提高狀態(tài)方程中姿態(tài)角精度；由于機(jī)動(dòng)頻繁且不同機(jī)動(dòng)間切換很快，考慮降低加速度觀測(cè)精度。機(jī)動(dòng)加速度均方差為0.002g，姿態(tài)角誤差噪聲均方差為0.01°/s，陀螺零偏誤差噪聲均方差為0.0003°/s，加速度計(jì)測(cè)量噪聲均方差為0.02g，磁傳感器測(cè)量噪聲均方差為0.02gauss，結(jié)果見(jiàn)圖4a～4f。
(2)原地連續(xù)轉(zhuǎn)彎試驗(yàn) 實(shí)驗(yàn)設(shè)計(jì)測(cè)試車原地沿半徑大約3米的圓周以大約40°/s的速度原地連續(xù)旋轉(zhuǎn)將近5分鐘，實(shí)際路況有明顯坑凹處(如下水道井蓋等)，但仍不會(huì)有3°以上的傾角變化。濾波參數(shù)選擇提高機(jī)動(dòng)加速度均方差為0.003g，姿態(tài)角誤差噪聲均方差為0.01°/s，陀螺零偏誤差噪聲均方差為0.0003°/s，加速度計(jì)測(cè)量噪聲均方差為0.01g，降低磁傳感器測(cè)量噪聲均方差為0.03gauss，結(jié)果見(jiàn)圖5a～5e。
(3)綜合跑車試驗(yàn) 實(shí)驗(yàn)設(shè)計(jì)測(cè)試車進(jìn)行超過(guò)半個(gè)小時(shí)的綜合跑車實(shí)驗(yàn)，包括加減速上下坡、原地轉(zhuǎn)動(dòng)、S形機(jī)動(dòng)、上臺(tái)階等，本次實(shí)驗(yàn)中設(shè)計(jì)了各種機(jī)動(dòng)情況和車載應(yīng)用可能遇到的狀況，結(jié)果見(jiàn)圖6。
(三)飛行試驗(yàn) 本航姿系統(tǒng)在某機(jī)型航拍系統(tǒng)進(jìn)行了飛行摸底，并同航拍高精度組合導(dǎo)航系統(tǒng)進(jìn)行了共計(jì)4000多秒的對(duì)比試驗(yàn)。航拍高精度組合導(dǎo)航系統(tǒng)采用加拿大Applanix公司的POS(Position and Orientation System)，航姿系統(tǒng)和POS系統(tǒng)數(shù)據(jù)采集用PDA。試驗(yàn)過(guò)程飛機(jī)在地面滑行一段時(shí)間后起飛，爬升至大約800米高空，在作業(yè)區(qū)域來(lái)回飛了3段折線，其中有3處明顯的長(zhǎng)時(shí)間協(xié)調(diào)轉(zhuǎn)彎，其他都處于平飛狀態(tài)。對(duì)比結(jié)果見(jiàn)圖7。
權(quán)利要求
1.機(jī)動(dòng)加速度輔助的擴(kuò)展卡爾曼濾波航姿系統(tǒng)姿態(tài)估計(jì)方法，其特征在于包括以下步驟
(1)慣性傳感器標(biāo)定，分別取標(biāo)定后載體坐標(biāo)系下的三軸角速度、三軸加速度及三軸地磁場(chǎng)，該慣性傳感器包括光纖陀螺儀、加速度計(jì)和磁傳感器；
(2)羅差校正，利用陀螺和地磁場(chǎng)的轉(zhuǎn)動(dòng)信息對(duì)三軸軟鐵和硬鐵效應(yīng)進(jìn)行校正；
(3)捷聯(lián)姿態(tài)解算采用四元數(shù)表示方法，進(jìn)行捷聯(lián)姿態(tài)解算；
(4)將載體機(jī)動(dòng)加速度描述為一種非零均值時(shí)間相關(guān)模型，即當(dāng)載體以某一加速度機(jī)動(dòng)時(shí)，下一瞬時(shí)的加速度只能在當(dāng)前加速度的鄰域內(nèi)，描述為
其中a為機(jī)動(dòng)加速度，
為下一時(shí)刻的機(jī)動(dòng)加速度，a為機(jī)動(dòng)加速度均值，取前一時(shí)刻的機(jī)動(dòng)加速度，且在每一采樣周期內(nèi)為常數(shù)，δa為零均值有色加速度噪聲，即為相鄰時(shí)刻的機(jī)動(dòng)加速度誤差，
為下一相鄰時(shí)刻的機(jī)動(dòng)加速度誤差，α為機(jī)動(dòng)頻率；wa為白噪聲；方差σa2為機(jī)動(dòng)加速度方差，取為常值；
(5)建立機(jī)動(dòng)加速度輔助的九態(tài)擴(kuò)展卡爾曼濾波算法，狀態(tài)量取為三個(gè)姿態(tài)角誤差、三軸陀螺的零偏誤差和載體系的三軸載體機(jī)動(dòng)加速度誤差，觀測(cè)量為三軸加速度誤差和三軸地磁場(chǎng)誤差；
(6)將機(jī)動(dòng)加速度輔助的九態(tài)擴(kuò)展卡爾曼濾波和捷聯(lián)姿態(tài)算法進(jìn)行數(shù)據(jù)融合，得到航姿系統(tǒng)姿態(tài)估計(jì)。
2.如權(quán)利要求1所述的機(jī)動(dòng)加速度輔助的擴(kuò)展卡爾曼濾波航姿系統(tǒng)姿態(tài)估計(jì)方法，其主要特征在于所述步驟(5)包括
(a)狀態(tài)方程
九態(tài)擴(kuò)展卡爾曼濾波器的誤差狀態(tài)量選取如下
其中δρ＝[∈x，∈y，∈z]T，是姿態(tài)角誤差向量；δb＝[bx，by，bz]T，是陀螺零偏誤差向量；δa＝[δax，δay，δaz]T，是載體系中的機(jī)動(dòng)加速度誤差向量，模型如式(2)所示；姿態(tài)角誤差方程忽略地球表觀運(yùn)動(dòng)角速度，以式(3)為狀態(tài)量的狀態(tài)方程為
其中
δb建模為一個(gè)隨機(jī)游走過(guò)程，αx，αy，αz分別為載體系內(nèi)三軸機(jī)動(dòng)加速度的機(jī)動(dòng)頻率；
(b)量測(cè)方程
量測(cè)方程中用到了兩種量測(cè)信息，分別是
①加速度誤差量測(cè)方程
取加速度計(jì)在載體系測(cè)得的加速度與估計(jì)加速度之差為觀測(cè)量；
②地磁場(chǎng)誤差量測(cè)方程
向量mn為地磁場(chǎng)向量在導(dǎo)航坐標(biāo)系的投影，mb為地磁場(chǎng)向量在載體坐標(biāo)系分量，航姿系統(tǒng)工作前在現(xiàn)場(chǎng)要首先進(jìn)行軟硬鐵補(bǔ)償，然后根據(jù)靜態(tài)初始化時(shí)的初始姿態(tài)矩陣和三軸磁傳感器測(cè)量值可得到當(dāng)?shù)氐膍n；取磁傳感器在載體系內(nèi)測(cè)得的三軸地磁場(chǎng)
與估計(jì)地磁場(chǎng)
之差δmb為觀測(cè)量
其中vm為載體系中磁場(chǎng)的量測(cè)噪聲，[mb×]是由向量mb構(gòu)成的反對(duì)稱陣
綜合加速度誤差量測(cè)方程中定義的載體系中加速度誤差和式(5)中定義的載體系中地磁場(chǎng)誤差，得到量測(cè)方程
Z(t)＝H(t)δx+v(t)(6)
其中
3.如權(quán)利要求2所述的機(jī)動(dòng)加速度輔助的擴(kuò)展卡爾曼濾波航姿系統(tǒng)姿態(tài)估計(jì)方法，其特征在于所述步驟(b)中的①具體為
取向量gn＝
T，為重力向量在導(dǎo)航坐標(biāo)系的投影，載體系中加速度計(jì)測(cè)得加速度為
為載體系中重力向量gb、載體加速度a、量測(cè)噪聲vf之和，其中加速度的常值零偏可歸結(jié)到載體加速度a中，
為加速度估計(jì)值，
為導(dǎo)航坐標(biāo)系到載體坐標(biāo)系變換矩陣的估計(jì)陣，取加速度計(jì)在載體系測(cè)得的加速度
與估計(jì)加速度
之差δfb為觀測(cè)量
全文摘要
本發(fā)明提出了一種機(jī)動(dòng)加速度輔助的擴(kuò)展卡爾曼濾波航姿系統(tǒng)姿態(tài)估計(jì)方法，其擴(kuò)展卡爾曼濾波的狀態(tài)量為三個(gè)姿態(tài)角誤差、三軸陀螺的零偏誤差和載體系的三軸載體機(jī)動(dòng)加速度誤差，觀測(cè)量為三軸加速度誤差和三軸地磁場(chǎng)誤差。將捷聯(lián)姿態(tài)算法和該九態(tài)擴(kuò)展卡爾曼濾波算法進(jìn)行數(shù)據(jù)融合，得到航姿系統(tǒng)姿態(tài)估計(jì)。通過(guò)單軸轉(zhuǎn)臺(tái)試驗(yàn)、車載動(dòng)態(tài)實(shí)驗(yàn)和飛行實(shí)驗(yàn)，驗(yàn)證了這種機(jī)動(dòng)加速度輔助的九態(tài)擴(kuò)展卡爾曼濾波數(shù)據(jù)融合算法在不同機(jī)動(dòng)情形下姿態(tài)角精度穩(wěn)定，其姿態(tài)角均方差可限制在2°以內(nèi)。
文檔編號(hào)G01C21/18GK101782391SQ20091008731
公開(kāi)日2010年7月21日 申請(qǐng)日期2009年6月22日 優(yōu)先權(quán)日2009年6月22日
發(fā)明者任章, 郭鵬飛 申請(qǐng)人:北京航空航天大學(xué)