本發(fā)明涉及一種無(wú)人飛行器導(dǎo)航控制領(lǐng)域，尤其涉及一種基于容錯(cuò)處理的無(wú)人飛行器高度測(cè)量控制系統(tǒng)及控制方法。

背景技術(shù)：

無(wú)人飛行器的絕對(duì)高度(即海拔高度)是飛行器飛行的重要參數(shù)，是巡航式飛行器飛行時(shí)須控制的關(guān)鍵參數(shù)。穩(wěn)定可靠地實(shí)時(shí)測(cè)量飛行器的絕對(duì)高度(即海拔高度)，是決定無(wú)人駕駛飛行器高度控制系統(tǒng)穩(wěn)定的保證。測(cè)量飛行器高度的裝置主要有衛(wèi)星導(dǎo)航系統(tǒng)和高度表，衛(wèi)星導(dǎo)航系統(tǒng)在空曠地帶下能夠全天候工作，且精度較高，但遇到遮擋易出現(xiàn)失捕而工作異常。高度表一般包括氣壓高度表和無(wú)線電高度表等類(lèi)型。氣壓高度表受多種外界因素如飛行器速度、溫度等等影響，測(cè)高誤差常高達(dá)幾百米，無(wú)線電高度表利用對(duì)地的無(wú)線電波發(fā)射原理工作，測(cè)量的是無(wú)人飛行器距地面高度，具有不受外界環(huán)境影響，精度高(尤其貼近地面時(shí))的特點(diǎn)，但當(dāng)無(wú)人飛行器姿態(tài)不穩(wěn)(尤其是大俯仰、橫滾)時(shí)，測(cè)高已出現(xiàn)不準(zhǔn)或異常情況。

目前，容錯(cuò)的策略主要有以下三種:

第一種是故障屏蔽，它是一種通過(guò)冗余設(shè)計(jì)防止故障引入系統(tǒng)的方式，故障的存在對(duì)系統(tǒng)的運(yùn)行是透明的，不產(chǎn)生影響，該種方式主要應(yīng)用于可靠性、實(shí)時(shí)性要求較高的系統(tǒng)中，第二種是重構(gòu)，它是通過(guò)對(duì)故障系統(tǒng)的移除，并重新恢復(fù)系統(tǒng)的運(yùn)行，其過(guò)程包括故障檢測(cè)、故障定位、故障隔離、故障恢復(fù)；第三種是上述兩種方式的混合。

通常實(shí)現(xiàn)容錯(cuò)的關(guān)鍵是冗余，現(xiàn)有技術(shù)中實(shí)現(xiàn)冗余一般都是通過(guò)增加額外的硬件資源，并增加額外的軟件實(shí)現(xiàn)故障檢測(cè)和容錯(cuò)，常常增加了設(shè)備成本而算法復(fù)雜，體現(xiàn)在專(zhuān)注于多類(lèi)型測(cè)量裝置的數(shù)據(jù)融合以提高測(cè)量精度，且組成系統(tǒng)的各個(gè)裝置均存在故障失效的可能，因此亟需研究針對(duì)多種高度測(cè)量裝置冗余系統(tǒng)的容錯(cuò)處理技術(shù)，以保證無(wú)人飛行器高度測(cè)量控制系統(tǒng)的工作穩(wěn)定。

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

本發(fā)明要解決的技術(shù)問(wèn)題是：如何提高多種高度測(cè)量裝置組成的冗余測(cè)高系統(tǒng)的可靠性及容錯(cuò)性。

本發(fā)明解決所述技術(shù)問(wèn)題所采取的技術(shù)方案是：針對(duì)兩種高度測(cè)量裝置組成的冗余系統(tǒng)采用容錯(cuò)處理技術(shù)以提高可靠性并基于該容錯(cuò)處理技術(shù)構(gòu)建無(wú)人飛行器高度測(cè)量控制系統(tǒng)。

本發(fā)明借鑒了容錯(cuò)處理的思想，但不備份相同硬件資源，如備份高度表模塊或衛(wèi)星導(dǎo)航模塊等，而是采用通過(guò)軟件計(jì)算方式使不同測(cè)量模塊均實(shí)現(xiàn)對(duì)無(wú)人飛行器高度測(cè)量，并在某一模塊故障時(shí)將測(cè)量值漸變切換另一模塊，以保證無(wú)人飛行器高度測(cè)量控制系統(tǒng)的工作穩(wěn)定。

本發(fā)明提供了一種基于容錯(cuò)處理的無(wú)人飛行器高度測(cè)量控制系統(tǒng)，其特征在于，該控制系統(tǒng)包括高度表模塊、衛(wèi)星導(dǎo)航模塊、慣性測(cè)量模塊以及計(jì)算處理模塊；

所述高度表模塊，實(shí)時(shí)測(cè)量所述無(wú)人飛行器對(duì)地距離；

所述衛(wèi)星導(dǎo)航模塊，實(shí)時(shí)接收處理衛(wèi)星信號(hào)，測(cè)量所述無(wú)人飛行器飛行位置及方向；

所述慣性測(cè)量模塊，實(shí)時(shí)測(cè)量所述無(wú)人飛行器姿態(tài)角速度及加速度；

所述計(jì)算處理模塊，以并行方式接收所述高度表模塊、所述衛(wèi)星導(dǎo)航模塊及所述慣性測(cè)量模塊的數(shù)據(jù)，計(jì)算獲得高度測(cè)量數(shù)據(jù)。

進(jìn)一步地，所述高度表模塊為無(wú)線電高度表。

進(jìn)一步地，所述衛(wèi)星導(dǎo)航模塊采用GPS接收機(jī)。

進(jìn)一步地，所述慣性測(cè)量模塊為MEMS慣性測(cè)量單元。

本發(fā)明還公開(kāi)了一種基于容錯(cuò)處理的無(wú)人飛行器高度測(cè)量控制系統(tǒng)的控制方法，其特征在于，所述控制方法包括如下步驟：

預(yù)先采集所述無(wú)人飛行器的工作區(qū)域的的地形數(shù)據(jù)并存儲(chǔ)在所述計(jì)算處理模塊中；所述地形數(shù)據(jù)采用如下二維網(wǎng)格數(shù)據(jù)格式：其中i＝1,2,…,n，j＝1,2,…,m，為將所述工作區(qū)域劃分的m×n大小的網(wǎng)格中一節(jié)點(diǎn)位置的經(jīng)緯度坐標(biāo)，H_i,j為網(wǎng)格節(jié)點(diǎn)對(duì)應(yīng)地點(diǎn)的地面海拔高度；

所述衛(wèi)星導(dǎo)航模塊獲得所述無(wú)人飛行器位置數(shù)據(jù)，所述位置數(shù)據(jù)主要為：經(jīng)度λ、緯度所述計(jì)算處理模塊(4)通過(guò)二維雙線性插值算法獲得所述無(wú)人飛行器正下方地面的海拔高度近似值H_DM；

所述計(jì)算處理模塊獲取所述衛(wèi)星導(dǎo)航模塊數(shù)據(jù)及所述慣性測(cè)量模塊數(shù)據(jù)，采用組合導(dǎo)航濾波算法計(jì)算得到所述無(wú)人飛行器的位置數(shù)據(jù)包括經(jīng)度λ、緯度海拔高度H_sat；姿態(tài)數(shù)據(jù)包括滾動(dòng)角速度ω_x、偏航角速度ω_y、俯仰角速度ω_z及滾動(dòng)角γ、偏航ψ、俯仰速度V。

進(jìn)一步地，所述計(jì)算處理模塊根據(jù)如下步驟對(duì)所述高度表模塊的數(shù)據(jù)、所述衛(wèi)星導(dǎo)航模塊數(shù)據(jù)、所述慣性測(cè)量模塊的數(shù)據(jù)進(jìn)行容錯(cuò)處理計(jì)算以獲得用于飛行器高度控制的綜合海拔高度H_ctrl：

情況1：當(dāng)所述高度表模塊、所述衛(wèi)星導(dǎo)航模塊、所述慣性測(cè)量模塊均工作正常，采用H_ctrl＝H_gdb+H_DM或H_ctrl＝H_sat計(jì)算綜合海拔高度，其中H_gdb為所述計(jì)算處理模塊采集所述高度表模塊數(shù)據(jù)獲得所述無(wú)人飛行器對(duì)地距離；

情況2：當(dāng)所述高度表模塊于t＝t_{gdb_F}時(shí)刻故障，但所述衛(wèi)星導(dǎo)航模塊、所述慣性測(cè)量模塊均工作正常，按以下公式計(jì)算綜合海拔高度：

其中表示所述高度表模塊故障前所述計(jì)算處理模塊最后一次記錄的綜合海拔高度正常值；

情況3：當(dāng)所述衛(wèi)星導(dǎo)航模塊于t＝t_{sat_F}時(shí)刻故障，但所述高度表模塊、所述慣性測(cè)量模塊均工作正常，按以下公式計(jì)算綜合海拔高度：

其中表示所述衛(wèi)星導(dǎo)航模塊故障前所述計(jì)算處理模塊最后一次記錄的綜合海拔高度正常值；

情況4：當(dāng)所述衛(wèi)星導(dǎo)航模塊及所述高度表模塊于t＝t_{sat_gdb_F}時(shí)刻均故障，但所述慣性測(cè)量模塊工作正常，按以下公式計(jì)算綜合海拔高度：

其中所述衛(wèi)星導(dǎo)航模塊及所述高度表模塊均故障前所述計(jì)算處理模塊最后一次記錄的所述無(wú)人飛行器海拔高度正常值，V_XH為飛行器的平均速度，預(yù)先存儲(chǔ)在所述計(jì)算處理模塊中。

進(jìn)一步地，所述計(jì)算處理模塊經(jīng)過(guò)PID控制算法控制所述無(wú)人飛行器高度穩(wěn)定。

按照本發(fā)明實(shí)現(xiàn)的基于容錯(cuò)處理的無(wú)人飛行器高度測(cè)量控制系統(tǒng)及其控制方法，能夠獲得如下的有益效果：

(1)本發(fā)明中對(duì)整體的控制系統(tǒng)進(jìn)行了模塊化設(shè)置的優(yōu)化，按照本發(fā)明組成的四個(gè)模塊中只要慣性測(cè)量模塊和計(jì)算處理模塊正常工作，且衛(wèi)星導(dǎo)航模塊及高度表模塊不發(fā)生同時(shí)長(zhǎng)時(shí)間故障的情況，飛行器的海拔高度可以測(cè)量獲得；

(2)另外一個(gè)方面，本發(fā)明中的飛行器高度測(cè)量控制系統(tǒng)采用了容錯(cuò)處理技術(shù)，上述的冗余容錯(cuò)處理技術(shù)允許產(chǎn)品設(shè)計(jì)采用可靠性等級(jí)較低的傳感器組成測(cè)量系統(tǒng)，但是又并沒(méi)有降低整體的測(cè)量控制系統(tǒng)的可靠性。

總而言之，按本發(fā)明實(shí)現(xiàn)的基于容錯(cuò)處理技術(shù)的無(wú)人飛行器高度測(cè)量控制系統(tǒng)，在很大程度上提高了高度測(cè)量的可靠性，從而實(shí)現(xiàn)了無(wú)人飛行器高度控制系統(tǒng)的可靠性。

附圖說(shuō)明

圖1為本發(fā)明實(shí)現(xiàn)的基于容錯(cuò)處理的無(wú)人飛行器高度測(cè)量控制系統(tǒng)的組成示意圖。

具體實(shí)施方式

為了使本發(fā)明的目的、技術(shù)方案及優(yōu)點(diǎn)更加清楚明白，以下結(jié)合附圖及實(shí)施例，對(duì)本發(fā)明進(jìn)行進(jìn)一步詳細(xì)說(shuō)明。

本發(fā)明實(shí)施例提供的一種基于容錯(cuò)處理技術(shù)的無(wú)人飛行器高度測(cè)量控制系統(tǒng)實(shí)例：

主要包括高度表模塊1、衛(wèi)星導(dǎo)航模塊2及慣性測(cè)量模塊3、計(jì)算處理模塊4；

高度表模塊1采用無(wú)線電高度表，實(shí)時(shí)測(cè)量飛行器對(duì)地距離；

衛(wèi)星導(dǎo)航模塊2采用GPS接收機(jī)，接收處理衛(wèi)星信號(hào)，實(shí)時(shí)測(cè)量無(wú)人飛行器飛行位置及方向；

慣性測(cè)量模塊3，采用AD公司的低成本MEMS慣性測(cè)量單元ADIS16405，實(shí)時(shí)測(cè)量飛行器姿態(tài)角速度及加速度；

計(jì)算處理模塊4以TI公司數(shù)字信號(hào)處理芯片TMS320C6713B為核心，擴(kuò)展接口電路以并行方式接收高度表模塊、衛(wèi)星導(dǎo)航模塊及慣性測(cè)量模塊數(shù)據(jù)，綜合計(jì)算后輸出控制信號(hào)。

計(jì)算處理模塊4中存儲(chǔ)了任務(wù)區(qū)域的地形數(shù)據(jù)；所述地形數(shù)據(jù)采用如下二維網(wǎng)格數(shù)據(jù)格式：如表1所列，其中i＝1,2,…,n，j＝1,2,…,m，為將任務(wù)區(qū)域劃分的m×n大小的網(wǎng)格中一節(jié)點(diǎn)位置(經(jīng)緯度)坐標(biāo)，H_i,j為網(wǎng)格節(jié)點(diǎn)對(duì)應(yīng)地點(diǎn)的地面海拔高度。

表1計(jì)算處理模塊4中存儲(chǔ)的任務(wù)地區(qū)的地形數(shù)據(jù)

工作時(shí)，計(jì)算處理模塊4采集衛(wèi)星導(dǎo)航模塊2數(shù)據(jù)及慣性測(cè)量模塊3數(shù)據(jù)進(jìn)行綜合計(jì)算得到無(wú)人飛行器的位置數(shù)據(jù)(經(jīng)度λ、緯度海拔高度H_sat)、姿態(tài)數(shù)據(jù)(滾動(dòng)角速度ω_x、偏航角速度ω_y、俯仰角速度ω_z及滾動(dòng)角γ、偏航ψ、俯仰)、速度V；

本發(fā)明的計(jì)算方法主要采用以卡爾曼濾波器為核心的組合算法，本發(fā)明所基于的卡爾曼濾波器為核心的組合算法主要是有效利用衛(wèi)星導(dǎo)航數(shù)據(jù)減小慣性測(cè)量組合的誤差，且當(dāng)所述衛(wèi)星導(dǎo)航模塊故障時(shí)，上述參數(shù)仍能繼續(xù)計(jì)算。本發(fā)明中的核心算法的流程如下，首先是衛(wèi)星導(dǎo)航模塊中的衛(wèi)星信號(hào)接收天線要能接收到相應(yīng)的信號(hào)數(shù)據(jù)，在接收到信號(hào)數(shù)據(jù)后首先進(jìn)行計(jì)算偽距、偽距率、相位等，接著利用組合卡爾曼濾波算法來(lái)進(jìn)行導(dǎo)航解算力學(xué)編排，最后輸出無(wú)人飛行器的位置數(shù)據(jù)、姿態(tài)數(shù)據(jù)等各類(lèi)數(shù)據(jù)，該算法為本領(lǐng)域的常規(guī)計(jì)算方法，在此不再贅述。

其中，計(jì)算處理模塊4利用獲得的飛行器姿態(tài)數(shù)據(jù)，采用如下形式的PID控制算法控制飛行器姿態(tài)穩(wěn)定：

其中是按飛行方案在計(jì)算處理模塊4中預(yù)先計(jì)算的飛行俯仰角指令，根據(jù)具體飛行任務(wù)等確定；是俯仰角偏差的放大系數(shù)，是俯仰角速度的放大系數(shù)，根據(jù)飛行器具體性能參數(shù)確定；是計(jì)算處理模塊4輸出的控制信號(hào)中操縱俯仰穩(wěn)定的部分。

計(jì)算處理模塊4獲得無(wú)人飛行器位置數(shù)據(jù)(經(jīng)度λ、緯度)后，通過(guò)二維雙線性插值算法獲得飛行器正下方地面的海拔高度近似值H_DM。

計(jì)算處理模塊4根據(jù)如下步驟對(duì)高度表模塊1數(shù)據(jù)、衛(wèi)星導(dǎo)航模塊2數(shù)據(jù)、慣性測(cè)量模塊3數(shù)據(jù)進(jìn)行容錯(cuò)處理計(jì)算以獲得用于飛行器高度控制的綜合海拔高度H_ctrl：其中，在下述步驟中計(jì)算的每個(gè)故障時(shí)刻的算法，均為計(jì)算處理模塊4對(duì)上述模塊進(jìn)行實(shí)時(shí)監(jiān)測(cè)記錄每個(gè)模塊產(chǎn)生故障的時(shí)間，當(dāng)然根據(jù)本領(lǐng)域常用的技術(shù)設(shè)置，上述故障數(shù)據(jù)可以來(lái)自于各個(gè)模塊在發(fā)生故障后能夠自動(dòng)監(jiān)檢測(cè)后反饋于計(jì)算處理模塊，在此不再贅述。

Step 1：當(dāng)高度表模塊1、衛(wèi)星導(dǎo)航模塊2、慣性測(cè)量模塊3均工作正常，用戶可根據(jù)實(shí)際情況選擇用H_ctrl＝H_gdb+H_DM或H_ctrl＝H_sat計(jì)算綜合海拔高度，其中H_gdb為計(jì)算處理模塊4采集高度表模塊1數(shù)據(jù)獲得的飛行器對(duì)地距離；

Step 2：當(dāng)高度表模塊1于t＝t_{gdb_F}時(shí)刻故障，但衛(wèi)星導(dǎo)航模塊2、慣性測(cè)量模塊3均工作正常，按以下公式計(jì)算綜合海拔高度：

其中表示高度表模塊1故障前計(jì)算處理模塊4最后一次記錄的綜合海拔高度正常值；

Step 3：當(dāng)衛(wèi)星導(dǎo)航模塊2于t＝t_{sat_F}時(shí)刻故障，但高度表模塊1、慣性測(cè)量模塊3均工作正常，按以下公式計(jì)算綜合海拔高度：

其中表示衛(wèi)星導(dǎo)航模塊2故障前計(jì)算處理模塊4最后一次記錄的綜合海拔高度正常值；

Step 4：當(dāng)衛(wèi)星導(dǎo)航模塊2及高度表模塊1于t＝t_{sat_gdb_F}時(shí)刻均故障，但慣性測(cè)量模塊3工作正常，按以下公式計(jì)算綜合海拔高度：

其中衛(wèi)星導(dǎo)航模塊2及高度表模塊1均故障前計(jì)算處理模塊4最后一次記錄的飛行器海拔高度正常值，V_XH為無(wú)人飛行器的平均速度，預(yù)先存儲(chǔ)在計(jì)算處理模塊4中。

更進(jìn)一步地，計(jì)算處理模塊4的方法獲得的飛行器綜合海拔高度H_ctrl，經(jīng)過(guò)PID控制算法控制飛行器高度穩(wěn)定。

其中H_c是按飛行方案在所述計(jì)算處理模塊中預(yù)先計(jì)算的飛行海拔高度指令，根據(jù)具體飛行任務(wù)等確定；K_PH是飛行器飛行高度偏差的放大系數(shù)，K_DH是飛行器高度偏差微分信號(hào)的放大系數(shù)，K_IH是飛行器飛行高度偏差積分的放大系數(shù)，K_PH、K_DH、K_IH根據(jù)飛行器具體性能參數(shù)確定；u_Z,H是所述計(jì)算處理模塊輸出的控制信號(hào)中控制飛行器高度穩(wěn)定的部分。

更進(jìn)一步地，所述的計(jì)算處理模塊按下述公式輸出控制信號(hào)u_Z給飛行器縱向姿態(tài)位置操縱機(jī)構(gòu)：

本領(lǐng)域的技術(shù)人員容易理解，以上所述僅為本發(fā)明的較佳實(shí)施例而已，并不用以限制本發(fā)明，凡在本發(fā)明的精神和原則之內(nèi)所作的任何修改、等同替換和改進(jìn)等，均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。