本發(fā)明屬于輪式移動(dòng)機(jī)器人室內(nèi)定位領(lǐng)域，尤其是涉及一種基于多傳感器融合的移動(dòng)機(jī)器人室內(nèi)定位方法。

背景技術(shù)：

移動(dòng)機(jī)器人在室內(nèi)環(huán)境中移動(dòng)時(shí)，首先要知道自己處于什么位置，這是機(jī)器人進(jìn)行環(huán)境地圖繪制、自主導(dǎo)航的重要前提就是精確的位置和位姿。因此，室內(nèi)移動(dòng)機(jī)器人定位技術(shù)一直是機(jī)器人領(lǐng)域的研究熱點(diǎn)和難點(diǎn)。

隨著傳感技術(shù)的進(jìn)步，用于室內(nèi)移動(dòng)機(jī)器人定位的傳感器也在不斷更新，總體來說，當(dāng)前的機(jī)器人定位方法主要分為兩類：相對(duì)定位法和絕對(duì)定位法。相對(duì)定位法指機(jī)器人根據(jù)自身傳感器，如里程計(jì)、慣性模塊等得到短時(shí)間內(nèi)的相對(duì)位移及轉(zhuǎn)向，結(jié)合上一采樣時(shí)刻的累加位姿得出機(jī)器人在當(dāng)前時(shí)刻的位姿，主要有航跡推算法和慣性導(dǎo)航法。

航跡推算法主要用于短距離定位，長距離時(shí)編碼器的累積誤差十分明顯；加速度計(jì)和陀螺儀存在系統(tǒng)誤差和零漂、溫漂等問題，也會(huì)造成累積誤差。

絕對(duì)定位法指機(jī)器人依靠外部傳感器直接確定其在世界坐標(biāo)系中的位姿，常使用路標(biāo)法、GPS、地圖匹配等。路標(biāo)法維護(hù)困難，并且需要對(duì)環(huán)境進(jìn)行改變；GPS多用于室外，不適合在室內(nèi)環(huán)境使用；地圖匹配常用激光雷達(dá)獲取環(huán)境信息，將前后采用數(shù)據(jù)進(jìn)行匹配以獲得機(jī)器人全局位姿，但大量程的激光雷達(dá)造價(jià)昂貴，小量程的激光雷達(dá)無法處理像樓道一般環(huán)境特征相似，變化不明顯的場(chǎng)景。

在以往的研究中，有人提出每隔一段時(shí)間采用激光掃描匹配校準(zhǔn)里程計(jì)定位。但這往往需要使用價(jià)格昂貴的大量程激光雷達(dá)，并且在一些特征單一的室內(nèi)環(huán)境中，激光匹配無法根據(jù)匹配結(jié)果推斷位移，因此利用其定位數(shù)據(jù)校準(zhǔn)里程計(jì)會(huì)造成數(shù)據(jù)錯(cuò)誤，因此，本發(fā)明提出一種基于多傳感器融合的移動(dòng)機(jī)器人室內(nèi)定位方法，使用基于航跡推算的里程計(jì)位置數(shù)據(jù)補(bǔ)償激光匹配定位中無法識(shí)別相似環(huán)境的情況，在提高室內(nèi)定位精度的基礎(chǔ)上，其環(huán)境適應(yīng)性更強(qiáng)。

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

有鑒于此，本發(fā)明旨在提出一種基于多傳感器融合的移動(dòng)機(jī)器人室內(nèi)定位方法，為機(jī)器人的地圖繪制和導(dǎo)航提供更為準(zhǔn)確的定位數(shù)據(jù)。

為達(dá)到上述目的，本發(fā)明的技術(shù)方案是這樣實(shí)現(xiàn)的：

一種基于多傳感器融合的移動(dòng)機(jī)器人室內(nèi)定位方法，利用激光數(shù)據(jù)匹配的ICP算法進(jìn)行定位估算，使用航跡推算法得到的位姿變化進(jìn)行定位結(jié)果補(bǔ)償。

進(jìn)一步的，所述的利用激光數(shù)據(jù)匹配的ICP算法進(jìn)行定位估算包括：

設(shè)采樣周期為Δt，在第i個(gè)采樣周期之前，機(jī)器人的準(zhǔn)確位姿為

p＝(x,y,θ)^T

在第i個(gè)采樣周期，通過激光掃描匹配得到機(jī)器人移動(dòng)的位姿變化為：

Δp_{icp_i}＝[Δx_{ipc_i} Δy_{ipc_i} Δθ_{ipc_i}]^T；

設(shè)此時(shí)機(jī)器人的移動(dòng)速度為v_{icp_i}＝[v_{icp_xi},v_{icp_yi},w_{icp_i}]^T，因此存在以下關(guān)系：

v_{icp_xi}＝Δx_{icp_i}/Δt或v_{icp_xi}＝Δy_{icp_i}/Δt，并且ΔS_{icp_i}＝Δx_{icp_i}或ΔS_{icp_i}＝Δy_{icp_i}，

其中ΔS_{icp_i}為通過匹配算法得到的機(jī)器人在本采樣周期內(nèi)的移動(dòng)距離。

進(jìn)一步的，利用激光數(shù)據(jù)匹配的ICP算法進(jìn)行定位估算具體包括：

(a1)將本時(shí)刻激光掃描記為當(dāng)前掃描D，上一時(shí)刻掃描記為參考掃描M；

(b1)通過點(diǎn)到線的匹配方法得到將D匹配到M的最優(yōu)變換(R,T)，其中R為旋轉(zhuǎn)變換矩陣，T為平移矢量：

(c1)根據(jù)(R,T)計(jì)算當(dāng)前機(jī)器人的位姿變化Δp_k＝(Δx_k,Δy_k,Δθ_k)^T，假定k時(shí)刻機(jī)器人位姿為p_k＝(x_k,y_k,θ_k)^T，則k+1時(shí)刻機(jī)器人位姿為：

(d1)將當(dāng)前掃描D記為新的參考掃描M，繼續(xù)采樣激光數(shù)據(jù)，由步驟(a1)開始重新迭代。

進(jìn)一步的，所述的使用航跡推算法得到的位姿變化進(jìn)行定位結(jié)果補(bǔ)償包括：

使用航跡推算法進(jìn)行定位估算，得到第i個(gè)采樣周期內(nèi)機(jī)器人移動(dòng)的位姿變化為：

Δp_{track_i}＝[Δx_{track_i},Δy_{track_i},Δθ_{track_i}]^T；

設(shè)此時(shí)機(jī)器人的移動(dòng)速度為v_{track_i}＝[v_{track_xi},v_{track_yi},w_{track_i}]^T，則：

v_{track_xi}＝ΔS_{track_i}/Δt，v_{track_yi}＝0.0，w_i＝Δθ_{track_i}/Δt；

其中ΔS_{track_i}為通過航跡推算法得到的機(jī)器人在本采樣周期內(nèi)的移動(dòng)距離。

進(jìn)一步的，所述的使用航跡推算法得到的位姿變化進(jìn)行定位結(jié)果補(bǔ)償具體包括：

(a2)假設(shè)在一個(gè)采樣周期內(nèi)，收到電機(jī)編碼器反饋，單位換算得到左輪、右輪的移動(dòng)距離分別為ΔS_l和ΔS_r，轉(zhuǎn)過的角度為Δθ，則可以推算得到機(jī)器人在這個(gè)采用周期內(nèi)的移動(dòng)距離和旋轉(zhuǎn)角角度：

其中，2R為機(jī)器人兩輪間距，ΔS為機(jī)器人移動(dòng)的距離，Δθ為機(jī)器人轉(zhuǎn)過的角度；

(b2)機(jī)器人在世界坐標(biāo)系下的運(yùn)動(dòng)可如下式所示：

其中，θ為此時(shí)刻之前機(jī)器人已旋轉(zhuǎn)過的累積角度；

(c2)由此，假定k時(shí)刻機(jī)器人位姿為p_k＝(x_k,y_k,θ_k)，單位采樣時(shí)間內(nèi)機(jī)器人位姿變化為Δp_k＝(Δx_k,Δy_k,Δθ_k)，則根據(jù)航跡推算法，機(jī)器人在k+1時(shí)刻的位姿為：

進(jìn)一步的，還包括利用激光數(shù)據(jù)匹配的ICP算法進(jìn)行定位估算得到的位姿變化以及使用航跡推算法得到的位姿變化做差，得到位姿差Δp。

進(jìn)一步的，若滿足

Δp≤ξ，其中ξ>0，

則認(rèn)為此時(shí)的激光掃描匹配得到的位姿變化正確

進(jìn)一步的，若不滿足

Δp≤ξ，其中ξ>0，

則啟用航跡推算法得到的位姿變化進(jìn)行定位結(jié)果補(bǔ)償。

相對(duì)于現(xiàn)有技術(shù)，本發(fā)明所述的一種基于多傳感器融合的移動(dòng)機(jī)器人室內(nèi)定位方法具有以下優(yōu)勢(shì)：

(1)本發(fā)明將基于激光雷達(dá)的環(huán)境匹配定位算法與基于里程計(jì)的航跡推算法結(jié)合，完成移動(dòng)機(jī)器人的室內(nèi)定位；

(2)本發(fā)明采用基于航跡推算的里程計(jì)數(shù)據(jù)補(bǔ)償激光雷達(dá)定位數(shù)據(jù)，解決了使用短量程激光雷達(dá)無法區(qū)分特征單一且長時(shí)間無變化的環(huán)境。

附圖說明

構(gòu)成本發(fā)明的一部分的附圖用來提供對(duì)本發(fā)明的進(jìn)一步理解，本發(fā)明的示意性實(shí)施例及其說明用于解釋本發(fā)明，并不構(gòu)成對(duì)本發(fā)明的不當(dāng)限定。在附圖中：

圖1為本發(fā)明實(shí)施例所述的一種基于多傳感器融合的移動(dòng)機(jī)器人室內(nèi)定位方法示意圖。

具體實(shí)施方式

需要說明的是，在不沖突的情況下，本發(fā)明中的實(shí)施例及實(shí)施例中的特征可以相互組合。

下面將參考附圖并結(jié)合實(shí)施例來詳細(xì)說明本發(fā)明。

本發(fā)明使用基于航跡推算的里程計(jì)定位數(shù)據(jù)補(bǔ)償激光雷達(dá)的掃描匹配定位，該方法的實(shí)現(xiàn)基于如下理論：

(1)基于編碼器的航跡推算模型

假設(shè)在一個(gè)采樣周期內(nèi)，收到電機(jī)編碼器反饋，單位換算得到左輪、右輪的移動(dòng)距離分別為ΔS_l和ΔS_r，轉(zhuǎn)過的角度為Δθ，則可以推算得到機(jī)器人在這個(gè)采用周期內(nèi)的移動(dòng)距離和旋轉(zhuǎn)角角度：

其中，2R為機(jī)器人兩輪間距，ΔS為機(jī)器人移動(dòng)的距離，Δθ為機(jī)器人轉(zhuǎn)過的角度。

由于采樣間隔十分短，可以認(rèn)為機(jī)器人位移近似于直線，則機(jī)器人在世界坐標(biāo)系下的運(yùn)動(dòng)可如下式所示：

其中，θ為此時(shí)刻之前機(jī)器人已旋轉(zhuǎn)過的累積角度。

由此，假定k時(shí)刻機(jī)器人位姿為p_k＝(x_k,y_k,θ_k)，單位采樣時(shí)間內(nèi)機(jī)器人位姿變化為Δp_k＝(Δx_k,Δy_k,Δθ_k)，則根據(jù)航跡推算法，機(jī)器人在k+1時(shí)刻的位姿為：

(2)基于激光數(shù)據(jù)匹配的ICP算法

ICP算法的思想是對(duì)連續(xù)采集的相鄰兩個(gè)激光數(shù)據(jù)幀進(jìn)行匹配，獲取兩者之間的相對(duì)位姿變換關(guān)系，從而增量式地更新機(jī)器人當(dāng)前位姿，算法步驟可概括為：

(a)將本時(shí)刻激光掃描記為當(dāng)前掃描D，上一時(shí)刻掃描記為參考掃描M；

(b)通過點(diǎn)到線的匹配方法得到將D匹配到M的最優(yōu)變換(R,T)，其中R為旋轉(zhuǎn)變換矩陣，T為平移矢量：

(c)根據(jù)(R,T)計(jì)算當(dāng)前機(jī)器人的位姿變化Δp_k＝(Δx_k,Δy_k,Δθ_k)^T，假定k時(shí)刻機(jī)器人位姿為p_k＝(x_k,y_k,θ_k)^T，則k+1時(shí)刻機(jī)器人位姿為：

(d)將當(dāng)前掃描D記為新的參考掃描M，繼續(xù)采樣激光數(shù)據(jù)，由步驟(a)開始重新迭代。

如圖1所示，本發(fā)明所述的多傳感器融合定位的算法步驟：

未知環(huán)境中，手動(dòng)控制機(jī)器人行走建立環(huán)境地圖過程中，只做直線前進(jìn)和原地旋轉(zhuǎn)動(dòng)作，這一約定的目的是減小地圖繪制過程中因誤差而引起的地圖傾斜。

假設(shè)采樣周期為Δt，在第i個(gè)采樣周期之前，機(jī)器人的準(zhǔn)確位姿為p＝(x,y,θ)^T。

第一步，采用關(guān)鍵理論(2)中提到的基于激光數(shù)據(jù)匹配的ICP算法進(jìn)行定位估算，在第i個(gè)采樣周期，通過激光掃描匹配得到機(jī)器人移動(dòng)的位姿變化為：Δp_{icp_i}＝[Δx_{ipc_i}Δy_{ipc_i} Δθ_{ipc_i}]^T

設(shè)此時(shí)機(jī)器人的移動(dòng)速度為v_{icp_i}＝[v_{icp_xi},v_{icp_yi},w_{icp_i}]^T，對(duì)于雙輪驅(qū)動(dòng)移動(dòng)移動(dòng)機(jī)器人，v_y＝0恒成立。根據(jù)前提約定，只存在向前直行和原地轉(zhuǎn)彎，因此以下等式恒成立：

v_{icp_xi}＝Δx_{icp_i}/Δt或v_{icp_xi}＝Δy_{icp_i}/Δt，并且ΔS_{icp_i}＝Δx_{icp_i}或ΔS_{icp_i}＝Δy_{icp_i}。

其中ΔS_{icp_i}為通過匹配算法得到的機(jī)器人在本采樣周期內(nèi)的移動(dòng)距離。

第二步，使用航跡推算法進(jìn)行定位估算，根據(jù)式(1)～(3)得到第i個(gè)采樣周期內(nèi)機(jī)器人移動(dòng)的位姿變化為：

Δp_{track_i}＝[Δx_{track_i},Δy_{track_i},Δθ_{track_i}]^T

設(shè)此時(shí)機(jī)器人的移動(dòng)速度為v_{track_i}＝[v_{track_xi},v_{track_yi},w_{track_i}]^T，則：

v_{track_xi}＝ΔS_{track_i}/Δt，v_{track_yi}＝0.0，w_i＝Δθ_{track_i}/Δt。

其中ΔS_{track_i}為通過航跡推算法得到的機(jī)器人在本采樣周期內(nèi)的移動(dòng)距離。

第三步，驗(yàn)證激光掃描匹配得到的位姿變化是否正確：

第一、二步均是對(duì)第i個(gè)采樣周期內(nèi)機(jī)器人移動(dòng)的位姿變化進(jìn)行估算，若兩者的估算均正確，應(yīng)有：

|Δp_{track_i}-Δp_{icp_i}|≤ξ (5)

其中ξ是一個(gè)大于零的數(shù)，其含義為兩種位姿估算法得到的位姿的最大允許誤差。

若結(jié)果滿足(5)式，則認(rèn)為此時(shí)的激光掃描匹配得到的位姿變化正確，則該采樣時(shí)間后，機(jī)器人位姿p_i可以表示為：

p_i＝p_i-1+Δp_{icp_i} (6)

若計(jì)算結(jié)果不滿足(5)式，進(jìn)一步判斷，如果此時(shí)v_{icp_i}≈0.0但v_{track_i}≠0.0，則判斷此時(shí)的激光掃描匹配得到的位姿變化錯(cuò)誤，猜想機(jī)器人進(jìn)入結(jié)構(gòu)特征單一路段，啟用航跡推算法得到的位姿變化進(jìn)行定位結(jié)果補(bǔ)償，即：

p_i＝p_i-1+Δp_{track_i} (7)

如此往復(fù)，直至機(jī)器人創(chuàng)建封閉的環(huán)境地圖為止。

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