專利名稱：：硅微航姿系統(tǒng)慣性/地磁組合方法
技術領域：
：本發(fā)明涉及一種測量方法，特別是涉及一種捷聯(lián)航姿系統(tǒng)的組合導航方法，尤其涉及一種硅微航姿系統(tǒng)慣性/地磁組合導航方法。
背景技術：
：硅微航姿系統(tǒng)包括由三個硅微陀螺儀、三個硅微加速度計和三軸磁強計組成的微慣性測量單元(MIMU)和導航計算機，是一種捷聯(lián)式慣性組合導航系統(tǒng)，硅微陀螺儀和加速度計直接固連在運載體上，測量出載體的旋轉運動角速率和線運動加速度信息(包括重力加速度)，然后送至導航計算機中進行實時的姿態(tài)矩陣解算，并從捷聯(lián)姿態(tài)矩陣的有關元素中計算出載體的姿態(tài)角。硅微加速度計及硅微陀螺是基于MEMS技術的一種新型慣性傳感器，目前其精度相對常規(guī)慣性器件要低很多，陀螺的漂移和噪聲水平要高于地球的自轉速率，因此目前航姿系統(tǒng)大多采用微慣性器件與磁強計組合使用的方案。硅微航姿系統(tǒng)用加速度計和磁強計測量的重力向量和地磁向量為參考信息通過Kalman組合濾波來補償陀螺的漂移、修正陀螺測量的載體運動角速度積分后的角度漂移，確定的載體姿態(tài)信息既有長期的穩(wěn)定精度又具有載體動態(tài)運動的快速實時性，明顯的提高了航姿系統(tǒng)的精度、增強了系統(tǒng)的魯棒性。因此如何構建一種更加充分利用加速度計、磁強計和陀螺測量信息的Kalman組合濾波方法對于實現(xiàn)硅微航姿系統(tǒng)具有重要的意義。在已報道和所能査閱到的國內外相關文獻中，給出文獻綜合分析結果由于國外的硅微慣性技術發(fā)展得較為成熟，目前研究內容均集中在硅微慣性組合系統(tǒng)在各行業(yè)的應用研究，而國外公司的產品技術出于商業(yè)保密原因，并未見諸詳細報道；而國內的相關研究則大多局限在系統(tǒng)的各項具體技術的理論研究上，如慣性器件的測試、誤差建模與補償、矩陣更新算法等。
發(fā)明內容本發(fā)明的目的是提供一種能夠有效抑制載體機動影響和地磁干擾的硅微航姿系統(tǒng)慣性/<formula>formulaseeoriginaldocumentpage9</formula>重力向量fl和地磁向量m在地理坐標系"系的投影a"=<formula>formulaseeoriginaldocumentpage9</formula>地磁組合方法。為了達到上述的發(fā)明目的，本發(fā)明包括下列步驟(1)利用微慣性測量組件MIMU中的硅微傳感器感應載體運動硅微陀螺敏感載體運動沿其軸向的角速度信號，硅微加速度計敏感載體運動沿其軸向的加速度及重力加速度信號；利用MIMU中的磁強計敏感地磁向量，并將信號送至導航計算機。(2)利用加速度計和磁強計對重力向量和地磁向量的觀測來確定載體的初始姿態(tài)為已知常量，|"為地磁場在當?shù)?系的投影向量，可根據(jù)經(jīng)緯度和海拔高度由國際參考地磁場查得。在載體坐標系6系中的投影？和m6，可由加速度計和磁強計測得。載體坐標系可通過導航坐標系(地理坐標系"系)按航向角(y)、俯仰角(-)、橫滾角(e)三次轉動確定，如附圖2所示。構造三個正交向量gf=<formula>formulaseeoriginaldocumentpage9</formula>則它們在w系中的投影構成矩陣[《";r";s"]，可由壙和附"求得；在6系中的投影構成矩陣[y;/;s，，可由^和i^求得。<formula>formulaseeoriginaldocumentpage9</formula>/=c、,，所以因為<formula>formulaseeoriginaldocumentpage9</formula>由此，可求出姿態(tài)矩陣《的估計^。具體地根據(jù)下面的兩個公式即可求得Kalman濾波器的初值&0)。<formula>formulaseeoriginaldocumentpage10</formula>(3)利用加速度計和磁強計對重力向量和地磁向量的觀測來修正陀螺給出的姿態(tài)角。具體地，把這兩個參考向量在"系中的理論值通過陀螺給出的姿態(tài)角投影到6系中，與加速度計和磁強計的測量值求差作為觀測量來完成對姿態(tài)的修正。本步驟包含如下步驟①建立卡爾曼濾波的狀態(tài)方程<formula>formulaseeoriginaldocumentpage10</formula>式中，ATO)是f時刻系統(tǒng)的狀態(tài)向量；F(O、g(,)分別為系統(tǒng)狀態(tài)矩陣和噪聲矩陣；『0)為系統(tǒng)的噪聲向量。系統(tǒng)的狀態(tài)矢量為<formula>formulaseeoriginaldocumentpage10</formula>(4)系統(tǒng)的狀態(tài)噪聲向量為<formula>formulaseeoriginaldocumentpage10</formula>其中，9e=kel&&3]T為姿態(tài)四元數(shù)向量估計誤差'A^=[A^A化丫為陀螺零偏估計誤差，^0)=(^為陀螺誤差噪聲，JT2(0=(fiVwgzf為陀螺零偏估計誤差隨機游走模型的零均值白噪聲。{『(0}為獨立零均值高斯白噪聲過程，其協(xié)方差矩陣其中，,<formula>formulaseeoriginaldocumentpage10</formula>為對角陣。q的對角線元素根據(jù)陀螺噪聲來確定，工程上可近似取為噪聲RMS值的平方除以帶寬/'系統(tǒng)量測向量為<formula>formulaseeoriginaldocumentpage11</formula>觀測向量<formula>formulaseeoriginaldocumentpage11</formula>式中，AA為三軸加速度計輸出的加速度。<formula>formulaseeoriginaldocumentpage11</formula>觀湖!1向量^^=5:附"，^i^-附A—<formula>formulaseeoriginaldocumentpage11</formula>式中，m,^m,為三軸磁強計輸出的磁強c觀測噪聲向量為<formula>formulaseeoriginaldocumentpage11</formula>{^(^為獨立均值高斯白噪聲過程，其協(xié)方差矩陣:<formula>formulaseeoriginaldocumentpage11</formula>込的對角線元素根據(jù)陀螺零位漂移的特點來確定<系統(tǒng)的噪聲系數(shù)矩陣為G(0=系統(tǒng)的狀態(tài)轉移矩陣為^(0=上式中，A=[5X&為載體角速度向量在載體坐標系中的估計值問=為向量&的反對稱矩陣，。②根據(jù)加速度計測量值判斷系統(tǒng)加速運動情況和磁強計測量值判斷地磁受干擾情況，如果重力向量和地磁向量沒有受到干擾，轉到歩驟③；如果重力向量或地磁向量受到干擾，轉到步驟④；③建立卡爾曼濾波的觀測方程，并轉到步驟⑤<formula>formulaseeoriginaldocumentpage11</formula>(8)式中，Z(/)為/時刻系統(tǒng)的量測向量；W(f)為系統(tǒng)量測矩陣；F(/)為系統(tǒng)的量測噪聲向及屏為對角陣，J^和及^的對角線元素分別根據(jù)加速度計和磁強計的噪聲上式中，A0》(11)(12)來確定，工程上可近似取為噪聲RMS值的平方&=(薦J2./3x3，4=(歸呵)2系統(tǒng)觀測矩陣為06x3]-2[^]—構造重力向量和地磁向量的法向量/=ax附重力向量、地磁向量和兩者叉乘構造的法向量如附圖3所不。在"系中投影的法向量理論值為l""=fl"XW"，可以由fl"和!《"求得；在6系中法向量的測量值為/=6><16，可以由滬和m^求得；建立卡爾曼濾波的觀測方程式中，Z(0為^時刻系統(tǒng)的量測向量；W(f)為系統(tǒng)量測矩陣；K(O為系統(tǒng)的量測噪聲向(a)地磁向量受到長期干擾的情況-系統(tǒng)量測向量為Z=觀測向量^5,=/—觀測噪聲向量為FO)=，)F"O為法向量的測量噪聲，與K(0和^0)有關(F(^為獨立均值高斯白噪聲過程，其協(xié)方差矩陣及屏03x3及d3.為對角陣系統(tǒng)觀測矩陣為0fe3]_2[,]_(b)重力向量受到載體長期機動運動干擾的情況上式中<formula>formulaseeoriginaldocumentpage12</formula>系統(tǒng)量測向量為Z=觀測噪聲向量為F0)<formula>formulaseeoriginaldocumentpage13</formula>(9)<formula>formulaseeoriginaldocumentpage13</formula>(10)(F(W為獨立均值高斯白噪聲過程，其協(xié)方差矩陣為對角陣上式中，<formula>formulaseeoriginaldocumentpage13</formula>(12)系統(tǒng)觀測矩陣為<formula>formulaseeoriginaldocumentpage13</formula>根據(jù)擴展卡爾曼濾波遞推方程和步驟①、③或④的結果，建立卡爾曼濾波的時間傳播方程式中06=[%^A]"為當前時刻三軸陀螺的輸出；為上一時刻更新后的陀螺零偏估計；Af為計算步長。<formula>formulaseeoriginaldocumentpage13</formula>式中，f5分別表示四元數(shù)向量，|705表示四元數(shù)乘法，具體為<formula>formulaseeoriginaldocumentpage13</formula>⑥根據(jù)擴展卡爾曼濾波遞推方程和歩驟①、②的結果，建立卡爾曼濾波的測量修正方<formula>formulaseeoriginaldocumentpage13</formula>(14)<formula>formulaseeoriginaldocumentpage13</formula>(15)<formula>formulaseeoriginaldocumentpage14</formula>S『)經(jīng)過補償更新后需進行規(guī)一化處理。6(,+)=o/o)-》o+)(4)輸出載體姿態(tài)參數(shù)由步驟(3)估計得到的姿態(tài)四元數(shù)^即可由公式2求得姿態(tài)矩陣^，姿態(tài)角可以由姿態(tài)矩陣f:的元素計算得出，具體步驟航向角主值俯仰角主值橫滾角主值^主=tan畫^主=一sin—1C13由上述主值按照如下公式判斷真值<formula>formulaseeoriginaldocumentpage14</formula>本發(fā)明使用的硅微航姿系統(tǒng)中硬件包括微慣性測量單元(簡稱MIMU,包括三個相互垂直的硅微陀螺、三個相互垂直的硅微加速度計和三個相互垂直的磁強計)、導航計算機，具體硬件組成如附圖l所示；MIMU的測量坐標系(為載體坐標系6系)與地理坐標系n系的關系及載體姿態(tài)角如附圖2所示。本發(fā)明的方法具有以下優(yōu)點(1)航姿系統(tǒng)體積小、重量輕；(2)具有完全的自主性，不需要外部輸入信息；(3)受載體機動和地磁干擾的影響小，能夠有效提高航姿系統(tǒng)的輸出姿態(tài)精度；(4)航姿系統(tǒng)可以為其它裝置提供每秒50次以上的實時姿態(tài)信號。對本發(fā)明的有益效果說明如下(1)航姿系統(tǒng)仿真實驗航姿系統(tǒng)置于姿態(tài)零位，從第10秒到第20秒，在南北方向加入0.1M(M為地磁場的垂直分量)的磁場偏置；從第35秒到第45秒，在東西方向加入0.1M的磁場偏置，如附圖4a所示。系統(tǒng)采用重力向量和地磁向量作為參考向量的姿態(tài)仿真結果如附圖4c所示。由于地磁場受到干擾，利用本發(fā)明所述方法的系統(tǒng)采用重力向量和法向量作為參考向量的姿態(tài)仿真結果如附圖4d所示。由圖4b可見法向量的方向總保持水平，因此圖4d中沒有產生圖4c的水平姿態(tài)的影響，本發(fā)明所述方法可以減小地磁場干擾對航姿系統(tǒng)姿態(tài)的影響。(2)航姿系統(tǒng)的三軸轉臺實驗將釆用本發(fā)明研制的硅微航姿系統(tǒng)安裝在三軸轉臺上進行靜態(tài)和三軸搖擺試驗，所用硅微航姿系統(tǒng)的器件精度如下硅微陀螺零位穩(wěn)定性57s硅微陀螺刻度因數(shù)線性度0.1%硅微陀螺刻度因數(shù)溫度系數(shù)0.1%/°C硅微陀螺噪聲<0.1°/S/V^硅微加速度計零位穩(wěn)定性0.02m/s2硅微加速度計刻度因數(shù)線性度0.2%硅微加速度計刻度因數(shù)穩(wěn)定性0.05m/s2(-25°C+80°C)硅微加速度計噪聲0.001m/s2/V^磁強計零位穩(wěn)定性0.5mGauss磁強計刻度因數(shù)線性度2%磁強計刻度因數(shù)穩(wěn)定性0.5mGauss磁強計噪聲0.5mGauss利用發(fā)明所述方法得到三軸轉臺指北、水平靜止狀態(tài)硅微航姿系統(tǒng)航向角、俯仰角、橫滾角曲線分別如附圖5a、圖5b和圖5c所示；三軸搖擺狀態(tài)航向角、俯仰角、橫滾角曲線分別如附圖6a、圖6b和圖6c所示。結果表明硅微航姿系統(tǒng)具有較好的精度。(3)航姿系統(tǒng)的跑車實驗動態(tài)跑車實驗在一段較為平直的公路上進行，車輛的姿態(tài)大致水平(其中一段較為顛簸)，航向角基本保持為315。，跑車過程包括勻速運動(時速40km/h)、加速運動、減速運動過程。以車輛上另外一套高精度光纖捷聯(lián)組合慣導系統(tǒng)(PHINS)提供的姿態(tài)輸出作為參考，得到了跑車過程中硅微航姿系統(tǒng)的航向姿態(tài)。顛簸路段橫滾角曲線圖7a所示，平坦路段橫滾角曲線圖7b所示，俯仰角、航向角曲線分別如圖8、圖9所示。動態(tài)跑車實驗結果表明，航姿系統(tǒng)利用本發(fā)明的方法在動態(tài)跑車實驗中達到了實用的水平。圖1為本發(fā)明的硅微航姿系統(tǒng)慣性/地磁組合原理圖；圖2為載體坐標系、(b系)與地理坐標系(n系)的關系圖3為重力向量、地磁向量與法向量的關系圖4a-圖4d為地磁場受干擾的航姿系統(tǒng)仿真結果圖5a-圖5c為靜態(tài)狀態(tài)航姿系統(tǒng)測試結果圖6a-圖6c為搖擺狀態(tài)航姿系統(tǒng)測試結果圖7a-圖7b為動態(tài)跑車航姿系統(tǒng)橫滾角測試結果圖8為動態(tài)跑車航姿系統(tǒng)俯仰角測試結果圖9為動態(tài)跑車航姿系統(tǒng)航向角測試結果圖10為本發(fā)明的硅微航姿系統(tǒng)軟件流程圖。具體實施例方式下面舉例對本發(fā)明作更詳細地描述，本實施例包括以下步驟(1)利用微慣性測量組件MIMU中的硅微傳感器感應載體運動硅微陀螺敏感載體運動沿其軸向的角速度信號，硅微加速度計敏感載體運動沿其軸向的加速度及重力加速度信號；利用MIMU中的磁強計敏感地磁向量，并將信號送至導航計算機。(2)利用加速度計和磁強計對重力向量和地磁向量的觀測來確定載體的初始姿態(tài)<formula>formulaseeoriginaldocumentpage16</formula>重力向量fl和地磁向量m在地理坐標系w系的投影a"=<formula>formulaseeoriginaldocumentpage16</formula>m"為地磁場在當?shù)豱系的投影向量，可根據(jù)經(jīng)緯度和海拔高度由國際參考地磁場查得。在載體坐標系6系中的投影Z和附6，可由加速度計和磁強計測得。載體坐標系可通過導航坐標系(地理坐標系"系)按航向角(y)、俯仰角(0)、橫滾角(e)三次轉動確定，如附圖2所示。構造三個正交向量《=a/=ax/n貝ij它們在"系中的投影構成矩陣iy;/"";s"]，可由《"和附"求得；在6系中的投影構成矩陣[y可由fl6和挑6求得。因為<formula>formulaseeoriginaldocumentpage17</formula>所以由此，可求出姿態(tài)矩陣《的估計《,器的初值S(o)。<formula>formulaseeoriginaldocumentpage17</formula>(i)具體地根據(jù)下面的兩個公式即可求得Kalman濾波<formula>formulaseeoriginaldocumentpage17</formula>(2)(3)利用加速度計和磁強計對重力向量和地磁向量的觀測來修正陀螺給出的姿態(tài)角。具體地，把這兩個參考向量在"系中的理論值通過陀螺給出的姿態(tài)角投影到6系中，與加速度計和磁強計的測量值求差作為觀測量來完成對姿態(tài)的修正。本步驟包含如下步驟①建立卡爾曼濾波的狀態(tài)方程<formula>formulaseeoriginaldocumentpage17</formula>(3)式中，X(0是^時刻系統(tǒng)的狀態(tài)向量；F(,)、G(O分別為系統(tǒng)狀態(tài)矩陣和噪聲矩陣；W(O為系統(tǒng)的噪聲向量。系統(tǒng)的狀態(tài)矢量為<formula>formulaseeoriginaldocumentpage17</formula>(4)系統(tǒng)的狀態(tài)噪聲向量為<formula>formulaseeoriginaldocumentpage17</formula>(5)其中，<formula>formulaseeoriginaldocumentpage17</formula>為姿態(tài)四元數(shù)向量估計誤差<formula>formulaseeoriginaldocumentpage17</formula>為陀螺零偏估計誤差，為陀螺誤差噪聲，『2(0=(^)為陀螺零偏估計誤差隨機游走模型的零均值白噪聲。{^^"為獨立零均值!#斯白噪聲過程，其協(xié)方差矩陣-<formula>formulaseeoriginaldocumentpage18</formula>其中，g(0=-03x3込似取為噪聲RMS值的平方除以帶寬/':a=[(iMv。)2//']./3x3fc的對角線元素根據(jù)陀螺零位漂移的特點來確定系統(tǒng)的噪聲系數(shù)矩陣為G(0=l_為對角陣。g的對角線元素根據(jù)陀螺噪聲來確定，工程上可近<formula>formulaseeoriginaldocumentpage18</formula>系統(tǒng)的狀態(tài)轉移矩陣為尸(0=上式中，&=&A了為載體角速度向量在載體坐標系中的估計值(6)<formula>formulaseeoriginaldocumentpage18</formula>為向量6的反對稱矩陣，。②根據(jù)加速度計測量值判斷系統(tǒng)加速運動情況和磁強計測量值判斷地磁受干擾情況，如果重力向量和地磁向量沒有受到干擾，轉到步驟③；如果重力向量或地磁向量受到干擾，轉到步驟④；③建立卡爾曼濾波的觀測方程，并轉到步驟⑤Z(0二則外)+K0)(8)式中，Z(0為/時刻系統(tǒng)的量測向量；開(0為系統(tǒng)量測矩陣；"(O為系統(tǒng)的量測噪聲向系統(tǒng)量測向量為Z=(9)觀測向量<formula>formulaseeoriginaldocumentpage18</formula>式中，&^",為三軸加速度計輸出的加速度。<formula>formulaseeoriginaldocumentpage19</formula>式中，^為三軸磁強計輸出的磁強c觀測噪聲向量為<formula>formulaseeoriginaldocumentpage19</formula>(F(^為獨立均值高斯白噪聲過程，其協(xié)方差矩陣<formula>formulaseeoriginaldocumentpage19</formula>為對角陣，J^和/^的對角線元素分別根據(jù)加速度計和磁強計的噪聲<formula>formulaseeoriginaldocumentpage19</formula>來確定，工程上可近似取為噪聲RMS值的平方系統(tǒng)觀測矩陣為<formula>formulaseeoriginaldocumentpage19</formula>上式中，(11)(12)構造重力向量和地磁向量的法向量重力向量、地磁向量和兩者叉乘構造的法向量如附圖3所示。在"系中投影的法向量理論值為r"=fl"xi"，可以由和m"求得；在6系中法向量的測量值為/="*乂附*，可以由^和挑*求得；建立卡爾曼濾波的觀測方程則=聰竭+，式中，Z()為f時刻系統(tǒng)的量測向量；H(/)為系統(tǒng)量測矩陣；F(/)為系統(tǒng)的量測噪聲向(a)地磁向量受到長期干擾的情況:系統(tǒng)量測向量為Z觀測向..觀測噪聲向量為F(O<formula>formulaseeoriginaldocumentpage19</formula>K"O為法向量的測量噪聲，與^(r)和F^)有關^F(/》為獨立均值高斯白噪聲過程，其協(xié)方差矩陣:￡{K(0「r(r)}=id(,)^-r)義0,03x3及rf3.為對角陣上式中，^(0=系統(tǒng)量測向量為Z=系統(tǒng)觀測矩陣為"(f)=[A(006x3]—2[一]—(b)重力向量受到載體長期機動運動干擾的情況觀測噪聲向量為H>)={^(/)}為獨立均值高斯白噪聲過程，其協(xié)方差矩陣為對角陣上式中，(12)(9)(10)(12)系統(tǒng)觀測矩陣為tf(/)=[A(006x3]—2[,]—⑤根據(jù)擴展卡爾曼濾波遞推方程和步驟①、③或④的結果，建立卡爾曼濾波的時間傳播方程式中《/=[&^了為當前時刻三軸陀螺的輸出；》(f-A0+為上一時刻更新后的陀螺零偏估計；A/為計算步長。一0丄1—1—^式中，f6分別表示四元數(shù)向量，丄f(8^表示四元數(shù)乘法，具體為"12<formula>formulaseeoriginaldocumentpage21</formula>⑥根據(jù)擴展卡爾曼濾波遞推方程和步驟①、②的結果，建立卡爾曼濾波的測量修正方<formula>formulaseeoriginaldocumentpage21</formula>^f+)經(jīng)過補償更新后需進行規(guī)一化處理。S0+)=w*(r)_~+)(4)輸出載體姿態(tài)參數(shù)由步驟(3)估計得到的姿態(tài)四元數(shù)^即可由公式2求得姿態(tài)矩陣^，姿態(tài)角可以由姿態(tài)矩陣f!的元素計算得出，具體步驟航向角主值俯仰角主值橫滾角主值<formula>formulaseeoriginaldocumentpage21</formula>由上述主值按照如下公式判斷真值:<formula>formulaseeoriginaldocumentpage21</formula><formula>formulaseeoriginaldocumentpage22</formula>如圖10所示，本實施例的工作流程如下(1)系統(tǒng)初始化硅微航姿系統(tǒng)在靜態(tài)條件下正常啟動。首先進行系統(tǒng)的初始化，包括導航計算機硬件自檢，各接口初始化，對MIMU信號的檢測，相應狀態(tài)的設置，以及卡爾曼濾波器的初始化等。(2)初始對準靜基座下，由MIMU輸出信號經(jīng)器件誤差補償后使用公式1和公式2解算出四元數(shù)初值"0)與初始姿態(tài)矩陣^"6(0)。(3)器件誤差補償定時采集到MIMU輸出信號后，根據(jù)采集的環(huán)境溫度和器件的溫度誤差模型完成器件的溫度誤差補償；補償器件的非正交安裝誤差、零位誤差、刻度因數(shù)誤差等。(4)姿態(tài)矩陣更新由經(jīng)過誤差補償?shù)耐勇轀y量角速度信號求解四元數(shù)姿態(tài)方程，根據(jù)姿態(tài)算法中的四元數(shù)方法完成四元數(shù)更新與姿態(tài)矩陣更新。(5)重力、地磁干擾判斷判斷重力、地磁向量是否被干擾，根據(jù)重力、地磁向量被干擾情況選取卡爾曼濾波器觀測向量，建立相應的觀測方程。(6)姿態(tài)組合卡爾曼濾波模塊根據(jù)公式3公式16，建立卡爾曼濾波器。(7)四元數(shù)與陀螺零位誤差修正由卡爾曼濾波估計出的陀螺零位誤差偏差和四元數(shù)誤差及時修正陀螺零位誤差和四元數(shù)，以便在下一循環(huán)中分別應用到流程(3)和流程(4)。(8)四元數(shù)歸一化處理將上一流程(7)得到的修正后的四元數(shù)進行歸一化處理。(9)航姿角計算將上一流程(8)經(jīng)過歸一化處理的四元數(shù)使用公式2得出姿態(tài)矩陣估計^\再使用步驟(4)可以計算出航向姿態(tài)角。(10)航姿角數(shù)據(jù)輸出系統(tǒng)根據(jù)外部設備的需要，設置航姿信息的格式、輸出頻率以及校驗方式等。權利要求1.硅微航姿系統(tǒng)慣性/地磁組合方法，其特征是(1)利用微慣性測量組件MIMU中的硅微傳感器感應載體運動硅微陀螺敏感載體運動沿其軸向的角速度信號，硅微加速度計敏感載體運動沿其軸向的加速度及重力加速度信號，利用MIMU中的磁強計敏感地磁向量，并將信號送至導航計算機；(2)利用加速度計和磁強計對重力向量和地磁向量的觀測來確定載體的初始姿態(tài)重力向量a和地磁向量m在地理坐標系n系的投影<mathsid="math0001"num="0001"><math><![CDATA[<mrow><msup><mi>a</mi><mi>n</mi></msup><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mi>g</mi></mtd></mtr></mtable></mfenced></mrow>]]></math>id="icf0001"file="A2008100647140002C1.tif"wi="14"he="17"top="72"left="121"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/></maths>和<mathsid="math0002"num="0002"><math><![CDATA[<mrow><msup><mi>m</mi><mi>n</mi></msup><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><msubsup><mi>m</mi><mrow><mn>0</mn><mi>x</mi></mrow><mi>n</mi></msubsup></mtd></mtr><mtr><mtd><msubsup><mi>m</mi><mrow><mn>0</mn><mi>y</mi></mrow><mi>n</mi></msubsup></mtd></mtr><mtr><mtd><msubsup><mi>m</mi><mrow><mn>0</mn><mi>z</mi></mrow><mi>n</mi></msubsup></mtd></mtr></mtable></mfenced></mrow>]]></math>id="icf0002"file="A2008100647140002C2.tif"wi="19"he="19"top="71"left="141"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/></maths>為已知常量，mn為地磁場在當?shù)氐乩碜鴺讼祅系中的投影向量，在載體坐標系b系中的投影為ab和mb，構造三個正交向量q＝ar＝a×ms＝r×a它們在n系中的投影構成矩陣[id="icf0003"file="A2008100647140002C3.tif"wi="14"he="5"top="126"left="86"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>]，由an和mn求得；在b系中的投影構成矩陣[id="icf0004"file="A2008100647140002C4.tif"wi="14"he="5"top="135"left="23"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>]，由ab和mb求得，<mathsid="math0003"num="0003"><math><![CDATA[<mrow><mfencedopen='{'close=''><mtable><mtr><mtd><msup><mi>q</mi><mi>b</mi></msup><mo>=</mo><msubsup><mi>C</mi><mi>n</mi><mi>b</mi></msubsup><mo>·</mo><msup><mi>q</mi><mi>n</mi></msup></mtd></mtr><mtr><mtd><msup><mi>r</mi><mi>b</mi></msup><mo>=</mo><msubsup><mi>C</mi><mi>n</mi><mi>b</mi></msubsup><mo>·</mo><msup><mi>r</mi><mi>n</mi></msup></mtd></mtr><mtr><mtd><msup><mi>s</mi><mi>b</mi></msup><mo>=</mo><msubsup><mi>C</mi><mi>n</mi><mi>b</mi></msubsup><mo>·</mo><msup><mi>s</mi><mi>n</mi></msup></mtd></mtr></mtable></mfenced><mo>,</mo></mrow>]]></math></maths><mathsid="math0004"num="0004"><math><![CDATA[<mrow><mrow><mo>[</mo><msup><mi>q</mi><mi>b</mi></msup><mfencedopen=''close=''><mtable><mtr><mtd><mo>·</mo></mtd></mtr><mtr><mtd><mo>·</mo></mtd></mtr><mtr><mtd><mo>·</mo></mtd></mtr></mtable></mfenced><msup><mi>r</mi><mi>b</mi></msup><mfencedopen=''close=''><mtable><mtr><mtd><mo>·</mo></mtd></mtr><mtr><mtd><mo>·</mo></mtd></mtr><mtr><mtd><mo>·</mo></mtd></mtr></mtable></mfenced><msup><mi>s</mi><mi>b</mi></msup><mo>]</mo></mrow><mo>=</mo><msubsup><mi>C</mi><mi>n</mi><mi>b</mi></msubsup><mo>·</mo><mo>[</mo><msup><mi>q</mi><mi>n</mi></msup><mfencedopen=''close=''><mtable><mtr><mtd><mo>·</mo></mtd></mtr><mtr><mtd><mo>·</mo></mtd></mtr><mtr><mtd><mo>·</mo></mtd></mtr></mtable></mfenced><msup><mi>r</mi><mi>n</mi></msup><mfencedopen=''close=''><mtable><mtr><mtd><mo>·</mo></mtd></mtr><mtr><mtd><mo>·</mo></mtd></mtr><mtr><mtd><mo>·</mo></mtd></mtr></mtable></mfenced><msup><mi>s</mi><mi>n</mi></msup><mo>]</mo></mrow>]]></math></maths><mathsid="math0005"num="0005"><math><![CDATA[<mrow><msubsup><mi>C</mi><mi>n</mi><mi>b</mi></msubsup><mo>=</mo><mo>[</mo><msup><mi>q</mi><mi>b</mi></msup><mfencedopen=''close=''><mtable><mtr><mtd><mo>·</mo></mtd></mtr><mtr><mtd><mo>·</mo></mtd></mtr><mtr><mtd><mo>·</mo></mtd></mtr></mtable></mfenced><msup><mi>r</mi><mi>b</mi></msup><mfencedopen=''close=''><mtable><mtr><mtd><mo>·</mo></mtd></mtr><mtr><mtd><mo>·</mo></mtd></mtr><mtr><mtd><mo>·</mo></mtd></mtr></mtable></mfenced><msup><mi>s</mi><mi>b</mi></msup><mo>]</mo><mo>·</mo><msup><mrow><mo>[</mo><msup><mi>q</mi><mi>n</mi></msup><mfencedopen=''close=''><mtable><mtr><mtd><mo>·</mo></mtd></mtr><mtr><mtd><mo>·</mo></mtd></mtr><mtr><mtd><mo>·</mo></mtd></mtr></mtable></mfenced><msup><mi>r</mi><mi>n</mi></msup><mfencedopen=''close=''><mtable><mtr><mtd><mo>·</mo></mtd></mtr><mtr><mtd><mo>·</mo></mtd></mtr><mtr><mtd><mo>·</mo></mtd></mtr></mtable></mfenced><msup><mi>s</mi><mi>n</mi></msup><mo>]</mo></mrow><mrow><mo>-</mo><mn>1</mn></mrow></msup><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>1</mn><mo>)</mo></mrow></mrow>]]></math></maths>由此，求出姿態(tài)矩陣Cnb的估計id="icf0008"file="A2008100647140002C8.tif"wi="6"he="4"top="185"left="83"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>根據(jù)下面的兩個公式求得Kalman濾波器的初值id="icf0009"file="A2008100647140002C9.tif"wi="8"he="4"top="185"left="175"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/><mathsid="math0006"num="0006"><math><![CDATA[<mrow><mo>|</mo><msub><mover><mi>q</mi><mo>^</mo></mover><mn>01</mn></msub><mo>|</mo><mo>=</mo><mfrac><mn>1</mn><mn>2</mn></mfrac><msqrt><mn>1</mn><mo>+</mo><msub><mi>C</mi><mn>11</mn></msub><mo>-</mo><msub><mi>C</mi><mn>22</mn></msub><mo>-</mo><msub><mi>C</mi><mn>33</mn></msub></msqrt></mrow>]]></math></maths><mathsid="math0007"num="0007"><math><![CDATA[<mrow><mi>sig</mi><mrow><mo>(</mo><msub><mover><mi>q</mi><mo>^</mo></mover><mn>00</mn></msub><mo>)</mo></mrow><mo>=</mo><mn>1</mn></mrow>]]></math>id="icf0011"file="A2008100647140002C11.tif"wi="18"he="4"top="202"left="103"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/></maths><mathsid="math0008"num="0008"><math><![CDATA[<mrow><mo>|</mo><msub><mover><mi>q</mi><mo>^</mo></mover><mn>02</mn></msub><mo>|</mo><mo>=</mo><mfrac><mn>1</mn><mn>2</mn></mfrac><msqrt><mn>1</mn><mo>-</mo><msub><mi>C</mi><mn>11</mn></msub><mo>+</mo><msub><mi>C</mi><mn>22</mn></msub><mo>-</mo><msub><mi>C</mi><mn>33</mn></msub></msqrt></mrow>]]></math>id="icf0012"file="A2008100647140002C12.tif"wi="44"he="8"top="204"left="49"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/></maths><mathsid="math0009"num="0009"><math><![CDATA[<mrow><mi>sig</mi><mrow><mo>(</mo><msub><mover><mi>q</mi><mo>^</mo></mover><mn>01</mn></msub><mo>)</mo></mrow><mo>=</mo><mi>sign</mi><mrow><mo>(</mo><msub><mi>C</mi><mn>32</mn></msub><mo>-</mo><msub><mi>C</mi><mn>23</mn></msub><mo>)</mo></mrow></mrow>]]></math>id="icf0013"file="A2008100647140002C13.tif"wi="40"he="4"top="208"left="103"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/></maths><mathsid="math0010"num="0010"><math><![CDATA[<mrow><mo>|</mo><msub><mover><mi>q</mi><mo>^</mo></mover><mn>03</mn></msub><mo>|</mo><mo>=</mo><mfrac><mn>1</mn><mn>2</mn></mfrac><msqrt><mn>1</mn><mo>-</mo><msub><mi>C</mi><mn>11</mn></msub><mo>-</mo><msub><mi>C</mi><mn>22</mn></msub><mo>+</mo><msub><mi>C</mi><mn>33</mn></msub></msqrt></mrow>]]></math>id="icf0014"file="A2008100647140002C14.tif"wi="44"he="8"top="215"left="49"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/></maths><mathsid="math0011"num="0011"><math><![CDATA[<mrow><mi>sig</mi><mrow><mo>(</mo><msub><mover><mi>q</mi><mo>^</mo></mover><mn>02</mn></msub><mo>)</mo></mrow><mo>=</mo><mi>sign</mi><mrow><mo>(</mo><msub><mi>C</mi><mn>13</mn></msub><mo>-</mo><msub><mi>C</mi><mn>31</mn></msub><mo>)</mo></mrow></mrow>]]></math>id="icf0015"file="A2008100647140002C15.tif"wi="40"he="4"top="214"left="103"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/></maths><mathsid="math0012"num="0012"><math><![CDATA[<mrow><mrow><mo>|</mo><msub><mover><mi>q</mi><mo>^</mo></mover><mn>00</mn></msub><mo>|</mo></mrow><mo>=</mo><mfrac><mn>1</mn><mn>2</mn></mfrac><msqrt><mn>1</mn><mo>+</mo><msubsup><mi>q</mi><mn>01</mn><mn>2</mn></msubsup><mo>-</mo><msubsup><mi>q</mi><mn>02</mn><mn>2</mn></msubsup><mo>-</mo><msubsup><mi>q</mi><mn>03</mn><mn>2</mn></msubsup></msqrt></mrow>]]></math></maths><mathsid="math0013"num="0013"><math><![CDATA[<mrow><mi>sig</mi><mrow><mo>(</mo><msub><mover><mi>q</mi><mo>^</mo></mover><mn>03</mn></msub><mo>)</mo></mrow><mo>=</mo><mi>sign</mi><mrow><mo>(</mo><msub><mi>C</mi><mn>21</mn></msub><mo>-</mo><msub><mi>C</mi><mn>12</mn></msub><mo>)</mo></mrow></mrow>]]></math>id="icf0017"file="A2008100647140002C17.tif"wi="40"he="4"top="221"left="103"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/></maths>則id="icf0018"file="A2008100647140002C18.tif"wi="110"he="18"top="237"left="36"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>(3)利用加速度計和磁強計對重力向量和地磁向量的觀測來修正陀螺給出的姿態(tài)角，即把這兩個參考向量在n系中的理論值通過陀螺給出的姿態(tài)角投影到b系中，與加速度計和磁強計的測量值求差作為觀測量來完成對姿態(tài)的修正；本步驟包含如下步驟①建立卡爾曼濾波的狀態(tài)方程<mathsid="math0014"num="0014"><math><![CDATA[<mrow><mover><mi>X</mi><mo>·</mo></mover><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>=</mo><mi>F</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mi>X</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>+</mo><mi>G</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mi>W</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>3</mn><mo>)</mo></mrow></mrow>]]></math></maths>式中，X(t)是t時刻系統(tǒng)的狀態(tài)向量，F(xiàn)(t)、G(t)分別為系統(tǒng)狀態(tài)矩陣和噪聲矩陣，W(t)為系統(tǒng)的噪聲向量；系統(tǒng)的狀態(tài)矢量為<mathsid="math0015"num="0015"><math><![CDATA[<mrow><mi>X</mi><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>q</mi><mi>e</mi></msub></mtd></mtr><mtr><mtd><mi>ΔB</mi></mtd></mtr></mtable></mfenced><mo>=</mo><msup><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>q</mi><mrow><mi>e</mi><mn>1</mn></mrow></msub></mtd><mtd><msub><mi>q</mi><mrow><mi>e</mi><mn>2</mn></mrow></msub></mtd><mtd><msub><mi>q</mi><mrow><mi>e</mi><mn>3</mn></mrow></msub></mtd><mtd><msub><mi>ΔB</mi><mi>x</mi></msub></mtd><mtd><mi>Δ</mi><msub><mi>B</mi><mi>y</mi></msub></mtd><mtd><mi>Δ</mi><msub><mi>B</mi><mi>z</mi></msub></mtd></mtr></mtable></mfenced><mi>T</mi></msup><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>4</mn><mo>)</mo></mrow></mrow>]]></math></maths>系統(tǒng)的狀態(tài)噪聲向量為<mathsid="math0016"num="0016"><math><![CDATA[<mrow><mi>W</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>=</mo><mfencedopen='('close=')'><mtable><mtr><mtd><msub><mi>W</mi><mn>1</mn></msub><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow></mtd></mtr><mtr><mtd><msub><mi>W</mi><mn>2</mn></msub><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow></mtd></mtr></mtable></mfenced><mo>=</mo><msup><mfencedopen='('close=')'><mtable><mtr><mtd><msub><mi>ω</mi><mi>rx</mi></msub></mtd><mtd><msub><mi>ω</mi><mi>ry</mi></msub></mtd><mtd><msub><mi>ω</mi><mi>rz</mi></msub></mtd><mtd><msub><mi>ω</mi><mi>gx</mi></msub></mtd><mtd><msub><mi>ω</mi><mi>gy</mi></msub></mtd><mtd><msub><mi>ω</mi><mi>gz</mi></msub></mtd></mtr></mtable></mfenced><mi>T</mi></msup><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>5</mn><mo>)</mo></mrow></mrow>]]></math></maths>其中，qe＝[qe1qe2qe3]T為姿態(tài)四元數(shù)向量估計誤差，ΔB＝[ΔBxΔByΔBz]T為陀螺零偏估計誤差，W1(t)＝(ωrxωryωrz)T為陀螺誤差噪聲，W2(t)＝(ωgxωgyωgz)T為陀螺零偏估計誤差隨機游走模型的零均值白噪聲；{W(t)}為獨立零均值高斯白噪聲過程，其協(xié)方差矩陣E{W(t)WT(τ)}＝Q(t)δ(t-τ)其中，<mathsid="math0017"num="0017"><math><![CDATA[<mrow><mi>Q</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>Q</mi><mn>1</mn></msub></mtd><mtd><msub><mn>0</mn><mrow><mn>3</mn><mo>×</mo><mn>3</mn></mrow></msub></mtd></mtr><mtr><mtd><msub><mn>0</mn><mrow><mn>3</mn><mo>×</mo><mn>3</mn></mrow></msub></mtd><mtd><msub><mi>Q</mi><mn>2</mn></msub></mtd></mtr></mtable></mfenced></mrow>]]></math>id="icf0022"file="A2008100647140003C4.tif"wi="30"he="11"top="164"left="41"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/></maths>為對角陣，Q1的對角線元素根據(jù)陀螺噪聲來確定，取為噪聲RMS值的平方除以帶寬f*Q1＝[(RMSgyro)2/f*]·I3×3Q2的對角線元素根據(jù)陀螺零位漂移的特點來確定，系統(tǒng)的噪聲系數(shù)矩陣為<mathsid="math0018"num="0018"><math><![CDATA[<mrow><mi>G</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><mo>-</mo><mfrac><mn>1</mn><mn>2</mn></mfrac><msub><mi>I</mi><mrow><mn>3</mn><mo>×</mo><mn>3</mn></mrow></msub></mtd><mtd><msub><mn>0</mn><mrow><mn>3</mn><mo>×</mo><mn>3</mn></mrow></msub></mtd></mtr><mtr><mtd><msub><mn>0</mn><mrow><mn>3</mn><mo>×</mo><mn>3</mn></mrow></msub></mtd><mtd><msub><mi>I</mi><mrow><mn>3</mn><mo>×</mo><mn>3</mn></mrow></msub></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>6</mn><mo>)</mo></mrow></mrow>]]></math>id="icf0023"file="A2008100647140003C5.tif"wi="72"he="16"top="202"left="75"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/></maths>系統(tǒng)的狀態(tài)轉移矩陣為id="icf0024"file="A2008100647140003C6.tif"wi="72"he="15"top="221"left="75"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>上式中，id="icf0025"file="A2008100647140003C7.tif"wi="29"he="6"top="240"left="51"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>為載體角速度向量在載體坐標系中的估計值，id="icf0026"file="A2008100647140004C1.tif"wi="41"he="18"top="29"left="22"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>為向量id="icf0027"file="A2008100647140004C2.tif"wi="2"he="3"top="36"left="76"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>的反對稱矩陣；②根據(jù)加速度計測量值判斷系統(tǒng)加速運動情況和磁強計測量值判斷地磁受干擾情況，如果重力向量和地磁向量沒有受到干擾，轉到步驟③；如果重力向量或地磁向量受到干擾，轉到步驟④③建立卡爾曼濾波的觀測方程，并轉到步驟⑤Z(t)＝H(t)X(t)+V(t)(8)式中，Z(t)為t時刻系統(tǒng)的量測向量，H(t)為系統(tǒng)量測矩陣，V(t)為系統(tǒng)的量測噪聲向量；系統(tǒng)量測向量為id="icf0028"file="A2008100647140004C3.tif"wi="84"he="12"top="98"left="63"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>觀測向量id="icf0029"file="A2008100647140004C4.tif"wi="18"he="5"top="120"left="47"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>id="icf0030"file="A2008100647140004C5.tif"wi="49"he="20"top="113"left="67"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>式中，axayaz為三軸加速度計輸出的加速度；觀測向量id="icf0031"file="A2008100647140004C6.tif"wi="20"he="4"top="149"left="47"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>id="icf0032"file="A2008100647140004C7.tif"wi="56"he="20"top="142"left="71"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>式中，mxmymz為三軸磁強計輸出的磁強；觀測噪聲向量為<mathsid="math0019"num="0019"><math><![CDATA[<mrow><mi>V</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>V</mi><mn>1</mn></msub><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow></mtd></mtr><mtr><mtd><msub><mi>V</mi><mn>2</mn></msub><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>10</mn><mo>)</mo></mrow></mrow>]]></math>id="icf0033"file="A2008100647140004C8.tif"wi="84"he="11"top="173"left="63"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/></maths>{V(t)}為獨立均值高斯白噪聲過程，其協(xié)方差矩陣E{V(t)VT(τ)}＝Rd(t)δ(t-τ)<mathsid="math0020"num="0020"><math><![CDATA[<mrow><msub><mi>R</mi><mi>d</mi></msub><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>R</mi><mrow><mi>d</mi><mn>1</mn></mrow></msub></mtd><mtd><msub><mn>0</mn><mrow><mn>3</mn><mo>×</mo><mn>3</mn></mrow></msub></mtd></mtr><mtr><mtd><msub><mn>0</mn><mrow><mn>3</mn><mo>×</mo><mn>3</mn></mrow></msub></mtd><mtd><msub><mi>R</mi><mrow><mi>d</mi><mn>2</mn></mrow></msub></mtd></mtr></mtable></mfenced></mrow>]]></math>id="icf0034"file="A2008100647140004C9.tif"wi="32"he="12"top="203"left="31"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/></maths>為對角陣，Rd1和Rd2的對角線元素分別根據(jù)加速度計和磁強計的噪聲來確定，工程上可近似取為噪聲RMS值的平方Rd1＝(RMSacc)2·I3×3，Rd2＝(RMSmag)2·I3×3系統(tǒng)觀測矩陣為H(t)＝[H1(t)06×3](11)上式中，id="icf0035"file="A2008100647140004C10.tif"wi="101"he="14"top="240"left="47"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>④構造重力向量和地磁向量的法向量r＝a×m在n系中投影的法向量理論值為rn＝an×mn，可以由an和mn求得；在b系中法向量的測量值為rb＝ab×mb，可以由ab和mb求得；建立卡爾曼濾波的觀測方程Z(t)＝H(t)X(t)+V(t)式中，Z(t)為t時刻系統(tǒng)的量測向量；H(t)為系統(tǒng)量測矩陣；V(t)為系統(tǒng)的量測噪聲向量；(a)地磁向量受到干擾的情況系統(tǒng)量測向量為id="icf0036"file="A2008100647140005C1.tif"wi="89"he="12"top="92"left="61"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>觀測向量id="icf0037"file="A2008100647140005C2.tif"wi="18"he="5"top="108"left="44"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>id="icf0038"file="A2008100647140005C3.tif"wi="21"he="4"top="108"left="66"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>觀測噪聲向量為<mathsid="math0021"num="0021"><math><![CDATA[<mrow><mi>V</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>V</mi><mn>1</mn></msub><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow></mtd></mtr><mtr><mtd><msub><mi>V</mi><mn>3</mn></msub><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>10</mn><mo>)</mo></mrow></mrow>]]></math>id="icf0039"file="A2008100647140005C4.tif"wi="89"he="12"top="116"left="61"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/></maths>V3(t)為法向量的測量噪聲，與V1(t)和V2(t)有關{V(t)}為獨立均值高斯白噪聲過程，其協(xié)方差矩陣E{V(t)VT(τ)}＝Rd(t)δ(t-τ)<mathsid="math0022"num="0022"><math><![CDATA[<mrow><msub><mi>R</mi><mi>d</mi></msub><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>=</mo><mo></mo><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>R</mi><mrow><mi>d</mi><mn>1</mn></mrow></msub></mtd><mtd><msub><mn>0</mn><mrow><mn>3</mn><mo>×</mo><mn>3</mn></mrow></msub></mtd></mtr><mtr><mtd><msub><mn>0</mn><mrow><mn>3</mn><mo>×</mo><mn>3</mn></mrow></msub></mtd><mtd><msub><mi>R</mi><mrow><mi>d</mi><mn>3</mn></mrow></msub></mtd></mtr></mtable></mfenced></mrow>]]></math>id="icf0040"file="A2008100647140005C5.tif"wi="33"he="11"top="157"left="27"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/></maths>為對角陣系統(tǒng)觀測矩陣為H(t)＝[H1(t)06×3]上式中，id="icf0041"file="A2008100647140005C6.tif"wi="106"he="12"top="180"left="44"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>(b)重力向量受到干擾的情況系統(tǒng)量測向量為id="icf0042"file="A2008100647140005C7.tif"wi="92"he="12"top="203"left="61"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>觀測噪聲向量為<mathsid="math0023"num="0023"><math><![CDATA[<mrow><mi>V</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>V</mi><mn>3</mn></msub><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow></mtd></mtr><mtr><mtd><msub><mi>V</mi><mn>2</mn></msub><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>10</mn><mo>)</mo></mrow></mrow>]]></math>id="icf0043"file="A2008100647140005C8.tif"wi="93"he="11"top="218"left="62"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/></maths>{V(t)}為獨立均值高斯白噪聲過程，其協(xié)方差矩陣E{V(t)VT(τ)}＝Rd(t)δ(t-τ)<mathsid="math0024"num="0024"><math><![CDATA[<mrow><msub><mi>R</mi><mi>d</mi></msub><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>=</mo><mo></mo><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>R</mi><mrow><mi>d</mi><mn>3</mn></mrow></msub></mtd><mtd><msub><mn>0</mn><mrow><mn>3</mn><mo>×</mo><mn>3</mn></mrow></msub></mtd></mtr><mtr><mtd><msub><mn>0</mn><mrow><mn>3</mn><mo>×</mo><mn>3</mn></mrow></msub></mtd><mtd><msub><mi>R</mi><mrow><mi>d</mi><mn>2</mn></mrow></msub></mtd></mtr></mtable></mfenced></mrow>]]></math>id="icf0044"file="A2008100647140005C9.tif"wi="33"he="11"top="250"left="28"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/></maths>為對角陣系統(tǒng)觀測矩陣為H(t)＝[H1(t)06×3]上式中，id="icf0045"file="A2008100647140006C1.tif"wi="101"he="12"top="38"left="46"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>⑤根據(jù)擴展卡爾曼濾波遞推方程和步驟①、③或④的結果，建立卡爾曼濾波的時間傳播方程式中ωb＝[ωxωyωz]T為當前時刻三軸陀螺的輸出；id="icf0047"file="A2008100647140006C3.tif"wi="16"he="4"top="74"left="127"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>為上一時刻更新后的陀螺零偏估計；Δt為計算步長；<mathsid="math0025"num="0025"><math><![CDATA[<mrow><mover><mover><mi>q</mi><mo>‾</mo></mover><mo>·</mo></mover><mo>=</mo><mfrac><mn>1</mn><mn>2</mn></mfrac><mover><mi>q</mi><mo>‾</mo></mover><mtext>⊗</mtext><mover><mi>ω</mi><mo>‾</mo></mover><mo>=</mo><mfrac><mn>1</mn><mn>2</mn></mfrac><mover><mi>q</mi><mo>‾</mo></mover><mo>⊗</mo><mfencedopen='['close=']'><mtable><mtr><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mover><mi>ω</mi><mo>‾</mo></mover><mrow><mo>(</mo><msup><mi>t</mi><mo>-</mo></msup><mo>)</mo></mrow></mtd></mtr></mtable></mfenced></mrow>]]></math></maths>式中，<overscore>q</overscore><overscore>ω</overscore>分別表示四元數(shù)向量，id="icf0049"file="A2008100647140006C5.tif"wi="12"he="8"top="106"left="91"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>表示四元數(shù)乘法，具體為P(t+Δt)-)＝Φ(t+Δt，t)P(t+)ΦT(t+Δt，t)+Qd(t)(13)式中，Φ(t+Δt，t)＝I+F(t)Δt，Qd(t)＝Q(t)Δt⑥根據(jù)擴展卡爾曼濾波遞推方程和步驟①、②的結果，建立卡爾曼濾波的測量修正方程K(t)＝P(t-)HT(HP(t-)HT+Rd)-1(14)P(t+)＝(I-K(t)H)P(t-)(15)id="icf0053"file="A2008100647140006C9.tif"wi="8"he="3"top="222"left="34"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>經(jīng)過補償更新后需進行規(guī)一化處理；(4)輸出載體姿態(tài)參數(shù)由步驟(3)估計得到的姿態(tài)四元數(shù)id="icf0056"file="A2008100647140006C12.tif"wi="2"he="3"top="244"left="140"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>即可由公式(2)求得姿態(tài)矩陣id="icf0057"file="A2008100647140006C13.tif"wi="5"he="4"top="251"left="34"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>由姿態(tài)矩陣id="icf0058"file="A2008100647140006C14.tif"wi="4"he="4"top="251"left="64"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>的元素計算得出姿態(tài)角，具體步驟航向角主值id="icf0059"file="A2008100647140007C1.tif"wi="24"he="9"top="29"left="65"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>俯仰角主值φ主＝-sin-1C13橫滾角主值id="icf0060"file="A2008100647140007C2.tif"wi="25"he="9"top="50"left="65"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>由上述主值按照如下公式判斷真值φ＝φ主2.如權利要求1所述的硅微航姿系統(tǒng)慣性/地磁組合方法，其特征是步驟(3)中步驟③或步驟④卡爾曼濾波器觀測方程的構造，量測向量z在載體坐標b系中構造。3.如權利要求1所述的硅微航姿系統(tǒng)慣性/地磁組合方法，其特征是重力向量或地磁向量受到干擾時，采用步驟(3)中步驟④的卡爾曼濾波器觀測方程。全文摘要本發(fā)明硅微航姿系統(tǒng)慣性/地磁組合方法。具體涉及利用重力向量和地磁向量的觀測量確定載體的初始姿態(tài)；基于重力場、地磁場這兩個姿態(tài)參考向量選取最佳觀測向量的方法；組合Kalman濾波器觀測方程坐標系的選取原則；慣性/地磁Kalman組合濾波器的實現(xiàn)方法。通過上述發(fā)明，可以用低精度硅微慣性元件與磁傳感器組合實現(xiàn)硅微航姿系統(tǒng)，實時給出載體的航向姿態(tài)。本發(fā)明對低精度慣性/地磁組合方法具有通用性，能減小載體運動加速度對重力的影響和外界磁場對地磁場的影響，可廣泛應用于硅微航姿系統(tǒng)。文檔編號G01C21/08GK101290229SQ20081006471公開日2008年10月22日申請日期2008年6月13日優(yōu)先權日2008年6月13日發(fā)明者許兆新,琳趙,勇郝,閆保中,黃衛(wèi)權申請人:哈爾濱工程大學