CN112444246B - 高精度的数字孪生场景中的激光融合定位方法 - Google Patents

Abstract

本发明公开了一种高精度的数字孪生场景中的激光融合定位方法，其包括：利用绝对定位的传感器数据与预存的数字孪生场景仿真地图进行匹配计算，得到当前时刻的观测绝对位姿及其协方差；获取上一时刻的实际绝对位姿及其协方差；根据用于相对定位的传感器数据计算得到当前时刻相对于上一时刻的相对位姿及其协方差；基于上一时刻的实际绝对位姿及其协方差，结合当前时刻相对于上一时刻的相对位姿及其协方差，计算得到当前时刻的预测绝对位姿及其协方差；将当前时刻的观测绝对位姿及其协方差和预测绝对位姿及其协方差融合，得到当前时刻的实际绝对位姿及其协方差。本发明实现了定位方法的高精度，高鲁棒性，高可扩展性以及高时效性等特点。

Description

高精度的数字孪生场景中的激光融合定位方法

技术领域

本发明涉及移动机器人定位技术领域。更具体地说，本发明涉及一种高精度的数字孪生场景中的激光融合定位方法。

背景技术

随着计算机技术和人工智能的发展，智能自主移动机器人成为机器人领域的一个重要研究方向和研究热点。移动机器人的定位是自主移动机器人的眼睛，是该领域的热点研究问题，也是决定机器人是否自主移动的关键问题之一。自主移动机器人导航过程需要回答3个问题：“我在哪里？”，“我要去哪里？”和“我怎样到达那里？”。移动机器人定位技术就是要解决第1个问题。准确来说，移动机器人定位的目的就是确定机器人在其运动环境中的世界地图的坐标，即机器人通过感知获取环境信息，经过相关的信息处理而确定自身位置的过程。而现有的机器人定位过程，可分为相对定位和绝对定位：相对定位也叫作位姿跟踪，假定机器人初始位姿，采用相邻时刻传感器信息对机器人位置进行跟踪估计，然而相对定位的计算过程中会有误差累积导致定位精度差；绝对定位又称为全局定位，完成机器人的全局定位需要预先确定好环境模型，通过外部传感器直接向机器人提供外界位置信息，计算机器人在全局坐标系中的位置，而绝对定位过程中需要传感器采集外界信息，这个过程中容易受到外界光线干扰，也容易导致定位不准确。

发明内容

本发明的一个目的是解决至少上述问题，并提供至少后面将说明的优点。

本发明还有一个目的是提供一种高精度的数字孪生场景中的激光融合定位方法，通过融合相对定位与绝对定位技术，采用松耦合及非线性优化方法，实现定位方法的高精度，高鲁棒性，高可扩展性以及高时效性等特点。

为了实现根据本发明的这些目的和其它优点，提供了一种高精度的数字孪生场景中的激光融合定位方法，包括：

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

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

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

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

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

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

优选的是，用于相对定位的传感器至少有两种，每种用于相对定位的传感器数据单独计算当前时刻相对于上一时刻的预测相对位姿及其协方差，再采用图优化的方法将所有用于相对定位的传感器数据得到的当前时刻相对于上一时刻的预测相对位姿进行融合、当前时刻相对于上一时刻的预测相对位姿的协方差进行融合。

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

优选的是，用于相对定位的传感器包括里程计和轮速计。

优选的是，根据用于相对定位的传感器数据计算当前时刻相对于上一时刻的相对位姿及其协方差前，先通过马氏距离判别法排除不可信的用于相对定位的传感器数据。

优选的是，根据用于绝对定位的传感器数据计算当前时刻的观测绝对位姿及其协方差前，先通过马氏距离判别法排除不可信的用于绝对定位的传感器数据。

优选的是，用于绝对定位的传感器包括激光雷达和GNSS传感器。

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

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

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

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

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

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

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

本发明至少包括以下有益效果：可以根据用户实际情况设置多种不同的传感器进行融合，可扩展性强，并且不同传感器可信度不同，因此，可以配置相应的测量协方差来达到不同的测量需求，同时，各个阶段都会对传感器异常值进行检查和排除，这保证了定位的高鲁棒性，而相对定位融合绝对定位可以消除相对定位带来的累积误差，通过融合不同传感器，得到高精度的定位数据。

本发明的其它优点、目标和特征将部分通过下面的说明体现，部分还将通过对本发明的研究和实践而为本领域的技术人员所理解。

附图说明

图1为本发明所述激光融合定位方法的流程图。

具体实施方式

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

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

如图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、对比当前时刻的实际绝对位姿的协方差和第一预设阈值，根据第一预设条件判断是否取信当前时刻的实际绝对位姿。

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

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

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

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

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

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

Claims (6)

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

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

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

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

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

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

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

其中，用于相对定位的传感器包括里程计和轮速计，每种用于相对定位的传感器数据单独计算当前时刻相对于上一时刻的预测相对位姿及其协方差，再采用图优化的方法将所有用于相对定位的传感器数据得到的当前时刻相对于上一时刻的预测相对位姿进行融合、当前时刻相对于上一时刻的预测相对位姿的协方差进行融合；

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

^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为轮速计传感器数据的协方差；

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

^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为当前时刻的信息矩阵；

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

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

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

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

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

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

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

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

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

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

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