[发明专利]一种融合反光柱及栅格地图的定位方法及装置

说明书

本发明公开一种融合反光柱及栅格地图的定位方法，包括如下步骤：获取当前帧中的反光柱和对应的里程计位姿数据；检测局部概率栅格地图是否初始化，若为否，则以当前帧反光柱和对应的里程计位姿数据，更新局部概率栅格地图；若为是，则以当前帧反光柱和对应的里程计位姿数据为初值，通过局部概率栅格地图跟踪，得到更准确的约束关系，通过约束关系优化待定位对象的位姿；获取上一帧反光柱数据和当前帧反光柱数据的对应关系，通过局部概率栅格地图跟踪并利用最小二乘方法对待定位对象的位姿进行寻优；以寻优后待定位对象的位姿以及当前帧的反光柱数据，更新栅格地图。本发明能够实时连续的反馈位姿信息，保证建图和定位的稳定性。

技术领域

本发明涉及定位技术领域，尤其涉及一种融合反光柱及栅格地图的定位方法及装置。

背景技术

基于激光的定位方式分为基于反光柱的定位方式和基于轮廓的定位方式；基于轮廓的定位方式计算量大，受环境影响严重，相对精度较差，无法适应移动机器人精密操作的要求；基于反光柱的定位方式计算量小，定位精度高、稳定性好。基于反光柱的定位方式关键是反光柱匹配，国内外现有的反光柱定位技术，均采用了几何三角定位原理；通过检测到的反光柱与反光柱地图几何匹配来获取激光位姿，此种方案利用视野内反光柱形状特征的唯一性，来保证重复定位的精度，且定位过程中需要保证环境部署中，反光柱不能出现对称分布及局部分布相似，这种需求对反光柱部署有着较高的要求，特别是在大场景下，实施难度很大；且在实际应用中，由于存在各种复杂运动及复杂地面环境，很难保证基于几何三角定位的定位位置的连续性和稳定性。

发明内容

为了解决上述技术问题，本发明提出一种融合反光柱及栅格地图的定位方法及装置。

为了达到上述目的，本发明的技术方案如下：

一种融合反光柱及栅格地图的定位方法，包括如下步骤：

获取当前帧中的反光柱数据和对应的里程计位姿数据；

检测局部概率栅格地图是否初始化，若检测结果为否，则以当前帧中反光柱数据和对应的里程计位姿数据，更新局部概率栅格地图；若检测结果为是，则以当前帧中反光柱数据和当前里程计位姿数据为初值，通过局部概率栅格地图跟踪，得到更准确的约束关系，通过所述约束关系优化待定位对象的位姿；

获取上一帧反光柱数据和当前帧反光柱数据的对应关系，通过局部概率栅格地图跟踪并利用最小二乘方法对待定位对象的位姿进行寻优，获取寻优后待定位对象的位姿；

以寻优后待定位对象的位姿以及当前帧的反光柱数据，更新栅格地图。

优选地，所述获取当前帧中的反光柱数据和对应的里程计位姿数据，具体包括如下步骤：

基于反光柱反射强度，提取当前帧数据中的反光柱数据；

以当前激光雷达中心为原点，计算当前帧数据的反光柱位置。

优选地，所述获取当前帧中的反光柱数据和对应的里程计位姿数据之后，还包括如下步骤：

检测当前帧数据中提取的反光柱数目，若检测到的相同的反光柱数目小于3个，则等待下一帧数据；若检测到的相同反光柱数目不小于3个，则进入下一步，检测局部概率栅格地图是否初始化。

优选地，通过所述约束关系优化待定位对象的位姿，是采用极大似然估计计算得到当前时刻待定位对象的位姿。

优选地，所述采用极大似然估计计算得到当前时刻待定位对象的位姿，采用如下公式获得：

式中，M^ref为局部反光柱地图数据，p_i为当前帧中的反光柱数据，符号代表旋转平移变换；符号||{M^ref,.}代表点到局部反光柱数据的欧氏距离，P为计算过程中的位姿。