一種基於多傳感器融合的移動機器人室內定位方法與流程
2023-05-22 02:02:26 1
本發明屬於輪式移動機器人室內定位領域，尤其是涉及一種基於多傳感器融合的移動機器人室內定位方法。
背景技術：
移動機器人在室內環境中移動時，首先要知道自己處於什麼位置，這是機器人進行環境地圖繪製、自主導航的重要前提就是精確的位置和位姿。因此，室內移動機器人定位技術一直是機器人領域的研究熱點和難點。
隨著傳感技術的進步，用於室內移動機器人定位的傳感器也在不斷更新，總體來說，當前的機器人定位方法主要分為兩類：相對定位法和絕對定位法。相對定位法指機器人根據自身傳感器，如裡程計、慣性模塊等得到短時間內的相對位移及轉向，結合上一採樣時刻的累加位姿得出機器人在當前時刻的位姿，主要有航跡推算法和慣性導航法。
航跡推算法主要用於短距離定位，長距離時編碼器的累積誤差十分明顯；加速度計和陀螺儀存在系統誤差和零漂、溫漂等問題，也會造成累積誤差。
絕對定位法指機器人依靠外部傳感器直接確定其在世界坐標系中的位姿，常使用路標法、GPS、地圖匹配等。路標法維護困難，並且需要對環境進行改變；GPS多用於室外，不適合在室內環境使用；地圖匹配常用雷射雷達獲取環境信息，將前後採用數據進行匹配以獲得機器人全局位姿，但大量程的雷射雷達造價昂貴，小量程的雷射雷達無法處理像樓道一般環境特徵相似，變化不明顯的場景。
在以往的研究中，有人提出每隔一段時間採用雷射掃描匹配校準裡程計定位。但這往往需要使用價格昂貴的大量程雷射雷達，並且在一些特徵單一的室內環境中，雷射匹配無法根據匹配結果推斷位移，因此利用其定位數據校準裡程計會造成數據錯誤，因此，本發明提出一種基於多傳感器融合的移動機器人室內定位方法，使用基於航跡推算的裡程計位置數據補償雷射匹配定位中無法識別相似環境的情況，在提高室內定位精度的基礎上，其環境適應性更強。
技術實現要素：
有鑑於此，本發明旨在提出一種基於多傳感器融合的移動機器人室內定位方法，為機器人的地圖繪製和導航提供更為準確的定位數據。
為達到上述目的，本發明的技術方案是這樣實現的：
一種基於多傳感器融合的移動機器人室內定位方法，利用雷射數據匹配的ICP算法進行定位估算，使用航跡推算法得到的位姿變化進行定位結果補償。
進一步的，所述的利用雷射數據匹配的ICP算法進行定位估算包括：
設採樣周期為Δt，在第i個採樣周期之前，機器人的準確位姿為
p＝(x,y,θ)T
在第i個採樣周期，通過雷射掃描匹配得到機器人移動的位姿變化為：
Δpicp_i＝[Δxipc_i Δyipc_i Δθipc_i]T；
設此時機器人的移動速度為vicp_i＝[vicp_xi,vicp_yi,wicp_i]T，因此存在以下關係：
vicp_xi＝Δxicp_i/Δt或vicp_xi＝Δyicp_i/Δt，並且ΔSicp_i＝Δxicp_i或ΔSicp_i＝Δyicp_i，
其中ΔSicp_i為通過匹配算法得到的機器人在本採樣周期內的移動距離。
進一步的，利用雷射數據匹配的ICP算法進行定位估算具體包括：
(a1)將本時刻雷射掃描記為當前掃描D，上一時刻掃描記為參考掃描M；
(b1)通過點到線的匹配方法得到將D匹配到M的最優變換(R,T)，其中R為旋轉變換矩陣，T為平移矢量：
(c1)根據(R,T)計算當前機器人的位姿變化Δpk＝(Δxk,Δyk,Δθk)T，假定k時刻機器人位姿為pk＝(xk,yk,θk)T，則k+1時刻機器人位姿為：
(d1)將當前掃描D記為新的參考掃描M，繼續採樣雷射數據，由步驟(a1)開始重新迭代。
進一步的，所述的使用航跡推算法得到的位姿變化進行定位結果補償包括：
使用航跡推算法進行定位估算，得到第i個採樣周期內機器人移動的位姿變化為：
Δptrack_i＝[Δxtrack_i,Δytrack_i,Δθtrack_i]T；
設此時機器人的移動速度為vtrack_i＝[vtrack_xi,vtrack_yi,wtrack_i]T，則：
vtrack_xi＝ΔStrack_i/Δt，vtrack_yi＝0.0，wi＝Δθtrack_i/Δt；
其中ΔStrack_i為通過航跡推算法得到的機器人在本採樣周期內的移動距離。
進一步的，所述的使用航跡推算法得到的位姿變化進行定位結果補償具體包括：
(a2)假設在一個採樣周期內，收到電機編碼器反饋，單位換算得到左輪、右輪的移動距離分別為ΔSl和ΔSr，轉過的角度為Δθ，則可以推算得到機器人在這個採用周期內的移動距離和旋轉角角度：
其中，2R為機器人兩輪間距，ΔS為機器人移動的距離，Δθ為機器人轉過的角度；
(b2)機器人在世界坐標系下的運動可如下式所示：
其中，θ為此時刻之前機器人已旋轉過的累積角度；
(c2)由此，假定k時刻機器人位姿為pk＝(xk,yk,θk)，單位採樣時間內機器人位姿變化為Δpk＝(Δxk,Δyk,Δθk)，則根據航跡推算法，機器人在k+1時刻的位姿為：
進一步的，還包括利用雷射數據匹配的ICP算法進行定位估算得到的位姿變化以及使用航跡推算法得到的位姿變化做差，得到位姿差Δp。
進一步的，若滿足
Δp≤ξ，其中ξ>0，
則認為此時的雷射掃描匹配得到的位姿變化正確
進一步的，若不滿足
Δp≤ξ，其中ξ>0，
則啟用航跡推算法得到的位姿變化進行定位結果補償。
相對於現有技術，本發明所述的一種基於多傳感器融合的移動機器人室內定位方法具有以下優勢：
(1)本發明將基於雷射雷達的環境匹配定位算法與基於裡程計的航跡推算法結合，完成移動機器人的室內定位；
(2)本發明採用基於航跡推算的裡程計數據補償雷射雷達定位數據，解決了使用短量程雷射雷達無法區分特徵單一且長時間無變化的環境。
附圖說明
構成本發明的一部分的附圖用來提供對本發明的進一步理解，本發明的示意性實施例及其說明用於解釋本發明，並不構成對本發明的不當限定。在附圖中：
圖1為本發明實施例所述的一種基於多傳感器融合的移動機器人室內定位方法示意圖。
具體實施方式
需要說明的是，在不衝突的情況下，本發明中的實施例及實施例中的特徵可以相互組合。
下面將參考附圖並結合實施例來詳細說明本發明。
本發明使用基於航跡推算的裡程計定位數據補償雷射雷達的掃描匹配定位，該方法的實現基於如下理論：
(1)基於編碼器的航跡推算模型
假設在一個採樣周期內，收到電機編碼器反饋，單位換算得到左輪、右輪的移動距離分別為ΔSl和ΔSr，轉過的角度為Δθ，則可以推算得到機器人在這個採用周期內的移動距離和旋轉角角度：
其中，2R為機器人兩輪間距，ΔS為機器人移動的距離，Δθ為機器人轉過的角度。
由於採樣間隔十分短，可以認為機器人位移近似於直線，則機器人在世界坐標系下的運動可如下式所示：
其中，θ為此時刻之前機器人已旋轉過的累積角度。
由此，假定k時刻機器人位姿為pk＝(xk,yk,θk)，單位採樣時間內機器人位姿變化為Δpk＝(Δxk,Δyk,Δθk)，則根據航跡推算法，機器人在k+1時刻的位姿為：
(2)基於雷射數據匹配的ICP算法
ICP算法的思想是對連續採集的相鄰兩個雷射數據幀進行匹配，獲取兩者之間的相對位姿變換關係，從而增量式地更新機器人當前位姿，算法步驟可概括為：
(a)將本時刻雷射掃描記為當前掃描D，上一時刻掃描記為參考掃描M；
(b)通過點到線的匹配方法得到將D匹配到M的最優變換(R,T)，其中R為旋轉變換矩陣，T為平移矢量：
(c)根據(R,T)計算當前機器人的位姿變化Δpk＝(Δxk,Δyk,Δθk)T，假定k時刻機器人位姿為pk＝(xk,yk,θk)T，則k+1時刻機器人位姿為：
(d)將當前掃描D記為新的參考掃描M，繼續採樣雷射數據，由步驟(a)開始重新迭代。
如圖1所示，本發明所述的多傳感器融合定位的算法步驟：
未知環境中，手動控制機器人行走建立環境地圖過程中，只做直線前進和原地旋轉動作，這一約定的目的是減小地圖繪製過程中因誤差而引起的地圖傾斜。
假設採樣周期為Δt，在第i個採樣周期之前，機器人的準確位姿為p＝(x,y,θ)T。
第一步，採用關鍵理論(2)中提到的基於雷射數據匹配的ICP算法進行定位估算，在第i個採樣周期，通過雷射掃描匹配得到機器人移動的位姿變化為：Δpicp_i＝[Δxipc_iΔyipc_i Δθipc_i]T
設此時機器人的移動速度為vicp_i＝[vicp_xi,vicp_yi,wicp_i]T，對於雙輪驅動移動移動機器人，vy＝0恆成立。根據前提約定，只存在向前直行和原地轉彎，因此以下等式恆成立：
vicp_xi＝Δxicp_i/Δt或vicp_xi＝Δyicp_i/Δt，並且ΔSicp_i＝Δxicp_i或ΔSicp_i＝Δyicp_i。
其中ΔSicp_i為通過匹配算法得到的機器人在本採樣周期內的移動距離。
第二步，使用航跡推算法進行定位估算，根據式(1)～(3)得到第i個採樣周期內機器人移動的位姿變化為：
Δptrack_i＝[Δxtrack_i,Δytrack_i,Δθtrack_i]T
設此時機器人的移動速度為vtrack_i＝[vtrack_xi,vtrack_yi,wtrack_i]T，則：
vtrack_xi＝ΔStrack_i/Δt，vtrack_yi＝0.0，wi＝Δθtrack_i/Δt。
其中ΔStrack_i為通過航跡推算法得到的機器人在本採樣周期內的移動距離。
第三步，驗證雷射掃描匹配得到的位姿變化是否正確：
第一、二步均是對第i個採樣周期內機器人移動的位姿變化進行估算，若兩者的估算均正確，應有：
|Δptrack_i-Δpicp_i|≤ξ (5)
其中ξ是一個大於零的數，其含義為兩種位姿估算法得到的位姿的最大允許誤差。
若結果滿足(5)式，則認為此時的雷射掃描匹配得到的位姿變化正確，則該採樣時間後，機器人位姿pi可以表示為：
pi＝pi-1+Δpicp_i (6)
若計算結果不滿足(5)式，進一步判斷，如果此時vicp_i≈0.0但vtrack_i≠0.0，則判斷此時的雷射掃描匹配得到的位姿變化錯誤，猜想機器人進入結構特徵單一路段，啟用航跡推算法得到的位姿變化進行定位結果補償，即：
pi＝pi-1+Δptrack_i (7)
如此往復，直至機器人創建封閉的環境地圖為止。
以上所述僅為本發明的較佳實施例而已，並不用以限制本發明，凡在本發明的精神和原則之內，所作的任何修改、等同替換、改進等，均應包含在本發明的保護範圍之內。