本發(fā)明涉及飛行器導(dǎo)航控制技術(shù)領(lǐng)域，具體來說是一種飛行器自主導(dǎo)航方法及系統(tǒng)。

背景技術(shù)：

隨著飛行器技術(shù)的發(fā)展，飛行器的飛行逐漸由人工控制飛行向自主飛行過渡，而自主可靠的導(dǎo)航技術(shù)是實現(xiàn)自主飛行亟需解決的關(guān)鍵技術(shù)之一。飛行器導(dǎo)航按照工作原理的不同可分為多種。例如現(xiàn)有技術(shù)中的地磁場導(dǎo)航，主要采用磁場匹配的方式，即將地磁場及對應(yīng)的經(jīng)度、緯度及高度等信息預(yù)存在飛行器的處理器中，飛行器在飛行過程中，將磁場測量儀采集到的磁場與預(yù)存的磁場強(qiáng)度進(jìn)行匹配，進(jìn)而得到相應(yīng)的經(jīng)度、緯度及高度，雖然因磁場測量儀體積小，在小量程時精度高、線性度高的優(yōu)點在導(dǎo)航技術(shù)中廣泛使用，但是這種導(dǎo)航方式對處理器的存儲容量及計算能力要求高，匹配精度較低。

針對該問題，目前較為常用的提高導(dǎo)航精確度的方法是采用多敏感器信息融合技術(shù)，即在一個導(dǎo)航系統(tǒng)中使用多種敏感器獲得的多種形式的導(dǎo)航信息，而后通過一定的算法“合并”來自多個信息源的信息，以產(chǎn)生比單個傳感器所得到數(shù)據(jù)更可靠、更準(zhǔn)確的信息，克服了單一導(dǎo)航敏感器可靠性及容錯性低、導(dǎo)航精度低的缺陷。而在組合導(dǎo)航技術(shù)中，關(guān)于gnss融合導(dǎo)航算法是采用慣測組合與gnss的融合，其中，慣性器件的選擇是關(guān)鍵，若慣性器件為石英加表，敏感器的體積太大；若慣性器件為微機(jī)械加表，則導(dǎo)航的精度及線性度太差，且慣性器件普遍存在零偏及零偏穩(wěn)定性的缺陷，會導(dǎo)致導(dǎo)航精度下降。

在上述基礎(chǔ)上，無疑會想到結(jié)合磁場測量儀和gnss導(dǎo)航的優(yōu)點而進(jìn)行導(dǎo)航，但磁場測量儀導(dǎo)航屬于無源導(dǎo)航，磁場測量儀由于飛行器通電時會產(chǎn)生磁場，難免會降低地磁場的測量精度，同時由于地磁場模型的誤差，因而基于磁場測量儀的導(dǎo)航算法精度較低；而gnss導(dǎo)航屬于有源導(dǎo)航，雖然導(dǎo)航精度高，但當(dāng)gps、glonass或者北斗等導(dǎo)航星不可見或者不可用時，gnss將失效，導(dǎo)致導(dǎo)航不可用，從而降低了導(dǎo)航的可靠性，因此，如何將兩者進(jìn)行自合形成自主導(dǎo)航成為了導(dǎo)航領(lǐng)域的一大難題。

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

為解決在絕對導(dǎo)航時磁場測量儀導(dǎo)航精度低，而gnss導(dǎo)航無效時無導(dǎo)航可用，導(dǎo)致可靠性低的問題，本發(fā)明提供一種基于地磁場測量儀和gnss的飛行器自主導(dǎo)航方法，以磁場測量儀導(dǎo)航、gnss導(dǎo)航的兩種導(dǎo)航方式，實現(xiàn)高精度、高可靠的導(dǎo)航，提高飛行器自主絕對導(dǎo)航方法的可靠性。

為了解決上述技術(shù)問題，本發(fā)明采用了如下的技術(shù)方案：

一種飛行器自主導(dǎo)航方法，所述的導(dǎo)航方法的具體步驟為：

s1.按周期獲取gnss模塊檢測到的位置信息；

s2.按周期獲取磁場測量儀檢測到磁場信息；

s3.根據(jù)gnss模塊的位置信息判斷gnss模塊是否有效：

若gnss模塊有效，將飛行器設(shè)置為gnss導(dǎo)航模式，依據(jù)gnss模塊輸出的位置信息進(jìn)行導(dǎo)航；

若gnss模塊無效，將飛行器設(shè)置為磁場測量儀導(dǎo)航模式，進(jìn)入步驟s4；

s4.依據(jù)上周期位置信息和本周期磁場信息進(jìn)行飛行器的位置計算，依據(jù)計算得出的位置信息進(jìn)行導(dǎo)航。

進(jìn)一步地，所述的位置計算具體包括以下步驟：

s4-1.根據(jù)磁場測量儀檢測到三軸磁場強(qiáng)度，在北東地坐標(biāo)系下進(jìn)行計算，計算公式如下：

將公式(1)進(jìn)行轉(zhuǎn)換分別得到公式(2)、(3)、(4)、(5)：

公式(1)、(2)、(3)、(4)、(5)中，re為地球半徑，g⁰1、g¹1、h¹1分別為高斯系數(shù)，λ為飛行器的經(jīng)度，θ為飛行器的地心余緯，r為飛行器到地心的距離；

s4-2.求解公式(2)，計算經(jīng)度λ；

s4-3.求解公式(3)，計算地心余緯θ；

s4-4.根據(jù)步驟s4-2及步驟s4-3計算得到的經(jīng)度λ和地心余緯θ，利用公式(5)，計算此時飛行器到地心的距離r；

s4-5.根據(jù)計算得到的經(jīng)度λ、地心余緯θ、飛行器到地心的距離r計算

飛行器在wgs-84系下的位置，計算方法如下：

s4-6.根據(jù)步驟s4-5分別計算飛行器在本周期k和上一周期k-1時的位置，計算飛行器在計算周期t內(nèi)的速度，計算方法為：

進(jìn)一步地，所述的導(dǎo)航方法還包括：將本周期內(nèi)gnss模塊的位置信息與上一周期內(nèi)依據(jù)磁場強(qiáng)度解算得到的位置信息進(jìn)行濾波融合計算，以濾波融合計算得到位置信息作為本周期位置計算的初始值。

進(jìn)一步地，所述的濾波融合計算具體為：

公式(8)式中，α、β為濾波系數(shù)，λgnss_k、θgnss_k、rgnss_k為k周期內(nèi)gnss模塊檢測到的位置信息，λe_(k-1)、θe_(k-1)、re_(k-1)為k-1周期內(nèi)依據(jù)磁場強(qiáng)度解算得到的位置信息。

進(jìn)一步地，所述的判斷gnss模塊是否有效的方法如下：

δp＝pk-pk-1

δv＝vk-vk-1

式中，pk為本周期位置，pk-1為上周期位置，δp為位置差，vk為本周期速度，vk-1為上周期速度，δv為速度差，假設(shè)ak-1為上周期加速度，若δv>3ak-1或者δp>3δv時，則gnss模塊無效。

進(jìn)一步地，對所述公式(2)采用牛頓迭代法進(jìn)行求解。

進(jìn)一步地，所述的位置信息包括飛行器的經(jīng)度λ、地心余緯θ及飛行器到地心的距離r。

進(jìn)一步地，所述的磁場信息包括北東地坐標(biāo)系下的三軸磁場強(qiáng)度bx、by、bz。

本發(fā)明另外還提供一種飛行器自主導(dǎo)航系統(tǒng)，包括gnss模塊、磁場測量儀及數(shù)據(jù)處理模塊，

所述的gnss模塊檢測飛行器的位置信息，并將飛行器的位置信息按周期發(fā)送至數(shù)據(jù)處理模塊；

所述的磁場測量儀檢測磁場信息，并將磁場信息按周期發(fā)送至數(shù)據(jù)處理模塊；

所述的數(shù)據(jù)處理模塊，依據(jù)gnss模塊的位置信息判斷gnss模塊是否有效，并根據(jù)gnss模塊檢測到的飛行器的位置信息和磁場測量儀檢測到的磁場信息進(jìn)行飛行器的位置計算，利用計算得到的位置信息，進(jìn)一步計算得出飛行器在wgs-84系下的位置和速度，并將計算后的飛行器的位置信息和速度作為導(dǎo)航數(shù)據(jù)輸出。

進(jìn)一步地，所述的數(shù)據(jù)處理模塊還包括：將本周期內(nèi)gnss模塊的位置信息與上一周期內(nèi)依據(jù)磁場強(qiáng)度解算得到的位置信息的進(jìn)行濾波融合計算，將濾波融合后的位置信息作為位置計算的初始值。

本發(fā)明由于采用以上技術(shù)方案，使之與現(xiàn)有技術(shù)相比，具有以下的優(yōu)點和積極效果：

1.本發(fā)明利用磁場測量儀和gnss進(jìn)行導(dǎo)航，在gnss有效時利用gnss輸出的導(dǎo)航數(shù)據(jù)進(jìn)行導(dǎo)航，而在gnss無效時切換至磁場測量儀導(dǎo)航模式進(jìn)行位置計算，按計算得出的位置信息作為導(dǎo)航數(shù)據(jù)進(jìn)行導(dǎo)航，既保證飛行器的導(dǎo)航精度，又有效提高了整個飛行器導(dǎo)航系統(tǒng)的可靠性；

2.在磁場測量儀導(dǎo)航模式的位置計算中，綜合gnss模塊和磁場測量儀的數(shù)據(jù)進(jìn)行位置計算，提高了磁場測量儀導(dǎo)航模式時的導(dǎo)航精度；

3.在位置計算前對本周期gnss模塊和上周期磁場測量儀的數(shù)據(jù)進(jìn)行濾波融合計算，提高導(dǎo)航精度的同時，實現(xiàn)了兩種導(dǎo)航模式之間數(shù)據(jù)的平滑切換，解決了gnss模塊導(dǎo)航及磁場測量儀模塊導(dǎo)航切換時數(shù)據(jù)突變導(dǎo)致的導(dǎo)航系統(tǒng)不穩(wěn)定的問題。

附圖說明

圖1是本發(fā)明中飛行器自主導(dǎo)航方法的流程示意圖；

圖2是本發(fā)明中飛行器自主導(dǎo)航系統(tǒng)的連接結(jié)構(gòu)示意圖。

具體實施方式

以下結(jié)合附圖和具體實施例對本發(fā)明提出的技術(shù)方案進(jìn)一步詳細(xì)說明。根據(jù)下面說明和權(quán)利要求書，本發(fā)明的優(yōu)點和特征將更清楚。需說明的是，附圖均采用非常簡化的形式且均使用非精準(zhǔn)的比率，僅用于方便、明晰地輔助說明本發(fā)明實施例的目的。

實施例1

參見圖1，為本發(fā)明中飛行器自主導(dǎo)航方法的流程示意圖，現(xiàn)根據(jù)該流程圖對本發(fā)明中的飛行器自主導(dǎo)航方法的具體過程進(jìn)行說明，飛行器自主導(dǎo)航方法的具體步驟如下：

s1.按周期獲取內(nèi)gnss模塊檢測到的位置信息；

s2.按周期獲取磁場測量儀檢測到的磁場信息；

s3.根據(jù)gnss模塊的位置信息判斷gnss模塊是否有效：

若gnss模塊有效，將飛行器設(shè)置為gnss導(dǎo)航模式，依據(jù)gnss模塊輸出的位置信息進(jìn)行導(dǎo)航；

若gnss模塊無效，將飛行器設(shè)置為磁場測量儀導(dǎo)航模式，進(jìn)入步驟s4；

s4.依據(jù)位置信息和磁場信息進(jìn)行飛行器的位置計算，依據(jù)計算得出的位置信息進(jìn)行導(dǎo)航。

其中，位置信息包括飛行器的經(jīng)度、地心余緯及飛行器到地心的距離，磁場信息包括北東地坐標(biāo)系下的三軸磁場強(qiáng)度。在gnss模塊有效的情況下，導(dǎo)航輸出采用gnss模塊采集的位置信息作為輸出值；當(dāng)gps、glonass或者北斗等導(dǎo)航星不可見或者不可用導(dǎo)致gnss模塊無效時，導(dǎo)航輸出則采用磁場測量儀和gnss模塊采集的位置信息進(jìn)行融合濾波計算，以計算得出的位置信息作為導(dǎo)航值進(jìn)行后續(xù)導(dǎo)航，提高了導(dǎo)航系統(tǒng)的可靠性。此外，融合濾波算法保證了磁場測量儀導(dǎo)航模式的導(dǎo)航精度。

判斷gnss模塊是否有效的方法如下：

δp＝pk-pk-1

δv＝vk-vk-1

式中，pk為本周期位置，pk-1為上周期位置，δp位置差，vk為本周期速度，vk-1為上周期速度，δv速度差，假設(shè)ak-1為上周期加速度，一般情況下，若δv>3ak-1或者δp>3δv時，則認(rèn)為gnss模塊無效。

為提高磁場測量儀導(dǎo)航模式時的導(dǎo)航精度，在磁場測量儀導(dǎo)航模式的位置計算中，需綜合gnss模塊和磁場測量儀的數(shù)據(jù)進(jìn)行位置計算，為便于說明，其中，位置信息分別以飛行器的經(jīng)度λ、地心余緯θ及飛行器到地心的距離r為例，磁場信息以北東地坐標(biāo)系下的三軸磁場強(qiáng)度bx、by、bz為例。

位置計算具體包括以下步驟：

首先，步驟一，根據(jù)磁場測量儀檢測到三軸磁場強(qiáng)度，在北東地坐標(biāo)系下進(jìn)行計算，計算公式如下：

將公式(1)進(jìn)行轉(zhuǎn)換分別得到公式(2)、(3)、(4)、(5)：

公式(1)、(2)、(3)、(4)、(5)中，re為地球半徑，一般取6378140m，g⁰1、g¹1、h¹1分別為高斯系數(shù)，g⁰1一般取值為-0.30339gs，g¹1一般取值為-0.02123gs，h¹1一般取值為0.05758gs。

步驟二，對公式進(jìn)行求解：求解公式(2)，計算經(jīng)度λ，可采用牛頓迭代法對公式(2)進(jìn)行求解，可以用公式(8)融合濾波計算得到數(shù)值作為迭代的初始值，以提高磁場測量儀導(dǎo)航的精度，下文中將會詳細(xì)介紹融合濾波計算過程；求解公式(3)，計算地心余緯θ；根據(jù)計算得到的經(jīng)度λ和地心余緯θ及公式(5)，計算此時飛行器到地心的距離r。

步驟三，根據(jù)計算得到的經(jīng)度λ、地心余緯θ、飛行器到地心的距離r計算飛行器在wgs-84系下的位置，計算方法如下：

步驟四，分別計算飛行器在本周期k和上一周期k-1時的位置，計算飛行器在計算周期t內(nèi)的速度，計算方法為：

為解決gnss模塊導(dǎo)航及磁場測量儀模塊導(dǎo)航切換時數(shù)據(jù)突變導(dǎo)致的導(dǎo)航系統(tǒng)不穩(wěn)定的問題，還可將本周期內(nèi)gnss模塊的位置信息與上一周期內(nèi)依據(jù)磁場強(qiáng)度解算得到的位置信息進(jìn)行濾波融合計算，以濾波融合計算得到位置信息作為本周期位置計算的初始值，濾波融合計算的公式具體為：

公式(8)式中，α、β為濾波系數(shù)，當(dāng)gnss有效時，α、β可根據(jù)導(dǎo)航系統(tǒng)特性取值；當(dāng)gnss無效時，β＝0，λgnss_k、θgnss_k、rgnss_k對應(yīng)依次為k周期內(nèi)gnss模塊檢測到的飛行器的經(jīng)度、地心余緯及飛行器到地心的距離，λe_(k-1)、θe_(k-1)、re_(k-1)為k-1周期內(nèi)依據(jù)磁場強(qiáng)度解算得到的飛行器的經(jīng)度、地心余緯及飛行器到地心的距離，將濾波融合計算計算得出的經(jīng)度λ、地心余緯θ、飛行器到地心的距離r作為公式(2)中的牛頓迭代的初始值，以此提高磁場測量儀模式的導(dǎo)航精度，并實現(xiàn)了gnss模塊導(dǎo)航模式和磁場測量儀導(dǎo)航模式之間的數(shù)據(jù)的平滑切換。

本發(fā)明是綜合地磁場測量儀和gnss的優(yōu)點，在gnss有效時利用gnss輸出的導(dǎo)航數(shù)據(jù)進(jìn)行導(dǎo)航，而在gnss無效時切換至磁場測量儀導(dǎo)航模式進(jìn)行位置計算，按計算得出的位置信息作為導(dǎo)航數(shù)據(jù)進(jìn)行導(dǎo)航，在保證飛行器的導(dǎo)航精度的情況下，有效提高了整個飛行器導(dǎo)航過程及導(dǎo)航系統(tǒng)的可靠性。

實施例2

本發(fā)明另外還提供一種飛行器自主導(dǎo)航系統(tǒng)，參見圖2，飛行器自主導(dǎo)航系統(tǒng)包括gnss模塊、磁場測量儀及數(shù)據(jù)處理模塊，gnss模塊檢測飛行器的位置信息，并將飛行器的位置信息按周期發(fā)送至數(shù)據(jù)處理模塊；磁場測量儀檢測磁場信息和位置信息，并將磁場信息和位置信息按周期發(fā)送至數(shù)據(jù)處理模塊；數(shù)據(jù)處理模塊，依據(jù)gnss模塊的位置信息判斷gnss模塊是否有效，并根據(jù)gnss模塊檢測到的飛行器的位置信息和磁場測量儀檢測到的磁場信息，進(jìn)行飛行器的位置計算，解算飛行器的位置信息，利用計算得到的位置信息進(jìn)一步計算得出飛行器在wgs-84系下的位置和速度，并將計算后的飛行器的位置信息和速度作為導(dǎo)航數(shù)據(jù)輸出。本系統(tǒng)利用磁場測量儀和gnss進(jìn)行導(dǎo)航，在gnss有效時利用gnss輸出的導(dǎo)航數(shù)據(jù)進(jìn)行導(dǎo)航，而在gnss無效時切換至磁場測量儀導(dǎo)航模式進(jìn)行位置計算，并按計算得出的位置信息作為導(dǎo)航數(shù)據(jù)進(jìn)行導(dǎo)航，可廣泛應(yīng)用于多種飛行器，例如飛機(jī)、衛(wèi)星等，以實現(xiàn)飛行器精確、可靠的導(dǎo)航過程。

另外，為實現(xiàn)gnss模塊導(dǎo)航及磁場測量儀模塊導(dǎo)航兩種導(dǎo)航模式之間的平滑切換，解決gnss模塊導(dǎo)航及磁場測量儀模塊導(dǎo)航切換時數(shù)據(jù)突變導(dǎo)致的導(dǎo)航系統(tǒng)不穩(wěn)定的問題，數(shù)據(jù)處理模塊還將本周期內(nèi)gnss模塊的位置信息與上一周期內(nèi)依據(jù)磁場強(qiáng)度解算得到的位置信息進(jìn)行濾波融合計算，將計算后的位置信息作為導(dǎo)航數(shù)據(jù)輸出。

飛行器自主導(dǎo)航系統(tǒng)中，數(shù)據(jù)處理模塊對飛行器進(jìn)行的位置計算和濾波融合計算與實施例1中的計算過程相似，在此不做贅述。

顯然，本領(lǐng)域的技術(shù)人員可以對發(fā)明進(jìn)行各種改動和變型而不脫離本發(fā)明的精神和范圍。這樣，倘若本發(fā)明的這些修改和變型屬于本發(fā)明權(quán)利要求及其等同技術(shù)的范圍之內(nèi)，則本發(fā)明也意圖包含這些改動和變型在內(nèi)。