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

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

Info

Publication number
CN112378399B
CN112378399B CN202010690934.8A CN202010690934A CN112378399B CN 112378399 B CN112378399 B CN 112378399B CN 202010690934 A CN202010690934 A CN 202010690934A CN 112378399 B CN112378399 B CN 112378399B
Authority
CN
China
Prior art keywords
inertial navigation
total station
error
strapdown inertial
digital total
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
Application number
CN202010690934.8A
Other languages
English (en)
Other versions
CN112378399A (zh
Inventor
马宏伟
毛金根
贺媛
毛清华
李磊
张旭辉
杜昱阳
张超
刘博兴
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenzhen Saiao Aviation Technology Co ltd
Xian University of Science and Technology
Original Assignee
Shenzhen Saiao Aviation Technology Co ltd
Xian University of Science and Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Shenzhen Saiao Aviation Technology Co ltd, Xian University of Science and Technology filed Critical Shenzhen Saiao Aviation Technology Co ltd
Priority to CN202010690934.8A priority Critical patent/CN112378399B/zh
Publication of CN112378399A publication Critical patent/CN112378399A/zh
Application granted granted Critical
Publication of CN112378399B publication Critical patent/CN112378399B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12Navigation; 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/16Navigation; 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
    • G01C21/165Navigation; 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 combined with non-inertial navigation instruments
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

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

Description

基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位 定向方法
技术领域
本发明涉及煤矿开采技术领域,具体涉及一种基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法。
背景技术
煤炭作为我国的主体能源之一,其所占据的地位在相当长的时间不会发生变化。虽然随着我国甚至整个世界煤炭存储量的不断减少,各个国家越发在意一次能源的开发与利用,但我国目前煤炭地下气化或是其他技术还是比较薄弱,而且某些行业依旧依赖于煤炭。因此在未来的一段时间内,煤炭的开采依旧会不断进行。随着煤矿开采深度的提高,煤矿开采越困难,危险系数越高,煤矿的无人化生产就显得极为重要。煤矿井下掘进机机器人的位姿检测是综掘工作面无人化生产的必要前提。目前对煤矿井下掘进机器人的位姿检测方法不足之处体现在:①利用激光指向器引导煤矿井下掘进机器人定向掘进,这种方式无法获得掘进机器人在巷道的准确的位姿状态,无法为掘进控制提供位姿信息②单纯利用全站仪对掘进机的位姿进行检测时,由于粉尘等原因无法实现实时得到掘进机器人的位置信息,并且无法检测航向角。
因为煤矿井下不存在GPS信号,而且煤矿底下环境恶劣,利用捷联惯导对煤矿井下掘进机器人的位姿进行检测是一种好的方法。捷联惯导具有不依赖外部信息也不向外部辐射能量,所以隐蔽性好且不受外界干扰,可全天候工作于地面、水下和空中各种环境等优点,但是单纯利用捷联惯导去检测掘进机的位姿并不准确,随着时间的推移,捷联惯导的检测误差会不断累积。
发明内容
为解决上述问题,本发明提供了一种基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法,利用捷联惯导和数字式全站仪的优势,实现了对掘进机的精准定位、定向。
为实现上述目的,本发明采取的技术方案为:
一种基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法,根据数字全站仪数据和捷联惯导系统数据融合后的数据拟合出数字式全站仪与捷联惯导系统测量出的位姿曲线,利用数字式全站仪测出的数据来校正捷联惯导系统的位姿,实现掘进工作面的实时、精准的定位定向。
进一步地,该方法包括以下步骤:
S1、在煤矿巷道掘进机器人上布置数字式全站仪配套的棱镜和捷联惯导系统,并将全站仪布放在掘进机器人后方的巷道内;
S2、建立载体坐标系(b系)与导航坐标系(n系),并构建载体坐标系(b系)与导航坐标系(n系)的转换矩阵:
S201、以掘进机器人重心为坐标原点,掘进机器人前进的方向作为Y轴正方向,以向上为Z轴正方向,垂直Y轴向右方向为X轴正方向,建立载体坐标系0XYZ(b系);
S202、根据煤矿井下的数字式全站仪参考点(当地经纬度)测定惯导所在位置,从而确定“东-北-天”导航坐标系O1X1Y1Z1(n系);
S203、掘进机器人在导航坐标系的位姿转换到载体坐标系的转换矩阵由三个欧拉角-航向角(ψ),横滚角(φ),俯仰角(θ)组成,转换矩阵如下:
Figure BDA0002587860150000021
通过转换矩阵,得出掘进机器人在载体坐标系的位姿;
S3、将捷联惯导系统测出的数据和数字全站仪测出的数据传入计算机,完成数据的融合滤波处理,具体的:
S301、建立捷联惯导和数字全站仪的系统状态方程
由于棱镜与捷联惯导系统的位置不在同一位置,需考虑外部的杆臂误差,将杆臂长度扩展到系统的状态量中,得到下面的系统状态矢量:
Figure BDA0002587860150000031
式中,X为状态向量,
Figure BDA0002587860150000032
为捷联惯导的数学平台误差角,
Figure BDA0002587860150000033
分别为捷联惯导三个坐标轴的数学平台误差角;δvn为载体的速度误差,
Figure BDA0002587860150000034
分别为载体在地理坐标系下的速度误差;δpn为载体的位置误差,δLδλδh分别为经度、纬度、高度误差;εb为陀螺仪的漂移误差,
Figure BDA0002587860150000035
εb z分别为陀螺仪三轴的漂移误差;
Figure BDA0002587860150000036
为加速度计的漂移误差,
Figure BDA0002587860150000037
分别为加速度计三轴的漂移误差;r为杆臂长度,rx,ry,rz为三个方向的杆臂分量;
将捷联惯导的测量值作为状态方程的输入变量,状态方程如下:
Figure BDA0002587860150000038
式中,
Figure BDA0002587860150000039
为地理系相对于惯性系的角速率误差,
Figure BDA00025878601500000310
为地理系相对于惯性系的角速率,
Figure BDA00025878601500000311
为地理系与载体系的坐标转换矩阵,εb为地理系角速率误差,fn为地理坐标系下加速度计的输出比力,
Figure BDA00025878601500000312
为地球系相对于惯性系的角速率误差,
Figure BDA00025878601500000313
为地理坐标系相对于地球系的角速率误差,
Figure BDA00025878601500000314
为地球系相对于惯性系的角速率,
Figure BDA00025878601500000315
为地理坐标系相对于地球系的角速率,δvn为载体的速度误差,δgn为重力加速度误差,
Figure BDA00025878601500000316
为载体在地理坐标系下y轴的速度,Rm为卯酉圈半径,Rn为子午圈半径,h为当地海拔高度,
Figure BDA0002587860150000041
载体在地理坐标系下x轴的速度误差,
Figure BDA0002587860150000042
为载体在地理坐标系下x轴的速度,
Figure BDA0002587860150000043
载体在地理坐标系下z轴的速度误差,L为当地经度、λ为当地纬度,Tc为白噪声,
Figure BDA0002587860150000044
为加速度计的漂移误差,wc为白噪声;
步骤302、建立捷联惯导和数字全站仪的系统量测方程
数字全站仪输出位置表示如下:
Figure BDA0002587860150000045
式中:x0,y0,z0表示测站点的N、E、Z坐标(已知);D为斜距;a为目标点与测站点连线与z轴的夹角,b为方位角,ih为仪器高;th为目标高;
数字全站仪的刻度系数误差ζ,方位角误差δb为数字全站仪的输出误差δP的主要因素,则
Figure BDA0002587860150000046
选取捷联惯导输出的位置与数字全站仪的位置的差值作为量测值,设惯导解算的位置和数字全站仪的位置分别为x惯导、y惯导、x全站仪、y全站仪,则捷联惯导与数字全站仪的组合量测方程式为:
Z(t)=H(t)X(t)+V(t);
式中:
Figure BDA0002587860150000047
H(t)=[02×6 I2×2 I2×3 02×3 H2×3]
V(t)为量测噪声矩阵;
由于存在杆臂误差,增加的矩阵为H2×3
Figure BDA0002587860150000048
式中:nx,ny为杆臂长度;V=[-nx -ny]T
步骤303、建立捷联惯导和数字全站仪无迹卡尔曼滤波方程
1)计算2n+1个Sigma(采样)点和这些采样点的权值(UT变换),公式如下:
Figure BDA0002587860150000051
式中,
Figure BDA0002587860150000052
表示矩阵方根第i列,下标m为均值,c为协方差,上标为第几个采样点,参数λ=a2(n+k)-n表示缩放比例参数,a的选取控制了采样点的分布。
Figure BDA0002587860150000053
为第i个点权值,i=1~2n;
2)获得一组采样点(sigma点)及其对应权值
Figure BDA0002587860150000054
式中:X(i)(k/k)为第i个采样点,X(k/k)为第k时刻的系统状态,P(k/k)为第k时刻的系统协方差。
3)计算2n+1个Sigma点集的一步预测:
X(i)(k+1/k)=f[k,X(i)(k/k)],i=1,2,...,2n+1;
式中:X(i)(k+1/k)为sigma点集合,f为非线性函数。
4)计算系统状态量的一步预测及协方差矩阵
Figure BDA0002587860150000055
Figure BDA0002587860150000056
式中:
Figure BDA0002587860150000061
为第k+1时刻的系统状态预测,P(k+1/k)为第k+1时刻的系统协方差。Q为系统噪声矩阵。
5)根据一步预测值,再次应用UT变换,产生新的Sigma点:
Figure BDA0002587860150000062
6)由上述5)得到的新Sigma点集带入观测方程,得到预测观测量:
Z(i)(k+1/k)=h[X(i)(k+1/k)],i=1,2,3,...,2n+1;
式中:Z(i)(k+1/k)为第i个采样点的量测值,h为量测方程系数阵。
7)由6)得到的观测预测值,并加权得到系统预测的均值及方差;
Figure BDA0002587860150000063
Figure BDA0002587860150000064
Figure BDA0002587860150000065
式中:
Figure BDA0002587860150000066
为观测量的预测均值,
Figure BDA0002587860150000067
为系统的系统的观测方差,R为观测噪声矩阵,
Figure BDA0002587860150000068
为组合系统的方差。
8)计算卡尔曼增益矩阵,计算系统的状态更新和协方差
Figure BDA0002587860150000069
Figure BDA00025878601500000610
式中: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、根据煤矿井下的数字式全站仪参考点(当地经纬度)测定惯导所在位置,从而确定“东-北-天”导航坐标系O1X1Y1Z1(n系);
S203、掘进机器人在导航坐标系的位姿转换到载体坐标系的转换矩阵由三个欧拉角-航向角(ψ),横滚角(φ),俯仰角(θ)组成,转换矩阵如下:
Figure BDA0002587860150000081
通过转换矩阵,得出掘进机器人在载体坐标系的位姿;
S3、将捷联惯导系统测出的数据和数字全站仪测出的数据传入计算机,完成数据的融合滤波处理,具体的:
S301、建立捷联惯导和数字全站仪的系统状态方程
由于棱镜与捷联惯导系统的位置不在同一位置,需考虑外部的杆臂误差,将杆臂长度扩展到系统的状态量中,得到下面的系统状态矢量:
Figure BDA0002587860150000082
式中,X为状态向量,
Figure BDA0002587860150000083
为捷联惯导的数学平台误差角,
Figure BDA0002587860150000084
分别为捷联惯导三个坐标轴的数学平台误差角;δvn为载体的速度误差,
Figure BDA0002587860150000091
分别为载体在地理坐标系下的速度误差;δpn为载体的位置误差,δLδλδh分别为经度、纬度、高度误差;εb为陀螺仪的漂移误差,
Figure BDA0002587860150000092
εb z分别为陀螺仪三轴的漂移误差;
Figure BDA0002587860150000093
为加速度计的漂移误差,
Figure BDA0002587860150000094
分别为加速度计三轴的漂移误差;r为杆臂长度,rx,ry,rz为三个方向的杆臂分量;
将捷联惯导的测量值作为状态方程的输入变量,状态方程如下:
Figure BDA0002587860150000095
式中,
Figure BDA0002587860150000096
为地理系相对于惯性系的角速率误差,
Figure BDA0002587860150000097
为地理系相对于惯性系的角速率,
Figure BDA0002587860150000098
为地理系与载体系的坐标转换矩阵,εb为地理系角速率误差,fn为地理坐标系下加速度计的输出比力,
Figure BDA0002587860150000099
为地球系相对于惯性系的角速率误差,
Figure BDA00025878601500000910
为地理坐标系相对于地球系的角速率误差,
Figure BDA00025878601500000911
为地球系相对于惯性系的角速率,
Figure BDA00025878601500000912
为地理坐标系相对于地球系的角速率,δνn为载体的速度误差,δgn为重力加速度误差,
Figure BDA00025878601500000913
为载体在地理坐标系下y轴的速度,Rm为卯酉圈半径,Rn为子午圈半径,h为当地海拔高度,
Figure BDA00025878601500000914
载体在地理坐标系下x轴的速度误差,
Figure BDA00025878601500000915
为载体在地理坐标系下x轴的速度,
Figure BDA00025878601500000916
载体在地理坐标系下z轴的速度误差,L为当地经度、λ为当地纬度,Tc为白噪声,
Figure BDA00025878601500000917
为加速度计的漂移误差,wc为白噪声;
步骤302、建立捷联惯导和数字全站仪的系统量测方程
数字全站仪输出位置表示如下:
Figure BDA0002587860150000101
式中:x0,y0,z0表示测站点的N、E、Z坐标(已知);D为斜距;a为目标点与测站点连线与z轴的夹角,b为方位角,ih为仪器高;th为目标高;
数字全站仪的刻度系数误差ζ,方位角误差δb为数字全站仪的输出误差δP的主要因素,则
Figure BDA0002587860150000102
选取捷联惯导输出的位置与数字全站仪的位置的差值作为量测值,设惯导解算的位置和数字全站仪的位置分别为x惯导、y惯导、x全站仪、y全站仪,则捷联惯导与数字全站仪的组合量测方程式为:
Z(t)=H(t)X(t)+V(t);
式中:
Figure BDA0002587860150000103
H(t)=[02×6 I2×2 I2×3 02×3 H2×3]
V(t)为量测噪声矩阵;
由于存在杆臂误差,增加的矩阵为H2×3
Figure BDA0002587860150000104
式中:nx,ny为杆臂长度;V=[-nx -ny]T
步骤303、建立捷联惯导和数字全站仪无迹卡尔曼滤波方程
1)计算2n+1个Sigma(采样)点和这些采样点的权值(UT变换),公式如下:
Figure BDA0002587860150000111
式中,
Figure BDA0002587860150000112
表示矩阵方根第i列,下标m为均值,c为协方差,上标为第几个采样点,参数λ=a2(n+k)-n表示缩放比例参数,a的选取控制了采样点的分布。
Figure BDA0002587860150000113
为第i个点权值,i=1~2n;
2)获得一组采样点(sigma点)及其对应权值
Figure BDA0002587860150000114
式中:X(i)(k/k)为第i个采样点,X(k/k)为第k时刻的系统状态,P(k/k)为第k时刻的系统协方差。
3)计算2n+1个Sigma点集的一步预测:
X(i)(k+1/k)=f[k,X(i)(k/k)],i=1,2,...,2n+1;
式中:X(i)(k+1/k)为sigma点集合,f为非线性函数。
4)计算系统状态量的一步预测及协方差矩阵
Figure BDA0002587860150000115
Figure BDA0002587860150000116
式中:
Figure BDA0002587860150000117
为第k+1时刻的系统状态预测,P(k+1/k)为第k+1时刻的系统协方差。Q为系统噪声矩阵。
5)根据一步预测值,再次应用UT变换,产生新的Sigma点:
Figure BDA0002587860150000121
6)由上述5)得到的新Sigma点集带入观测方程,得到预测观测量:
Z(i)(k+1/k)=h[X(i)(k+1/k)],i=1,2,3,...,2n+1;
式中:Z(i)(k+1/k)为第i个采样点的量测值,h为量测方程系数阵。
7)由6)得到的观测预测值,并加权得到系统预测的均值及方差;
Figure BDA0002587860150000122
Figure BDA0002587860150000123
Figure BDA0002587860150000124
式中:
Figure BDA0002587860150000125
为观测量的预测均值,
Figure BDA0002587860150000126
为系统的系统的观测方差,R为观测噪声矩阵,
Figure BDA0002587860150000127
为组合系统的方差。
8)计算卡尔曼增益矩阵,计算系统的状态更新和协方差
Figure BDA0002587860150000128
Figure BDA0002587860150000129
式中:K(k+1)为卡尔曼增益,其余参数见步骤1)-7)。
S4、根据数字全站仪数据和捷联惯导系统数据融合后的数据拟合出数字式全站仪与捷联惯导系统测量出的位姿曲线,利用数字式全站仪测出的数据来校正捷联惯导系统的位姿,实现掘进工作面的实时、精准的定位定向。
以上对本发明的具体实施例进行了描述。需要理解的是,本发明并不局限于上述特定实施方式,本领域技术人员可以在权利要求的范围内做出各种变化或修改,这并不影响本发明的实质内容。在不冲突的情况下,本申请的实施例和实施例中的特征可以任意相互组合。

Claims (5)

1.一种基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法,其特征在于,根据数字全站仪数据和捷联惯导系统数据融合后的数据拟合出数字式全站仪与捷联惯导系统测量出的位姿曲线,利用数字式全站仪测出的数据来校正捷联惯导系统的位姿,实现掘进工作面的实时、精准的定位定向;
该方法包括以下步骤:
S1、在煤矿巷道掘进机器人上布置数字式全站仪配套的棱镜和捷联惯导系统,并将全站仪布放在掘进机器人后方的巷道内;
S2、建立载体坐标系与导航坐标系,并构建载体坐标系与导航坐标系的转换矩阵:
S201、以掘进机器人重心为坐标原点,掘进机器人前进的方向作为Y轴正方向,以向上为Z轴正方向,垂直Y轴向右方向为X轴正方向,建立载体坐标系OXYZ;
S202、根据煤矿井下的数字式全站仪参考点测定惯导所在位置,从而确定“东-北-天”导航坐标系O1X1Y1Z1
S203、掘进机器人在地理坐标系的位姿转换到载体坐标系的转换矩阵由三个欧拉角-航向角ψ,横滚角φ,俯仰角θ组成,转换矩阵如下:
Figure FDA0003973254110000011
通过转换矩阵,得出掘进机器人在载体坐标系的位姿;
S3、将捷联惯导系统测出的数据和数字全站仪测出的数据传入计算机,完成数据的融合滤波处理,具体的:
S301、建立捷联惯导和数字全站仪的系统状态方程
由于棱镜与捷联惯导系统的位置不在同一位置,需考虑外部的杆臂误差,将杆臂长度扩展到系统的状态量中,得到下面的系统状态矢量:
Figure FDA0003973254110000021
式中,X为状态向量,
Figure FDA0003973254110000022
为捷联惯导的数学平台误差角,
Figure FDA0003973254110000023
分别为捷联惯导三个坐标轴的数学平台误差角;δvn为载体的速度误差,
Figure FDA0003973254110000024
分别为载体在地理坐标系下的速度误差;δpn为载体的位置误差,δL δλ δh分别为经度、纬度、高度误差;εb为陀螺仪的漂移误差,
Figure FDA0003973254110000025
分别为陀螺仪三轴的漂移误差;
Figure FDA0003973254110000026
为加速度计的漂移误差,
Figure FDA0003973254110000027
分别为加速度计三轴的漂移误差;r为杆臂长度,rx,ry,rz为三个方向的杆臂分量;
将捷联惯导的测量值作为状态方程的输入变量,状态方程如下:
Figure FDA0003973254110000028
式中,
Figure FDA0003973254110000029
为地理系相对于惯性系的角速率误差,
Figure FDA00039732541100000210
为地理系相对于惯性系的角速率,
Figure FDA00039732541100000211
为地理系与载体系的坐标转换矩阵,εb为地理系角速率误差,fn为地理坐标系下加速度计的输出比力,
Figure FDA00039732541100000212
为地球系相对于惯性系的角速率误差,
Figure FDA00039732541100000213
为地理坐标系相对于地球系的角速率误差,
Figure FDA00039732541100000214
为地球系相对于惯性系的角速率,
Figure FDA00039732541100000215
为地理坐标系相对于地球系的角速率,δvn为载体的速度误差,δgn为重力加速度误差,
Figure FDA00039732541100000216
为载体在地理坐标系下y轴的速度,Rm为卯酉圈半径,Rn为子午圈半径,h为当地海拔高度,
Figure FDA00039732541100000217
载体在地理坐标系下x轴的速度误差,
Figure FDA0003973254110000031
为载体在地理坐标系下x轴的速度,
Figure FDA0003973254110000032
载体在地理坐标系下z轴的速度误差,L为当地经度、λ为当地纬度,Tc为白噪声,
Figure FDA0003973254110000033
为加速度计的漂移误差,wc为白噪声;
S302、建立捷联惯导和数字全站仪的系统量测方程
数字全站仪输出位置表示如下:
Figure FDA0003973254110000034
式中:x0,y0,z0表示测站点的N、E、Z坐标;D为斜距;a为目标点与测站点连线与z轴的夹角,b为方位角,ih为仪器高;th为目标高;
数字全站仪的刻度系数误差ζ,方位角误差δb为数字全站仪的输出误差δP的主要因素,则
Figure FDA0003973254110000035
选取捷联惯导输出的位置与数字全站仪的位置的差值作为量测值,设惯导解算的位置和数字全站仪的位置分别为x惯导、y惯导、x全站仪、y全站仪,则捷联惯导与数字全站仪的组合量测方程式为:
Z(t)=H(t)X(t)+V(t);
式中:
Figure FDA0003973254110000036
H(t)=[02×6 I2×2 I2×3 02×3 H2×3]
增加的矩阵为杆臂误差矩阵H2×3
Figure FDA0003973254110000037
nx,ny为杆臂长度;V=[-nx -ny]T
步骤303、建立捷联惯导和数字全站仪无迹卡尔曼滤波方程
1)计算2n+1个Sigma点和这些采样点的权值,公式如下:
Figure FDA0003973254110000041
式中,
Figure FDA0003973254110000042
表示矩阵方根第i列,下标m为均值,c为协方差,上标为第几个采样点,参数λ=a2(n+k)-n表示缩放比例参数,a的选取控制了采样点的分布;
Figure FDA0003973254110000043
为第i个点权值,i=1~2n;
2)获得一组采样点及其对应权值
Figure FDA0003973254110000044
式中:X(i)(k/k)为第i个采样点,X(k/k)为第k时刻的系统状态,P(k/k)为第k时刻的系统协方差;
3)计算2n+1个Sigma点集的一步预测:
X(i)(k+1/k)=f[k,X(i)(k/k)],i=1,2,...,2n+1;
式中:X(i)(k+1/k)为sigma点集合,f为非线性函数;
4)计算系统状态量的一步预测及协方差矩阵
Figure FDA0003973254110000045
Figure FDA0003973254110000046
式中:
Figure FDA0003973254110000047
为第k+1时刻的系统状态预测,P(k+1/k)为第k+1时刻的系统协方差;Q为系统噪声矩阵;
5)根据一步预测值,再次应用UT变换,产生新的Sigma点:
Figure FDA0003973254110000051
6)由上述5)得到的新Sigma点集带入观测方程,得到预测观测量:
Z(i)(k+1/k)=h[X(i)(k+1/k)],i=1,2,3,...,2n+1;
式中:Z(i)(k+1/k)为第i个采样点的量测值,h为量测方程系数阵;
7)由6)得到的观测预测值,并加权得到系统预测的均值及方差;
Figure FDA0003973254110000052
Figure FDA0003973254110000053
Figure FDA0003973254110000054
式中:
Figure FDA0003973254110000059
为观测量的预测均值,
Figure FDA00039732541100000510
为系统的系统的观测方差,R为观测噪声矩阵,
Figure FDA0003973254110000056
为组合系统的方差;
8)计算卡尔曼增益矩阵,计算系统的状态更新和协方差
Figure FDA0003973254110000057
Figure FDA0003973254110000058
式中:K(k+1)为卡尔曼增益,其余参数见步骤1)-7);
S4、根据数字全站仪数据和捷联惯导系统数据融合后的数据拟合出数字式全站仪与捷联惯导系统测量出的位姿曲线,利用数字式全站仪测出的数据来校正捷联惯导系统的位姿,实现掘进工作面的实时、精准的定位定向。
2.如权利要求1所述的基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法,其特征在于:通过数字全站仪检测的位置信息实时校正捷联惯导系统累积误差。
3.如权利要求1所述的基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法,其特征在于:采用高精度捷联惯导系统,该系统具有零速自动校正功能,自动修正惯导航向。
4.如权利要求1所述的基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法,其特征在于:利用无迹卡尔曼滤波方法实现捷联惯导系统和数字全站仪数据的融合。
5.如权利要求1所述的基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法,其特征在于:考虑棱镜与捷联惯导系统的布放位置不同产生的杆臂误差,并通过无迹卡尔曼滤波方法减少这种误差,提高融合精度。
CN202010690934.8A 2020-07-16 2020-07-16 基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法 Active CN112378399B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010690934.8A CN112378399B (zh) 2020-07-16 2020-07-16 基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010690934.8A CN112378399B (zh) 2020-07-16 2020-07-16 基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法

Publications (2)

Publication Number Publication Date
CN112378399A CN112378399A (zh) 2021-02-19
CN112378399B true CN112378399B (zh) 2023-02-28

Family

ID=74586351

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010690934.8A Active CN112378399B (zh) 2020-07-16 2020-07-16 基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法

Country Status (1)

Country Link
CN (1) CN112378399B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113075650A (zh) * 2021-04-02 2021-07-06 中国铁建重工集团股份有限公司 一种基于uwb与惯性单元的地下巷道掘进装备实时定位方法
CN114440876B (zh) * 2022-01-21 2024-04-02 北京自动化控制设备研究所 一种井下掘进机定位导向方法及井下掘进机定位导向系统
CN115900692A (zh) * 2022-08-25 2023-04-04 中铁隧道局集团有限公司 一种多传感器融合高精度隧道定位定姿方法
CN115855116B (zh) * 2023-02-08 2023-05-30 中国船舶集团有限公司第七〇七研究所 误差标定工艺生成方法及系统
CN116182743A (zh) * 2023-04-27 2023-05-30 太原理工大学 基于激光雷达的煤矿井下掘进巷道轮廓标示系统

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103344259A (zh) * 2013-07-11 2013-10-09 北京航空航天大学 一种基于杆臂估计的ins/gps组合导航系统反馈校正方法
CN107238385A (zh) * 2017-05-31 2017-10-10 中国矿业大学 一种采煤机绝对位姿检测方法
CN108775899A (zh) * 2018-05-31 2018-11-09 中国矿业大学 基于伪卫星和惯性信息的深部开采井上下坐标系连接方法
CN110631573A (zh) * 2018-06-22 2019-12-31 北京自动化控制设备研究所 一种惯性/里程计/全站仪多信息融合方法
CN111044042A (zh) * 2020-01-03 2020-04-21 中国船舶重工集团公司第七0七研究所 基于陀螺全站仪和惯导设备的掘进机定位导航系统及方法
CN111307133A (zh) * 2020-03-04 2020-06-19 中国铁建重工集团股份有限公司 一种竖井垂直度测量方法、竖井姿态检测设备及存储介质
CN111412911A (zh) * 2020-04-07 2020-07-14 中国煤炭科工集团太原研究院有限公司 一种煤矿井下连续采煤机器人多传感器组合导航系统

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20170234129A1 (en) * 2016-02-11 2017-08-17 Eagle Harbor Holdings, Llc System and method for real-time guidance and mapping of a tunnel boring machine and tunnel

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103344259A (zh) * 2013-07-11 2013-10-09 北京航空航天大学 一种基于杆臂估计的ins/gps组合导航系统反馈校正方法
CN107238385A (zh) * 2017-05-31 2017-10-10 中国矿业大学 一种采煤机绝对位姿检测方法
CN108775899A (zh) * 2018-05-31 2018-11-09 中国矿业大学 基于伪卫星和惯性信息的深部开采井上下坐标系连接方法
CN110631573A (zh) * 2018-06-22 2019-12-31 北京自动化控制设备研究所 一种惯性/里程计/全站仪多信息融合方法
CN111044042A (zh) * 2020-01-03 2020-04-21 中国船舶重工集团公司第七0七研究所 基于陀螺全站仪和惯导设备的掘进机定位导航系统及方法
CN111307133A (zh) * 2020-03-04 2020-06-19 中国铁建重工集团股份有限公司 一种竖井垂直度测量方法、竖井姿态检测设备及存储介质
CN111412911A (zh) * 2020-04-07 2020-07-14 中国煤炭科工集团太原研究院有限公司 一种煤矿井下连续采煤机器人多传感器组合导航系统

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于四点式光靶的掘进机自动定位方法研究;田原;《煤炭科学技术》;20181231;第46卷(第12期);第35-40页 *

Also Published As

Publication number Publication date
CN112378399A (zh) 2021-02-19

Similar Documents

Publication Publication Date Title
CN112378399B (zh) 基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法
CN103674030B (zh) 基于天文姿态基准保持的垂线偏差动态测量装置和方法
CN107270893B (zh) 面向不动产测量的杆臂、时间不同步误差估计与补偿方法
CN101788296B (zh) 一种sins/cns深组合导航系统及其实现方法
CN101608920B (zh) 一种组合式空间位姿精密动态测量装置及方法
CN104006787B (zh) 空间飞行器姿态运动模拟平台高精度姿态确定方法
CN102519470B (zh) 多级嵌入式组合导航系统及导航方法
CN104880192B (zh) 一种基于偏振罗盘的载体航向角计算方法
CN111207744B (zh) 一种基于厚尾鲁棒滤波的管线地理位置信息测量方法
CN105091907B (zh) Sins/dvl组合中dvl方位安装误差估计方法
CN111637888B (zh) 基于惯导和激光雷达单点测距的掘进机定位方法及系统
CN113236363B (zh) 开采设备导航定位方法、系统、设备及可读存储介质
CN114739425A (zh) 基于rtk-gnss及全站仪的采煤机定位标定系统及应用方法
CN110631573B (zh) 一种惯性/里程计/全站仪多信息融合方法
CN101246012A (zh) 一种基于鲁棒耗散滤波的组合导航方法
CN102645223A (zh) 一种基于比力观测的捷联惯导真空滤波修正方法
Han et al. An orientation navigation approach based on INS and odometer integration for underground unmanned excavating machine
CN109612460A (zh) 一种基于静止修正的垂线偏差测量方法
CN111207773B (zh) 一种用于仿生偏振光导航的姿态无约束优化求解方法
CN104501809B (zh) 一种基于姿态耦合的捷联惯导/星敏感器组合导航方法
CN102183263A (zh) 一种光纤陀螺常值漂移的标定方法
CN115079287B (zh) 陀螺组件姿态观测的捷联式车载动态重力矢量测量方法
CN109945857A (zh) 一种面向不动产实地测量的车载惯性定位方法及其装置
CN115166856A (zh) 一种无人船重磁测量方法、系统、设备及计算机可读存储介质
CN113324539A (zh) 一种sins/srs/cns多源融合自主组合导航方法

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