基于A*算法優(yōu)化的月面巡視器路徑規(guī)劃研究

(1 中國科學院遙感與數字地球研究所，北京 100101)(2 中國科學院大學，北京 100049)(3 北京空間飛行器總體設計部，北京 100094)

在我國的探月工程中，月面巡視器作為一個核心而又關鍵的系統(tǒng)，是目前對月球進行近距離探測的最直接有效的工具。月球表面地形復雜難以直觀觀測，巡視器的工作環(huán)境復雜且危險。因此在錯綜復雜的月球表面為巡視器規(guī)劃出一條安全、高效的路線是保證巡視器能穩(wěn)定工作的前提[1-2]。

月面巡視器所面臨的任務環(huán)境非常復雜，尤其是月球的非結構化月面環(huán)境使得月面巡視器的導航與控制研究極具挑戰(zhàn)性[3]。為了保證巡視器能夠適應其復雜任務環(huán)境并順利完成各種探測任務，需要巡視器具有高度的自主導航與控制能力。

國外，文獻[4]提出一種適用于裝備激光測距儀巡視器的路徑算法，該算法以Dijkstra算法為基礎，將地形傾斜度，地形粗糙度和路徑長度三個指標作為該算法的成本函數，在通過激光測距儀生成的數字高程圖(Digital Elevation Model，DEM)上規(guī)劃路徑。文獻[5]提出了一種基于人工支持的自動路徑規(guī)劃算法，該算法可根據地圖本身的特點設定路徑成本函數用以優(yōu)化數據，并與簡單的基于時間的成本函數相比，參與者能夠更好地優(yōu)化更復雜的成本函數。國內，文獻[6]探索出一種將A*算法和B樣條函數相結合的路徑規(guī)劃技術，適用于月面環(huán)境靜態(tài)且完全已知時做全局路徑規(guī)劃。文獻[7]提出一種基于膨脹柵格直方圖的方法，該方法考慮到傳感器測量的不確定性和巡視器的尺寸，在柵格地圖上對探測到的每個障礙物柵格點膨脹，該方法僅考慮路徑避障但沒有考慮到路徑的平滑性和連續(xù)性，需要巡視器多次進行原地轉向。文獻[8]給出一種改進的蟻群算法，解決傳統(tǒng)蟻群算法收斂速度慢、易陷入局部最優(yōu)解的缺陷，提高月球車導航過程中路徑規(guī)劃的能力，但需要在運動中不斷轉向，降低了行駛安全性。文獻[9]在柵格法的基礎上提出了一種基于Theta*算法的Block-Guided Theta*算法，但算法存在局限性，只對于研究巡視器在廣闊而平坦的月海區(qū)域的路徑規(guī)劃問題中有一定的參考價值。在月面路徑規(guī)劃研究方面已經提出了許多策略，并取得了不少成功的研究成果，但巡視器路徑規(guī)劃算法仍處在不斷的發(fā)展和完善之中。

本文提出了一種基于巡視器運動約束與月表地形特點的路徑規(guī)劃算法，引入輔助線思想，將全局地圖信息與局部地圖信息相結合，生成一種規(guī)劃成功率更高、行走效率更高的路徑規(guī)劃算法。

1 月面巡視器概況

六輪月面巡視器如圖1所示,長1.5 m，寬1 m，左右兩側每側3個車輪,車體前部桅桿上配置全景相機和導航相機用于獲取巡視器前方大范圍的圖像，生成全局地圖；車體前端避障相機用于獲取巡視器前方一定范圍內的精細地圖，為巡視器的局部路徑規(guī)劃提供詳細輸入條件。巡視器具備曲率轉向能力和原地轉向能力，一般情況下巡視器采用曲率轉向，最小轉彎半徑為1.5 m，特殊情況下采用原地轉向。該巡視器具有至少20°的爬坡越障能力[10]。

路徑規(guī)劃的主要輸入是依據立體相機拍攝生成的DEM，并根據DEM生成坡度圖。依據巡視器本身的爬坡性能將坡度圖轉化為可行區(qū)域與不可行區(qū)域的二值地圖(坡度閾值記為θT)。為了可以將巡視器視為質點進行路徑規(guī)劃，需要對地圖中的不可行區(qū)域進行膨脹操作，膨脹半徑rE一般大于巡視器長度的1/2。

2 基于局部信息的路徑規(guī)劃算法

2.1 基本原理

基于局部信息提出了滾動窗口避障算法，該算法利用局部地圖信息，通過評價函數在規(guī)定的可選圓弧中選取最優(yōu)圓弧作為執(zhí)行路徑，工作思路如下。

基于事先獲得的全局地圖(分辨率較低，主要用于指明前進方向)，設定起始點S和目標點G。然后巡視器開啟避障相機，進行感知路況，有可能探測到新的障礙，巡視器結合新的探測環(huán)境和原來的全局路徑方向進行局部路徑規(guī)劃和行走。每走一步，只考慮當前窗口即避障相機視場范圍內的環(huán)境。避障相機的視場范圍為巡視器前方半徑為Rv的半圓區(qū)域，以此來設置滾動窗口。依據巡視器的轉彎能力在滾動窗口內設置若干條待選圓弧，然后根據評價函數挑選一條最優(yōu)的圓弧作為規(guī)劃路徑。

依據六輪巡視器的尺寸與越障能力，本文設置二值地圖的閾值θT為巡視器最大越障坡度20°，膨脹半徑rE為1 m。圖2為根據坡度圖生成的二值地圖，該地圖尺寸為17 m×17 m，分辨率為0.01 m。圖中黑色區(qū)域為存在障礙物的不可行區(qū)域或相機視場外未知區(qū)域，白色區(qū)域為無障礙可行區(qū)域。

圖2 二值地圖Fig.2 Binary map

依據巡視器避障相機的性能，滾動窗口半徑Rv=3 m[11]，如圖3所示。依據巡視器的轉彎能力，在滾動窗口內設置11條待選圓弧，圓弧半徑長度固定為3 m，曲率半徑不同，左右兩邊分別取5檔，曲率半徑分別為10 m，5 m，3 m，2 m和1.5 m，中間是直線(可以認為是曲率半徑為∞的圓弧)。

圖3 滾動窗口11條可選圓弧Fig.3 Optional arcs in rolling window

選擇圓弧進行行走前需對圓弧進行二值判斷，分成可行圓弧和不可行圓弧，然后對可行圓弧的可行程度值進行計算。若滾動窗口內的圓弧沒有交到障礙，定義這樣的圓弧為可行圓弧，否則為不可行圓弧。對于可行圓弧，為保證留有足夠的空間進行轉向，取其前2/3弧段為待評價的行走圓弧，即對于3 m的弧段，選取前2 m進行行走和評價。

將圓弧終點到規(guī)劃目標的距離LEG作為搜索的啟發(fā)函數。行走圓弧的可行程度值評價函數為

fcir=LEG

(1)

在待選圓弧中選取fcir為最小值的一個作為行走圓弧。

滾動窗口避障算法規(guī)劃流程如圖4所示，具體步驟如下。

步驟1：初始時，巡視器朝向目標。

步驟2：對其前方的滾動窗口進行感知，擴展出11條圓弧。

步驟3：根據判斷條件選擇出可行的行走圓弧，若無可行圓弧，則判斷巡視器是否朝向目標：若不是則進行原地轉向使巡視器朝向目標，進行步驟2；若是則規(guī)劃結束，標記規(guī)劃失敗。

步驟4：對行走圓弧的可行程度值fcir進行評價。

步驟5：挑選fcir為最小值的行走圓弧作為行走路徑。

步驟6：路徑執(zhí)行后，到達新的地點后，更新滾動窗口，執(zhí)行步驟2至步驟6，直至巡視器到目標的距離小于一個行走步長。

步驟7：最后一步，巡視器原地轉向，嘗試沿直線到達目標。

圖4 局部路徑規(guī)劃流程圖Fig.4 Local path planning flow chart

2.2 仿真結果

以圖 2中的二值障礙圖為例，根據巡視器拍攝成像位置，設定起始點S像素坐標為(1460,1240)，目標點G像素坐標為(420,780)，其規(guī)劃路徑如圖5所示。圖5中黃色曲線為擴展圓弧，綠色曲線為可行的行走圓弧，藍色曲線為最終的執(zhí)行路徑。本次路徑規(guī)劃共執(zhí)行5次圓弧行走，在距離小于一個步長時原地轉向，通過直線到達目標點，路徑規(guī)劃成功。

該算法對于起始點與目標點的選擇具有一定的約束性。若選擇不當，如起始點S坐標不變，將目標點G坐標改為(540，710)，如圖6所示，巡視器執(zhí)行首段圓弧后過于靠近障礙物，轉向空間過小，待選圓弧皆不滿足安全性要求，嘗試原點轉向后仍無可行圓弧，路徑規(guī)劃失敗。

滾動窗口避障算法為啟發(fā)式路勁規(guī)劃算法，具有能快速趨向目標的優(yōu)點。其輸出路徑是連續(xù)平滑圓弧，符合實際應用的巡視器行走與控制要求。缺點在于容易陷入局部死區(qū)導致規(guī)劃失敗。

3 引入全局輔助線的路徑規(guī)劃算法

3.1 基本原理

為彌補基于局部信息的滾動窗口避障算法的缺點，本文引入輔助線思想進行優(yōu)化，同時使用局部信息與全局信息，首先生成一段從起點指向終點的折線段，在保證路徑連續(xù)平滑的前提下，使巡視器盡可能的沿著輔助線運動，最終到達終點。

本文擬使用A*算法生成輔助線。A*算法是人工智能中的一種啟發(fā)式搜索算法，它通過定義的代價函數來評估代價大小而確定最優(yōu)路徑，具有擴展節(jié)點少、魯棒性好的優(yōu)點，在全局路徑上應用廣泛[12-14]。引入全局輔助線的路徑規(guī)劃算法具體流程如下。

1.3 評定標準 3組患兒主要照顧者于治療前、治療1個月及3個月時填寫SAS及SDS評定量表。SAS以標準分≥50分為陽性，SDS以標準分≥53分為陽性。比較3組治療前后SAS、SDS評分及SAS、SDS陽性率。

進行A*算法規(guī)劃路線前需先為二值地圖劃分柵格，柵格半徑RG的設置會影響柵格賦值誤差、輔助線規(guī)劃速度與巡視器預留轉向空間尺寸。為減少誤差與計算時間以及保留一定的轉向空間，本文將柵格半徑RG設置為膨脹半徑rE的1/2。

接著對柵格賦值，本文認為柵格內的黑色像素數量大于柵格總像素的1/2，或者柵格正中心像素為黑色像素則該柵格為障礙柵格。圖7為賦值后的柵格地圖，其中白色柵格為可通行柵格，黑色柵格為障礙柵格。

圖7 柵格地圖Fig.7 Grid map

選取起點所在柵格作為A*算法的起始柵格，終點所在柵格作為A*算法的終點柵格。這里會引發(fā)一個問題，即起點(或終點)所在柵格有可能被劃定為障礙柵格，因而無法進行A*算法規(guī)劃。為解決這個問題，選取起點(或終點)周圍的8個柵格作為待選柵格，從中尋找非障礙柵格，并將此柵格作為新的起點(終點)柵格。若待選柵格中無非障礙柵格，則該起點(終點)實際位置可能為被障礙包圍的不可到達區(qū)域，需從新選擇起點(終點)坐標。

為了構造輔助線，需要尋找A*規(guī)劃路徑上的關鍵柵格，通過將關鍵柵格依次連接來生成輔助折線。

關鍵柵格定義如下：①相鄰關鍵柵格之間的線段不穿越障礙柵格；②非相鄰關鍵柵格之間的線段經過障礙柵格；③任意三個關鍵柵格不在同一直線上；④起始柵格與終點柵格均為關鍵柵格。

圖8中綠色柵格為A*算法規(guī)劃的路徑，藍色柵格為該路徑中的除起始點與目標點外的關鍵柵格。

圖8 提取關鍵柵格Fig.8 Extracting the key grids

依次連接關鍵柵格中心點，生成輔助線。圖9中藍色折線為生成的輔助線。

輔助線的使用方法如下。

首先將起始點巡視器的運動方向設置為和巡視器距離最近的輔助線段的方向，輔助線段方向為從靠近起始點的端點指向靠近目標點的端點的向量方向。更新滾動窗口并判定可行圓弧。擴展路徑的評價函數為

fcir=w1LEG+w2LEA+w3θEA

(2)

式中：w1，w2，w3為權重常數；LEG為圓弧末端點距終點的距離；LEA為圓弧末端點距輔助線的距離；θEA為圓弧末端點運動方向與距離最近的輔助線段的角度差值。

其中w1LEG為啟發(fā)函數，作用為使巡視器可以快速趨近于目標點，w1越大，趨近的速度越快。w2LEA+w3θEA為相似度函數，用來衡量可選圓弧與輔助線的相似程度。其中w2越大，選擇的執(zhí)行圓弧末端點越接近輔助線。w3越大，選擇的執(zhí)行圓弧末端點切線方向即巡視器行駛方向與輔助線方向更接近。w1決定了規(guī)劃路徑的長度，w2和w3共同決定了路徑與輔助線的相似度，即規(guī)劃的成功率，越相似成功率越高。當w1取1，w2和w3同時取0時評價函數變?yōu)閮?yōu)化前算法的評價函數。

選取fcir值最小的圓弧作為最終的執(zhí)行圓弧。

若無可行圓弧，則將巡視器原地轉向，使其朝向與最近的輔助線段方向相同。直至與終點的距離小于一個步長，再進行原地轉向，使巡視器朝向終點，嘗試直線行進到達終點。

圖9 生成輔助線Fig.9 Generates auxiliary lines

3.2 仿真結果

使用局部信息算例2相同的輸入，權重系數分別為w1=1，w2=1，w3=1；引入全局輔助線的規(guī)劃路徑如圖10所示。其中黃色圓弧為不可行圓弧，綠色圓弧為可行圓弧，藍色為選擇的執(zhí)行圓弧。

對于相同起始點與目標點的路徑規(guī)劃，引入全局輔助線前規(guī)劃失敗，引入后成功規(guī)劃出一條平滑連續(xù)的路徑。

圖10 輔助線算例1路徑規(guī)劃結果Fig.10 Auxiliary line example 1 path planning result

同時引入全局輔助線后可以有效的縮短行進路程。如圖 11所示，起始點坐標S(1460,1240)，目標點坐標G(420,780)。優(yōu)化后算法首先進行五次圓弧行走后距離目標點距離小于一個步長，與局部信息算例2對比可以清楚的發(fā)現(xiàn)，優(yōu)化后算法的最后一段直線距離小于優(yōu)化前算法最后一段直線的距離。

圖11 輔助線算例2路徑規(guī)劃結果Fig.11 Auxiliary line example 2 path planning result

4 權重系數優(yōu)化及對比試驗

本文對兩幅地圖分別使用優(yōu)化前算法(局部信息)和3組不同權重系數的優(yōu)化后算法(引入全局輔助線)進行4組仿真實驗。其中3組優(yōu)化后算法的權重參數分別設置如表1所示。

表1 3組優(yōu)化后算法權重參數

兩幅地圖的起始點與目標點設置圖12所示，其中紅點為起點，藍點為終點，每幅地圖共有3個不同起點，其中圖12(a)有60個不同終點，圖12(b)有65個不同終點，共組成375對起始點與目標點。對比實驗數據如表2所示。

圖12 起始點與目標點數據組示意圖Fig.12 Schematic diagram of starting point and target point data set

類別平均運動距離/m平均原地轉向次數/次規(guī)劃成功率/%圖12(a)圖12(b)圖12(a)圖12(b)圖12(a)圖12(b)優(yōu)化前起點A12.2011.191.211.1076.8193.84起點B11.8611.261.031.0292.7598.46起點C10.4811.201.131.0379.7198.46平均11.5111.221.121.0583.0996.92優(yōu)化后第一組起點A10.5111.511.001.00100.00100.00起點B9.9111.521.251.0088.2498.46起點C9.5611.301.001.00100.0098.46平均9.9911.441.081.0096.0798.97優(yōu)化后第二組起點A10.5111.481.001.00100.00100.00起點B9.9511.371.001.0079.41100.00起點C9.5411.251.001.00100.0096.92平均10.0011.371.001.0093.1498.97優(yōu)化后第三組起點A10.6811.521.001.00100.00100.00起點B10.0411.591.001.0098.53100.00起點C9.5711.461.001.00100.0098.46平均10.1011.521.001.0099.5199.49

由表2中數據可以看出，基于A*輔助線的路徑規(guī)劃方法優(yōu)點：

(1)有效縮短巡視器的運動距離；

(2)有效減少巡視器的原地轉向次數(受限起始點與目標點之間的障礙分布，極個別情況下會增加，如圖12(a)優(yōu)化后第一組起點B工況)；

(3)提高規(guī)劃成功率，且當w2與w3的值越大時，成功率越高。

實際應用時，應選擇權重常數使得w1LEG和w2LEA+w3θEA在一個數量級上，這既能保證路徑快速趨向目標，又能保證較好的沿著輔助線運動。僅對于兩幅地圖的仿真試驗，w1=0.1,w2=1.0,w3=1.0試驗組相較其他兩組試驗組能較好的滿足以上要求；試驗也證明此權重常數的試驗組相較于其他兩組試驗組成功率更高，且轉動次數更少，圖12(a)中算法的運動距離略長于其他兩組優(yōu)化后算法，但小于優(yōu)化前算法，路徑縮短量達到12.25%，且規(guī)劃成功率顯著提升，較優(yōu)化前算法提升了16%，規(guī)劃成功率超過99.00%。對于圖12(b)，算法的運動距離雖有增加，但提高了成功率。在實際應用中，應針對不同的地圖數據設定不同的權重系數，以滿足w1LEG和w2LEA+w3θEA在一個數量級上的要求。

本文同時使用月面巡視器運用優(yōu)化后算法進行試驗，圖13顯示了本文算法規(guī)劃的路徑與巡視器真實行走的車轍印，如圖所示，巡視器成功避開障礙物且行走路徑平滑。試驗表明，本文提出的算法在實際應用中具有可行性。

圖13 規(guī)劃路徑與車轍印Fig.13 Planning path and track

5 結束語

本文根據巡視器的感知能力和移動能力，采用滾動窗口和基于輔助線的行走圓弧評價方法規(guī)劃巡視器的局部路徑。該算法將全局路徑信息和局部路徑規(guī)劃結合在一起，避免路徑陷入局部死區(qū)，規(guī)劃成功率超過99.00%，顯著提高了成功率；面對復雜路況可獲取更短路徑，路徑縮短量達到12%；優(yōu)化后算法平均轉向次數較優(yōu)化前算法減少5%，提高了巡視器的行走效率。仿真和試驗證明了該方法的可行性，并在月面巡視器上進行了應用。后續(xù)將全局輔助線優(yōu)化為曲線以進一步提高規(guī)劃路徑的全局平滑性。