一种基于冗余惯性单元的防冲钻孔机器人钻具定姿方法与流程

1.本发明涉及防冲钻孔机器人钻具定姿方法的技术领域，具体涉及一种基于冗余惯性单元的防冲钻孔机器人钻具定姿方法。


背景技术：

2.随着科技的发展，井下钻探设备的自动化程度有了显著的提升。煤矿防冲击钻孔机器人用于防冲卸压孔等多种工程孔的施工。常规的煤矿钻机通常在一个固定的位置对钻机的姿态进行调节，同时还需要1或2人进行辅助，观察机架的运行情况，钻孔效率低，施工人员劳动工作强度大。不能够满足快速防冲卸压钻孔要求。为了提高钻机的使用性能，实现减人、增效，需要钻机对钻具的姿态进行自动调节，而钻具的自动调节的基础是要准确的钻具的姿态。
3.因此，亟需一种防冲钻孔机器人钻具定姿的方法来解决上述问题。


技术实现要素：

4.为全面解决上述问题，尤其是针对现有技术所存在的不足，本发明提供了一种基于冗余惯性单元的防冲钻孔机器人钻具定姿方法能够全面解决上述问题。
5.为实现上述目的，本发明采用以下技术手段：
6.一种基于冗余惯性单元的防冲钻孔机器人钻具定姿方法，包括以下步骤：
7.s1、将五路惯性单元分别固定连接在四面体的顶点和中心处，构成惯性传感器；
8.s2、将五路惯性单元数据按照设定的比例进行自定义融合公式进行运算；
9.s3、将惯性传感器测得的姿态误差模型导入神经网络进行训练，得到训练有素的神经网络预测模型；
10.s4、将惯性传感器测得数据导入训练有素的神经网络模型进行误差预测；
11.s5、将神经网络预测误差导入惯性传感器的解算结果，并对其进行补偿。
12.优选的，所述惯性传感器固定连接在防冲击钻孔机器人运动机构的侧部，所述惯性传感器采集运动机构的角速度、加速度和磁力信息；
13.所述惯性传感器通过通讯线与微型电脑通讯连接，所述微型电脑读取并保存惯性传感器采集到的数据；
14.所述微型电脑与pc上位机连接，所述微型电脑将保存的数据发送至pc上位机，所述pc上位机进行五路数据的融合，将融合后的数据进行去噪和滤波操作并将数据进行显示。
15.优选的，所述防冲击钻孔机器人包括防冲击钻孔机器人底座，所述防冲击钻孔机器人底座的顶端设有转动连接的方位角回转机构，所述方位角回转机构的顶端设置有俯仰角运动机构，所述俯仰角运动机构通过液压缸与方位角回转机构升降连接，所述俯仰角运动机构还通过导向柱与方位角回转机构导向连接，所述俯仰角运动机构的外侧设置有钻杆，所述钻杆通过机架与俯仰角运动机构连接，所述惯性传感器与俯仰角运动机构的侧壁
连接。
16.优选的，所述惯性单元采用invernsense公司生产的mpu9250惯性单元。
17.优选的，所述微型电脑为树莓派，所述树莓派采用亚马逊公司生产的raspberry pi4model b。
18.优选的，所述惯性单元mpu9250与微型电脑之通过i2c协议进行通讯.
19.优选的，所述微型电脑通过i2c接口同时连接5个惯性单元，微型电脑逐个访问每个mpu9250读取数据，实现惯性传感器数据的采集。
20.优选的，所述微型电脑通过串口线将微型电脑保存的数据传送至pc上位机。
21.优选的，所述五路数据的融合按照4:1.5:1.5:1.5:1.5的比例进行自定义融合公式进行运算。
22.优选的，所述去噪算法采用小波阈值去噪。
23.本发明的有益效果：本发明无需人工观察机架的运行情况，通过惯性传感器即可采集防冲击钻孔机器人运动机构的角速度、加速度和磁力信息，惯性传感器通过通讯线把采集到的信息传输给微型电脑，微型电脑将获得数据信息发送给pc上位机，pc上位机进行五路数据的融合，将融合后的数据进行去噪和滤波操作并将数据进行显示，通过最终获得的数据钻机能够快速，准确地对钻具的姿态进行自动调节，从而有效降低了工作人数，有效减少了劳动量，有效提高了钻孔效率，能够满足快速防冲卸压钻孔的要求。
附图说明
24.为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
25.图1是本发明基于冗余惯性单元的防冲钻孔机器人钻具定姿方法的流程图；
26.图2是本发明五个惯性单元冗余排列的分布图；
27.图3是本发明防冲击钻孔机器人的结构示意图一；
28.图4是本发明防冲击钻孔机器人的结构示意图二；
29.图5是本发明基于遗传算法优化bp神经网络的惯性单元误差补偿算法流程图。
具体实施方式
30.下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
31.在本发明的描述中，需要说明的是，术语“中心”、“上”、“下”、“左”、“右”、“竖直”、“水平”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”、“第三”仅用于描述目的，而不能理解为指示或暗示相对重要性。
32.在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相
连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。
33.如图1至图5所示，本发明提供了一种基于冗余惯性单元的防冲钻孔机器人钻具定姿方法，防冲击钻孔机器人包括防冲击钻孔机器人底座11，防冲击钻孔机器人底座11的顶端设有转动连接的方位角回转机构12，方位角回转机构12的顶端设置有俯仰角运动机构17，俯仰角运动机构17通过液压缸16与方位角回转机构12升降连接，俯仰角运动机构17还通过导向柱13与方位角回转机构12导向连接，俯仰角运动机构17的外侧设置有钻杆15，钻杆15通过机架14与俯仰角运动机构17连接。首先将五路惯性单元布置在四面体的顶点处和中心处，构成惯性传感器18，将其固定连接在防冲击钻孔机器人钻具的俯仰角运动机构17的侧壁，然后将惯性传感器18通过通讯线与树莓派连接通讯，之后将树莓派与pc上位机连接。该平台通过安装在防冲击钻孔机器人机架的惯性传感器18采集角速度、加速度和磁力信息，并通过树莓派读取并保存惯性传感器18采集得数据，然后将树莓派保存的数据发送至pc上位机，pc上位机进行五路数据的融合，将融合后的数据进行去噪和滤波操作并将数据进行显示。其中惯性单元采用invernsense公司生产的mpu9250惯性单元，树莓派采用亚马逊公司生产的raspberry pi4model b。惯性单元mpu9250与树莓派之间的通信采用i2c协议进行通讯。树莓派通过i2c接口同时接5个mpu9250,树莓派逐个访问每个mpu9250读取数据，实现惯性传感器18数据的采集。树莓派处理器通过串口线将保存的数据传送至pc上位机。
34.五路数据的融合根据惯性单元在载体的中心位置测量的更准确的原理按照4:1.5:1.5:1.5:1.5的比例进行自定义融合公式进行运算。
35.去噪算法采用小波阈值去噪，其原理为给小波系数设定一个阈值，高于该阈值的小波系数完整保留或适当收缩后保留，低于该阈值的小波系数全部归零，然后选择不为零的小波重构信号，就可以得到去噪后的信号。
36.如图2所示，图中的每个圈代表一个mpu9250惯性单元，根据右手螺旋定则，圈中叉号表示右手四指从x轴转向y轴时，大拇指指向里面，即z轴由外指向里。圈中如果是点表示右手四指从x轴转向y轴时，拇指向外，即z轴由里指向外。由图可知，在三菱锥的四个顶点上惯性单元的敏感轴指向不是完全一致，不一致指向的两轴夹角为平角。此结构极大的减少了在惯性单元测量时由于温度、振动等外界因素导致安装在钻进钻具身上出现的固定偏移误差，也在很大程度上使得惯性单元测量的非确定误差得到抵消，并且也可以消除一定的圆锥误差等多种误差，从而较大提高惯性系统的测量精度。
37.根据惯性单元采用的四面体对称安装布局方式，并依据惯性单元的测量原理可以得到传感器的融合方程：
38.惯性单元mpu9250角速度数据融合方程为：
[0039][0040]
惯性单元mpu9250加速度融合方程为：
[0041][0042]
建立了以上公式后，就可以得到冗余惯性单元测得载体的参数
[0043]
如图5所示，具体惯性单元误差补偿算法的流程如下：
[0044]
(1)bp神经网络结构确定部分根据拟合函数输入输出参数个数确定，进而确定遗传算法个体的长度。用遗传算法来优化bp神经网络的权值和阈值，种群中的每个个体都包含了一个网络所有权值和阈值，个体通过适应度函数计算个体适应度值，遗传算法通过选择、交叉和变异操作找到最优适应度值对应个体。bp神经网络预测用遗传算法得到最优个体对网络初试权值和阈值赋值，网络经训练后预测函数输出。
[0045]
(2)将多冗余惯性单元传感器融合后的数据导入训练好的神经网络中，然后会得到惯性单元的测量误差预测值。
[0046]
(3)根据预测的惯性单元测量误差对融合后的数据进行补偿，可以得到更精确的数据，得到的精确数据导入姿态计算程序，可以得到防冲击钻孔机器人较为准确的钻具姿态。
[0047]
本发明所作的举例，而并非对实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动，这里无需也无法对所有的实施方式予以穷举，而由此所引申出的显而易见的变化或变动仍处于本发明的保护范围中。