超宽带雷达和激光雷达融合定位方法、设备及存储介质技术

本发明专利技术公开了一种超宽带雷达和激光雷达融合定位方法、设备及存储介质，属移动机器人导航领域，包括：步骤1，分别从超宽带雷达和激光雷达获得数据；步骤2，分别确定超宽带雷达信息的退化程度与激光雷达信息的退化程度；步骤3，利用超宽带雷达的距离信息和激光雷达的点云特征分别进行特征匹配融合，根据超宽带雷达信息的退化程度和激光雷达信息的退化程度自适应调整两者融合比例，得出精确的移动机器人的估计位姿。该方法能在激光雷达和超宽带雷达的几何信息发生退化以及超宽带雷达出现NLOS误差时，及时纠正定位误差，从而使移动机器人在各种复杂环境中实现高精度定位。在各种复杂环境中实现高精度定位。在各种复杂环境中实现高精度定位。

【技术实现步骤摘要】
超宽带雷达和激光雷达融合定位方法、设备及存储介质


[0001]本专利技术涉及移动机器人导航领域，尤其涉及一种超宽带雷达和激光雷达融合定位方法。

技术介绍

[0002]移动机器人在复杂的环境中进行导航的关键技术之一就是准确的计算自身的位置，即定位(Localization)。而室内应用环境(如工厂、仓库、商场等)往往具有“非静态”、“布局易变化”等特点，这对移动机器人的准确定位带来了巨大的挑战。
[0003]目前，激光雷达(Lidar)和超宽带雷达(Ultra
‑
WideBand，UWB)在室内机器人定位研究中应用广泛。并由于激光雷达与UWB可以优势互补、克服彼此的短处，进而提高姿态估计的精度和定位算法的鲁棒性，形成了激光雷达与UWB融合定位方法。但现有的激光雷达与UWB融合定位方法都没有考虑到激光雷达和UWB的传感器信息退化，激光雷达在几何特征退化的环境中会导致特征点较少影响定位精度，UWB定位准确性也与UWB基站的几何分布有关，UWB基站的不良几何分布将导致UWB的位姿估计精确度低。现有的融合定位方法无法根据环境的特点进行动态调节，做到自适应的融合定位。此外当信标与UWB基站之间出现遮挡时，非视距(Non Line of Sight，NLOS)误差也会对定位精度有很大影响。对于UWB的非视距误差，基于接收信号特征的方法需要预先人为设定判别非视距误差的阈值，不具备自适应性，且不同地点不同环境中的阈值可能有所不同，缺少泛化能力。基于测距残差信息的方法信号传播路径损失模型受到许多因素的影响，人工选择的特征可能不足以用这些分类器进行分类，同时进行特征提取以及训练也需要大量的计算开销。在非视距误差严重区域，标签与多个基站同时出现非视距误差将导致测量数据不足。因此，不根据定位的需求，简单的排斥基站信号将导致定位精度的进一步下降。
[0004]综合上述可知，如何寻求一种能自适应超宽带雷达和激光雷达融合的定位方法，是移动机器人导航领域急需解决的问题。
[0005]有鉴于此，特提出本专利技术。

技术实现思路

[0006]本专利技术的目的是提供了一种超宽带雷达和激光雷达融合定位方法、设备及存储存储介质，能在超宽带雷达和激光雷达的几何信息发生退化以及超宽带雷达出现非视距误差时及时纠正，为在具有高复杂性以及空间范围较广的环境中的移动机器人提供高精度定位，进而解决现有技术中存在的上述技术问题。
[0007]本专利技术的目的是通过以下技术方案实现的：
[0008]一种超宽带雷达和激光雷达融合定位方法，包括：
[0009]步骤1，分别从超宽带雷达获得原始距离数据以及从激光雷达获得原始点云数据；
[0010]步骤2，分别确定超宽带雷达信息的退化程度与激光雷达信息的退化程度；其中，通过非视距误差消除神经网络模型对从超宽带雷达获得的原始距离数据进行状态判断，去
除非视距误差后得到距离信息，利用距离信息判断超宽带雷达信息的退化程度；通过将从激光雷达获得的原始点云数据通过点云特征提取后得到点云特征，利用点云特征判断激光雷达信息的退化程度；
[0011]步骤3，利用激光雷达的点云特征进行特征匹配，将激光雷达特征匹配点与超宽带雷达的距离信息进行融合，根据超宽带雷达信息的退化程度和激光雷达信息的退化程度自适应调整两者融合比例，得出精确的移动机器人的位姿估计。
[0012]一种处理设备，包括：
[0013]至少一个存储器，用于存储一个或多个程序；
[0014]至少一个处理器，能执行所述存储器所存储的一个或多个程序，在一个或多个程序被处理器执行时，使得所述处理器能实现本专利技术所述的方法。
[0015]一种可读存储介质，存储有计算机程序，当计算机程序被处理器执行时能实现本专利技术所述的方法。
[0016]与现有技术相比，本专利技术所提供的超宽带雷达和激光雷达融合定位方法、设备及存储介质，其有益效果包括：
[0017]通过先确定激光雷达和超宽带雷达的信息退化程度，再在利用超宽带雷达的距离信息和激光雷达的点云特征进行融合定位时，根据两者信息的退化程度自适应调整两者融合比例，使得该方法能在激光雷达和超宽带雷达的几何信息发生退化以及超宽带雷达出现NLOS误差时，及时纠正定位误差，从而使移动机器人在各种复杂环境中实现高精度定位。
附图说明
[0018]为了更清楚地说明本专利技术实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本专利技术的一些实施例，对于本领域的普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他附图。
[0019]图1为本专利技术实施例提供的超宽带雷达和激光雷达融合定位方法的流程图。
[0020]图2为本专利技术实施例提供的超宽带雷达和激光雷达融合定位方法的技术路线图。
[0021]图3为本专利技术实施例提供的方法中的UWB和激光雷达融合策略示意图。
[0022]图4为本专利技术实施例提供的方法中消除NLOS影响的技术路线图。
[0023]图5为本专利技术实施例提供的方法中非视距误差消除神经网络模型的结构图。
具体实施方式
[0024]下面结合本专利技术的具体内容，对本专利技术实施例中的技术方案进行清楚、完整地描述；显然，所描述的实施例仅仅是本专利技术一部分实施例，而不是全部的实施例，这并不构成对本专利技术的限制。基于本专利技术的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本专利技术的保护范围。
[0025]首先对本文中可能使用的术语进行如下说明：
[0026]术语“和/或”是表示两者任一或两者同时均可实现，例如，X和/或Y表示既包括“X”或“Y”的情况也包括“X和Y”的三种情况。
[0027]术语“包括”、“包含”、“含有”、“具有”或其它类似语义的描述，应被解释为非排它
性的包括。例如：包括某技术特征要素(如原料、组分、成分、载体、剂型、材料、尺寸、零件、部件、机构、装置、步骤、工序、方法、反应条件、加工条件、参数、算法、信号、数据、产品或制品等)，应被解释为不仅包括明确列出的某技术特征要素，还可以包括未明确列出的本领域公知的其它技术特征要素。
[0028]术语“由
……
组成”表示排除任何未明确列出的技术特征要素。若将该术语用于权利要求中，则该术语将使权利要求成为封闭式，使其不包含除明确列出的技术特征要素以外的技术特征要素，但与其相关的常规杂质除外。如果该术语只是出现在权利要求的某子句中，那么其仅限定在该子句中明确列出的要素，其他子句中所记载的要素并不被排除在整体权利要求之外。
[0029]除另有明确的规定或限定外，术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解，例如：可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以根本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种超宽带雷达和激光雷达融合定位方法，其特征在于，包括：步骤1，分别从超宽带雷达获得原始距离数据以及从激光雷达获得原始点云数据；步骤2，分别确定超宽带雷达信息的退化程度与激光雷达信息的退化程度；其中，通过非视距误差消除神经网络模型对从超宽带雷达获得的原始距离数据进行状态判断，去除非视距误差后得到距离信息，利用距离信息判断超宽带雷达信息的退化程度；通过将从激光雷达获得的原始点云数据通过点云特征提取后得到点云特征，利用点云特征判断激光雷达信息的退化程度；步骤3，利用激光雷达的点云特征进行特征匹配，将激光雷达特征匹配点与超宽带雷达的距离信息进行融合，根据超宽带雷达信息的退化程度和激光雷达信息的退化程度自适应调整两者融合比例，得出精确的移动机器人的位姿估计。2.根据权利要求1所述的超宽带雷达和激光雷达融合定位方法，其特征在于，所述步骤3中，按以下方式利用超宽带雷达的距离信息和激光雷达的点云特征分别进行特征匹配融合，包括：通过找出移动机器人在世界坐标系的姿态ξ
k
在每个时刻t
k
的最优估计作为定位目标，ξ
k
定义为：ξ
k
＝[ex
k ey
k ez
k x
k y
k z
k
]
T
；其中，ex
k
、ey
k
、ez
k
分别表示移动机器人在世界坐标系中在x、y、z轴上的朝向，x
k
、y
k
、z
k
分别表示移动机器人在世界坐标系中在x、y、z轴上的位置；将上述最优估计表述为如下非线性最小二乘问题，再进行求解：其中，r
Lidar
表示激光雷达的测量残差，r
UWB
表示超宽带雷达的测量残差；n与m分别为激光雷达测量残差的个数与超宽带雷达测量残差的个数；γ为调整激光雷达测量残差和超宽带雷达测量残差在状态估计中贡献比例的比例因子，能根据激光雷达信息和超宽带雷达信息退化程度自动增加和减少该比例因子γ进行自适应调整激光雷达信息与超宽带雷达信息两者融合比例。最后，通过以下非线性迭代来更新位置的状态向量ξ:3.根据权利要求1或2所述的超宽带雷达和激光雷达融合定位方法，其特征在于，所述步骤2中，用超宽带雷达距离信息相关的费舍尔信息矩阵特征值确定超宽带雷达信息的退化程度，包括：超宽带雷达中，超宽带雷达基站P
a
的坐标为(x
a
,y
a
,z
a
)，测量的距离为dis，则距离残差为：于是
即超宽带雷达雅可比矩阵根据超宽带雷达的距离信息得到与超宽带雷达相关的费舍尔信息矩阵I
u
(ξ)为：其中，σ
uwb
表示超宽带雷达测距信息协方差；如果超宽带雷达相关的费舍尔信息矩阵I
u
(ξ)的第i个特征值e
i,u
<e
thr,u
，则超宽带雷达的信息是退化的，退化方向是e
i,u
对应的特征向量v
i,u
，记录退化个数为n
u
；用激光雷达点云特征相关的费舍尔信息矩阵特征值确定激光雷达信息的退化程度，包括：激光雷达点云特征中，当前特征点P
iB
相对于状态ξ的似然函数P(P
iB
；ξ)表示为：其中，P
B
是激光雷达点云特征中所有特征点的集合；于是：其中，σ
Lidar
表示激光雷达数据的协方差矩阵；通过上述似然函数P(P
iB
；ξ)能计算出与激光雷达点云特征相关的费舍尔信息矩阵的特征值I
l
(ξ)为：其中，J
l,k
表示激光雷达测量残差的雅可比矩阵；如果激光雷达点云特征相关的费舍尔信息矩阵I
l
(ξ)的第i个特征值e
...

【专利技术属性】
技术研发人员：吴锋，阮志康，
申请(专利权)人：中国科学技术大学，
类型：发明
国别省市：