一种深井探孔机器人姿态控制方法技术

本发明专利技术提出了一种深井探孔机器人姿态控制方法，包括：搭建深井探孔机器人控制系统，控制系统设有陀螺仪

【技术实现步骤摘要】
一种深井探孔机器人姿态控制方法


[0001]本专利技术属于探孔机器人领域，特别涉及一种深井探孔机器人姿态控制方法
。

技术介绍

[0002]深层石油天然气的开发是油田增产
、
稳产的重要措施，深井下钻孔施工中，开孔精度影响成孔质量，成孔质量又会直接影响瓦斯抽采
、
探放水注浆加固
、
防突和防冲效果，因此精确调姿不但有利于瓦斯抽采
、
探放水
、
防突和防冲的效果，更是实现精准钻孔
、
精准快速抽采的基础和前提
。
而现有钻机位姿调节借助开孔定向仪和人工测量等，与手动
、
电控调姿配合，操作繁琐，并需要反复校准才能达到所需开孔位置，对工人操作要求高
、
位姿调节耗时
、
费力
。
即使少数钻机具有自动位姿控制功能，但采用开环控制，存在精度低
、
定位误差大
、
误差无法补偿的问题，难于实现钻孔快速自动精确定位，成为制约瓦斯钻孔全自动施工的关键因素
。
[0003]深井下探孔精确定位是实现探孔机器人自主操作的难点和重要内容，也是实现少人化
、
无人化的关键
。
目前钻机位姿调节多采用液压或遥控调节与人工复测结合，调节过程复杂，未实现自动调节，不满足自动化矿井的发展趋势，因此实现深井探孔机器人的高精度姿态控制算法是实现深井探孔作业少人化
、
无人化的关键
。

技术实现思路

[0004]本专利技术的目的在于提供一种深井探孔机器人姿态控制方法，解决探孔机器人的姿态控制问题
。
[0005]实现本专利技术目的的技术解决方案为：
[0006]一种深井探孔机器人姿态控制方法，包括以下步骤：
[0007]步骤1：搭建深井探孔机器人控制系统，控制系统设有陀螺仪
、
加速度传感器
、
编码器
、
控制器
、
驱动器及电机；
[0008]步骤2：将陀螺仪设置在探孔机器人俯仰角和方位角回转的中心位置，通过陀螺仪检测深井探孔机器人的俯仰角
、
方位角和横滚角；
[0009]步骤3：将陀螺仪坐标系中的方位角
、
俯仰角
、
横滚角转换为探孔机器人的姿态角；
[0010]步骤4：利用步骤3所解算横滚姿态角
、
俯仰姿态角和方位姿态角对深井探孔机器人的姿态进行角度闭环，将驱动器的运动模式调为速度环，通过位置环
+
速度环的方式实现深井探孔机器人的姿态控制；
[0011]步骤5：通过加速度传感器，实时检测深井探孔机器人位移，推算深井探孔机器人在三维空间中的位移轨迹；
[0012]步骤6：根据三维空间中的位移轨迹，进行初步三维建模，根据初步三维建模结果，对孔径群不同层高的切片进行分析及处理，并设计算法分析孔径群分布效果好坏，为爆破设计提供参考；
[0013]步骤7：将步骤6中的分析结果通过井下无线网线，传输至需要的计算存储系统中
。
[0014]首先，将参照陀螺仪坐标系的方位角
、
俯仰角
、
横滚角指令通过步骤3计算横滚姿态角
、
俯仰姿态角和方位姿态角，再将解算的横滚姿态角
、
俯仰姿态角和方位姿态角作为方位回转机构
、
俯仰调节机构和横滚调节机构的指令，每个机构编码器所采集的值作为反馈，然后把指令和反馈作为输入，电机速度作为输出，每个机构各构建一个
P
闭环算法，最终实现深井探孔机器人的姿态闭环控制
。
[0015]方位回转机构
P
闭环算法可表示为：
[0016]V1＝
P1*(
ψ
'
‑
d1)
ꢀꢀ
(6)
[0017]式中，
V1表示方位回转机构电机的速度输出，
d1表示方位回转机构编码器所采集的方位姿态角，
P1表示比例参数；
[0018]俯仰调节机构
P
闭环算法可表示为：
[0019]V2＝
P2*(
θ
'
‑
d2)
ꢀꢀ
(7)
[0020]式中，
V2表示俯仰调节机构电机的速度输出，
d2表示俯仰调节机构编码器所采集的方位姿态角，
P2表示比例参数；
[0021]横滚调节机构
P
闭环算法可表示为：
[0022]V3＝
P3*(
γ
'
‑
d3)
ꢀꢀ
(8)
[0023]式中，
V3表示横滚调节机构电机的速度输出，
d3表示横滚调节机构编码器所采集的方位姿态角，
P3表示比例参数
。
[0024](1)
本专利技术可根据反馈信号实时调整控制输出，以使探孔机器人姿态保持稳定
。
它可以通过比较期望姿态和实际姿态之间的误差，并根据闭环的调整来减小误差，从而实现稳定的探孔；
[0025](2)
本专利技术控制器可以根据不同的探孔条件和外部干扰进行自适应调整，快速地响应变化，并对干扰进行补偿，保持探孔姿态的稳定性
。
附图说明
[0026]图1是本专利技术的控制流程图；
[0027]图2是本专利技术加速度传感器产生力示意图；
[0028]图3为本专利技术中速度与时间的曲线图；
[0029]图4是本专利技术中
xyz
坐标轴中加速度方向分解图；
[0030]图5是本专利技术中
yz
坐标轴中
AY
加速度方向分解图；
[0031]图6是本专利技术中
xz
坐标轴中
AZ
加速度方向分解图；
[0032]图7是本专利技术中
xz
坐标轴中
AX
加速度方向分解图；
[0033]图8是本专利技术中加速度的值
。
具体实施方式
[0034]下面结合附图及具体实施例对本专利技术做进一步的介绍
。
[0035]结合图1，本专利技术的一种深井探孔机器人姿态控制方法，包括以下步骤：
[0036]步骤1：搭建深井探孔机器人控制系统，控制系统设有陀螺仪
、
加速度传感器
、
编码器
、
控制器
、
驱动器及电机；
[0037]深井探孔机器人设有方位回转机构
、
俯仰调节机构和横滚调节机构，每个机构各
安装一个编码器，编码器采集机构的姿态角，其中方位回转机构可实现机本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.
一种深井探孔机器人姿态控制方法，其特征在于，包括以下步骤：步骤1：搭建深井探孔机器人控制系统，控制系统设有陀螺仪
、
加速度传感器
、
编码器
、
控制器
、
驱动器及电机；步骤2：将陀螺仪设置在探孔机器人俯仰角和方位角回转的中心位置，通过陀螺仪检测深井探孔机器人的俯仰角
、
方位角和横滚角；步骤3：将陀螺仪坐标系中的方位角
、
俯仰角
、
横滚角转换为探孔机器人的姿态角；步骤4：利用步骤3所解算横滚姿态角
、
俯仰姿态角和方位姿态角对深井探孔机器人的姿态进行角度闭环，将驱动器的运动模式调为速度环，通过位置环
+
速度环的方式实现深井探孔机器人的姿态控制；步骤5：通过加速度传感器，实时检测深井探孔机器人位移，推算深井探孔机器人在三维空间中的位移轨迹；步骤6：根据三维空间中的位移轨迹，进行初步三维建模，根据初步三维建模结果，对孔径群不同层高的切片进行分析及处理，并设计算法分析孔径群分布效果好坏，为爆破设计提供参考；步骤7：将步骤6中的分析结果通过井下无线网线，传输至需要的计算存储系统中
。2.
根据权利要求1所述的一种深井探孔机器人姿态控制方法，其特征在于，步骤1，具体包括以下步骤：深井探孔机器人设有方位回转机构
、
俯仰调节机构和横滚调节机构，每个机构各安装一个编码器，编码器采集机构的姿态角，其中方位回转机构可实现机器人方位角的调节
、
俯仰调节机构机器人俯仰角的调节
、
横滚调节机构可实现机器人横滚角的调节，同时每个机构的执行器均由驱动器
+
电机组成，驱动器接收控制器的指令来驱动电机
。3.
根据权利要求1所述的一种深井探孔机器人姿态控制方法，其特征在于，步骤3具体包括：将方位姿态角转换矩阵
R
ψ
表示为：式中，
ψ
为方位角；将俯仰姿态角转换矩阵
R
θ
表示为：式中，
θ
为俯仰角；将横滚姿态角转换矩阵
R
γ
表示为：式中，
γ
为横滚角；
姿态角的转换矩阵由
R
ψ
R
θ
R
γ
的乘积来表示，转换矩阵乘积的顺序与机器人的运动顺序一致，例如机器人的运动步骤是横滚
‑
方位
‑
俯仰时，姿态角的转换矩阵
R
可表示为：
R
＝
R
γ
×
R
ψ
×
R
θ
姿态角可表示为：式中，
[
γ
',
θ
',
ψ
']
T
是换算后的横滚姿态角
、
俯仰姿态角和方位姿态角
。4.
根据权利要求1所述的一种深井探孔机器人姿态控制方法，其特征在于，步骤4具体包括：首先，将参照陀螺仪坐标系的方位角
、
俯仰角
、
横滚角指令通过步骤3计算横滚姿态角
、
俯仰姿态角和方位姿态角，再将解算的横滚姿态角
、
俯仰姿态角和方位姿态角作为方位回转机构
、
俯仰调节机构和横滚调节机构的指令，每个机构编码器所采集的值作为反馈，然后把指令和反馈作为输入，电机速度作为输出，每个机构各构建一个
P
闭环算法，最终实现深井探孔机器人的姿态闭环控制；方位回转机构
P
闭环算法可表示为：
V1＝
P1*(
ψ
'
‑
d1)
式中，
V1表示方位回转机构电机的速度输出，
d1表示方位回转机构编码器所采集的方位姿态角，
P1表示比例参数；俯仰调节机构
P
闭环算法可表示为：
V2＝
P2*(
θ
'
‑
d2)
式中，
V2表示俯仰调节机构电机的速度输出，
d2表示俯仰调节机构编码器所采集的方位姿态角，
P2表示比例参数；横滚调节机构
P
闭环算法可表示为：
V3＝
P3*(
γ
'
‑
d3)
式中，
V3表示横滚调节机构电机的速度输出，
d3表示横滚调节机构编码器所采集的方位姿态角，
P3表示比例参数
。5.
根据权利要求1所述的一种深井探孔机器人姿态控制方法，其特征在于，步骤5具体包括：通过加速度传感器采集三维空间
X,Y,Z
三轴的加速度和初速度，计算三轴的位移轨迹，
X,Y,Z
三轴的瞬时速度分别如下：三轴的瞬时速度分别如下：三轴的瞬时速度分别如下：式中，
t
为时间参数，
Δ

【专利技术属性】
技术研发人员：于继明，陈淼，王婷，
申请(专利权)人：南京迈思物联网科技有限公司，
类型：发明
国别省市：