專利名稱：：一種基于微慣性和地磁技術(shù)的自適應(yīng)三維姿態(tài)定位方法
技術(shù)領(lǐng)域：
：本發(fā)明屬于三維姿態(tài)控制
技術(shù)領(lǐng)域：
，涉及一種基于微慣性和地磁技術(shù)的高效自適應(yīng)三維姿態(tài)定位方法。
背景技術(shù)：
：隨著微電子機(jī)械系統(tǒng)(MEMS)的發(fā)展，特別是微慣性技術(shù)的發(fā)展，載體的位姿獲取已經(jīng)從原來的平臺(tái)慣導(dǎo)發(fā)展到捷聯(lián)慣導(dǎo)和組合導(dǎo)航技術(shù)，其系統(tǒng)的體積、重量和成本都大大降低，因此采用慣性/地磁技術(shù)進(jìn)行姿態(tài)的跟蹤檢測有著廣泛的前景。目前，國內(nèi)外已經(jīng)設(shè)計(jì)出了相當(dāng)多的微慣性姿態(tài)測量裝置用于姿態(tài)測量跟蹤，本申請人在申請?zhí)枮?200810114391.4”的中國專利“基于ZigBee無線單片機(jī)的微慣性測量裝置”中提供了一種可用于運(yùn)動(dòng)載體姿態(tài)測量的裝置，該專利在本申請中引入作為參考。在該申請中，采用六軸微慣性傳感器(集成了三軸微陀螺傳感器和三軸微加速度傳感器)和三軸磁場傳感器來測量運(yùn)動(dòng)載體的姿態(tài)，通過基于ZigBee無線單片機(jī)對所測得的信號進(jìn)行姿態(tài)解算，并將解算得到的姿態(tài)信息以無線方式傳送給其他系統(tǒng)或者上位機(jī)。慣性跟蹤技術(shù)的基本原理在于在已知初始位置和初始姿態(tài)等初始信息的基礎(chǔ)上，依據(jù)慣性原理，對三軸微陀螺傳感器獲取的角速度信息進(jìn)行積分獲得姿態(tài)；對三軸維加速度傳感器獲取的加速度信息進(jìn)行兩次積分獲取位置。但是由于存在積分累計(jì)誤差效應(yīng)，需要結(jié)合其他傳感器對其誤差值進(jìn)行實(shí)時(shí)更正，從而保證其數(shù)據(jù)的準(zhǔn)確性?；谌S微陀螺傳感器、三軸微加速度傳感器和三軸磁場傳感器進(jìn)行姿態(tài)定位的方法，其姿態(tài)值往往用四元數(shù)進(jìn)行描述，四元數(shù)的引入不僅簡化了計(jì)算，而且避免了歐拉角系統(tǒng)的萬向節(jié)死鎖問題。三軸微陀螺傳感器的一次積分獲得預(yù)測的姿態(tài)值，三軸微加速度傳感器和三軸磁場傳感器對預(yù)測的姿態(tài)值進(jìn)行實(shí)時(shí)更正。目前的主流姿態(tài)跟蹤算法為卡爾曼濾波通過三軸微陀螺傳感器的預(yù)測以及三軸微加速度傳感器和三軸磁場傳感器的更正來獲取姿態(tài)信息。但是該算法復(fù)雜，卡爾曼濾波涉及到的觀測噪聲以及系統(tǒng)噪聲對結(jié)果精度影響較大，而周圍的噪聲往往可能不是假定的白噪聲，且對噪聲參數(shù)的獲取往往較為困難。Bachmann提出了一種基于四元數(shù)的“互補(bǔ)“濾波的方法(見Bachmarm，E.R.，Duman,I.,Usta,U.Y.,McGhee,R.B.,Yun,X.P.,Zyda,M.J.,“OrientationTrackingforHumansandRobotsUsingInertialSensors,"Proc.of1999SymposiumonComputationalIntelligenceinRobotics&Automation,Monterey,CA,November1999)。該方法通過使用高斯-牛頓迭代方法，利用三軸微加速度傳感器和三軸磁場傳感器的數(shù)據(jù)獲得當(dāng)前姿態(tài)的最佳誤差值，然后與三軸微陀螺傳感器的數(shù)據(jù)融合，從而獲得姿態(tài)值。與卡爾曼濾波相比，該算法大大降低了計(jì)算復(fù)雜性，并能取得較好的姿態(tài)準(zhǔn)確性。然而該算法沒有考慮到噪聲干擾等問題。在此基礎(chǔ)上，JungKeunLee提出了利用虛擬旋轉(zhuǎn)的概念來避免高斯-牛頓算法中的矩陣逆運(yùn)算(見JungKeunLee；Park,Ε.J,"AFastQuaternion-BasedOrientationOptimizerviaVirtualRotationforHumanMotionTracking，，，BiomedicalEngineering,IEEETransactionsonVolume56,Issue5,May2009Page(S)1574-1582)，并提出了一種通過檢測三軸微加速度傳感器和三軸磁場傳感器噪聲是否超出閾值來確定某時(shí)刻獲取的加速傳感器和磁場傳感器數(shù)據(jù)是否有效。但是該方法對于當(dāng)加速度傳感器和磁場傳感器數(shù)據(jù)有效時(shí)的存在的噪聲沒有進(jìn)行考慮。國內(nèi)方面也有一些關(guān)于利用三軸微陀螺傳感器、三軸微加速度傳感器和三軸磁場傳感器來獲取姿態(tài)的相關(guān)專利，本申請人在申請?zhí)枮椤?00810224898.5”的中國專利“考慮加速度補(bǔ)償和基于無跡卡爾曼濾波的慣性位姿跟蹤方法”中提供一種對加速度進(jìn)行補(bǔ)償?shù)臒o跡卡爾曼濾波慣性位姿跟蹤方法，該專利在本申請中引入作為參考。
發(fā)明內(nèi)容為了解決現(xiàn)有技術(shù)的問題，本發(fā)明的目的是充分考慮計(jì)算復(fù)雜度以及運(yùn)動(dòng)載體瞬時(shí)加速度或周圍環(huán)境電磁場的干擾對三軸微加速度傳感器和三軸磁場傳感器的影響，提出一種考慮微慣性組合測量裝置瞬時(shí)加速度和周圍環(huán)境電磁場干擾的基于降階高斯-牛頓方法的快速自適應(yīng)三維姿態(tài)的定位方法。為了達(dá)到上述的發(fā)明目的，本發(fā)明提出了一種基于微慣性和地磁技術(shù)的自適應(yīng)三維姿態(tài)定位方法，該方法包括以下步驟步驟Sl利用集成了三軸微陀螺傳感器、三軸微加速度傳感器和三軸磁場傳感器的微慣性組合測量裝置來感應(yīng)運(yùn)動(dòng)載體的運(yùn)動(dòng)姿態(tài)三軸微陀螺傳感器感應(yīng)運(yùn)動(dòng)載體沿軸向的角速度信號，三軸微加速度傳感器感應(yīng)運(yùn)動(dòng)載體加速度信號，三軸磁場傳感器感應(yīng)地球磁場信號；步驟S2設(shè)置微慣性組合測量裝置的初始姿態(tài)值和全局坐標(biāo)系下的三軸微加速度傳感器的加速度數(shù)據(jù)和三軸磁場傳感器的地球磁場數(shù)據(jù)在確保三軸微陀螺傳感器、三軸微加速度傳感器和三軸磁場傳感器坐標(biāo)系對準(zhǔn)的情況下，保持微慣性組合測量裝置固定不動(dòng)，采集三軸微加速度傳感器的加速度數(shù)據(jù)和三軸磁場傳感器感應(yīng)的地球磁場數(shù)據(jù)；設(shè)置微慣性組合測量裝置初始時(shí)刻即i=0時(shí)的四元數(shù)形式姿態(tài)值qO=[1000]τ、三軸微加速度傳感器矢量nYA=[a0xa0yaQz]T和三軸磁場傳感器矢量1^=[m0xm0ymQz]T，對三軸微加速度傳感器矢量nYA和三軸磁場傳感器矢量nYM進(jìn)行單位化，得到單位三軸微加速度傳感器矢量nsA和單位三軸磁場傳感器矢量nSM，所述單位化公式為nsA=1VιιnYA11和nsM=1VιιnYM11，式中aQx、aQy、a0z表示初始時(shí)刻即i=0時(shí)的全局坐標(biāo)系下的三軸微加速度傳感器的加速度數(shù)據(jù)，m0x,m0y,Hitlz表示初始時(shí)刻即i=0時(shí)的全局坐標(biāo)系下的是三軸磁場傳感器的地球磁場數(shù)據(jù)，下標(biāo)0表示的是初始時(shí)刻，下標(biāo)x、y、z表示的是每個(gè)傳感器坐標(biāo)系下的三個(gè)坐標(biāo)軸；上標(biāo)η表示的是全局坐標(biāo)系下的傳感器數(shù)據(jù)、下標(biāo)A和M分別表示的是三軸微加速度傳感器和三軸磁場傳感器的數(shù)據(jù)；S表示的是單位化后相應(yīng)傳感器數(shù)據(jù)；上標(biāo)T表示矢量的轉(zhuǎn)置；步驟S3對微慣性組合測量裝置的姿態(tài)值進(jìn)行求解利用當(dāng)前時(shí)刻即i=t時(shí)的三軸微陀螺傳感器的角速度數(shù)據(jù)、三軸微加速度傳感器的加速度數(shù)據(jù)和三軸磁場傳感器的地球磁場數(shù)據(jù)，當(dāng)前時(shí)刻t的姿態(tài)值qt以及步驟2獲得的初始姿態(tài)值％、三軸微加速度傳感器的加速度數(shù)據(jù)nYA、nSA和三軸磁場傳感器的地球磁場數(shù)據(jù)nYM、nSM，來獲取微慣性組合測量裝置下一時(shí)刻即i=t+At時(shí)的姿態(tài)值qt+M；步驟S4利用三軸微陀螺傳感器數(shù)據(jù)對i=t+At時(shí)刻微慣性組合測量裝置的姿態(tài)值進(jìn)行預(yù)測將當(dāng)前時(shí)刻t的三軸微陀螺傳感器的角速度數(shù)據(jù)表示為當(dāng)前姿態(tài)下的旋轉(zhuǎn)角速度四元數(shù)Qe=W"txcotyωtx]T，當(dāng)前時(shí)刻t的三軸微加速度傳感器的加速度數(shù)據(jù)和三軸磁場傳感器的磁場強(qiáng)度數(shù)據(jù)分別為bYA=[atxatyatz]mbYM=[mtxmtymtz]T，其中G、b為標(biāo)記號，ωχ、coty、ωζ表示的是當(dāng)前時(shí)刻t的三軸陀螺傳感器的角速度數(shù)據(jù)，atx、aty、atz表示的是當(dāng)前時(shí)刻t的三軸微加速度傳感器的加速度數(shù)據(jù)，mtx、mty、mtz表示的是當(dāng)前時(shí)刻t的三軸磁場傳感器的地球磁場數(shù)據(jù)；利用微慣性組合測量裝置當(dāng)前時(shí)刻t的姿態(tài)值qt和當(dāng)前時(shí)刻t的三軸微陀螺傳感器的旋轉(zhuǎn)角速度四元數(shù)Qe，對i=t+Δt時(shí)刻的微慣性組合測量裝置的姿態(tài)值么進(jìn)行預(yù)測，由于兩個(gè)時(shí)刻之間的間隔Δt較小，假設(shè)認(rèn)為t和t+Δt兩個(gè)時(shí)刻之間三軸微陀螺傳感器輸出沒有變化，從而獲得姿態(tài)預(yù)測纟…,方程中式對預(yù)測后的四元數(shù)進(jìn)行單位化式中，qt為當(dāng)前時(shí)刻t的微慣性組合測量裝置的姿態(tài)值義為下一時(shí)刻預(yù)測姿態(tài)值，At為采樣時(shí)間間隔，么為四元數(shù)形式的當(dāng)前時(shí)刻t全局姿態(tài)下的旋轉(zhuǎn)角速度；步驟S5對步驟S4獲得的三軸微加速度傳感器的加速度數(shù)據(jù)和三軸磁場傳感器地球磁場數(shù)據(jù)進(jìn)行置信判斷，判定周圍環(huán)境是否存在電磁場干擾或瞬時(shí)加速度干擾；若干擾過大，則放棄該時(shí)刻采集的三軸磁場傳感器的地球磁場數(shù)據(jù)或三軸微加速度傳感器的加速度數(shù)據(jù)，否則以測得的周圍環(huán)境噪聲的大小設(shè)置自適應(yīng)因子，該自適應(yīng)因子是表征不存在較大的電磁干擾或是瞬時(shí)加速度情況下的三軸微加速度傳感器和三軸磁場傳感器的噪聲大小，從而基于該自適應(yīng)因子對三軸微加速度傳感器的加速度數(shù)據(jù)或是三軸磁場傳感器的地球磁場數(shù)據(jù)的影響進(jìn)行權(quán)重設(shè)置如下Μ=\式中bSM為調(diào)整后的三軸磁場傳感器矢量、bSA為調(diào)整后的三軸微加速度傳感器矢量；kA為三軸微加速度傳感器自適應(yīng)因子，kM為三軸磁場傳感器自適應(yīng)因子；εΑ為三軸微加速度傳感器噪聲上界，εΜ為三軸磁場傳感器噪聲上界；所述干擾過大是超過公式(4)中的三軸微加速度傳感器噪聲上界εΑ或三軸磁場傳感器噪聲上界εΜ，εΑ、εΜ的取值范圍是(0-1)，周圍環(huán)境噪聲越大，該噪聲上界εΑ、εΜ值應(yīng)取值越小，具體取值應(yīng)依據(jù)在現(xiàn)場環(huán)境中的實(shí)驗(yàn)決定；步驟S6利用步驟S5調(diào)整后的三軸微加速度傳感器的加速度數(shù)據(jù)和三軸磁場傳感器的地球磁場數(shù)據(jù)獲得微慣性組合測量裝置的姿態(tài)誤差值瓦，該姿態(tài)誤差值通過高斯_牛頓方法獲得式中bnm,+J=2[λ]Χ==個(gè)分量，式中λ表示為Wl+&,ysA，μ表示為:U"SM，λχ、λy、λz為向量λ的Jμχ、yy、μζ為向量μ的三個(gè)分量，qQ、如q2、q3為四元數(shù)禮J勺四個(gè)分量，1(《+Δ,)為四元數(shù)禮Δ,轉(zhuǎn)換成的旋轉(zhuǎn)矩陣；矩陣Ξ只與初始標(biāo)定時(shí)刻的三軸微加速度傳感器的加速度數(shù)據(jù)和三軸磁場傳感器的地球磁場數(shù)據(jù)有關(guān)，所以在姿態(tài)方法中只要進(jìn)行一次標(biāo)定，獲取Ξ矩陣及其逆矩陣，無需在其他時(shí)刻進(jìn)行矩陣逆運(yùn)算；步驟S7基于步驟S4得到的姿態(tài)預(yù)測信息、步驟S6得到微慣性組合測量裝置的姿態(tài)誤差值和步驟S5得到的自適應(yīng)因子進(jìn)行融合，獲得t+Δt時(shí)刻微慣性組合測量裝置姿態(tài)值qt+At=qt+At+qt+At[1qe]kAkMAt然后再對其單位化+Δ=qt+.t/||qt+M(21)；其中表示的是四元數(shù)的乘法運(yùn)算；步驟S8輸出姿態(tài)信息根據(jù)四元數(shù)和歐拉角之間的轉(zhuǎn)換關(guān)系，將四元數(shù)轉(zhuǎn)換為具有直觀意義的、自適應(yīng)三維姿態(tài)的俯仰角α、橫滾角β和航向角γ如下a=arctg(P=arcsm(-2(qiq3+q0q2))r=arctg(ae(-180,180]β&(-90,90]re(-180,180](22)。其中利用三軸微陀螺傳感器的預(yù)測以及三軸微加速度傳感器和三軸磁場傳感器數(shù)據(jù)的更正獲取姿態(tài)信息。其中微慣性組合測量裝置的初始姿態(tài)標(biāo)定需要進(jìn)行一次矩陣逆求解，在預(yù)測過程和更正過程所有的姿態(tài)信息均以四元數(shù)的形式進(jìn)行表示和運(yùn)算。其中考慮周圍環(huán)境中的電磁場對地球磁場的干擾和瞬時(shí)加速度的影響，如果干擾變化超過閾值，則三軸微加速度傳感器和三軸磁場傳感器的數(shù)據(jù)進(jìn)行舍棄；如果干擾變化沒超過閾值，則按照干擾的大小設(shè)置三軸微加速度傳感器和三軸磁場傳感器數(shù)據(jù)的自適應(yīng)因子。本發(fā)明的有益效果采用本發(fā)明方法可以得到如下優(yōu)點(diǎn)一方面，本發(fā)明與傳統(tǒng)的卡爾曼濾波算法相比，大大降低了算法的復(fù)雜性，而算法的準(zhǔn)確性與其他算法相比卻沒有明顯的降低，甚至有些情況下與傳統(tǒng)算法相比計(jì)算精度有了提高。從圖4、圖5、圖6和表7中能夠看到本發(fā)明方法與主流的姿態(tài)解算算法的輸出沒有差別，表1中列出了與主流的姿態(tài)解算算法的比較數(shù)據(jù)。另一方面，本發(fā)明方法中的自適應(yīng)性使得在周圍環(huán)境存在電磁場干擾以及瞬時(shí)加速度的情況下，降低了干擾對裝置姿態(tài)值準(zhǔn)確性的影響。如圖4示出為靜止、周圍環(huán)境的無磁干擾的情況下，本發(fā)明方法與主流姿態(tài)傳感器MTi結(jié)果的比較。其中本發(fā)明方法的姿態(tài)輸出為實(shí)線表示，MTi傳感器的輸出為虛線表示。圖4中含有的(a)圖為MTi傳感器與四階龍格庫塔方法的航向角比較；圖4中含有的(b-d)圖為MTi傳感器和本算法的橫滾角、航向角和俯仰角的比較；如圖5示出為靜止，周圍環(huán)境存在磁干擾的情況下，本發(fā)明方法與主流姿態(tài)傳感器MTi結(jié)果的比較。其中本發(fā)明方法的姿態(tài)輸出為實(shí)線表示，MTi傳感器的輸出為虛線表示。圖5中含有的(a-c)圖三個(gè)坐標(biāo)軸的測得的地磁場變化；圖5中含有的(d-e)圖為MTi傳感器和本算法的橫滾角、俯仰角和航向角的比較；在圖5的情況下，當(dāng)裝置靜止時(shí)，本發(fā)明方法和傳統(tǒng)方法都可以準(zhǔn)確得出當(dāng)前姿態(tài)值，但是對于周圍環(huán)境突然的磁場干擾，傳統(tǒng)方法已經(jīng)無法準(zhǔn)確計(jì)算姿態(tài)值，而本發(fā)明方法卻不受到相應(yīng)的干擾。如圖6示出為周圍環(huán)境比較理想，瞬時(shí)加速情況下，本發(fā)明方法與主流姿態(tài)傳感器MTi結(jié)果的比較。其中本發(fā)明方法的姿態(tài)輸出為實(shí)線表示，MTi傳感器的輸出為虛線表示。圖6中含有的(a-c)圖為三個(gè)坐標(biāo)軸的測得的加速度的變化；圖6中含有的(d-e)圖為MTi傳感器和本發(fā)明方法的橫滾角、俯仰角和航向角的比較；在圖6情況下，對于瞬時(shí)的加速度變化，可以看到本發(fā)明方法提出的自適應(yīng)因子能夠緩和姿態(tài)值的突然的較大波動(dòng)，從而提高了姿態(tài)值的準(zhǔn)確性。如下示出表1為在圖4和圖6測試環(huán)境下，MTi和本發(fā)明方法的姿態(tài)準(zhǔn)確性的比較，RMSE表示為均方誤差大小。MTi本發(fā)明差異RMSEroll0.40/0.350.38/0.20-0.02/-0.15RMSEpitch0.31/4.810.14/4.64-0.17/-0.17RMSEyaw0.18/0.640.44/0.880.26/0.2圖1為本發(fā)明所適用的慣性/地磁姿態(tài)跟蹤裝置的結(jié)構(gòu)圖；圖2為本發(fā)明的一種基于微慣性/地磁技術(shù)的快速自適應(yīng)算法的結(jié)構(gòu)框圖3進(jìn)行高斯_牛頓迭代次數(shù)為1次，2次，3次的誤差更正值的模值變化；圖4為靜止，無周圍環(huán)境的磁干擾的情況下，本發(fā)明與主流姿態(tài)傳感器MTi結(jié)果的比較(內(nèi)部為卡爾曼濾波算法)。圖5為靜止，存在周圍磁干擾的情況下，本發(fā)明與主流姿態(tài)傳感器MTi結(jié)果的比較(內(nèi)部為卡爾曼濾波算法)。圖6為周圍環(huán)境比較理想時(shí)，突然加速情況下，本發(fā)明與主流姿態(tài)傳感器MTi結(jié)果的比較(內(nèi)部為卡爾曼濾波算法)。圖7為三維姿態(tài)定位算法的軟件流程圖。具體實(shí)施例方式為使本發(fā)明的目的、技術(shù)方案和優(yōu)點(diǎn)更加清楚明白，以下結(jié)合具體實(shí)施例，并參照附圖，對本發(fā)明進(jìn)一步詳細(xì)說明。如圖1示出利用本發(fā)明三維微慣性和地磁姿態(tài)跟蹤裝置，在三軸坐標(biāo)系的X軸、Y軸和Z軸上分別設(shè)有由微加速度傳感器、微陀螺傳感器和磁場傳感器組成的一組傳感器，圖1中“□”表示微加速度傳感器、“〇”表示微陀螺傳感器、“□”表示磁場傳感器。該慣性和地磁姿態(tài)跟蹤裝置以正交方式集成了三個(gè)微加速度傳感器、三個(gè)微陀螺傳感器和三個(gè)磁場傳感器，三組傳感器所在的正交軸構(gòu)成了裝置X軸、Y軸和Z軸的三軸坐標(biāo)系。如圖2所示，本發(fā)明一種基于微慣性和地磁技術(shù)的自適應(yīng)三維姿態(tài)的定位方法，在此僅對本發(fā)明方法相關(guān)步驟中的具體實(shí)現(xiàn)細(xì)節(jié)進(jìn)行如下說明本發(fā)明所描述的算法的采樣周期At依據(jù)不同的運(yùn)行環(huán)境而不同。步驟Sl利用集成的三軸微陀螺傳感器、三軸微加速度傳感器和三軸磁場傳感器的微慣性組合測量裝置來感應(yīng)運(yùn)動(dòng)載體的運(yùn)動(dòng)姿態(tài)三軸微陀螺傳感器感應(yīng)運(yùn)動(dòng)載體沿軸向的角速度信號，三軸微加速度傳感器感應(yīng)運(yùn)動(dòng)載體加速度信號，三軸磁場傳感器感應(yīng)地球磁場信號；步驟S2設(shè)置微慣性組合測量裝置的初始姿態(tài)值和全局坐標(biāo)系下的三軸微加速度傳感器的加速度數(shù)據(jù)和三軸磁場傳感器的地球磁場數(shù)據(jù)在確保三軸微陀螺傳感器、三軸微加速度傳感器和三軸磁場傳感器坐標(biāo)系對準(zhǔn)的情況下，保持微慣性組合測量裝置固定不動(dòng)，采集三軸微加速度傳感器的加速度數(shù)據(jù)和三軸磁場傳感器感應(yīng)的地球磁場數(shù)據(jù)；設(shè)置微慣性組合測量裝置初始時(shí)刻即i=0時(shí)的四元數(shù)形式姿態(tài)值Qtl=[1000]τ、三軸微加速度傳感器矢量nYA=[a0xa0yaQz]T和三軸磁場傳感器矢量1^=[m0xm0ymQz]T，對三軸微加速度傳感器矢量nYA和三軸磁場傳感器矢量nYM進(jìn)行單位化，得到單位三軸微加速度傳感器矢量nSA和單位三軸磁場傳感器矢量nSM，所述單位化公式為nsA=1VιιnYA11和nsM=1VιιnYM11，其中aQx、aQy、a0z表示初始時(shí)刻即i=0時(shí)的全局坐標(biāo)系下的三軸微加速度傳感器的加速度數(shù)據(jù)，m0x,m0y,Hitlz表示初始時(shí)刻即i=0時(shí)的全局坐標(biāo)系下的是三軸磁場傳感器的地球磁場數(shù)據(jù)，下標(biāo)0表示的是初始時(shí)刻，下標(biāo)x、y、z表示的是每個(gè)傳感器坐標(biāo)系下的三個(gè)坐標(biāo)軸；上標(biāo)η表示的是全局坐標(biāo)系下的傳感器數(shù)據(jù)、下標(biāo)A和M分別表示的是三軸微加速度傳感器和三軸磁場傳感器的數(shù)據(jù)；S表示的是單位化后相應(yīng)傳感器數(shù)據(jù)；上標(biāo)T表示矢量的轉(zhuǎn)置；步驟S3對微慣性組合測量裝置的姿態(tài)值進(jìn)行求解利用當(dāng)前時(shí)刻i=t的三軸微陀螺傳感器的角速度數(shù)據(jù)、三軸微加速度傳感器的加速度數(shù)據(jù)和三軸磁場傳感器的地球磁場數(shù)據(jù)，當(dāng)前時(shí)刻t的姿態(tài)值qt以及步驟2設(shè)定的初始姿態(tài)值％、三軸微加速度傳感器的加速度數(shù)據(jù)nYA、nsA和三軸磁場傳感器的地球磁場數(shù)據(jù)nYM、nSM，來獲取微慣性組合測量裝置下一時(shí)刻即i=t+Δt時(shí)的姿態(tài)值qt+,t；步驟S4利用三軸微陀螺傳感器數(shù)據(jù)對i=t+Δt時(shí)刻微慣性組合測量裝置的姿態(tài)值進(jìn)行預(yù)測將當(dāng)前時(shí)刻t的三軸微陀螺傳感器的角速度數(shù)據(jù)表示為當(dāng)前姿態(tài)下的旋轉(zhuǎn)角速度四元數(shù)Qe=W"txcotyωtz]T，當(dāng)前時(shí)刻t的三軸微加速度傳感器的加速度數(shù)據(jù)和三軸磁場傳感器的地球磁場數(shù)據(jù)分別為bYA=[atxatyatz]mbYM=[mtxmtymtz]T，其中G、b為標(biāo)記號，ωχ、coty、ωζ表示的是當(dāng)前時(shí)刻t的三軸陀螺傳感器的角速度數(shù)據(jù)，atx、aty、atz表示的是當(dāng)前時(shí)刻t的三軸微加速度傳感器的加速度數(shù)據(jù)，mtx、mty、mtz表示的是當(dāng)前時(shí)刻t的三軸磁場傳感器的地球磁場數(shù)據(jù)；利用微慣性組合測量裝置當(dāng)前時(shí)刻t的姿態(tài)值qt和當(dāng)前時(shí)刻t的三軸微陀螺傳感器的旋轉(zhuǎn)角速度四元數(shù)Qe，對i=t+Δt時(shí)刻的微慣性組合測量裝置的姿態(tài)值么行預(yù)測，由于兩個(gè)時(shí)刻之間的間隔Δt較小，假設(shè)認(rèn)為t和t+Δt兩個(gè)時(shí)刻之間三軸微陀螺傳感器輸出沒有變化，從而獲得姿態(tài)預(yù)測方程其中么二(2)對預(yù)測后的四元數(shù)進(jìn)行單位化(3)其中，qt為當(dāng)前時(shí)刻t的微慣性組合測量裝置的姿態(tài)值，么為下一時(shí)刻預(yù)測姿態(tài)值，At為采樣時(shí)間間隔，么為四元數(shù)形式的當(dāng)前時(shí)刻t全局姿態(tài)下的旋轉(zhuǎn)角速度；步驟S5對步驟S4獲得的三軸微加速度傳感器的加速度數(shù)據(jù)和三軸磁場傳感器地球磁場數(shù)據(jù)進(jìn)行置信判斷，判定周圍環(huán)境是否存在電磁場干擾或瞬時(shí)加速度干擾；若干擾過大，則放棄該時(shí)刻采集的三軸磁場傳感器的地球磁場數(shù)據(jù)或三軸微加速度傳感器的加速度數(shù)據(jù)，否則以測得的周圍環(huán)境噪聲的大小設(shè)置自適應(yīng)因子(該自適應(yīng)因子是表征不存在較大的電磁干擾或是瞬時(shí)加速度情況下的三軸微加速度傳感器和三軸磁場傳感器的噪聲大小)從而基于該自適應(yīng)因子對三軸微加速度傳感器的加速度數(shù)據(jù)或是三軸磁場傳感器的地球磁場數(shù)據(jù)的影響進(jìn)行權(quán)重設(shè)置；置信判斷標(biāo)準(zhǔn)如下假設(shè)目前時(shí)刻的姿態(tài)數(shù)據(jù)準(zhǔn)確，將初始標(biāo)定時(shí)刻獲得的三軸微加速度傳感器和三軸磁場傳感器數(shù)據(jù)結(jié)合當(dāng)前的姿態(tài)數(shù)據(jù)預(yù)測出當(dāng)前測得的三軸微加速度傳感器和三軸磁場傳感器數(shù)據(jù)，通過將預(yù)測的三軸微加速度傳感器和三軸磁場傳感器數(shù)據(jù)與當(dāng)前時(shí)刻的三軸微加速度傳感器和三軸磁場傳感器數(shù)據(jù)進(jìn)行模值比較如果不存在任何干擾和噪聲，二者應(yīng)該大小一致；如果二者之間的差值超過了設(shè)定的閾值，則認(rèn)為當(dāng)前測得的三軸微加速度傳感器或是三軸磁場傳感器數(shù)據(jù)不準(zhǔn)確，應(yīng)該予以舍棄，并以預(yù)測的三軸微加速度傳感器和三軸磁場傳感器的數(shù)據(jù)為準(zhǔn)；如果二者之間的差值在設(shè)定的閾值內(nèi)，則表明當(dāng)前時(shí)刻測得的三軸微加速度傳感器和三軸磁場傳感器不存在干擾，只存在噪聲，并且差值的大小表明了噪聲的強(qiáng)弱，對差值信息進(jìn)行處理，從而獲得三軸微加速度傳感器和三軸磁場傳感器的自適應(yīng)因子，具體的實(shí)現(xiàn)公式如下其中bSM為調(diào)整后的三軸磁場傳感器矢量、bSA為調(diào)整后的三軸微加速度傳感器矢量；kA為三軸微加速度傳感器自適應(yīng)因子，kM為三軸磁場傳感器自適應(yīng)因子；εΑ為三軸微加速度傳感器噪聲上界，εΜ為三軸磁場傳感器噪聲上界；所述干擾過大是超過公式(4)中的三軸微加速度傳感器噪聲上界εΑ或三軸磁場傳感器噪聲上界εΜ，εΑ、εΜ的取值范圍是(0-1)，周圍環(huán)境噪聲越大，該噪聲上界εΑ、εΜ值應(yīng)取值越小，實(shí)施時(shí)三軸微加速度傳感器的噪聲上界εΑ選取可為0.8;三軸磁場傳感器噪聲上界εΜ選取可為0.9。噪聲上界εΑ、εΜ的數(shù)值選擇，不僅僅限于實(shí)施例的數(shù)據(jù)，噪聲上界εΑ、εΜ的具體取值應(yīng)依據(jù)在現(xiàn)場環(huán)境中的實(shí)驗(yàn)決定；步驟S6利用步驟S5處理后的三軸微加速度傳感器的加速度數(shù)據(jù)和三軸磁場傳感器的地球磁場數(shù)據(jù)獲得微慣性組合測量裝置的姿態(tài)誤差值瓦，該誤差值基于高斯_牛頓迭代法獲得。利用高斯牛頓方法來獲得誤差更正值的原理如下給定預(yù)測的單位化的姿態(tài)值，初始標(biāo)定的三軸微加速度傳感器和三軸磁場傳感器的矢量nsA和nSM，以及對干擾判定后的三軸微加速度傳感器和三軸磁場傳感器的矢量bsA，bsM，可以使用高斯-牛頓迭代來尋求當(dāng)前最優(yōu)的四元數(shù)，從而滿足如下公式其中α，β為三軸微加速度傳感器和三軸磁場傳感器的在高斯牛頓方法中的姿態(tài)更正的影響權(quán)重，一般設(shè)置為1。高斯-牛頓方法可以找到使得誤差值豹最小的最優(yōu)姿態(tài)四元數(shù)$。一般情況下，預(yù)測值與最優(yōu)姿態(tài)四元數(shù)之間只存在一個(gè)較小的旋轉(zhuǎn)誤差其中θ^表示旋轉(zhuǎn)的角度誤差，w表示的是旋轉(zhuǎn)軸。從而高斯-牛頓迭代轉(zhuǎn)化為尋求更正誤差值瓦1qe]))假定初始更正誤差值為0，則一次迭代后的更正誤差值為其中，高斯_牛頓迭代的雅各比矩陣為經(jīng)過相應(yīng)的矩陣化簡運(yùn)算其中，J為/(.J的縮寫，纟為纟的縮寫。從而得到了下面表達(dá)式。又由于從而得到一次迭代的更正誤差值為^其中α，β設(shè)置為1，從而獲得更正誤差值為其中Ξ=21-CSa"S/+"SM"Smt)(17)bnR(q'l+J=20.5-W-WH-她H+<1^2mi+<h<h0.51212財(cái)3-H則3—紐2H+<h<h0·512~2(18)W其中λ表示為M;+J"SA,U表示為XUSm,，λχ、λ”λζ為向量λ的三個(gè)分量，μχ、yy、μζ為向量μ的三個(gè)分量，qQ、Ql、q2、q3為四元數(shù)禮J勺四個(gè)分量，為四元數(shù)么、,轉(zhuǎn)換成的旋轉(zhuǎn)矩陣；因?yàn)榫仃嚘皇桥c初始標(biāo)定時(shí)刻的三軸微加速度傳感器的加速度數(shù)據(jù)和三軸磁場傳感器地球磁場數(shù)據(jù)有關(guān)，所以在算法中只要進(jìn)行一次標(biāo)定，獲取Ξ矩陣及其逆矩陣，無需在其他時(shí)刻進(jìn)行矩陣逆運(yùn)算；對于高斯-牛頓迭代次數(shù)問題，通過實(shí)驗(yàn)——迭代一次，兩次，三次后姿態(tài)誤差值的模值變化，見附圖3中含有的(a)圖、(b)圖、(c)圖，得知迭代次數(shù)的增加對姿態(tài)的準(zhǔn)確性沒有明顯的變化(姿態(tài)誤差值模值沒有明顯的減小)，在本方法中只需進(jìn)行一次高斯-牛頓迭代即可獲得較為準(zhǔn)確的姿態(tài)誤差值；步驟S7對步驟S4得到的姿態(tài)預(yù)測信息以及步驟S6得到微慣性組合測量裝置的姿態(tài)誤差值以及步驟S5得到的自適應(yīng)因子進(jìn)行融合，獲得t+Δt時(shí)刻微慣性組合測量裝置姿態(tài)值^t^i^t+L·.,^IKkuAt(21)然后再對其單位化qt+At=qt+At/|qt+.t|；其中，為步驟S3獲取，kAkM為步驟S5獲取，表示的是四元數(shù)的乘法運(yùn)算；步驟S8輸出姿態(tài)信息根據(jù)四元數(shù)和歐拉角之間的轉(zhuǎn)換關(guān)系，將四元數(shù)轉(zhuǎn)換為具有直觀意義、自適應(yīng)三維姿態(tài)的的俯仰角α、橫滾角β和航向角γ如下a=arctg(^2|3ZMll)廠=arcsin(-2(仍妁+犰辦))r=arctg(ae(-180,180])8e(-90,90]re(-180,180](22)。如圖7所示，本實(shí)施例的工作流程如下1)系統(tǒng)初始化模塊姿態(tài)跟蹤設(shè)備一般在靜態(tài)情況下啟動(dòng)。首先是進(jìn)行系統(tǒng)初始化，如對各個(gè)傳感器的信號的檢測，處理器芯片的初始化設(shè)置，通信系統(tǒng)的初始化以及自適應(yīng)三維姿態(tài)方法的步驟S2的初始信息的設(shè)定。2)坐標(biāo)軸校準(zhǔn)14通過相應(yīng)的轉(zhuǎn)動(dòng)平臺(tái)等設(shè)備，檢測三軸微陀螺傳感器，三軸微加速度傳感器和三軸磁場傳感器的坐標(biāo)軸校準(zhǔn)情況，對于三個(gè)坐標(biāo)軸之間的校準(zhǔn)偏差，可以通過相應(yīng)的旋轉(zhuǎn)矩陣來進(jìn)行補(bǔ)償。如當(dāng)某傳感器坐標(biāo)軸與設(shè)定的標(biāo)準(zhǔn)坐標(biāo)軸之間存在橫滾角Y，俯仰角θ，和航向角Ψ的偏差時(shí)，可以通過如下公式進(jìn)行傳感器數(shù)據(jù)的標(biāo)定其中s是某傳感器的原始數(shù)據(jù)，S'為坐標(biāo)軸對準(zhǔn)后的傳感器的數(shù)據(jù)。3)傳感器數(shù)據(jù)的采集采集三軸微陀螺傳感器、三軸微加速度傳感器和三軸磁場傳感器的數(shù)據(jù)，為下一步的做準(zhǔn)備。4)自適應(yīng)三維姿態(tài)定位方法對于采集到的傳感器數(shù)據(jù)，基于自適應(yīng)三維姿態(tài)定位方法的步驟S4，S5，S6，S7，S8進(jìn)行裝置姿態(tài)值的計(jì)算。5)姿態(tài)參數(shù)輸出對自適應(yīng)三維姿態(tài)定位算法得到的姿態(tài)信息，通過相應(yīng)的通信接口與外接的顯示設(shè)備或是其他姿態(tài)處理設(shè)備進(jìn)行通信輸出。以上所述，僅為本發(fā)明中的具體實(shí)施方式，但本發(fā)明的保護(hù)范圍并不局限于此，任何熟悉該技術(shù)的人在本發(fā)明所揭露的技術(shù)范圍內(nèi)，可理解想到的變換或替換，都應(yīng)涵蓋在本發(fā)明的權(quán)利要求書的保護(hù)范圍之內(nèi)。權(quán)利要求一種基于微慣性和地磁技術(shù)的自適應(yīng)三維姿態(tài)定位方法，其特征在于，包括以下步驟步驟S1利用集成了三軸微陀螺傳感器、三軸微加速度傳感器和三軸磁場傳感器的微慣性組合測量裝置來感應(yīng)運(yùn)動(dòng)載體的運(yùn)動(dòng)姿態(tài)三軸微陀螺傳感器感應(yīng)運(yùn)動(dòng)載體沿軸向的角速度信號，三軸微加速度傳感器感應(yīng)運(yùn)動(dòng)載體加速度信號，三軸磁場傳感器感應(yīng)地球磁場信號；步驟S2設(shè)置微慣性組合測量裝置的初始姿態(tài)值和全局坐標(biāo)系下的三軸微加速度傳感器的加速度數(shù)據(jù)和三軸磁場傳感器的地球磁場數(shù)據(jù)在確保三軸微陀螺傳感器、三軸微加速度傳感器和三軸磁場傳感器坐標(biāo)系對準(zhǔn)的情況下，保持微慣性組合測量裝置固定不動(dòng)，采集三軸微加速度傳感器的加速度數(shù)據(jù)和三軸磁場傳感器感應(yīng)的地球磁場數(shù)據(jù)；設(shè)置微慣性組合測量裝置初始時(shí)刻即i＝0時(shí)的四元數(shù)形式姿態(tài)值q0＝[1000]T、三軸微加速度傳感器矢量nYA＝[a0xa0ya0z]T和三軸磁場傳感器矢量nYM＝[m0xm0ym0z]T，對三軸微加速度傳感器矢量nYA和三軸磁場傳感器矢量nYM進(jìn)行單位化，得到單位三軸微加速度傳感器矢量nSA和單位三軸磁場傳感器矢量nSM，所述單位化公式為nSA＝nYA/||nYA||和nSM＝nYM/||nYM||，式中，a0x、a0y、a0z表示初始時(shí)刻即i＝0時(shí)的全局坐標(biāo)系下的三軸微加速度傳感器的加速度數(shù)據(jù)，m0x、m0y、m0z表示初始時(shí)刻即i＝0時(shí)的全局坐標(biāo)系下的是三軸磁場傳感器的地球磁場數(shù)據(jù)，下標(biāo)0表示的是初始時(shí)刻，下標(biāo)x、y、z表示的是每個(gè)傳感器坐標(biāo)系下的三個(gè)坐標(biāo)軸；上標(biāo)n表示的是全局坐標(biāo)系下的傳感器數(shù)據(jù)、下標(biāo)A和M分別表示的是三軸微加速度傳感器和三軸磁場傳感器的數(shù)據(jù)；S表示的是單位化后相應(yīng)傳感器數(shù)據(jù)；上標(biāo)T表示矢量的轉(zhuǎn)置；步驟S3對微慣性組合測量裝置的姿態(tài)值進(jìn)行求解利用當(dāng)前時(shí)刻即i＝t時(shí)的三軸微陀螺傳感器的角速度數(shù)據(jù)、三軸微加速度傳感器的加速度數(shù)據(jù)和三軸磁場傳感器的地球磁場數(shù)據(jù)，當(dāng)前時(shí)刻t的姿態(tài)值qt以及步驟2獲得的初始姿態(tài)值q0、三軸微加速度傳感器的加速度數(shù)據(jù)nYA、nSA和三軸磁場傳感器的地球磁場數(shù)據(jù)nYM、nSM，來獲取微慣性組合測量裝置下一時(shí)刻即i＝t+Δt時(shí)的姿態(tài)值qt+Δt；步驟S4利用三軸微陀螺傳感器數(shù)據(jù)對i＝t+Δt時(shí)刻微慣性組合測量裝置的姿態(tài)值進(jìn)行預(yù)測將當(dāng)前時(shí)刻t的三軸微陀螺傳感器的角速度數(shù)據(jù)表示為當(dāng)前姿態(tài)下的旋轉(zhuǎn)角速度四元數(shù)QG＝T，當(dāng)前時(shí)刻t的三軸微加速度傳感器的加速度數(shù)據(jù)和三軸磁場傳感器的磁場強(qiáng)度數(shù)據(jù)分別為bYA＝[atxatyatz]T和bYM＝[mtxmtymtz]T，其中G、b為標(biāo)記號，ωtx、ωty、ωtz表示的是當(dāng)前時(shí)刻t的三軸陀螺傳感器的角速度數(shù)據(jù)，atx、aty、atz表示的是當(dāng)前時(shí)刻t的三軸微加速度傳感器的加速度數(shù)據(jù)，mtx、mty、mtz表示的是當(dāng)前時(shí)刻t的三軸磁場傳感器的地球磁場數(shù)據(jù)；利用微慣性組合測量裝置當(dāng)前時(shí)刻t的姿態(tài)值qt和當(dāng)前時(shí)刻t的三軸微陀螺傳感器的旋轉(zhuǎn)角速度四元數(shù)QG，對i＝t+Δt時(shí)刻的微慣性組合測量裝置的姿態(tài)值進(jìn)行預(yù)測，由于兩個(gè)時(shí)刻之間的間隔Δt較小，假設(shè)認(rèn)為t和t+Δt兩個(gè)時(shí)刻之間三軸微陀螺傳感器輸出沒有變化，從而獲得姿態(tài)預(yù)測方程<mrow><msub><mover><mi>q</mi><mo>^</mo></mover><mrow><mi>t</mi><mo>+</mo><mi>Δt</mi></mrow></msub><mo>=</mo><msub><mi>q</mi><mi>t</mi></msub><mo>+</mo><msub><mover><mi>q</mi><mo>·</mo></mover><mi>t</mi></msub><mi>Δt</mi><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>1</mn><mo>)</mo></mrow></mrow>式中<mrow><msub><mover><mi>q</mi><mo>·</mo></mover><mi>t</mi></msub><mo>=</mo><mfrac><mn>1</mn><mn>2</mn></mfrac><msub><mi>q</mi><mi>t</mi></msub><mo>⊗</mo><msub><mi>Q</mi><mi>G</mi></msub><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>2</mn><mo>)</mo></mrow></mrow>對預(yù)測后的四元數(shù)進(jìn)行單位化<mrow><msubsup><mover><mi>q</mi><mo>^</mo></mover><mrow><mi>t</mi><mo>+</mo><mi>Δt</mi></mrow><mo>′</mo></msubsup><mo>=</mo><msub><mover><mi>q</mi><mo>^</mo></mover><mrow><mi>t</mi><mo>+</mo><mi>Δt</mi></mrow></msub><mo>/</mo><mo>|</mo><mo>|</mo><msub><mover><mi>q</mi><mo>^</mo></mover><mrow><mi>t</mi><mo>+</mo><mi>Δt</mi></mrow></msub><mo>|</mo><mo>|</mo><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>3</mn><mo>)</mo></mrow></mrow>式中，qt為當(dāng)前時(shí)刻t的微慣性組合測量裝置的姿態(tài)值，為下一時(shí)刻預(yù)測姿態(tài)值，Δt為采樣時(shí)間間隔，為四元數(shù)形式的當(dāng)前時(shí)刻t全局姿態(tài)下的旋轉(zhuǎn)角速度；步驟S5對步驟S4獲得的三軸微加速度傳感器的加速度數(shù)據(jù)和三軸磁場傳感器地球磁場數(shù)據(jù)進(jìn)行置信判斷，判定周圍環(huán)境是否存在電磁場干擾或瞬時(shí)加速度干擾；若干擾過大，則放棄該時(shí)刻采集的三軸磁場傳感器的地球磁場數(shù)據(jù)或三軸微加速度傳感器的加速度數(shù)據(jù)，否則以測得的周圍環(huán)境噪聲的大小設(shè)置自適應(yīng)因子，該自適應(yīng)因子是表征不存在較大的電磁干擾或是瞬時(shí)加速度情況下的三軸微加速度傳感器和三軸磁場傳感器的噪聲大小，從而基于該自適應(yīng)因子對三軸微加速度傳感器的加速度數(shù)據(jù)或是三軸磁場傳感器的地球磁場數(shù)據(jù)的影響進(jìn)行權(quán)重設(shè)置如下<mrow><mmultiscripts><mi>S</mi><mi>M</mi><none/><mprescripts/><none/><mi>b</mi></mmultiscripts><mo>=</mo><mfencedopen='{'close=''><mtable><mtr><mtd><mmultiscripts><mi>Y</mi><mi>M</mi><none/><mprescripts/><none/><mi>b</mi></mmultiscripts><mo>/</mo><mo>|</mo><mo>|</mo><mmultiscripts><mi>Y</mi><mi>M</mi><none/><mprescripts/><none/><mi>b</mi></mmultiscripts><mo>|</mo><mo>|</mo></mtd><mtd><mi>if</mi></mtd><mtd><mrow><mo>(</mo><msub><mi>k</mi><mi>M</mi></msub><mo>=</mo><mn>1</mn><mo>-</mo><mo>|</mo><mo>|</mo><mo>|</mo><mmultiscripts><mi>Y</mi><mi>M</mi><none/><mprescripts/><none/><mi>b</mi></mmultiscripts><mo>|</mo><mo>|</mo><mo>-</mo><mo>|</mo><mo>|</mo><msubsup><mi>q</mi><mi>t</mi><mrow><mo>-</mo><mn>1</mn></mrow></msubsup><mo>⊗</mo><mmultiscripts><mi>Y</mi><mi>M</mi><none/><mprescripts/><none/><mi>n</mi></mmultiscripts><mo>⊗</mo><msub><mi>q</mi><mi>t</mi></msub><mo>|</mo><mo>|</mo><mo>|</mo><mo>/</mo><msubsup><mi>q</mi><mi>t</mi><mrow><mo>-</mo><mn>1</mn></mrow></msubsup><mo>⊗</mo><mmultiscripts><mi>Y</mi><mi>M</mi><none/><mprescripts/><none/><mi>n</mi></mmultiscripts><mo>⊗</mo><msub><mi>q</mi><mi>t</mi></msub><mo>|</mo><mo>|</mo><mo><</mo><msub><mi>ϵ</mi><mi>M</mi></msub><mo>)</mo></mrow></mtd></mtr><mtr><mtd><msubsup><mi>q</mi><mi>t</mi><mrow><mo>-</mo><mn>1</mn></mrow></msubsup><mo>⊗</mo><mmultiscripts><mi>S</mi><mi>M</mi><none/><mprescripts/><none/><mi>n</mi></mmultiscripts><mo>⊗</mo><msub><mi>q</mi><mi>t</mi></msub></mtd><mtd><mi>else</mi></mtd><mtd><msub><mi>k</mi><mi>M</mi></msub><mo>=</mo><mn>1</mn></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>4</mn><mo>)</mo></mrow></mrow><mrow><mmultiscripts><mi>S</mi><mi>A</mi><none/><mprescripts/><none/><mi>b</mi></mmultiscripts><mo>=</mo><mfencedopen='{'close=''><mtable><mtr><mtd><mmultiscripts><mi>Y</mi><mi>A</mi><none/><mprescripts/><none/><mi>b</mi></mmultiscripts><mo>/</mo><mo>|</mo><mo>|</mo><mmultiscripts><mi>Y</mi><mi>A</mi><none/><mprescripts/><none/><mi>b</mi></mmultiscripts><mo>|</mo><mo>|</mo></mtd><mtd><mi>if</mi></mtd><mtd><mrow><mo>(</mo><msub><mi>k</mi><mi>A</mi></msub><mo>=</mo><mn>1</mn><mo>-</mo><mo>|</mo><mo>|</mo><mo>|</mo><mmultiscripts><mi>Y</mi><mi>A</mi><none/><mprescripts/><none/><mi>b</mi></mmultiscripts><mo>|</mo><mo>|</mo><mo>-</mo><msubsup><mi>q</mi><mi>t</mi><mrow><mo>-</mo><mn>1</mn></mrow></msubsup><mo>⊗</mo><mmultiscripts><mi>Y</mi><mi>A</mi><none/><mprescripts/><none/><mi>n</mi></mmultiscripts><mo>⊗</mo><msub><mi>q</mi><mi>t</mi></msub><mo>|</mo><mo>|</mo><mo>|</mo><mo>/</mo><mo>|</mo><mo>|</mo><msubsup><mi>q</mi><mi>t</mi><mrow><mo>-</mo><mn>1</mn></mrow></msubsup><mo>⊗</mo><mmultiscripts><mi>Y</mi><mi>A</mi><none/><mprescripts/><none/><mi>n</mi></mmultiscripts><mo>⊗</mo><msub><mi>q</mi><mi>t</mi></msub><mo>|</mo><mo>|</mo><mo><</mo><msub><mi>ϵ</mi><mi>A</mi></msub><mo>)</mo></mrow></mtd></mtr><mtr><mtd><msubsup><mi>q</mi><mi>t</mi><mrow><mo>-</mo><mn>1</mn></mrow></msubsup><mo>⊗</mo><mmultiscripts><mi>S</mi><mi>A</mi><none/><mprescripts/><none/><mi>n</mi></mmultiscripts><mo>⊗</mo><msub><mi>q</mi><mi>t</mi></msub></mtd><mtd><mi>else</mi></mtd><mtd><msub><mi>k</mi><mrow><mi>A</mi><mo>=</mo><mn>1</mn></mrow></msub></mtd></mtr></mtable></mfenced></mrow>式中，bSM為調(diào)整后的三軸磁場傳感器矢量、bSA為調(diào)整后的三軸微加速度傳感器矢量；kA為三軸微加速度傳感器自適應(yīng)因子，kM為三軸磁場傳感器自適應(yīng)因子；εA為三軸微加速度傳感器噪聲上界，εM為三軸磁場傳感器噪聲上界；所述干擾過大是超過公式(4)中的三軸微加速度傳感器噪聲上界εA或三軸磁場傳感器噪聲上界εM，εA、εM的取值范圍是(01)，周圍環(huán)境噪聲越大，該噪聲上界εA、εM值應(yīng)取值越小，具體取值應(yīng)依據(jù)在現(xiàn)場環(huán)境中的實(shí)驗(yàn)決定；步驟S6利用步驟S5調(diào)整后的三軸微加速度傳感器的加速度數(shù)據(jù)和三軸磁場傳感器的地球磁場數(shù)據(jù)獲得微慣性組合測量裝置的姿態(tài)誤差值該姿態(tài)誤差值通過高斯牛頓方法獲得<mrow><msub><mover><mi>q</mi><mo>→</mo></mover><mi>e</mi></msub><mo>=</mo><mo>-</mo><mfrac><mn>1</mn><mn>2</mn></mfrac><mmultiscripts><mi>R</mi><mprescripts/><mi>n</mi><mi>b</mi></mmultiscripts><mrow><mo>(</mo><msubsup><mover><mi>q</mi><mo>^</mo></mover><mrow><mi>t</mi><mo>+</mo><mi>Δt</mi></mrow><mo>′</mo></msubsup><mo>)</mo></mrow><msup><mi>Ξ</mi><mrow><mo>-</mo><mn>1</mn></mrow></msup><mmultiscripts><mi>R</mi><mprescripts/><mi>n</mi><mi>b</mi></mmultiscripts><msup><mrow><mo>(</mo><msubsup><mover><mi>q</mi><mo>^</mo></mover><mrow><mi>t</mi><mo>+</mo><mi>Δt</mi></mrow><mo>′</mo></msubsup><mo>)</mo></mrow><mi>T</mi></msup><mrow><mo>(</mo><msup><mrow><mo>[</mo><mmultiscripts><mi>R</mi><mprescripts/><mi>n</mi><mi>b</mi></mmultiscripts><mrow><mo>(</mo><msubsup><mover><mi>q</mi><mo>^</mo></mover><mrow><mi>t</mi><mo>+</mo><mi>Δt</mi></mrow><mo>′</mo></msubsup><mo>)</mo></mrow><mmultiscripts><mi>S</mi><mi>A</mi><none/><mprescripts/><none/><mi>n</mi></mmultiscripts><mo>]</mo></mrow><mo>×</mo></msup><mmultiscripts><mi>S</mi><mi>A</mi><none/><mprescripts/><none/><mi>b</mi></mmultiscripts><mo>+</mo><msup><mrow><mo>[</mo><mmultiscripts><mi>R</mi><mprescripts/><mi>n</mi><mi>b</mi></mmultiscripts><msup><mrow><mo>(</mo><msubsup><mover><mi>q</mi><mo>^</mo></mover><mrow><mi>t</mi><mo>+</mo><mi>Δt</mi></mrow><mo>′</mo></msubsup><mo>)</mo></mrow><mi>n</mi></msup><msub><mi>S</mi><mi>M</mi></msub><mo>]</mo></mrow><mo>×</mo></msup><mmultiscripts><mi>S</mi><mi>M</mi><none/><mprescripts/><none/><mi>b</mi></mmultiscripts><mo>)</mo></mrow><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>16</mn><mo>)</mo></mrow></mrow>式中<mrow><mi>Ξ</mi><mo>=</mo><mn>2</mn><mi>I</mi><mo>-</mo><mrow><mo>(</mo><mmultiscripts><mi>S</mi><mi>A</mi><none/><mprescripts/><none/><mi>n</mi></mmultiscripts><mmultiscripts><mi>S</mi><mi>A</mi><mi>T</mi><mprescripts/><none/><mi>n</mi></mmultiscripts><mo>+</mo><mmultiscripts><mi>S</mi><mi>M</mi><none/><mprescripts/><none/><mi>n</mi></mmultiscripts><mmultiscripts><mi>S</mi><mi>M</mi><mi>T</mi><mprescripts/><none/><mi>n</mi></mmultiscripts><mo>)</mo></mrow><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>17</mn><mo>)</mo></mrow></mrow><mrow><mmultiscripts><mi>R</mi><mprescripts/><mi>n</mi><mi>b</mi></mmultiscripts><mrow><mo>(</mo><msubsup><mover><mi>q</mi><mo>^</mo></mover><mrow><mi>t</mi><mo>+</mo><mi>Δt</mi></mrow><mo>′</mo></msubsup><mo>)</mo></mrow><mo>=</mo><mn>2</mn><mfencedopen='['close=']'><mtable><mtr><mtd><mn>0.5</mn><mo>-</mo><msubsup><mi>q</mi><mn>2</mn><mn>2</mn></msubsup><mo>-</mo><msubsup><mi>q</mi><mn>3</mn><mn>2</mn></msubsup></mtd><mtd><msub><mi>q</mi><mn>1</mn></msub><msub><mi>q</mi><mn>2</mn></msub><mo>+</mo><msub><mi>q</mi><mn>0</mn></msub><msub><mi>q</mi><mn>3</mn></msub></mtd><mtd><msub><mi>q</mi><mn>1</mn></msub><msub><mi>q</mi><mn>3</mn></msub><mo>-</mo><msub><mi>q</mi><mn>0</mn></msub><msub><mi>q</mi><mn>2</mn></msub></mtd></mtr><mtr><mtd><msub><mi>q</mi><mn>1</mn></msub><msub><mi>q</mi><mn>2</mn></msub><mo>-</mo><msub><mi>q</mi><mn>0</mn></msub><msub><mi>q</mi><mn>3</mn></msub></mtd><mtd><mn>0.5</mn><mo>-</mo><msubsup><mi>q</mi><mn>1</mn><mn>2</mn></msubsup><mo>-</mo><msubsup><mi>q</mi><mn>3</mn><mn>2</mn></msubsup></mtd><mtd><msub><mi>q</mi><mn>2</mn></msub><msub><mi>q</mi><mn>3</mn></msub><mo>+</mo><msub><mi>q</mi><mn>0</mn></msub><msub><mi>q</mi><mn>1</mn></msub></mtd></mtr><mtr><mtd><msub><mi>q</mi><mn>1</mn></msub><msub><mi>q</mi><mn>3</mn></msub><mo>+</mo><msub><mi>q</mi><mn>0</mn></msub><msub><mi>q</mi><mn>2</mn></msub></mtd><mtd><msub><mi>q</mi><mn>2</mn></msub><msub><mi>q</mi><mn>3</mn></msub><mo>-</mo><msub><mi>q</mi><mn>0</mn></msub><msub><mi>q</mi><mn>1</mn></msub></mtd><mtd><mn>0.5</mn><mo>-</mo><msubsup><mi>q</mi><mn>1</mn><mn>2</mn></msubsup><mo>-</mo><msubsup><mi>q</mi><mn>2</mn><mn>2</mn></msubsup></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>18</mn><mo>)</mo></mrow></mrow><mrow><msup><mrow><mo>[</mo><mi>λ</mi><mo>]</mo></mrow><mo>×</mo></msup><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><mn>0</mn></mtd><mtd><mo>-</mo><msub><mi>λ</mi><mi>z</mi></msub></mtd><mtd><msub><mi>λ</mi><mi>y</mi></msub></mtd></mtr><mtr><mtd><msub><mi>λ</mi><mi>z</mi></msub></mtd><mtd><mn>0</mn></mtd><mtd><mo>-</mo><msub><mi>λ</mi><mi>x</mi></msub></mtd></mtr><mtr><mtd><mo>-</mo><msub><mi>λ</mi><mi>y</mi></msub></mtd><mtd><msub><mi>λ</mi><mi>x</mi></msub></mtd><mtd><mn>0</mn></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>19</mn><mo>)</mo></mrow></mrow><mrow><msup><mrow><mo>[</mo><mi>μ</mi><mo>]</mo></mrow><mi>x</mi></msup><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><mn>0</mn></mtd><mtd><mo>-</mo><msub><mi>μ</mi><mi>z</mi></msub></mtd><mtd><msub><mi>μ</mi><mi>y</mi></msub></mtd></mtr><mtr><mtd><msub><mi>μ</mi><mi>z</mi></msub></mtd><mtd><mn>0</mn></mtd><mtd><mo>-</mo><msub><mi>μ</mi><mi>x</mi></msub></mtd></mtr><mtr><mtd><mo>-</mo><msub><mi>μ</mi><mi>y</mi></msub></mtd><mtd><msub><mi>μ</mi><mi>x</mi></msub></mtd><mtd><mn>0</mn></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>20</mn><mo>)</mo></mrow></mrow>式中λ表示為μ表示為λx、λy、λz為向量λ的三個(gè)分量，μx、μy、μz為向量μ的三個(gè)分量，q0、q1、q2、q3為四元數(shù)的四個(gè)分量，為四元數(shù)轉(zhuǎn)換成的旋轉(zhuǎn)矩陣；矩陣Ξ只與初始標(biāo)定時(shí)刻的三軸微加速度傳感器的加速度數(shù)據(jù)和三軸磁場傳感器的地球磁場數(shù)據(jù)有關(guān)，所以在姿態(tài)方法中只要進(jìn)行一次標(biāo)定，獲取Ξ矩陣及其逆矩陣，無需在其他時(shí)刻進(jìn)行矩陣逆運(yùn)算；步驟S7基于步驟S4得到的姿態(tài)預(yù)測信息、步驟S6得到微慣性組合測量裝置的姿態(tài)誤差值和步驟S5得到的自適應(yīng)因子進(jìn)行融合，獲得t+Δt時(shí)刻微慣性組合測量裝置姿態(tài)值<mrow><msub><mi>q</mi><mrow><mi>t</mi><mo>+</mo><mi>Δt</mi></mrow></msub><mo>=</mo><msub><mover><mi>q</mi><mo>^</mo></mover><mrow><mi>t</mi><mo>+</mo><mi>Δt</mi></mrow></msub><mo>+</mo><msub><mover><mi>q</mi><mo>^</mo></mover><mrow><mi>t</mi><mo>+</mo><mi>Δt</mi></mrow></msub><mo>⊗</mo><mfencedopen='['close=']'><mtable><mtr><mtd><mn>1</mn></mtd><mtd><msub><mover><mi>q</mi><mo>→</mo></mover><mi>e</mi></msub></mtd></mtr></mtable></mfenced><msub><mi>k</mi><mi>A</mi></msub><msub><mi>k</mi><mi>M</mi></msub><mi>Δt</mi><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>21</mn><mo>)</mo></mrow></mrow>然后再對其單位化qt+Δt＝qt+Δt/||qt+Δt||；其中表示的是四元數(shù)的乘法運(yùn)算；步驟S8輸出姿態(tài)信息根據(jù)四元數(shù)和歐拉角之間的轉(zhuǎn)換關(guān)系，將四元數(shù)轉(zhuǎn)換為具有直觀意義的、自適應(yīng)三維姿態(tài)的俯仰角α、橫滾角β和航向角γ如下<mrow><mfencedopen='{'close=''><mtable><mtr><mtd><mi>α</mi><mo>=</mo><mi>arctg</mi><mrow><mo>(</mo><mfrac><mrow><mn>2</mn><mrow><mo>(</mo><msub><mi>q</mi><mn>2</mn></msub><msub><mi>q</mi><mn>3</mn></msub><mo>-</mo><msub><mi>q</mi><mn>0</mn></msub><msub><mi>q</mi><mn>1</mn></msub><mo>)</mo></mrow></mrow><mrow><msubsup><mi>q</mi><mn>0</mn><mn>2</mn></msubsup><mo>-</mo><msubsup><mi>q</mi><mn>1</mn><mn>2</mn></msubsup><mo>-</mo><msubsup><mi>q</mi><mn>2</mn><mn>2</mn></msubsup><mo>+</mo><msubsup><mi>q</mi><mn>3</mn><mn>2</mn></msubsup></mrow></mfrac><mo>)</mo></mrow></mtd><mtd><mi>α</mi><mo>∈</mo><mo>(</mo><mo>-</mo><mn>180,180</mn><mo>]</mo></mtd></mtr><mtr><mtd><mi>β</mi><mo>=</mo><mi>arcsin</mi><mrow><mo>(</mo><mo>-</mo><mn>2</mn><mrow><mo>(</mo><msub><mi>q</mi><mn>1</mn></msub><msub><mi>q</mi><mn>3</mn></msub><mo>+</mo><msub><mi>q</mi><mn>0</mn></msub><msub><mi>q</mi><mn>2</mn></msub><mo>)</mo></mrow><mo>)</mo></mrow></mtd><mtd><mi>β</mi><mo>∈</mo><mo>(</mo><mo>-</mo><mn>90,90</mn><mo>]</mo></mtd></mtr><mtr><mtd><mi>γ</mi><mo>=</mo><mi>arctg</mi><mrow><mo>(</mo><mfrac><mrow><mn>2</mn><mrow><mo>(</mo><msub><mi>q</mi><mn>1</mn></msub><msub><mi>q</mi><mn>2</mn></msub><mo>-</mo><msub><mi>q</mi><mn>0</mn></msub><msub><mi>q</mi><mn>3</mn></msub><mo>)</mo></mrow></mrow><mrow><msubsup><mi>q</mi><mn>0</mn><mn>2</mn></msubsup><mo>+</mo><msubsup><mi>q</mi><mn>1</mn><mn>2</mn></msubsup><mo>-</mo><msubsup><mi>q</mi><mn>2</mn><mn>2</mn></msubsup><mo>-</mo><msubsup><mi>q</mi><mn>3</mn><mn>2</mn></msubsup></mrow></mfrac><mo>)</mo></mrow></mtd><mtd><mi>γ</mi><mo>∈</mo><mo>(</mo><mo>-</mo><mn>180,180</mn><mo>]</mo></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>22</mn><mo>)</mo></mrow><mo>.</mo></mrow>FSA00000198254300021.tif,FSA00000198254300022.tif,FSA00000198254300026.tif,FSA00000198254300027.tif,FSA00000198254300033.tif,FSA00000198254300042.tif,FSA00000198254300043.tif,FSA00000198254300044.tif,FSA00000198254300045.tif,FSA00000198254300046.tif,FSA00000198254300048.tif2.根據(jù)權(quán)利要求1中所述的自適應(yīng)三維姿態(tài)定位方法，其特征在于利用三軸微陀螺傳感器的預(yù)測以及三軸微加速度傳感器和三軸磁場傳感器數(shù)據(jù)的更正獲取姿態(tài)信息。3.根據(jù)權(quán)利要求1中所述自適應(yīng)三維姿態(tài)定位方法，其特征在于微慣性組合測量裝置的初始姿態(tài)標(biāo)定需要進(jìn)行一次矩陣逆求解，在預(yù)測過程和更正過程所有的姿態(tài)信息均以四元數(shù)的形式進(jìn)行表示和運(yùn)算。4.據(jù)權(quán)利要求1中所述的自適應(yīng)三維姿態(tài)定位方法，其特征在于考慮周圍環(huán)境中的電磁場對地球磁場的干擾和瞬時(shí)加速度的影響，如果干擾變化超過閾值，則三軸微加速度傳感器和三軸磁場傳感器的數(shù)據(jù)進(jìn)行舍棄；如果干擾變化沒超過閾值，則按照干擾的大小設(shè)置三軸微加速度傳感器和三軸磁場傳感器數(shù)據(jù)的自適應(yīng)因子。全文摘要本發(fā)明公開一種基于微慣性和地磁技術(shù)的自適應(yīng)三維姿態(tài)定位方法，包括1)利用微慣性測量裝置中的傳感器感應(yīng)載體的運(yùn)動(dòng)姿態(tài)；2)設(shè)置裝置的初始姿態(tài)及全局坐標(biāo)系下的加速度和地磁場信息；3)對裝置的姿態(tài)值進(jìn)行求解；4)同時(shí)利用三軸微陀螺傳感器數(shù)據(jù)對裝置姿態(tài)值進(jìn)行預(yù)測；5)對三軸微加速度傳感器和三軸磁場傳感器數(shù)據(jù)進(jìn)行置信判定，檢測周圍環(huán)境干擾，設(shè)置自適應(yīng)參數(shù)；6)利用步驟5處理后的三軸微加速度傳感器數(shù)據(jù)和三軸磁場傳感器數(shù)據(jù)獲得裝置姿態(tài)誤差值；7)對步驟4得到的姿態(tài)預(yù)測值以及步驟6得到的更正信息以及步驟5得到的自適應(yīng)參數(shù)進(jìn)行融合，獲得裝置姿態(tài)值；8)輸出姿態(tài)信息。文檔編號G01C21/20GK101915580SQ20101023121公開日2010年12月15日申請日期2010年7月14日優(yōu)先權(quán)日2010年7月14日發(fā)明者弭鵬,杜清秀申請人:中國科學(xué)院自動(dòng)化研究所