一种基于激光与微波协同的地图重构融合方法

1.本发明属于地图测绘领域，特别涉及一种存在视野盲区的三维高精度地图测绘技术。


背景技术：

2.高精度三维地图测绘在自动驾驶、森林资源调查、城市建筑重建等领域具有重要意义。激光雷达在地图重构中起着关键作用，因为它可以提供较小误差的高精度环境感知信息。
3.激光雷达主要有两类地图重构方法。第一类基于全点云匹配，第二类基于特征点云匹配。在第一种类型中，迭代最近点法(icp)最具代表性(p.j.besl and n.d.mckay,“a method for registration of 3
‑
d shapes,”ieee transactions on pattern analysis and machine intelligence,vol.14,no.2,pp.239
‑
256,feb.1992.)：它通过使用相邻帧的所有点云来估计激光雷达里程，之后激光雷达里程信息将用于地图构建。但是，icp计算工作量太大，不适合大规模场景重建。
4.第二种方法称为激光雷达里程和测绘(loam)方法，研究人员已经在此领域做了较长时间的研究。在(j.zhang and s.singh,“loam:lidar odometry and mapping in realtime,”in robotics:science and systems conference(rss),jul.2014.)中，作者提取了两个相邻数据帧的特征点进行匹配，结果表明该算法可以提高地图构建精度与速度。在论文(t.shan and b.englot,“lego
‑
loam:lightweight and ground
‑
optimized lidar odometry and mapping on variable terrain,”in 2018ieee/rsj international conference on intelligent robots and systems(iros),pp.4758
‑
4765,2018.)中，作者通过使用两步levenberg
‑
marquardt(d.marquardt,“an algorithm for least
‑
squares estimation of nonlinear parameters,”journal of the society for industrial and applied mathematics.pp.431
‑
441,nov.1963.)方法求解非线性运动估计方程，提出了一种改进loam的轻型激光雷达里程测量算法(t.shan and b.englot,“lego
‑
loam:lightweight and ground
‑
optimized lidar odometry and mapping on variable terrain,”in 2018ieee/rsj international conference on intelligent robots and systems(iros),pp.4758
‑
4765,2018.)。结果表明，该算法的制图精度高于loam。此外，一种用于小视场固态激光雷达的快速激光雷达测绘算法(j.lin and f.zhang,“loam livox:a fast,robust,high
‑
precision lidar odometry and mapping package for lidars of small fov,”in 2020ieee international conference on robotics and automation(icra),pp.3126
‑
3131,2020.)其精度也较高，由于该算法采用的激光雷达有着非重复扫描特性，因此可以获得更高精度的地图绘制结果。然而，所有基于激光雷达的地图重构方法只能局限于激光雷达在视距范围内感知的结果，无法同时对遮挡区域进行地图构建。


技术实现要素：

5.为解决上述技术问题，本发明提出了一种基于激光与微波协同的地图重构融合方法，用于重建未知场景，能同时精准绘制视距范围与非视距范围内的环境信息。
6.本发明采用的技术方案为：一种基于激光与微波协同的地图重构融合方法，所述方法基于的数据采集装置包括：车载系统、激光雷达以及微波雷达，所述激光雷达与微波雷达固定于车载系统上，数据采集装置运行时，激光雷达与微波雷达面向存在遮挡的未知场景；
7.所述方法具体包括以下步骤：
8.s1、激光雷达与微波雷达同步开始采集数据；
9.s2、基于激光雷达的点云数据，运用livox loam算法提取出相邻数据帧的特征点作为匹配，以匹配误差作为约束构建激光雷达位姿求解优化方程，并基于求解出的激光雷达位姿对可视范围场景进行三维地图重构，得到激光雷达构图结果；
10.s3、在时间域上基于激光雷达的位姿用线性插值的方法，预估出微波雷达在移动过程中分布的具体子孔径的位姿；
11.s4、以子孔径位姿和激光雷达估计的可视范围物体距离为先验信息对微波雷达回波进行滤波处理，并运用多视角后向投影成像算法，得到微波雷达成像结果；
12.s5、最后以激光雷达与微波雷达分布的子孔径位姿参数为参考，进行激光雷达构图结果与微波雷达成像结果融合，得到融合后的可视与遮蔽区域的地图测绘结果。
13.对激光雷达与微波雷达各自采集的每帧数据都打上时间戳。
14.步骤s3具体为：记第k帧激光雷达数据，对应的位姿为这里寻找第m帧微波雷达数据，且第m帧微波雷达数据对应的时间点位于第k帧激光雷达数据的起始时刻到结束时刻之间，记第m帧微波雷达数据对应位姿为对应位姿为是激光雷达第k帧激光雷达数据结束时刻的时间点，为第m帧微波雷达数据所对应的时间点，通过下式求得其中和分别为激光雷达坐标系l到微波雷达坐标系b的旋转矩阵和平移向量：
[0015][0016][0017][0018][0019]
其中，f
τ
作为中间变量。
[0020]
微波雷达搭载于可上下移动的导轨上，通过导轨上下移动的方式使得微波雷达的二维平面成像结果累加成三维结果图。
[0021]
步骤s4所示激光雷达估计的可视范围物体距离，计算式为：
[0022][0023]
其中，d
l(k)
表示激光雷达预估的可视范围物体距离，l表示激光雷达坐标系中，表示第k帧第i个点云，n为在第k帧数据中属于空间采样区内的点云总数，d
w
为固定经验值，v(d
l
)为空间采样区。
[0024]
对第m帧微波雷达数据做脉冲压缩得到z
b
(t)，将z
b
(t)中回波时间小于的部分所对应的幅度值置零，c为光速。
[0025]
本发明的有益效果：本发明的方法能够同时对视距范围与遮蔽场景内的地图进行重构，其具备视距范围与非视距范围环境的感知能力，本发明的融合算法提高了存在遮蔽场景的激光雷达绘图能力；同时以激光雷达预估的微波雷达位姿作为先验信息使得微波雷达成像算法不受限于合作式的自定位系统(如gps等)，所以本发明融合算法可在场景未知且无定位系统的情况下进行地图重构。
附图说明
[0026]
图1为融合算法的程序框图。
[0027]
图2为具体实施方式中的实验场景设置示意图；
[0028]
图3为具体实施方式中试验场景示意图；
[0029]
其中(a)和(b)为实验场景两个不同视角，外围木墙为遮挡物，内部砖墙处于遮蔽区域。
[0030]
图4为车载系统示意图。雷达方位角与车载系统移动路径始终保持90度，微波雷达挂载于可上下移动的滑轨上。
[0031]
图5为具体实施方式提供的现有技术中微波雷达成像结果；
[0032]
其中(a)为未滤波的成像结果，(b)为结合激光雷达预估的距离信息滤波后的结果。
[0033]
图6为具体实施方式提供的现有技术中激光雷达的地图重构结果；
[0034]
图7中(a)为本发明的融合算法处理后的地图重构结果，(b)地图重构结果俯视图。
具体实施方式
[0035]
本发明的算法流程如图1所示。下面根据图2搭建的实验场景给出本发明的具体实施方式：车载系统搭载激光雷达与微波雷达按照预设路线(虚线)行进。由于木墙的遮挡，砖墙在车载系统行进过程中处于视野盲区。
[0036]
实验场景光学图像如图3所示。车载系统如图4所示，激光雷达与微波雷达固定于车载系统上并统一面向车体右侧，微波雷达搭载于可上下移动的导轨上，可通过导轨上下移动的方式使得微波雷达的二维平面成像结果累加成三维结果图。小车的行进路线与雷达朝向始终相差90度。本发明中的车载系统包括可行走的小车，以及小车上的搭载的数据处理模块等；具体处理方法详见步骤s1
‑
s5；
[0037]
本发明采用的微波雷达为步进频体制，频率间隔为2mhz，频率为1.6ghz
‑
2.2ghz。
激光雷达采用大疆livox horizon，其检测范围为60m，角度分辨率为0.05
°
，距离分别率为0.02m，激光雷达视场角为81.7
°
(水平)
×
25.1
°
(垂直)。本次实验中设置激光雷达坐标系正前方为x轴，上方为z轴，左方为y轴。
[0038]
步骤s1：当车载系统启动时，激光雷达与微波雷达同步开始采集数据，车载系统停止时，结束数据采集，每帧数据都打上时间戳。
[0039]
步骤s2：激光雷达地图重构；
[0040]
图6为(j.lin and f.zhang,“loam livox:a fast,robust,high
‑
precision lidar odometry and mapping package for lidars of small fov,”in 2020ieee international conference on robotics and automation(icra),pp.3126
‑
3131,2020.)所提算法的结果，可以看出现有的单激光雷达的成像结果中内墙成像是不完整的。
[0041]
假设激光雷达坐标系中，第k帧数据中的第i个点云为x
(k,i)
，将x
(k,i)
投影到第k帧数据帧的初始时刻得到
[0042][0043]
r
(k,i)
和τ
(k,i)
分别是旋转矩阵和平移向量。
[0044]
对于位于k帧中的所有点云的集合记为每帧点云接收时由于车体是一直运动的，所以存在点云畸变，在每帧数据接收后通过imu陀螺仪进行矫正。因为当前第k帧数据的处理是假设在上一帧数据已矫正的情况下进行的，假设已矫正的k
‑
1帧数据为分别匹配和未校正的中的特征点得到两数据帧间的位移d：
[0045][0046]
d(
·
,
·
)为计算不同数据帧位移的运算符。建立优化方程：
[0047][0048]
得到的旋转矩阵和平移向量代表了当前激光雷达在k帧点云中产生的位移。
[0049]
步骤s3：微波雷达子孔径位姿估计；
[0050]
微波雷达成像算法需要车载系统移动过程中产生的子孔径位姿参数，这里通过激光雷达在步骤s2中估计出来的自身位姿参数来估计微波雷达的子孔径位姿。
[0051]
假设当前处理激光雷达的第k帧数据帧，则对应的位姿为这里寻找第m帧微波雷达数据，且此数据帧对应的时间点位于第k帧数据帧的起始时刻到结束时刻之间，其对应位姿为假设是第k帧数据结束时刻的时间点，为微波雷达第m帧数据回波所对应的时间点，通过下式求得其中和分别为激光雷达坐标系到微波雷达坐标系的旋转矩阵和平移向量：
[0052]
[0053][0054][0055][0056]
其中，f
τ
作为中间变量，用于简化运算；和分别为激光雷达坐标系l到微波雷达坐标系b的旋转矩阵和平移向量，实际中由自己的系统两个雷达的空间摆放位置来确定。
[0057]
步骤s4：以子孔径位姿和激光雷达估计的可视范围物体距离为先验信息对微波雷达回波进行滤波处理，并运用多视角后向投影成像算法进行微波雷达成像；具体包括以下分步骤：
[0058]
s41、预估可视范围物体距离d
l(k)
：
[0059]
本发明中微波雷达障碍物的回波消除是基于激光雷达预估的可视范围物体距离d
l(k)
：
[0060][0061]
在激光雷达坐标系l中，第k帧第i个点云为n为在第k帧数据中属于空间采样区内的点云总数，取出中的第一个变量x做n点平均，其d
w
为固定经验值，大概为平均墙体厚度，本次实验取0.20m，v(d
l
)为空间采样区。
[0062]
同样找到步骤s3中所描述的微波雷达第m帧回波并做脉冲压缩得到z
b
(t)，z
b
(t)中回波时间小于的索引所对应的幅度值会被置零，c为光速。
[0063]
s42、多视角后向投影成像算法
[0064]
根据论文(y.jia,g.cui,l.kong and x.yang,“multichannel and multiview imaging approach to building layout determination of through
‑
wall radar,”ieee geoscience and remote sensing letters,vol.11,no.5,pp.970
‑
974,may.2014.)中所描述的后向投影成像算法完成微波雷达的成像，m
(b,h)
(x,y)代表高度为h的微波雷达二维成像平面结果图，通过调节滑轨的高度h使得处于不同高度项的m
(b,h)
(x,y)层层累加可得到微波雷达三维建图结果。m
(b,h)
(x,y)被平均像素格点化为p
n
,p
q
个点，本次实验均为500，收发阵元(子孔径)的位置分别为(x
bt
,0)和(x
br
,0)(b＝1,2,...,n
l
)，可以得到m
(b,h)
(x,y)的每个像素点(x
n
,y
m
)到各收发阵元的回波延迟为：
[0065][0066]
其中，p
n
表示m
(b,h)
(x,y)在像素格点化后的二维网格上其中一个维度的点的总个数，p
q
表示m
(b,h)
(x,y)在像素格点化后的二维网格上另一个维度的点的像素点个数，n
l
的个数是由收发的回波数决定，运行时间越长，回波个数也就越多；
[0067]
每个像素点(x
n
,y
m
)的值可以由下式得到，本发明的微波雷达成像结果如图5(b)所示，通过微波雷达可以滤除可视范围内的墙体：
[0068][0069]
步骤s5：m
(b,h)
(x,y)中像素值小于阈值ψ会被置零，本次实验中ψ为最大像素值的40％。此时m
(b,h)
(x,y)中非零像素点会被选为微波雷达点云x
r
＝(x
m
,y
n
,h)，此步骤称为像素格点化。x
r
以(雷达在地图中的位置)为参考信息投影到激光雷达建图结果中可得到最终的融合地图m
f
，如图7所示，可见本发明方法内墙与外墙的成像结果都比较完整。
[0070]
在本发明中，我们考虑了使用激光雷达和微波雷达同步对存在遮蔽环境的场景进行地图重构的问题。激光雷达采用livox
‑
loam算法重建可视范围区域，微波雷达采用后向投影成像算法重建遮挡区域。本发明提出的数据融合算法，将激光雷达和微波雷达的成像结果进行融合。实验结果表明，该算法能够高精度地重建未知遮挡场景，与激光雷达的地图生成策略相比，该算法能够同时获得遮蔽区域的成像结果，二者融合有效填补了视野盲区，提升了建图视野范围。
[0071]
本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解本发明的原理，应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的权利要求范围之内。