【技术实现步骤摘要】
基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法
[0001]本专利技术涉及煤矿开采
，具体涉及一种基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法。
技术介绍
[0002]煤炭作为我国的主体能源之一，其所占据的地位在相当长的时间不会发生变化。虽然随着我国甚至整个世界煤炭存储量的不断减少，各个国家越发在意一次能源的开发与利用，但我国目前煤炭地下气化或是其他技术还是比较薄弱，而且某些行业依旧依赖于煤炭。因此在未来的一段时间内，煤炭的开采依旧会不断进行。随着煤矿开采深度的提高，煤矿开采越困难，危险系数越高，煤矿的无人化生产就显得极为重要。煤矿井下掘进机机器人的位姿检测是综掘工作面无人化生产的必要前提。目前对煤矿井下掘进机器人的位姿检测方法不足之处体现在：
①
利用激光指向器引导煤矿井下掘进机器人定向掘进，这种方式无法获得掘进机器人在巷道的准确的位姿状态，无法为掘进控制提供位姿信息
②
单纯利用全站仪对掘进机的位姿进行检测时，由于粉尘等原因无法实现实时得到掘进机器人的位...
【技术保护点】
【技术特征摘要】
1.一种基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法，其特征在于，根据数字全站仪数据和捷联惯导系统数据融合后的数据拟合出数字式全站仪与捷联惯导系统测量出的位姿曲线，利用数字式全站仪测出的数据来校正捷联惯导系统的位姿，实现掘进工作面的实时、精准的定位定向。2.如权利要求1所述的一种基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法，其特征在于，该方法包括以下步骤：S1、在煤矿巷道掘进机器人上布置数字式全站仪配套的棱镜和捷联惯导系统，并将全站仪布放在掘进机器人后方的巷道内；S2、建立载体坐标系与导航坐标系，并构建载体坐标系与导航坐标系的转换矩阵：S201、以掘进机器人重心为坐标原点，掘进机器人前进的方向作为Y轴正方向，以向上为Z轴正方向，垂直Y轴向右方向为X轴正方向，建立载体坐标系OXYZ；S202、根据煤矿井下的数字式全站仪参考点测定惯导所在位置，从而确定“东-北-天”导航坐标系O1X1Y1Z1；S203、掘进机器人在导航坐标系的位姿转换到载体坐标系的转换矩阵由三个欧拉角-航向角(ψ)，横滚角(φ)，俯仰角(θ)组成，转换矩阵如下：通过转换矩阵，得出掘进机器人在载体坐标系的位姿；S3、将捷联惯导系统测出的数据和数字全站仪测出的数据传入计算机，完成数据的融合滤波处理，具体的：S301、建立捷联惯导和数字全站仪的系统状态方程由于棱镜与捷联惯导系统的位置不在同一位置，需考虑外部的杆臂误差，将杆臂长度扩展到系统的状态量中，得到下面的系统状态矢量：式中，X为状态向量，为捷联惯导的数学平台误差角，分别为捷联惯导三个坐标轴的数学平台误差角；δv
n
为载体的速度误差，分别为载体在地理坐标系下的速度误差；δp
n
为载体的位置误差，δL δλ δh分别为经度、纬度、高度误差；ε
b
为陀螺仪的漂移误差，分别为陀螺仪三轴的漂移误差；为加速度计的漂移误差，分别为加速度计三轴的漂移误差；r为杆臂长度，r
x
，r
y
，r
z
为三个方向的杆臂分量；将捷联惯导的测量值作为状态方程的输入变量，状态方程如下：
式中，为地理系相对于惯性系的角速率误差，为地理系相对于惯性系的角速率，为地理系与载体系的坐标转换矩阵，ε
b
为地理系角速率误差，f
n
为地理坐标系下加速度计的输出比力，为地球系相对于惯性系的角速率误差，为地理坐标系相对于地球系的角速率误差，为地球系相对于惯性系的角速率，为地理坐标系相对于地球系的角速率，δv
n
为载体的速度误差，δg
n
为重力加速度误差，为载体在地理坐标系下y轴的速度，R
m
为卯酉圈半径，R
n
为子午圈半径，h为当地海拔高度，载体在地理坐标系下x轴的速度误差，为载体在地理坐标系下x轴的速度，载体在地理坐标系下z轴的速度误差，L为当地经度、λ为当地纬度，T
c
为白噪声，为加速度计的漂移误差，w
c
为白噪声；S302、建立捷联惯导和数字全站仪的系统量测方程数字全站仪输出位置表示如下：式中：x0，y0，z0表示测站点的N、E、Z坐标(已知)；D为斜距；a为目标点与测站点连线与z轴的夹角，b为方位角，ih为仪器高；th为目标高；数字全站仪的刻度系数误差ζ，方位角误差δb为数字全站仪的输出误差δP的主要因素，则选取捷联惯导输出的位置与数字全站仪的位置的差值作为量测值，设惯导解算的位置和数字全站仪的位置分别为x
惯导
、y
惯导...
【专利技术属性】
技术研发人员：马宏伟，毛金根，贺媛，毛清华，李磊，张旭辉，杜昱阳，张超，刘博兴，
申请(专利权)人：深圳赛奥航空科技有限公司，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。