行星車自主探測融合路徑規(guī)劃

郭繼峰，于曉強(qiáng)，鄭紅星，顏 鵬

（哈爾濱工業(yè)大學(xué)，哈爾濱150001）

1 引 言

隨著航天技術(shù)的不斷發(fā)展，各航天大國都相繼制定了目標(biāo)宏偉、各具特色的深空探測計(jì)劃［1］，在這些計(jì)劃中，具備自主感知、規(guī)劃和控制能力的行星車是各國研究計(jì)劃中的重要組成部分。例如，美國噴氣推進(jìn)實(shí)驗(yàn)室提出的全地形六角形地形探測者計(jì)劃［2］以及美國國家航空航天局（NASA）提出的空間探索飛行器［3］等，旨在開發(fā)具備高自主、全地形轉(zhuǎn)移能力的巡視機(jī)器人。歐空局火星外空生物學(xué)計(jì)劃擬開發(fā)出可以在火星表面自動導(dǎo)航的行星車，用于尋找火星生命過去在火星上的生物標(biāo)記［4］。日本宇航局和NASA 合作參與阿爾忒彌斯計(jì)劃，正在共同研發(fā)具備超過10000 公里巡視能力的氫動力行星車，以支持未來月球基地建設(shè)的計(jì)劃［5］。我國的嫦娥四號探月工程及天問一號火星探測也分別開發(fā)出了具備自主探測能力的行星車。行星車的自主移動及探測能力無疑會給行星探測提供更加自主、魯棒、高效的探測模式，在深空探測領(lǐng)域具有重要的研究意義。

路徑規(guī)劃技術(shù)作為行星車自主探測中最重要的一環(huán)，是行星車能夠進(jìn)行安全巡視的必需技術(shù)。行星車的路徑規(guī)劃方法可按照環(huán)境信息獲取手段的不同分為基于離線地圖信息的全局路徑規(guī)劃和基于實(shí)時(shí)傳感信息的局部路徑規(guī)劃。

針對行星探測的全局路徑規(guī)劃，文獻(xiàn)［6］提出了一種基于月面數(shù)字高程地圖的大范圍自主探測快速安全路徑規(guī)劃算法，在保證路徑安全性的基礎(chǔ)上加快了搜索速度，提高了月球車自主探測過程的效率及安全性。文獻(xiàn)［7］根據(jù)履帶式和輪式巡視機(jī)器人不同的運(yùn)動特性，分析了行星車自身能力及外部環(huán)境約束對規(guī)劃路徑的影響，并對其規(guī)劃路徑進(jìn)行了評估，驗(yàn)證了方法的有效性。目前全局規(guī)劃算法著重于在一定評價(jià)標(biāo)準(zhǔn)下（如路徑長度最短、消耗能量最少等）求解出最優(yōu)路徑，缺少對行星車動力學(xué)約束以及路徑可行性的考慮。

針對行星探測局部路徑規(guī)劃，文獻(xiàn)［8］提出了基于深度強(qiáng)化學(xué)習(xí)理論的路徑規(guī)劃方法，解決了傳統(tǒng)規(guī)劃算法對先驗(yàn)地圖信息的依賴性。該方法直接從傳感器信息映射出動作指令，再發(fā)布給行星車，仿真實(shí)驗(yàn)表明，該算法對不同環(huán)境均具有一定的適應(yīng)性。文獻(xiàn)［9］提出基于深度強(qiáng)化學(xué)習(xí)方法的行星車協(xié)同探測方法，可以根據(jù)不同的環(huán)境調(diào)整自身的策略，使行星車具備自學(xué)習(xí)能力，可以快速有效地探測給定區(qū)域。目前局部路徑規(guī)劃主要針對較小區(qū)域且考慮精確約束的避障路徑規(guī)劃，缺少對路徑全局最優(yōu)性的考慮，會出現(xiàn)探測結(jié)果偏離目標(biāo)點(diǎn)甚至使全局目標(biāo)點(diǎn)不可達(dá)的情況。

綜上，本文提出了一種全局與局部融合的行星探測路徑規(guī)劃方法。首先基于行星表面數(shù)字高程模型（Digital Elevation Model，DEM）生成了綜合平滑度地圖，然后提出了一種融合改進(jìn)A*算法和動態(tài)窗口法的融合路徑規(guī)劃方法，基于綜合平滑度信息設(shè)計(jì)了啟發(fā)式函數(shù)，用于指導(dǎo)A*算法生成全局最優(yōu)路徑，然后應(yīng)用動態(tài)窗口法進(jìn)行實(shí)時(shí)路徑規(guī)劃，生成滿足行星車能力約束的平滑路徑，從而在探測路徑全局最優(yōu)的基礎(chǔ)上保證路徑可行，提高了行星探測任務(wù)的成功性和可靠性。最后選取月球赤道附近地形具有代表性特征的區(qū)域作為仿真場景，驗(yàn)證了本文方法的有效性。

2 綜合平滑度地圖

2.1 地形特征提取

行星表面DEM 地圖是獲取行星地形信息的主要方式。DEM 數(shù)據(jù)以柵格形式存儲了行星表面每個(gè)位置的高程信息，不能直接作為路徑規(guī)劃中的環(huán)境地圖，需要在DEM 數(shù)據(jù)中分析提取地形特征。本文主要從坡度、起伏度以及粗糙度三個(gè)方面對行星表面的基本地形要素進(jìn)行分析和提取，采用滾動窗口的形式計(jì)算DEM 地圖每個(gè)柵格內(nèi)的地形特征，即通過設(shè)置一個(gè)3×3 大小的滾動窗口，將窗口內(nèi)的地形信息作為中心柵格的地形特征。

地形坡度θ可由下式計(jì)算：

式中，fx、fy為中心柵格東西方向和南北方向高程變化率，采用三階不帶權(quán)差分方法計(jì)算得到：

式中，H1～H9分別為3×3窗口內(nèi)部的9個(gè)柵格對應(yīng)的高程值，p為DEM數(shù)據(jù)的分辨率大小。

地形起伏度R可由窗口內(nèi)高程最大值與高程最小值的差計(jì)算而來，即：

式中，Hmax、Hmin分別表示窗口內(nèi)的最大高程值與最小高程值。

地形粗糙度δ可由窗口內(nèi)所有點(diǎn)的高程值標(biāo)準(zhǔn)差計(jì)算而來，即：

式中，Hi為窗口內(nèi)所有柵格對應(yīng)的高程值為窗口內(nèi)所有高程值的平均值。

通過滾動窗口在DEM 地圖上依次計(jì)算，可以得到地圖中所有位置的地形信息，為后續(xù)生成綜合平滑度地圖提供地形特征。

2.2 綜合平滑度地圖生成

為了保證行星車的安全探測，需要根據(jù)地形特征和行星車的運(yùn)動能力來分析地形的可通過性，本節(jié)將可通過性分析轉(zhuǎn)化為對地形綜合平滑度的分析，地形綜合平滑度越高，可通過性越高，否則可通過性越低。本節(jié)分別定義了坡度平滑度Sslope、起伏度平滑度Srelief和粗略平滑度Srough，如下所示：

式中，根據(jù)行星車運(yùn)動能力約束，設(shè)置最大坡度、起伏度、粗糙度的閾值限制分別為θmax=20°，Rmax=p/2，δmax=p/5。由于行星車在移動過程中要綜合考慮坡度、起伏度、粗糙度等各種地形約束，所以本文將綜合平滑度Scom定義為三者的最小值：

通過上節(jié)計(jì)算得到的地圖中各點(diǎn)的地形特征，就可計(jì)算各平滑度信息，最終生成綜合平滑度地圖，為后續(xù)進(jìn)行融合路徑規(guī)劃提供參考。

3 融合路徑規(guī)劃

由于行星探測任務(wù)的高風(fēng)險(xiǎn)性與高代價(jià)性，必須保證路徑規(guī)劃算法的結(jié)果具有全局最優(yōu)性，避免出現(xiàn)探測結(jié)果偏離目標(biāo)點(diǎn)甚至全局目標(biāo)點(diǎn)不可達(dá)的情況。本節(jié)選用具有路徑最優(yōu)性保證的A*算法進(jìn)行改進(jìn)，首先結(jié)合地形綜合平滑度信息設(shè)計(jì)了一種綜合代價(jià)函數(shù)，作為改進(jìn)A*算法的移動代價(jià)估計(jì)，最終規(guī)劃出路徑平滑度最優(yōu)的全局路徑。然后應(yīng)用融合動態(tài)窗口算法進(jìn)行實(shí)時(shí)路徑規(guī)劃，生成滿足行星車能力約束的平滑路徑，從而在探測路徑全局最優(yōu)的基礎(chǔ)上保證路徑可行，提高了行星探測任務(wù)的成功性和可靠性。

3.1 改進(jìn)A*算法

A*即A-Star算法，是一種常用的求解最短路徑最有效的啟發(fā)式搜索方法［10］，根據(jù)啟發(fā)式函數(shù)f(n)指導(dǎo)搜索節(jié)點(diǎn)的擴(kuò)展，定義為：f（n）=g（n）+h（n），其中，f(n)是從初始節(jié)點(diǎn)經(jīng)由節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)的代價(jià)估計(jì)，g(n)是在狀態(tài)空間中從初始節(jié)點(diǎn)到節(jié)點(diǎn)n的實(shí)際代價(jià)，h(n)是從節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)的估計(jì)代價(jià)。當(dāng)啟發(fā)式函數(shù)f(n)滿足一致性條件時(shí)，A*可以找到最優(yōu)解。A*算法在擴(kuò)展搜索過程中，待檢測的節(jié)點(diǎn)儲存在OPEN 表中，而已檢測過的節(jié)點(diǎn)則儲存在CLOSE 表中，然后擴(kuò)展當(dāng)前節(jié)點(diǎn)n的所有鄰居節(jié)點(diǎn)加入OPEN 表中，再選擇f(n)最小的節(jié)點(diǎn)進(jìn)行擴(kuò)展，如此迭代直至搜索到目標(biāo)節(jié)點(diǎn)。然后使用父節(jié)點(diǎn)儲存回溯節(jié)點(diǎn)，從目標(biāo)節(jié)點(diǎn)回溯最終路徑。具體算法詳見算法1。

?算法1:A*算法1. 輸入：Map，Start，Goal 2. Openlist=Start//初始化OPEN表3. Closelist=［］//初始化CLOSE表4. f_score（start）=heuristic_safe（Start，Goal）//初始化f（n）5. g_score（start）=0//初始化g（n）6. parent=Map//初始化父節(jié)點(diǎn)查詢表7. while Openlist≠NULL do 8. current=the node in openlist having the lowest f_score value 9. if current=goal//到達(dá)目標(biāo)節(jié)點(diǎn)10. return reconstruct_path（parent，current）11. end 12. Openlist.Remove（current）13. Closelist.Add（current）14. for each free neighbor v of current do 15. g_score_temp=g_score（current）+diagonal_dist（current，v）16. if g_score_temp ＜g_score［v］or v not in Openlist 17. g_score［v］=g_score_temp 18. f_score［v］=g_score［v］+heuristic_safe（v，Goal）19. parent（v）=current 20. if v not in Openlist 21. Openlist=［Openlist，v］

22. end 23. end 24. end 25. end 26. return failure 27. function reconstruct_path（parent，current）28. path=［current］29. while current≠Start do 30. current=parent（current）31. path=［path，current］32. end 33. return path

本節(jié)結(jié)合地形綜合平滑度定義了一個(gè)綜合啟發(fā)式函數(shù)f_com（），指導(dǎo)A*算法進(jìn)行全局最優(yōu)路徑搜索，最終規(guī)劃出路徑平滑度最優(yōu)的全局路徑，定義為：

式中，g（n）是從起始節(jié)點(diǎn)到節(jié)點(diǎn)n的實(shí)際距離代價(jià)，Diagonal_heuristic（）為對角啟發(fā)式距離函數(shù)，也可替換為歐氏距離函數(shù)或其他滿足一致性條件的啟發(fā)式函數(shù)。αs∈[0，1]為地形權(quán)重因子，可以人為設(shè)定來調(diào)節(jié)算法在不同區(qū)域、不同場景下的側(cè)重情況，增大αs可以使算法更側(cè)重于選擇平坦區(qū)域，確保探測安全性，減小αs可以使算法更側(cè)重于選擇距離最優(yōu)路徑，確保探測效率。

3.2 動態(tài)窗口法

動態(tài)窗口法（Dynamic Window Approach，DWA）是一種常用的平面移動機(jī)器人在線避障規(guī)劃算法［11］。其原理是在速度空間(v，ω)中采樣多組不同的速度，將搜索空間減小到在動態(tài)約束下可達(dá)的速度，從而將機(jī)器人的動力學(xué)特性融入到該方法中。然后預(yù)測該速度在一段時(shí)間后的軌跡，采用一定的軌跡評價(jià)函數(shù)對軌跡評價(jià)，選擇出最優(yōu)軌跡對應(yīng)的速度指令，發(fā)送給機(jī)器人控制系統(tǒng)。DWA 算法的基本思路如下：

（1）速度空間中離散采樣

由于在二維采樣空間中存在多組速度，需要根據(jù)行星車本身的能力約束和環(huán)境約束將采樣速度控制在一定范圍中，具體限制如下：

①自身可達(dá)速度的能力限制：

②行星車安全距離限制：

式中，dist(v，ω)是該采樣速度下對應(yīng)軌跡上與最近障礙物的距離，v?b、ω?b分別是制動線加速度與角加速度。

③自身加速度限制：

（2）軌跡評價(jià)

根據(jù)采樣得到的速度樣本，預(yù)測得到的多數(shù)軌跡都具有可行性，所以需要設(shè)計(jì)評價(jià)函數(shù)對可行軌跡進(jìn)行評價(jià)，選擇最優(yōu)的采樣速度組。本節(jié)采用的評價(jià)函數(shù)如下：

式中：heading(v，ω)是方位角評價(jià)函數(shù)，評價(jià)預(yù)測軌跡末端朝向和目標(biāo)之間的角度差，定義如下：

式中，ω為預(yù)測軌跡末端朝向方位角，ωgoal為終點(diǎn)方位角。

dist(v，ω)是安全距離評價(jià)函數(shù)，表示預(yù)測軌跡上與最近的障礙物距離。velocity(v，ω)是速度評價(jià)函數(shù)，用于評價(jià)當(dāng)前預(yù)測軌跡的速度大??；σ為歸一化平滑函數(shù)，為了防止不同量綱的參數(shù)綜合導(dǎo)致某項(xiàng)參數(shù)占比過高，通過歸一化平滑處理得到最終的評價(jià)指標(biāo)，函數(shù)形式如下。

式中，Ei為第i項(xiàng)評估函數(shù)的值。

評價(jià)函數(shù)的作用是選取行星車在避開障礙物的同時(shí)以較快的速度朝向目標(biāo)點(diǎn)運(yùn)動的最優(yōu)軌跡。選擇評價(jià)函數(shù)最高的軌跡后，將相關(guān)速度作為控制指令控制行星車前進(jìn)，并在下個(gè)步長重復(fù)上述過程，最終實(shí)現(xiàn)行星車的實(shí)時(shí)避障規(guī)劃。

3.3 融合算法

DWA 算法根據(jù)行星車檢測到的局部窗口環(huán)境信息，可實(shí)現(xiàn)在線實(shí)時(shí)規(guī)劃避障路徑。但是，該方法沒有考慮路徑的全局最優(yōu)性，存在陷入局部最優(yōu)的致命問題，故本節(jié)融合改進(jìn)A*算法和DWA 算法進(jìn)行融合路徑規(guī)劃，以互相彌補(bǔ)全局及局部規(guī)劃算法存在的主要弊端。

本節(jié)根據(jù)改進(jìn)A*算法生成的全局最優(yōu)路徑，設(shè)計(jì)了DWA 算法的動態(tài)窗口評價(jià)函數(shù)，將動態(tài)窗口的目標(biāo)點(diǎn)設(shè)置為與行星車距離最近的全局最優(yōu)路徑點(diǎn)。這樣改進(jìn)使得局部路徑規(guī)劃可以遵循全局最優(yōu)路徑，從而避免了陷入局部最優(yōu)。融合后算法可以通過修改改進(jìn)A*算法的地形權(quán)重因子調(diào)節(jié)全局路徑的側(cè)重，應(yīng)對不同的探測需求和地形生成不同的全局最優(yōu)路徑，同時(shí)通過修改DWA 算法的參數(shù)可以根據(jù)行星車能力范圍及地形情況調(diào)節(jié)局部避障路徑，最終在探測路徑全局最優(yōu)的基礎(chǔ)上保證路徑穩(wěn)妥可行，提高了行星探測任務(wù)的可行性和可靠性。

4 仿真結(jié)果及分析

4.1 仿真場景

由于行星表面DEM 地圖是獲取行星地形信息的主要方式，本文采用月球嫦娥二號CE2TMap2015數(shù)據(jù)集作為行星表面DEM 地圖數(shù)據(jù)代表，驗(yàn)證本文所提方法有效性。首先選取月球赤道附近地形具有代表性特征的區(qū)域作為行星車自主探測仿真場景，該區(qū)域DEM 地圖及地形特征信息如圖1 所示，范圍約150km×250km（3000×5000柵格）。

圖1 仿真場景DEM地圖及主要地形特征Fig.1 The DEM map and main terrain features of simulation scene

本文所有算法的仿真測試軟件環(huán)境為Windows 10 + MATLAB 2016，硬件環(huán)境為Intel（R）Core（TM）i5-7200U CPU+12.0GB RAM。

4.2 綜合平滑度地圖仿真結(jié)果

本節(jié)首先根據(jù)第二章所提地圖處理方法對該區(qū)域生成綜合平滑度地圖，如圖2 所示。由圖2 可以看出，本節(jié)所提出的地形計(jì)算方法可以較好的還原行星表面地形特征，對環(huán)形山邊緣、小型山脈等危險(xiǎn)區(qū)域有很好的提取效果。并且平滑度地圖可以很好地表示出平坦區(qū)域（圖中較亮區(qū)域），為后續(xù)路徑規(guī)劃提供參考。

圖2 綜合平滑度地圖Fig.2 Comprehensive smoothness map

4.3 融合路徑規(guī)劃仿真結(jié)果

根據(jù)上節(jié)生成的綜合平滑度地圖，本節(jié)分析本文融合路徑規(guī)劃算法有效性。首先測試改進(jìn)A*算法中不同地形權(quán)重因子對生成路徑的影響，本節(jié)分別設(shè)定以下三種情況進(jìn)行仿真：路徑1：αs= 0.1；路徑2：αs= 0.5；路徑3：αs= 1。改進(jìn)A*算法參數(shù)設(shè)置如下：仿真步長為1柵格（50m），起點(diǎn)（50，50），終點(diǎn)（4950，2950）。DWA 算法參數(shù)設(shè)置如下：最大線速度為vmax= 1.0m/s，最大線加速度為vamax=0.2m/s2，最大角速度為wmax= 20.0°/s，最大角加速度為wamax= 5.0°/s2，線速度采樣間隔為0.01m/s，角速度采樣間隔為1.0°/s，時(shí)間步長為0.1s，預(yù)測步長為2.0s，評價(jià)函數(shù)參數(shù)α= 0.1、β= 0.2、γ= 0.1。仿真結(jié)果如圖3所示。

由圖3 可以看出，融合路徑規(guī)劃算法中，地形權(quán)重因子αs較大的路徑能更好地選擇圖中的平坦區(qū)域（較明亮區(qū)域）。本文對不同地形因子αs情況下算法的性能表現(xiàn)做了對比，結(jié)果如表1 所示?？梢钥闯觯S著地形因子αs的增大，算法生成的路徑長度以及搜索時(shí)間變大，但路徑整體綜合平滑度更好，證明αs可以調(diào)節(jié)算法的距離最優(yōu)性和安全最優(yōu)性，可以在平坦的區(qū)域增大αs以增強(qiáng)算法的性能，而在危險(xiǎn)的區(qū)域減小αs來確保生成路徑的安全性。

圖3 不同地形權(quán)重因子路徑對比Fig.3 Comparison of paths under different factors

表1 不同地形權(quán)重因子下算法的性能表現(xiàn)Table 1 Performance of the algorithm under different terrain weight factors

本節(jié)將融合路徑規(guī)劃算法（αs=1）與標(biāo)準(zhǔn)A*算法，以及JPS算法［12］進(jìn)行對比，各算法生成路徑如圖4 所示。由圖4 可以看出，相比于其他算法，本文所提出的融合算法搜索出的路徑會盡量選擇離障礙較遠(yuǎn)的安全區(qū)域，提高了探測路徑的安全性。

圖4 不同算法生成路徑對比Fig.4 Comparison of paths generated by different algorithms

本節(jié)對比了三種算法的性能表現(xiàn)，如表2 所示。由表2 可以看出，相比于其他兩種算法，融合算法生成路徑的平均綜合平滑度提高大約20%，同時(shí)最小平滑度也明顯提高，保證了探測過程的安全性。而由于本文所提出的融合算法包括局部實(shí)時(shí)規(guī)劃過程，所以算法搜索時(shí)間明顯高于其他兩個(gè)全局規(guī)劃算法，但可保證規(guī)劃出的路徑真實(shí)可行，提高了探測任務(wù)的可靠性。

表2 不同算法的性能表現(xiàn)對比Table 2 Performance comparison of different algorithms

5 結(jié) 論

本文提出了一種基于行星表面數(shù)字高程圖的行星車自主探測融合路徑規(guī)劃算法，主要得出了以下結(jié)論：

（1）根據(jù)行星表面DEM地圖，分別提出了行星表面地形特征提取方法、綜合平滑度計(jì)算方法。在此基礎(chǔ)上生成綜合平滑度圖，為后續(xù)的融合路徑規(guī)劃提供參考。

（2）提出了改進(jìn)A*算法和DWA 算法的融合路徑規(guī)劃算法，設(shè)計(jì)了基于綜合平滑度信息的綜合啟發(fā)式函數(shù)，可以指導(dǎo)算法根據(jù)相關(guān)性能生成不同的全局最優(yōu)路徑，并利用DWA 算法生成實(shí)時(shí)避障路徑，保證規(guī)劃路徑真實(shí)可行，提高了行星探測任務(wù)的成功性和可靠性。最后選取月球赤道附近地形具有代表性特征的區(qū)域作為仿真場景，驗(yàn)證了本文方法的有效性。