本發(fā)明涉及設(shè)備精度評(píng)估領(lǐng)域，尤其涉及一種抗加速度干擾的姿態(tài)確定方法。

背景技術(shù)：

微小型無人機(jī)、云臺(tái)或手持云臺(tái)等需要測量和感知姿態(tài)的相關(guān)MEMS導(dǎo)航應(yīng)用領(lǐng)域，都需要使用MEMS陀螺儀對(duì)姿態(tài)變化進(jìn)行測量，但是MEMS陀螺儀在計(jì)算姿態(tài)過程中，由于器件誤差的存在，積分得到姿態(tài)過程中會(huì)引入較大的計(jì)算誤差，為此需要結(jié)合MEMS加速度計(jì)及MEMS磁強(qiáng)計(jì)對(duì)陀螺積分的姿態(tài)角進(jìn)行修正。現(xiàn)有綜合利用三種MEMS傳感器進(jìn)行姿態(tài)姿態(tài)確定的方法主要有互補(bǔ)濾波算法、卡爾曼濾波算法以及梯度下降法。當(dāng)只利用MEMS陀螺儀和MEMS加速度計(jì)時(shí)，主要是利用MEMS加速度計(jì)在無線加速度情況下能夠準(zhǔn)確敏感重力加速度，從而獲取載體姿態(tài)對(duì)MEMS陀螺儀積分得到的姿態(tài)進(jìn)行修正，當(dāng)載體有線加速度時(shí)，現(xiàn)有方法主要是通過減小MEMS加速度計(jì)計(jì)算的姿態(tài)對(duì)MEMS陀螺儀積分得到的姿態(tài)的修正力度，來減少線加速度的干擾。但是這種抗加速度干擾的方法，由于不能對(duì)線加速度干擾進(jìn)行有效估計(jì)，因此抗干擾效果較差。針對(duì)這一問題，提出了一種抗加速度干擾的姿態(tài)確定方法，提高姿態(tài)確定方法的精度和穩(wěn)定性。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的是提供一種抗加速度干擾的姿態(tài)確定方法，鑒于上述問題，它彌補(bǔ)了上述的缺陷并提供以下優(yōu)點(diǎn)：

一種抗加速度干擾的姿態(tài)確定方法，應(yīng)用于需要測量和感知姿態(tài)的設(shè)備中，其特征中，包括：

建立載體坐標(biāo)系的線加速度估計(jì)模型；

建立包含所述載體坐標(biāo)系的線加速度估計(jì)誤差的狀態(tài)方程；

建立以重力加速度矢量觀測誤差為量測量的量測方程；

利用所述線加速度估計(jì)誤差的狀態(tài)方程、所述以重力加速度矢量觀測誤差為量測量的量測方程和線加速度估計(jì)模型實(shí)時(shí)自適應(yīng)估計(jì)線加速度，以此確定所述設(shè)備的抗加速度干擾的姿態(tài)。

上述的方法，其中，所述方法還包括：

將載體系下的線加速度變化近似為一階馬爾科夫過程。

上述的方法，其中，所述方法還包括：

選取載體姿態(tài)角估計(jì)誤差、陀螺儀零偏估計(jì)誤差和載體系下的線加速度估計(jì)誤差作為濾波器的狀態(tài)量，以此構(gòu)建卡爾曼濾波狀態(tài)誤差向量。

上述的方法，其中，所述方法還包括：

將所述量測量選取由加速度計(jì)估計(jì)的重力加速度以及由陀螺儀估計(jì)的重力加速度的差值構(gòu)成三維觀測矢量。

上述的方法，其中，所述方法還包括：

自適應(yīng)調(diào)整所述線加速度估計(jì)模型中的濾波衰減系數(shù)。

綜上所述，本發(fā)明公開設(shè)計(jì)的一種抗加速度干擾的姿態(tài)確定方 法，與傳統(tǒng)的姿態(tài)確定方法相比，提出的方法能夠自適應(yīng)在線實(shí)時(shí)估計(jì)載體線加速度。與傳統(tǒng)的姿態(tài)確定方法相比，提出的姿態(tài)確定方法有效削弱了載體線加速度干擾，提高了姿態(tài)估計(jì)精度和穩(wěn)定性。

附圖說明

參考所附附圖，以更加充分的描述本發(fā)明的實(shí)施例。然而，所附附圖僅用于說明和闡述，并不構(gòu)成對(duì)本發(fā)明范圍的限制。

圖1是本發(fā)明的流程圖。

具體實(shí)施方式

下面結(jié)合附圖和具體的實(shí)施例對(duì)本發(fā)明作進(jìn)一步的說明，但是不作為本發(fā)明的限定。

如圖1所示，本申請(qǐng)?jiān)O(shè)計(jì)一種抗加速度干擾的姿態(tài)確定方法，具體的操作原理如下：

1、建立了載體坐標(biāo)系線加速度估計(jì)模型；

當(dāng)載體進(jìn)行加減速機(jī)動(dòng)或在振動(dòng)環(huán)境下，載體存在線加速度，可以將載體系下的線加速度變化近似為一階馬爾科夫過程，如式(1)所示，其中為t_k時(shí)刻的載體系線加速度，為t_k-1時(shí)刻的載體系線加速度，上標(biāo)B代表載體系。τ為一階馬爾科夫相關(guān)時(shí)間。在提出的方法中，c_a稱為線加速度估計(jì)模型的衰減系數(shù)，從模型中可以看出c_a與τ有關(guān)。

2、建立了包含載體坐標(biāo)系線加速度估計(jì)誤差的狀態(tài)方程；

選取載體姿態(tài)角估計(jì)誤差(Δγ為橫滾角誤差，Δφ為俯仰角誤差，為航向角誤差)、陀螺儀零偏估計(jì)誤差Δε_b＝[Δε_bx Δε_by Δε_bz]以及載體系下的線加速度估計(jì)誤差 作為濾波器的狀態(tài)量，構(gòu)建的卡爾曼濾波狀態(tài)誤差向量為ΔX＝[Δθ Δε_b Δa^B]^T。建立的狀態(tài)方程如式(2)所示，其中A_k+1/k＝0_9×9，狀態(tài)估計(jì)的一步預(yù)測方程簡寫為式(3)，狀態(tài)估值方程簡寫為式(4)。其中為誤差狀態(tài)量的先驗(yàn)估計(jì)值，為誤差狀態(tài)量的后驗(yàn)估計(jì)值，K_k為t_k時(shí)刻的卡爾曼濾波增益，Z_k為量測量，H_k為量測矩陣。

ΔX_k＝A_k/k-1ΔX_k-1+W_k-1 (2)

3、建立以重力加速度矢量觀測誤差為量測量的量測方程；

為了求解式(4)中的狀態(tài)估值，需要構(gòu)建量測量Z_k的模型。將量測量選取為由加速度計(jì)估計(jì)的重力加速度以及由陀螺儀估計(jì)的重力加速度的差值所構(gòu)成的3維觀測矢量，估計(jì)的重力加速度差值均投影在機(jī)體載體坐標(biāo)系中，構(gòu)建的量測方程如式(5)所示。

式中表示由加速度計(jì)估計(jì)的重力加速度在載體系的投影，其計(jì)算過程如式(6)所示，其中為t_k時(shí)刻的加速度計(jì)輸出，為 t_k-1時(shí)刻載體系下的線加速后驗(yàn)估計(jì)值，為t_k時(shí)刻載體系下的線加速度先驗(yàn)估計(jì)值。

式(5)中是陀螺儀估計(jì)的重力加速度在載體系的投影，其計(jì)算過程如式(7)所示。其中為t_k時(shí)刻導(dǎo)航系下的重力加速度矢量，為t_k時(shí)刻方向余弦矩陣的先驗(yàn)估計(jì)值，為t_k-1時(shí)刻的方向余弦矩陣后驗(yàn)估計(jì)值，下標(biāo)N代表導(dǎo)航系，為陀螺輸出修正陀螺零偏后的先驗(yàn)估計(jì)值積分得到的姿態(tài)變化矩陣。

通過量測量的獲取，經(jīng)過進(jìn)一步的推導(dǎo)，可以得到量測量與狀態(tài)量之間的關(guān)系如式(8)所示，而式中量測H_k可表示為式(9)，T為卡爾曼濾波周期。

4、自適應(yīng)調(diào)整線加速度估計(jì)模型中的濾波衰減系數(shù)；

當(dāng)姿態(tài)確定方法應(yīng)用于具體載體時(shí)，當(dāng)載體做加減速運(yùn)動(dòng)或者是在振動(dòng)環(huán)境中，可以通過自適應(yīng)調(diào)整式(1)所示的線加速度估計(jì)模型中的衰減系數(shù)c_a進(jìn)行線加速度干擾估計(jì)。c_a的自適應(yīng)調(diào)節(jié)過程見式(10)所示，其中為平穩(wěn)狀態(tài)下的線加速度誤差估值均方差， 為t_k時(shí)刻的線加速度誤差估計(jì)值。

5、利用線加速度估計(jì)誤差和線加速度估計(jì)模型，實(shí)時(shí)自適應(yīng)估計(jì)線加速度；

依據(jù)式(3)～式(9)構(gòu)建的狀態(tài)及量測方程，對(duì)載體姿態(tài)角誤差Δθ、陀螺儀零偏估計(jì)誤差Δε_b以及載體系下的線加速度估計(jì)誤差Δa^B的估計(jì)過程如式(11)所示。Q_k-1為t_k-1時(shí)刻的狀態(tài)噪聲矩陣，P_k/k-1為t_k時(shí)刻的狀態(tài)協(xié)方差先驗(yàn)估計(jì)矩陣，P_k為t_k時(shí)刻的狀態(tài)協(xié)方差后驗(yàn)估計(jì)矩陣，R_k為t_k時(shí)刻的量測噪聲矩陣。

基于估計(jì)出來的狀態(tài)誤差量進(jìn)一步對(duì)姿態(tài)角(主要是對(duì)姿態(tài)四元數(shù)進(jìn)行修正估計(jì))、陀螺零偏和載體系下的線加速度進(jìn)行修正估計(jì)。姿態(tài)角四元數(shù)和陀螺零偏的修正估計(jì)過程如式(12)所示。其中為t_k時(shí)刻的姿態(tài)四元數(shù)后驗(yàn)估計(jì)值，為t_k-1時(shí)刻的姿態(tài)四元數(shù)后驗(yàn)估計(jì)值，為t_k時(shí)刻的姿態(tài)四元數(shù)先驗(yàn)估計(jì)值，為由t_k時(shí)刻估計(jì)的姿態(tài)角誤差求解得到的姿態(tài)四元數(shù)增量，為陀螺輸出修正零偏后的先驗(yàn)估計(jì)值積分得到的姿態(tài)四元數(shù)增量，式(7)中的可通過進(jìn)行求解，可通過進(jìn)行求解。為t_k時(shí)刻的陀螺儀輸出，為t_k時(shí)刻的陀螺儀零偏先驗(yàn)估 計(jì)值，為t_k-1時(shí)刻的陀螺儀零偏后驗(yàn)估計(jì)值，為t_k時(shí)刻的陀螺儀零偏誤差估計(jì)值。

載體系下的線加速度修正估計(jì)過程如式(13)所示。式中為t_k時(shí)刻載體系下的線加速度后驗(yàn)估計(jì)值，為t_k時(shí)刻載體系下的線加速度先驗(yàn)估計(jì)值，為t_k時(shí)刻載體系下的線加速度估計(jì)誤差，為t_k-1時(shí)刻載體系下的線加速度后驗(yàn)估計(jì)值，c_a為線加速度估計(jì)模型中的濾波衰減系數(shù)。

本申請(qǐng)的抗加速度干擾的姿態(tài)確定方法可以應(yīng)用于無人機(jī)、云臺(tái)及手持云臺(tái)、穿戴式行人導(dǎo)航等。

綜上所述，本申請(qǐng)?jiān)O(shè)計(jì)的一種抗加速度干擾的姿態(tài)確定方法，與傳統(tǒng)的姿態(tài)確定方法相比，提出的方法能夠自適應(yīng)在線實(shí)時(shí)估計(jì)載體線加速度。與傳統(tǒng)的姿態(tài)確定方法相比，提出的姿態(tài)確定方法有效削弱了載體線加速度干擾，提高了姿態(tài)估計(jì)精度和穩(wěn)定性。

通過說明和附圖，給出了具體實(shí)施方式的特定結(jié)構(gòu)的典型實(shí)施例，基于本發(fā)明精神，還可作其他的轉(zhuǎn)換。盡管上述發(fā)明提出了現(xiàn)有的較佳實(shí)施例，然而，這些內(nèi)容并不作為局限。

對(duì)于本領(lǐng)域的技術(shù)人員而言，閱讀上述說明后，各種變化和修正 無疑將顯而易見。因此，所附的權(quán)利要求書應(yīng)看作是涵蓋本發(fā)明的真實(shí)意圖和范圍的全部變化和修正。在權(quán)利要求書范圍內(nèi)任何和所有等價(jià)的范圍與內(nèi)容，都應(yīng)認(rèn)為仍屬本發(fā)明的意圖和范圍內(nèi)。