CN114323003A - 一种基于umb、imu及激光雷达的井工矿融合定位方法 - Google Patents

Abstract

本发明提供一种基于UMB、IMU及激光雷达的井工矿融合定位方法，包括如下步骤：构建基于IMU的车辆位置和姿态预估模型；构建基于UWB测距的EKF测量融合模型；构建激光雷达点云的匹配定位模型；构建融合定位模型。本发明所述的基于UMB、IMU及激光雷达的井工矿融合定位方法，实现了井下无人驾驶车辆的精确和稳定定位。

Description

一种基于UMB、IMU及激光雷达的井工矿融合定位方法

技术领域

本发明属于融合定位领域，具体涉及基于UMB、IMU及激光雷达的井工矿融合定位方法。

背景技术

井工矿无轨胶轮车包括物料车、装载车、皮卡车、巡检车等类型，这些车辆目前都大多采用人工驾驶的方式。将无人驾驶技术应用于井工矿，将提高矿井的装备智能化水平和集中管理能力，提升生产监控和风险识别手段，有效减少井下作业人员，实现井下少人则安、无人则安的目标，提升井工矿管理的经济效益和社会效益。

井工矿无人驾驶的难点在于井下精确定位，由于井下没有GNSS信号，而且巷道狭长、灯光昏暗，给车辆精确定位造成很大困扰，这也成为制约实施井工矿无人驾驶的技术瓶颈。超宽带(UWB)定位具有定位精度高、低功耗、安全性高、信号穿透性强等特点，利用井下安装的UWB定位基站，通过车载UWB设备与UWB基站进行通信，可以实现车辆与UWB基站之间测距，为井下精确定位提供有效信息。但由于井下UWB基站数量有限，通常沿途间隔200m-400m才配置一个UWB基站，***可观测性较弱。本发明通过构建车载UWB、激光雷达、惯性测量单元(IMU)的UMB、IMU及激光雷达融合定位方法，实现了井下无人驾驶车辆的精确和稳定定位。

鉴于此，目前亟待提出一种基于UMB、IMU及激光雷达的井工矿融合定位方法。

发明内容

为此，本发明提供一种UMB、IMU及激光雷达融合定位方法，通过构建车载UWB、激光雷达、惯性测量单元(IMU)的UMB、IMU及激光雷达融合定位方法，实现了井下无人驾驶车辆的精确和稳定定位。

本发明的UMB、IMU及激光雷达融合定位方法，包括如下步骤：

S1、构建基于IMU的车辆位置和姿态预估模型；

S2、构建基于UWB测距的EKF测量融合模型；

S3、构建激光雷达点云的匹配定位模型；

S4、构建基于S1、S2和S3的融合定位模型。

进一步的，步骤S1中的车辆位置和姿态预估模型的构建方法具体包括如下步骤：

S101、计算IMU惯性导航***的确定性误差，包括：

平台角误差：

其中，

为导航坐标系下惯性导航***失准角；

为地固系相对于惯性系旋转角速度矢量；

为导航系相对于地固系旋转角速度矢量；εⁿ为陀螺仪随机误差。

速度误差：

其中，fⁿ为导航坐标系下的加速度矢量；vⁿ为载体在导航坐标系下的速度矢量；

为加速度计随机误差。

位置误差：

其中，

分别为载体纬度、经度和高度误差变化率；R_N为子午圈曲率半径；R_E为子午圈曲率半径；

分别为北、西、天方向的速度误差。

S102、建立车辆位置和姿态预估模型：

首先，建立IMU惯性导航***的状态方程：

离散化后得到：

X_k+1＝ΦX_k+Γw_k；

其中，

X为惯导状态变量；

F_INS为惯导状态矩阵；

W为惯导误差变量；

G为惯导误差敏感矩阵；

Φ为状态转移矩阵；

Γ为离散化的输入矩阵；

设EKF滤波更新周期为T，计算得：

Φ＝I+FT；

Γ＝GT；

然后，计算状态变量的积分更新表达式：

对四元数标准化后，重新计算F和G矩阵，得到：

P_k+1＝(I+FT)P_k(I+FT)^T+T²GQG^T；

以对协方差进行预测。

进一步的，步骤S3中构建基于UWB测距的EKF测量融合模型，具体包括如下步骤：

S201、计算UWB标签在以UWB基站为原点的北西天坐标系中的位置矢量r:

其中，L_U、λ_U、h_U分别为标签的纬度、经度和高度；L_B、λ_B、h_B分别为基站的纬度、经度和高度。

S202、计算UWB标签与基站之间的距离观测值:

ρ_u＝ρ_true+ε_u；

其中，ρ_true、ρ分别为标签到基站的实际距离和观测距离；δr为标签到基站的距离误差矢量；

为体坐标系到导航坐标系的旋转矩阵；l_u为标签到惯导中心的杆臂矢量；φ为惯导安装角度误差矢量。

S203、得到观测方程：

Z_u＝H_uX+V_u。

其中，Z_u为UWB观测量；H_u为观测敏感性矩阵；V_u为观测噪声。

进一步的，基于激光雷达位置、航向的组合***的观测方程为

Z_lidar＝H_lidarX+V_lidar；

其中：

计算滤波更新增益矩阵：

K＝P_k+1H^T(HP_k+1H^T+R)^-1；

对滤波器状态变量进行更新：

x_k+1＝x_k+1|k+K(Z_k+1-y_k+1)；

对滤波器协方差进行更新：

P_k+1＝P_k+1|k-KHP_k+1|k。

本发明的上述技术方案，相比现有技术具有以下优点：

本发明通过通过构建基于扩展卡尔曼滤波(EKF)状态估计模型，利用IMU对车辆位置和姿态进行预测，利用UWB测距信息和激光雷达点云匹配定位数据进行更新，实时估计车辆在井下的精确位置和姿态，解决井下精确定位难题。

附图说明

图1是本发明实施例提供的方法的原理示意图。

具体实施方式

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

惯性导航***的误差源有很多，主要有惯性仪表本身的误差，惯性仪表的安装误差和标度误差，***的初始条件误差，***的计算误差以及各种干扰引起的误差等。惯导误差可分为两类：确定性误差和随机误差。确定性误差包括平台角误差、速度误差以及位置误差，随机误差主要是陀螺漂移和加速度计的零位偏置等。

本实施例所述的方法，如图1所示，包括如下步骤：

惯性导航***的平台误差角方程为：

其中，

为导航坐标系下惯性导航***失准角；

为地固系相对于惯性系旋转角速度矢量；

为导航系相对于地固系旋转角速度矢量；εⁿ为陀螺仪随机误差。

其中：

下面说明惯性导航***的速度误差方程

由

可得

考虑δgⁿ＝0，δfⁿ＝f^p-fⁿ，其中f^p为加速度计的实际输出。设加速度计的测量误差为

则

故

于是得到速度误差方程为：

其中，fⁿ为导航坐标系下的加速度矢量；vⁿ为载体在导航坐标系下的速度矢量；

为加速度计随机误差。

由

可得位置误差方程为：

其中，

分别为载体纬度、经度和高度误差变化率；R_N为子午圈曲率半径；R_E为子午圈曲率半径；

分别为北、西、天方向的速度误差。

综合以上各式，可得组合导航***的状态方程：

基于上述雅可比矩阵，可建立如下离散化的状态方程：

X_k+1＝ΦX_k+Γw_k

其中，Φ为可获得状态转移矩阵，Γ为离散化的输入矩阵。设EKF滤波更新周期为T，则它们的表达式分别为：

Φ＝I+FT

Γ＝GT

为提高状态变量预测精度，采用四阶龙格库塔法对微分方程进行求解。

状态变量的积分更新表达式为：

在完成状态预测后，需要对四元数进行标准化，并重新计算F和G矩阵，而后采用下式对协方差进行预测

P_k+1＝(I+FT)P_k(I+FT)^T+T²GQG^T

下面构建UWB测距的EKF测量融合模型；

UWB标签在以UWB基站为原点的北西天坐标系中的位置矢量r可以表示为

其中，L_U、λ_U、h_U分别为标签的纬度、经度和高度；L_B、λ_B、h_B分别为基站的纬度、经度和高度。

UWB标签在以UWB基站为原点的北西天坐标系中的位置矢量r可以表示为

车载UWB标签与基站之间的距离观测值可以表示为

车载UWB标签与基站之间的距离观测值可以表示为

其中，ρ_true、ρ分别为标签到基站的实际距离和观测距离；δr为标签到基站的距离误差矢量；

为体坐标系到导航坐标系的旋转矩阵；l_u为标签到惯导中心的杆臂矢量；φ为惯导安装角度误差矢量。

UWB距离测量值可以表示为

ρ_u＝ρ_true+ε_u

所以，观测方程可以表示为

Z_u＝H_uX+V_u

其中，Z_u为UWB观测量；H_u为观测敏感性矩阵；V_u为观测噪声。

下面构建激光雷达点云匹配定位模型

在获取激光雷达传感器数据后，首先需要进行无效点剔除和点云滤波，得到有效的点云信息，并传输到激光点云定位模块，作为实时扫描定位的输入项，IMU数据处理模块进行数据同步和预积分处理，得到相邻时刻内IMU传感器的变换信息，作为激光点云定位模块的扫描匹配初始值输入，地图加载模块加载甲方提供的地图，并输入到激光点云定位模[t，t+Δt]时间序列内的IMU观测量。IMU的观测模型为：

在前面获取到了当前扫描的点云信息和IMU预积分的信息之后，就可以实现基于当前扫描帧和已有地图之间的扫描匹配，得到激光点云的数据约束，然后和IMU预积分的结果融合在一起，使用基于无迹卡尔曼滤波(UKF)的优化方法来进行位姿优化，最终得到激光点云定位结果。

下面构建激光雷达/IMU融合定位模型。

采用激光雷达点云与三维点云地图进行匹配，可以获得车辆在大地坐标系下的位置和姿态数据，位置包括经度、纬度和高度，根据姿态数据可以提取出车辆的航向。

基于激光雷达位置、航向的组合***的观测方程为

Z_lidar＝H_lidarX+V_lidar

其中

位置观测方程

在激光雷达/IMU融合定位***中，惯性导航***给出的实时位置是地理纬经度和高度，但含有误差。设L_I、λ_I和h_I分别是惯导输出的纬度、经度和高度，L、λ和h分别是真实纬度、经度和高度，则有

激光雷达点云匹配也给出车辆的实时位置，其主要误差源是点云匹配过程中的误差。设L_lidar、λ_lidar、h_lidar分别是激光雷达匹配输出的纬度、经度和高度，则有

式中，N_N、N_W、N_U为激光雷达北、西、天方向的位置匹配误差。

取INS和GPS的位置差作为观测量时，观测方程可由下式表示

把上式表示为

Z_P＝H_PX+V_P

式中

V_P＝[N_N N_W N_U]^T

距离测量噪声V_P作为白噪声处理。

计算航向观测方程：

设车辆真实方位角为ψ，惯导解算误差为δψ_I，激光雷达点云匹配得到的误差角为δψ_G，则惯导和激光雷达的方位角分别为：

ψ_I＝ψ+δψ_I

ψ_G＝ψ+δψ_G

***的方位角观测量为：

Z_ψ＝ψ_I-ψ_M＝δψ_I-δψ_M＝H_ψX+V_ψ

其中：

V_ψ为激光雷达点云匹配方位角观测噪声。

根据前面构建的UWB、激光雷达观测方程，可以计算滤波更新增益矩阵。

K＝P_k+1H^T(HP_k+1H^T+R)^-1

对滤波器状态变量进行更新。

X_k+1＝X_k+1|k+K(Z_k+1-y_k+1)

对滤波器协方差进行更新。

P_k+1＝P_k+1|k-KHP_k+1|k

根据上述状态方程、观测方程和融合处理模型，可实现UWB、激光雷达、IMU的融合定位。

本发明采用1个车载UWB设备、若干个UWB基站、1个IMU、1个多线激光雷达，在构建井下高精度地图的基础上，采用激光雷达进行点云匹配获得位置和姿态数据。通过构建以IMU为核心的状态方程，并建立UWB和激光雷达观测方程，实现了在无GNSS信号的井工矿环境下的无人车精确位置、速度和姿态融合解算，可为井工矿无人驾驶***提供精确、稳定的导航定位信息。

显然，上述实施例仅仅是为清楚地说明所作的举例，而并非对实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。而由此所引伸出的显而易见的变化或变动仍处于本发明创造的保护范围之中。

Claims (4)

1.一种基于UMB、IMU及激光雷达的井工矿融合定位方法，其特征在于，包括如下步骤：

S1、构建基于IMU的车辆位置和姿态预估模型；

S2、构建基于UWB测距的EKF测量融合模型；

S3、构建激光雷达点云的匹配定位模型；

S4、构建基于S1、S2和S3的融合定位模型。

2.根据权利要求1所述的基于UMB、IMU及激光雷达的井工矿融合定位方法，其特征在于，步骤S1中的车辆位置和姿态预估模型的构建方法具体包括如下步骤：

S101、计算IMU惯性导航***的确定性误差，包括：

平台角误差：

其中，

为导航坐标系下惯性导航***失准角；

为地固系相对于惯性系旋转角速度矢量；

为导航系相对于地固系旋转角速度矢量；εⁿ为陀螺仪随机误差。

速度误差：

其中，fⁿ为导航坐标系下的加速度矢量；vⁿ为载体在导航坐标系下的速度矢量；

为加速度计随机误差。

位置误差：

其中，

分别为载体纬度、经度和高度误差变化率；R_N为子午圈曲率半径；R_E为子午圈曲率半径；δv_N、δv_W、δv_U分别为北、西、天方向的速度误差。

S102、建立车辆位置和姿态预估模型：

首先，建立IMU惯性导航***的状态方程：

离散化后得到：

X_k+1＝ΦX_k+Γw_k；

其中，

X为惯导状态变量；

F_INS为惯导状态矩阵；

W为惯导误差变量；

G为惯导误差敏感矩阵；

Φ为状态转移矩阵；

Γ为离散化的输入矩阵；

设EKF滤波更新周期为T，计算得：

Φ＝I+FT；

Γ＝GT；

然后，计算状态变量的积分更新表达式：

对四元数标准化后，重新计算F和G矩阵，得到：

P_k+1＝(I+FT)P_k(I+FT)^T+T²GQG^T；

其中，Q为过程噪声矩阵。根据上式可对协方差进行预测。

3.根据权利要求2所述的基于UMB、IMU及激光雷达的井工矿融合定位方法，其特征在于，步骤S3中构建基于UWB测距的EKF测量融合模型，具体包括如下步骤：

S201、计算UWB标签在以UWB基站为原点的北西天坐标系中的位置矢量r:

其中，L_U、λ_U、h_U分别为标签的纬度、经度和高度；L_B、λ_B、h_B分别为基站的纬度、经度和高度。

S202、计算UWB标签与基站之间的距离观测值:

ρ_u＝ρ_true+ε_u；

其中，ρ_true、ρ分别为标签到基站的实际距离和观测距离；δr为标签到基站的距离误差矢量；

为体坐标系到导航坐标系的旋转矩阵；l_u为标签到惯导中心的杆臂矢量；φ为惯导安装角度误差矢量。

S203、得到观测方程：

Z_u＝H_uX+V_u。

其中，Z_u为UWB观测量；H_u为观测敏感性矩阵；V_u为观测噪声。

4.根据权利要求3所述的基于UMB、IMU及激光雷达的井工矿融合定位方法，其特征在于，所述步骤S4的具体过程如下：

基于激光雷达位置、航向的组合***的观测方程为

Z_lidar＝H_lidarX+V_lidar；

其中：

计算滤波更新增益矩阵：

K＝P_k+1H^T(HP_k+1H^T+R)^-1；

对滤波器状态变量进行更新：

x_k+1＝x_k+1|k+K(Z_k+1-y_k+1)；

对滤波器协方差进行更新：

P_k+1＝P_k+1|k-KHP_k+1|k；

然后，根据步骤S1中的状态方程，步骤S2中的观测方程，得到融合模型。

Images