一种基于串联弹性执行器的六关节机械臂接触力控制方法与流程

本发明涉及一种机械臂接触力控制方法。

背景技术：

虽然工业机器人的使用越来越广泛，但是在某些需要机器人与环境相互接触的场合，比如，工业机器人抛光打磨的接触力控制，仅仅依靠单一的位置控制是无法实现接触力的控制的。并且，随着人们的要求越来越高，对精加工控制要求也相应提高。

近年来，随着机器人柔顺控制技术的不断发展，将柔顺控制应用于机器人控制系统中，使得机器人向智能化方向迈了一大步。当前，柔顺控制分为主动柔顺和被动柔顺两种。机器人凭借一些辅助的柔顺机构，使其在与环境接触时能够对外部作用力产生自然顺从，称为被动柔顺；机器人利用力的反馈信息采用一定的控制策略去主动控制作用力，称为主动柔顺。在工业机器人的工作过程中需要与环境接触，机械臂与环境直接的接触力太大会造成对环境的伤害，接触力太小又达不到要求，对于一些高精度要求的加工工件，需要更高精度的控制策略。因此，将柔顺控制应用在机械臂中，来实现接触力的恒力控制。但是在很多论文中只考虑了用弹簧力作为输出接触力，而忽略了重力对弹簧的影响，因此，本人提出一种串联弹性执行器的重力补偿方法，来提高力控制精度。

技术实现要素：

本发明要克服现有技术的上述缺点，采用具有柔顺作用的串联弹性执行器，提供一种基于串联弹性执行器的六关节机械臂接触力控制方法，同时对不同姿态下串联弹性执行器的重力进行实时补偿，来提高接触力控制精度，旨在解决工业机械加工过程中，机械臂与环境或工件之间接触力的控制问题，实现机械臂与环境接触的恒力控制。

为达到上述目的，本发明的技术方案如下：

一种基于串联弹性执行器的六关节机械臂的接触力控制方法，包括如下步骤：

step1：设定期望的接触力fd；

step2：获取接触过程中的实际接触力fs；

step3：串联弹性执行器开环传递函数为通过对速度环化简，kvp＝jmωsc，kvi＝bmωsc，单轴伺服机电一体化简化模型为

简化后系统的闭环传递函数模型为

其中，jm为电机转动惯量，bm为电机阻尼系数，ks为弹簧的弹性系数，s为拉氏变换复变量算子，n为电机旋转运动转换成直线位移的转换系数，kpp为位置环的比例系数，ωsc为速度环的截止频率，kvp为速度环的比例参数，kvi为速度环的积分参数，kp为力控制器的比例系数，kd为力控制器的微分系数；

step4：确定串联弹性执行器重力的大小mg，同时算出不同姿态下重力与接触力方向之间夹角α的余弦值cosα，cosα表达式为

其中，q2，q3，q4，q5，q6为机械臂第2～6个关节角的角度，β为机械臂第六个关节的初始角度。得出估计重力补偿量为其中m为串联弹性执行器的质量，g为重力加速度；

step5：设计pd力控制器，传递函数表达式为c(s)＝kp+kds；

step6：将步骤step1中的期望接触力fd和步骤step2中的实际接触力fs作比较后，输出给步骤step5中的pd控制器，得出位移增量yout＝(kp+kds)(fd-fs)；

step7：获取电机的当前实际位置p；

step8：将步骤step6中的位移增量yout与步骤step7中的电机实际位置p以及估计重力的补偿量相加，作为输出信号输出信号μ1给电机，从而控制电机下一步到达的位置，使得弹簧的压缩量保持恒定值，进而实现恒力接触。

其中，接触力与弹簧压缩量的关系：fs＝ksδx，其中，δx＝xm-xl，xm为电机端输出直线位移，xl为负载端位移，δx为弹簧压缩量。

位移传感器的数字信号与弹簧压缩量δx的关系d＝k2δx+b1。其中，k2为数字信号和弹簧压缩量的系数关系，b1为弹簧初始压缩量对应的位移传感器数字信号值。

由此可得，接触力与位移传感器数字信号的关系为

fs＝ks(d-b1)/k2

作为上述技术方案的优选，所述步骤1中的串联弹性执行器包括：滑动支座、导向杆、弹簧、导向杆固定支架、固定支座及法兰盘，所述的导向杆通过通孔穿插在滑动支座内，滑动支座的前后各穿两组弹簧，弹簧由导杆固定支架进行固定；固定支座用于将串联弹性执行器进一步固定在机械臂上；法兰盘固定于导向杆的最前端。

本发明的有益效果在于：相对于单一的位置控制方式，能够显著提高机械臂末端的柔顺性能，实现对接触力的控制；相对于主动柔顺控制方式，无需使用昂贵的高精度力传感器，采用直线位移传感器检测弹簧压缩量即可实现对力的采集，大大降低了成本。通过对重力估计与补偿，提高接触力的控制精度。

附图说明

图1是本发明方法采用的串联弹性执行器的立体结构图。

图2是本发明方法采用的基于串联弹性执行器的力控制原理图。

图3是本发明的仿真实验结果曲线图。

图4是本发明的实验平台。

图5a～图5b是本发明的实验结果图，其中图5a表示无重力补偿情况下接触力变化，图5b表示有补偿情况下接触力变化。

具体实施方式

以下结合附图和实施例对本发明的技术方案作进一步描述。

如图1所示，本发明的一种基于串联弹性执行器的六关节机械臂接触力控制方法，其采用的串联弹性执行器的结构包括：滑动支座1、导向杆2、弹簧3、导杆固定支架4、固定支座5、法兰盘6。

滑动支座1固定在串联弹性执行器滚珠丝杆轴(主要由线性模组构成)的滑块下方，导向杆2通过通孔穿插在滑动支座1内；滑动支座1的前后各穿两组弹簧3，分别用导杆固定支架4进行固定；固定支座5呈u型状，通过六角螺母固定在机械臂末端，对串联弹性执行器进一步固定；法兰盘6呈扁圆柱型，固定于导向杆的最前端，上面带有螺纹孔，用来安装tcp接触导杆。

如图2所示，本发明是一种基于串联弹性执行器的六关节机械臂接触力控制方法，六关节机械臂采用的是史陶比尔的tx90，串联弹性执行器安装在其末端，串联弹性执行器原理为：采用ks1-15弹簧自复位直线位移传感器进行检测，其工作量程15mm，标准电阻5k。当弹簧产生不同形变时，其反应出不同的数字信号d(数字信号的范围为0～4096)。利用i/o模块带的12位精度adc进行实时检测，将检测出的位移传感器数字信号d转化为实际接触力fs，再与设定的期望接触力fd作比较后，输出给pd控制器。经过pd控制器，得出位置增量yout，再与读取的电机实时位置p和重力的补偿量相加，作为输出信号μ1输出给电机，实现重力的前馈补偿控制，从而控制电机下一步到达的位置，使得弹簧的压缩量保持恒定值，进而实现恒力控制。其中，表1为接触力fs和数字信号d的对应关系。

表1

本发明方法的具体实施步骤如下：

step1：设定期望的接触力fd；

step2：根据实验测得的接触力fs与弹簧形变量δx＝xm-xl的关系以及弹簧形变量δx与位移传感器数字信号d之间的关系，得出接触力与位移传感器数字信号d之间的关系；根据该关系，将12位分辨率的adc所采集的自复位线性位移传感器的数字信号d转化为接触过程中的实际接触力fs＝ks(d-b1)/b2，其中，k2为数字信号和弹簧压缩量的系数关系，b1为弹簧初始压缩量对应的位移传感器数字信号值。

step3：串联弹性执行器开环传递函数为通过对速度环化简，kvp＝jmωsc，kvi＝bmωsc，单轴伺服机电一体化简化模型为

简化后系统的闭环传递函数模型为

其中，jm为电机转动惯量，bm为电机阻尼系数，ks为弹簧的弹性系数，s为拉氏变换复变量算子，n为电机旋转运动转换成直线位移的转换系数，kpp为位置环的比例系数，ωsc为速度环的截止频率，kvp为速度环的比例参数，kvi为速度环的积分参数，kp为力控制器的比例系数，kd为力控制器的微分系数；

step4：确定串联弹性执行器重力的大小mg，同时算出不同姿态下重力与接触力方向之间夹角α的余弦值cosα，得出估计重力补偿量为其中，m为串联弹性执行器的质量，g为重力加速度；

step5：设计pd力控制器传递函数为c(s)＝kp+kds，并确定pd力控制器的参数；

step6：将步骤step1中的期望接触力fd和步骤step2中的实际接触力fs作比较后，输出给步骤step5中的pd控制器，得出位移增量yout＝(kp+kds)(fd-fs)；

step7：获取电机的当前实际位置p；

step8：将步骤step6中的位移增量yout与步骤step7中的电机实际位置p以及估计重力的补偿量相加，作为输出信号输出信号μ1给电机，从而控制电机下一步到达的位置，使得弹簧的压缩量保持恒定值，进而实现恒力接触。

通过对step5中添加重力补偿量与不添加重力补偿量的实验结果进行对比，比较接触力的控制结果。仿真实验结果如图3所示，实验平台如图4所示，实验结果如图5所示。

步骤step2中的实际接触力由采集的位移传感器数字信号d转化得到，利用ati公司的mini45net六维力/力矩传感器，多次测量，得出接触力与位移传感器数字信号d之间的关系。

其中，接触力与弹簧压缩量的关系：fs＝ksδx，ks为弹性系数，δx为弹簧压缩量。

位移传感器的数字信号d与弹簧压缩量δx的关系d＝k2δx+b1。其中，k2＝270，b1＝300。

由此可得，接触力与位移传感器数字信号的关系

fs＝ks(d-b1)/k2。

其中，step4中重力与接触力方向之间夹角的余弦值cosα，由机械臂不同姿态下各个关节角来得到的，表达式为

其中，q2，q3，q4，q5，q6为机械臂第2～6个关节角的角度，β为机械臂第六个关节的初始角度，β＝59.65。

其中，步骤step7中的电机实际位置p，由codesys软件中mc_readactualposition(读取真实位置)功能块直接读取获得。

本发明的优点是：采用串联弹性执行器实现机械臂的柔顺力控制，同时通过对执行器的重力进行补偿，提高力控制精度，满足工业上对精加工的要求，采用串联弹性执行器，提高机械臂作业的安全性，低阻抗。通过使用普通的直线位移传感器代替昂贵的力传感器，大大降低了成本。

本说明书实施例所述的内容仅仅是对发明构思的实现形式的列举，本发明的保护范围不应当被视为仅限于实施例所陈述的具体形式，本发明的保护范围也及于本领域技术人员根据本发明构思所能够想到的等同技术手段。