付振秋，季 光，楊 瑛

(中國(guó)航空綜合技術(shù)研究所，北京 100028)

0 引 言

隨著科學(xué)技術(shù)的發(fā)展，AUV在海洋資源開(kāi)發(fā)、環(huán)境質(zhì)量監(jiān)測(cè)、水下作戰(zhàn)等領(lǐng)域發(fā)揮著越來(lái)越重要的作用。面對(duì)復(fù)雜多變的水域環(huán)境，三維路徑規(guī)劃是保證AUV安全完成任務(wù)的關(guān)鍵技術(shù)之一。全局路徑規(guī)劃和局部路徑規(guī)劃是復(fù)雜多變環(huán)境下的AUV根據(jù)障礙物類型進(jìn)行的分類，由于全局路徑規(guī)劃主要針對(duì)環(huán)境中靜止不變且信息已知的障礙物，能夠更好地提供路徑規(guī)劃的解決方案，也因?yàn)槿绱私陙?lái)，AUV三維空間下的全局路徑規(guī)劃成為大家研究的熱點(diǎn)之一。目前，人工勢(shì)場(chǎng)法、A*算法、神經(jīng)網(wǎng)絡(luò)算法、遺傳算法、人工免疫算法、模糊推理算法、粒子群算法等是最常用的路徑規(guī)劃算法。

本研究在蟻群算法的基礎(chǔ)上對(duì)該算法進(jìn)行優(yōu)化，融合了粒子群算法和人工勢(shì)場(chǎng)法，并在多目標(biāo)環(huán)境路徑規(guī)劃中增加了路徑代價(jià)的函數(shù)，使在融合算法下得到最優(yōu)路徑長(zhǎng)度基礎(chǔ)上充分考慮外界環(huán)境及探測(cè)干擾和自身能源的消耗問(wèn)題。

1 路徑規(guī)劃相關(guān)模型建立

1.1 環(huán)境模型建立

環(huán)境模型是AUV路徑規(guī)劃的首要環(huán)節(jié)，將三維現(xiàn)實(shí)環(huán)境特征進(jìn)行提取和解析，并轉(zhuǎn)化為計(jì)算機(jī)模擬路徑規(guī)劃過(guò)程中的地圖模型數(shù)據(jù)。環(huán)境建模的方法很多，如可視圖法、自由空間法、柵格法等。由于柵格法的地圖創(chuàng)建和維護(hù)比較容易，主要內(nèi)容是以柵格為基礎(chǔ)單位，離散化三維環(huán)境模型，所以本研究采用柵格法。設(shè)置三維空間為：水平面與XY平面重合，AUV的運(yùn)動(dòng)主方向定位X軸且機(jī)身頭為X軸的正方向（起點(diǎn)與終點(diǎn)分別對(duì)應(yīng)對(duì)應(yīng)X軸的最大值和最小值），垂直于水平面方向定位Z軸。這樣三維空間就可以分為若干基本單位柵格，以AUV開(kāi)始運(yùn)動(dòng)的點(diǎn)作為空間的零點(diǎn)位置，其他障礙物及空位區(qū)域也就進(jìn)行了相應(yīng)的方位坐標(biāo)確定。柵格[1]在計(jì)算時(shí)可選擇使用坐標(biāo)法或是序號(hào)法，本研究為了直觀地判斷點(diǎn)與點(diǎn)的位置關(guān)系將采用坐標(biāo)法。

在路徑規(guī)劃過(guò)程中對(duì)環(huán)境模型建立的要求：

1）將AUV的中心位置用質(zhì)點(diǎn)表示，同時(shí)對(duì)障礙物的尺寸按AUV的半徑作適當(dāng)擴(kuò)展，以保證AUV能夠無(wú)碰撞地運(yùn)動(dòng)；

2）AUV每次運(yùn)動(dòng)只能在所有柵格的中心位置（也就是柵格體的中心），不可在邊界或是端點(diǎn)處；

3）AUV的下一個(gè)位置只能是相鄰的26個(gè)柵格體的自由柵格體中；

4）為避免沒(méi)有必要的局部最優(yōu)，某一個(gè)柵格體的上下左右前后有5個(gè)是障礙柵格，則此柵格體就默認(rèn)為障礙柵格體；

5）不考慮環(huán)境中海流、潮流等對(duì)路徑規(guī)劃的干擾。

1.2 干擾模型建立

1.2.1 障礙模型

AUV在障礙環(huán)境下進(jìn)行路徑規(guī)劃，可使用人工勢(shì)場(chǎng)法計(jì)算AUV與障礙間的障礙代價(jià)，對(duì)AUV的運(yùn)動(dòng)路徑進(jìn)行調(diào)整。AUV在運(yùn)動(dòng)過(guò)程中，始終遠(yuǎn)離障礙代價(jià)函數(shù)值大的點(diǎn)運(yùn)動(dòng)，以實(shí)現(xiàn)避障。在此過(guò)程中還需要不斷朝目標(biāo)點(diǎn)運(yùn)動(dòng)，因此建立目標(biāo)代價(jià)函數(shù)，越趨近于目標(biāo)點(diǎn)則目標(biāo)代價(jià)函數(shù)值越小，AUV會(huì)始終尋找趨近目標(biāo)代價(jià)函數(shù)值小的點(diǎn)運(yùn)動(dòng)，以實(shí)現(xiàn)趨向目標(biāo)點(diǎn)的運(yùn)動(dòng)。

1）障礙代價(jià)函數(shù)

障礙代價(jià)函數(shù)是采用人工勢(shì)場(chǎng)法的思想建立，將AUV所識(shí)別的障礙物看作斥力勢(shì)場(chǎng)，其勢(shì)能函數(shù)為：

其中： q 為AUV的位置矢量； ρ (q,qob)為AUV到障礙物中心的距離，m； ρ0為距離障礙物中心的最小安全距離，m； k 為勢(shì)能系數(shù)； r0為障礙物的外圍半徑，m。

由上式可知當(dāng)AUV越靠近障礙物時(shí)其勢(shì)能越高，其障礙代價(jià)函數(shù)值越大，令其為

2）目標(biāo)代價(jià)函數(shù)

AUV在空間中運(yùn)行，在其一段路徑中目標(biāo)點(diǎn)只有一個(gè)，暫定離目標(biāo)點(diǎn)距離越遠(yuǎn)其目標(biāo)代價(jià)函數(shù)值越大，相反當(dāng)離目標(biāo)點(diǎn)距離越近其目標(biāo)代價(jià)函數(shù)值越小，從而建立目標(biāo)代價(jià)函數(shù)公式為：

其中，ρ（q,qgoa）為AUV當(dāng)前點(diǎn)到目標(biāo)點(diǎn)中心的距離。

3）障礙總代價(jià)函數(shù)

式中， ki為權(quán)重系數(shù)（ ki＞0）， ki權(quán)重系數(shù)選取與AUV路徑規(guī)劃過(guò)程中障礙代價(jià)與目標(biāo)代價(jià)那個(gè)更為重要，其中AUV路徑規(guī)劃總是趨于向總代價(jià)函數(shù)最小的點(diǎn)運(yùn)動(dòng)。

1.2.2 探測(cè)模型

AUV在水下運(yùn)動(dòng)過(guò)程中，除了躲避可見(jiàn)的障礙之外，還需要躲避來(lái)自人為因素的影響，其中最主要的是雷達(dá)的探測(cè)，其主要來(lái)自水面及水上探測(cè)雷達(dá)和水下潛艇及其他AUV的探測(cè)雷達(dá)。根據(jù)雷達(dá)回波強(qiáng)度與位置的關(guān)系，得到雷達(dá)探測(cè)威脅如下：

其中， Jradar為雷達(dá)探測(cè)威脅； n為威脅的數(shù)量或編號(hào)，個(gè)； li為路徑范圍內(nèi)的第i 個(gè)長(zhǎng)度值，m； di(l)為路徑上當(dāng)前計(jì)算點(diǎn)與第i個(gè)雷達(dá)探測(cè)威脅的距離，m。

AUV在水下某點(diǎn)收到第i個(gè)雷達(dá)探測(cè)位置的影響指數(shù)為：

式中： H(x)為雷達(dá)探測(cè)影響指數(shù)可限制AUV在威脅較小的區(qū)域航行； Ki為第i 個(gè)威脅的強(qiáng)度； βi為乘數(shù)因子，當(dāng)?shù)?i 個(gè)威脅沒(méi)有收到預(yù)警信息時(shí)， βi=1，否則βi＞ 1。

由障礙模型中的目標(biāo)代價(jià)函數(shù)可知，當(dāng)AUV在路徑規(guī)劃過(guò)程中，雖然目標(biāo)點(diǎn)確定，但是還是存在多個(gè)路線去選擇的情況，如圖1所示。

圖 1 AUV 路徑規(guī)劃目標(biāo)點(diǎn)確定Fig. 1 Determine the AUV path planning target point

AUV在路徑規(guī)劃過(guò)程中始終需要滿足其物理性能要求和任務(wù)約束，保證其穩(wěn)定安全的完成任務(wù)，多條路徑必定有長(zhǎng)度和安全的差異，因此建立路徑代價(jià)函數(shù)為：

式中： g為路徑代價(jià)函數(shù)； hi為第i段路徑上的高度變化，km； |l|為AUV在水下環(huán)境下左右搖擺機(jī)體的位移 ；ω1，ω2， ω3， ω4為權(quán)重系數(shù)，通過(guò)調(diào)整的相對(duì)大小，可以確定AUV是在障礙間穿行或繞過(guò)障礙航行，調(diào)整的相對(duì)大小可以確定其航行路徑是在雷達(dá)探測(cè)威脅區(qū)間穿行或繞過(guò)威脅區(qū)間航行。

1.2.3 路徑規(guī)劃過(guò)程中總代價(jià)函數(shù)

在AUV的路徑規(guī)劃過(guò)程中必定優(yōu)先在環(huán)境模型的基礎(chǔ)上進(jìn)行，考慮到外界環(huán)境的干擾，AUV在航行過(guò)程中總代價(jià)函數(shù)為障礙模型和探測(cè)模型的總和，也就是其干擾模型下AUV為獲得最優(yōu)路徑所作出的決策和付出的代價(jià)，公式為：

2 改進(jìn)型蟻群算法的 AUV 三維路徑規(guī)劃

2.1 蟻群算法的各參數(shù)優(yōu)化

在路徑規(guī)劃過(guò)程中，將AUV看作一只螞蟻，設(shè)有n 只螞蟻，螞蟻 M 在運(yùn)動(dòng)過(guò)程中收到來(lái)自多個(gè)路徑上的信息素濃度的“吸引”，在 t 時(shí)刻，螞蟻 M 位置為i，那么向 j 運(yùn)動(dòng)的轉(zhuǎn)移概率為Pij，常規(guī)公式為：

式中： τij(t)為t 時(shí)刻位置i 與位置 j之間路徑上的信息素濃度； ηij(t)為啟發(fā)函數(shù)，表示螞蟻 M 從位置i 到位置 j 的期望程度， dij為位置i 到位置j的距離；allowM為螞蟻M待訪問(wèn)位置的集合。

2.1.1 信息素分布的優(yōu)化

信息素濃度是衡量路徑規(guī)劃過(guò)程中路徑價(jià)值，為螞蟻的運(yùn)動(dòng)作引導(dǎo)依據(jù)。信息素只要涉及3個(gè)基本概念，分別是初始信息素、信息素更新、信息素?fù)]發(fā)。初始信息素由螞蟻的當(dāng)然位置來(lái)確定，不會(huì)有任何其他因素的影響，但是信息素的更新與揮發(fā)對(duì)信息素濃度影響最大，這也對(duì)最優(yōu)路徑的選擇起到了至關(guān)重要的作用。

1）信息素更新的優(yōu)化

螞蟻每移動(dòng)一次稱為一次搜索，當(dāng)螞蟻完成一次搜索后，就會(huì)對(duì)路徑上的信息素進(jìn)行局部更新，更新公式如下：

式 中 ： ε為 局 部 信 息 素 揮 發(fā) 因 子 ；動(dòng)過(guò)程中第 k 只螞蟻所構(gòu)造的總代價(jià)函數(shù)值，而且Dk(t)只計(jì)算兩位置點(diǎn)間的總代價(jià)。

由此可見(jiàn)，當(dāng)路徑最優(yōu)時(shí)，其總代價(jià)函數(shù)越小，局部更新中信息素的增量越大，對(duì)應(yīng)路徑上信息素的保留就越多，后續(xù)迭代中被選擇的幾率就高[2]。

當(dāng)所有螞蟻完成一次迭代后，對(duì)路徑的全局信息素進(jìn)行更新，公式如下：

式中： ρ為全局信息素?fù)]發(fā)因子， ? τij由以下公式確定：

式中： LB為當(dāng)前歷史最優(yōu)路徑長(zhǎng)度， L為本次迭代循環(huán)的最優(yōu)路徑長(zhǎng)度， σ 為根據(jù)實(shí)際路徑規(guī)劃需求與最亂路徑的偏移量來(lái)確定。

2）信息素?fù)]發(fā)的優(yōu)化

在一次搜素路徑結(jié)束后，路徑還是那個(gè)的信息素會(huì)進(jìn)行一定的揮發(fā)，這樣可以使信息素不會(huì)積累過(guò)多，對(duì)于提高路徑規(guī)劃效率和全局搜索能力有一定的作用。在信息素?fù)]發(fā)時(shí)最重要的是保留最佳路勁優(yōu)勢(shì)的同時(shí)也不能影響后續(xù)算法的進(jìn)行，也就是不影響后面螞蟻對(duì)路徑的選擇。

為了能有更好的最初決策，對(duì)路徑上的信息素進(jìn)行不同的揮發(fā)，將以通過(guò)路徑螞蟻數(shù)量 m 和路徑長(zhǎng)度L為參考變量，其公式為：

式中： L 為螞蟻搜索得到的路徑長(zhǎng)度， Lb為當(dāng)前周期最短路徑， s 為通過(guò)最短路徑的螞蟻數(shù)量， m 為全部的螞蟻數(shù)量， ρ為揮發(fā)權(quán)值，根據(jù)路徑長(zhǎng)度的變化改變相應(yīng)的數(shù)值。

2.1.2 啟發(fā)函數(shù)的優(yōu)化

在路徑規(guī)劃相關(guān)模型建立的過(guò)程中，可以發(fā)現(xiàn)常規(guī)蟻群算法中，啟發(fā)因子只考慮了目標(biāo)代價(jià)中即兩位置之間的距離，并沒(méi)有考慮下一點(diǎn)與終點(diǎn)和障礙代價(jià)、路徑代價(jià)這些因素的影響，優(yōu)化后啟發(fā)因子的公式為：

式中： q1， q2， q3均為上述因素的相對(duì)重要程度，djE為位置 j到終點(diǎn)位置的距離。

2.1.3 轉(zhuǎn)移概率的優(yōu)化

在三維柵格地圖中，當(dāng)一只螞蟻從當(dāng)前位置向下移位置運(yùn)動(dòng)過(guò)程中可能都是遍歷過(guò)的柵格體時(shí)，它將無(wú)法選擇下一位置進(jìn)行運(yùn)動(dòng)轉(zhuǎn)移，此時(shí)，它可能陷入死循環(huán)或死鎖狀態(tài)[3]。因此，在計(jì)算轉(zhuǎn)移概率時(shí)在考慮距離和信息素濃度的同時(shí)也需要考慮柵格體區(qū)域的安全性。在一個(gè)柵格體的周圍障礙體占大多數(shù)，那么在運(yùn)動(dòng)到這個(gè)柵格體后，其運(yùn)動(dòng)方向的可能性會(huì)變得很小，也就是其柵格體區(qū)域內(nèi)附件的安全性相對(duì)較差。那么，優(yōu)化后的公式為：

式中： Bgrid為與當(dāng)前柵格體相鄰柵格體中障礙柵格體所占的比例， A Bgrid為與當(dāng)前柵格體相鄰的柵格體中障礙柵格體的數(shù)量， Agrid為與當(dāng)前柵格體相鄰的柵格體的數(shù)量。

在轉(zhuǎn)移概率公式中加入?yún)^(qū)域安全信因素后的公式為：

其中， μ為安全信息的權(quán)重值。注意：盡管在這個(gè)柵格體周圍障礙占多數(shù)，但這個(gè)柵格體依然屬于可通過(guò)性高且有時(shí)又是路徑的必經(jīng)之路，所以其值不能過(guò)高。

2.2 粒子群算法

粒子群算法是一種群體的迭代算法，通過(guò)群體性性能在解空間進(jìn)行結(jié)果的優(yōu)化計(jì)算[5]。其公式為：

由公式可以看出，粒子群算法在每次迭代過(guò)程中通過(guò)粒子跟蹤2個(gè)最優(yōu)解來(lái)更新自己。

式中： wmax為慣性權(quán)重的最大值， wmin為慣性權(quán)重的最小值，為最大迭代次數(shù)， λ為當(dāng)前迭代次數(shù)。

2.3 改進(jìn)型蟻群——粒子群融合算法

蟻群算法是啟發(fā)式優(yōu)化算法中比較常用和經(jīng)典的算法，具有較強(qiáng)的魯棒性與正反饋機(jī)制，但是由于其初期信息素信息匱乏，搜索比較盲目容易陷入局部最優(yōu)和過(guò)早收斂，為了追求搜索精度使其整個(gè)算法的搜索速度慢。而粒子群算法雖然其搜索精度不高，但卻具有較強(qiáng)的全局搜索能力和較快的搜索速度。將蟻群算法和粒子群算法進(jìn)行融合，先利用粒子群算法較強(qiáng)的全局搜索能力和搜索速度對(duì)整個(gè)區(qū)域空間進(jìn)行路徑搜索得到最優(yōu)解或是次優(yōu)解。接下來(lái)，將得到的最優(yōu)解或是次優(yōu)解轉(zhuǎn)化為蟻群算法中的初始信息素，并進(jìn)行重新分布，利用優(yōu)化后的蟻群算法的正反饋機(jī)制對(duì)次優(yōu)解進(jìn)行精確搜索，得到當(dāng)前路徑信息素豐富的最優(yōu)解。這樣不僅解決了蟻群算法容易陷入局部最優(yōu)、搜索盲目的缺點(diǎn)，也通過(guò)粒子群算法加快了路徑規(guī)劃搜索的速度。

改進(jìn)型蟻群—粒子群融合算法中2種算法的銜接點(diǎn)為利用粒子群算法產(chǎn)生蟻群算法的初始信息素分布，也就是利用蟻群算法中根據(jù)外界環(huán)境信息等確定的信息素的初始值 τ，利用粒子群算法求得的次優(yōu)或是最優(yōu)路徑轉(zhuǎn)化為信息素的增量，最終得到蟻群算法中各路徑上的信息素的初始值為 τij，其公式為

算法流程框架圖如圖2所示。

3 仿真與實(shí)驗(yàn)驗(yàn)證分析

在Matlab環(huán)境下編程對(duì)AUV三維路徑規(guī)劃進(jìn)行仿真實(shí)驗(yàn)，實(shí)驗(yàn)環(huán)境在跨度為21×21 km的一片海域中搜索從起點(diǎn)到目標(biāo)終點(diǎn)并且避開(kāi)所有障礙物及干擾的路徑。為了方便問(wèn)題的求解，取該片區(qū)域內(nèi)最深的點(diǎn)的高度為0，其他高度根據(jù)最深點(diǎn)來(lái)確定。起點(diǎn)坐標(biāo)為（1，0，800），目標(biāo)終點(diǎn)坐標(biāo)為（21，4，100）。粒子群算法參數(shù)設(shè)置為： c1與 c2都為2， wmax為0.9，wmin為0.6，最大迭代次數(shù)為100；蟻群算法參數(shù)設(shè)置為： q1， q2， q3都設(shè)置為1，為0.9，全局信息素?fù)]發(fā)因子 ρ為0.2；分別采用未融合、融合前未優(yōu)化及優(yōu)化并融合的蟻群——粒子群融合算法對(duì)AUV路徑規(guī)劃進(jìn)行仿真，結(jié)果如圖3所示。

可以看出融合后的算法在路徑規(guī)劃時(shí)適應(yīng)度值較小，且響應(yīng)時(shí)間較快，而且在優(yōu)化后的融合算法中效果更加明顯。

圖 2 算法流程框架圖Fig. 2 Framework of the algorithm flow

圖 3 未采用融合優(yōu)化算法的路徑規(guī)劃；Fig. 3 The path planning without fusion optimization algorithm

4 結(jié) 語(yǔ)

圖 4 未采用融合優(yōu)化算法的蟻群路徑規(guī)劃Fig. 4 The path planning with fusion optimization algorithm

圖 5 采用優(yōu)化后的蟻群——粒子群融合算法路徑規(guī)劃Fig. 5 The path planning with improved ant colony optimization and particle swarm optimization

本文提出了基于改進(jìn)型蟻群—粒子群融合算法對(duì)AUV在水下三維空間的路徑規(guī)劃進(jìn)行研究。首先利用粒子群算法較強(qiáng)的全局搜索能力和搜索速度對(duì)整個(gè)區(qū)域空間進(jìn)行路徑搜索得到最優(yōu)解或是次優(yōu)解，然后再利用蟻群算法的正反饋機(jī)制對(duì)次優(yōu)解進(jìn)行精確搜索，得到當(dāng)前路徑信息素豐富的最優(yōu)解。根據(jù)仿真結(jié)果顯示分析可知，改進(jìn)型蟻群—粒子群融合算法獲得的最終結(jié)果優(yōu)于單一以蟻群算法或粒子群算法以及傳統(tǒng)蟻群—粒子群融合算法進(jìn)行路徑規(guī)劃尋優(yōu)的結(jié)果。該算法對(duì)于AUV路徑規(guī)劃的應(yīng)用研究以及多AUV水下環(huán)境協(xié)調(diào)路徑規(guī)劃方法的研究具有一定的參考意義。