[發(fā)明專利]一種基于雙目立體視覺的巡視探測(cè)器自主避障規(guī)劃方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201110412394.8
• 申請(qǐng)日: 2011-12-08
• 公開（公告）號(hào): CN102520721A
• 公開（公告）日: 2012-06-27
• 發(fā)明（設(shè)計(jì)）人: 劉祥;邢琰;滕寶毅;賈永;毛曉艷
• 申請(qǐng)（專利權(quán)）人: 北京控制工程研究所
• 主分類號(hào): G05D1/02
• 分類號(hào): G05D1/02;G06T17/05
• 代理公司: 中國航天科技專利中心 11009
• 代理人: 安麗

技術(shù)領(lǐng)域

本發(fā)明涉及一種自主避障規(guī)劃方法，適用于一類復(fù)雜未知地形中巡視探測(cè)器的自主避障規(guī)劃。

背景技術(shù)

自主避障規(guī)劃是移動(dòng)機(jī)器人路徑規(guī)劃研究領(lǐng)域中的一個(gè)分支。所謂路徑規(guī)劃，是指：在具有障礙物的環(huán)境中，為移動(dòng)機(jī)器人尋找一條從起始點(diǎn)到目標(biāo)點(diǎn)的安全、無碰的運(yùn)動(dòng)路徑。路徑規(guī)劃的研究一般涉及環(huán)境描述和路徑搜索兩個(gè)問題。其中，環(huán)境描述是指如何將機(jī)器人所處的環(huán)境有效地表達(dá)出來，建立可用于進(jìn)行規(guī)劃的環(huán)境模型；路徑搜索是指采用有效的方法，從所建立的環(huán)境模型中搜索出合適的路徑。

根據(jù)對(duì)環(huán)境信息掌握程度的不同，路徑規(guī)劃算法一般可分為兩類：全局路徑規(guī)劃和局部路徑規(guī)劃。

全局路徑規(guī)劃利用現(xiàn)有的環(huán)境信息進(jìn)行路徑規(guī)劃，如果環(huán)境信息完全已知，則能夠求解出在一定評(píng)價(jià)標(biāo)準(zhǔn)下的最優(yōu)(如路徑長(zhǎng)度最短、消耗能量最小等)路徑。全局路徑規(guī)劃在環(huán)境模型的構(gòu)建和更新方面一般具有較大的計(jì)算負(fù)擔(dān)，而且算法的復(fù)雜性高，比較適合進(jìn)行離線規(guī)劃。

局部路徑規(guī)劃不需要先驗(yàn)的環(huán)境信息，此類規(guī)劃主要依靠移動(dòng)機(jī)器人裝載的傳感器感知環(huán)境，獲取局部的環(huán)境信息，通過對(duì)環(huán)境信息的分析，按照一定的策略尋找合適的路徑。與全局路徑規(guī)劃相比，局部路徑規(guī)劃能夠在環(huán)境完全未知的情況下進(jìn)行規(guī)劃，實(shí)時(shí)性高，適合在線規(guī)劃。但由于僅依靠局部環(huán)境信息進(jìn)行規(guī)劃，往往無法得到到目標(biāo)點(diǎn)的最優(yōu)路徑，局部路徑規(guī)劃更多地是用來進(jìn)行局部避障，指引移動(dòng)機(jī)器人在躲避障礙物的同時(shí)朝向目標(biāo)點(diǎn)行駛，并最終走到目標(biāo)點(diǎn)。所以，局部路徑規(guī)劃也可稱為局部避障規(guī)劃。

目前針對(duì)行星探測(cè)器的避障規(guī)劃的研究主要有：基于Bug算法的一類避障規(guī)劃算法，例如TangentBug、WedgeBug、RoverBug等，其中RoverBug成功應(yīng)用在了JPL研制的火星探測(cè)車原理樣機(jī)Rocky7上；基于Morphin算法的避障規(guī)劃。這些規(guī)劃方法在應(yīng)用中都存在不同的缺點(diǎn)，Bug類算法建立的環(huán)境模型為二值地圖，這種模型過于簡(jiǎn)單，不適用于粗糙復(fù)雜地形上的規(guī)劃；Morphin算法僅考慮了地形的安全性，沒有結(jié)合目標(biāo)點(diǎn)的位置進(jìn)行路徑搜索，不具備路徑規(guī)劃的完備性，另外該方法需對(duì)巡視探測(cè)器的不同航向進(jìn)行分析，建模復(fù)雜，自主避障規(guī)劃的效率較低。

發(fā)明內(nèi)容

本發(fā)明的技術(shù)解決問題：克服現(xiàn)有技術(shù)的不足，提出了一種基于雙目立體視覺的行星巡視探測(cè)器自主避障規(guī)劃方法，該方法的避障規(guī)劃結(jié)果安全性高，規(guī)劃效率高，且簡(jiǎn)單，易于實(shí)現(xiàn)。

本發(fā)明的技術(shù)解決方案：一種基于雙目立體視覺的巡視探測(cè)器自主避障規(guī)劃方法，實(shí)現(xiàn)步驟如下：

(1)生成局部地形的DEM數(shù)據(jù)

根據(jù)雙目立體視覺成像原理，利用圖像匹配得到的視差數(shù)據(jù)，計(jì)算出相機(jī)圖像中像素點(diǎn)對(duì)應(yīng)的在相機(jī)坐標(biāo)系下的三維坐標(biāo)(x_c，y_c，z_c)，根據(jù)相機(jī)在巡視探測(cè)器的安裝位置和相機(jī)相對(duì)于巡視探測(cè)器的姿態(tài)，將三維坐標(biāo)(x_c，y_c，z_c)轉(zhuǎn)換到本體坐標(biāo)系下，求得(x_b，y_b，z_b)；結(jié)合巡視探測(cè)器當(dāng)前的姿態(tài)信息，將三維坐標(biāo)(x_b，y_b，z_b)轉(zhuǎn)換到水平投影坐標(biāo)系下，求得(x_r，y_r，z_r)，從而得到巡視探測(cè)器周圍局部地形的三維信息，即局部地形的DEM數(shù)據(jù)；

(2)構(gòu)建柵格地圖，進(jìn)行局部地形的DEM數(shù)據(jù)去噪處理

根據(jù)雙目立體視覺系統(tǒng)的視場(chǎng)范圍，在巡視探測(cè)器前方構(gòu)建一張柵格地圖，用(i，j)標(biāo)記柵格地圖中的單元格，其中i和j分別代表單元格所在柵格地圖中的行數(shù)和列數(shù)，對(duì)于柵格地圖中的每一個(gè)單元格(i，j)，定義一個(gè)以該單元格為中心、以巡視探測(cè)器原地轉(zhuǎn)彎一圈形成的圓，即回轉(zhuǎn)圓的直徑為邊長(zhǎng)的方形的地形塊Patch；

從步驟(1)得到的DEM數(shù)據(jù)中找出落在柵格地圖單元格對(duì)應(yīng)的地形塊Patch區(qū)域內(nèi)的數(shù)據(jù)點(diǎn)，統(tǒng)計(jì)這些數(shù)據(jù)點(diǎn)中z_r坐標(biāo)的平均值和方差σ_z，根據(jù)統(tǒng)計(jì)得到的z_r坐標(biāo)的平均值和方差σ_z，對(duì)地形塊Patch中的DEM數(shù)據(jù)點(diǎn)進(jìn)行噪點(diǎn)去除；

所述步驟(2)地圖單元格的尺寸取為巡視探測(cè)器車輪直徑大小。

(3)地形可通過性分析，生成適宜度地圖