[发明专利]基于捷联惯导和油缸的智能掘进机器人定位定向方法

权利要求书

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

所述基于捷联惯导和油缸的智能掘进机器人定位定向方法，包括以下步骤：

S1、将捷联惯导安装在机器人I上，并建立载体坐标系和导航坐标系；

S101、将捷联惯导安装在机器人I上，以智能掘进机器人的重心作为坐标原点，智能掘进机器人前进方向为Y轴正方向，智能掘进机器人右侧垂直于Y轴方向为X轴正方向，垂直智能掘进机器人向上方向作为Z轴正方向，建立载体坐标系OXYZ；

S102、以捷联惯导安装所在位置的“东-北-天”坐标系作为导航坐标系O₁X₁Y₁Z₁；

S103、智能掘进机器人在载体坐标系下，绕Z轴旋转的角度称为航向角，记为绕X轴旋转的角度称为俯仰角，记为θ，绕Y轴旋转的角度称为横滚角，记为γ，则智能掘进机器人从导航坐标系到载体坐标系的姿态转换矩阵如下：

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

S201、构建捷联惯导误差模型：

式中，φ_E、φ_N、φ_U分别为计算导航坐标系与理想导航坐标系之间的失准角误差，ω_E、ω_N、ω_U分别为陀螺仪的测量值，v_E、v_N、v_U分别为智能掘进机器人在导航坐标系中的速度，δv_E、δv_N、δv_U分别为智能掘进机器人在导航坐标系中的速度误差，R_Nh、R_Mh分别为子午圈半径和卯酉圈半径，f_E、f_N、f_U分别为加速度计测量值，L为当地的纬度，δL、δλ、δh分别为纬度、经度和高度误差，ε_E、ε_N、ε_U分别为陀螺仪零偏误差，分别为加速度计零偏误差，g_e为赤道重力，β、β₁、β₂分别为0.005302、3.08×10^-6、8.08×10^-9；

S202、构建航位推算的位置误差模型：

式中，L_D、λ_D、h_D分别为航位推算时智能掘进机器人所在位置的纬度、经度和高度，α_θ为捷联惯导和智能掘进机器人之间的安装偏差角，δK_D为油缸推移系数误差；

S203、构建杆臂误差模型：

式中，δl为油缸相对于捷联惯导的位置矢量；

S3、通过标准卡尔曼滤波算法，实现捷联惯导和油缸的数据的融合；

S301、综合考虑捷联惯导误差、航位推算误差和杆臂误差，建立捷联惯导和油缸的系统状态方程；

其中，系统的状态变量X为：

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

组合测量系统的状态方程为：

其中，

和分别为陀螺角速度测量白噪声和加速度计比力测量白噪声；

根据S201,S202,S203中的误差模型，状态转移矩阵F中的参数如下所示：

S302、建立捷联惯导和油缸的系统量测方程

以捷联惯导解算的位置与航位推算的位置之差来构建观测向量Z，即

Z＝δp-δp_D

则系统的量测方程为Z＝HX+V

其中，H＝[0_3×6 I_3×3 -I_3×3 0_3×15],V为测量噪声；

S303、建立捷联惯导和油缸标准卡尔曼滤波方程，得到系统状态的最优估计值，其迭代过程可分为以下5步：

1)通过状态方程和系统上一时刻的状态对当前时刻状态进行一步预测

X(k/k-1)＝F·X(k-1)

其中，X(k/k-1)为一步预测状态量，X(k-1)为k-1时刻系统的状态量；

2)对当前时刻预测状态的误差的均方差阵P(k/k-1)进行求解

P(k/k-1)＝FP(k-1)F^T+Q

P(k-1)为k-1时刻系统状态误差的均方差矩阵，Q为系统噪声矩阵；

3)求解卡尔曼增益K_k

K_k＝P(k/k-1)H^T(HP(k/k-1)H^T+R)^-1

其中，R为量测噪声矩阵；

4)结合卡尔曼增益K_k，对当前时刻状态的最优估计值进行更新

X(k)＝X(k/k-1)+K_k(Z_k-HX(k/k-1))

5)对当前时刻状态的最优估计值误差的均方差矩阵进行更新

P(k)＝(I-K_kH)P(k/k-1)

S4、根据油缸推移智能掘进机器人前进的位移数据和捷联惯导数据的融合结果得出智能掘进机器人的位姿曲线，实现掘进工作面精准定位定向。