一種基于粒子群優(yōu)化算法的多無(wú)人機(jī)航路規(guī)劃方法
【專利摘要】本專利公開了一種基于粒子群優(yōu)化算法的多無(wú)人機(jī)航路規(guī)劃方法。首先，建立多無(wú)人機(jī)航路規(guī)劃空間的三維地圖。在此基礎(chǔ)上，建立三維地圖下多無(wú)人機(jī)航路規(guī)劃模型，主要包括：障礙物模型、路徑模型、無(wú)人機(jī)狀態(tài)模型、約束模型和多無(wú)人機(jī)航路規(guī)劃數(shù)學(xué)模型。最后，采用粒子群優(yōu)化算法求解三維地圖下的多無(wú)人機(jī)航路規(guī)劃問題。本發(fā)明可有效提高復(fù)雜環(huán)境下多無(wú)人機(jī)的航路規(guī)劃能力，為無(wú)人機(jī)空中交通管理平臺(tái)、多無(wú)人機(jī)全自主飛行系統(tǒng)等提供技術(shù)支撐。
【專利說明】
一種基于粒子群優(yōu)化算法的多無(wú)人機(jī)航路規(guī)劃方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及一種基于粒子群優(yōu)化算法的多無(wú)人機(jī)航路規(guī)劃方法，屬于無(wú)人機(jī)航路 規(guī)劃領(lǐng)域。
【背景技術(shù)】
[0002] 目前，無(wú)人機(jī)已在電力、通信、氣象、監(jiān)測(cè)等多個(gè)領(lǐng)域得到成功應(yīng)用，取得了良好的 經(jīng)濟(jì)效益和社會(huì)效果。由于單架無(wú)人機(jī)自身軟硬件條件的限制，已很難勝任日益復(fù)雜的應(yīng) 用環(huán)境和多樣化的任務(wù)需求。多無(wú)人機(jī)協(xié)同完成任務(wù)的模式是未來(lái)無(wú)人機(jī)發(fā)展應(yīng)用的重要 趨勢(shì)，是提升無(wú)人機(jī)任務(wù)執(zhí)行效率、拓展新的任務(wù)方式、提高系統(tǒng)可靠性的有效途徑。美國(guó) 空軍科學(xué)顧問委員會(huì)就曾指出，無(wú)人機(jī)應(yīng)當(dāng)以機(jī)群的方式工作，而不是單獨(dú)行動(dòng)。
[0003] 多無(wú)人機(jī)航路規(guī)劃是實(shí)現(xiàn)多人機(jī)有效協(xié)同和管理的關(guān)鍵技術(shù)之一。目前，現(xiàn)有的 多無(wú)人機(jī)航路規(guī)劃方法一般采用最優(yōu)式算法，主要包括窮舉法、動(dòng)態(tài)規(guī)劃法、數(shù)學(xué)規(guī)劃、牛 頓法和梯度法等。這些方法在實(shí)際應(yīng)用存在較大的局限性，如缺乏對(duì)多無(wú)人機(jī)航路規(guī)劃的 有效決策，即對(duì)多無(wú)人機(jī)之間信息觀測(cè)的沖突性、相關(guān)性的冗余性考慮不足;加權(quán)算法的權(quán) 值的分配帶有很大的主觀性，缺乏有效、實(shí)用的權(quán)值分配方法等。因此，迫切需要更為有效、 實(shí)用的多無(wú)人機(jī)航路規(guī)劃方法。

【發(fā)明內(nèi)容】

[0004] 本發(fā)明的目的是解決現(xiàn)有的多無(wú)人機(jī)航路規(guī)劃方法所存在的上述問題，提出一種 基于粒子群優(yōu)化算法的多無(wú)人機(jī)航路規(guī)劃方法。
[0005] 本發(fā)明的技術(shù)方案主要包括以下步驟：
[0006] 步驟1:建立多無(wú)人機(jī)航路規(guī)劃空間的三維地圖。將無(wú)人機(jī)飛行空間以數(shù)據(jù)形式進(jìn) 行存儲(chǔ)，將其表示為規(guī)劃空間里的所有點(diǎn)(X，y，Z)的集合{(X，y，z) I Xmin<X彡Xmax，Ymin彡y彡 Ymax，Z_?Zmax}，其中（x，y)表示該點(diǎn)的水平位置，z為高程數(shù)據(jù)。離散化后的規(guī)劃空間采 用柵格的形式保存數(shù)字地形高程數(shù)據(jù)。
[0007] 步驟2:建立三維地圖下多無(wú)人機(jī)航路規(guī)劃模型。主要包括以下步驟：
[0008] 步驟2.1、建立障礙物模型。將無(wú)人機(jī)的障礙分為軟障礙物和硬障礙物。其中軟障 礙物的描述包括:障礙物的中心位置，障礙物的作用半徑，障礙物對(duì)無(wú)人機(jī)的損傷概率。硬 障礙物一般是由于特定因素限制不能飛躍的區(qū)域，一旦穿越則就會(huì)損毀。硬障礙物可以看 成軟障礙物的特殊情況，其描述和軟障礙物的描述一樣，只是障礙的損傷概率取值〇或1?？?的損傷概率ω等于各個(gè)并行損傷概率ω ,的疊加，而各個(gè)并行損傷概率ω ,的計(jì)算與其包含 串行損傷概鑾ω η滿圮以下關(guān)系，
[0009]
[0010] 步驟2.2、建立路徑模型。對(duì)于每架無(wú)人機(jī)，可行路徑可以看作是一條從起點(diǎn)到終 點(diǎn)、由若干線段組成的折線，可以用這些折線的端點(diǎn)序列表示。為避免帶有過于尖銳的角的 路徑。采用"修正"的處理方法:在尖銳拐角處加入一端圓弧，進(jìn)行對(duì)應(yīng)的平滑過渡處理。這 個(gè)圓弧的半徑選擇為無(wú)人機(jī)的最小轉(zhuǎn)彎半徑，滿足無(wú)人機(jī)的機(jī)動(dòng)性能約束，并且保證構(gòu)成 該圓弧與連的兩條線之間相切，用得到的兩個(gè)切點(diǎn)代替原來(lái)的拐角點(diǎn)。
[0011] 步驟2.3、建立無(wú)人機(jī)的狀態(tài)模型。本專利按照無(wú)人機(jī)的實(shí)際運(yùn)行情況，將無(wú)人機(jī) 區(qū)分為:就緒、工作、返航、失控4個(gè)狀態(tài)。
[0012] 步驟2.4、建立無(wú)人機(jī)航路規(guī)劃主要約束模型。本專利選擇無(wú)人機(jī)垂直方向最大轉(zhuǎn) 彎角約束、水平方向最大轉(zhuǎn)彎角約束、最大轉(zhuǎn)彎半徑約束、最遠(yuǎn)飛行距離約束、飛行高度約 束、特殊航路點(diǎn)等作為進(jìn)行航路規(guī)劃的約束條件。具體如下：
[0013] (1)垂直方向最大轉(zhuǎn)彎角約束
[0014]
[0015]式中，i表示當(dāng)前為第i段航跡，（Xi，yi，Zi)與（ Xi+1，yi+1，Zi+1)分另Ij表示當(dāng)前航路點(diǎn) 與待選航路點(diǎn)在規(guī)劃空間中的位置坐標(biāo)，Smax表示無(wú)人機(jī)在垂直方向上的最大轉(zhuǎn)彎角度。
[0016] (2)水平方向最大轉(zhuǎn)彎角約束
[0017]
[0018] 式中，仍nax表示無(wú)人機(jī)在水平方向上的最大轉(zhuǎn)彎角度。
[0019] (3)最小轉(zhuǎn)彎半徑約束
[0020] Ri^Rmin
[0021]式中，R1為規(guī)劃航跡進(jìn)行第i次轉(zhuǎn)彎時(shí)的轉(zhuǎn)彎半徑，Rmin為無(wú)人機(jī)的最大轉(zhuǎn)彎半徑。 Rmin由下式計(jì)算：
[0022]
[0023]式中，Vmin為無(wú)人機(jī)的最小飛行速度，ny max為無(wú)人機(jī)的最大法向過載。
[0024] (4)最遠(yuǎn)飛行距離約束
[0025]
[0026] 式中，I1表示第i段航跡的飛行距離，Lmax為允許最遠(yuǎn)飛行距離。
[0027] (5)飛行高度約束
[0028] Hmin^Hi^Hmax
[0029] 式中，出為當(dāng)前飛行高度，H_為最低可飛高度，Hmax為最高可飛高度。
[0030] (6)特殊航路點(diǎn)
[0031] 特殊航路點(diǎn)包括為在航路上為無(wú)人機(jī)分配的充電或更換電池的驛站，用于提高無(wú) 人機(jī)的續(xù)航能力。將此類點(diǎn)視作特殊航路點(diǎn)對(duì)待，當(dāng)無(wú)人機(jī)電量過低報(bào)警后，就近選擇相應(yīng) 的特殊航路點(diǎn)，對(duì)無(wú)人機(jī)電池充電或是更換。對(duì)于剛離開特殊航路點(diǎn)的無(wú)人機(jī)而言，在對(duì)其 進(jìn)行航路規(guī)劃時(shí)其機(jī)動(dòng)和續(xù)航能力按最高值考慮。
[0032] 步驟2.5、建立多無(wú)人機(jī)航路規(guī)劃數(shù)學(xué)模型。多無(wú)人機(jī)的航跡規(guī)劃問題為規(guī)劃空間 內(nèi)滿足特定要求，且飛行代價(jià)最小，從飛行起點(diǎn)到終飛行點(diǎn)之間的一系列航跡節(jié)點(diǎn)的集合， 表示為
[0033]
[0034]式中，C(p)為所規(guī)劃無(wú)人機(jī)飛行航路p的代價(jià)函數(shù)，g(p)為約束條件。
[0035]步驟3:采用粒子群優(yōu)化算法求解多無(wú)人機(jī)航路規(guī)劃。具體包括以下步驟：
[0036]步驟3.1、根據(jù)建立的三維地圖下多無(wú)人機(jī)航路規(guī)劃模型，選取粒子群優(yōu)化算法的 決策變量，并確定決策變量的上下界；
[0037]步驟3.2:根據(jù)建立的三維地圖下多無(wú)人機(jī)航路規(guī)劃模型，設(shè)置粒子群優(yōu)化算法的 目標(biāo)函數(shù)。
[0038] 步驟3.3:設(shè)置粒子群算法迭代過程中種群數(shù)目、最大迭代次數(shù)、粒子最大飛行速 度、學(xué)習(xí)因子、慣性權(quán)重等算法基本參數(shù)。
[0039] 步驟3.4:在不同初始條件下，通過算法迭代獲得滿足約束要求的航路規(guī)劃結(jié)果。
[0040] 本發(fā)明的特點(diǎn)在于：
[0041] 1.本發(fā)明提供的多無(wú)人機(jī)航路規(guī)劃方法基于三維數(shù)字地圖實(shí)現(xiàn)，不受限于多無(wú)人 機(jī)的任務(wù)背景和工作環(huán)境。通過建立不同的三維數(shù)字地圖，可以很容易的實(shí)現(xiàn)本方法應(yīng)用 場(chǎng)景的擴(kuò)展。
[0042] 2.本發(fā)明采用粒子群優(yōu)化算法求解多無(wú)人機(jī)航路規(guī)劃問題。粒子群優(yōu)化算法具有 不依賴于問題的遞度信息，需要調(diào)整的參數(shù)少，收斂精度高等特點(diǎn)；同時(shí)，算法能夠同時(shí)處 理多無(wú)人機(jī)航路規(guī)劃中的不同約束以及某些特殊要求。
[0043] 3.本發(fā)明在多無(wú)人機(jī)航路規(guī)劃建模時(shí)，對(duì)空中管理平臺(tái)中的一些特殊功能（如給 無(wú)人機(jī)充電、更換電池的驛站，臨時(shí)停機(jī)點(diǎn)等)予以體現(xiàn)。因此，本發(fā)明提出的多無(wú)人機(jī)航路 規(guī)劃方法可應(yīng)用于未來(lái)無(wú)人機(jī)空中管理平臺(tái)之中。
【附圖說明】
[0044]圖1是本發(fā)明的實(shí)現(xiàn)流程圖。
【具體實(shí)施方式】
[0045] 為了使本技術(shù)領(lǐng)域的人員更好地理解本申請(qǐng)方案，下面將結(jié)合本申請(qǐng)中的附圖1， 對(duì)本申請(qǐng)中的技術(shù)實(shí)現(xiàn)方案進(jìn)行說明。
[0046] 步驟1:建立多無(wú)人機(jī)航路規(guī)劃空間的三維地圖。將無(wú)人機(jī)飛行空間以數(shù)據(jù)形式進(jìn) 行存儲(chǔ)，將其表示為規(guī)劃空間里的所有點(diǎn)(X，y，Z )的集合{( X，y，Z ) I XmiA X彡Xmax，Ymin彡y彡 Ymax，z_?zmax}，其中（x，y)表示該點(diǎn)的水平位置，Z為高程數(shù)據(jù)。離散化后的規(guī)劃空間采 用柵格的形式保存數(shù)字地形高程數(shù)據(jù)。
[0047]步驟2:建立三維地圖下多無(wú)人機(jī)航路規(guī)劃模型。主要包括障礙物模型、路徑模型、 無(wú)人機(jī)狀態(tài)模型、約束模型和多無(wú)人機(jī)航路規(guī)劃數(shù)學(xué)模型。其中約束模型又包括垂直方向 最大轉(zhuǎn)彎角約束、水平方向最大轉(zhuǎn)彎角約束、最小轉(zhuǎn)彎半徑約束、最遠(yuǎn)飛行跟蹤約束、飛行 高度約束和特殊航路點(diǎn)等。
[0048]步驟3:采用粒子群優(yōu)化算法求解多無(wú)人機(jī)航路規(guī)劃。具體包括選擇粒子群優(yōu)化算 法決策變量并確定其上下界，設(shè)置算法的目標(biāo)函數(shù)，設(shè)置粒子群算法迭代過程中種群數(shù)目、 最大迭代次數(shù)、粒子最大飛行速度、學(xué)習(xí)因子、慣性權(quán)重等算法基本參數(shù)。針對(duì)多無(wú)人機(jī)航 路規(guī)劃問題，粒子群優(yōu)化算法主要參數(shù)的參考設(shè)置值為:粒子群種群數(shù)目20~50，算法最大 迭代次數(shù)50~100，粒子最大飛行速度為相應(yīng)變量搜索范圍的1/10~1/5，學(xué)習(xí)因子為2，慣 性權(quán)重的初始值為0.9，終止值為0.4。最后，通過算法迭代獲得滿足約束要求的航路規(guī)劃結(jié) 果D
【主權(quán)項(xiàng)】
1. 一種基于粒子群優(yōu)化算法的多無(wú)人機(jī)航路規(guī)劃方法，主要包括W下步驟： 步驟1:建立多無(wú)人機(jī)航路規(guī)劃空間的Ξ維地圖。將無(wú)人機(jī)飛行空間W數(shù)據(jù)形式進(jìn)行存 儲(chǔ)，將其表示為規(guī)劃空間里的所有點(diǎn)(X，y，Z)的集合{(X，y，Z) |Xmin《X《Xmax，Ymin《y《Ymax， Zmln《Z《Zmax}，其中（X，y)表示該點(diǎn)的水平位置，Z為高程數(shù)據(jù)。離散化后的規(guī)劃空間采用柵 格的形式保存數(shù)字地形高程數(shù)據(jù)。 步驟2:建立Ξ維地圖下多無(wú)人機(jī)航路規(guī)劃模型。主要包括W下步驟： 步驟2.1、建立障礙物模型。將無(wú)人機(jī)的障礙分為軟障礙物和硬障礙物。其中軟障礙物 的描述包括:障礙物的中屯、位置，障礙物的作用半徑，障礙物對(duì)無(wú)人機(jī)的損傷概率。硬障礙 物一般是由于特定因素限制不能飛躍的區(qū)域，一旦穿越則就會(huì)損毀。硬障礙物可W看成軟 障礙物的特殊情況，其描述和軟障礙物的描述一樣，只是障礙的損傷概率取值0或1。總的損 傷概率ω等于各個(gè)并行損傷概率ω 1的疊加，而各個(gè)并行損傷概率ω 1的計(jì)算與其包含串行 損傷概率ω U滿足W下關(guān)系：步驟2.2、建立路徑模型。對(duì)于每架無(wú)人機(jī)，可行路徑可W看作是一條從起點(diǎn)到終點(diǎn)、由 若干線段組成的折線，可W用運(yùn)些折線的端點(diǎn)序列表示。為避免帶有過于尖銳的角的路 徑，。采用"修正"的處理方法:在尖銳拐角處加入一端圓弧，進(jìn)行對(duì)應(yīng)的平滑過渡處理。運(yùn)個(gè) 圓弧的半徑選擇為無(wú)人機(jī)的最小轉(zhuǎn)彎半徑，滿足無(wú)人機(jī)的機(jī)動(dòng)性能約束，并且保證構(gòu)成該 圓弧與連的兩條線之間相切，用得到的兩個(gè)切點(diǎn)代替原來(lái)的拐角點(diǎn) 步驟2.3、建立無(wú)人機(jī)的狀態(tài)模型。本專利按照無(wú)人機(jī)的實(shí)際運(yùn)行情況，將無(wú)人機(jī)區(qū)分 為:就緒、工作、返航、失控4個(gè)狀態(tài)。 步驟2.4、建立無(wú)人機(jī)航路規(guī)劃主要約束模型。本專利選擇無(wú)人機(jī)垂直方向最大轉(zhuǎn)彎角 約束、水平方向最大轉(zhuǎn)彎角約束、最大轉(zhuǎn)彎半徑約束、最遠(yuǎn)飛行距離約束、飛行高度約束、特 殊航路點(diǎn)等作為進(jìn)行航路規(guī)劃的約束條件。具體如下： (1) 垂直方向最大轉(zhuǎn)彎角約束式中，i表示當(dāng)前為第i段航跡，（xi，yi，zi)與（xi+l，yi+l，zi+l)分別表示當(dāng)前航路點(diǎn)與待 選航路點(diǎn)在規(guī)劃空間中的位置坐標(biāo)，表示無(wú)人機(jī)在垂直方向上的最大轉(zhuǎn)彎角度。 (2) 水平方向最大轉(zhuǎn)彎角約束式中，Pmax表示無(wú)人機(jī)在水平方向上的最大轉(zhuǎn)彎角度。 (3) 最小轉(zhuǎn)彎半徑約束 Ri -5- Rmin 式中，R功規(guī)劃航跡進(jìn)行第i次轉(zhuǎn)彎時(shí)的轉(zhuǎn)彎半徑，Rmin為無(wú)人機(jī)的最大轉(zhuǎn)彎半徑。Rmin由 下式計(jì)算：式中，Vmin為無(wú)人機(jī)的最小飛行速度，nymax為無(wú)人機(jī)的最大法向過載。 (4) 最遠(yuǎn)飛行距離約束式中，1康示第i段航跡的飛行距離，Lmax為允許最遠(yuǎn)飛行距離。 (5) 飛行高度約束 Hmin《出《Hmax 式中，出為當(dāng)前飛行高度，Hmin為最低可飛高度，Hmax為最高可飛高度。 (6) 特殊航路點(diǎn) 特殊航路點(diǎn)包括為在航路上為無(wú)人機(jī)分配的充電或更換電池的釋站，用于提高無(wú)人機(jī) 的續(xù)航能力。將此類點(diǎn)視作特殊航路點(diǎn)對(duì)待，當(dāng)無(wú)人機(jī)電量過低報(bào)警后，就近選擇相應(yīng)的特 殊航路點(diǎn)，對(duì)無(wú)人機(jī)電池充電或是更換。對(duì)于剛離開特殊航路點(diǎn)的無(wú)人機(jī)而言，在對(duì)其進(jìn)行 航路規(guī)劃時(shí)其機(jī)動(dòng)和續(xù)航能力按最高值考慮。 步驟2.5、建立多無(wú)人機(jī)航路規(guī)劃數(shù)學(xué)模型。多無(wú)人機(jī)的航跡規(guī)劃問題為規(guī)劃空間內(nèi)滿 足特定要求，且飛行代價(jià)最小，從飛行起點(diǎn)到終飛行點(diǎn)之間的一系列航跡節(jié)點(diǎn)的集合，表示 為式中，C(p)為所規(guī)劃無(wú)人機(jī)飛行航路P的代價(jià)函數(shù)，g(p)為約束條件。 步驟3:采用粒子群優(yōu)化算法求解多無(wú)人機(jī)航路規(guī)劃。具體包括W下步驟： 步驟3.1、根據(jù)建立的Ξ維地圖下多無(wú)人機(jī)航路規(guī)劃模型，選取粒子群優(yōu)化算法的決策 變量，并確定決策變量的上下界； 步驟3.2:根據(jù)建立的Ξ維地圖下多無(wú)人機(jī)航路規(guī)劃模型，設(shè)置粒子群優(yōu)化算法的目標(biāo) 函數(shù)。 步驟3.3:設(shè)置粒子群算法迭代過程中種群數(shù)目、最大迭代次數(shù)、粒子最大飛行速度、學(xué) 習(xí)因子、慣性權(quán)重等算法基本參數(shù)。 步驟3.4:在不同初始條件下，通過算法迭代獲得滿足約束要求的航路規(guī)劃結(jié)果。
【文檔編號(hào)】G01C21/20GK105841702SQ201610135573
【公開日】2016年8月10日
【申請(qǐng)日】2016年3月10日
【發(fā)明人】徐利杰, 冉茂鵬, 董朝陽(yáng)
【申請(qǐng)人】賽度科技（北京）有限責(zé)任公司