周相坡， 周依爾， 徐立軍， 李小路

0 引 言

月球巡航車的路徑規(guī)劃是指月球巡航車通過自身傳感器感知周圍環(huán)境，自行規(guī)劃出一條從起點(diǎn)到目標(biāo)點(diǎn)的安全無碰撞路徑，是巡航車完成自主探測任務(wù)的關(guān)鍵技術(shù)[1-2].月球表面的復(fù)雜地形增大了巡航車的探測難度，為保障巡航車探測過程中的安全，需要為其規(guī)劃安全可靠的路徑[3].傳統(tǒng)的路徑規(guī)劃算法難以滿足月球巡航車在安全方面的需求，本文旨在研究安全程度高的路徑規(guī)劃算法，保障月球巡航車能夠安全完成探測任務(wù).

路徑規(guī)劃算法主要分為基于圖搜索的路徑規(guī)劃算法和基于采樣的路徑規(guī)劃算法[4].基于圖搜索的算法主要包括Dijstra，A*和D*等算法，其中Dijstra算法是一種經(jīng)典的廣度優(yōu)先搜索算法，它從起點(diǎn)一層層向外擴(kuò)展搜索，能夠保證路徑搜索結(jié)果是最短的；A*算法在Dijstra的基礎(chǔ)上，增加了啟發(fā)式函數(shù)，能夠有目標(biāo)性地向著目標(biāo)點(diǎn)進(jìn)行搜索，提高了算法的效率[5]；D*算法對(duì)A*算法進(jìn)行了擴(kuò)展，可適用于動(dòng)態(tài)環(huán)境，D*算法可以智能緩存中間數(shù)據(jù)，在環(huán)境變化后，可以根據(jù)已規(guī)劃的路徑快速規(guī)劃新的路徑，“勇氣號(hào)”火星巡航車采用了基于D*算法改進(jìn)的Field-D*算法[6].基于采樣的路徑規(guī)劃算法主要包括RRT和PRM算法，其中RRT算法以起點(diǎn)作為根節(jié)點(diǎn)，通過隨機(jī)采樣增加葉子節(jié)點(diǎn)的方式，生成一棵隨機(jī)擴(kuò)展樹，當(dāng)隨機(jī)樹中的葉子節(jié)點(diǎn)進(jìn)入了目標(biāo)區(qū)域，便可以在隨機(jī)樹上找到一條從起始點(diǎn)到目標(biāo)點(diǎn)的路徑[7]；PRM算法則對(duì)地圖進(jìn)行了采樣處理，使用概率路線圖的形式表示地圖中的可通行區(qū)域，在生成概率路線圖后，可基于Dijstra或A*算法在概率路線圖上搜索最短路徑[8].目前對(duì)于月球巡航車的路徑規(guī)劃算法，主要考慮月球巡航車行駛過程中的代價(jià).文獻(xiàn)[9]基于Dijstra算法進(jìn)行路徑規(guī)劃，考慮了路徑的距離代價(jià)與影響太陽能板發(fā)電效率的光照代價(jià)，并分析它們的不同權(quán)重對(duì)路徑的影響.文獻(xiàn)[10]綜合考慮了地形可達(dá)、光照情況、通信可達(dá)等多種因素，基于A*算法設(shè)計(jì)了考慮地形行走代價(jià)、移動(dòng)里程代價(jià)、操作控制代價(jià)等因素的平滑曲線路徑搜索算法.

對(duì)于月球巡航車的路徑規(guī)劃算法，為了減少行駛代價(jià)，規(guī)劃的路徑往往會(huì)沿著障礙物邊緣，降低了路徑的安全性.月球巡航車沿著安全性低的路徑行駛時(shí)，存在碰撞障礙物的潛在風(fēng)險(xiǎn)，難以滿足月球巡航車在安全方面的需求.為提高路徑的安全性，保障月球巡航車的安全，本文提出了基于PRM的改進(jìn)算法，對(duì)PRM的采樣方式進(jìn)行改進(jìn)，生成遠(yuǎn)離障礙物的安全路徑.為了評(píng)價(jià)算法的安全性，提出了安全警戒系數(shù)和最小安全警戒系數(shù)兩個(gè)指標(biāo)，分別評(píng)估路徑整體和局部的安全水平.最后在月球仿真環(huán)境下進(jìn)行試驗(yàn)，對(duì)比PRM算法和A*，PRM算法，驗(yàn)證了改進(jìn)PRM算法的有效性.

1 方 法

1.1 PRM算法

PRM是一種基于采樣的路徑規(guī)劃算法，可以對(duì)地圖進(jìn)行簡化，提高路徑規(guī)劃的效率.

在PRM算法中，移動(dòng)機(jī)器人被看成一個(gè)質(zhì)點(diǎn)，且不考慮機(jī)器人的運(yùn)動(dòng)學(xué)約束.假設(shè)機(jī)器人的運(yùn)動(dòng)環(huán)境為一個(gè)含有若干障礙物的二維平面空間，記為C，機(jī)器人可以自動(dòng)移動(dòng)的區(qū)域記為Cfree.PRM的基本思想是通過概率路線圖表示機(jī)器人可運(yùn)行的自由空間Cfree.概率路線圖是一個(gè)無向圖，使用G(V,E)表示，其中V表示點(diǎn)集，表示在地圖中的位置；E表示邊集，表示位置間可直線通行的路徑.PRM算法包括學(xué)習(xí)階段和查詢階段.在學(xué)習(xí)階段，PRM算法對(duì)地圖進(jìn)行采樣，生成概率路線圖；在查詢階段，使用Dijstra或A*算法對(duì)概率路線圖進(jìn)行搜索，尋找起點(diǎn)到終點(diǎn)的最短路徑.

(1)學(xué)習(xí)階段.學(xué)習(xí)階段的算法流程如算法1所示.

算法1:Learning Phase輸入:map, k, l, 輸出:G(V,E)1: V = [], E = []2: while length(V) < k3: p=random(2)×size(map)4: if feasible(p)5: V=[V, p]6: end if7: end while8: forh in V9: forq in V10: if feasible() and |||| < l11: E=[E, ]12: end if13: end for14: end for15: return G(V, E)

圖1 概率路線圖Fig.1 Probabilistic roadmap

(2)查詢階段.首先將給定的起點(diǎn)s和終點(diǎn)g連接到概率路線圖中與其距離最近的節(jié)點(diǎn)，然后使用Dijstra或A*算法，從概率路線圖中搜索出起點(diǎn)到終點(diǎn)的最短路徑.

1.2 改進(jìn)的PRM算法

由PRM算法原理可知，PRM算法的采樣結(jié)果決定了概率路線圖，當(dāng)概率路線圖生成后，概率路線圖上的最短路徑是確定的，所以采樣結(jié)果決定了規(guī)劃的路徑.改進(jìn)PRM算法的基本思想是對(duì)原始PRM算法的采樣方式進(jìn)行改進(jìn)，通過增加遠(yuǎn)離障礙物區(qū)域的采樣概率，減少距離障礙物近的區(qū)域的采樣概率，控制采樣點(diǎn)盡量遠(yuǎn)離障礙物，從而控制規(guī)劃的路徑盡量遠(yuǎn)離障礙物，提升路徑的安全性.

為控制采樣點(diǎn)盡量遠(yuǎn)離障礙物，借助距離變換地圖進(jìn)行采樣.在圖像處理領(lǐng)域中，距離變換可用于計(jì)算圖像中的每一個(gè)非零點(diǎn)距離最近零點(diǎn)的距離，把二值圖像轉(zhuǎn)換成灰度圖像，每個(gè)點(diǎn)的灰度值等于它到最近非零點(diǎn)的距離[11-12].因此可借助距離變換將原始地圖中的每個(gè)點(diǎn)的值變換為該點(diǎn)到距其最近障礙物的距離，這樣的地圖稱為距離變換地圖.生成的距離變換地圖如圖2(b)所示，顏色越暗表示距離障礙物越近，顏色越亮表示距離障礙物越遠(yuǎn).

圖2 地圖和距離變換地圖Fig.2 Map and distance transform map

改進(jìn)PRM算法的采樣方式如算法2所示：

算法2:Sampling輸入:map, dmap, k, t輸出:V1: V = []2: while length(V) < k3: ps = []4: while length(ps) < t5: p=random(2)×size(map)6: if feasible(p)7: ps=[ps, p]8: end if9: end while10: pmax = max(dmap(ps))11: V= [V, pmax]12: end while13: returnV

采樣算法輸入為柵格地圖map，距離變換地圖dmap，采樣點(diǎn)數(shù)量k和采樣系數(shù)t.為了生成一個(gè)遠(yuǎn)離障礙物的采樣點(diǎn)pmax，首先在柵格地圖map中進(jìn)行多次隨機(jī)采樣，剔除落在障礙物上的掃描點(diǎn)后，得到t個(gè)采樣點(diǎn)ps；然后在距離變換地圖dmap中獲得t個(gè)采樣點(diǎn)ps中距離障礙物的距離，并選取距離障礙物最遠(yuǎn)的采樣點(diǎn)作為最終的采樣點(diǎn)pmax，t值越大，采樣點(diǎn)pmax遠(yuǎn)離障礙物的概率越大.將上述方法進(jìn)行k次，得到k個(gè)遠(yuǎn)離障礙物的采樣點(diǎn)pmax，將k個(gè)pmax組成點(diǎn)集V，然后將其輸出.

通過Sampling算法，可采樣得到遠(yuǎn)離障礙物的采樣點(diǎn)集V.改進(jìn)PRM算法的后續(xù)步驟與原始PRM相同，使用邊集V構(gòu)建概率路徑圖，并基于A*算法在概率路線圖中搜索最短路徑.改進(jìn)的PRM算法，可以控制采樣點(diǎn)遠(yuǎn)離障礙物，使規(guī)劃的路徑遠(yuǎn)離障礙物，提高路徑的安全性.

2 試 驗(yàn)

2.1 月球環(huán)境仿真

在月球表面地形數(shù)據(jù)中，月表撞擊坑和障礙物是決定建模逼真度的關(guān)鍵[13].假設(shè)只存在月表撞擊坑和障礙物，對(duì)月球仿真環(huán)境進(jìn)行建模.

撞擊坑是指布滿月球表面的環(huán)形凹坑構(gòu)造，包括撞擊坑環(huán)形山、輻射紋以及與撞擊坑有關(guān)的隆起構(gòu)造，撞擊坑主要由坑底、坑唇組成.經(jīng)過簡化處理的撞擊坑模型如圖3(a)所示.

圖3 撞擊坑與石塊模型Fig.3 Impact crater and rock model

采用雙拋物線擬合并繞z軸旋轉(zhuǎn)來構(gòu)造月球坑模型.坑底部分采用公式(1)計(jì)算，坑唇部分模型采用式(2)計(jì)算.

(1)

(2)

式(1)、(2)中，(x,y,z)是撞擊坑上的坐標(biāo)，Dp為坑直徑，dp為坑唇寬度，Hp為坑深，hp為坑唇高度.

障礙物主要是針對(duì)月面散落的大巖石塊.一個(gè)標(biāo)準(zhǔn)的月球巖石的形狀被認(rèn)為是它的最小尺度和最大尺度的比介于1和1/5之間；標(biāo)準(zhǔn)高度等于直徑的一半.經(jīng)過簡化處理的石塊模型如圖3(b)所示.

石塊模型選用理想的拋物線模型，采用下式計(jì)算：

(3)

式(3)中，hs為石塊的高度，ds為石塊寬度.

建立100 m×100 m的月球表面環(huán)境，基于建立的撞擊坑和石塊的數(shù)學(xué)模型基礎(chǔ)與月球地形地貌數(shù)據(jù)分析，在月球表面上，生成數(shù)目隨機(jī)，大小隨機(jī)的撞擊坑和石塊，結(jié)果如圖4所示.其中撞擊坑數(shù)目為3，石塊的數(shù)目為6，撞擊坑的最大寬度在5 m～25 m范圍內(nèi)，石塊的最大寬度在2 m～15 m范圍內(nèi).

圖4 月球環(huán)境仿真結(jié)果Fig.4 Lunar environment simulation results

2.2 柵格地圖生成

柵格地圖是用二進(jìn)制編碼的柵格來表示環(huán)境地圖，把不可通行的障礙柵格記為1(黑色)，可通行的自由柵格記為0(白色)，以此為基礎(chǔ)進(jìn)行路徑搜索.

為生成柵格地圖，首先對(duì)月球仿真環(huán)境進(jìn)行采樣，生成DEM數(shù)據(jù)，分辨率為0.2 m；然后從坡度，起伏度兩個(gè)方面提取月球地形信息，通過對(duì)坡度和起伏度的限制，生成柵格地圖[14-15].

為計(jì)算坡度和起伏度，設(shè)置如圖5所示的3×3滑動(dòng)窗口，其中Hi為滑動(dòng)窗口中第i個(gè)柵格的高程值.使用滑動(dòng)窗口遍歷月球DEM的所有數(shù)據(jù)，并將窗口內(nèi)的地形信息作為中心柵格的地形特征.

圖5 滑動(dòng)窗口Fig.5 Sliding window

地形的坡度α使用下式計(jì)算：

(4)

式(4)中，sx為中心柵格橫向高程變化率；sy為中心柵格縱向高程變化率.sx和sy的計(jì)算公式如下：

(5)

(6)

式(5)、(6)中，R為DEM數(shù)據(jù)的分辨率.

地形的起伏度Hg使用下式計(jì)算：

Hg=Hmax-Hmin

(7)

式(7)中，Hmax為滑動(dòng)窗口中高程最大值，Hmin為滑動(dòng)窗口中高程最小值.

使用滑動(dòng)窗口在月球DEM數(shù)據(jù)中依次計(jì)算，可以獲得DEM數(shù)據(jù)中每個(gè)點(diǎn)的地形信息.將坡度閾值設(shè)置為20o，起伏度閾值設(shè)置為R/2，對(duì)于超出閾值的柵格設(shè)置為障礙.據(jù)此將月球DEM數(shù)據(jù)轉(zhuǎn)換成生成的柵格地圖如圖6(a)所示.

圖6 柵格地圖Fig.6 Grid map

由圖6(a)可知，通過地形信息能夠有效的檢測出障礙物，但撞擊坑和石塊模型的中部會(huì)被判斷為可通行區(qū)域.在圖6(a)的基礎(chǔ)上，使用連通域?qū)ψ矒艨雍褪瘔K模型的中部進(jìn)行檢測，并在柵格地圖上將其修改為障礙物，結(jié)果如圖6(b)所示.

2.3 安全指標(biāo)提出

為評(píng)估路徑安全性，需要安全指標(biāo)對(duì)其進(jìn)行評(píng)估.目前，路徑的評(píng)估指標(biāo)主要包括路徑長度和規(guī)劃路徑消耗時(shí)間，評(píng)估其安全性的指標(biāo)較少.因此提出兩個(gè)安全指標(biāo)：安全警戒系數(shù)和最小安全警戒系數(shù).

為評(píng)價(jià)路徑的安全性，需要對(duì)路徑進(jìn)行均勻采樣，并在距離變化地圖上獲得每個(gè)采樣點(diǎn)距離障礙物的最小距離，得到集合D={d1,d2,…,dn}.

安全警戒系數(shù)為路徑中所有采樣點(diǎn)距離障礙物的平均距離，它反應(yīng)了路徑的整體安全水平.安全警戒系數(shù)Sf的計(jì)算公式表示如下：

(8)

最小安全警戒系數(shù)為路徑中所有采樣點(diǎn)距離障礙物的最近距離，它反應(yīng)了路徑的局部安全水平.最小安全警戒系數(shù)Sfmin的計(jì)算公式表示如下：

Sfmin=min(d1,d2,…,dn)

(9)

對(duì)于安全警戒系數(shù)和最小安全警戒系數(shù)，其計(jì)算結(jié)果數(shù)值越大，表明路徑安全性越高.

2.4 試驗(yàn)分析

在月球仿真環(huán)境下，將改進(jìn)的PRM算法與A*和PRM算法進(jìn)行對(duì)比.A*算法的啟發(fā)式函數(shù)設(shè)置為歐式距離，該啟發(fā)式函數(shù)可以保證A*算法搜索的路徑是最短的.A*算法在月球仿真環(huán)境下進(jìn)行路徑規(guī)劃的結(jié)果如圖7所示，靛藍(lán)色線為A*算法規(guī)劃的路徑，當(dāng)起點(diǎn)到終點(diǎn)存在可通行路徑，A*算法一定能夠搜索出一條可行路徑，所以A*算法具有完備性.

圖7 A*路徑規(guī)劃結(jié)果Fig.7 A* path planning results

不同于完備的A*算法，PRM算法是概率完備的，當(dāng)采樣點(diǎn)數(shù)較低時(shí)，PRM算法可能無法找到路徑，當(dāng)采樣點(diǎn)數(shù)逐漸增加時(shí)，PRM算法能夠找到可通行路徑的概率逐漸增加到1.將PRM算法的采樣點(diǎn)個(gè)數(shù)設(shè)置為340，步長閾值設(shè)置為13m，該參數(shù)能夠保障PRM算法以接近1的概率找到起點(diǎn)到終點(diǎn)的路徑.在上述參數(shù)下，PRM算法在月球仿真環(huán)境下進(jìn)行路徑規(guī)劃的結(jié)果如圖8所示，其中紅色點(diǎn)為點(diǎn)集，藍(lán)色線為邊集，靛藍(lán)色線為PRM算法規(guī)劃的路徑.由圖7和圖8可知，為保證路徑較短，A*算法和PRM算法都會(huì)選擇沿著障礙物的邊緣前進(jìn)，造成路徑的安全性低.月球巡航車沿著低安全性的路徑行駛時(shí)，存在碰撞障礙物的潛在風(fēng)險(xiǎn)，難以滿足月球巡航車在安全方面的需求.

圖8 PRM路徑規(guī)劃結(jié)果Fig.8 PRM path planning results

為提高路徑的安全性，基于PRM算法進(jìn)行改進(jìn).改進(jìn)后的PRM算法，不僅需要確定采樣點(diǎn)個(gè)數(shù)和步長閾值兩個(gè)參數(shù)，還需要確定采樣系數(shù)t.

當(dāng)采樣系數(shù)t過小時(shí)，如圖9(a)所示，采樣方式退化為隨機(jī)采樣，采樣點(diǎn)會(huì)均勻分布在柵格地圖上，規(guī)劃的路徑會(huì)沿著障礙物邊緣，無法提高路徑的安全性；當(dāng)采樣系數(shù)t過大時(shí)，如圖9(b)所示，采樣點(diǎn)會(huì)集中在距離障礙物較遠(yuǎn)的區(qū)域，柵格地圖中的起點(diǎn)和終點(diǎn)間無法找到可行路徑.

圖9 采樣系數(shù)對(duì)采樣結(jié)果的影響Fig.9 The influence of sampling coefficient on sampling results

為確定合適的采樣系數(shù)t，在采樣點(diǎn)數(shù)和步長閾值相同，采樣系數(shù)t不同的條件下，各進(jìn)行100次實(shí)驗(yàn)，對(duì)路徑規(guī)劃的成功率，安全警戒系數(shù)和最小警戒安全系數(shù)進(jìn)行統(tǒng)計(jì)，可得圖10所示曲線.由圖10可知，當(dāng)采樣系數(shù)t增大時(shí)，路徑規(guī)劃的成功率下降，安全警戒系數(shù)和最小警戒安全系數(shù)上升.當(dāng)采樣系數(shù)t大于3時(shí)，路徑規(guī)劃的成功率迅速下降至50%以下，安全警戒系數(shù)穩(wěn)步增長，最小安全警戒系數(shù)穩(wěn)定在1.10 m左右.為了平衡路徑規(guī)劃成功率，安全警戒系數(shù)和最小安全警戒系數(shù)，采樣系數(shù)t選擇為3.在保證改進(jìn)PRM算法有較高的安全警戒系數(shù)和最小安全警戒系數(shù)的前提下，具有較高的路徑規(guī)劃的成功率.對(duì)于不同的地形，可以提前計(jì)算出成功率和安全指標(biāo)的變化曲線，確定合適的采樣系數(shù)，并將其存放于采樣系數(shù)查找表.當(dāng)月球巡航車行駛在不同地形下，可以通過采樣系數(shù)查找表選取最合適的采樣系數(shù).

圖10 采樣系數(shù)的影響Fig.10 Effect of sampling factor

將采樣點(diǎn)個(gè)數(shù)設(shè)置為340，步長閾值設(shè)置為13 m，采樣系數(shù)t設(shè)置為3，改進(jìn)的PRM算法路徑規(guī)劃結(jié)果如圖11所示.由圖11可知，改進(jìn)的PRM算法，采樣點(diǎn)會(huì)遠(yuǎn)離障礙物，所以最終規(guī)劃的路徑也會(huì)遠(yuǎn)離障礙物，并且對(duì)于鄰近的兩個(gè)障礙物，采樣點(diǎn)有較大概率落在兩個(gè)障礙物的中心位置，所以路徑會(huì)從兩個(gè)障礙物中心穿過，避免緊貼障礙物前進(jìn)的危險(xiǎn)情況，提高了路徑的安全性.

圖11 改進(jìn)PRM算法的路徑規(guī)劃結(jié)果Fig.11 Improved PRM algorithm path planning results

在月球仿真環(huán)境下，對(duì)比A*算法，PRM算法和改進(jìn)PRM算法的路徑長度，消耗時(shí)間和安全警戒系數(shù)和最小安全警戒系數(shù)，數(shù)據(jù)整理為表1.計(jì)算機(jī)環(huán)境為CPU主頻2.2 GHz@Intel(R) i5，內(nèi)存8 GB，操作系統(tǒng)為Windows 10，實(shí)現(xiàn)軟件為MATLAB 2016.在路徑長度方面，A*算法具有規(guī)劃路徑最短的特點(diǎn)；PRM算法由于隨機(jī)采樣，規(guī)劃的路徑也是隨機(jī)的，不具備路徑最短的特點(diǎn)，其規(guī)劃的路徑較長；改進(jìn)的PRM算法為了尋找較安全的路徑而忽視了算法的長度，其規(guī)劃的路徑最長.在消耗時(shí)間方面，A*算法具有啟發(fā)式搜索的特點(diǎn)，能夠有目標(biāo)性地向著終點(diǎn)搜索路徑，其消耗時(shí)間最短；PRM算法在采樣方式是隨機(jī)、沒有方向性的，在構(gòu)建概率路線圖時(shí)消耗了大量時(shí)間；改進(jìn)的PRM算法，確定每一個(gè)采樣點(diǎn)時(shí)，都要隨機(jī)采樣t次，消耗時(shí)間高于PRM算法.在安全指標(biāo)方面，A*算法和PRM算法為使路徑盡量短，生成的路徑常沿著障礙物邊緣，所以安全警戒系數(shù)和最小安全警戒系數(shù)較低；改進(jìn)的PRM算法采用改進(jìn)的采樣方式，控制采樣點(diǎn)盡量遠(yuǎn)離障礙物，所以生成的路徑也會(huì)盡量遠(yuǎn)離障礙物，其安全警戒系數(shù)和最小安全警戒數(shù)值最高，說明其最安全.實(shí)際月球環(huán)境相對(duì)于仿真月球環(huán)境更加復(fù)雜，路徑規(guī)劃算法的安全性都會(huì)降低，A*算法、PRM算法和改善的PRM算法在路徑安全性上的差距會(huì)更加明顯.

表1 路徑規(guī)劃指標(biāo)Tab.1 Path planning indicators

3 結(jié) 論

為提高月球巡航車在復(fù)雜環(huán)境下行駛的安全性，本文提出一種基于PRM的改進(jìn)路徑規(guī)劃算法.該算法借助距離變換地圖對(duì)PRM算法的采樣方式進(jìn)行改善，令采樣點(diǎn)盡量遠(yuǎn)離障礙物，提高了路徑的安全性.為評(píng)估路徑的安全性，本文提出了安全指標(biāo)：安全警戒系數(shù)和最小安全警戒系數(shù)，并在月球表面仿真環(huán)境下對(duì)A*，PRM和改進(jìn)的PRM算法生成的路徑進(jìn)行安全評(píng)估，結(jié)果表明改進(jìn)的PRM算法能夠有效地提升路徑的安全性.