一种基于雷达图像的导航地图制作和使用方法及系统

摘要

本发明提供一种基于雷达图像的导航地图制作和使用方法及系统，包括：步骤S1，同步采集多源定位传感器数据，并利用多源定位传感器数据制作特征点云里程计训练数据和特征图匹配里程计训练数据；步骤S2，基于特征点云里程计训练数据和特征图匹配里程计训练数据，分别实现探地雷达特征点云提取模型和探地雷达特征图匹配模型；步骤S3，采用因子图技术实现基于探地雷达图像的组合导航。本发明能够将探地雷达机器人自动引导到隐患位置，为长隧道、跨江通道以及大型地下交通设施等无GPS场景下的地下隐患探测作业所需要的高精度自主定位提供了一种新型的技术方案，能够大幅度降低作业成本，提高作业效率以及作业的可靠性能。

权利要求

1.一种基于雷达图像的导航地图制作和使用方法，其特征在于，包括以下步骤：步骤S1，同步采集多源定位传感器数据，并利用多源定位传感器数据制作特征点云里程计训练数据和特征图匹配里程计训练数据；

步骤S2，基于特征点云里程计训练数据和特征图匹配里程计训练数据，分别实现探地雷达特征点云提取模型和探地雷达特征图匹配模型；

步骤S3，采用因子图技术实现基于探地雷达图像的组合导航；

所述步骤S2中，实现探地雷达特征图匹配模型的过程包括以下子步骤：

步骤S221，搭建神经网络，通过求解 获取重叠序列雷达图像

与对应的位姿 之间的关联，其中 为神经网络参数， 为重叠序列雷达

图像GPRt对应的位姿Poset随时间t变化的条件概率， 为最优的神经网络参数， 表示获取当条件概率 取得极大值且自变量取值为神经网络参数 的函数；

步骤S222，通过马氏距离来计算真值 和网络预测值之间的差，并利用Huber loss损失函数进行回归来训练探地雷达特征图匹配模型的深度学习；其中，， 和 分别为t时刻和t‑k时刻的序列雷达图像位姿， 为特征图传递给下游回归网络的输出预测值，k为两帧重叠序列图像的时间间隔。

2.根据权利要求1所述的基于雷达图像的导航地图制作和使用方法，其特征在于，所述步骤S1包括以下步骤：步骤S101，同步采集多源定位传感器数据，所述多源定位传感器数据包括探地雷达数据、组合式惯性导航数据以及编码器数据；

步骤S102，将所有通道数据组合起来沿前进方向形成一个雷达数据立方体，通过多次扫描和对齐获取测线雷达数据立方体，并进行预处理，所述预处理包括直达波过滤处理以及面层跟踪与消除的预处理；

步骤S103，制作特征点云里程计训练数据；

步骤S104，制作特征图匹配里程计训练数据。

3.根据权利要求2所述的基于雷达图像的导航地图制作和使用方法，其特征在于，所述步骤S101中，探地雷达设置为随编码器距离触发模式，按照预先设置的固定间隔触发并采集全部通道数据，以此作为探地雷达数据；实时差分和惯性导航采用组合式设备，按照同步触发模式与探地雷达同步触发并采集定位数据，采集的数据与测距轮数据对齐后统一存储，采集的数据集为  ，其中，n为总道数， 为实时差分和惯性导航的组合导航输出的位姿数据，TRACEn为各通道同步采集的多通道雷达数据矩阵，每个TRACE由m个A‑SCAN记录组成，m为雷达通道数，A‑SCAN为N个采样点对应的振幅数组，N为单次扫描中的采样数目；Wn为测距轮触发记录，所述测距轮触发记录包括触发次数、距离以及触发时间；Wn、 、Tracen组成同步采集并对齐的数据集。

4.根据权利要求3所述的基于雷达图像的导航地图制作和使用方法，其特征在于，所述步骤S103包括以下子步骤：步骤S1031，通过地下目标检测模型对雷达数据进行扫描并自动标注出特征明显的目标作为导航点，特征明显的目标包括地下管廊、管线、沙井、空洞、脱空以及富水的异常体；

步骤S1032，对检测出的目标按照深度学习中实例分割的要求进行审核及轮廓标注，其中，对检测出的目标的一次波、多次波以及绕射波的轮廓进行全部标注；

步骤S1033，将标注好的轮廓按照预先设置的格式进行保存；

所述步骤S104包括以下子步骤：

步骤S1041，通过公式 遍历数据集 的

探地雷达子集 以固定间隔依次提取两帧数据GPR1、GPR2，其中， 为当前道数的编号， 为道数步长， 为道数宽度，以此形成两幅维度为16×512×512，行进方向相互间重叠256的探地雷达3D体素；

步骤S1042，将探地雷达3D体素与数据集 中的对应实时差分和惯性导航的组合数据以预先设置的格式进行保存。

5.根据权利要求1至4任意一项所述的基于雷达图像的导航地图制作和使用方法，其特征在于，所述步骤S2中，实现探地雷达特征点云提取模型的过程包括以下子步骤：步骤S211，基于深度学习的全通道2D B‑SCAN及C‑SCAN实例分割和结果聚合的方式生成3D感兴趣区域，将异常体从背景中精确的分割出来；

步骤S212，对分割出来的图像进行轮廓处理；

步骤S213，将获得的各轮廓线按常规逐点转换为带强度的PCL格式3D点云，用于创建基于点云的导航控制点。

6.根据权利要求5所述的基于雷达图像的导航地图制作和使用方法，其特征在于，所述步骤S212包括以下子步骤：步骤S2121，采用基于边界跟踪的拓扑结构分析算法从实例分割输出的Mask二值图像中提取出目标外围轮廓；

步骤S2122，通过计算轮廓线内面积是否大于指定阈值来进行判断，将轮廓线内面积小于指定阈值的小目标进行清除；

步骤S2123，通过凸包算法查找包含原轮廓点的最小凸多边形来代替直接使用原来的轮廓点集，以进行轮廓曲线平滑处理。

7.根据权利要求5所述的基于雷达图像的导航地图制作和使用方法，其特征在于，所述步骤S2中，所述探地雷达特征点云提取模型的特征提取网络中，采用yolov5s的骨干网络CSPDarknet53，并通过幻影卷积模块和幻影瓶颈模块取代对应的常规卷积和CSP Bottleneck模块成为yolov5s‑ghost。

8.根据权利要求1至4任意一项所述的基于雷达图像的导航地图制作和使用方法，其特征在于，所述步骤S3中，通过公式实现探地雷

达的组合导航，其中， 为加入探地雷达特征点云里程计和探地雷达特征图里程计之后的后验概率，  为非线性最小二乘求解函数，gpr_c、gpr_f和imu分别对应基于探地雷达特征点云的探地雷达里程计、基于特征图的探地雷达里程计和基于惯性导航的里程计；

、 和 分别为因子图中对应基于探地雷达特征点云、基于

探地雷达特征图和基于惯性导航的里程计中两相邻状态节点的相对位姿变化， 、和 分别为基于探地雷达特征点云的里程计输出、基于探地雷达特征图的里程计输出和基于惯性导航的里程计测量输出。

9.一种基于雷达图像的导航地图制作和使用系统，其特征在于，采用了如权利要求1至

8任意一项所述的基于雷达图像的导航地图制作和使用方法， 并包括组合导航装置、三维探地雷达和编码器，所述组合导航装置、三维探地雷达和编码器安装于探测车上并校准，建立各坐标系变换矩阵；其中，所述组合导航装置为能提供惯性导航、全球定位导航以及里程计组合导航的设备；

在采集时，从实时差分的位置开始，以惯性导航单元为核心，全覆盖采集地下空间数据创建导航地图，在实时差分有效区以常规实时差分里程计为主修正惯性导航单元，在特征图里程计修正区通过所述探地雷达特征图匹配模型提取和匹配特征图对惯性导航单元进行修正，在特征点云里程计修正区通过所述探地雷达特征点云提取模型提取地下空间特征点云作为导航点并进行点云匹配定位修正惯性导航单元；在无实时差分的情况下通过连续修正惯性导航单元输出以获得导航地图；

在复测验证时，与采集一致，从实时差分的位置开始初始校准，然后按照因子图导航模式根据各子系统精度动态加入和解除各因子，完成复测验证导航任务。

说明书

一种基于雷达图像的导航地图制作和使用方法及系统

技术领域

[0001] 本发明涉及一种导航地图的制作和使用方法，尤其涉及一种基于雷达图像的导航地图制作和使用方法，并进一步涉及采用了该基于雷达图像的导航地图制作和使用方法的系统。

背景技术

[0002] 全球卫星导航系统GNSS，包括GPS全球定位系统和北斗卫星导航系统；实时差分技术RTK，实时差分技术RTK包括多频载波相位差分(厘米级)和单频载波相位差分(亚米级))；惯性导航技术(INS，IMU惯性导航单元)，如果能结合这三种导航方式可以获得分米级甚至厘米级定位精度，不足之处在于复杂环境下由于覆盖范围不足、信号弱以及多路径等问题造成无固定解甚至无法工作。并且，在长达数公里甚至数十公里的长隧道、跨江通道等无全球定位系统(GNSS)场景下，常用的定位方法如GPS/RTK无法使用，且视觉特征匮乏，即使加入激光雷达、惯导、视觉导航、WIFI定位等方法均存在累计误差大、需要第三方辅助系统配合等不利因素，难以满足定位需求，因此，复杂环境下探地雷达作业时的高精度定位问题仍然是长期困扰工程作业实际的一个难题，常规的改进方法由于各子系统特性限制改进余地有限，如常规民用惯性导航累计误差可达里程的0.5 1%左右，高精度的惯性导航过于昂贵~
无法使用。激光雷达(Lidar)和视觉定位方案在此场景下也因为外界特征不足而难以获得满意的应用效果。

发明内容

[0003] 本发明所要解决的技术问题是需要提供一种在无GNSS场景下，基于雷达图像的导航地图制作和使用方法及系统，进而实现导航地图的制作和融合定位，具有充分挖掘现有探地雷达设备潜力、无需其它外界系统配合且鲁棒性强等优点。

[0004] 对此，本发明提供一种基于雷达图像的导航地图制作和使用方法，包括以下步骤：

[0005] 步骤S1，同步采集多源定位传感器数据，并利用多源定位传感器数据制作特征点云里程计训练数据和特征图匹配里程计训练数据；

[0006] 步骤S2，基于特征点云里程计训练数据和特征图匹配里程计训练数据，分别实现探地雷达特征点云提取模型和探地雷达特征图匹配模型；

[0007] 步骤S3，采用因子图技术实现基于探地雷达图像的组合导航。

[0008] 本发明的进一步改进在于，所述步骤S1包括以下步骤：

[0009] 步骤S101，同步采集多源定位传感器数据，所述多源定位传感器数据包括探地雷达数据、组合式惯性导航数据以及编码器数据；

[0010] 步骤S102，将所有通道数据组合起来沿前进方向形成一个雷达数据立方体，通过多次扫描和对齐获取测线雷达数据立方体，并进行预处理，所述预处理包括且不限于直达波过滤处理以及面层跟踪与消除的预处理；

[0011] 步骤S103，制作特征点云里程计训练数据；

[0012] 步骤S104，制作特征图匹配里程计训练数据。

[0013] 本发明的进一步改进在于，所述步骤S101中，探地雷达设置为随编码器距离触发模式，按照预先设置的固定间隔触发并采集全部通道数据，以此作为探地雷达数据；实时差分和惯性导航采用组合式设备，按照同步触发模式与探地雷达同步触发并采集定位数据，采集的数据与测距轮数据对齐后统一存储，采集的数据集为，其中，n为总道数， 为实时差分和惯性导航的组合导航输出的位姿数据，TRACEn为各通道同步采集的多通道雷达数据矩阵，每个TRACE由m个A‑SCAN记录组成，m为雷达通道数，A‑SCAN为N个采样点对应的振幅数组，N为单次扫描中的采样数目(SAMPLES)；Wn为测距轮触发记录，所述测距轮触发记录包括触发次数、距离以及触发时间；Wn、 、Tracen组成同步采集并对齐的数据集。

[0014] 本发明的进一步改进在于，所述步骤S103包括以下子步骤：

[0015] 步骤S1031，通过地下目标检测模型对雷达数据进行扫描并自动标注出特征明显的目标作为导航点，特征明显的目标包括地下管廊、管线、沙井、空洞、脱空以及富水等异常体；

[0016] 步骤S1032，对检测出的目标按照深度学习中实例分割的要求进行审核及轮廓标注，其中，对检测出的目标的一次波、多次波以及绕射波的轮廓进行全部标注；

[0017] 步骤S1033，将标注好的轮廓按照预先设置的格式进行保存；

[0018] 所述步骤S104包括以下子步骤：

[0019] 步骤S1041，通过公式 遍历数据集的探地雷达子集 以固定间隔依次提取两帧数据GPR1、GPR2，其中， 为当前道数的编号， 为道数步长， 为道数宽度，以此形成两幅维度为16×512×512，行进方向相互间重叠256的探地雷达3D体素；

[0020] 步骤S1042，将探地雷达3D体素与数据集 中的对应实时差分和惯性导航的组合数据以预先设置的格式进行保存。

[0021] 本发明的进一步改进在于，所述步骤S2中，实现探地雷达特征点云提取模型的过程包括以下子步骤：

[0022] 步骤S211，基于深度学习的全通道2D B‑SCAN及C‑SCAN实例分割和结果聚合的方式生成3D感兴趣区域，将异常体从背景中精确的分割出来；

[0023] 步骤S212，对分割出来的图像进行轮廓处理；

[0024] 步骤S213，将获得的各轮廓线按常规逐点转换为带强度的PCL格式3D点云，用于创建基于点云的导航控制点。

[0025] 本发明的进一步改进在于，所述步骤S212包括以下子步骤：

[0026] 步骤S2121，采用基于边界跟踪的拓扑结构分析算法从实例分割输出的Mask二值图像中提取出目标外围轮廓；

[0027] 步骤S2122，通过计算轮廓线内面积是否大于指定阈值来进行判断，将轮廓线内面积小于指定阈值的小目标进行清除；

[0028] 步骤S2123，通过凸包算法查找包含原轮廓点的最小凸多边形来代替直接使用原来的轮廓点集，以进行轮廓曲线平滑处理。

[0029] 本发明的进一步改进在于，所述步骤S2中，实现探地雷达特征图匹配模型的过程包括以下子步骤：

[0030] 步骤S221，搭建神经网络，通过求解 获取重叠序列雷达图像 与对应的位姿 之间的关联，其中 为神经网络参数， 为重叠序
列雷达图像GPRt对应的位姿Poset随时间t变化的条件概率， 为最优的神经网络参数，表示获取当条件概率 取得极大值且自变量取值为神经网络参数 的函数；步骤S222，通过马氏距离来计算真值和网络预测值 之间的差，并利用Huber loss损
失函数进行回归来训练探地雷达特征图匹配模型的深 度学 习；其中 ，
， 和 分别为t时刻和t‑k时刻的序列雷达图像
位姿， 为特征图传递给下游回归网络的输出预测值， k为两帧重叠序列
图像的时间间隔。

[0031] 本发明的进一步改进在于，所述步骤S2中，所述探地雷达特征点云提取模型的特征提取网络中，采用yolov5s的骨干网络CSPDarknet53，并通过幻影卷积(GhostConv)模块和幻影瓶颈(GhostBottleneck)模块取代对应的常规卷积和CSP Bottleneck模块成为yolov5s‑ghost，从而在不影响模型的性能的前提下有效降低了对计算资源的需求。其中， 幻影卷积模块优选指的是华为Ghostnet提出的幻影卷积(GhostConv)模块，幻影瓶颈模块优选指的是华为Ghostnet提出的幻影瓶颈(GhostBottleneck)模块模块，CSP Bottleneck模块指的是yolov5原生的跨阶段局部融合瓶颈(CSP Bottleneck)模块。

[0032] 本发明的进一步改进在于，所述步骤S3中，通过公式实现探地
雷达的组合导航，其中， 为加入探地雷达特征点云里程计和探地雷达特征图里程计之后的后验概率， 为非线性最小二乘求解函数，gpr_c、gpr_f和imu分别对应基于探地雷达特征点云的探地雷达里程计、基于特征图的探地雷达里程计和基于惯性导航的里程计；
、 和 分别为因子图中对应基于探地雷达特征点云、基
于探地雷达特征图和基于惯性导航的里程计中两相邻状态节点的相对位姿变化， 、和 分别为基于探地雷达特征点云的里程计输出、基于探地雷达特征图的里程计输出和基于惯性导航的里程计测量输出。

[0033] 本发明还提供一种基于雷达图像的导航地图制作和使用系统，采用了如上所述的基于雷达图像的导航地图制作和使用方法，并包括组合导航装置、三维探地雷达和编码器，所述组合导航装置、三维探地雷达和编码器安装于探测车上并校准，建立各坐标系变换矩阵；其中，所述组合导航装置为能提供惯性导航、全球定位导航以及里程计组合导航的设备；

[0034] 在采集时，从实时差分的位置开始，以惯性导航单元为核心，全覆盖采集地下空间数据创建导航地图，在实时差分有效区以常规实时差分里程计为主修正惯性导航单元，在特征图里程计修正区通过所述探地雷达特征图匹配模型提取和匹配特征图对惯性导航单元进行修正，在特征点云里程计修正区通过所述探地雷达特征点云提取模型提取地下空间特征点云作为导航点并进行点云匹配定位修正惯性导航单元；在无实时差分的情况下通过连续修正惯性导航单元输出以获得导航地图；

[0035] 在复测验证时，与采集一致，从实时差分的位置开始初始校准，然后按照因子图导航模式根据各子系统精度动态加入和解除各因子，完成复测验证导航任务。

[0036] 与现有技术相比，本发明的有益效果在于：充分利用了地下隐患探测作业中探地雷达可以获取高密度地下空间探测数据的优点，在探测作业时就通过探地雷达数据构造高精度地下空间地图，并结合惯性导航实现高精度隐患定位和导航目的，可在复测验证阶段将探地雷达机器人自动引导到隐患位置；为长隧道、跨江通道以及大型地下交通设施等无GPS场景下的地下隐患探测作业所需要的高精度自主定位提供了一种新型定位和导航方式，能够大幅度降低作业成本，提高作业效率以及作业的可靠性能。本发明具有充分挖掘现有探地雷达设备潜力、无需其它外界系统配合且鲁棒性强等优点。

附图说明

[0037] 图1是本发明一种实施例的工作流程结构示意图；

[0038] 图2是本发明一种实施例的系统原理示意图；

[0039] 图3是本发明一种实施例在步骤S102对训练数据制作前后的对比示意图；

[0040] 图4是本发明一种实施例实现探地雷达特征点云提取模型的原理示意图；

[0041] 图5是本发明一种实施例实现探地雷达特征图匹配模型的神经网络框图；

[0042] 图6是本发明一种实施例的步骤S3的算法框图；

[0043] 图7是本发明另一种实施例的作业示意图。

具体实施方式

[0044] 下面结合附图，对本发明的较优的实施例作进一步的详细说明。

[0045] RTK（实时差分）+惯性导航组合导航在无RTK信号时的精度，超过10s时已经无法满足探地雷达机器人定位需求：

[0046]RTK信号中断时间(秒) 水平位置精度(米) 垂直位置精度(米)
0s 0.02 0.03
10s 0.35 0.15
60s 4.00 1.50

[0047] 探地雷达作业时使用的三维探地雷达能高速采集多维数据，典型车载产品通常可以60Km/h以上速度按照道间距7.5cm采集幅宽1.6m、16通道以上高密度地下空间数据，分辨率可达5‑10cm，按照全覆盖的普查作业要求可以快速形成覆盖所有车道的地下空间三维数据立方体。这些数据配合编码器等设备采集的数据可以构成高精度地下空间地图，并可以作为辅助导航定位使用，为自动驾驶等应用提供了一种新的定位手段。

[0048] 探地雷达在复测验证时需要在道路中间围绕异常点反复蛇形覆盖作业具备一定危险性，引入无人驾驶自动机器人是一种可能的方案，园区、大坝、电站和油库等特殊复杂场景下 也有强烈的自动化作业需求。

[0049] 近年来，基于GNSS(GPS/RTK)、Lidar（激光雷达）、IMU、里程计(测距轮)等方式在地下空间探测作业中得到了广泛的应用，基于探地雷达子系统和自动导引小车(  AGV: Automatic Guided Vehicle)构建的探地雷达机器人可以自动完成复测验证作业，使得系统的安全性、适应性和鲁棒性得到了较大的提升，可以进一步增强地下空间探测行业的智能化水平。

[0050] 为解决隧道、恶劣天气以及城市复杂环境等场景下无全球定位信号、视频/激光雷达特征不足导致常规导航方案不能使用的问题，本申请将探地雷达信号作为定位子系统之一，综合使用惯性导航、探地雷达以及编码器(里程计)等定位手段，自主提供持续的导航定位信息，充分发挥雷达数据密度大精度高、惯导/里程数据在地下特征较少的部分可以持续工作的特点，满足探地雷达(GPR)探测和复测验证作业特别是无人驾驶复测雷达车对导航定位的需求。

[0051] 如图2所示，本申请采用在自动引导小车领域广泛应用的基于因子图的SLAM方案，通过融合采用基于探地雷达、惯导以及RTK（实时差分）、激光雷达等多源传感器融合的组合定位导航方式，提高系统自动作业时的定位精度，能够在包含环境退化(包括恶劣天气、隧道以及高大建筑物遮挡反射)、传统感知障碍(RTK/激光雷达无法精确定位)的路段情况下自动切换到以惯导为核心工作模式，分别利用基于特征点云匹配的探地雷达特征点云里程计和基于特征图匹配的探地雷达特征图里程计两种里程计对惯导进行修正以保持精度的高精度定位导航模式，完成探地雷达作业任务，提升了作业系统的智能化水平和环境适应能力。

[0052] 具体的，如图1所示，本实施例提供一种基于雷达图像的导航地图制作和使用方法，包括以下步骤：

[0053] 步骤S1，同步采集多源定位传感器数据，并利用多源定位传感器数据制作特征点云里程计训练数据和特征图匹配里程计训练数据；

[0054] 步骤S2，基于特征点云里程计训练数据和特征图匹配里程计训练数据，分别实现探地雷达特征点云提取模型和探地雷达特征图匹配模型；

[0055] 步骤S3，采用因子图技术实现基于探地雷达图像的组合导航。

[0056] 本实施例所述步骤S1包括以下步骤：

[0057] 步骤S101，同步采集多源定位传感器数据，所述多源定位传感器数据包括探地雷达数据、组合式惯性导航数据以及编码器数据；

[0058] 步骤S102，将所有通道数据组合起来沿前进方向形成一个雷达数据立方体，通过多次扫描和对齐获取测线雷达数据立方体，并进行直达波过滤处理以及面层跟踪与消除等常规预处理；

[0059] 步骤S103，制作特征点云里程计训练数据；

[0060] 步骤S104，制作特征图匹配里程计训练数据。

[0061] 本实施例所述步骤S101中，在RTK信号良好路段，以厘米级多频载波相位差分RTK为位姿真值（Ground Truth），同步采集探地雷达、惯性导航（IMU）以及编码器（里程计）数据。其中，探地雷达设置为随编码器距离触发模式，按照预先设置的固定间隔触发并采集全部通道数据，以此作为探地雷达数据；实时差分（RTK）和惯性导航(IMU)采用组合式设备，按照同步触发模式与探地雷达同步触发并采集定位数据，采集的数据与测距轮数据对齐后统一存储，采集的数据集为 ，其中，n为总道数Trace，为实时差分和惯性导航的组合导航（RTK+IMU）输出的位姿数据，位姿数据的格式优选为GPCHC格式，TRACEn为各通道同步采集的多通道雷达数据矩阵，每个TRACE由m个A‑SCAN记录组成，m为雷达通道数，A‑SCAN为N个采样点对应的振幅数组，N为单次扫描中的采样数目(SAMPLES)；Wn为测距轮触发记录，所述测距轮触发记录包括触发次数、距离以及触发时间；Wn、 、Tracen组成同步采集并对齐的数据集。

[0062] 本实施例步骤S102用于对训练数据进行制作。所有通道数据组合起来沿前进方向形成一个雷达数据立方体。即 ，其中 为一条测线(Swath)的雷达数据立方体,  为三维探地雷达通道号， 为各通道对应A‑SCAN数据。
为对应通道号的由各Trace的A‑SCAN数据按照常规进行标准的数据处理后处理后转换到图像域形成的二维B‑SCAN图像。

[0063] 多次扫描可以对齐为更大的立方体如下： ；其中，为多测线雷达数据立方体，Swath为测线编号， 为第n条测线对应的雷达数据立方体。

[0064] 以16通道雷达为例，Δd触发= 5cm，表示前进方向每5cm触发一次；深度方向转换为512像素点，总测线长度为1000m, 则一次扫描可以形成一个维度为16×512×20000(宽×深×长) 的图像域雷达数据立方体，长度方向每一点对应一组定位信息，定位信息包括RTK、惯性导航以及里程计，可以通过对起点的相对距离计算得到。

[0065] 作为定位系统，沥青层和面层等浅表层连续重复目标对定位作用不大甚至有副作用，故通过直达波过滤、面层跟踪与消除等技术将这些因素消除。图3所示的左边为因素切除前的示意图，右边为因素切除后的示意图。

[0066] 本实施例所述步骤S103用于实现特征点云里程计训练数据制作；所述步骤S103优选包括以下子步骤：

[0067] 步骤S1031，通过地下目标检测模型对雷达数据进行扫描并自动标注出特征明显的目标作为导航点，特征明显的目标包括地下管廊、管线、沙井、空洞、脱空以及富水等常见异常体；所述地下目标检测模型可以直接采用现有的地下目标检测模型；

[0068] 步骤S1032，对检测出的目标按照深度学习中实例分割的要求进行审核及轮廓标注，其中，对检测出的目标的一次波、多次波以及绕射波的轮廓进行全部标注；

[0069] 步骤S1033，将标注好的轮廓按照预先设置的格式进行保存；所述预先设置的格式可以根据实际情况和需求进行调整，默认为coco格式。

[0070] 本实施例所述步骤S104用于实现特征图匹配里程计训练数据制作；所述步骤S104优选包括以下子步骤：

[0071] 步骤S1041，通过公式 遍历数据集的探地雷达子集 以固定间隔依次提取两帧数据GPR1、GPR2，其中， 为当前道数的编号； 为道数步长，优选为256； 为道数宽度，优选为512；以此形成两幅维度为16×512×512，行进方向相互间重叠256的探地雷达3D体素；所述固定间隔可以根据实际情况进行自定义设置和调整，所述探地雷达3D体素也称重叠序列图像，推理时按照同样方式即可形成重叠序列图像；

[0072] 步骤S1042，将探地雷达3D体素与数据集 中的对应实时差分和惯性导航的组合数据（RTK‑IMU数据）以预先设置的格式进行保存，比如通过coco 格式进行保存。其中，探地雷达3D体素默认以多帧tiff格式保存，轮廓标注信息默认以json点形式保存。

[0073] 本实施例所述步骤S2用于实现基于特征点云和特征图匹配的探地雷达里程计，需要用到两种探地雷达里程计，即基于特征点云和特征图匹配的探地雷达里程计，分别对应两种基于深度学习的辅助定位模型，即探地雷达特征点云提取和探地雷达特征图匹配模型。

[0074] 本实施例所述步骤S2实现探地雷达特征点云提取模型的过程包括以下子步骤：

[0075] 步骤S211，基于深度学习的全通道2D B‑SCAN及C‑SCAN实例分割和结果聚合的方式生成3D感兴趣区域，将异常体从背景中精确的分割出来；

[0076] 步骤S212，对分割出来的图像进行轮廓处理；

[0077] 步骤S213，将获得的各轮廓线按常规逐点转换为带强度的PCL格式3D点云，用于创建基于点云的导航控制点。

[0078] 本实施例所述步骤S211用于实现所述探地雷达特征点云提取模型。现有技术中，基于激光点云的导航地图制作与应用已经比较成熟。一般说来点云地图的精度依赖于高质量的点云特征，但由于探地雷达的信噪比远低于激光雷达等高精度传感设备，存在大量的噪声，传统的图像处理采用的各类阈值分割由于地下空间各类干扰信号的存在效果并不理想，只能作为辅助手段。而基于2D图像做对比的方案，如直接比较B‑SCAN间欧式距离/余弦距离等算法、以及改进的基于从B‑SCAN中利用深度神经网络提取的特征进行匹配的算法，由于2D图像自身信息量较少，常常无法达到精确匹配的目的。

[0079] 基于3D卷积神经网络的实例分割可以高精度的将地下异常体从背景中分离，然而高昂的数据标注及三维卷积操作对人力和计算资源的巨大消耗导致此方案难以实行。因此，本实施例采用简化的基于深度学习的全通道2D B‑SCAN及C‑SCAN实例分割和结果聚合的方式替代3D实例分割生成3D ROI（region of interest），将异常体从背景中精确的分割出来，并通过后处理转换为用于点云定位算法的3D地下特征点云，其原理图如图4所示。

[0080] 在探地雷达图像中，B‑SCAN（xy轴）的目标特征最为明显，C‑SCAN(xz轴)有助于确定一些管线等目标的轮廓，而D‑SCAN（yz轴）目标特征较为不明显，为节约资源，本实施例优选采用B‑SCAN和C‑SCAN检测。

[0081] 针对B‑SCAN/C‑SCAN中目标特征复杂程度不同的特点，Mask R‑CNN分别采用Resnet50/18骨干网，并添加FPN以及采用了SyncBN和ROIAlignV2结构增强的实例分割头(Mask head)进行实例分割，该网络以Mask的形式逐像素点输出实例分割结果，需要通过对Mask进行后处理获得最终的点云。

[0082] 本实施例所述步骤S212用于实现轮廓处理；所述步骤S212包括以下子步骤：

[0083] 步骤S2121，采用基于边界跟踪的拓扑结构分析算法(Suzuki, S. and Abe, K., Topological Structural Analysis of Digitized Binary Images by Border Following.CVGIP 30 1, pp 32‑46 (1985))从实例分割输出的Mask二值图像中提取出目标外围轮廓；值得说明的是，本实施例只提取最外层轮廓，并且采用Teh‑Chin 链近似方法进行轮廓近似；

[0084] 步骤S2122，通过计算轮廓线内面积是否大于指定阈值来进行判断，将轮廓线内面积小于指定阈值的小目标进行清除，进而实现清除小目标以最大限度去掉干扰；本实施例所述指定阈值可以根据实际情况进行自定义设置和调整，优选为宽度大于20像素，总面积大于20×10像素，否则判断为小于指定阈值，进行清除；

[0085] 步骤S2123，通过凸包算法查找包含原轮廓点的最小凸多边形来代替直接使用原来的轮廓点集，以进行轮廓曲线平滑处理。本实施例这样设计的原因在于模型输出的Mask经常带有大量的细小的随机锯齿结构，转换为点云后不但增大了数据量而且客观上形成了噪点影响点云匹配精度，通过轮廓曲线平滑处理进而旨在达到平滑轮廓曲线降低噪声的目的。

[0086] 轮廓处理后，分别将获得的各轮廓线按常规逐点转换为带强度(Intensity)的PCL格式3D点云用于创建基于点云的导航控制点。对于上述3D ROI无输出的路段，需要通过常规算法制作点云用于辅助定位。本实施例通过采用多阈值OSTU算法进行分割，并按照亮度Intensity阈值进一步过滤噪点，通过聚类和降采样等处理得到目标点云参与点云建图以及定位。

[0087] 在实现探地雷达特征图匹配模型的过程中，在某些路段地下空间可能存在特征稀疏的情况，点云定位难以提供必要的精度，需要直接对惯导进行修正来保证必要的精度。传统的直接通过单张B‑SCAN图像或扩展的B‑SCAN特征图像计算欧式距离等算法进行匹配定位的方法在某些场景下鲁棒性不强，本例通过建立重叠序列雷达图像与对应的位姿之间的关联，连续计算定位损失进行回归来训练深度学习模型，达到对惯导数据进行修正的目的，详细描述如下：重叠序列雷达图像由雷达数据GPRt=(GPR1,… GPRt)组成，其中各元素按照固定间隔（宽度width=512，步长stride=256，  两帧间重叠256）依次中获得。

[0088] 序列雷达图像对应的位姿Poset = (Pose1,… Poset)，其中Pose = (p, v, q)， p、v、q分别为为位置，速度，角速度分量。则问题可以化为计算重叠序列雷达图像GPRt的位姿Poset随 时间t变化的条件概率问题： 采用利*
用 神经 网 络来 拟 合 p ，则 转化 为 寻 找最 优的 神经 网 络参 数θ 问 题 ：
。

[0089] 利用均方误差来作为损失函数（mean‑square error, MSE loss），其中N为总样本数， 为马氏距离(Mahalanobis distance)二范数， 和 分别为位姿真值和网络预测值，计算公式为： 。

[0090] 因此，本实施例所述步骤S2中，实现探地雷达特征图匹配模型的过程包括以下子步骤：

[0091] 步骤S221，搭建神经网络，通过求解 获取重叠序列雷达图像 与对应的位姿 之间的关联，其中 为神经网络参数，为重叠序列雷达图像GPRt对应的位姿Poset随时间t变化的条件概率， 为最优的神经网络参数， 表示获取当条件概率 取得极大值且自变量取值为神经网络参数 的值的函数；

[0092] 步骤S222，通过马氏距离来计算真值 和网络预测值之间的差，并利用Huber loss损失函数进行回归来训练探地雷达特征图
匹配模型的深度学习；其中， ， 和 分别为t
时刻和t‑k时刻的序列雷达图像位姿， 为特征图传递给下游回归网络的
输出预测值， k为两帧重叠序列图像的时间间隔。

[0093] 在本实施例步骤S222所使用的神经网络框图如图5所示，骨干网输出的特征图的个数为n。利用皮尔森积矩(Pearson product‑moment )来计算特征图对[Pt‑k,1, Pt,1], [Pt‑k,n, Pt,n]之间的相关系数并传递给下游回归网络输出预测值 )。

[0094] t时刻和t‑k时刻的相对位姿变化值利用公式计算而得。利用马氏距离(Mahalanobis distance)来计算真值 和网络预测值 之间的差，并利用Huber loss损失函数进行回归。

[0095] 值得说明的是，为加速特征提取，本实施例所述步骤S2中，所述探地雷达特征点云提取模型的特征提取网络中，采用yolov5s的骨干网络CSPDarknet53，并借鉴华为Ghostnet(GhostNet: More Features from Cheap Operations. Han, Kai and Wang, Yunhe and Tian, Qi and Guo, Jianyuan and Xu, Chunjing and Xu, Chang. CVPR 2020)用幻影卷积(GhostConv)模块和幻影瓶颈(GhostBottleneck)模块取代了对应的常规卷积和CSP Bottleneck模块成为yolov5s‑ghost，从而在不影响模型的性能的前提下有效降低了对计算资源的需求。

[0096] 本实施例所述步骤S3用于实现基于因子图的探地雷达机器人组合应用。本实施例提供基于因子图的一个实施例：基于超级里程计的融合定位SLAM。相对于传统的基于卡尔曼滤波和粒子群优化等导航方式，基于因子图(Factor Graph)优化的多传感器融合定位与导航可以实现各感知子系统的动态加入与退出，特别是以IMU为核心的超级里程计算法（Super Odometry: IMU‑centric LiDAR‑Visual‑Inertial Estimator for Challenging Environments）提供了一种简单而有效的方法来融合多个传感器, 可在极端和感知挑战的环境中实现准确的实时状态估计。本实施例在此算法基础上，通过加入基于探地雷达特征点云的和基于特征图的GPR里程计来构成SLAM算法完成探地雷达机器人导航任务。

[0097] 高斯噪声下的因子图组合导航可以转化为求解各参数最大后验概率(Maximum a posteriori)问题： ；其中，i为因子图组合导航中第i个因子，  是根据状态x给出预测值的量测函数， 是实际测量值， 为马氏距离(Mahalanobis distance)二范数。

[0098] 加入GPR特征点云里程计和GPR特征图里程计后，本实施例所述步骤S3中，通过公式 实现探地雷达的组合导航，其中， 为加入探地雷达特征点云里程计和探地雷达特征图里程计之后的后验概率， 为非线性最小二乘求解函数，gpr_c、gpr_f和imu分别对应基于探地雷达特征点云的探地雷达里程计、基于特征图的探地雷达里程计和基于惯性导航的里程计； 、 和 分别为因子图中对应基于探地雷达特征点
云、基于探地雷达特征图和基于惯性导航的里程计中两相邻状态节点的相对位姿变化，、 和 分别为基于探地雷达特征点云的里程计输出、基于探地雷达特征图
的里程计输出和基于惯性导航的里程计测量输出，算法框图如图6所示。

[0099] 在图6中，惯性导航里程计按照预积分因子连续修正位姿，并利用GPR特征点云里程计和GPR特征图里程计对惯导累计误差进行修订，同时在各里程计约束下的惯性导航里程计也将预测值输出给各里程计。各节点可以根据实际情况动态调整，由惯性导航里程计输出最终结果。

[0100] 在本申请的另一种实施例中，探地雷达机器人由安装在车辆前部的探地雷达以及辅助定位及视频等相关设备组成，作业示意图如7所示。

[0101] 本实施例还提供一种基于雷达图像的导航地图制作和使用系统，采用了如上所述的基于雷达图像的导航地图制作和使用方法，并包括组合导航装置、三维探地雷达和编码器，所述组合导航装置、三维探地雷达和编码器安装于探测车上并校准，建立各坐标系变换矩阵；其中，所述组合导航装置为能提供惯性导航、全球定位导航以及里程计组合导航的设备；

[0102] 在采集时，从有实时差分RTK精确解的位置开始，以惯性导航单元IMU为核心，全覆盖采集地下空间数据创建导航地图，创建导航地图优选基于SLAM，在实时差分有效区以常规实时差分里程计为主修正惯性导航单元，在特征图里程计修正区通过所述探地雷达特征图匹配模型提取和匹配特征图对惯性导航单元进行修正，在特征点云里程计修正区通过所述探地雷达特征点云提取模型提取地下空间特征点云作为导航点并进行点云匹配定位修正惯性导航单元；在无实时差分的情况下通过连续修正惯性导航单元输出以获得高精度的导航地图；

[0103] 在复测验证时，与采集一致，从有实时差分RTK的位置开始初始校准，然后按照因子图导航模式根据各子系统精度动态加入和解除各因子，完成复测验证导航任务。

[0104] 综上所述，本实施例充分利用了地下隐患探测作业中探地雷达可以获取高密度地下空间探测数据的优点，在探测作业时就通过探地雷达数据构造高精度地下空间地图，并结合惯性导航实现高精度隐患定位和导航目的，可在复测验证阶段将探地雷达机器人自动引导到隐患位置；为长隧道、跨江通道以及大型地下交通设施等无GPS场景下的地下隐患探测作业所需要的高精度自主定位提供了一种新型定位和导航方式，能够大幅度降低作业成本，提高作业效率以及作业的可靠性能。

[0105] 以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干简单推演或替换，都应当视为属于本发明的保护范围。