本申請無人機領域，尤其涉及一種用衛(wèi)星導航和本機慣性測量手段導航的方法和裝置。

背景技術：

每種單一導航系統(tǒng)都有各自的獨特性能和局限性。小型光纖或微機械慣性導航裝置有成本低、體積小、重量輕的優(yōu)勢，可以自主的為載體提供姿態(tài)、位置、速度信息；缺點是導航誤差隨時間增長快，特別是mems慣性器件漂移、零偏重復性等指標差，難于通過預先標校達到提高慣性導航精度的目標。

北斗衛(wèi)星導航系統(tǒng)是我國自行研制的衛(wèi)星導航系統(tǒng)，它可以在gps、glonass等國外導航系統(tǒng)關閉的情況下為我國用戶提供導航、定位、授時服務。目前北斗系統(tǒng)已經(jīng)形成了5geo+5igso+5meo的星座框架，可以實現(xiàn)亞太地區(qū)的定位、導航和授時，定位精度10m、測速精度0.2m/s、授時精度50ns。衛(wèi)星導航有全天候、導航精度高的優(yōu)勢，但受使用環(huán)境的制約，在干擾和遮擋條件下可用性差，并且不能提供載體姿態(tài)信息用于載體飛行控制。

因此，需要設計一種組合導航系統(tǒng)實現(xiàn)方法和系統(tǒng)，將微型慣性測量單元(mimu)與北斗衛(wèi)星接收機組合構成一種可以提供完整的載體運動參數(shù)、導航準確度更高的系統(tǒng)。

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

本發(fā)明公開一種導航方法和裝置，解決未標定條件下利用微慣性測量單元(mimu)導航的問題。

本申請實施例提供一種導航方法，用于無人機，所述無人機包含微慣性測量單元、衛(wèi)星接收機，包括以下步驟：

在初始時刻，用所述衛(wèi)星接收機采集無人機位置和速度，分別作為位置初始值和速度初始值；

從所述初始時刻到校準時刻，用所述微慣性測量單元按測量周期采集無人機加速度瞬時值、陀螺瞬時值；根據(jù)加速度瞬時值、陀螺瞬時值計算姿態(tài)參數(shù)瞬時值；所述校準時刻和所述初始時刻之間為濾波周期，包含多個所述測量周期；

根據(jù)所述位置初始值、速度初始值、姿態(tài)參數(shù)瞬時值，用捷聯(lián)慣性導航算法得出無人機在每一個測量周期的位置計算值、速度計算值和姿態(tài)參數(shù)計算值；

在校準時刻，用所述衛(wèi)星接收機采集無人機位置和速度，分別作為位置校準值和速度校準值；

根據(jù)校準時刻的位置計算值、速度計算值、位置校準值、速度校準值，用離散卡爾曼濾波方法計算姿態(tài)參數(shù)誤差、速度誤差、位置誤差、陀螺誤差、加速度誤差；所述離散卡爾曼濾波方法使用的誤差變量包含：俯仰角誤差、橫滾角誤差、航向角誤差、東向速度誤差、北向速度誤差、精度誤差、緯度誤差、x、y、z方向陀螺誤差、x、y方向加速度誤差共12個參數(shù)；其中，x、y、z構成直角坐標系，x方向指向無人機右側方，y方向沿無人機縱軸方向指向前。

進一步地，以所述校準時刻作為新的初始時刻，循環(huán)執(zhí)行所述步驟。

本申請的最佳實施例還包含以下步驟：用所述加速度誤差對下一濾波周期的加速度瞬時值進行修正；用所述陀螺誤差對下一濾波周期的陀螺瞬時值進行修正。

本申請實施例還提供一種導航裝置，用于無人機，包含微慣性測量單元、衛(wèi)星接收機、處理器；所述微慣性測量單元，用于從初始時刻開始，按照測量周期產(chǎn)生陀螺瞬時值、加速度瞬時值；所述衛(wèi)星接收機，用于接收衛(wèi)星定位信息，在初始時刻輸出位置初始值和速度初始值；從初始時刻起按照濾波周期輸出位置校準值和速度校準值；所述處理器，用于根據(jù)所述位置初始值、速度初始值、陀螺瞬時值、加速度瞬時值，用捷聯(lián)慣性導航算法得出無人機在每一個測量周期的位置計算值、速度計算值和姿態(tài)參數(shù)計算值；根據(jù)校準時刻的位置計算值、速度計算值、位置校準值、速度校準值，用離散卡爾曼濾波方法計算姿態(tài)參數(shù)誤差、速度誤差、位置誤差、陀螺誤差、加速度誤差；所述離散卡爾曼濾波方法使用的誤差變量包含：俯仰角誤差、橫滾角誤差、航向角誤差、東向速度誤差、北向速度誤差、精度誤差、緯度誤差、x、y、z方向陀螺誤差、x、y方向加速度誤差共12個參數(shù)；其中，x、y、z構成直角坐標系，x方向指向無人機右側方，y方向沿無人機縱軸方向指向前。

進一步地，所述處理器進一步包含導航參數(shù)初始化單元、捷聯(lián)慣性導航計算單元、誤差估計濾波器；所述導航參數(shù)初始化單元，用于接收加速度瞬時值、陀螺瞬時值、根據(jù)加速度瞬時值、陀螺瞬時值計算姿態(tài)參數(shù)瞬時值；所述捷聯(lián)慣性導航導航單元，用于接收所述位置初始值、速度初始值、姿態(tài)參數(shù)瞬時值，用捷聯(lián)慣性導航算法得出無人機在每一個測量周期的位置計算值、速度計算值和姿態(tài)參數(shù)計算值；所述誤差估計濾波器，用離散卡爾曼濾波方法計算姿態(tài)參數(shù)誤差、速度誤差、位置誤差、陀螺誤差、加速度誤差。

優(yōu)選地，所述導航裝置進一步包含輸出校正單元；所述輸出校正單元，用所述姿態(tài)參數(shù)誤差對所述姿態(tài)參數(shù)計算值進行修正。

在本發(fā)明任意一項實施例中，優(yōu)選地，所述導航裝置進一步包含輸入校正單元，用所述加速度誤差對下一濾波周期的加速度瞬時值進行修正、用所述陀螺誤差對下一濾波周期的陀螺瞬時值進行修正，所述導航參數(shù)初始化單元的輸入。

本申請實施例采用的上述至少一個技術方案能夠達到以下有益效果：

對比其他組合導航系統(tǒng)實現(xiàn)方法，本設計具有完備的數(shù)據(jù)處理過程，采用本發(fā)明可以很好地實現(xiàn)基于微慣性測量單元(mimu)、北斗衛(wèi)星接收機的組合導航。

本發(fā)明微慣性器件誤差不需要預先標定，一方面周期性的利用北斗衛(wèi)星接收機輸出的位置、速度以及速度航向初始化慣性導航計算參數(shù)，另一方面誤差估計濾波器實時估計慣性導航誤差并進行修正，最終可以得到精度可用的組合導航結果。

本發(fā)明采用小型光纖或微機械慣性測量單元與傳統(tǒng)的激光或光纖慣性單元相比成本更低、體積更小、重量更輕；本發(fā)明采用北斗衛(wèi)星接收機可使組合導航系統(tǒng)在其他衛(wèi)星導航系統(tǒng)(gps、glonass等)關閉的情況下仍正常工作。

附圖說明

此處所說明的附圖用來提供對本申請的進一步理解，構成本申請的一部分，本申請的示意性實施例及其說明用于解釋本申請，并不構成對本申請的不當限定。在附圖中：

圖1為本發(fā)明導航方法的實施例流程圖；

圖2為本發(fā)明導航方法包含循環(huán)校準的實施例流程圖；

圖3為用離散卡爾曼濾波方法進行誤差估計的實施例流程圖；

圖4為本發(fā)明導航裝置的實施例框圖；

圖5為本發(fā)明導航裝置進一步優(yōu)化的實施例框圖；

圖6為本發(fā)明導航裝置進行輸入、輸出校準的實施例示意圖。

具體實施方式

為使本申請的目的、技術方案和優(yōu)點更加清楚，下面將結合本申請具體實施例及相應的附圖對本申請技術方案進行清楚、完整地描述。顯然，所描述的實施例僅是本申請一部分實施例，而不是全部的實施例?；诒旧暾堉械膶嵤├?，本領域普通技術人員在沒有做出創(chuàng)造性勞動前提下所獲得的所有其他實施例，都屬于本申請保護的范圍。

在本申請全部實施例中，x、y、z構成直角坐標系，x方向指向無人機右側方，y方向沿無人機縱軸方向指向前。

以下結合附圖，詳細說明本申請各實施例提供的技術方案。

圖1為本發(fā)明導航方法的實施例流程圖。具體包含以下步驟：

步驟11、在初始時刻，用所述衛(wèi)星接收機采集無人機位置和速度，分別作為位置初始值和速度初始值；

步驟12、從所述初始時刻到校準時刻，用所述微慣性測量單元按測量周期采集無人機加速度瞬時值、陀螺瞬時值；根據(jù)加速度瞬時值、陀螺瞬時值計算姿態(tài)參數(shù)瞬時值；所述校準時刻和所述初始時刻之間的時長為一個濾波周期，包含多個所述測量周期；

其中，在第一個初始時刻，所述姿態(tài)參數(shù)瞬時值的計算方法為：

pitch0＝arcsin(ay/g0)公式1.1

roll0＝-arcsin(ax/g0)公式1.2

式中pitch0、roll0為初始俯仰角、橫滾角；ax、ay為加速度計x、y方向輸出值；g0為重力加速度。

例如，以測量周期t1(20ms)采集微慣性測量單元(mimu)的陀螺、加速度計信息。

需要說明的是，慣性導航輸出載體姿態(tài)用于載體姿態(tài)控制，要求實時性高，因此設計慣性導航為短周期t1(為20ms)計算；隨計算時間增長慣性導航輸出結果(姿態(tài)、位置、速度)誤差增大，需要周期性利用北斗衛(wèi)星接收機的信息輔助以提高精度，本方法實施例采用濾波周期t2(為1s)。

在步驟12中，例如可以用北斗衛(wèi)星接收機的經(jīng)度、緯度、高度、地速航向和所述初始俯仰角、初始橫滾角，計算無人機姿態(tài)、速度、位置初值；

步驟13、根據(jù)所述位置初始值、速度初始值、姿態(tài)參數(shù)瞬時值，用捷聯(lián)慣性導航算法得出無人機在每一個測量周期的位置計算值、速度計算值和姿態(tài)參數(shù)計算值；

位置參數(shù)包含經(jīng)度、緯度、高度，速度參數(shù)包含東向速度、北向速度、天向速度，姿態(tài)參數(shù)包含航向、俯仰角、橫滾角；

步驟14、按所述濾波周期，在校準時刻，用所述衛(wèi)星接收機采集無人機位置和速度，分別作為位置校準值和速度校準值；

例如，以濾波周期t2(1s)采集北斗衛(wèi)星接收機的信息(經(jīng)度、緯度、高度、東向速度、北向速度、天向速度、地速航向)。

步驟15、根據(jù)校準時刻的位置計算值、速度計算值、位置校準值、速度校準值，用離散卡爾曼濾波方法計算姿態(tài)參數(shù)誤差、速度誤差、位置誤差、陀螺誤差、加速度誤差，所述離散卡爾曼濾波方法使用的誤差變量包含：俯仰角誤差、橫滾角誤差、航向角誤差、東向速度誤差、北向速度誤差、精度誤差、緯度誤差、x、y、z方向陀螺誤差、x、y方向加速度誤差共12個參數(shù)。

步驟16、使用12個參數(shù)的誤差變量對所述位置計算值、速度計算值和姿態(tài)參數(shù)計算值進行校準；

例如當衛(wèi)星接收機參與定位衛(wèi)星數(shù)較多，幾何精度因子小于閾值時，組合導航天向速度、高度直接輸出衛(wèi)星接收機天向速度、高度；否則組合導航輸出微慣性測量單元(mimu)導航天向速度、高度。

本申請各項實施例中，所述衛(wèi)星接收機接收導航信號，具體地，應用于北斗衛(wèi)星、gps衛(wèi)星或其他導航用衛(wèi)星。當應用北斗衛(wèi)星時，所述衛(wèi)星接收機為北斗文星接收機。

圖2為本發(fā)明導航方法包含循環(huán)校準的實施例流程圖。

本申請的最佳實施例還包含以下步驟：

步驟17、用所述加速度誤差對下一濾波周期的加速度瞬時值進行修正；用所述陀螺誤差對下一濾波周期的陀螺瞬時值進行修正。每個卡爾曼濾波周期(t2)得到的陀螺、加速度計誤差估計值對下一周期陀螺、加速度計原始數(shù)據(jù)修正。

進一步地，失鎖重捕時需要重新初始化，重復步驟11～17。

當所述衛(wèi)星接收機連續(xù)跟蹤衛(wèi)星定位信號時，進一步地，以所述校準時刻作為新的初始時刻，循環(huán)執(zhí)行所述步驟13～17。

經(jīng)過上述過程，微慣性器件誤差無需預先標定，周期性的利用北斗衛(wèi)星接收機輸出的位置、速度以及速度航向初始化慣性導航計算參數(shù)，以及設計誤差估計濾波器實時估計慣性導航誤差并進行修正，可以最終得到精度可用的導航結果。

圖3為用離散卡爾曼濾波方法進行誤差估計的實施例流程圖。采用誤差估計濾波器進行慣性導航誤差估計，考慮微慣性測量單元(mimu)特點和運算時間，濾波器選擇12參數(shù)的誤差狀態(tài)變量，具體的，誤差狀態(tài)變量x的元素由以下物理量組成：俯仰角誤差δφe、橫滾角誤差δφn、航向角誤差δφu；東向速度誤差δve、北向速度誤差δvn；經(jīng)度誤差δλ、緯度誤差δl；x、y、z方向陀螺誤差εbx，εby，εbz；x、y方向加速度計誤差即

步驟21、根據(jù)慣導系統(tǒng)誤差、基本導航參數(shù)之間的關系建立所述12參數(shù)誤差狀態(tài)模型。具體地，誤差狀態(tài)模型為

其中f為狀態(tài)轉移矩陣，維數(shù)為12×12，非零項為：

f(0,1)＝ωiesinl；f(0,2)＝-ωiecosl；f(1,0)＝-ωiesinl；f(1,5)＝-ωiesinl；f(2,0)＝ωiecosl；f(3,1)＝-g；f(4,0)＝g；

式中，ωie為地球自轉角速度，l為緯度，r為地球半徑，為姿態(tài)矩陣(維數(shù)為3×3)，ve、vn為東向速度、北向速度，g為重力加速度，τgyro為陀螺相關時間，τacc為加速度計相關時間，在本發(fā)明技術方案中，陀螺、加速度計相關時間取值5s。

慣導系統(tǒng)誤差來源有很多種，如位置誤差、速度誤差、初始姿態(tài)誤差、陀螺偏差、加速度計偏差、刻度系數(shù)誤差等，各項誤差又可分為固定偏差和噪聲項。慣導系統(tǒng)誤差、基本導航參數(shù)之間存在近似的對應關系，固定采樣間隔內(nèi)慣導系統(tǒng)誤差的變化可以表示為系數(shù)乘以誤差項加上噪聲項的形式。表達式中每一個誤差變化量對應的誤差項系數(shù)是確定的，可以由慣性導航誤差傳遞關系得出。根據(jù)選擇的誤差狀態(tài)變量不同，誤差狀態(tài)模型寫成的表達式形式不同。具體的可以建立公式2形式的誤差狀態(tài)模型。

誤差狀態(tài)模型中，g為噪聲矩陣，維數(shù)為12×1，滿足

其中，wgyro＝[wgxwgywgz]^t，wacc＝[waxwaywaz]^t，wgx、wgy、wgz為陀螺儀測量噪聲，wax、way、waz為加速度計測量噪聲。

誤差估計濾波器按卡爾曼濾波方法計算，計算周期為所述濾波周期t2(例如1s)，每個濾波周期計算得到狀態(tài)一步估計值：

式中，分別為俯仰角誤差、橫滾角誤差、航向角誤差估計值；分別為東向速度誤差估計值、北向速度誤差估計值；分別為經(jīng)度、緯度誤差估計值；分別為x、y、z方向陀螺誤差估計值；分別為x、y方向加速度計誤差估計值。

步驟22、用狀態(tài)一步估計值修正誤差，更新姿態(tài)矩陣。姿態(tài)修正模型為

式中，為當前周期姿態(tài)矩陣，為修正后姿態(tài)矩陣，用于下一周期s5計算。

當前周期姿態(tài)矩陣經(jīng)姿態(tài)偏差修正可以得到修正后的姿態(tài)矩陣用于后續(xù)捷聯(lián)慣導計算。姿態(tài)修正按照航向角誤差、俯仰角誤差、橫滾角誤差的順序進行。按照上述角度旋轉順序可以得到修正后姿態(tài)矩陣。具體的修正后姿態(tài)矩陣與當前周期姿態(tài)矩陣、系數(shù)矩陣關系表達式如圈出部分所示。

步驟23、根據(jù)速度、位置修正模型，使用狀態(tài)一步估計值對微慣性測量單元輸出的東向速度、北向速度、緯度、經(jīng)度進行修正。

建立誤差估計濾波器觀測模型如下式：

公式6中建立的誤差觀測模型采用速度位置差作為觀測量，微慣性測量單元(mimu)輸出的速度、位置與北斗衛(wèi)星接收機輸出的位置速度差值可以表示為慣導誤差加測量噪聲的形式。

其中ve_ins、vn_ins、lins、λins為根據(jù)微慣性測量單元輸出得到速度計算值和位置計算值的東向速度、北向速度、緯度、經(jīng)度ve_bd、vn_bd、lbd、λbd為北斗衛(wèi)星接收機輸出的東向速度、北向速度、緯度、經(jīng)度；wve、wvn為速度測量噪聲；wλ、wl為位置測量噪聲。wve、wvn為北斗接收機速度測量噪聲，由接收機速度精度參數(shù)決定，wλ、wl為北斗接收機位置測量噪聲，由接收機位置精度參數(shù)決定。

從公式6中看只能直接求出四個慣導誤差參數(shù)，但是從公式2的狀態(tài)模型f矩陣關系可以看出這四個誤差參數(shù)的變化與其余慣導誤差參數(shù)有關。通過卡爾曼濾波可以解出全部誤差參數(shù)。包括：姿態(tài)參數(shù)(航向、俯仰角、橫滾角)、位置參數(shù)(經(jīng)度λins、緯度lins、高度)、速度參數(shù)(東向速度ve_ins、北向速度vn_ins、天向速度)。

利用狀態(tài)一步估計值，使用速度、位置修正模型

式中ve、vn、l、λ為修正后的東向速度、北向速度、緯度、經(jīng)度導航結果。

圖4為本發(fā)明導航裝置的實施例框圖。本申請實施例還提供一種導航裝置，用于無人機，包含微慣性測量單元100、衛(wèi)星接收機200、處理器300；所述微慣性測量單元，用于從初始時刻開始，按照測量周期產(chǎn)生陀螺瞬時值、加速度瞬時值；所述衛(wèi)星接收機，用于接收衛(wèi)星定位信息，在初始時刻輸出位置初始值和速度初始值；從初始時刻起按照濾波周期輸出位置校準值和速度校準值；所述處理器，用于根據(jù)所述位置初始值、速度初始值、陀螺瞬時值、加速度瞬時值，用捷聯(lián)慣性導航算法得出無人機在每一個測量周期的位置計算值、速度計算值和姿態(tài)參數(shù)計算值；根據(jù)校準時刻的位置計算值、速度計算值、位置校準值、速度校準值，用離散卡爾曼濾波方法計算姿態(tài)參數(shù)誤差、速度誤差、位置誤差、陀螺誤差、加速度誤差；所述離散卡爾曼濾波方法使用的誤差變量包含：俯仰角誤差、橫滾角誤差、航向角誤差、東向速度誤差、北向速度誤差、精度誤差、緯度誤差、x、y、z方向陀螺誤差、x、y方向加速度誤差共12個參數(shù)；其中，x、y、z構成直角坐標系，x方向指向無人機右側方，y方向沿無人機縱軸方向指向前。

圖5為本發(fā)明導航裝置進一步優(yōu)化的實施例框圖。進一步地，所述處理器300進一步包含導航參數(shù)初始化單元301、捷聯(lián)慣性導航計算單元302、誤差估計濾波器303；所述導航參數(shù)初始化單元，用于接收加速度瞬時值、陀螺瞬時值、根據(jù)加速度瞬時值、陀螺瞬時值計算姿態(tài)參數(shù)瞬時值(如公式1.1～1.2)；所述捷聯(lián)慣性導航計算單元，用于接收所述位置初始值、速度初始值、姿態(tài)參數(shù)瞬時值，用捷聯(lián)慣性導航算法得出無人機在每一個測量周期的位置計算值、速度計算值和姿態(tài)參數(shù)計算值；所述誤差估計濾波器，用離散卡爾曼濾波方法計算姿態(tài)參數(shù)誤差、速度誤差、位置誤差、陀螺誤差、加速度誤差共12個變量(如公式2～5)。

圖6為本發(fā)明導航裝置進行輸入、輸出校準的實施例示意圖。所述導航裝置進一步包含輸出校正單元400；所述輸出校正單元，用所述姿態(tài)參數(shù)誤差對所述姿態(tài)參數(shù)計算值進行修正，得到修正后的位置、速度、姿態(tài)參數(shù)。在本發(fā)明任意一項實施例中，優(yōu)選地，所述導航裝置進一步包含輸入校正單元500，用所述加速度誤差對下一濾波周期的加速度瞬時值進行修正、用所述陀螺誤差對下一濾波周期的陀螺瞬時值進行修正，所述導航參數(shù)初始化單元的輸入。

本領域內(nèi)的技術人員應明白，本發(fā)明的實施例可提供為方法、系統(tǒng)、或計算機程序產(chǎn)品。因此，本發(fā)明可采用完全硬件實施例、完全軟件實施例、或結合軟件和硬件方面的實施例的形式。而且，本發(fā)明可采用在一個或多個其中包含有計算機可用程序代碼的計算機可用存儲介質(包括但不限于磁盤存儲器、cd-rom、光學存儲器等)上實施的計算機程序產(chǎn)品的形式。

本發(fā)明是參照根據(jù)本發(fā)明實施例的方法、設備(系統(tǒng))、和計算機程序產(chǎn)品的流程圖和/或方框圖來描述的。應理解可由計算機程序指令實現(xiàn)流程圖和/或方框圖中的每一流程和/或方框、以及流程圖和/或方框圖中的流程和/或方框的結合?？商峁┻@些計算機程序指令到通用計算機、專用計算機、嵌入式處理機或其他可編程數(shù)據(jù)處理設備的處理器以產(chǎn)生一個機器，使得通過計算機或其他可編程數(shù)據(jù)處理設備的處理器執(zhí)行的指令產(chǎn)生用于實現(xiàn)在流程圖一個流程或多個流程和/或方框圖一個方框或多個方框中指定的功能的裝置。

還需要說明的是，術語“包括”、“包含”或者其任何其他變體意在涵蓋非排他性的包含，從而使得包括一系列要素的過程、方法、商品或者設備不僅包括那些要素，而且還包括沒有明確列出的其他要素，或者是還包括為這種過程、方法、商品或者設備所固有的要素。在沒有更多限制的情況下，由語句“包括一個……”限定的要素，并不排除在包括所述要素的過程、方法、商品或者設備中還存在另外的相同要素。

以上所述僅為本申請的實施例而已，并不用于限制本申請。對于本領域技術人員來說，本申請可以有各種更改和變化。凡在本申請的精神和原理之內(nèi)所作的任何修改、等同替換、改進等，均應包含在本申請的權利要求范圍之內(nèi)。