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

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

[0003] 本發(fā)明所要解決的技術(shù)問題是提供一種能夠準(zhǔn)確反映無人機的姿態(tài)和位置信息 的無人機導(dǎo)航系統(tǒng)。
[0004] 本發(fā)明解決其技術(shù)問題所采用的技術(shù)方案為:該無人機導(dǎo)航系統(tǒng)，包括INS慣性導(dǎo) 航系統(tǒng)模塊、磁力計、GPS導(dǎo)航系統(tǒng)模塊、控制中心，所述INS慣性導(dǎo)航系統(tǒng)模塊包括陀螺儀 和加速度計，所述陀螺儀、加速度計、磁力計、GPS導(dǎo)航系統(tǒng)模塊分別與控制中心信號連接， 所述陀螺儀用于測量無人機的角速度參數(shù)并將其測得的角速度參數(shù)傳遞給控制中心，所述 磁力計用于測量無人機的偏航角并將其測得的偏航角參數(shù)傳遞給控制中心，所述加速度計 用于測量無人機的加速度并將其測得的加速度參數(shù)傳遞給控制中心，所述GPS導(dǎo)航系統(tǒng)模 塊用于測量無人機在GPS導(dǎo)航系統(tǒng)中的位置速度信息，所述控制中心根據(jù)接收到的角速度 參數(shù)、偏航角參數(shù)、加速度參數(shù)計算出無人機在INS慣性導(dǎo)航系統(tǒng)中的位置速度信息，控制 中心將無人機在GPS導(dǎo)航系統(tǒng)中的位置速度信息與無人機在GPS導(dǎo)航系統(tǒng)中的位置速度信 息進行Kalman濾波并進行導(dǎo)航誤差估計，然后利用導(dǎo)航誤差估計修正無人機在INS慣性導(dǎo) 航系統(tǒng)中的位置速度信息從而得到最終的導(dǎo)航信息。
[0005] 進一步的是，所述陀螺儀、加速度計、磁力計、分別與控制中心通過12C總線連接。
[0006] 進一步的是，所述GPS導(dǎo)航系統(tǒng)模塊與控制中心通過USART串口連接。
[0007] 進一步的是，所述控制中心采用如下方法計算得到最終的導(dǎo)航信息，其具體計算 方法如下所述：
[0008] A、將陀螺儀測得的無人機角速度參數(shù)代入四元數(shù)微分方程求解得 到四元數(shù)9〇^142^3;其中》1，《1.，》^為陀螺儀在無人機自身坐標(biāo)系下的測得的三個 軸的角速度信息；
[0009] 所述四元數(shù)微分方程為： % 0 -°thy fo 0 ①L -^ihy <h
[0010] ? =0.5 b b n b % ^nhy 0 組-€2 _4」 卜L < -《 0」U」<
[0011] B、將步驟A中求解的卯，(11，(12，(13代入下式求解得到姿態(tài)矩陣0， qQ+q]+q2+q% 2(明2+她)2(q'q:「q(、c，2)
[0012] Cbn= 2(q]q1-q{)q-) q.-q.+q.-q, 2(^+M) _2(q]q:,+q{)q1) 2\q2q,-q()q,) q{)-q,-q2^q,_
[0013] 根據(jù)下述C"6與方向余弦的關(guān)系式
[0014] cos 妒cos / -sin f/>cos v+a^s (pm\ Osmy -cos 沒sin / Qt - sin<^cos6? cospcos 夕 sin 汐 cos (psm /-sin ^sin ^cos y -sinpsiiif-cos 供sin0eosf cos沒cos,
[0015] 計算得出無人機的INS慣性導(dǎo)航模塊姿態(tài)角0、丫，；
[0016] C、利用磁力計測得的偏航角P替換步驟B計算得到的偏航角
[0017] D、將加速度計測得的加速度參數(shù)爐和步驟B中求解得到姿態(tài)矩陣g代入下述微分 方程中求解得到無人機在INS慣性導(dǎo)航坐標(biāo)系下的東、北、天三個方向上的速度信息VN VE VU，所述微分方程為：
[0018] vw=C；/-(2<+<)xvw+gw
[0019] 其中，vn=[VN VE vu]'分別為INS慣性導(dǎo)航坐標(biāo)系中東、北、天方向上的速度，<為 地球自轉(zhuǎn)角速度，?i為無人機繞INS慣性導(dǎo)航坐標(biāo)系各軸向的轉(zhuǎn)動角速率，g n為重力加速 度；
[0020] E、將步驟D計算得出的VN VE VU分別代入下式求解得出無人機在INS慣性導(dǎo)航系統(tǒng) 中的位置信息，其中L為煒度，A為經(jīng)度，h為高度，
h = h(0)+/vudt，其中L(0)表示無人機初始 位置的煒度值A(chǔ)(0)表示無人機初始位置的經(jīng)度值，h(0)表示無人機距離地球表面的初始 高度。Rm表示地球子午圈上的曲率半徑，Rn表示煒度圈上的曲率半徑；
[0022] F、建立狀態(tài)方程尤（/) = 6(/) A', (/) + 0)(/)%(/)和觀測方程ZU^mOXKO+V (thXKt)表示INS慣性導(dǎo)航系統(tǒng)在t時刻的誤差狀態(tài)，它是一個15維的向量，如下所示： &(/)=[加 r %為疼銓見況浼盡今巧%]，51， 5々，^為1奶慣性 導(dǎo)航系統(tǒng)沿東、北、天方向上的速度誤差；4>x，4>y，4>z為無人機的姿態(tài)角誤差;SL，SA，Sh分 別代表無人機所在煒度、經(jīng)度和高度誤差；ex，ey，ez分別代表陀螺儀的隨機漂移； 分別為加速度計的隨機漂移，其中，是一個15X15的矩陣;其中FN M V /Jl5xl'5 (t)對應(yīng)于"，^，^，(}^，(^，傘2，乩，6入，恥這9個參數(shù)的1吧慣性導(dǎo)航系統(tǒng)矩陣，其非零 元素如下：
[0036] 卩5(1:)為5~，5%，"，（^，<^，傘2,81，5人，5]1這9個參數(shù)與陀螺儀及加速度計漂移 _u_ 之間的變換矩陣，其維數(shù)是9\6,6(『)=C 〇3x3 ; _〇3x3 〇3x3_
[0037] ?(^)為￡\士，￡2，1，￥,.，1與陀螺儀及加速度計漂移對應(yīng)的1吧慣性導(dǎo)航系統(tǒng)矩 陣，是一個維數(shù)為6 X 6的對角線矩陣，表示如下：
[0038] FM(t) = diag[_l/Tgx _1/Tgy _1/Tgz _1/Tax _1/Tay _1/Taz];其中，Tgx表示陀螺儀x 軸的誤差模型的時間常數(shù)，T gy表示陀螺儀y軸的誤差模型的時間常數(shù)，Tgz表示陀螺儀z軸的 誤差模型的時間常數(shù)，T ax表示加速度計x軸誤差模型的時間常數(shù)，Tay表示加速度計y軸誤差 模型的時間常數(shù)，T az表示加速度計z軸誤差模型的時間常數(shù)；
[0039] Gi(t) = diag[l 1 1......11] 15X15 ；
[0040] Wi(t)是一個15維的向量，如下所示：
[0041] Wi(t) = [ai a2 a3 a4 as a6 a7 as ag aio an ai2 ai3 ai4 ai5]，
[0042] ai a2 a3 a4 a5 a6 a7 as ag ai。aii ai2 ai3 ai4 ai5表不系統(tǒng)過程噪聲序
[0043] 列；
[0044] Z(t)為無人機在INS慣性導(dǎo)航系統(tǒng)中的位置速度信息與無人機在GPS導(dǎo)航系統(tǒng)中 的位置速度信息的差值，是一個6維向量，
[0045] Z(t) = [5vx+Nvx 5vy+NVy 5vz+Nvz (RM+h)5L+Ny (RM+h)cosL5A+Nx 5h+Nh]T，其中，Nvx 表示GPS導(dǎo)航系統(tǒng)在x方向上的速度誤差，Nvy表示GPS導(dǎo)航系統(tǒng)在y方向上的速度誤差，Nvz表 示GPS導(dǎo)航系統(tǒng)在z方向上的速度誤差，N x表示GPS導(dǎo)航系統(tǒng)在x方向上的位置誤差，Ny表示 GPS導(dǎo)航系統(tǒng)在y方向上的位置誤差，N h表示GPS導(dǎo)航系統(tǒng)在z方向上的位置誤差；
[0046]沖)=un 其中
[0047] //,.(/) = {?'/V/g[l 1 l]：〇.xl2}
[0048] //,,(/) = {〇￥;必喂[(/?,,,+/?)(義+/?)cosL
[0049] Vv(t) = [Nvx Nvy Nvz]t
[0050] VP(t) = [Nx Ny Nz]t
[0051] G、將上述得到的連續(xù)狀態(tài)方程鳥(今⑷:+句的心⑴離散化后得到xk = ① k, k-lXk-l+Wk-1，其中
[0052] 將上述得到的連續(xù)觀測方程ZUhmOXKtHVU)離散化后得到Zk = HkXk+Vk;
[0053] 其中I是單位矩陣，F(xiàn)是INS慣性導(dǎo)航系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣，A t是離散化后INS慣性 導(dǎo)航系統(tǒng)的采樣時間；
[0054] H、將無人機在INS慣性導(dǎo)航系統(tǒng)中的位置速度信息與無人機在GPS導(dǎo)航系統(tǒng)中的 位置速度信息作差得到Z(t)在k時刻的觀測信息z;
[0055] I、計算k時刻INS慣性導(dǎo)航系統(tǒng)狀態(tài)的最優(yōu)估計值％，夂|lt = + Aw)， 其中，之|iM = 為在k-1時刻，INS慣性導(dǎo)航系統(tǒng)狀態(tài)的最優(yōu)估計值， 心=&_|//((//而_,扣'+從1，匕_1=~_心(^， 1+0._14-1是1吣慣性導(dǎo)航系統(tǒng)的噪聲矩陣，其 'Bl T 大小是由INS慣性導(dǎo)航元件的性能決定，私= +KkRkKl， Rk是系統(tǒng)測量噪聲的方差陣，其大小是由GPS接收機的性能決定；
[0056] J、將計算得到的值與無人機在INS慣性導(dǎo)航系統(tǒng)中的位置速度信息作差得到 最優(yōu)的導(dǎo)航參數(shù)；
[0057] K、重復(fù)步驟H-J，得到連續(xù)的無人機導(dǎo)航信息。
[0058]本發(fā)明的有益效果:該無人機導(dǎo)航系統(tǒng)包括INS慣性導(dǎo)航系統(tǒng)模塊、磁力計、GPS導(dǎo) 航系統(tǒng)模塊、控制中心，通過利用INS慣性導(dǎo)航和GPS導(dǎo)航組合導(dǎo)航的方法解決了單一的GPS 導(dǎo)航技術(shù)易受干擾和遮擋，短時定位精度不高，輸出頻率有限并且輸出不連續(xù)的缺點；同時 也解決了單一的INS慣性導(dǎo)航參數(shù)累計誤差越來越大，長時間定位精度發(fā)散的缺點，利用 GPS導(dǎo)航長時間具有高的定位精度的優(yōu)點來彌補INS慣性導(dǎo)航累計誤差隨時間的增加而發(fā) 散的缺點；利用INS慣性導(dǎo)航不受外界干擾、輸出的導(dǎo)航信息連續(xù)的特點彌補GPS易受干擾 和輸出頻率有限的缺點，并且為了解決由慣性導(dǎo)航計算出的偏航角無法找到真北，以及漂 移較大的情況，本系統(tǒng)利用磁力計計算出的偏航角來校正，獲得地理真北方向和穩(wěn)定的偏 航角，另外，此系統(tǒng)成本較低，系統(tǒng)穩(wěn)定性較強，能夠輸出比較滿意的導(dǎo)航定位信息，從而能 夠準(zhǔn)確反映無人機的姿態(tài)和位置信息。
【附圖說明】
[0059] 圖1為本發(fā)明所述無人機導(dǎo)航系統(tǒng)計算得出的煒度誤差值；
[0060] 圖2為本發(fā)明所述無人機導(dǎo)航系統(tǒng)計算得出的煒度誤差方差值；
[0061] 圖3為本發(fā)明所述無人機導(dǎo)航系統(tǒng)計算得出的經(jīng)度誤差值；
[0062] 圖4為本發(fā)明所述無人機導(dǎo)航系統(tǒng)計算得出的經(jīng)度誤差方差值；
[0063] 圖5為本發(fā)明所述無人機導(dǎo)航系統(tǒng)計算得出的高度誤差值；
[0064] 圖6為本發(fā)明所述無人機導(dǎo)航系統(tǒng)計算得出的高度誤差方差值；
[0065] 圖7為本發(fā)明所述無人機導(dǎo)航系統(tǒng)計算得出的東向速度誤差值；
[0066] 圖8為本發(fā)明所述無人機導(dǎo)航系統(tǒng)計算得出的東向速度誤差方差值；
[0067] 圖9為本發(fā)明所述無人機導(dǎo)航系統(tǒng)計算得出的北向速度誤差值；
[0068] 圖10為本發(fā)明所述無人機導(dǎo)航系統(tǒng)計算得出的北向速度誤差方差值；
[0069] 圖11為本發(fā)明所述無人機導(dǎo)航系統(tǒng)計算得出的天向速度誤差值；
[0070] 圖12為本發(fā)明所述無人機導(dǎo)航系統(tǒng)計算得出的天向速度誤差方差值。
【具體實施方式】
[0071] 本發(fā)明所述的無人機導(dǎo)航系統(tǒng)，包括INS慣性導(dǎo)航系統(tǒng)模塊、磁力計、GPS導(dǎo)航系統(tǒng) 模塊、控制中心，所述INS慣性導(dǎo)航系統(tǒng)模塊包括陀螺儀和加速度計，所述陀螺儀、加速度 計、磁力計、GPS導(dǎo)航系統(tǒng)模塊分別與控制中心信號連接，所述陀螺儀用于測量無人機的角 速度參數(shù)并將其測得的角速度參數(shù)傳遞給控制中心，所述磁力計用于測量無人機的偏航角 并將其測得的偏航角參數(shù)傳遞給控制中心，所述加速度計用于測量無人機的加速度并將其 測得的加速度參數(shù)傳遞給控制中心，所述GPS導(dǎo)航系統(tǒng)模塊用于測量無人機在GPS導(dǎo)航系統(tǒng) 中的位置速度信息，所述控制中心根據(jù)接收到的角速度參數(shù)、偏航角參數(shù)、加速度參數(shù)計算 出無人機在INS慣性導(dǎo)航系統(tǒng)中的位置速度信息，控制中心將無人機在GPS導(dǎo)航系統(tǒng)中的位 置速度信息與無人機在GPS導(dǎo)航系統(tǒng)中的位置速度信息進行Kalman濾波并進行導(dǎo)航誤差估 計，然后利用導(dǎo)航誤差估計修正無人機在INS慣性導(dǎo)航系統(tǒng)中的位置速度信息從而得到最 終的導(dǎo)航信息。該無人機導(dǎo)航系統(tǒng)包括INS慣性導(dǎo)航系統(tǒng)模塊、磁力計、GPS導(dǎo)航系統(tǒng)模塊、 控制中心，通過利用INS慣性導(dǎo)航和GPS導(dǎo)航組合導(dǎo)航的方法解決了單一的GPS導(dǎo)航技術(shù)易 受干擾和遮擋，短時定位精度不高，輸出頻率有限并且輸出不連續(xù)的缺點；同時也解決了單 一的INS慣性導(dǎo)航參數(shù)累計誤差越來越大，長時間定位精度發(fā)散的缺點，利用GPS導(dǎo)航長時 間具有高的定位精度的優(yōu)點來彌補INS慣性導(dǎo)航累計誤差隨時間的增加而發(fā)散的缺點；利 用INS慣性導(dǎo)航不受外界干擾、輸出的導(dǎo)航信息連續(xù)的特點彌補GPS易受干擾和輸出頻率有 限的缺點，并且為了解決由慣性導(dǎo)航計算出的偏航角無法找到真北，以及漂移較大的情況， 本系統(tǒng)利用磁力計計算出的偏航角來校正，獲得地理真北方向和穩(wěn)定的偏航角，另外，此系 統(tǒng)成本較低，能夠輸出比較滿意的導(dǎo)航定位信息，從而能夠準(zhǔn)確反映無人機的姿態(tài)和位置 fg息。
[0072]在上述實施方式中，為了保證信號傳遞的時效性與準(zhǔn)確性，所述陀螺儀、加速度 計、磁力計、分別與控制中心通過I2C總線連接。所述GPS導(dǎo)航系統(tǒng)模塊與控制中心通過 USART串口連接。
[0073] 所述控制中心采用如下方法計算得到最終的導(dǎo)航信息，其具體計算方法如下所 述：
[0074] A、將陀螺儀測得的無人機角速度參數(shù)代入四元數(shù)微分方程求解得 到四元數(shù)9〇^142^3;其中《4，《4，￡4為陀螺儀在無人機自身坐標(biāo)系下的測得的三個 軸的角速度信息；
[0075]所述四元數(shù)微分方程為： 4 0 -^nby ^nbz 4 A.似L 〇 -°tby %
[0076] . =〇-5 , b b % ^nby ^nbz 0 % _d Q」U_. *
[0077] B、將步驟A中求解的(^，(^，(^，陽代入下式求解得到姿態(tài)矩陣^；4， %+%+(h+^ 2(循
[0078] C=: 2(桃-姚)q^-q.+q.-q, l(q2qi+q0ql) -
[0079] 根據(jù)下述與方向余弦的關(guān)系式
[0080] cos (pees/ -sin rpcos/+ccs (psm 0siny -cos 沒 sin/ Cj = sir^9cos<9 cosf>cos^ sin/9 cospsinv-siru^sin^cos/ -sin^sin;/-cos^sin 6?cos/ cos?9cos;/
[0081] 計算得出無人機的INS慣性導(dǎo)航模塊姿態(tài)角0、y、P;
[0082] C、利用磁力計測得的偏航角識替換步驟B計算得到的偏航角
[0083] D、將加速度計測得的加速度參數(shù)護和步驟B中求解得到姿態(tài)矩陣Cf代入下述微分 方程中求解得到無人機在INS慣性導(dǎo)航坐標(biāo)系下的東、北、天三個方向上的速度信息VN VE VU，所述微分方程為：
[0084] vw =Clfb-( 2< + < ) x vw +
[0085] 其中，vn=[VN ve vu] '分別為INS慣性導(dǎo)航坐標(biāo)系中東、北、天方向上的速度，<丨為 地球自轉(zhuǎn)角速度，為無人機繞INS慣性導(dǎo)航坐標(biāo)系各軸向的轉(zhuǎn)動角速率，g n為重力加速 度；
[0086] E、將步驟D計算得出的VN VE vu分別代入下式求解得出無人機在INS慣性導(dǎo)航系統(tǒng) 中的位置信息，其中L為煒度，A為經(jīng)度，h為高度，
h = h (0) +/vudt，其中L (0)表示無人機初始 位置的煒度值A(chǔ)(0)表示無人機初始位置的經(jīng)度值，h(0)表示無人機距離地球表面的初始 高度。Rm表示地球子午圈上的曲率半徑，Rn表示煒度圈上的曲率半徑；
[0088] F、建立狀態(tài)方程尤（〇 = 6 (/)A(/) + G,,(/)%(/)和觀測方程ZUhmOXKO+V (thXKt)表示INS慣性導(dǎo)航系統(tǒng)在t時刻的誤差狀態(tài)，它是一個15維的向量，如下所示： 不(和% 喪略4:說說激今今4 ▽2]，~，^，^為1奶慣性 導(dǎo)航系統(tǒng)沿東、北、天方向上的速度誤差；4>x，4>y，為無人機的姿態(tài)角誤差;SL，SA，Sh分 別代表無人機所在煒度、經(jīng)度和高度誤差；ex，ey，ez分別代表陀螺儀的隨機漂移；V.pV,.，V z 分別為加速度計的隨機漂移，其中巧(0= ，是一個15X15的矩陣;其中FN L 0 ~(，)L (t)對應(yīng)于"，^，^，(}^，(^，傘2，乩，6入，恥這9個參數(shù)的1吧慣性導(dǎo)航系統(tǒng)矩陣，其非零 元素如下：

[0102] 卩5(1:)為5~，5%，"，（^，<^，傘2,81，5入，5]1這9個參數(shù)與陀螺儀及加速度計漂移 _03x3 c_ 之間的變換矩陣，其維數(shù)是96,巧(/)= q 03x3 ; _〇3x3 〇3x3_
[0103] FM(t)為￡^￡^￡2，'^，％，'?2與陀螺儀及加速度計漂移對應(yīng)的1奶慣性導(dǎo)航系統(tǒng)矩 陣，是一個維數(shù)為66的對角線矩陣，表示如下：
[0104] FM(t) = diag[-l/Tgx -1/Tgy -1/Tgz -1/Tax -1/Tay -1/Taz];其中，Tgx表示陀螺儀x 軸的誤差模型的時間常數(shù)，T gy表示陀螺儀y軸的誤差模型的時間常數(shù)，Tgz表示陀螺儀z軸的 誤差模型的時間常數(shù)，T ax表示加速度計x軸誤差模型的時間常數(shù)，Tay表示加速度計y軸誤差 模型的時間常數(shù)，T az表示加速度計z軸誤差模型的時間常數(shù)；
[0105] Gi(t) = diag[l 1 1......1 I]i5i5;
[0106] Wi(t)是一個15維的向量，如下所示：
[0107] Wi(t) = [ai a2 a3 a4 as a6 a7 as ag aio an ai2 ai3 ai4 ai5]，
[0108] ai a2 a3 a4 a5 a6 a7 as ag ai。aii ai2 ai3 ai4 ai5表不系統(tǒng)過程噪聲序
[0109] 列；
[0110] z(t)為無人機在INS慣性導(dǎo)航系統(tǒng)中的位置速度信息與無人機在GPS導(dǎo)航系統(tǒng)中 的位置速度信息的差值，是一個6維向量，
[0111] Z(t) = [5vx+Nvx 5vy+NVy 5vz+Nvz (RM+h)5L+Ny (RM+h)cosL5入+NX 5h+Nh]T，其中，Nvx 表示GPS導(dǎo)航系統(tǒng)在x方向上的速度誤差，Nvy表示GPS導(dǎo)航系統(tǒng)在y方向上的速度誤差，Nvz表 示GPS導(dǎo)航系統(tǒng)在z方向上的速度誤差，N x表示GPS導(dǎo)航系統(tǒng)在x方向上的位置誤差，Ny表示 GPS導(dǎo)航系統(tǒng)在y方向上的位置誤差，Nh表示GPS導(dǎo)航系統(tǒng)在z方向上的位置誤差；
[0112]沖)=U,(樸作)=k(,)J，其中
[0113] //, (/) = |^/Mg[l 1 1 ]!〇,,,：}
[0114] (/^； +/?) +/?)cosL l]：〇w,}
[0115] Vv(t) = [Nvx Nvy Nvz]T
[0116] Vp(t) = [Nx Ny Nz]t
[0117] G、將上述得到的連續(xù)狀態(tài)方程< = F, (/)H) + G,, (/)%(/)離散化后得到Xk = ① k, k-lXk-l+Wk-1，其中
[0118] 將上述得到的連續(xù)觀測方程Z(t)=H(t)XI(t)+V(t)離散化后得到Zk = HkXk+Vk; [0119]其中I是單位矩陣，F(xiàn)是INS慣性導(dǎo)航系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣，A t是離散化后INS慣性 導(dǎo)航系統(tǒng)的采樣時間；
[0120] H、將無人機在INS慣性導(dǎo)航系統(tǒng)中的位置速度信息與無人機在GPS導(dǎo)航系統(tǒng)中的 位置速度信息作差得到Z(t)在k時刻的觀測信息z;
[0121] I、計算k時刻INS慣性導(dǎo)航系統(tǒng)狀態(tài)的最優(yōu)估計值鳥t&卜-耳之, 其中，H，尤-^為在k-1時刻，INS慣性導(dǎo)航系統(tǒng)狀態(tài)的最優(yōu)估計值， A ,祀(巧I此+4)'巧卜,-尤死,-,:是燃慣性導(dǎo)航系統(tǒng)的噪聲矩陣， 其大小是由INS慣性導(dǎo)航元件的性能決定，巧，t-[/--人A人Y， Rk是系統(tǒng)測量噪聲的方差陣，其大小是由GPS接收機的性能決定；
[0122] J、將計算得到的值與無人機在INS慣性導(dǎo)航系統(tǒng)中的位置速度信息作差得到 最優(yōu)的導(dǎo)航參數(shù)；
[0123] K、重復(fù)步驟H-J，得到連續(xù)的無人機導(dǎo)航信息。
[0124] 利用上述方法計算得出的導(dǎo)航信息，能夠準(zhǔn)確反映無人機的姿態(tài)和位置信息，可 以實現(xiàn)較好的定位導(dǎo)航精度，圖1為本發(fā)明所述無人機導(dǎo)航系統(tǒng)計算得出的煒度誤差值;圖 2為本發(fā)明所述無人機導(dǎo)航系統(tǒng)計算得出的煒度誤差方差值；圖3為本發(fā)明所述無人機導(dǎo)航 系統(tǒng)計算得出的經(jīng)度誤差值；圖4為本發(fā)明所述無人機導(dǎo)航系統(tǒng)計算得出的經(jīng)度誤差方差 值；圖5為本發(fā)明所述無人機導(dǎo)航系統(tǒng)計算得出的高度誤差值；圖6為本發(fā)明所述無人機導(dǎo) 航系統(tǒng)計算得出的高度誤差方差值；圖7為本發(fā)明所述無人機導(dǎo)航系統(tǒng)計算得出的東向速 度誤差值；圖8為本發(fā)明所述無人機導(dǎo)航系統(tǒng)計算得出的東向速度誤差方差值；圖9為本發(fā) 明所述無人機導(dǎo)航系統(tǒng)計算得出的北向速度誤差值;圖10為本發(fā)明所述無人機導(dǎo)航系統(tǒng)計 算得出的北向速度誤差方差值；圖11為本發(fā)明所述無人機導(dǎo)航系統(tǒng)計算得出的天向速度誤 差值;圖12為本發(fā)明所述無人機導(dǎo)航系統(tǒng)計算得出的天向速度誤差方差值；
[0125] 從上述測試結(jié)果圖可以看出，本發(fā)明所述無人機導(dǎo)航系統(tǒng)得出的經(jīng)度、煒度、高度 的誤差方差均能快速收斂至比較小的數(shù)值;對位置、速度等導(dǎo)航信息也能實現(xiàn)濾平滑作用， 不會產(chǎn)生大的跳變，系統(tǒng)的穩(wěn)定性較強。
【主權(quán)項】
1. 一種無人機導(dǎo)航系統(tǒng)，其特征在于:包括INS慣性導(dǎo)航系統(tǒng)模塊、磁力計、GPS導(dǎo)航系 統(tǒng)模塊、控制中心，所述INS慣性導(dǎo)航系統(tǒng)模塊包括陀螺儀和加速度計，所述陀螺儀、加速度 計、磁力計、GPS導(dǎo)航系統(tǒng)模塊分別與控制中心信號連接，所述陀螺儀用于測量無人機的角 速度參數(shù)并將其測得的角速度參數(shù)傳遞給控制中心，所述磁力計用于測量無人機的偏航角 并將其測得的偏航角參數(shù)傳遞給控制中心，所述加速度計用于測量無人機的加速度并將其 測得的加速度參數(shù)傳遞給控制中心，所述GPS導(dǎo)航系統(tǒng)模塊用于測量無人機在GPS導(dǎo)航系統(tǒng) 中的位置速度信息，所述控制中心根據(jù)接收到的角速度參數(shù)、偏航角參數(shù)、加速度參數(shù)計算 出無人機在INS慣性導(dǎo)航系統(tǒng)中的位置速度信息，控制中心將無人機在GPS導(dǎo)航系統(tǒng)中的位 置速度信息與無人機在GPS導(dǎo)航系統(tǒng)中的位置速度信息進行Kalman濾波并進行導(dǎo)航誤差估 計，然后利用導(dǎo)航誤差估計修正無人機在INS慣性導(dǎo)航系統(tǒng)中的位置速度信息從而得到最 終的導(dǎo)航信息。2. 如權(quán)利要求1所述的無人機導(dǎo)航系統(tǒng)，其特征在于:所述陀螺儀、加速度計、磁力計、 分別與控制中心通過I2C總線連接。3. 如權(quán)利要求2所述的無人機導(dǎo)航系統(tǒng)，其特征在于:所述GPS導(dǎo)航系統(tǒng)模塊與控制中 心通過USART串口連接。4. 如權(quán)利要求1所述的無人機導(dǎo)航系統(tǒng)，其特征在于:所述控制中心采用如下方法計算 得到最終的導(dǎo)航信息，其具體計算方法如下所述： A、 將陀螺儀測得的無人機角速度參數(shù)4,，代入四元數(shù)微分方程求解得到四 元數(shù)9〇4142^3;其中<、.，《1.，》^為陀螺儀在無人機自身坐標(biāo)系下的測得的三個軸的 角速度信息； 所述四元數(shù)微分方程為：B、 將步驟A中求解的qo，qi，q2，q3代入下式求解得到姿態(tài)矩陣0，根據(jù)下述Cf與方向余弦的關(guān)系式計算得出無人機的INS慣性導(dǎo)航模塊姿態(tài)角θ、γ C、 利用磁力計測得的偏航角免替換步驟B計算得到的偏航角 D、 將加速度計測得的加速度參數(shù)護和步驟B中求解得到姿態(tài)矩陣 < 代入下述微分方程 中求解得到無人機在INS慣性導(dǎo)航坐標(biāo)系下的東、北、天三個方向上的速度信息VN VE vu，所 述微分方程為：其中，Vn=[VN VE VU]'分別為INS慣性導(dǎo)航坐標(biāo)系中東、北、天方向上的速度，科〗為地球 自轉(zhuǎn)角速度，< 為無人機繞INS慣性導(dǎo)航坐標(biāo)系各軸向的轉(zhuǎn)動角速率，gns重力加速度； E、 將步驟D計算得出的VN VE vu分別代入下式求解得出無人機在INS慣性導(dǎo)航系統(tǒng)中的 位置信息，其中L為煒度，λ為經(jīng)度，h為高度，h = h (0) + Jvudt，其中L (0)表示無人機初始位置 的煒度值，λ(0)表示無人機初始位置的經(jīng)度值，h(0)表示無人機距離地球表面的初始高度。 Rm表不地球子午圈上的曲率半徑，Rn表不韓度圈上的曲率半徑； F、 建立狀態(tài)方程矣(i) = 6 (f)JT, (V)+:(? (4% (?)和觀測方程Z(t) =H(t )Xi(t) +V(t)， XMt)表示INS慣性導(dǎo)航系統(tǒng)在t時刻的誤差狀態(tài)，它是一個15維的向量，如下所示： 不⑴二A dV:我 $ 4 況.況漁 < A 6 ▽i].，.5vx，3v y，5vz*INS 慣性導(dǎo)航系統(tǒng)沿東、北、天方向上的速度誤差；Φχ，Φγ，Φζ為無人機的姿態(tài)角誤差;δ?，δλ，δ h分別代表無人機所在煒度、經(jīng)度和高度誤差；ex，ey，％分別代表陀螺儀的隨機漂移； 別為加速度計的隨機漂移，，是一個15X15的矩 陣；其中FN(t)對應(yīng)于δνχ，δνγ，δνζ，φχ, φγ, φζ，δΙ^，δλ，δ]ι這9個參數(shù)的INS慣性導(dǎo)航系統(tǒng)矩 陣，其非零元素如下：Fs(t)為3"，5%，5￥2，(^，(^，(|)2況，3入，5]1這9個參數(shù)與陀螺儀及加速度計漂移之間的 變換矩陣，其維數(shù)是9X6FM( t)為εχ，ey，εz，▽,:，▽,.，▽。與陀螺儀及加速度計漂移對應(yīng)的INS慣性導(dǎo)航系統(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慣性導(dǎo)航系統(tǒng)中的位置速度信息與無人機在GPS導(dǎo)航系統(tǒng)中的位 置速度信息的差值，是一個6維向量， Ζ(?) = [δνχ+Ννχ δνγ+Ννγ δνζ+Ννζ (RM+h)3L+Ny (RM+h)cosL3A+Nx 3h+Nh]T，其中，Ννχ表不 GPS導(dǎo)航系統(tǒng)在x方向上的速度誤差，Nvy表示GPS導(dǎo)航系統(tǒng)在y方向上的速度誤差，Nvz表示 GPS導(dǎo)航系統(tǒng)在ζ方向上的速度誤差，Νχ表示GPS導(dǎo)航系統(tǒng)在X方向上的位置誤差，Ny表示GPS 導(dǎo)航系統(tǒng)在y方向上的位置誤差，Nh表示GPS導(dǎo)航系統(tǒng)在z方向上的位置誤差；Vv(t) = [Nvx Nvy Nvz]T Vp(t) = [Nx Ny Nz]T G、 將上述得到的連續(xù)狀態(tài)方秤尤(/) = 6 (/) A (/) + (7,⑴it; (/)離散化后得到Xk = ?k,k-lXk-l+Wk-l:將上述得到的連續(xù)觀測方程z (t) =H( t )X: (t) +V( t)離散化后得到Zk=HkXk+Vk; 其中I是單位矩陣，F(xiàn)是INS慣性導(dǎo)航系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣，△ t是離散化后INS慣性導(dǎo)航 系統(tǒng)的采樣時間； H、 將無人機在INS慣性導(dǎo)航系統(tǒng)中的位置速度信息與無人機在GPS導(dǎo)航系統(tǒng)中的位置 速度息作差得到Z( t)在k時刻的觀測fg息z; I、 計算k時刻INS慣性導(dǎo)航系統(tǒng)狀態(tài)的最優(yōu)估計值爲(wèi)_ +[4Z-氧尤-iA--小 其中，之,^，之H為在k-1時刻，IN S慣性導(dǎo)航系統(tǒng)狀態(tài)的最優(yōu)估計值， 心=山，"；（";VX + 鳥)' = φ?--Α,φΙη +a-丨辦―!是1 咐 矩陣，其大小是由INS慣性導(dǎo)航元件的性能決定，，pk，」KkHAHK kH；f +KkRkKl， Rk是系統(tǒng)測量噪聲的方差陣，其大小是由GPS接收機的性能決定； J、 將計算得到的名^值與無人機在INS慣性導(dǎo)航系統(tǒng)中的位置速度信息作差得到最優(yōu) 的導(dǎo)航參數(shù)； K、 重復(fù)步驟H-J，得到連續(xù)的無人機導(dǎo)航信息。
【文檔編號】G01S19/49GK105928515SQ201610242663
【公開日】2016年9月7日
【申請日】2016年4月19日
【發(fā)明人】張瑜, 李詩揚
【申請人】成都翼比特自動化設(shè)備有限公司