CN114166221B - 动态复杂矿井环境中辅助运输机器人定位方法及系统 - Google Patents

Abstract

本发明公开一种动态复杂矿井环境中辅助运输机器人定位方法及系统，通过获得辅助运输机器人的加速度信息和UWB距离量测信息，利用扩展卡尔曼滤波得到机器人位置估计，当UWB量测没有丢失时，利用扩展卡尔曼滤波融合UWB与IMU信息，并对UWB量测进行异常值检测，若UWB量测异常，通过修正新息协方差矩阵改进扩展卡尔曼滤波从而提高定位精度。在UWB量测丢失时，对系统状态向量设定约束条件，利用拉格朗日乘数法对状态估计进行约束更新得到位置估计，进而得到定位信息；若辅助运输机器人处于UWB测距盲区，重构UWB量测，并进行异常值的检测与修正，得到辅助运输机器人的定位信息。本发明鲁棒性高、定位准确。

Description

动态复杂矿井环境中辅助运输机器人定位方法及系统

技术领域

本发明涉及一种定位方法及系统，具体是一种动态复杂矿井环境中辅助运输机器人定位方法及系统，属于井下定位技术领域。

背景技术

随着煤矿设备自动化的发展，辅助运输机器人的精确定位将在未来少人或无人采矿中发挥重要的作用,既可以降低煤矿工人的劳动强度和危险性，同时又提高了生产效率。现有的基于距离的定位方法基本上依赖于环境相对静态的假设，这使得定位可以精确地进行范围定位。然而，金属设备、主运输车辆、障碍物遮挡、环境噪声等因素会对射频在煤矿井下中的传播造成显著影响。在这种情况下，往往会出现测距信息不准确或间歇性测距故障，导致定位性能严重下降。

现有的相关定位技术主要有基于惯性导航系统的辅助运输机器人自主定位技术，基于机器视觉的辅助运输机器人自主定位技术，基于激光雷达的辅助运输机器人定位技术以及基于超宽带的辅助运输机器人自主定位技术，但上述方法存在以下不足：

(1)基于惯性导航系统的辅助运输机器人自主定位技术，该方法利用惯性测量单元测量辅助运输机器人的加速度，然后经过积分得到位置信息。然而惯性测量单元中的陀螺仪偏差等误差会随着时间累积，定位精度随时间降低，无法对辅助运输机器人进行长期的精确定位。

(2)基于机器视觉的辅助运输机器人自主定位技术，该方法利用摄像头来获得辅助运输机器人的图像信息，从而得到位置信息。但是，在煤矿井下中，高温，高湿，高粉尘，且环境照明度低等因素，严重影响摄像头的工作，得到错误的定位信息，实用性不高。

(3)基于激光雷达的辅助运输机器人定位技术，该方法采用三边测量原理来确定辅助运输机器人的位置，但是受环境影响大，影响定位精度，且系统复杂度较高，造价昂贵，不适用于复杂矿井环境中的辅助运输机器人定位。

(4)基于超宽带的辅助运输机器人自主定位，该方法利用超宽带移动节点与锚点的距离信息，采用卡尔曼滤波得到辅助运输机器人的位置估计。然而该方法假设辅助运输机器人处于恒速运动状态，导致定位出现延迟，精度不高。

上述定位技术常用卡尔曼滤波包括经典卡尔曼滤波、扩展卡尔曼滤波以及无迹卡尔曼滤波进行数据处理及融合得到定位信息，即对目标建立运动方程，通过得到的量测信息进行目标定位。然而，对于复杂矿井条件下，得到的量测信息可能是不正确的，且会出现量测丢失的情况，严重降低了定位系统的鲁棒性以及准确性。

发明内容

针对上述现有技术存在的问题，本发明目的之一是提供一种动态复杂矿井环境中辅助运输机器人定位方法，能够对IMU的累计误差进行修正，能够对不准确或丢失的量测信息进行修正、约束和重构，从而得到准确的量测数据，进而实现对井下辅助运输机器人进行准确定位，定位成本低、不易受干扰、精度高、鲁棒性高。

本发明目的之二是提供一种动态复杂矿井环境中辅助运输机器人定位系统，能够对井下辅助运输机器人进行准确定位，定位成本低、不易受干扰、精度高、鲁棒性高，且结构简单，易于制造。

为达到以上目的，本发明提供一种动态复杂矿井环境中辅助运输机器人定位方法，包括以下步骤：

1)获取辅助运输机器人与坐标已知的UWB锚点之间的UWB距离信息，以及辅助运输机器人的IMU加速度信息；将UWB距离信息作为UWB量测输入，将IMU加速度信息作为控制输入，执行步骤2)；

2)利用扩展卡尔曼滤波融合IMU加速度信息与UWB距离信息，并对UWB量测进行异常值检测，若UWB量测为异常值，则计算自适应因子λ_k修正新息协方差矩阵，从而通过改进扩展卡尔曼滤波，得到辅助运输机器人的状态估计，进而辅助运输机器人得到位置估计；

3)若在采样周期内，辅助运输机器人获取到UWB距离信息，则执行步骤2)；

若在当前采样周期内，辅助运输机器人没有获取到UWB距离信息，则认为UWB距离量测丢失，将距离当前时刻最近的新息作为状态约束条件，利用拉格朗日乘数法计算约束后的状态估计，进而获得辅助运输机器人的位置估计；

若在多个采样周期内，没有获取到UWB距离信息，则认为辅助运输机器人处于UWB测距盲区，利用自回归模型重构UWB量测，作为新的UWB量测，对缺失的UWB量测数据进行补偿后，执行步骤2)。

一种动态复杂矿井环境中辅助运输机器人定位系统，包括

辅助运输机器人；

UWB模块，安装在辅助运输机器人上，用于测量辅助运输机器人到UWB锚点之间的UWB距离信息；

UWB锚点，坐标已知，至少设有4个，分别安装在辅助运输机器人周围，且至少有一个UWB锚点与其他UWB锚点不在同一水平面上；

IMU模块，安装在辅助运输机器人上，用于提供辅助运输机器人在导航坐标系中的IMU加速度信息；

定位导航计算机，设有UWB接口和IMU接口，通过UWB接口与UWB模块相连，用于接收UWB距离信息；通过IMU接口与IMU模块相连，用于接收IMU加速度信息；所述定位导航计算机利用上述所述定位方法对辅助运输机器人进行位置估计，从而得到辅助运输机器人的位置信息，对辅助运输机器人进行定位

电源设备，用于给辅助运输机器人、UWB测距模块、IMU模块和定位导航计算机提供电源。

与现有技术相比，本发明具有以下优点：

1)本发明根据扩展卡尔曼滤波融合UWB和IMU信息获得辅助运输机器人的位置估计，不依赖GPS卫星定位，在动态复杂矿井环境也能完全依靠系统自身实现定位，具有不易受干扰、定位成本低的优点；

2)辅助运输机器人通常需要在地底长时间工作，本发明方法满足了辅助运输机器人长时间的高精度鲁棒定位需求，本发明通过对UWB量测的检测、修正、补偿及重构，减小了辅助运输机器人的定位误差，保证了辅助运输机器人长时间高精度定位要求，保证了辅助运输机器人动态复杂矿井环境中三个方向的最大误差在40cm以内，且具有不易受干扰、成本低、定位精度高的优点。

附图说明

图1为本发明一实施例辅助运输机器人定位方法的流程图；

图2为本发明UWB距离信息(超宽带测距信息)和IMU信息(惯性测量单元信息)融合流程图；

图3为本发明一实施例辅助运输机器人结构示意图。

图4为本发明一实施例辅助运输机器人与UWB锚点的位置示意图。

附图标记：1、电源设备；2、UWB模块；3、IMU测距模块；4、定位导航计算机；5、UWB接口；6、IMU接口；7、辅助运输机器人。

具体实施方式

下面结合附图对本发明作进一步说明。

如图1和图2所示，本发明一种强干扰矿井环境中辅助运输机器人的高精度鲁棒定位方法，首先测量辅助运输机器人7到UWB锚点(UWB锚点坐标已知)的距离，再将该距离值与IMU提供的IMU加速度信息利用扩展卡尔曼滤波原理进行融合，得到高精度鲁棒的辅助运输机器人7位置信息，包括以下步骤：

1)获取辅助运输机器人7与坐标已知的UWB锚点之间的UWB距离信息，以及辅助运输机器人7的IMU加速度信息；将UWB距离信息作为UWB量测输入，将IMU加速度信息作为控制输入；

所述UWB距离信息通过以下方法获得：在辅助运输机器人7上安装的UWB模块2，在辅助运输机器人7周围布置UWB锚点，UWB锚块与UWB锚点进行通信，一个采样周期得到一个辅助运输机器人7到UWB锚点的距离，从而获取辅助运输机器人7与UWB锚点之间的UWB距离信息；

所述IMU加速度信息通过在辅助机运输机器人安装IMU模块3，由IMU模块3获取辅助机运输机器人7的IMU加速度信息。

2)利用扩展卡尔曼滤波融合IMU加速度信息与UWB距离信息，并对UWB量测进行异常值检测，若UWB量测为异常值，则计算自适应因子λ_k修正新息协方差矩阵，从而通过改进扩展卡尔曼滤波，得到辅助运输机器人7的状态估计，进而辅助运输机器人7得到位置估计。具体步骤如下：

2.1)建立基于扩展卡尔曼滤波的UWB与IMU信息融合的系统运动模型

x_k＝A_kx_k-1+B_k-1u_k-1+w_k-1 (1)

其中，

u_k-1＝[a_x,0,0,a_y,0,0,a_z,0,0]^T； (5)

式中，w_k-1为系统噪声，服从均值为0,方差为Q_k-1的高斯分布，x_k为系统状态向量，A_k为系统状态矩阵，B_k-1为输入增益矩阵，u_k-1为控制输入；位置p＝[p_x,p_y,p_z]^T，速度v＝[v_x,v_y,v_z]^T以及加速度偏差a_b＝[a_bx,a_by,a_bz]^T，加速度信息a＝[a_x,a_y,a_z]^T,k为时间序列，T为采样间隔；

因此，得到扩展卡尔曼滤波的一步状态预测估计

以及一步预测误差协方差矩阵P_k|k-1，通过一步状态预测估计

获得辅助运输机器人7位置、速度、加速度偏差的预测值：

其中，

式中，τ_a为加速度量测噪声，τ_b为加速度测量偏差；

2.2)建立扩展卡尔曼滤波的量测方程：

r_k＝h(x_k)+v_k (8)

式中，r_k表示UWB量测向量，x_k表示系统状态向量，v_k为系统量测噪声，服从均值为0，方差为R_k的高斯分布，h(·)为量测函数；

因此，量测矩阵H_k由公式(9)的一阶泰勒展开定义为：

式中，UWB锚点的位置记作p_A＝[p_Ax,p_Ay,p_Az]^T，

为一步预测位置；通过公式(9)计算得到UWB锚点和辅助运输机器人7之间的预测距离为

所述公式(9)为一阶泰勒公式。

2.3)由量测方程得到扩展卡尔曼滤波的新息e_k|k-1及其协方差矩阵：

2.4)检测异常值：

当新息协方差矩阵M_k|k-1小于或等于设定阈值ψ，判断UWB量测为正常值，执行步骤2.5)；

当新息协方差矩阵M_k|k-1大于设定阈值ψ，判断UWB量测为异常值，利用扩展卡尔曼滤波融合UWB与IMU信息得到状态估计的过程中，通过计算自适应因子λ_k得对新息协方差矩阵M_k|k-1进行修正，得到新的新息协方差矩阵

令

执行步骤2.5)；

其中，λ_k为自适应因子，通过如下公式获得：

2.5)由步骤2.2)和步骤2.3)得到辅助运输机器人7的状态估计

及其误差协方差矩阵P_k：

P_k＝(I-K_kH_k)P_k|k-1 (16)

其中，

式中，K_k滤波增益矩阵，P_k|k-1为k时刻一步预测误差协方差矩阵，H_k为量测矩阵，M_k|k-1为新息协方差矩阵；

为一步状态预测估计，r_k为UWB量测，

为UWB锚点和辅助运输机器人之间的预测距离，I为n维单位矩阵。

3)若在采样周期内，辅助运输机器人7获取到UWB距离信息(UWB量测数据)，则判断UWB量测数据未丢失，执行步骤2)；

若在当前采样周期内，辅助运输机器人7没有获取到UWB距离信息，则认为UWB量测数据丢失，将距离当前时刻最近的新息作为状态约束条件，利用拉格朗日乘数法计算约束后的状态估计，从而获得辅助运输机器人7的位置估计，进而实现对辅助运输机器人7的定位，具体步骤如下：

设置状态约束条件：

式中，

为距离当前时刻最近的新息；将无约束的状态估计

投影到约束平面即公式(18)上，得到约束状态估计

满足如下公式：

其中，

W是一个正定矩阵；

利用拉格朗日乘数法最小化如下公式：

式中，S_k表示拉格朗日乘数因子，因此，得到基于约束条件的扩展卡尔曼滤波状态估计：

式中，W表示一个正定矩阵，S_k为拉格朗日乘数因子，H_k为量测矩阵，

为UWB锚点和辅助运输机器人之间的预测距离，

和

分别为更新后的扩展卡尔曼滤波的状态估计

及其误差协方差矩阵P_k，由更新后的

得到最终的辅助运输机器人的位置估计，进一步提高了定位的准确性。

若在多个采样周期内，没有获取到UWB距离信息，则判断辅助运输机器人7处于UWB测距盲区，利用自回归模型重构UWB量测

作为新的UWB量测，对缺失的UWB量测数据进行补偿后，执行步骤2)；

其中，

为重构后的UWB量测，r_k,i为存储在内存中的UWB量测数据，n为存储UWB量测数据的数量，α_i为线性权重系数；所述权重系数α_i通过最小化如下公式获得：

所述辅助运输机器人7的状态估计包括位置、速度、加速度偏差估计，因此，通过辅助运输机器人7的状态估计即可得到辅助运输机器人7的位置估计，即位置信息，从而实现了对辅助运输机器人7进行定位。

上述定位方法通过对UWB量测的检测、修正、补偿及重构，减小了辅助运输机器人7的定位误差，保证了辅助运输机器人7长时间高精度定位要求，保证了辅助运输机器人7动态复杂矿井环境中三个方向的最大误差在40cm以内，且具有不易受干扰、成本低、定位精度高的优点。

如图3所示，本发明还提供了一种基于上述定位方法的动态复杂矿井环境中辅助运输机器人7高精度鲁棒定位系统，包括

辅助运输机器人7；

UWB锚点，坐标已知，共设有4个，分别安装在辅助运输机器人7周围，且4个UWB锚点不在同一水平面上，如图4所示，UWB锚点A0、A1、A2、A3连在一起组成一个四边形方形，辅助运输机器人7位于四边形内，即活动区域在四边形内，其中UWB锚点A1、A2、A3位于同一水平面，UWB锚点A0与UWB锚点A1、A2、A3不在同一水平面；

UWB模块2，安装在辅助运输机器人7上，用于测量辅助运输机器人7到UWB锚点之间的UWB距离信息；所述UWB模块2分别与布置在辅助运输机器人7周围的UWB锚点进行通信，一个采样周期得到一个辅助运输机器人7到UWB锚点的距离；

IMU模块3，安装在辅助运输机器人7上，用于提供辅助运输机器人7在导航坐标系中的IMU加速度信息；

定位导航计算机4，设有UWB接口5和IMU接口6，通过UWB接口5与UWB模块2相连，用于接收UWB距离信息；通过IMU接口6与IMU模块3相连，用于IMU加速度信息；

电源设备1，用于给辅助运输机器人7、UWB模块2、IMU模块3和定位导航计算机4提供电源；

所述定位导航计算机4利用上述所述辅助运输机器人7定位方法对辅助运输机器人7进行位置估计，从而得到辅助运输机器人7在井下的位置信息，进而实现了对辅助运输机器人7进行定位。本定位系统定位精度高、不易受干扰、成本低。

Claims (4)

1.一种动态复杂矿井环境中辅助运输机器人定位方法，其特征在于，包括以下步骤：

1)获取辅助运输机器人与坐标已知的UWB锚点之间的UWB距离信息，以及辅助运输机器人的IMU加速度信息；将UWB距离信息作为UWB量测输入，将IMU加速度信息作为控制输入，执行步骤2)；

2)利用扩展卡尔曼滤波融合IMU加速度信息与UWB距离信息，并对UWB量测进行异常值检测，若UWB量测为异常值，则通过公式(13)和公式(14)计算自适应因子λ_k修正新息协方差矩阵M_k|k-1，从而通过改进扩展卡尔曼滤波，得到辅助运输机器人的状态估计，进而辅助运输机器人得到位置估计；

其中，λ_k为自适应因子，通过如下公式获得：

3)若在采样周期内，辅助运输机器人获取到UWB距离信息，则执行步骤2)；

若在当前采样周期内，辅助运输机器人没有获取到UWB距离信息，则认为UWB距离量测丢失，将距离当前时刻最近的新息作为状态约束条件，利用拉格朗日乘数法计算约束后的状态估计，进而获得辅助运输机器人的位置估计；

若在多个采样周期内，没有获取到UWB距离信息，则认为辅助运输机器人处于UWB测距盲区，利用自回归模型重构UWB量测，作为新的UWB量测，对缺失的UWB量测数据进行补偿后，执行步骤2)；

所述自回归模型为：

其中，

为重构后的UWB量测，r_k,i为存储在内存中的UWB量测数据，n为存储UWB量测数据的数量，α_i为线性权重系数；

所述权重系数α_i通过最小化如下公式获得：

2.根据权利要求1所述的一种动态复杂矿井环境中辅助运输机器人定位方法，其特征在于，步骤2)具体包括以下步骤：

2.1)建立基于扩展卡尔曼滤波的UWB与IMU信息融合的系统运动模型：

x_k＝A_kx_k-1+B_k-1u_k-1+w_k-1 (1)

其中，

u_k-1＝[a_x,0,0,a_y,0,0,a_z,0,0]^T (5)

式中，w_k-1为系统噪声，服从均值为0，方差为Q_k-1的高斯分布，x_k为系统状态向量，A_k为系统状态矩阵，B_k-1为输入增益矩阵，u_k-1为控制输入；位置p＝[p_x,p_y,p_z]^T，速度v＝[v_x,v_y,v_z]^T以及加速度偏差a_b＝[a_bx,a_by,a_bz]^T，加速度信息a＝[a_x,a_y,a_z]^T，k为时间序列，T为采样间隔；

因此，得到扩展卡尔曼滤波的一步状态预测估计

以及一步预测误差协方差矩阵P_k|k-1：

其中，

式中，τ_a为加速度量测噪声，τ_b为加速度测量偏差；

2.2)建立扩展卡尔曼滤波的量测方程：

r_k＝h(x_k)+v_k (8)

式中，r_k为UWB量测，v_k为系统量测噪声，服从均值为0，方差为R_k的高斯分布，h(·)为量测函数；

因此，量测矩阵H_k由公式(9)的一阶泰勒展开定义为：

式中，UWB锚点的位置记作p_A＝[p_Ax,p_Ay,p_Az]^T，

为一步预测位置估计；通过公式(9)计算得到UWB锚点和辅助运输机器人之间的预测距离为

2.3)由量测方程得到扩展卡尔曼滤波的新息e_k|k-1及其协方差矩阵：

2.4)检测异常值：

当新息协方差矩阵M_k|k-1小于或等于设定阈值ψ，判断UWB量测为正常值，执行步骤2.5)；

当新息协方差矩阵M_k|k-1大于设定阈值ψ，判断UWB量测为异常值，利用扩展卡尔曼滤波融合UWB与IMU信息得到状态估计的过程中，通过计算自适应因子λ_k得对新息协方差矩阵M_k|k-1进行修正，得到新的新息协方差矩阵

令

执行步骤2.5)；

2.5)由步骤2.2)和步骤2.3)得到辅助运输机器人的状态估计

及其误差协方差矩阵P_k：

P_k＝(I-K_kH_k)P_k|k-1 (16)

其中，

式中，K_k为滤波增益矩阵，P_k|k-1为k时刻一步预测误差协方差矩阵，H_k为量测矩阵，M_k|k-1为新息协方差矩阵；

为一步状态预测估计，r_k为UWB量测，

为UWB锚点和辅助运输机器人之间的预测距离，I为n维单位矩阵。

3.根据权利要求1所述的一种动态复杂矿井环境中辅助运输机器人定位方法，其特征在于，若当前采样周期内，辅助运输机器人没有获取到UWB量测数据，则将距离当前时刻最近的新息作为状态约束条件，利用拉格朗日乘数法计算约束后的位置估计，具体步骤如下：

设置状态约束条件为：

式中，

为距离当前时刻最近的新息；将无约束的状态估计

投影到约束平面即公式(18)上，得到约束状态估计

满足如下公式：

其中，

W是一个正定矩阵；

利用拉格朗日乘数法最小化如下公式：

式中，S_k表示拉格朗日乘数因子，因此，得到基于约束条件的扩展卡尔曼滤波状态估计：

式中，H_k为量测矩阵，

为UWB锚点和辅助运输机器人之间的预测距离，

和

分别为更新后的扩展卡尔曼滤波的状态估计

及其误差协方差矩阵P_k，由更新后的

得到最终的辅助运输机器人的位置估计。

4.一种动态复杂矿井环境中辅助运输机器人定位系统，其特征在于，包括

辅助运输机器人；

UWB模块，安装在辅助运输机器人上，用于测量辅助运输机器人到UWB锚点之间的UWB距离信息；

UWB锚点，坐标已知，至少设有4个，分别安装在辅助运输机器人周围，且至少有一个UWB锚点与其他UWB锚点不在同一水平面上；

IMU模块，安装在辅助运输机器人上，用于提供辅助运输机器人在导航坐标系中的IMU加速度信息；

定位导航计算机，设有UWB接口和IMU接口，通过UWB接口与UWB模块相连，用于接收UWB距离信息；通过IMU接口与IMU模块相连，用于接收IMU加速度信息；所述定位导航计算机利用权利要求1-3任一项所述的定位方法对辅助运输机器人进行位置估计，得到辅助运输机器人的位置信息；

电源设备，用于给辅助运输机器人、UWB测距模块、IMU模块和定位导航计算机提供电源。

Images