環(huán)境地圖的格柵化及路徑規(guī)劃研究

劉正鋒，張隆輝，魏納新，匡曉峰

(中國(guó)船舶科學(xué)研究中心 水動(dòng)力學(xué)重點(diǎn)實(shí)驗(yàn)室，江蘇 無錫 214082)

0 引 言

隨著人工智能技術(shù)的不斷發(fā)展，水面無人艇（Unmanned Surface Vehicle，USV）作為一種無人化與智能化作戰(zhàn)平臺(tái)，在軍用和民用領(lǐng)域都有著廣泛的應(yīng)用前景。根據(jù)底層任務(wù)要求，水面無人艇需要具備在包含靜態(tài)和動(dòng)態(tài)障礙物的復(fù)雜海域環(huán)境下進(jìn)行安全有效自主航行的能力。路徑規(guī)劃是無人艇自主航行的前提，是無人艇智能化程度的重要體現(xiàn)，也是無人艇研究領(lǐng)域的熱點(diǎn)問題[1-2]。

環(huán)境地圖是開展路徑規(guī)劃任務(wù)的前提。一般來講，水面無人艇在航行過程中，可以通過雷達(dá)、攝像機(jī)等傳感設(shè)備獲得船舶周圍局部環(huán)境信息，還需要電子海圖提供全局環(huán)境信息。環(huán)境地圖通常由復(fù)雜幾何圖形構(gòu)成，使得路徑規(guī)劃時(shí)大多數(shù)搜索算法不能被直接應(yīng)用，并且地圖的規(guī)模直接影響著路徑規(guī)劃的效率，因此需要對(duì)環(huán)境地圖進(jìn)行處理[3-6]。柵格地圖是最常用的處理及表達(dá)形式。格柵地圖將環(huán)境表示為具有一定分辨率的方形柵格網(wǎng)，柵格根據(jù)是否被障礙物占據(jù)分為自由狀態(tài)和占據(jù)狀態(tài)，柵格之間相互獨(dú)立。格柵地圖創(chuàng)建簡(jiǎn)單，便于路徑規(guī)劃實(shí)現(xiàn)，因此被廣泛應(yīng)用于機(jī)器人、自動(dòng)駕駛等領(lǐng)域。地圖的格柵化有2個(gè)重要參數(shù)—— 地圖尺寸和柵格大小。地圖尺寸需要足夠大，能覆蓋自由航行區(qū)域，同時(shí)又希望盡可能小，提高存儲(chǔ)以及計(jì)算的效率。顯然，地圖大小一定時(shí)，柵格的數(shù)量和柵格大小是成反比的。太小太精細(xì)的柵格會(huì)導(dǎo)致柵格的數(shù)量迅猛增長(zhǎng)，急劇地降低路徑規(guī)劃的效率；太大的網(wǎng)格會(huì)提高路徑規(guī)劃的計(jì)算速度，但是會(huì)影響規(guī)劃路徑的精度。有學(xué)者提出了“有義地圖率”的概念，討論了柵格大小、傳感器精度與有義地圖率之間的關(guān)系；并提出了柵格準(zhǔn)確度和柵格信息量為變量的代價(jià)函數(shù)來評(píng)估占據(jù)地圖精度的方法[7]。因此，選擇合理的柵格尺度降低地圖規(guī)模并盡量保持地圖的邊界特征，是柵格地圖建模及提高路徑規(guī)劃效率的關(guān)鍵。

環(huán)境地圖的格柵化降低了實(shí)際環(huán)境中障礙物的處理難度，因此基于柵格地圖的搜索方法在路徑規(guī)劃中得到了廣泛的關(guān)注。當(dāng)規(guī)劃環(huán)境稀疏時(shí)，可以采用深度優(yōu)先搜索、廣度優(yōu)先搜索、Dijkstra算法[8]等準(zhǔn)確高效地找到全局最短路徑。然而隨著規(guī)劃環(huán)境的規(guī)模和復(fù)雜度增加時(shí)，求解全局最優(yōu)路徑的計(jì)算量會(huì)急劇增加，此時(shí)精確算法很難在短時(shí)間內(nèi)得到滿意解。啟發(fā)式搜索算法犧牲了求解質(zhì)量換取計(jì)算速度，可以在較短時(shí)間內(nèi)得到最優(yōu)解。A*（A-star）算法由Dijkstra算法發(fā)展而來，是目前最常用，也是應(yīng)用最廣的一種啟發(fā)式搜索算法[9-12]。算法中加入了目標(biāo)點(diǎn)到當(dāng)前路徑點(diǎn)的距離估計(jì)代價(jià)，綜合考慮了從起點(diǎn)到當(dāng)前路徑點(diǎn)的距離以及經(jīng)由路徑點(diǎn)到達(dá)終點(diǎn)的距離來決定搜索的方向。A*算法主要用于靜態(tài)柵格地圖的最優(yōu)路徑搜索，通常有兩方面的改進(jìn)內(nèi)容：提高搜索速度以及提高規(guī)劃的可行性。目前，提高路徑搜索速度的方法主要通過使用曼哈頓距離和對(duì)角線距離作為代價(jià)函數(shù)，引入決勝法來處理評(píng)價(jià)值相等的情況，同時(shí)進(jìn)行跳躍點(diǎn)搜索、雙向搜索等手段。

本文對(duì)環(huán)境地圖的格柵化及路徑規(guī)劃進(jìn)行研究，介紹了柵格地圖建模方法，利用等效網(wǎng)格對(duì)障礙物邊界區(qū)域網(wǎng)格進(jìn)行改進(jìn)，保證了規(guī)劃環(huán)境的空間連通性。闡述了A*算法在柵格地圖路徑規(guī)劃的實(shí)現(xiàn)流程，最后以某海域環(huán)境為例進(jìn)行了仿真驗(yàn)證，對(duì)比分析了環(huán)境地圖格柵化對(duì)路徑規(guī)劃的影響。

1 柵格地圖建模

一般來講，水面船舶在航行過程中，可以通過雷達(dá)、攝像機(jī)等傳感設(shè)備來獲得船舶周圍局部環(huán)境信息，還需要根據(jù)電子海圖來獲取全局環(huán)境信息。這些環(huán)境信息無法直接應(yīng)用到水面無人艇路徑規(guī)劃中，需要將電子海圖轉(zhuǎn)化為可被利用的環(huán)境模型。柵格法表述簡(jiǎn)單且容易實(shí)現(xiàn)，可以應(yīng)用于不同的搜索算法，是目前地圖建模中應(yīng)用最為廣泛的一種方法。通常，柵格法在處理路徑規(guī)劃問題時(shí)，利用柵格對(duì)規(guī)劃環(huán)境空間進(jìn)行劃分，并用2種顏色（白與黑）或數(shù)字（1和0）將柵格分為可行區(qū)（自由柵格）與不可行區(qū)（障礙柵格）2種情況，同時(shí)對(duì)劃分后的柵格進(jìn)行編碼，構(gòu)建柵格地圖模型。障礙柵格是環(huán)境中障礙物進(jìn)過膨脹或者腐蝕處理后填充到柵格中形成的，黑色表示被障礙占據(jù)，白色表示空白區(qū)域，如圖1所示。

圖1 不同網(wǎng)格尺度下的柵格地圖Fig.1 Grid maps in different grid scales

柵格的分辨率（網(wǎng)格尺度）是柵格地圖的特征體現(xiàn)。當(dāng)柵格的分辨率過高時(shí)（網(wǎng)格尺度過?。?，柵格數(shù)量的急劇增加，使得路徑規(guī)劃計(jì)算存儲(chǔ)量顯著增加（圖1（a）和圖1（b）），而柵格的分辨率過低會(huì)導(dǎo)致規(guī)劃區(qū)域特征過于粗糙，無法得到有效路徑，甚至導(dǎo)致規(guī)劃區(qū)域的連通性被破壞，使得路徑規(guī)劃失敗（圖1（c））。圖1所示格柵地圖中，障礙物邊界在柵格內(nèi)，該柵格就被處理為不可通行網(wǎng)格，這種極端的做法勢(shì)必會(huì)影響規(guī)劃地圖的連通性，當(dāng)網(wǎng)格尺度較大時(shí)尤為明顯（圖1（c））。可以看出，選取合理的柵格尺度是規(guī)劃環(huán)境建模的關(guān)鍵，同時(shí)障礙物邊界網(wǎng)格的處理也會(huì)影響規(guī)劃空間的連通性。為尋求最大限度降低路徑規(guī)劃的復(fù)雜度并保證路徑規(guī)劃的成效，需要尋求網(wǎng)格尺度和規(guī)劃區(qū)域特征之間的平衡，在降低規(guī)劃空間規(guī)模的同時(shí)保證規(guī)劃空間的連通性。岳偉韜等[7]提出了“有義地圖率”表征占據(jù)格柵地圖準(zhǔn)確度的概念，分析了柵格大小、有義地圖率的影響。

為保證規(guī)劃空間的局部特征及連通性，本文對(duì)障礙物邊界采用等效網(wǎng)格來替代。類比多孔介質(zhì)流動(dòng)特性，設(shè)定一個(gè)閥值，當(dāng)網(wǎng)格中障礙物占比大于閥值，則認(rèn)為該網(wǎng)格是障礙柵格，不可通行；反之則認(rèn)為該網(wǎng)格是自由網(wǎng)格，可通行。假設(shè)規(guī)劃區(qū)域范圍為X×Y，以尺度Scale進(jìn)行格柵化處理，則輸出的柵格地圖規(guī)?？杀硎緸閚x×ny，其中：

障礙物Obs在區(qū)域 [i-1:i,j-1:j]·Scale所對(duì)應(yīng)柵格 (j,i)的占據(jù)率為：

設(shè)障礙占比閥值為a0，則柵格的狀態(tài)可以通過下式確定：

圖2給出了圖1（a）在較粗網(wǎng)格下不同閥值時(shí)的格柵地圖，表1給出了不同閥值改進(jìn)后的障礙物占比統(tǒng)計(jì)值。不難看出，自由空間與障礙物交界面網(wǎng)格的狀態(tài)得到了顯著改進(jìn)，柵格地圖在維持規(guī)劃空間圖形特征的同時(shí)連通性也得到了保障。需要注意的是，較小的閥值同樣會(huì)導(dǎo)致邊界處自由空間被占據(jù)，而較大的閥值會(huì)使得邊界處障礙信息被抹掉。結(jié)合表1并參考二維多孔介質(zhì)格子模型逾滲概率的臨界值0.6，一般情況下障礙占比臨界閥值a0可取為0.4來保持規(guī)劃空間格柵化后的連通性特征。

圖2 不同閥值的柵格地圖(32×24)Fig.2 Grid maps with different thresholds( 32×24)

表1 不同閥值下障礙物柵格占比Tab.1 Percentage of obstacle grid under different thresholds

2 A*算法

環(huán)境地圖的格柵化處理降低了實(shí)際環(huán)境中障礙物處理的難度。當(dāng)環(huán)境地圖經(jīng)格柵化轉(zhuǎn)化為柵格地圖，無人艇路徑規(guī)劃問題就轉(zhuǎn)化為在柵格地圖中尋找2個(gè)給定網(wǎng)格點(diǎn)之間的最優(yōu)路徑問題，可以通過基于柵格的搜索方法來解決。A*搜索算法是一種啟發(fā)式的搜索算法，也是目前路徑規(guī)劃技術(shù)中最受歡迎的算法，在無人系統(tǒng)路徑規(guī)劃和路徑尋優(yōu)等領(lǐng)域有著廣泛的應(yīng)用。本文采用A*算法來進(jìn)行最優(yōu)路徑的搜索。

A*算法是在Dijkstra 算法的基礎(chǔ)上發(fā)展起來的，它通過構(gòu)建以當(dāng)前節(jié)點(diǎn)與起始節(jié)點(diǎn)兩者間的實(shí)際距離代價(jià)加上當(dāng)前節(jié)點(diǎn)與目標(biāo)節(jié)點(diǎn)的預(yù)估代價(jià)的啟發(fā)式評(píng)價(jià)函數(shù)來對(duì)當(dāng)前節(jié)點(diǎn)進(jìn)行對(duì)比篩選。在路徑規(guī)劃過程中，A*算法的評(píng)價(jià)函數(shù)如下式：

其中：f(Vi)表示起始節(jié)點(diǎn)S經(jīng)由當(dāng)前節(jié)點(diǎn)Vi和目標(biāo)節(jié)點(diǎn)G構(gòu)成的評(píng)價(jià)函數(shù)；g(Vi)表示起始節(jié)點(diǎn)S到當(dāng)前節(jié)點(diǎn)Vi的實(shí)際距離代價(jià)，h(Vi)表示當(dāng)前節(jié)點(diǎn)Vi與目標(biāo)節(jié)點(diǎn)G之間的估算距離代價(jià)。f(Vi)值越小，表示經(jīng)由Vi到達(dá)目標(biāo)點(diǎn)G的總代價(jià)越小。A*算法具有超強(qiáng)的靈活性與適應(yīng)性，根據(jù)不同的搜索任務(wù)可以設(shè)計(jì)針對(duì)性的代價(jià)函數(shù)。估算距離代價(jià)h(Vi)屬于啟發(fā)函數(shù)，決定A*算法效率的高低，在評(píng)價(jià)函數(shù)中起關(guān)鍵作用。若h(Vi)為 0，就只有g(shù)(Vi)起作用，A*算法簡(jiǎn)化為 Dijkstra算法。

對(duì)于啟發(fā)函數(shù)的計(jì)算方法由很多種，對(duì)于柵格法建立的環(huán)境模型，傳統(tǒng)的計(jì)算方法有2種，一種是歐氏距離估計(jì)，另外一種是曼哈頓距離估計(jì)。為了便于計(jì)算，在充分考慮算法效率的基礎(chǔ)上，本文選取歐氏距離估計(jì)作為啟發(fā)式評(píng)價(jià)函數(shù)。

歐氏距離估計(jì)：

曼哈頓距離估計(jì)：

其中當(dāng)前節(jié)點(diǎn)Vi的坐標(biāo)為 (xi,yi)，目標(biāo)節(jié)點(diǎn)G的坐標(biāo)為(xg,yg)，a表示單位長(zhǎng)度代價(jià)，即正方形柵格的邊長(zhǎng)。從起點(diǎn)S到達(dá)目標(biāo)點(diǎn)G，在柵格地圖中搜索最優(yōu)路徑的流程如圖3所示。

A*算法的搜索流程如圖3所示。

圖3 A*算法流程圖Fig.3 Flow chart of A* algorithm

3 仿真研究

為分析地圖格柵化對(duì)路徑規(guī)劃的影響，以某海域地圖為例進(jìn)行路徑規(guī)劃仿真。規(guī)劃環(huán)境地圖區(qū)域范圍為18km×9km，假設(shè)水面無人艇需要從某水域自主航行至某水域，起點(diǎn)至目標(biāo)點(diǎn)直線距離約1 2 km。仿真環(huán)境用Matlab軟件開發(fā)，并將水面無人艇簡(jiǎn)化為質(zhì)點(diǎn)處理。圖4為二值化地圖，黑色范圍表示陸地、島嶼等無人艇無法通行的區(qū)域，白色范圍表示可以通行的水域。

圖4 二值化地圖Fig.4 Binary harbor map

二值化后地圖規(guī)模約100萬量級(jí)像素點(diǎn)，每個(gè)像素點(diǎn)代表距離約12.8 m。若將每個(gè)像素點(diǎn)作為柵格處理，地圖規(guī)模龐大，不利于路徑規(guī)劃的高效實(shí)現(xiàn)，需要適當(dāng)增大柵格尺度來降低柵格地圖規(guī)模，并通過障礙物占比閥值來調(diào)節(jié)障礙物邊界處柵格的狀態(tài)。

圖5～圖7及表2給出了障礙物占比閥值為0.4時(shí)，不同柵格尺度時(shí)的規(guī)劃路徑圖及結(jié)果統(tǒng)計(jì)。當(dāng)柵格尺度變大時(shí)，柵格地圖規(guī)模顯著下降，路徑規(guī)劃的效率得到了提升，但柵格地圖信息變得粗糙，規(guī)劃路徑結(jié)果差異顯著。另外，從圖5～圖7對(duì)比可以看出，選擇10個(gè)像素點(diǎn)作為網(wǎng)格尺度進(jìn)行格柵化，在保證地圖信息完整的同時(shí)，得到的規(guī)劃路徑與精細(xì)網(wǎng)格下的路徑相近，并且規(guī)劃效率得到了顯著的提升。

圖5 Scale=5時(shí)路徑規(guī)劃結(jié)果Fig.5 Path planning result (Scale=5)

圖6 Scale=10時(shí)路徑規(guī)劃結(jié)果Fig.6 Path planning result (Scale=10)

圖7 Scale=20時(shí)路徑規(guī)劃結(jié)果Fig.7 Path planning result (Scale=20)

圖8～圖11及表3給出了相同柵格尺度（10個(gè)像素點(diǎn)）下，不同障礙占比閥值時(shí)的規(guī)劃路徑及結(jié)果統(tǒng)計(jì)。不難看出，當(dāng)前尺度格柵化后的環(huán)境地圖基本都能保持原始地圖區(qū)域特征，并且都能順利完成路徑規(guī)劃任務(wù)，得到規(guī)劃路徑。但是隨著障礙占比閥值的減小，交界面處的網(wǎng)格信息會(huì)有所不同，障礙物柵格會(huì)有所增加，減弱了規(guī)劃區(qū)域的通行性，在狹窄航行區(qū)域尤為明顯，同時(shí)路徑規(guī)劃所需的時(shí)間會(huì)明顯增加。而從規(guī)劃路徑的結(jié)果對(duì)比可以看出，閥值大于0.4時(shí)，所得的規(guī)劃路徑幾乎一致，所需花費(fèi)時(shí)間也大致相同；當(dāng)閥值小于0.4時(shí)，所得的規(guī)劃路徑差異明顯，這從一定程度上說明障礙占比閥值取0.4可行。需要說明的是，障礙物占比閥值取得越小，允許柵格內(nèi)部存在障礙物的可能性就越低，網(wǎng)格通行的成功率就越高，所獲得的規(guī)劃路徑就更為安全。

圖8 a0 = 0.8時(shí)路徑規(guī)劃結(jié)果Fig.8 Path planning result (a0 = 0.8)

圖9 a0 = 0.4時(shí)路徑規(guī)劃結(jié)果Fig.9 Path planning result (a0 = 0.4)

圖10 a0 = 0.2時(shí)路徑規(guī)劃結(jié)果Fig.10 Path planning result (a0 = 0.2)

圖11 a0 = 0時(shí)路徑規(guī)劃結(jié)果Fig.11 Path planning result (a0 = 0)

表3 不同占據(jù)率下的路徑長(zhǎng)度Tab.3 Path length of different obstacle occupancy

4 結(jié) 語

環(huán)境地圖建模是無人艇導(dǎo)航和路徑規(guī)劃的重要組成部分。本文主要討論了網(wǎng)格尺度及障礙物邊界處理對(duì)柵格地圖建模及路徑規(guī)劃的影響，并以某海域?yàn)槔M(jìn)行了路徑規(guī)劃對(duì)比分析。結(jié)果表明：

1）選擇適中的網(wǎng)格尺度和障礙占比閥值對(duì)環(huán)境地圖進(jìn)行格柵化，規(guī)劃空間的圖形特征能嚴(yán)格保持，利用A*算法可以快速高效地在規(guī)劃空間求得規(guī)劃路徑；

2）大網(wǎng)格尺度會(huì)顯著降低柵格地圖的規(guī)模，提高路徑規(guī)劃的效率，但是可能降低規(guī)劃地圖的空間連通性，甚至?xí)?dǎo)致路徑規(guī)劃失敗，因此需要通過障礙物占比閥值來適當(dāng)?shù)卣{(diào)節(jié)障礙物邊界處網(wǎng)格的狀態(tài)；

3）障礙占比閥值的大小影響著障礙物邊界占據(jù)網(wǎng)格的狀態(tài)，也會(huì)影響規(guī)劃空間的連通性。當(dāng)柵格尺度較大時(shí)，較大的閥值會(huì)提高規(guī)劃空間的連通性；當(dāng)柵格尺度較小時(shí)，較小的閥值會(huì)提高規(guī)劃路徑的安全性，但是會(huì)導(dǎo)致規(guī)劃效率的降低。