火星車自主導(dǎo)航與路徑規(guī)劃技術(shù)研究

魏祥泉，黃建明，顧冬晴，陳鳳

（上海宇航系統(tǒng)工程研究所，上海 201109）

火星車自主導(dǎo)航與路徑規(guī)劃技術(shù)研究

魏祥泉，黃建明，顧冬晴，陳鳳

（上海宇航系統(tǒng)工程研究所，上海 201109）

針對(duì)火星車工作任務(wù)環(huán)境，借鑒國(guó)外成功研究經(jīng)驗(yàn)，給出了火星車自主導(dǎo)航敏感器配置，研究可在實(shí)際工程中應(yīng)用的火星車自主導(dǎo)航與路徑規(guī)劃技術(shù)，包括DEM圖構(gòu)建與障礙物檢測(cè)、全局路徑規(guī)劃、基于立體視覺(jué)相機(jī)的視覺(jué)里程計(jì)、多傳感器融合位姿確定以及局部路徑規(guī)劃技術(shù)，并搭建了實(shí)驗(yàn)系統(tǒng)，在實(shí)驗(yàn)室內(nèi)進(jìn)行了仿真驗(yàn)證。相關(guān)研究結(jié)果可為我國(guó)后續(xù)開(kāi)展火星車自主導(dǎo)航系統(tǒng)研究提供一定的基礎(chǔ)。

火星車自主導(dǎo)航；路徑規(guī)劃；視覺(jué)里程計(jì)；多傳感器信息融合

引用格式：魏祥泉，黃建明，顧冬晴，等. 火星車自主導(dǎo)航與路徑規(guī)劃技術(shù)研究［J］. 深空探測(cè)學(xué)報(bào)，2016，3（3）：275-281.

Reference format: Wei X Q，Huang J M，Gu D Q，et al. Researches on the techniques of autonomous navigation and path planning for mars rover ［J］. Journal of Deep Space Exploration，2016，3（3）：275-281.

0　引　言

火星由于其獨(dú)特的科學(xué)價(jià)值和技術(shù)跨越性，成為人類開(kāi)展深空探測(cè)的首選目標(biāo)之一?；鹦擒囁媾R的任務(wù)環(huán)境非常復(fù)雜，使得火星車的導(dǎo)航研究極具挑戰(zhàn)性和特殊性，主要表現(xiàn)在以下幾個(gè)方面。

1）非確定性導(dǎo)航環(huán)境。星面環(huán)境是一個(gè)非確定、非結(jié)構(gòu)化的未知復(fù)雜環(huán)境，難以通過(guò)各種手段獲得星面環(huán)境的全部信息，通過(guò)航拍圖預(yù)先建立的星面地圖受到分辨率和準(zhǔn)確度等方面的限制。

2）大時(shí)延及帶寬限制。由于通信延遲、持續(xù)通信時(shí)間的限制，難以實(shí)現(xiàn)連續(xù)控制，有限帶寬難以支持連續(xù)的反饋信息（包括視頻圖像）傳輸。

3）環(huán)境感知的復(fù)雜性。由于地外星體環(huán)境的特殊性和復(fù)雜性，使星面環(huán)境感知手段受到限制，感知難度也大幅度增加，這將影響到導(dǎo)航系統(tǒng)所依賴的各種感知信息的精度、穩(wěn)定性和可靠性。

4）火星車的安全性和可持續(xù)工作能力是行星車導(dǎo)航系統(tǒng)優(yōu)先考慮的因素。

此外，行星車的正常運(yùn)行還面臨資源（如能源、存儲(chǔ)器等）、操作時(shí)間有限、意外障礙等因素的影響。

綜上所述，火星車是一個(gè)對(duì)外界環(huán)境高度開(kāi)放的智能系統(tǒng)，為了保證火星車能夠適應(yīng)復(fù)雜的星面環(huán)境并順利完成各種探測(cè)任務(wù)，需要火星車具有高度的自主導(dǎo)航能力［1］。

國(guó)外開(kāi)展火星車研制的有美國(guó)、歐空局（ESA）和俄羅斯，其中只有美國(guó)的火星車成功發(fā)射并成功運(yùn)行。美國(guó)共研制了三代火星車，1996年發(fā)射的“索杰納”是首個(gè)火星車［2］，2004年發(fā)射的第二代火星車“勇氣號(hào)”和“機(jī)遇號(hào)”找到了大量的地質(zhì)學(xué)和礦物學(xué)證據(jù)證明火星上確實(shí)曾經(jīng)存在大量的水［3］，第三代火星車“好奇號(hào)”于2012年8月登陸火星，是第一輛采用了“核動(dòng)力驅(qū)動(dòng)”的火星車［4-5］。ESA將在2018年4月發(fā)射的任務(wù)中使用ExoMars火星車［6］。表1對(duì)國(guó)外火星車自主導(dǎo)航系統(tǒng)進(jìn)行了比較。

為使火星車能夠安全平穩(wěn)地到達(dá)科學(xué)探測(cè)的目標(biāo)點(diǎn)，自主導(dǎo)航與路徑規(guī)劃主要完成如下功能。

1）環(huán)境感知與識(shí)別：利用相機(jī)和其他敏感器建立全局三維地形圖并識(shí)別可視范圍內(nèi)障礙與危險(xiǎn)。

2） 位姿確定：自主確定當(dāng)前位置和姿態(tài)。

3）路徑規(guī)劃與避障：根據(jù)當(dāng)前位置和姿態(tài)、環(huán)境感知結(jié)果及地面遙操作系統(tǒng)指定的遠(yuǎn)距離目標(biāo)點(diǎn)位置，進(jìn)行局部路徑點(diǎn)分解和局部路徑規(guī)劃，保證火星車安全到達(dá)目標(biāo)點(diǎn)。

表1　國(guó)外火星車自主導(dǎo)航系統(tǒng)比較Table 1　Comparison of autonomous navigation system for foreign mars rover

國(guó)內(nèi)，寧曉琳等（2006）研究了一種基于純天文觀測(cè)的火星車自主導(dǎo)航方法，但天文導(dǎo)航不能輸出連續(xù)的位置信息，需要與其他導(dǎo)航方法組合使用［7］。邱凱昌等（2010）研究了一種基于地面圖像和衛(wèi)星圖像集成的火星車定位方法，定位精度和定位范圍受限于地面圖像的分辨率和覆蓋范圍［8］。婁路（2012）提出一種單目視覺(jué)與MEMS-IMU融合的導(dǎo)航定向方法，需要與其他定位方法結(jié)合導(dǎo)航和長(zhǎng)距離定向［9］。國(guó)外對(duì)火星車導(dǎo)航與路徑規(guī)劃的研究較多［10-13］，可為我們開(kāi)展相關(guān)工作提供一定的參考。

本文的目的是研究可在實(shí)際工程中應(yīng)用的火星車自主導(dǎo)航與路徑規(guī)劃技術(shù)，為我國(guó)未來(lái)的火星車自主探測(cè)任務(wù)奠定技術(shù)基礎(chǔ)。

1　火星車自主導(dǎo)航敏感器配置

火星車導(dǎo)航系統(tǒng)擬由一對(duì)全景相機(jī)、一對(duì)導(dǎo)航相機(jī)、兩對(duì)避障相機(jī)、慣性測(cè)量組件、數(shù)字太陽(yáng)敏感器和光電編碼器組成，其布局和視場(chǎng)如圖1所示。

圖1　火星車自主導(dǎo)航與控制分系統(tǒng)組成示意圖Fig.1　The schematic diagram of the autonomous navigation and control system of mars rover

導(dǎo)航相機(jī)視場(chǎng)45°，安裝在火星車的桅桿上，在火星表面上方約1.6 m的高度，采用20 cm的基線和平行光軸；桅桿可以水平旋轉(zhuǎn)360°，安裝臺(tái)可以上下旋轉(zhuǎn)90°。一對(duì)導(dǎo)航相機(jī)組成被動(dòng)立體視覺(jué)系統(tǒng)來(lái)探測(cè)潛在的地形危險(xiǎn)，為火星車路徑規(guī)劃提供周圍100 m范圍內(nèi)的局部或全景地形圖像，可以與避障相機(jī)一起進(jìn)行車載自主導(dǎo)航，也可以協(xié)助地面操作系統(tǒng)行進(jìn)路線規(guī)劃。視覺(jué)里程計(jì)（Visual Odometry，VO）用于在比較短的行駛距離范圍內(nèi)（＜10 m），根據(jù)連續(xù)的導(dǎo)航相機(jī)立體圖像進(jìn)行里程計(jì)算。

全景相機(jī)視場(chǎng)14°，安裝在桅桿上，可對(duì)火星車周邊地貌進(jìn)行全景成像，既可用于形態(tài)學(xué)、大氣和地質(zhì)研究，也可用于三維現(xiàn)場(chǎng)描繪，為火星車導(dǎo)航提供輔助。采用30 cm的基線和平行光軸，一個(gè)是中分辨率相機(jī)，一個(gè)是高分辨率相機(jī)。需要進(jìn)行遠(yuǎn)距離路徑規(guī)劃時(shí)，使用分辨率高于導(dǎo)航相機(jī)的全景相機(jī)。

兩對(duì)避障相機(jī)分別安裝在火星車的前部和后部，在火星表面上方大約50 cm高度，并且指向水平大約50°，均采用10 cm基線和平行光軸。避障相機(jī)主要用于危險(xiǎn)檢測(cè)、行駛過(guò)程中精確定位、獲取火星表面清晰的近攝圖像，以及輔助科學(xué)實(shí)驗(yàn)中儀器設(shè)備部署等，是進(jìn)行近距離路徑規(guī)劃的主要依據(jù)。

慣性測(cè)量組件主要用于靜態(tài)傾角測(cè)量和動(dòng)態(tài)姿態(tài)預(yù)估。數(shù)字太陽(yáng)敏感器主要用于確定火星車的偏航方位。光電編碼器是一種通過(guò)光電轉(zhuǎn)換將輸出軸上的機(jī)械集合位移量轉(zhuǎn)換成脈沖或數(shù)字量的傳感器。

2　火星車自主導(dǎo)航與路徑規(guī)劃技術(shù)

火星車自主導(dǎo)航與路徑規(guī)劃技術(shù)流程如圖2所示。

圖2　火星車自主導(dǎo)航與路徑規(guī)劃技術(shù)流程Fig.2　Technological process for autonomous navigation and path planning

2.1DEM圖構(gòu)建與障礙物檢測(cè)

2.1.1圖像預(yù)處理

重點(diǎn)進(jìn)行畸變校正、濾波和均衡化處理，以便于進(jìn)行立體視覺(jué)匹配和視覺(jué)里程處理。圖像濾波采用LoG算子對(duì)圖像進(jìn)行濾波，主要目的有兩個(gè)：一是使圖像中的弱紋理區(qū)域在一定程度上得到增強(qiáng)，另一個(gè)作用是消除兩個(gè)成像圖片之間的整體亮度差異。

圖像分割的主要目的是將紋理增強(qiáng)后圖像中紋理仍然較弱的區(qū)域提取出來(lái)，利用區(qū)域的整體性質(zhì)對(duì)弱紋理區(qū)域進(jìn)行障礙判別，采用k-均值聚類算法。

2.1.2立體視覺(jué)匹配

火星表面由各種各樣的礦物質(zhì)組成，表面粗糙度比環(huán)境光波長(zhǎng)大得多，表面反射主要是漫散射，局域范圍內(nèi)的紋理色彩并不是很豐富，采用基于特征提取的方法來(lái)進(jìn)行立體匹配獲得的正確率不會(huì)很高。因此，所采用的算法不應(yīng)致力于特征點(diǎn)、線或面的提取，而是應(yīng)直接利用圖像對(duì)進(jìn)行特征匹配。

根據(jù)上述分析，使用基于灰度值相關(guān)的目標(biāo)匹配方法。設(shè)圖像對(duì)中的左幅圖像為參考圖像，對(duì)于其中的每一點(diǎn)選擇一定尺寸（5×5，7×7或9×9）的窗口作為模板，沿這一點(diǎn)在右幅圖像中對(duì)應(yīng)的極線上采用同樣大小的模板進(jìn)行搜索，參考模板和候選模板內(nèi)的各個(gè)像素灰度值之差最小時(shí)，兩點(diǎn)被認(rèn)為是對(duì)應(yīng)點(diǎn)。這種方法也叫像素灰度差的絕對(duì)值（Sum of Absolute Differences，SAD）區(qū)域匹配法。公式為

其中，dmax和dmin是系統(tǒng)可能的最小和最大的視差值；m是窗口尺寸；Iright和Ileft是左右圖像的灰度值。

2.1.3DEM圖重建

DEM圖的準(zhǔn)確構(gòu)建和障礙物的精確檢測(cè)是火星車進(jìn)行自主導(dǎo)航與控制的前提。進(jìn)行DEM圖重建主要包括DEM數(shù)據(jù)預(yù)處理、柵格地形高度估計(jì)和DEM圖后處理、幀間融合等步驟。

1）DEM數(shù)據(jù)預(yù)處理

火星車DEM數(shù)據(jù)源于車載雙目立體導(dǎo)航相機(jī)獲取的立體圖像，通過(guò)立體圖像匹配和三維重建得到致密的點(diǎn)云數(shù)據(jù)。由于立體匹配過(guò)程中的誤匹配點(diǎn)往往會(huì)導(dǎo)致重建后的點(diǎn)云數(shù)據(jù)出現(xiàn)錯(cuò)誤點(diǎn)和懸浮點(diǎn)，因此需要對(duì)原始點(diǎn)云數(shù)據(jù)進(jìn)行預(yù)處理，將不可靠點(diǎn)和錯(cuò)誤點(diǎn)去除。此外還需將立體匹配和重建得到的三維點(diǎn)云數(shù)據(jù)轉(zhuǎn)換到慣導(dǎo)坐標(biāo)系下，需要進(jìn)行坐標(biāo)變換。

2）柵格地形高度估計(jì)

采用DEM圖來(lái)表示地形時(shí)，一個(gè)柵格內(nèi)通常包含了多個(gè)三維點(diǎn)數(shù)據(jù)。為每個(gè)柵格創(chuàng)建高度直方圖以保存這些數(shù)據(jù)點(diǎn)的高度分布，然后根據(jù)直方圖決定每個(gè)格子的高度。

若落入某一柵格的三維點(diǎn)個(gè)數(shù)n＜N（N為點(diǎn)數(shù)閾值），則認(rèn)為該點(diǎn)云對(duì)于該柵格數(shù)據(jù)不造成影響，即沒(méi)有得到該柵格的有效數(shù)據(jù)。若落入點(diǎn)數(shù)大于N的柵格，則對(duì)該柵格的平均高度進(jìn)行估算。首先濾除柵格中所有高度Y＜H（H為高度閾值）的點(diǎn)，以減小大量地面點(diǎn)對(duì)障礙物高度估算的影響。對(duì)剩余點(diǎn)計(jì)算代數(shù)平均，得到平均高度為h，則最高點(diǎn)高度的估計(jì)為hmax=2×h。

由于誤匹配可能造成點(diǎn)云數(shù)據(jù)中存在許多懸浮點(diǎn)，因此需要去除懸浮點(diǎn)對(duì)于柵格高度的影響。采用密度判斷法解決懸浮點(diǎn)問(wèn)題。柵格的估計(jì)密度為ρ=NTotal/h，其中，NTotal為所有落入該柵格的點(diǎn)數(shù)。若ρ＜ρ（其中ρ為密度閾值），則認(rèn)為該柵格的估計(jì)高度h無(wú)效。若落入柵格點(diǎn)總數(shù)大于點(diǎn)數(shù)閾值，且柵格估算密度大于密度閾值的柵格，將該柵格的高度賦值為hmax，其余柵格高度則賦值為0。

3）DEM圖后處理

后處理目標(biāo)是修正格子的錯(cuò)誤高度和為某些無(wú)數(shù)據(jù)區(qū)作插值。判斷虛格子高度錯(cuò)誤的依據(jù)是可視性原則：假想相機(jī)拍攝重建場(chǎng)景，所有高度的格子應(yīng)該都能被拍攝到，沒(méi)被拍到的格子其高度或者有錯(cuò)或者被遮擋。具體算法如下。

①檢測(cè)障礙物，判據(jù)為格子高度比其前面或者左右格子的高度高30 cm。若檢測(cè)到障礙物則繼續(xù)，否則算法結(jié)束。

②如果障礙物格子后面的格子有高度且不符合可視性原則，而且障礙物格子的高度直方圖是尖峰，則此障礙物格子是虛假的，把它設(shè)為無(wú)數(shù)據(jù)區(qū)。重復(fù)步驟①。

由于空間分辨率和誤配的原因，有些格子會(huì)沒(méi)有數(shù)據(jù)點(diǎn)落入，即存在無(wú)數(shù)據(jù)區(qū)，而且檢測(cè)虛假高度時(shí)，把虛假高度的格子直接設(shè)為無(wú)數(shù)據(jù)區(qū)，因此需要作插值。把無(wú)數(shù)據(jù)格子八鄰域中高度相近的格子高度的平均值作為其高度。

③幀間融合

視覺(jué)處理一部分場(chǎng)景會(huì)在連續(xù)好幾幀出現(xiàn)，即存在幀間的數(shù)據(jù)冗余，因此可以運(yùn)用幀間融合來(lái)消除障礙物虛報(bào)。具體做法為：當(dāng)在當(dāng)前幀發(fā)現(xiàn)障礙物時(shí)，到前n幀查看是否有這個(gè)障礙物。如果沒(méi)有，則認(rèn)為是虛假障礙，有則認(rèn)為是真正的障礙。

2.2全局路徑規(guī)劃

常用的全局路徑規(guī)劃方法有模板匹配法、地圖構(gòu)建法、人工勢(shì)場(chǎng)法以及其他一些人工智能方法?；鹦擒嚾致窂揭?guī)劃算法要求計(jì)算速度快，占用內(nèi)存小，可靠性高，重復(fù)計(jì)算簡(jiǎn)單，選擇A*算法作為火星車全局路徑規(guī)劃方法。A*算法是一種典型的啟發(fā)式搜索算法，是求解靜態(tài)網(wǎng)絡(luò)中最短路徑的最有 效方法［14］。A*的估價(jià)函數(shù)是：f（n）=g（n）+h（n）。其中，f（n）是節(jié)點(diǎn)n從初始點(diǎn)到目標(biāo)點(diǎn)的估價(jià)函數(shù)，g（n）是在狀態(tài)空間中從初始節(jié)點(diǎn)到n節(jié)點(diǎn)的實(shí)際代價(jià)，h（n）是從h（n）到目標(biāo)節(jié)點(diǎn)最佳路徑的估計(jì)代價(jià)。A*算法的搜索步驟如下。

1）標(biāo)記起始節(jié)點(diǎn)，并對(duì)其周圍的未標(biāo)記過(guò)的子節(jié)點(diǎn)進(jìn)行擴(kuò)展。

2）對(duì)每一個(gè)子節(jié)點(diǎn)，計(jì)算其估價(jià)函數(shù)值，按照估價(jià)函數(shù)值的大小進(jìn)行排序，找出估價(jià)值最小的節(jié)點(diǎn)進(jìn)行標(biāo)記。如果被標(biāo)記的子節(jié)點(diǎn)就是目標(biāo)點(diǎn)，則停止搜索。

3）對(duì)新標(biāo)記的子節(jié)點(diǎn)重復(fù)步驟2）。

2.3視覺(jué)里程計(jì)

視覺(jué)里程計(jì)是利用安裝在移動(dòng)自主探測(cè)車上的立體視覺(jué)相機(jī)獲取圖像序列，通過(guò)跟蹤前后幀的相同特征點(diǎn)，精確估計(jì)車體6個(gè)自由度的更新（位置和姿態(tài)）。利用視覺(jué)里程計(jì)可以獲得更為精確的火星車定位結(jié)果，并且結(jié)合方向傳感器定期糾正視覺(jué)里程計(jì)方向的定位誤差可以使得誤差積累非常緩慢地隨漫游距離線性增加。視覺(jué)里程計(jì)的工作分為三步：特征點(diǎn)提取、特征點(diǎn)匹配和跟蹤、魯棒運(yùn)動(dòng)估計(jì)。

通過(guò)性能與計(jì)算量等方面的比較與仿真分析，選擇SIFT算法進(jìn)行特征提取與特征匹配，SIFT特征關(guān)鍵點(diǎn)雖然不如角點(diǎn)、邊緣檢測(cè)等具有明顯的視覺(jué)意義，但其描述了圖像局部的結(jié)構(gòu)性信息，體現(xiàn)了圖像在尺度、局部結(jié)構(gòu)方面的特征，并具有多量性和獨(dú)特性。特征點(diǎn)立體匹配和特征點(diǎn)跟蹤匹配完成后，經(jīng)過(guò)坐標(biāo)轉(zhuǎn)換就可以得到一系列匹配點(diǎn)對(duì)在車體運(yùn)動(dòng)前后的攝相機(jī)坐標(biāo)系下的三維坐標(biāo)。為了兼顧算法的實(shí)時(shí)性和精確度，采用RANSAC結(jié)合線性最小二乘法求出車體運(yùn)動(dòng)參數(shù)的預(yù)估值，再?gòu)念A(yù)估值出發(fā)運(yùn)用非線性最小二乘法求出車體運(yùn)動(dòng)參數(shù)的精確估計(jì)值的方法。

結(jié)合RANSAC的線性最小二乘估計(jì)雖然可以有效去除外點(diǎn)，但所得的結(jié)果可能是不精確的。為了得到精確的估計(jì)結(jié)果，可采用Levenberg-Marquardt算法進(jìn)行非線性最小二乘估計(jì)［15］。當(dāng)某些前后幀之間圖像變化較大，匹配點(diǎn)數(shù)量很少時(shí)就不再適用。在這種情況下，可以利用正交化方法來(lái)大致求取運(yùn)動(dòng)估計(jì)參數(shù)。在應(yīng)用中要注意把握好各幀圖像的采集間隔，保證前后兩幀圖像有較大的重疊率以提供足夠多的特征跟蹤匹配點(diǎn)對(duì)。

2.4多傳感器融合位姿確定

火星車自主位姿確定采用了多種測(cè)量傳感器，這些傳感器的信息要通過(guò)有效的信息融合手段，才能獲得高精度的火星車位姿參數(shù)估計(jì)。多傳感器位姿確定的 信息融合將采用集中式卡爾曼濾波（Kalman Filter，KF）算法。圖3為多傳感器信息融合位姿確定原理框圖。

圖3　多傳感器信息融合位姿確定原理框圖Fig.3　Block diagram of multisensor information fusion for pose determination

多傳感器位姿確定卡爾曼濾波將以捷聯(lián)導(dǎo)航計(jì)算的位置誤差、速度誤差、姿態(tài)誤差以及IMU的陀螺和加速度計(jì)測(cè)量誤差，作為待估計(jì)的系統(tǒng)狀態(tài)向量，采用卡爾曼濾波的狀態(tài)更新和量測(cè)更新算法，可以實(shí)時(shí)得到系統(tǒng)狀態(tài)的最優(yōu)估計(jì)，然后采用反饋修正方式對(duì)捷聯(lián)導(dǎo)航計(jì)算的位置、速度、姿態(tài)參數(shù)以及IMU的陀螺和加速度計(jì)測(cè)量值進(jìn)行誤差修正。因此，捷聯(lián)導(dǎo)航計(jì)算輸出的經(jīng)誤差修正的位姿參數(shù)，即為多傳感器位姿確定信息融合的全局最優(yōu)估計(jì)。

2.5局部路徑規(guī)劃

對(duì)于靜態(tài)建立的全局路徑，其為離散柵格點(diǎn)路徑，并且由于全局地圖的精確度較差，全局規(guī)劃時(shí)得到的全局路徑并不能完全避開(kāi)所有的障礙物，這就需要局部路徑規(guī)劃過(guò)程來(lái)完成局部避障和全局路徑點(diǎn)跟蹤。常用局部規(guī)劃方法為：VFH法、Ranger算法、Morphin算法、GESRALT算法和ND法等［16］。

由于已經(jīng)建立了全局路徑，因此對(duì)GESTALT方法進(jìn)行簡(jiǎn)化來(lái)實(shí)現(xiàn)局部路徑生成。在路徑生成過(guò)程中，忽略GESTALT建立優(yōu)異值地圖過(guò)程，并且忽略其確定值，僅考慮建立二值柵格地圖，如上述柵格地圖建立中所描述，將整個(gè)地圖標(biāo)記為可行區(qū)域與不可行區(qū)域。之后建立一系列的軌跡組，從中選擇接近最遠(yuǎn)全局路徑點(diǎn)，并且不經(jīng)過(guò)不可行區(qū)域的軌跡為下一步執(zhí)行軌跡。具體方法如下。

1）建立一組符合火星車運(yùn)動(dòng)學(xué)的軌跡；

2）分析每條軌跡所經(jīng)過(guò)的柵格，得到每條軌跡經(jīng)過(guò)的全局路徑柵格和不可行柵格，將其經(jīng)過(guò)的離目標(biāo)點(diǎn)最近的柵格作為其最終選擇路徑點(diǎn)；

3）選擇經(jīng)過(guò)最遠(yuǎn)全局路徑點(diǎn)并且在可行區(qū)域的軌跡為下一步執(zhí)行軌跡；

4）當(dāng)沒(méi)有局部軌跡經(jīng)過(guò)路徑點(diǎn)，并且沒(méi)有穿過(guò)不可行區(qū)域時(shí)，選擇最容易實(shí)現(xiàn)的軌跡，也就是弧度最小的軌跡，先繞過(guò)局部障礙物，之后再按照上述方法，尋找經(jīng)過(guò)最遠(yuǎn)路徑點(diǎn)并且不經(jīng)過(guò)不可行區(qū)域的軌跡，直到到達(dá)目標(biāo)點(diǎn)為止。

如圖4所示，細(xì)弧線為規(guī)劃軌跡組，其中粗軌跡為選擇的不經(jīng)過(guò)不可行區(qū)域，并且經(jīng)過(guò)第三個(gè)路徑點(diǎn)的軌跡。

圖4　局部路徑選擇示意圖Fig.4　Sketch map for local path selection

由于全局路徑的精確度較差，因此規(guī)劃好的全局路徑點(diǎn)周圍有可能出現(xiàn)障礙物，導(dǎo)致此路徑點(diǎn)成為不可行區(qū)域，這種障礙物可稱為局部障礙物，對(duì)于此種情況，使用上述介紹方法是并不能完成避障，并且可能會(huì)產(chǎn)生錯(cuò)誤，因此需要采用如圖5 所示的避障方案。當(dāng)沒(méi)有局部軌跡經(jīng)過(guò)路徑點(diǎn)，并且沒(méi)有穿過(guò)不可行區(qū)域時(shí)，選擇最容易實(shí)現(xiàn)的軌跡，也就是弧度最小的軌跡，先繞過(guò)局部障礙物，之后再按照上述方法，尋找經(jīng)過(guò)最遠(yuǎn)路徑點(diǎn)并且不經(jīng)過(guò)不可行區(qū)域的軌跡，直到到達(dá)目標(biāo)點(diǎn)為止。

圖5　出現(xiàn)局部障礙物時(shí)的局部路徑規(guī)劃示意圖Fig.5　Sketch map for local path selection in the case of local obstacle

3　火星車自主導(dǎo)航與路徑規(guī)劃仿真

為了驗(yàn)證火星車自主導(dǎo)航與路徑規(guī)劃方法的可行性，構(gòu)建了實(shí)驗(yàn)系統(tǒng)進(jìn)行仿真驗(yàn)證。

火星車自主導(dǎo)航與路徑規(guī)劃實(shí)驗(yàn)系統(tǒng)如圖6所示，利用導(dǎo)航相機(jī)拍攝的實(shí)驗(yàn)地形圖如圖7所示，經(jīng)處理后構(gòu)建的DEM圖如圖8所示。使用A*算法建立的全局最優(yōu)路徑如圖9所示，其中細(xì)線條即為最優(yōu)全局路徑。局部路徑規(guī)劃結(jié)果圖10所示，其中細(xì)線軌跡為全局規(guī)劃路徑，粗線軌跡為局部跟蹤之后的路徑。

圖6　火星車自主導(dǎo)航與路徑規(guī)劃實(shí)驗(yàn)系統(tǒng)組成Fig.6　Composition of experiment system of autonomous navigation and path planning for Mars Rover

圖7　實(shí)驗(yàn)地形圖Fig.7　Topographic map for experiment

圖8　構(gòu)建的DEM圖Fig.8　Constructed DEM diagram

4　結(jié)束語(yǔ)

本文首先在火星車工作任務(wù)環(huán)境特性和國(guó)內(nèi)外相關(guān)研究情況分析的基礎(chǔ)上，給出了火星車自主導(dǎo)航敏感器配置；然后，重點(diǎn)研究可實(shí)際工程應(yīng)用的火星車自主導(dǎo)航與路徑規(guī)劃技術(shù)，給出了自主導(dǎo)航與路徑規(guī)劃技術(shù)流程，研究了所涉及的DEM圖構(gòu)建與障礙物檢測(cè)、全局路徑規(guī)劃、基于立體視覺(jué)相機(jī)的視覺(jué)里程計(jì)、多傳感器融合位姿確定以及局部路徑規(guī)劃技術(shù)；最后利用實(shí)驗(yàn)搭建的半物理仿真系統(tǒng)在實(shí)驗(yàn)室內(nèi)進(jìn)行了仿真驗(yàn)證。相關(guān)研究結(jié)果可為我國(guó)后續(xù)開(kāi)展火星車自主導(dǎo)航系統(tǒng)研究提供一定的基礎(chǔ)。

圖9　A*路徑搜索結(jié)果Fig.9　Path search results for A* method

圖10　局部路徑規(guī)劃結(jié)果Fig.10　Local path planning results

［1］Li R，Di K，Howard A B，et al. Rock modeling and matching for autonomous long-range Mars rover localization［J］. Journal of Field Robotics，2007，24（3）：187-203

［2］Matthies L，Gat E，Harrison R，et al. Mars microrover navigation：performance evaluation and enhance- ment［J］. Autonomous Robots，1995，2（4）：291-31

［3］Cheng Y，Maimone M W，Matthies L H. Visual odometry on the Mars exploration rovers［J］. IEEE Robotics and Automation Special Issue（MER），2006，13（2）：54-62

［4］張揚(yáng)眉. 性能優(yōu)異的美國(guó)“火星科學(xué)實(shí)驗(yàn)室”［J］. 國(guó)際太空. 2011（11）：17-23. Zhang Y M. American Mars Science Laboratory with excellent properties［J］. Space International，2011（11）：17-23.

［5］天力. 美好奇號(hào)漫游車登陸火星［J］. 中國(guó)航天，2012（9）：16-18 Tian L. Curiosity Mars Rover landed on Mars［J］. Erospace China，2012（9）：16-18.

［6］Griffiths A D，Coates A，Jaumann R. et. al. Context for the ESA ExoMars rover：the Panoramic Camera（PanCam） instrument［J］. International Journal of Astrobiology，2006，5（3）：269-275.

［7］寧曉琳，房建成. 一種基于純天文觀測(cè)的火星車自主導(dǎo)航方法［J］.空間科學(xué)學(xué)報(bào). 2006，26（2）：142-147. Ning X L，F(xiàn)ang J C. A new autonomous navigation method for martian rover based on celestial observation［J］. Chin. J. Space Sci.，2006，26（2）：142-147.

［8］邸凱昌，岳宗玉，劉召芹. 基于地面圖像和衛(wèi)星圖像集成的火星車定位新方法［J］. 航天器工程，2010，19（4）：11-16. Di K C，Yue Z Y，Liu Z Q. A new approach to Mars rover localization based on integration of ground and orbital images［J］. Spacecraft engineering，2010，19（4）：11-16.

［9］婁路. 基于視覺(jué)和MEMS-IMU融合的火星車導(dǎo)航定向技術(shù)［J］. 航天控制，2012，30（4）：32-36，48. Lou L. An approach of attitude estimation by using vision-aided IMU for Mars rover［J］. Aerospace Control，2012，30（4）：32-36，48.

［10］Estlin T，Gaines D. Automated decision-making for Mars rover onboard science［C］//AIAA 2007 Conference and Exhibit. Rohnert Park，California ：AIAA，2007.

［11］Joudrier L，Elfving A. Challenges of the ExoMars rover control［C］//AIAA Unmanned... Unlimited Conference. Seattle，Washington：AIAA，2009.

［12］Helmick D M，Angelova A. Terrain adaptive navigation for Mars rovers［C］// 2007 IEEE Aerospace Conference. ［S.l.］：IEEE，2007.

［13］Estlin T，Castano R. Automated targeting for the MER Rovers［C］// AIAA Unmanned... Unlimited Conference. Seattle，Washington：AIAA，2009.

［14］Stentz A. The focussed D* algorithm for real-time replanning［C］// Proceedings of IJCAI-95. ［S.l.］：IJCAI，1995.

［15］DUR E. Optical flow-based obstacle detection and avoidance behaviors for mobile robots used in unmaned planetary exploration［C］//2009 4th International Conference on Recent Advances in Space Technologies.［S.l.］：IEEE，2009.

［16］Goldberg S，Maimone M. Stereo vision and rover navigation software for planetary exploration［C］//IEEE Aerospace Conference. Big Sky，Montana：IEEE，2002.

通信地址：上海市閔行區(qū)元江路3888號(hào)805所20室（201109）

電話：（021）24186224

E-mail：weixiangquan@163.com

Researches on the Techniques of Autonomous Navigation and Path Planning for Mars Rover

WEI Xiangquan，HUANG Jianming，GU Dongqing，CHEN Feng
（Aerospace System Engineering Shanghai，Shanghai 201109， China）

Considering the mission environment，through drawing lessons from international research experience，the sensor configuration of autonomous navigation of mars rover is given，and the autonomous navigation and path planning techniques which can be used in aerospace engineering are investigated in this paper. According to the requirement of autonomous navigation and riding for rover on mars，the involved techniques are deeply researched，such as DEM（Digital Elevation Model）construction and obstacle detection，global path planning，video odometer based on stereo vision，pose determination based on multi-sensor fusion as well as local path planning. A set of experimental system was established，and the simulation experiment was carried in laboratory. The relative research results can offer some basis for the researches of autonomous navigation system for succeeding mars rover.

autonomous navigation；path planning；video odometer；multi-sensor fusion

V448.224

A

2095-7777（2016）03-0275-7

10.15982/j.issn.2095-7777.2016.03.012

魏祥泉（1974- ），男，博士，研究員， 主要研究方向：空間目標(biāo)特性，視覺(jué)導(dǎo)航，模式識(shí)別與智能系統(tǒng)。

［責(zé)任編輯：宋宏］

2015-11-01；

2016-6-30

國(guó)家自然科學(xué)基金（61573247）