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

Abstract

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

Description

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

技术领域

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

背景技术

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

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

发明内容

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

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

通过移动机器人自带传感器，获取室内环境的数据信息，并通过SLAM技术，构建关于室内环境完整的平面地图。设整个应用环境为∑，在该环境下选取多个矩形的子区域，取地图中位姿为零点所对应的环境区域为∑₀，其它的子区域分别为∑₁，∑₂，...，∑_n；

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

步骤2数据点的采集

步骤1中在移动机器人应用环境中选取矩形的子区域，它的宽度和长度分别设为d、l，宽度d表示子区域沿垂直于移动机器人运行方向的宽度，长度l表示子区域沿移动机器人运行方向的长度，那么每个子区域宽度与长度分别表示为d₁，d₂，...，d_n，l₁，l₂，...，l_n，将第i个子区域分别沿宽度与长度方向上分别等距分割为m_i、k_i等份，得到m_i*k_i个矩形，将移动机器人放在分割得到的每个小矩形的几何中心，可获取沿前进或后退方向上移动机器人自带传感器扫描数据Q_i，Q_i包含m_i*k_i组匹配数据，并保存在全局定位数据库中。每个子区域获取的数据都存在一个到移动机器人在构建地图中位姿的一一映射ψ：Q_it→(x_it，y_it，θ_it)(1≤i≤n，1≤t≤m_i*k_i)；

设用于Delaunay迭代最近点算法匹配的函数为L：(P，Q)→(R，T)，该函数的输入为待匹配的两个点集，定义P为目标点集，Q为模型点集，输出为两个点集进行匹配的旋转矩阵R与平移向量T，旋转矩阵R(θ)表示沿逆时针方向旋转θ(θ＞0)，是2×2的矩阵，而平移向量T表示2×1的列向量，并定义每个点集中点的个数为N(Q_i)；

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

步骤3A设第i个子区域，第r组匹配数据，表示为Q_ir，初始值i＝1，r＝1，P表示数据库中任意一组模型点集，匹配误差ε_ir表示与第i个子区域中第r组数据匹配的误差，ε_ir计算方法为：其中p_j、q_j分别表示点集P、Q_ir中的任一点坐标；

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

${\mathrm{\ϵ}}_s^1=\{{\mathrm{\ϵ}}_{s1},{\mathrm{\ϵ}}_{s2},...,{\mathrm{\ϵ}}_{s(m_s*k_s)}\}(s=i)$

步骤3C当中的任何一个元素的值都小于中任何一个元素值，r＝r+1，再返回步骤3B，否则退出循环，重新从应用环境中选取子区域∑_i。当r＝m_i*k_i+1时，执行步骤3D；

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

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

步骤4A将移动机器人置于应用环境中已选取的任一子区域∑_i内，任何位置获取一组激光传感器原始数据P，数据Q_i表示在第i个子区域∑_i采集的数据。设在定位程序启动前移动机器人在地图中的位姿为(x₀，y₀，θ₀)，定义初始变量c＝0，0＜δ＜π/6，定义初始总误差E_ic＝0，i＝1；

步骤4B先将数据P进行处理得P_h＝R(cδ)*P，然后通过定义的ICP匹配函数L，与全局定位数据库中的数据Q_i进行匹配得到m_i*k_i组变换矩阵(R_t，T_t)＝L(P_h，Q_it)(1≤t≤m_i*k_i)，其中Q_it表示第i个子区域采集的第t组匹配数据；

步骤4C根据每组变换矩阵先计算变换后的点集可表示为P_t＝R_t*P_h+T_t，然后计算每组匹配数据的误差为其中p_r、q_r分别表示点集P_t、Q_it中的任一点坐标，在子区域∑_i内将所有组数据匹配一次得到总的匹配误差可表示为

步骤4D将子区域∑_i的数据库与激光传感器扫描的数据P在2π弧度内进行多次匹配，改变参数的值c＝c+1，当满足时继续执行步骤4E，否则返回步骤4B，其中Γ(x)(x＞0)表示取不超过x的最大整数；

步骤4E改变全局定位数据库中匹配点集的子区域，i＝i+1，当i＝n时继续执行步骤4F，否则返回步骤4A；

步骤4F共获得多组总的匹配误差E₁，E₂，...，E_c 取误差中最小的两组E_min1与E_min2作为移动机器人全局定位的数据，设此时对应的min1＝(x₁，y₁)，min2＝(x₂，y₂)，由于移动机器人在全局中的位姿只能在某一个子区域内，因此，该结果必须同时满足条件 其中l(x₁)与d(x₁)分别表示第x₁子区间所分割的小矩形的长度与宽度，当满足条件时继续执行步骤4G，否则让移动机器人自动向前或者向后移动一定的距离，再返回步骤4A重新执行；

步骤4G根据上述结果确定移动机器人在全局地图中的位置。根据第x个子区域内采集的m_xk_x组点集Q_x与P_h(c＝y₁∪y₂)之间匹配可得到m_xk_x组误差，将其按照从小到大顺序排列，分别取前三个最小的误差，设结果分别为 ${\mathrm{\ϵ}}_{{\mathrm{xy}}_1}=\{{\mathrm{\ϵ}}_{y_{1}1},{\mathrm{\ϵ}}_{y_{1}2},{\mathrm{\ϵ}}_{y_{1}3}\},{\mathrm{\ϵ}}_{{\mathrm{xy}}_2}=\{{\mathrm{\ϵ}}_{y_{2}1},{\mathrm{\ϵ}}_{y_{2}2},{\mathrm{\ϵ}}_{y_{2}3}\},$ 其对应的Q_x中的六组匹配点集可设为 $\{Q_{{\mathrm{xy}}_{1}1},Q_{{\mathrm{xy}}_{1}2},Q_{{\mathrm{xy}}_{1}3},Q_{{\mathrm{xy}}_{2}1},Q_{{\mathrm{xy}}_{2}2},Q_{{\mathrm{xy}}_{2}3}\};$

步骤4H步骤4G中共得到的六个误差，同样按照从小到大的顺序排列，并取前三个最小的误差，设得到对应误差为{ε₁′，ε₂′，ε₃′}，对应的点集可分别设为{Q_x1′，Q_x2′，Q_x3′}，根据映射关系ψ，设三个点集对应的移动机器人在地图中的位姿分别为

步骤4I设定方差阈值σ，计算三个误差的方差σ₃，如果满足σ₃＜σ，则继续执行步骤4J，否则去掉三个误差中最大的那个，然后再计算两个误差的方差σ₂，如果满足σ₂＜σ，则继续执行步骤4K，否则去掉两个误差中最大的那个，然后继续执行步骤4L；

步骤4J假设最终有三个误差{ε₁′，ε₂′，ε₃′}，每个误差对应的匹配变换矩阵分别为(x₀，y₀，θ₀)表示移动机器人丢失位姿后保存的位姿，首先根据R(cδ)变换其位姿为：

$\left[\begin{array}{c}{\mathrm{\θ}}_{01}\\ {\mathrm{\θ}}_{02}\\ {\mathrm{\θ}}_{03}\end{array}\right]={\mathrm{\θ}}_0+\mathrm{\δ}\left[\begin{array}{c}{C}_1\\ C_2\\ C_3\end{array}\right]$

其中x₀与y₀不变，然后根据匹配变换矩阵计算移动机器人三个全局地图的位姿可表示如下；

[θ₁θ₂θ₃]^T＝[θ₀₁θ₀₂θ₀₃]^T+[β₁β₂β₃]^T

$\left[\begin{array}{cc}{x}_1& y_1\\ x_2& y_2\\ x_3& y_3\end{array}\right]=\left[\begin{array}{cc}{x}_0& y_0\\ x_0& y_0\\ x_0& y_0\end{array}\right]+\left[\begin{array}{cc}{T}_1\left(1\right)& T_1\left(2\right)\\ T_2\left(1\right)& T_2\left(2\right)\\ T_3\left(1\right)& T_3\left(2\right)\end{array}\right]$

计算三个权重其中i＝1，2，3，根据权重可最终确定移动机器人全局定位的位姿可表示如下：

$\left[\begin{array}{c}x\\ y\\ \mathrm{\θ}\end{array}\right]=\left[\begin{array}{ccc}{x}_1& x_2& x_3\\ y_1& y_2& y_3\\ {\mathrm{\θ}}_1& {\mathrm{\θ}}_2& {\mathrm{\θ}}_3\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_1\\ {\mathrm{\ω}}_2\\ {\mathrm{\ω}}_3\end{array}\right]$

步骤4K假设最终有两个误差{ε₁′，ε₂′，}，每个误差对应的匹配变换矩阵分别为表示移动机器人丢失位姿后保存的位姿，首先根据R(cδ)变换其位姿为：

$\left[\begin{array}{c}{\mathrm{\θ}}_{01}\\ {\mathrm{\θ}}_{02}\end{array}\right]={\mathrm{\θ}}_0+\mathrm{\δ}\left[\begin{array}{c}{C}_1\\ C_2\end{array}\right]$

其中x₀与y₀不变，然后根据匹配变换矩阵计算移动机器人两个全局地图的位姿可表示如下；

[θ₁θ₂]^T＝[θ₀₁θ₀₂]^T+[β₁β₂]^T

$\left[\begin{array}{cc}{x}_1& y_1\\ x_2& y_2\end{array}\right]=\left[\begin{array}{cc}{x}_0& y_0\\ x_0& y_0\end{array}\right]+\left[\begin{array}{cc}{T}_1\left(1\right)& T_1\left(2\right)\\ T_2\left(1\right)& T_2\left(2\right)\end{array}\right]$

计算两个权重其中i＝1，2，根据权重可最终确定移动机器人全局定位的位姿可表示如下：

$\left[\begin{array}{c}x\\ y\\ \mathrm{\θ}\end{array}\right]=\left[\begin{array}{cc}{x}_1& x_2\\ y_1& y_2\\ {\mathrm{\θ}}_1& {\mathrm{\θ}}_2\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_1\\ {\mathrm{\ω}}_2\end{array}\right]$

步骤4L假设最只有误差ε₁′，，对应的匹配变换矩阵分别为(R(β_l)，T_l)，(x₀，y₀，θ₀)表示移动机器人丢失位姿后保存的位姿，首先根据R(cδ)变换其位姿为：

θ₀₁＝θ₀+δc₁

其中x₀与y₀不变，然后根据匹配变换矩阵计算移动机器人全局地图的位姿可表示如下；

θ₁＝θ₀₁+β₁

$\left[\begin{array}{c}{x}_1\\ y_1\end{array}\right]=\left[\begin{array}{c}{x}_0\\ y_0\end{array}\right]+\left[\begin{array}{c}{T}_1\left(1\right)\\ T_1\left(2\right)\end{array}\right]$

计算权重ω₁＝1，根据权重可最终确定移动机器人全局定位的位姿可表示如下：

$\left[\begin{array}{c}x\\ y\\ \mathrm{\θ}\end{array}\right]={\mathrm{\ω}}_1\left[\begin{array}{c}{x}_1\\ y_1\\ {\mathrm{\θ}}_1\end{array}\right].$

进一步的，步骤1中移动机器人自带的传感器为激光传感器或视觉传感器。

进一步的，所述方法步骤4I中方差阈值σ根据下述公式确定：

σ＝(2～5)×min({ε₁′，ε₂′，ε₃′})。

本发明的有益效果在于：本发明在实现基于SLAM导航移动机器人的全局定位的基础上，使得方案更为可行可靠、成本更低、操作简单易于实现。该方法只是通过软件实现的一段程序，且进行了自动、智能优化的判断分析，结合简单的人工操作，即可完成本方案的实现与布局。由于该方法采用了构建地图过程中获取的原始数据，并进行不断的更新，可采用ICP(迭代最近点算法)匹配的方法，根据匹配的效果综合分析，以确定移动机器人在全局地图中的位姿，易于实现、定位准确、速度快。本发明不需要对环境进行改造，实现简单可行，易于操作，降低了产品成本，提高了生产效率。本发明的方法可适用各种移动机器人，尤其是AGV类型的移动机器人，只需移动机器人自带的传感器可获得数据实现SLAM导航即可。

附图说明

图1为本发明一种基于SLAM导航移动机器人的全局定位方法的流程图；

图2为具体实施方式中AGV在任一子区域获取的激光数据与数据库中的数据，在基于ICP全局定位匹配前位姿状态示意图；

图3为具体实施方式中AGV在任一子区域获取的激光数据与数据库中的数据，在基于ICP全局定位匹配后位姿状态示意图。

具体实施方式

下面结合附图对本发明的具体实施方式进行说明：

以配备激光SLAM导航系统的自动导航叉车AGV为平台进行本发明基于激光SLAM导航的AGV全局定位方法的具体操作，如图1所示的方法流程，包括以下步骤：

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

通过AGV自带的激光传感器，获取室内环境的数据信息，并通过SLAM技术，构建关于室内环境完整的平面地图。设整个应用环境为∑，在该环境下选取多个矩形的子区域，取地图中位姿为零点所对应的环境区域为Σ₀，其它的子区域分别为∑₁，∑₂，...，Σ_n；

本发明选取应用环境中的多个子区域，将AGV置于其中任一子区域中，不需要重启初始化，就可以实现自动全局定位，且人工移动的距离远远小于只有零点的子区域时所移动的距离。假设AGV人工移动最大距离不超过D，那么任意相邻的两个子区域的几何中心的距离都不应该超过2D，且应该满足2D＞l，l表示子区域沿AGV运行方向的长度。

步骤2数据点的采集

步骤1中在AGV应用环境中选取矩形的子区域，它的宽度和长度分别设为d、l，宽度d表示子区域沿垂直于AGV运行方向的宽度，长度l表示子区域沿AGV运行方向的长度，那么每个子区域宽度与长度分别表示为d₁，d₂，...，d_n，l₁，l₂，...，l_n，将第i个子区域分别沿宽度与长度方向上分别等距分割为m_i、k_i等份，得到m_i*k_i个矩形，将AGV放在分割得到的每个小矩形的几何中心，可获取沿前进或后退方向上激光传感器扫描数据Q_i，Q_i包含m_i*k_i组匹配数据，并保存在全局定位数据库中。每个子区域获取的数据都存在一个到AGV在构建地图中位姿的一一映射ψ：Q_it→(x_it，y_it，θ_it)(1≤i≤n，1≤t≤m_i*k_i)；

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

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

步骤3A设第i个子区域，第r组匹配数据，表示为Q_ir，初始值i＝1，r＝1，P表示数据库中任意一组模型点集，匹配误差ε_ir表示与第i个子区域中第r组数据匹配的误差，ε_ir计算方法为：其中p_j、q_j分别表示点集P、Q_ir中的任一点坐标；

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

${\mathrm{\ϵ}}_s^1=\{{\mathrm{\ϵ}}_{s1},{\mathrm{\ϵ}}_{s2},...,{\mathrm{\ϵ}}_{s(m_s*k_s)}\}(s=i)$

步骤3C当中的任何一个元素的值都小于中任何一个元素值，r＝r+1，再返回步骤3B，否则退出循环，重新从应用环境中选取子区域Σ_i。当r＝m_i*k_i+1时，执行步骤3D；

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

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

步骤4A将AGV置于应用环境中已选取的任一子区域∑_i内，任何位置获取一组激光传感器原始数据P，数据Q_i表示在第i个子区域Σ_i采集的数据。设在定位程序启动前AGV在地图中的位姿为(x₀，y₀，θ₀)，定义初始变量c＝0，0＜δ＜π/6，定义初始总误差E_ic＝0，i＝1；

步骤4B先将数据P进行处理得P_h＝R(cδ)*P，然后通过定义的ICP匹配函数L，与全局定位数据库中的数据Q_i进行匹配得到m_i*k_i组变换矩阵(R_t，T_t)＝L(P_h，Q_it)(1≤t≤m_i*k_i)，其中Q_it表示第i个子区域采集的第t组匹配数据；

步骤4C根据每组变换矩阵先计算变换后的点集可表示为P_t＝R_t*P_h+T_t，然后计算每组匹配数据的误差为其中p_r、q_r分别表示点集P_t、Q_it中的任一点坐标，在子区域Σ_i内将所有组数据匹配一次得到总的匹配误差可表示为

步骤4D将子区域Σ_i的数据库与激光传感器扫描的数据P在2π弧度内进行多次匹配，改变参数的值c＝c+1，当满足时继续执行步骤4E，否则返回步骤4B，其中Γ(x)(x＞0)表示取不超过x的最大整数；

步骤4E改变全局定位数据库中匹配点集的子区域，i＝i+1，当i＝n时继续执行步骤4F，否则返回步骤4A；

步骤4F共获得多组总的匹配误差取误差中最小的两组E_min1与E_min2作为AGV全局定位的数据，设此时对应的min1＝(x₁，y₁)，min2＝(x₂，y₂)，由于AGV在全局中的位姿只能在某一个子区域内，因此，该结果必须同时满足条件x₁＝x₂＝x，其中l(x₁)与d(x₁)分别表示第x₁子区间所分割的小矩形沿AGV运行方向的长度与沿垂直于AGV运行方向的宽度，当满足条件时继续执行步骤4G，否则让AGV自动向前或者向后移动一定的距离，再返回步骤4A重新执行；

步骤4G根据上述结果确定AGV在全局地图中的位置。根据第x个子区域内采集的m_xk_x组点集Q_x与P_h(c＝y₁∪y₂)之间匹配可得到m_xk_x组误差，将其按照从小到大顺序排列，分别取前三个最小的误差，设结果分别为 ${\mathrm{\ϵ}}_{{\mathrm{xy}}_1}=\{{\mathrm{\ϵ}}_{y_{1}1},{\mathrm{\ϵ}}_{y_{1}2},{\mathrm{\ϵ}}_{y_{1}3}\},{\mathrm{\ϵ}}_{{\mathrm{xy}}_2}=\{{\mathrm{\ϵ}}_{y_{2}1},{\mathrm{\ϵ}}_{y_{2}2},{\mathrm{\ϵ}}_{y_{2}3}\},$ 其对应的Q_x中的六组匹配点集可设为 $\{Q_{{\mathrm{xy}}_{1}1},Q_{{\mathrm{xy}}_{1}2},Q_{{\mathrm{xy}}_{1}3},Q_{{\mathrm{xy}}_{2}1},Q_{{\mathrm{xy}}_{2}2},Q_{{\mathrm{xy}}_{2}3}\};$

步骤4H步骤4G中共得到的六个误差，同样按照从小到大的顺序排列，并取前三个最小的误差，设得到对应误差为{ε₁′，ε₂′，ε₃′}，对应的点集可分别设为{Q_x1′，Q_x2′，Q_x3′}，根据映射关系ψ，设三个点集对应的AGV在地图中的位姿分别为

步骤4I设定方差阈值σ，方差阈值σ根据下述公式确定：

σ＝(2～5)×min({ε₁′，ε₂′，ε₃′})。

计算三个误差的方差σ₃，如果满足σ₃＜σ，则继续执行步骤4J，否则去掉三个误差中最大的那个，然后再计算两个误差的方差σ₂，如果满足σ₂＜σ，则继续执行步骤4K，否则去掉两个误差中最大的那个，然后继续执行步骤4L。

步骤4J假设最终有三个误差{ε₁′，ε₂′，ε₃′}，每个误差对应的匹配变换矩阵分别为表示AGV丢失位姿后保存的位姿，首先根据R(cδ)变换其位姿为：

$\left[\begin{array}{c}{\mathrm{\θ}}_{01}\\ {\mathrm{\θ}}_{02}\\ {\mathrm{\θ}}_{03}\end{array}\right]={\mathrm{\θ}}_0+\mathrm{\δ}\left[\begin{array}{c}{C}_1\\ C_2\\ C_3\end{array}\right]$

其中x₀与y₀不变，然后根据匹配变换矩阵计算AGV三个全局地图的位姿可表示如下；

[θ₁θ₂θ₃]^T＝[θ₀₁θ₀₂θ₀₃]^T+[β₁β₂β₃]^T

$\left[\begin{array}{cc}{x}_1& y_1\\ x_2& y_2\\ x_3& y_3\end{array}\right]=\left[\begin{array}{cc}{x}_0& y_0\\ x_0& y_0\\ x_0& y_0\end{array}\right]+\left[\begin{array}{cc}{T}_1\left(1\right)& T_1\left(2\right)\\ T_2\left(1\right)& T_2\left(2\right)\\ T_3\left(1\right)& T_3\left(2\right)\end{array}\right]$

计算三个权重其中i＝1，2，3，根据权重可最终确定AGV全局定位的位姿可表示如下：

$\left[\begin{array}{c}x\\ y\\ \mathrm{\θ}\end{array}\right]=\left[\begin{array}{ccc}{x}_1& x_2& x_3\\ y_1& y_2& y_3\\ {\mathrm{\θ}}_1& {\mathrm{\θ}}_2& {\mathrm{\θ}}_3\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_1\\ {\mathrm{\ω}}_2\\ {\mathrm{\ω}}_3\end{array}\right]$

步骤4K假设最终有两个误差{ε₁′，ε₂′，}，每个误差对应的匹配变换矩阵分别为表示AGV丢失位姿后保存的位姿，首先根据R(cδ)变换其位姿为：

$\left[\begin{array}{c}{\mathrm{\θ}}_{01}\\ {\mathrm{\θ}}_{02}\end{array}\right]={\mathrm{\θ}}_0+\mathrm{\δ}\left[\begin{array}{c}{C}_1\\ C_2\end{array}\right]$

其中x₀与y₀不变，然后根据匹配变换矩阵计算AGV两个全局地图的位姿可表示如下；

[θ₁θ₂]^T＝[θ₀₁θ₀₂]^T+[β₁β₂]^T

$\left[\begin{array}{cc}{x}_1& y_1\\ x_2& y_2\end{array}\right]=\left[\begin{array}{cc}{x}_0& y_0\\ x_0& y_0\end{array}\right]+\left[\begin{array}{cc}{T}_1\left(1\right)& T_1\left(2\right)\\ T_2\left(1\right)& T_2\left(2\right)\end{array}\right]$

计算两个权重其中i＝1，2，根据权重可最终确定AGV全局定位的位姿可表示如下：

$\left[\begin{array}{c}x\\ y\\ \mathrm{\θ}\end{array}\right]=\left[\begin{array}{cc}{x}_1& x_2\\ y_1& y_2\\ {\mathrm{\θ}}_1& {\mathrm{\θ}}_2\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_1\\ {\mathrm{\ω}}_2\end{array}\right]$

步骤4L假设最只有误差ε₁′，，对应的匹配变换矩阵分别为(R(β_l)，T_l)，(x₀，y₀，θ₀)表示AGV丢失位姿后保存的位姿，首先根据R(cδ)变换其位姿为：

θ₀₁＝θ₀+δc₁

其中x₀与y₀不变，然后根据匹配变换矩阵计算AGV全局地图的位姿可表示如下；

θ₁＝θ₀₁+β₁

$\left[\begin{array}{c}{x}_1\\ y_1\end{array}\right]=\left[\begin{array}{c}{x}_0\\ y_0\end{array}\right]+\left[\begin{array}{c}{T}_1\left(1\right)\\ T_1\left(2\right)\end{array}\right]$

计算权重ω₁＝1，根据权重可最终确定AGV全局定位的位姿可表示如下：

$\left[\begin{array}{c}x\\ y\\ \mathrm{\θ}\end{array}\right]={\mathrm{\ω}}_1\left[\begin{array}{c}{x}_1\\ y_1\\ {\mathrm{\θ}}_1\end{array}\right]$

本具体实施方式使用的自动导航叉车AGV平台，AGV在任一子区域获取的激光数据与数据库中的数据如图2所示，AGV在任一子区域获取的激光数据与数据库中的数据，在基于ICP全局定位匹配后位姿状态如图3所示，图2及图3中加号与星号分别表示模型点集与目标点集，通过图3与图2对比可以看出，采用基于激光SLAM导航AGV全局定位方法即图3中模型点集与目标点集更为接近，定位更加精确。

上述实施方式中使用的是自动导航叉车AGV平台，本发明也可以应用到其它各种移动机器人如餐厅服务机器人和家用机器人等领域。只需移动机器人自带的传感器能够获得数据进行SLAM导航即可，传感器可以是具体实施方式中的激光传感器，也可以是视觉传感器，通过视觉传感器获得点云数据，使用相同的算法进行计算，实现全局定位，也可以应用到其它类型的传感器。

以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明所述原理的前提下，还可以作出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (4)

1.一种基于SLAM导航移动机器人的全局定位方法，其特征在于，所述方法包括以下步骤：

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

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

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

步骤2数据点的采集

步骤1中在移动机器人应用环境中选取矩形的子区域，它的宽度和长度分别设为d、l，宽度d表示子区域沿垂直于移动机器人运行方向的宽度，长度l表示子区域沿移动机器人运行方向的长度，那么每个子区域宽度与长度分别表示为d₁,d₂,…,d_n，l₁,l₂,…,l_n，将第i个子区域分别沿宽度与长度方向上分别等距分割为m_i、k_i等份，得到m_i*k_i个矩形，将移动机器人放在分割得到的每个小矩形的几何中心，可获取沿前进或后退方向上移动机器人自带传感器扫描数据Q_i，Q_i包含m_i*k_i组匹配数据，并保存在全局定位数据库中，每个子区域获取的数据都存在一个到移动机器人在构建地图中位姿的一一映射ψ:Q_it→(x_it,y_it,θ_it)(1≤i≤n,1≤t≤m_i*k_i)；

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

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

步骤3A设第i个子区域，第r组匹配数据，表示为Q_ir，初始值i＝1，r＝1，P表示数据库中任意一组模型点集，匹配误差ε_ir表示与第i个子区域中第r组数据匹配的误差，ε_ir计算方法为：其中p_j、q_j分别表示点集P、Q_ir中的任一点坐标；

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

${\mathrm{\ϵ}}_s^1=\left\{{\mathrm{\ϵ}}_{s1},{\mathrm{\ϵ}}_{s2},...,{\mathrm{\ϵ}}_{s(m_s*k_s)}\right\},(s=i)$

步骤3C当中的任何一个元素的值都小于中任何一个元素值，r＝r+1，再返回步骤3B，否则退出循环，重新从应用环境中选取子区域Σ_i，当r＝m_i*k_i+1时，执行步骤3D；

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

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

步骤4A将移动机器人置于应用环境中已选取的任一子区域Σ_i内，任何位置获取一组传感器原始数据P，数据Q_i表示在第i个子区域Σ_i采集的数据，设在定位程序启动前移动机器人在地图中的位姿为(x₀,y₀,θ₀)，定义初始变量c＝0,0<δ<π/6，定义初始总误差E_ic＝0，i＝1；

步骤4B先将数据P进行处理得P_h＝R(cδ)*P，然后通过定义的ICP匹配函数L，与全局定位数据库中的数据Q_i进行匹配得到m_i*k_i组变换矩阵(R_t,T_t)＝L(P_h,Q_it)(1≤t≤m_i*k_i),其中Q_it表示第i个子区域采集的第t组匹配数据；

步骤4C根据每组变换矩阵先计算变换后的点集可表示为P_t＝R_t*P_h+T_t，然后计算每组匹配数据的误差为其中p_r、q_r分别表示点集P_t、Q_it中的任一点，在子区域Σ_i内将所有组数据匹配一次得到总的匹配误差可表示为 $E_{ic}=\sqrt{\left({\&Sigma;}_{t=1}^{m_i*k_i}{{\mathrm{\&epsiv;}}_t}^2\right)/\left(m_i*k_i\right)};$

步骤4D将子区域Σ_i的数据库与激光传感器扫描的数据P在2π弧度内进行多次匹配，改变参数的值c＝c+1，当满足时继续执行步骤4E，否则返回步骤4B，其中Γ(x)(x>0)表示取不超过x的最大整数；

步骤4E改变全局定位数据库中匹配点集的子区域，i＝i+1，当i＝n时继续执行步骤4F，否则返回步骤4A；

步骤4F共获得多组总的匹配误差取误差中最小的两组E_min1与E_min2作为移动机器人全局定位的数据，设此时对应的min1＝(x₁,y₁)，mim2＝(x₂,y₂)，由于移动机器人在全局中的位姿只能在某一个子区域内，因此，该结果必须同时满足条件x₁＝x₂＝x， 其中l(x₁)与d(x₁)分别表示第x₁子区间所分割的小矩形的长度与宽度，当满足条件时继续执行步骤4G，否则让移动机器人自动向前或者向后移动一定的距离，再返回步骤4A重新执行；

步骤4G根据上述结果确定移动机器人在全局地图中的位置，根据第x个子区域内采集的m_xk_x组点集Q_x与P_h(c＝y₁∪y₂)之间匹配可得到m_xk_x组误差，将其按照从小到大顺序排列，分别取前三个最小的误差，设结果分别为 ${\mathrm{\&epsiv;}}_{{\mathrm{xy}}_1}=\{{\mathrm{\&epsiv;}}_{y_{1}1},{\mathrm{\&epsiv;}}_{y_{1}2},{\mathrm{\&epsiv;}}_{y_{1}3}\},$ ${\mathrm{\&epsiv;}}_{{\mathrm{xy}}_2}=\{{\mathrm{\&epsiv;}}_{y_{2}1},{\mathrm{\&epsiv;}}_{y_{2}2},{\mathrm{\&epsiv;}}_{y_{2}3}\},$ 其对应的Q_x中的六组匹配点集可设为

步骤4H步骤4G中共得到的六个误差，同样按照从小到大的顺序排列，并取前三个最小的误差，设得到对应误差为{ε₁′,ε₂′,ε₃′}，对应的点集可分别设为{Q_x1′,Q_x2′,Q_x2′}，根据映射关系ψ，设三个点集对应的移动机器人在地图中的位姿分别为

步骤4I设定方差阈值σ，计算三个误差的方差σ₂，如果满足σ₃<σ，则继续执行步骤4J，否则去掉三个误差中最大的那个，然后再计算两个误差的方差σ₁，如果满足σ₁<σ，则继续执行步骤4K，否则去掉两个误差中最大的那个，然后继续执行步骤4L；

步骤4J假设最终有三个误差{ε₁′,ε₂′,ε₃′}，每个误差对应的匹配变换矩阵分别为(x₀,y₀,θ₀)表示移动机器人丢失位姿后保存的位姿，首先根据R(cδ)变换其位姿为：

$\left[\begin{array}{c}{\mathrm{\&theta;}}_{01}\\ {\mathrm{\&theta;}}_{02}\\ {\mathrm{\&theta;}}_{03}\end{array}\right]={\mathrm{\&theta;}}_0+\mathrm{\&delta;}\left[\begin{array}{c}{c}_1\\ c_2\\ c_3\end{array}\right]$

其中x_y与y₀不变，然后根据匹配变换矩阵计算移动机器人三个全局地图的位姿可表示如下；

[θ₁θ₂θ₃]^T＝[θ₀₁θ₀₂θ₀₃]^T+[β₁β₂β₃]^T

$\left[\begin{array}{cc}{x}_1& y_1\\ x_2& y_2\\ x_3& y_3\end{array}\right]=\left[\begin{array}{cc}{x}_0& y_0\\ x_0& y_0\\ x_0& y_0\end{array}\right]+\left[\begin{array}{cc}{T}_1\left(1\right)& T_1\left(2\right)\\ T_2\left(1\right)& T_2\left(2\right)\\ T_3\left(1\right)& T_3\left(2\right)\end{array}\right]$

计算三个权重其中i＝1,2,3，根据权重可最终确定移动机器人全局定位的位姿可表示如下：

$\left[\begin{array}{c}x\\ y\\ \mathrm{\&theta;}\end{array}\right]=\left[\begin{array}{ccc}{x}_1& x_2& x_3\\ y_1& y_2& y_3\\ {\mathrm{\&theta;}}_1& {\mathrm{\&theta;}}_2& {\mathrm{\&theta;}}_3\end{array}\right]\left[\begin{array}{c}{\mathrm{\&omega;}}_1\\ {\mathrm{\&omega;}}_2\\ {\mathrm{\&omega;}}_3\end{array}\right]$

步骤4K假设最终有两个误差{ε₁′,ε₂′,}，每个误差对应的匹配变换矩阵分别为(x₀,y₀,θ₀)表示移动机器人丢失位姿后保存的位姿，首先根据R(cδ)变换其位姿为：

$\left[\begin{array}{c}{\mathrm{\&theta;}}_{01}\\ {\mathrm{\&theta;}}_{02}\end{array}\right]={\mathrm{\&theta;}}_0+\mathrm{\&delta;}\left[\begin{array}{c}{c}_1\\ c_2\end{array}\right]$

其中x₀与y₀不变，然后根据匹配变换矩阵计算移动机器人两个全局地图的位姿可表示如下；

[θ₁θ₂]^T＝[θ₀₁θ₀₂]^T+[β₁β₂]^T

$\left[\begin{array}{cc}{x}_1& y_1\\ x_2& y_2\end{array}\right]=\left[\begin{array}{cc}{x}_0& y_0\\ x_0& y_0\end{array}\right]+\left[\begin{array}{cc}{T}_1\left(1\right)& T_1\left(2\right)\\ T_2\left(1\right)& T_2\left(2\right)\end{array}\right]$

计算两个权重其中i＝1,2，根据权重可最终确定移动机器人全局定位的位姿可表示如下：

$\left[\begin{array}{c}x\\ y\\ \mathrm{\&theta;}\end{array}\right]=\left[\begin{array}{cc}{x}_1& x_2\\ y_1& y_2\\ {\mathrm{\&theta;}}_1& {\mathrm{\&theta;}}_2\end{array}\right]\left[\begin{array}{c}{\mathrm{\&omega;}}_1\\ {\mathrm{\&omega;}}_2\end{array}\right]$

步骤4L假设最只有误差ε₁′,，对应的匹配变换矩阵分别为(R(β_l),T_l),(x₀,y₀,θ₀)表示移动机器人丢失位姿后保存的位姿，首先根据R(cδ)变换其位姿为：

θ₀₁＝θ₀+δc₁

其中x₀与y₀不变，然后根据匹配变换矩阵计算移动机器人全局地图的位姿可表示如下；

θ₁＝θ₀₁+β₁

$\left[\begin{array}{c}{x}_1\\ y_1\end{array}\right]=\left[\begin{array}{c}{x}_0\\ y_0\end{array}\right]+\left[\begin{array}{c}{T}_1\left(1\right)\\ T_1\left(2\right)\end{array}\right]$

计算权重ω₁＝1，根据权重可最终确定移动机器人全局定位的位姿可表示如下：

$\left[\begin{array}{c}x\\ y\\ \mathrm{\&theta;}\end{array}\right]={\mathrm{\&omega;}}_1\left[\begin{array}{c}{x}_1\\ y_1\\ {\mathrm{\&theta;}}_1\end{array}\right].$

2.如权利要求1所述的基于SLAM导航移动机器人的全局定位方法，其特征在于，所述移动机器人为AGV。

3.如权利要求1所述的基于SLAM导航移动机器人的全局定位方法，其特征在于，所述方法步骤1中移动机器人自带传感器为激光传感器或视觉传感器。

4.如权利要求1所述的基于SLAM导航移动机器人的全局定位方法，其特征在于，所述方法步骤4I中方差阈值σ根据下述公式确定：

σ＝(2～5)×min({ε₁′,ε₂′,ε₃′})。