基于捷联惯导和油缸的智能掘进机器人定位定向方法技术

本发明专利技术涉及煤矿开采技术领域，具体涉及一种基于捷联惯导和油缸的智能掘进机器人定位定向方法，该定位方法基于油缸推移智能掘进机器人前进的位移数据实现捷联惯导解算出的智能掘进机器人的位姿数据的校正，从而抑制惯导解算结果的发散，实现智能掘进机器人高精度的定位定向。本发明专利技术利用油缸推移智能掘进机器人前进的位移数据来补偿捷联惯导的累计误差，克服了纯惯导解算会导致智能掘进机器人定位定向误差随时间发散的缺点；能够适应煤矿井下复杂的坏境，实现智能掘进机器人位姿数据的自主测量。

【技术实现步骤摘要】
基于捷联惯导和油缸的智能掘进机器人定位定向方法
本专利技术涉及煤矿开采
，具体涉及一种基于捷联惯导和油缸的智能掘进机器人定位定向方法。
技术介绍
煤炭作为我国能源重要的组成部分，在未来相当长的一段时间内仍将保持其主体地位。随着科技创新能力的不断提升，煤矿开采智能化水平大大提高，开采效率和安全性得到了保障。在目前煤矿开采过程中，掘进机的精确定位定向是综掘工作面智能化重要的研究方向。现阶段掘进机的掘进方向大多利用激光进行引导，该方法无法对掘进机的姿态信息进行测量，这就导致不能为掘进机掘进方向的自动纠偏提供姿态信息，在煤矿朝着智能化发展的过程中，该方法已经不能适应现在的需求。由于井下环境过于复杂，一些地面上可以使用的位姿测量方法并不适用于井下，而惯导凭借着不依赖外部信息，可全天候工作于地面、地下和空中各种环境的特点，使得其成为煤矿井下设备定位、定向的首选方式。然而纯惯导测量系统会随着时间的增加，其测量误差也会越来越大。
技术实现思路
针对上述问题，本专利技术专利提供了一种基于捷联惯导和油缸的智能掘进机器人定位定向方法，利用油缸数据对捷联惯带数据进行校准，实现智能掘进机器人精准定位定向。为实现上述目的，本专利技术采取的技术方案为：一种基于捷联惯导和油缸的智能掘进机器人定位定向方法，基于油缸推移智能掘进机器人前进的位移数据实现捷联惯导解算出的智能掘进机器人的位姿数据的校正，从而实现智能掘进机器人高精度的定位定向，其中，智能掘进机器人由机器人I和机器人II组成，机器人I和机器人II之间通过油缸进行连接，智能掘进机器人前进时，首先通过油缸将机器人I推送的预定位置，然后在通过油缸将机器人II拉到预定位置，具体的，包括以下步骤：S1、将捷联惯导安装在机器人I上，并建立载体坐标系和导航坐标系；S101、将捷联惯导安装在机器人I上，以智能掘进机器人的重心作为坐标原点，智能掘进机器人前进方向为Y轴正方向，智能掘进机器人右侧垂直于Y轴方向为X轴正方向，垂直智能掘进机器人向上方向作为Z轴正方向，建立载体坐标系OXYZ；S102、以捷联惯导安装所在位置的“东-北-天”坐标系作为导航坐标系O1X1Y1Z1；S103、智能掘进机器人在载体坐标系下，绕Z轴旋转的角度称为航向角，记为绕X轴旋转的角度称为俯仰角，记为θ，绕Y轴旋转的角度称为横滚角，记为γ，则智能掘进机器人从导航坐标系到载体坐标系的姿态矩阵如下：S2、将捷联惯导和油缸构成航位推算系统，并对其中包含的误差进行分析；S201、构建捷联惯导误差模型：式中，φE、φN、φU分别为计算导航坐标系与理想导航坐标系之间的失准角误差，ωE、ωN、ωU分别为陀螺仪的测量值，vE、vN、vU分别为智能掘进机器人在导航坐标系中的速度，分别为智能掘进机器人在导航坐标系中的速度误差，RNh、RMh分别为子午圈半径和卯酉圈半径，fE、fN、fU分别为加速度计测量值，L为当地的纬度.δL、δλ、δh分别为纬度、经度和高度误差，εE、εN、εU分别为陀螺仪零偏误差，分别为加速度计零偏误差，ge为赤道重力，β、β1、β2分别为0.005302、3.08×10-6、8.08×10-9；S202、构建航位推算的位置误差模型：式中，LD、λD、hD分别为航位推算时智能掘进机器人所在位置的纬度、经度和高度，VDE、VDN、VDU分别为航位推算时智能掘进机器人的东向速度、北向速度和天向速度，αθ为捷联惯导和智能掘进机器人之间的安装偏差角，δKD为油缸推移系数误差；S203、构建杆臂误差模型：式中，δl为油缸相对于捷联惯导的位置矢量；S3、通过标准卡尔曼滤波算法，实现捷联惯导和油缸的数据的融合；S301、综合考虑捷联惯导误差、航位推算误差和杆臂误差，建立捷联惯导和油缸的系统状态方程；其中，系统的状态变量为：式中，φT为计算导航坐标系与理想导航坐标系之间的失准角误差，(δv)T为智能掘进机器人的速度误差，(δp)T为智能掘进机器人的位置误差，(δpD)T为航位推算时的位置误差，(δpGL)T为杆臂误差，(εb)T为陀螺仪零偏误差，为加速度计零偏误差，l为捷联惯导和油缸的位置矢量；组合测量系统的状态方程为：其中，和分别为陀螺角速度测量白噪声和加速度计比力测量白噪声；根据S201，S202，S203中的误差模型，状态转移矩阵F中的参数如下所示：S302、建立捷联惯导和油缸的系统量测方程以捷联惯导解算的位置与航位推算的位置之差来构建观测向量，即Z＝δp-δpD则系统的量测方程为Z＝HX+V其中，H＝[03×6I3×3-I3×303×15]，V为测量噪声；S303、建立捷联惯导和油缸标准卡尔曼滤波方程，得到系统状态的最优估计值，其迭代过程可分为以下5步1)通过状态方程和系统上一时刻的状态对当前时刻状态进行一步预测X(k/k-1)＝F·X(k-1)其中X(k-1)为k-1时刻系统的状态量；2)对当前时刻预测状态的误差的均方差阵进行求解P(k/k-1)＝FP(k-1)FT+QP(k-1)为k-1时刻系统状态误差的均方差矩阵，Q为系统噪声矩阵；3)求解卡尔曼增益KKk＝P(k/k-1)HT(HP(k/k-1)HT+R)-1R为量测噪声矩阵4)结合卡尔曼增益K，对当前时刻状态的最优估计值进行更新X(k)＝X(k/k-1)+Kk(Zk-HX(k/k-1))5)对当前时刻状态的最优估计值误差的均方差矩阵进行更新P(k)＝(I-KkH)P(k/k-1)S4、根据油缸推移智能掘进机器人前进的位移数据和捷联惯导数据的融合结果得出智能掘进机器人的位姿曲线，实现掘进工作面精准定位定向。进一步地，智能掘进机器人直线前进时，智能掘进机器人左右两侧油缸推移量相等，此时智能掘进机器人位移量与油缸推移量相等；当智能掘进机器人纠偏时，左右两侧油缸推移量不等，由于捷联惯导与机器人I固连，所以机器人I的位移与捷联惯导的位移等效，而捷联惯导的位移可以用其在机器人I盾体后面的投影位移近似等效，此时智能掘进机器人位移的位移数据如下：式中，a为捷联惯导投影与机器人I盾体左表面的距离，b为捷联惯导投影与机器人I盾体右表面的距离，Ll为智能掘进机器人左侧油缸的推移量，Lr为智能掘进机器人右侧油缸的推移量。进一步地，将捷联惯导解算得到的智能掘进机器人姿态数据和油缸推移智能掘进机器人前进的位移数据结合，构成航位推算系统，并构建航位推算系统的误差模型。进一步地，考虑油缸位置与捷联惯导安装位置本文档来自技高网...

【技术保护点】
1.一种基于捷联惯导和油缸的智能掘进机器人定位定向方法，其特征在于，基于油缸推移智能掘进机器人前进的位移数据实现捷联惯导解算出的智能掘进机器人的位姿数据的校正，从而实现智能掘进机器人高精度的定位定向，其中，智能掘进机器人由机器人I和机器人II组成，机器人I和机器人II之间通过油缸进行连接，智能掘进机器人前进时，首先通过油缸将机器人I推送到预定位置，然后在通过油缸将机器人II拉到预定位置。/n

【技术特征摘要】
1.一种基于捷联惯导和油缸的智能掘进机器人定位定向方法，其特征在于，基于油缸推移智能掘进机器人前进的位移数据实现捷联惯导解算出的智能掘进机器人的位姿数据的校正，从而实现智能掘进机器人高精度的定位定向，其中，智能掘进机器人由机器人I和机器人II组成，机器人I和机器人II之间通过油缸进行连接，智能掘进机器人前进时，首先通过油缸将机器人I推送到预定位置，然后在通过油缸将机器人II拉到预定位置。


2.如权利要求1所述的一种基于捷联惯导和油缸的智能掘进机器人定位定向方法，其特征在于，包括以下步骤：
S1、将捷联惯导安装在机器人I上，并建立载体坐标系和导航坐标系；
S101、将捷联惯导安装在机器人I上，以智能掘进机器人的重心作为坐标原点，智能掘进机器人前进方向为Y轴正方向，智能掘进机器人右侧垂直于Y轴方向为X轴正方向，垂直智能掘进机器人向上方向作为Z轴正方向，建立载体坐标系OXYZ；
S102、以捷联惯导安装所在位置的“东-北-天”坐标系作为导航坐标系O1X1Y1Z1；
S103、智能掘进机器人在载体坐标系下，绕Z轴旋转的角度称为航向角，记为绕X轴旋转的角度称为俯仰角，记为θ，绕Y轴旋转的角度称为横滚角，记为γ，则智能掘进机器人从导航坐标系到载体坐标系的姿态矩阵如下：



S2、将捷联惯导和油缸构成航位推算系统，并对其中包含的误差进行分析；
S201、构建捷联惯导误差模型：



























式中，φE、φN、φU分别为计算导航坐标系与理想导航坐标系之间的失准角误差，ωE、ωN、ωU分别为陀螺仪的测量值，vE、vN、vU分别为智能掘进机器人在导航坐标系中的速度，分别为智能掘进机器人在导航坐标系中的速度误差，RNh、RMh分别为子午圈半径和卯酉圈半径，fE、fN、fU分别为加速度计测量值，L为当地的纬度，δL、δλ、δh分别为纬度、经度和高度误差，εE、εN、εU分别为陀螺仪零偏误差，分别为加速度计零偏误差，ge为赤道重力，β、β1、β2分别为0.005302、3.08×10-6、8.08×10-9；
S202、构建航位推算的位置误差模型：









式中，LD、λD、hD分别为航位推算时智能掘进机器人所在位置的纬度、经度和高度，VDE、VDN、VDU分别为航位推算时智能掘进机器人的东向速度、北向速度和天向速度，αθ为捷联惯导和智能掘进机器人之间的安装偏差角，δKD为油缸推移系数误差；
S203、构建杆臂误差模型：



式中，δl为油缸相对于捷联惯导的位置矢量；
S3、通过标准卡尔曼滤波算法，实现捷联惯导和油缸的数据的融合；
S301、综合考虑捷联惯导误差、航位推算误差和杆臂误差，建立捷联惯导和油缸的系统状态方程；
其中，系统的状态变量为：



式中，φT为计算导航坐标系与理想导航坐标系之间的失准角误差，(δv)T为智能掘进机器人的速度误差，(δp)T为智能掘进机器人的位置误差，(δpD)T为航位推算时的位置误差，(δpGL)T为杆臂误差，(εb)T为陀螺仪零偏误差，为加速度计零偏误差，l为捷联惯导和油缸的位置矢量...

【专利技术属性】
技术研发人员：马宏伟，华洪涛，贺媛，毛清华，李磊，张羽飞，石金龙，
申请(专利权)人：西安科技大学，深圳赛奥航空科技有限公司，
类型：发明
国别省市：陕西;61