一种基于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组数据匹配的误 差，计算方法为：
I其中Pj、q.j分别表不点集P、Q ir 中的任一点坐标；
[0013] 步骤3B将Qu与数据库中已选取的所有子区域内的所有组数据进行匹配，获取对 应的匹配误差如下：
[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，然后计算 每组匹配数据的误差为
其中P1^ 分别表示点集P t、 Qlt中的任一点坐标，在子区域Σ i内将所有组数据匹配一次得到总的匹配误差可表示为
[0022] 步骤4D将子区域乙i的数据库与激光传感器扫描的数据P在2 π弧度内进行多 次匹配，改变参数的值c = c+1，当满足
时继续执行步骤4E，否则返回步骤4B，其 中Γ (X) (X > 〇)表示取不超过X的最大整数；
[0023] 步骤4E改变全局定位数据库中匹配点集的子区域，i = i+Ι，当i = η时继续执行 步骤4F，否则返回步骤4Α ;
[0024] 步骤4F共获得多组总的匹配误差E1, E2, · · ·，Ec
取误差中最小 的两组EminA E min2作为移动机器人全局定位的数据，设此时对应的mini =