CN110095135B - 一种用于掘进机定位定向的方法及装置 - Google Patents
一种用于掘进机定位定向的方法及装置 Download PDFInfo
- Publication number
- CN110095135B CN110095135B CN201910477554.3A CN201910477554A CN110095135B CN 110095135 B CN110095135 B CN 110095135B CN 201910477554 A CN201910477554 A CN 201910477554A CN 110095135 B CN110095135 B CN 110095135B
- Authority
- CN
- China
- Prior art keywords
- axis
- heading machine
- speed
- expression
- inertial
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 58
- 238000004364 calculation method Methods 0.000 claims abstract description 28
- 230000008569 process Effects 0.000 claims abstract description 28
- 230000014509 gene expression Effects 0.000 claims description 58
- 230000001133 acceleration Effects 0.000 claims description 27
- 230000005641 tunneling Effects 0.000 claims description 26
- 238000013016 damping Methods 0.000 claims description 18
- 239000011159 matrix material Substances 0.000 claims description 15
- 238000005070 sampling Methods 0.000 claims description 15
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 238000006073 displacement reaction Methods 0.000 claims description 3
- 238000009434 installation Methods 0.000 claims description 3
- 238000010606 normalization Methods 0.000 claims description 3
- 230000010355 oscillation Effects 0.000 claims description 3
- 238000010276 construction Methods 0.000 abstract description 6
- 238000005516 engineering process Methods 0.000 description 4
- 239000000428 dust Substances 0.000 description 3
- 238000004422 calculation algorithm Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 239000003245 coal Substances 0.000 description 1
- 238000005094 computer simulation Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000002452 interceptive effect Effects 0.000 description 1
- 239000003550 marker Substances 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C23/00—Combined instruments indicating more than one navigational value, e.g. for aircraft; Combined measuring devices for measuring two or more variables of movement, e.g. distance, speed or acceleration
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
- Y02D30/00—Reducing energy consumption in communication networks
- Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
本发明提供了一种用于掘进机定位定向的方法,包括用于获取掘进机实时速度及位置信息的航位推算过程和用于获取掘进机实时姿态信息的惯性解算过程;在所述惯性解算过程中引入姿态四元数以计算得到掘进机的实时姿态信息,所述姿态四元数通过计算导航坐标系各方向轴上的对应速度和中间速度获得,所述导航坐标系包括x轴、y轴和z轴。本发明的定位定向方法可以有效的解决工作环境对掘进机定位定向精度的影响,可以实现全自主导航,省去了需要人为设置后方巷道基准的麻烦,大大的提高了施工效率。本发明还提供了一种用于所述掘进机定位定向的方法的装置,该装置组成简单,可以解决现有技术中工作环境导致掘进机的定位精度低的问题。
Description
技术领域
本发明涉及掘进机定位导航技术领域,具体涉及一种用于掘进机定位定向的方法及装置。
背景技术
掘进机是用于平直地面开凿巷道的机器,掘进机的导航是自动行走的关键技术,而为了实现掘进机的导航,其位置与方向的确定是非常重要的环节。目前国内掘进机的导航方式主要是根据地测部门提前绘制好的掘进路线,然后将激光标记器安装在巷道后方,激光束投射到需要挖掘的煤矿上,根据激光的指向,由挖掘机操控员进行操作,操作十分繁琐,而巷道中工作环境恶劣,此方式效率低下,而且精度有限。
现有技术如下:
中国专利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,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)中:
其中,Δ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,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)可以求出横摇θ,纵摇φ,航向ψ,即得到掘进机的实时姿态信息。
以上所述仅为本发明的优选实施例而已,并不用于限制本发明,对于本领域的技术人员来说,本发明可以有各种更改和变化。凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (6)
1.一种用于掘进机定位定向的方法,其特征在于,包括用于获取掘进机实时速度及位置信息的航位推算过程和用于获取掘进机实时姿态信息的惯性解算过程;在所述惯性解算过程中引入姿态四元数以计算得到掘进机的实时姿态信息,所述姿态四元数通过计算导航坐标系各方向轴上的对应速度和中间速度获得,所述导航坐标系包括x轴、y轴和z轴;
所述掘进机定位定向的方法具体包括以下步骤:
步骤1:启动导航系统,导航计算机开启,惯性罗经以及里程计启动;
步骤2:对惯性罗经进行初始对准,获得精确的初始姿态信息和初始速度及位置信息;
步骤3:掘进机运动后对里程计进行实时采样,通过计算得出掘进机当前行进的距离和速度;
步骤4:将所得的掘进机的姿态信息、行进距离和行进速度投影到掘进机载体坐标系中,通过导航计算机进行航位推算以获得当前掘进机的实时速度及位置信息;
步骤5:将里程计的速度阻尼输入到惯性罗经中抑制姿态误差振荡,导航计算机根据惯性罗经的采样值进行惯性解算,得到当前掘进机的实时姿态信息;
步骤6:重复步骤3-5,以当前掘进机的姿态信息、速度及位置信息计算下一时刻掘进机的姿态信息、速度及位置信息,直至导航系统工作结束;
所述步骤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),
其中φ为姿态信息中的纵摇,ψ为姿态信息中的航向,V(t)为掘进机当前行进的速度,RM是子午圈半径,RN是卯酉圈半径;
所述步骤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)中的计算方法为,设姿态信息横摇为θ,纵摇为φ,航向为ψ,则经过初始对准后四元数/>的初始值表达式为:
求解出后,即可根据/>求出/>的值,再对/>进行归一化,即/>由此得到最终/>的值。
2.根据权利要求1所述掘进机定位定向的方法,其特征在于,所述步骤3中计算掘进机当前行进距离和速度的具体过程为:里程计在采样时间长度T内输出的脉冲数为n,单个脉冲所代表的距离为X,则掘进机的行进距离为S(t)=nX,速度为
3.根据权利要求1所述掘进机定位定向的方法,其特征在于,表达式(1)中的计算过程如下:
通过对进行欧拉角到四元数变换即可得到/>故求得/>即可求得/>的计算表达式为:
其中,为x轴方向的对应速度,VNx(t)、VNy(t)分别为x轴、y轴方向的中间速度,K7为水平速度阻尼系数。
4.根据权利要求3所述掘进机定位定向的方法,其特征在于,惯性罗经中加速度计的输出为速度增量ΔVb,则加速度的计算表达式为:
其中,T为采样时间长度,ΔA为加速度计零偏,为加速度计标度矩阵,其中kax、kay、kaz分别为x轴、y轴、z轴三个方向上的标度因数误差,/>为加速度计的安装误差矩阵,其中γij(i=x,y,z;j=x,y,z;i≠j)为i轴上的加速度计安装误差,/>为姿态矩阵的转置矩阵,即/> 的计算表达式为:
5.根据权利要求4所述掘进机定位定向的方法,其特征在于,计算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轴三个方向的分量,/>在初始时刻为0,/>在初始时刻为0;
将x轴、y轴、z轴方向的中间速度带入到以下表达式中,可以解算出x轴、y轴、z轴方向的对应速度为:
其中,
其中,K1为水平速度积分系数,K2和K3为水平速度阻尼系数,K4为天向速度阻尼系数,K5和K6为天向位移阻尼系数,Zr(t)是外界得出的高度信息,Dx(0)、Dy(0)、Dz(0)初始值为0。
6.一种配合如权利要求1-5中任意一项所述掘进机定位定向的方法使用的装置,其特征在于,包括均设置在掘进机上的惯性罗经、导航计算机以及里程计,所述惯性罗经和里程计均连接导航计算机,所述里程计输出速度信息给导航计算机,所述惯性罗经输出角度增量脉冲数和加速度给导航计算机,所述导航计算机通过航位推算和惯性解算获得掘进机的实时姿态信息和实时速度及位置信息。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910477554.3A CN110095135B (zh) | 2019-06-03 | 2019-06-03 | 一种用于掘进机定位定向的方法及装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910477554.3A CN110095135B (zh) | 2019-06-03 | 2019-06-03 | 一种用于掘进机定位定向的方法及装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110095135A CN110095135A (zh) | 2019-08-06 |
CN110095135B true CN110095135B (zh) | 2023-07-18 |
Family
ID=67450129
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910477554.3A Active CN110095135B (zh) | 2019-06-03 | 2019-06-03 | 一种用于掘进机定位定向的方法及装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110095135B (zh) |
Families Citing this family (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111323044A (zh) * | 2020-04-14 | 2020-06-23 | 中国矿业大学(北京) | 掘进机二维位移测量装置及测量方法 |
CN112033401B (zh) * | 2020-09-10 | 2023-06-20 | 西安科技大学 | 基于捷联惯导和油缸的智能掘进机器人定位定向方法 |
CN114485582B (zh) * | 2021-12-16 | 2023-09-26 | 山东科技大学 | 基于全站仪测量的挖掘机关键点的角度及距离的标定方法 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102997916A (zh) * | 2011-09-15 | 2013-03-27 | 北京自动化控制设备研究所 | 一种自主提高定位定向系统惯性姿态解算精度的方法 |
CN105547288A (zh) * | 2015-12-08 | 2016-05-04 | 华中科技大学 | 一种煤矿井下移动设备自主定位的方法及系统 |
CN109631938A (zh) * | 2018-12-28 | 2019-04-16 | 湖南海迅自动化技术有限公司 | 掘进机自主定位定向系统及方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US9182237B2 (en) * | 2013-12-06 | 2015-11-10 | Novatel Inc. | Navigation system with rapid GNSS and inertial initialization |
-
2019
- 2019-06-03 CN CN201910477554.3A patent/CN110095135B/zh active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102997916A (zh) * | 2011-09-15 | 2013-03-27 | 北京自动化控制设备研究所 | 一种自主提高定位定向系统惯性姿态解算精度的方法 |
CN105547288A (zh) * | 2015-12-08 | 2016-05-04 | 华中科技大学 | 一种煤矿井下移动设备自主定位的方法及系统 |
CN109631938A (zh) * | 2018-12-28 | 2019-04-16 | 湖南海迅自动化技术有限公司 | 掘进机自主定位定向系统及方法 |
Also Published As
Publication number | Publication date |
---|---|
CN110095135A (zh) | 2019-08-06 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109813311B (zh) | 一种无人机编队协同导航方法 | |
CN110411444B (zh) | 一种地面下采掘移动设备用惯性导航定位系统与定位方法 | |
CN107588769B (zh) | 一种车载捷联惯导、里程计及高程计组合导航方法 | |
US9026263B2 (en) | Automotive navigation system and method to utilize internal geometry of sensor position with respect to rear wheel axis | |
US5331578A (en) | Procedure for measuring angles and trajectories by means of gyros and inertial systems | |
CN110095135B (zh) | 一种用于掘进机定位定向的方法及装置 | |
CN100587641C (zh) | 一种适用于任意运动微小型系统的定姿系统 | |
CN111637888B (zh) | 基于惯导和激光雷达单点测距的掘进机定位方法及系统 | |
AU2016101951A4 (en) | Navigation of mining machines | |
CN201955092U (zh) | 一种基于地磁辅助的平台式惯性导航装置 | |
CN1322311C (zh) | 车载快速定位定向系统 | |
CN103674034B (zh) | 多波束测速测距修正的鲁棒导航方法 | |
CN103389092B (zh) | 一种系留飞艇姿态测量装置及测量方法 | |
CN109540130A (zh) | 一种连采机惯性导航定位定向方法 | |
CN111307133B (zh) | 一种竖井垂直度测量方法、竖井姿态检测设备及存储介质 | |
CN112378399B (zh) | 基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法 | |
CN112414394A (zh) | 一种地下巷道掘进装备实时定位系统及方法 | |
CN104515527A (zh) | 一种无gps信号环境下的抗粗差组合导航方法 | |
CN114739425A (zh) | 基于rtk-gnss及全站仪的采煤机定位标定系统及应用方法 | |
CN103712622A (zh) | 基于惯性测量单元旋转的陀螺漂移估计补偿方法及装置 | |
CN110702109A (zh) | 一种采煤机惯性导航/无线传感器网络组合定位方法 | |
CN108868772A (zh) | 一种连采机快速准直控制方法 | |
CN105841698A (zh) | 一种无需调零的auv舵角精确实时测量系统 | |
CN113075650A (zh) | 一种基于uwb与惯性单元的地下巷道掘进装备实时定位方法 | |
CN103712621A (zh) | 偏振光及红外传感器辅助惯导系统定姿方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |