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

权利要求书

1.一种移动机器人粒子滤波定位的位姿校正方法，其特征在于，包括如下步骤：

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

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

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

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

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

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

2.根据权利要求1所述移动机器人粒子滤波定位的位姿校正方法，其特征在于，步骤1中，所述静态环境地图包括概率占据栅格地图文件、相应配置文件和可选的路标特征高清地图文件；

所述第一估计位姿的获取过程为：

加载概率占据栅格地图文件，采用移动机器人的运动模型和传感器测量模型，使用自适应蒙特卡洛AMCL定位算法进行移动机器人的粒子滤波定位，获得当前定位时刻的第一估计位姿。

3.根据权利要求1所述移动机器人粒子滤波定位的位姿校正方法，其特征在于，步骤2中，所述第二估计位姿的获取过程为：

将校正位姿增量与移动机器人前一定位时刻的输出估计位姿进行叠加，得到第二估计位姿。

4.根据权利要求3所述移动机器人粒子滤波定位的位姿校正方法，其特征在于，所述校正位姿增量为移动机器人前一定位时刻与当前定位时刻之间的里程增量值。

5.根据权利要求1或2或3或4所述移动机器人粒子滤波定位的位姿校正方法，其特征在于，步骤1中，所述第一估计位姿的置信度由第一置信占比与第二置信占比加权求和得到；

所述第一置信占比为：

按照第一估计位姿，将当前激光测量点云有效端点的测量坐标投影到预设的静态栅格地图的过程中，所有命中栅格的占据状态之和与有效端点总数所成的比值；

所述第二置信占比为：

按照第一估计位姿，将当前激光测量点云有效端点的测量坐标投影到预设的静态栅格地图的过程中，占据栅格的最近距离小于预设邻域阈值的栅格数量与有效端点的总数的比值。

6.根据权利要求1或2或3或4所述移动机器人粒子滤波定位的位姿校正方法，其特征在于，步骤4中，所述航位推算数据由GNSS/INS融合输出。