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

权利要求书

1.一种融合反光柱及栅格地图的定位方法，其特征在于，包括如下步骤：

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

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

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

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

2.根据权利要求1所述的一种融合反光柱及栅格地图的定位方法，其特征在于，所述获取当前帧中的反光柱数据和对应的里程计位姿数据，具体包括如下步骤：

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

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

3.根据权利要求1所述的一种融合反光柱及栅格地图的定位方法，其特征在于，所述获取当前帧中的反光柱数据和对应的里程计位姿数据之后，还包括如下步骤：

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

4.根据权利要求1所述的一种融合反光柱及栅格地图的定位方法，其特征在于，通过所述约束关系优化待定位对象的位姿，是采用极大似然估计计算得到当前时刻待定位对象的位姿。

5.根据权利要求4所述的一种融合反光柱及栅格地图的定位方法，其特征在于，所述采用极大似然估计计算得到当前时刻待定位对象的位姿，采用如下公式获得：

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

6.根据权利要求1所述的一种融合反光柱及栅格地图的定位方法，其特征在于，所述通过局部概率栅格地图跟踪并利用最小二乘方法对待定位对象的位姿进行寻优，包括如下步骤：

检测上一帧中反光柱数据和当前帧中反光柱数据中相同反光柱的情况，若检测到的相同反光柱数目小于3个，则等待下一帧数据；若检测到的相同反光柱数目不小于3个，则利用最小二乘方法对前后两帧数据的待定位对象的运动量进行寻优，以寻优后的运动量进一步优化当前待定位对象的位姿。

7.根据权利要求6所述的一种融合反光柱及栅格地图的定位方法，其特征在于，所述检测上一帧中反光柱数据和当前帧中反光柱数据中相同反光柱的情况，包括如下步骤：

获取上一帧中反光柱数据生成的概率栅格地图；

根据当前帧待定位对象的位姿，计算当前帧数据中反光柱在概率栅格地图上的概率；

判断概率是否低于预设阈值，若是，则当前帧中反光柱与上一帧中反光柱数据为不相同观测数据；若否，且当前帧中反光柱数据离当前激光雷达中心位置及上一帧中激光雷达中心的位置、距离和夹角均满足预设阈值条件，则当前帧中反光柱与上一帧中反光柱数据为相同观测数据。

8.根据权利要求7所述的一种融合反光柱及栅格地图的定位方法，其特征在于，还包括如下步骤：当当前帧定位位姿与栅格地图第一帧数据的相对运动量达到预设阈值时，重置栅格地图，所述重置栅格地图包括重置栅格地图的概率值、重置地图原点和更新局部概率栅格地图。