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

• 摘要：

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

• 专利类型：

  发明专利

• 申请/专利号：

  CN202010950448.5

• 申请日期：

  2020.09.10

• 公开/公告号：

  CN112033401A

• 公开/公告日：

  2020-12-04

• 发明人：

  马宏伟 华洪涛 贺媛 毛清华 李磊 张羽飞 石金龙

• 申请人：

  西安科技大学

• 主分类号：

  G01C21/16(2006.01),G,G01,G01C,G01C21

• 分类号：

  G01C21/16(2006.01),G01C21/20(2006.01),G,G01,G01C,G01C21,G01C21/16,G01C21/20

• 主权项：

  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为捷联惯导和油缸的位置矢量; 组合测量系统的状态方程为: 其中, 和分别为陀螺角速度测量白噪声和加速度计比力测量白噪声; 根据S201,S202,S203中的误差模型,状态转移矩阵F中的参数如下所示: S302、建立捷联惯导和油缸的系统量测方程 以捷联惯导解算的位置与航位推算的位置之差来构建观测向量,即 Z＝δp-δpD 则系统的量测方程为 Z＝HX+V 其中,H＝[03*6 I3*3 -I3*3 03*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+Q P(k-1)为k-1时刻系统状态误差的均方差矩阵,Q为系统噪声矩阵; 3)求解卡尔曼增益K Kk＝P(k/k-1)HT(HP(k/k-1)HT+R)-1 其中,R为量测噪声矩阵; 4)结合卡尔曼增益K,对当前时刻状态的最优估计值进行更新 X(k)＝X(k/k-1)+Kk(Zk-HX(k/k-1)) 5)对当前时刻状态的最优估计值误差的均方差矩阵进行更新 P(k)＝(I-KkH)P(k/k-1) S4、根据油缸推移智能掘进机器人前进的位移数据和捷联惯导数据的融合结果得出智能掘进机器人的位姿曲线,实现掘进工作面精准定位定向. 3.如权利要求1所述的一种基于捷联惯导和油缸的智能掘进机器人定位定向方法,其特征在于:智能掘进机器人直线前进时,智能掘进机器人左右两侧油缸推移量相等,此时智能掘进机器人位移量与油缸推移量相等;当智能掘进机器人纠偏时,左右两侧油缸推移量不等,由于捷联惯导与机器人I固连,所以机器人I的位移与捷联惯导的位移等效,而捷联惯导的位移可以用其在机器人I盾体后面的投影位移近似等效,此时智能掘进机器人位移的位移数据如下: 式中,a为捷联惯导投影与机器人I盾体左表面的距离,b为捷联惯导投影与机器人I盾体右表面的距离,Ll为智能掘进机器人左侧油缸的推移量,Lr为智能掘进机器人右侧油缸的推移量. 4.如权利要求1所述的一种基于捷联惯导和油缸的智能掘进机器人定位定向方法,其特征在于:将捷联惯导解算得到的智能掘进机器人姿态数据和油缸推移智能掘进机器人前进的位移数据结合,构成航位推算系统,并构建航位推算系统的误差模型. 5.如权利要求1所述的一种基于捷联惯导和油缸的智能掘进机器人定位定向方法,其特征在于:考虑油缸位置与捷联惯导安装位置不同而产生的杆臂误差,并构建杆臂误差模型. 6.如权利要求1所述的一种基于捷联惯导和油缸的智能掘进机器人定位定向方法,其特征在于:综合考虑捷联惯导系统的误差、航位推算系统的误差和杆臂误差,利用标准卡尔曼滤波实现捷联惯导数据和油缸数据的融合,得到智能掘进机器人高精度的定位定向数据.