CN112378399B - 基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法 - Google Patents

Abstract

本发明涉及煤矿开采技术领域，具体涉及一种基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法，该定向方法根据数字全站仪数据和捷联惯导系统数据融合后的数据拟合出数字式全站仪与捷联惯导系统测量出的位姿曲线，利用数字式全站仪测出的数据来校正捷联惯导系统的位姿，实现掘进工作面的实时、精准的定位定向。本发明利用捷联惯导和数字式全站仪的优势，实现了对掘进机的精准定位、定向。

Description

基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位 定向方法

技术领域

本发明涉及煤矿开采技术领域，具体涉及一种基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法。

背景技术

煤炭作为我国的主体能源之一，其所占据的地位在相当长的时间不会发生变化。虽然随着我国甚至整个世界煤炭存储量的不断减少，各个国家越发在意一次能源的开发与利用，但我国目前煤炭地下气化或是其他技术还是比较薄弱，而且某些行业依旧依赖于煤炭。因此在未来的一段时间内，煤炭的开采依旧会不断进行。随着煤矿开采深度的提高，煤矿开采越困难，危险系数越高，煤矿的无人化生产就显得极为重要。煤矿井下掘进机机器人的位姿检测是综掘工作面无人化生产的必要前提。目前对煤矿井下掘进机器人的位姿检测方法不足之处体现在：①利用激光指向器引导煤矿井下掘进机器人定向掘进，这种方式无法获得掘进机器人在巷道的准确的位姿状态，无法为掘进控制提供位姿信息②单纯利用全站仪对掘进机的位姿进行检测时，由于粉尘等原因无法实现实时得到掘进机器人的位置信息，并且无法检测航向角。

因为煤矿井下不存在GPS信号，而且煤矿底下环境恶劣，利用捷联惯导对煤矿井下掘进机器人的位姿进行检测是一种好的方法。捷联惯导具有不依赖外部信息也不向外部辐射能量，所以隐蔽性好且不受外界干扰，可全天候工作于地面、水下和空中各种环境等优点，但是单纯利用捷联惯导去检测掘进机的位姿并不准确，随着时间的推移，捷联惯导的检测误差会不断累积。

发明内容

为解决上述问题，本发明提供了一种基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法，利用捷联惯导和数字式全站仪的优势，实现了对掘进机的精准定位、定向。

为实现上述目的，本发明采取的技术方案为：

一种基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法，根据数字全站仪数据和捷联惯导系统数据融合后的数据拟合出数字式全站仪与捷联惯导系统测量出的位姿曲线，利用数字式全站仪测出的数据来校正捷联惯导系统的位姿，实现掘进工作面的实时、精准的定位定向。

进一步地，该方法包括以下步骤：

S1、在煤矿巷道掘进机器人上布置数字式全站仪配套的棱镜和捷联惯导系统，并将全站仪布放在掘进机器人后方的巷道内；

S2、建立载体坐标系(b系)与导航坐标系(n系)，并构建载体坐标系(b系)与导航坐标系(n系)的转换矩阵：

S201、以掘进机器人重心为坐标原点，掘进机器人前进的方向作为Y轴正方向，以向上为Z轴正方向，垂直Y轴向右方向为X轴正方向，建立载体坐标系0XYZ(b系)；

S202、根据煤矿井下的数字式全站仪参考点(当地经纬度)测定惯导所在位置，从而确定“东-北-天”导航坐标系O₁X₁Y₁Z₁(n系)；

S203、掘进机器人在导航坐标系的位姿转换到载体坐标系的转换矩阵由三个欧拉角-航向角(ψ)，横滚角(φ)，俯仰角(θ)组成，转换矩阵如下：

通过转换矩阵，得出掘进机器人在载体坐标系的位姿；

S3、将捷联惯导系统测出的数据和数字全站仪测出的数据传入计算机，完成数据的融合滤波处理，具体的：

S301、建立捷联惯导和数字全站仪的系统状态方程

由于棱镜与捷联惯导系统的位置不在同一位置，需考虑外部的杆臂误差，将杆臂长度扩展到系统的状态量中，得到下面的系统状态矢量：

式中，X为状态向量，

为捷联惯导的数学平台误差角，

分别为捷联惯导三个坐标轴的数学平台误差角；δvⁿ为载体的速度误差，

分别为载体在地理坐标系下的速度误差；δpⁿ为载体的位置误差，δLδλδh分别为经度、纬度、高度误差；ε^b为陀螺仪的漂移误差，

ε^b _z分别为陀螺仪三轴的漂移误差；

为加速度计的漂移误差，

分别为加速度计三轴的漂移误差；r为杆臂长度，r_x，r_y，r_z为三个方向的杆臂分量；

将捷联惯导的测量值作为状态方程的输入变量，状态方程如下：

式中，

为地理系相对于惯性系的角速率误差，

为地理系相对于惯性系的角速率，

为地理系与载体系的坐标转换矩阵，ε_b为地理系角速率误差，fⁿ为地理坐标系下加速度计的输出比力，

为地球系相对于惯性系的角速率误差，

为地理坐标系相对于地球系的角速率误差，

为地球系相对于惯性系的角速率，

为地理坐标系相对于地球系的角速率，δvⁿ为载体的速度误差，δgⁿ为重力加速度误差，

为载体在地理坐标系下y轴的速度，R_m为卯酉圈半径，R_n为子午圈半径，h为当地海拔高度，

载体在地理坐标系下x轴的速度误差，

为载体在地理坐标系下x轴的速度，

载体在地理坐标系下z轴的速度误差，L为当地经度、λ为当地纬度，T_c为白噪声，

为加速度计的漂移误差，w_c为白噪声；

步骤302、建立捷联惯导和数字全站仪的系统量测方程

数字全站仪输出位置表示如下：

式中：x₀，y₀，z₀表示测站点的N、E、Z坐标(已知)；D为斜距；a为目标点与测站点连线与z轴的夹角，b为方位角，ih为仪器高；th为目标高；

数字全站仪的刻度系数误差ζ，方位角误差δb为数字全站仪的输出误差δP的主要因素，则

选取捷联惯导输出的位置与数字全站仪的位置的差值作为量测值，设惯导解算的位置和数字全站仪的位置分别为x^惯导、y^惯导、x^全站仪、y^全站仪，则捷联惯导与数字全站仪的组合量测方程式为：

Z(t)＝H(t)X(t)+V(t)；

式中：

H(t)＝[0_2×6 I_2×2 I_2×3 0_2×3 H_2×3]

V(t)为量测噪声矩阵；

由于存在杆臂误差，增加的矩阵为H_2×3；

式中：n_x，n_y为杆臂长度；V＝[-n_x -n_y]^T；

步骤303、建立捷联惯导和数字全站仪无迹卡尔曼滤波方程

1)计算2n+1个Sigma(采样)点和这些采样点的权值(UT变换)，公式如下：

式中，

表示矩阵方根第i列，下标m为均值，c为协方差，上标为第几个采样点，参数λ＝a²(n+k)-n表示缩放比例参数，a的选取控制了采样点的分布。

为第i个点权值，i＝1～2n；

2)获得一组采样点(sigma点)及其对应权值

式中：X⁽ⁱ⁾(k/k)为第i个采样点，X(k/k)为第k时刻的系统状态，P(k/k)为第k时刻的系统协方差。

3)计算2n+1个Sigma点集的一步预测：

X⁽ⁱ⁾(k+1/k)＝f[k，X⁽ⁱ⁾(k/k)]，i＝1，2，...，2n+1；

式中：X⁽ⁱ⁾(k+1/k)为sigma点集合，f为非线性函数。

4)计算系统状态量的一步预测及协方差矩阵

式中：

为第k+1时刻的系统状态预测，P(k+1/k)为第k+1时刻的系统协方差。Q为系统噪声矩阵。

5)根据一步预测值，再次应用UT变换，产生新的Sigma点：

6)由上述5)得到的新Sigma点集带入观测方程，得到预测观测量：

Z⁽ⁱ⁾(k+1/k)＝h[X⁽ⁱ⁾(k+1/k)]，i＝1，2，3，...，2n+1；

式中：Z⁽ⁱ⁾(k+1/k)为第i个采样点的量测值，h为量测方程系数阵。

7)由6)得到的观测预测值，并加权得到系统预测的均值及方差；

式中：

为观测量的预测均值，

为系统的系统的观测方差，R为观测噪声矩阵，

为组合系统的方差。

8)计算卡尔曼增益矩阵，计算系统的状态更新和协方差

式中：K(k+1)为卡尔曼增益，其余参数见步骤1)-7)。

S4、根据数字全站仪数据和捷联惯导系统数据融合后的数据拟合出数字式全站仪与捷联惯导系统测量出的位姿曲线，利用数字式全站仪测出的数据来校正捷联惯导系统的位姿，实现掘进工作面的实时、精准的定位定向。

进一步地，通过数字全站仪检测的位置信息实时校正捷联惯导系统累积误差。

进一步地，采用高精度捷联惯导系统，该系统具有零速自动校正功能，自动修正惯导航向。

进一步地，利用无迹卡尔曼滤波方法实现捷联惯导系统和数字全站仪数据的融合。

进一步地，考虑棱镜与捷联惯导系统的布放位置不同产生的杆臂误差，并通过无迹卡尔曼滤波方法减少这种误差，提高融合精度。

本发明具有以下有益效果：

(1)利用全站仪测出的数据对捷联惯导的累积误差进行自动补偿，克服纯惯导的定位累积误差；

(2)实现对掘进机的位姿实时检测，克服全站仪连续性差的缺点；

(3)很好的适应井下的复杂环境，提高掘进机定位定向精度。

附图说明

图1是本发明的原理图。

图2是本发明的装置布置示意图。

图3是本发明的流程图。

图4是本发明的x方向的定位误差图。

图5是本发明的y方向的定位误差图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进。这些都属于本发明的保护范围。

本发明实施例提供了一种基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法，如图1所示，该方法包括以下步骤：

S1、在煤矿巷道掘进机器人上布置数字式全站仪配套的棱镜和捷联惯导系统，并将全站仪布放在掘进机器人后方的巷道内；

S2、建立载体坐标系(b系)与导航坐标系(n系)，并构建载体坐标系(b系)与导航坐标系(n系)的转换矩阵：

S201、以掘进机器人重心为坐标原点，掘进机器人前进的方向作为Y轴正方向，以向上为Z轴正方向，垂直Y轴向右方向为X轴正方向，建立载体坐标系0XYZ(b系)；

S202、根据煤矿井下的数字式全站仪参考点(当地经纬度)测定惯导所在位置，从而确定“东-北-天”导航坐标系O₁X₁Y₁Z₁(n系)；

S203、掘进机器人在导航坐标系的位姿转换到载体坐标系的转换矩阵由三个欧拉角-航向角(ψ)，横滚角(φ)，俯仰角(θ)组成，转换矩阵如下：

通过转换矩阵，得出掘进机器人在载体坐标系的位姿；

S3、将捷联惯导系统测出的数据和数字全站仪测出的数据传入计算机，完成数据的融合滤波处理，具体的：

S301、建立捷联惯导和数字全站仪的系统状态方程

由于棱镜与捷联惯导系统的位置不在同一位置，需考虑外部的杆臂误差，将杆臂长度扩展到系统的状态量中，得到下面的系统状态矢量：

式中，X为状态向量，

为捷联惯导的数学平台误差角，

分别为捷联惯导三个坐标轴的数学平台误差角；δvⁿ为载体的速度误差，

分别为载体在地理坐标系下的速度误差；δpⁿ为载体的位置误差，δLδλδh分别为经度、纬度、高度误差；ε^b为陀螺仪的漂移误差，

ε^b _z分别为陀螺仪三轴的漂移误差；

为加速度计的漂移误差，

分别为加速度计三轴的漂移误差；r为杆臂长度，r_x，r_y，r_z为三个方向的杆臂分量；

将捷联惯导的测量值作为状态方程的输入变量，状态方程如下：

式中，

为地理系相对于惯性系的角速率误差，

为地理系相对于惯性系的角速率，

为地理系与载体系的坐标转换矩阵，ε_b为地理系角速率误差，fⁿ为地理坐标系下加速度计的输出比力，

为地球系相对于惯性系的角速率误差，

为地理坐标系相对于地球系的角速率误差，

为地球系相对于惯性系的角速率，

为地理坐标系相对于地球系的角速率，δνⁿ为载体的速度误差，δgⁿ为重力加速度误差，

为载体在地理坐标系下y轴的速度，R_m为卯酉圈半径，R_n为子午圈半径，h为当地海拔高度，

载体在地理坐标系下x轴的速度误差，

为载体在地理坐标系下x轴的速度，

载体在地理坐标系下z轴的速度误差，L为当地经度、λ为当地纬度，T_c为白噪声，

为加速度计的漂移误差，w_c为白噪声；

步骤302、建立捷联惯导和数字全站仪的系统量测方程

数字全站仪输出位置表示如下：

式中：x₀，y₀，z₀表示测站点的N、E、Z坐标(已知)；D为斜距；a为目标点与测站点连线与z轴的夹角，b为方位角，ih为仪器高；th为目标高；

数字全站仪的刻度系数误差ζ，方位角误差δb为数字全站仪的输出误差δP的主要因素，则

选取捷联惯导输出的位置与数字全站仪的位置的差值作为量测值，设惯导解算的位置和数字全站仪的位置分别为x^惯导、y^惯导、x^全站仪、y^全站仪，则捷联惯导与数字全站仪的组合量测方程式为：

Z(t)＝H(t)X(t)+V(t)；

式中：

H(t)＝[0_2×6 I_2×2 I_2×3 0_2×3 H_2×3]

V(t)为量测噪声矩阵；

由于存在杆臂误差，增加的矩阵为H_2×3；

式中：n_x，n_y为杆臂长度；V＝[-n_x -n_y]^T；

步骤303、建立捷联惯导和数字全站仪无迹卡尔曼滤波方程

1)计算2n+1个Sigma(采样)点和这些采样点的权值(UT变换)，公式如下：

式中，

表示矩阵方根第i列，下标m为均值，c为协方差，上标为第几个采样点，参数λ＝a²(n+k)-n表示缩放比例参数，a的选取控制了采样点的分布。

为第i个点权值，i＝1～2n；

2)获得一组采样点(sigma点)及其对应权值

式中：X⁽ⁱ⁾(k/k)为第i个采样点，X(k/k)为第k时刻的系统状态，P(k/k)为第k时刻的系统协方差。

3)计算2n+1个Sigma点集的一步预测：

X⁽ⁱ⁾(k+1/k)＝f[k，X⁽ⁱ⁾(k/k)]，i＝1，2，...，2n+1；

式中：X⁽ⁱ⁾(k+1/k)为sigma点集合，f为非线性函数。

4)计算系统状态量的一步预测及协方差矩阵

式中：

为第k+1时刻的系统状态预测，P(k+1/k)为第k+1时刻的系统协方差。Q为系统噪声矩阵。

5)根据一步预测值，再次应用UT变换，产生新的Sigma点：

6)由上述5)得到的新Sigma点集带入观测方程，得到预测观测量：

Z⁽ⁱ⁾(k+1/k)＝h[X⁽ⁱ⁾(k+1/k)]，i＝1，2，3，...，2n+1；

式中：Z⁽ⁱ⁾(k+1/k)为第i个采样点的量测值，h为量测方程系数阵。

7)由6)得到的观测预测值，并加权得到系统预测的均值及方差；

式中：

为观测量的预测均值，

为系统的系统的观测方差，R为观测噪声矩阵，

为组合系统的方差。

8)计算卡尔曼增益矩阵，计算系统的状态更新和协方差

式中：K(k+1)为卡尔曼增益，其余参数见步骤1)-7)。

S4、根据数字全站仪数据和捷联惯导系统数据融合后的数据拟合出数字式全站仪与捷联惯导系统测量出的位姿曲线，利用数字式全站仪测出的数据来校正捷联惯导系统的位姿，实现掘进工作面的实时、精准的定位定向。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (5)

1.一种基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法，其特征在于，根据数字全站仪数据和捷联惯导系统数据融合后的数据拟合出数字式全站仪与捷联惯导系统测量出的位姿曲线，利用数字式全站仪测出的数据来校正捷联惯导系统的位姿，实现掘进工作面的实时、精准的定位定向；

该方法包括以下步骤：

S1、在煤矿巷道掘进机器人上布置数字式全站仪配套的棱镜和捷联惯导系统，并将全站仪布放在掘进机器人后方的巷道内；

S2、建立载体坐标系与导航坐标系，并构建载体坐标系与导航坐标系的转换矩阵：

S201、以掘进机器人重心为坐标原点，掘进机器人前进的方向作为Y轴正方向，以向上为Z轴正方向，垂直Y轴向右方向为X轴正方向，建立载体坐标系OXYZ；

S202、根据煤矿井下的数字式全站仪参考点测定惯导所在位置，从而确定“东-北-天”导航坐标系O₁X₁Y₁Z₁；

S203、掘进机器人在地理坐标系的位姿转换到载体坐标系的转换矩阵由三个欧拉角-航向角ψ，横滚角φ，俯仰角θ组成，转换矩阵如下：

通过转换矩阵，得出掘进机器人在载体坐标系的位姿；

S3、将捷联惯导系统测出的数据和数字全站仪测出的数据传入计算机，完成数据的融合滤波处理，具体的：

S301、建立捷联惯导和数字全站仪的系统状态方程

由于棱镜与捷联惯导系统的位置不在同一位置，需考虑外部的杆臂误差，将杆臂长度扩展到系统的状态量中，得到下面的系统状态矢量：

式中，X为状态向量，

为捷联惯导的数学平台误差角，

分别为捷联惯导三个坐标轴的数学平台误差角；δvⁿ为载体的速度误差，

分别为载体在地理坐标系下的速度误差；δpⁿ为载体的位置误差，δL δλ δh分别为经度、纬度、高度误差；ε^b为陀螺仪的漂移误差，

分别为陀螺仪三轴的漂移误差；

为加速度计的漂移误差，

分别为加速度计三轴的漂移误差；r为杆臂长度，r_x，r_y，r_z为三个方向的杆臂分量；

将捷联惯导的测量值作为状态方程的输入变量，状态方程如下：

式中，

为地理系相对于惯性系的角速率误差，

为地理系相对于惯性系的角速率，

为地理系与载体系的坐标转换矩阵，ε_b为地理系角速率误差，fⁿ为地理坐标系下加速度计的输出比力，

为地球系相对于惯性系的角速率误差，

为地理坐标系相对于地球系的角速率误差，

为地球系相对于惯性系的角速率，

为地理坐标系相对于地球系的角速率，δvⁿ为载体的速度误差，δgⁿ为重力加速度误差，

为载体在地理坐标系下y轴的速度，R_m为卯酉圈半径，R_n为子午圈半径，h为当地海拔高度，

载体在地理坐标系下x轴的速度误差，

为载体在地理坐标系下x轴的速度，

载体在地理坐标系下z轴的速度误差，L为当地经度、λ为当地纬度，T_c为白噪声，

为加速度计的漂移误差，w_c为白噪声；

S302、建立捷联惯导和数字全站仪的系统量测方程

数字全站仪输出位置表示如下：

式中：x₀，y₀，z₀表示测站点的N、E、Z坐标；D为斜距；a为目标点与测站点连线与z轴的夹角，b为方位角，ih为仪器高；th为目标高；

数字全站仪的刻度系数误差ζ，方位角误差δb为数字全站仪的输出误差δP的主要因素，则

选取捷联惯导输出的位置与数字全站仪的位置的差值作为量测值，设惯导解算的位置和数字全站仪的位置分别为x^惯导、y^惯导、x^全站仪、y^全站仪，则捷联惯导与数字全站仪的组合量测方程式为：

Z(t)＝H(t)X(t)+V(t)；

式中：

H(t)＝[0_2×6 I_2×2 I_2×3 0_2×3 H_2×3]

增加的矩阵为杆臂误差矩阵H_2×3，

n_x，n_y为杆臂长度；V＝[-n_x -n_y]^T；

步骤303、建立捷联惯导和数字全站仪无迹卡尔曼滤波方程

1)计算2n+1个Sigma点和这些采样点的权值，公式如下：

式中，

表示矩阵方根第i列，下标m为均值，c为协方差，上标为第几个采样点，参数λ＝a²(n+k)-n表示缩放比例参数，a的选取控制了采样点的分布；

为第i个点权值，i＝1～2n；

2)获得一组采样点及其对应权值

式中：X⁽ⁱ⁾(k/k)为第i个采样点，X(k/k)为第k时刻的系统状态，P(k/k)为第k时刻的系统协方差；

3)计算2n+1个Sigma点集的一步预测：

X⁽ⁱ⁾(k+1/k)＝f[k，X⁽ⁱ⁾(k/k)]，i＝1，2，...，2n+1；

式中：X⁽ⁱ⁾(k+1/k)为sigma点集合，f为非线性函数；

4)计算系统状态量的一步预测及协方差矩阵

式中：

为第k+1时刻的系统状态预测，P(k+1/k)为第k+1时刻的系统协方差；Q为系统噪声矩阵；

5)根据一步预测值，再次应用UT变换，产生新的Sigma点：

6)由上述5)得到的新Sigma点集带入观测方程，得到预测观测量：

Z⁽ⁱ⁾(k+1/k)＝h[X⁽ⁱ⁾(k+1/k)]，i＝1，2，3，...，2n+1；

式中：Z⁽ⁱ⁾(k+1/k)为第i个采样点的量测值，h为量测方程系数阵；

7)由6)得到的观测预测值，并加权得到系统预测的均值及方差；

式中：

为观测量的预测均值，

为系统的系统的观测方差，R为观测噪声矩阵，

为组合系统的方差；

8)计算卡尔曼增益矩阵，计算系统的状态更新和协方差

式中：K(k+1)为卡尔曼增益，其余参数见步骤1)-7)；

S4、根据数字全站仪数据和捷联惯导系统数据融合后的数据拟合出数字式全站仪与捷联惯导系统测量出的位姿曲线，利用数字式全站仪测出的数据来校正捷联惯导系统的位姿，实现掘进工作面的实时、精准的定位定向。

2.如权利要求1所述的基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法，其特征在于：通过数字全站仪检测的位置信息实时校正捷联惯导系统累积误差。

3.如权利要求1所述的基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法，其特征在于：采用高精度捷联惯导系统，该系统具有零速自动校正功能，自动修正惯导航向。

4.如权利要求1所述的基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法，其特征在于：利用无迹卡尔曼滤波方法实现捷联惯导系统和数字全站仪数据的融合。

5.如权利要求1所述的基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法，其特征在于：考虑棱镜与捷联惯导系统的布放位置不同产生的杆臂误差，并通过无迹卡尔曼滤波方法减少这种误差，提高融合精度。

Images