(X i，Y1)，min2 =(x2，y2)，由于移動(dòng)機(jī)器人在全局中的位姿只能在某一個(gè)子區(qū)域內(nèi)，因此，該結(jié)果必須同時(shí) 滿(mǎn)足條件
其中I (X1)與d(X1)分別表示第 ^子區(qū)間所分割的小矩形的長(zhǎng)度與寬度，當(dāng)滿(mǎn)足條件時(shí)繼續(xù)執(zhí)行步驟4G，否則讓移動(dòng)機(jī)器 人自動(dòng)向前或者向后移動(dòng)一定的距離，再返回步驟4A重新執(zhí)行；
[0025] 步驟4G根據(jù)上述結(jié)果確定移動(dòng)機(jī)器人在全局地圖中的位置。根據(jù)第 X個(gè)子區(qū)域內(nèi)采集的mxkx組點(diǎn)集Q )!與P h (c = Y1 U y 2)之間匹配可得到mxkx組 誤差，將其按照從小到大順序排列，分別取前三個(gè)最小的誤差，設(shè)結(jié)果分別為
>其對(duì)應(yīng)的Qx中的六組匹配點(diǎn)集可 設(shè)為
[0026] 步驟4H步驟4G中共得到的六個(gè)誤差，同樣按照從小到大的順序排列，并取前三 個(gè)最小的誤差，設(shè)得到對(duì)應(yīng)誤差為{>/，ε 2'，ε3' }，對(duì)應(yīng)的點(diǎn)集可分別設(shè)為{Qxl'， Qx/，QJ }，根據(jù)映射關(guān)系Φ，設(shè)三個(gè)點(diǎn)集對(duì)應(yīng)的移動(dòng)機(jī)器人在地圖中的位姿分別為
[0027] 步驟41設(shè)定方差閾值σ，計(jì)算三個(gè)誤差的方差σ 3，如果滿(mǎn)足σ 3< σ，則繼續(xù)執(zhí) 行步驟4J，否則去掉三個(gè)誤差中最大的那個(gè)，然后再計(jì)算兩個(gè)誤差的方差〇2，如果滿(mǎn)足 〇2 < σ，則繼續(xù)執(zhí)行步驟4Κ，否則去掉兩個(gè)誤差中最大的那個(gè)，然后繼續(xù)執(zhí)行步驟4L;
[0028] 步驟4J假設(shè)最終有三個(gè)誤差{ε/，ε2'，ε3' }，每個(gè)誤差對(duì)應(yīng)的匹配變換矩 陣分別，
表示移動(dòng)機(jī)器人丟失位姿后保存的位姿，首先根據(jù) R(cS)變換其位姿為：
[0030] 其中X。與y。不變，然后根據(jù)匹配變換矩陣計(jì)算移動(dòng)機(jī)器人三個(gè)全局地圖的位姿可 表示如下；
[0033] 計(jì)算三個(gè)權(quán)重
根據(jù)權(quán)重可最終確定移動(dòng)機(jī) 器人全局定位的位姿可表示如下：
[0035] 步驟4K假設(shè)最終有兩個(gè)誤差{ε/，ε2'，}，每個(gè)誤差對(duì)應(yīng)的匹配變換矩陣分 別為
表示移動(dòng)機(jī)器人丟失位姿后保存的位姿，首先根據(jù) R(cS)變換其位姿為：
[0037] 其中X。與y。不變，然后根據(jù)匹配變換矩陣計(jì)算移動(dòng)機(jī)器人兩個(gè)全局地圖的位姿可 表示如下；
[0040] 計(jì)算兩個(gè)權(quán)重
，根據(jù)權(quán)重可最終確定移動(dòng)機(jī)器 人全局定位的位姿可表示如下：
[0042] 步驟4L假設(shè)最只有誤差ε/，，對(duì)應(yīng)的匹配變換矩陣分別為 Θ。）表示移動(dòng)機(jī)器人丟失位姿后保存的位姿，首先根據(jù)R(c δ )變換其位姿為：
[0043] θ 〇1= θ 〇+ δ C1
[0044] 其中X。與y。不變，然后根據(jù)匹配變換矩陣計(jì)算移動(dòng)機(jī)器人全局地圖的位姿可表示 如下；
[0047] 計(jì)算權(quán)重CO1= 1，根據(jù)權(quán)重可最終確定移動(dòng)機(jī)器人全局定位的位姿可表示如下：
[0049] 進(jìn)一步的，步驟1中移動(dòng)機(jī)器人自帶的傳感器為激光傳感器或視覺(jué)傳感器。
[0050] 進(jìn)一步的，所述方法步驟41中方差閾值〇根據(jù)下述公式確定：
[0051] σ = (2 ~5) Xmin({ ε /，ε 2'，ε 3' })。
[0052] 本發(fā)明的有益效果在于：本發(fā)明在實(shí)現(xiàn)基于SLAM導(dǎo)航移動(dòng)機(jī)器人的全局定位的 基礎(chǔ)上，使得方案更為可行可靠、成本更低、操作簡(jiǎn)單易于實(shí)現(xiàn)。該方法只是通過(guò)軟件實(shí)現(xiàn) 的一段程序，且進(jìn)行了自動(dòng)、智能優(yōu)化的判斷分析，結(jié)合簡(jiǎn)單的人工操作，即可完成本方案 的實(shí)現(xiàn)與布局。由于該方法采用了構(gòu)建地圖過(guò)程中獲取的原始數(shù)據(jù)，并進(jìn)行不斷的更新，可 采用ICP (迭代最近點(diǎn)算法）匹配的方法，根據(jù)匹配的效果綜合分析，以確定移動(dòng)機(jī)器人在 全局地圖中的位姿，易于實(shí)現(xiàn)、定位準(zhǔn)確、速度快。本發(fā)明不需要對(duì)環(huán)境進(jìn)行改造，實(shí)現(xiàn)簡(jiǎn)單 可行，易于操作，降低了產(chǎn)品成本，提高了生產(chǎn)效率。本發(fā)明的方法可適用各種移動(dòng)機(jī)器人， 尤其是AGV類(lèi)型的移動(dòng)機(jī)器人，只需移動(dòng)機(jī)器人自帶的傳感器可獲得數(shù)據(jù)實(shí)現(xiàn)SLAM導(dǎo)航即 可。
【附圖說(shuō)明】
[0053] 圖1為本發(fā)明一種基于SLAM導(dǎo)航移動(dòng)機(jī)器人的全局定位方法的流程圖；
[0054] 圖2為【具體實(shí)施方式】中AGV在任一子區(qū)域獲取的激光數(shù)據(jù)與數(shù)據(jù)庫(kù)中的數(shù)據(jù)，在 基于ICP全局定位匹配前位姿狀態(tài)示意圖；
[0055] 圖3為【具體實(shí)施方式】中AGV在任一子區(qū)域獲取的激光數(shù)據(jù)與數(shù)據(jù)庫(kù)中的數(shù)據(jù)，在 基于ICP全局定位匹配后位姿狀態(tài)示意圖。
【具體實(shí)施方式】
[0056] 下面結(jié)合附圖對(duì)本發(fā)明的【具體實(shí)施方式】進(jìn)行說(shuō)明：
[0057] 以配備激光SLAM導(dǎo)航系統(tǒng)的自動(dòng)導(dǎo)航叉車(chē)AGV為平臺(tái)進(jìn)行本發(fā)明基于激光SLAM 導(dǎo)航的AGV全局定位方法的具體操作，如圖1所示的方法流程，包括以下步驟：
[0058] 步驟1移動(dòng)機(jī)器人應(yīng)用環(huán)境的子區(qū)域的選取
[0059] 通過(guò)AGV自帶的激光傳感器，獲取室內(nèi)環(huán)境的數(shù)據(jù)信息，并通過(guò)SLAM技術(shù)，構(gòu)建關(guān) 于室內(nèi)環(huán)境完整的平面地圖。設(shè)整個(gè)應(yīng)用環(huán)境為Σ，在該環(huán)境下選取多個(gè)矩形的子區(qū)域，取 地圖中位姿為零點(diǎn)所對(duì)應(yīng)的環(huán)境區(qū)域?yàn)棣?。，其它的子區(qū)域分別為Σ i，Σ 2, . . .，Ση;
[0060] 本發(fā)明選取應(yīng)用環(huán)境中的多個(gè)子區(qū)域，將AGV置于其中任一子區(qū)域中，不需要重 啟初始化，就可以實(shí)現(xiàn)自動(dòng)全局定位，且人工移動(dòng)的距離遠(yuǎn)遠(yuǎn)小于只有零點(diǎn)的子區(qū)域時(shí)所 移動(dòng)的距離。假設(shè)AGV人工移動(dòng)最大距離不超過(guò)D，那么任意相鄰的兩個(gè)子區(qū)域的幾何中心 的距離都不應(yīng)該超過(guò)2D，且應(yīng)該滿(mǎn)足2D > 1，1表示子區(qū)域沿AGV運(yùn)行方向的長(zhǎng)度。
[0061] 步驟2數(shù)據(jù)點(diǎn)的采集
[0062] 步驟1中在AGV應(yīng)用環(huán)境中選取矩形的子區(qū)域，它的寬度和長(zhǎng)度分別設(shè)為d、l，寬 度d表示子區(qū)域沿垂直于AGV運(yùn)行方向的寬度，長(zhǎng)度1表示子區(qū)域沿AGV運(yùn)行方向的長(zhǎng)度， 那么每個(gè)子區(qū)域?qū)挾扰c長(zhǎng)度分別表示為山，d2, . . .，dn，I1, 12, . . .，In，將第i個(gè)子區(qū)域分別 沿寬度與長(zhǎng)度方向上分別等距分割為Hi1 A1等份，得到m ^k1個(gè)矩形，將AGV放在分割得到的 每個(gè)小矩形的幾何中心，可獲取沿前進(jìn)或后退方向上激光傳感器掃描數(shù)據(jù)Q 1, Q1包含m ^k1 組匹配數(shù)據(jù)，并保存在全局定位數(shù)據(jù)庫(kù)中。每個(gè)子區(qū)域獲取的數(shù)據(jù)都存在一個(gè)到AGV在構(gòu) 建地圖中位姿的--映射Φ :Qit- (X it，yit，Θ it) (1彡i彡n，1彡t彡IT^ki);
[0063] 設(shè)用于Delaunay迭代最近點(diǎn)算法匹配的函數(shù)為L(zhǎng) : (P，Q) - (R，T)，該函數(shù)的輸入 為待匹配的兩個(gè)點(diǎn)集，定義P為目標(biāo)點(diǎn)集，Q為模型點(diǎn)集，輸出為兩個(gè)點(diǎn)集進(jìn)行匹配的旋轉(zhuǎn) 矩陣R與平移向量T，旋轉(zhuǎn)矩陣R(0)表示沿逆時(shí)針?lè)较蛐D(zhuǎn)θ (Θ >〇)，是2X2的矩陣， 而平移向量T表示2X 1的列向量，并定義每個(gè)點(diǎn)集中點(diǎn)的個(gè)數(shù)為N(Qi)。
[0064] 步驟3子區(qū)域選取合理與否的分析判斷
[0065] 步驟3A設(shè)第i個(gè)子區(qū)域，第r組匹配數(shù)據(jù)，表示為I，初始值i = 1，r = 1，P表 示數(shù)據(jù)庫(kù)中任意一組模型點(diǎn)集，匹配誤差ε u表示與第i個(gè)子區(qū)域中第r組數(shù)據(jù)匹配的誤 差，計(jì)算方法為：
其中Pj、q.j分別表不點(diǎn)集P、Q ir 中的任一點(diǎn)坐標(biāo)；
[0066] 步驟3B將Qu與數(shù)據(jù)庫(kù)中已選取的所有子區(qū)域內(nèi)的所有組數(shù)據(jù)進(jìn)行匹配，獲取對(duì) 應(yīng)的匹配誤差如下：