本發(fā)明屬于機(jī)器人導(dǎo)航領(lǐng)域，尤其涉及一種基于深度攝像機(jī)的移動(dòng)機(jī)器人定位和導(dǎo)航方法。

背景技術(shù)：

對于具有有限計(jì)算能力的室內(nèi)移動(dòng)機(jī)器人，采用深度攝像頭用于機(jī)器人自主移動(dòng)主要有以下兩方面障礙：首先，深度攝像頭通常會(huì)生成冗長的數(shù)據(jù)，這些數(shù)據(jù)在機(jī)器人的實(shí)時(shí)定位中不能被完全處理；其次，考慮到所使用的內(nèi)存和計(jì)算復(fù)雜度是在3d環(huán)境下，2d環(huán)境中的定位和繪制地圖技術(shù)(占用柵格)不能很好地去衡量。相反地，將3d環(huán)境中的觀察物映射到現(xiàn)有的2d地圖中這一問題并不簡單。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的在于針對上述技術(shù)中存在的缺陷，提出了一種基于深度攝像機(jī)的移動(dòng)機(jī)器人定位和導(dǎo)航方法。

為了實(shí)現(xiàn)上述目的，本發(fā)明提供的技術(shù)方案如下：一種基于深度攝像機(jī)的移動(dòng)機(jī)器人定位和導(dǎo)航方法，其特征在于包括如下步驟：

步驟1、快速采樣平面濾波算法(fspf)：fspf將深度圖像i作為輸入，并創(chuàng)建一個(gè)具有n個(gè)3d點(diǎn)的列表p、一個(gè)對應(yīng)平面向量的列表r以及一個(gè)不與任何平面相對應(yīng)的異常點(diǎn)列表o，此算法旨在生成一個(gè)最多含有nmax個(gè)3d點(diǎn)和它們所對應(yīng)的平面向量的列表。

步驟2、平面濾波點(diǎn)云的定位：采用蒙特卡羅定位(mcl)和矯正的梯度調(diào)整(cgr)來實(shí)現(xiàn)；

步驟2.1、3d濾波點(diǎn)云p和對應(yīng)的平面向量i首先被投影到2d來生成一個(gè)2d點(diǎn)云p’和對應(yīng)的規(guī)范化的向量r’；

步驟2.2、對于每個(gè)p’中的點(diǎn)pi，找到線li(li∈l)，使得pi-x1方向和從x1出發(fā)的射線與li相交；

步驟2.3、如果存在找不到線li的點(diǎn)，那么將這些點(diǎn)舍棄，對應(yīng)的向量估計(jì)值ri與線li的差異值比閾值θi大的點(diǎn)也會(huì)被舍棄；計(jì)算來自于(擴(kuò)展的)線li對應(yīng)的點(diǎn)pi的垂直距離di；

步驟2.4、所有的(非標(biāo)準(zhǔn)化)觀察可能性p(y/x)由下式計(jì)算得到：

其中，σ是一個(gè)距離測量的標(biāo)準(zhǔn)偏差，f:f>1是一個(gè)用來折損 觀察點(diǎn)間相關(guān)性的貼現(xiàn)系數(shù)；

步驟3、自主導(dǎo)航：通過計(jì)算機(jī)器人到不同方向角度的開放路徑長度來完成機(jī)器人自主導(dǎo)航，確保能夠成功完成環(huán)境中的障礙物檢測；

所述步驟3中障礙物檢測通常采用來自步驟1中集合p和o中的3d點(diǎn)來實(shí)現(xiàn)，對于給定的機(jī)器人半徑r和期望的前進(jìn)方向θd，開放路徑長度d(θ)是一個(gè)關(guān)于前進(jìn)方向θ的函數(shù)，篩選后的障礙物避免方向θ*由如下計(jì)算得到：

其中，是角度θ方向上的單位向量并且坐標(biāo)系原點(diǎn)和機(jī)器人中心點(diǎn)相一致。

本發(fā)明具有如下有益效果：

1)本發(fā)明提出的快速采樣平面濾波算法(fspf)，通過從深度圖像中采集樣本點(diǎn)和對屬于3d平面的點(diǎn)集(也稱作平面濾波點(diǎn))或者與平面不相對應(yīng)的點(diǎn)(也稱作異常點(diǎn))進(jìn)行分類，減少3d點(diǎn)云數(shù)量的同時(shí)不被異常值影響。

2)本發(fā)明基于一個(gè)觀察模型提出了一個(gè)定位算法，此算法能夠?qū)⑵矫鏋V波點(diǎn)向下映射到2d并且對每個(gè)2d地圖上的點(diǎn)到線分配對應(yīng)的值，并采用完全采樣的點(diǎn)云(由平面濾波點(diǎn)和異常點(diǎn)組成)作為自主導(dǎo)航的障礙物避免算法，成功完成環(huán)境中障礙物檢測，提高了機(jī)器人自主定位和導(dǎo)航的準(zhǔn)確性。

附圖說明

圖1為本發(fā)明所述的基于深度攝像機(jī)的移動(dòng)機(jī)器人定位和導(dǎo)航方法流程圖。

圖2為實(shí)現(xiàn)本發(fā)明采用的kinect和激光測距儀偏移誤差對照圖。

具體實(shí)施方式

下面結(jié)合附圖對本發(fā)明做進(jìn)一步的說明。

一種基于深度攝像機(jī)的移動(dòng)機(jī)器人定位和導(dǎo)航方法，該方法包括以下步驟：如附圖1所示，

步驟1、快速采樣平面濾波算法(fspf)：

步驟1.1、fspf將深度圖像i作為輸入，采樣三個(gè)位置分別記作d0，d1，d2；首先，從圖像中的任何一個(gè)地方隨機(jī)選擇第一個(gè)位置d0，接著在d0的附近隨機(jī)選擇鄰居大小為η的位置d1和d2；

步驟1.2、創(chuàng)建一個(gè)具有n個(gè)3d點(diǎn)的列表p，步驟1.1中采樣的三個(gè)位置d0，d1，d2對應(yīng)的3d坐標(biāo)能夠被計(jì)算出來并記作p0，p1，p2，基于點(diǎn)p0，p1，p2的平均深度(z坐標(biāo))和環(huán)境中的平面最小期望值s能夠計(jì)算得到具有寬度為w’高度為h’的搜索窗口， 從搜索窗口采樣額外的l-3局部樣本dj以得到總共l個(gè)局部樣本；

步驟1.3、由步驟1.2中所述點(diǎn)p0，p1，p2定義的用于從平面中重建3d點(diǎn)pj的平面擬合誤差被計(jì)算以用于確定該點(diǎn)是否是異常值，如果在搜索窗口中有超過αinl個(gè)點(diǎn)被歸為非異常點(diǎn)，那么所有其他非異常點(diǎn)被加入到列表p中并且相關(guān)的向量被加入到列表r中，此算法最多運(yùn)行mmax次來生成一個(gè)最多含有nmax個(gè)3d點(diǎn)列表p和它們所對應(yīng)的平面向量的列表r以及一個(gè)不與任何平面相對應(yīng)的異常點(diǎn)列表o；

步驟2、平面濾波點(diǎn)云的定位：采用蒙特卡羅定位(mcl)和矯正的梯度調(diào)整(cgr)來實(shí)現(xiàn)；

步驟2.1、3d濾波點(diǎn)云p和對應(yīng)的平面向量i首先被投影到2d來生成一個(gè)2d點(diǎn)云p’和對應(yīng)的規(guī)范化的向量r’；對應(yīng)的地面平面檢測點(diǎn)在這一步中并不需要考慮。假設(shè)機(jī)器人x的位置用x＝{x1，x2}來表示，其中x1是機(jī)器人的2d位置，x2是它的方向角度，可觀察的場景線列表l可以用解析光線投射計(jì)算得到；

步驟2.2、對于每個(gè)p’中的點(diǎn)pi，找到線li(li∈l)，使得pi-x1方向和從x1出發(fā)的射線與li相交；

步驟2.3、如果存在找不到線li的點(diǎn)，那么將這些點(diǎn)舍棄，對應(yīng)的向量估計(jì)值ri與線li的差異值比閾值θi大的點(diǎn)也會(huì)被舍棄；計(jì)算來自于(擴(kuò)展的)線li對應(yīng)的點(diǎn)pi的垂直距離di；

步驟2.4、所有的(非標(biāo)準(zhǔn)化)觀察可能性p(y/x)由下式計(jì)算得到：

其中，σ是一個(gè)距離測量的標(biāo)準(zhǔn)偏差，f:f>1是一個(gè)用來折損觀察點(diǎn)間相關(guān)性的貼現(xiàn)系數(shù)；

步驟3、自主導(dǎo)航：通過計(jì)算機(jī)器人到不同方向角度的開放路徑長度來完成機(jī)器人自主導(dǎo)航，確保能夠成功完成環(huán)境中的障礙物檢測；

所述障礙物檢測通常采用來自步驟1中集合p和o中的3d點(diǎn)來實(shí)現(xiàn)，對于給定的機(jī)器人半徑r和期望的前進(jìn)方向θd，開放路徑長度d(θ)是一個(gè)關(guān)于前進(jìn)方向θ的函數(shù)，篩選后的障礙物避免方向θ*由如下計(jì)算得到：

其中，是角度θ方向上的單位向量并且坐標(biāo)系原點(diǎn)和機(jī)器人中心點(diǎn)相一致。

本發(fā)明實(shí)驗(yàn)的實(shí)現(xiàn)是基于傳統(tǒng)全向建立的室內(nèi)移動(dòng)機(jī)器人，該機(jī)器人備有微軟公 司旗下的kinect攝像頭傳感器。為了與此攝像頭傳感器形成對比，本發(fā)明同時(shí)也采用了hokuyourg-04lx2d激光測距掃描儀并采用mcl和cgr的點(diǎn)云傳感模型來實(shí)現(xiàn)其定位功能。

圖2顯示當(dāng)機(jī)器人行走374m后，kinect和激光測距感應(yīng)器在定位方面的相對應(yīng)的平均誤差。即使kinect在定位方面并沒有激光測距感應(yīng)器那么準(zhǔn)確，但是對于室內(nèi)移動(dòng)機(jī)器人導(dǎo)航方面來說其結(jié)果已經(jīng)相當(dāng)精確了，其平均偏移誤差比20cm要小同時(shí)其平均角度誤差大概在1.5°左右。值得注意的是，在成本方面kinect要比激光測距儀便宜。因此對于機(jī)器人研究的預(yù)算方面而言kinect性價(jià)比更高。

為了驗(yàn)證深度攝像頭在定位和導(dǎo)航解決方案方面的穩(wěn)健性，還設(shè)置了一系列隨機(jī)導(dǎo)航點(diǎn)用于機(jī)器人的導(dǎo)航并將這些點(diǎn)分布到地圖上。其中，路徑的總長度超過了4.1km。在整個(gè)實(shí)驗(yàn)過程中，只有kinect傳感器被用于定位和障礙物避免。機(jī)器人能夠成功地被導(dǎo)航到所有的導(dǎo)航點(diǎn)，但是定位在三處被重設(shè)置，這些地方都是地圖中kinect傳感器在一段時(shí)間內(nèi)于開放地區(qū)中不能檢測到墻的位置。

以上所述僅為本發(fā)明的較佳實(shí)施例而已，并不用以限制本發(fā)明，凡在本發(fā)明的精神和原則之內(nèi)的所作的任何修改、等同替換和改進(jìn)等，均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。