本發(fā)明涉及地面移動(dòng)機(jī)器人在室內(nèi)環(huán)境中的導(dǎo)航控制領(lǐng)域，更具體地涉及一種機(jī)器人室內(nèi)定位方法。

背景技術(shù)：

現(xiàn)有智能移動(dòng)機(jī)器人是一類能夠通過(guò)傳感器感知環(huán)境和自身狀態(tài)，實(shí)現(xiàn)在有障礙物的環(huán)境中面向目標(biāo)的自主導(dǎo)航運(yùn)動(dòng)，從而完成預(yù)定任務(wù)的機(jī)器人系統(tǒng)。要實(shí)現(xiàn)機(jī)器人自主導(dǎo)航運(yùn)動(dòng)，必須要解決環(huán)境建模、實(shí)時(shí)定位、路徑規(guī)劃、運(yùn)動(dòng)控制等一系列問題；其中，移動(dòng)機(jī)器人必須具有定位的能力，其目的就是確定機(jī)器人在運(yùn)行環(huán)境中相對(duì)于世界坐標(biāo)系的位置及航向。

目前存在多種定位技術(shù)，例如GPS定位技術(shù)、慣性導(dǎo)航系統(tǒng)定位技術(shù)以及超寬帶(UWB，Ultra Wide Band)技術(shù)等。然而，在室內(nèi)環(huán)境下，機(jī)器人無(wú)法使用GPS進(jìn)行定位，而使用慣性導(dǎo)航系統(tǒng)成本較高且不能保證長(zhǎng)時(shí)間定位精度。

超寬帶技術(shù)具有對(duì)信道衰落不敏感、發(fā)射信號(hào)功率譜密度低、低截獲能力等特點(diǎn)，能夠提供分米級(jí)定位精度。然而，在室內(nèi)環(huán)境下，由于空間尺寸較小，無(wú)線系統(tǒng)多徑效應(yīng)強(qiáng)，高精度定位部署困難；并且，由于建筑支柱、隔斷墻體、玻璃、家具等遮擋物體的存在，更加惡化了無(wú)線定位的工作環(huán)境。因此，機(jī)器人在室內(nèi)環(huán)境下自主移動(dòng)不能單純依靠超寬帶定位系統(tǒng)，應(yīng)與其他定位方式進(jìn)行融合。

另外，一般室內(nèi)機(jī)器人由兩個(gè)直流伺服電機(jī)驅(qū)動(dòng)左右主動(dòng)輪，通過(guò)左右輪差速實(shí)現(xiàn)轉(zhuǎn)向運(yùn)動(dòng)。當(dāng)以安裝在兩個(gè)伺服電機(jī)軸端的光電編碼器輸出進(jìn)行航位推算時(shí)，由于電機(jī)減速器存在間隙，而且車輪與地面之間不可避免的存在打滑現(xiàn)象，尤其在轉(zhuǎn)彎時(shí)滑動(dòng)更為明顯。因此航位推算的軌跡為理想的輸出軌跡，在轉(zhuǎn)彎時(shí)航向誤差有明顯增大的趨勢(shì)。

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

鑒于以上問題，本發(fā)明提供了一種結(jié)合了超寬帶定位和航位推算的特點(diǎn)的機(jī)器人室內(nèi)定位方法：當(dāng)超寬帶信號(hào)正常時(shí)，采用無(wú)跡卡爾曼濾波(Unscented Kalman Filter，UKF)方法對(duì)超寬帶、編碼器、電子羅盤信息進(jìn)行數(shù)據(jù)融合，在獲得位置、航向的同時(shí)對(duì)航位推算的航向失準(zhǔn)角進(jìn)行估算；當(dāng)超寬帶信號(hào)異常時(shí)，通過(guò)航位推算進(jìn)行位姿解算，由于失準(zhǔn)角可以實(shí)時(shí)補(bǔ)償，因此可以保證航位推算的定位定向精度。

根據(jù)本發(fā)明的一方面，提供了一種機(jī)器人室內(nèi)定位方法，包括：

數(shù)據(jù)存儲(chǔ)步驟，用于存儲(chǔ)上一時(shí)刻的編碼器數(shù)據(jù)以及超寬帶數(shù)據(jù)；

數(shù)據(jù)采集步驟，用于通過(guò)設(shè)置在驅(qū)動(dòng)所述機(jī)器人的左右輪的電機(jī)的軸端的光電編碼器采集當(dāng)前時(shí)刻的編碼器數(shù)據(jù)以及通過(guò)超寬帶定位系統(tǒng)采集當(dāng)前時(shí)刻的超寬帶數(shù)據(jù)；

增量數(shù)據(jù)計(jì)算步驟，用于根據(jù)所存儲(chǔ)的上一時(shí)刻的編碼器數(shù)據(jù)和超寬帶數(shù)據(jù)以及所采集的當(dāng)前時(shí)刻的編碼器數(shù)據(jù)和超寬帶數(shù)據(jù)來(lái)計(jì)算里程增量和位置增量；以及

定位信息評(píng)估步驟，用于根據(jù)所計(jì)算的里程增量和位置增量來(lái)判斷所述編碼器數(shù)據(jù)和所述超寬帶數(shù)據(jù)是否異常，從而確定使用無(wú)跡卡爾曼濾波(UKF)方法、航位推算方法或由所述超寬帶定位系統(tǒng)獲得的超寬帶位姿信息來(lái)獲得所述機(jī)器人在當(dāng)前時(shí)刻的定位信息。

根據(jù)實(shí)施例，所述定位信息評(píng)估步驟包括：如果所述里程增量為零并且所述位置增量不為零，則判斷所述編碼器數(shù)據(jù)正常并且所述超寬帶數(shù)據(jù)異常，從而確定使用所述航位推算方法來(lái)獲得所述機(jī)器人在當(dāng)前時(shí)刻的位置和航向，其中使用所述航位推算方法來(lái)獲得所述機(jī)器人在當(dāng)前時(shí)刻的位置和航向包括：裝訂航向失準(zhǔn)角的步驟，以使得所述航位推算方法使用當(dāng)前時(shí)刻的編碼器數(shù)據(jù)和已裝訂的航向失準(zhǔn)角來(lái)計(jì)算所述機(jī)器人在當(dāng)前時(shí)刻的位置和航向。

根據(jù)實(shí)施例，所述定位信息評(píng)估步驟包括：如果所述位置增量小于預(yù)定值并且所述里程增量的絕對(duì)值大于等于預(yù)定倍數(shù)的所述位置增量，則判斷所述編碼器數(shù)據(jù)異常并且所述超寬帶數(shù)據(jù)正常，從而使用所述超寬帶位姿信息獲得所述機(jī)器人在當(dāng)前時(shí)刻的位置以及由所述電子羅盤確定的當(dāng)前時(shí)刻的航向。

根據(jù)實(shí)施例，所述定位信息評(píng)估步驟包括：如果所述里程增量的絕對(duì)值小于預(yù)定值并且所述位置增量大于等于預(yù)定倍數(shù)的所述里程增量的絕對(duì)值，則判斷所述編碼器數(shù)據(jù)正常并且所述超寬帶數(shù)據(jù)異常，從而確定使用所述航位推算方法來(lái)獲得所述機(jī)器人在當(dāng)前時(shí)刻的位置和航向，其中使用所述航位推算方法來(lái)獲得所述機(jī)器人在當(dāng)前時(shí)刻的位置和航向包括：裝訂航向失準(zhǔn)角的步驟，以使得所述航位推算方法使用當(dāng)前時(shí)刻的編碼器數(shù)據(jù)和已裝訂的航向失準(zhǔn)角來(lái)計(jì)算所述機(jī)器人在當(dāng)前時(shí)刻的位置和航向。

根據(jù)實(shí)施例，所述定位信息評(píng)估步驟包括：如果所述里程增量的絕對(duì)值小于預(yù)定值并且所述位置增量小于預(yù)定倍數(shù)的所述里程增量的絕對(duì)值，則判斷所述編碼器數(shù)據(jù)正常并且所述超寬帶數(shù)據(jù)正常，從而使用所述無(wú)跡卡爾曼濾波(UKF)方法來(lái)獲得所述機(jī)器人在當(dāng)前時(shí)刻的位置和航向，其中使用所述無(wú)跡卡爾曼濾波(UKF)方法來(lái)獲得所述機(jī)器人在當(dāng)前時(shí)刻的位置和航向包括通過(guò)設(shè)置在機(jī)器人上的電子羅盤采集當(dāng)前時(shí)刻的航向數(shù)據(jù)的步驟，以使得所述無(wú)跡卡爾曼濾波(UKF)方法使用當(dāng)前時(shí)刻的編碼器數(shù)據(jù)、超寬帶數(shù)據(jù)以及航向數(shù)據(jù)來(lái)計(jì)算所述機(jī)器人在當(dāng)前時(shí)刻的位置、航向以及航位推算失準(zhǔn)角。

根據(jù)實(shí)施例，在判斷所述編碼器數(shù)據(jù)正常并且所述超寬帶數(shù)據(jù)異常的情況下，如果連續(xù)多次滿足所述里程增量的絕對(duì)值小于預(yù)定值并且所述位置增量小于預(yù)定倍數(shù)的所述里程增量的絕對(duì)值，則判斷所述編碼器數(shù)據(jù)正常并且所述超寬帶數(shù)據(jù)正常，從而使用所述無(wú)跡卡爾曼濾波(UKF)方法來(lái)獲得所述機(jī)器人在當(dāng)前時(shí)刻的位置和航向，其中使用所述無(wú)跡卡爾曼濾波(UKF)方法來(lái)獲得所述機(jī)器人在當(dāng)前時(shí)刻的位置和航向包括通過(guò)設(shè)置在機(jī)器人上的電子羅盤采集當(dāng)前時(shí)刻的航向數(shù)據(jù)的步驟，以使得所述無(wú)跡卡爾曼濾波(UKF)方法使用當(dāng)前時(shí)刻的編碼器數(shù)據(jù)、超寬帶數(shù)據(jù)以及航向數(shù)據(jù)來(lái)計(jì)算所述機(jī)器人在當(dāng)前時(shí)刻的位置、航向以及航位推算失準(zhǔn)角。

根據(jù)實(shí)施例，所述定位信息評(píng)估步驟包括：如果所述里程增量的絕對(duì)值大于等于預(yù)定值并且所述位置增量大于等于所述預(yù)定值，則判斷所述編碼器數(shù)據(jù)異常并且所述超寬帶數(shù)據(jù)異常，從而停止計(jì)算所述機(jī)器人在當(dāng)前時(shí)刻的位置數(shù)據(jù)和航向數(shù)據(jù)并且輸出上一時(shí)刻的位置和航向。

根據(jù)實(shí)施例，所述數(shù)據(jù)存儲(chǔ)步驟包括：存儲(chǔ)所獲得的所述機(jī)器人在當(dāng)前時(shí)刻的位置、航向以及航向失準(zhǔn)角以用于下一時(shí)刻的計(jì)算。

根據(jù)本發(fā)明的實(shí)施例，所述倍數(shù)優(yōu)選為10倍，并且所述預(yù)定值優(yōu)選為15cm。

根據(jù)本發(fā)明的實(shí)施例，所述里程增量ds根據(jù)以下公式計(jì)算：

其中，L＝π·D·η·P為航位推算的比例系數(shù)，其中編碼器精度為P(單位：每脈沖轉(zhuǎn)數(shù)PPR)，驅(qū)動(dòng)電機(jī)的減速比為η，車輪直徑為D(單位：m)，兩車輪輪間距為w(單位：m)；并且N_l和N_r為上一時(shí)刻k-1至當(dāng)前時(shí)刻k左右車輪編碼器輸出的脈沖增量。

根據(jù)本發(fā)明的實(shí)施例，所述位置增量dl根據(jù)以下公式計(jì)算：

其中，(x_k-1，y_k-1)為前一時(shí)刻k-1的超寬帶位置，(x_k，y_k)為當(dāng)前時(shí)刻k的超寬帶位置。

根據(jù)本發(fā)明的實(shí)施例，所述航位推算方法根據(jù)以下公式來(lái)獲得機(jī)器人在當(dāng)前時(shí)刻的位置數(shù)據(jù)和航向數(shù)據(jù)：

其中，(x_dk，y_dk)和為當(dāng)前時(shí)刻k的機(jī)器人的位置和航向，(x_dk-1，y_dk-1)和為上一時(shí)刻k-1的機(jī)器人的位置和航向，并且為航向偏差，是高斯白噪聲。

附圖說(shuō)明

圖1是根據(jù)本發(fā)明實(shí)施例的機(jī)器人室內(nèi)定位方法的總體流程圖。

具體實(shí)施方式

圖1示出根據(jù)本發(fā)明實(shí)施例的機(jī)器人室內(nèi)定位方法的總體流程圖。如圖1所示，在已經(jīng)獲得上一時(shí)刻的編碼器數(shù)據(jù)以及超寬帶數(shù)據(jù)的情況下，通過(guò)設(shè)置在驅(qū)動(dòng)機(jī)器人的左右輪的電機(jī)的軸端的光電編碼器來(lái)采集當(dāng)前時(shí)刻的編碼器數(shù)據(jù)以及通過(guò)超寬帶定位系統(tǒng)采集當(dāng)前時(shí)刻的超寬帶數(shù)據(jù)，采樣間隔例如為100ms?；谏鲜鰯?shù)據(jù)分別計(jì)算采樣間隔內(nèi)的里程增量ds和位置增量dl。根據(jù)ds、dl判斷定位信息可用性，并采取不同的定位策略：

當(dāng)根據(jù)超寬帶數(shù)據(jù)判斷航位推算異常時(shí)，通常是車輪嚴(yán)重打滑或空轉(zhuǎn)，此時(shí)以超寬帶位姿信息為準(zhǔn)；

當(dāng)超寬帶數(shù)據(jù)和編碼器數(shù)據(jù)均在有效值范圍內(nèi)，此時(shí)通過(guò)UKF進(jìn)行組合定位解算，并估算航位推算的失準(zhǔn)角；

當(dāng)根據(jù)編碼器信息判斷超寬帶數(shù)據(jù)異常時(shí)，可能超寬帶信號(hào)受到遮擋，或到達(dá)信號(hào)有效區(qū)域邊界，此時(shí)以航位推算位姿信息為準(zhǔn)(需初始裝訂航位失準(zhǔn)角)。

以下詳細(xì)說(shuō)明ds、dl的計(jì)算、定位信息的評(píng)估、UKF算法以及航位推算方法：

1、ds、dl計(jì)算方法

記兩個(gè)編碼器精度為P(單位：每脈沖轉(zhuǎn)數(shù)PPR)，驅(qū)動(dòng)電機(jī)的減速比為η，車輪直徑為D(單位：m)，兩車輪的輪間距為w(單位：m)。令L＝π·D·η·P，為航位推算的比例系數(shù)。設(shè)k-1至k時(shí)刻左右車輪編碼器輸出的脈沖增量為N_l和N_r，則根據(jù)編碼器信息計(jì)算的里程增量ds的計(jì)算公式如下：

設(shè)k-1時(shí)刻超寬帶位置為(x_k-1，y_k-1)，k時(shí)刻超寬帶位置為(x_k，y_k)，則根據(jù)超寬帶信息計(jì)算的位置增量dl的計(jì)算公式如下：

2、定位信息評(píng)估

根據(jù)ds和dl的定位信息評(píng)估準(zhǔn)則如下：

如果ds＝0，dl≠0，機(jī)器人應(yīng)處于靜止?fàn)顟B(tài)，而超寬帶由于信號(hào)擾動(dòng)存在位置跳變，視為“編碼數(shù)據(jù)正常，超寬帶數(shù)據(jù)異?！保ㄎ恍畔⒁院轿煌扑銥闇?zhǔn)；

如果dl<15cm，|ds|≥10dl，機(jī)器人存在打滑或空轉(zhuǎn)現(xiàn)象，判定為“編碼數(shù)據(jù)異常，超寬帶數(shù)據(jù)正?！?，定位信息以超寬帶定位為準(zhǔn)；

如果|ds|<15cm，dl≥10|ds|，認(rèn)為超寬帶數(shù)據(jù)存在較大跳變，判定“編碼數(shù)據(jù)正常，超寬帶數(shù)據(jù)異?！保ㄎ恍畔⒁院轿煌扑銥闇?zhǔn)；

在“編碼數(shù)據(jù)正常，超寬帶數(shù)據(jù)異?！睜顟B(tài)下，如果連續(xù)10次滿足|ds|<15cm，dl<10|ds|，則認(rèn)為超寬帶恢復(fù)正常，判定“編碼數(shù)據(jù)正常，超寬帶數(shù)據(jù)正?！保ㄎ恍畔⒁訳KF為準(zhǔn)；

如果|ds|≥15cm，dl≥15cm，機(jī)器人運(yùn)行超過(guò)最大速度上限，可能已處于失控狀態(tài)，停止位置解算并且輸出上一時(shí)刻的位置和航向。

3、UKF算法

(1)狀態(tài)方程和觀測(cè)方程

濾波狀態(tài)其中(x，y)為機(jī)器人的位置坐標(biāo)；為機(jī)器人當(dāng)前航向，包括根據(jù)航位推算獲得的機(jī)器人航向和由于傳動(dòng)間隙和打滑造成的航位推算的航向偏差狀態(tài)方程如下：

其中，是高斯白噪聲，COV[w(k)，w(l)]＝Q·δ_kl。N_R、N_L分別為左右輪驅(qū)動(dòng)電機(jī)的編碼器脈沖，可看作狀態(tài)方程的控制變量。上述狀態(tài)方程可簡(jiǎn)記為：

取測(cè)量值Z＝[z₁，z₂，z₃]分別為超寬帶測(cè)量的XY軸坐標(biāo)，和電子羅盤測(cè)量的航向(已轉(zhuǎn)換到超寬帶定位坐標(biāo)系下)。則測(cè)量方程如下：

Z(k)＝H·X(k)+v(k) (8)

其中，

測(cè)量噪聲v＝[v₁，v₂，v₃]為高斯白噪聲，COV[v(k)，v(l)]＝R·δ_kl。

將UKF計(jì)算過(guò)程分為Sigma點(diǎn)計(jì)算、一步預(yù)測(cè)和測(cè)量更新三部分，濾波的過(guò)程就是這三部分運(yùn)算的反復(fù)迭代。

(2)Sigma點(diǎn)計(jì)算

Sigma采樣點(diǎn)計(jì)算公式如下：

尺度因子λ＝α²(n+κ)-n，其中n為狀態(tài)維數(shù),κ為次級(jí)尺度因子，通常取為0。為第k步時(shí)狀態(tài)誤差方差陣，則表示矩陣的平方根矩陣的第i行。

利用各采樣點(diǎn)計(jì)算的權(quán)重設(shè)定如下：

其中β＝2。當(dāng)用于向量估計(jì)時(shí)，選用加權(quán)系數(shù)當(dāng)用于矩陣估計(jì)時(shí)，選用加權(quán)系數(shù)

(3)一步預(yù)測(cè)

(4)測(cè)量更新

4、航位推算

k時(shí)刻根據(jù)機(jī)器人的位置(x_dk，y_dk)和航向的遞推公式如下：

其中，(x_dk-1，y_dk-1)和為上一時(shí)刻k-1的機(jī)器人的位置和航向，并且為航向偏差，是高斯白噪聲。

如上所述，本發(fā)明提供了一種結(jié)合了超寬帶定位和航位推算的特點(diǎn)的機(jī)器人室內(nèi)定位方法：當(dāng)超寬帶信號(hào)正常時(shí)，采用無(wú)跡卡爾曼濾波(Unscented Kalman Filter，UKF)方法對(duì)超寬帶、編碼器、電子羅盤信息進(jìn)行數(shù)據(jù)融合，在獲得位置、航向的同時(shí)對(duì)航位推算的航向失準(zhǔn)角進(jìn)行估算；當(dāng)超寬帶信號(hào)異常時(shí)，通過(guò)航位推算進(jìn)行位姿解算，由于失準(zhǔn)角可以實(shí)時(shí)補(bǔ)償，因此可以保證航位推算的定位定向精度。

顯然，上述實(shí)施例僅僅是為清楚地說(shuō)明本發(fā)明所作的舉例，而并非是對(duì)本發(fā)明的實(shí)施方式的限定。對(duì)于所屬領(lǐng)域的普通技術(shù)人員來(lái)說(shuō)，在上述說(shuō)明的基礎(chǔ)上還可以做出其它不同形式的變化或變動(dòng)。這里無(wú)需也無(wú)法對(duì)所有的實(shí)施方式予以窮舉。而這些屬于本發(fā)明的精神所引伸出的顯而易見的變化或變動(dòng)仍處于本發(fā)明的保護(hù)范圍之中。