粒子群算法在慣性/地磁組合導(dǎo)航航跡規(guī)劃中的應(yīng)用

喬 楠，王立輝，孫德勝，馬明珠，余 樂

（1. 東南大學(xué) 微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，南京 210096；2. 北京航天自動(dòng)控制研究所，北京 100854；3. 宇航智能控制技術(shù)國(guó)家級(jí)重點(diǎn)實(shí)驗(yàn)室，北京 100854）

慣性/地磁組合導(dǎo)航利用慣性導(dǎo)航提供的位置信息，結(jié)合磁力儀測(cè)得的位置磁場(chǎng)信息，在地磁圖上實(shí)現(xiàn)地磁特征匹配，慣性導(dǎo)航系統(tǒng)的解算軌跡誤差可由匹配后的軌跡修正，從而實(shí)現(xiàn)匹配定位和導(dǎo)航。慣性/地磁組合導(dǎo)航應(yīng)用中涉及的航跡規(guī)劃方法是基于地磁基準(zhǔn)庫數(shù)據(jù)，利用航跡規(guī)劃算法為航行器規(guī)劃一條符合航行任務(wù)、機(jī)動(dòng)性能、威脅規(guī)避等各類邊界控制條件的，有利于開展組合導(dǎo)航應(yīng)用的最短路徑，是一個(gè)非確定性多項(xiàng)式的復(fù)雜問題[1]，無法用已知的多項(xiàng)式時(shí)間問題來求取最優(yōu)解。

相對(duì)于啟發(fā)式算法和隨機(jī)型算法[2]，最優(yōu)化算法[3]以航跡代價(jià)模型為適應(yīng)度函數(shù)，根據(jù)粒子的進(jìn)化和種群的演練，獲得更好的優(yōu)化性能，而最優(yōu)化算法往往存在收斂速度慢、易陷入局部最優(yōu)的缺點(diǎn)。

本文以提高航行器航跡規(guī)劃效率和精度為目的，綜合考慮地磁導(dǎo)航匹配區(qū)分布及各項(xiàng)環(huán)境約束因素，探討一種采用粒子群算法的高精度的航跡規(guī)劃方法，利用最短路徑算法在地磁特征約束下進(jìn)行航行器離線規(guī)劃，將粗選航跡點(diǎn)作為粒子群優(yōu)化算法的初始輸入，采用粒子群優(yōu)化算法改進(jìn)編碼方式，將時(shí)間信息作為搜索空間中的粒子，改善算法的收斂性能和全局尋優(yōu)能力。

1 建立航跡代價(jià)評(píng)估函數(shù)

航行器航跡規(guī)劃的目的是設(shè)計(jì)一條符合航行任務(wù)、機(jī)動(dòng)性能、威脅規(guī)避等各類邊界控制條件的最短路徑，涉及到地磁特征代價(jià)函數(shù)Jterrain、坡度跟隨代價(jià)函數(shù)Jslope、轉(zhuǎn)向角代價(jià)函數(shù)Jangle、障礙物代價(jià)函數(shù)Jmon、航行長(zhǎng)度約束Jlength等[2]，而各代價(jià)函數(shù)之間的量化關(guān)系很難確定。為了降低計(jì)算復(fù)雜度，采用簡(jiǎn)化的航跡代價(jià)函數(shù)計(jì)算公式[4]。賦予各代價(jià)函數(shù)不同權(quán)值，航跡評(píng)價(jià)函數(shù)模型為：

其中，fit(xi,yi)為航行器進(jìn)行路徑規(guī)劃的航跡評(píng)價(jià)函數(shù)，ω1、ω2、ω3、ω4、ω5分別為各約束條件的代價(jià)函數(shù)的加權(quán)因子，各因子量值區(qū)間為(0,1)。地磁特征代價(jià)函數(shù)Jterrain，坡度跟隨代價(jià)函數(shù)Jslope，轉(zhuǎn)向角代價(jià)函數(shù)Jangle，障礙物代價(jià)函數(shù)Jmon，航行長(zhǎng)度約束Jlength均經(jīng)過歸一化處理。

地磁特征代價(jià)函數(shù)Jterrain為滿足安全航行的疊加地磁強(qiáng)度代價(jià)函數(shù)，用于防止航行器與海底發(fā)生撞擊，設(shè)置有安全航行深度區(qū)間。地磁坡度越大，水下航行器的跟隨越困難，當(dāng)坡度比航行器的最大縱傾角還要大時(shí)，航行器的地磁跟隨將受到限制，通常選擇坡度跟隨代價(jià)函數(shù)Jslope作為量化的威脅因素[5]。為避免急劇轉(zhuǎn)彎引起的碰撞風(fēng)險(xiǎn)，最大轉(zhuǎn)向角限制了航行器運(yùn)行的軌跡，航行器的最大轉(zhuǎn)角必須滿足轉(zhuǎn)向角代價(jià)函數(shù)Jangle。航行器在行進(jìn)過程中，需要采用障礙物代價(jià)函數(shù)Jmon對(duì)障礙物進(jìn)行有效規(guī)避。航行長(zhǎng)度約束Jlength體現(xiàn)了從起點(diǎn)到目標(biāo)點(diǎn)算法規(guī)劃的航行路徑長(zhǎng)度和目標(biāo)路徑長(zhǎng)度的約束關(guān)系。

2 最短路徑算法實(shí)現(xiàn)航跡的粗規(guī)劃

在滿足航跡代價(jià)評(píng)估函數(shù)約束的前提下，最短路徑算法按路徑長(zhǎng)度的次序遞增，迭代產(chǎn)生最短路徑，生成以固定的起點(diǎn)為樹結(jié)構(gòu)的根，根到各個(gè)節(jié)點(diǎn)的路徑即為根到該節(jié)點(diǎn)的最短路徑[6]。

將水下地磁信息網(wǎng)絡(luò)分割為子網(wǎng)，建立各子網(wǎng)拓?fù)浣Y(jié)構(gòu)關(guān)系，體現(xiàn)了地磁空間網(wǎng)絡(luò)數(shù)據(jù)的空間屬性特征。地理網(wǎng)絡(luò)信息為節(jié)點(diǎn)與弧段構(gòu)成的拓?fù)浣Y(jié)構(gòu)點(diǎn)群。網(wǎng)絡(luò)節(jié)點(diǎn)間不存在負(fù)權(quán)值，由最短路徑生成樹的過程中對(duì)是否加入新的節(jié)點(diǎn)取決于新節(jié)點(diǎn)到根的距離以及節(jié)點(diǎn)間的鄰接關(guān)系，任兩點(diǎn)間的權(quán)值就是該路徑上所有邊的權(quán)值總和。最短路徑算法按不遞減次序依次計(jì)算出長(zhǎng)度最小的路徑，然后是長(zhǎng)度次小的最短路徑，最后是長(zhǎng)度最大的最短路徑。最短路徑算法將節(jié)點(diǎn)分為兩部分[7]，一部分是臨時(shí)標(biāo)記節(jié)點(diǎn)集合S，一部分是未標(biāo)記節(jié)點(diǎn)集合U。初始時(shí)S只有起始點(diǎn)，以后每求出一條最短路徑，則將節(jié)點(diǎn)加入集合S。

步驟 1：初始化節(jié)點(diǎn)列表NodeList，初始化所有節(jié)點(diǎn)的權(quán)值列表maxtrix，調(diào)用Insert操作初始化優(yōu)先級(jí)隊(duì)列Heap，將源節(jié)點(diǎn)入隊(duì)列，同時(shí)創(chuàng)建Heap與NodeList對(duì)應(yīng)節(jié)點(diǎn)的索引Index。

步驟 2：抽取優(yōu)先級(jí)隊(duì)列中最短距離節(jié)點(diǎn)Node[j]，取隊(duì)列頭結(jié)點(diǎn)Vj，此時(shí)Node[j]為起始點(diǎn)V1到節(jié)點(diǎn)Vj的最短路徑終點(diǎn)。若節(jié)點(diǎn)Vj是目標(biāo)節(jié)點(diǎn)時(shí)，釋放隊(duì)列Heap中剩余節(jié)點(diǎn)及其索引關(guān)系，算法結(jié)束。

步驟 3：將從節(jié)點(diǎn)Vj出發(fā)的鄰接節(jié)點(diǎn)Vk入隊(duì)列，選擇Vk的原則是：

disk[k]=min{matrix1k,dist[j]+matrixjk}，

并相應(yīng)更新索引表Index。若節(jié)點(diǎn)Vk已出隊(duì)列，則不作處理。

步驟 4：重復(fù)步驟2，直到獲得目標(biāo)節(jié)點(diǎn)VD。

最短路徑算法的運(yùn)算效率與航跡代價(jià)數(shù)值、鄰接矩陣、適配區(qū)分布、節(jié)點(diǎn)數(shù)目等因素有關(guān)，耗時(shí)較大，無法達(dá)到水下航行器實(shí)時(shí)性的要求。最短路徑算法適用于在局部海域范圍內(nèi)，對(duì)水下地磁導(dǎo)航路徑信息進(jìn)行推算，實(shí)現(xiàn)粗選航跡的規(guī)劃，所求得的粗航跡作為最優(yōu)解的一個(gè)初始估計(jì)，用于后續(xù)的航跡路徑優(yōu)化，逐步優(yōu)化有限迭代序列，提高全局規(guī)劃能力。

3 粒子群算法實(shí)現(xiàn)航跡優(yōu)化

粒子群優(yōu)化算法采用群體智能的隨機(jī)搜索方法，可以彌補(bǔ)最短路徑算法的不足[8]，運(yùn)算過程具有并行性、收斂速度快等優(yōu)勢(shì)，可以與最短路徑算法優(yōu)勢(shì)互補(bǔ)，實(shí)現(xiàn)路徑優(yōu)化求解算法。

粒子群優(yōu)化算法中所有的粒子都有一個(gè)被優(yōu)化函數(shù)決定的適應(yīng)值，且粒子在搜索空間由速度向量來決定運(yùn)動(dòng)的速度和方向。粒子間的信息傳遞和相互作用，引導(dǎo)粒子群向目標(biāo)解移動(dòng)，通過迭代求解空間的目標(biāo)解。粒子群優(yōu)化算法根據(jù)其特有的記憶功能可以動(dòng)態(tài)跟蹤當(dāng)前的搜索情況并做出相應(yīng)的調(diào)整[9]，每一次迭代過程，每個(gè)個(gè)體都能夠存儲(chǔ)當(dāng)前位置，并找到最佳位置，且通過跟蹤兩個(gè)極值來更新。一個(gè)是單個(gè)粒子經(jīng)歷過的最好位置，稱作“局部最優(yōu)值”，記為Pid，在D維搜索空間，群體規(guī)模為m，則Pid是當(dāng)前第i(i=1,2,,m)個(gè)粒子所獲得的最好的適應(yīng)度函數(shù)值；另一個(gè)是群體中所有粒子找到的最佳位置，稱為“全局最優(yōu)值”，d=1,2,,D，記為Gid。

第i個(gè)粒子在搜索空間的位置表示為xi=(xi1,xi2,,xiD)，對(duì)應(yīng)的粒子位置改變的速度vi=(vi1,vi2,,viD)。在第t迭代中，各粒子的位置和速度由式(2)和式(3)進(jìn)行更新：

粒子速度向量滿足動(dòng)態(tài)系統(tǒng)Lipschitz約束條件：

其中，ω為慣性權(quán)重，能夠動(dòng)態(tài)調(diào)整粒子運(yùn)動(dòng)速度，使粒子趨于局部收斂；vt、xt為第i粒子d維分量在idid第t次迭代中的速度和位置；c1、c2為加速因子，取非負(fù)值，控制每個(gè)粒子向位置Pid和Gid運(yùn)動(dòng)的速度快慢；r1、r2為隨機(jī)數(shù)，服從區(qū)間[0,1]上的均勻分布；vid∈[ -vmax,vmax]，常數(shù)vmax是速度最大值，可由用戶根據(jù)實(shí)際情況設(shè)定；Vmax為速度向量的最大常數(shù)。

3.1 慣性權(quán)重的改進(jìn)

慣性權(quán)重ω作為線性變化的系數(shù)引入粒子群優(yōu)化算法，慣性權(quán)重系數(shù)能夠平衡全局搜索和局部搜索，合適的慣性權(quán)重值能夠以最少的迭代次數(shù)搜索到最優(yōu)解。一個(gè)搜索性能優(yōu)秀的粒子群優(yōu)化算法，在運(yùn)行初期應(yīng)當(dāng)具有較大的慣性權(quán)重值，從而保證算法的全局搜索能力。隨著迭代次數(shù)的遞增，慣性權(quán)重減小，以較小的權(quán)重值來提高算法的局部搜索能力。

設(shè)Imax為最大迭代次數(shù)，ωmax和ωmin分別為最大慣性權(quán)重值和最小慣性權(quán)重值。則第i次迭代的慣性權(quán)重值ωi的計(jì)算公式為：

或

對(duì)初始化過程的慣性權(quán)重進(jìn)行約束，有利于算法快速定位至全局最優(yōu)解[8,10]，在少數(shù)次迭代后，快速減小權(quán)值使算法進(jìn)入局部搜索階段，對(duì)慣性權(quán)重ω的線性變化過程分為兩個(gè)部分，其數(shù)學(xué)描述為：其中，Iλ為轉(zhuǎn)折迭代次數(shù)。此分段權(quán)重函數(shù)可以根據(jù)實(shí)際情況切換全局搜索和局部搜索，有效提高粒子群優(yōu)化算法的靈活性。

3.2 編碼方式的改進(jìn)

由粗選航跡點(diǎn)組成的折線，即粗選航跡L0={S,V1,V1,,E}，其中，S為起始節(jié)點(diǎn)，E為目標(biāo)節(jié)點(diǎn)，Vi為折線的端點(diǎn)，各端點(diǎn)分別對(duì)應(yīng)S（x0,y0）,V1（x1,y1）,E（xD,yD）。保持起始節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)不變，根據(jù)約束條件對(duì)粗選航跡中間節(jié)點(diǎn)的位置優(yōu)化調(diào)整，得出由新的航跡點(diǎn)Vi′（xi′,yi′）組成的優(yōu)化航跡L0′={S,V1′,V2′,,E}。調(diào)整策略為：

將時(shí)間信息ti作為粒子，在取值范圍為 [0,1]內(nèi)進(jìn)行搜索，使航跡評(píng)價(jià)函數(shù)最小，其中ti∈[0,1]，新航跡點(diǎn)在粗選航跡點(diǎn)周圍搜索，而不是在整個(gè)地磁圖平面進(jìn)行搜索，提高了搜索效率。

4 仿真驗(yàn)證與分析

利用Matlab程序構(gòu)建某海域的地磁強(qiáng)度特征圖如圖1所示，平均地磁強(qiáng)度為1831.4016 nT，最小地磁強(qiáng)度為1344 nT，最大地磁強(qiáng)度為2329 nT，忽略測(cè)量誤差。地磁強(qiáng)度為經(jīng)度范圍66°N～66.0357°N，緯度范圍78°E～78.0523°E，以柵格的形式存儲(chǔ)，網(wǎng)格數(shù)為44×44，網(wǎng)格間距0.00083°，約為90 m。

圖2為該海域地磁圖適配區(qū)分布，陰影部分代表不可匹配區(qū)，空白部分代表適配區(qū)。確立航跡規(guī)劃的任務(wù)為以S點(diǎn)為起始節(jié)點(diǎn)，E點(diǎn)為終止節(jié)點(diǎn)，規(guī)劃出一條最優(yōu)航跡，利用Dijkstra算法規(guī)劃出航跡如圖2所示。

圖1 地磁強(qiáng)度三維視圖Fig.1 Three-dimensional view of geomagnetic intensity

由于Dijkstra算法的遍歷所有節(jié)點(diǎn)，其時(shí)間復(fù)雜度為O(n2)，在節(jié)點(diǎn)數(shù)目很大時(shí)，耗時(shí)較大，無法達(dá)到水下航行器實(shí)時(shí)性的要求。在所選進(jìn)行航跡規(guī)劃的海域范圍不是很大時(shí)，Dijkstra算法適用于運(yùn)行前對(duì)水下地磁數(shù)據(jù)庫的地磁信息進(jìn)行分析，完成粗選航跡規(guī)劃。

圖2 適配區(qū)分布及粗選航跡Fig.2 Distribution of adaptation area and primary selection of trajectory

將粗選航跡作為粒子群優(yōu)化算法的粗選航跡，獲得粒子群優(yōu)化算法航跡，如圖3所示。粒子群的參數(shù)設(shè)置如表1所示。

繪制標(biāo)準(zhǔn)粒子群算法和混合粒子群算法下的適應(yīng)度對(duì)比圖如圖4所示。標(biāo)準(zhǔn)粒子群算法的初始適應(yīng)度函數(shù)值比混合算法的適應(yīng)度函數(shù)值更大，這是由于標(biāo)準(zhǔn)粒子群算法的搜索空間是整個(gè)平面，初始函數(shù)值更大。隨著迭代次數(shù)的增加，兩種算法都達(dá)到全局最優(yōu)值，標(biāo)準(zhǔn)粒子群算法適應(yīng)度達(dá)到49.211，混合算法的適應(yīng)度達(dá)到43.304，混合算法的適應(yīng)度函數(shù)值更加優(yōu)化，所需代價(jià)更小?；旌纤惴ㄔ诒ＷC全局最優(yōu)的同時(shí)也保證了局部最優(yōu)，可以有效規(guī)避非適配區(qū)。

表1 粒子群算法參數(shù)設(shè)置Tab.1 Parameter settings of particle swarm optimization algorithm

圖3 航跡規(guī)劃對(duì)比結(jié)果Fig.3 Comparison results of track planning

圖4 適應(yīng)度對(duì)比圖Fig.4 Comparison of fitness

由圖3直接對(duì)規(guī)劃的航跡進(jìn)行分析可得：從起始節(jié)點(diǎn)S到目標(biāo)節(jié)點(diǎn)E，由混合算法求得的航跡明顯比由 Dijkstra算法求得的粗選航跡更加優(yōu)化。相對(duì)于由Dijkstra算法求得的航跡，混合算法優(yōu)化了轉(zhuǎn)彎處的節(jié)點(diǎn)，使得所求得的航跡長(zhǎng)度更短。標(biāo)準(zhǔn)粒子群算法的搜索空間是整個(gè)平面，初始函數(shù)值更大，其初始適應(yīng)度函數(shù)值比粒子群優(yōu)化算法的適應(yīng)度函數(shù)值更大。隨著迭代次數(shù)的增加，粒子群優(yōu)化算法的適應(yīng)度函數(shù)值更加優(yōu)化，所需代價(jià)更小，在保證全局最優(yōu)的同時(shí)也保證了局部最優(yōu)，可以有效規(guī)避非適配區(qū)。

5 結(jié) 論

針對(duì)地磁匹配導(dǎo)航的特點(diǎn)，建立了水下航行器航跡規(guī)劃代價(jià)模型，提出采用最短路徑算法、粒子群算法相組合來進(jìn)行航跡規(guī)劃。利用 Dijkstra算法確保航跡經(jīng)過適配區(qū)，得出粗選航跡，利用改進(jìn)權(quán)值和改進(jìn)編碼的粒子群優(yōu)化算法對(duì)粗選航跡進(jìn)行全局優(yōu)化。

仿真結(jié)果證明，用于水下地磁航跡規(guī)劃的組合規(guī)劃算法將時(shí)間信息作為搜索空間的粒子量，使評(píng)價(jià)函數(shù)最小，縮小了搜索范圍，提高了搜索效率。此外，混合算法在保證全局最優(yōu)的同時(shí)，也保證了局部最優(yōu)，將適應(yīng)度由49.211提高至43.304，所需代價(jià)更小，可以有效規(guī)避非適配區(qū)，實(shí)現(xiàn)高效的航跡規(guī)劃。