一種基于slam導(dǎo)航移動機器人的全局定位方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明具體涉及一種基于SLAM導(dǎo)航移動機器人的全局定位方法，屬于移動機器 人自動導(dǎo)航技術(shù)領(lǐng)域。
【背景技術(shù)】
[0002] 智能移動機器人是在復(fù)雜環(huán)境下工作的，具有自主規(guī)劃、自組織、自適應(yīng)能力的機 器人?；诩す釹LAM(同時定位與構(gòu)建地圖）導(dǎo)航的AGV(自動導(dǎo)航小車）是智能移動機 器人的一種，該類AGV不需要設(shè)置固定的軌道，可通過構(gòu)建室內(nèi)完整的地圖，獲得周圍環(huán)境 完整的信息，在移動過程中，通過傳感器實時獲取周圍環(huán)境的信息，與已構(gòu)建好的室內(nèi)地圖 進行匹配，并采用粒子濾波算法精確確定其在全局地圖中的位置，同時也就確定了在周圍 環(huán)境中的位置。
[0003] 由于激光SLAM導(dǎo)航技術(shù)本身存在的缺陷，或者其不完善的地方，使其在一定情況 下，比如在走廊、遇到體積較大的運動物體等，導(dǎo)致其定位不準確，在沒有及時糾正的情況 下，會出現(xiàn)脫離虛擬任務(wù)路線的可能；或者在運行過程中出現(xiàn)故障等問題停止運行，需維 修。上述問題都會導(dǎo)致該類導(dǎo)航AGV丟失自身的位姿，為了解決該全局定位的問題，目前可 行的方案主要有使得AGV回到設(shè)定的零點區(qū)域、視覺輔助定位（需要改造環(huán)境）、無線定位 等。對于該類AGV，使得AGV回到設(shè)定的零點區(qū)域的方法，系統(tǒng)內(nèi)部設(shè)定其每次重新啟動的 起點位姿為零點。當其丟失自己的位姿時，通常將其人工移動到地圖的零點區(qū)域，由于只有 一個零點區(qū)域，因此移動距離較遠，然后再重新啟動執(zhí)行未完成的任務(wù)。該方法是目前使用 最多的方法，但該方法浪費人力、效率低下，很不方便；采用視覺輔助定位，通過安裝在AGV 上的相機，采集已經(jīng)存儲其對應(yīng)位姿的二維碼信息，來確定AGV當前的位姿。該方法需要在 AGV上安裝輔助設(shè)備，增加了設(shè)備制造與維護成本，且需要對工廠的環(huán)境進行一定程度的 人工改造，增加了人力成本；無線定位的方法是在應(yīng)用環(huán)境內(nèi)設(shè)定多個無線發(fā)射器（至少 3個），根據(jù)接受到的無線信號，通過時間延遲，采用三角測量等方法，獲取AGV在全局地圖 中的位姿，該方法測量誤差較大，且容易受環(huán)境中障礙物或其它物體的影響，導(dǎo)致定位不準 確。另外需要安裝無線發(fā)射器與接收器，大大增加了成本。

【發(fā)明內(nèi)容】

[0004] 因此，本發(fā)明針對現(xiàn)有技術(shù)中實現(xiàn)移動機器人全局定位需要對應(yīng)用環(huán)境進行一定 程度的改造，增加輔助定位的特征物體，或者需要在移動機器人設(shè)備上安裝輔助設(shè)備等。這 些方案本身對較為復(fù)雜環(huán)境的適應(yīng)能力低下、成本較高、定位精度較差、極為不方便，且由 于某些方法固有的缺陷難以消除，因此定位精度難以有更大的優(yōu)化與提高的問題，提供一 種基于SLAM導(dǎo)航移動機器人的全局定位方法，所述方法包括以下步驟：
[0005] 步驟1移動機器人應(yīng)用環(huán)境的子區(qū)域的選取
[0006] 通過移動機器人自帶傳感器，獲取室內(nèi)環(huán)境的數(shù)據(jù)信息，并通過SLAM技術(shù)，構(gòu)建 關(guān)于室內(nèi)環(huán)境完整的平面地圖。設(shè)整個應(yīng)用環(huán)境為Σ，在該環(huán)境下選取多個矩形的子區(qū)域， 取地圖中位姿為零點所對應(yīng)的環(huán)境區(qū)域為Σ。，其它的子區(qū)域分別為Σ i，Σ 2,...，Σ n;
[0007] 假設(shè)移動機器人人工移動最大距離不超過D，那么任意相鄰的兩個子區(qū)域的幾何 中心的距離都不應(yīng)該超過2D，且應(yīng)該滿足2D > 1，1表示子區(qū)域沿移動機器人運行方向的 長度；
[0008] 步驟2數(shù)據(jù)點的采集
[0009] 步驟1中在移動機器人應(yīng)用環(huán)境中選取矩形的子區(qū)域，它的寬度和長度分別設(shè)為 d、1，寬度d表示子區(qū)域沿垂直于移動機器人運行方向的寬度，長度1表示子區(qū)域沿移動機 器人運行方向的長度，那么每個子區(qū)域?qū)挾扰c長度分別表示為山，d 2,...，dn，I1,12,...，ln， 將第i個子區(qū)域分別沿寬度與長度方向上分別等距分割為叫、Ic 1等份，得到m '!^個矩形， 將移動機器人放在分割得到的每個小矩形的幾何中心，可獲取沿前進或后退方向上移動機 器人自帶傳感器掃描數(shù)據(jù)Q 1, Q1包含m ^k1組匹配數(shù)據(jù)，并保存在全局定位數(shù)據(jù)庫中。每個 子區(qū)域獲取的數(shù)據(jù)都存在一個到移動機器人在構(gòu)建地圖中位姿的一一映射Φ :Qlt- (X lt， Yif θ it) (I ^ i ^ η, I ^ t ^ IHi^ki)；
[0010] 設(shè)用于Delaunay迭代最近點算法匹配的函數(shù)為L :(P，Q) - (R，T)，該函數(shù)的輸入 為待匹配的兩個點集，定義P為目標點集，Q為模型點集，輸出為兩個點集進行匹配的旋轉(zhuǎn) 矩陣R與平移向量T，旋轉(zhuǎn)矩陣R(0)表示沿逆時針方向旋轉(zhuǎn)θ (Θ >〇)，是2X2的矩陣， 而平移向量T表示2X 1的列向量，并定義每個點集中點的個數(shù)為N(Qi);
[0011] 步驟3子區(qū)域選取合理與否的分析判斷
[0012] 步驟3A設(shè)第i個子區(qū)域，第r組匹配數(shù)據(jù)，表示為I，初始值i = 1，r = 1，P表 示數(shù)據(jù)庫中任意一組模型點集，匹配誤差ε u表示與第i個子區(qū)域中第r組數(shù)據(jù)匹配的誤 差，計算方法為：
I其中Pj、q.j分別表不點集P、Q ir 中的任一點坐標；
[0013] 步驟3B將Qu與數(shù)據(jù)庫中已選取的所有子區(qū)域內(nèi)的所有組數(shù)據(jù)進行匹配，獲取對 應(yīng)的匹配誤差如下：
[0016] 步驟3C當^3中的任何一個元素的值都小于中任何一個元素值，r = r+Ι，再返 回步驟3B，否則退出循環(huán)，重新從應(yīng)用環(huán)境中選取子區(qū)域Σ i。當r = n^kjl時，執(zhí)行步驟 3D ；
[0017] 步驟3D改變變量i = i+1，r = 1，然后返回步驟3B，繼續(xù)執(zhí)行，直至i = n+l，退 出循環(huán)；
[0018] 步驟4基于ICP實現(xiàn)移動機器人全局定位：
[0019] 步驟4A將移動機器人置于應(yīng)用環(huán)境中已選取的任一子區(qū)域Σ i內(nèi)，任何位置獲取 一組激光傳感器原始數(shù)據(jù)P，數(shù)據(jù)％表示在第i個子區(qū)域Σ i采集的數(shù)據(jù)。設(shè)在定位程序 啟動前移動機器人在地圖中的位姿為（^，7。，0。)，定義初始變量(3 = 〇,〇<5<31/6，定 義初始總誤差Eitl= 0, i = I ;
[0020] 步驟4B先將數(shù)據(jù)P進行處理得Ph= R(c δ )*P，然后通過定義的ICP匹配函數(shù) L，與全局定位數(shù)據(jù)庫中的數(shù)據(jù)Q1進行匹配得到Hijk1組變換矩陣（Rt，T t) =L(Ph，Qlt) (1彡t彡Hi1^k1)，其中Q lt表示第i個子區(qū)域采集的第t組匹配數(shù)據(jù)；
[0021] 步驟4C根據(jù)每組變換矩陣先計算變換后的點集可表示為Pt= R t*Ph+Tt，然后計算 每組匹配數(shù)據(jù)的誤差為
其中P1^ 分別表示點集P t、 Qlt中的任一點坐標，在子區(qū)域Σ i內(nèi)將所有組數(shù)據(jù)匹配一次得到總的匹配誤差可表示為
[0022] 步驟4D將子區(qū)域乙i的數(shù)據(jù)庫與激光傳感器掃描的數(shù)據(jù)P在2 π弧度內(nèi)進行多 次匹配，改變參數(shù)的值c = c+1，當滿足
時繼續(xù)執(zhí)行步驟4E，否則返回步驟4B，其 中Γ (X) (X > 〇)表示取不超過X的最大整數(shù)；
[0023] 步驟4E改變?nèi)侄ㄎ粩?shù)據(jù)庫中匹配點集的子區(qū)域，i = i+Ι，當i = η時繼續(xù)執(zhí)行 步驟4F，否則返回步驟4Α ;
[0024] 步驟4F共獲得多組總的匹配誤差E1, E2, · · ·，Ec
取誤差中最小 的兩組EminA E min2作為移動機器人全局定位的數(shù)據(jù)，設(shè)此時對應(yīng)的mini =