本發(fā)明涉及無(wú)人機(jī)編隊(duì)路徑規(guī)劃和無(wú)人機(jī)編隊(duì)隊(duì)形控制領(lǐng)域，具體是一種基于三維全局人工勢(shì)函數(shù)的無(wú)人機(jī)編隊(duì)路徑規(guī)劃算法。

背景技術(shù)：

當(dāng)前隨著航空航天技術(shù)的不斷發(fā)展，無(wú)人機(jī)無(wú)論在軍事偵查、地面打擊等軍事領(lǐng)域還是在森林火災(zāi)、災(zāi)害救援、電力巡線等民用領(lǐng)域都起到了舉足輕重的作用。盡管如此，我們更要看到單架無(wú)人機(jī)在執(zhí)行任務(wù)時(shí)由于受到個(gè)體的智能化程度和可靠性等諸多的因素的制約，造成在執(zhí)行相關(guān)任務(wù)時(shí)仍然存在很多問(wèn)題甚至無(wú)法完成指定任務(wù)。例如：?jiǎn)螣o(wú)人機(jī)由于受到自身傳感器數(shù)量或者傳感器角度限制等因素，不能夠完整的獲取目標(biāo)區(qū)域環(huán)境的整體環(huán)境信息；在進(jìn)行對(duì)地面目標(biāo)進(jìn)行軍事打擊時(shí)，由于單機(jī)作戰(zhàn)范圍較窄、載彈量有限造成執(zhí)行任務(wù)效率較低等問(wèn)題。

人工勢(shì)場(chǎng)法(artificialpotentialfield)自上世紀(jì)八十年代khatib’s提出以來(lái)，憑借建模容易、數(shù)學(xué)計(jì)算簡(jiǎn)單等優(yōu)點(diǎn)迅速引起了廣大研究人員的青睞，并且在機(jī)器人領(lǐng)域得到了廣泛的應(yīng)用。然而經(jīng)過(guò)幾十年的發(fā)展人工勢(shì)場(chǎng)法依然局限于對(duì)經(jīng)典二維人工勢(shì)場(chǎng)法的改進(jìn)和使用方面。

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

本發(fā)明要解決的技術(shù)問(wèn)題是提供一種基于三維全局人工勢(shì)函數(shù)的無(wú)人機(jī)編隊(duì)路徑規(guī)劃算法，該算法能夠突破二維經(jīng)典人工勢(shì)函數(shù)的局限性和不足，解決無(wú)人機(jī)編隊(duì)在三維空間環(huán)境下的隊(duì)形保持、路徑規(guī)劃以及三維避障等問(wèn)題。

為了實(shí)現(xiàn)上述發(fā)明目的，本發(fā)明采用以下技術(shù)方案：

首先建立無(wú)人機(jī)編隊(duì)的三維環(huán)境、自由飛行空間和三維空間內(nèi)障礙物數(shù)學(xué)模型，得到三維空間全局人工勢(shì)函數(shù)；然后根據(jù)三維空間全局人工勢(shì)函數(shù)對(duì)無(wú)人機(jī)編隊(duì)進(jìn)行受力分析，得出無(wú)人機(jī)編隊(duì)在三維空間內(nèi)的所受力的大小和方向；最后建立三維空間內(nèi)無(wú)人機(jī)編隊(duì)期望隊(duì)形的幾何方程，結(jié)合隊(duì)形幾何方程引入拉格朗日乘子建立含有隊(duì)形約束的無(wú)人機(jī)編隊(duì)約束動(dòng)力學(xué)方程；最后求解約束動(dòng)力學(xué)方程。

進(jìn)一步的，所述建立無(wú)人機(jī)編隊(duì)的三維環(huán)境、自由飛行空間和三維空間內(nèi)障礙物的數(shù)學(xué)模型過(guò)程包括：建立三維空間簡(jiǎn)化和抽象的三維慣性坐標(biāo)系og；其中xg軸在水平面內(nèi)指向某一方向，zg軸垂直于地面指向正上方，yg軸在水平面內(nèi)垂直于xg軸，方向滿足右手定則；對(duì)三維空間內(nèi)障礙物進(jìn)行抽象；

無(wú)人機(jī)編隊(duì)的三維環(huán)境、自由飛行空間w和三維空間內(nèi)所有障礙物模型統(tǒng)一函數(shù)γ分別為：

其中：γi表示三維空間內(nèi)障礙物模型i的三維幾何方程；i＝0時(shí)γi表示三維空間的邊界幾何方程；i＝1,2,3…n-1表示三維空間內(nèi)的(n-1)個(gè)障礙物；其中，γ＜0表示點(diǎn)位于障礙物內(nèi)部或者不在三維自由空間內(nèi)；γ≥0表示點(diǎn)位于除障礙物的三維自由空間內(nèi)；表示三維立體模型的幾何方程。

進(jìn)一步的，所述根據(jù)建立的三維環(huán)境和三維空間內(nèi)障礙物數(shù)學(xué)模型得到三維空間全局人工勢(shì)函數(shù)的過(guò)程包括：

建立三維空間內(nèi)無(wú)人機(jī)編隊(duì)與目標(biāo)區(qū)域的之間的函數(shù)：

其中，qi表示在無(wú)人機(jī)i的空間位置，qi＝[xi,yi,zi]^t；qg表示目標(biāo)區(qū)域位置,qg＝[xg,yg,zg]^t；||qi-qg||表示無(wú)人機(jī)i與目標(biāo)區(qū)域之間的歐式距離；k為比例因子(k>0)；vi表示無(wú)人機(jī)i速度因子；

定義實(shí)值函數(shù)運(yùn)算關(guān)系：

f1*f2＝f1(f2(x))(12)

其中，f1(x)、f2(x)均為實(shí)值映射函數(shù)；

建立同胚映射函數(shù)：定義將實(shí)值[0,+∞]映射到[0,μ]的微分同胚映射函數(shù)為：

其中，λ為映射控制參數(shù)；μ為映射參數(shù)。

建立銳化函數(shù)：定義銳化函數(shù)σk為關(guān)于x的k次方根函數(shù)：

σk(x)＝x^1/k(11)

其中，在算法中銳化函數(shù)主要應(yīng)用在降低自變量幅角，使降幅角后的函數(shù)滿足莫爾斯函數(shù)條件；

建立三維全局人工勢(shì)函數(shù)：根據(jù)上述步驟，定義三維全局人工勢(shì)函數(shù)的表達(dá)式為：

其中，n表示空間內(nèi)障礙物的個(gè)數(shù)(n＝0,1，2…n-1)；q為立體空間內(nèi)三維坐標(biāo)q=[x,y,z]^t。

上述得到三維空間全局人工勢(shì)函數(shù)的過(guò)程中，步驟可以調(diào)換順序。

進(jìn)一步的，對(duì)無(wú)人機(jī)編隊(duì)進(jìn)行受力分析的過(guò)程為：在對(duì)無(wú)人機(jī)編隊(duì)進(jìn)行受力分析時(shí)，首先對(duì)無(wú)人機(jī)編隊(duì)進(jìn)行總體受力分析，然后對(duì)單無(wú)人機(jī)在三維各個(gè)方向進(jìn)行受力分析；無(wú)人機(jī)編隊(duì)總體受力為ftotal，單無(wú)人機(jī)在三維各個(gè)方向的受力為

其中，utotal表示三維全局勢(shì)函數(shù)；表示無(wú)人機(jī)i分別在三維x,y,z方向上的力。

進(jìn)一步的，所述建立三維空間內(nèi)無(wú)人機(jī)期望隊(duì)形幾何方程的過(guò)程為：算法定義無(wú)人機(jī)在三維空間的運(yùn)動(dòng)視為剛體運(yùn)動(dòng)，并定義三維空間內(nèi)無(wú)人機(jī)編隊(duì)的隊(duì)形為無(wú)人機(jī)之間的相對(duì)歐氏距離不變；無(wú)人機(jī)編隊(duì)個(gè)數(shù)與約束方程個(gè)數(shù)的關(guān)系為：

m＝2n-3(19)

其中m表示約束方程個(gè)數(shù)，n表示參與編隊(duì)的無(wú)人機(jī)個(gè)數(shù)。

進(jìn)一步的，所述建立含有隊(duì)形約束的無(wú)人機(jī)編隊(duì)約束動(dòng)力學(xué)方程的過(guò)程包括：

對(duì)無(wú)人機(jī)編隊(duì)運(yùn)動(dòng)學(xué)建模：對(duì)無(wú)人機(jī)編隊(duì)進(jìn)行運(yùn)動(dòng)學(xué)建模時(shí)，首先將無(wú)人機(jī)編隊(duì)運(yùn)動(dòng)視為無(wú)人機(jī)在受三維人工勢(shì)函數(shù)產(chǎn)生的虛擬力的作用下向目標(biāo)點(diǎn)飛行的正向動(dòng)力學(xué)問(wèn)題；同時(shí)將無(wú)人機(jī)編隊(duì)的隊(duì)形視為彈性隊(duì)形；因此，通過(guò)對(duì)無(wú)人機(jī)編隊(duì)的受力分析，結(jié)合無(wú)人機(jī)編隊(duì)期望隊(duì)形建立無(wú)人機(jī)編隊(duì)的動(dòng)力學(xué)方程如下：

ρ＝ρ(x,y,z,t)＝0(22)

其中，q＝[x,y,z]^t為無(wú)人機(jī)三維空間位置矩陣；λ為拉格朗日乘子；為n維的合外力矩陣；l為比例因子；m無(wú)人機(jī)編隊(duì)的權(quán)重矩陣；為無(wú)人機(jī)在三維空間內(nèi)所受合外力；ρ(x,y,z,t)為隊(duì)形約束方程；為隊(duì)形約束方程的雅可比矩陣；

建立無(wú)人機(jī)編隊(duì)罰函數(shù)數(shù)學(xué)模型；建立罰函數(shù)數(shù)學(xué)模型時(shí)，合并隊(duì)形約束方程作為一個(gè)輔助動(dòng)力學(xué)系統(tǒng)并通過(guò)罰函數(shù)因子予以懲罰；無(wú)人機(jī)編隊(duì)罰函數(shù)模型為：

其中，μ為罰函數(shù)懲罰因子(μ→+∞)；μα(x)為懲罰項(xiàng)；p為正整數(shù)，通常取p＝2。

罰函數(shù)的可行域?yàn)椋?/p>

d＝{x,y,z|ρ(x,y,z,t)＝0}(25)

因此懲罰項(xiàng)μα(x)為：

求解無(wú)人機(jī)編隊(duì)運(yùn)動(dòng)學(xué)罰函數(shù)模型：在對(duì)無(wú)人機(jī)編隊(duì)約束動(dòng)力學(xué)方程進(jìn)行求解時(shí)，首先利用線性或者非線性的虛擬彈簧(阻尼器)代替無(wú)人機(jī)編隊(duì)的隊(duì)形完整性約束；此時(shí)，λ為一個(gè)基于違反隊(duì)形約束程度的虛擬彈簧(阻尼器)產(chǎn)生的力，并且假定此時(shí)彈簧的剛度和阻尼系數(shù)恒定。因此在滿足罰函數(shù)可行域d的情況下，λ的表達(dá)式為：

其中，ls為彈性因子為n*n維的對(duì)角矩陣；ld為阻尼因子為n*n維的對(duì)角矩陣；ρ為隊(duì)形約束方程。

結(jié)合公式(20)，(26)可得罰函數(shù)下無(wú)人機(jī)編隊(duì)的約束動(dòng)力學(xué)方程為：

無(wú)人機(jī)編隊(duì)的約束動(dòng)力學(xué)方程轉(zhuǎn)換到狀態(tài)空間下為：

本發(fā)明的一種基于三維全局人工勢(shì)函數(shù)的無(wú)人機(jī)編隊(duì)路徑規(guī)劃算法，與二維經(jīng)典勢(shì)函數(shù)相比較，不僅可以解決經(jīng)典人工勢(shì)函數(shù)的二維空間局限問(wèn)題，而且可以解決經(jīng)典勢(shì)函數(shù)存在局部極小值等問(wèn)題。通過(guò)仿真實(shí)驗(yàn)表明，本發(fā)明的算法能夠很好的解決無(wú)人機(jī)編隊(duì)在三維空間下的路徑規(guī)劃和隊(duì)形保持問(wèn)題。在解決二維經(jīng)典人工勢(shì)函數(shù)的局限性和存在的問(wèn)題時(shí)，有很好的靈活性和有效性。

附圖說(shuō)明

圖1為本發(fā)明無(wú)人機(jī)編隊(duì)三維典型運(yùn)行環(huán)境示意圖；

圖2為本發(fā)明三維空間典型運(yùn)行環(huán)境的簡(jiǎn)化和抽象結(jié)果示意圖；

圖3為三維人工勢(shì)函數(shù)模型建模流程圖；

圖4為以6架無(wú)人機(jī)三角形編隊(duì)為例的彈性隊(duì)形結(jié)構(gòu)示意圖；

圖5為為求解無(wú)人機(jī)編隊(duì)約束動(dòng)力學(xué)方程流程圖。

具體實(shí)施方式

下面結(jié)合附圖，對(duì)本發(fā)明提出的一種基于三維全局人工勢(shì)函數(shù)的無(wú)人機(jī)編隊(duì)路徑規(guī)劃算法進(jìn)行詳細(xì)說(shuō)明。

本發(fā)明的基于三維全局人工勢(shì)函數(shù)的無(wú)人機(jī)編隊(duì)路徑規(guī)劃算法主要包括以下步驟：

首先建立無(wú)人機(jī)編隊(duì)的三維環(huán)境和三維空間內(nèi)障礙物數(shù)學(xué)模型，得到三維空間全局人工勢(shì)函數(shù)；然后根據(jù)三維空間全局人工勢(shì)函數(shù)對(duì)無(wú)人機(jī)編隊(duì)進(jìn)行受力分析，得出無(wú)人機(jī)編隊(duì)在三維空間內(nèi)的所受力的大小和方向；最后建立三維空間內(nèi)無(wú)人機(jī)編隊(duì)期望隊(duì)形的幾何方程，結(jié)合隊(duì)形幾何方程引入拉格朗日乘子建立含有隊(duì)形約束的無(wú)人機(jī)編隊(duì)約束動(dòng)力學(xué)方程；最后求解約束動(dòng)力學(xué)方程。

具體來(lái)講，如圖1所示，為本發(fā)明公開(kāi)的一種基于三維全局人工勢(shì)函數(shù)的無(wú)人機(jī)編隊(duì)路徑規(guī)劃算法的三維典型運(yùn)行環(huán)境示意圖。在三維空間環(huán)境中主要包括防空導(dǎo)彈基地1，地面軍事基地2、空中移動(dòng)目標(biāo)3、無(wú)人機(jī)編隊(duì)4、目標(biāo)空域5、山地6、地面雷達(dá)探測(cè)基地7等。在進(jìn)行無(wú)人機(jī)編隊(duì)路徑規(guī)劃時(shí)，要求無(wú)人機(jī)編隊(duì)繞開(kāi)所有的地面防空設(shè)施和障礙，同時(shí)保持完整的預(yù)定隊(duì)形順利到達(dá)目標(biāo)空域

如圖2所示，為無(wú)人機(jī)編隊(duì)三維典型運(yùn)行環(huán)境的簡(jiǎn)化和抽象結(jié)果示意圖。在對(duì)三維空間環(huán)境進(jìn)行抽象和簡(jiǎn)化時(shí)，首先建立無(wú)人機(jī)編隊(duì)飛行環(huán)境的三維空間慣性坐標(biāo)系，同時(shí)根據(jù)地面防空設(shè)施的特點(diǎn)，將它們抽象為不同的三維模型，并建立相應(yīng)的三維數(shù)學(xué)障礙物模型。

具體抽象和簡(jiǎn)化步驟如下：

11)建立無(wú)人機(jī)編隊(duì)三維典型運(yùn)行環(huán)境慣性坐標(biāo)系og；

12)建立三維空間簡(jiǎn)化和抽象的三維慣性坐標(biāo)系og；其中xg軸在水平面內(nèi)指向某一方向，zg軸垂直于地面指向正上方，yg軸在水平面內(nèi)垂直于xg軸，方向滿足右手定則；

13)根據(jù)地面和空中不同物體的特點(diǎn)，將它們分別進(jìn)行簡(jiǎn)化和抽象。地面防空導(dǎo)彈1、地面軍事基地2抽象為半球形；空中移動(dòng)目標(biāo)3抽象為球形；目標(biāo)空域5抽象為圓形；山地6、地面雷達(dá)探測(cè)基地7抽象為圓錐形。

詳細(xì)的抽象和簡(jiǎn)化步驟在下面的描述中會(huì)更加詳細(xì)。

如圖3所示，為無(wú)人機(jī)編隊(duì)三維人工勢(shì)函數(shù)模型建模流程圖。具體步驟如下：

201)首先建立三維空間的邊界數(shù)學(xué)模型和三維空間內(nèi)障礙物數(shù)學(xué)模型γi，該算法定義三維空間環(huán)境的數(shù)學(xué)模型為三維立體模型的幾何方程

公式(1)中三維空間邊界為球形三維空間邊界。其中：γi表示三維空間內(nèi)障礙物模型i的三維幾何方程；i＝0時(shí)γi表示三維空間的邊界幾何方程；i＝1,2,3…n-1表示三維空間內(nèi)的(n-1)個(gè)障礙物；其中，γ＜0表示點(diǎn)位于障礙物內(nèi)部或者不在三維自由空間內(nèi)；γ≥0表示點(diǎn)位于除障礙物的三維自由空間內(nèi)。

以橢球型障礙物數(shù)學(xué)模型為例時(shí)，可以得到如下障礙物數(shù)學(xué)模型：

其中，表示是n維歐式空間的子集；q表示三維空間內(nèi)的坐標(biāo)q＝[x,y,z]^t；q0表示三維球形空間的中心坐標(biāo)，如果以原點(diǎn)為中心則q0＝[0,0,0]^t；r0表示三維球形空間的半徑，a,b分別表示x,y方向的半周長(zhǎng)，c＝a或c＝b表示分別繞y軸和x軸旋轉(zhuǎn)得到的橢球，dx,dy,dz表示橢球在x,y,z方向上的偏移量。

此時(shí)，

半球型障礙物數(shù)學(xué)模型為例時(shí)，障礙物的方程為：

其中，rx,ry表示半球中心，r表示半球半徑。

球型障礙物數(shù)學(xué)模型為例時(shí)，障礙物的方程為：

(x-ax)²+(y-ay)²+(z-az)²-r²≤0(7)

其中，ax,ay,az表示球形中心，r表示球體半徑。

圓形障礙物的數(shù)學(xué)模型為：

其中，atx,aty表示目標(biāo)空域的水平面坐標(biāo)，rt表示目標(biāo)空域半徑，zt目標(biāo)空域半徑。

圓錐形障礙物數(shù)學(xué)模型為例時(shí)，障礙物的方程為：

其中，kc為比例因子，acx,acy為圓錐的平面坐標(biāo)，h圓錐的高度。

202)建立同胚映射函數(shù)。其目的主要是將實(shí)值函數(shù)[0,+∞]映射到[0,μ]實(shí)值函數(shù)，同胚映射函數(shù)如下：

其中，λ為映射控制參數(shù)；μ為映射參數(shù)。

203)建立相應(yīng)的銳化函數(shù)。其目的主要是為了降低實(shí)值函數(shù)的自變量幅角，使降幅角后的實(shí)值函數(shù)滿足莫爾斯函數(shù)條件。因此定義銳化函數(shù)σk為關(guān)于x的k次方根函數(shù)：

σk(x)＝x^1/k(11)

204)重新定義函數(shù)復(fù)合運(yùn)算。該算法定義實(shí)值映射函數(shù)的運(yùn)算關(guān)系為：

f1*f2＝f1(f2(x))(12)

其中，f1(x)、f2(x)均為實(shí)值映射函數(shù)。

205)建立目標(biāo)距離函數(shù)。該算法定義三維空間內(nèi)無(wú)人機(jī)編隊(duì)與目標(biāo)區(qū)域的之間的目標(biāo)距離函數(shù)為：

其中，qi表示在無(wú)人機(jī)i的空間位置，qi＝[xi,yi,zi]^t；qg表示目標(biāo)區(qū)域位置,qg＝[xg,yg,zg]^t；||qi-qg||表示無(wú)人機(jī)i與目標(biāo)區(qū)域之間的歐式距離；k為比例因子(k>0)；vi表示無(wú)人機(jī)i速度因子。

206)判斷目標(biāo)距離函數(shù)是否滿足莫爾斯函數(shù)條件。在建立三維全局勢(shì)函數(shù)之前，首先要判斷目標(biāo)距離函數(shù)fk是否滿足莫爾斯函數(shù)條件，如果不滿足則將目標(biāo)距離函數(shù)fk帶入銳化函數(shù)，按照公式(12)的運(yùn)算規(guī)則計(jì)算，直到fk滿足莫爾斯函數(shù)條件。

例如：當(dāng)公式(13)中的k＝2時(shí)，目標(biāo)距離函數(shù)不滿足莫爾斯函數(shù)條件，因?yàn)槠湓谠c(diǎn)時(shí)海森矩陣為零矩陣。然而利用銳化函數(shù)σk可得：滿足莫爾斯函數(shù)條件。

207)建立三維全局人工勢(shì)函數(shù)。由公式(1)-(13)定義三維全局人工勢(shì)函數(shù)的表達(dá)式ψκ(q)為：

其中，n表示空間內(nèi)障礙物的個(gè)數(shù)(n＝0,1,2…n-1)；q為立體空間內(nèi)三維坐標(biāo)q＝[x,y,z]^t。

208)對(duì)無(wú)人機(jī)編隊(duì)進(jìn)行受力分析。在對(duì)無(wú)人機(jī)編隊(duì)進(jìn)行受力分析時(shí)，首先對(duì)無(wú)人機(jī)編隊(duì)進(jìn)行總體受力分析，然后對(duì)單無(wú)人機(jī)在三維各個(gè)方向進(jìn)行受力分析；無(wú)人機(jī)編隊(duì)總體受力為ftotal，單無(wú)人機(jī)在三維各個(gè)方向的受力為

其中，utotal表示三維全局勢(shì)函數(shù)；表示無(wú)人機(jī)i分別在三維x,y,z方向上的力。

209)建立三維空間內(nèi)無(wú)人機(jī)期望隊(duì)形幾何方程：算法定義無(wú)人機(jī)在三維空間的運(yùn)動(dòng)視為剛體運(yùn)動(dòng)，并定義三維空間內(nèi)無(wú)人機(jī)編隊(duì)的隊(duì)形為無(wú)人機(jī)之間的相對(duì)歐氏距離不變；無(wú)人機(jī)編隊(duì)個(gè)數(shù)與約束方程個(gè)數(shù)的關(guān)系為：

m＝2n-3(19)

其中m表示約束方程個(gè)數(shù)，n表示參與編隊(duì)的無(wú)人機(jī)個(gè)數(shù)。

進(jìn)一步的，所述建立含有隊(duì)形約束的無(wú)人機(jī)編隊(duì)約束動(dòng)力學(xué)方程的過(guò)程包括：

210)對(duì)無(wú)人機(jī)編隊(duì)運(yùn)動(dòng)學(xué)建模：對(duì)無(wú)人機(jī)編隊(duì)進(jìn)行運(yùn)動(dòng)學(xué)建模時(shí)，首先將無(wú)人機(jī)編隊(duì)運(yùn)動(dòng)視為無(wú)人機(jī)在受三維人工勢(shì)函數(shù)產(chǎn)生的虛擬力的作用下向目標(biāo)點(diǎn)飛行的正向動(dòng)力學(xué)問(wèn)題；同時(shí)將無(wú)人機(jī)編隊(duì)的隊(duì)形視為彈性隊(duì)形；因此，通過(guò)對(duì)無(wú)人機(jī)編隊(duì)的受力分析，結(jié)合無(wú)人機(jī)編隊(duì)期望隊(duì)形建立無(wú)人機(jī)編隊(duì)的動(dòng)力學(xué)方程如下：

ρ＝ρ(x,y,z,t)＝0(22)

其中，q＝[x,y,z]^t為無(wú)人機(jī)三維空間位置矩陣；λ為拉格朗日乘子；為n維的合外力矩陣；l為比例因子；m無(wú)人機(jī)編隊(duì)的權(quán)重矩陣；為無(wú)人機(jī)在三維空間內(nèi)所受合外力；ρ(x,y,z,t)為隊(duì)形約束方程；為隊(duì)形約束方程的雅可比矩陣；

211)建立無(wú)人機(jī)編隊(duì)罰函數(shù)數(shù)學(xué)模型。建立罰函數(shù)數(shù)學(xué)模型時(shí)，合并隊(duì)形約束方程作為一個(gè)輔助動(dòng)力學(xué)系統(tǒng)并通過(guò)罰函數(shù)因子予以懲罰；無(wú)人機(jī)編隊(duì)罰函數(shù)模型為：

其中，μ為罰函數(shù)懲罰因子(μ→+∞)；μα(x)為懲罰項(xiàng)；p為正整數(shù)，通常取p＝2。

罰函數(shù)的可行域?yàn)椋?/p>

d＝{x,y,z|ρ(x,y,z,t)＝0}(25)

因此懲罰項(xiàng)μα(x)為：

212)求解無(wú)人機(jī)編隊(duì)運(yùn)動(dòng)學(xué)罰函數(shù)模型。如圖4所示，為以6架無(wú)人機(jī)三角形編隊(duì)為例的彈性隊(duì)形結(jié)構(gòu)示意圖。在利用罰函數(shù)法求解無(wú)人機(jī)編隊(duì)運(yùn)動(dòng)學(xué)方程時(shí)，首先利用線性或者非線性的虛擬彈簧(阻尼器)代替無(wú)人機(jī)編隊(duì)的隊(duì)形完整性約束，這使得該算法可以合并隊(duì)形約束方程作為一個(gè)輔助動(dòng)力學(xué)系統(tǒng)并通過(guò)罰函數(shù)因子予以懲罰。此時(shí)，λ為一個(gè)基于違反隊(duì)形約束程度的虛擬彈簧(阻尼器)產(chǎn)生的力，并且假定此時(shí)彈簧的剛度和阻尼系數(shù)恒定。λ的表達(dá)式為：

其中，ls為彈性因子為n*n維的對(duì)角矩陣；ld為阻尼因子為n*n維的對(duì)角矩陣；ρ為隊(duì)形約束方程。

結(jié)合公式(23)，(27)可得罰函數(shù)下無(wú)人機(jī)編隊(duì)的約束動(dòng)力學(xué)方程為：

無(wú)人機(jī)編隊(duì)的約束動(dòng)力學(xué)方程轉(zhuǎn)換到狀態(tài)空間下為：

根據(jù)公式(19)可得，以6無(wú)人機(jī)為例時(shí)無(wú)人機(jī)編隊(duì)的固定隊(duì)形約束方程為：

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

如圖5所示，為無(wú)人機(jī)編隊(duì)約束動(dòng)力學(xué)方程求解流程圖。具體步驟如下：

31)首先無(wú)人機(jī)編隊(duì)三維飛行環(huán)境建模；

無(wú)人機(jī)編隊(duì)初始化，其中包括編隊(duì)中個(gè)無(wú)人機(jī)的位置、速度以及期望隊(duì)形等；

32)對(duì)編隊(duì)中個(gè)無(wú)人機(jī)進(jìn)行受力分析。利用公式(14)-(18)計(jì)算無(wú)人機(jī)編隊(duì)在當(dāng)前位置所受到的合力與分力，得到編隊(duì)中各無(wú)人機(jī)在三維方向上所受的分力和合力；

33)把無(wú)人機(jī)編隊(duì)在三維空間中的運(yùn)動(dòng)視為僅受到三維虛擬力的作用下的正向動(dòng)力學(xué)問(wèn)題。利用matlab每0.05步長(zhǎng)求解無(wú)人機(jī)編隊(duì)約束動(dòng)力學(xué)方程得到編隊(duì)中各個(gè)無(wú)人機(jī)下一個(gè)位置點(diǎn)和飛行方向角度信息；

34)判斷無(wú)人機(jī)編隊(duì)受到的合力是否為零，來(lái)確定當(dāng)前無(wú)人機(jī)編隊(duì)；

35)飛行到目標(biāo)區(qū)域。如果是，則輸出無(wú)人機(jī)編隊(duì)的路徑規(guī)劃曲線、路徑信息以及誤差曲線，程序轉(zhuǎn)到步驟36)；如果否，則跳轉(zhuǎn)至步驟33)；

36)無(wú)人機(jī)編隊(duì)約束動(dòng)力學(xué)方程結(jié)算結(jié)束。

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