一种用于掘进机定位定向的方法及装置与流程

本发明涉及掘进机定位导航技术领域，具体涉及一种用于掘进机定位定向的方法及装置。

背景技术：

掘进机是用于平直地面开凿巷道的机器，掘进机的导航是自动行走的关键技术，而为了实现掘进机的导航，其位置与方向的确定是非常重要的环节。目前国内掘进机的导航方式主要是根据地测部门提前绘制好的掘进路线，然后将激光标记器安装在巷道后方，激光束投射到需要挖掘的煤矿上，根据激光的指向，由挖掘机操控员进行操作，操作十分繁琐，而巷道中工作环境恶劣，此方式效率低下，而且精度有限。

现有技术如下：

中国专利cn101266134公开了一种悬臂掘进机机头位姿测量系统及其方法，将激光机动全站仪设置在巷道壁上，测量掘进机车体在大地坐标系的坐标，测量掘进机车体偏摆角，双轴倾角传感器测量掘进机车体仰俯角和横滚角、油缸行程，计算悬臂掘进机头相对车体以及大地坐标系的位姿，实现悬臂掘进机头位姿的测量。

中国专利cn101975063a公开了一种掘进机激光引导定位定向装置及方法，利用在掘进机后方的巷道顶板上设置一个激光导向装置，在悬臂上安装一个激光接收器，输出掘进机偏离激光引导方向的偏差和掘进悬臂对准掘进断面中心坐标的信息，从而推导出掘进机的位姿信息。

中国专利cn102589514a公开了一种掘进机位姿参数测量装置及其方法，主要依靠激光指向仪以及安装在机身上的两个激光接收光栅，当光栅接收到激光束后通过计算得到掘进机机身的左右偏移、上下偏移、相对于掘进机机身重心的摇摆角、俯仰角、偏转角。

中国专利cn101169038a公开了一种全自动掘进机，通过航天导航定位仪将掘进机特征提取值与导航计算值传送到上位机机型数据融合，由下位机对掘进机实施位置控制，实现自动寻迹功能。

中国专利cn106840137a公开了一种四点式掘进机自动定位定向方法，基于机器视觉技术，由工业智能摄像机获取其图像，从中解算掘进机机身相对于工业智能摄像机的偏转角、横滚角、俯仰角以及机身的横向偏移量、纵向偏移量和机身到工业智能摄像机的距离等参数，实现掘进机自动定位定向。

然而上述方法存在很多不足：

(1)巷道中环境较为恶劣，存在较多粉尘，激光、红外线以及可见光等光的传输方式易受到影响，从而导致精度下降。

(2)需要人为设置后方巷道基准，当掘进距离超出施工范围，需要重新人为建立基准，重新进行标定。故工程施工上较为繁琐，不利于高效安全的进行施工。

(3)公开号为cn101169038a的专利将惯性导航系统应用至掘进机的定位定向过程中，但是惯性导航系统的误差随时间积累迅速增大，从实用价值角度看来该系统具有很大的局限。

(4)公开号为cn106840137a的专利基于机器视觉技术，但是由于在巷道内，可见光条件恶劣，且粉尘较多，只采用视觉技术受影响较大，精度有限。

综上所述，急需一种用于掘进机定位定向的方法及装置以解决现有技术中存在的问题。

技术实现要素：

本发明目的在于提供一种用于掘进机定位定向的方法，具体技术方案如下：

一种用于掘进机定位定向的方法，包括用于获取掘进机实时速度及位置信息的航位推算过程和用于获取掘进机实时姿态信息的惯性解算过程；在所述惯性解算过程中引入姿态四元数以计算得到掘进机的实时姿态信息，所述姿态四元数通过计算导航坐标系各方向轴上的对应速度和中间速度获得，所述导航坐标系包括x轴、y轴和z轴。

以上技术方案中优选的，具体包括以下步骤：

步骤1：启动导航系统，导航计算机开启，惯性罗经以及里程计启动；

步骤2：对惯性罗经进行初始对准，获得精确的初始姿态信息和初始速度及位置信息；

步骤3：掘进机运动后对里程计进行实时采样，通过计算得出掘进机当前行进的距离和速度；

步骤4：将所得的掘进机的姿态信息、行进距离和行进速度投影到掘进机载体坐标系中，通过导航计算机进行航位推算以获得当前掘进机的实时速度及位置信息；

步骤5：将里程计的速度阻尼输入到惯性罗经中抑制姿态误差振荡，导航计算机根据惯性罗经的采样值进行惯性解算，得到当前掘进机的实时姿态信息；

步骤6：重复步骤3-5，以当前掘进机的姿态信息、速度及位置信息计算下一时刻掘进机的姿态信息、速度及位置信息，直至导航系统工作结束。

以上技术方案中优选的，所述步骤3中计算掘进机当前行进距离和速度的具体过程为：里程计在采样时间长度t内输出的脉冲数为n，单个脉冲所代表的距离为x，则掘进机的行进距离为s(t)＝nx，速度为

以上技术方案中优选的，所述步骤4中导航计算机进行航位推算的具体过程如下：

速度计算：

东向/x轴速度为：vx(t)＝v(t)cosφsinψ，

北向/y轴速度为：vy(t)＝v(t)cosφcosψ，

天向/z轴速度为：vz(t)＝v(t)sinφ，

位置计算：

纬度/x轴为：

经度/y轴为：

高度/z轴：z(t)＝vz(t)·t+z(t-1)，

其中φ为姿态信息中的纵摇，ψ为姿态信息中的航向。

以上技术方案中优选的，所述步骤5中导航计算机进行惯性解算的具体过程如下：

姿态四元数的计算表达式为：

在表达式(1)中：

在表达式(2)中：

其中，δp^g为惯性罗经中的陀螺仪以固定频率输出的角度增量脉冲数，为陀螺标度矩阵，其中kgx,kgy,kgz分别为x轴、y轴、z轴三个方向上的标度因数误差，为安装误差矩阵，其中μij(i＝x,y,z；j＝x,y,z；i≠j)为i轴上的陀螺安装误差，δg为陀螺常值漂移，t为采样时间长度；

表达式(1)中的计算方法为，设姿态信息横摇为θ，纵摇为φ，航向为ψ，则经过初始对准后四元数的初始值表达式为：

求解出后，即可根据求出的值，再对进行归一化，即由此得到最终的值。

以上技术方案中优选的，表达式(1)中的计算过程如下：

通过对进行欧拉角到四元数变换即可得到故求得即可求得的计算表达式为：

其中，为x轴方向的对应速度，v^nx(t)、v^ny(t)分别为x轴、y轴方向的中间速度，k7为水平速度阻尼系数。

以上技术方案中优选的，惯性罗经中加速度计的输出为速度增量δv^b，则加速度的计算表达式为：

其中，t为采样时间长度，δa为加速度计零偏，为加速度计标度矩阵，其中kax、kay、kaz分别为x轴、y轴、z轴三个方向上的标度因数误差，为加速度计的安装误差矩阵，其中γij(i＝x,y,z；j＝x,y,z；i≠j)为i轴上的加速度计安装误差，为姿态矩阵的转置矩阵，即的计算表达式为：

以上技术方案中优选的，计算x轴、y轴、z轴方向的对应速度和中间速度的过程如下：

x轴方向有害加速度的计算表达式为：

y轴方向有害加速度的计算表达式为：

z轴方向有害加速度的计算表达式为：

其中：

rm是子午圈半径，

rn是卯酉圈半径，

g0＝9.78045m/s²，

a＝6378.245km为地球半径，e为椭球的偏心率，z(t-1)为t-1时刻的实时高度，ω为地球自转角速度；

将表达式(5)求出的加速度以及表达式(7)、表达式(8)、表达式(9)求出的有害加速度，带入以下表达式分别求出x轴、y轴、z轴方向的中间速度：

x轴方向的中间速度解算表达式为：

y轴方向的中间速度解算表达式为：

z轴方向的中间速度解算表达式为：

其中为里程计输出的速度信息，即为里程计输入到惯性罗经中的速度阻尼，fnx、fny、fnz分别为加速度fn在x轴、y轴、z轴三个方向的分量，v^nx(0)、v^ny(0)、v^nz(0)在初始时刻为0，vr^nx(0)、vr^ny(0)在初始时刻为0；

将x轴、y轴、z轴方向的中间速度带入到以下表达式中，可以解算出x轴、y轴、z轴方向的对应速度为：

其中，

其中，k1为水平速度积分系数，k2和k3为水平速度阻尼系数，k4为天向速度阻尼系数，k5和k6为天向位移阻尼系数，zr(t)是外界得出的高度信息，dx(0)、dy(0)、dz(0)初始值为0。

应用本发明的技术方案，具有以下有益效果：

(1)本发明利用惯性罗经以及里程计组合导航方式，无需外界人为输入信息或进行基准标定，可以使掘进机全自动工作，并且省去了繁琐的劳动力；利用惯性罗经提供的姿态信息以及里程计提供的精确的速度信息来进行掘进机的定位定向，使得掘进机在地下工作环境中可以获得精确的姿态速度位置信息，便于施工进行。

(2)本发明可以实现全自主导航，解决了现有技术中需要人为设置后方巷道基准的问题，大大提高了施工的效率，同时本发明的定位定向方法不会产生人为标定基准所带来的误差，使得导航精度下降，有效的保证导航系统的精确性。

(3)本发明将里程计输出的速度信息作为输入到惯性罗经中的反馈信息，通过里程计的速度信息的约束控制惯性罗经姿态误差发散，使得惯性罗经精度提升，使得航向角误差减小，保证了姿态角(姿态信息)的精确性。

(4)本发明未采用光学手段，避免了现有技术中由于恶劣的工作环境(粉尘较多、可见光较弱)导致精度降低的情况，本发明的定位定向方法有效的避免工作环境对掘进机工作造成干扰，因此使用本发明的定位定向方法对掘进机进行定位导航不会受工作环境或地理位置等干扰。

本发明还提供了一种用于上述掘进机定位定向的方法的装置，包括均设置在掘进机上的惯性罗经、导航计算机以及里程计，所述惯性罗经和里程计均连接导航计算机，所述里程计输出速度信息给导航计算机，所述惯性罗经输出角度增量脉冲数和加速度给导航计算机，所述导航计算机中进行航位推算法和惯性解算获得掘进机的实时姿态信息和实时速度及位置信息。

本发明利用惯性罗经输出的角度增量脉冲数、加速度输入到导航计算机中进行惯性解算获得实时姿态信息，利用里程计输出的速度信息给导航计算机进行航位推算获得实时速度及位置信息，从而实现对掘进机进行定位定向，实现精确导航，惯性罗经、里程计以及导航计算机的组合结构简单，能解决现有技术中工作环境导致掘进机的定位精度低的问题。

除了上面所描述的目的、特征和优点之外，本发明还有其它的目的、特征和优点。下面将参照图，对本发明作进一步详细的说明。

附图说明

构成本申请的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1是本发明提供的掘进机定位定向方法的组成图；

图2是本发明提供的掘进机定位定向方法的流程图；

图3是本发明提供的掘进机定位定向装置的部件连接关系图；

其中，1、掘进机，2、惯性罗经，3、导航计算机，4、里程计。

具体实施方式

以下结合附图对本发明的实施例进行详细说明，但是本发明可以根据权利要求限定和覆盖的多种不同方式实施。

实施例1：

参见图3，一种用于掘进机定位定向的装置，包括均设置在掘进机1上的惯性罗经2、导航计算机3以及里程计4，所述惯性罗经2和里程计4均连接导航计算机3，所述里程计4输出速度信息给导航计算机3(具体的，所述里程计输出给导航计算机的为脉冲信号)，所述惯性罗经2输出角度增量脉冲数和加速度给导航计算机3，所述导航计算机3中进行航位推算法和惯性解算获得掘进机1的实时姿态信息和实时速度及位置信息。

优选的，所述惯性罗经为陀螺罗经，所述惯性罗经内部包括陀螺仪和加速度计。

参见图1，使用上述用于掘进机的定位定向的装置进行定位的方法详情如下：

一种用于掘进机定位定向的方法，包括用于获取掘进机实时速度及位置信息的航位推算过程和用于获取掘进机实时姿态信息的惯性解算过程；在所述惯性解算过程中引入姿态四元数以计算得到掘进机的实时姿态信息，所述姿态四元数通过计算导航坐标系各方向轴上的对应速度和中间速度获得，所述导航坐标系包括x轴、y轴和z轴。

所述导航坐标系为东北天坐标系。

参见图2，所述用于掘进机定位定向的方法具体包括以下步骤：

步骤1：启动导航系统，导航计算机开启，惯性罗经以及里程计启动；

步骤2：对惯性罗经进行初始对准，获得精确的初始姿态信息和初始速度及位置信息；

步骤3：掘进机运动后对里程计进行实时采样，通过计算得出掘进机当前行进的距离和速度；

步骤4：将所得的掘进机的姿态信息、行进距离和行进速度投影到掘进机载体坐标系中，通过导航计算机进行航位推算以获得当前掘进机的实时速度及位置信息；

步骤5：将里程计的速度阻尼输入到惯性罗经中抑制姿态误差振荡，导航计算机根据惯性罗经的采样值进行惯性解算，得到当前掘进机的实时姿态信息；

步骤6：重复步骤3-5，以当前掘进机的姿态信息、速度及位置信息计算下一时刻掘进机的姿态信息、速度及位置信息，直至导航系统工作结束。

对惯性罗经进行初始对准请参照现有技术，在此不再详细描述。

所述步骤3中计算掘进机当前行进的距离和速度的具体过程为：里程计在采样时间长度t内输出的脉冲数为n，单个脉冲所代表的距离为x，则掘进机的行进距离为s(t)＝nx，速度为

所述步骤4中导航计算机进行航位推算的具体过程如下：

速度计算：

东向(x轴)速度为：vx(t)＝v(t)cosφsinψ，

北向(y轴)速度为：vy(t)＝v(t)cosφcosψ，

天向(z轴)速度为：vz(t)＝v(t)sinφ，

位置计算：

纬度(x轴)为：

经度(y轴)为：

高度(z轴)：z(t)＝vz(t)·t+z(t-1)，

其中φ为姿态信息中的纵摇，ψ为姿态信息中的航向。

所述步骤5中导航计算机进行惯性解算的具体过程如下：

姿态四元数的计算表达式为：

在表达式(1)中

在表达式(2)中：

其中，δp^g为惯性罗经中的陀螺仪以固定频率输出的角度增量脉冲数，为陀螺标度矩阵，其中kgx,kgy,kgz分别为x轴、y轴、z轴三个方向上的标度因数误差，为安装误差矩阵，其中μij(i＝x,y,z；j＝x,y,z；i≠j)为i轴上的陀螺安装误差，δg为陀螺常值漂移，t为采样时间长度；

表达式(1)中的计算方法为，设姿态信息横摇为θ，纵摇为φ，航向为ψ，则经过初始对准后四元数的初始值为表达式为：

求解出后，即可根据求出的值，再对进行归一化，即由此得到最终的值。

表达式(1)中的计算过程如下：

通过对进行欧拉角到四元数变换即可得到故求得即可求得的计算表达式为：

其中，为x轴方向的对应速度，v^nx(t)、v^ny(t)分别为x轴、y轴方向的中间速度，k7为水平速度阻尼系数。

惯性罗经中加速度计的输出为速度增量δv^b，则加速度的计算表达式为：

其中，t为采样时间长度，δa为加速度计零偏，为加速度计标度矩阵，其中kax、kay、kaz分别为x轴、y轴、z轴三个方向上的标度因数误差，为加速度计的安装误差矩阵，其中γij(i＝x,y,z；j＝x,y,z；i≠j)为i轴上的加速度计安装误差，为姿态矩阵的转置矩阵，即的计算表达式为：

计算x轴、y轴、z轴方向的对应速度和中间速度的过程如下：

x轴方向有害加速度的计算表达式为：

y轴方向有害加速度的计算表达式为：

z轴方向有害加速度的计算表达式为：

其中：

rm是子午圈半径，

rn是卯酉圈半径，

g0＝9.78045m/s²，

a＝6378.245km为地球半径，e为椭球的偏心率，z(t-1)为t-1时刻的实时高度，ω为地球自转角速度；

将表达式(5)求出的加速度以及表达式(7)、表达式(8)、表达式(9)求出的有害加速度，带入以下表达式分别求出x轴、y轴、z轴方向的中间速度：

x轴方向的中间速度解算表达式为：

y轴方向的中间速度解算表达式为：

z轴方向的中间速度解算表达式为：

其中为里程计输出的速度信息，即为里程计输入到惯性罗经中的速度阻尼，将里程计输出的速度信息输入到惯性罗经中的反馈信息，通过里程计的速度信息的约束控制惯性罗经姿态误差发散，使得惯性罗经精度提升；fnx、fny、fnz分别为加速度fn在x轴、y轴、z轴三个方向的分量，v^nx(0)、v^ny(0)、v^nz(0)在初始时刻为0，vr^nx(0)、vr^ny(0)在初始时刻为0；

将x轴、y轴、z轴方向的中间速度带入到以下表达式中，可以解算出x轴、y轴、z轴方向的对应速度为：

其中，

其中，k1为水平速度积分系数，k2和k3为水平速度阻尼系数，k4为天向速度阻尼系数，k5和k6为天向位移阻尼系数，zr(t)是外界得出的高度信息(例如：气压计得到的高度信息)，dx(0)、dy(0)、dz(0)初始值为0。

k1、k2、k3、k4、k5、k6、k7根据具体导航系统可自行给定。

本领域的人员可以理解，通过表达式(1)求得姿态四元数之后，通过表达式(3)可以求出横摇θ，纵摇φ，航向ψ，即得到掘进机的实时姿态信息。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。