具体实施方式

下面结合附图对本发明做进一步的详细说明，以令本领域技术人员参照说明书文字能够据以实施。

需要说明的是，下述实施方案中所述实验方法，如无特殊说明，均为常规方法，所述试剂和材料，如无特殊说明，均可从商业途径获得；在本发明的描述中，术语“横向”、“纵向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，并不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

如图1所示，本发明提供一种高精度的数字孪生场景中的激光融合定位方法，包括：

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

这里用于绝对定位的传感器可以采用激光雷达和GNSS传感器。

通过GNSS传感器获取当前时刻的位置信息，通过遍历姿态信息获取每个姿态信息下数字孪生场景仿真地图的图像信息，这里位置信息即移动机器人的位置坐标，姿态信息即移动机器人的姿态角，由于姿态角在360°范围内取值，可以等差数列的形式试算一系列的姿态角，如30°、60°、90°……360°逐一尝试；

这里获取当前时刻的位置信息还可以采取其他方式，如采用人工输入位置坐标的方式或者直接载入移动机器人最后一次保存的位置坐标的方式。

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

通过马氏距离判别法排除不可信的GNSS传感器数据和真实场景的图像信息；

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

这里当数字孪生场景仿真地图为点云地图时，计算每个姿态信息下点云地图的图像信息与真实场景的图像信息的匹配得分的算法为ICP算法或者NDT算法；

ICP算法能够使不同的坐标下的点云数据合并到同一个坐标系统中，首先是找到一个可用的变换，配准操作实际是要找到从坐标系1到坐标系2的一个刚性变换。ICP算法本质上是基于最小二乘法的最优配准方法。该算法重复进行选择对应关系点对，计算最优刚体变换，直到满足正确配准的收敛精度要求。ICP算法的目的是要找到待配准点云数据与参考云数据之间的旋转参数和平移参数，使得两点数据之间满足某种度量准则下的最优匹配。假设给两个三维点集X1和X2，ICP方法的配准步骤如下：第一步，计算X2中的每一个点在X1点集中的对应近点；第二步，求得使上述对应点对平均距离最小的刚体变换，求得平移参数和旋转参数；第三步，对X2使用上一步求得的平移和旋转参数，得到新的变换点集；第四步，如果新的变换点集与参考点集满足两点集的平均距离小于某一给定阈值，则停止迭代计算，否则新的变换点集作为新的X2继续迭代，直到达到目标函数的要求。

NDT算法是一种很有用途的点云配准方法，是一个一次性初始化工作，不需要消耗大量的代价计算最近邻搜索匹配点，并且概率密度函数在两幅图像采集之间的额时间可以离线计算出来，但仍在存在的问题很多，包括收敛域差、NDT代价函数的不连续性以及稀疏室外环境下不可靠的姿态估计等。NDT算法的步骤(1)该算法的第一步是将扫描占用的空间细分为单元格网格(2D图像中的正方形或3D中的立方体)，基于单元内的点分布计算每个单元的概率分布函数。每个单元格中的概率分布函数可以解释为单元格内表面点的生成过程。将点云投票到各个格子中，计算每个格子的概率分布函数，概率分布函数可以当做表面的近似表达，协方差矩阵的特征向量和特征值可以表达表面信息(朝向、平整度)格子内少于3个点，经常会协方差矩阵不存在逆矩阵，所以只计算点数大于5的单元格，涉及到下采样方法。正态分布给出了点云的分段平滑表示，具有连续导数。每个概率分布函数都可以看作是局部表面的近似值，描述了表面的位置以及它的方向和平滑度。在多维的情况下，平均值和方差由平均向量μ和协方差来描述矩阵Σ。协方差矩阵的对角元素表示方差每个变量，非对角线元素表示的是协方差变量。(2)将第二幅扫描点云的每个点按转移矩阵的变换。(3)第二幅扫描点落于参考帧点云的哪个格子，计算响应的概率分布函数。(4)当使用NDT进行扫描点配准时，目标是找到当前扫描点的位姿，以最大化当前扫描的点位于参考扫描表面上的可能性，该位姿是要优化的参数，也就是说，当前扫描的点云估计的旋转和平移向量。最后求所有点的最优值。

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

高斯牛顿迭代法是非线性回归模型中求回归参数进行最小二乘的一种迭代方法，该法使用泰勒级数展开式去近似地代替非线性回归模型，然后通过多次迭代，多次修正回归系数，使回归系数不断逼近非线性回归模型的最佳回归系数，最后使原模型的残差平方和达到最小。

LM算法是介于牛顿法与梯度下降法之间的一种非线性优化方法，对于过参数化问题不敏感，能有效处理冗余参数问题，使代价函数陷入局部极小值的机会大大减小，这些特性使得LM算法在计算机视觉等领域得到广泛应用。

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

这里第三预设条件由用户自定义设置，上述每种配准算法的匹配得分及其协方差的计算方式在现有技术中已经公开，故不再赘述。

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

这里移动机器人每更新一次实际绝对位姿及其协方差，均将实际绝对位姿及其协方差数据保存于存储设备中，故可直接获取上一时刻的实际绝对位姿及其协方差。

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

这里用于相对定位的传感器可以采用里程计和轮速计。

通过马氏距离判别法排除不可信的里程计传感器数据和轮速计传感器数据。

根据里程计传感器数据，计算当前时刻相对于上一时刻的相对位姿，公式如下：

^k-1P_k＝(^oP_k-1)^-1·^oP_k

其中^k-1P_k为当前时刻相对于上一时刻的相对位姿，^oP_k-1为里程计上一时刻相对于初始时刻的相对位姿，^oP_k为里程计当前时刻相对于初始时刻的相对位姿，协方差的计算则由如下公式计算得到：

首先初始化得到(R,p)＝(I,0)，然后迭代计算以下公式：

(dR,dp)＝(^oP_k-1,m-1)^-1oP_k-1,m

C_m＝F·C_m-1·F^T+G·Q·G^T

(R,p)＝(R·dR,p+R·dp)

其中R为相对位姿，p为相对平移，C为相对位姿的协方差，Q为里程计传感器数据的协方差。

根据轮速计传感器数据，预积分计算得到相对位姿及其协方差，迭代公式如下：

ω＝0.5·(ω_m-1+ω_m)

dR＝exp(ω·dt)

v＝0.5·(v_m-1+dR·v_m)

dp＝v·dt

R_n＝R·dR

p_n＝p+R·dp

C_m＝F·C_m-1·F^T+G·Q·G^T+H·Q·H^T

(R,p)＝(R_n,p_n)

其中R为相对位姿，p为相对平移，C为相对位姿的协方差，ω为角速度，v为线速度，dt为相对时间，Q为轮速计传感器数据的协方差。

采用图优化的方法将通过里程计传感器数据得到的当前时刻相对于上一时刻的预测相对位姿和通过轮速计传感器数据得到的当前时刻相对于上一时刻的预测相对位姿进行融合，将通过里程计传感器数据得到的当前时刻相对于上一时刻的预测相对位姿协方差和通过轮速计传感器数据得到的当前时刻相对于上一时刻的预测相对位姿协方差进行融合。

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

^wR_k＝^wR_k-1·^k-1R_k

C_k＝F·C_k-1·F^T+G·C_rel·G^T

M_k＝C_k ^-1

其中^wR_k为当前时刻的预测绝对位姿，^wR_k-1为上一时刻的实际绝对位姿，^k-1R_k为当前时刻相对于上一时刻的相对位姿，C_k为当前时刻的预测绝对位姿的协方差，C_k-1为当前时刻相对于上一时刻的相对位姿的协方差，C_rel为上一时刻的实际绝对位姿的协方差，M_k为当前时刻的信息矩阵。

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

这里需要注意的是由于GNSS传感器无法获取姿态信息，因此其对应的信息权重应为0。

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

这里第一预设阈值和第一预设条件可用户自定义设置，如设置为若当前时刻的实际绝对位姿的协方差小于第一预设阈值，则取信当前时刻的实际绝对位姿，若当前时刻的实际绝对位姿的协方差大于第一预设阈值，则不取信当前时刻的实际绝对位姿。

这里当出现不取信当前时刻的实际绝对位姿，则通过相对定位的传感器和绝对定位的传感器重新采集数据，重复上述激光融合定位方法，重新计算实际绝对位姿。

在另一实施例中，将当前时刻的观测绝对位姿及其协方差和预测绝对位姿及其协方差通过非线性优化的方式融合前，先对比当前时刻的预测绝对位姿的协方差和第二预设阈值，根据第二预设条件判断是否取信当前时刻的预测绝对位姿，若当前时刻的预测绝对位姿不可信，则将当前时刻的观测绝对位姿及其协方差直接作为当前时刻的实际绝对位姿及其协方差。

这里第二预设阈值和第二预设条件可用户自定义设置，如设置为若当前时刻的预测绝对位姿的协方差小于第二预设阈值，则取信当前时刻的预测绝对位姿，若当前时刻的预测绝对位姿的协方差大于第二预设阈值，则不取信当前时刻的预测绝对位姿。

这里当出现取信当前时刻的预测绝对位姿，则继续将当前时刻的观测绝对位姿和预测绝对位姿进行融合，否则当前时刻的观测绝对位姿及其协方差直接作为当前时刻的实际绝对位姿及其协方差。

尽管本发明的实施方案已公开如上，但其并不仅仅限于说明书和实施方式中所列运用，它完全可以被适用于各种适合本发明的领域，对于熟悉本领域的人员而言，可容易地实现另外的修改，因此在不背离权利要求及等同范围所限定的一般概念下，本发明并不限于特定的细节和这里示出与描述的图例。