高精度的井下车辆定位方法及其系统技术方案

本发明专利技术公开了一种高精度的井下车辆定位方法及其系统，包括以下步骤：通过UWB定位标签卡获取车辆与UWB基站之间的相对位置坐标，通过IMU传感器获取车辆的位姿数据，通过激光雷达获取井下巷道点云数据，将车辆与UWB基站之间的相对位置坐标、车辆的位姿数据以及井下巷道点云数据进行融合处理，得到井下车辆的最终定位信息。本发明专利技术将UWB、IMU和激光雷达三种定位数据进行融合处理，激光雷达可以降低UWB因障碍物导致的定位误差，UWB可以提供粗定位，减小IMU和激光雷达的误差累计，IMU可以减小因环境因素导致的误差，三种定位方式相辅相成，最终实现井下高精度的车辆定位与轨迹跟踪，使井下车辆的自动驾驶成为可能。下车辆的自动驾驶成为可能。下车辆的自动驾驶成为可能。

【技术实现步骤摘要】
高精度的井下车辆定位方法及其系统


[0001]本专利技术涉及井下车辆自动驾驶
，尤其涉及一种高精度的井下车辆定位方法及其系统。

技术介绍

[0002]现阶段，自动驾驶车辆已逐渐成为研究的热点，在一些港口、园区、物流中心等物权明确且相对封闭的场景中，自动驾驶车辆已经有了相对成熟的商业案列。自动驾驶作为未来的一大科技重点，实现高精准度自主导航是最终目标，在自主导航系统中，定位问题无疑是首先要解决的难题，准确的位姿估计是实现自主导航的关键技术。
[0003]在煤矿行业内，自动驾驶矿卡在露天煤矿的应用也逐渐成熟。但是，现阶段的自动驾驶车辆定位方案大多是基于GNSS(全球导航卫星系统)信号，而井工煤矿处于地下封闭空间，GNSS信号弱，井下自动驾驶定位无法获取GNSS信号，并且井工煤矿环境复杂，巷道多、光线昏暗，地面自动驾驶相关定位方法无法直接应用于井下车辆。
[0004]为了解决井下车辆定位的问题，目前已有基于UWB、IMU或激光雷达的定位方案，但是，UWB属于有源定位方法，UWB信号容易受到遮挡物的影响，且当UWB信号受到干扰时，会产生较大的误差；IMU属于无源定位，IMU对于井下辅助运输车辆来说价格高昂，且长时间连续使用存在误差累积的问题，自身也存在漂移的问题；激光雷达在环境单一或车辆移动较快时，会产生数据错位、遮挡、速度快、视角不断变化等问题。因此，在复杂的煤矿井下环境中，现有的井下车辆定位只依靠某一种定位方法，很难取得高精度的定位结果，无法为井下车辆自动驾驶提供可靠的技术支撑。
专利技术内容
[0005]本专利技术要解决的技术问题是：为了解决现有的定位方法无法适用于井下车辆自动驾驶场景的技术问题，本专利技术提供一种高精度的井下车辆定位方法及其系统，能够进一步提高井下车辆定位的精度。
[0006]本专利技术解决其技术问题所采用的技术方案是：一种高精度的井下车辆定位方法，包括以下步骤：
[0007]S1、通过UWB定位标签卡获取车辆与UWB基站之间的相对位置坐标；
[0008]S2、通过IMU传感器获取车辆的位姿数据；
[0009]S3、通过激光雷达获取井下巷道点云数据；
[0010]S4、将所述车辆与UWB基站之间的相对位置坐标、车辆的位姿数据以及井下巷道点云数据进行融合处理，得到井下车辆的最终定位信息。
[0011]进一步的，步骤S4中的融合处理，包括以下步骤：
[0012]根据所述车辆与UWB基站之间的相对位置坐标构建UWB位置预测值；
[0013]根据所述车辆的位姿数据构建IMU位置预测值；
[0014]根据所述井下巷道点云数据构建雷达位置预测值；
[0015]根据所述UWB位置预测值、IMU位置预测值、雷达位置预测值、相对位置坐标、位姿数据和井下巷道点云数据，构建融合函数F(p)，p表示车辆位置坐标；
[0016]计算融合函数F(p)的最优解，得到井下车辆的最终定位信息。
[0017]进一步的，构建融合函数F(p)包括以下步骤：
[0018]计算相对位置坐标与UWB位置预测值之间的残差
[0019]计算位姿数据与IMU位置预测值之间的残差
[0020]计算井下巷道点云数据与雷达位置预测值之间的残差
[0021]分别获取相对位置坐标、位姿数据和井下巷道点云数据的权重和
[0022]融合函数F(p)的公式为：
[0023][0024]其中，p表示坐标位置，t表示时间，C表示通过闭环检测的雷达位置预测值的集合，c
i
和c
i
分别表示集合C里的两个数据点，分别表示残差的转置。
[0025]进一步的，所述权重和的比值为1：1：1。
[0026]进一步的，构建UWB位置预测值，具体包括：
[0027]获取车辆与m个UWB基站之间的距离R
m
；
[0028]计算车辆与第A个UWB基站之间的距离R
A
，计算车辆与第B个UWB基站之间的距离R
B
，计算距离R
A
与R
B
之间的差值R
A,B
＝R
A
‑
R
B
；
[0029]根据距离R
m
以及差值R
A,B
能够获得以下关系式：
[0030]R
A,12
+2R
A,1
R1＝K
A
‑
2x
A,1
x
‑
2y
A,1
‑
K1[0031]其中，(x,y)表示UWB位置预测值，K
A
＝(x
A2
+y
A2
)，K1＝(x
12
+y
12
)，x
A,1
＝x
A
‑
x1，y
A,1
＝y
A
‑
y1，R1表示车辆与第1个UWB基站之间的距离。
[0032]进一步的，构建雷达位置预测值，具体包括：
[0033]通过激光雷达扫描获取井下巷道的点云数据集；
[0034]获取井下巷道地图的点云匹配集；
[0035]计算点云数据集和点云匹配集之间的相对变换函数E(Q,G)，Q表示旋转矩阵，G表示平移矩阵；
[0036]对所述相对变换函数E(Q,G)进行闭环检测，若E(Q,G)小于设定阈值，则表明点云数据集和点云匹配集之间匹配良好，符合闭环要求；否则，重新计算相对变换函数；
[0037]利用符合闭环要求的相对变换函数E(Q,G)对所述点云数据集进行空间变换，确定车辆在井下巷道地图中的预测位置，即雷达位置预测值。
[0038]进一步的，计算融合函数F(p)的最优解，具体包括：
[0039]当F(p)＝0时，解出的p值即为最优解；
[0040]对所述融合函数F(p)进行非线性优化，得到优化函数：
[0041]F(p+Δp)≈F(p)+J(p)Δp
[0042]其中，表示F(p)的雅可比矩阵，Δp表示增量；
[0043]令F(p+Δp)≈F(p)+J(p)Δp＝0，利用最小二乘法求解出最优增量Δp
*
，则p+Δp
*
即为最优解。
[0044]进一步的，获取车辆与m个UWB基站之间的距离R
m
，包括：
[0045]通过UWB标签卡发送UWB信号，在UWB信号覆盖范围内的所有UWB基站均能够接收到该UWB信号；
[0046]获取UWB标签卡与m个UWB基站之间的报文飞行时间差；
[0047]根据报文飞行时间差计算UWB标签卡与UWB基站之间的距离。
[0048]进一步的，构建IMU位置预测值，具体包括：
[0049]获取车辆在t...

【技术保护点】

【技术特征摘要】
其中，(x,y)表示UWB位置预测值，K
A
＝(x
A2
+y
A2
)，K1＝(x
12
+y
12
)，x
A,1
＝x
A
‑
x1，y
A,1
＝y
A
‑
y1，R1表示车辆与第1个UWB基站之间的距离。6.如权利要求3所述的高精度的井下车辆定位方法，其特征在于，构建雷达位置预测值，具体包括：通过激光雷达扫描获取井下巷道的点云数据集；获取井下巷道地图的点云匹配集；计算点云数据集和点云匹配集之间的相对变换函数E(Q,G)，Q表示旋转矩阵，G表示平移矩阵；对所述相对变换函数E(Q,G)进行闭环检测，若E(Q,G)小于设定阈值，则表明点云数据集和点云匹配集之间匹配良好，符合闭环要求；否则，重新计算相对变换函数；利用符合闭环要求的相对变换函数E(Q,G)对所述点云数据集进行空间变换，确定车辆在井下巷道地图中的预测位置，即雷达位置预测值。7.如权利要求3所述的高精度的井下车辆定位方法，其特征在于，计算融合函数F(p)的最优解，具体包括：当F(p)＝0时，解出的p值即为最优解；对所述融合函数F(p)进行非线性优化，得到优化函数：F(p+Δp)≈F(p)+J(p)Δp其中，表示F(p)的雅可比矩阵，Δp表示增量；令F(p+Δp)≈F(p)+J(p)Δp＝0，利用最小二乘法求解出最优增量Δp
*
，则p+Δp
*
即为最优解。8.如权利要求5所述的高精度的井下车辆定位方法，其特征在于，获取车辆与m个UWB基站之间的距离R
m
...

【专利技术属性】
技术研发人员：王天宇，周李兵，陈晓晶，邹盛，于政乾，赵叶鑫，王国庆，卫健健，蒋雪利，
申请(专利权)人：中煤科工集团常州研究院有限公司，
类型：发明
国别省市：