一种基于高斯模型的AMCL算法的定位方法及存储介质

技术领域

本发明涉及机器人定位领域，尤其涉及一种基于高斯模型的AMCL算法的定位方法及存储介质。

背景技术

AMCL(自适应蒙特卡洛算法)算法是一种常见的用于机器人在拥有先验地图信息下利用激光雷达等感知传感器对机器人周边环境感知后进行定位的算法，其使用粒子滤波器来针对已知的地图跟踪机器人的姿势。相对于普通的蒙特卡洛算法，自适应蒙特卡洛算法体现在：解决了机器人绑架问题，它会在发现粒子们的平均分数突然降低了，意味着正确的粒子在某次迭代中被抛弃了的时候，在全局再重新的撒一些粒子；解决了粒子数固定的问题，因为有时候当机器人定位成功时，粒子都集中在一块了，还要维持这么多的粒子没必要，这个时候可以适当的减少粒子的数量，降低定位的系统资源消耗。本质上算法还是采用蒙特卡洛的算法和粒子滤波的算法进行定位，蒙特卡洛算法指的是在一个已知的有先验环境信息的环境下采用粒子的分布的数量多少来作为机器人在该区域内位置的概率。

AMCL定位算法虽然采用了似然域模型去解决波束模型带来的不平滑性以及计算量大的问题，但是整体定位思路还是基于粒子滤波的思想。鲁棒性在静态环境下能得到保证但是在实际应用过程中经常会遇到行人较多导致环境中动态障碍物比较多的情况。在这种情况下，传统意义上的AMCL方法很难解决定位的初始化问题即定位初始化容易失败以及在环境变化较大如传感器被动态障碍遮挡下的连续定位问题。

发明内容

针对上述技术问题，本发明的主要目的在于提供一种基于高斯模型的AMCL算法的定位方法，以解决机器人定位初始化容易失败以及在环境变化较大如传感器被动态障碍遮挡下的连续定位问题。

为解决上述问题，本发明提供一种基于高斯模型的AMCL算法的定位方法，包括步骤：

S1，响应接收到的激光数据，根据设定好的转换参数将激光数据从激光的坐标系数据转换到地图坐标系数据。在机器人定位中激光传感器向固定的方向发射激光束，发射出的激光遇到障碍物会被反射，传感器响应并接收反射回来的激光数据并传输给计算机系统，计算机系统根据设定好的转换参数将激光坐标系数据转换为地图坐标系数据。

S2，根据地图坐标系数据滤除超过地图范围和超过传感器量程的数据，以对激光数据的合法性进行检查。根据地图坐标的纵横坐标系数可以判断已接收到的激光数据是否在地图范围内，如超出范围由计算机直接滤除超范围的激光数据。

S3，判断地图坐标系数据中测量点的最近占据栅格是否小于阈值，若小于阈值，标记测量点为激光匹配点；通过将测量到的实际位置与该位置附近的先验地图信息匹配，如果实际位置附近小于阈值范围内有先验地图信息，则表示当前位置信息是正确的，系统标记当前测量点为激光匹配点。

S4，将激光匹配点与占据栅格的实际距离通过高斯模型进行归一化，并将归一化后数据分为三组分别计算累计后的归一化距离；归一化的目的是使激光匹配点距离缩放到相同的数据区间和范围，以减少规模、特征、分布差异对模型的影响，将归一化后激光匹配点与占据栅格的距离分为三组，分别对这三组数据进行累加计算。

S5，将三组归一化距离进行加权平均，计算单帧激光数据的激光匹配分数；将三组通过累加计算后的数据进行加权平均，得到单帧激光数据的激光匹配分数。

S6，根据激光匹配分数确定激光数据的定位准确性。激光匹配分数会一直在收到激光数据时累加，在AMCL算法中原有的位置发布之前增加一个与原有的判断条件逻辑相与的条件即如果匹配分数的均值小于设定的阈值则不发布当前帧的定位数据，此时认定为定位不够准确的状态。否则则发布当前帧的定位数据信息。

优选地，步骤S3具体包括步骤：

S31，设置占据栅格阈值K；根据占据栅格地图的栅格大小设定占据栅格阈值。

S32，判断地图坐标系数据中测量点的最近占据栅格距离是否小于阈值K；通过将测量到的实际位置与该位置附近的先验地图信息匹配，判断测量点附近是否先验占据栅格信息。

S33，若小于阈值K，标记测量点为激光匹配点；

S34，若大于阈值K，则滤除测量点。

优选地，判断地图坐标系数据中测量点的最近占据栅格距离，具体包括：将激光测距传感器反馈的信息与先验地图信息进行匹配，若激光测距传感器测量到的实际位置附近小于阈值K的范围内有先验占据栅格信息，则认定此处的激光点与实际情况相符，反馈激光点的位置信息。

优选地，在步骤S4中，高斯模型归一化具体包括：通过高斯模型归一化将传感器的距离信息通过高斯过程重整至[0,1]的范围内。将距离信息归一化可以减少数据规模、特征、分布差异对数据模型的影响。

优选地，所述步骤S6还包括步骤：

S61，在持续收到所述激光数据时对所述激光数据对应的所述激光匹配分数进行累加；

S62，在AMCL算法中增加所述激光匹配分数判断条件，并设定阈值M；

S63，判断所述累加后激光匹配分数的均值是否小于M，若小于M，则认定定位不够准确，不发布当前帧激光数据对应的定位数据。

优选地，所述步骤S62还设定有阈值N，其中，M>N；所述步骤S6还包括步骤：

S64，判断所述累加激光匹配分数的均值是否小于N，若小于N，则认定实际定位失效，上报定位失败信息。为了确保定位信息的准确率，系统会设定一个阈值范围，通过系统判断，只有在阈值范围内的定位信息才会被发布出去。

本发明还提供一种计算机可读存储介质，计算机可读存储介质上存储有的计算机程序，计算机程序被处理器执行时实现一种基于高斯模型的AMCL算法的定位方法的各个步骤。

本发明的一种基于高斯模型的AMCL算法的定位方法，通过步骤S1，响应接收到的激光数据，根据设定好的转换参数将激光数据从激光的坐标系数据转换到地图坐标系数据；S2，根据地图坐标系数据滤除超过地图范围和超过传感器量程的数据，以对激光数据的合法性进行检查；S3，判断地图坐标系数据中测量点的最近占据栅格是否小于阈值，若小于阈值，标记测量点为激光匹配点；S4，将激光匹配点与占据栅格的实际距离通过高斯模型进行归一化，并将归一化后数据分为三组分别计算累计后的归一化距离；S5，将三组归一化距离进行加权平均，计算单帧激光数据的激光匹配分数；S6，根据激光匹配分数确定激光数据的定位准确性。提高了AMCL算法在动态环境下的定位效率与定位准确性，解决了机器人定位初始化容易失败以及在环境变化较大如传感器被动态障碍遮挡下的连续定位问题。

附图说明

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

图1为本发明一个实施例中基于高斯模型的AMCL算法的定位方法的流程示意图。

图2为本发明一个实施例中步骤S3子步骤的流程示意图。

图3为本发明一个实施例中步骤S6子步骤的流程示意图。

具体实施方式

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

参考图1，本发明提供一种基于高斯模型的AMCL算法的定位方法，包括步骤：

S1，响应接收到的激光数据，根据设定好的转换参数将激光数据从激光的坐标系数据转换到地图坐标系数据。在机器人定位中激光传感器向固定的方向发射激光束，发射出的激光遇到障碍物会被反射，传感器响应并接收反射回来的激光数据形成激光距离数据并传输给计算机系统，计算机系统根据设定好的转换参数将激光坐标系数据转换为地图坐标系数据。在本实施例中用到的机器人地图为占据栅格地图，选用占据栅格地图可以滤除传感器获取到的噪音数据。

S2，根据地图坐标系数据滤除超过地图范围和超过传感器量程的数据，以对激光数据的合法性进行检查。根据占据栅格地图坐标的纵横坐标系数和占据栅格可以判断已接收到的激光数据是否在地图范围内，如超出地图范围由计算机直接滤除超范围的激光数据。

S3，判断地图坐标系数据中测量点的最近占据栅格是否小于阈值，若小于阈值，标记测量点为激光匹配点；通过将激光传感器测量到的每个测量点的实际位置与该位置附近的先验地图信息匹配，如果实际位置附近小于阈值范围内有先验地图信息，则表示当前测量到的位置信息是正确的，系统标记当前测量点为激光匹配点。

S4，将激光匹配点与占据栅格的实际距离通过高斯模型进行归一化，并将归一化后数据分为三组分别计算累计后的归一化距离；归一化的目的是使激光匹配点距离缩放到相同的数据区间和范围，以减少规模、特征、分布差异对模型的影响，本实施例中的高斯模型归一化指的是将传感器的距离信息通过高斯过程将其重整至[0,1]的范围内。高斯过程是一种抛弃参数模型，直接定义函数上的先验概率分布的算法，在函数组成的不可数的无穷空间中对概率分布进行计算是比较困难的。但是，对于一个有限的训练数据集，只需要考虑训练数据集合测试数据集的输入处的函数值即可，因此在实际应用中基于高斯过程可以在有限空间中进行计算输出结果。将高斯模型归一化后激光匹配点与占据栅格的距离数据分为三组，分别对这三组数据进行累加计算。

S5，将三组归一化距离进行加权平均，计算单帧激光数据的激光匹配分数；将三组通过累加计算后的数据进行加权平均，得到单帧激光数据的激光匹配分数，通过加权平均求平均值，平均值可作为位置是否都是的强判断依据。

S6，根据激光匹配分数确定激光数据的定位准确性。激光匹配分数会一直在收到激光数据时累加，在AMCL算法中原有的位置发布之前增加一个与原有的判断条件逻辑相与的条件即如果匹配分数的均值小于设定的阈值则不发布当前帧的定位数据，此时认定为定位不够准确的状态。否则则发布当前帧的定位数据信息。

本发明的实施例中提供的一种基于高斯模型的AMCL算法的定位方法，通过高斯模型在AMCL算法中的运用，提高AMCL算法在动态环境下的定位效率与定位准确性，AMCL算法采用了似然域模型去解决波束模型带来的不平滑性以及计算量大的问题，在静态环境中能够很好的定位，但是在动态环境中因为动态障碍物存在导致很难收敛到正确的位置，通过在AMCL算法中加入基于高斯过程的匹配分数，来判断动态环境下定位的连续性和准确性，从而提高定位效率。基于高斯过程的匹配分数是通过计算激光测量点与占据栅格地图的距离得到激光匹配点，因为是基于静态的地图匹配，可以有效滤除人群导致的定位丢失问题，通过对正确匹配点与地图之间距离的高斯模型归一化得到匹配分数，通过匹配分数的量化评估可以判断出定位的连续性和准确性。

其中，步骤S3具体包括步骤：

S31，设置占据栅格阈值K；系统根据占据栅格地图的栅格大小设定占据栅格阈值。

S32，判断地图坐标系数据中测量点的最近占据栅格距离是否小于阈值K；通过将测量到的实际位置与该位置附近的先验地图信息匹配，判断测量点附近是否先验占据栅格信息。如果有先验占据栅格信息，则说明该栅格被占据，即该测量点的位置与实际激光点位置相符。

S33，若小于阈值K，标记测量点为激光匹配点；若小于阈值范围内有先验占据栅格信息，则该测量点的位置与实际激光点位置相符，标记该测量点为激光匹配点。

S34，若大于阈值K，则滤除测量点。若大于阈值范围内有或者没有先验占据栅格信息，则该测量点的位置与实际激光点位置不相符，则滤除该测量点。

其中，判断地图坐标系数据中测量点的最近占据栅格距离，具体包括：将激光测距传感器反馈的信息与先验地图信息进行匹配，若激光测距传感器测量到的实际位置附近小于阈值K的范围内有先验占据栅格信息，则认定此处的激光点与实际情况相符，反馈激光点的位置信息。

其中，在步骤S4中，高斯模型归一化具体包括：通过高斯模型归一化将传感器的距离信息通过高斯过程重整至[0,1]的范围内。将距离信息归一化可以减少数据规模、特征、分布差异对数据模型的影响。

其中，步骤S6还包括步骤：

S61，在持续收到激光数据时对激光数据对应的激光匹配分数进行累加；

S62，在AMCL算法中增加激光匹配分数判断条件，并设定阈值M；

S63，判断累加后激光匹配分数的均值是否小于M，若小于M，则认定定位不够准确，不发布当前帧激光数据对应的定位数据。

其中，步骤S62还设定有阈值N，其中，M>N；所述步骤S6还包括步骤：

S64，判断累加激光匹配分数的均值是否小于N，若小于N，则认定实际定位失效，上报定位失败信息。为了确保定位信息的准确率，系统会设定一个阈值范围，通过系统判断，只有在阈值范围内的定位信息才会被发布出去。平均值是判断定位是否丢失的强判断依据，只有均值保持在一定范围内，才能说明当前定位是连续且准确的。然而通过设定M和N两个阈值，可以对判断结果进行分级，匹配分数小于M表示定位不够准确，匹配分数小于N表示定位已经失效，由此判断定位的连续性和准确性。

本发明还提供一种计算机可读存储介质，计算机可读存储介质上存储有基于高斯模型的AMCL算法的定位方法的计算机程序，计算机程序被处理器执行时实现基于高斯模型的AMCL算法的定位方法的各个步骤。

需要说明的是，由于计算机可读存储介质的计算机程序被处理器执行时实现上述的方法的步骤，因此上述方法的所有实施例均适用于该计算机可读存储介质，且均能达到相同或相似的有益效果。

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

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