[发明专利]一种多楼层室内环境中配送机器人的定位方法及存储介质

权利要求书

1.一种多楼层室内环境中配送机器人的定位方法，其特征在于，包括：

利用二维激光雷达，通过同步定位与构图技术独立构建移动机器人运行的每一楼层地图，并基于各楼层地图坐标系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为机器人定位结果；其中第五位姿P5＝η*P3+γ*P4，其中η、γ分别为两次定位结果的加权系数，且两者满足：η+γ＝1，η≥0，γ≥0；所述第五位姿P5为第三位姿P3和第四位姿P4的融合结果。

2.根据权利要求1所述的多楼层室内环境中配送机器人的定位方法，其特征在于，得到各楼层参考特征集F1的步骤具体包括：

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

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

3.根据权利要求2所述的多楼层室内环境中配送机器人的定位方法，其特征在于，所述各楼层参考特征集F1基于全局坐标系global_frame；所述参考特征集F1包括角点特征集C1和直线特征集L1；所述角点特征集C1中任一角点特征参数包括：角点编号id_c、角点X轴坐标x_c和角点Y轴坐标y_c；所述直线特征集L1中任一直线特征参数包括：直线编号id_l、直线的法线方向坐标系原点与直线的距离ρ、直线长度l、起始端点X轴坐标x_s、起始端点Y轴坐标y_s、结束端点X轴坐标x_e和结束端点Y轴坐标y_e。