[發明專利]一種基于拉格朗日方程的無人機編隊隊形控制算法

有效

• 申請號: 201710105504.3
• 申請日: 2017-02-26
• 公開（公告）號: CN106873621B
• 公開（公告）日: 2020-02-21
• 發明（設計）人: 陳志明;牛康;李磊
• 申請（專利權）人: 南京航空航天大學
• 主分類號: G05D1/10
• 分類號: G05D1/10
• 代理公司: 江蘇圣典律師事務所 32237
• 代理人: 賀翔;劉輝

1.一種基于拉格朗日方程的無人機編隊隊形控制算法，其特征在于，包括以下步驟：

1)對無人機編隊的運行環境進行簡化和抽象，包括將無人機編隊飛行三維慣性坐標系簡化到二維慣性平面坐標系，將地面標志物抽象為障礙物；

2)對所述無人機編隊進行受力分析，包括求解在當前位置時目標區域對所述無人機編隊中各個無人機產生的引力，以及所述障礙物對所述無人機編隊產生的斥力；

3)求解所述無人機編隊中各個無人機受到的合力，以及整個所述無人機編隊收受到的合力；

4)引入拉格朗日乘子,建立含有隊形約束的無人機編隊約束動力學方程組：

g＝g(q,t)＝0 (10)

其中，q為2n維的編隊無人機位置矩陣；λ為拉格朗日乘子；M為2n*2n編隊無人機的權重矩陣；F(q,v,t,F_Total)為2n維的外力矩陣；為無人機當前速度；t為時間；F_Total為合外力向量，g為隊形約束方程；為隊形約束函數的雅可比矩陣；

5)采用直接消除拉格朗日乘子方法，并結合鮑姆加特穩定性對編隊無人機約束動力學方程組進行求解,得到拉格朗日乘子方程:

其中,α、δ為比例因子；

6)通過判斷所述無人機編隊受到的合力是否為零，來判斷編隊無人機是否運行到目標區域：如果所述無人機編隊受到的合力為零，則輸出所述無人機編隊飛行軌跡，同時輸出誤差曲線，解算結束；如果所述無人機編隊受到的合力不為零，則返回步驟2)。

2.根據權利要求1所述的基于拉格朗日方程的無人機編隊隊形控制算法，其特征在于，所述步驟2)中求解所述引力的方程為：

其中：(ρ_g)＝||ρ_q-ρ_goal||為無人機與目標點空域之間的歐氏距離；ε為正的比例因子；m是大于0的常數；E_qg表示編隊無人機與目標區域之間的單位矢量；v為無人機的速度；K_p與無人機性能相關的比例因子。

3.根據權利要求1所述的基于拉格朗日方程的無人機編隊隊形控制算法，其特征在于，所述步驟2)中求解所述斥力的方程為：

其中，ρ_ob(q)表示無人機于障礙物最小影響范圍邊界的最短距離；K_s為常值系數；D_limit障礙物最小影響范圍；E_qo為編隊無人機與障礙物之間的單位矢量。

4.根據權利要求1所述的基于拉格朗日方程的無人機編隊隊形控制算法，其特征在于，所述步驟3)中求解所述各個無人機的合力的公式為：

5.根據權利要求1所述的基于拉格朗日方程的無人機編隊隊形控制算法，其特征在于，所述步驟3)中求解所述整個無人機編隊的合力的公式為：

6.根據權利要求1所述的基于拉格朗日方程的無人機編隊隊形控制算法，其特征在于，建立含有隊形約束的無人機編隊動力學方程時，首先定義編隊無人機的隊形的為編隊中無人機之間的相對位置保持不變；然后把編隊無人機的隊形作為剛體隊形看待，得到無人機個數與約束方程個數的關系為：

M＝2N-3 (6)

其中M表示約束方程個數，N表示參與編隊的無人機個數；

最后，編隊無人機的約束方程為g＝g(q,t) (7)。

7.根據權利要求1所述的基于拉格朗日方程的無人機編隊隊形控制算法，其特征在于，所述步驟1)中將地面標志物抽象為圓形障礙物。