一种多楼层室内环境中配送机器人的定位方法及存储介质与流程

本发明涉及移动机器人定位技术领域，具体涉及一种多楼层室内环境中配送机器人的定位方法及存储介质。

背景技术：

移动机器人定位指的是：在给定传感器数据和环境地图的情况下，判断机器人位姿的过程。定位作为移动机器人的一种基础能力，对于机器人成功完成各项任务具有决定性的作用；定位通常可以分为局部定位、全局定位。

定位方法有很多种，如航程推演、粒子滤波定位方法。这些定位方法都可能存在定位不准确，出现较大的误差，例如粒子滤波定位方法中的自适应蒙特卡罗定位(adaptivemontecarlolocalization，amcl)算法，作为一种当前流行的粒子滤波类定位方法，虽然多数情况下有着不错的表现，但由于该方法需要机器人的运动控制信息(这通常由里程计提供)，因此amcl算法严重依赖里程计。而里程计的输出数据易受到各种干扰，如轮子的磨损、地面打滑、车体震动等，这就造成amcl算法的定位结果会出现较大的误差，造成定位不准确。

技术实现要素：

鉴于以上技术问题，本发明的目的在于提供一种多楼层室内环境中配送机器人的定位方法及存储介质，解决现有定位方法的可能定位不准确的问题。

本发明采用技术方案如下：

一种多楼层室内环境中配送机器人的定位方法，包括：

利用二维激光雷达，通过同步定位与构图技术独立构建移动机器人运行的每一楼层地图，并基于各楼层地图坐标系map_frame，提取每一楼层环境中的特征；

在构建地图时，利用惯性测量传感器，通过惯性导航技术获取各楼层地图坐标系map_frame相对于全局坐标系global_frame的变换关系，并映射各楼层特征到全局坐标系global_frame，得到各楼层参考特征集f1；

机器人在线运行阶段，利用惯性测量传感器，通过惯性导航技术初步定位机器人，得到移动机器人第一定位结果的第一位姿p1，并确定机器人当前所在楼层；

根据机器人当前所在楼层获取机器人当前所在楼层地图，在机器人当前所在楼层地图中，通过粒子滤波定位技术得到机器人第二定位结果的第二位姿p2；

根据第一位姿p1和第二位姿p2，计算得到机器人第三位姿p3，其中第三位姿p3＝α*p1+β*(p2*a2)，其中α、β分别为第一定位结果和第二定位结果的加权系数，且两者满足：α+β＝1，α≥0，β≥0，a2为各楼层地图坐标系相对于全局坐标系global_frame的变换关系；所述第三位姿p3为将第一定位结果和第二定位结果进行融合后第三定位结果；

基于移动机器人本体坐标系base_frame，通过二维激光雷达提取机器人当前周围运行环境的局部特征集f2；

利用特征间的几何相似关系，初步匹配当前楼层参考特征集f1和局部特征集f2，并计算得到移动机器人的可能位姿集合{p}；

计算可能位姿集合{p}中各可能位姿与第三位姿p3的欧式距离，初步过滤可能位姿中误匹配结果，得到可能位姿子集；

对可能位姿子集中每一个可能位姿进行评分，选取分数最优的可能位姿作为机器人第四位姿p4；

根据第三位姿p3和第四位姿p4，计算得到机器人第五位姿p5，以第五位姿p5为机器人定位结果；其中第五位姿其中γ分别为两次定位结果的加权系数，且两者满足：γ≥0；所述p5为第三位姿p3和第四位姿p4的融合结果。

进一步的，得到各楼层参考特征集f1的步骤具体包括：

所述各楼层地图分别基于独立的地图坐标系map_frame；利用惯性测量传感器，通过积分机器人运动时的角速度和加速度，得到各楼层地图之间的变换关系a1；

所述全局坐标系global_frame与某一楼层的地图坐标系map_frame重合；根据各楼层地图之间的变换关系，可以得到各楼层地图坐标系相对于全局坐标系global_frame的变换关系a2；根据所述变换关系a2，通过坐标变换将各楼层特征映射到全局坐标系global_frame，得到各楼层参考特征集f1。

进一步的，所述各楼层参考特征集f1基于全局坐标系global_frame；所述参考特征集f1包括角点特征集c1和直线特征集l1；所述角点特征集c1中任一角点特征参数包括：角点编号idc、角点x轴坐标xc和角点y轴坐标yc；所述直线特征集l1中任一直线特征参数包括：直线编号idl、直线的法线方向坐标系原点与直线的距离(极径)ρ、直线长度l、起始端点x轴坐标xs、起始端点y轴坐标ys、结束端点x轴坐标xe和结束端点y轴坐标ye。

进一步的，所述第一位姿p1的参数(x,y,z,θ)包括：机器人本体坐标系base_frame原点在全局坐标系global_frame中的x轴坐标x、y轴坐标y、z轴坐标z；机器人本体坐标系base_framex轴相对于全局坐标系global_framex轴的旋转角度θ；

所述第二位姿p2的参数(x,y,z,θ)包括：机器人本体坐标系base_frame原点在当前楼层地图坐标系map_frame中的x轴坐标x、y轴坐标y、z轴坐标z；机器人本体坐标系base_framex轴相对于当前楼层地图坐标系map_framex轴的旋转角度θ，所述第二位姿p2的z值为0；

所述第三位姿p3的参数(x,y,z,θ)包括：机器人本体坐标系base_frame原点在全局坐标系global_frame中的x轴坐标x、y轴坐标y、z轴坐标z；机器人本体坐标系base_framex轴相对于全局坐标系global_framex轴的旋转角度θ；

所述第四位姿p4的参数(x,y,z,θ)包括：机器人本体坐标系base_frame原点在全局坐标系global_frame中的x轴坐标x、y轴坐标y、z轴坐标z；机器人本体坐标系base_framex轴相对于全局坐标系global_framex轴的旋转角度θ；

所述最终位姿p5的参数(x,y,z,θ)包括：机器人本体坐标系base_frame原点在全局坐标系global_frame中的x轴坐标x、y轴坐标y、z轴坐标z；机器人本体坐标系base_framex轴相对于全局坐标系global_framex轴的旋转角度θ。

进一步的，所述局部特征集f2基于移动机器人本体坐标系base_frame；所述局部特征集f2包括角点特征集c2和直线特征集l2；所述角点特征集c2中任一角点特征参数包括：角点编号角点x轴坐标和角点y轴坐标所述直线特征集l2中任一直线特征参数包括：直线编号直线的法线方向坐标系原点与直线的距离(极径)直线长度起始端点x轴坐标起始端点y轴坐标结束端点x轴坐标结束端点y轴坐标

进一步的，利用特征间的几何相似关系，初步匹配当前楼层参考特征集f1和局部特征集f2，并计算得到移动机器人的可能位姿集合{p}的步骤包括：提取局部特征集f2中的几何关系；所述几何关系包括直线夹角，和/或角点间距，和/或点线距离。

在当前楼层参考特征集f1中获取与局部特征集f2几何关系相似的参考特征集f3，所述参考特征集f3包括角点特征集c3和直线特征集l3；所述参考特征集f3是所述当前楼层参考特征集f1的子集；

所述可能位姿集合{p}中每一位姿的参数为(x,y,z,θ)，具体包括：机器人本体坐标系base_frame原点在全局坐标系global_frame中的x轴坐标x、y轴坐标y、z轴坐标z；机器人本体坐标系base_framex轴相对于全局坐标系global_framex轴的旋转角度θ。

通过参考特征集f3和局部特征集f2的初步匹配得到一个特征对集合，不同的特征对可能解算出不同的位姿，根据初步匹配的参考特征集f3和局部特征集f2，得到移动机器人的可能位姿集合{p}中每一位姿，得到可能位姿集合{p}。

进一步的，根据初步匹配的参考特征集f3和局部特征集f2，得到移动机器人的可能位姿集合{p}中每一位姿的步骤具体包括：通过一对直线特征解算机器人的姿态θ；通过两对直线特征或者一对角点特征解算机器人的位置(x,y)，z值始终为0。

进一步的，计算可能位姿集合中各位姿与p3的欧式距离，初步过滤误匹配结果，得到可能位姿子集的步骤包括：遍历所述可能位姿集合{p}中所有位姿，计算所述位姿与所述机器人第三位姿p3的欧式距离；若欧式距离超过给定阈值，则抛弃所述欧式距离对应的可能位姿；若所述欧式距离不超过给定的阈值，则将所述可能位姿加入所述可能位姿子集。

进一步的，对可能位姿子集中每一个可能位姿进行评分，选取分数最优的可能位姿作为机器人第四位姿p4的步骤包括：基于机器人当前所在楼层地图，根据所述可能位姿子集中任一位姿，获取与所述二维激光雷达的实时测量数据相对应的参考测量数据；根据所述实时测量数据与所述参考测量数据的误差，计算所述位姿的评分；根据评分结果，选取可能位姿子集中评分最高的位姿作为机器人第四位姿p4。

一种计算机存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时，实现所述的多楼层室内环境中配送机器人的定位方法。

相比现有技术，本发明的有益效果在于：

本发明的多楼层室内环境中配送机器人的定位方法，通过机器人在线运行阶段，通过积分惯性测量数据初步定位机器人，得到机器人在全局坐标系global_frame下的第一位姿p1，以及机器人当前所在楼层；基于激光雷达测量数据及机器人当前所在楼层地图，通过粒子滤波定位技术进一步定位机器人，得到机器人在当前楼层地图坐标系map_frame下的第二位姿p2；通过惯性导航定位技术确定机器人当前所在楼层，并给出机器人第一位姿p1；通过粒子滤波定位技术，进一步给出机器人第二位姿p2；基于二维激光雷达测量数据，提取机器人运行环境的角点及直线特征；通过匹配点线特征，给出机器人第四位姿p4；融合三种定位技术给出的定位结果，得到机器人最终位姿p5，完成机器人的定位。

本发明通过惯性导航定位、粒子滤波定位、点线特征匹配定位三种定位技术的融合，有效地降低了由于里程计被干扰造成的定位误差，也降低了惯性测量传感器的累积误差，提高了机器人定位的精确性、鲁棒性，实现了机器人在室内多楼层间的自主交叉配送。

附图说明

图1为本发明多楼层室内环境中配送机器人的定位方法的流程示意图；

图2为本发明多楼层室内环境中配送机器人的定位方法应用实施例的流程示意图；

图3为本发明多楼层室内环境中配送机器人的定位方法应用实施例中相似几何关系的示意图。

具体实施方式

下面，结合附图以及具体实施方式，对本发明做进一步描述，需要说明的是，在不相冲突的前提下，以下描述的各实施例之间或各技术特征之间可以任意组合形成新的实施例。

实施例：

请参考图1-3所示，一种多楼层室内环境中配送机器人的定位方法，请参考图1所示，包括：

步骤s1:在机器人离线运行阶段，利用二维激光雷达，通过同步定位与构图技术独立构建移动机器人运行的每一楼层地图，并基于各楼层地图坐标系map_frame，提取每一楼层环境中的特征；所述特征包括直线特征，以及角点特征。这里，提取每一楼层环境中的特征，是指在移动机器人的精度范围内，提取每一楼层环境空间中的所有直线的直线特征，所有环境空间中的所有角点的角点特征。

作为实施例，角点特征可包括：角点编号idc、角点x轴坐标xc和角点y轴坐标yc；直线特征集可包括：直线编号idl、直线的法线方向坐标系原点与直线的距离(极径)ρ、直线长度l、起始端点x轴坐标xs、起始端点y轴坐标ys、结束端点x轴坐标xe和结束端点y轴坐标ye。

地图可包括栅格地图和矢量地图，作为具体实施例，本发明的地图可采用栅格地图。所述各楼层地图分别基于独立的地图坐标系map_frame。

具体的，针对各楼层，用户操控机器人移动，或者机器人自主探索移动，并利用二维激光雷达实现对运行环境的感知，离线构建地图，并提取当前楼层中的角点及直线特征。

步骤s2:在构建地图时，利用惯性测量传感器，通过惯性导航技术获取各楼层地图坐标系map_frame相对于全局坐标系global_frame的变换关系，并映射各楼层特征到全局坐标系global_frame，得到各楼层参考特征集f1；

具体的，机器人在各楼层之间穿梭构建地图时，利用惯性测量传感器，通过惯性导航技术积分机器人运动时的角速度和加速度，得到各楼层地图之间的变换关系a1。

在这里，全局坐标系global_frame可与某一楼层的地图坐标系map_frame重合；具体的，本发明的全局坐标系global_frame与最低楼层的地图坐标系map_frame重合。

根据各楼层地图之间的变换关系，可以得到各楼层地图坐标系相对于全局坐标系global_frame的变换关系a2；根据所述变换关系a2，通过坐标变换将各楼层特征映射到全局坐标系global_frame，得到各楼层参考特征集f1。

具体的，所述各楼层参考特征集f1基于全局坐标系global_frame；所述参考特征集f1包括角点特征集c1和直线特征集l1；所述角点特征集c1中任一角点特征参数包括：角点编号idc、角点x轴坐标xc和角点y轴坐标yc；所述直线特征集l1中任一直线特征参数包括：直线编号idl、直线的法线方向坐标系原点与直线的距离(极径)ρ、直线长度l、起始端点x轴坐标xs、起始端点y轴坐标ys、结束端点x轴坐标xe和结束端点y轴坐标ye。

步骤s3:机器人在线运行阶段，利用惯性测量传感器，通过积分惯性测量数据初步定位机器人，得到机器人在全局坐标系global_frame下的第一位姿p1，以及机器人当前所在楼层。

第一位姿p1的数据包括(x,y,z,θ)，具体的，第一位姿p1的参数(x,y,z,θ)包括：机器人本体坐标系base_frame原点在全局坐标系global_frame中的x轴坐标x、y轴坐标y、z轴坐标z；机器人本体坐标系base_framex轴相对于全局坐标系global_framex轴的旋转角度θ。

步骤s4:基于激光雷达测量数据及机器人当前所在楼层地图，通过粒子滤波定位技术进一步定位机器人，得到机器人在当前楼层地图坐标系map_frame下的第二位姿p2；

具体的，所述粒子滤波定位算法可以采用自适应蒙特卡罗定位(adaptivemontecarlolocalization，amcl)算法，该算法的定位实现可以简述为：不断迭代预测、测量更新、重采样的三个步骤，逐步收敛粒子集，计算机器人的后验位姿，最终实现机器人的定位。具体如下：

一、预测：基于t-1时刻的后验粒子集st-1和t时刻的控制信息ut，根据运动模型计算得到每个粒子t时刻的预测状态生成预测粒子集

二、测量更新：基于t时刻的感知信息zt，根据测量模型更新中每一个粒子的权重wt，并归一化粒子权重；

三、重采样：以正比于粒子权重wt的概率，从中重采样m个粒子并加入后验粒子集st，并归一化粒子权重。

amcl算法通过循环执行上面三个步骤，迭代计算机器人的后验位姿,以实现机器人的定位，得到机器人第二位姿p2。

所述第二位姿p2的参数(x,y,z,θ)包括：机器人本体坐标系base_frame原点在当前楼层地图坐标系map_frame中的x轴坐标x、y轴坐标y、z轴坐标z；机器人本体坐标系base_framex轴相对于当前楼层地图坐标系map_framex轴的旋转角度θ。所述第二位姿p2的z值始终为0。

步骤s5:基于机器人当前所在楼层地图坐标系map_frame相对于全局坐标系global_frame的变换关系a2，融合两次定位结果p1和p2，通过p3＝α*p1+β*(p2*a2)公式计算机器人第三位姿p3，得到机器人的两次融合定位结果。

其中α、β分别为两次定位结果的加权系数，且两者满足：α+β＝1，α≥0，β≥0。

所述第三位姿p3的参数(x,y,z,θ)包括：机器人本体坐标系base_frame原点在全局坐标系global_frame中的x轴坐标x、y轴坐标y、z轴坐标z；机器人本体坐标系base_framex轴相对于全局坐标系global_framex轴的旋转角度θ。

步骤s6:通过二维激光雷达获取机器人周围环境的距离信息，并从中提取局部特征集f2。

具体的，提取局部特征集f2的步骤如下：从所述距离信息经过离群点过滤、搜索断点、分割点集、拟合直线等步骤，可以提取当前环境的直线特征、角点特征；直线特征描述了环境中具有明显特征的直线段，角点特征描述了环境中具有明显特征的角点；将这些直线特征、角点特征进行组合，即可得到直线特征集、角点特征集；直线特征集与角点特征集共同组成了所述局部特征集f2。

具体的，所述局部特征集f2基于移动机器人本体坐标系base_frame；所述局部特征集f2包括角点特征集c2和直线特征集l2；所述角点特征集c2中任一角点特征参数包括：角点编号角点x轴坐标和角点y轴坐标所述直线特征集l2中任一直线特征参数包括：直线编号直线的法线方向坐标系原点与直线的距离(极径)直线长度起始端点x轴坐标起始端点y轴坐标结束端点x轴坐标结束端点y轴坐标

步骤s7:利用特征间的几何相似关系，初步匹配当前楼层参考特征集f1和局部特征集f2，并计算得到移动机器人的可能位姿集合{p}。

具体的，在当前楼层参考特征集f1中获取与局部特征集f2几何关系相似的参考特征集f3，所述参考特征集f3包括角点特征集c3和直线特征集l3；所述参考特征集f3是所述当前楼层参考特征集f1的子集；所述几何关系包括直线夹角、角点间距、点线距离等。

所述可能位姿集合{p}中每一位姿的参数(x,y,z,θ)包括：机器人本体坐标系base_frame原点在全局坐标系global_frame中的x轴坐标x、y轴坐标y、z轴坐标z；机器人本体坐标系base_framex轴相对于全局坐标系global_framex轴的旋转角度θ。

根据初步匹配的参考特征集f3和局部特征集f2，得到移动机器人的可能位姿集合{p}中每一位姿的步骤包括：通过一对直线特征解算机器人的姿态θ；通过两对直线特征或者一对角点特征解算机器人的位置(x,y)，z值始终为0。

在特征匹配技术中，大多是从二维图像中基于像素梯度等进行特征提取，并利用优化技术进行特征匹配；本申请的特征匹配技术，是从二维激光雷达测量数据提取角点及直线特征，并利用特征间的几何相似关系，初步匹配全局和局部特征集；本申请所述初步匹配过程并不会得到严格的匹配结果，而是有一个全局和局部特征的初步匹配关系，即有正确的匹配关系，也可能包含有错误的匹配关系。

通过参考特征集f3和局部特征集f2的初步匹配得到一个特征对集合，不同的特征对可能解算出不同的位姿，根据特征对集合可以解算出可能位姿集合{p}。

所述姿态θ通过公式计算；

所述位置(x,y)通过公式计算；

其中，为直线特征集l2中某一直线特征的法线方向，为直线特征集l3中对应匹配直线特征的法线方向；分别为角点特征集c2中某一角点特征的x轴坐标、y轴坐标,xc、yc分别为角点特征集c3中对应匹配角点特征的x轴坐标、y轴坐标。

步骤s8:计算可能位姿集合中各位姿与p3的欧式距离，(欧式距离指欧几里得度量，在数学中，欧几里得距离或欧几里得度量是欧几里得空间中两点间“普通”，可通过欧式距离公式计算得到)，初步过滤误匹配结果，得到可能位姿子集。

具体的，遍历所述可能位姿集合{p}中所有位姿，计算所述位姿与所述机器人第三位姿p3的欧式距离；若所述欧式距离超过给定阈值，则抛弃所述距离对应的可能位姿；若所述距离不超过给定阈值，则将所述位姿加入所述可能位姿子集。

步骤s9:评分可能位姿子集中每一个位姿，选取分数最优的位姿作为机器人第四位姿p4。

具体的，基于机器人当前所在楼层地图，根据所述可能位姿子集中任一位姿，获取与所述二维激光雷达的实时测量数据相对应的参考测量数据；根据所述实时测量数据与所述参考测量数据的误差，计算所述可能位姿的评分；根据评分结果，选取可能位姿子集中评分最高的位姿作为机器人第四位姿p4。

所述第四位姿p4的参数(x,y,z,θ)包括：机器人本体坐标系base_frame原点在全局坐标系global_frame中的x轴坐标x、y轴坐标y、z轴坐标z；机器人本体坐标系base_framex轴相对于全局坐标系global_framex轴的旋转角度θ。

步骤s10:融合三次定位结果，得到机器人最终位姿，即第五位姿p5，作为机器人定位结果。

具体的，通过公式计算机器人最终位姿p5，得到机器人的最终定位结果。

其中γ分别为两次定位结果的加权系数，且两者满足：γ≥0。

所述最终位姿p5的参数(x,y,z,θ)包括：机器人本体坐标系base_frame原点在全局坐标系global_frame中的x轴坐标x、y轴坐标y、z轴坐标z；机器人本体坐标系base_framex轴相对于全局坐标系global_framex轴的旋转角度θ。

本发明通过惯性导航定位技术确定机器人当前所在楼层，并给出机器人第一位姿p1；通过粒子滤波定位技术，进一步给出机器人第二位姿p2；基于二维激光雷达测量数据，提取机器人运行环境的角点及直线特征；通过匹配点线特征，给出机器人第四位姿p4；融合三种定位技术给出的定位结果，得到机器人最终位姿p5，完成机器人的定位。本发明有效地降低了由于里程计被干扰造成的定位误差，也降低了惯性测量传感器的累积误差，提高了机器人定位的精确性、鲁棒性，实现了机器人在室内多楼层间的自主交叉配送。

实施例以室内配送机器人为例，请参照图2所示，本发明的具体应用实施例包括如下步骤：

步骤s11:在机器人离线运行阶段，利用二维激光雷达，通过同步定位与构图技术独立构建移动机器人运行的每一楼层地图，并基于各楼层地图坐标系map_frame，提取每一楼层环境中的特征；所述特征包括直线特征，以及角点特征。

地图可包括栅格地图和矢量地图，作为具体实施例，本发明的地图可采用栅格地图。所述各楼层地图分别基于独立的地图坐标系map_frame。

具体的，针对各楼层，用户操控机器人移动，或者机器人自主探索移动，并利用二维激光雷达实现对运行环境的感知，离线构建地图，并提取当前楼层中的角点及直线特征。

步骤s12:机器人在各楼层之间穿梭构建地图时，利用惯性测量传感器，通过惯性导航技术积分机器人运动时的角速度和加速度，得到各楼层地图之间的变换关系a1。

所述全局坐标系global_frame与某一楼层的地图坐标系map_frame重合；具体的，本发明的全局坐标系global_frame与最低楼层的地图坐标系map_frame重合。

根据各楼层地图之间的变换关系，可以得到各楼层地图坐标系相对于全局坐标系global_frame的变换关系a2；根据所述变换关系a2，通过坐标变换将各楼层特征映射到全局坐标系global_frame，得到各楼层参考特征集f1。

具体的，所述各楼层参考特征集f1基于全局坐标系global_frame；所述参考特征集f1包括角点特征集c1和直线特征集l1；所述角点特征集c1中任一角点特征参数包括：角点编号idc、角点x轴坐标xc和角点y轴坐标yc；所述直线特征集l1中任一直线特征参数包括：直线编号idl、直线的法线方向坐标系原点与直线的距离(极径)ρ、直线长度l、起始端点x轴坐标xs、起始端点y轴坐标ys、结束端点x轴坐标xe和结束端点y轴坐标ye。

步骤s13:机器人在线运行阶段，利用惯性测量传感器，通过积分惯性测量数据初步定位机器人，得到机器人在全局坐标系global_frame下的第一位姿p1，以及机器人当前所在楼层。

所述第一位姿p1的参数(x,y,z,θ)包括：机器人本体坐标系base_frame原点在全局坐标系global_frame中的x轴坐标x、y轴坐标y、z轴坐标z；机器人本体坐标系base_framex轴相对于全局坐标系global_framex轴的旋转角度θ。

步骤s14:基于激光雷达测量数据及机器人当前所在楼层地图，通过粒子滤波定位技术进一步定位机器人，得到机器人在当前楼层地图坐标系map_frame下的第二位姿p2；

具体的，所述粒子滤波定位算法可以采用自适应蒙特卡罗定位(adaptivemontecarlolocalization，amcl)算法，该算法的定位实现可以简述为：不断迭代预测、测量更新、重采样的三个步骤，逐步收敛粒子集，计算机器人的后验位姿，最终实现机器人的定位。具体如下：

一、预测：基于t-1时刻的后验粒子集st-1和t时刻的控制信息ut，根据运动模型计算得到每个粒子t时刻的预测状态生成预测粒子集

二、测量更新：基于t时刻的感知信息zt，根据测量模型更新中每一个粒子的权重wt，并归一化粒子权重；

三、重采样：以正比于粒子权重wt的概率，从中重采样m个粒子并加入后验粒子集st，并归一化粒子权重。

amcl算法通过循环执行上面三个步骤，迭代计算机器人的后验位姿,以实现机器人的定位，得到机器人第二位姿p2。

所述第二位姿p2的参数(x,y,z,θ)包括：机器人本体坐标系base_frame原点在当前楼层地图坐标系map_frame中的x轴坐标x、y轴坐标y、z轴坐标z；机器人本体坐标系base_framex轴相对于当前楼层地图坐标系map_framex轴的旋转角度θ。所述第二位姿p2的z值始终为0。

步骤s15:基于机器人当前所在楼层地图坐标系map_frame相对于全局坐标系global_frame的变换关系a2，融合两次定位结果p1和p2，通过p3＝α*p1+β*(p2*a2)公式计算机器人第三位姿p3，得到机器人的两次融合定位结果。

其中α、β分别为两次定位结果的加权系数，且两者满足：α+β＝1，α≥0，β≥0。根据经验值，通常α值设为0.1，常β值设为0.9。

所述第三位姿p3的参数(x,y,z,θ)包括：机器人本体坐标系base_frame原点在全局坐标系global_frame中的x轴坐标x、y轴坐标y、z轴坐标z；机器人本体坐标系base_framex轴相对于全局坐标系global_framex轴的旋转角度θ。

步骤s16:通过二维激光雷达获取机器人周围环境的距离信息，并从中提取局部特征集f2。

具体的，所述局部特征集f2基于移动机器人本体坐标系base_frame；所述局部特征集f2包括角点特征集c2和直线特征集l2；所述角点特征集c2中任一角点特征参数包括：角点编号角点x轴坐标和角点y轴坐标所述直线特征集l2中任一直线特征参数包括：直线编号直线的法线方向坐标系原点与直线的距离(极径)直线长度起始端点x轴坐标起始端点y轴坐标结束端点x轴坐标结束端点y轴坐标

步骤s17:利用特征间的几何相似关系，初步匹配当前楼层参考特征集f1和局部特征集f2，并计算得到移动机器人的可能位姿集合{p}。

具体的，在当前楼层参考特征集f1中获取与局部特征集f2几何关系相似的参考特征集f3，所述参考特征集f3包括角点特征集c3和直线特征集l3；所述参考特征集f3是所述当前楼层参考特征集f1的子集；所述几何关系包括直线夹角、角点间距、点线距离等。

其中相似的几何关系示例请参照图3所示，参考特征集和局部特征集具有相同的直线夹角δ和角点间距d，判定为具有相似的几何关系；

所述可能位姿集合{p}中每一位姿的参数(x,y,z,θ)包括：机器人本体坐标系base_frame原点在全局坐标系global_frame中的x轴坐标x、y轴坐标y、z轴坐标z；机器人本体坐标系base_framex轴相对于全局坐标系global_framex轴的旋转角度θ。

根据初步匹配的参考特征集f3和局部特征集f2，得到移动机器人的可能位姿集合{p}中每一位姿的步骤包括：通过一对直线特征解算机器人的姿态θ；通过两对直线特征或者一对角点特征解算机器人的位置(x,y)，z值始终为0。

通过参考特征集f3和局部特征集f2的初步匹配得到一个特征对集合，不同的特征对可能解算出不同的位姿，根据特征对集合可以解算出可能位姿集合{p}。

所述姿态θ通过公式计算；

所述位置(x,y)通过公式计算；

其中，为直线特征集l2中某一直线特征的法线方向，为直线特征集l3中对应匹配直线特征的法线方向；分别为角点特征集c2中某一角点特征的x轴坐标、y轴坐标,xc、yc分别为角点特征集c3中对应匹配角点特征的x轴坐标、y轴坐标。

步骤s18:计算可能位姿集合中各位姿与p3的欧式距离，初步过滤误匹配结果，得到可能位姿子集。

具体的，遍历所述可能位姿集合{p}中所有位姿，计算所述位姿与所述机器人第三位姿p3的欧式距离；若所述距离超过给定阈值，则抛弃所述距离对应的可能位姿；若所述距离不超过给定的阈值，则将所述位姿加入所述可能位姿子集。根据经验，所述阈值通常可设为2m。

步骤s19:评分可能位姿子集中每一个位姿，选取分数最优的位姿作为机器人第四位姿p4。

具体的，基于机器人当前所在楼层地图，根据所述可能位姿子集中任一位姿，获取与所述二维激光雷达的实时测量数据相对应的参考测量数据；根据所述实时测量数据与所述参考测量数据的误差，计算所述位姿的评分；根据评分结果，选取可能位姿子集中评分最高的位姿作为机器人第四位姿p4。

具体的，所述实时测量数据是一个距离集合，集合中每一个元素表示：基于当前真实的机器人位姿，在机器人某一方向上，距离机器人最近的环境物体与机器人的距离。所述实时测量数据通过激光雷达扫描得到。所述参考测量数据是一个距离集合，集合中每一个元素表示：基于当前假设的机器人位姿，在机器人某一方向上，距离机器人最近的地图中物体与机器人的距离。所述参考测量数据通过在所述楼层地图中查询得到。

所述第四位姿p4的参数(x,y,z,θ)包括：机器人本体坐标系base_frame原点在全局坐标系global_frame中的x轴坐标x、y轴坐标y、z轴坐标z；机器人本体坐标系base_framex轴相对于全局坐标系global_framex轴的旋转角度θ。

所述评分可通过公式计算。

其中scorei、disti分别为在所述可能位姿下第i束激光的评分、最近参考障碍物距离；λ、σ分别为归一化系数、标准差系数；通常σ可取值为0.1。

步骤s20:融合三次定位结果，得到机器人最终位姿p5，作为机器人定位结果。

具体的，通过公式计算机器人最终位姿p5，得到机器人的最终定位结果。

其中γ分别为两次定位结果的加权系数，且两者满足：

所述最终位姿p5的参数(x,y,z,θ)包括：机器人本体坐标系base_frame原点在全局坐标系global_frame中的x轴坐标x、y轴坐标y、z轴坐标z；机器人本体坐标系base_framex轴相对于全局坐标系global_framex轴的旋转角度θ。

本发明的运行于多楼层室内环境中配送机器人的定位方法，通过惯性导航定位、粒子滤波定位、点线特征匹配定位三种定位技术的融合，有效地降低了由于里程计被干扰造成的定位误差，也降低了惯性测量传感器的累积误差，提高了机器人定位的精确性、鲁棒性，实现了机器人在室内多楼层间的自主交叉配送。

本发明还提供一种计算机存储介质，其上存储有计算机程序，本发明的方法如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在该计算机存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机存储介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、u盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(rom，read-onlymemory)、随机存取存储器(ram，randomaccessmemory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机存储介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机存储介质不包括电载波信号和电信信号。

对本领域的技术人员来说，可根据以上描述的技术方案以及构思，做出其它各种相应的改变以及形变，而所有的这些改变以及形变都应该属于本发明权利要求的保护范围之内。