基于电流的力控制技术与安全保护研究*

邹风山，赵 彬

(1. 沈阳新松机器人自动化股份有限公司， 沈阳 110168；2. 中国科学院 沈阳自动化研究所， 沈阳 110016；3. 东北财经大学 工商管理学院，辽宁 大连 116025)

基于电流的力控制技术与安全保护研究*

邹风山1,2，赵 彬1,3

(1. 沈阳新松机器人自动化股份有限公司， 沈阳 110168；2. 中国科学院 沈阳自动化研究所， 沈阳 110016；3. 东北财经大学 工商管理学院，辽宁 大连 116025)

文章解决了电机电流进行机器人主动柔顺控制的问题，同时对七自由度关节一体机器人进行拖曳式示教实验。首先针对关节一体化机器人进行运动学分析，采用多层前向神经网络解决机器人运动学冗余轴反解问题，将复杂的非线性优化问题转化为简单的代数方程组求解问题。然后由于机器人特殊的构型及关节耦合复杂，采用牛顿—欧拉动力学建模。将永磁同步电机的转矩模型与机器人动力学模型结合，得到基于电流模型的力检测。最后实现基于电机电流的机器人主动柔顺控制，并在七自由度关节一体机器人上进行拖曳式示教实验，实际运行结果验证了算法的有效性。

电流力控制；神经网络；机器人动力学；拖曳示教

0 引言

随着机器人技术的兴起，为了能够满足批量化生产和实际工程的需求，独立决策、机器人感知和最优控制等功能越来越受到国内外学者的关注。新一代机器人在提高生产效率的同时，要求具有安全性，能够在发生危险时及时保护机器人自身和外部环境的安全[1]。并且往往具有简单易用的示教功能，只需用人手把持机器人的“手腕”，直接拖动机器人进行示教工作，机器人内部控制系统会自动记录示教过程的运动轨迹。对工业机器人力控制的研究主要使用其力矩传感器来实现，通过ATI力矩传感器来获得精确的力矩，再对机器人实现力位混合控制。而在这些新兴技术背后是近年来随着机械、控制、传感三方面技术的进步，迸发出崭新活力的机器人主动柔顺控制这一经典研究课题。

鉴于此，基于电机电流或转矩传感器的机器人力控制属于关节转矩力控制。它通过直接检测和控制机器人的关节转矩来达到控制机器人各关节动作，最终使机器人末端执行器实现各种功能运动和操作任务的目的。可以说，关节转矩力控制技术的研发对新一代工业机器人、协作机器人简易示教与碰撞保护，医疗康复机器人的主动运动与被动运动，工业机器人的软浮动装配，服务机器人手臂的碰撞保护等领域都具有重要的应用意义。

根据传感方式的不同，多种策略被应用于机器人主动柔顺控制。对此，国内外学者做了比较深入的研究。文献[2]中Daniel Kruse和John T. Wen将Smith-Åström预测器应用到了机器人力控制技术中[2]。文献[3]中Alexander Winkler和Jozef Suchy将末端效应器速度的变化作为力控制误差的函数改善了力控制轮廓跟随算法，可以将这种力控制应用于诸如抛光，去毛刺或磨削的表面精加工任务中[3]。F. M. Kulakov通过使用控制对象的详细动态模型分析现有力/转矩控制方法的结果，解决了弹性机器人力控制理论的泛化问题[4]。H. P. Qian，J. De Schutter通过阻尼和低通滤波在离散时间稳定性中的作用实现了机器人的力控制[5]。

以往前人研究的力控制算法都是基于力矩传感器实现的。虽然效果较好，但是力矩传感器价格较高，不适用于批量生产。本文为了改善这一缺点，提出了基于电流的力控制算法。首先建立机器人运动学方程，运用神经网络技术对运动学进行训练学习来更新权值，间接求解冗余轴数值解，其次构建动力学方程，并利用牛顿欧拉法对动力学进行简化。再次建立机器人与环境相互作用的反应转矩模型。最后在动力学控制、关节转矩控制的基础上，将位置控制与力控制结合，实现主动柔顺控制，并进行拖曳式示教验证，为机器人批量化生产奠定了基础。

1 七自由度机器人机械结构

如图1所示，为七自由度机器人机械构型。在机械设计上，有前期开发的自重30kg、负载5kg的手臂；直流版手臂采用自主研制的一体化关节；各驱动元件及末端执行器的线缆均高度集成于一体化关节内部而非裸露在手臂外侧。

图1 七自由度机器人机械构型

本文在该七自由度机器人关节转矩力控制技术可以实现的功能包括：①机器人拖曳式示教；②机器人碰撞保护；③机器人软浮动技术；④机器人安全柔顺控制等。

2 机器人运动学建模

七自由度机械手比以往的普通工业六轴机械手多了一个自由度，该自由度的加入使得机器人会产生无数解。冗余轴的加入会给机器人模型推导带来一定困难，使用以往机械运动学算法难以解决，本文采用神经网络算法，改善了以往运动学算法的不足。并且可以较快的得到最优解。为后续研究奠定了一定基础。

2.1 正运动学建模

表1 机械臂的连杆DH参数

(1)

(2)

2.2 逆运动学建模

建立如图2所示的机器人连杆坐标系框图，进而求解逆运动学方程。

图2 七自由度机器人连杆坐标系框图

2.2.1 求解第四轴θ4

这里，先引入肘部关节翻转与否标识elbow；

推导出：

(3)

θ4求解完毕。

鉴于此，采用神经网络算法，可以不需要考虑机器人无数解的情况，采用最优解来得到运动学算法。

2.2.2 求解第一轴θ1

图3 三层前向神经网络结构

将肘部关节位置代入公式(4)中，

(4)

如图所示，θ1=-α；

(5)

θ1求解完毕。

2.2.3 求解第二轴θ2

如图所示，因为θ2=-γ，显然可以得到公式如下：

(6)

θ2求解完毕。

2.2.4 求解第三轴θ3

已知：

两边元素(2，4)和元素(2，4)：

设常量：

利用arctan2可以解出唯一解，

(7)

θ3求解完毕。

2.2.5 求解腕关节

上面已经求得θ1、θ2、θ3、θ4。

求解θ5：

(1)由于第一行，第二列相等可以得到：

(2)由于第三行，第二列相等可以得到：

(8)

考虑θ5的值受θ6的影响，如果θ6=0，则θ5为任意解。

求解θ7：

(1)由于第二行，第一列相等可以得到：

(2)由于第二行，第三列相等可以得到：

θ7=arctan2(s6s7,s6c7)

(9)

考虑θ7的值受θ6的影响，如果θ6=0，则θ7为任意解。在这个情况下，所以结果都是θ5和θ7的和或者差。

求解θ6：

(1)由于第二行，第三列相等可以得到：

(2)由于第二行，第二列相等可以得到：

(10)

先求θ4、θ6，由于周期性，必须进行跟踪求解。

由于操作臂关节(翻转)可以得到2组解：

(11)

θ4、θ5、θ6求解完毕。

3 工业机器人动力学建模

基于电流的力控制技术首先要解决的就是动力学建模[7-9]。本次开发的动力学模型是在加入EtherCAT的基础上，RC每4ms周期计算1次动力学结果，发送给力控制补偿器，力控制补偿器根据其计算结果来控制位置指令输出给驱动器。

3.1 机械参数

七自由度工业机器人的理论基本机械参数由软件计算得到。惯性张量可以用3×3矩阵表示如下：

(12)

表2为七自由度关节一体化机器人的惯性张量参数。

表2 机器人的机械参数

3.2 牛顿-欧拉动力学

牛顿-欧拉外推法

外推：i：0—6

(13)

(14)

牛顿-欧拉内推法

内推：i：7—1

(15)

4 机器人力控制

4.1 基于电流的力控制

完整的机器人动力学方程为：

(16)

方程(16)代表以时间t为非独立变量的系统非线性微分方程。方程的左边各项含义为(n为机器人的关节数也是自由度数)：

M∈Rn×n：关节空间惯性矩阵(对称，正定)；

C∈Rn×n：哥氏力和向心力计算矩阵；

G∈Rn×1：重力项向量；

q∈Rn×1：广义关节偏差向量；

τ∈Rn×1：广义驱动转矩向量；

τ=Nτm

(17)

并考虑电机角度与关节角度传递关系，得到计及电机转子动力学的完整动力学模型为：

(18)

(19)

在机器人主动柔顺控制的研究中，将基于电流模型的力检测与力/位混合控制结合，设计基于模型的新的力/位混合控制结构如图4所示。

图4 电流力控制结构图

图4中关节转矩的传感采用电机电流，外部转矩τext通过基于电流模型的力检测获得，并反馈给力闭环进行柔顺控制。每个关节既有位置控制器，又有力控制器。为了根据约束条件改变每个自由度所要求的控制模式，图中引入了选择矩阵S和S′，其为3×3矩阵，是两组互锁开关。如要求第i个关节进行位置(或力)控制则矩阵S(或S′)对角线上的第i个元素为1，否则为0。

4.2 实验

为验证基于电流模型的力检测和基于模型的力/位混合控制结构的有效性，采用关节一体化七自由度机器人为控制对象，进行拖曳式示教实验。

图5所示，为拖曳式示教中电机转矩曲线图。可以看出，拖曳式示教时由于手部施加给机器人的外作用力，电机转动带动机器人的运动方向与外力方向一致，电机转矩比执行模式小，此时电机转动起到助力作用。在正反向运动的起始阶段，电机电流较大，这是由于起始段机器人处于静止状态，反向的电流起到运动启动的作用。实验过程中机器人运行平稳，满足拖曳式示教的需求。

图5 拖曳式示教中电机转矩曲线图

5 结论

本文针对采用力矩传感器进行力控制的技术方案成本较高的缺点，提出了一种基于电机电流模型的力控制方法。本文采用多层前向神经网络解决机器人运动学冗余轴多解的问题，将复杂的非线性优化问题转化为简单的代数方程组求解问题。然后利用牛顿—欧拉简化计算系统模型。最后进行了拖曳式示教实验，通过电流力控制方法简化了主动柔顺控制功能。经过实验验证了该方法的有效性，为后续机器人算法研究和开拓机器人市场奠定了基础。

[1] 姜昆鹏，陈敏,丁建军. 六自由度并联机构的控制系统及仿真试验探究[J], 组合机床与自动化加工技术, 2016(9): 43-46.

[2] Daniel Kruse，John T. Wen. Application of the Smith-Åström Predictor to robot force control[C]. 2015 IEEE International Conference on Automation Science and Engineering (CASE), 2015: 383-388.

[3] Alexander Winkler，Jozef Suchy. Force controlled contour following on unknown objects with an industrial robot[C]. Robotic and Sensors Environments (ROSE), 2013: 208-213.

[4] F M Kulakov. Generalization of robot force control theory for elastic robot[J]. roceedings of the IEEE/RSJ/GI International Conference on, 1994,11(1): 1610-1617.

[5] H P Qian，J De Schutter. The role of damping and low pass filtering in the stability of discrete time implemented robot force control[J]. Robotics and Automation, 1992，2(11): 1368-1373.

[6] Guivant J, Nebot E, and Baiker S. Autonomous navigation and map building using laser range sensors in outdoor applications[J]. Journal of Robotics Systems, 2000, 17(10) : 565-583.

[7] 史旭东，庄立东，郭显鹏. 基于PSO的6R装配机器人手臂刚度优化[J].组合机床与自动化加工技术, 2016(10): 40-43.

[8] 赵彬. 基于机器视觉的工业机器人分拣技术研究[D]. 沈阳：沈阳工业大学，2013.

[9] Koren Y，Borenstein J. Potential field methods and their inherent limitations for mobile robot navigation[C].In: Proceedings of the 1991 IEEE International Conference on Robotics and Automation. Sacramento，California：IEEE，1991; 1398-1404.

(编辑 李秀敏)

Autonomous Mobile Robot Map Building, Localization and Path Planning

ZOU Feng-shan1,2，ZHAO Bin1,3

(1. SIASUN Robot & Automation Co., Ltd., Shenyang 110168，China; 2. Shenyang Institute of Automation, Chinese Academy of Science, Shenyang 110016, China)

Based on the motor current, the active compliant control of the robot is studied, and the drag-and-drop teaching experiment is carried out for the 7-DOF joint one-body robot. Firstly, the multi-layer feedforward neural network is used to solve the inverse kinematics problem of the robot kinematics, and the complex nonlinear optimization problem is transformed into a simple algebraic equation group for solving the kinematic analysis of the joint-integrated robot. Then, due to the special configuration of the robot and the complicated coupling of the joints, the Newton-Euler dynamics model is adopted. The torque model of the permanent magnet synchronous motor is combined with the robot kinetic model to obtain the force detection based on the current model. Finally, the active compliant control of the robot based on the motor current is realized, and the teaching experiment of the robot is carried out. The actual running result verifies the effectiveness of the method.

current force control; neural networks; robot dynamics; drag teaching

1001-2265(2017)08-0100-05

10.13462/j.cnki.mmtamt.2017.08.025

2016-11-03；

2016-11-22

国家科技重大专项：硅片集成传输系统研发和示范应用(2014ZX02103)；国家高技术研究发展863计划(2012AA041405)；沈阳市工业科技攻关项目(F12-010-2-00)

邹风山(1978—)，男，山东泰安人，沈阳新松机器人中央研究院院长，博士，研究方向为机器人控制与应用，(E-mail)zoufengshan123@sohu.com。

TH162;TG506

A