移动机器人粒子滤波定位的位姿校正方法及移动机器人

技术领域

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

背景技术

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

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

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

发明内容

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

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

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

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

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

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

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

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

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

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

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

在其中一个实施例中，步骤2中，所述第二估计位姿的获取过程为：

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

在其中一个实施例中，所述校正位姿增量为移动机器人前一定位时刻与当前定位时刻之间的里程增量值。

在其中一个实施例中，步骤1中，所述第一估计位姿的置信度由第一置信占比与第二置信占比加权求和得到；

所述第一置信占比为：

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

所述第二置信占比为：

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

在其中一个实施例中，步骤4中，所述航位推算数据由GNSS/INS融合输出。

在其中一个实施例中，步骤6中，所述基于移动机器人当前时刻的候选输出位姿得到移动机器人当前时刻的输出估计位姿，具体为：

若当前周边环境与预设静态环境地图中局部区域的变动程度小于或等于预设变动程度阈值：

根据移动机器人当前时刻的候选输出位姿与预设静态环境地图生成虚拟点云，将虚拟点云与当前激光测量点云进行扫描匹配，得到点云位姿校正量，并将点云位姿校正量与候选输出位姿进行叠加结合，得到移动机器人当前时刻的输出估计位姿；

若当前周边环境与预设静态环境地图中局部区域的变动程度大于预设变动程度阈值，且航位推算数据可靠性低于预设阈值：

将候选输出位姿作为移动机器人当前时刻的输出估计位姿；

若当前周边环境与预设静态环境地图中局部区域的变动程度大于预设变动程度阈值，且航位推算数据可靠性高于预设阈值：

将航位推算数据转换到地图坐标系下的数值作为移动机器人当前时刻的输出估计位姿。

在其中一个实施例中，所述虚拟点云的生成过程为：

根据当前候选输出位姿和栅格概率地图，以激光扫描传感器按照其测量分辨率产生虚拟射线，虚拟射线与静态环境地图中障碍物相交的方式生成的虚拟点，生成虚拟点云；其中，虚拟点云包括二维点云和三维点云。

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

本发明提供的一种移动机器人粒子滤波定位的位姿校正方法及移动机器人，基于粒子滤波定位的位姿校正算法提高了定位精度，同时提供当前估计位姿的置信度，而且减小了当前运行环境与预设静态地图局部区域所发生变动造成的影响。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图示出的结构获得其他的附图。

图1为本发明实施例中位姿校正方法的流程示意图；

图2为本发明实施例中环境栅格地图。

本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要说明，本发明实施例中所有方向性指示（诸如上、下、左、右、前、后……）仅用于解释在某一特定姿态（如附图所示）下各部件之间的相对位置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。

另外，在本发明中如涉及“第一”、“第二”等的描述仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。

在本发明中，除非另有明确的规定和限定，术语“连接”、“固定”等应做广义理解，例如，“固定”可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接，还可以是物理连接或无线通信连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系，除非另有明确的限定。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。

另外，本发明各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本发明要求的保护范围之内。

如图1所示为本实施例公开的一种移动机器人粒子滤波定位的位姿校正方法，其具体包括如下步骤：

步骤1，基于预先构建好的预设静态环境地图得到当前定位时刻移动机器人的第一估计位姿。其中，静态环境地图包括概率占据栅格地图文件、相应配置文件和可选的路标特征高清地图文件。所述第一估计位姿的获取过程为：加载概率占据栅格地图文件，采用移动机器人的运动模型和传感器测量模型，使用自适应蒙特卡洛AMCL定位算法进行移动机器人的粒子滤波定位，获得当前定位时刻的第一估计位姿。

参考图2，为本实施例中概率占据栅格地图文件中的环境栅格地图。在图2中，箭头表达移动机器人的第一估计位姿的方向角，若干个小方框表示当前二维激光点云测量值中的有效端点，黑色栅格表示被环境中的物体占据，白色栅格表示空闲，灰色栅格表示占据状态未知。其中，黑色栅格、白色栅格和灰色栅格的占据状态与预设环境地图创建算法的配置一致。在本实施例中，黑色格栅为占据栅格，其占据状态为100；白色格栅为空闲栅格，其占据状态为0；灰色格栅为未知栅格。其占据状态为-1。

本实施例中，可选的路标特征高清地图文件中的路标可以由多线激光或单线激光或视觉等方法提取出，包括灯柱、汽车和电动自行车等类别。

本实施例中，所述第一估计位姿的置信度由第一置信占比与第二置信占比加权求和得到，例如第一置信占比与第二置信占比的权重系数分别为0.4与0.6。其中，第一置信占比为：按照第一估计位姿，将当前激光测量点云有效端点的测量坐标投影到预设的静态栅格地图的过程中，所有命中栅格的占据状态之和与有效端点总数所成的比值；所述第二置信占比为：按照第一估计位姿，将当前激光测量点云有效端点的测量坐标投影到预设的静态栅格地图的过程中，占据栅格的最近距离小于预设邻域阈值的栅格数量与有效端点的总数的比值。其中，激光测量点云是激光测量得到的二维或三维点云。

具体地，按照第一估计位姿将当前激光测量点云有效端点的测量坐标投影到预设的静态栅格地图的过程中，每一命中栅格与相距最近的占据栅格之间的距离，为：

式中，

步骤2，判断第一估计位姿的置信度是否高于置信度预设阈值，若是，则输出第一估计位姿作为移动机器人当前定位时刻的候选输出位姿，否则基于校正位姿增量得到当前定位时刻移动机器人的第二估计位姿。即当第一估计位姿的置信度低于置信度预设阈值时，认为第一估计位姿具有定位误差，将前一时刻的输出估计位姿作为第一位姿矫正运算的起始值，与第二校正位姿增量进行叠加结合。其中，第二估计位姿的获取过程为：将校正位姿增量与移动机器人前一定位时刻的输出估计位姿进行叠加，得到第二估计位姿。

本实施例中，校正位姿增量为移动机器人前一定位时刻与当前定位时刻之间的里程增量值。例如扫描匹配里程、轮式里程、惯性导航位姿增量、全球卫星定位转换里程或各里程增量的组合。前一定位时刻的输出估计位姿是经过位姿置信度确认的。由于短时段内来源于里程推算的校正位姿增量的精度较高，其与前一定位时刻的输出估计位姿进行叠加结合，所得到的第二估计位姿能够比第一估计位姿更接近机器人真实位姿。

在具体实施过程中，采用扫描匹配里程计算，扫描匹配里程通过激光里程计计算，激光里程计采用帧间匹配计算的帧间位姿增量。具体为采用laser_scan_matcher开源ROS包进行二维扫描匹配获得扫描匹配里程，或者采用PCL开源点云库中的NDT（正态分布变换）算法和ICP(迭代最近点)算法进行二维或三维点云帧间匹配。

步骤3，判断第二估计位姿的置信度是否大于第一估计位姿的置信度与预设差异阈值之差，若是，则输出第二估计位姿作为移动机器人当前定位时刻的候选输出位姿，否则计算当前周边环境与预设静态环境地图中局部区域的变动程度。由于在当前周边环境相对于预设静态环境地图发生变动等场合下，第二估计位姿的置信度可能略低于第一估计位姿的置信度，在两个置信度之差的绝对值小于预设差异阈值时，取第二估计位姿作为当前候选输出位姿。

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

本实施例中，航位推算数据使用配置基准站的GNSS-RTK或精密单点定位(GNSS-PPP)作为卫星定位数据源，GNSS卫星定位在空旷环境中的精度较高，但在有树木、高楼等建筑较多的环境中受到多径传播及视野中卫星数量较少等因素的影响，卫星定位精度会下降。而且装备GNSS-RTK的移动机器人在运行时会发生卫星定位数据误差变化的现象。在卫星定位的数据存在误差的情况下，GNSS/INS融合提供的航位推算数据精度受到影响。INS惯性导航系统中的惯性元件(IMU)的随机常值零偏漂移及测量误差也对GNSS/INS融合输出的推算数据数据造成影响。

本实施例中，当第二估计位姿的置信度与预设差异阈值之和仍小于第一估计位姿的置信度时，进行当前周边环境与预设静态环境地图中局部区域的变动程度判断。如果当前周边环境与预设静态环境地图中局部区域的变动程度大于预设变动程度阈值，认为当前周边环境相比于预设静态环境地图中局部区域有明显差异，取第二估计位姿作为当前候选输出位姿。否则，认为当前周边环境与预设静态环境地图中局部区域接近，取第一估计位姿作为当前候选输出位姿。当前周边环境与预设静态环境地图中局部区域的变动程度判断，包括：在构建静态环境地图时，对机器人行驶环境的局部区域进行主要特征路标的提取或分类，其具体过程为自动驾驶高清地图领域的常规技术手段，例如：

当前周边环境的子图与预设静态环境地图中局部区域的子图分别进行线段等自然轮廓几何特征提取和聚类，提取主要的自然轮廓几何特征的尺寸和角度范围，通过主要的几何特征对两个子图进行比较。在实施例中，根据当前周边环境及预设静态环境地图中相应的局部区域内的特征路标几何信息，比较各个类别的路标个数、各路标的极坐标及包围框角度范围。路标可以由多线激光或单线激光或视觉等方法提取出。可能包括灯柱、汽车、电动自行车等类别。当前周边环境与预设静态环境地图的变动程度小于预设变动程度阈值时，认为当前周边环境与预设静态环境中的局部区域相似，即两时刻环境中各物体的几何属性相近。当前周边环境与预设静态环境地图中局部区域有差异的情况，将出现在周边环境在局部子图中但不属于静态环境地图局部区域的主要静止物体更新到静态环境高清地图中。

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

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

若当前周边环境与预设静态环境地图中局部区域的变动程度小于或等于预设变动程度阈值：

根据移动机器人当前时刻的候选输出位姿与预设静态环境地图生成虚拟点云，将虚拟点云与当前激光测量点云进行扫描匹配，得到点云位姿校正量，并将点云位姿校正量与候选输出位姿进行叠加结合，得到移动机器人当前时刻的输出估计位姿；

若当前周边环境与预设静态环境地图中局部区域的变动程度大于预设变动程度阈值，且航位推算数据可靠性低于预设阈值：

将候选输出位姿作为移动机器人当前时刻的输出估计位姿；

若当前周边环境与预设静态环境地图中局部区域的变动程度大于预设变动程度阈值，且航位推算数据可靠性高于预设阈值：

将航位推算数据转换到地图坐标系下的数值作为移动机器人当前时刻的输出估计位姿。

本实施例中，虚拟点云的生成过程为：

根据当前候选输出位姿和栅格概率地图，以激光扫描传感器按照其测量分辨率产生虚拟射线，虚拟射线与静态环境地图中障碍物相交的方式生成的虚拟点，生成虚拟点云；其中，虚拟点云包括二维点云和三维点云。

本实施例中，输出估计位姿使用ROS(机器人操作系统)框架和修改其中的AMCL自适应蒙特卡洛定位源码，使用ROS消息机制。移动机器人在全局环境中运行时，预设静态环境地图能够在至少一部分行驶路段上能够与移动机器人周边环境相似，即在一些路段上，机器人周边环境的几何物体有一部分与预设静态环境地图场景相比未发生实质的重大改变。

在可选地实施例中，仅仅在当前周边环境与预设静态环境地图中局部区域的变动程度小于预设变动程度阈值时，根据当前候选输出位姿和概率栅格地图，依据传感器的测量分辨率产生各虚拟射线构成虚拟点云。

在可选地实施例中，在当前周边环境与预设静态环境地图中局部区域的变动程度小于预设变动程度阈值时，当前激光测量点云与虚拟点云进行扫描匹配，计算出点云位姿校正量。扫描匹配使用NDT或PL-ICP等技术，初始值选取可以为零。所述点云位姿校正量与当前候选输出位姿进行叠加结合，得到当前输出估计位姿。

在可选地实施例中，在将点云位姿校正量与当前候选输出位姿进行叠加结合，得到当前输出估计位姿的过程中，附加地，可以计算当前输出估计位姿的置信度。

在可选地实施例中，在当前周边环境与预设静态环境地图的变动程度大于预设变动程度阈值时，例如道路及停车场的两侧车辆泊车及移车等造成环境变动大的场景，允许所述估计位姿置信度小于预设置信度阈值。附加地，可以将输出估计位姿的置信度及对应的运算环节编号作为消息发布。同时，可以将第一估计位姿的置信度作为消息发布。

以上所述仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是在本发明的发明构思下，利用本发明说明书及附图内容所作的等效结构变换，或直接/间接运用在其他相关的技术领域均包括在本发明的专利保护范围内。