[发明专利]移动机器人粒子滤波定位的位姿校正方法及移动机器人

说明书

本发明公开了一种移动机器人粒子滤波定位的位姿校正方法及移动机器人，包括如下步骤：基于预先构建好的预设静态环境地图得到当前定位时刻移动机器人的第一估计位姿；判断第一估计位姿与第二估计位姿是否可用，并判断当前周边环境与预设静态环境地图中局部区域的变动程度是否小于或等于预设变动程度阈值，以及判断航位推算数据是否可用，进而得到移动机器人当前定位时刻的候选输出位姿，并基于移动机器人当前定位时刻的候选输出位姿得到移动机器人当前定位时刻的输出估计位姿。其基于粒子滤波定位的位姿校正算法提高了定位精度，同时提供当前估计位姿的置信度，而且减小了当前运行环境与预设静态地图局部区域所发生变动造成的影响。

技术领域

本发明涉及移动机器人技术领域，具体是一种移动机器人粒子滤波定位的位姿校正方法及移动机器人。

背景技术

移动机器人包括无人驾驶车、AGV自动导引车等移动平台。目前移动机器人的定位技术通常提供移动机器人在环境中世界坐标系下的位姿(包括位置和航向角)，同时，移动机器人定位技术是移动机器人实现自主导航的关键技术。

针对机器人在预设环境地图的全局定位，现有方法采用粒子滤波定位技术对移动机器人进行定位。粒子滤波定位的自适应蒙特卡洛算法采用概率栅格地图、激光雷达的测量数据和里程位移数据，同时通过带随机噪声的运动模型结合实现随机粒子的运动更新，粒子滤波定位的自适应蒙特卡洛算法根据当前观测数据与预设静态局部环境的匹配程度，调节粒子权重，实现观测更新，用自适应重采样保持有效粒子数，使用权重较高的粒子簇输出估计位姿。

但是，现有的自适应蒙特卡洛定位算法的粒子滤波位姿估计存在如下缺点：1、不估算粒子簇的估计位姿的可靠程度，以及估计位姿与机器人当前实际位姿之间存在的误差；2、里程位移数据有明显误差时，估计位姿也相应发生明显误差；3、机器人当前运行的局部环境与预设静态地图局部区域相比已经发生较大改变时，当前观测数据与预设静态局部环境之间存在差异，针对已有的预设静态局部环境进行匹配进而调节粒子权重会导致位姿估计误差。同时，在传感数据较少的户外稀疏环境中可能存在两个或更多的高权重粒子簇，导致估计位姿出现跳动。

发明内容

针对上述现有技术中的不足，本发明提供一种移动机器人粒子滤波定位的位姿校正方法及移动机器人，可有效地对粒子滤波定位的位姿进行校正。

为实现上述目的，本发明提供一种移动机器人粒子滤波定位的位姿校正方法，包括如下步骤：

步骤1，基于预先构建好的预设静态环境地图得到当前定位时刻移动机器人的第一估计位姿；

步骤2，判断第一估计位姿的置信度是否高于置信度预设阈值，若是，则输出第一估计位姿作为移动机器人当前定位时刻的候选输出位姿，否则基于校正位姿增量得到当前定位时刻移动机器人的第二估计位姿；

步骤3，判断第二估计位姿的置信度是否大于第一估计位姿的置信度与预设差异阈值之差，若是，则输出第二估计位姿作为移动机器人当前定位时刻的候选输出位姿，否则计算当前周边环境与预设静态环境地图中局部区域的变动程度；

步骤4，判断当前周边环境与预设静态环境地图中局部区域的变动程度是否小于或等于预设变动程度阈值，若是，则输出第一估计位姿作为移动机器人当前定位时刻的候选输出位姿，否则判断航位推算数据是否可用；

步骤5，若航位推算数据不可用或航位推算数据可靠性小于预设可靠性阈值，则输出第二估计位姿作为移动机器人当前定位时刻的候选输出位姿；若航位推算数据可用且航位推算数据可靠性大于预设可靠性阈值，则将航位推算数据转换至预设静态环境地图坐标系中作为移动机器人当前定位时刻的候选输出位姿；

步骤6，基于移动机器人当前定位时刻的候选输出位姿得到移动机器人当前定位时刻的输出估计位姿。

在其中一个实施例中，步骤1中，所述静态环境地图包括概率占据栅格地图文件、相应配置文件和可选的路标特征高清地图文件；