一种SLAM与UWB的实时融合与建图方法

一种slam与uwb的实时融合与建图方法
技术领域
1.本发明涉及机器人定位与导航技术领域，特别是一种slam与uwb的实时融合与建图方法。


背景技术：

2.slam(simulaneous localizaion and mapping)已在移动机器人领域中有着广泛的应用。由于雷达(lidar)能够准确测量到附近物体的距离，因此许多方法都使用了雷达，但slam性能非常容易受到累积错误的影响。
3.在基于lidar的slam中可以使用uwb传感器融合的方法消除累积误差并增强鲁棒性。但是uwb的距离分辨率低于lidar(激光测距误差约为1cm，约为uwb测距误差的十分之一)，直接在uwb定位结果之上构建lidar地图的融合效果并不理想。


技术实现要素：

4.鉴于此，本发明提供一种slam与uwb的实时融合与建图方法，将lidar传感器与uwb(ultra wide band)传感器融合在一起，融合uwb和lidar以提高基于uwb的定位和映射的准确性和鲁棒性，并提供锚点坐标以及周围环境的详细地图信息。
5.本发明公开了一种slam与uwb的实时融合与建图方法，包括：
6.步骤1、在携带激光雷达的智能体的移动过程中，根据分布式算法，得到当前时刻所述智能体与各超宽带节点的定位信息；
7.步骤2、基于扩展卡尔曼滤波算法，对当前时刻的所述定位信息进行修正；
8.步骤3、基于高斯-牛顿方法求取当前时刻所述智能体的最优位姿，采用所述最优位姿矫正所述智能体当前时刻的位姿估计以及各超宽带节点的位置，同时更新激光雷达地图以及uwb地图；将步骤3输出的当前时刻的所述智能体的位姿估计以及各超宽带节点的位置作为步骤2的输入；
9.步骤4、当所述智能体移动到下一时刻时，令下一时刻等于当前时刻，重复执行步骤1至步骤3，直至所述智能体完成预设的任务。
10.进一步地，所述步骤1包括：
11.所述智能体进入未知区域后，抛投位置已知的超宽带节点和位置未知的超宽带节点，并与各超宽带节点建立通讯，测得相互之间的距离；智能体在区域中不断移动，收集各个位置的激光雷达反馈数据。
12.进一步地，所述步骤1中的智能体在移动的过程中，当不满足预设条件时，需要继续在其周围抛投超宽带节点；所述预设条件为所述智能体周围可测量范围内的超宽带节点数量大于3个。
13.进一步地，所述步骤2包括：
14.根据步骤1输出的所述智能体和各超宽带节点的定位信息构建动态观测模型；
15.采用扩展卡尔曼滤波算法对所述动态观测模型进行更新，得到更新的状态估计和
更新的协方差估计，即完成对对所述定位信息的修正。
16.进一步地，所述动态观测模型包括状态噪声；所述定位信息包括位姿及速度。
17.进一步地，利用基于高斯-牛顿方法的扫描匹配过程，将当前时刻观测到的光束端点与当前时刻的激光雷达地图进行匹配，以找到最佳的变换从而更好地匹配。
18.进一步地，所述扫描匹配过程在一个多分辨率网格中进行，从低分辨率的网格映射到高分辨率的网格映射，从而更有可能找到全局解而不是被困在局部解中。
19.进一步地，在建立所述激光雷达地图时，通过变换和旋转，设定一个超宽带节点固定在原点，设定另一个超宽带节点始终位于正x轴上；两个设定的超宽带节点的速度为零；在以两个设定的超宽带节点建立的局部框架下构建和更新激光雷达地图。
20.由于采用了上述技术方案，本发明具有如下的优点：将lidar传感器与uwb(ultra wide band)传感器融合在一起，融合uwb和lidar以提高基于uwb的定位和映射的准确性和鲁棒性，并提供锚点坐标以及周围环境的详细地图信息。
附图说明
21.为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明实施例中记载的一些实施例，对于本领域普通技术人员来讲，还可以根据这些附图获得其他的附图。
22.图1为本发明实施例的一种slam与uwb的实时融合与建图方法的流程示意图；
23.图2为本发明实施例的uwb节点定位坐标系示意图；
24.图3为本发明实施例的节点拓扑网络结构图；
25.图4为本发明实施例的仿真环境示意图；
26.图5为本发明实施例的建图过程示意图；
27.图6为本发明实施例的建图效果示意图；
28.图7为本发明实施例的建图地图输出图。
具体实施方式
29.结合附图和实施例对本发明作进一步说明，显然，所描述的实施例仅是本发明实施例一部分实施例，而不是全部的实施例。本领域普通技术人员所获得的所有其他实施例，都应当属于本发明实施例保护的范围。
30.实施例一：
31.参见图1，本发明提供了一种slam与uwb的实时融合与建图方法的实施例，包括：
32.s1、在携带激光雷达的智能体的移动过程中，根据分布式算法，得到当前时刻智能体与各超宽带节点的定位信息；
33.s2、基于扩展卡尔曼滤波算法，对当前时刻的定位信息进行修正；
34.s3、基于高斯-牛顿方法求取当前时刻智能体的最优位姿，采用最优位姿矫正智能体当前时刻的位姿估计以及各超宽带节点的位置，同时更新激光雷达地图以及uwb地图；将s3输出的当前时刻的智能体的位姿估计以及各超宽带节点的位置作为s2的输入；
35.s4、智能体移动到下一时刻，令下一时刻等于当前时刻，重复执行s1至s3，直至智能体完成预设的任务。
36.本实施例中，s1包括：
37.智能体进入未知区域后，抛投位置已知的超宽带节点和位置未知的超宽带节点，并与各超宽带节点建立通讯，测得相互之间的距离；智能体在区域中不断移动，收集各个位置的激光雷达反馈数据。
38.本实施例中，s1中的智能体在移动的过程中，当不满足预设条件时，需要继续在其周围抛投超宽带节点；预设条件为智能体周围可测量范围内的超宽带节点数量大于3个。
39.本实施例中，s2包括：
40.根据s1输出的智能体和各超宽带节点的定位信息构建动态观测模型；
41.采用扩展卡尔曼滤波算法对动态观测模型进行更新，得到更新的状态估计和更新的协方差估计，即完成对对定位信息的修正。
42.本实施例中，动态观测模型包括状态噪声；定位信息包括位姿及速度。
43.本实施例中，利用基于高斯-牛顿方法的扫描匹配过程，将当前时刻观测到的光束端点与当前时刻的激光雷达地图进行匹配，以找到最佳的变换从而更好地匹配。
44.本实施例中，扫描匹配过程在一个多分辨率网格中进行，从低分辨率的网格映射到高分辨率的网格映射，从而更有可能找到全局解而不是被困在局部解中。
45.本实施例中，在建立激光雷达地图时，通过变换和旋转，设定一个超宽带节点固定在原点，设定另一个超宽带节点始终位于正x轴上；两个设定的超宽带节点的速度为零；在以两个设定的超宽带节点建立的局部框架下构建和更新激光雷达地图。
46.nlos(non line of sight)传播对于高分辨率定位系统影响很大，因为在距离测量中会引入不可忽略的偏差，从而降低了定位精度。如果功率差(即接收功率减去第一路径功率)大于10db，则该信道很可能是非视距(nlos)，那么nlos下的测距在ekf更新中可被忽略。
47.由于uwb测距和lidar测距的精度差异，不能直接在uwb的定位基础上构建lidar地图，这促使通过优化机器人的位姿和方向估计来匹配扫描端点。hectorslam利用基于高斯-牛顿方法的扫描匹配过程，将时刻t观测到的光束端点与最新的lidar地图进行匹配，以找到最佳的变换从而更好地匹配。扫描匹配的过程在一个多分辨率网格中进行，从低分辨率的网格映射到高分辨率的网格映射，使它更有可能找到全局解而不是被困在局部解中。基于ekf(extended kalman filter)的uwb测距可以获得机器人以及锚点的位姿和速度估计，并用在扫描匹配过程的初始化。
48.实施例二：
49.本实施例以9个超宽带节点为例，其包括4个位置已知的超宽带节点和5个位置未知的超宽带节点，阐述本实施例的方法。
50.在一个人类无法进入且gps信号弱，无其他可参考信号的区域，需要派遣智能体进行区域探测，并建立本地区的地图。智能体均携带超宽带模块，且无定位的先验信息。智能体进入未知区域后，抛投初始uwb定位节点，并与各节点建立通讯，测得相互之间的距离。智能体在区域中不断移动，收集各个位置的雷达反馈数据。在远离初始位置的区域可以再次抛投uwb定位节点进行协同定位。基于扩展卡尔曼滤波方法，将uwb融合信息进行滤波处理，得到当前智能体的位置、速度、朝向等信息，以此位置为先验信息，利用高斯-牛顿方法对雷达数据进行匹配，得到当前位置的准确信息，并建立地图。
51.在本实例中应用提供的基于slam与uwb的实时融合与建图方法，通过分布式定位算法，实现三维空间内的未知超宽带节点的定位。同时，智能体的运动信息在噪声的影响下会出现偏差，而uwb的定位结果能够及时纠正位置信息，减小匹配算法收敛与局部最优解的概率。以下通过具体实例来实施和验证其准确性。实验环境如图4所示。
52.步骤一：分布式定位。
53.分布式定位系统整体框架：多个uwb模块采用dr分布式测距，并且与智能体搭载的计算设备串口通讯；计算设备与主计算机建立无线通讯。假设网络节点的拓扑结构如图3所示。节点p1，p2，p3，p4是位置已知的锚点，其余的是坐标未知的网络中的普通节点，对于空间中由p1，p2，p3，p4组成的凸四面体中的任何点p0，存在重心坐标w
01
,w
02
,w
03
,w
04
，使得
54.w
01
w
02
w
03
w
04
＝1
55.p0＝w
01
*p1 w
02
*p2 w
03
*p3 w
04
*p456.则：
[0057][0058][0059]
所有要定位的节点都可以表示为重心坐标与其对应邻居的乘积之和：
[0060]
p5＝w
51
p1 w
56
p6 w
58
p8 w
59
p9[0061]
p6＝w
62
p2 w
65
p5 w
67
p7 w
69
p9[0062]
p7＝w
73
p3 w
76
p6 w
78
p8 w
79
p9[0063]
p8＝w
84
p4 w
85
p5 w
87
p7 w
89
p9[0064]
p9＝w
95
p5 w
96
p6 w
97
p7 w
98
p8[0065]
将上式转换为矩阵表示形式：
[0066][0067]
pn＝cpn bpa[0068]
其中pa表示锚点，即pa＝[p1,p2,p3,p4]
t
；pn表示待定位的普通节点，pn＝[p5,p6,p7,p8,p9]
t
，b、c是由重心坐标构成的矩阵，在本例中b、c如下所示
[0069][0070]
[0071]
可以等价写为：
[0072]
(i-c)pn＝bpa[0073]
令m＝i-c，并且给等式两边同乘以m
t
，令a＝m
t
m且b＝m
t
bpa，可以将定位问题表示为一个最小二乘问题如下所示：
[0074]
apn＝b
[0075]
将矩阵a进行分解：
[0076]
a＝d-l-u
[0077]
其中矩阵d表示a的对角线构成的矩阵，-l和-u分别表示矩阵a的严格下三角矩阵和严格上三角矩阵。此时取k＝d且n＝l u，将该问题表示为迭代的形式：
[0078][0079]
然后，按如下方式进行jacobi迭代估计：
[0080][0081]
接下来，修改jacobi迭代估计方法以提高其收敛速度，并获得更适合恶劣环境的迭代形式。
[0082]
在迭代中引入松弛参数α∈(0,1)，使得的表达式是和上式的带权和。将此形式称为jor(jacobi over-relaxation)迭代，由下式给出：
[0083][0084]
接下来将上式递归算法开发为一种分布式的算法如下：
[0085][0086]
其中a
ii
代表a的元素，bi代表的元素，而代表矩阵pn的第i行，即第i个节点的坐标位置。
[0087]
步骤二：扩展卡尔曼滤波定位。
[0088]
用r
(uwb)
表示基于uwb的测距。
[0089]
x
t
＝[p
0,x,t
,p
0,y,t
,v
0,x,t
,v
0,y,t
]
t
[0090][0091]
其中，x
t
包含机器人在t时刻的位姿及速度，表示uwb地图下锚点的位姿及速度。
[0092]
动态观测模型中
[0093]
x
t
＝f
t
x
t-1
g
twt
[0094]
其中转换矩阵：
[0095]
[0096][0097]
状态噪声w
t
为零均值且具有协方差：
[0098][0099]
δ是采样间隙，由标准扩展卡尔曼滤波(ekf)更新：
[0100][0101][0102][0103][0104][0105]
p
t|t
＝(i
4(nt 1)-k
tht
)p
t|t-1
[0106]
是更新的状态估计，p
t|t
是更新的协方差估计.h
t
被定义为
[0107]
由于处理的一个没有全局定位信息的锚点定位问题，初始只知道四个节点，所以基于uwb测距下只能得到可以任意变换和旋转的所有节点的相对几何位置关系，如图2。在建立lidar地图时，需要消除这种几何不确定性。为此，通过变换和旋转，使一个锚点总在原点，另一个锚点始终位于正x轴上。另外，强制这两个选定的锚点的速度为零。便可以在以这两个锚点建立的局部框架下构建和更新lidar地图。建图过程如图5所示。
[0108]
步骤三：基于高斯-牛顿法的匹配建图。
[0109]
假设机器人在世界坐标系二维平面的表示为ξ＝(p
x
,py,ψ)
t
，函数m为地图信息，其值为栅格地图的占用值。利用最小二乘法求解最优机器人位姿：ξ
*
为本次最优位姿，即扫描匹配目标结果。
[0110]
机器人t时刻的位姿ξ
*
和上一时刻位姿ξ存在关系ξ δξ＝ξ
*
，要求所有激光点在栅格地图占用偏差总量尽可能小，即要求δξ满足以下要求：
[0111][0112]
在δξ处展开：
[0113][0114]
对δξ求导：
[0115][0116]
令：
[0117][0118][0119]
则求导后式子可变为：
[0120]
2(k-h
·
δξ)＝0
[0121]
即：
[0122]
δξ＝h-1
·k[0123]
根据定义：
[0124][0125][0126]
栅格地图每个栅格都用0-1表示障碍物占据的概率。利用双线性插值法计算占用值m(pm)，p
i,j
＝(s
i,x
,s
i,y
)
t
[0127][0128]
对上式求偏导：
[0129][0130][0131]
将上式中的pm替换为si(ξ),则得到结果m(s
i(ξ)
)。建图效果如图6所示，建图结果如图7所示。
[0132]
经过仿真验证，相比于市面上常用的锚点-标签定位方案，本实例提出的基于slam与uwb的实时融合与建图方法，目前能够实现基于uwb的多智能体系统分布式协同定位的精度小于2cm，如图7所示，远超目前市面上现有的uwb设备定位精度(通常为15cm)。
[0133]
最后应当说明的是：以上实施例仅用以说明本发明的技术方案而非对其限制，尽管参照上述实施例对本发明进行了详细的说明，所属领域的普通技术人员应当理解：依然可以对本发明的具体实施方式进行修改或者等同替换，而未脱离本发明精神和范围的任何修改或者等同替换，其均应涵盖在本发明的权利要求保护范围之内。