專利名稱：一種基于雙目立體視覺的巡視探測(cè)器自主避障規(guī)劃方法
技術(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。，y。，Ζ。)，根據(jù)相機(jī)在巡視探測(cè)器的安裝位置和相機(jī)相對(duì)于巡視探測(cè)器的姿態(tài)，將三維坐標(biāo)(χ。，yc, zc)轉(zhuǎn)換到本體坐標(biāo)系下，求得Ub， yb，zb)；結(jié)合巡視探測(cè)器當(dāng)前的姿態(tài)信息，將三維坐標(biāo)(xb，yb, zb)轉(zhuǎn)換到水平投影坐標(biāo)系下，求得(χ,，yr, ζ,)，從而得到巡視探測(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)的方形的地形塊I^atch;
從步驟(1)得到的DEM數(shù)據(jù)中找出落在柵格地圖單元格對(duì)應(yīng)的地形塊I^atch區(qū)域內(nèi)的數(shù)據(jù)點(diǎn)，統(tǒng)計(jì)這些數(shù)據(jù)點(diǎn)中％坐標(biāo)的平均值I和方差ο z，根據(jù)統(tǒng)計(jì)得到的％坐標(biāo)的平均值I和方差σ ζ，對(duì)地形塊I^atch中的DEM數(shù)據(jù)點(diǎn)進(jìn)行噪點(diǎn)去除；
所述步驟( 地圖單元格的尺寸取為巡視探測(cè)器車輪直徑大小。
(3)地形可通過性分析，生成適宜度地圖
針對(duì)步驟O)的柵格地圖，利用去噪后的DEM數(shù)據(jù)對(duì)地圖中的每一個(gè)單元格(i， j)進(jìn)行地形可通過性分析，生成適宜度地圖，分析步驟為
(31)首先找到所述單元格(i，j)對(duì)應(yīng)的地形塊I^atch中包含的DEM數(shù)據(jù)點(diǎn)，對(duì)所述DEM數(shù)據(jù)點(diǎn)的三維坐標(biāo)進(jìn)行平面擬合；
(32)計(jì)算擬合平面的法向量與垂直向量的夾角識(shí)，爐即表示該地形區(qū)域的坡度；由地形塊I^atch中每個(gè)DEM數(shù)據(jù)點(diǎn)到擬合平面的垂直距離計(jì)算擬合殘差r，r表示該地形區(qū)域的粗糙度；計(jì)算地形塊I^atch中每個(gè)DEM數(shù)據(jù)點(diǎn)到擬合平面垂直距離的高度差A(yù)h，Ah表示該地形區(qū)域的高度落差；從所述坡度、粗糙度和高度三方面，對(duì)該地形塊Patch對(duì)應(yīng)的地形區(qū)域的可通過性指標(biāo)進(jìn)行評(píng)價(jià)，分別得到坡度適宜度 、粗糙度適宜度rg和高度適宜度 hg，從三者中取最小值，作為當(dāng)前分析的單元格(i，j)的適宜度」；
對(duì)柵格地圖中的所有單元格進(jìn)行上述計(jì)算，從而得到適宜度地(4)歷史數(shù)據(jù)融合，得到融合后的適宜度地圖
若當(dāng)前規(guī)劃是第一次規(guī)則，則執(zhí)行步驟(5)；
若當(dāng)前規(guī)劃不是第一次規(guī)劃，則將前一次規(guī)劃獲得的適宜度地圖與當(dāng)前由步驟 (3)獲得的適宜度地圖進(jìn)行數(shù)據(jù)融合，得到融合后的適宜度地(5)在適宜度地圖上生成備選路徑弧
在步驟(4)得到的融合后的適宜度地圖上均勻設(shè)置若干條備選路徑弧，每條備選路徑弧的弧長(zhǎng)Li相同，但對(duì)應(yīng)的方向角α i和半徑氏不同，其中，
第i條備選路徑弧的方向角
權(quán)利要求
1. 一種基于雙目立體視覺的巡視探測(cè)器自主避障規(guī)劃方法，其特征在于實(shí)現(xiàn)步驟如下(1)生成局部地形的三維數(shù)據(jù)(DEM)根據(jù)雙目立體視覺成像原理，利用圖像匹配得到的視差數(shù)據(jù)，計(jì)算出相機(jī)圖像中像素點(diǎn)對(duì)應(yīng)的在相機(jī)坐標(biāo)系下的三維坐標(biāo)(χ。，yc, ζ。)，根據(jù)相機(jī)在巡視探測(cè)器的安裝位置和相機(jī)相對(duì)于巡視探測(cè)器的姿態(tài)，將三維坐標(biāo)U。，y。，z。)轉(zhuǎn)換到本體坐標(biāo)系下，求得Ub，yb，zb)； 結(jié)合巡視探測(cè)器當(dāng)前的姿態(tài)信息，將三維坐標(biāo)Ub，yb, zb)轉(zhuǎn)換到水平投影坐標(biāo)系下，求得 (xr, yr, ζ,)，從而得到巡視探測(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)的方形的地形塊I^atch;從步驟(1)得到的DEM數(shù)據(jù)中找出落在柵格地圖單元格對(duì)應(yīng)的地形塊I^atch區(qū)域內(nèi)的數(shù)據(jù)點(diǎn)，統(tǒng)計(jì)這些數(shù)據(jù)點(diǎn)中^坐標(biāo)的平均值I和方差ο z，根據(jù)統(tǒng)計(jì)得到的％坐標(biāo)的平均值 I和方差σ ζ，對(duì)地形塊I^atch中的DEM數(shù)據(jù)點(diǎn)進(jìn)行噪點(diǎn)去除；(3)地形可通過性分析，生成適宜度地圖針對(duì)步驟O)的柵格地圖，利用去噪后的DEM數(shù)據(jù)對(duì)地圖中的每一個(gè)單元格(i，j)進(jìn)行地形可通過性分析，生成適宜度地圖，分析步驟為(31)首先找到所述單元格(i，j)對(duì)應(yīng)的地形塊I^atch中包含的DEM數(shù)據(jù)點(diǎn)，對(duì)所述DEM 數(shù)據(jù)點(diǎn)的三維坐標(biāo)進(jìn)行平面擬合；(32)計(jì)算擬合平面的法向量與垂直向量的夾角P，爐即表示該地形區(qū)域的坡度；由地形塊I^atch中每個(gè)DEM數(shù)據(jù)點(diǎn)到擬合平面的垂直距離計(jì)算擬合殘差r，r表示該地形區(qū)域的粗糙度；計(jì)算地形塊I^atch中每個(gè)DEM數(shù)據(jù)點(diǎn)到擬合平面垂直距離的高度差A(yù)h，Ah表示該地形區(qū)域的高度落差；從所述坡度、粗糙度和高度三方面，對(duì)該地形塊I^atch對(duì)應(yīng)的地形區(qū)域的可通過性指標(biāo)進(jìn)行評(píng)價(jià)，分別得到坡度適宜度 、粗糙度適宜度rg和高度適宜度hg，從三者中取最小值，作為當(dāng)前分析的單元格(i，j)的適宜度；對(duì)柵格地圖中的所有單元格進(jìn)行上述計(jì)算，從而得到適宜度地圖；(4)歷史數(shù)據(jù)融合，得到融合后的適宜度地圖若當(dāng)前規(guī)劃是第一次規(guī)則，則執(zhí)行步驟(5)；若當(dāng)前規(guī)劃不是第一次規(guī)劃，則將前一次規(guī)劃獲得的適宜度地圖與當(dāng)前由步驟(3)獲得的適宜度地圖進(jìn)行數(shù)據(jù)融合，得到融合后的適宜度地圖；(5)在適宜度地圖上生成備選路徑弧在步驟(4)得到的融合后的適宜度地圖上均勻設(shè)置若干條備選路徑弧，每條備選路徑弧的弧長(zhǎng)Li相同，但對(duì)應(yīng)的方向角α i和半徑氏不同，其中，第i條備選路徑弧的方向角
2.根據(jù)權(quán)利要求1所述的基于雙目立體視覺的巡視探測(cè)器自主避障規(guī)劃技術(shù)，其特征在于所述步驟(1)中相機(jī)坐標(biāo)系定義為原點(diǎn)位于相機(jī)光心，&軸沿相機(jī)的光軸向前，與圖像平面垂直，Xc軸與\軸垂直，指向相機(jī)系統(tǒng)的右側(cè)，Yc軸與X。軸、Z。軸滿足右手定則。
3.根據(jù)權(quán)利要求1所述的基于雙目立體視覺的巡視探測(cè)器自主避障規(guī)劃技術(shù)，其特征在于所述步驟(1)中本體坐標(biāo)系定義為原點(diǎn)位于巡視探測(cè)器質(zhì)心，\軸指向巡視探測(cè)器的前進(jìn)方向，Yb軸與\軸垂直，指向巡視探測(cè)器的右側(cè)，Zb軸與)(b軸、Yb軸滿足右手定則。
4.根據(jù)權(quán)利要求1所述的基于雙目立體視覺的巡視探測(cè)器自主避障規(guī)劃技術(shù)，其特征在于所述步驟(1)中水平投影坐標(biāo)系定義為原點(diǎn)位于巡視探測(cè)器質(zhì)心，&軸為本體坐標(biāo)系\軸在水平面上的投影，Yr軸在水平面上，與\軸垂直，指向巡視探測(cè)器的右側(cè)，&軸與 I軸、乙軸滿足右手定則。
5.根據(jù)權(quán)利要求1所述的基于雙目立體視覺的巡視探測(cè)器自主避障規(guī)劃技術(shù)，其特征在于所述步驟(32)中坡度適宜度\=1-1^11(1，識(shí) 其中灼為巡視探測(cè)器的最大安全傾角；粗糙度適宜度。=l-min(l, r/3Hs)，其中Hs為巡視探測(cè)器的最大越障高度；高度適宜度、=i-(Ah/Hs)20
6.根據(jù)權(quán)利要求1所述的基于雙目立體視覺的巡視探測(cè)器自主避障規(guī)劃技術(shù)，其特征在于所述步驟(4)的將前一次規(guī)劃獲得的適宜度地圖與當(dāng)前由步驟C3)獲得的適宜度地圖進(jìn)行數(shù)據(jù)融合方法為首先，根據(jù)雙目立體視覺系統(tǒng)的視場(chǎng)范圍，確定巡視探測(cè)器前方無數(shù)據(jù)區(qū)域的大致范圍；然后，根據(jù)該區(qū)域的范圍，從步驟O)中的柵格地圖中確定該區(qū)域所包含的柵格地圖單元格；最后，利用前后兩次規(guī)劃得到的適宜度地圖數(shù)據(jù)，對(duì)所找出的柵格地圖單元格的適宜度進(jìn)行賦值計(jì)算，從而得到融合后的適宜度地圖。
7.根據(jù)權(quán)利要求6所述的基于雙目立體視覺的巡視探測(cè)器自主避障規(guī)劃技術(shù)，其特征在于所述賦值計(jì)算的方法采用雙線性插值的方法。
8.根據(jù)權(quán)利要求1所述的基于雙目立體視覺的巡視探測(cè)器自主避障規(guī)劃技術(shù)，其特征在于所述步驟(5)中的備選路徑弧方向角α i的定義為備選路徑弧末端的切線與巡視探測(cè)器前進(jìn)方向的夾角。
9.根據(jù)權(quán)利要求1所述的基于雙目立體視覺的巡視探測(cè)器自主避障規(guī)劃技術(shù)，其特征在于所述步驟( 地圖單元格的尺寸取為巡視探測(cè)器車輪直徑大小。
全文摘要
一種基于雙目立體視覺的巡視探測(cè)器自主避障規(guī)劃方法生成局部地形的DEM數(shù)據(jù)；根據(jù)雙目立體視覺系統(tǒng)的視場(chǎng)范圍構(gòu)建柵格地圖，對(duì)地圖上每個(gè)柵格進(jìn)行DEM數(shù)據(jù)統(tǒng)計(jì)，并對(duì)DEM進(jìn)行去噪處理；進(jìn)行地形可通過性分析，生成適宜度地圖；利用前后兩次規(guī)劃時(shí)生成的適宜度地圖，進(jìn)行數(shù)據(jù)融合，得到融合后的用于避障規(guī)劃的適宜度地圖；在適宜度地圖上放置若干條備選路徑??；對(duì)每一條備選路徑弧，從到目標(biāo)點(diǎn)的距離和安全避障能力兩個(gè)方面進(jìn)行評(píng)價(jià)；從所有備選路徑弧中篩選出評(píng)價(jià)值高于設(shè)定閾值的備選路徑弧，并從中選擇評(píng)價(jià)值最高的路徑弧作為本次避障規(guī)劃的結(jié)果。本發(fā)明簡(jiǎn)單完備、安全性高，規(guī)劃效率高，易于工程實(shí)現(xiàn)，可用于復(fù)雜地形上的巡視探測(cè)任務(wù)。
文檔編號(hào)G06T17/05GK102520721SQ20111041239
公開日2012年6月27日 申請(qǐng)日期2011年12月8日 優(yōu)先權(quán)日2011年12月8日
發(fā)明者劉祥, 毛曉艷, 滕寶毅, 賈永, 邢琰 申請(qǐng)人:北京控制工程研究所