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

顾 涛1，万佑红2

（1.苏州工业职业技术学院，江苏 苏州 215000；2.南京邮电大学 自动化学院，江苏 南京 210042）

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

关键词：关节一体化机器人；拖拽示教；动力学；阻抗控制

1 引言

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

从全国机器人技术发展现状来看，机器人动力学技术和阻抗控制已经逐渐从实验室的理论阶段逐步走向实际现场的应用阶段。文献[2]指出，针对基于动态动力学模型的的四足机器人做了深入的研究，将动力学模型映射到机器人的腿部轨迹。文献[3]指出，对6自由度并联机器人采用基于粒子群优化的方法解决了的正运动学问题。文献[4]指出，提出了一种高阶机器人动力学的安全轨迹生成算法。文献[5]指出，对移动机器人运动控制和动力学算法控制做了深入的研究。

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

2 关节一体化机器人

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

图1 关节一体化机器人结构图
Fig.1 Joint Integrated Robot Structure Chart

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

3 工业机器人动力学建模

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

3.1 动力学控制结构

图2 关节一体化机器人动力学控制框图
Fig.2 Dynamics Control Diagram of Joint Integrated Robot

在控制精度和高性动态控制指标的要求下，传统的控制很难实现，如图2所示。采用力矩前馈控制，动力学前馈补偿控制，这种方法已经被应用到非线性动力学模型中去了，在这种控制率中不需要以伺服速率进行复杂的耗时运算。动力学前馈补偿是将基于模型的部分放到了伺服环的外面[9]。

3.1.1 离心力和哥氏力项

按Chiristoffell符号法，离心力和哥氏力项的C矩阵可由M矩阵求得：

故在求出M矩阵后代入Chiristoffell公式，可得到C矩阵。

3.1.2 重力项

机械手总的重力势能为：

式中：g—机器人基坐标系的重力向量，V（q）与关节变量q相关，而与关节速度q˙无关。

重力项：

3.1.3 摩擦模型

完整的动力学模型表达式为：

式中：Fv∈Rn×n、Fs∈Rn×n—粘性摩擦和静摩擦系数对角矩阵；

向量 sgn（q˙）∈Rn×1—关节速度的符号函数。

模型中的摩擦项能够很大的程度上提高链式机器人的模型精度，而动力学方程中的参数可以通过系统辨识算法和实验数据获得。

3.2 牛顿-欧拉动力学

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

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

（2）操作臂的模型是不精确的。

4 阻抗控制算法控制

4.1 笛卡尔空间推广

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

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

式中：xθ，d—笛卡尔坐标系下期望电机位置的映射。

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

新的阻抗控制律为：

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

4.2 无源性分析

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

无源性的分析针对闭环控制系统。非线性系统中对输入输出无源性的定义为：对于输入输出分别为u和y的系统来说，其是无源的指的是存在一个连续的有界存储函数S，并且S对时间的导数满足S˙≤yTu。

机器人的环境常常为描述为无源映射（q→τext）。方程的其无源映射为（（τ+τext）→q˙），由于纯物理系统的原因具有明显的无源性，并且可以选择存储函数为：

其中V（q）如第2张所述，为机器人的重力势能：

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

根据方程，存储函数 Sq（q，q˙）的微分为：

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

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

4.3 稳定性分析

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

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

选择Lyapunov函数：

可以看出 V（q，q˙，θ，θ˙）主要包括动能项和势能项量部分，动能项为：

由于惯性矩阵 M（q）和 Jθ是正定的，所以 Vkin（q，q˙，θ˙）是正定的。

势能项为：

刚度矩阵K和弹性矩阵Kd是正定的，所以Vpot（q，θ）是正定的。由于已经证明 Vkin（q，q˙，θ˙）也是正定的，故 Lyapunov函数 V（q，q˙，θ，θ˙）是正定的。

根据方程可以求出 V（q，q˙，θ，θ˙）的微分为：

由于阻尼矩阵 Dd是正定的，故V˙（q，q˙，θ，θ˙）是负定的，根据Lyapunov稳定性原理可以得到在平衡点附近闭环系统是稳定的。根据系统方程，除了平衡点外不存在其他轨迹点满足x˙θ=0，根据LaSalle不变原理，系统状态将收敛于满足大正定不变集x˙θ=0，故系统满足渐进稳定性。通过稳定性的分析可以得到关节空间阻抗控制器的全局稳定性，由于当前的笛卡尔阻抗控制器是定义的是局部笛卡尔坐标下的阻抗，故阻抗控制器只能得到局部稳定性。

5 算法验证

5.1 动力学验证

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

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

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

图3 采集扭矩与动力学计算值比较曲线
Fig.3 Comparison Curve of Sampled Torque and Dynamic Calculation

通过对比二者关系可以看出，计算动力学与关节转矩变化曲线保持一致，验证了方法的有效性。

5.2 阻抗控制器验证

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

图4 拖曳式示教中电机转矩曲线图
Fig.4 The Motor Torque Curve in Process of Towing Teach

根据拖曳式示教过程中所采集的电机转矩曲线可以看出，不开启阻抗控制的执行模式下，电机转矩范围在（0.22～0.33）Nm间，拖曳式示教启动阶段反向的电机转矩可以作为外力输入的判断。当拖曳判断进入运行阶段，电机转矩范围在（0.1～0.2）Nm之间，相当于感应到的外部反应转矩τext约为0.1Nm。机器人连杆长度约0.5m，则转换到机器人末端的外作用力Fext约为0.2N。

传统的采用六维力传感器的方式，传感器检测的精度一般为满量程的百分之一，以传感器量程20N为例，末端的力保护精度约为0.2N。

对比可见，基于模型的力/力矩检测方法在检测和保护精度上与采用六维力传感器的方式相当。同时，从曲线可以看出，电机转矩在10ms左右增加约0.1Nm时，也就是采用电机转矩的阻抗控制反应时间为10ms，满足系统需求。且该阻抗控制方法成本低廉，只采用机器人本身的电流和位置传感器，且不会因传感器的安装降低机器人自身的刚性，具有一定的优越性。

6 结论

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

参考文献

［1］S.Iwashita Developing a service robot，IEEE International Conference on Mechatronics and Automation，ICMA 2005，2005：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 InternationalConferenceonAdvancedIntelligentMechatronics，2009（5）：1637-1642.

［7］Performance improvement of tracking control for a planar parallel robot using synchronized control［C］.International Conference on Intelligent Robots and Systems October 9-15，2006，Beijing，China.

［8］ Honghai Liu，Senior Member，David J.Brown，George M.Coghill.Fuzzy qualitative robot kinematics［J］.Fuzzy Systems，june 2008：808-822.

［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．

Research on Dynamics and Impedance Control Algorithm of Joint Integrated Robot

GU Tao1，WAN You-hong2
（1.Suzhou Institute of Industrial Technology，Jiangsu Suzhou 215000，China；2.School of Automation，Nanjing University of Posts and Telecommunications，Jiangsu Nanjing 210042，China）

Abstract：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.

Key Words：Joint Integrated Robot；Drag Teach；Dynamics；Impedance Control

中图分类号：TH16；TP242.3

文献标识码：A

文章编号：1001-3997（2017）11-0259-04

来稿日期：2017-05-20

基金项目：江苏高校品牌专业建设工程资助项目（PPZY2015B186）；国家自然科学基金项目（81071234）

作者简介：顾 涛，（1977-），男，江苏苏州人，硕士研究生，高校讲师，主要研究方向：机械结构设计及有限元分析、机器人技术；

万佑红，（1974-），女，江苏南京人，硕士研究生，硕士生导师，副教授，主要研究方向：控制理论、方法与技术及其在通信中的应用