一种用于掘进机定位定向的方法及装置制造方法及图纸

本发明专利技术提供了一种用于掘进机定位定向的方法，包括用于获取掘进机实时速度及位置信息的航位推算过程和用于获取掘进机实时姿态信息的惯性解算过程；在所述惯性解算过程中引入姿态四元数以计算得到掘进机的实时姿态信息，所述姿态四元数通过计算导航坐标系各方向轴上的对应速度和中间速度获得，所述导航坐标系包括x轴、y轴和z轴。本发明专利技术的定位定向方法可以有效的解决工作环境对掘进机定位定向精度的影响，可以实现全自主导航，省去了需要人为设置后方巷道基准的麻烦，大大的提高了施工效率。本发明专利技术还提供了一种用于所述掘进机定位定向的方法的装置，该装置组成简单，可以解决现有技术中工作环境导致掘进机的定位精度低的问题。

A Method and Device for Location and Orientation of Roadheader

【技术实现步骤摘要】
一种用于掘进机定位定向的方法及装置
本专利技术涉及掘进机定位导航
，具体涉及一种用于掘进机定位定向的方法及装置。
技术介绍
掘进机是用于平直地面开凿巷道的机器，掘进机的导航是自动行走的关键技术，而为了实现掘进机的导航，其位置与方向的确定是非常重要的环节。目前国内掘进机的导航方式主要是根据地测部门提前绘制好的掘进路线，然后将激光标记器安装在巷道后方，激光束投射到需要挖掘的煤矿上，根据激光的指向，由挖掘机操控员进行操作，操作十分繁琐，而巷道中工作环境恶劣，此方式效率低下，而且精度有限。现有技术如下：中国专利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)中：其中，ΔPG为惯性罗经中的陀螺仪以固定频率输出的角度增量脉冲数，为陀螺标度矩阵，其中kgx,kgy,kgz分别为x轴、y轴、z轴三个方向上的标度因数误差，为安装误差矩阵，其中μij(i＝x,y,z；j＝x,y,z；i≠j)为i轴上的陀螺安装误差，ΔG为陀螺常值漂移，T为采样时间长度；表达式(1)中的计算方法为，设姿态信息横摇为θ，纵摇为φ，航向为ψ，则经过初始对准后四元数的初始值表达式为：求解出后，即可根据求出的值，再对进行归一化，即由此得到最终的值。以上技术方案中优选的，表达式(1)中的计算过程如下：通过对进行欧拉角到四元数变换即可得到故求得即可求得的计算表达式为：其中，为x轴方向的对应速度，VNx(t)、VNy(t)分别为x轴、y轴方向的中间速度，K7为水平速度阻尼系数。以上技术方案中优选的，惯性罗经中加速度计的输出为速度增量ΔVb，则加速度的计算表达式为：其中，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/s2，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轴三个方向的分量，VNx(0)、VNy(0)、VNz(0)在初始时刻为0，VrNx(0)、本文档来自技高网...

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

【技术特征摘要】
1.一种用于掘进机定位定向的方法，其特征在于，包括用于获取掘进机实时速度及位置信息的航位推算过程和用于获取掘进机实时姿态信息的惯性解算过程；在所述惯性解算过程中引入姿态四元数以计算得到掘进机的实时姿态信息，所述姿态四元数通过计算导航坐标系各方向轴上的对应速度和中间速度获得，所述导航坐标系包括x轴、y轴和z轴。2.根据权利要求1所述掘进机定位定向的方法，其特征在于，具体包括以下步骤：步骤1：启动导航系统，导航计算机开启，惯性罗经以及里程计启动；步骤2：对惯性罗经进行初始对准，获得精确的初始姿态信息和初始速度及位置信息；步骤3：掘进机运动后对里程计进行实时采样，通过计算得出掘进机当前行进的距离和速度；步骤4：将所得的掘进机的姿态信息、行进距离和行进速度投影到掘进机载体坐标系中，通过导航计算机进行航位推算以获得当前掘进机的实时速度及位置信息；步骤5：将里程计的速度阻尼输入到惯性罗经中抑制姿态误差振荡，导航计算机根据惯性罗经的采样值进行惯性解算，得到当前掘进机的实时姿态信息；步骤6：重复步骤3-5，以当前掘进机的姿态信息、速度及位置信息计算下一时刻掘进机的姿态信息、速度及位置信息，直至导航系统工作结束。3.根据权利要求2所述掘进机定位定向的方法，其特征在于，所述步骤3中计算掘进机当前行进距离和速度的具体过程为：里程计在采样时间长度T内输出的脉冲数为n，单个脉冲所代表的距离为X，则掘进机的行进距离为S(t)＝nX，速度为4.根据权利要求3所述掘进机定位定向的方法，其特征在于，所述步骤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.根据权利要求2所述掘进机定位定向的方法，其特征在于，所述步骤5中导航计算机进行惯性解算的具体过程如下：姿态四元数的计算表达式为：在表达式(1)中：在表达式(2)中：Δθ＝|ΔθB|，其中，ΔPG为惯性罗经中的陀螺仪以固定频率输出的角度增量脉冲数，为陀螺标度矩阵，其中kgx,kgy,kgz分别为x轴、y轴、z轴三个方向上的标度因数误差，为安装误差矩阵，其中μij(i＝x,y,z；j＝x,y,z；i≠j)为i轴上的陀螺安装误差，ΔG为陀螺常值漂移，T为采样时间长度；表达式(1)中的计算方法为，设姿态信息横摇为θ，纵摇为φ，航向为ψ，则经过初始对准后四元数的初始值表达式为...

【专利技术属性】
技术研发人员：韩松来，王思远，芦佳振，罗世彬，
申请(专利权)人：中南大学，
类型：发明
国别省市：湖南,43