基于三维力信息的探进作业机械臂自主避障方法及系统技术方案

本发明专利技术公开了一种基于三维力信息的探进作业机械臂自主避障方法及系统，至少包括数据采集单元、运动执行单元和控制单元，数据采集单元用于收集土壤探进采样时机构末端的三维受力数据并发送给控制单元；运动执行单元用于接收来自控制单元的指令并将机械臂各关节的位置信息发送给控制单元；控制单元首先用于接收来自数据采集单元的数据并转化为三维力信息并接收来自运动执行单元的位姿数据信息，而后根据受力情况判断控制方式，最后将运动控制指令发送至运动执行单元完成控制。本发明专利技术通过将末端受力信息结合到探进作业的运动控制中并进行实时策略调整，有效减小了末端机构作业时的受力，保护机构不受损坏且提高了作业成功率。率。率。

【技术实现步骤摘要】
基于三维力信息的探进作业机械臂自主避障方法及系统


[0001]本专利技术属于信息
以及地质及岩土工程勘察
，特别是涉及一种基于三维力信息的探进作业机械臂自主避障方法及系统。

技术介绍

[0002]地外风化层采样任务对研究宇宙发展、生命起源等方面问题具有重要意义。通过搭载采样器机械臂，星球漫游车能够在天体表面风化层进行样本取样与原位分析。来自美国火星科学实验室的好奇号漫游车，在执行探索夏普峰任务时，就于火星表面风化层进行连续的原位采样以及样本分析工作。浅层钻取作为主流的地外采样方式之一，能够有效保存土壤层理信息，应用较广，典型的钻取采样任务有美国的阿波罗系列任务、苏联的月球系列任务以及中国的嫦娥五号探月任务。此外，通信延时的存在使得地面遥操作的控制方式难以在火星等更远的天体上实施，实现自动采样过程尤其重要。
[0003]地外自动采样任务的一大难点在于环境信息的未知性，研究发现，月球和火星浅层可能出现岩石带与岩石碎片。因此，采样装置在地外作业过程中很可能触及硬物，产生较大的接触力，引起采样器损伤或采样任务失败的后果。而在地外采样任务中，根据末端机构与环境介质的相互作用进行策略调整的研究较少，相关方法有基于振动信号的岩石接触识别方法以及通过识别介质性质来实时调整钻取策略的等，但均不能通过姿态调整对障碍物进行规避。

技术实现思路

[0004]为克服上述现有技术存在的不足，本专利技术之目的在于提供一种基于三维力信息的探进作业机械臂自主避障方法及系统，以解决现有技术中不能根据环境实时调整作业姿态、易受障碍干扰的问题，进一步提升采样系统环境适应性，改善末端采样器工作时的受力情况，减小机构磨损，提高采样作业的成功率。
[0005]为达上述及其它目的，本专利技术提出一种基于三维力信息的探进作业机械臂自主避障系统，至少包括数据采集单元、运动执行单元和控制单元，
[0006]所述数据采集单元：用于收集土壤探进采样时机构末端采样器的三维受力数据，将受力产生的电信号经过放大后转化为模拟信号和数字信号后，发送给控制单元；
[0007]所述运动执行单元：运动执行单元用于接收来自控制单元的指令并将机械臂各关节的位置信息发送给控制单元，实现二自由度的旋转和单自由度的平移运动；
[0008]所述控制单元：首先用于接收来自数据采集单元的数据并转化为三维力信息并接收来自运动执行单元的位姿数据信息，再根据受力情况判断控制方式，实时调整策略。设置安全受力阈值和危险受力阈值，若受力低于于安全受力阈值则继续作业，若受力高于危险受力阈值则中止作业，若受力处于二者之间则再通过导纳控制和旋转避让的方式实现避障运动，而后将运动控制指令发送至运动执行单元完成控制。
[0009]优选地，所述数据采集单元至少包括多维力传感器和数据采集卡；所述控制单元
的力信息采集功能中，采用滑动滤波方法减小数据噪声。
[0010]优选地，所述运动执行单元至少包括三自由度机械臂和下位机控制系统，其中三自由度机械臂结构包括机构基座坐标系X、Y轴方向的旋转关节和Z轴方向的平移关节，下位机控制系统的功能包括：与机械臂各关节通过CAN总线进行通信、实时读取各关节位置并发送给控制单元、接收来自控制单元的控制指令并解析发送给各关节控制器。
[0011]为达到上述及其他目的，提出一种基于三维力信息的探进作业机械臂自主避障方法，具体包括如下步骤：
[0012]S1：读取来自多维力传感器的数据并进行数据处理得到有效外力；
[0013]S2：将有效外力与阈值进行比较，若超过危险阈值则处于危险状态，立即中止作业，若超过安全阈值且不超过危险阈值则处于遇障状态，进行避让调整后继续将有效外力与阈值比较；若低于安全阈值则处于安全状态，继续作业。
[0014]S3：读取来自下位机的各个关节角度值，通过正运动学解算获得当前深度，并于目标深度比较，若达到目标深度则探进作业完毕，若没有达到目标深度则平移关节继续探进，重复步骤S1读取多维力传感器数据。
[0015]优选地，所述控制单元的控制过程中S1步骤进一步包括如下步骤：
[0016]S11：对来自数据采集单元中的多维力传感器的数据进行滑动滤波处理，减小信号噪声，得到所受外力F，且外力包括末端采样器X、Y、Z三方向的受力值F
x
、F
y
、F
z
，并令X与Y方向合力为侧向力，Z方向受力为探进方向阻力；S12：设定侧向力安全阈值F
side
和探进方向阻力安全阈值F
main
；
[0017]S13：将所测侧向力以及探进方向阻力与各自安全阈值进行比较，若任一项高于安全阈值则保留，反之则清零，由此得到有效外力Fe：
[0018][0019]优选地，所述控制单元的控制过程中S2步骤中避让调整一步通过导纳控制和旋转避让实现，进一步包括如下步骤：
[0020]S21：通过旋转关节的转动响应工具坐标系X、Y轴受力来进行障碍规避，转速与有效外力F
e
成正比，当受力安全时转速为0，转速经过积分并叠加初始角度θ0后得到期望关节角度θ
d
，其中θ
x0
、θ
y0
分别表示X与Y轴旋转电机的初始角度，θ
xd
、θ
yd
分别表示X与Y轴旋转电机的目标角度，转换关系式如下：
[0021][0022][0023]式中，为关节转角的一阶微分，且表示关节旋转速度，其中分别为X与Y轴旋转电机的转速，与外力大小成正比，A为旋转速度与外力的增益系数矩阵且a
x
为x轴旋转增益系数，a
y
为y轴旋转增益系数；
[0024]S22：期望关节角度θ
d
经过正运动学解算得到期望位置信息X
d
，而后将期望位置信息X
d
送入导纳控制器中根据有效外力F
e
进行调整，得到实际目标位置X
r
，其中，其中x
d
、y
d
、z
d
分别表示末端采样器在笛卡尔坐标系下的X、Y、Z轴期望坐标，x
r
、y
r
、z
r
分别表示末端采样器在笛卡尔坐标系下的X、Y、Z轴实际目标坐标，导纳控制器运算过程如下：
[0025][0026]式中，M表示质量系数矩阵且m
x
、m
y
、m
z
分别末端采样器在笛卡尔坐标系下的X、Y、Z轴质量系数，B表示阻尼系数矩阵且b
x
、b
y
、b
z
分别末端采样器在笛卡尔坐标系下的X、Y、Z轴阻尼系数，K表示刚度系数矩阵且k
x
、k
y
、k
z
分别末端采样器在笛卡尔坐标系下的X、Y、Z轴刚度系数，为实际目标位置的二阶导本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.基于三维力信息的探进作业机械臂自主避障系统，其特征在于：该系统至少包括数据采集单元、运动执行单元和控制单元，所述数据采集单元：用于收集土壤探进采样时机构末端采样器的三维受力数据，将受力产生的电信号经过放大后转化为模拟信号和数字信号后，发送给控制单元；所述运动执行单元：运动执行单元用于接收来自控制单元的指令并将机械臂各关节的位置信息发送给控制单元，实现二自由度的旋转和单自由度的平移运动；所述控制单元：首先用于接收来自数据采集单元的数据并转化为三维力信息并接收来自运动执行单元的位姿数据信息，再根据受力情况判断控制方式，实时调整策略，设置安全受力阈值和危险受力阈值，若受力低于于安全受力阈值则继续作业；若受力高于危险受力阈值则中止作业；若受力处于二者之间则再通过导纳控制和旋转避让的方式实现避障运动，而后将运动控制指令发送至运动执行单元完成控制。2.如权利要求1所述的基于三维力信息的探进作业机械臂自主避障系统，其特征在于：所述数据采集单元至少包括多维力传感器和数据采集卡。3.如权利要求1或2所述的基于三维力信息的探进作业机械臂自主避障方法及系统，其特征在于：所述运动执行单元至少包括三自由度机械臂和下位机控制系统；所述三自由度机械臂结构包括机构基座坐标系X、Y轴方向的旋转关节和Z轴方向的平移关节；所述下位机控制系统用于与机械臂各关节通过CAN总线进行通信、实时读取各关节位置并发送给控制单元、接收来自控制单元的控制指令并解析发送给各关节控制器。4.如权利要求1所述的基于三维力信息的探进作业机械臂自主避障系统，其特征在于：所述控制单元采用搭载Ubuntu系统的微型电脑NUC实现，程序运行于ROS机器人操作控制系统中。5.如权利要求3所述的基于三维力信息的探进作业机械臂自主避障系统，其特征在于：所述控制单元中下位机控制系统采用STM32单片机实现，采用串口通信与控制单元的微型电脑NUC进行交互，波特率为115200。6.一种基于三维力信息的探进作业机械臂自主避障方法，其特征在于：该方法采用权利要求1
‑
5任一项所述的基于三维力信息的探进作业机械臂自主避障系统，该方法具体包括如下步骤：S1：读取来自数据采集单元中的多维力传感器的数据并进行数据处理得到有效外力；S2：将步骤S1得到的有效外力与设定阈值进行比较，若超过危险阈值则处于危险状态，立即中止作业，若超过安全阈值且不超过危险阈值则处于遇障状态，进行避让调整后继续将有效外力与阈值比较；若低于安全阈值则处于安全状态，继续作业；S3：读取来自下位机的各个关节角度值，通过正运动学解算获得当前深度，并于目标深度比较，若达到目标深度则探进作业完毕，若没有达到目标深度则平移关节继续探进，重复步骤S1读取多维力传感器数据。7.如权利要求6所述的基于三维力信息的探进作业机械臂自主避障方法，其特征在于：步骤S1对多维力传感器进行数据处理至少包括如下步骤：S1：对来自数据采集单元中的多维力传感器的数据进行滑动滤波处理，减小信号噪声，
得到所受外力F，且外力包括末端采样器X、Y、Z三方向的受力值F
x
、F
y
、F
z
，并令X与Y方向合力为侧向力，Z方向受力为探进方向阻力；S2：设定侧向力安全阈值...

【专利技术属性】
技术研发人员：赵子弈，闫禹霏，李明雨，朱利丰，宋爱国，
申请(专利权)人：东南大学，
类型：发明
国别省市：