[发明专利]高精度的数字孪生场景中的激光融合定位方法

权利要求书

1.高精度的数字孪生场景中的激光融合定位方法，其特征在于，包括：

获取用于绝对定位的传感器数据，利用绝对定位的传感器数据与预存的数字孪生场景仿真地图进行匹配计算，得到当前时刻的观测绝对位姿及其协方差；

获取上一时刻的实际绝对位姿及其协方差；

获取用于相对定位的传感器数据，根据用于相对定位的传感器数据通过预积分计算得到当前时刻相对于上一时刻的相对位姿及其协方差；

基于上一时刻的实际绝对位姿及其协方差，结合当前时刻相对于上一时刻的相对位姿及其协方差，通过预积分计算得到当前时刻的预测绝对位姿及其协方差；

将当前时刻的观测绝对位姿及其协方差和预测绝对位姿及其协方差通过非线性优化的方式融合，得到当前时刻的实际绝对位姿及其协方差；

对比当前时刻的实际绝对位姿的协方差和第一预设阈值，根据第一预设条件判断是否取信当前时刻的实际绝对位姿。

2.如权利要求1所述的高精度的数字孪生场景中的激光融合定位方法，其特征在于，用于相对定位的传感器至少有两种，每种用于相对定位的传感器数据单独计算当前时刻相对于上一时刻的预测相对位姿及其协方差，再采用图优化的方法将所有用于相对定位的传感器数据得到的当前时刻相对于上一时刻的预测相对位姿进行融合、当前时刻相对于上一时刻的预测相对位姿的协方差进行融合。

3.如权利要求1所述的高精度的数字孪生场景中的激光融合定位方法，其特征在于，将当前时刻的观测绝对位姿及其协方差和预测绝对位姿及其协方差通过非线性优化的方式融合前，先对比当前时刻的预测绝对位姿的协方差和第二预设阈值，根据第二预设条件判断是否取信当前时刻的预测绝对位姿，若当前时刻的预测绝对位姿不可信，则将当前时刻的观测绝对位姿及其协方差直接作为当前时刻的实际绝对位姿及其协方差。

4.如权利要求2所述的高精度的数字孪生场景中的激光融合定位方法，其特征在于，用于相对定位的传感器包括里程计和轮速计。

5.如权利要求1所述的高精度的数字孪生场景中的激光融合定位方法，其特征在于，根据用于相对定位的传感器数据计算当前时刻相对于上一时刻的相对位姿及其协方差前，先通过马氏距离判别法排除不可信的用于相对定位的传感器数据。

6.如权利要求1所述的高精度的数字孪生场景中的激光融合定位方法，其特征在于，根据用于绝对定位的传感器数据计算当前时刻的观测绝对位姿及其协方差前，先通过马氏距离判别法排除不可信的用于绝对定位的传感器数据。

7.如权利要求1所述的高精度的数字孪生场景中的激光融合定位方法，其特征在于，用于绝对定位的传感器包括激光雷达和GNSS传感器。

8.如权利要求7所述的高精度的数字孪生场景中的激光融合定位方法，其特征在于，利用绝对定位的传感器数据与预存的数字孪生场景仿真地图进行匹配计算，得到当前时刻的观测绝对位姿及其协方差的方法包括：

通过GNSS传感器获取当前时刻的位置信息，通过遍历姿态信息获取每个姿态信息下数字孪生场景仿真地图的图像信息；

通过激光雷达获取真实场景的图像信息；

计算每个姿态信息下数字孪生场景仿真地图的图像信息与真实场景的图像信息的匹配得分；

当某一姿态信息下数字孪生场景仿真地图的图像信息与真实场景的图像信息的匹配得分符合第三预设条件时，将该姿态信息和当前时刻的位置信息作为当前时刻的观测绝对位姿，根据该姿态信息下的匹配得分计算当前时刻的观测绝对位姿的协方差。

9.如权利要求8所述的高精度的数字孪生场景中的激光融合定位方法，其特征在于，当数字孪生场景仿真地图为点云地图时，计算每个姿态信息下点云地图的图像信息与真实场景的图像信息的匹配得分的算法为ICP算法或者NDT算法；

当数字孪生场景仿真地图为栅格地图时，计算每个姿态信息下栅格地图的图像信息与真实场景的图像信息的匹配得分的算法为高斯牛顿算法或者LM算法。