[發(fā)明專利]基于粒子群算法的航跡規(guī)劃地形輔助導航方法

有效

• 申請?zhí)枺?/td>: 202110491740.X
• 申請日: 2021-05-06
• 公開（公告）號: CN113252038B
• 公開（公告）日: 2022-10-28
• 發(fā)明（設計）人: 許斌;張睿;李萬睿;孔紅華;黃銳;王瑞;馮小珂
• 申請（專利權(quán)）人: 西北工業(yè)大學;中國航空工業(yè)集團公司成都飛機設計研究所
• 主分類號: G01C21/20
• 分類號: G01C21/20;G06N3/00
• 代理公司: 西安凱多思知識產(chǎn)權(quán)代理事務所(普通合伙) 61290
• 代理人: 劉新瓊

本發(fā)明涉及一種基于粒子群算法的航跡規(guī)劃地形輔助導航方法，用于解決現(xiàn)有地形輔助導航方法在地形平坦區(qū)域精度較差的技術問題。該方法的步驟為：得到無人機飛行任務后，首先將任務區(qū)間的數(shù)字高程地圖劃分為一定大小的網(wǎng)格，并采用地形差異熵信息表示各網(wǎng)格地形起伏的復雜程度，選擇地形差異熵小的區(qū)域作為地形適配區(qū)；其次，基于地形適配區(qū)建立無人機航跡適應度函數(shù)，并利用粒子群算法得到優(yōu)化航跡，從而保證無人機的航跡均位于地形適配區(qū)內(nèi)；之后，利用無人機飛行路徑正下方的地形高度和存儲的參考高程地圖進行比較，得到無人機的位置信息；最后利用TERCOM最佳匹配位置和SINS輸出的位置信息的差值作為量測值，利用Kalman濾波估計SINS的誤差，并校正SINS。

技術領域

本發(fā)明涉及一種地形輔助導航方法，特別是涉及一種基于粒子群算法的航跡規(guī)劃地形輔助導航方法，屬于導航、制導與控制領域。

背景技術

地形輔助導航系統(tǒng)是近年來受到廣泛重視并已成功使用的組合導航系統(tǒng)，地形輔助導航是一種利用地形高程特征來進行輔助定位的方法，它具有自主、隱蔽、連續(xù)、全天候工作、導航定位誤差不隨時間積累等優(yōu)點，是理想的輔助導航定位手段。然而，地形輔助導航要求地形高程有明顯的變化，對于地形變化過于平緩、地形特征不明顯的區(qū)域，用地形輔助導航方法來降低慣導系統(tǒng)的定位誤差是不可行的。

《一種適用于無人機的地形匹配輔助導航算法》(姚軍,紀紅霞,王乃甲，《光學與光電技術》，2019年第2期)一文提出了一種適用于無人機的地形輔助導航算法。但是由于地形輔助導航的結(jié)果與地形變化密切相關，甚至無法在地形平坦區(qū)域使用，所以地形輔助導航前必須進行引入地形信息的航跡規(guī)劃，為無人機規(guī)劃一條符合航行任務、機動性能、威脅規(guī)避、有利于實現(xiàn)地形輔助導航的航線。

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

要解決的技術問題

由于地形輔助導航受地形因素影響大，無法在地形平坦區(qū)域使用，本發(fā)明設計了一種基于粒子群算法的航跡規(guī)劃地形輔助導航方法，用于解決現(xiàn)有地形輔助導航方法在地形平坦區(qū)域精度較差的技術問題。該方法在航跡規(guī)劃時引入地形信息，利用航跡規(guī)劃算法為無人機規(guī)劃一條在地形起伏較大區(qū)域的航線，以提升地形輔助導航的精度。

技術方案

一種基于粒子群算法的航跡規(guī)劃地形輔助導航方法，其特征在于步驟如下：

步驟1：基于地形差異熵的地形適配區(qū)選擇

確定無人機飛行任務后，將任務區(qū)間的數(shù)字高程地圖劃分為一定大小的網(wǎng)格，采用地形差異熵信息表示各網(wǎng)格地形起伏的復雜程度，地形差異熵H_mean定義為：

式中，h_i表示高程值；M為該區(qū)域高程點總數(shù)；表示平均高程值；C_i表示高程差異值；P_i表示某一高程差異值出現(xiàn)的概率；

步驟2：將地形適配區(qū)加入無人機航跡約束建立航跡適應度函數(shù)

在二維航跡規(guī)劃中，其代價函數(shù)包括航跡的長度代價、威脅代價以及水平轉(zhuǎn)角代價；

所述的航跡的長度代價函數(shù)為：

式中，X_i為航跡路徑；d_i,0，d_i,n+1分別為第i條航跡的起點到第1個節(jié)點的距離和最后一個節(jié)點到目標點的距離；n為第i條航跡的節(jié)點總數(shù)；x_i,j為第i條航跡的第j個節(jié)點。

所述的航跡的水平轉(zhuǎn)角代價函數(shù)為：

式中，α為無人機當前的水平轉(zhuǎn)角；α_max為無人機預設的最大轉(zhuǎn)角，由無人機型號確定；