（1.苏州工业职业技术学院，江苏苏州215000；2.南京邮电大学自动化学院，江苏南京210042）
摘要：针对关节一体化机器人在高速复杂运动下能保持手臂平滑运动同时具有拖曳式示教的功能问题。首先利用DH法建立了转换坐标系，得到了各关节连杆的相对关系，运用弦位迭代的算法解决了手臂运动解耦问题。其次，针对关节一体化机器人空机械手的结构特点，建立了用牛顿—欧拉法的动力学方程。再次，在分析阻抗控制原理的基础上，设计了笛卡尔空间基于力的阻抗控制结构，并在关节空间实现笛卡尔空间经典阻抗控制问题。最后在机器人实验平台，基于关节转矩反馈设计阻抗控制器，并进行阻抗控制实验，实现机器人拖曳式示教功能，验证了所设计算法的有效性。
关键词：关节一体化机器人；拖拽示教；动力学；阻抗控制
在航空航天、军工以及核工业等领域，作业环境通常具有空间狭小、危险性大等特点。以双臂机器人代替人工进行作业尤其适应此种场景。双臂机械臂的手腕机构主要的功能是调节末端姿态，来实现对于特殊位置的抓取，手腕机构的紧凑度和灵活性直接决定了机械双臂是否适合在狭小空间内作业，并影响机械臂的定位精度。机器人手臂如果需要达到拟人化动作时，在工作任务时需要保持手臂稳定不抖动工作，从而避免加速度过大抖动导致降低重复精度，提高作业的正常执行和生产率是一个关键性的问题[1]。鉴于此，针对关节一体化机器人研究其动力学算法和阻抗控制算法是对机器人大环境发展的必然趋势。
从全国机器人技术发展现状来看，机器人动力学技术和阻抗控制已经逐渐从实验室的理论阶段逐步走向实际现场的应用阶段。文献[2]指出，针对基于动态动力学模型的的四足机器人做了深入的研究，将动力学模型映射到机器人的腿部轨迹。文献[3]指出，对6自由度并联机器人采用基于粒子群优化的方法解决了的正运动学问题。文献[4]指出，提出了一种高阶机器人动力学的安全轨迹生成算法。文献[5]指出，对移动机器人运动控制和动力学算法控制做了深入的研究。
首先将关节一体化机器人双臂分为两个单臂进行研究，在将单独手臂分为位置部分、姿态部分两个部分。文献中所述算法在迭代计算过程中收敛域较窄，且无法解决关节与方程数不相等的问题。本算法通过降低所求解运动学方程维度的方式解决了这些问题。然后根据已知机械参数详细分析了如何建立动力学模型。最后提出基于无源性理论对关节空间基于力的机器人阻抗控制策略进行理论分析，引入一种新的关节转矩反馈实现对电机转子惯量的整定，从物理上对关节转矩反馈进行解释，从而实现了拖拽示教。
关于关节一体化机器人，该机器人拥有两个单臂，关节一体化机器人控制系统的特点是分层开放式系统，便与维护与升级，如图2所示。
图1关节一体化机器人结构图Fig.1JointIntegratedRobotStructureChart
直流版手臂采用自主研制的一体化关节，在手臂安装尺寸上采用和交流版手臂尽量接近的结构，以便共享同一模具，节约成本。关节一体化机器人拥有独一无二的小型化设计，以及中空孔结构。执行元件中央的贯通孔内可穿过配线、配管、激光等，简化了机械装置的整体构造。并且具备可变选项多样化的特点，可根据需求选择输出轴承、减速机、电动机线圈等。手臂内置肘部电机与腕部电机的驱动板卡，便于后期的调试与维护。同时手臂内可安装触摸传感器模块，通过对人体的感知可做出相应的语音或表情等动作，提高与人的互动性与趣味性。
动力学分析的方法有很多种，如Lagrange方法、Newton-Euler方法、Gauss方法、Kane方法、旋量（对偶数）方法和Roberson-Wittenberg方法等。分别求取惯性力项、离心力和哥氏力项、重力项。此方法可以分别求取各项，便于应用到多关节机器人中。
3.1动力学控制结构
图2关节一体化机器人动力学控制框图Fig.2DynamicsControlDiagramofJointIntegratedRobot
在控制精度和高性动态控制指标的要求下，传统的控制很难实现，如图2所示。采用力矩前馈控制，动力学前馈补偿控制，这种方法已经被应用到非线性动力学模型中去了，在这种控制率中不需要以伺服速率进行复杂的耗时运算。动力学前馈补偿是将基于模型的部分放到了伺服环的外面[9]。
3.1.1离心力和哥氏力项
按Chiristoffell符号法，离心力和哥氏力项的C矩阵可由M矩阵求得：
故在求出M矩阵后代入Chiristoffell公式，可得到C矩阵。
3.1.2重力项
机械手总的重力势能为：
重力项：
3.1.3摩擦模型
完整的动力学模型表达式为：
式中：Fv∈Rn×n、Fs∈Rn×n—粘性摩擦和静摩擦系数对角矩阵；
向量sgn（q˙）∈Rn×1—关节速度的符号函数。
模型中的摩擦项能够很大的程度上提高链式机器人的模型精度，而动力学方程中的参数可以通过系统辨识算法和实验数据获得。
3.2牛顿-欧拉动力学
这种理想的控制是不容易实现的，其中最重要的因素包括：
（2）操作臂的模型是不精确的。
4.1笛卡尔空间推广
机器人的应用和操作通常都在笛卡尔空间描述，阻抗控制行为也是在笛卡尔坐标下定义的，笛卡尔坐标描述的是机器人末端执行器的位置和姿态。从关节空间到笛卡尔空间的变换需要正运动学和雅克比矩阵，假设二者已知，则方程可以方便的推广到笛卡尔空间。
正运动学的计算使用电机角度转换到关节后的角度θ，则：
式中：xθ，d—笛卡尔坐标系下期望电机位置的映射。
而关节速度与机器人末端速度的转换关系为：
新的阻抗控制律为：
由此得到笛卡尔坐标系下新的闭环控制系统为：
4.2无源性分析
总结带有重力补偿的关节转矩阻抗控制，完整的控制律为：
机器人的环境常常为描述为无源映射（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.1动力学验证
为验证方法的正确性，将惯性参数、摩擦参数辨识的结果代入完整的动力学方程计算伸缩轴转矩：
同时，通过采集电机电流isq计算关节转矩。
在整个伸缩轴运动的过程中，对比二者的数值曲线，如图3所示。
图3采集扭矩与动力学计算值比较曲线Fig.3ComparisonCurveofSampledTorqueandDynamicCalculation
通过对比二者关系可以看出，计算动力学与关节转矩变化曲线保持一致，验证了方法的有效性。
5.2阻抗控制器验证
水平伸缩两方向运动时的电机转矩曲线，拖曳式示教时由于手部施加给机器人的外作用力，电机转动带动机器人的运动方向与外力方向一致，电机转矩比执行模式小，此时电机转动起到助力作用，如图4所示。在正反向运动的起始阶段，电机电流较大，这是由于起始段机器人处于静止状态，反向的电流起到运动启动的作用。
图4拖曳式示教中电机转矩曲线图Fig.4TheMotorTorqueCurveinProcessofTowingTeach
根据拖曳式示教过程中所采集的电机转矩曲线可以看出，不开启阻抗控制的执行模式下，电机转矩范围在（0.22～0.33）Nm间，拖曳式示教启动阶段反向的电机转矩可以作为外力输入的判断。当拖曳判断进入运行阶段，电机转矩范围在（0.1～0.2）Nm之间，相当于感应到的外部反应转矩τext约为0.1Nm。机器人连杆长度约0.5m，则转换到机器人末端的外作用力Fext约为0.2N。
传统的采用六维力传感器的方式，传感器检测的精度一般为满量程的百分之一，以传感器量程20N为例，末端的力保护精度约为0.2N。
针对关节一体化机器人在高速复杂运动下需要能保持手臂平滑运动同时具有拖曳式示教的功能问题，详尽的介绍了动力学建模和阻抗控制算法的过程。针对关节一体化机器人的特点，根据已知的机器人的参数利用牛顿—欧拉法计算了动力学参数。同时提出一种阻抗控制算法。该算法通过关节转矩反馈，电机动力学与关节转矩反馈所组成的系统是一个无源子系统，同时刚性机器人的动力学也是一个无源子系统，二者构成两个内部反馈连接的两个无源系统，根据无源性定理，两个子系统内部反馈连接所组成的闭环系统也是无源的。通过建立Lyapunov函数证明了系统的稳定性。最后在机器人实验平台，基于关节转矩反馈设计阻抗控制器，并进行阻抗控制实验，实现机器人拖曳式示教功能，验证了所设计算法的有效性。
参考文献
［1］S.IwashitaDevelopingaservicerobot，IEEEInternationalConferenceonMechatronicsandAutomation，ICMA2005，2005：1057-1062.
［2］CarlosA.Jara，FranciscoA.Candelas，PabloGil.Aninteractivetoolforindustrialrobotssimulation，computervisionandremoteoperation［J］.RoboticsandAutonomousSystems，2011（59）：389-401.
［3］Chung-LiChen，Tso-KangWang，Chia-JuiHu.Model-baseddynamicgaitinaquadrupedrobotwithwaistactuation［C］.IntelligentRobotsandSystems（IROS），2016：2153-0866.
［4］SarahTang，VijayKumar，Safeandcompletetrajectorygenerationforrobotteamswithhigher-orderdynamics［C］.IntelligentRobotsandSystems（IROS），2016：2153-0866.
［5］JingjingJiang，PierluigiDiFranco，AlessandroAstolfi，Sharedcontrolforthekinematicanddynamicmodelsofamobilerobot［J］.IEEETransactionsonControlSystemsTechnology，2016，24（6）：2112-2124.
［6］LucRolland，RohitashChandra.Forwardkinematicsofthe6-6generalparallelmanipulatorusingrealcodedgeneticalgorithms［J］.IEEE/ASMEInternationalConferenceonAdvancedIntelligentMechatronics，2009（5）：1637-1642.
［7］Performanceimprovementoftrackingcontrolforaplanarparallelrobotusingsynchronizedcontrol［C］.InternationalConferenceonIntelligentRobotsandSystemsOctober9-15，2006，Beijing，China.
［8］HonghaiLiu，SeniorMember，DavidJ.Brown，GeorgeM.Coghill.Fuzzyqualitativerobotkinematics［J］.FuzzySystems，june2008：808-822.
［9］GaoXS，HuangZ．Acharacteristicsetmethodforequationsolvinginfinitefields［J］．JournalofSymbolicComputation，2012，47（6）：655-679．
［10］MouCQ，WangDM，LiXL．Decomposingpolynomialsetsintosimplesetsoverfinitefields：thepositive-dimensionalcase［J］．TheoreticalComputingScience，2013（468）：102-113．
ResearchonDynamicsandImpedanceControlAlgorithmofJointIntegratedRobot
GUTao1，WANYou-hong2（1.SuzhouInstituteofIndustrialTechnology，JiangsuSuzhou215000，China；2.SchoolofAutomation，NanjingUniversityofPostsandTelecommunications，JiangsuNanjing210042，China）
Abstract：Inthispaper，thejointrobotcanmaintainthesmoothmovementofthearmunderthehighspeedandcomplicatedmovement，andhasthefunctionofdraggingteaching.Firstly，thetransformationcoordinatesystemisestablishedbyDHmethod，andtherelativerelationofeachjointisobtained.Thealgorithmofsolvingthearmmotionissolvedbyusingtheiterativealgorithm.Secondly，thedynamicequationofNewton-Eulermethodisestablishedforthestructuralcharacteristicsofjointrobot.Thirdly，onthebasisofanalyzingtheprincipleofimpedancecontrol，theimpedancecontrolstructurebasedonforceinCartesianspaceisdesigned，andtheclassicalimpedancecontrolproblemofCartesianspaceisrealizedinjointspace.Finally，theimpedancecontrollerisdesignedbasedonthejointtorquefeedbackoftherobotexperimentplatform，andtheimpedancecontrolexperimentiscarriedouttorealizethefunctionoftherobottowingteaching，whichverifiesthevalidityofthedesignedalgorithm.
KeyWords：JointIntegratedRobot；DragTeach；Dynamics；ImpedanceControl
中图分类号：TH16；TP242.3
文献标识码：A
来稿日期：2017-05-20
基金项目：江苏高校品牌专业建设工程资助项目（PPZY2015B186）；国家自然科学基金项目（81071234）
作者简介：顾涛，（1977-），男，江苏苏州人，硕士研究生，高校讲师，主要研究方向：机械结构设计及有限元分析、机器人技术；
万佑红，（1974-），女，江苏南京人，硕士研究生，硕士生导师，副教授，主要研究方向：控制理论、方法与技术及其在通信中的应用