[發明專利]一種基于路徑優選的飛行器繞多禁飛區軌跡優化方法

有效

• 申請號: 202110301974.3
• 申請日: 2021-03-22
• 公開（公告）號: CN112965532B
• 公開（公告）日: 2022-03-15
• 發明（設計）人: 李惠峰;張源;張冉;侯忻宜
• 申請（專利權）人: 北京航空航天大學
• 主分類號: G05D1/12
• 分類號: G05D1/12
• 代理公司: 北京慧泉知識產權代理有限公司 11232
• 代理人: 王順榮;唐愛華

1.一種基于路徑優選的飛行器繞多禁飛區軌跡優化方法，其特征在于：其具體步驟如下：

步驟一、建立路徑優選問題模型；

首先，基于各禁飛區的相對位置設計虛擬路徑點，對每個禁飛區分別建立上、下兩個節點，坐標為圓心±半徑×調整系數，作為路徑的決策點或拐點，反映是從禁飛區上方還是下方繞過；調整系數的選擇根據實際情況制定；

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

使用深度優先遍歷方法對從起點到終點的所有路徑進行遍歷，得到飛行器可行的所有繞飛路徑；

步驟二、路徑選擇與規劃；

對每條路徑進行路徑點跟蹤制導，基于飛行器的運動模型規劃現有路徑；飛行器的動力學模型如下：

其中，r為地球中心到飛行器重心的徑向距離，θ和分別為對應的經度和緯度，V為飛行器相對于地球的速度，γ為航跡角，ψ為航向角，定義為速度向量在當地水平面的投影與正北方向的夾角，順時針方向旋轉為正，飛行器通過改變其橫向加速度a來改變其運動方向，ω為地球旋轉角速度；

設飛行器需要經過N個路徑點，根據飛行器與第i個路徑點的相對幾何關系，其運動學模型如下：

其中，s_i和ψ_LOSi表示相對大圓距離和視線角，Δψ_i表示航向誤差；

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

通過參考相關文獻，利用零脫靶量轉換以及施瓦茲不等式擴展引理，得到存在N個拉格朗日乘子λ_i,i＝1,2,…,N，使得橫向加速度a最優，且：

其中，拉格朗日乘子向量λ＝[λ₁,λ₂,…,λ_N]^T解析求解，該制導律本質上是一種改進的自適應比例導引律，且普適性強；

使用控制指令a自動規劃每個路徑，并根據凸優化需求選擇路徑評價指標；這樣通過指標排序則得到性能指標最好的一條路徑，并將該路徑作為后續軌跡優化的初始參考軌跡；

步驟三、建立禁飛區規避軌跡優化的凸優化問題；

將非凸非線性模型的運動模型和過程約束在參考軌跡處線性化，將問題轉化為一系列凸子問題，通過內點法進行求解，在多項式時間內獲得最優解，且每個子問題的解能夠確保收斂到極小值；

首先，建立數學模型；在動力學處理的過程中忽略地球自轉，令ψ＝π/2-ψ，將r、V、t、m分別用R₀、m₀進行歸一化操作，R₀為地球半徑，g₀為海平面處重力加速度，m₀為標稱質量，得到歸一化之后的運動方程如下所示：

其中σ為傾側角，L和D分別為升力和阻力，T為推力，其表達式如下所示：

其中ρ(h)為大氣密度，它是海拔高度h的函數；S_r為發動機參考面積，m為飛行器質量；α為攻角，M為馬赫數，s為燃油當量比，C_L和C_D分別為升力系數和阻力系數，C_T為推力系數；其中，ρ、C_L、C_D、聲速均采用多項式擬合；

飛行過程除了狀態量約束、控制量約束、終端約束外，禁飛區不等式約束如下：

其中θ_C、為圓形禁飛區圓心坐標，d為禁飛區半徑；

然后，對該軌跡規劃問題進行凸化；原動力學方程寫為其中u＝[α,σ,s]，在參考軌跡處泰勒展開，得到：

其中，為動力學右端對狀態量的雅可比矩陣，為動力學右端對控制量的雅可比矩陣；

同樣，對禁飛區約束進行泰勒展開，綜合得到：

為防止線性化帶來的人為不可行，在性能指標中加入鄰近項；

然后，將連續的最優控制問題轉化為離散的參數優化問題；取離散點數為K，間隔為Δt，離散點t_k，k＝1,2,…,K；將控制輸入參數化為樣本點之間的線性函數：

結合上述處理，離散的凸規劃子問題表示如下：

其中J為性能指標，為臨近項，k為離散點序列號，k＝1,2,…,K，K為離散點數；為飛行器的狀態量，u＝[α,σ,s]為控制量；A、B、C、Z為系數矩陣；為狀態量初值，u_k＝[α_k,σ_k,s_k]，為經緯度的終端約束，D為狀態量和控制量的定義域；為參考軌跡，θ_C、為圓形禁飛區圓心坐標，d為禁飛區半徑；

步驟四、基于優選路徑設計初始猜想，采用模型補償方法迭代求解凸優化問題；

采用“CVX”算法包求解上述凸優化問題，求解器選為SDPT3，在設計初始猜想時，繞飛策略采用優選路徑得到的最優路徑的策略；在之后的序列迭代過程中，將上一次優化結果作為本次線性化的參考軌跡，以此對線性化模型進行補償，增加外環迭代收斂速度；當求解結果滿足收斂條件ε取0.0001時，停止求解。