[发明专利]一种多传感器融合的定位方法及系统

权利要求书

1.一种多传感器融合的定位方法，其特征在于，包括：

获取目标区域的先验地图和多传感器数据；所述多传感器数据包括惯性测量数据、激光雷达数据和GPS数据；

根据所述先验地图和所述多传感器数据建立位姿因子；所述位姿因子包括GPS因子和地图定位因子；

根据所述先验地图、所述惯性测量数据和所述激光雷达数据建立转换因子；所述转换因子包括激光里程计因子、惯性测量传感器预积分因子和闭环因子；

根据所述GPS因子和所述转换因子，采用因子图优化方法确定所述目标区域的优化位姿；

判断所述优化位姿的协方差是否小于设定协方差阈值；

若是，则根据所述优化位姿对所述目标区域进行定位；

若否，则采用所述地图定位因子对所述优化位姿进行更新，得到优化更新位姿，并根据所述优化更新位姿对所述目标区域进行定位。

2.根据权利要求1所述的多传感器融合的定位方法，其特征在于，所述根据所述先验地图和所述多传感器数据建立位姿因子，具体包括：

对所述GPS数据进行滤波，将滤波后的GPS数据作为GPS因子；

根据所述先验地图、所述激光雷达数据、所述惯性测量数据和所述GPS因子进行正态分布转换点云配准定位，得到地图定位因子。

3.根据权利要求2所述的多传感器融合的定位方法，其特征在于，所述对所述GPS数据进行滤波，将滤波后的GPS数据作为GPS因子，具体包括：

将所述GPS数据进行格式转换，得到局部笛卡尔坐标系下的GPS数据；

对所述局部笛卡尔坐标系下的GPS数据进行扩展卡尔曼滤波，得到GPS因子。

4.根据权利要求2所述的多传感器融合的定位方法，其特征在于，所述根据所述先验地图、所述激光雷达数据、所述惯性测量数据和所述GPS因子进行正态分布转换点云配准定位，得到地图定位因子，具体包括：

对所述激光雷达数据进行降采样，得到降采样后的激光雷达数据；

根据所述降采样后的激光雷达数据、所述惯性测量数据和所述GPS因子构建局部地图；

将所述先验地图与所述局部地图进行正态分布转换点云配准定位，得到地图定位因子。

5.根据权利要求1所述的多传感器融合的定位方法，其特征在于，所述根据所述先验地图、所述惯性测量数据和所述激光雷达数据建立转换因子，具体包括：

根据所述激光雷达数据和所述先验地图进行特征点匹配运算，得到激光里程计因子；

根据所述惯性测量数据和所述激光雷达数据进行预积分运算，得到惯性测量传感器预积分因子；

根据所述激光里程计因子和所述惯性测量数据进行回环检测，并根据回环检测结果构建闭环因子。

6.根据权利要求5所述的多传感器融合的定位方法，其特征在于，所述根据所述激光雷达数据和所述先验地图进行特征点匹配运算，得到激光里程计因子，具体包括：

对所述激光雷达数据进行特征提取，得到当前帧特征点；

对所述先验地图进行特征提取，得到地图特征点；

将所述地图特征点和所述当前帧特征点进行特征点匹配运算，得到激光里程计因子。

7.根据权利要求5所述的多传感器融合的定位方法，其特征在于，所述根据所述惯性测量数据和所述激光雷达数据进行预积分运算，得到惯性测量传感器预积分因子，具体包括：

对所述激光雷达数据进行关键帧提取，得到激光雷达关键帧信息；

根据所述惯性测量数据和所述激光雷达关键帧信息进行预积分运算，得到惯性测量传感器预积分因子。

8.根据权利要求5所述的多传感器融合的定位方法，其特征在于，所述根据所述激光里程计因子和所述惯性测量数据进行回环检测，并根据回环检测结果构建闭环因子，具体包括：

对所述激光里程计因子进行位姿优化，得到当前帧优化位姿；

将所述当前帧优化位姿和所述惯性测量数据进行数据融合，得到融合后的当前帧位姿；

对所述融合后的当前帧位姿进行回环检测，当所述融合后的当前帧位姿满足所述回环检测的判别条件时，构建闭环因子。