本發(fā)明涉及一種導(dǎo)航方法，具體是一種基于多臺(tái)3d結(jié)構(gòu)光相機(jī)拼接的室內(nèi)導(dǎo)航方法。

背景技術(shù)：

實(shí)時(shí)定位及導(dǎo)航技術(shù)作為機(jī)器人智能化的第一步正不斷引起行業(yè)內(nèi)的重視，實(shí)時(shí)定位及導(dǎo)航是指機(jī)器人在自身位置不確定的條件下，在完全未知的環(huán)境中創(chuàng)建地圖，同時(shí)利用地圖進(jìn)行自主定位和導(dǎo)航的一種技術(shù)，在此過程中，機(jī)器人一般通過位姿估計(jì)及測距單元來確定自己的位置信息。常見的測距單元包括激光測距、超聲波傳感器以及圖像測距等。

常見的測距單元包括激光測距、超聲波傳感器以及圖像測距等。激光測距是利用激光對目標(biāo)的距離進(jìn)行準(zhǔn)確測定；超聲波傳感器采用超聲波回波測距原理，運(yùn)用精確的時(shí)差測量技術(shù)，檢測傳感器與目標(biāo)物之間的距離。

激光測距精度較低且成本高；超聲波傳感器測距的適用頻率范圍和靈敏度都過小。

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

本發(fā)明的目的在于提供一種基于多臺(tái)3d結(jié)構(gòu)光相機(jī)拼接的室內(nèi)導(dǎo)航方法，以解決上述背景技術(shù)中提出的問題。

為實(shí)現(xiàn)上述目的，本發(fā)明提供如下技術(shù)方案：

一種基于多臺(tái)3d結(jié)構(gòu)光相機(jī)拼接的室內(nèi)導(dǎo)航方法，包括如下步驟：（1）采用柵格地圖的方式來表達(dá)地圖，初始地圖設(shè)置為空，設(shè)機(jī)器人的初始位姿為全局坐標(biāo)系的原點(diǎn)，構(gòu)建地圖過程中，里程計(jì)數(shù)據(jù)通過對機(jī)器人運(yùn)動(dòng)模型計(jì)算機(jī)器人位姿預(yù)測量，同時(shí)采用特征提取技術(shù)從不斷更新傳感器數(shù)據(jù)中提取觀測量，通過設(shè)計(jì)的觀測模型，觀測預(yù)測量由機(jī)器人位姿預(yù)測量和可視全局地圖計(jì)算得到，在獲得觀測量和觀測預(yù)測量后，利用特征關(guān)聯(lián)算法來實(shí)現(xiàn)特征匹配，匹配特征之間的信息，通過粒子濾波更新公式實(shí)現(xiàn)位姿更新和特征更新，未匹配的新特征通過特征增加技術(shù)加入全局地圖，實(shí)現(xiàn)增量地圖的構(gòu)建；（2）使用一組離散的帶權(quán)粒子模擬機(jī)器人位置的后驗(yàn)分布，并重復(fù)執(zhí)行狀態(tài)預(yù)測、更新權(quán)重和重采樣等步驟完成濾波，實(shí)現(xiàn)機(jī)器人的定位；（3）采用路徑規(guī)劃單元對路徑進(jìn)行最優(yōu)設(shè)計(jì)，路徑規(guī)劃單元包括全局路徑規(guī)劃模塊和局部路徑規(guī)劃模塊，全局路徑規(guī)劃單元基于靜態(tài)地圖采用a*算法搜索一條從起始狀態(tài)到目標(biāo)狀態(tài)的最優(yōu)路徑或近似最優(yōu)路徑；局部路徑規(guī)劃單元，基于局部障礙物地圖采用動(dòng)態(tài)窗口法尋找一段時(shí)間內(nèi)的最優(yōu)軌跡，選取最優(yōu)軌跡所對應(yīng)的速度來驅(qū)動(dòng)機(jī)器人運(yùn)動(dòng)；（4）通過多臺(tái)3d結(jié)構(gòu)光相機(jī)按特定位置的組合拼接，對機(jī)器人前方大于90度及小于6米范圍的障礙物探測，實(shí)現(xiàn)模擬激光雷達(dá)的操作，獲取探測范圍內(nèi)物體的坐標(biāo)，為地圖構(gòu)建提供坐標(biāo)數(shù)據(jù)，并實(shí)現(xiàn)運(yùn)動(dòng)控制同時(shí)進(jìn)行避障操作。

作為本發(fā)明再進(jìn)一步的方案：所述3d結(jié)構(gòu)光相機(jī)大于2。

與現(xiàn)有技術(shù)相比，本發(fā)明的有益效果是：本發(fā)明采用多臺(tái)3d結(jié)構(gòu)光相機(jī)拼接模擬激光雷達(dá)方法，以及基于概率學(xué)的多種濾波算法進(jìn)行位姿估計(jì)，實(shí)現(xiàn)了機(jī)器人定位與導(dǎo)航。通過3d結(jié)構(gòu)光相機(jī)采集數(shù)據(jù)不容易受到環(huán)境的影響，且數(shù)據(jù)誤差小，并具有低成本的優(yōu)勢，能夠填補(bǔ)激光雷達(dá)的缺點(diǎn)。

附圖說明

圖1為基于多臺(tái)3d結(jié)構(gòu)光相機(jī)拼接的室內(nèi)導(dǎo)航方法中多臺(tái)相機(jī)拼接方式。

圖2為基于多臺(tái)3d結(jié)構(gòu)光相機(jī)拼接的室內(nèi)導(dǎo)航方法中相機(jī)平行面簡單模型。

圖3為基于多臺(tái)3d結(jié)構(gòu)光相機(jī)拼接的室內(nèi)導(dǎo)航方法中相機(jī)垂直面簡單模型。

圖4為基于多臺(tái)3d結(jié)構(gòu)光相機(jī)拼接的室內(nèi)導(dǎo)航方法的流程圖。

具體實(shí)施方式

下面將結(jié)合本發(fā)明實(shí)施例中的附圖，對本發(fā)明實(shí)施例中的技術(shù)方案進(jìn)行清楚、完整地描述，顯然，所描述的實(shí)施例僅僅是本發(fā)明一部分實(shí)施例，而不是全部的實(shí)施例?；诒景l(fā)明中的實(shí)施例，本領(lǐng)域普通技術(shù)人員在沒有做出創(chuàng)造性勞動(dòng)前提下所獲得的所有其他實(shí)施例，都屬于本發(fā)明保護(hù)的范圍。

請參閱圖1～4，本發(fā)明實(shí)施例中，一種基于多臺(tái)3d結(jié)構(gòu)光相機(jī)拼接的室內(nèi)導(dǎo)航方法，包括如下步驟：（1）采用柵格地圖的方式來表達(dá)地圖，初始地圖設(shè)置為空，設(shè)機(jī)器人的初始位姿為全局坐標(biāo)系的原點(diǎn)，構(gòu)建地圖過程中，里程計(jì)數(shù)據(jù)通過對機(jī)器人運(yùn)動(dòng)模型計(jì)算機(jī)器人位姿預(yù)測量，同時(shí)采用特征提取技術(shù)從不斷更新傳感器數(shù)據(jù)中提取觀測量，通過設(shè)計(jì)的觀測模型，觀測預(yù)測量由機(jī)器人位姿預(yù)測量和可視全局地圖計(jì)算得到，在獲得觀測量和觀測預(yù)測量后，利用特征關(guān)聯(lián)算法來實(shí)現(xiàn)特征匹配，匹配特征之間的信息，通過粒子濾波更新公式實(shí)現(xiàn)位姿更新和特征更新，未匹配的新特征通過特征增加技術(shù)加入全局地圖，實(shí)現(xiàn)增量地圖的構(gòu)建；（2）使用一組離散的帶權(quán)粒子模擬機(jī)器人位置的后驗(yàn)分布，并重復(fù)執(zhí)行狀態(tài)預(yù)測、更新權(quán)重和重采樣等步驟完成濾波，實(shí)現(xiàn)機(jī)器人的定位；（3）采用路徑規(guī)劃單元對路徑進(jìn)行最優(yōu)設(shè)計(jì)，路徑規(guī)劃單元包括全局路徑規(guī)劃模塊和局部路徑規(guī)劃模塊，全局路徑規(guī)劃單元基于靜態(tài)地圖采用a*算法搜索一條從起始狀態(tài)到目標(biāo)狀態(tài)的最優(yōu)路徑或近似最優(yōu)路徑；局部路徑規(guī)劃單元，基于局部障礙物地圖采用動(dòng)態(tài)窗口法尋找一段時(shí)間內(nèi)的最優(yōu)軌跡，選取最優(yōu)軌跡所對應(yīng)的速度來驅(qū)動(dòng)機(jī)器人運(yùn)動(dòng)；（4）通過多臺(tái)3d結(jié)構(gòu)光相機(jī)按特定位置的組合拼接，對機(jī)器人前方大于90度及小于6米范圍的障礙物探測，實(shí)現(xiàn)模擬激光雷達(dá)的操作，獲取探測范圍內(nèi)物體的坐標(biāo)，為地圖構(gòu)建提供坐標(biāo)數(shù)據(jù)，并實(shí)現(xiàn)運(yùn)動(dòng)控制同時(shí)進(jìn)行避障操作。

所述3d結(jié)構(gòu)光相機(jī)大于2。

通過多臺(tái)3d結(jié)構(gòu)光相機(jī)按特定位置的組合拼接，可以對機(jī)器人前方大于90度及小于6米范圍的障礙物探測，實(shí)現(xiàn)模擬激光雷達(dá)的操作。如圖1所示為多臺(tái)3d結(jié)構(gòu)光相機(jī)拼接方式，四臺(tái)3d結(jié)構(gòu)光相機(jī)按照一定的角度固定在機(jī)械固定裝置上，通過棋盤格標(biāo)定方法獲得各個(gè)相機(jī)基于中心點(diǎn)的旋轉(zhuǎn)分量r和平移分量t，利用r和t將多臺(tái)相機(jī)的深度數(shù)據(jù)映射到同一坐標(biāo)系中拼接，去除兩臺(tái)相機(jī)重疊部分，最終得到一個(gè)探測范圍更大激光雷達(dá)數(shù)據(jù)。如圖1所示，單臺(tái)深度數(shù)據(jù)水平探測范圍為58.4°,去除其余相鄰相機(jī)重疊部分，其真正的水平探測范圍為45°。

所述3d結(jié)構(gòu)光相機(jī)水平方向測距方式，其水平方向視角范圍為到(不同的3d結(jié)構(gòu)光相機(jī)是不一樣，這里的3d結(jié)構(gòu)光相機(jī)為29.2°)。對3d結(jié)構(gòu)光相機(jī)的深度圖像用簡單模型描述，如圖2所示，模型平面為與地面平行的平面，坐標(biāo)原點(diǎn)為3d結(jié)構(gòu)光相機(jī)中心，其對應(yīng)像素平面的寬度，相機(jī)獲取到的障礙物離成像平面的深度值，通過轉(zhuǎn)換后得到障礙物離光心距離的激光量程值。轉(zhuǎn)換方式如下：對于深度圖像中第行第列的某一點(diǎn)深度值，3d結(jié)構(gòu)光相機(jī)水平方向掃描范圍，其角度步長的計(jì)算公式為，因此該像素點(diǎn)對應(yīng)相機(jī)坐標(biāo)系x軸方向的夾角，故轉(zhuǎn)換成激光點(diǎn)數(shù)據(jù)就可以根據(jù)公式得到。

所述3d結(jié)構(gòu)光相機(jī)垂直方向測距，對3d結(jié)構(gòu)光相機(jī)的深度圖像用簡單模型描述，如圖3所示，模型平面為與地面垂直的平面，坐標(biāo)原點(diǎn)為3d結(jié)構(gòu)光相機(jī)中心，其垂直方向視角范圍為到(不同的3d結(jié)構(gòu)光相機(jī)是不一樣，這里的3d結(jié)構(gòu)光相機(jī)為22.5°)。對應(yīng)像素平面的高度，可以根據(jù)應(yīng)用對垂直方向掃描的高度不同設(shè)置不同的垂直方向掃描范圍，例如設(shè)置為-22.5°到5°。

如上所述掃描3d結(jié)構(gòu)光相機(jī)一行所有像素進(jìn)行水平方向測距計(jì)算，即可建立起當(dāng)前局部環(huán)境，模擬實(shí)現(xiàn)二位激光雷達(dá)功能。也可以根據(jù)實(shí)際需要，取3d結(jié)構(gòu)光相機(jī)若干行并對其求最小深度值作為激光點(diǎn)數(shù)據(jù)的深度值進(jìn)行計(jì)算，即可達(dá)到近似三維雷達(dá)效果的二維激光雷達(dá)數(shù)據(jù)。

基于多臺(tái)3d結(jié)構(gòu)光相機(jī)拼接的室內(nèi)導(dǎo)航方法流程如下：

1.啟動(dòng)機(jī)器人定位導(dǎo)航功能。

2.系統(tǒng)建立初始地圖，并設(shè)機(jī)器人當(dāng)前位置為全局坐標(biāo)系的原點(diǎn)。

3.多臺(tái)3d結(jié)構(gòu)光相機(jī)按特定位置的組合拼接，獲取機(jī)器人前方大于90度及小于6米范圍的物體的坐標(biāo)，為地圖構(gòu)建提供坐標(biāo)數(shù)據(jù)。

4.基于靜態(tài)地圖采用a*算法，計(jì)算出一條從起始狀態(tài)到目標(biāo)狀態(tài)的最優(yōu)路徑或近似最優(yōu)路徑。

5.系統(tǒng)通過獲取里程計(jì)信息對機(jī)器人進(jìn)行位姿估計(jì)，并提取周圍環(huán)境特征信息與先驗(yàn)地圖進(jìn)行特征匹配，采用mlc定位方法對機(jī)器人進(jìn)行實(shí)時(shí)定位。

6.在局部運(yùn)動(dòng)中，利用多臺(tái)3d結(jié)構(gòu)光相機(jī)拼接模擬激光雷達(dá)方法對機(jī)器人探測范圍的障礙物進(jìn)行識(shí)別。系統(tǒng)進(jìn)行局部路徑規(guī)劃，采用動(dòng)態(tài)窗口法尋找一段時(shí)間內(nèi)的最優(yōu)軌跡，選取最優(yōu)軌跡所對應(yīng)的速度來實(shí)現(xiàn)機(jī)器人運(yùn)動(dòng)控制同時(shí)進(jìn)行避障操作。

對于本領(lǐng)域技術(shù)人員而言，顯然本發(fā)明不限于上述示范性實(shí)施例的細(xì)節(jié)，而且在不背離本發(fā)明的精神或基本特征的情況下，能夠以其他的具體形式實(shí)現(xiàn)本發(fā)明。因此，無論從哪一點(diǎn)來看，均應(yīng)將實(shí)施例看作是示范性的，而且是非限制性的，本發(fā)明的范圍由所附權(quán)利要求而不是上述說明限定，因此旨在將落在權(quán)利要求的等同要件的含義和范圍內(nèi)的所有變化囊括在本發(fā)明內(nèi)。不應(yīng)將權(quán)利要求中的任何附圖標(biāo)記視為限制所涉及的權(quán)利要求。

此外，應(yīng)當(dāng)理解，雖然本說明書按照實(shí)施方式加以描述，但并非每個(gè)實(shí)施方式僅包含一個(gè)獨(dú)立的技術(shù)方案，說明書的這種敘述方式僅僅是為清楚起見，本領(lǐng)域技術(shù)人員應(yīng)當(dāng)將說明書作為一個(gè)整體，各實(shí)施例中的技術(shù)方案也可以經(jīng)適當(dāng)組合，形成本領(lǐng)域技術(shù)人員可以理解的其他實(shí)施方式。