CN105258702A一种基于slam导航移动机器人的全局定位方法

摘要
本发明公开了一种基于SLAM导航移动机器人的全局定位方法，属于移动机器人自动导航技术领域。为解决现有技术中实现移动机器人全局定位需要对应用环境进行改造，增加辅助定位的特征物体，或者需要在移动机器人设备上安装辅助设备等。这些方案本身不能适应较为复杂环境、成本较高、定位精度较差、极为不方便，因此定位精度难以有更大的优化与提高的问题，本方法包括以下4个步骤：步骤1移动机器人应用环境的子区域的选取，步骤2数据点的采集，步骤3子区域选取合理与否的分析判断，步骤4基于ICP实现移动机器人全局定位。用于基于激光SLAM导航移动机器人尤其是AGV(自动导航小车)复杂环境下的全局定位。

公开号 CN105258702 A
发布类型 申请
专利申请号 CN 201510644986
公开日 2016年1月20日
申请日期 2015年10月6日
优先权日 2015年10月6日
发明者 王斌, 李国飞
申请人 深圳力子机器人有限公司

说明
一种基于SLAM导航移动机器人的全局定位方法
技术领域

[0001] 本发明具体涉及一种基于SLAM导航移动机器人的全局定位方法，属于移动机器 人自动导航技术领域。

背景技术

[0002] 智能移动机器人是在复杂环境下工作的，具有自主规划、自组织、自适应能力的机 器人。基于激光SLAM(同时定位与构建地图）导航的AGV(自动导航小车）是智能移动机 器人的一种，该类AGV不需要设置固定的轨道，可通过构建室内完整的地图，获得周围环境 完整的信息，在移动过程中，通过传感器实时获取周围环境的信息，与已构建好的室内地图 进行匹配，并采用粒子滤波算法精确确定其在全局地图中的位置，同时也就确定了在周围 环境中的位置。

[0003] 由于激光SLAM导航技术本身存在的缺陷，或者其不完善的地方，使其在一定情况 下，比如在走廊、遇到体积较大的运动物体等，导致其定位不准确，在没有及时纠正的情况 下，会出现脱离虚拟任务路线的可能；或者在运行过程中出现故障等问题停止运行，需维 修。上述问题都会导致该类导航AGV丢失自身的位姿，为了解决该全局定位的问题，目前可 行的方案主要有使得AGV回到设定的零点区域、视觉辅助定位（需要改造环境）、无线定位 等。对于该类AGV，使得AGV回到设定的零点区域的方法，系统内部设定其每次重新启动的 起点位姿为零点。当其丢失自己的位姿时，通常将其人工移动到地图的零点区域，由于只有 一个零点区域，因此移动距离较远，然后再重新启动执行未完成的任务。该方法是目前使用 最多的方法，但该方法浪费人力、效率低下，很不方便；采用视觉辅助定位，通过安装在AGV 上的相机，采集已经存储其对应位姿的二维码信息，来确定AGV当前的位姿。该方法需要在 AGV上安装辅助设备，增加了设备制造与维护成本，且需要对工厂的环境进行一定程度的 人工改造，增加了人力成本；无线定位的方法是在应用环境内设定多个无线发射器（至少 3个），根据接受到的无线信号，通过时间延迟，采用三角测量等方法，获取AGV在全局地图 中的位姿，该方法测量误差较大，且容易受环境中障碍物或其它物体的影响，导致定位不准 确。另外需要安装无线发射器与接收器，大大增加了成本。

发明内容

[0004] 因此，本发明针对现有技术中实现移动机器人全局定位需要对应用环境进行一定 程度的改造，增加辅助定位的特征物体，或者需要在移动机器人设备上安装辅助设备等。这 些方案本身对较为复杂环境的适应能力低下、成本较高、定位精度较差、极为不方便，且由 于某些方法固有的缺陷难以消除，因此定位精度难以有更大的优化与提高的问题，提供一 种基于SLAM导航移动机器人的全局定位方法，所述方法包括以下步骤：

[0005] 步骤1移动机器人应用环境的子区域的选取

[0006] 通过移动机器人自带传感器，获取室内环境的数据信息，并通过SLAM技术，构建 关于室内环境完整的平面地图。设整个应用环境为Σ，在该环境下选取多个矩形的子区域， 取地图中位姿为零点所对应的环境区域为Σ。，其它的子区域分别为Σ i，Σ 2,…，Σ n;

[0007] 假设移动机器人人工移动最大距离不超过D，那么任意相邻的两个子区域的几何 中心的距离都不应该超过2D，且应该满足2D > 1，1表示子区域沿移动机器人运行方向的 长度；

[0008] 步骤2数据点的采集

[0009] 步骤1中在移动机器人应用环境中选取矩形的子区域，它的宽度和长度分别设为 d、1，宽度d表示子区域沿垂直于移动机器人运行方向的宽度，长度1表示子区域沿移动机 器人运行方向的长度，那么每个子区域宽度与长度分别表示为山，d 2,…，dn，I1,12,…，ln， 将第i个子区域分别沿宽度与长度方向上分别等距分割为叫、Ic 1等份，得到m ‘!^个矩形， 将移动机器人放在分割得到的每个小矩形的几何中心，可获取沿前进或后退方向上移动机 器人自带传感器扫描数据Q 1, Q1包含m ^k1组匹配数据，并保存在全局定位数据库中。每个 子区域获取的数据都存在一个到移动机器人在构建地图中位姿的一一映射Φ :Qlt- (X lt， Yif θ it) (I ^ i ^ η, I ^ t ^ IHi^ki)；

[0010] 设用于Delaunay迭代最近点算法匹配的函数为L :(P，Q) – (R，T)，该函数的输入 为待匹配的两个点集，定义P为目标点集，Q为模型点集，输出为两个点集进行匹配的旋转 矩阵R与平移向量T，旋转矩阵R(0)表示沿逆时针方向旋转θ (Θ >〇)，是2X2的矩阵， 而平移向量T表示2X 1的列向量，并定义每个点集中点的个数为N(Qi);

[0011] 步骤3子区域选取合理与否的分析判断

[0012] 步骤3A设第i个子区域，第r组匹配数据，表示为I，初始值i = 1，r = 1，P表 示数据库中任意一组模型点集，匹配误差ε u表示与第i个子区域中第r组数据匹配的误 差，计算方法为：

Figure CN105258702AD00071
I其中Pj、q.j分别表不点集P、Q ir 中的任一点坐标；

[0013] 步骤3B将Qu与数据库中已选取的所有子区域内的所有组数据进行匹配，获取对 应的匹配误差如下：

Figure CN105258702AD00072
[0016] 步骤3C当^3中的任何一个元素的值都小于中任何一个元素值，r = r+Ι，再返 回步骤3B，否则退出循环，重新从应用环境中选取子区域Σ i。当r = n^kjl时，执行步骤 3D ；

[0017] 步骤3D改变变量i = i+1，r = 1，然后返回步骤3B，继续执行，直至i = n+l，退 出循环；

[0018] 步骤4基于ICP实现移动机器人全局定位：

[0019] 步骤4A将移动机器人置于应用环境中已选取的任一子区域Σ i内，任何位置获取 一组激光传感器原始数据P，数据％表示在第i个子区域Σ i采集的数据。设在定位程序 启动前移动机器人在地图中的位姿为（^，7。，0。)，定义初始变量(3 = 〇,〇<5<31/6，定 义初始总误差Eitl= 0, i = I ; [0020] 步骤4B先将数据P进行处理得Ph= R(c δ )*P，然后通过定义的ICP匹配函数 L，与全局定位数据库中的数据Q1进行匹配得到Hijk1组变换矩阵（Rt，T t) =L(Ph，Qlt) (1彡t彡Hi1^k1)，其中Q lt表示第i个子区域采集的第t组匹配数据； [0021] 步骤4C根据每组变换矩阵先计算变换后的点集可表示为Pt= R t*Ph+Tt，然后计算 每组匹配数据的误差为 Figure CN105258702AD00081 其中P1^ 分别表示点集P t、 Qlt中的任一点坐标，在子区域Σ i内将所有组数据匹配一次得到总的匹配误差可表示为 Figure CN105258702AD00082 [0022] 步骤4D将子区域乙i的数据库与激光传感器扫描的数据P在2 π弧度内进行多 次匹配，改变参数的值c = c+1，当满足 Figure CN105258702AD00083 时继续执行步骤4E，否则返回步骤4B，其 中Γ (X) (X > 〇)表示取不超过X的最大整数；

[0023] 步骤4E改变全局定位数据库中匹配点集的子区域，i = i+Ι，当i = η时继续执行 步骤4F，否则返回步骤4Α ;

[0024] 步骤4F共获得多组总的匹配误差E1, E2, · · ·，Ec

Figure CN105258702AD00084
取误差中最小 的两组EminA E min2作为移动机器人全局定位的数据，设此时对应的mini = (X i，Y1)，min2 =(x2，y2)，由于移动机器人在全局中的位姿只能在某一个子区域内，因此，该结果必须同时 满足条件

Figure CN105258702AD00085
其中I (X1)与d(X1)分别表示第 ^子区间所分割的小矩形的长度与宽度，当满足条件时继续执行步骤4G，否则让移动机器 人自动向前或者向后移动一定的距离，再返回步骤4A重新执行；

[0025] 步骤4G根据上述结果确定移动机器人在全局地图中的位置。根据第 X个子区域内采集的mxkx组点集Q )!与P h (c = Y1 U y 2)之间匹配可得到mxkx组 误差，将其按照从小到大顺序排列，分别取前三个最小的误差，设结果分别为

Figure CN105258702AD00086
>其对应的Qx中的六组匹配点集可 设为

Figure CN105258702AD00087
[0026] 步骤4H步骤4G中共得到的六个误差，同样按照从小到大的顺序排列，并取前三 个最小的误差，设得到对应误差为{>/，ε 2’，ε3′ }，对应的点集可分别设为{Qxl’， Qx/，QJ }，根据映射关系Φ，设三个点集对应的移动机器人在地图中的位姿分别为

Figure CN105258702AD00088
[0027] 步骤41设定方差阈值σ，计算三个误差的方差σ 3，如果满足σ 3< σ，则继续执 行步骤4J，否则去掉三个误差中最大的那个，然后再计算两个误差的方差〇2，如果满足 〇2 < σ，则继续执行步骤4Κ，否则去掉两个误差中最大的那个，然后继续执行步骤4L; [0028] 步骤4J假设最终有三个误差{ε/，ε2'，ε3' }，每个误差对应的匹配变换矩 阵分别， Figure CN105258702AD00091 表示移动机器人丢失位姿后保存的位姿，首先根据 R(cS)变换其位姿为： Figure CN105258702AD00092 [0030] 其中X。与y。不变，然后根据匹配变换矩阵计算移动机器人三个全局地图的位姿可 表示如下； Figure CN105258702AD00093 [0033] 计算三个权重 Figure CN105258702AD00094 根据权重可最终确定移动机 器人全局定位的位姿可表示如下： Figure CN105258702AD00095 [0035] 步骤4K假设最终有两个误差{ε/，ε2'，}，每个误差对应的匹配变换矩阵分 别为 Figure CN105258702AD00096 表示移动机器人丢失位姿后保存的位姿，首先根据 R(cS)变换其位姿为： Figure CN105258702AD00097 [0037] 其中X。与y。不变，然后根据匹配变换矩阵计算移动机器人两个全局地图的位姿可 表示如下； Figure CN105258702AD00098 [0040] 计算两个权重 Figure CN105258702AD00099 ，根据权重可最终确定移动机器 人全局定位的位姿可表示如下： Figure CN105258702AD000910 [0042] 步骤4L假设最只有误差ε/，，对应的匹配变换矩阵分别为 Θ。）表示移动机器人丢失位姿后保存的位姿，首先根据R(c δ )变换其位姿为： [0043] θ 〇1= θ 〇+ δ C1 [0044] 其中X。与y。不变，然后根据匹配变换矩阵计算移动机器人全局地图的位姿可表示 如下； Figure CN105258702AD00101 [0047] 计算权重CO1= 1，根据权重可最终确定移动机器人全局定位的位姿可表示如下： Figure CN105258702AD00102 [0049] 进一步的，步骤1中移动机器人自带的传感器为激光传感器或视觉传感器。 [0050] 进一步的，所述方法步骤41中方差阈值〇根据下述公式确定： [0051] σ = (2 ~5) Xmin({ ε /，ε 2'，ε 3' })。 [0052] 本发明的有益效果在于：本发明在实现基于SLAM导航移动机器人的全局定位的 基础上，使得方案更为可行可靠、成本更低、操作简单易于实现。该方法只是通过软件实现 的一段程序，且进行了自动、智能优化的判断分析，结合简单的人工操作，即可完成本方案 的实现与布局。由于该方法采用了构建地图过程中获取的原始数据，并进行不断的更新，可 采用ICP (迭代最近点算法）匹配的方法，根据匹配的效果综合分析，以确定移动机器人在 全局地图中的位姿，易于实现、定位准确、速度快。本发明不需要对环境进行改造，实现简单 可行，易于操作，降低了产品成本，提高了生产效率。本发明的方法可适用各种移动机器人， 尤其是AGV类型的移动机器人，只需移动机器人自带的传感器可获得数据实现SLAM导航即 可。 附图说明 [0053] 图1为本发明一种基于SLAM导航移动机器人的全局定位方法的流程图； [0054] 图2为具体实施方式中AGV在任一子区域获取的激光数据与数据库中的数据，在 基于ICP全局定位匹配前位姿状态示意图； [0055] 图3为具体实施方式中AGV在任一子区域获取的激光数据与数据库中的数据，在 基于ICP全局定位匹配后位姿状态示意图。 具体实施方式 [0056] 下面结合附图对本发明的具体实施方式进行说明： [0057] 以配备激光SLAM导航系统的自动导航叉车AGV为平台进行本发明基于激光SLAM 导航的AGV全局定位方法的具体操作，如图1所示的方法流程，包括以下步骤： [0058] 步骤1移动机器人应用环境的子区域的选取 [0059] 通过AGV自带的激光传感器，获取室内环境的数据信息，并通过SLAM技术，构建关 于室内环境完整的平面地图。设整个应用环境为Σ，在该环境下选取多个矩形的子区域，取 地图中位姿为零点所对应的环境区域为Σ。，其它的子区域分别为Σ i，Σ 2, . . .，Ση; [0060] 本发明选取应用环境中的多个子区域，将AGV置于其中任一子区域中，不需要重 启初始化，就可以实现自动全局定位，且人工移动的距离远远小于只有零点的子区域时所 移动的距离。假设AGV人工移动最大距离不超过D，那么任意相邻的两个子区域的几何中心 的距离都不应该超过2D，且应该满足2D > 1，1表示子区域沿AGV运行方向的长度。

[0061] 步骤2数据点的采集

[0062] 步骤1中在AGV应用环境中选取矩形的子区域，它的宽度和长度分别设为d、l，宽 度d表示子区域沿垂直于AGV运行方向的宽度，长度1表示子区域沿AGV运行方向的长度， 那么每个子区域宽度与长度分别表示为山，d2, . . .，dn，I1, 12, . . .，In，将第i个子区域分别 沿宽度与长度方向上分别等距分割为Hi1 A1等份，得到m ^k1个矩形，将AGV放在分割得到的 每个小矩形的几何中心，可获取沿前进或后退方向上激光传感器扫描数据Q 1, Q1包含m ^k1 组匹配数据，并保存在全局定位数据库中。每个子区域获取的数据都存在一个到AGV在构 建地图中位姿的–映射Φ :Qit- (X it，yit，Θ it) (1彡i彡n，1彡t彡IT^ki);

[0063] 设用于Delaunay迭代最近点算法匹配的函数为L : (P，Q) – (R，T)，该函数的输入 为待匹配的两个点集，定义P为目标点集，Q为模型点集，输出为两个点集进行匹配的旋转 矩阵R与平移向量T，旋转矩阵R(0)表示沿逆时针方向旋转θ (Θ >〇)，是2X2的矩阵， 而平移向量T表示2X 1的列向量，并定义每个点集中点的个数为N(Qi)。

[0064] 步骤3子区域选取合理与否的分析判断

[0065] 步骤3A设第i个子区域，第r组匹配数据，表示为I，初始值i = 1，r = 1，P表 示数据库中任意一组模型点集，匹配误差ε u表示与第i个子区域中第r组数据匹配的误 差，计算方法为：

Figure CN105258702AD00111
其中Pj、q.j分别表不点集P、Q ir 中的任一点坐标；

[0066] 步骤3B将Qu与数据库中已选取的所有子区域内的所有组数据进行匹配，获取对 应的匹配误差如下：

Figure CN105258702AD00112
[0069] 步骤3C当Es1中的任何一个元素的值都小于中任何一个元素值，r = r+Ι，再返 回步骤3B，否则退出循环，重新从应用环境中选取子区域Σ i。当r = n^kjl时，执行步骤 3D ；

[0070] 步骤3D改变变量i = i+1，r = 1，然后返回步骤3B，继续执行，直至i = n+1，退 出循环。

[0071] 步骤4基于ICP实现移动机器人全局定位：

[0072] 步骤4A将AGV置于应用环境中已选取的任一子区域乙i内，任何位置获取一组激 光传感器原始数据P，数据％表示在第i个子区域Σ i采集的数据。设在定位程序启动前 AGV在地图中的位姿为（xQ，y。，Θ。)，定义初始变量C = 〇,〇< δ < π/6,定义初始总误差 Eic= 0, i = 1 ; [0073] 步骤4B先将数据P进行处理得Ph= R(c δ )*P，然后通过定义的ICP匹配函数 L，与全局定位数据库中的数据Q1进行匹配得到Hijk1组变换矩阵（Rt，T t) =L(Ph，Qlt) (1彡t彡Hi1^k1)，其中Q lt表示第i个子区域采集的第t组匹配数据； [0074] 步骤4C根据每组变换矩阵先计算变换后的点集可表示为Pt= R t*Ph+Tt，然后计算 每组匹配数据的误差为 Figure CN105258702AD00121 其中P1^ 分别表示点集P t、 Qlt中的任一点坐标，在子区域Σ i内将所有组数据匹配一次得到总的匹配误差可表示为 Figure CN105258702AD00122 [0075] 步骤4D将子区域S1的数据库与激光传感器扫描的数据P在231弧度内进行多 次匹配，改变参数的值c = c+1，当满足 Figure CN105258702AD00123 时继续执行步骤4E，否则返回步骤4B，其 中Γ (X) (X > 〇)表示取不超过X的最大整数；

[0076] 步骤4E改变全局定位数据库中匹配点集的子区域，i = i+Ι，当i = η时继续执行 步骤4F，否则返回步骤4Α ;

[0077] 步骤4F共获得多组总的匹配误差

Figure CN105258702AD00124
取误差中最小 的两组Emin^ E min2作为AGV全局定位的数据，设此时对应的mini = (X i，Y1)，min2 = (χ2， y2)，由于AGV在全局中的位姿只能在某一个子区域内，因此，该结果必须同时满足条件X1 = X2= X，

Figure CN105258702AD00125
y其中I (X1)与Cl(X1)分别表示第X1子区间所分割 的小矩形沿AGV运行方向的长度与沿垂直于AGV运行方向的宽度，当满足条件时继续执行 步骤4G，否则让AGV自动向前或者向后移动一定的距离，再返回步骤4A重新执行；

[0078] 步骤4G根据上述结果确定AGV在全局地图中的位置。根据第X个 子区域内采集的mxk x组点集Q )!与P h (c = Y1 U y 2)之间匹配可得到mxkx组 误差，将其按照从小到大顺序排列，分别取前三个最小的误差，设结果分别为

Figure CN105258702AD00126
其对应的Qx中的六组匹配点集可

[0079] 步骤4H步骤4G中共得到的六个误差，同样按照从小到大的顺序排列，并取 前三个最小的误差，设得到对应误差为{> /，ε2’，ε3′ }，对应的点集可分别设为 {Qxl’，Qx2’，Qx3′ }，根据映射关系Φ，设三个点集对应的AGV在地图中的位姿分别为

Figure CN105258702AD00127
[0080] 步骤41设定方差阈值σ，方差阈值σ根据下述公式确定：

[0081] σ = (2 ~5) Xmin({ ε /，ε 2’，ε 3′ })。

[0082] 计算三个误差的方差σ 3，如果满足σ 3< σ，则继续执行步骤4J，否则去掉三个误 差中最大的那个，然后再计算两个误差的方差σ2，如果满足 〇2< σ，则继续执行步骤4K， 否则去掉两个误差中最大的那个，然后继续执行步骤4L。 [0083] 步骤4J假设最终有三个误差{ε/，ε2'，ε3' }，每个误差对应的匹配变换 矩阵分别为 Figure CN105258702AD00128 表示AGV丢失位姿后保存的位姿，首先根据 R(cS)变换其位姿为： Figure CN105258702AD00131 [0085] 其中x。与y。不变，然后根据匹配变换矩阵计算AGV三个全局地图的位姿可表示如 下； Figure CN105258702AD00132 [0088] 计算三个权重 Figure CN105258702AD00133 其中i = 1，2,3,根据权重可最终确定AGV全 局定位的位姿可表示如下： Figure CN105258702AD00134 [0090] 步骤4K假设最终有两个误差{ε/，ε2'，}，每个误差对应的匹配变换矩阵分别 为 Figure CN105258702AD00135 .表示AGV丢失位姿后保存的位姿，首先根据R(c δ )变 换其位姿为： Figure CN105258702AD00136 [0092] 其中X。与y。不变，然后根据匹配变换矩阵计算AGV两个全局地图的位姿可表示如 下； Figure CN105258702AD00137 [0095] 计算两个权重 Figure CN105258702AD00138 其中i = 1，2,根据权重可最终确定AGV全局 定位的位姿可表示如下： Figure CN105258702AD00139 [0097] 步骤4L假设最只有误差ε/，，对应的匹配变换矩阵分别为 Θ。）表示AGV丢失位姿后保存的位姿，首先根据R (c δ )变换其位姿为： [0098] θ 〇1= Θ 〇+ δ C1 [0099] 其中X。与y。不变，然后根据匹配变换矩阵计算AGV全局地图的位姿可表示如下； [0100] θ 1= θ 〇1+β 1 Figure CN105258702AD00141 [0102] 计算权重ω 1= 1，根据权重可最终确定AGV全局定位的位姿可表示如下： Figure CN105258702AD00142 [0104] 本具体实施方式使用的自动导航叉车AGV平台，AGV在任一子区域获取的激光数 据与数据库中的数据如图2所示，AGV在任一子区域获取的激光数据与数据库中的数据，在 基于ICP全局定位匹配后位姿状态如图3所示，图2及图3中加号与星号分别表示模型点 集与目标点集，通过图3与图2对比可以看出，采用基于激光SLAM导航AGV全局定位方法 即图3中模型点集与目标点集更为接近，定位更加精确。 [0105] 上述实施方式中使用的是自动导航叉车AGV平台，本发明也可以应用到其它各种 移动机器人如餐厅服务机器人和家用机器人等领域。只需移动机器人自带的传感器能够获 得数据进行SLAM导航即可，传感器可以是具体实施方式中的激光传感器，也可以是视觉传 感器，通过视觉传感器获得点云数据，使用相同的算法进行计算，实现全局定位，也可以应 用到其它类型的传感器。 [0106] 以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员 来说，在不脱离本发明所述原理的前提下，还可以作出若干改进和润饰，这些改进和润饰也 应视为本发明的保护范围。 权利要求(4) 1. 一种基于SLAM导航移动机器人的全局定位方法，其特征在于，所述方法包括以下步 骤： 步骤1移动机器人应用环境的子区域的选取 通过移动机器人自带的传感器，获取室内环境的数据信息，并通过SLAM技术，构建关 于室内环境完整的平面地图，设整个应用环境为Σ，在该环境下选取多个矩形的子区域，取 地图中位姿为零点所对应的环境区域为Σ。，其它的子区域分别为Σ1;Σ2，…，Ση; 假设移动机器人人工移动最大距离不超过D，那么任意相邻的两个子区域的几何中心 的距离都不应该超过2D，且应该满足2D>1，1表示子区域沿移动机器人运行方向的长度； 步骤2数据点的采集 步骤1中在移动机器人应用环境中选取矩形的子区域，它的宽度和长度分别设为d、l， 宽度d表示子区域沿垂直于移动机器人运行方向的宽度，长度1表示子区域沿移动机器人 运行方向的长度，那么每个子区域宽度与长度分别表示为山，d2，…，dn，11; 12，…，ln，将第i 个子区域分别沿宽度与长度方向上分别等距分割为mi、h等份，得到m 个矩形，将移动 机器人放在分割得到的每个小矩形的几何中心，可获取沿前进或后退方向上移动机器人自 带传感器扫描数据％，Qi包含mjki组匹配数据，并保存在全局定位数据库中，每个子区域 获取的数据都存在一个到移动机器人在构建地图中位姿的一一映射Φ:Qlt- (xlt，ylt，Θlt) (l^i^n, m^kj)； 设用于Delaunay迭代最近点算法匹配的函数为L:(P，Q) – (R，T)，该函数的输入为待 匹配的两个点集，定义P为目标点集，Q为模型点集，输出为两个点集进行匹配的旋转矩阵 R与平移向量T，旋转矩阵R(0)表示沿逆时针方向旋转θ(θ>〇)，是2X2的矩阵，而平移 向量Τ表示2Χ1的列向量，并定义每个点集中点的个数为N(Qi); 步骤3子区域选取合理与否的分析判断 步骤3A设第i个子区域，第r组匹配数据，表示为I，初始值i= 1，r= 1，P表示数 据库中任意一组模型点集，匹配误差εu表示与第i个子区域中第r组数据匹配的误差， ε 3十算方法为
Figure CN105258702AC00021
实中p.j、q.j分别表示点集P、Q中 的任一点坐标； 步骤3B将Qu与数据库中已选取的所有子区域内的所有组数据进行匹配，获取对应的 匹配误差如下：
Figure CN105258702AC00022
步骤3C当4中的任何一个元素的值都小于_£|中任何一个元素值，r=r+Ι，再返回步 骤3B，否则退出循环，重新从应用环境中选取子区域Σi，当r =叫*1^+1时，执行步骤3D; 步骤3D改变变量i=i+1，r= 1，然后返回步骤3B，继续执行，直至i=n+l，退出循 环； 步骤4基于ICP实现移动机器人全局定位 步骤4A将移动机器人置于应用环境中已选取的任一子区域，任何位置获取一组 传感器原始数据P，数据％表示在第i个子区域Σi采集的数据，设在定位程序启动前移动 机器人在地图中的位姿为（X。，y。，Θ。)，定义初始变量c = 〇, 〇〈δ〈π/6,定义初始总误差Em =0,i= 1 ; 步骤4B先将数据P进行处理得Ph=R(cδ)*P，然后通过定义的ICP匹配函数L， 与全局定位数据库中的数据4进行匹配得到组变换矩阵（Rt，Tt) =L(Ph，Qlt) (1彡t彡mjlO,其中Qlt表示第i个子区域采集的第t组匹配数据； 步骤4C根据每组变换矩阵先计算变换后的点集可表示为Pt=Rt*Ph+Tt，然后计算 每组匹配数据的误差戈
Figure CN105258702AC00031
其中Pp分别表示点集 Pt、Qlt中的任一点，在子区域Σi内将所有组数据匹配一次得到总的匹配误差可表示为
Figure CN105258702AC00032
步骤4D将子区域21的数据库与激光传感器扫描的数据P在2π弧度内进行多次匹 配，改变参数的值c=c+1，当满足X=iXf)时继续执行步骤4E，否则返回步骤4B，其中 Γ(X) (χ>〇)表示取不超过X的最大整数； 步骤4Ε改变全局定位数据库中匹配点集的子区域，i=i+Ι，当i=η时继续执行步 骤4F，否则返回步骤4Α; 步骤4F共获得多组总的匹配误差E2,…,Uf=1 心，取误差中最小的 两组£_1与Emin2作为移动机器人全局定位的数据，设此时对应的mini=(Xd，mim2 = (x2,y2)，由于移动机器人在全局中的位姿只能在某一个子区域内，因此，该结果必须同时满 足条件Xi=x2=x
Figure CN105258702AC00033
其中1 (X》与dUi)分别表示第x# 区间所分割的小矩形的长度与宽度，当满足条件时继续执行步骤4G，否则让移动机器人自 动向前或者向后移动一定的距离，再返回步骤4A重新执行； 步骤4G根据上述结果确定移动机器人在全局地图中的位置，根据第X个 子区域内采集的mxkx组点集Q)!与Ph(c=yiUy2)之间匹配可得到mxkx组 误差，将其按照从小到大顺序排列，分别取前三个最小的误差，设结果分别为
Figure CN105258702AC00034
，其对应的1中的六组匹配点集可设 为1
Figure CN105258702AC00035
步骤4H步骤4G中共得到的六个误差，同样按照从小到大的顺序排列，并取前 三个最小的误差，设得到对应误差为{>/，ε2’，ε3′ }，对应的点集可分别设为 }，根据映射关系Φ，设三个点集对应的移动机器人在地图中的位姿分 别为為）： 步骤41设定方差阈值〇，计算三个误差的方差〇2，如果满足 〇3〈〇，则继续执行步骤 4J，否则去掉三个误差中最大的那个，然后再计算两个误差的方差〇1，如果满足〇1〈 〇,则 继续执行步骤4Κ，否则去掉两个误差中最大的那个，然后继续执行步骤4L; 步骤4J假设最终有三个误差{ε/，ε2’，ε3′ }，每个误差对应的匹配变换矩阵 分别为UU/?〇^//^(X(]，y。，Θ。）表示移动机器人丢失位姿后保存的位姿，首先根据 R(cS)变换其位姿为：
Figure CN105258702AC00041
其中\与7。不变，然后根据匹配变换矩阵计算移动机器人三个全局地图的位姿可表示 如下； [01 9 2 Θ3]Τ=[Θ 01 9 02 θ〇3]Τ+[βΐ β2 β3]Τ
Figure CN105258702AC00042
计算三个权重(¾ = 其中i= 1，2,3,根据权重可最终确定移动机器人 全局定位的位姿可表示如下：
Figure CN105258702AC00043
步骤4K假设最终有两个误差{ε/，ε2’，}，每个误差对应的匹配变换矩阵分别为U?=1〇R(/?m(xD，yD，Θ。）表示移动机器人丢失位姿后保存的位姿，首先根据R(cS) 变换其位姿为：
Figure CN105258702AC00044
其中X。与y。不变，然后根据匹配变换矩阵计算移动机器人两个全局地图的位姿可表示 如下； [θ,ΘJT=[θη1θη,]τ+[β,βJT
Figure CN105258702AC00045
计算两个权重：
Figure CN105258702AC00046
其中i= 1，2,根据权重可最终确定移动机器人全 局定位的位姿可表示如下：
Figure CN105258702AC00047
步骤4L假设最只有误差ε/，，对应的匹配变换矩阵分别为（RWH)，（X(]，y。，Θ。） 表示移动机器人丢失位姿后保存的位姿，首先根据R(cδ)变换其位姿为： Θ 〇1= Θ 〇+ δ 其中X。与y。不变，然后根据匹配变换矩阵计算移动机器人全局地图的位姿可表示如 下； 9 1= 9 01+β1
Figure CN105258702AC00051
计算权重ω1= 1，根据权重可最终确定移动机器人全局定位的位姿可表示如下：
Figure CN105258702AC00052
2. 如权利要求1所述的基于SLAM导航移动机器人的全局定位方法，其特征在于，所述 移动机器人为AGV。
3. 如权利要求1所述的基于SLAM导航移动机器人的全局定位方法，其特征在于，所述 方法步骤1中移动机器人自带传感器为激光传感器或视觉传感器。
4. 如权利要求1所述的基于SLAM导航移动机器人的全局定位方法，其特征在于，所述 方法步骤41中方差阈值〇根据下述公式确定： σ = (2~SAminGe/，ε2’，ε3′ })0