CN115540858A - 高精度的井下车辆定位方法及其系统 - Google Patents

高精度的井下车辆定位方法及其系统 Download PDF

Info

Publication number
CN115540858A
CN115540858A CN202211108291.7A CN202211108291A CN115540858A CN 115540858 A CN115540858 A CN 115540858A CN 202211108291 A CN202211108291 A CN 202211108291A CN 115540858 A CN115540858 A CN 115540858A
Authority
CN
China
Prior art keywords
vehicle
uwb
point cloud
underground
imu
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202211108291.7A
Other languages
English (en)
Inventor
王天宇
周李兵
陈晓晶
邹盛
于政乾
赵叶鑫
王国庆
卫健健
蒋雪利
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Tiandi Changzhou Automation Co Ltd
Changzhou Research Institute of China Coal Technology and Engineering Group Corp
Original Assignee
Tiandi Changzhou Automation Co Ltd
Changzhou Research Institute of China Coal Technology and Engineering Group Corp
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Tiandi Changzhou Automation Co Ltd, Changzhou Research Institute of China Coal Technology and Engineering Group Corp filed Critical Tiandi Changzhou Automation Co Ltd
Priority to CN202211108291.7A priority Critical patent/CN115540858A/zh
Publication of CN115540858A publication Critical patent/CN115540858A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0257—Hybrid positioning
    • G01S5/0263—Hybrid positioning by combining or switching between positions derived from two or more separate positioning systems
    • G01S5/0264—Hybrid positioning by combining or switching between positions derived from two or more separate positioning systems at least one of the systems being a non-radio wave positioning system

Abstract

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

Description

高精度的井下车辆定位方法及其系统
技术领域
本发明涉及井下车辆自动驾驶技术领域，尤其涉及一种高精度的井下车辆定位方法及其系统。
背景技术
现阶段，自动驾驶车辆已逐渐成为研究的热点，在一些港口、园区、物流中心等物权明确且相对封闭的场景中，自动驾驶车辆已经有了相对成熟的商业案列。自动驾驶作为未来的一大科技重点，实现高精准度自主导航是最终目标，在自主导航系统中，定位问题无疑是首先要解决的难题，准确的位姿估计是实现自主导航的关键技术。
在煤矿行业内，自动驾驶矿卡在露天煤矿的应用也逐渐成熟。但是，现阶段的自动驾驶车辆定位方案大多是基于GNSS(全球导航卫星系统)信号，而井工煤矿处于地下封闭空间，GNSS信号弱，井下自动驾驶定位无法获取GNSS信号，并且井工煤矿环境复杂，巷道多、光线昏暗，地面自动驾驶相关定位方法无法直接应用于井下车辆。
为了解决井下车辆定位的问题，目前已有基于UWB、IMU或激光雷达的定位方案，但是，UWB属于有源定位方法，UWB信号容易受到遮挡物的影响，且当UWB信号受到干扰时，会产生较大的误差；IMU属于无源定位，IMU对于井下辅助运输车辆来说价格高昂，且长时间连续使用存在误差累积的问题，自身也存在漂移的问题；激光雷达在环境单一或车辆移动较快时，会产生数据错位、遮挡、速度快、视角不断变化等问题。因此，在复杂的煤矿井下环境中，现有的井下车辆定位只依靠某一种定位方法，很难取得高精度的定位结果，无法为井下车辆自动驾驶提供可靠的技术支撑。
发明内容
本发明要解决的技术问题是：为了解决现有的定位方法无法适用于井下车辆自动驾驶场景的技术问题，本发明提供一种高精度的井下车辆定位方法及其系统，能够进一步提高井下车辆定位的精度。
本发明解决其技术问题所采用的技术方案是：一种高精度的井下车辆定位方法，包括以下步骤：
S1、通过UWB定位标签卡获取车辆与UWB基站之间的相对位置坐标；
S2、通过IMU传感器获取车辆的位姿数据；
S3、通过激光雷达获取井下巷道点云数据；
S4、将所述车辆与UWB基站之间的相对位置坐标、车辆的位姿数据以及井下巷道点云数据进行融合处理，得到井下车辆的最终定位信息。
进一步的，步骤S4中的融合处理，包括以下步骤：
根据所述车辆与UWB基站之间的相对位置坐标构建UWB位置预测值；
根据所述车辆的位姿数据构建IMU位置预测值；
根据所述井下巷道点云数据构建雷达位置预测值；
根据所述UWB位置预测值、IMU位置预测值、雷达位置预测值、相对位置坐标、位姿数据和井下巷道点云数据，构建融合函数F(p)，p表示车辆位置坐标；
计算融合函数F(p)的最优解，得到井下车辆的最终定位信息。
进一步的，构建融合函数F(p)包括以下步骤：
计算相对位置坐标与UWB位置预测值之间的残差
Figure BDA0003842630020000021
计算位姿数据与IMU位置预测值之间的残差
Figure BDA0003842630020000022
计算井下巷道点云数据与雷达位置预测值之间的残差
Figure BDA0003842630020000031
分别获取相对位置坐标、位姿数据和井下巷道点云数据的权重
Figure BDA0003842630020000032
和
Figure BDA0003842630020000033
融合函数F(p)的公式为：
Figure BDA0003842630020000034
其中，p表示坐标位置，t表示时间，C表示通过闭环检测的雷达位置预测值的集合，ci和ci分别表示集合C里的两个数据点，
Figure BDA0003842630020000035
分别表示残差的转置。
进一步的，所述权重
Figure BDA0003842630020000036
和
Figure BDA0003842630020000037
的比值为1：1：1。
进一步的，构建UWB位置预测值，具体包括：
获取车辆与m个UWB基站之间的距离Rm；
计算车辆与第A个UWB基站之间的距离RA，计算车辆与第B个UWB基站之间的距离RB，计算距离RA与RB之间的差值RA,B＝RA-RB；
根据距离Rm以及差值RA,B能够获得以下关系式：
RA,1 2+2RA,1R1＝KA-2xA,1x-2yA,1-K1
其中，(x,y)表示UWB位置预测值，KA＝(xA 2+yA 2)，K1＝(x1 2+y1 2)，xA,1＝xA-x1，yA,1＝yA-y1，R1表示车辆与第1个UWB基站之间的距离。
进一步的，构建雷达位置预测值，具体包括：
通过激光雷达扫描获取井下巷道的点云数据集；
获取井下巷道地图的点云匹配集；
计算点云数据集和点云匹配集之间的相对变换函数E(Q,G)，Q表示旋转矩阵，G表示平移矩阵；
对所述相对变换函数E(Q,G)进行闭环检测，若E(Q,G)小于设定阈值，则表明点云数据集和点云匹配集之间匹配良好，符合闭环要求；否则，重新计算相对变换函数；
利用符合闭环要求的相对变换函数E(Q,G)对所述点云数据集进行空间变换，确定车辆在井下巷道地图中的预测位置，即雷达位置预测值。
进一步的，计算融合函数F(p)的最优解，具体包括：
当F(p)＝0时，解出的p值即为最优解；
对所述融合函数F(p)进行非线性优化，得到优化函数：
F(p+Δp)≈F(p)+J(p)Δp
其中，
Figure BDA0003842630020000041
表示F(p)的雅可比矩阵，Δp表示增量；
令F(p+Δp)≈F(p)+J(p)Δp＝0，利用最小二乘法求解出最优增量Δp*，则p+Δp*即为最优解。
进一步的，获取车辆与m个UWB基站之间的距离Rm，包括：
通过UWB标签卡发送UWB信号，在UWB信号覆盖范围内的所有UWB基站均能够接收到该UWB信号；
获取UWB标签卡与m个UWB基站之间的报文飞行时间差；
根据报文飞行时间差计算UWB标签卡与UWB基站之间的距离。
进一步的，构建IMU位置预测值，具体包括：
获取车辆在ti时刻的位置Pwbi以及在世界坐标系下的速度Vi w；
车辆当前在tj时刻的IMU位置预测值为：
Figure BDA0003842630020000051
其中，
Figure BDA0003842630020000055
表示车辆在第tj时刻的预测位置，Vj w表示车辆在第tj时刻在世界坐标系下的预测速度，gw表示世界坐标系下的车辆重力加速度，Δt表示tj和ti的时间间隔，
Figure BDA0003842630020000052
分别表示在ti、tj时刻位姿的旋转矩阵，
Figure BDA0003842630020000053
和
Figure BDA0003842630020000054
表示tj时刻相对于ti时刻的IMU预积分量。
本发明还提供了一种高精度的井下车辆定位系统，使用所述的高精度的井下车辆定位方法，所述系统包括：
UWB标签卡，所述UWB标签卡安装在车辆上；
IMU传感器，所述IMU传感器安装在车辆上；
激光雷达，所述激光雷达安装在车辆上；
UWB基站，所述UWB基站安装在井下巷道内，所述UWB基站与所述UWB标签卡通讯连接；
控制器，所述UWB标签卡、IMU传感器、激光雷达及UWB基站均与所述控制器通讯连接，所述控制器用于执行如权利要求1-9任一项所述的高精度的井下车辆定位方法的步骤。
本发明的有益效果是，本发明将UWB、IMU和激光雷达三种定位数据进行融合处理，激光雷达可以降低UWB因障碍物导致的定位误差，UWB可以提供粗定位，减小IMU和激光雷达的误差累计，IMU可以减小因环境因素导致的误差，三种定位方式相辅相成，最终实现井下高精度的车辆定位与轨迹跟踪，使井下车辆的自动驾驶成为可能。
附图说明
下面结合附图和实施例对本发明进一步说明。
图1是本发明的高精度的井下车辆定位方法的流程图。
图2是本发明的高精度的井下车辆定位方法的具体流程框图。
图3是本发明的高精度的井下车辆定位系统的示意图。
图中：1、UWB标签卡；2、IMU传感器；3、激光雷达；4、UWB基站；5、控制器。
具体实施方式
现在结合附图对本发明作进一步详细的说明。这些附图均为简化的示意图，仅以示意方式说明本发明的基本结构，因此其仅显示与本发明有关的构成。
在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“长度”、“宽度”、“厚度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”、“顺时针”、“逆时针”、“轴向”、“径向”、“周向”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明的描述中，除非另有说明，“多个”的含义是两个或两个以上。
在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。
如图1至图2所示，本发明的高精度的井下车辆定位方法，包括以下步骤：
S1、通过UWB定位标签卡获取车辆与UWB基站之间的相对位置坐标。
S2、通过IMU传感器获取车辆的位姿数据。
S3、通过激光雷达获取井下巷道点云数据。
S4、将车辆与UWB基站之间的相对位置坐标、车辆的位姿数据以及井下巷道点云数据进行融合处理，得到井下车辆的最终定位信息。
换言之，本发明将UWB、IMU和激光雷达三种定位数据进行融合处理，激光雷达可以降低UWB因障碍物导致的定位误差，UWB可以提供粗定位，减小IMU和激光雷达的误差累计，IMU可以减小因环境因素导致的误差，三种定位方式相辅相成，最终实现井下高精度的车辆定位与轨迹跟踪，使井下车辆的自动驾驶成为可能。
具体的，步骤S4中的融合处理，包括以下步骤：根据车辆与UWB基站之间的相对位置坐标构建UWB位置预测值；根据车辆的位姿数据构建IMU位置预测值；根据井下巷道点云数据构建雷达位置预测值；根据UWB位置预测值、IMU位置预测值、雷达位置预测值、相对位置坐标、位姿数据和井下巷道点云数据，构建融合函数F(p)，p表示车辆位置坐标；计算融合函数F(p)的最优解，得到井下车辆的最终定位信息。
也就是说，首先可以通过UWB、IMU和激光雷达获得各自的测量数据(即相对位置坐标、车辆的位姿数据、井下巷道点云数据)，然后构建三种定位方式的预测值，根据三种测量数据和三种预测值，可以构建融合函数F(p)。融合函数的作用是找到误差最小情况下的车辆位置，进而确定井下车辆的定位信息。
构建融合函数F(p)包括以下步骤：计算相对位置坐标与UWB位置预测值之间的残差
Figure BDA0003842630020000081
计算位姿数据与IMU位置预测值之间的残差
Figure BDA0003842630020000082
计算井下巷道点云数据与雷达位置预测值之间的残差
Figure BDA0003842630020000083
分别获取相对位置坐标、位姿数据和井下巷道点云数据的权重
Figure BDA0003842630020000084
和
Figure BDA0003842630020000085
融合函数F(p)的公式为：
Figure BDA0003842630020000086
其中，p表示坐标位置，t表示时间，C表示通过闭环检测的雷达位置预测值的集合，ci和ci分别表示集合C里的两个数据点，
Figure BDA0003842630020000087
分别表示残差的转置。
可以理解的是，残差是指测量值与预测值之间的差，残差越小，表明预测值越准确。测量值与预测值之间的残差计算公式采用现有技术。此处字母p实际表示的是坐标(x，y)，即，融合函数可以有两种表达式F(x)和F(y)，分别可以解出x方向的最优解和y方向的最优解，进而得到车辆的定位信息。其中，权重
Figure BDA0003842630020000088
Figure BDA0003842630020000089
和
Figure BDA00038426300200000810
的比值为1：1：1，表明对这三种方法的信任度相同，这样能够提高最终的定位精度。
例如，构建UWB位置预测值，具体包括：获取车辆与m个UWB基站之间的距离Rm；计算车辆与第A个UWB基站之间的距离RA，计算车辆与第B个UWB基站之间的距离RB，计算距离RA与RB之间的差值RA,B＝RA-RB；根据距离Rm以及差值RA,B能够获得以下关系式：
RA,1 2+2RA,1R1＝KA-2xA,1x-2yA,1-K1
其中，(x,y)表示UWB位置预测值，KA＝(xA 2+yA 2)，K1＝(x1 2+y1 2)，xA,1＝xA-x1，yA,1＝yA-y1，R1表示车辆与第1个UWB基站之间的距离。
例如，可以通过UWB标签卡发送UWB信号，在UWB信号覆盖范围内的所有UWB基站均能够接收到该UWB信号；获取UWB标签卡与m个UWB基站之间的报文飞行时间差；根据报文飞行时间差计算UWB标签卡与UWB基站之间的距离。需要说明的是，UWB基站在井下巷道的位置坐标是已知的，通过获取车辆与各个UWB基站之间的距离推算出车辆当前的位置坐标。其中，Rm的计算公式为：
Figure BDA0003842630020000091
(x,y)表示UWB位置预测值(即井下车辆的位置坐标)，(xm,ym)表示UWB基站的坐标，由此，可以得到车辆与不同UWB基站之间的距离。例如，车辆与第A个UWB基站之间的距离为RA，车辆与第B个UWB基站之间的距离为RB，计算距离RA与RB之间的差值RA,B＝RA-RB＝ctA,B，c为光速，tA,B表示UWB信号飞行到A基站、B基站的飞行时间差(A不等于B)。由此，根据公式
Figure BDA0003842630020000092
和RA,B＝RA-RB＝ctA,B，可以得到关系式：
RA,1 2+2RA,1R1＝KA-2xA,1x-2yA,1-K1，其中，KA、xA,1、yA,1为已知量，x，y，R1为未知量。根据该关系式可以解算出(x,y)。
具体的，构建雷达位置预测值，具体包括：通过激光雷达扫描获取井下巷道的点云数据集；获取井下巷道地图的点云匹配集；计算点云数据集和点云匹配集之间的相对变换函数E(Q,G)，Q表示旋转矩阵，G表示平移矩阵；对所述相对变换函数E(Q,G)进行闭环检测，若E(Q,G)小于设定阈值，则表明点云数据集和点云匹配集之间匹配良好，符合闭环要求；否则，重新计算相对变换函数；利用符合闭环要求的相对变换函数E(Q,G)对所述点云数据集进行空间变换，确定车辆在井下巷道地图中的预测位置，即雷达位置预测值。
需要说明的是，激光雷达的点云数据是指在一个三维坐标系统中的一组向量的集合，点云数据除了具有几何位置以外，有的还包含有颜色信息或反射强度信息，利用点云数据可以构建井下巷道的3D模型。点云匹配集即为井下巷道地图的标准点云数据。点云数据集和点云匹配集进行匹配时，需要先计算两个点云集之间的旋转矩阵Q和平移矩阵G，假设点云数据集为{q1,q2,q3,...,qi}，点云匹配集为{p1,p2,p3,...,pi}，每一个数据点qi均可以在点云匹配集中找到一个最接近的点pi，该数据点qi与pi可以组成最接近点对。计算点云数据集和点云匹配集的均值，根据均值构建两个数据集间的协方差矩阵，由协方差矩阵构建对称矩阵，将对称矩阵做特征值分解，可以获得旋转向量，由旋转向量可以计算出旋转矩阵Q。根据两个点云集的均值和旋转矩阵Q可以平移矩阵G。根据旋转矩阵Q和平移矩阵G可以对点云数据集中的数据点qi做坐标变换，得到一个新点qi new＝Qqi+G。由此，可以得到两个集合之间的相对变换函数：
Figure BDA0003842630020000101
相对变换函数E(Q,G)可以计算出点云数据集的数据点和点云匹配集中的数据点之间的距离，记设定阈值为dmin(例如可以设为5cm)，将E(Q,G)与dmin进行比较，若E(Q,G)>dmin,，则表明点云数据集与点云匹配集之间的匹配效果不好，若E(Q,G)<dmin,,则表明点云数据集和点云匹配集之间匹配良好，符合闭环要求，该相对变换函数E(Q,G)为最优的相对变换函数。利用相对变换函数E(Q,G)可以将点云数据集的数据点空间变换到井下巷道地图中，从而得到车辆的预测位置。
具体的，构建IMU位置预测值，具体包括：获取车辆在ti时刻的位置Pwbi以及在世界坐标系下的速度Vi w；车辆当前在tj时刻的IMU位置预测值为：
Figure BDA0003842630020000111
其中，
Figure BDA0003842630020000112
表示车辆在第tj时刻的预测位置，
Figure BDA0003842630020000113
表示车辆在第tj时刻在世界坐标系下的预测速度，gw表示世界坐标系下的车辆重力加速度，Δt表示tj和ti的时间间隔，
Figure BDA0003842630020000114
分别表示在ti、tj时刻位姿的旋转矩阵，
Figure BDA0003842630020000115
和
Figure BDA0003842630020000116
表示tj时刻相对于ti时刻的IMU预积分量。需要说明的是，IMU定位是从一个已知坐标的位置点出发，根据连续测得的车辆的航向角和加速度推算出车辆当前位置的坐标。例如，ti时刻的车辆位置是已知的，可以推算出下一时刻tj的车辆位置。其中，
Figure BDA0003842630020000117
Figure BDA0003842630020000118
分别表示IMU传感器的加速度计和陀螺仪的整体偏移量，
Figure BDA0003842630020000119
分别表示车辆整体偏移时使用的随机游走模型的参数变量，a和w分别表示滤波计算后的车辆当前加速度和角速度，k表示时刻。
需要说明的是，融合函数中包含了UWB、IMU、激光雷达三种方式的数据，融合处理的目的是为了解出一组最优的(x，y)。计算融合函数F(p)的最优解，具体包括：当F(p)＝0时，解出的p值即为最优解，对融合函数F(p)进行非线性优化，得到优化函数：F(p+Δp)≈F(p)+J(p)Δp，其中，
Figure BDA00038426300200001110
表示F(p)的雅可比矩阵，Δp表示增量；令F(p+Δp)≈F(p)+J(p)Δp＝0，利用最小二乘法求解出最优增量Δp*，则p+Δp*即为最优解。
例如，可以给定一个初始值p，通过添加增量Δp，使得F(p+Δp)达到最小(为0或接近0)，通过多次迭代，可以找到一组最优解p+Δp*，该最优解即表示车辆位置坐标，由此可以实现对车辆的定位。
如图3所示，本发明还提供了一种高精度的井下车辆定位系统，使用高精度的井下车辆定位方法，系统包括：UWB标签卡1，UWB标签卡1安装在车辆上；IMU传感器2，IMU传感器2安装在车辆上；激光雷达3，激光雷达3安装在车辆上；UWB基站4，UWB基站4安装在井下巷道内，UWB基站4与UWB标签卡1通讯连接；控制器5，UWB标签卡1、IMU传感器2、激光雷达3及UWB基站4均与控制器5通讯连接，控制器5用于执行高精度的井下车辆定位方法的步骤。也就是说，本发明在井下车辆上安装了UWB标签卡1、IMU传感器2和激光雷达3，三种方式采集的数据可以发送给控制器5，控制器5将三种定位数据进行融合处理，得到一个最终的车辆位置坐标。
综上所述，本发明将三种定位数据进行融合处理，激光雷达可以降低UWB因障碍物导致的定位误差，UWB可以提供粗定位，减小IMU传感器和激光雷达的误差累计，IMU传感器还可以减小因环境因素导致的误差；本发明利用融合处理的方式，有利于更加提高井下车辆定位的精度，为井下车辆实现自动驾驶提供支持。
以上述依据本发明的理想实施例为启示，通过上述的说明内容，相关工作人员完全可以在不偏离本项发明技术思想的范围内，进行多样的变更以及修改。本项发明的技术性范围并不局限于说明书上的内容，必须要如权利要求范围来确定其技术性范围。

Claims (10)

1.一种高精度的井下车辆定位方法，其特征在于，包括以下步骤：
S1、通过UWB定位标签卡获取车辆与UWB基站之间的相对位置坐标；
S2、通过IMU传感器获取车辆的位姿数据；
S3、通过激光雷达获取井下巷道点云数据；
S4、将所述车辆与UWB基站之间的相对位置坐标、车辆的位姿数据以及井下巷道点云数据进行融合处理，得到井下车辆的最终定位信息。
2.如权利要求1所述的高精度的井下车辆定位方法，其特征在于，步骤S4中的融合处理，包括以下步骤：
根据所述车辆与UWB基站之间的相对位置坐标构建UWB位置预测值；
根据所述车辆的位姿数据构建IMU位置预测值；
根据所述井下巷道点云数据构建雷达位置预测值；
根据所述UWB位置预测值、IMU位置预测值、雷达位置预测值、相对位置坐标、位姿数据和井下巷道点云数据，构建融合函数F(p)，p表示车辆位置坐标；
计算融合函数F(p)的最优解，得到井下车辆的最终定位信息。
3.如权利要求2所述的高精度的井下车辆定位方法，其特征在于，构建融合函数F(p)包括以下步骤：
计算相对位置坐标与UWB位置预测值之间的残差
Figure FDA0003842630010000011
计算位姿数据与IMU位置预测值之间的残差
Figure FDA0003842630010000012
计算井下巷道点云数据与雷达位置预测值之间的残差
Figure FDA0003842630010000013
分别获取相对位置坐标、位姿数据和井下巷道点云数据的权重
Figure FDA0003842630010000014
和
Figure FDA0003842630010000015
融合函数F(p)的公式为：
Figure FDA0003842630010000021
其中，p表示坐标位置，t表示时间，C表示通过闭环检测的雷达位置预测值的集合，ci和ci分别表示集合C里的两个数据点，
Figure FDA0003842630010000022
分别表示残差的转置。
4.如权利要求3所述的高精度的井下车辆定位方法，其特征在于，所述权重
Figure FDA0003842630010000023
和
Figure FDA0003842630010000024
的比值为1：1：1。
5.如权利要求3所述的高精度的井下车辆定位方法，其特征在于，构建UWB位置预测值，具体包括：
获取车辆与m个UWB基站之间的距离Rm；
计算车辆与第A个UWB基站之间的距离RA，计算车辆与第B个UWB基站之间的距离RB，计算距离RA与RB之间的差值RA,B＝RA-RB；
根据距离Rm以及差值RA,B能够获得以下关系式：
RA,1 2+2RA,1R1＝KA-2xA,1x-2yA,1-K1
其中，(x,y)表示UWB位置预测值，KA＝(xA 2+yA 2)，K1＝(x1 2+y1 2)，xA,1＝xA-x1，yA,1＝yA-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
其中，
Figure FDA0003842630010000031
表示F(p)的雅可比矩阵，Δp表示增量；
令F(p+Δp)≈F(p)+J(p)Δp＝0，利用最小二乘法求解出最优增量Δp*，则p+Δp*即为最优解。
8.如权利要求5所述的高精度的井下车辆定位方法，其特征在于，获取车辆与m个UWB基站之间的距离Rm，包括：
通过UWB标签卡发送UWB信号，在UWB信号覆盖范围内的所有UWB基站均能够接收到该UWB信号；
获取UWB标签卡与m个UWB基站之间的报文飞行时间差；
根据报文飞行时间差计算UWB标签卡与UWB基站之间的距离。
9.如权利要求3所述的高精度的井下车辆定位方法，其特征在于，构建IMU位置预测值，具体包括：
获取车辆在ti时刻的位置
Figure FDA0003842630010000041
以及在世界坐标系下的速度Vi w；
车辆当前在tj时刻的IMU位置预测值为：
Figure FDA0003842630010000042
其中，
Figure FDA0003842630010000043
表示车辆在第tj时刻的预测位置，
Figure FDA0003842630010000044
表示车辆在第tj时刻在世界坐标系下的预测速度，gw表示世界坐标系下的车辆重力加速度，Δt表示tj和ti的时间间隔，
Figure FDA0003842630010000045
分别表示在ti、tj时刻位姿的旋转矩阵，
Figure FDA0003842630010000046
和
Figure FDA0003842630010000047
表示tj时刻相对于ti时刻的IMU预积分量。
10.一种高精度的井下车辆定位系统，其特征在于，使用如权利要求1-9任一项所述的高精度的井下车辆定位方法，所述系统包括：
UWB标签卡，所述UWB标签卡安装在车辆上；
IMU传感器，所述IMU传感器安装在车辆上；
激光雷达，所述激光雷达安装在车辆上；
UWB基站，所述UWB基站安装在井下巷道内，所述UWB基站与所述UWB标签卡通讯连接；
控制器，所述UWB标签卡、IMU传感器、激光雷达及UWB基站均与所述控制器通讯连接，所述控制器用于执行如权利要求1-9任一项所述的高精度的井下车辆定位方法的步骤。
CN202211108291.7A 2022-09-13 2022-09-13 高精度的井下车辆定位方法及其系统 Pending CN115540858A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211108291.7A CN115540858A (zh) 2022-09-13 2022-09-13 高精度的井下车辆定位方法及其系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211108291.7A CN115540858A (zh) 2022-09-13 2022-09-13 高精度的井下车辆定位方法及其系统

Publications (1)

Publication Number Publication Date
CN115540858A true CN115540858A (zh) 2022-12-30

Family

ID=84726784

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211108291.7A Pending CN115540858A (zh) 2022-09-13 2022-09-13 高精度的井下车辆定位方法及其系统

Country Status (1)

Country Link
CN (1) CN115540858A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116067379A (zh) * 2023-03-07 2023-05-05 青岛慧拓智能机器有限公司 一种基于动态点云的长隧道场景下多传感器融合定位方法
CN116518984A (zh) * 2023-07-05 2023-08-01 中国矿业大学 一种煤矿井下辅助运输机器人车路协同定位系统及方法

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116067379A (zh) * 2023-03-07 2023-05-05 青岛慧拓智能机器有限公司 一种基于动态点云的长隧道场景下多传感器融合定位方法
CN116518984A (zh) * 2023-07-05 2023-08-01 中国矿业大学 一种煤矿井下辅助运输机器人车路协同定位系统及方法
CN116518984B (zh) * 2023-07-05 2023-09-08 中国矿业大学 一种煤矿井下辅助运输机器人车路协同定位系统及方法

Similar Documents

Publication Publication Date Title
Wan et al. Robust and precise vehicle localization based on multi-sensor fusion in diverse city scenes
Chiang et al. Seamless navigation and mapping using an INS/GNSS/grid-based SLAM semi-tightly coupled integration scheme
EP3497405B1 (en) System and method for precision localization and mapping
Chiang et al. Performance enhancement of INS/GNSS/refreshed-SLAM integration for acceptable lane-level navigation accuracy
CN106969768B (zh) 一种无轨导航agv的精确定位及停车方法
US10365363B2 (en) Mobile localization using sparse time-of-flight ranges and dead reckoning
CN103926925B (zh) 一种基于改进的vfh算法的定位与避障方法及机器人
US20170023659A1 (en) Adaptive positioning system
CN109416256B (zh) 行驶车道推定系统
CN115540858A (zh) 高精度的井下车辆定位方法及其系统
EP3617749B1 (en) Method and arrangement for sourcing of location information, generating and updating maps representing the location
CN104848867A (zh) 基于视觉筛选的无人驾驶汽车组合导航方法
CN110208842A (zh) 一种车联网环境下车辆高精度定位方法
Sharath et al. A dynamic two-dimensional (D2D) weight-based map-matching algorithm
CN111426320B (zh) 一种基于图像匹配/惯导/里程计的车辆自主导航方法
Pfaff et al. Towards mapping of cities
Kealy et al. Collaborative navigation as a solution for PNT applications in GNSS challenged environments–report on field trials of a joint FIG/IAG working group
EP4102327A1 (en) Position recognition method and position recognition system for vehicle
US11579628B2 (en) Method for localizing a vehicle
Gustafsson et al. Navigation and tracking of road-bound vehicles
Wang et al. UGV‐UAV robust cooperative positioning algorithm with object detection
WO2016196717A2 (en) Mobile localization using sparse time-of-flight ranges and dead reckoning
Chiang et al. Semantic proximity update of GNSS/INS/VINS for Seamless Vehicular Navigation using Smartphone sensors
Deusch Random finite set-based localization and SLAM for highly automated vehicles
Noureldin et al. a Framework for Multi-Sensor Positioning and Mapping for Autonomous Vehicles

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination