CN114660589B

CN114660589B - 一种地下隧道的定位方法、系统及装置

Info

Publication number: CN114660589B
Application number: CN202210300735.0A
Authority: CN
Inventors: 秦念稳; 肖正航; 黎胜根; 向宙; 邓泽; 刘加妮
Original assignee: China Railway Construction Heavy Industry Group Co Ltd
Current assignee: China Railway Construction Heavy Industry Group Co Ltd
Priority date: 2022-03-25
Filing date: 2022-03-25
Publication date: 2023-03-10
Anticipated expiration: 2042-03-25
Also published as: CN114660589A

Abstract

本申请公开了一种地下隧道的定位方法、系统及装置，应用于工程机械领域，该方法包括：获取目标车辆通过雷达扫描得到的扫描信息；获取所述目标车辆的惯导单元发送的、对应所述扫描信息的惯导信息；利用扫描信息提取特征点；利用惯导信息进行预积分得到初步预测位姿信息；在预存地图中，根据初步预测位姿信息，将在预存地图的所有观测值中最接近每个特征点的观测值作为特征点的观测值；利用初步预测位姿信息和所有特征点的观测值进行迭代卡尔曼滤波，得到最终位姿信息以进行定位。本申请用于在地下隧道中定位，结合扫描信息和初步预测位姿信息确定预存地图中的观测值，并使用卡尔曼滤波进行位姿优化，定位结果具有较高的准确性。

Description

一种地下隧道的定位方法、系统及装置

技术领域

本发明涉及工程机械领域，特别涉及一种地下隧道的定位方法、系统及装置。

背景技术

地下工程是指深入地面以下，为开发利用地下空间资源所建造的地下土木工程。地下工程建造期间的施工环境，往往具有危险性和恶劣性，因而施工机械的智能化和无人化已经成为必然趋势，然而无论是远程遥控还是无人驾驶，对工程车辆进行地下环境定位都是先决条件；进一步的，地下工程的运营期间，无人化巡检以及为进入地下空间的用户提供定位信息也是重要的增值服务。现有地面定位技术大多依靠GPS(Global PositioningSystem，全球定位系统)提供的精准定位信息，而地下工程中由于卫星信号被地面遮挡，GPS基本没有可行性。

当前地下工程常见的定位技术有以下几种：采用UWB(Ultra Wide Band，超宽带)或RFID(Radio Frequency Identification System，射频识别)等手段进行定位，通过地下工程内部架设基站可以获取类似于GPS的绝对定位信息，但是这些信号受环境干扰大，定位精度比较低，测量稳定性也不高；视觉相机可以获取丰富的环境信息来进行定位，但是相机对光源较敏感，在地下工程环境中提供满足相机感知所需的光源环境成本较高；基于车轮里程计的模型在长距离定位中具有误差累积特性；单纯依靠惯性元件进行定位，在长时间后定位会出现漂移；基于同步定位与建图理论(Simultaneous Localization andMapping，SLAM)的定位方法往往受到地下工程中特征退化的影响，定位结果在里程方向上误差较大。

因此，如何提供一种准确度高、计算成本和使用成本较低的地下定位的方案是目前本领域技术人员需要解决的问题。

发明内容

有鉴于此，本发明的目的在于提供一种准确度高、使用成本较低的地下隧道的定位方法、系统及装置。其具体方案如下：

一种地下隧道的定位方法，包括：

获取目标车辆通过雷达扫描得到的扫描信息；

获取所述目标车辆的惯导单元发送的、对应所述扫描信息的惯导信息；

利用所述扫描信息提取特征点；

利用所述惯导信息进行预积分得到初步预测位姿信息；

在预存地图中，根据所述初步预测位姿信息，将在所述预存地图的所有观测值中最接近每个所述特征点的所述观测值作为所述特征点的观测值；

利用所述初步预测位姿信息和所有所述特征点的所述观测值进行迭代卡尔曼滤波，得到最终位姿信息以进行定位。

优选的，所述获取目标车辆通过雷达扫描得到的扫描信息的过程，包括：

获取目标车辆通过雷达扫描隧道的地下环境得到的扫描信息；

所述隧道壁面分散设有多个高反射率的反光靶。

优选的，所述特征点具体为边缘特征点、平面特征点或高反射率特征点。

优选的，所述利用所述扫描信息提取特征点的过程，包括：

对所述扫描信息中每帧雷达点云分块；

对每块所述雷达点云进行以下操作：

确定该雷达点云的协方差矩阵；

对所述协方差矩阵进行特征值分解，得到以数值大小确定的最大特征值、中间特征值和最小特征值；

若所述中间特征值与所述最小特征值的差小于第一差量，则确定该雷达点云对应的特征点为所述平面特征点；

若所述最大特征值与所述中间特征值的差大于第二差量，则确定该雷达点云对应的特征点为所述边缘特征点；

若所述雷达点云的反射强度超过预设强度，则确定该雷达点云对应的特征点为所述高反射率特征点。

优选的，所述若所述最大特征值与所述中间特征值的差大于第二差量，则确定该雷达点云对应的特征点为所述边缘特征点的过程，包括：

若所述最大特征值与所述中间特征值的差大于第二差量，则确定该雷达点云对应的特征点为可疑边缘特征点；

确定所述可疑边缘特征点的曲率、方向向量、多个线向量；每个所述线向量具体为该雷电点云中每线点云首尾连接得到的向量；

若所述曲率大于预设曲率、且存在任一线向量与所述方向向量的夹角处于预设角度范围，则确定该雷达点云对应的特征点为所述边缘特征点；

所述预设角度范围为90°±θ，θ为预设误差角度。

优选的，所述利用所述扫描信息提取特征点之前，还包括：

利用所述位姿信息对所述扫描信息去畸变。

优选的，所述利用所述初步预测位姿信息和所有所述特征点的所述观测值进行迭代卡尔曼滤波，得到最终位姿信息以进行定位的过程，包括：

逐个利用所有所述特征点的所述观测值，结合所述初步预测位姿信息进行迭代卡尔曼滤波，得到最终位姿信息以进行定位。

优选的，所述定位方法还包括：

利用最终位姿信息更新所述预存地图。

相应的，本申请还公开了一种地下隧道的定位系统，包括：

第一获取模块，用于获取目标车辆通过雷达扫描得到的扫描信息；

第二获取模块，用于获取所述目标车辆的惯导单元发送的、对应所述扫描信息的惯导信息；

特征点模块，用于利用所述扫描信息提取特征点；

初步位姿预测模块，用于利用所述惯导信息进行预积分得到初步预测位姿信息；

观测值模块，用于在预存地图中，根据所述初步预测位姿信息，将在所述预存地图的所有观测值中最接近每个所述特征点的所述观测值作为所述特征点的观测值；

计算模块，用于利用所述初步预测位姿信息和所有所述特征点的所述观测值进行迭代卡尔曼滤波，得到最终位姿信息以进行定位。

相应的，本申请还公开了一种地下隧道的定位装置，包括：

位于目标车辆的雷达和惯导单元；

存储器，用于存储计算机程序；

处理器，用于执行所述计算机程序时实现如上文任一项所述地下隧道的定位方法的步骤。

本申请公开了一种地下隧道的定位方法，包括：获取目标车辆通过雷达扫描得到的扫描信息；获取所述目标车辆的惯导单元发送的、对应所述扫描信息的惯导信息；利用所述扫描信息提取特征点；利用所述惯导信息进行预积分得到初步预测位姿信息；在预存地图中，根据所述初步预测位姿信息，将在所述预存地图的所有观测值中最接近每个所述特征点的所述观测值作为所述特征点的观测值；利用所述初步预测位姿信息和所有所述特征点的所述观测值进行迭代卡尔曼滤波，得到最终位姿信息以进行定位。本申请结合扫描信息和初步预测位姿信息确定预存地图中的观测值，并使用卡尔曼滤波进行位姿优化，定位结果具有较高的准确性，适用于地下工程，且对地下光照条件要求不高，使用成本低。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。

图1为本发明实施例中一种地下隧道的定位方法的步骤流程图；

图2a为本发明实施例中一种反光靶两侧设置的结构分布图；

图2b为本发明实施例中一种反光靶单侧设置的结构分布图；

图3为本发明实施例中一种雷达点云的结构分布图；

图4为本发明实施例中一种伪边缘点的状态示意图；

图5为本发明实施例中一种地下隧道的定位方法的方案流程图；

图6为本发明实施例中一种地下隧道的定位系统的结构分布图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

现有地面定位技术大多依靠GPS提供的精准定位信息，而地下工程中由于卫星信号被地面遮挡，GPS基本没有可行性。当前地下工程常见的定位技术中，基于UWB、RFID、里程计、惯性元件、SLAM等的定位方法，由于地下环境或技术本身误差原因，准确度较低，视觉相机则对光源环境的要求较高，地下环境难以满足。

本申请结合扫描信息和初步预测位姿信息确定预存地图中的观测值，并使用卡尔曼滤波进行位姿优化，定位结果具有较高的准确性，适用于地下工程，且对地下光照条件要求不高，使用成本低。

本发明实施例公开了一种地下隧道的定位方法，参见图1所示，包括：

S1：获取目标车辆通过雷达扫描得到的扫描信息；

可以理解的是，为了提高扫描信息的特征识别度，有助于定位，步骤S1具体可包括：获取目标车辆通过雷达扫描隧道的地下环境得到的扫描信息；隧道壁面分散设有多个高反射率的反光靶。

其中，反光靶可分别设置于隧道通道两侧或单侧，以平行或垂直于道路行进方向等距设置，间距可设为50m或100m，当然也可设为其他长度，如图2a及图2b所示，每侧的反光靶可以平行、垂直、平行、垂直的形式设置，两侧的反光靶可相对而设如图2a或图2b所示，也可间隔设置，即一侧的反光靶对应另一侧两个反光靶的中点。具体的反光靶设置方案，可根据实际的地下环境调整方向、距离、高度等参数，此处不作限制。

S2：根据目标车辆的惯导单元发送的、对应扫描信息的惯导信息；

可以理解的是，惯导信息包括加速度和角速度，通常有以下预处理：

将车辆静止超过10s，取10s内的静止段惯导信息，将角速度ω进行累计作为其零偏估计值bias，并以此确定残差累计量eInt修正值：

假设初始时刻惯导坐标系与地理坐标系重合，旋转矩阵为单位阵I_3×3＝(R_xo,R_yo,R_zo)，将该段时间内的加速度a进行累计用于校正初始时刻地理坐标系在惯导坐标系下的旋转矩阵^IR_G(R_x,R_y,R_z)：

其中^IR_G(R_x,R_y,R_z)的三个向量均需单位化，R’_y为向量R_y的初值；

由角速度ω和采样时间Δt更新旋转矩阵^IR_G：

取^IR_G的第三个分量R_z与加速度a作叉积运算得到残差e：

e＝a×R_z。

设置积分系数k_i，比例系数为k_p，残差累计量eInt为：

eInt＝eInt+k_i·e·t；

滤波后角速度为：

ω＝ω+k_p·e+eInt；

S3：利用扫描信息提取特征点；

进一步的，步骤S3中提取的特征点，可按照类别分为几何特征点和高反射率特征点，其中几何特征点又分为边缘特征点和平面特征点。具体的，步骤S3利用扫描信息提取特征点的过程，包括：

对扫描信息中每帧雷达点云分块；

对每块雷达点云进行以下操作：

确定该雷达点云的协方差矩阵；

对协方差矩阵进行特征值分解，得到以数值大小确定的最大特征值、中间特征值和最小特征值；

若中间特征值与最小特征值的差小于第一差量，则确定该雷达点云对应的特征点为平面特征点；

若最大特征值与中间特征值的差大于第二差量，则确定该雷达点云对应的特征点为边缘特征点；

若雷达点云的反射强度超过预设强度，则确定该雷达点云对应的特征点为高反射率特征点。

例如，本实施例中进行雷达扫描的雷达可选择固态激光雷达，将获取的一帧雷达点云根据时间间隔均匀取24000个点，如图3所示，每一帧扫描为6线，由于每一线获得的点云的排布为曲线，因此可将一帧点云分为若干块进行特征提取，保持所取每块的几何特征，每块雷达点云的大小为6*6，六条线取六个点，在计算协方差矩阵前，可向进行无效点筛查，之后计算每块雷电点云的协方差矩阵为：H＝∑(p-p_mean)×(p-p_mean)^T；其中p为点云，p_mean为点云中心值，也即均值。

进一步的，将协方差矩阵进行特征值分解，得到三个特征值，按数值大小可排序为最大特征值、中间特征值和最小特征值；若中间特征值与最小特征值的差小于第一差量，则确定该雷达点云对应的特征点为平面特征点，进一步可以计算平面特征点的法向量及其到雷达坐标系原点的距离；若最大特征值与中间特征值的差大于第二差量，则确定该雷达点云对应的特征点为边缘特征点；其中第一差量或第二差量可以是一个绝对数值差量，也可以是一个预设百分比与当前减数或被减数的乘积，此处如何不作限定；若雷达点云的反射强度超过预设强度，则确定该雷达点云对应的特征点为高反射率特征点。

进一步的，由于固态激光雷达在扫描平面时存在一线点云的弯折，为了避免被认为边缘点，还需进一步对边缘点是否有效进行再确认，即：若最大特征值与中间特征值的差大于第二差量，则确定该雷达点云对应的特征点为边缘特征点的过程，包括：

若最大特征值与中间特征值的差大于第二差量，则确定该雷达点云对应的特征点为可疑边缘特征点；

确定可疑边缘特征点的曲率、方向向量、多个线向量；每个线向量具体为该雷电点云中每线点云首尾连接得到的向量；

若曲率大于预设曲率、且存在任一线向量与方向向量的夹角处于预设角度范围，则确定该雷达点云对应的特征点为边缘特征点；

预设角度范围为90°±θ，θ为预设误差角度。

其中，曲率的计算过程如下：

其中g_i为边缘点i的曲率，d_i为边缘点i与坐标系原点的点距离。如果曲率大于预设曲率，则再求解后续的方向向量n，以及每线点云首尾连接得到的线向量v；如果曲率不大于预设曲率，则可直接确定其不是边缘点；

进一步的，判断任一线向量与方向向量的夹角是否处于预设角度范围，实际上是在判断是否存在线向量与方向向量接近垂直，具体计算可通过夹角余弦值的方式进行，如果在预设角度范围内，则认为特征点为有效的边缘特征点，如果不在预设角度范围内，如图4所示的伪边缘点，则被判定为无效，剔出边缘特征点。

可以理解的是，由于雷达扫描过程中目标车辆可能处于行进状态，扫描信息受到运动状态的干扰，因此利用扫描信息提取特征点之前，还可以包括：利用位姿信息对扫描信息去畸变。

具体的，对该时间段内的惯导的角速度ω(ω_x,ω_y,ω_z)进行积分，得到该帧时间段内目标车辆的变化位姿ΔR，按照每个激光扫描点与帧首的时间间隔对每个扫描点p_i插值得到位姿ΔR_i，计算当前车辆位姿下的每个扫描点去畸变后的位置：p_i(x,y,z)＝ΔR_ip_i(x,y,z)。

S4：利用惯导信息进行预积分得到初步预测位姿信息；

可以理解的是，本实施例中，步骤S3发生于步骤S1之后，步骤S4发生于步骤S2之后，步骤S3和步骤S4本身不存在必需的先后顺序，可根据实际运算情况进行任务分配，但如果步骤S3中的扫描信息需要提前去畸变，去畸变使用的位姿信息可以直接作为初步预测位姿信息，此时应先执行步骤S4再执行步骤S3。

S5：在预存地图中，根据初步预测位姿信息，将在预存地图的所有观测值中最接近每个特征点的观测值作为特征点的观测值；

具体的，预存地图被存入kd树中，根据初步预测位姿信息(R,T)将特征点映射到预存地图，将得到特征点的映射值，将映射值在预存地图的kd树中进行临近值查找，也即预存地图的所有观测值中最接近映射值的观测值作为其对应的特征点的观测值。

S6：利用初步预测位姿信息和所有特征点的观测值进行迭代卡尔曼滤波，得到最终位姿信息以进行定位。

进一步的，步骤S6包括：逐个利用所有特征点的观测值，结合初步预测位姿信息进行迭代卡尔曼滤波，得到最终位姿信息以进行定位。

具体的，本实施例中，变量空间为目标车辆的位置、速度、以及姿态X(T,V,R)的9维向量，惯导EKF预测方程为：

预测方程的雅可比矩阵为：

对于边缘特征点，其观测方程为：

其观测值对变量的雅可比矩阵为：

H_k＝[I 0-[R_kp]_×]；

对平面特征点，取其映射点的在地图坐标系下的法向量n_e与到地图坐标系原点距离D_e，观测方程为：

观测值对变量的雅可比矩阵为：

H_k,n＝[0 0 -[R_kn]_×]；

H_k,D＝[(R_kn)^T 0 -T_k ^T[R_kn]_×]；

对高反射率特征点，其观测方程为：

其观测值对变量的雅可比矩阵为：

H_k＝[I 0 -[R_kp]_×]；

I为3维的单位阵，P为状态协方差，Q为惯导系统噪声，N为观测噪声，经过惯导滤波后已经得到一步预测值

按照上述步骤线性化后即可按照线性卡尔曼滤波进行计算：

其中，K为卡尔曼滤波增益，F为状态转移方程的雅克比矩阵，H为某一特征点观测方程的雅克比矩阵。

迭代所有的特征点，每次将上次优化的变量值作为预测值，用特征点作为观测值优化解，直至迭代完所有的特征点，得到最终位姿信息以进行定位。

进一步的，还可利用最终位姿信息更新预存地图。

可以理解的是，将实施例中以上所有计算细节整理后，可以得到图5作为整体方案流程图。

本实施例中，高反射率特征构造以及平面特征、边缘特征提取的手段，可有效地解决地下工程空间特征退化问题，实现地下工程空间的观测定位；将雷达测量与惯导信息进行融合的定位方法可实现物体在地下工程环境下的独立自主实时定位；该定位方法定位精度高，程序冗余性小，处理速度快，实时性好，适合批量化使用。

相应的，本申请实施例还公开了一种地下隧道的定位系统，参见图6所示，包括：

第一获取模块1，用于获取目标车辆通过雷达扫描得到的扫描信息；

第二获取模块2，用于获取所述目标车辆的惯导单元发送的、对应所述扫描信息的惯导信息；

特征点模块3，用于利用所述扫描信息提取特征点；

初步位姿预测模块4，用于利用所述惯导信息进行预积分得到初步预测位姿信息；

观测值模块5，用于在预存地图中，根据所述初步预测位姿信息，将在所述预存地图的所有观测值中最接近每个所述特征点的所述观测值作为所述特征点的观测值；

计算模块6，用于利用所述初步预测位姿信息和所有所述特征点的所述观测值进行迭代卡尔曼滤波，得到最终位姿信息以进行定位。

在一些具体的实施例中，第一获取模块1用于：

所述隧道壁面分散设有多个高反射率的反光靶。

在一些具体的实施例中，所述特征点具体为边缘特征点、平面特征点或高反射率特征点。

在一些具体的实施例中，特征点模块3用于：

对所述扫描信息中每帧雷达点云分块；

对每块所述雷达点云进行以下操作：

确定该雷达点云的协方差矩阵；

在一些具体的实施例中，所述若所述最大特征值与所述中间特征值的差大于第二差量，则确定该雷达点云对应的特征点为所述边缘特征点的过程，包括：

所述预设角度范围为90°±θ，θ为预设误差角度。

在一些具体的实施例中，特征点模块3利用所述扫描信息提取特征点之前，还用于：

利用所述位姿信息对所述扫描信息去畸变。

在一些具体的实施例中，计算模块6具体用于：

在一些具体的实施例中，计算模块6还用于：

利用最终位姿信息更新所述预存地图。

相应的，本申请还公开了一种地下隧道的定位装置，包括：

位于目标车辆的雷达和惯导单元；

存储器，用于存储计算机程序；

其中，具体有关所述地下隧道的定位方法的细节内容，可以参照上文实施例中的相关描述，此处不再赘述。

其中，本实施例中所述地下隧道的定位装置具有与上文实施例中所述地下隧道的定位方法相同的技术效果，此处不再赘述。

最后，还需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

以上对本发明所提供的一种地下隧道的定位方法、系统及装置进行了详细介绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims

1.一种地下隧道的定位方法，其特征在于，包括：

获取目标车辆通过雷达扫描得到的扫描信息；

利用所述扫描信息提取特征点；

利用所述惯导信息进行预积分得到初步预测位姿信息；

利用所述初步预测位姿信息和所有所述特征点的所述观测值进行迭代卡尔曼滤波，得到最终位姿信息以进行定位；

所述获取目标车辆通过雷达扫描得到的扫描信息的过程，包括：

所述隧道壁面分散设有多个高反射率的反光靶；

所述特征点具体为边缘特征点、平面特征点或高反射率特征点；

所述利用所述扫描信息提取特征点的过程，包括：

对所述扫描信息中每帧雷达点云分块；

对每块所述雷达点云进行以下操作：

确定该雷达点云的协方差矩阵；

2.根据权利要求1所述定位方法，其特征在于，所述若所述最大特征值与所述中间特征值的差大于第二差量，则确定该雷达点云对应的特征点为所述边缘特征点的过程，包括：

确定所述可疑边缘特征点的曲率、方向向量、多个线向量；每个所述线向量具体为该雷达点云中每线点云首尾连接得到的向量；

所述预设角度范围为90°±θ，θ为预设误差角度。

3.根据权利要求1所述定位方法，其特征在于，所述利用所述扫描信息提取特征点之前，还包括：

利用所述位姿信息对所述扫描信息去畸变。

4.根据权利要求1至3任一项所述定位方法，其特征在于，所述利用所述初步预测位姿信息和所有所述特征点的所述观测值进行迭代卡尔曼滤波，得到最终位姿信息以进行定位的过程，包括：

5.根据权利要求4所述定位方法，其特征在于，还包括：

利用最终位姿信息更新所述预存地图。

6.一种地下隧道的定位系统，其特征在于，包括：

特征点模块，用于利用所述扫描信息提取特征点；

计算模块，用于利用所述初步预测位姿信息和所有所述特征点的所述观测值进行迭代卡尔曼滤波，得到最终位姿信息以进行定位；

所述隧道壁面分散设有多个高反射率的反光靶；

所述利用所述扫描信息提取特征点的过程，包括：

对所述扫描信息中每帧雷达点云分块；

对每块所述雷达点云进行以下操作：

确定该雷达点云的协方差矩阵；

7.一种地下隧道的定位装置，其特征在于，包括：

位于目标车辆的雷达和惯导单元；

存储器，用于存储计算机程序；

处理器，用于执行所述计算机程序时实现如权利要求1至5任一项所述地下隧道的定位方法的步骤。