一种割胶机器人的力反馈控制系统和方法以及割胶机器人

技术领域

本发明属于割胶机器人控制领域，具体涉及一种基于六维力传感器的割胶机器人力反馈控制系统和方法以及割胶机器人。

背景技术

天然橡胶与钢铁、石油、煤炭并称为四大工业原料，也是其中唯一的可再生资源，被广泛应用到工业、农业和国防等领域，是国计民生和国防战略不可或缺的资源紧缺性战略物资。割胶作业是获取天然橡胶的第一步，机械化割胶不仅可以把人从繁重的体力劳动以及恶劣的工作环境中解放出来，而且能够降低对胶工的技术依赖，极大地提高生产效率。

目前割胶机器人领域的机械臂的应用多为简单的运动位置控制，割胶机器人无法感受到末端的力和力矩的变化，并及时做出反馈处理；在树皮表面出现复杂工况，如结节、凸起等工况时，刀片在行进过程中会发生受力突变，从而导致无法达到理想的轨迹。因此针对割胶机器人在割胶过程中出现的难点，基于六维力传感器，使用机器人力反馈控制技术，优化刀片在橡胶树表面复杂受力情况下的运动控制，降低割胶轨迹误差，使割胶机器人作业具有一定的柔顺性，提高切割深度和耗皮量的均匀性。从而积极推动农业机械化在割胶领域的应用，减轻胶工高强度的劳动压力。

发明内容

针对上述技术问题，本发明的目的是提供一种基于六维力传感器的割胶机器人的力反馈控制系统和方法以及割胶机器人，实现自动化割胶作业，优化刀片在橡胶树表面复杂受力情况下的运动控制，降低割胶轨迹误差，提高工作效率。

为了实现上述目的，本发明提供了如下技术方案：

一种割胶机器人的力反馈控制系统，割胶机器人包括刀具2、机械臂7、底座8和机械臂控制柜；所述刀具2包括刀架9和刀片10，所述力反馈控制系统包括线激光传感器3、固定盘4、六维力传感器5、法兰6和上位机。

所述六维力传感器5通过法兰6与机械臂7的末端关节固接，所述刀具2的刀架9通过固定盘4与六维力传感器5连接。

所述线激光传感器3垂直固接在固定盘4上，线激光传感器3位于刀片10的割胶方向的前方；线激光传感器3的激光发射方向指向橡胶树1，且线激光传感器3的激光发射端与刀片10的端部齐平，用于实时检测刀片10与橡胶树1的切割面11之间的距离。

所述六维力传感器5能够检测到笛卡尔坐标系下XYZ三个方向上的力Fx、Fy、Fz和力矩Mx、My、Mz。

所述上位机分别与线激光传感器3、六维力传感器5和机械臂控制柜连接，根据预先获得的橡胶树1的起割点、起割点的橡胶树半径和树干中心线信息，以及线激光传感器3和六维力传感器5的实时检测数据，向机械臂控制柜发出控制指令，控制机械臂7调整刀具2的运动轨迹，从而控制切割深度q和耗皮量p的均匀性。

所述六维力传感器5为套筒结构，包括外板、传感器主体以及内部芯片；外板和传感器主体连接；芯片具有应变检测功能，位于传感器主体内部。

一种包含所述的力反馈控制系统的割胶机器人。所述机械臂7通过底座8安装在移动车体上，机械臂7为六自由度串联机械臂，机械臂控制柜控制机械臂7完成多维运动，实现刀具2在上橡胶树1沿割线12进行高精度割胶作业。

所述刀片10与水平面夹角为30°，与割线12的螺旋升角一致；刀片10与橡胶树1的树干表面垂直。

一种割胶机器人的力反馈控制方法，包括如下步骤：

S1、机械臂控制柜控制机械臂7将刀具2运动到预先获得的起割点，上位机以预先获得的起割点的橡胶树半径，以及线激光传感器3检测的刀片10与橡胶树1的切割面11上的起割点之间的距离为基准，得到实时的树干半径信息；再结合预定的螺旋线轨迹数学模型和预先获得的树干中心线信息，得到期望割胶轨迹；期望割胶轨迹即是割线12在机械臂坐标空间的数学表达；

S2、机械臂控制柜根据上位机输入的期望割胶轨迹控制机械臂7，使刀片10从预设起割点沿期望割胶轨迹开始运动；

S2.1、上位机将六维力传感器5实时检测到的力和力矩信息转换分解为刀片10与橡胶树1的割线12的表面的实际接触力F

S2.2、根据割胶机器人力反馈控制模型公式1，得到接触力偏差ΔF与位置修正量ΔX之间的关系公式8；

上述公式中，M

F

x为3×1阶列向量，三个元素值为刀片10的实际位置在机械臂基底坐标系下X轴上的分量，Y轴上的分量和Z轴上的分量，单位为m；

x

F

ΔF为接触力偏差，ΔF＝F

ΔX为位置修正量，是一个3×1阶列向量，单位为m；

k为当前点，k-1为上一个点；k-2为上上一个点；T为采样周期，单位为s，即机械臂7运动单个步长需要的时间；

S3、在得到机械臂基底坐标系下X轴Y轴Z轴三个方向上的位置修正量后，将其分别与机械臂基底坐标系下期望位置在X轴Y轴Z轴三个方向上的分量作代数加法运算，生成刀片10运动至线激光传感器3所在当前点时的目标位置x’，机械臂控制柜根据目标位置x’控制机械臂7的控制器驱动关节电机运动，带动关节末端的刀具2运动，使得刀片10在沿割线切割时可以根据接触力的大小调整运动轨迹，控制切割深度q和耗皮量p的均匀性。

所述步骤S2.2中，接触力偏差ΔF与位置修正量ΔX之间的关系公式获得过程如下：

将公式1转换到频域上，则函数表达为：

则：

ΔF＝(M

变成时域的形式：

采用后向差分离散可得：

公式6可以变为：

则：

上述公式中，S为频域下函数的复变量；

与现有技术相比，本发明的有益效果在于：

本发明的割胶机器人的力反馈控制系统，基于线激光传感器与六维力传感器结合的方式进行机械臂的位姿反馈与调整，有效地结合机械臂运动的高精度特性，对于橡胶树割线表面的复杂工况，可以根据力传感器得到的接触力信息实时反馈调节刀片的运动，实现了割胶过程中的受力偏差测量与矫正，提高了切割深度和耗皮量的均匀性，有效保证割胶运动的准确性，大大提高了割胶质量。本发明为模块化设计，适应性强，可以针对不同作业环境进行调整。

附图说明

图1为本发明具有力反馈控制系统的割胶机器人的局部结构示意图；

图2为本发明机械臂7末端的局部结构示意图；

图3为本发明工作流程图；

图4为本发明的橡胶树割线局部示意图。

其中的附图标记为：

1 橡胶树 2 刀具

3 线激光传感器 4 固定盘

5 六维力传感器 6 法兰

7 机械臂 8 底座

9 刀架 10 刀片

11 切割面 12 割线

13 未切割树干表面

具体实施方式

下面结合附图和实施例对本发明进行进一步说明。

本发明的割胶机器人包括刀具2、机械臂7、底座8和机械臂控制柜。所述机械臂7通过底座8安装在移动车体上，机械臂7为六自由度串联机械臂，机械臂控制柜控制机械臂7完成多维运动，实现刀具2在上橡胶树1沿割线12进行高精度割胶作业。所述刀具2包括刀架9和刀片10，刀片10与刀架9之间紧配合，同时采用螺栓紧固。刀片10倾斜安装，刀片10与水平面夹角为30°，与割线12的螺旋升角一致。刀片10与橡胶树1的树干表面垂直。

本发明的力反馈控制系统包括线激光传感器3、固定盘4、六维力传感器5、法兰6和上位机。

如图1所示，所述六维力传感器5通过法兰6与机械臂7的第六关节固接，所述刀具2的刀架9通过固定盘4与六维力传感器5连接。

所述线激光传感器3通过螺栓垂直固接在固定盘4上，线激光传感器3位于刀片10的割胶方向的前方；线激光传感器3的激光发射方向指向橡胶树1，且线激光传感器3的激光发射端与刀片10的端部齐平，用于实时检测刀片10与橡胶树1的切割面11之间的距离。

其中，如图2所示，所述六维力传感器5为套筒结构，包括外板、传感器主体以及内部芯片；外板和传感器主体通过螺栓连接；芯片具有应变检测功能，位于传感器主体内部，能够检测到笛卡尔坐标系下XYZ三个方向上的力Fx、Fy、Fz和力矩Mx、My、Mz。

所述上位机分别与线激光传感器3、六维力传感器5和机械臂控制柜连接，根据预先(人工测量)获得的橡胶树1的起割点、起割点的橡胶树半径和树干中心线信息，以及线激光传感器3和六维力传感器5的实时检测数据，向机械臂控制柜发出控制指令，控制机械臂7调整刀具2的运动轨迹，从而控制切割深度q和耗皮量p的均匀性。

割胶作业前，需要对六维力传感器5置零并标定，对线激光传感器3进行参数调整完成标定，保证刀具2沿割线运动过程的运动准确度。

如图3所示，本发明的割胶机器人的力反馈控制方法，包括如下步骤：

S1、机械臂控制柜控制机械臂7将刀具2运动到预先获得的起割点，由于橡胶树半径不均匀，因此线激光传感器3检测的刀片10与橡胶树1的切割面11之间的距离也在变化，上位机以预先获得的起割点的橡胶树半径，以及线激光传感器3检测的刀片10与橡胶树1的切割面11上的起割点之间的距离为基准，即可得到实时的树干半径信息；再结合预定的螺旋线轨迹数学模型和预先获得的树干中心线信息，得到期望割胶轨迹；期望割胶轨迹即是割线12在机械臂坐标空间的数学表达；

S2、机械臂控制柜根据上位机输入的期望割胶轨迹控制机械臂7，使刀片10从预设起割点沿期望割胶轨迹开始运动；

割胶过程中，刀片10与橡胶树1的割线12的表面的接触力应保持稳定，将稳定的接触力称为期望接触力。但实际作业过程中的实际接触力由于橡胶树1的割线处表面环境复杂，导致刀片10运动至线激光传感器3所在当前点时的实际位置x与期望割胶轨迹里该点的期望位置x

S2.1、上位机将六维力传感器5实时检测到的力和力矩信息转换分解为刀片10与橡胶树1的割线12的表面的实际接触力F

割胶机器人力反馈控制是将机械臂力/位置控制系统等效为一个弹簧质量阻尼系统，使用该系统描述机械臂与环境间接触力和位置的关系。如公式1所示，可以通过调节该系统的惯性参数M

本申请中涉及到的实际接触力F

S2.2、根据割胶机器人力反馈控制模型公式1，得到接触力偏差ΔF与位置修正量ΔX之间的关系公式8；

将公式1转换到频域上，则函数表达为：

则：

ΔF＝(M

变成时域的形式：

采用后向差分离散可得：

公式6可以变为：

即：

上述公式中，M

F

x为3×1阶列向量，三个元素值为刀片10的实际位置在机械臂基底坐标系下X轴上的分量，Y轴上的分量和Z轴上的分量，单位为m；

x

F

ΔF为接触力偏差，ΔF＝F

当前位置的修正量ΔX与当前周期力和力矩的偏差、上一个周期的位置修正量以及上上一个周期的位置修正量有关。在计算第一个周期的位置修正量的时候可以令上一个周期和上上一个周期的位置修正量设为零，这样就可以得到每一个周期的位置修正量ΔX。

得到的位置的修正量ΔX为3×1阶列向量，列向量的三个元素分别代表机械臂基底坐标系下X轴Y轴Z轴三个方向上的位置修正量。

S3、在得到机械臂基底坐标系下X轴Y轴Z轴三个方向上的位置修正量后，将其分别与机械臂基底坐标系下期望位置在X轴Y轴Z轴三个方向上的分量作代数加法运算，生成刀片10运动至线激光传感器3所在当前点时的目标位置x’，机械臂控制柜根据目标位置x’控制机械臂7的控制器驱动关节电机运动，带动关节末端的刀具2运动，因此刀片10在沿割线切割时可以根据接触力的大小调整运动轨迹，较好地控制切割深度q和耗皮量p的均匀性。一直到本段割线轨迹运动完毕，六维力传感器5和线激光传感器3结束工作，机械臂7恢复到零位姿态，上位机断电，割胶力反馈控制系统停止工作。