基于可解空間法的視景路徑規(guī)劃仿真系統(tǒng)設(shè)計(jì)

周靜，付緒昌

（1.江漢大學(xué)數(shù)學(xué)與計(jì)算機(jī)科學(xué)學(xué)院，湖北武漢430056；2.鴻富錦精密工業(yè)有限公司制造執(zhí)行系統(tǒng)開(kāi)發(fā)課，湖北武漢430204）

基于可解空間法的視景路徑規(guī)劃仿真系統(tǒng)設(shè)計(jì)

周靜1，付緒昌2

（1.江漢大學(xué)數(shù)學(xué)與計(jì)算機(jī)科學(xué)學(xué)院，湖北武漢430056；2.鴻富錦精密工業(yè)有限公司制造執(zhí)行系統(tǒng)開(kāi)發(fā)課，湖北武漢430204）

提出一種新的可解空間粒子群尋路算法，并實(shí)現(xiàn)Vega Prime（VP）三維視景路徑規(guī)劃仿真系統(tǒng)。算法假設(shè)初始化的每個(gè)粒子都是一個(gè)潛在的可行解，都可到達(dá)終點(diǎn)。每個(gè)粒子在迭代的過(guò)程中不斷更新位置，并記錄下它們所經(jīng)過(guò)的路徑點(diǎn)的個(gè)數(shù)；迭代結(jié)束后，找出到達(dá)終點(diǎn)的粒子中路徑點(diǎn)個(gè)數(shù)最少的粒子，將該粒子每次迭代時(shí)所經(jīng)過(guò)的最優(yōu)位置作為尋路路線。同時(shí)在VP大地圖場(chǎng)景中，為矩形障礙物添加一個(gè)圓形的坡度，較好地解決了算法易陷入局部最優(yōu)解的缺陷。該仿真系統(tǒng)的正確運(yùn)行驗(yàn)證了算法的有效性及系統(tǒng)的實(shí)用性。

可解空間法；視景仿真；路徑規(guī)劃；圓形坡度

0 引言

隨著視景仿真技術(shù)的應(yīng)用越來(lái)越廣泛，基于Vega Prime（VP）的視景仿真技術(shù)也得到了不斷的發(fā)展，在民用、商用、軍事等各個(gè)領(lǐng)域中都得到了廣泛的應(yīng)用［1-2］。利用VP構(gòu)建三維場(chǎng)景及模型，并建立相應(yīng)的不同對(duì)象之間的關(guān)系以及地面檢測(cè)、碰撞檢測(cè)［3］。在MFC中搭建好VP環(huán)境，調(diào)用VP文件，并實(shí)現(xiàn)相關(guān)消息響應(yīng)，計(jì)算出場(chǎng)景地形中的障礙坐標(biāo)列表。然后將改進(jìn)的粒子群算法移植到MFC程序，粒子群算法獲取障礙坐標(biāo)，構(gòu)建相應(yīng)的地圖矩陣，計(jì)算出尋路路徑后將路徑坐標(biāo)列表返回到MFC程序，MFC程序根據(jù)獲取的路徑坐標(biāo)實(shí)現(xiàn)自動(dòng)尋路。

1 VP視景仿真平臺(tái)的設(shè)計(jì)與實(shí)現(xiàn)

基于MFC的VP應(yīng)用程序主要用到VP庫(kù)中的API?；赟TL（標(biāo)準(zhǔn)模板庫(kù)）和C++的API顯得非常緊湊和靈活。VP的API通過(guò)模板和繼承性的使用使得仿真循環(huán)更加簡(jiǎn)潔而有效。實(shí)時(shí)控制包括定義ACF、配置ACF和系統(tǒng)、運(yùn)行仿真循環(huán)以及最后退出仿真循環(huán)。

1.1 VP應(yīng)用程序的主循環(huán)

主循環(huán)｛

獲取對(duì)象（如前視觀察者等）的位姿矢量（上一幀）；

獲取用戶(hù)鍵盤(pán)輸入控制；

根據(jù)對(duì)象上一幀的位姿和輸入控制來(lái)計(jì)算位姿；

碰撞檢測(cè)并對(duì)計(jì)算所得位姿進(jìn)行調(diào)整；

賦予對(duì)象調(diào)整后的位姿；

更新幀；｝

1.2 VP地圖編碼

地圖編碼主要實(shí)現(xiàn)步驟如下：1）定義點(diǎn)坐標(biāo)結(jié)構(gòu)體。

2）地圖編碼初始化，即無(wú)障礙圖。

3）障礙坐標(biāo)初始化，在定義的地圖矩陣中賦值。

1.3 通道添加

實(shí)現(xiàn)俯視通道并綁定觀察者位置的實(shí)現(xiàn)步驟如下：

1）實(shí)現(xiàn)俯視效果。

2）定義俯視通道并設(shè)置其在窗口中的位置，關(guān)聯(lián)窗口，關(guān)聯(lián)圖形狀態(tài)。

3）定義俯視觀察者，并將觀察者和通道并聯(lián)。

4）在主循環(huán)中根據(jù)控制更改俯視觀察者的位姿，即可實(shí)現(xiàn)通過(guò)俯視通道觀察的效果。

圖1為添加通道的示意圖，根據(jù)需求控制通道寬度就能符合使用者的要求。

圖1 通道添加示意圖Fig.1Schematic diagram of adding channel

1.4 固定地圖俯視圖

如果將俯視圖通道的觀察者設(shè)置為看向小車(chē)，這樣會(huì)出現(xiàn)當(dāng)小車(chē)在轉(zhuǎn)向時(shí)，小車(chē)的方向沒(méi)有發(fā)生偏轉(zhuǎn)，而是地圖在旋轉(zhuǎn)，不利于觀察小車(chē)行進(jìn)路線。利用LynX Prime（LP）修改通道觀察者參數(shù)設(shè)置，將觀察者的俯視通道固定在地圖中心點(diǎn)，這樣地形就不會(huì)旋轉(zhuǎn)了，只有小車(chē)在行進(jìn)的過(guò)程中因改變方向而偏轉(zhuǎn)。

圖2是由像素點(diǎn)1 080×720轉(zhuǎn)化為的1 000×1 000大小的三維視景地圖，障礙物和坐標(biāo)系均進(jìn)行了計(jì)算轉(zhuǎn)化：X坐標(biāo)乘以1 000/1 080；Y坐標(biāo)乘以1 000/720。圖2中左邊是整個(gè)設(shè)計(jì)地圖的全景俯視通道；右上為當(dāng)前智能體行走主視圖通道，右下是粒子將要到達(dá)的目標(biāo)點(diǎn)的俯視圖通道。

圖2 初始化的系統(tǒng)界面圖Fig.2Interface of the initialization system

2 可解空間粒子群尋路算法設(shè)計(jì)

2.1 算法設(shè)計(jì)思想

在改進(jìn)標(biāo)準(zhǔn)粒子群算法［4-5］的基礎(chǔ)上，提出可解空間粒子群尋路算法（以下簡(jiǎn)稱(chēng)可解空間法）,其流程圖如圖3所示。

圖3 可解空間法流程圖Fig.3Solvable-space algorithm flow chart

可解空間法的設(shè)計(jì)思想為：假設(shè)初始化的每個(gè)粒子都是一個(gè)潛在的可行解空間，每個(gè)粒子在不斷更新位置（只要比其歷史最優(yōu)適應(yīng)度值pbest小，則更新至當(dāng)前路徑點(diǎn)，這里的粒子適應(yīng)度表示當(dāng)前粒子所處位置距離目標(biāo)的馬氏距離），經(jīng)過(guò)完全迭代后，每個(gè)粒子都保存了生命周期中所經(jīng)過(guò)的路徑點(diǎn)坐標(biāo)，在滿足搜索結(jié)束條件后，需要對(duì)每個(gè)粒子的路徑進(jìn)行一個(gè)評(píng)估，就是找出群體中到達(dá)終點(diǎn)的所有粒子中所走路徑最短的一個(gè)個(gè)體的路徑（即該個(gè)體所經(jīng)歷的路徑點(diǎn)個(gè)數(shù)最少），此路徑便是此次尋路最優(yōu)的解。實(shí)現(xiàn)本算法也就是比較每個(gè)粒子的路徑長(zhǎng)度（即路徑點(diǎn)計(jì)數(shù)count值）的大小，可以借用選擇排序中第一次循環(huán)找到所有數(shù)據(jù)中最大（或最?。┑闹捣旁诘谝粋€(gè)位置，將擁有count值最小的粒子路徑點(diǎn)逐個(gè)輸出并顯示在地圖上即可。將這種思想的粒子群尋路算法稱(chēng)為“可解空間法”。

2.2 尋找最短路徑代碼實(shí)現(xiàn)

找出群體中到達(dá)終點(diǎn)的所有粒子中所走路徑最短的一個(gè)個(gè)體的路徑代碼如下：

for(i=1;i＜PSO_popsize;i++){//搜索最優(yōu)，相當(dāng)于選擇排序第一步的過(guò)程

cout＜＜PSO_pop[i].pbest＜＜endl;//每個(gè)粒子的最優(yōu)適應(yīng)度值

if(PSO_pop[i].pbest＜=sqrt(2.0))//粒子最終最佳位置=終點(diǎn)

if(PSO_pop[i].count＜PSO_pop[i-1].count)

bestpath=i;}

if(bestpath＞-1){//判斷路徑存在與否，若存在，將路徑保存

pathfound=true;

for(i=0;i＜PSO_pop[bestpath].count;i++){

setPos(PSO_pop[bestpath].Epbest_posX[i],PSO_pop[bestpath].Epbest_posY[i]);cout＜＜"("＜＜PSO_pop [bestpath].Epbest_posX[i]＜＜","＜＜PSO_pop[bestpath].Epbest_posY[i]＜＜")"＜＜endl;}}

else{cout＜＜"尋路失??！"＜＜endl;

return;}

代碼中的Epbest_posX變量是指路徑最短的粒子的x坐標(biāo)，Epbest_posY變量是指路徑最短的粒子的y坐標(biāo)。整個(gè)粒子群的迭代次數(shù)為PSO_maxgen，大小為PSO_popsize，算法時(shí)間復(fù)雜度為O(PSO_max?gen*PSO_popsize*PSO_popsize)。

2.3 算法運(yùn)行結(jié)果

在平面空間模型的尋路算法研究中，應(yīng)用可解空間法運(yùn)行結(jié)果如圖4所示。從圖4可看出，可解空間法可以完成路徑規(guī)劃任務(wù)。從可解空間法的思路可看出，它屬于先局部路徑規(guī)劃出可解路徑，然后再在這些可解路徑上求出長(zhǎng)度最短的路徑并輸出的局部啟發(fā)性規(guī)劃算法。

圖4 可解空間法在平面空間中的尋路效果Fig.4Effect with solvable-space algorithm to find path in planar pace

3 VP平臺(tái)下尋路算法的優(yōu)化

3.1 尋路算法引入VP-MFC程序

VP程序提供給可解空間粒子群尋路算法的參數(shù)為：地圖大?。ㄩL(zhǎng)、寬）、起點(diǎn)和終點(diǎn)坐標(biāo)（x，y，z=0）、point結(jié)構(gòu)體，所有障礙點(diǎn)坐標(biāo)采用List＜point＞進(jìn)行存儲(chǔ)。尋路算法返回給VP程序的參數(shù)：尋路成功后，所有路徑點(diǎn)按順序存入list＜point＞列表ListPoint llPath。將該列表返回給VP程序，VP程序再根據(jù)返回的路徑列表中每一個(gè)點(diǎn)的坐標(biāo)值進(jìn)行尋路。

將所設(shè)計(jì)的可解空間法應(yīng)用于較大的VP地圖環(huán)境尋路時(shí)，發(fā)現(xiàn)其仍會(huì)陷入局部最優(yōu)解，這時(shí)采用給障礙物一個(gè)坡度的思想可使陷入局部最優(yōu)解的粒子繞開(kāi)障礙物。

3.2 局部最優(yōu)解的改進(jìn)

以下對(duì)該種算法原理進(jìn)行建模分析，建模圖如圖5所示。圖5中，起點(diǎn)A和目標(biāo)點(diǎn)B之間有一個(gè)較大的矩形障礙，并且起點(diǎn)A和目標(biāo)點(diǎn)B在障礙物對(duì)稱(chēng)線兩邊。按照粒子群路徑規(guī)劃方法，粒子群在尋路時(shí)，會(huì)不斷逼近圖中C點(diǎn)，在沒(méi)有設(shè)置避障時(shí)，由于兩點(diǎn)直線距離符合路徑長(zhǎng)度最短的條件，粒子會(huì)穿過(guò)圖中黑色障礙物直線到達(dá)目標(biāo)點(diǎn)B，這顯然是不符合設(shè)計(jì)標(biāo)準(zhǔn)的，但是在由前文介紹的粒子群尋路算法可知，算法在迭代過(guò)程中，在C點(diǎn)鄰域范圍內(nèi)沒(méi)有比當(dāng)前位置（C點(diǎn)）更優(yōu)的位置，所以粒子群在搜索路徑的時(shí)候，會(huì)更新停止在C點(diǎn)，直到迭代結(jié)束PSO算法程序運(yùn)行完畢，導(dǎo)致粒子群尋路失敗（走了一段，并且停滯在某一坐標(biāo)點(diǎn)，C點(diǎn)就是一個(gè)代表），問(wèn)題仿真如圖6所示。從圖6可以看出，坐標(biāo)（8，7）充當(dāng)了上述C點(diǎn)的角色，導(dǎo)致了粒子正在尋路中得到局部最優(yōu)。

圖5 基本粒子群算法陷入局部最優(yōu)建模圖Fig.5Modeling diagram of basic particle swarm algorithm result in falling into local optimal

圖6 陷入局部最優(yōu)實(shí)驗(yàn)運(yùn)行結(jié)果圖Fig.6Experiment result of falling into local optimal

粒子群尋路算法出現(xiàn)上述問(wèn)題的主要原因是粒子無(wú)法在障礙物的“凹角”里回退出來(lái)，沒(méi)有一個(gè)坡度（弧度）讓粒子群向地勢(shì)低的方向行進(jìn)就導(dǎo)致了陷入局部最優(yōu)。如果將障礙物視為一個(gè)圓形（見(jiàn)圖7），則無(wú)論粒子群在障礙物的哪一個(gè)方向（360°視角范圍內(nèi)）都產(chǎn)生了一個(gè)坡度，這樣粒子群在尋路的時(shí)候就不會(huì)向著障礙物趨近，因?yàn)樵诰嚯x障礙物還有段微小距離時(shí)，已經(jīng)“感知”到了障礙物的威脅［6］。按照上述理論模擬擴(kuò)展障礙物得到如圖8結(jié)果。

圖7 解決局部最優(yōu)模型Fig.7The model to solve the local optimal

圖8 解決局部最優(yōu)仿真結(jié)果Fig.8Simulation result of solving the local optimal

在LP地圖（ACF文件）中，障礙物較大設(shè)置擴(kuò)展填充坐標(biāo)點(diǎn)［7］較為繁雜，因此填充的具體做法為：將黑色障礙物坐標(biāo)進(jìn)行擴(kuò)展，已知VP中的障礙物矩陣對(duì)頂角兩個(gè)坐標(biāo)點(diǎn)，通過(guò)這兩個(gè)坐標(biāo)點(diǎn)可以計(jì)算出對(duì)角線的中心坐標(biāo)即圓心（centerX，centerY）和對(duì)角線長(zhǎng)度的一半值即為半徑R，然后進(jìn)行逐個(gè)像素點(diǎn)填充。

為避免逐個(gè)障礙物手動(dòng)擴(kuò)展的龐大工作量，將障礙物擴(kuò)展成為一個(gè)圓形。具體代碼如下：

void setOneBarrier（int xMin，int yMin，int xMax，int yMax）｛

int centerX=（xMin+xMax+1）/2；

int centerY=（yMin+yMax+1）/2；

int R=int（sqrt（double（（xMin-xMax）*（xMin-xMax）+（yMin-yMax）*（yMin-yMax）））/2+0.5）；

for（int i=-R；i＜=R；i++）｛

int dx=int（sqrt（double（R*R-i*i））+0.5）；

for（int j=-dx；j＜=dx；j++）｛

lBarrier.push_back（point（j+centerX，i+centerY，0））；｝｝｝

3.3 算法優(yōu)化效果分析

將障礙物擴(kuò)展成圓形的避障算法示意圖如圖9所示。在畫(huà)圓過(guò)程中也會(huì)出現(xiàn)鋸齒狀邊緣（見(jiàn)圖9），計(jì)算機(jī)填充無(wú)法達(dá)到標(biāo)準(zhǔn)的圓，在柵格中更是無(wú)法避免出現(xiàn)鋸齒的波紋，這樣在較小程度上影響本次仿真效果，因?yàn)樵诹Ｗ尤簩ぢ分袝?huì)出現(xiàn)坡度不明顯而陷入之前提到的那種局部最優(yōu)情形，但是在實(shí)際應(yīng)用中地圖往往遠(yuǎn)大于本文中所使用的1 000×1 000大小的地圖。

圖9 粒子在擴(kuò)展的圓形障礙物中的路徑Fig.9Path of particles in extended circular obstacle

圖9是將障礙物用“#”表示，粒子繞過(guò)障礙物用“0”表示路徑，將程序中的路徑規(guī)劃寫(xiě)到TXT文檔中以顯示仿真優(yōu)化的效果，這樣可以很明顯地看出粒子避障的軌跡細(xì)節(jié)，從圖9看出陷入局部極值通過(guò)畫(huà)圓算法在仿真實(shí)例中得到了較好的優(yōu)化效果，在仿真中的效果切合理論預(yù)期。

4 路徑規(guī)劃仿真系統(tǒng)設(shè)計(jì)

仿真地圖是由像素點(diǎn)1 080×720轉(zhuǎn)化為的1 000×1 000大小的三維視景地圖，其中設(shè)定23個(gè)均勻分布在地圖上的障礙物坐標(biāo)。將可解空間粒子群算法［8-10］用于此仿真環(huán)境中并進(jìn)一步優(yōu)化，大大降低粒子群陷入局部最優(yōu)解找不到路徑的概率，提高了粒子成功獲得路徑的能力。高效完成在擁有多障礙的復(fù)雜場(chǎng)景下展示智能避障的路徑規(guī)劃系統(tǒng)。

該仿真系統(tǒng)通過(guò)點(diǎn)擊屏幕視角的任意位置，粒子群就開(kāi)始尋路，待尋路算法運(yùn)行完畢就開(kāi)始行走，到達(dá)目標(biāo)點(diǎn)后自動(dòng)停止下來(lái)，等待用戶(hù)指定下一次任務(wù)。

初始小車(chē)只能按照程序中設(shè)置好的起點(diǎn)和終點(diǎn)進(jìn)行一次性的尋路，經(jīng)改進(jìn)后程序可以實(shí)現(xiàn)用鼠標(biāo)點(diǎn)擊俯視圖自定義終點(diǎn)的多次尋路，這樣可以方便有效地測(cè)試各種不同情況的尋路。由于每次尋路速度較快，不能較好地觀察比較尋路路線的優(yōu)劣，因此采用OpenGL畫(huà)圖，將粒子群所行走的路徑用連續(xù)線段標(biāo)記出來(lái)。

從仿真圖10和圖11中可以看出，使用可解空間法在大地圖中尋路能力相對(duì)較好，滿足了基于粒子群算法（PSO）路徑規(guī)劃系統(tǒng)設(shè)計(jì)的要求。

圖10 可解空間法尋路整體效果圖Fig.10Overall effect of path finding with solvable-space algorithm

圖11 俯視通道放大圖Fig.11Enlarge figure of overlooking channel

5 結(jié)語(yǔ)

提出的可解空間粒子群算法應(yīng)用于路徑規(guī)劃中，將地圖中的障礙物擴(kuò)展成一定的坡度進(jìn)而優(yōu)化，從而改善粒子群尋路時(shí)陷入局部最優(yōu)的情況。從仿真結(jié)果上看提高了粒子群搜索空間的成功率，在實(shí)際應(yīng)用中滿足了路徑規(guī)劃系統(tǒng)設(shè)計(jì)的要求。

（References）

［1］張德鋒，王華兵，薛原，等.基于Vega Prime的視景仿真技術(shù)研究與應(yīng)用［J］.計(jì)算機(jī)仿真，2006，23（7）：191-195.

［2］王孝平，董秀成，鄭海春，等.Vega Prime實(shí)時(shí)三維虛擬現(xiàn)實(shí)開(kāi)發(fā)技術(shù)［M］.成都：西南交通大學(xué)出版社，2012：20-90.

［3］魏博，吳國(guó)平，朱士峰.基于MFC框架的Vega虛擬場(chǎng)景設(shè)計(jì)和實(shí)現(xiàn)研究［J］.河南科學(xué)，2008，26（4）：454-458.

［4］劉波.粒子群優(yōu)化算法及其工程應(yīng)用［M］.北京：電子工業(yè)出版社，2010：70-110.

［5］孫超鋒.基于離散粒子群算法的機(jī)器人最優(yōu)路徑規(guī)劃研究［D］.南昌：華東交通大學(xué)，2011：21-27.

［6］張昉，張雷.基于改進(jìn)粒子群算法的UCAV二維路徑規(guī)劃［J］.航空兵器，2008（6）：3-7.

［7］陸楓，何云峰.計(jì)算機(jī)圖形學(xué)基礎(chǔ)［M］.北京：電子工業(yè)出版社，2008：30-50.

［8］LIU X，WEI H G，ZHOU C P，et al.The path planning for UAV based on orthogonal particle swarm optimization［C］//Pro?ceedings of international society for optical engineering.Wuhan：SPIE，2013，8921：89210X1-89210X9.

［9］ALEJO D，COBANO J A，HEREDIA G，et al.Particle Swarm Optimization for collision-free 4D trajectory planning in Un?manned Aerial Vehicles［C］//2013 International Conference on Unmanned Aircraft Systems.Atlanta：IEEE，2013：298-307.

［10］DAI Y，LIU L，LI Y，et al.An improved particle swarm optimization based on cellular automata［J］.International Journal of Computing Science and Mathematics，2014，5（1）：94-106.

（責(zé)任編輯：曾婷）

Scene Path Planning Simulation System Design by Solvable-Space Algorithm

ZHOU Jing1，F(xiàn)U Xuchang2
（1.School of Mathematics and Computer Sciences，Jianghan University，Wuhan 430056，Hubei，China；2.Department of Manufacturing Execution System Devlopment，Hongfujin Percision Industry Company Limited，Wuhan 430204，Hubei，China）

A new particle swarm solvable-space pathfinding algorithm is proposed，and the Vega Prime（VP）three-dimensional visual path planning simulation system is achieved.The algorithm as?sumes that each initial particle is a potentially viable solution and can reach the terminal point.Dur?ing the full iterations，each particle updates its position continually and saves the numbers of path points.When the search ended，find the particle whose number of the path points is fewest，and set this particle′s optimal position in each iteration as the wayfinding route.Meanwhile in VP large map scene，adding a circular gradient for the rectangular obstacles can easily solve the algorithm′s de?fect.The simulation system runs correctly proves the practicality of the algorithm and effectiveness of the system.

solvable-space algorithm；scene simulation；path planning；circular gradient

TP391.9

A

1673-0143（2014）06-0085-07

2014-09-09

武漢市教育局教研項(xiàng)目（2011059，2014016）；湖北省住房和城鄉(xiāng)建設(shè)廳科研開(kāi)發(fā)項(xiàng)目——空間信息融合的多目標(biāo)協(xié)同軌跡規(guī)劃應(yīng)用研究

周靜（1981—），女，講師，博士，研究方向：計(jì)算機(jī)可視化仿真、圖形圖像處理。