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

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

A location method and storage medium of distribution robot in multi floor indoor environment

【技术实现步骤摘要】
一种多楼层室内环境中配送机器人的定位方法及存储介质
本专利技术涉及移动机器人定位
，具体涉及一种多楼层室内环境中配送机器人的定位方法及存储介质。
技术介绍
移动机器人定位指的是：在给定传感器数据和环境地图的情况下，判断机器人位姿的过程。定位作为移动机器人的一种基础能力，对于机器人成功完成各项任务具有决定性的作用；定位通常可以分为局部定位、全局定位。定位方法有很多种，如航程推演、粒子滤波定位方法。这些定位方法都可能存在定位不准确，出现较大的误差，例如粒子滤波定位方法中的自适应蒙特卡罗定位(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是所述当前楼层参考特征本文档来自技高网...

【技术保护点】
1.一种多楼层室内环境中配送机器人的定位方法，其特征在于，包括：/n利用二维激光雷达，通过同步定位与构图技术独立构建移动机器人运行的每一楼层地图，并基于各楼层地图坐标系map_frame，提取每一楼层环境中的特征；/n在构建地图时，利用惯性测量传感器，通过惯性导航技术获取各楼层地图坐标系map_frame相对于全局坐标系global_frame的变换关系，并映射各楼层特征到全局坐标系global_frame，得到各楼层参考特征集F1；/n机器人在线运行阶段，利用惯性测量传感器，通过惯性导航技术初步定位机器人，得到移动机器人第一定位结果的第一位姿P1，并确定机器人当前所在楼层；/n根据机器人当前所在楼层获取机器人当前所在楼层地图，在机器人当前所在楼层地图中，通过粒子滤波定位技术得到机器人第二定位结果的第二位姿P2；/n根据第一位姿P1和第二位姿P2，计算得到机器人第三位姿P3，其中第三位姿P3＝α*P1+β*(P2*A2)，其中α、β分别为第一定位结果和第二定位结果的加权系数，且两者满足：α+β＝1，α≥0，β≥0，A2为各楼层地图坐标系相对于全局坐标系global_frame的变换关系；所述第三位姿P3为将第一定位结果和第二定位结果进行融合后第三定位结果；/n基于移动机器人本体坐标系base_frame，通过二维激光雷达提取机器人当前周围运行环境的局部特征集F2；/n利用特征间的几何相似关系，初步匹配当前楼层参考特征集F1和局部特征集F2，并计算得到移动机器人的可能位姿集合{P}；/n计算可能位姿集合{P}中各可能位姿与第三位姿P3的欧式距离，初步过滤可能位姿中误匹配结果，得到可能位姿子集；/n对可能位姿子集中每一个可能位姿进行评分，选取分数最优的可能位姿作为机器人第四位姿P4；/n根据第三位姿P3和第四位姿P4，计算得到机器人第五位姿P5，以第五位姿P5为机器人定位结果；其中第五位姿...

【技术特征摘要】
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为机器人定位结果；其中第五位姿其中γ分别为两次定位结果的加权系数，且两者满足：γ≥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中任一角点特征参数包括：角点编号idc、角点X轴坐标xc和角点Y轴坐标yc；所述直线特征集L1中任一直线特征参数包括：直线编号idl、直线的法线方向坐标系原点与直线的距离(极径)ρ、直线长度l、起始端点X轴坐标xs、起始端点Y轴坐标ys、结束端点X轴坐标xe和结束端点Y轴坐标ye。


4.根据权利要求3所述的多楼层室内环境中配送机器人的定位方法，其特征在于，所述第一位姿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,...

【专利技术属性】
技术研发人员：赖志林，李睿，李良源，
申请(专利权)人：广州赛特智能科技有限公司，
类型：发明
国别省市：广东;44