为山，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十算方法为实中p.j、q.j分别表示点集P、Q中 的任一点坐标； 步骤3B将Qu与数据库中已选取的所有子区域内的所有组数据进行匹配，获取对应的 匹配误差如下：步骤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，然后计算 每组匹配数据的误差戈其中Pp分别表示点集 Pt、Qlt中的任一点，在子区域Σi内将所有组数据匹配一次得到总的匹配误差可表示为步骤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其中1 (X》与dUi)分别表示第x# 区间所分割的小矩形的长度与宽度，当满足条件时继续执行步骤4G，否则让移动机器人自 动向前或者向后移动一定的距离，再返回步骤4A重新执行； 步骤4G根据上述结果确定移动机器人在全局地图中的位置，根据第X个 子区域内采集的mxkx组点集Q)!与Ph(c=yiUy2)之间匹配可得到mxkx组 误差，将其按照从小到大顺序排列，分别取前三个最小的误差，设结果分别为，其对应的1中的六组匹配点集可设 为1步骤4H步骤4G中共得到的六个误差，同样按照从小到大的顺序排列，并取前 三个最小的误差，设得到对应误差为{>/，ε2'，ε3' }，对应的点集可分别设为 }，根据映射关系Φ，设三个点集对应的移动机器人在地图中的位姿分 别为為）： 步骤41设定方差阈值〇，计算三个误差的方差〇2，如果满足 〇3〈〇，则继续执行步骤 4J，否则去掉三个误差中最大的那个，然后再计算两个误差的方差〇1，如果满足〇1〈 〇,则 继续执行步骤4Κ，否则去掉两个误差中最大的那个，然后继续执行步骤4L; 步骤4J假设最终有三个误差{ε/，ε2'，ε3' }，每个误差对应的匹配变换矩阵 分别为UU/?〇^//^(X(]，y。，Θ。）表示移动机器人丢失位姿后保存的位姿，首先根据 R(cS)变换其位姿为：其中\与7。不变，然后根据匹配变换矩阵计算移动机器人三个全局地图的位姿可表示 如下； [01 9 2 Θ3]Τ=[Θ 01 9 02 θ〇3]Τ+[β? β2 β3]Τ计算三个权重(? = 其中i= 1，2,3,根据权重可最终确定移动机器人 全局定位的位姿可表示如下：步骤4K假设最终有两个误差{ε/，ε2'，}，每个误差对应的匹配变换矩阵分别为U?=1〇R(/?m(xD，yD，Θ。）表示移动机器人丢失位姿后保存的位姿，首先根据R(cS) 变换其位姿为：其中X。与y。不变，然后根据匹配变换矩阵计算移动机器人两个全局地图的位姿可表示 如下； [θ,ΘJT=[θη1θη,]τ+[β,βJT计算两个权重：其中i= 1，2,根据权重可最终确定移动机器人全 局定位的位姿可表示如下：步骤4L假设最只有误差ε/，，对应的匹配变换矩阵分别为（RWH)，（X(]，y。，Θ。） 表示移动机器人丢失位姿后保存的位姿，首先根据R(cδ)变换其位姿为： Θ 〇1= Θ 〇+ δ 其中X。与y。不变，然后根据匹配变换矩阵计算移动机器人全局地图的位姿可表示如 下； 9 1= 9 01+β1计算权重ω1= 1，根据权重可最终确定移动机器人全局定位的位姿可表示如下：2. 如权利要求1所述的基于SLAM导航移动机器人的全局定位方法，其特征在于，所述 移动机器人为AGV。3. 如权利要求1所述的基于SLAM导航移动机器人的全局定位方法，其特征在于，所述 方法步骤1中移动机器人自带传感器为激光传感器或视觉传感器。4. 如权利要求1所述的基于SLAM导航移动机器人的全局定位方法，其特征在于，所述 方法步骤41中方差阈值〇根据下述公式确定： σ = (2~SAminGe/，ε2'，ε3' })0
【专利摘要】本发明公开了一种基于SLAM导航移动机器人的全局定位方法，属于移动机器人自动导航技术领域。为解决现有技术中实现移动机器人全局定位需要对应用环境进行改造，增加辅助定位的特征物体，或者需要在移动机器人设备上安装辅助设备等。这些方案本身不能适应较为复杂环境、成本较高、定位精度较差、极为不方便，因此定位精度难以有更大的优化与提高的问题，本方法包括以下4个步骤：步骤1移动机器人应用环境的子区域的选取，步骤2数据点的采集，步骤3子区域选取合理与否的分析判断，步骤4基于ICP实现移动机器人全局定位。用于基于激光SLAM导航移动机器人尤其是AGV(自动导航小车)复杂环境下的全局定位。
【IPC分类】G01C21/20
【公开号】CN105258702
【申请号】CN201510644986
【发明人】王斌, 李国飞
【申请人】深圳力子机器人有限公司
【公开日】2016年1月20日
【申请日】2015年10月6日