本發(fā)明涉及一種無人機(jī)控制方法，具體是一種基于拉格朗日方程的無人機(jī)編隊(duì)隊(duì)形控制算法。

背景技術(shù)：

隨著無人機(jī)技術(shù)的快速發(fā)展，無人機(jī)在軍事、災(zāi)害救援等方面都起到了不可替代的作用，但是隨著任務(wù)環(huán)境越來越復(fù)雜，單無人機(jī)執(zhí)行任務(wù)的缺點(diǎn)也迅速體現(xiàn)出來。比如：采用單一無人機(jī)執(zhí)行任務(wù)時偵測范圍較窄；在復(fù)雜地形環(huán)境下，采用單機(jī)完成任務(wù)相對比較困難，甚至難以完成等等。反之采用多機(jī)協(xié)同編隊(duì)飛行則能夠很好的解決單機(jī)暴露的很多問題，同時可以提高系統(tǒng)的魯棒性和可靠性。因此，無人機(jī)編隊(duì)的概念一經(jīng)提出就迅速成為當(dāng)今國內(nèi)外無人機(jī)研究的一個重要熱點(diǎn)問題。因此對無人機(jī)編隊(duì)的研究具有重要的現(xiàn)實(shí)意義和研究價值，特別是對無人機(jī)編隊(duì)的路徑規(guī)劃和隊(duì)形控制等方面的研究。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明要解決的技術(shù)問題是提供一種基于拉格朗日方程的無人機(jī)編隊(duì)隊(duì)形控制算法，該方法能夠很好地解決編隊(duì)無人機(jī)的路徑規(guī)劃問題和飛行過程中隊(duì)形的保持問題。

為實(shí)現(xiàn)上述發(fā)明目的，本發(fā)明的一種基于拉格朗日方程的無人機(jī)編隊(duì)隊(duì)形控制算法，在編隊(duì)無人機(jī)路徑規(guī)劃方面采用改進(jìn)的人工勢能場；在隊(duì)形保持方面將編隊(duì)無人機(jī)運(yùn)動看作一個約束動力學(xué)系統(tǒng)。在建立約束動力學(xué)系統(tǒng)時，首先利用多個無人機(jī)相對于慣性坐標(biāo)系的空間幾何關(guān)系，根據(jù)需要的編隊(duì)形狀建立空間隊(duì)形約束方程；其次然后引入拉格朗日乘子建立含有隊(duì)形約束的編隊(duì)無人機(jī)的約束動力學(xué)方程組，最后結(jié)合鮑姆加特穩(wěn)定性采用直接消除拉格朗日乘子方法并求解拉格朗日乘子。具體如下：

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

1)對無人機(jī)編隊(duì)的運(yùn)行環(huán)境進(jìn)行簡化和抽象，包括將無人機(jī)編隊(duì)飛行三維慣性坐標(biāo)系簡化到二維慣性平面坐標(biāo)系，將地面標(biāo)志物抽象為障礙物；為了盡可能的避免局部極小值的存在以及陷阱區(qū)域的存在，該方法將障礙物抽象為圓形。

2)對所述無人機(jī)編隊(duì)進(jìn)行受力分析，包括求解在當(dāng)前位置時目標(biāo)區(qū)域?qū)λ鰺o人機(jī)編隊(duì)中各個無人機(jī)產(chǎn)生的引力，以及所述障礙物對所述無人機(jī)編隊(duì)產(chǎn)生的斥力；

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

4)引入朗格朗日乘子，建立含有隊(duì)形約束的無人機(jī)編隊(duì)的運(yùn)動學(xué)方程。首先將編隊(duì)無人機(jī)在整個勢場中的運(yùn)動學(xué)作為正向動力學(xué)問題，其中障礙物產(chǎn)生的斥力與目標(biāo)空域產(chǎn)生的引力的合力作為為輸入。同時將編隊(duì)無人機(jī)的運(yùn)動作為受隊(duì)形約束的約束動力學(xué)系統(tǒng)，系統(tǒng)的輸入為編隊(duì)無人機(jī)受到的合力，系統(tǒng)的輸出為編隊(duì)無人機(jī)的位置軌跡。

最后，含有隊(duì)形約束的編隊(duì)無人機(jī)的運(yùn)動學(xué)方程組為：

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

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

5)采用直接消除拉格朗日乘子方法，并結(jié)合鮑姆加特穩(wěn)定性求解編隊(duì)無人機(jī)的運(yùn)動學(xué)方程組中的朗格朗日乘子。首先將編隊(duì)無人機(jī)的運(yùn)動學(xué)方程利用狀態(tài)空間表示如下：

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

其中，為隊(duì)形約束函數(shù)的雅可比矩陣；λ為拉格朗日乘子。

然后，由方程(12)得

利用鮑姆加特穩(wěn)定性得：

結(jié)合方程(9)得：

因此，可得拉格朗日乘子的表達(dá)式為：

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

6)通過判斷所述無人機(jī)編隊(duì)受到的合力是否為零，來判斷編隊(duì)無人機(jī)是否運(yùn)行到目標(biāo)區(qū)域：如果所述無人機(jī)編隊(duì)受到的合力為零，則輸出所述無人機(jī)編隊(duì)飛行軌跡，同時輸出誤差曲線，解算結(jié)束；如果所述無人機(jī)編隊(duì)受到的合力不為零，則返回步驟2)。

進(jìn)一步的，所述步驟2)中求解所述引力的方程為：

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

進(jìn)一步的，所述步驟2)中求解所述斥力的方程為：

進(jìn)一步的，所述步驟3)中求解所述各個無人機(jī)的合力的公式為：

進(jìn)一步的，所述步驟3)中求解所述整個無人機(jī)編隊(duì)的合力的公式為：

進(jìn)一步的，建立含有隊(duì)形約束的無人機(jī)編隊(duì)動力學(xué)方程時，首先定義編隊(duì)無人機(jī)的隊(duì)形的為編隊(duì)中無人機(jī)之間的相對位置保持不變；然后把編隊(duì)無人機(jī)的隊(duì)形作為剛體隊(duì)形看待，得到無人機(jī)個數(shù)與約束方程個數(shù)的關(guān)系為：

M＝2N-3 (6)

其中M表示約束方程個數(shù)，N表示參與編隊(duì)的無人機(jī)個數(shù)；

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

進(jìn)一步的，所述步驟1)中將地面標(biāo)志物抽象為圓形障礙物。

本發(fā)明的一種基于拉格朗日方程的無人機(jī)編隊(duì)隊(duì)形控制算法，該控制方法基于改進(jìn)人工勢能場和拉格朗日方程，主要應(yīng)用于編隊(duì)無人機(jī)的路徑規(guī)劃和編隊(duì)隊(duì)形保持。該方法能夠很好的解決編隊(duì)無人機(jī)的路徑規(guī)劃問題和飛行過程中對性的保持問題。仿真實(shí)驗(yàn)表明在解決無人機(jī)編隊(duì)飛行時隊(duì)形保持和變換該方法具有較強(qiáng)的靈活性和有效性。

附圖說明

圖1為本發(fā)明無人機(jī)編隊(duì)的典型運(yùn)行環(huán)境進(jìn)行簡化和抽象示意圖；

圖2為本發(fā)明無人機(jī)編隊(duì)在當(dāng)前位置時的受力分析圖；

圖3為以三架無人機(jī)菱形編隊(duì)示意圖；；

圖4為本發(fā)明基于拉格朗日方程的無人機(jī)編隊(duì)隊(duì)形控制算法流程圖。

具體實(shí)施方式

下面結(jié)合附圖，對本發(fā)明提出的一種基于拉格朗日方程的無人機(jī)編隊(duì)隊(duì)形控制算法進(jìn)行詳細(xì)說明。

如圖1所示，本發(fā)明公開的一種基于改進(jìn)人工勢能場和拉格朗日方程的無人機(jī)編隊(duì)控制算法典型的運(yùn)行環(huán)境，包括地面雷達(dá)1、防空導(dǎo)彈2、地面軍事基地3以及障礙物4、編隊(duì)無人機(jī)5、目標(biāo)區(qū)域6等。其中，R1表示抽象的障礙物半徑，R2表示障礙物的影響范圍。這里要求編隊(duì)無人機(jī)能夠以編隊(duì)的形式繞開所有的障礙物順利的到達(dá)目標(biāo)區(qū)域。在進(jìn)行簡化的時候本發(fā)明將編隊(duì)無人的運(yùn)動簡化到抽象的平面內(nèi)運(yùn)動，并要求編隊(duì)無人機(jī)繞開所有的障礙物順利到達(dá)目標(biāo)區(qū)域。

在進(jìn)行簡化和抽象時具體步驟如下：

(1)建立三維慣性坐標(biāo)系S1；

(2)建立二維慣性平面坐標(biāo)系S2；

(3)對地面雷達(dá)1、防空導(dǎo)彈2、地面軍事基地3以及高山4等抽象為圓形障礙物，且半徑大小R1(黑色區(qū)域半徑)為障礙物的中心到其邊界的最大距離，并建立障礙物的影響范圍R2(外圈半徑)；

如圖2所示：對運(yùn)行環(huán)境進(jìn)行抽象和簡化完成之后要對編隊(duì)無人機(jī)進(jìn)行受力分析，具體步驟如下：

(1)編隊(duì)無人機(jī)在運(yùn)行環(huán)境中目標(biāo)區(qū)域始終對編隊(duì)無人機(jī)產(chǎn)生引力作用，只有編隊(duì)無人機(jī)進(jìn)入障礙物的影響范圍時編隊(duì)無人機(jī)收到相應(yīng)的障礙物產(chǎn)生的斥力作用。

將經(jīng)典引力勢函數(shù)改進(jìn)為：

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

然后設(shè)置斥力函數(shù)的改進(jìn)條件為a:所有的障礙物產(chǎn)生的斥力函數(shù)必須連續(xù)；b:所有的障礙都都有一個大于障礙物半徑的最小影響范圍。將斥力函數(shù)改進(jìn)為如下函數(shù)：

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

得到每個無人所受的引力為：

每個無人所受的斥力為：

(2)求解編隊(duì)中單個無人機(jī)的合力。對于單個無人機(jī)來說，所受的合力等于目標(biāo)區(qū)域產(chǎn)生的引力和障礙物產(chǎn)生斥力的合力。

(3)求解整個編隊(duì)無人機(jī)的合力。對于整個編隊(duì)無人機(jī)來說所受到的合力等于對編隊(duì)中所有無人機(jī)所受的合力進(jìn)行求和。

(4)如圖2所示，其中F_Total＝F，另外，編隊(duì)無人機(jī)在沒有接近R2時，因?yàn)镕_rep＝0，所以F_Total＝F_attr＝F。

如圖3所示：為對三架無人機(jī)進(jìn)行菱形編隊(duì)。本發(fā)明以三架無人機(jī)編隊(duì)來說明編隊(duì)約束方程與編隊(duì)無人機(jī)數(shù)量之間的關(guān)系，具體步驟如下：

(1)首先，本發(fā)明定義隊(duì)形的概念為編隊(duì)中無人機(jī)之間的相對位置保持不變。

(2)然后，將編隊(duì)無人機(jī)飛行時的隊(duì)形作為剛體隊(duì)形看待得到編隊(duì)無人機(jī)個數(shù)與約束方程的關(guān)系為：

M＝2N-3 (6)

其中，M表示約束方程個數(shù)，N表示參與編隊(duì)的無人機(jī)個數(shù)。

(3)最后得到約束方程為如下方程：

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

是一個與無人機(jī)位置相關(guān)的函數(shù)，根據(jù)要求決定是否和時間相關(guān)；如果要求整個編隊(duì)無人機(jī)飛行過程中要求隊(duì)形不改變時，約束方程與時間不相關(guān)，否則約束方程與時間相關(guān)。

(4)以三架無人機(jī)為例進(jìn)行編隊(duì)時約束方程如下：

其中，D_ij為無人機(jī)i，j之間的期望距離。

如圖4所示：為求解該算法的程序流程圖，具體步驟如下：

(1)首先給定編隊(duì)中各個無人機(jī)的初始位置，初始速度；

(2)利用公式(2),(3)分別求出在當(dāng)前位置時目標(biāo)區(qū)域?qū)庩?duì)中各個無人機(jī)產(chǎn)生的引力，以及障礙物對編隊(duì)無人機(jī)產(chǎn)生的斥力；

(3)利用公式(4)求解編隊(duì)中各個無人機(jī)受到的合力，同時利用公式(27)求解整個無人機(jī)編隊(duì)受到的合力；

(4)利用MATLAB每0.1步長求解出編隊(duì)中各個無人機(jī)的下一個位置點(diǎn)、方向和角度；

(5)通過判斷無人機(jī)編隊(duì)受到的合力是否為零，來判斷編隊(duì)無人機(jī)是否運(yùn)行到目標(biāo)區(qū)域。如果是，則輸出編隊(duì)無人機(jī)飛行軌跡，同時輸出誤差曲線，程序跳轉(zhuǎn)至步驟6)，否則程序跳轉(zhuǎn)到步驟2)；

(6)解算結(jié)束。

基于對本發(fā)明優(yōu)選實(shí)施方式的描述，應(yīng)該清楚，由所附的權(quán)利要求書所限定的本發(fā)明并不僅僅局限于上面說明書中所闡述的特定細(xì)節(jié)，未脫離本發(fā)明宗旨或范圍的對本發(fā)明的許多顯而易見的改變同樣可能達(dá)到本發(fā)明的目的。