基于ins慣性導航與gps導航以及磁力計的導航算法
【專利摘要】本發(fā)明公開了一種能夠準確反映無人機的姿態(tài)和位置信息的基于INS慣性導航與GPS導航以及磁力計的導航算法。該導航算法利用GPS導航長時間具有高的定位精度的優(yōu)點來彌補INS慣性導航累計誤差隨時間的增加而發(fā)散的缺點；利用INS慣性導航不受外界干擾、輸出的導航信息連續(xù)的特點彌補GPS易受干擾和輸出頻率有限的缺點，并且為了解決由慣性導航計算出的偏航角無法找到真北，以及漂移較大的情況，本算法利用磁力計計算出的偏航角來校正，獲得地理真北方向和穩(wěn)定的偏航角，另外，此算法穩(wěn)定性較強，能夠輸出比較滿意的導航定位信息，從而能夠準確反映載體的姿態(tài)和位置信息。適合在導航技術(shù)領(lǐng)域推廣應用。
【專利說明】
基于I NS慣性導航與GPS導航以及磁力計的導航算法
技術(shù)領(lǐng)域
[0001]本發(fā)明涉及導航技術(shù)領(lǐng)域，尤其是一種基于INS慣性導航與GPS導航以及磁力計的 導航算法。
【背景技術(shù)】
[0002] 在導航技術(shù)方面，目前應用得最多，最成熟的導航方式有INS慣性導航和衛(wèi)星導 航。GPS衛(wèi)星導航的優(yōu)點是具有全球性、全天候、長時間定位精度高的特點，但缺點是信號易 受干擾和遮擋，在強電磁環(huán)境下和有高樓遮擋時，信號質(zhì)量變差，并且其輸出頻率有限，一 般為1 一 10Hz，輸出不連續(xù)，在需要快速更新信息的場合，如機動性和實時性要求較高的無 人機系統(tǒng)上，GPS衛(wèi)星導航的缺點便凸顯出來。而INS慣性導航系統(tǒng)是一種全自主式的導航 方式，因此具有很強的隱蔽性和抗干擾的能力，并且輸出信息連續(xù)，短時間內(nèi)定位精度高。 但由于MEMS-INS器件自身的特點，陀螺儀和加速度計有初始零偏、隨機漂移等誤差，隨著時 間的累計作用，其誤差越來越大，長時間定位精度較差，最終無法準確反映無人機的姿態(tài)和 位置信息。

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

[0003] 本發(fā)明所要解決的技術(shù)問題是提供一種能夠準確得出載體的姿態(tài)和位置信息的 基于INS慣性導航與GPS導航以及磁力計的導航算法。
[0004] 本發(fā)明解決其技術(shù)問題所采用的技術(shù)方案為:該基于INS慣性導航與GPS導航以及 磁力計的導航算法，包括以下步驟：
[0005] A、將INS慣性導航系統(tǒng)的陀螺儀測得的載體角速度參數(shù)乂代入四元 數(shù)微分方程求解得到四元數(shù)耶^^^^其中^^^^^^^為陀螺儀在載體自身坐標系 下的測得的三個軸的角速度信息；
[0006] 所述四元數(shù)微分方程為： % 0 % 4l n c fl
[0007] . =0.5 b h n b mnby ^nbz 0 .?-% _么」 卜二：< 0 _Lf3_.
[0008] B、將步驟A中求解的(^，(^，(^，(^代入下式求解得到姿態(tài)矩陣^， q〇+%+q2+<h 2(m-關(guān)2)
[0009] 2(頓-祕）qo-q.+q.-q, 2(q2q, +q0ql) _2{q,tq：+q0q2) 2{q1qy-q0q]) q()-q,-q^qy _
[0010] 根據(jù)下述與方向余弦的關(guān)系式
[0011] ccs (pees/ -sin pcos f+cos psin Psin, -cos 汐 sin / €^= sin^cos# cos^cos^ sin^ cousin;/-sinpsinPeos/ -sin ^?sin;/ - cos (psm Goes y cos^cos/
[0012] 計算得出載體的INS慣性導航模塊姿態(tài)角0、y，；
[0013] C、利用磁力計測得的偏航角逆替換步驟B計算得到的偏航角
[0014] D、將INS慣性導航系統(tǒng)的加速度計測得的加速度參數(shù)護和步驟B中求解得到姿態(tài) 矩陣 < 代入下述微分方程中求解得到載體在INS慣性導航坐標系下的東、北、天三個方向上 的速度信息VN VE VU，所述微分方程為：
[0015] r=C：fb-{2〇i+m：n)xv^gn
[0016] 其中，vn=[VN VE VU] '分別為INS慣性導航坐標系中東、北、天方向上的速度，<為 地球自轉(zhuǎn)角速度，為載體繞INS慣性導航坐標系各軸向的轉(zhuǎn)動角速率，g nS重力加速度；
[0017] E、將步驟D計算得出的VN VE VU分別代入下式求解得出載體在INS慣性導航系統(tǒng)中 的位置信息，其中L為煒度，A為經(jīng)度，h為高度，
h = h (0) +/vudt，其中L (0)表示載體初始位 - ,丨 置的煒度值A(chǔ)(〇)表示載體初始位置的經(jīng)度值，h (0)表示載體距離地球表面的初始高度。RM 表示地球子午圈上的曲率半徑，Rn表示煒度圈上的曲率半徑；
[0019] F、建立狀態(tài)方程元（，）=f;⑴義,,（〇 + (/) % (〇和觀測方程Z (t) = H(t) X〗(t) +V ⑴，
[0020] XHt)表示INS慣性導航系統(tǒng)在t時刻的誤差狀態(tài)，它是一個15維的向量，如下所 示：
[0021] INS慣性導航系統(tǒng)沿東、北、天方向上的速度誤差；(i>x，4>y，4> z為載體的姿態(tài)角誤差;SL，S入， Sh分別代表載體所在煒度、經(jīng)度和高度誤差；ex，ey，￡2分別代表陀螺儀的隨機漂移； FN(t] F\(t) ▽,.，Uz分別為加速度計的隨機漂移，其中巧(〇= ，是一個15X15的矩 _ U ~Ir)」i5xl5 陣；其中？〃(〇對應于61，6%，6^，(}^，<^，4)2,81，從，611這9個參數(shù)的1吧慣性導航系統(tǒng)矩 陣，其非零元素如下：

[0035] 卩5(1:)為5~，5%，"，（^，<^，傘2,81，5人，5]1這9個參數(shù)與陀螺儀及加速度計漂移 u;- 之間的變換矩陣，其維數(shù)是9X6，&(0= C( 03x5 ?： -〇3x3 〇3x3 _
[0036] ?(^)為￡\士，￡2，\，'^，孓與陀螺儀及加速度計漂移對應的1吧慣性導航系統(tǒng)矩 陣，是一個維數(shù)為6 X 6的對角線矩陣，表示如下：
[0037] FM(t) = diag[_l/Tgx _1/Tgy _1/Tgz _1/Tax _1/Tay _1/Taz];其中，Tgx表示陀螺儀x 軸的誤差模型的時間常數(shù)，Tgy表示陀螺儀y軸的誤差模型的時間常數(shù)，Tgz表示陀螺儀z軸的 誤差模型的時間常數(shù)，T ax表示加速度計x軸誤差模型的時間常數(shù)，Tay表示加速度計y軸誤差 模型的時間常數(shù)，T az表示加速度計z軸誤差模型的時間常數(shù)；
[0038] Gi(t) = diag[l 1 1......11] 15X15 ；
[0039] Wi(t)是一個15維的向量，如下所示：
[0040] ffi(t) = [ai a2 a3 a4 as a6 a7 as a9 aio an ai2 ai3 ai4 ais]，
[0041] ai a2 a3 a4 as a6 a7 as a9 aio aii ai2 ai3 ai4 ais表;^系統(tǒng)過程噪聲序列；
[0042] Z(t)為載體在INS慣性導航中的位置速度信息與載體在GPS導航系統(tǒng)中的位置速 度信息的差值，是一個6維向量，
[0043] Z(t) = [5vx+NVx 5vy+NVy 5vz+Nvz (RM+h)5L+Ny (RM+h)cosL5A+Nx 5h+Nh]T，其中，Nvx 表示GPS導航系統(tǒng)在x方向上的速度誤差，Nvy表示GPS導航系統(tǒng)在y方向上的速度誤差，Nvz表 示GPS導航系統(tǒng)在z方向上的速度誤差，Nx表示GPS導航系統(tǒng)在x方向上的位置誤差，Ny表示 GPS導航系統(tǒng)在y方向上的位置誤差，Nh表示GPS導航系統(tǒng)在z方向上的位置誤差； /、'氧(01 ^
[_ H(和:k:(/)}，r(今卞
[0045] ii.{t) = \diag[\ 1 l]：〇3xl2}
[0046] //4〇 = {〇,x(,^^[(^1/{Rx+h)cmL l];03x6}
[0047] Vv(t) = [Nvx Nvy Nvz]t
[0048] VP(t) = [Nx Ny Nz]t
[0049] G、將上述得到的連續(xù)狀態(tài)方程尤G') = M0H) + G, (0% (0離散化后得到知= ① k, k-lXk-l+Wk-1，其中
[0050] 將上述得到的連續(xù)觀測方程ZUhmOXKtHVU)離散化后得到Zk = HkXk+Vk;
[0051] 其中I是單位矩陣，F(xiàn)是INS慣性導航系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣，A t是離散化后INS慣性 導航系統(tǒng)的采樣時間；
[0052] H、將載體在INS慣性導航系統(tǒng)中的位置速度信息與載體在GPS導航系統(tǒng)中的位置 速度信息作差得到Z( t)在k時刻的觀測fg息z;
[0053] I、計算k時亥ijlNS慣性導航系統(tǒng)狀態(tài)的最優(yōu)估計值 其中，矣^ =氣wm，義/MM為在k -1時刻1 N S慣性導航系統(tǒng)狀態(tài)的最優(yōu)估計值， & =化I杧是INS慣性導航系統(tǒng)的噪聲矩陣，其大 小是由INS慣性導航元件的性能決定，ffp # $ = [/ -Rk是
[HP__ 系統(tǒng)測量噪聲的方差陣，其大小是郵PS接收機的性能決定；
[0054] J、將計算得到的值與載體在INS慣性導航系統(tǒng)中的位置速度信息作差得到最 優(yōu)的導航參數(shù)；
[0055] K、重復步驟H-J，得到連續(xù)的載體的導航信息參數(shù)。
[0056]本發(fā)明的有益效果：該基于INS慣性導航與GPS導航以及磁力計的導航算法利用 INS慣性導航和GPS導航組合導航的方法解決了單一的GPS導航技術(shù)易受干擾和遮擋，短時 定位精度不高，輸出頻率有限并且輸出不連續(xù)的缺點；同時也解決了單一的INS慣性導航參 數(shù)累計誤差越來越大，長時間定位精度發(fā)散的缺點，利用GPS導航長時間具有高的定位精度 的優(yōu)點來彌補INS慣性導航累計誤差隨時間的增加而發(fā)散的缺點;利用INS慣性導航不受外 界干擾、輸出的導航信息連續(xù)的特點彌補GPS易受干擾和輸出頻率有限的缺點，并且為了解 決由慣性導航計算出的偏航角無法找到真北，以及漂移較大的情況，本算法利用磁力計計 算出的偏航角來校正，獲得地理真北方向和穩(wěn)定的偏航角，另外，此算法穩(wěn)定性較強，能夠 輸出比較滿意的導航定位信息，從而能夠準確反映載體的姿態(tài)和位置信息。
【附圖說明】
[0057]圖1為本發(fā)明所述基于INS慣性導航與GPS導航以及磁力計的導航算法計算得出的 煒度誤差值；
[0058]圖2為本發(fā)明所述基于INS慣性導航與GPS導航以及磁力計的導航算法計算得出的 煒度誤差方差值；
[0059]圖3為本發(fā)明所述基于INS慣性導航與GPS導航以及磁力計的導航算法計算得出的 經(jīng)度誤差值；
[0060] 圖4為本發(fā)明所述基于INS慣性導航與GPS導航以及磁力計的導航算法計算得出的 經(jīng)度誤差方差值；
[0061] 圖5為本發(fā)明所述基于INS慣性導航與GPS導航以及磁力計的導航算法計算得出的 高度誤差值；
[0062]圖6為本發(fā)明所述基于INS慣性導航與GPS導航以及磁力計的導航算法計算得出的 高度誤差方差值；
[0063]圖7為本發(fā)明所述基于INS慣性導航與GPS導航以及磁力計的導航算法計算得出的 東向速度誤差值；
[0064]圖8為本發(fā)明所述基于INS慣性導航與GPS導航以及磁力計的導航算法計算得出的 東向速度誤差方差值；
[0065]圖9為本發(fā)明所述基于INS慣性導航與GPS導航以及磁力計的導航算法計算得出的 北向速度誤差值；
[0066]圖10為本發(fā)明所述基于INS慣性導航與GPS導航以及磁力計的導航算法計算得出 的北向速度誤差方差值；
[0067]圖11為本發(fā)明所述基于INS慣性導航與GPS導航以及磁力計的導航算法計算得出 的天向速度誤差值；
[0068]圖12為本發(fā)明所述基于INS慣性導航與GPS導航以及磁力計的導航算法計算得出 的天向速度誤差方差值。
【具體實施方式】
[0069]本發(fā)明所述的基于INS慣性導航與GPS導航以及磁力計的導航算法，包括以下步 驟：
[0070] A、將INS慣性導航系統(tǒng)的陀螺儀測得的載體角速度參數(shù)^4, <2代入四元 數(shù)微分方程求解得到四元數(shù)9()，91，92^;其中《4,64，< &為陀螺儀在載體自身坐標系 下的測得的三個軸的角速度信息；
[0071 ]所述四元數(shù)微分方程為： 4) 0 --ml-y ~minbz q〇 4i _ A ^
[0072] ? -0.5 b b (\ b % 份nby -①- 〇 ⑴nbx q! _4」 卜L <, -0 jLft」，
[0073] B、將步驟A中求解的階叫沿沖代入下式求解得到姿態(tài)矩陣勺 ? %n.i+% 2[關(guān)2+鶴)2(muh)
[0074] Cbn= 2(q'q2-制')q^-q.+q.-q, 2{q1q^q {)q]) _2(肌+%七)2(%n 而）〇n)~q,~q2-^q,_
[0075] 根據(jù)下述0與方向余弦的關(guān)系式
[0076] cos (pees/ -sin rpcos/+ccs (psm 0siny -cos 沒 sin/ = sinpeos^ cos^?cos^ sin/9 cospsinv-siru^sin^cos/ -sin^sin;/-cos^sin 6?cos/ cos?9cos;/
[0077] 計算得出載體的INS慣性導航模塊姿態(tài)角0、Y、A
[0078] C、利用磁力計測得的偏航角口替換步驟B計算得到的偏航角口:；
[0079] D、將INS慣性導航系統(tǒng)的加速度計測得的加速度參數(shù)護和步驟B中求解得到姿態(tài) 矩陣0代入下述微分方程中求解得到載體在INS慣性導航坐標系下的東、北、天三個方向上 的速度信息VN VE VU，所述微分方程為：
[0080] V" = c；jb - (2< + <) x vw +
[0081] 其中，vn=[VN vE vu] '分別為INS慣性導航坐標系中東、北、天方向上的速度，6^為 地球自轉(zhuǎn)角速度，為載體繞INS慣性導航坐標系各軸向的轉(zhuǎn)動角速率，g nS重力加速度； [0082] E、將步驟D計算得出的VN VE vu分別代入下式求解得出載體在INS慣性導航系統(tǒng)中 的位置信息，其中L為煒度，A為經(jīng)度，h為高度，
h = h(0)+/vudt，其中L(0)表示載體初始位 置的煒度值A(chǔ)(〇)表示載體初始位置的經(jīng)度值，h (0)表示載體距離地球表面的初始高度。Rm 表示地球子午圈上的曲率半徑，Rn表示煒度圈上的曲率半徑；
[0084] F、建立狀態(tài)方程乂,（/) = (/)尤,（/) + G, 〇聲;（〇 和觀測方程Z(t)=H(t)Xi(t)+V ⑴，
[0085] X〗（t)表示INS慣性導航系統(tǒng)在t時刻的誤差狀態(tài)，它是一個15維的向量，如下所 示：
[0086] X，⑴5vr (% 戎終於況說漁.4 5 ，5Vx，Svy，Sv^ INS慣性導航系統(tǒng)沿東、北、天方向上的速度誤差；(K，(i>y，(^為載體的姿態(tài)角誤差;SL，S入， Sh分別代表載體所在煒度、經(jīng)度和高度誤差；ex，ey，ez分別代表陀螺儀的隨機漂移； Vt，V,.，V2分別為加速度計的隨機漂移，其中，是一個15X15的矩 L 0巧々)丄&15 陣；其中？〃(1:)對應于5^，5%，^，(^，<^，巾2,81，5\，5]1這9個參數(shù)的1吧慣性導航系統(tǒng)矩 陣，其非零元素如下：
[0100]卩5(1:)為5~，5%，^，(^，<^，巾2,81，5人，5]1這9個參數(shù)與陀螺儀及加速度計漂移 '〇3x3 Cf 之間的變換矩陣，其維數(shù)是9X6，&⑴=C： 03x3 ; _〇3x3 〇r?3_
[0101] FM(t)為￡\士，￡2，1，￥^'^與陀螺儀及加速度計漂移對應的1吧慣性導航系統(tǒng)矩 陣，是一個維數(shù)為6 X 6的對角線矩陣，表示如下：
[0102] FM(t) = diag[_l/Tgx _1/Tgy _1/Tgz _1/Tax _1/Tay _1/Taz];其中，Tgx表示陀螺儀x 軸的誤差模型的時間常數(shù)，Tgy表示陀螺儀y軸的誤差模型的時間常數(shù)，Tgz表示陀螺儀z軸的 誤差模型的時間常數(shù)，T ax表示加速度計x軸誤差模型的時間常數(shù)，Tay表示加速度計y軸誤差 模型的時間常數(shù)，T az表示加速度計z軸誤差模型的時間常數(shù)；
[0103] Gi(t) = diag[l 1 1......11] 15X15 ；
[0104] Wi(t)是一個15維的向量，如下所示：
[0105] Wi(t) = [ai a2 a3 a4 as a6 a7 as ag ai。an ai2 ai3 ai4 ai5]，
[0106] ai a2 a3 a4 a5 a6 a7 as ag ai。aii ai2 ai3 ai4 ai5表不系統(tǒng)過程噪聲序列；
[0107] Z(t)為載體在INS慣性導航中的位置速度信息與載體在GPS導航系統(tǒng)中的位置速 度信息的差值，是一個6維向量，
[0108] Z(t) = [5vx+Nvx 5vy+NVy 5vz+Nvz (RM+h)5L+Ny (RM+h)cosL5人+NX 5h+Nh]T，其中，Nvx 表示GPS導航系統(tǒng)在x方向上的速度誤差，Nvy表示GPS導航系統(tǒng)在y方向上的速度誤差，Nvz表 示GPS導航系統(tǒng)在z方向上的速度誤差，N x表示GPS導航系統(tǒng)在x方向上的位置誤差，Ny表示 GPS導航系統(tǒng)在y方向上的位置誤差，Nh表示GPS導航系統(tǒng)在z方向上的位置誤差； 邱):[説，?[，其中
[0110] H,(i) = {diag[\ 1 l]；〇K12}
[0111] Hp {t) = {〇3x6 Idiag[(i?A/ + h) {R, + h)cosL l]:03x(l}
[0112] Vv(t) = [Nvx Nvy Nvz]T
[0113] VP(t) = [Nx Ny Nz]t
[0114] G、將上述得到的連續(xù)狀態(tài)方程尤（?）= M/) .U〇 + ⑴離散化后得到Xk = ① k，k-iXk-i+Wk-丄，其中 ! =/ + 姻 +士 (廠泣)2 + 士 '21 3! 4!
[0115] 將上述得到的連續(xù)觀測方程2(〇=訊〇乂1(〖)+￥(〇離散化后得到2 1< = 111^1<+￥1<; [0116]其中I是單位矩陣，F(xiàn)是INS慣性導航系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣，A t是離散化后INS慣性 導航系統(tǒng)的采樣時間；
[0117] H、將載體在INS慣性導航系統(tǒng)中的位置速度信息與載體在GPS導航系統(tǒng)中的位置 速度彳g息作差得到Z(t)在k時刻的觀測彳g息z;
[0118] I、計算k時刻INS慣性導航系統(tǒng)狀態(tài)的最優(yōu)估計值矣丨，為l:i_ = + A， 其中， 尤，-1 =① A--1 之-l|/r~l， 之為在k-1時刻INS慣性導航系統(tǒng)狀態(tài)的最優(yōu)估計值， 《=X (私 V此 + 及)' 巧-m = I-AAL,# 其大小是由ins慣性導航元件的性能決定，-心凡kw 系統(tǒng)測量噪聲的方差陣，其大小是郵PS接收機的性能決定；
[0119] J、將計算得到的武^值與載體在INS慣性導航系統(tǒng)中的位置速度信息作差得到最 優(yōu)的導航參數(shù)；
[0120] K、重復步驟H-J，得到連續(xù)的載體的導航信息參數(shù)。
[0121]本發(fā)明的有益效果：該基于INS慣性導航與GPS導航以及磁力計的導航算法利用 INS慣性導航和GPS導航組合導航的方法解決了單一的GPS導航技術(shù)易受干擾和遮擋，短時 定位精度不高，輸出頻率有限并且輸出不連續(xù)的缺點；同時也解決了單一的INS慣性導航參 數(shù)累計誤差越來越大，長時間定位精度發(fā)散的缺點，利用GPS導航長時間具有高的定位精度 的優(yōu)點來彌補INS慣性導航累計誤差隨時間的增加而發(fā)散的缺點;利用INS慣性導航不受外 界干擾、輸出的導航信息連續(xù)的特點彌補GPS易受干擾和輸出頻率有限的缺點，并且為了解 決由慣性導航計算出的偏航角無法找到真北，以及漂移較大的情況，本算法利用磁力計計 算出的偏航角來校正，獲得地理真北方向和穩(wěn)定的偏航角，另外，此算法穩(wěn)定性較強，能夠 輸出比較滿意的導航定位信息，從而能夠準確反映載體的姿態(tài)和位置信息。
[0122] 圖1為本發(fā)明所述基于INS慣性導航與GPS導航以及磁力計的導航算法計算得出的 煒度誤差值；圖2為本發(fā)明所述基于INS慣性導航與GPS導航以及磁力計的導航算法計算得 出的煒度誤差方差值；圖3為本發(fā)明所述基于INS慣性導航與GPS導航以及磁力計的導航算 法計算得出的經(jīng)度誤差值；圖4為本發(fā)明所述基于INS慣性導航與GPS導航以及磁力計的導 航算法計算得出的經(jīng)度誤差方差值；圖5為本發(fā)明所述基于INS慣性導航與GPS導航以及磁 力計的導航算法計算得出的高度誤差值；圖6為本發(fā)明所述基于INS慣性導航與GPS導航以 及磁力計的導航算法計算得出的高度誤差方差值；圖7為本發(fā)明所述基于INS慣性導航與 GPS導航以及磁力計的導航算法計算得出的東向速度誤差值；圖8為本發(fā)明所述基于INS慣 性導航與GPS導航以及磁力計的導航算法計算得出的東向速度誤差方差值；圖9為本發(fā)明所 述基于INS慣性導航與GPS導航以及磁力計的導航算法計算得出的北向速度誤差值；圖10為 本發(fā)明所述基于INS慣性導航與GPS導航以及磁力計的導航算法計算得出的北向速度誤差 方差值；圖11為本發(fā)明所述基于INS慣性導航與GPS導航以及磁力計的導航算法計算得出的 天向速度誤差值；圖12為本發(fā)明所述基于INS慣性導航與GPS導航以及磁力計的導航算法計 算得出的天向速度誤差方差值；
[0123] 從上述測試結(jié)果圖可以看出，本發(fā)明所述基于INS慣性導航與GPS導航以及磁力計 的導航算法得出的經(jīng)度、煒度、高度的誤差方差均能快速收斂至比較小的數(shù)值;對位置、速 度等導航信息也能實現(xiàn)濾平滑作用，不會產(chǎn)生大的跳變，算法的穩(wěn)定性較強。
【主權(quán)項】
1.基于INS慣性導航與GPS導航以及磁力計的導航算法，其特征在于包括以下步驟： A、 將INS慣性導航系統(tǒng)的陀螺儀測得的載體角速度參數(shù)《1，代入四元數(shù)微 分方程求解得到四元數(shù)耶^1^243;其中6匕,，^4/64 2為陀螺儀在載體自身坐標系下的 測得的三個軸的角速度信息； 所述四元數(shù)微分方程為：B、 將步驟A中求解的qo，qi，q2，q3代入下式求解得到姿態(tài)矩陣Cf，根據(jù)下述與方向余弦的關(guān)系式計算得出載體的INS慣性導航模塊姿態(tài)角θ、γ C、 利用磁力計測得的偏航角Ρ替換步驟Β計算得到的偏航角 D、 將INS慣性導航系統(tǒng)的加速度計測得的加速度參數(shù)戶和步驟Β中求解得到姿態(tài)矩陣0 代入下述微分方程中求解得到載體在INS慣性導航坐標系下的東、北、天三個方向上的速度 信息VN VE VU，所述微分方程為：其中，vn=[VN VE VU]'分別為INS慣性導航坐標系中東、北、天方向上的速度，貧〗為地球 自轉(zhuǎn)角速度，<為載體繞INS慣性導航坐標系各軸向的轉(zhuǎn)動角速率，gns重力加速度； E、 將步驟D計算得出的VN VE Vu分別代入下式求解得出載體在INS慣性導航系統(tǒng)中的位 置信息，其中L為煒度，λ為經(jīng)度，h為高度，h = h(0)+Jvudt,其中L(0)表示載體初始位置的煒 度值，λ(0)表示載體初始位置的經(jīng)度值，h(0)表示載體距離地球表面的初始高度。RM表示地 球子午圈上的曲率半徑，RN表示煒度圈上的曲率半徑； F、建立狀態(tài)方程夂⑴⑴.\^) + 6(/)沙:,(/)和觀測方程2(〇=11(〇沿(〇+￥(七）， XMt)表示INS慣性導航系統(tǒng)在t時刻的誤差狀態(tài)，它是一個15維的向量，如下所示： ΑΜψ、'·'.. A 況：戎 4 於說汾浼 $ € Vv. VjjvxJvyJvAlNS 慣性導航系統(tǒng)沿東、北、天方向上的速度誤差；Φχ，Φγ，Φζ為載體的姿態(tài)角誤差;δ?，δλ，δ?! 分別代表載體所在煒度、經(jīng)度和高度誤差；ε χ，4，εζ分別代表陀螺儀的隨機漂移; 分別為加速度計的隨機漂移，，是一個15 X 15的矩陣;其中F N (t)對應于δνχ，δνγ，δνζ，φ χ，φ y，φ z，SL，δλ，Sh這9個參數(shù)的INS慣性導航系統(tǒng)矩陣，其非零 元素如下：Fs(t)為3"，5%，5￥2，(^，(^，(|)2況，3入，5]1這9個參數(shù)與陀螺儀及加速度計漂移之間的 變換矩陣，F(xiàn)m⑴為εχ，ey，εζ，V,，V,.，丨與陀螺儀及加速度計漂移對應的 INS慣性導航系統(tǒng)矩陣，是 一個維數(shù)為6 X 6的對角線矩陣，表示如下： FM(t)=diag[-l/Tgx -1/Tgy -1/Tgz -1/Tax -1/Tay -1/Taz];其中，Tgx表示陀螺儀X軸的 誤差模型的時間常數(shù)，Tgy表示陀螺儀y軸的誤差模型的時間常數(shù)，Tgz表示陀螺儀z軸的誤差 模型的時間常數(shù)，T ax表示加速度計X軸誤差模型的時間常數(shù)，Tay表示加速度計y軸誤差模型 的時間常數(shù)，T az表示加速度計z軸誤差模型的時間常數(shù)； Gi(t)=diag[l 11......1 1]?5χ?5; Wi(t)是一個15維的向量，如下所示： Wi(t) = [ai a2 a3 a4 as a6 a7 as ag aio an ai2 ai3 ai4 ai5]， ai a2 a3 a4 as a6 a7 as ag ai。aii ai2 ai3 ai4 ai5表不系統(tǒng)過程噪聲序列； Z(t)為載體在INS慣性導航中的位置速度信息與載體在GPS導航系統(tǒng)中的位置速度信 息的差值，是一個6維向量， Ζ(?) = [δνχ+Ννχ δνγ+Ννγ δνζ+Ννζ (RM+h)3L+Ny (RM+h)cosL3A+Nx 3h+Nh]T，其中，Ννχ表不 GPS導航系統(tǒng)在x方向上的速度誤差，Nvy表示GPS導航系統(tǒng)在y方向上的速度誤差，Nvz表示 GPS導航系統(tǒng)在ζ方向上的速度誤差，Νχ表示GPS導航系統(tǒng)在X方向上的位置誤差，Ny表示GPS 導航系統(tǒng)在y方向上的位置誤差，Nh表示GPS導航系統(tǒng)在z方向上的位置誤差；Vv(t) = [Nvx Nvy Nvz]T Vp(t) = [Nx Ny Nz]T G、 將上述得到的連續(xù)狀態(tài)方柷夂（/)二dR, (/) + 6,(/)%⑴離散化后得到Xk = ?k,k-lXk-l+Wk-l:將上述得到的連續(xù)觀測方程z (t) =H( t )X: (t) +V( t)離散化后得到Zk=HkXk+Vk; 其中I是單位矩陣，F(xiàn)是INS慣性導航系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣，△ t是離散化后INS慣性導航 系統(tǒng)的采樣時間； H、 將載體在INS慣性導航系統(tǒng)中的位置速度信息與載體在GPS導航系統(tǒng)中的位置速度 信息作差得到Z(t)在k時刻的觀測信息ζ; I、 計算k時亥[|INS慣性導航系統(tǒng)狀態(tài)的最優(yōu)估計值之|it，之> =矣+ 卜-i/Jn)， 其中，之Η = 龍為在k -1時刻IN S慣性導航系統(tǒng)狀態(tài)的最優(yōu)估計值， [，二慫)-1，尸…=Φ.-+α-^INS慣性導航系統(tǒng)的噪 聲矩陣，其大小是由I N S慣性導航元件的性能決定：6 =[/-人J/-人ν? ，Rk是系統(tǒng)測量噪聲的方差陣，其大小是由GPS接 收機的性能決定； J、 將計算得到的龍^值與載體在INS慣性導航系統(tǒng)中的位置速度信息作差得到最優(yōu)的 導航參數(shù)； K、 重復步驟H-J，得到連續(xù)的載體的導航信息參數(shù)。
【文檔編號】G01C21/20GK105928519SQ201610244759
【公開日】2016年9月7日
【申請日】2016年4月19日
【發(fā)明人】張瑜, 李詩揚
【申請人】成都翼比特自動化設(shè)備有限公司