專利名稱：一種基于視覺(jué)信息校正的室內(nèi)移動(dòng)機(jī)器人實(shí)時(shí)導(dǎo)航方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種基于視覺(jué)信息校正的室內(nèi)移動(dòng)機(jī)器人實(shí)時(shí)導(dǎo)航方法，屬于機(jī)器人導(dǎo)航領(lǐng)域。

背景技術(shù)：
導(dǎo)航系統(tǒng)是移動(dòng)機(jī)器人的重要組成部分之一，它能夠提供給機(jī)器人正確的位置和姿態(tài)信息，從而使得機(jī)器人能在一定的環(huán)境中進(jìn)行工作，完成指定的任務(wù)。移動(dòng)機(jī)器人的導(dǎo)航系統(tǒng)也是實(shí)現(xiàn)移動(dòng)機(jī)器人路徑規(guī)劃，避障避碰，智能自主控制的基礎(chǔ)。
目前常見(jiàn)的機(jī)器人導(dǎo)航系統(tǒng)的方式有電磁導(dǎo)航、光反射導(dǎo)航、GPS導(dǎo)航、環(huán)境地圖模型匹配導(dǎo)航、路標(biāo)導(dǎo)航、視覺(jué)導(dǎo)航等。
(1)電磁導(dǎo)航也稱地下埋線導(dǎo)航，其原理是在路徑上連續(xù)的埋設(shè)多條引導(dǎo)電纜，分別流過(guò)不同頻率的電流，通過(guò)感應(yīng)線圈對(duì)電流的檢測(cè)來(lái)獲得感知信息。該技術(shù)簡(jiǎn)單實(shí)用，但是成本高、改造和維護(hù)困難。
(2)光反射導(dǎo)航的原理是在路徑上連續(xù)的鋪設(shè)光反射條，同電磁導(dǎo)航一樣，該技術(shù)也相當(dāng)?shù)某墒?，目前?guó)內(nèi)制造行業(yè)使用的移動(dòng)機(jī)器人大多是基于光反射導(dǎo)航與電磁導(dǎo)航這兩種導(dǎo)航方式的，但是它們應(yīng)用范圍比較窄，不適合在動(dòng)態(tài)、變換工作環(huán)境下的移動(dòng)機(jī)器人導(dǎo)航。
(3)GPS導(dǎo)航全球定位系統(tǒng)(GPS)在陸地車輛導(dǎo)航系統(tǒng)中得到了廣泛的應(yīng)用。但是在諸如城市地區(qū)，高樓大廈GPS信號(hào)經(jīng)常阻塞，這就意味著有一大部分空間無(wú)法獲得GPS信號(hào)，在室內(nèi)也同樣存在同樣的問(wèn)題，所以不能采用逐點(diǎn)定位的導(dǎo)航系統(tǒng)。
(4)環(huán)境地圖模型匹配導(dǎo)航是機(jī)器人通過(guò)自身的各種傳感器探測(cè)周圍環(huán)境，利用感知到的局部環(huán)境信息進(jìn)行局部的地圖構(gòu)造，并與其內(nèi)部事先存儲(chǔ)的完整地圖進(jìn)行匹配。通過(guò)匹配，機(jī)器人可確定自身的位置，并根據(jù)預(yù)先規(guī)劃的一條全局路線，采用路徑跟蹤和相關(guān)避障技術(shù)來(lái)實(shí)現(xiàn)導(dǎo)航。
(5)路標(biāo)導(dǎo)航是指路標(biāo)在環(huán)境中的坐標(biāo)、形狀等特征已知的前提下，機(jī)器人通過(guò)對(duì)路標(biāo)的探測(cè)來(lái)確定自身的位置，同時(shí)將全局路線分解成為路標(biāo)與路標(biāo)間的片段，逐段行進(jìn)完成導(dǎo)航。雖然人工路標(biāo)導(dǎo)航比較容易實(shí)現(xiàn)，但它人為地改變了機(jī)器人工作的環(huán)境，自然路標(biāo)導(dǎo)航不改變工作環(huán)境，但機(jī)器人需要通過(guò)對(duì)工作環(huán)境中的自然特征進(jìn)行識(shí)別以完成導(dǎo)航。路標(biāo)探測(cè)的穩(wěn)定性和魯棒性是采用該方法時(shí)需要研究的主要問(wèn)題。
(6)視覺(jué)導(dǎo)航具有信號(hào)探測(cè)范圍寬、獲取信息完整等優(yōu)點(diǎn)，將成為未來(lái)機(jī)器人導(dǎo)航的一個(gè)主要發(fā)展方向。在視覺(jué)導(dǎo)航方式中，目前應(yīng)用最多的還是采用在機(jī)器人上安裝車載攝像機(jī)的基于局部視覺(jué)的導(dǎo)航方式，如D.L.Boley等研制的移動(dòng)機(jī)器人，利用車載攝像機(jī)和較少的傳感器通過(guò)識(shí)別路標(biāo)進(jìn)行導(dǎo)航，比直接采用卡爾曼濾波器具有更好的實(shí)時(shí)性，并可有效抑制噪聲。采用局部視覺(jué)這種導(dǎo)航方式，所有的計(jì)算設(shè)備和傳感器都裝載在機(jī)器人車體上。圖像識(shí)別、路徑規(guī)劃等高層決策都由車載計(jì)算機(jī)完成，所以車載計(jì)算機(jī)的工作量較大，延遲問(wèn)題也較為明顯。
(7)基于傳感器數(shù)據(jù)的機(jī)器人導(dǎo)航方式一般機(jī)器人都安裝了一些非視覺(jué)傳感器，如里程計(jì)，磁慣傳感器，超聲傳感器、紅外傳感器、接觸傳感器等。這類傳感器可以實(shí)現(xiàn)對(duì)機(jī)器人的定位與導(dǎo)航。比如，當(dāng)機(jī)器人在光線很暗的環(huán)境中的時(shí)候，視覺(jué)導(dǎo)航方式將會(huì)失效。在這種情況下，可以利用里程計(jì)或磁慣傳感器來(lái)進(jìn)行導(dǎo)航。
根據(jù)室內(nèi)移動(dòng)機(jī)器人導(dǎo)航的要求，目前最常見(jiàn)的是基于里程計(jì)或磁慣傳感器的導(dǎo)航系統(tǒng)以及基于視覺(jué)的導(dǎo)航系統(tǒng)，這兩類系統(tǒng)本身都存在著一定的局限性，首先里程計(jì)或磁慣傳感器通常處理速度較快，但是由于它們都是利用航跡遞推的方法進(jìn)行導(dǎo)航定位，因此必然會(huì)引入較大的累積誤差。而基于視覺(jué)的導(dǎo)航系統(tǒng)通常精度較高，但是圖像的獲取以及處理所消耗的時(shí)間較大，因此實(shí)時(shí)性較差。


發(fā)明內(nèi)容
本發(fā)明的目的是為了克服現(xiàn)有技術(shù)的不足，提出一種基于視覺(jué)信息校正的室內(nèi)移動(dòng)機(jī)器人實(shí)時(shí)導(dǎo)航方法，該方法通過(guò)綜合集成視覺(jué)信息以及里程計(jì)信息的優(yōu)勢(shì)和特點(diǎn)，使得該導(dǎo)航系統(tǒng)同時(shí)兼顧實(shí)時(shí)性要求和導(dǎo)航精度要求。
本發(fā)明的一種基于視覺(jué)信息校正的室內(nèi)移動(dòng)機(jī)器人實(shí)時(shí)導(dǎo)航方法，其特征在于采用將視覺(jué)信息與里程計(jì)信息相融合的方式，通過(guò)對(duì)各濾波器估計(jì)位姿的最優(yōu)融合，獲得機(jī)器人的最優(yōu)估計(jì)位姿，從而提高系統(tǒng)的實(shí)時(shí)性和精確度。其主要步驟為 步驟一初始化機(jī)器人導(dǎo)航系統(tǒng)，啟動(dòng)里程計(jì)和視覺(jué)傳感器； 步驟二里程計(jì)和視覺(jué)傳感器得到機(jī)器人當(dāng)前位資，構(gòu)造卡爾曼濾波器，獲得里程計(jì)濾波估計(jì)值和視覺(jué)傳感器濾波估計(jì)值； 步驟三里程計(jì)濾波估計(jì)值和視覺(jué)傳感器濾波估計(jì)值融合； 步驟四參數(shù)重置； 本發(fā)明的優(yōu)點(diǎn)在于 (1)本發(fā)明充分利用了視覺(jué)信息和里程計(jì)信息的各自優(yōu)點(diǎn)，將視覺(jué)信息的精確性和里程計(jì)信息的實(shí)時(shí)性相結(jié)合起來(lái)，在大部分時(shí)間下，利用里程計(jì)自身信息進(jìn)行遞推計(jì)算，獲得導(dǎo)航數(shù)據(jù)，保證了導(dǎo)航系統(tǒng)的實(shí)時(shí)性要求。同時(shí)利用視覺(jué)信息來(lái)矯正里程計(jì)航跡推算過(guò)程中產(chǎn)生的累積誤差，從而大大提高了導(dǎo)航系統(tǒng)的準(zhǔn)確性； (2)本發(fā)明采用了基于混合高斯背景建模的全局運(yùn)動(dòng)目標(biāo)檢測(cè)方法，大大提高了運(yùn)動(dòng)目標(biāo)檢測(cè)的抗干擾能力，對(duì)光照的變化以及緩慢變化的背景噪聲有抑制作用； (3)本發(fā)明在獲得最優(yōu)濾波估計(jì)之后，加入了參數(shù)的重置，使得各濾波器始終以最優(yōu)濾波估計(jì)值為基礎(chǔ)進(jìn)行遞推估計(jì)，加快了濾波器的收斂速度； (4)本發(fā)明利用當(dāng)前獲得的最優(yōu)濾波估計(jì)值，對(duì)下一幀圖像中目標(biāo)可能出現(xiàn)的位置進(jìn)行了預(yù)測(cè)，大大提高了目標(biāo)檢測(cè)的速度。



圖1是本發(fā)明所述方法的流程圖； 圖2是本發(fā)明卡爾曼濾波器的示意圖。

具體實(shí)施例方式 下面將結(jié)合附圖和實(shí)施例對(duì)本發(fā)明作進(jìn)一步的詳細(xì)說(shuō)明。
本發(fā)明是一種基于視覺(jué)信息校正的室內(nèi)移動(dòng)機(jī)器人實(shí)時(shí)導(dǎo)航方法，流程如圖1所示，主要包括以下步驟 步驟一初始化機(jī)器人導(dǎo)航系統(tǒng)，啟動(dòng)里程計(jì)和視覺(jué)傳感器； 對(duì)導(dǎo)航系統(tǒng)進(jìn)行初始化，包括確定移動(dòng)機(jī)器人的初始位置以及初始化所有濾波器的各個(gè)參數(shù)，其中濾波器的主要參數(shù)為移動(dòng)機(jī)器人運(yùn)動(dòng)模型的系統(tǒng)噪聲方差矩陣和各傳感器的測(cè)量噪聲方差矩陣，啟動(dòng)里程計(jì)和視覺(jué)傳感器。
步驟二里程計(jì)和視覺(jué)傳感器得到機(jī)器人當(dāng)前位資，構(gòu)造卡爾曼濾波器，獲得里程計(jì)濾波估計(jì)值和視覺(jué)傳感器濾波估計(jì)值； (1)里程計(jì)測(cè)量通道； 利用里程計(jì)獲得移動(dòng)機(jī)器人左、右輪的移動(dòng)距離和移動(dòng)速度，采用航跡遞推獲的方法獲得當(dāng)前機(jī)器人位姿。
(2)視覺(jué)傳感器測(cè)量通道； 通過(guò)全局運(yùn)動(dòng)目標(biāo)檢測(cè)算法檢測(cè)移動(dòng)機(jī)器人在圖像中的位置，并通過(guò)離線的標(biāo)定建立圖像位置與實(shí)際空間的映射關(guān)系，從而測(cè)量獲得移動(dòng)機(jī)器人當(dāng)前位姿，其具體步驟如下 ①采用基于混合高斯背景建模的運(yùn)動(dòng)目標(biāo)檢測(cè)方法獲得移動(dòng)機(jī)器人在圖像中的位置； 采用基于混合高斯背景建模方法進(jìn)行移動(dòng)機(jī)器人目標(biāo)的運(yùn)動(dòng)檢測(cè)，用K個(gè)不同均值與方差的高斯分布來(lái)描述非完全靜止條件下的背景圖像。設(shè)像素點(diǎn)(u，v)的噪聲強(qiáng)度z(u，v)服從混合高斯分布，則該點(diǎn)的概率密度函數(shù)可用式(1)表示 其中η(zuv|μj，uv，∑j，uv)表示用于描述采集到的圖像中(u，v)點(diǎn)的第j個(gè)高斯分布，其均值為μj，uv，方差為∑j，uv，該點(diǎn)當(dāng)前灰度值為zuv，而wj，uv為第j個(gè)高斯分布的加權(quán)權(quán)重，P(zuv)為圖像中(u，v)點(diǎn)的概率分布函數(shù)。
基于混合高斯背景建模的過(guò)程包括以下步驟 i)初始化模型； 用視覺(jué)傳感器采集的第一幅圖像中每個(gè)像素點(diǎn)的像素值作為均值、方差和權(quán)值任取，建立一個(gè)高斯模型。
ii)模型學(xué)習(xí)； 視覺(jué)傳感器采集到新圖像，將新圖像中的每個(gè)像素與該像素的已有的k個(gè)高斯模型相比較，其中k≤K； 若滿足|zuv-μj，uv|＜2.5∑j，uv，則利用EM算法估計(jì)第j個(gè)高斯參數(shù)和權(quán)重； 若不滿足，且k＜K，則增加一個(gè)高斯模型，若k＝K，則用新的高斯分布代替優(yōu)先級(jí)最低的高斯分布，新的高斯分布取zuv的值為均值、并賦予新的方差和權(quán)值； 視覺(jué)傳感器繼續(xù)采集圖像，不斷地訓(xùn)練初始化模型，最終得到K個(gè)|zuv-μj，uv|＜2.5∑j，uv的高斯分布函數(shù)時(shí)，停止訓(xùn)練過(guò)程，從而得到高斯混合分布的背景圖像模型。
iii)前景圖像評(píng)估； 由于有噪聲的影響或運(yùn)動(dòng)目標(biāo)的存在，背景圖像模型中些像素點(diǎn)并不能真正地代表背景，用這類像素建立的高斯分布模型應(yīng)該被去除。假設(shè)運(yùn)動(dòng)目標(biāo)及噪聲在場(chǎng)景區(qū)域中不會(huì)長(zhǎng)時(shí)間停留在某一位置上，這樣，噪聲和運(yùn)動(dòng)目標(biāo)相對(duì)應(yīng)的高斯模型的權(quán)值和優(yōu)先級(jí)非常小，將K個(gè)高斯分布按照優(yōu)先級(jí)高低次序排列，前B個(gè)分布作為背景模型，B的定義如下 M是預(yù)先定義的閾值，它表示能真正反映背景的數(shù)據(jù)占總數(shù)據(jù)的最小比重，如果M＝1，就是個(gè)單高斯分布的背景模型；M＞1，就是高斯混合分布的背景模型；其中b＜K。
對(duì)每一幅采集到的新圖像，將圖像的每一個(gè)像素點(diǎn)與該像素點(diǎn)對(duì)應(yīng)的高斯混合模型的各個(gè)高斯模型進(jìn)行比較，若有|zuv-μj，uv|＜2.5∑j，uv，則該點(diǎn)屬于背景，否則屬于前景。所有屬于前景的點(diǎn)組成了圖像中運(yùn)動(dòng)目標(biāo)，從而獲得了移動(dòng)機(jī)器人在圖像中的位置。
②離線的標(biāo)定建立圖像位置與實(shí)際空間的映射關(guān)系，從而測(cè)量獲得移動(dòng)機(jī)器人當(dāng)前位姿； 根據(jù)步驟①獲得移動(dòng)機(jī)器人在圖像中的位置，利用全局?jǐn)z像機(jī)的離線標(biāo)定所獲得的圖像坐標(biāo)系到實(shí)際空間坐標(biāo)系的映射關(guān)系，計(jì)算得到移動(dòng)機(jī)器人在實(shí)際空間中的位姿信息。
其中映射關(guān)系的標(biāo)定過(guò)程如下 將畸變的因素考慮到從圖像平面到實(shí)際空間平面的映射關(guān)系中去，建立三次多項(xiàng)式，如式(3)所示，獲得校正后的空間坐標(biāo)位置 式中(Xi，Yi)為機(jī)器人在空間中的實(shí)際位置，(xi，yi)為機(jī)器人在圖像中的坐標(biāo)位置，在地面上選取48個(gè)控制點(diǎn)，利用最小二乘法對(duì)式(3)進(jìn)行標(biāo)定，可以獲得多項(xiàng)式系數(shù)ai，bi，i＝1，2...9。得到了該映射關(guān)系后，每次檢測(cè)到移動(dòng)機(jī)器人在圖像中的位置后，都可以將它映射到實(shí)際空間中，得到其在實(shí)際空間中的位置，從而獲得視覺(jué)傳感器測(cè)量的位姿。
(3)構(gòu)造卡爾曼濾波器，獲得里程計(jì)濾波估計(jì)值和視覺(jué)傳感器濾波估計(jì)值； 結(jié)合機(jī)器人運(yùn)動(dòng)學(xué)方程，為兩個(gè)測(cè)量通道構(gòu)造卡爾曼濾波器，將視覺(jué)傳感器測(cè)量通道和里程計(jì)測(cè)量通道所得到的測(cè)量值，通過(guò)卡爾曼濾波得到里程計(jì)濾波估計(jì)值和視覺(jué)傳感器濾波估計(jì)值。
根據(jù)移動(dòng)機(jī)器人運(yùn)動(dòng)模型以及各個(gè)噪聲統(tǒng)計(jì)信息構(gòu)造擴(kuò)展卡爾曼濾波器，結(jié)合移動(dòng)機(jī)器人的非完整運(yùn)動(dòng)學(xué)方程，卡爾曼濾波器的濾波方程組如式(4)～式(8) Pk+1/k＝ΦPk/kΦT+Q (7) Pk+1＝[I-Kk+1Hk+1]Pk+1/k(8) 其中Q為導(dǎo)航系統(tǒng)噪聲的方差矩陣，R為測(cè)量噪聲的方差矩陣，兩者均為零均值時(shí)的高斯噪聲，P為估計(jì)誤差的協(xié)方差矩陣，Pk/k為k時(shí)刻P的估計(jì)值，Pk+1/k為k+1時(shí)刻P的預(yù)測(cè)值。

為系統(tǒng)狀態(tài)，

為k時(shí)刻的狀態(tài)估計(jì)值，

為k+1時(shí)刻的狀態(tài)預(yù)測(cè)值，K為卡爾曼濾波增益，Kk為k時(shí)刻的卡爾曼濾波增益，Kk+1為k+1時(shí)刻的卡爾曼濾波增益，Φ為線性化的狀態(tài)轉(zhuǎn)移矩陣，通過(guò)式(5)～式(9)得到的濾波估計(jì)值為

其工作結(jié)構(gòu)示意圖如圖2所示，在

的基礎(chǔ)上，利用式(4)獲得

同時(shí)根據(jù)Pk，利用式(7)獲得Pk+1/k，利用式(6)獲得Kk+1，利用Kk+1和

根據(jù)式(5)就可以獲得

同時(shí)利用Kk+1和Pk+1/k，根據(jù)式(8)就可以獲得Pk+1，

為第k+1步的濾波估計(jì)值，即獲得里程計(jì)濾波估計(jì)值和視覺(jué)傳感器濾波估計(jì)值。
步驟三里程計(jì)濾波估計(jì)值和視覺(jué)傳感器濾波估計(jì)值融合； 由于視覺(jué)傳感器的處理速度遠(yuǎn)遠(yuǎn)小于里程計(jì)的處理速度，可能出現(xiàn)里程計(jì)已獲得測(cè)量數(shù)據(jù)，其濾波器計(jì)算獲得了濾波估計(jì)值，而視覺(jué)傳感器還未完成其圖像處理的過(guò)程，沒(méi)有獲得測(cè)量數(shù)據(jù)，也沒(méi)有獲得濾波估計(jì)值，因此濾波估計(jì)值的融合有以下兩種情況 (1)在未獲得視覺(jué)傳感器濾波器濾波估計(jì)值時(shí)； 利用里程計(jì)濾波器濾波估計(jì)值與移動(dòng)機(jī)器人模型公共參考系統(tǒng)進(jìn)行信息的融合，獲得最優(yōu)濾波值
(2)當(dāng)獲得視覺(jué)傳感器濾波器濾波估計(jì)值后； 利用里程計(jì)濾波器濾波估計(jì)值，視覺(jué)傳感器濾波器濾波估計(jì)值與移動(dòng)機(jī)器人模型公共參考系統(tǒng)進(jìn)行信息的融合，獲得最優(yōu)濾波值
按照最優(yōu)融合公式(9)，將各濾波器的濾波估計(jì)值進(jìn)行融合 式中

表示最優(yōu)濾波值，Pg為融合后的估計(jì)誤差方差矩陣，Pii為第i個(gè)濾波器的估計(jì)誤差方差矩陣，

為第i個(gè)濾波器的濾波估計(jì)值，N為自然數(shù)，表示包括濾波器的個(gè)數(shù)； 步驟四參數(shù)重置； 判斷導(dǎo)航是否結(jié)束，若未結(jié)束導(dǎo)航，則將獲得的最優(yōu)濾波估計(jì)值，重置到里程計(jì)濾波器和視覺(jué)傳感器濾波器，代替各濾波器的濾波估計(jì)值，返回步驟二，進(jìn)行下一次的濾波； 同時(shí)根據(jù)獲得的最優(yōu)位姿估計(jì)值，利用移動(dòng)機(jī)器人運(yùn)動(dòng)模型，預(yù)測(cè)下一步移動(dòng)機(jī)器人的位姿信息，從而確定在下一幀圖像中移動(dòng)機(jī)器人目標(biāo)所在的大概位置，以該位置作為下一次運(yùn)動(dòng)目標(biāo)檢測(cè)時(shí)的搜索啟發(fā)點(diǎn)，加快整個(gè)目標(biāo)檢測(cè)的速度。
若導(dǎo)航結(jié)束，則本方法結(jié)束。
權(quán)利要求
1、一種基于視覺(jué)信息校正的室內(nèi)移動(dòng)機(jī)器人實(shí)時(shí)導(dǎo)航方法，其特征在于，包含以下步驟
步驟一初始化機(jī)器人導(dǎo)航系統(tǒng)，啟動(dòng)里程計(jì)和視覺(jué)傳感器；
初始化包括確定移動(dòng)機(jī)器人的初始位置、初始化導(dǎo)航系統(tǒng)中所有濾波器的參數(shù)；
步驟二里程計(jì)和視覺(jué)傳感器得到機(jī)器人當(dāng)前位姿，構(gòu)造卡爾曼濾波器，獲得里程計(jì)濾波估計(jì)值和視覺(jué)傳感器濾波估計(jì)值；
(1)里程計(jì)測(cè)量通道；
利用里程計(jì)獲得移動(dòng)機(jī)器人左、右輪的移動(dòng)距離和移動(dòng)速度，采用航跡遞推的方法獲得當(dāng)前機(jī)器人位姿；
(2)視覺(jué)傳感器測(cè)量通道；
通過(guò)全局運(yùn)動(dòng)目標(biāo)檢測(cè)算法檢測(cè)移動(dòng)機(jī)器人在圖像中的位置，并通過(guò)離線的標(biāo)定建立圖像位置與實(shí)際空間的映射關(guān)系，獲得移動(dòng)機(jī)器人當(dāng)前位姿；
(3)構(gòu)造卡爾曼濾波器，獲得里程計(jì)濾波估計(jì)值和視覺(jué)傳感器濾波估計(jì)值；
結(jié)合機(jī)器人運(yùn)動(dòng)學(xué)方程，構(gòu)造卡爾曼濾波器，將視覺(jué)傳感器測(cè)量通道和里程計(jì)測(cè)量通道所得到的測(cè)量值，通過(guò)卡爾曼濾波得到里程計(jì)濾波估計(jì)值和視覺(jué)傳感器濾波估計(jì)值；
步驟三里程計(jì)濾波估計(jì)值和視覺(jué)傳感器濾波估計(jì)值融合；
(1)在未獲得視覺(jué)傳感器濾波器濾波估計(jì)值時(shí)；
利用里程計(jì)濾波器濾波估計(jì)值與移動(dòng)機(jī)器人模型公共參考系統(tǒng)進(jìn)行信息融合，獲得最優(yōu)濾波值
(2)當(dāng)獲得視覺(jué)傳感器濾波器濾波估計(jì)值后；
利用里程計(jì)濾波器濾波估計(jì)值，視覺(jué)傳感器濾波器濾波估計(jì)值與移動(dòng)機(jī)器人模型公共參考系統(tǒng)進(jìn)行信息融合，獲得最優(yōu)濾波值
按照最優(yōu)融合公式(1)，將各濾波器的濾波估計(jì)值進(jìn)行融合
式中
表示最優(yōu)濾波值，Pg為融合后的估計(jì)誤差方差矩陣，Pii為第i個(gè)濾波器的估計(jì)誤差方差矩陣，
為第i個(gè)濾波器的濾波估計(jì)值，N為自然數(shù)，表示包括濾波器的個(gè)數(shù)；
步驟四參數(shù)重置；
判斷導(dǎo)航是否結(jié)束，若導(dǎo)航未結(jié)束，則將獲得的最優(yōu)濾波估計(jì)值
或者
重置到里程計(jì)濾波器和視覺(jué)傳感器濾波器，返回步驟二，進(jìn)行下一次的濾波；
同時(shí)根據(jù)獲得的最優(yōu)濾波估計(jì)值
或者
利用移動(dòng)機(jī)器人運(yùn)動(dòng)模型，預(yù)測(cè)下一步移動(dòng)機(jī)器人的位姿信息，從而確定在下一幀圖像中移動(dòng)機(jī)器人目標(biāo)所在的位置，以該位置作為下一次運(yùn)動(dòng)目標(biāo)檢測(cè)時(shí)的搜索啟發(fā)點(diǎn)；
若導(dǎo)航結(jié)束，則本方法結(jié)束。
2、根據(jù)權(quán)利要求1所述的一種基于視覺(jué)信息校正的室內(nèi)移動(dòng)機(jī)器人實(shí)時(shí)導(dǎo)航方法，其特征在于，步驟二(2)中視覺(jué)傳感器測(cè)量通道的具體步驟如下
①采用基于混合高斯背景建模的運(yùn)動(dòng)目標(biāo)檢測(cè)方法獲得移動(dòng)機(jī)器人在圖像中的位置；
用K個(gè)不同均值與方差的高斯分布描述非完全靜止條件下的背景圖像；設(shè)像素點(diǎn)(u，v)的噪聲強(qiáng)度z(u，v)服從混合高斯分布，則該點(diǎn)的概率密度函數(shù)為
其中η(zuv|μj，uv，∑j，uv)表示采集到的圖像中(u，v)點(diǎn)的第j個(gè)高斯分布，其均值為μj，uv，方差為∑j，uv，該點(diǎn)當(dāng)前灰度值為zuv，wj，uv為第j個(gè)高斯分布的加權(quán)權(quán)重，P(zuv)為圖像中(u，v)點(diǎn)的概率分布函數(shù)；
基于混合高斯背景建模的過(guò)程包括以下步驟
i)初始化模型；
視覺(jué)傳感器采集的第一幅圖像中每個(gè)像素點(diǎn)的像素值作為均值，方差和權(quán)值任取，建立一個(gè)高斯模型；
ii)模型學(xué)習(xí)；
視覺(jué)傳感器采集到新圖像，將新圖像中的每個(gè)像素與該像素的已有的k個(gè)高斯模型相比較，其中k≤K；
若滿足|zuv-μj，uv|＜2.5∑j，uv，采用EM算法估計(jì)第j個(gè)高斯參數(shù)和權(quán)重；
若不滿足，且k＜K，則增加一個(gè)高斯模型，若k＝K，則用新的高斯分布代替優(yōu)先級(jí)最低的高斯分布，新的高斯分布取zuv的值為均值、并賦予新的方差和權(quán)值；
視覺(jué)傳感器繼續(xù)采集圖像，不斷地訓(xùn)練初始化模型，最終得到K個(gè)|zuv-μj，uv|＜2.5∑j，uv的高斯分布函數(shù)時(shí)，停止訓(xùn)練過(guò)程，得到高斯混合分布的背景圖像模型；
iii)前景圖像評(píng)估；
去除背景模型中用噪聲或運(yùn)動(dòng)目標(biāo)建立的高斯分布模型；將K個(gè)高斯分布模型按照優(yōu)先級(jí)高低次序排列，前B個(gè)分布作為背景模型，B的定義如下
M是預(yù)先定義的閾值，其中b＜K；
對(duì)每一幅采集到的新圖像，將圖像的每一個(gè)像素點(diǎn)與該像素點(diǎn)對(duì)應(yīng)的高斯混合模型的各個(gè)高斯模型進(jìn)行比較，若有|zuv-μj，uv|＜2.5∑j，uv，則該點(diǎn)屬于背景，否則屬于前景；所有屬于前景的點(diǎn)組成了圖像中運(yùn)動(dòng)目標(biāo)，從而獲得了移動(dòng)機(jī)器人在圖像中的位置；
②離線的標(biāo)定建立圖像位置與實(shí)際空間的映射關(guān)系，從而測(cè)量獲得移動(dòng)機(jī)器人當(dāng)前位姿；
根據(jù)步驟①獲得移動(dòng)機(jī)器人在圖像中的位置，利用全局?jǐn)z像機(jī)的離線標(biāo)定所獲得的圖像坐標(biāo)系到實(shí)際空間坐標(biāo)系的映射關(guān)系，計(jì)算得到移動(dòng)機(jī)器人在實(shí)際空間中的位姿信息；
其中映射關(guān)系的如下
式中(Xi，Yi)為機(jī)器人在空間中的實(shí)際位置，(xi，yi)為機(jī)器人在圖像中的坐標(biāo)位置，在地面上選取48個(gè)控制點(diǎn)，利用最小二乘法對(duì)式(4)進(jìn)行標(biāo)定，獲得多項(xiàng)式系數(shù)ai，bi，i＝1，2...9；得到了該映射關(guān)系后，每次檢測(cè)到移動(dòng)機(jī)器人在圖像中的位置后，都將它映射到實(shí)際空間中，得到其在實(shí)際空間中的位置，從而獲得視覺(jué)傳感器測(cè)量的位姿。
3、根據(jù)權(quán)利要求1所述的一種基于視覺(jué)信息校正的室內(nèi)移動(dòng)機(jī)器人實(shí)時(shí)導(dǎo)航方法，其特征在于，步驟二(3)中構(gòu)造卡爾曼濾波器，獲得里程計(jì)濾波估計(jì)值和視覺(jué)傳感器濾波估計(jì)值的具體步驟如下
根據(jù)移動(dòng)機(jī)器人運(yùn)動(dòng)模型以及各個(gè)噪聲統(tǒng)計(jì)信息構(gòu)造擴(kuò)展卡爾曼濾波器，結(jié)合移動(dòng)機(jī)器人的非完整運(yùn)動(dòng)學(xué)方程，卡爾曼濾波器的濾波方程組如式(5)～式(9)
Pk+1/k＝ΦPk/kΦT+Q (8)
Pk+1＝[I-Kk+1Hk+1]Pk+1/k(9)
其中Q為導(dǎo)航系統(tǒng)噪聲的方差矩陣，R為測(cè)量噪聲的方差矩陣，兩者均為零均值時(shí)的高斯噪聲，P為估計(jì)誤差的協(xié)方差矩陣，Pk/k為k時(shí)刻P的估計(jì)值，Pk+1/k為k+1時(shí)刻P的預(yù)測(cè)值；
為系統(tǒng)狀態(tài)，
為k時(shí)刻的狀態(tài)估計(jì)值，
為k+1時(shí)刻的狀態(tài)預(yù)測(cè)值，K為卡爾曼濾波增益，Kk為k時(shí)刻的卡爾曼濾波增益，Kk+1為k+1時(shí)刻的卡爾曼濾波增益，Φ為線性化的狀態(tài)轉(zhuǎn)移矩陣；
通過(guò)式(5)～式(9)得到的濾波估計(jì)值為
全文摘要
本發(fā)明公開(kāi)了一種基于視覺(jué)信息校正的室內(nèi)移動(dòng)機(jī)器人實(shí)時(shí)導(dǎo)航方法，包括步驟一初始化機(jī)器人導(dǎo)航系統(tǒng)，啟動(dòng)里程計(jì)和視覺(jué)傳感器；步驟二里程計(jì)和視覺(jué)傳感器得到機(jī)器人當(dāng)前位資，構(gòu)造卡爾曼濾波器，獲得里程計(jì)濾波估計(jì)值和視覺(jué)傳感器濾波估計(jì)值；步驟三里程計(jì)濾波估計(jì)值和視覺(jué)傳感器濾波估計(jì)值融合；步驟四參數(shù)重置；本發(fā)明充分利用了視覺(jué)信息和里程計(jì)信息的各自優(yōu)點(diǎn)，將視覺(jué)信息的精確性和里程計(jì)信息的實(shí)時(shí)性相結(jié)合起來(lái)，在大部分時(shí)間下，利用里程計(jì)自身信息進(jìn)行遞推計(jì)算，獲得導(dǎo)航數(shù)據(jù)，保證了導(dǎo)航系統(tǒng)的實(shí)時(shí)性要求，同時(shí)利用視覺(jué)信息來(lái)矯正里程計(jì)航跡推算過(guò)程中產(chǎn)生的累積誤差，從而大大提高了導(dǎo)航系統(tǒng)的準(zhǔn)確性。
文檔編號(hào)G01C21/00GK101576384SQ20091008723
公開(kāi)日2009年11月11日 申請(qǐng)日期2009年6月18日 優(yōu)先權(quán)日2009年6月18日
發(fā)明者秦世引, 凡 謝 申請(qǐng)人:北京航空航天大學(xué)