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

有效

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

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

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

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

式中，h_i表示高程值；M為每個網格高程點總數；表示平均高程值；C_i表示高程差異值；P_i表示某一高程差異值出現的概率；

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

在二維航跡規劃中，其代價函數包括航跡的長度代價、威脅代價以及水平轉角代價；

所述的航跡的長度代價函數為：

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

所述的航跡的水平轉角代價函數為：

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

將步驟1選出的地形非適配區視為威脅圈，所述的航跡的威脅代價函數為：

式中，s_k為威脅強度系數；J_k,j為第k個威脅圈對第j段航跡段的威脅度，且

式中，d_k為第k個威脅圈到第j段航跡段之間的垂線距離；

綜上所述，無人機航跡規劃的適應度函數為：

J＝w₁×J_length+w₂×J_threat+w₃×J_{turn_angle} (6)

式中，w₁,w₂,w₃分別為權重系數，須滿足w₁+w₂+w₃＝1；

步驟3：基于粒子群優化的航跡規劃

應用粒子群算法求解無人機航跡規劃問題時，每一條可行的航路就是一個“粒子”；假設航路控制點有D個，則粒子的維數為D維；第i個粒子的位置表示為x_i＝(x_i1,x_i2,…,x_iD)，該粒子對應位置速度的變化量為v_i＝(v_i1,v_i2,…,v_iD)，粒子的速度和位置更新公式為：

式中，ω為慣性權重，可以動態調整粒子運動速度；為第i個粒子的d維分量在第t次迭代中的速度和位置；c₁，c₂為加速因子，取非負值；P_best為粒子本身所找到的最好解；G_best為整個種群目前找到的最好解；r₁，r₂為隨機數，服從區間[0,1]上的均勻分布；

粒子群算法初始化生成一群隨機粒子，在每一次迭代中，根據式(6)評價粒子的適應度值，粒子通過跟蹤P_best和G_best更新：如果適應度值優于該粒子當前的個體極值，則將P_best設置為該粒子的當前位置，且更新個體極值；如果所有粒子的最優值的優于當前的全局極值，則將G_best設置為最優粒子的位置，記錄該粒子的序號，且更新全局極值，直到迭代至預先設置的迭代次數，則停止迭代，輸出最優解；

步驟4：基于軌跡規劃的地形輪廓匹配

地形輪廓匹配TERCOM的步驟為：測得實時高程后，首先確定搜索區域范圍；其次在基準圖上搜索范圍內采用序貫相似度算法匹配得到基準子圖；最后利用相似度分析算法選出與實測圖最相似的基準子圖即為最佳匹配位置；

步驟5：SINS/TERCOM組合導航

得到TERCOM提供的最佳匹配位置后，選取SINS的誤差為SINS/TERCOM組合導航系統的狀態變量構建狀態方程；選擇TERCOM最佳匹配位置和SINS輸出的位置信息的差值作為量測值；對建立的狀態方程和量測方程使用卡爾曼濾波器進行濾波更新，并反饋修正SINS，得到組合導航的導航參數。