[發明專利]一種飛行器繞多禁飛區路徑規劃與決策方法

有效

• 申請號: 202010941572.5
• 申請日: 2020-09-09
• 公開（公告）號: CN112162569B
• 公開（公告）日: 2022-02-18
• 發明（設計）人: 李惠峰;張源;張冉;師鵬
• 申請（專利權）人: 北京航空航天大學
• 主分類號: G05D1/12
• 分類號: G05D1/12
• 代理公司: 北京慧泉知識產權代理有限公司 11232
• 代理人: 王順榮;唐愛華

1.一種飛行器繞多禁飛區路徑規劃與決策方法，其特征在于：其具體步驟如下：

步驟一、建立有向圖模型；

先將禁飛區進行膨脹化處理，并建立坐標系以及標定禁飛區位置；這里基于圖論來進行建模；

首先合并禁飛區；將禁飛區看成一個大的橢圓形禁飛區；

其次，設定虛擬路徑點；對每個合并障礙分別建立上、下兩個節點，坐標為圓心±短軸/長軸半徑×調整系數，作為路徑的決策點或拐點，反映是從禁飛區上方還是下方繞過；

然后，建立有向圖模型；選擇串行依次繞過的禁飛區放入一個列表中，則所有可能的串行線就會形成一個多維列表；每一個列表中障礙上下的節點與起點，目標點之間將會形成上下兩個包絡，且相鄰兩障礙上下的節點之間交叉穿行，連接可行路徑后形成每條串行線的所有路徑；刪除重復邊后便形成一個具有節點和有向邊的有向圖；

最后，完成環境建模；完成節點的選取與節點間的連線后，給所有的有向邊加上權重，即w_ij＝d_ij，其中d_ij為節點i與j之間的距離；則形成一個完整的有向圖；

步驟二、遍歷從起點到終點的所有路徑；

基于步驟一產生的有向圖，需要對從起點到終點的所有路徑進行遍歷；使用深度優先原理進行遍歷；

將有向圖用鄰接表結構表示；設給定圖G的初態是所有節點均未曾訪問過；在G中節點S為起點，首先訪問出發點S，并將其標記為已訪問過；然后依次從S出發搜索S的每個鄰接點w；若w未曾訪問過，則以w為新的出發點繼續進行深度優先遍歷，直至圖中所有從起點S到終點E有路徑相通的節點均已被訪問為止；

深度優先遍歷方法：

A.初始化堆棧，將起點設置為已訪問，將其入棧；

B.查看棧頂節點I在圖中，有沒有可以到達、且沒有入棧、且沒有從這個節點I出發訪問過的節點；

C.如果有，則將找到的這個節點入棧；

D.如果沒有則將節點I訪問到下一個節點的集合中每個元素賦值為零，I出棧；

E.當棧頂元素為終點時，設置終點沒有被訪問過，輸出棧中元素，彈出棧頂節點；

F.重復執行B–E，直到棧中元素為空；

通過上述A-F，得到有向圖模型中從起點到終點的所有路徑，即所有繞飛禁飛區的路徑；

步驟三、規劃路徑并設計路徑評價指標；

使用一種最小控制力路徑點跟蹤制導的方法，以最小化路徑點跟蹤的能量消耗為優化目標，基于飛行器的運動學模型對每一條路徑設計最優導引律，得到能夠反映對飛行器機動能力要求的路徑規劃；

首先，建立運動學模型；設飛行器有一個高性能的飛行控制系統，該系統提供飛行器的橫搖、俯仰和偏航穩定性以及速度跟蹤、航向和高度保持功能；設飛行器需要經過N個路徑點；

其中，U表示飛行器，W_i表示第i個路徑點，XOY是慣性坐標系；γ表示航跡角，飛行器速度V；飛行器通過改變其橫向加速度a來改變其運動方向；設飛行器是一個理想的質點，自動駕駛儀沒有延時；r_i和σ_i表示相對距離和視線角，θ_i表示速度超前角，r_i和σ_i初始值由有向邊權重計算得到；基于運動學模型，微分方程表示為：

不喪失一般性，設N個路徑點是按其相應到達時間t_f,i增加排序的，即t_f,it_f,i+1；在飛行器接近路徑點時，到達時間為：

其中r_i(0)表示飛行器與第i個路徑點之間的初始相對距離；

其次，設計路徑評價指標；引入以下二次積分控制力性能指標，并將此性能指標作為路徑的評價指標；

然后，設計導引律；首先考慮t≤t_f,1的情況，定義拉格朗日乘子向量λ＝[λ₁,λ₂,…,λ_N]^T以及脫靶量向量Z＝[Z₁,Z₂,…,Z_N]^T，Z_i,i＝1,2,…,N為脫靶量；

令R∈R^N×N是一個對稱矩陣，使得Rλ＝Z，且：

同理，當tt_f,1時，得到上式的控制指令；其中，精簡化得到：

式(1)至(7)中：γ表示航跡角，V是飛行器速度，a是橫向加速度，r_i和σ_i表示飛行器與第i個路徑點的相對距離和視線角，θ_i表示速度超前角，路徑點個數為N，t為時間，飛行器到達第i個路徑點的時間表示為t_f,i，r_i(0)表示飛行器與第i個路徑點之間的初始相對距離，J表示二次積分控制力性能指標，λ_i為拉格朗日乘子，Z_i為脫靶量，i＝1,2,…,N，中間變量R∈R^N×N是一個對稱矩陣；

使用控制指令a，以最小化能量消耗為目標規劃每個路徑，并通過計算性能指標J得到該路徑的評價指標；

步驟四、制定路徑決策方案；

通過上述步驟，得到每一條繞飛路徑和評價指標；因此，得到所有路徑的排序；

步驟三的路徑規劃只為決策的制定提供評價指標，對于每一條路徑，針對每一個禁飛區，從上方繞過記作0，從下方繞過記為1，就能根據路徑穿過的虛擬路徑點與禁飛區的相對關系轉化為0/1數組；通過上述0/1轉化合并重復方案；設需要繞飛n個禁飛區，合并后的路徑有q條，最終輸出一個q×n維0/1矩陣；

最后，根據需求提供前f個方案，完成決策。