关节一体化机器人动力学与阻抗控制算法研究*

李文锦，竺晓程

(1. 福建电力职业技术学院 机电工程系，福建 泉州 362000；2. 上海交通大学 机械与动力工程学院，上海 200240)

关节一体化机器人动力学与阻抗控制算法研究*

李文锦1，竺晓程2

(1. 福建电力职业技术学院 机电工程系，福建 泉州 362000；2. 上海交通大学 机械与动力工程学院，上海 200240)

文章针对关节一体化机器人在高速复杂运动下能保持手臂平滑运动同时具有拖曳式示教的功能问题。首先文章利用DH法建立了转换坐标系，得到了各关节连杆的相对关系，运用弦位迭代的算法解决了手臂运动解耦问题。其次，针对关节一体化机器人空机械手的结构特点，建立了用牛顿—欧拉法的动力学方程。再次，在分析阻抗控制原理的基础上，设计了笛卡尔空间基于力的阻抗控制结构，并在关节空间实现笛卡尔空间经典阻抗控制问题。最后在机器人实验平台，基于关节转矩反馈设计阻抗控制器，并进行阻抗控制实验，实现机器人拖曳式示教功能，验证了所设计算法的有效性。

关节一体化机器人；拖拽示教；阻抗控制

0 引言

在航空航天、军工以及核工业等领域，作业环境通常具有空间狭小、危险性大等特点。以双臂一体化机器人代替人工进行作业尤其适应此种场景。双臂一体化机器人的手腕机构如果需要达到拟人化动作时，在工作任务时需要保持手臂稳定不抖动工作，从而避免加速度过大抖动导致降低重复精度，提高作业的正常执行和生产率是一个关键性的问题[1]。鉴于此，针对关节一体化机器人研究其动力学算法和阻抗控制算法是对机器人大环境发展的必然趋势。

从全国机器人技术发展现状来看，Krzysztof Tchoń和Janusz Jakubiak针对移动机器人逆向运动学做了深入的研究[2]。H R Choi与S M Ryew提出了一种双主动万向节两自由度的拟人手腕关节机械臂，运动模式可模仿人类手臂[3]。Raghavan M与Roth B通过对机械臂奇异位置的处理和迭代阻尼因子的调整，实现了机械臂实时运动学求解[4]。Luc Rolland和Rohitash Chandra针对移动机器人的逆向运动学做了深入的研究[5]。

本文首先将关节一体化机器人双臂分为两个单臂进行研究，再将单独手臂分为位置部分、姿态部分两个部分。文献中所述算法在迭代计算过程中收敛域较窄，且无法解决关节与方程数不相等的问题。本算法通过降低所求解运动学方程维度的方式解决了这些问题。然后根据已知机械参数详细分析了如何建立动力学模型。最后提出基于无源性理论对关节空间基于力的机器人阻抗控制策略进行理论分析，引入一种新的关节转矩反馈实现对电机转子惯量的整定，从物理上对关节转矩反馈进行解释，从而实现了拖拽示教。

1 关节一体化机器人

如图1所示为关节一体化机器人，该机器人拥有两个单臂，关节一体化机器人控制系统的特点是分层开放式系统，便与维护与升级。

图1 关节一体化机器人结构图

直流版手臂采用自主研制的一体化关节，在手臂安装尺寸上采用和交流版手臂尽量接近的结构，以便共享同一模具，节约成本。关节一体化机器人拥有小型化设计，以及中空孔结构。执行元件中央的贯通孔内可穿过配线、配管、激光等，简化了机械装置的整体构造。并且具备可变选项多样化的特点，可根据需求选择输出轴承、减速机、电动机线圈等。手臂内置肘部电机与腕部电机的驱动板卡，便于后期的调试与维护。同时手臂内可安装触摸传感器模块，通过对人体的感知可做出相应的语音或表情等动作，提高与人的互动性与趣味性。

2 动力学建模

动力学分析的方法有很多种，如Lagrange方法、Newton-Euler方法、Gauss方法、Kane方法、旋量(对偶数)方法和Roberson-Wittenberg方法等。本文分别求取惯性力项、离心力和哥氏力项、重力项。此方法可以分别求取各项，便于应用到多关节机器人中。

对于任何机械系统，拉格朗日函数L定义为系统的动能Ek和势能Ep之差：L=Ek-Ep。

系统的动力学方程(称第二类拉格朗日方程)为：

(1)

当主动力中有非势力时：

当含有粘性阻尼时，方程变为：

(2)

2.1 拉格朗日动力学

2.1.1 机器人动能

首先讨论操作臂动能的表达式，第i根连杆的动能可以表示为：

(3)

公式中第一项是由连杆质心线速度产生的动能，第二项是由连杆的角速度产生的动能，整个操作臂的动能是各个连杆的动能之和，即：

将上面公式带入动能公式，得到：

2.1.2 机器人势能

(4)

C矩阵：

C=[cij]i=1,…,n,j=1,…,n,

cij,k=cik,j(对称)

离心力和哥氏力各资料中都统一按Chiristoffell符号法：

其中：

或写成：

cij,k=cik,j

2.1.3 重力项

将上面公式化简为：

(5)

M∈Rn×n：关节空间惯性力项(对称、正定)。

C∈Rn×n：耦合力项，包括向心力和(哥氏力)科里奥利力。

G∈Rn×1：重力项；

θ∈Rn×1：关节状态向量；

τ∈Rn×1：一般激励向量。

2.2 动力学框图

在控制精度和动态控制指标的要求下，传统的控制效果很差，难以满足较高的性能作业指标需求[9]。如图2所示，本次开发的动力学模型是在加入EtherCAT的基础上，机器人控制器每10ms周期计算1次动力学结果，发送给驱动器，将其按前馈方式加到速度环的计算结果，输出给驱动器。

图2 柔性多关节机器人动力学控制框图

这种理想的控制是不容易实现的，其中最重要的因素包括：

(1)数字计算机的特性是离散的，而不是理想的连续时间控制律。

(2)操作臂的模型是不精确的。

3 阻抗控制

3.1 笛卡尔空间推广

机器人的应用和操作通常都在笛卡尔空间描述，阻抗控制行为也是在笛卡尔坐标下定义的，笛卡尔坐标描述的是机器人末端执行器的位置和姿态。从关节空间到笛卡尔空间的变换需要正运动学和雅克比矩阵，假设二者已知，则方程可以方便地推广到笛卡尔空间。

正运动学的计算使用电机角度转换到关节后的角度θ，则：

其中，xθ,d是笛卡尔坐标系下期望电机位置的映射。

而关节速度与机器人末端速度的转换关系为：

新的阻抗控制律为：

由此得到笛卡尔坐标系下新的闭环控制系统为：

(6)

(7)

3.2 无源性分析

总结带有重力补偿的关节转矩阻抗控制，完整的控制律为：

其中V(q) 为机器人的重力势能：

重力势能的有界性将影响系统无源性的分析。由机器人重力项的性质可知，重力项G(q)和重力势能V(q)是有界的。即存在β>0,使得：

|V(q)|<β∀(q)∈Rn

方程的无源性类似，可以选择存储函数为：

(8)

根据无源性定理，两个无源系统的(负)反馈连接仍是无源的。由于方程则由这两个子系统内部反馈连接所组成的闭环系统也是无源的，闭环系统的无源性对于系统的鲁棒稳定性具有重要意义。

3.3 稳定性分析

为证明系统的稳定性，考虑稳定状态关节角度为θ0，相应的稳定状态满足：

J(θ0)TKd(f(θ0)-xθ,d)=0

且稳定状态的输入转矩仅为重力项：

τm,0=τ0=N-1G(θ0)

选择Lyapunov函数：

(9)

(10)

4 算法验证

4.1 动力学验证

为验证本文方法的正确性，将惯性参数、摩擦参数辨识的结果代入完整的动力学方程计算伸缩轴转矩：

同时，通过采集电机电流isq计算关节转矩。

在整个伸缩轴运动过程中，对比二者的数值曲线如图3所示。

图3 采集扭矩与动力学计算值比较曲线

通过对比二者关系可以看出，计算动力学与关节转矩变化趋势保持一致，验证了本文方法的有效性。两个曲线差异在摩擦力部分，因为本方法只补偿动力学部分，未补偿摩擦力。

4.2 阻抗控制器验证

图4为水平伸缩两方向运动时的电机转矩曲线，拖曳式示教时由于手部施加给机器人的外作用力，电机转动带动机器人的运动方向与外力方向一致，电机转矩比执行模式小，此时电机转动起到助力作用。在正反向运动的起始阶段，电机电流较大，这是由于起始段机器人处于静止状态，反向的电流起到运动启动的作用。

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

根据拖曳式示教过程中所采集的电机转矩曲线可以看出，不开启阻抗控制的执行模式下，电机转矩范围在0.22Nm～0.33Nm间，拖曳式示教启动阶段反向的电机转矩可以作为外力输入的判断。当拖曳判断进入运行阶段，电机转矩范围在0.1 Nm ～0.2 Nm之间，相当于感应到的外部反应转矩τext约为0.1Nm。机器人连杆长度约0.5m，则转换到机器人末端的外作用力Fext约为0.2N。传统的采用六维力传感器的方式，传感器检测的精度一般为满量程的百分之一，以传感器量程20N为例，末端的力保护精度约为0.2N。对比可见，基于模型的力/力矩检测方法在检测和保护精度上与采用六维力传感器的方式相当。同时，从曲线可以看出，电机转矩在10ms左右增加约0.1Nm时，也就是采用电机转矩的阻抗控制反应时间为10ms，满足系统需求。且该阻抗控制方法成本低廉，只采用机器人本身的电流和位置传感器，且不会因传感器的安装降低机器人自身的刚性，具有一定的优越性。

5 结论

本文针对关节一体化机器人在高速复杂运动下需要能保持手臂平滑运动同时具有拖曳式示教的功能问题，详尽的介绍了动力学建模和阻抗控制算法的过程。针对关节一体化机器人的特点，根据已知的机器人的参数利用牛顿—欧拉法计算了动力学参数。同时提出一种阻抗控制算法。该算法通过关节转矩反馈，电机动力学与关节转矩反馈所组成的系统是一个无源子系统，同时刚性机器人的动力学也是一个无源子系统，二者构成两个内部反馈连接的两个无源系统，根据无源性定理，两个子系统内部反馈连接所组成的闭环系统也是无源的。最后在机器人实验平台，基于关节转矩反馈设计阻抗控制器，并进行阻抗控制实验，实现机器人拖曳式示教功能，验证了所设计算法的有效性。

[1] S Iwashita.Developing a service robot[J].IEEE International Conference on Mechatronics and Automation, 2005,2: 1057-1062.

[2] Carlos A Jara，Francisco A Candelas，Pablo Gil. An interactive tool for industrial robots simulation, Computer Vision and remote operation [J]. Robotics and Autonomous Systems，2011,59:389-401.

[3] Chung-Li Chen, Tso-Kang Wang, Chia-Jui Hu. Model-based dynamic gait in a quadruped robot with waist actuation[C].Intelligent Robots and Systems (IROS), 2016: 2153-0866.

[4] Sarah Tang, Vijay Kumar. Safe and complete trajectory generation for robot teams with higher-order dynamics[C].Intelligent Robots and Systems (IROS), 2016: 2153-0866.

[5] Jingjing Jiang, Pierluigi Di Franco. Alessandro Astolfi.Shared Control for the Kinematic and Dynamic Models of a Mobile Robot[J].IEEE Transactions on Control Systems Technology, 2016,24(6): 2112-2124.

[6] Luc Rolland，Rohitash Chandra. Forward kinematics of the 6-6 general parallel manipulator using Real Coded Genetic Algorithms[J]. IEEE/ASME International Conference on Advanced Intelligent Mechatronics, 2009(5): 1637-1642.

[7] Performance Improvement of Tracking Control for a Planar Parallel Robot Using Synchronized Control[C]. Beijing:International Conference on Intelligent Robots and Systems, 2006.

[8] 黄贺福,杨志宏,李娜.基于修正矩阵的刚柔混合模型公差分析方法[J]. 组合机床与自动化加工技术,2017(7):80-82.

[9] Gao X S，Huang Z． A characteristic set method for equation solving in finite fields[J]．Journal of Symbolic Computation, 2012, 47(6):655-679．

[10] Mou C Q，Wang D M，Li X L．Decomposing polynomial sets into simple sets over finite fields:the positive-dimensional case[J]．Theoretical Computing Science，2013，468:102-113．

ResearchonDynamicsandImpedanceControlAlgorithmofJointIntegratedRobot

LI Wen-Jin1，ZHU Xiao-cheng2

(1. Department of Mechatronic Engineering，Fujian Electric Vocational and Technical College，Quanzhou Fujian 362000，China; 2. School of Mechanical and Power Engineering, Shanghai Jiao Tong University, Shanghai 200240,China)

In this paper, the joint robot can maintain the smooth movement of the arm under the high speed and complicated movement, and has the function of dragging teaching. Firstly, the transformation coordinate system is established by DH method, and the relative relation of each joint is obtained. The algorithm of solving the arm motion is solved by using the iterative algorithm. Secondly, the dynamic equation of Newton-Euler method is established for the structural characteristics of joint robot. Thirdly, on the basis of analyzing the principle of impedance control, the impedance control structure based on force in Cartesian space is designed, and the classical impedance control problem of Cartesian space is realized in joint space. Finally, the impedance controller is designed based on the joint torque feedback of the robot experiment platform, and the impedance control experiment is carried out to realize the function of the robot towing teaching, which verifies the validity of the designed algorithm.

joint integrated robot; drag teach; impedance control

TH165;TG659

A

1001-2265(2017)12-0033-04

10.13462/j.cnki.mmtamt.2017.12.008

2017-08-03;

2017-08-25

国家自然科学基金(81071234)

李文锦(1979—)，男，福建仙游人，福建电力职业技术学院讲师，硕士，研究方向为机器人应用技术，(E-mail)liwenjinggg33@163.com。

(编辑李秀敏)