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

1.本发明涉及移动机器人导航领域，尤其涉及一种超宽带雷达和激光雷达融合定位方法。


背景技术：

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


技术实现要素：

6.本发明的目的是提供了一种超宽带雷达和激光雷达融合定位方法、设备及存储存储介质，能在超宽带雷达和激光雷达的几何信息发生退化以及超宽带雷达出现非视距误差时及时纠正，为在具有高复杂性以及空间范围较广的环境中的移动机器人提供高精度定位，进而解决现有技术中存在的上述技术问题。
7.本发明的目的是通过以下技术方案实现的：
8.一种超宽带雷达和激光雷达融合定位方法，包括：
9.步骤1，分别从超宽带雷达获得原始距离数据以及从激光雷达获得原始点云数据；
10.步骤2，分别确定超宽带雷达信息的退化程度与激光雷达信息的退化程度；其中，通过非视距误差消除神经网络模型对从超宽带雷达获得的原始距离数据进行状态判断，去
除非视距误差后得到距离信息，利用距离信息判断超宽带雷达信息的退化程度；通过将从激光雷达获得的原始点云数据通过点云特征提取后得到点云特征，利用点云特征判断激光雷达信息的退化程度；
11.步骤3，利用激光雷达的点云特征进行特征匹配，将激光雷达特征匹配点与超宽带雷达的距离信息进行融合，根据超宽带雷达信息的退化程度和激光雷达信息的退化程度自适应调整两者融合比例，得出精确的移动机器人的位姿估计。
12.一种处理设备，包括：
13.至少一个存储器，用于存储一个或多个程序；
14.至少一个处理器，能执行所述存储器所存储的一个或多个程序，在一个或多个程序被处理器执行时，使得所述处理器能实现本发明所述的方法。
15.一种可读存储介质，存储有计算机程序，当计算机程序被处理器执行时能实现本发明所述的方法。
16.与现有技术相比，本发明所提供的超宽带雷达和激光雷达融合定位方法、设备及存储介质，其有益效果包括：
17.通过先确定激光雷达和超宽带雷达的信息退化程度，再在利用超宽带雷达的距离信息和激光雷达的点云特征进行融合定位时，根据两者信息的退化程度自适应调整两者融合比例，使得该方法能在激光雷达和超宽带雷达的几何信息发生退化以及超宽带雷达出现nlos误差时，及时纠正定位误差，从而使移动机器人在各种复杂环境中实现高精度定位。
附图说明
18.为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域的普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他附图。
19.图1为本发明实施例提供的超宽带雷达和激光雷达融合定位方法的流程图。
20.图2为本发明实施例提供的超宽带雷达和激光雷达融合定位方法的技术路线图。
21.图3为本发明实施例提供的方法中的uwb和激光雷达融合策略示意图。
22.图4为本发明实施例提供的方法中消除nlos影响的技术路线图。
23.图5为本发明实施例提供的方法中非视距误差消除神经网络模型的结构图。
具体实施方式
24.下面结合本发明的具体内容，对本发明实施例中的技术方案进行清楚、完整地描述；显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例，这并不构成对本发明的限制。基于本发明的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明的保护范围。
25.首先对本文中可能使用的术语进行如下说明：
26.术语“和/或”是表示两者任一或两者同时均可实现，例如，x和/或y表示既包括“x”或“y”的情况也包括“x和y”的三种情况。
27.术语“包括”、“包含”、“含有”、“具有”或其它类似语义的描述，应被解释为非排它
性的包括。例如：包括某技术特征要素(如原料、组分、成分、载体、剂型、材料、尺寸、零件、部件、机构、装置、步骤、工序、方法、反应条件、加工条件、参数、算法、信号、数据、产品或制品等)，应被解释为不仅包括明确列出的某技术特征要素，还可以包括未明确列出的本领域公知的其它技术特征要素。
28.术语“由
……
组成”表示排除任何未明确列出的技术特征要素。若将该术语用于权利要求中，则该术语将使权利要求成为封闭式，使其不包含除明确列出的技术特征要素以外的技术特征要素，但与其相关的常规杂质除外。如果该术语只是出现在权利要求的某子句中，那么其仅限定在该子句中明确列出的要素，其他子句中所记载的要素并不被排除在整体权利要求之外。
29.除另有明确的规定或限定外，术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解，例如：可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本文中的具体含义。
30.术语“中心”、“纵向”、“横向”、“长度”、“宽度”、“厚度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”、“顺时针”、“逆时针”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述和简化描述，而不是明示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本文的限制。
31.下面对本发明所提供的超宽带雷达和激光雷达融合定位方法进行详细描述。本发明实施例中未作详细描述的内容属于本领域专业技术人员公知的现有技术。本发明实施例中未注明具体条件者，按照本领域常规条件或制造商建议的条件进行。本发明实施例中所用试剂或仪器未注明生产厂商者，均为可以通过市售购买获得的常规产品。
32.如图1所示，本发明实施例提供一种超宽带雷达和激光雷达融合定位方法，能实现自适应的超宽带雷达和激光雷达融合定位，包括如下步骤：
33.步骤1，分别从超宽带雷达获得原始距离数据以及从激光雷达获得原始点云数据；
34.步骤2，分别确定超宽带雷达信息的退化程度与激光雷达信息的退化程度；其中，通过非视距误差消除神经网络模型对从超宽带雷达获得的原始距离数据进行状态判断，去除非视距误差后得到距离信息，利用距离信息判断超宽带雷达信息的退化程度；通过将从激光雷达获得的原始点云数据通过点云特征提取后得到点云特征，利用点云特征判断激光雷达信息的退化程度；
35.步骤3，利用激光雷达的点云特征进行特征匹配，将激光雷达特征匹配点与超宽带雷达的距离信息进行融合，根据超宽带雷达信息的退化程度和激光雷达信息的退化程度自适应调整两者融合比例，得出精确的移动机器人的位姿估计。
36.上述方法的步骤3中，按以下方式利用超宽带雷达的距离信息和激光雷达的点云特征分别进行特征匹配融合，包括：
37.通过找出移动机器人在世界坐标系的姿态ξk在每个时刻tk的最优估计作为定位目标，ξk定义为：
38.ξk＝[exkeykezkx
k y
k zk]
t
；
[0039]
其中，exk、eyk、ezk分别表示移动机器人在世界坐标系中在x、y、z轴上的朝向xk、yk、zk分别表示移动机器人在世界坐标系中在x、y、z轴上的位置；
[0040]
将上述最优估计表述为如下非线性最小二乘问题，再进行求解：
[0041][0042]
其中，r
lidar
表示激光雷达的测量残差，r
uwb
表示超宽带雷达的测量残差；n与m分别为激光雷达测量残差的个数与超宽带雷达测量残差的个数；γ为调整激光雷达测量残差和超宽带雷达测量残差在状态估计中贡献比例的比例因子，能根据激光雷达信息和超宽带雷达信息退化程度自动增加和减少该比例因子γ进行自适应调整激光雷达信息与超宽带雷达信息两者融合比例；
[0043]
最后，通过以下非线性迭代来更新位置的状态向量ξ：
[0044][0045]
上述方法的步骤2中，用超宽带雷达距离信息相关的费舍尔信息矩阵特征值确定超宽带雷达信息的退化程度，包括：
[0046]
超宽带雷达中，超宽带雷达基站pa的坐标为(xa，ya，za)，测量的距离为dis，则距离残差为：
[0047][0048]
于是
[0049][0050][0051][0052]
即超宽带雷达雅可比矩阵
[0053]
根据超宽带雷达的距离信息可以得到与超宽带雷达相关的费舍尔信息矩阵iu(ξ)如下：
[0054][0055]
其中σ
uwb
表示uwb测距信息协方差。
[0056]
如果超宽带雷达相关的费舍尔信息矩阵iu(ξ)的第i个特征值e
i，u
＜e
thr，u
，则超宽带雷达的信息是退化的，退化方向是e
i，u
对应的特征向量v
i，u
，记录退化个数为nu。
[0057]
用激光雷达点云特征相关的费舍尔信息矩阵特征值确定激光雷达信息的退化程度，包括：
[0058]
激光雷达点云特征中，当前特征点p
ib
相对于状态ξ的似然函数p(p
ib
；ξ)表示为：
[0059][0060]
其中，pb是激光雷达点云特征中所有特征点的集合；
[0061]
于是：
[0062][0063]
其中，σ
lidar
表示激光雷达数据的协方差矩阵；
[0064]
通过上述似然函数p(p
ib
；ξ)能计算出与激光雷达点云特征相关的费舍尔信息矩阵的特征值i
l
(ξ)为：
[0065][0066]
其中，j
l，k
表示激光雷达测量残差的雅可比矩阵；
[0067]
如果激光雷达点云特征相关的费舍尔信息矩阵i
l
(ξ)的第i个特征值e
i，l
＜e
thr，l
，则确定激光雷达信息是退化的，退化方向是e
i，l
对应的特征向量v
i，l
，记录退化个数为n
l
；
[0068]
根据激光雷达信息的退化个数和超宽带雷达信息的退化个数按以下公式自适应调整比例因子γ，公式为：
[0069][0070]
上述方法的步骤2中，按以下方式通过非视距误差消除神经网络模型对从超宽带雷达获得的原始距离数据进行状态判断，去除非视距误差后得到距离信息，包括：
[0071]
步骤21，构建一个由超宽带雷达标签与基站k测距距离的变化量δd
kt
、超宽带雷达标签接收到的基站k返回信号强度的变化量δr
kt
和当前时刻移动机器人的里程计的测量数据odom
t
以及每一个时刻的移动机器人的位置误差poserr
t
作为标签的数据集；
[0072]
步骤22，利用所述数据集训练非视距误差消除神经网络；
[0073]
步骤23，通过训练好的非视距误差消除神经网络对超宽带雷达
[0074]
获得的原始距离数据进行状态判断，根据判断结果得到去除非视距误差的距离信息。
[0075]
上述的步骤4中，按以下方式计算得出超宽带雷达标签与基站k测距距离的变化量δd
kt
，包括：
[0076]
δd
kt
＝d
kt-d
k(t-1)
[0077]
其中，d
kt
为在时刻t记录的超宽带雷达标签与基站k的测距距离，k＝1，2
…
n；
[0078]
按以下方式计算得出超宽带雷达标签接收到的基站k返回信号强度的变化量δr
kt
，包括：
[0079]
δr
kt
＝r
kt-r
k(t-1)
[0080]
其中，r
kt
为在时刻t记录的超宽带雷达标签接收到的基站k返回信号强度；
[0081]
按以下方式计算得出移动机器人的位置误差poserr
t
，包括：
[0082]
poserr
t
＝posw
t-pos
t
[0083]
其中，pos
t
为时刻t的移动机器人的估计位置；posw
t
为时刻t的移动机器人的真实位置。
[0084]
上述的步骤23中，通过训练好的非视距误差消除神经网络对超宽带雷达获得的原始距离数据进行状态判断，给出对应超宽带雷达的原始距离数据下基站移除的概率，根据给出的概率移除对应基站后，得到去除非视距误差的距离信息。
[0085]
上述的步骤23中，非视距误差消除神经网络模型的结构采用由输入层、全连接层和输出层组成的线性神经网络模型；其中，
[0086]
输入层的初始输入为超宽带雷达标签与基站k测距距离的变化量δd
kt
、超宽带雷达标签接收到的基站k返回信号强度的变化量δr
kt
和当前时刻移动机器人的里程计的测量数据odom
t
，经过特征提取后作为中间输出，该中间输出作为全连接层的输入；
[0087]
全连接层由两层隐藏层组成，激活函数使用relu函数；
[0088]
输出层的激活函数使用sigmoid函数，该输出层的输出为基站是否保留的浮点概率值向量，每个浮点概率值范围为0～1，对该浮点概率值向量中的每维元素所对应的基站进行按概率增删。
[0089]
本发明实施例还提供一种处理设备，包括：
[0090]
至少一个存储器，用于存储一个或多个程序；
[0091]
至少一个处理器，能执行所述存储器所存储的一个或多个程序，在一个或多个程序被处理器执行时，使得所述处理器能实现本发明所述的方法。
[0092]
本发明实施例进一步提供一种可读存储介质，存储有计算机程序，当计算机程序被处理器执行时能实现本发明所述的方法。
[0093]
综上可见，本发明实施例的融合定位方法，能在激光雷达和uwb的几何信息发生退化以及uwb出现nlos误差时及时纠正定位误差，进而达到了使移动机器人在具有高复杂性以及空间范围较广的环境中实现高精度定位的效果。
[0094]
为了更加清晰地展现出本发明所提供的技术方案及所产生的技术效果，下面以具体实施例对本发明实施例所提供的自适应的超宽带雷达和激光雷达融合定位方法进行详细描述。
[0095]
实施例1
[0096]
如图1、图2所示，本发明提供一种自适应的uwb和激光雷达融合定位方法，该方法由以下三个部分组成：
[0097]
(1)构建uwb和激光雷达融合策略(参见图3)：对于提出的融合算法，在传感器信息退化条件下，能够及时调整偏好策略降低退化影响并快速恢复定位精度。通过调整融合比例实现当激光雷达信息退化很严重时，应更偏向于uwb信息，当uwb信息退化严重时，应更偏向与激光雷达信息。
[0098]
(2)判断传感器信息退化程度：退化分析的目的是回答来自特定传感器的当前测量值是否包含足够的信息来估计机器人的状态。具有良好几何分布的测量可以准确地估计机器人的状态，而较差几何分布的测量则会导致较大的估计误差。
[0099]
(3)消除uwb的nlos误差(即非视距误差)的影响(参见图4)：当出现nlos误差时，能够及时检测处于nlos状态的uwb基站并及时处理。可以知道，对于nlos的判断可以利用惯性导航系统短期定位精确的特点，分析uwb的距离测量来检测nlos误差，算出基站与估计位姿之间的距离，将其与uwb的测量距离比较，通过设定阈值判断uwb状态。但人为设定阈值需考虑该泛化问题。
[0100]
下面结合本发明实施例中的附图，对本发明实施例中的技术方案进行完整地描述。
[0101]
图1、图2给出了本发明方法的主要技术路线，首先，分别从uwb和激光雷达这两个传感器获得数据。对于激光雷达，将激光雷达获得的原始点云数据通过点云特征提取之后得到点云特征，进而判断激光雷达信息的退化程度。对于uwb，将uwb的原始距离信息经过一个神经网络，去除nlos误差之后，再判断uwb信息的退化程度。接下来进行uwb和激光雷达的融合定位，利用激光雷达的点云特征和uwb的距离信息分别进行特征匹配融合，根据激光雷达和uwb的退化分析确定的退化程度，调整两者融合比例，进而得出精确的机器人位姿估计。
[0102]
上述方法中，uwb和激光雷达融合策略具体如下：
[0103]
对于构建uwb和激光雷达融合策略的问题，本发明实施例直接使用激光雷达扫描点云和由uwb测量距离的原始数据，如图3所示，其中，移动机器人的姿态在世界坐标系的定义如下：
[0104]
ξk＝[exkeykezkx
k y
k zk]
t
[0105]
上式中，前三项为移动机器人在世界坐标系中的朝向，后三项为移动机器人在世界坐标系中的位置；
[0106]
实现定位的目标就是在每个时刻tk上找到ξk的最优估计；具体做法是将该估计表述为如下非线性最小二乘问题，再进行求解：
[0107][0108]
其中，r为传感器测量残差，n和m为激光雷达测量残差的个数以及uwb测量残差个数。利用比例因子γ来调整激光雷达残差和uwb残差在状态估计中提供的贡献比例，这也是需要自适应调整的项。
[0109]
最后，通过以下非线性迭代来更新位置的状态向量ξ：
[0110][0111]
传感器信息退化分析方式如下：
[0112]
对于判断传感器信息退化程度的问题，本发明实施例采用传感器信息相关的费舍尔信息矩阵(fishers information matrix，fim)的特征值来确定退化程度。
‘
[0113]
超宽带雷达中，uwb基站pa，其坐标为(xa，ya，za)，测量的距离为dis，则距离残差：
[0114][0115]
于是
[0116][0117][0118][0119]
即uwb雅可比矩阵
[0120]
根据uwb的距离信息可以得到与uwb相关的费舍尔信息矩阵iu(ξ)如下：
[0121][0122]
其中σ
uwb
表示uwb测距信息协方差
[0123]
对于iu(ξ)的第i个特征值e
i，u
，如果e
i，u
＜e
thr，u
，则uwb的信息是退化的。退化方向是e
i，u
对应的特征向量v
i，u
，并记录退化个数nu；
[0124]
激光雷达点云特征中，当前特征点p
ib
相对于状态ξ的似然函数p(p
ib
；ξ)表示为：
[0125][0126]
其中，pb是激光雷达点云特征中所有特征点的集合，σ
lidar
表示激光雷达数据的协方差，
[0127]
于是：
[0128][0129]
其中，σ
lidar
表示激光雷达数据的协方差矩阵；
[0130]
通过上述似然函数p(p
ib
；ξ)能计算出与激光雷达测量相关的费舍尔信息矩阵的特征值i
l
(ξ)为：
[0131][0132]
其中，j
l，k
表示激光雷达测量残差的雅可比矩阵；
[0133]
对于i
l
(ξ)的第i个特征值e
i，l
，如果e
i，l
＜e
thr，l
，则确定激光雷达信息是退化的，退化方向是e
i，l
对应的特征向量v
i，l
，并记录退化个数n
l
；
[0134]
参见图3，比例因子的调整策略为：
[0135]
上述方法中，消除uwb的nlos误差的影响的方式如下：
[0136]
对于消除nlos误差影响的问题，本发明实施例中通过一种基于机器学习对uwb基
站进行动态增删，采用的为本发明实例训练神经网络所用的数据集是由多个仿真环境中采集到的数据以及在多个真实环境中采集到的数据。具体做法是，先搭建非视距误差消除神经网络模型开始训练，再在真实环境和仿真环境中进行测试，如图5所示。
[0137]
数据集按以下方式构建：在某个时刻t，记录uwb标签与基站k的测距距离d
kt
(k＝1，2...n)，uwb标签接收到的基站k返回信号的强度r
kt
，以及当前时刻移动机器人的里程计(odometry)的测量数据odom
t
，以及移动机器人当前时刻t的估计位置pos
t
和真实位置posw
t
，于是在t时刻，uwb标签与基站k测距距离的变化量δd
kt
，该δd
kt
计算方式如下：
[0138]
δd
kt
＝d
kt-d
k(t-1)
；
[0139]
uwb标签接收到基站k的返回信号强度变化量为δr
kt
，该δr
kt
计算方式如下：
[0140]
δr
kt
＝r
kt-r
k(t-1)
；
[0141]
移动机器人的位置的误差poserr
t
通过以下公式计算得出：
[0142]
poserr
t
＝posw
t-pos
t
。
[0143]
本发明实施例的机器学习算法训练所用的数据集由δd
kt
，δr
kt
，odom
t
以及每一个时刻的poserr
t
作为标签组成。在本步骤中，训练完成后的非视距误差消除神经网络模型将给出特定状态下基站移除的概率，该特定状态指的是uwb的测距信息，概率是指对应基站按照概率移除；
[0144]
根据给出的概率移除某些基站后，接着利用上述的融合定位策略，对机器人的估计位置进行更新，从而产生更准确的估计位置。
[0145]
该方法可以在激光雷达和uwb的几何信息发生退化以及uwb出现nlos误差时及时纠正定位误差，以达到机器人在具有高复杂性以及空间范围较广的环境中实现高精度定位效果。
[0146]
实施例2
[0147]
为了便于理解，下面结合一个具体示例进行说明。显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明的保护范围。
[0148]
在本实施例中，将本发明的自适应的uwb和激光雷达融合定位方法，在真实环境中进行测试。首先，选取一个移动机器人作为本实施例的机器人平台，选用常见的velodyne16线激光雷达以及dwm1000模组作为uwb传感器。根据uwb定位的特点，选取8个基站的定位方式，在无遮挡的情况下，误差10em以内。机器人平台上搭配配有amd ryzen 5600h的计算机。
[0149]
采用的深度学习算法训练非视距误差消除神经网络模型，其输出向量为基站所对应的是否增删的概率值，该概率值为连续值，范围0～1，神经网络算法的训练在一台操作系统为ubuntu的上位机(该机内存为16g，cpu为intel i7，显卡为rtx 3090)上进行，采用的深度学习框架为keras，其后端为tensorflow；所述训练神经网络全连接层中的两层隐藏层神经元个数分别为512和128，输出层为8，分别对应8个uwb的基站。
[0150]
如图5所示，为机器学习算法训练的非视距误差消除神经网络模型，该模型结构是线性神经网络，由输入层、全连接层和输出层组成；全连接层则由两层隐藏层(激活函数使用relu函数)和输出层(激活函数使用sigmoid函数)组成，该全连接层可根据实际需求自行定义；用于该训练神经网络的训练数据集为筛选后的uwb数据集：δd
kt
，δr
kt
，odom
t
以及每一个时刻的poserr
t
作为标签组成；δd
kt
，δr
kt
，odom
t
为预训练神经网络的输入，经过特征
提取后的输出作为全连接层的输入，整个预训练神经网络的输出为基站是否保留的浮点概率值向量(每个浮点概率值范围为0～1)，根据该浮点概率值向量中的每维元素，对其所对应的基站进行按概率增删。由于训练神经网络所用数据包含uwb的距离信息，强度信息以及当前时刻机器人的位移信息，输入信息包含全面，所以使得uwb基站的动态增删更加智能化，可以适用于不同的环境。由于对神经网络进行训练时，其网络输出为基站是否进行移除的概率，所以当uwb处于严重的nlos时，能最大限度的保留有用信息。
[0151]
综上可见，本发明实施例的融合定位方法，相比于目前的单独基于激光雷达的定位算法，可以在激光雷达的几何信息发生退化以及产生累计误差时或在uwb基站几何分布较差时及时纠正定位误差，并且通过调整比例因子γ来调整激光雷达残差和uwb残差在状态估计中提供的贡献比例，达到自适应的效果。
[0152]
本领域普通技术人员可以理解：实现上述实施例方法中的全部或部分流程是可以通过程序来指令相关的硬件来完成，所述的程序可存储于一计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，所述的存储介质可为磁碟、光盘、只读存储记忆体(read-only memory，rom)或随机存储记忆体(random access memory，ram)等。
[0153]
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明披露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求书的保护范围为准。本文背景技术部分公开的信息仅仅旨在加深对本发明的总体背景技术的理解，而不应当被视为承认或以任何形式暗示该信息构成已为本领域技术人员所公知的现有技术。