[发明专利]基于捷联惯导和油缸的智能掘进机器人定位定向方法有效
申请号: | 202010950448.5 | 申请日: | 2020-09-10 |
公开(公告)号: | CN112033401B | 公开(公告)日: | 2023-06-20 |
发明(设计)人: | 马宏伟;华洪涛;贺媛;毛清华;李磊;张羽飞;石金龙 | 申请(专利权)人: | 西安科技大学;深圳赛奥航空科技有限公司 |
主分类号: | G01C21/16 | 分类号: | G01C21/16;G01C21/20 |
代理公司: | 北京彭丽芳知识产权代理有限公司 11407 | 代理人: | 彭丽芳 |
地址: | 710054 陕*** | 国省代码: | 陕西;61 |
权利要求书: | 查看更多 | 说明书: | 查看更多 |
摘要: | |||
搜索关键词: | 基于 捷联惯导 智能 掘进 机器人 定位 定向 方法 | ||
1.一种基于捷联惯导和油缸的智能掘进机器人定位定向方法,其特征在于,基于油缸推移智能掘进机器人前进的位移数据实现捷联惯导解算出的智能掘进机器人的位姿数据的校正,从而实现智能掘进机器人高精度的定位定向,其中,智能掘进机器人由机器人I和机器人Ⅱ组成,机器人I和机器人Ⅱ之间通过油缸进行连接,智能掘进机器人前进时,首先通过油缸将机器人I推送到预定位置,然后再通过油缸将机器人Ⅱ拉到预定位置;
所述基于捷联惯导和油缸的智能掘进机器人定位定向方法,包括以下步骤:
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分别为智能掘进机器人在导航坐标系中的速度,δ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分别为航位推算时智能掘进机器人所在位置的纬度、经度和高度,αθ为捷联惯导和智能掘进机器人之间的安装偏差角,δKD为油缸推移系数误差;
S203、构建杆臂误差模型:
式中,δl为油缸相对于捷联惯导的位置矢量;
S3、通过标准卡尔曼滤波算法,实现捷联惯导和油缸的数据的融合;
S301、综合考虑捷联惯导误差、航位推算误差和杆臂误差,建立捷联惯导和油缸的系统状态方程;
其中,系统的状态变量X为:
式中,φT为计算导航坐标系与理想导航坐标系之间的失准角误差,(δv)T为智能掘进机器人的速度误差,(δp)T为智能掘进机器人的位置误差,(δpD)T为航位推算时的位置误差,(δpGL)T为杆臂误差,(εb)T为陀螺仪零偏误差,为加速度计零偏误差,l为捷联惯导和油缸的位置矢量;
组合测量系统的状态方程为:
其中,
和分别为陀螺角速度测量白噪声和加速度计比力测量白噪声;
根据S201,S202,S203中的误差模型,状态转移矩阵F中的参数如下所示:
S302、建立捷联惯导和油缸的系统量测方程
以捷联惯导解算的位置与航位推算的位置之差来构建观测向量Z,即
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/k-1)为一步预测状态量,X(k-1)为k-1时刻系统的状态量;
2)对当前时刻预测状态的误差的均方差阵P(k/k-1)进行求解
P(k/k-1)=FP(k-1)FT+Q
P(k-1)为k-1时刻系统状态误差的均方差矩阵,Q为系统噪声矩阵;
3)求解卡尔曼增益Kk
Kk=P(k/k-1)HT(HP(k/k-1)HT+R)-1
其中,R为量测噪声矩阵;
4)结合卡尔曼增益Kk,对当前时刻状态的最优估计值进行更新
X(k)=X(k/k-1)+Kk(Zk-HX(k/k-1))
5)对当前时刻状态的最优估计值误差的均方差矩阵进行更新
P(k)=(I-KkH)P(k/k-1)
S4、根据油缸推移智能掘进机器人前进的位移数据和捷联惯导数据的融合结果得出智能掘进机器人的位姿曲线,实现掘进工作面精准定位定向。
该专利技术资料仅供研究查看技术是否侵权等信息,商用须获得专利权人授权。该专利全部权利属于西安科技大学;深圳赛奥航空科技有限公司,未经西安科技大学;深圳赛奥航空科技有限公司许可,擅自商用是侵权行为。如果您想购买此专利、获得商业授权和技术合作,请联系【客服】
本文链接:http://www.vipzhuanli.com/pat/books/202010950448.5/1.html,转载请声明来源钻瓜专利网。