面向六关节机器人的位置域控制

工程科学学报 Chinese Journal of Engineering 面向六关节机器人的位置域控制 崔旭东邓少丰王平江 Position domain control technology for six-joint robots CUI Xu-dong,DENG Shao-feng.WANG Ping-jiang 引用本文： 崔旭东，邓少丰，王平江.面向六关节机器人的位置域控制.工程科学学报，2022,44(2)：244-253.doi:10.13374issm2095- 9389.2020.08.07.002 CUI Xu-dong,DENG Shao-feng,WANG Ping-jiang.Position domain control technology for six-joint robots[J].Chinese Journal of Engineering,2022,44(2:244-253.doi:10.13374j.issn2095-9389.2020.08.07.002 在线阅读View online::https://doi..org10.13374/.issn2095-9389.2020.08.07.002 您可能感兴趣的其他文章 Articles you may be interested in 具有状态约束与输入饱和的全向移动机器人自适应跟踪控制 Adaptive tracking control for omnidirectional mobile robots with full-state constraints and input saturation 工程科学学报.2019,41(9：1176 https::/doi.org10.13374j.issn2095-9389.2019.09.009 多机器人编队控制研究进展 Research development of multi-robot formation control 工程科学学报.2018,40(8)：893 https:ioi.org10.13374.issn2095-9389.2018.08.001 机器人负载的动力学参数辨识 Identification methods for robot payload dynamical parameters 工程科学学报.2017,3912：1907htps:/1doi.org10.13374.issn2095-9389.2017.12.018 集总干扰下六旋翼飞行器的轨迹跟踪控制 Trajectory tracking control for an unmanned hexrotor with lumped disturbance 工程科学学报.2018,40(5：622 https:1doi.org10.13374.issn2095-9389.2018.05.013 基于BP神经网络的机器人波动摩擦力矩修正方法 Wave friction correction method for a robot based on BP neural network 工程科学学报.2019.41(8：1085 https:/doi.org10.13374.issn2095-9389.2019.08.014 螺旋桨清洗机器人超灵巧机械臂设计 Ultra-smart manipulator design for propeller-cleaning robots 工程科学学报.2017,39(6：924htps:/doi.org10.13374issn2095-9389.2017.06.016

面向六关节机器人的位置域控制 崔旭东 邓少丰 王平江 Position domain control technology for six-joint robots CUI Xu-dong, DENG Shao-feng, WANG Ping-jiang 引用本文: 崔旭东, 邓少丰, 王平江. 面向六关节机器人的位置域控制[J]. 工程科学学报, 2022, 44(2): 244-253. doi: 10.13374/j.issn2095- 9389.2020.08.07.002 CUI Xu-dong, DENG Shao-feng, WANG Ping-jiang. Position domain control technology for six-joint robots[J]. Chinese Journal of Engineering, 2022, 44(2): 244-253. doi: 10.13374/j.issn2095-9389.2020.08.07.002 在线阅读 View online: https://doi.org/10.13374/j.issn2095-9389.2020.08.07.002 您可能感兴趣的其他文章 Articles you may be interested in 具有状态约束与输入饱和的全向移动机器人自适应跟踪控制 Adaptive tracking control for omnidirectional mobile robots with full-state constraints and input saturation 工程科学学报. 2019, 41(9): 1176 https://doi.org/10.13374/j.issn2095-9389.2019.09.009 多机器人编队控制研究进展 Research development of multi-robot formation control 工程科学学报. 2018, 40(8): 893 https://doi.org/10.13374/j.issn2095-9389.2018.08.001 机器人负载的动力学参数辨识 Identification methods for robot payload dynamical parameters 工程科学学报. 2017, 39(12): 1907 https://doi.org/10.13374/j.issn2095-9389.2017.12.018 集总干扰下六旋翼飞行器的轨迹跟踪控制 Trajectory tracking control for an unmanned hexrotor with lumped disturbance 工程科学学报. 2018, 40(5): 622 https://doi.org/10.13374/j.issn2095-9389.2018.05.013 基于BP神经网络的机器人波动摩擦力矩修正方法 Wave friction correction method for a robot based on BP neural network 工程科学学报. 2019, 41(8): 1085 https://doi.org/10.13374/j.issn2095-9389.2019.08.014 螺旋桨清洗机器人超灵巧机械臂设计 Ultra-smart manipulator design for propeller-cleaning robots 工程科学学报. 2017, 39(6): 924 https://doi.org/10.13374/j.issn2095-9389.2017.06.016

工程科学学报.第44卷，第2期：244-253.2022年2月 Chinese Journal of Engineering,Vol.44,No.2:244-253,February 2022 https://doi.org/10.13374/j.issn2095-9389.2020.08.07.002;http://cje.ustb.edu.cn 面向六关节机器人的位置域控制 崔旭东”，邓少丰)，王平江2) 1)鞍山师范学院计算中心，鞍山1140072)华中科技大学机械科学与工程学院，武汉430074 ☒通信作者，E-mail:Pi_wang@hust.edu.cn 摘要多轴联动下的串联多关节工业机器人在空间轨迹运动时，在时间上保证各关节轴单独具有良好的跟踪性能，而由于 机械电气的迟滞效应，并不能完全保证理想的轮廓轨迹，这说明各个伺服轴的运动在几何空间中的同步非常重要.针对运动 指令与实际位置之间的迟滞所带来的机器人末端轮廓精度不高的问题，本文结合工业机器人现有的运动学和动力学以及传 统的PD控制理论，研究了六关节机器人位置域控制算法.将机器人空间轮廓轨迹的控制，通过采用主-从运动关系实时建 立的方法，将时域中的各个同服关节的同步控制方法，变换到位置域的各个同服关节的主一从跟随的控制方法，在实现位置域 的同步控制的同时，引入基于位置域的PD控制，减少了主-从跟随控制的跟随误差，从而整体提高机器人末端的轮廓运动精 度.该方法在Linux CNC(Computerized Numerical Control)数控系统上，以某公司HSR-JR6Os机器人为对象进行了实验，证明 采用位置域控制方法对六关节机器人空间运动轨迹精度的提高有积极作用. 关键词轮廓跟踪；六关节机器人；位置域控制：PID控制：LinuxCNC 分类号TP242.2 Position domain control technology for six-joint robots CUI Xu-dong,DENG Shao-feng,WANG Ping-jiang 1)Computing Center,Anshan Normal University,Anshan 114007,China 2)School of Mechanical Science Engineering,Huazhong University of Science and Technology,Wuhan 430074,China Corresponding author,E-mail:pj_wang @hust.edu.cn ABSTRACT In the context of the rapid development of intelligent manufacturing,high-precision motion control of industrial robots has attracted increasing research attention.High-speed and high-precision motion control is a development trend associated with the current industrial robots.Industrial transformation and upgradation can be accelerated only via the independent research and development of robot core technologies,including robot high-precision control systems,ensuring the transition of the manufacturing industry in China toward intelligent and digital development.Currently,the contour tracking error associated with majority of the multiaxes CNC machine tools and multijoint industrial robots is a common problem.A good tracking performance can be achieved in time with respect to each joint axis when the space trajectory of the serial multijoint industrial robot changes under multiaxes linkage. The ideal contour trajectory cannot be fully ensured because of the mechanical and electrical hysteresis effects.Therefore,the synchronization of the motion of each servo axis in the geometric space is very important.In this paper,the existing kinematics and dynamics of industrial robots were combined with the traditional PID control theory for studying position domain control algorithm of the six-joint robot to resolve the problem of low accuracy associated with the robot end profile,which can be attributed to the lag between the motion instruction and the actual position.The algorithm uses the method of real-time establishment of the master-slave motion relation for controlling the spatial contour trajectory of the robot.It further transforms the synchronization control method of each 收稿日期：2020-08-07 基金项目：国家科技重大(04)专项“核工业专用零部件制造装备换脑工程”资助项目(2017ZX04011006-005):国家科技重大(04)专项“高 档数控系统关键共性技术创新能力平台（二期）”资助项目(2015ZX04005007)

面向六关节机器人的位置域控制 崔旭东1)，邓少丰2)，王平江2) 苣 1) 鞍山师范学院计算中心，鞍山 114007    2) 华中科技大学机械科学与工程学院，武汉 430074 苣通信作者， E-mail：pj_wang@hust.edu.cn 摘    要    多轴联动下的串联多关节工业机器人在空间轨迹运动时，在时间上保证各关节轴单独具有良好的跟踪性能，而由于 机械电气的迟滞效应，并不能完全保证理想的轮廓轨迹，这说明各个伺服轴的运动在几何空间中的同步非常重要. 针对运动 指令与实际位置之间的迟滞所带来的机器人末端轮廓精度不高的问题，本文结合工业机器人现有的运动学和动力学以及传 统的 PID 控制理论，研究了六关节机器人位置域控制算法. 将机器人空间轮廓轨迹的控制，通过采用主−从运动关系实时建 立的方法，将时域中的各个伺服关节的同步控制方法，变换到位置域的各个伺服关节的主−从跟随的控制方法，在实现位置域 的同步控制的同时，引入基于位置域的 PD 控制，减少了主−从跟随控制的跟随误差，从而整体提高机器人末端的轮廓运动精 度. 该方法在 Linux CNC(Computerized Numerical Control) 数控系统上，以某公司 HSR-JR605 机器人为对象进行了实验，证明 采用位置域控制方法对六关节机器人空间运动轨迹精度的提高有积极作用. 关键词    轮廓跟踪；六关节机器人；位置域控制；PID 控制；LinuxCNC 分类号    TP242.2 Position domain control technology for six-joint robots CUI Xu-dong1) ，DENG Shao-feng2) ，WANG Ping-jiang2) 苣 1) Computing Center, Anshan Normal University, Anshan 114007, China 2) School of Mechanical Science & Engineering, Huazhong University of Science and Technology, Wuhan 430074, China 苣 Corresponding author, E-mail: pj_wang@hust.edu.cn ABSTRACT    In the context of the rapid development of intelligent manufacturing, high-precision motion control of industrial robots has  attracted  increasing  research  attention.  High-speed  and  high-precision  motion  control  is  a  development  trend  associated  with  the current  industrial  robots.  Industrial  transformation  and  upgradation  can  be  accelerated  only via the  independent  research  and development  of  robot  core  technologies,  including  robot  high-precision  control  systems,  ensuring  the  transition  of  the  manufacturing industry  in  China  toward  intelligent  and  digital  development.  Currently,  the  contour  tracking  error  associated  with  majority  of  the multiaxes CNC machine tools and multijoint industrial robots is a common problem. A good tracking performance can be achieved in time with respect to each joint axis when the space trajectory of the serial multijoint industrial robot changes under multiaxes linkage. The  ideal  contour  trajectory  cannot  be  fully  ensured  because  of  the  mechanical  and  electrical  hysteresis  effects.  Therefore,  the synchronization of the motion of each servo axis in the geometric space is very important. In this paper, the existing kinematics and dynamics of industrial robots were combined with the traditional PID control theory for studying position domain control algorithm of the  six-joint  robot  to  resolve  the  problem  of  low  accuracy  associated  with  the  robot  end  profile,  which  can  be  attributed  to  the  lag between the motion instruction and the actual position. The algorithm uses the method of real-time establishment of the master–slave motion relation for controlling the spatial contour trajectory of the robot. It further transforms the synchronization control method of each 收稿日期: 2020−08−07 基金项目: 国家科技重大（04）专项“核工业专用零部件制造装备换脑工程” 资助项目（2017ZX04011006-005）；国家科技重大（04）专项“高 档数控系统关键共性技术创新能力平台（二期）” 资助项目（2015ZX04005007） 工程科学学报，第 44 卷，第 2 期：244−253，2022 年 2 月 Chinese Journal of Engineering, Vol. 44, No. 2: 244−253, February 2022 https://doi.org/10.13374/j.issn2095-9389.2020.08.07.002; http://cje.ustb.edu.cn

崔旭东等：面向六关节机器人的位置域控制 245· servo joint in the time domain into the master-slave follow control method of each servo joint in the position domain.While realizing synchronization control in the position domain,PD control based on position domain was introduced to reduce the following error of the master-slave following control,improving the overall accuracy of the contour motion of the robot end.The method proposed in this paper has been tested on the Linux CNC numerical control system with a company's HSR-JR605 robot,indicating that the position of the domain control method employed positively affects the accuracy of six articulated robot space trajectory. KEY WORDS contour tracking;six-joint robot;position domain control;PID control;LinuxCNC 制造业是国民经济的主要支柱，也是今后我 置域表达的轮廓轨迹函数，并采用基于动力学的 国经济“创新驱动、转型升级”的主战场山.在此背 位置域PD控制方法来提高机器人末端的轮廓运 景下，工业机器人领域成为各国提高制造业综合 动精度 实力，使制造业实现数字化、网络化、智能化的必争 相比于直角坐标系下的常用的三轴铣床控制 之地P-)D控制是工业控制领域中最经典也是 系统，由于机器人运动控制的复杂性，尤其是运动 最常用的算法，因其原理简单、容易实现，并且具 学正逆解的多解性，在实时控制环节构建主从运 有较强的鲁棒性，而被广泛应用于各种工业装备 动的以主动轴位置为自变量的从动轴位置函数， 的实时控制.PID控制技术已经在理论上被证明 是极其复杂的，因此位置域的控制方式，在关节型 对于工业机器人的高耦合、非线性系统是有效的， 机器人的应用尚未深入开展 且PID主要应用于已解耦的关节臂单轴的时间域 伺服控制层面.虽然解耦控制可以为具有串、并 1机器人的位置域PD控制模型 联性质的多轴机器人系统简单轨迹提供好的跟踪 对于多自由度机器人系统，时间域下的PD 性能，但是实践证明，对于高精度轮廓跟踪，由于 控制器标准形式可表示为： 末端的运动是所有关节同步运动的结果，分别确 T(r)=Kpe(t)+Ki e(t)dt+Kae(t) (1) 保每个关节在时间域的良好跟踪性能，并不能保 证理想的轮廓跟踪误差，相关运动关节的不良同 式中，e(t)是系统的位置误差矢量，e()是系统的速 步误差，往往导致轮廓跟踪的轨迹精度恶化-山 度误差矢量，τ()是系统的控制转矩矢量，1表示 近年来，针对工业机器人轮廓跟踪中各关节 时间，Kp、K;和K分别为比例增益、积分增益和微 的运动同步问题已经进行了大量研究.为了进一 分增益的对角矩阵.在高速插补控制中，由于插补 步提高轮廓运动精度，外国学者Koren提出了一种 控制周期很短(1ms),稳态误差比较小，一般令积 交叉耦合控制(Cross-coupled control,.CCC)方法m 分项的系数为零，将PID控制简化为PD控制，可 实际上，CCC方法是针对单个轴的PID控制和针 以在不增加计算量的基础上，获得同样的控制精 对多个轴的耦合误差反馈控制的一种组合.CCC 度.因此，对于公式(1)所描述的从运动轴在时间 的主要思想是实现两个运动轴的位置同步.实验 域中PID控制模型下，可以表示为式(2)的位置域 表明，通过选择合适的耦合增益，CCC可以通过同 的PD控制： 步性能的改善来提高轮廓跟踪精度，然而由于机 Ts(qm)=Kpsoes(qm)+Kasoes'(qm) (2) 器人系统中确定耦合增益的复杂性)，以及每个 式中，下标m表示主运动master轴所在关节轴的 运动关节仍然需要单独控制，且必然存在一定的 序号，s表示从运动salve轴所在的关节轴的序号 跟踪误差等原因，CCC方法也同样具有局限性. (下文同样适用).式(2)中，es(qm)表示以主动轴位 针对上述问题，加拿大学者结合PID控制和 置qm为自变量，计算从动轴的期望位置qsd(gm)与 CCC控制的优势，提出一个新的控制方法：位置域 实际位置q,(gm)之差的矢量；es(qm)表示从运动轴 控制技术3-刀与传统时域ID控制中轨迹规划 的相对速度（相对于主运动轴位置）之差的矢量； 被设计为时间1的函数不同，位置域控制方法是在 Kso和Kdso则分别为具有恒定值的从动轴的比例 位置域中构建运动轨迹，称之为位置域PD控制. 增益和微分增益对角矩阵 本文主要解决的问题为：在将位置域PD控 注意：在任意时刻，对于六关节机器人的关节 制技术应用于多自由度、非线性、串联的关节机 轴，只有一个主运动轴，可以有五个从运动轴；亦 器人的轮廓运动中，如何在机器人轨迹代码执行 即公式(2)对应于五个从关节轴的位置域PD控制 中，实时将时间域表达的轮廓轨迹函数，转变为位 方程.但是，不同的机器人的运动轨迹，主运动

servo joint in the time domain into the master–slave follow control method of each servo joint in the position domain. While realizing synchronization control in the position domain, PD control based on position domain was introduced to reduce the following error of the master –slave following control, improving the overall accuracy of the contour motion of the robot end. The method proposed in this paper has been tested on the Linux CNC numerical control system with a company’s HSR-JR605 robot, indicating that the position of the domain control method employed positively affects the accuracy of six articulated robot space trajectory. KEY WORDS    contour tracking；six-joint robot；position domain control；PID control；LinuxCNC 制造业是国民经济的主要支柱，也是今后我 国经济“创新驱动、转型升级”的主战场[1] . 在此背 景下，工业机器人领域成为各国提高制造业综合 实力，使制造业实现数字化、网络化、智能化的必争 之地[2−5] . PID 控制是工业控制领域中最经典也是 最常用的算法，因其原理简单、容易实现，并且具 有较强的鲁棒性，而被广泛应用于各种工业装备 的实时控制[6] . PID 控制技术已经在理论上被证明 对于工业机器人的高耦合、非线性系统是有效的， 且 PID 主要应用于已解耦的关节臂单轴的时间域 伺服控制层面. 虽然解耦控制可以为具有串、并 联性质的多轴机器人系统简单轨迹提供好的跟踪 性能，但是实践证明，对于高精度轮廓跟踪，由于 末端的运动是所有关节同步运动的结果，分别确 保每个关节在时间域的良好跟踪性能，并不能保 证理想的轮廓跟踪误差，相关运动关节的不良同 步误差，往往导致轮廓跟踪的轨迹精度恶化[7−11] . 近年来，针对工业机器人轮廓跟踪中各关节 的运动同步问题已经进行了大量研究. 为了进一 步提高轮廓运动精度，外国学者 Koren 提出了一种 交叉耦合控制（Cross-coupled control, CCC）方法[7] . 实际上，CCC 方法是针对单个轴的 PID 控制和针 对多个轴的耦合误差反馈控制的一种组合. CCC 的主要思想是实现两个运动轴的位置同步. 实验 表明，通过选择合适的耦合增益，CCC 可以通过同 步性能的改善来提高轮廓跟踪精度. 然而由于机 器人系统中确定耦合增益的复杂性[12] ，以及每个 运动关节仍然需要单独控制，且必然存在一定的 跟踪误差等原因，CCC 方法也同样具有局限性. 针对上述问题，加拿大学者结合 PID 控制和 CCC 控制的优势，提出一个新的控制方法：位置域 控制技术[13−17] . 与传统时域 PID 控制中轨迹规划 被设计为时间 t 的函数不同，位置域控制方法是在 位置域中构建运动轨迹，称之为位置域 PID 控制. 本文主要解决的问题为：在将位置域 PID 控 制技术应用于多自由度、非线性、串联的关节机 器人的轮廓运动中，如何在机器人轨迹代码执行 中，实时将时间域表达的轮廓轨迹函数，转变为位 置域表达的轮廓轨迹函数，并采用基于动力学的 位置域 PID 控制方法来提高机器人末端的轮廓运 动精度. 相比于直角坐标系下的常用的三轴铣床控制 系统，由于机器人运动控制的复杂性，尤其是运动 学正逆解的多解性，在实时控制环节构建主从运 动的以主动轴位置为自变量的从动轴位置函数， 是极其复杂的，因此位置域的控制方式，在关节型 机器人的应用尚未深入开展. 1    机器人的位置域 PID 控制模型 对于多自由度机器人系统，时间域下的 PID 控制器标准形式可表示为： τ(t) = Kpe (t)+ Ki w e (t)dt+ Kde˙(t) （1） e (t) e˙(t) τ(t) Kp Ki Kd 式中， 是系统的位置误差矢量， 是系统的速 度误差矢量， 是系统的控制转矩矢量，t 表示 时间， 、 和 分别为比例增益、积分增益和微 分增益的对角矩阵. 在高速插补控制中，由于插补 控制周期很短（1 ms），稳态误差比较小，一般令积 分项的系数为零，将 PID 控制简化为 PD 控制，可 以在不增加计算量的基础上，获得同样的控制精 度. 因此，对于公式（1）所描述的从运动轴在时间 域中 PID 控制模型下，可以表示为式（2）的位置域 的 PD 控制： τs (qm) = Kps0es (qm)+ Kds0es ′ (qm) （2） es (qm) qm qsd (qm) qs (qm) es ′ (qm) Kds0 式中，下标 m 表示主运动 master 轴所在关节轴的 序号，s 表示从运动 salve 轴所在的关节轴的序号 （下文同样适用）. 式（2）中， 表示以主动轴位 置 为自变量，计算从动轴的期望位置 与 实际位置 之差的矢量； 表示从运动轴 的相对速度（相对于主运动轴位置）之差的矢量； Kps0 和 则分别为具有恒定值的从动轴的比例 增益和微分增益对角矩阵. 注意：在任意时刻，对于六关节机器人的关节 轴，只有一个主运动轴，可以有五个从运动轴；亦 即公式（2）对应于五个从关节轴的位置域 PD 控制 方程. 但是，不同的机器人的运动轨迹，主运动、 崔旭东等： 面向六关节机器人的位置域控制 · 245 ·

246 工程科学学报，第44卷，第2期 从运动关节轴所对应的序号是动态变化的，既使 Ts(qm)=Kpsou(es(qm),ap,6p)+Kasou(es'(qm).ad.6a) 是同一段机器人运动轨迹描述的指令代码中，主 (3) 运动轴、从运动轴的关系，也有可能随时间或者位 其中，Kso和Kaso是系统的初始非线性PD控制增 姿的变化而发生变化：而且在任意时刻，所有从动 益矩阵，是恒定值的从动轴的比例增益和微分增 轴的位置，都要实时表达为主动轴位置的函数.这 益对角矩阵；y是三元非线性函数；而ap,6p,aa, 就是为什么位置域控制算法在多关节机器人控制 6a则是定义非线性函数的附加参数 中，难以实际应用的症结所在 三元非线性函数山的一般表达式定义如下叫 如图1所示为位置域PD控制原理框图阁，首 Ixlsign(x).>6 先根据理论轮廓运用逆运动学分析，获得时间域 山(x,,0)= 1-6 (4) 中某段轮廓轨迹中各个关节轴的期望位置，并分 析在该段轨迹运动中，哪个关节轴的运动位置增 从公式(4)可以看出，α为非线性函数中的幂指数 量最大，则将哪个关节轴指定为主运动关节轴，其 部分，而δ则为线性与非线性分界的阈值.在阈值范 他的关节轴为从运动关节轴.然后通过轨迹规划 围内，函数表现为线性：当超过该阈值时，函数表现为 非线性：此时非线性函数的数学属性由参数α决定 方法，获得在位置域中表示的从运动轴的、以主动 轴位置q1为自变量的从动轴期望位置qsd(q1)和期 在位置域PD控制中，参考位置qm是类似时域中时间 望位置相对导数qd(q1)(此处假设与关节轴1相 1的自变量.e,(gm)和e,(gm)是自变量qm的函数 将公式(4)代入公式(3)中，该控制律也可以 对应的运动为主运动)，最后通过位置域PD控制， 采用式(5)表示： 实现从运动相对于主运动位置的实时跟随控制， 以提高整个机器人末端件空间运动轨迹的精度. Ts(qm)=Kps(es(9m).Qp.6p)es(qm)+ Kas(es(qm),ad.6d)es(qm) (5) Main 4491).9391) 9 其中，该控制律的增益系数定义如下： movement Trajectory planning OSlave movement Kpsoles (gm)p-,les(gm)>6p Position-domain Kps(es(qm),ap,6p)= Inverse kinematics NPD control Kpsoopap-1.les(gm)s6p (6) Desired profile 以及 图1位置域PD控制原理框图 Fig.1 Schematic of PD control in the position domain Kds(e,(qm),ad,dd） Kasoles(qm)es(qm)>6a 综上所述，位置域PD控制，从控制规律上可看 Kasood-l,eg(qm≤6a 7) 作是一种通用的空间同步的位置和速度控制，其 其中，Ks和Ks表示位置域非线性PD控制的与系 中从运动根据轨迹轮廓要求，相对于主运动的位置、 统误差e,(qm)和e,(gm)有关的增益矩阵 速度，在位置和速度上进行跟随控制，使得主运动 公式(5)至公式(7)表明，位置域非线性PD控制 和从运动之间综合的运动轨迹尽可能地精确 器可视为变增益的控制器，其增益为系统误差的函数 另外，从实现上来说，位置域PD控制其实是 以时间域控制和位置域控制的混合控制的形式进 2 基于LinuxCNC系统的位置域控制系统 行的920主运动在时域中受到公式(1)所示控制 设计 律的控制，同时从运动则在位置域中受到公式(2) LinuxCNC系统具有以下优点： 所示控制律的控制.应当注意的是，主运动在时间 (I)开源：LinuxCNC系统的源码完全公开，用 域的控制过程中必须满足单调递增或递减的条 户可以方便地获得其源码并进行有针对性的开发； 件，以便形成从时间域到位置域的映射是一对一 (2)实时性：LinuxCNC系统具有实时性，支持 的，才能使得主动轴因时间域控制所产生的位置 EtherCAT现场总线及Ims的插补周期，可以满足 误差，折算到从动轴的位置误差为减少的 机器人运动控制对实时性的要求： 更进一步地，采用非线性函数替换公式(2)中 (3)可扩展性：LinuxCNC系统的硬件抽象层 的误差信号e(qm)、es'(qm),形成一种基于多自由 提供了内核模块接口，用户可以根据自身需要灵 度机器人的位置域非线性PD控制律.该控制律用 活设计内核实时运行的HAL组件，以实现特定的 以下形式表示： 实时插补控制功能，如位置域控制功能

从运动关节轴所对应的序号是动态变化的，既使 是同一段机器人运动轨迹描述的指令代码中，主 运动轴、从运动轴的关系，也有可能随时间或者位 姿的变化而发生变化；而且在任意时刻，所有从动 轴的位置，都要实时表达为主动轴位置的函数. 这 就是为什么位置域控制算法在多关节机器人控制 中，难以实际应用的症结所在. q1 qsd (q1) qsd ′ (q1) 如图 1 所示为位置域 PD 控制原理框图[18] ，首 先根据理论轮廓运用逆运动学分析，获得时间域 中某段轮廓轨迹中各个关节轴的期望位置，并分 析在该段轨迹运动中，哪个关节轴的运动位置增 量最大，则将哪个关节轴指定为主运动关节轴，其 他的关节轴为从运动关节轴. 然后通过轨迹规划 方法，获得在位置域中表示的从运动轴的、以主动 轴位置 为自变量的从动轴期望位置 和期 望位置相对导数 （此处假设与关节轴 1 相 对应的运动为主运动），最后通过位置域 PD 控制， 实现从运动相对于主运动位置的实时跟随控制， 以提高整个机器人末端件空间运动轨迹的精度. Main movement Trajectory planning Slave movement Inverse kinematics Desired profile q1 + − Position-domain NPD control qsd(q1 ), q′ sd(q1 ) qs (q1 ) 图 1    位置域 PD 控制原理框图 Fig.1    Schematic of PD control in the position domain 综上所述，位置域 PD 控制，从控制规律上可看 作是一种通用的空间同步的位置和速度控制，其 中从运动根据轨迹轮廓要求，相对于主运动的位置、 速度，在位置和速度上进行跟随控制，使得主运动 和从运动之间综合的运动轨迹尽可能地精确. 另外，从实现上来说，位置域 PD 控制其实是 以时间域控制和位置域控制的混合控制的形式进 行的[19−20] . 主运动在时域中受到公式（1）所示控制 律的控制，同时从运动则在位置域中受到公式（2） 所示控制律的控制. 应当注意的是，主运动在时间 域的控制过程中必须满足单调递增或递减的条 件，以便形成从时间域到位置域的映射是一对一 的，才能使得主动轴因时间域控制所产生的位置 误差，折算到从动轴的位置误差为减少的. es (qm) es ′ (qm) 更进一步地，采用非线性函数替换公式（2）中 的误差信号 、 ，形成一种基于多自由 度机器人的位置域非线性 PD 控制律. 该控制律用 以下形式表示： τs (qm) = Kps0ψ ( es (qm),αp,δp ) + Kds0ψ ( es ′ (qm),αd,δd ) （3） Kps0 Kds0 αp δp αd δd 其中， 和 是系统的初始非线性 PD 控制增 益矩阵，是恒定值的从动轴的比例增益和微分增 益对角矩阵；ψ 是三元非线性函数；而 ， ， ， 则是定义非线性函数的附加参数. 三元非线性函数 ψ 的一般表达式定义如下[21] ： ψ(x,α,δ) =    |x| α sign(x),|x| > δ x δ 1−α ,|x| ⩽ δ （4） qm es (qm) e ′ s (qm) qm 从公式（4）可以看出，α 为非线性函数中的幂指数 部分，而 δ 则为线性与非线性分界的阈值. 在阈值范 围内，函数表现为线性；当超过该阈值时，函数表现为 非线性；此时非线性函数的数学属性由参数 α 决定. 在位置域 PD 控制中，参考位置 是类似时域中时间 t 的自变量. 和 是自变量 的函数. 将公式（4）代入公式（3）中，该控制律也可以 采用式（5）表示： τs (qm) = Kps ( es (qm),αp,δp ) es (qm)+ Kds ( e ′ s (qm),αd,δd ) e ′ s (qm) （5） 其中，该控制律的增益系数定义如下： Kps ( es (qm),αp,δp ) =    Kps0|es (qm)| αp−1 ,|es (qm)| > δp Kps0δp αp−1 ,|es (qm)| ⩽ δp （6） 以及 Kds ( e ′ s (qm),αd,δd ) =    Kds0   e ′ s (qm)    αd−1 ,   e ′ s (qm)    > δd Kds0δd αd−1 ,   e ′ s (qm)    ⩽ δd （7） Kps Kds es (qm) e ′ s (qm) 其中， 和 表示位置域非线性 PD 控制的与系 统误差 和 有关的增益矩阵. 公式（5）至公式（7）表明，位置域非线性 PD 控制 器可视为变增益的控制器，其增益为系统误差的函数. 2    基于 LinuxCNC 系统的位置域控制系统 设计 LinuxCNC 系统具有以下优点： （1）开源：LinuxCNC 系统的源码完全公开，用 户可以方便地获得其源码并进行有针对性的开发； （2）实时性：LinuxCNC 系统具有实时性，支持 EtherCAT 现场总线及 1 ms 的插补周期，可以满足 机器人运动控制对实时性的要求； （3）可扩展性：LinuxCNC 系统的硬件抽象层 提供了内核模块接口，用户可以根据自身需要灵 活设计内核实时运行的 HAL 组件，以实现特定的 实时插补控制功能，如位置域控制功能. · 246 · 工程科学学报，第 44 卷，第 2 期

崔旭东等：面向六关节机器人的位置域控制 247· 因此，本文采用LinuxCNC数控系统作为多关 算法可以在LinuxCNC中的硬件抽象层中，以 节机器人位置域PD控制算法实现的软件平台 HAL组件的方式予以实现，由此设计出如图2所 2.1位置域控制软件系统总体方案 示的基于LinuxCNC系统的位置域PD控制系统总 经过上述分析，本文所提出的位置域PD控制 体方案2-2 Graphical user interface NML EMCTASK RS-274 NGC Sequential logic interpreter Shared memory buffer Cartesian Movement Status EMCMOT position instruction Trajectory planning Forward Inverse Limit&home kinematics kinematics status ------ Axis 1 Interpolator Axis M(N=2,3,4) PD servo Unit convert Unit convert Location domain PD Hardware abstraction layer control module Encoder counte A/D converter Limit switches Encoder Electric Realtime hardware devices machinery 图2基于LinuxCNC系统的位置域PD控制系统总体方案 Fig.2 Overall scheme of the PD control system based on the LinuxCNC system in position domain 由图2所示整个控制系统的控制流程如下： (3)将处理后的NML轨迹控制命令发送到用 (1)用户首先向图形用户界面输入轨迹运动 户态与内核态的共享内存，由此传输至运行于内 命令（或轨迹运动的文件），将运动指令通过NML 核实时环境中的运动控制器(EMCMOT); 通信机制传输至任务执行模块(EMCTASK); (4)EMCMOT主控程序是实时线程，周期性执 (2)EMCTASK主控模块周期性地调用RS-274NGC 行笛卡尔坐标系下的运动插补程序及运动学逆 解释器，对类似于G代码的机器人轨迹运动代码 解，并将各个关节伺服电机的运动指令，通过Ether 进行解析，并将转换后的操作命令进行后续处理： CAT总线，发往各个伺服驱动器.主控程序从共享

因此，本文采用 LinuxCNC 数控系统作为多关 节机器人位置域 PD 控制算法实现的软件平台. 2.1    位置域控制软件系统总体方案 经过上述分析，本文所提出的位置域 PD 控制 算法可以 在 LinuxCNC 中的硬件抽象层中 ， 以 HAL 组件的方式予以实现，由此设计出如图 2 所 示的基于 LinuxCNC 系统的位置域 PD 控制系统总 体方案[22−25] . Graphical user interface EMCTASK Sequential logic Shared memory buffer Cartesian position Movement instruction Status Trajectory planning Forward kinematics Inverse kinematics Interpolator EMCMOT + − PD servo Unit convert Unit convert Hardware abstraction layer Realtime hardware devices Encoder counter Limit switches Encoder Electric machinery NML RS-274 NGC interpreter Limit&home status Axis 1 Location domain PD control module A/D converter Axis N(N=2, 3, 4) 图 2    基于 LinuxCNC 系统的位置域 PD 控制系统总体方案 Fig.2    Overall scheme of the PD control system based on the LinuxCNC system in position domain 由图 2 所示整个控制系统的控制流程如下： （1）用户首先向图形用户界面输入轨迹运动 命令（或轨迹运动的文件），将运动指令通过 NML 通信机制传输至任务执行模块（EMCTASK）； （2）EMCTASK主控模块周期性地调用RS-274NGC 解释器，对类似于 G 代码的机器人轨迹运动代码 进行解析，并将转换后的操作命令进行后续处理； （3）将处理后的 NML 轨迹控制命令发送到用 户态与内核态的共享内存，由此传输至运行于内 核实时环境中的运动控制器（EMCMOT）； （4）EMCMOT 主控程序是实时线程，周期性执 行笛卡尔坐标系下的运动插补程序及运动学逆 解，并将各个关节伺服电机的运动指令，通过 Ether CAT 总线，发往各个伺服驱动器. 主控程序从共享 崔旭东等： 面向六关节机器人的位置域控制 · 247 ·

248 工程科学学报，第44卷，第2期 内存中读取由任务控制器发送的运动轨迹命令， (1)主动轴选择模块 并进行机器人的运动速度规划（小线段拟合、速度 位置域控制算法的关键是，在某段轨迹上实 规划等)、运动学正逆变换（关键点：同时确定主运 时确定各个关节轴之间的主、从运动关系，即确定 动轴、从运动轴所对应的关节轴以及构造以主运 主动关节轴与从动关节轴的索引号.本文采用将 动关节轴位置为自变量的其他各个从动关节轴位 运动控制周期内位移增量最大的关节轴，作为该 置的函数等)，并实现时域/位置域的混合插补计 周期的主运动轴，其他关节轴作为从运动轴 算、动力学前馈等运动控制功能： (2)位置域PD控制补偿量计算模块. (5)将位置域PD控制算法模块，以HAL组件 对于主动轴选择模块确定的从动轴，本模块 的方式加入到硬件抽象层(HAL).HAL通过定义 将各个从动轴的实际位置与理论位置的差值，从 的引脚(HAL pin)获取运动控制器中的各种实时 时间域转化到位置域上，并将根据公式(5)分别求 数据，该数据经过HAL中各种功能组件的运算 得各个从运动关节轴所需力矩的补偿量 后，写入到实时EtherCAT总线的硬件驱动程序； (3)动力学力矩前馈模块 (6)EtherCAT总线将各个关节轴的运动控制指令 通过力矩与电流间的转化关系，根据各个关 (含位置增量、力矩前馈)发送到对应的伺服驱动器， 节所需的力矩补偿量，求出各伺服电机所需的电 实现对机器人各个关节轴的运动控制：同时EtherCAT 流补偿，采用电流/力矩前馈的方法，将其补偿电流 总线将各个轴的实际位置反馈回位置域PD控制算法 叠加到伺服控制器的电流环上，以改善伺服电机 模块，以便计算各个从动关节轴的、以主动轴位置为 的动力学特性，提高整个机器人运动轨迹的准确性 自变量的位置、相对速度误差以及力矩前馈值，为下 (4)驱动电流限流调整模块 一个控制周期准备好位置域PD控制相关的数据， 该模块主要对各个关节轴的驱动电流进行限 2.2位置域控制算法 流，防止由于驱动伺服电机运动的电流过大而造 如图3所示，本文设计的位置域PD控制算法 成的机器人抖动等性能变差以及安全问题 分解为以下四个功能模块 2.3位置域控制程序的实现 位置域PD控制算法模块是通过HAL组件实 Joint command position Encoder feedback joint generated by interpolator 现的.由于该组件中所包含的动力学力矩前馈模 actual position 块与驱动电流限流模块不是本文关注的重点，且 动力学力矩前馈模块涉及惯性矩阵、摩擦阻尼矩 阵、重力/伐氏力矩阵等基于旋量理论的动力学建模 Active axis selection module (take the joint axis with the largest displacement in the motion 和摩擦力模型建立等，较为冗长，这里不再赘述 cycle as the active axis of the cycle) 本文所设计的位置域PD控制算法模块，首先 定义了机器人各关节的期望角位移、期望角速 Position domain PD control compensation calculation module(determine the compensation amount of the torque 度、期望角加速度、实际角位移、实际角速度等引 required by the driven shaft and add it to the dynamic 脚；然后通过HAL引脚之间的连接，将LinuxCNC torque feedforward module) 系统中的各类数据传递到对应的引脚中：随后进 8 行位置域PD控制补偿量计算：最终将计算得到的 Dynamic torque feedforward module(calculate the current 力矩补偿值以伺服电机的力矩前馈的方式传递给 required by each servo motor through the transformation relationship between torque and current) 各关节对应的伺服电机驱动器，实现对六关节机 器人的位置域控制 Driving current limiting module(limiting the 按照HAL编程语法，位置域PD控制算法模 driving current) 块的comp文件（如pdc605.comp)编写完成后，可 以通过halcompile命令对该文件进行编译，编译步 骤如下： Electric (1)首先将pdc605.comp放置在src文件夹下； machinery Encoder feedback (2)然后在src/scrips目录下配置HAL程序的 图3位置域PD控制算法模块结构图 编译环境source../rip-environment; Fig.3 Module structure of the PD control algorithm in position domain (3)在Linux的终端上输入halcompilepdce605.comp

内存中读取由任务控制器发送的运动轨迹命令， 并进行机器人的运动速度规划（小线段拟合、速度 规划等）、运动学正逆变换（关键点：同时确定主运 动轴、从运动轴所对应的关节轴以及构造以主运 动关节轴位置为自变量的其他各个从动关节轴位 置的函数等），并实现时域/位置域的混合插补计 算、动力学前馈等运动控制功能； （5）将位置域 PD 控制算法模块，以 HAL 组件 的方式加入到硬件抽象层（HAL）. HAL 通过定义 的引脚（HAL pin）获取运动控制器中的各种实时 数据，该数据经过 HAL 中各种功能组件的运算 后，写入到实时 EtherCAT 总线的硬件驱动程序； （6）EtherCAT 总线将各个关节轴的运动控制指令 （含位置增量、力矩前馈）发送到对应的伺服驱动器， 实现对机器人各个关节轴的运动控制；同时 EtherCAT 总线将各个轴的实际位置反馈回位置域 PD 控制算法 模块，以便计算各个从动关节轴的、以主动轴位置为 自变量的位置、相对速度误差以及力矩前馈值，为下 一个控制周期准备好位置域 PD 控制相关的数据. 2.2    位置域控制算法 如图 3 所示，本文设计的位置域 PD 控制算法 分解为以下四个功能模块. Position domain PD control algorithm module HAL pin HAL pin HAL pin Electric machinery Encoder feedback Joint command position generated by interpolator Encoder feedback joint actual position Active axis selection module (take the joint axis with the largest displacement in the motion cycle as the active axis of the cycle) Position domain PD control compensation calculation module (determine the compensation amount of the torque required by the driven shaft and add it to the dynamic torque feedforward module) Dynamic torque feedforward module (calculate the current required by each servo motor through the transformation relationship between torque and current) Driving current limiting module (limiting the driving current) 图 3    位置域 PD 控制算法模块结构图 Fig.3    Module structure of the PD control algorithm in position domain （1）主动轴选择模块. 位置域控制算法的关键是，在某段轨迹上实 时确定各个关节轴之间的主、从运动关系，即确定 主动关节轴与从动关节轴的索引号. 本文采用将 运动控制周期内位移增量最大的关节轴，作为该 周期的主运动轴，其他关节轴作为从运动轴. （2）位置域 PD 控制补偿量计算模块. 对于主动轴选择模块确定的从动轴，本模块 将各个从动轴的实际位置与理论位置的差值，从 时间域转化到位置域上，并将根据公式（5）分别求 得各个从运动关节轴所需力矩的补偿量. （3）动力学力矩前馈模块. 通过力矩与电流间的转化关系，根据各个关 节所需的力矩补偿量，求出各伺服电机所需的电 流补偿，采用电流/力矩前馈的方法，将其补偿电流 叠加到伺服控制器的电流环上，以改善伺服电机 的动力学特性，提高整个机器人运动轨迹的准确性. （4）驱动电流限流调整模块. 该模块主要对各个关节轴的驱动电流进行限 流，防止由于驱动伺服电机运动的电流过大而造 成的机器人抖动等性能变差以及安全问题. 2.3    位置域控制程序的实现 位置域 PD 控制算法模块是通过 HAL 组件实 现的. 由于该组件中所包含的动力学力矩前馈模 块与驱动电流限流模块不是本文关注的重点，且 动力学力矩前馈模块涉及惯性矩阵、摩擦阻尼矩 阵、重力/戈氏力矩阵等基于旋量理论的动力学建模 和摩擦力模型建立等，较为冗长，这里不再赘述. 本文所设计的位置域 PD 控制算法模块，首先 定义了机器人各关节的期望角位移、期望角速 度、期望角加速度、实际角位移、实际角速度等引 脚；然后通过 HAL 引脚之间的连接，将 LinuxCNC 系统中的各类数据传递到对应的引脚中；随后进 行位置域 PD 控制补偿量计算；最终将计算得到的 力矩补偿值以伺服电机的力矩前馈的方式传递给 各关节对应的伺服电机驱动器，实现对六关节机 器人的位置域控制. 按照 HAL 编程语法，位置域 PD 控制算法模 块的 comp 文件（如 pdc605.comp）编写完成后，可 以通过 halcompile 命令对该文件进行编译，编译步 骤如下： （1）首先将 pdc605.comp 放置在 src 文件夹下； （2）然后在 src/scrips 目录下配置 HAL 程序的 编译环境 source./rip-environment； （3）在Linux 的终端上输入halcompile pdc605.comp， · 248 · 工程科学学报，第 44 卷，第 2 期

崔旭东等：面向六关节机器人的位置域控制 249 则src文件夹中会生成一个pdc605.c文件： 3.1圆形轨迹控制实验及轨迹精度分析 (4)随后在Linux的终端上输入halcompile- 为验证本文所提出的位置域PD控制系统在 install pdc605,则rtlib文件夹中会生成一个pdc605.so 改善机器人末端轮廓轨迹精度的有效性，以某公 内核实时运行的动态库文件： 司HSR-JR605工业机器人为实验对象，进行平面 (5)最后在configs文件夹中的puma605.hal文 圆形轨迹跟踪实验.本实验设计的平面圆形轮廓 件里添加loadrt pdc605(加载HAL组件pdc605)以及 的理论轨迹如下：在机器人基坐标系下圆心为 addf pdc605.0.pdc servo-thread(添加函数pdc到1ms O(450,0,250),半径R为100，轨迹运行起点为 实时线程)： A(450,-100,250)单位为mm,运动方向为顺时针 (6)利用HAL的net命令和setp命令进行相关 方向，进给速率F分别为30000和15000，单位为 引脚的连接和设置. mm:min,下同 编译过程的终端命令窗口如图4所示 为验证位置域PD控制算法的有效性，在运行 相同轨迹的情况下，分别进行两组实验以形成对 cnts/linuxcnc masters cd scripts 照：第一组实验为未加入位置域PD控制算法模 块，在原有的时域控制下进行轨迹实时采集；第二 er/scriptss cd 组为加入位置域PD控制算法模块后进行轨迹跟 nts/linuxcnc-masters cd src ster/srcs halcomile.-install.comp 踪与实时采集 er/rtlib/ 使用HAL示波器实时采集轨迹运行过程中机 ts/linucnc-aster/srcs linuxcn 2.8.0-prel 器人各关节轴的相关数据，HAL示波器设置8个 采样通道，分别为关节1、2、3、5的理论位置与实 际位置（关节4、6在轨迹运行过程中未发生运动， 相关数据均设为0)，采样周期为6ms,采样数量 图4终端命令窗口 为2000个.数据采集完成后，根据运动学正变换， Fig.4 Terminal command window 可得到在机器人笛卡尔基坐标系下机器人未端的 运动轨迹，并对两组实验的未端轨迹进行对比 3实验与分析 设定进给速率为30000 mm:min时，普通时 本文所研究的对象，允许有一定的稳态误差， 域控制下末端的三维运动轨迹及其三个方向投影 所以实际使用时，PD参数中的I参数为0，亦即实 与加入位置域PD控制算法模块后的情况对比如 际为PD调节.PD参数的整定，采用的是4：1的 图5所示 衰减曲线法.对于本文所作的研究中，PD控制周 注意：图5至图8中，黑色线条为理论轨迹，红 期为1ms,也就是在PD参数整定的时候，采样观 色为时域控制下的轨迹，蓝色为位置域控制下的 察的周期为1ms.各个关节轴在位置域控制中，经 轨迹.本文所有的位置域控制采用的是非线性的 过整定后的PID参数如表1所示 位置域控制方式. 设定进给速率为15000，时域控制下末端的三 表1PID参数表 维运动轨迹及其三个方向投影与加入位置域PD Table 1 PID parameters 控制算法模块后的情况对比如图6所示 The shaft no. Parameter P Parameter I Parameter D 通过图5、图6可以明显地看出，位置域PD JI 0.31 0 6.2 控制下的平面圆形轨迹具有更好的轮廓跟踪精 J2 0.38 0 5.7 度，证明了本文所提出的位置域PD控制系统，能 J3 0.45 0 4.95 够有效提高六关节机器人末端的轮廓轨迹精度 J4 0.53 0 3.71 3.2矩形轨迹控制实验及轨迹精度分析 J5 0.78 0 3.12 如图7、图8所示，为不同运动速度下平面矩 J6 0.93 0 1.86 形轨迹运动精度检验的实验.本实验设计的平面 矩形理论轨迹如下：在机器人基坐标系下，轨迹运 每个关节轴的惯量以及电气、机械传动的属 行起点为A(350,-100,250),分别经过B(550,-100, 性不一样，所以各个轴的PD参数不一样 250),C(550,100,250),D(350,100,250)三个点后回

则 src 文件夹中会生成一个 pdc605.c 文件； （ 4）随后在 Linux 的终端上输入 halcompile - install pdc605，则 rtlib 文件夹中会生成一个 pdc605.so 内核实时运行的动态库文件； （5）最后在 configs 文件夹中的 puma605.hal 文 件里添加 loadrt pdc605（加载 HAL 组件 pdc605）以及 addf pdc605.0.pdc servo-thread（添加函数 pdc 到 1 ms 实时线程）； （6）利用 HAL 的 net 命令和 setp 命令进行相关 引脚的连接和设置. 编译过程的终端命令窗口如图 4 所示. 图 4    终端命令窗口 Fig.4    Terminal command window 3    实验与分析 本文所研究的对象，允许有一定的稳态误差， 所以实际使用时，PID 参数中的 I 参数为 0，亦即实 际为 PD 调节. PD 参数的整定，采用的是 4∶1 的 衰减曲线法. 对于本文所作的研究中，PD 控制周 期为 1 ms，也就是在 PD 参数整定的时候，采样观 察的周期为 1 ms. 各个关节轴在位置域控制中，经 过整定后的 PID 参数如表 1 所示. 表 1  PID 参数表 Table 1   PID parameters The shaft no. Parameter P Parameter I Parameter D J1 0.31 0 6.2 J2 0.38 0 5.7 J3 0.45 0 4.95 J4 0.53 0 3.71 J5 0.78 0 3.12 J6 0.93 0 1.86 每个关节轴的惯量以及电气、机械传动的属 性不一样，所以各个轴的 PD 参数不一样. 3.1    圆形轨迹控制实验及轨迹精度分析 为验证本文所提出的位置域 PD 控制系统在 改善机器人末端轮廓轨迹精度的有效性，以某公 司 HSR-JR605 工业机器人为实验对象，进行平面 圆形轨迹跟踪实验. 本实验设计的平面圆形轮廓 的理论轨迹如下 ：在机器人基坐标系下圆心为 O(450,  0,  250)， 半 径 R 为 100， 轨 迹 运 行 起 点 为 A(450, −100, 250)，单位为 mm，运动方向为顺时针 方向，进给速率 F 分别为 30000 和 15000，单位为 mm·min−1，下同. 为验证位置域 PD 控制算法的有效性，在运行 相同轨迹的情况下，分别进行两组实验以形成对 照：第一组实验为未加入位置域 PD 控制算法模 块，在原有的时域控制下进行轨迹实时采集；第二 组为加入位置域 PD 控制算法模块后进行轨迹跟 踪与实时采集. 使用 HAL 示波器实时采集轨迹运行过程中机 器人各关节轴的相关数据，HAL 示波器设置 8 个 采样通道，分别为关节 1、2、3、5 的理论位置与实 际位置（关节 4、6 在轨迹运行过程中未发生运动， 相关数据均设为 0），采样周期为 6 ms，采样数量 为 2000 个. 数据采集完成后，根据运动学正变换， 可得到在机器人笛卡尔基坐标系下机器人末端的 运动轨迹，并对两组实验的末端轨迹进行对比. 设定进给速率为 30000 mm·min−1 时，普通时 域控制下末端的三维运动轨迹及其三个方向投影 与加入位置域 PD 控制算法模块后的情况对比如 图 5 所示. 注意：图 5 至图 8 中，黑色线条为理论轨迹，红 色为时域控制下的轨迹，蓝色为位置域控制下的 轨迹. 本文所有的位置域控制采用的是非线性的 位置域控制方式. 设定进给速率为 15000，时域控制下末端的三 维运动轨迹及其三个方向投影与加入位置域 PD 控制算法模块后的情况对比如图 6 所示. 通过图 5、图 6 可以明显地看出，位置域 PD 控制下的平面圆形轨迹具有更好的轮廓跟踪精 度，证明了本文所提出的位置域 PD 控制系统，能 够有效提高六关节机器人末端的轮廓轨迹精度. 3.2    矩形轨迹控制实验及轨迹精度分析 如图 7、图 8 所示，为不同运动速度下平面矩 形轨迹运动精度检验的实验. 本实验设计的平面 矩形理论轨迹如下：在机器人基坐标系下，轨迹运 行起点为 A(350, −100, 250)，分别经过 B(550, −100, 250)，C(550, 100, 250)，D(350, 100, 250) 三个点后回 崔旭东等： 面向六关节机器人的位置域控制 · 249 ·

250 工程科学学报，第44卷，第2期 100 Common control track (b) 250.10r(a) End theory trac 60 250.05 % 20 Common control track 250.00 0 职or 249.95 -20 249.90 40 100 50 600 -60 0 500 -50 -80 400 y/mm -100300 x/mm -100 300 350 400 450 500550 660 x/mm 250.10 250.10 (c) Common control track (d) 250.08 250.08 PID control track 250.06 End theory track 250.06 250.04 250.04 目250.02 目250.02 250.00 号250.00 249.98 249.98 249.96 249.96 Common control track 249.94 249.94 PID control track -End theory track 249.9 249.92 600 550 500450400 350300 -100 -60 -20 20 60 100 x/mm 图5进给速率为30000 mm-min'时不同控制下末端的平面圆形跟踪轨迹对比图.(a)三维图：(b)xy平面投影：(c)x-:平面投影；(d)y-:平面投影 Fig5 Comparison of the planar circular tracking trajectories at the ends under different controls when the feed rate is 30000 mmmin (a)three- dimensional figure;(b)x-y plane projection;(c)xplane projection,(d)y-=plane projection 100r (b) 80 250.10r(a) 6 -End theory tra % 250.05 Sommon control track 20 control track End theory track 250.00 0 249.95 -20 24980 -40 50 600 -60 0 500 -80 -50 y/mm 400 -100300 x/mm -100 00 350 400 450 500550660 x/mm 250.10 250.10 (c) (d)_ -Common control track 250.08 Common control track 250.08 PID control track PID control track -End theory track 250.06 End theory track 250.06 250.04 250.04 目250.02 =250.02 元250.00 i250.00 e 249.98 249.98 249.96 249.96 249.94 249.94 249.92 249.92 600 550 500450400350300 -100 -60 -2020 60100 x/mm y/mm 图6进给速率为15000 mm-min时不同控制下末端的平面圆形跟踪轨迹对比图.(a)三维图：(b)x-y平面投影：(c)x-平面投影：(d)-平面投影 Fig.6 Comparison of the planar circular tracking trajectories at ends under different controls when the feed rate is 15000 mm'min:(a)three- dimensional figure;(b)x-yplane projection;(c)x-plane projection,(d)y=plane projection

250.10 250.05 250.00 249.95 249.90 100 50 −50 −100 300 400 500 600 0 (a) y/mm x/mm x/mm z/mm 40 20 −40 −20 −60 −80 −100 300 350 400 450 500 550 660 0 100 80 60 (b) y/mm 250.10 250.08 250.06 250.04 250.02 250.00 249.98 249.96 249.94 249.92 −100 −60 −20 20 60 100 z/mm y/mm (d) Common control track PID control track End theory track 250.10 250.08 250.06 250.04 250.00 250.02 249.98 249.96 249.94 249.92 600 550 500 450 400 350 300 z/mm x/mm (c) Common control track PID control track End theory track Common control track PID control track End theory track Common control track PID control track End theory track 图 5    进给速率为 30000 mm·min−1 时不同控制下末端的平面圆形跟踪轨迹对比图. （a）三维图；（b）x−y 平面投影；（c）x−z 平面投影；（d）y−z 平面投影 Fig.5     Comparison  of  the  planar  circular  tracking  trajectories  at  the  ends  under  different  controls  when  the  feed  rate  is 30000 mm·min−1:  (a)  three￾dimensional figure; (b) x−y plane projection; (c) x−z plane projection; (d) y−z plane projection 250.10 250.05 250.00 249.95 249.90 100 50 −50 −100 300 400 500 600 0 Common control track PID control track End theory track (a) y/mm x/mm x/mm z/mm 40 20 −40 −20 −60 −80 −100 300 350 400 450 500 550 660 0 100 80 60 (b) y/mm Common control track PID control track End theory track 250.10 250.08 250.06 250.04 250.00 250.02 249.98 249.96 249.94 249.92 600 550 500 450 400 350 300 z/mm x/mm (c) Common control track PID control track End theory track 250.10 250.08 250.06 250.04 250.02 250.00 249.98 249.96 249.94 249.92 −100 −60 −20 20 60 100 z/mm y/mm (d) Common control track PID control track End theory track 图 6    进给速率为 15000 mm·min−1 时不同控制下末端的平面圆形跟踪轨迹对比图. （a）三维图；（b）x−y 平面投影；（c）x−z 平面投影；（d）y−z 平面投影 Fig.6     Comparison  of  the  planar  circular  tracking  trajectories  at  ends  under  different  controls  when  the  feed  rate  is 15000 mm·min−1:  (a)  three￾dimensional figure; (b) x–y plane projection; (c) x–z plane projection; (d) y–z plane projection · 250 · 工程科学学报，第 44 卷，第 2 期

崔旭东等：面向六关节机器人的位置域控制 251· 150 (b) 100 250.10r(a) Common control track 250.05 0 PID ol t End theory track 250.00 ww 0 249.95 249280 Common control track -50 control 00 600 0 -100 500 100 400 wmm -200300 x/mm -15000 350 400450500550660 x/mm 250.10 250.10 250.08 (c) Common control track 250.08 PID control track oroo rack 250.06 End theory track 250.06 End theory track 250.04 250.04 目250.02 号250.00 249.98 249.98 249.96 249.96 249.94 249.94 249.9 00 550 500450 400 350 300 249.92150 -100 -50 020100150 x/mm wmm 图7进给速率为15000mm:mim时不同控制下末端的平面矩形跟踪轨迹对比图.(a)三维图：(b)x-y平面投影：(c)x-:平面投影：(d)y一：平面投影 Fig.7 Comparison of the planar rectangular tracking tracks at ends under different controls at a feed rate of 15000 mm-min:(a)three-dimensional figure;(b)x-y plane projection;(c)x-plane projection;(d)y=plane projection 150 (b) 250.10〔(a) 100 ontrol track 250.05 50 End th heory track 250.0 0 249.95 249.9 mon control track -50 200 100 End thec ory track 600 500 -100 -100 400 y/mm -200300 x/mm 150300 350 400 450 500550600 x/mm 250.10 250.10 (d) ntrol track 250.08 250.08 End theory trac ck 250.06 -PID -End theory track 250.06 250.04 250.04 250.00 250.00 249.98 249.98 249.96 249.96 249.9 249.9 00 550 500 450 400 350 -150 -100 -50 0 50100150 x/mm 图8进给速率为10000 mm:min时不同控制下末端的平面矩形跟踪轨迹对比图.(a)三维图：(b)x-y平面投影：(c)x-:平面投影：(d)-:平面投影 Fig.8 Comparison of the planar rectangular tracking tracks at ends under different controls at a feed rate of 10000 mmmin:(a)three-dimensional figure;(b)xy plane projection;(c)x-plane projection;(d)y-=plane projection

250.10 250.05 250.00 249.95 249.90 200 100 −100 −200 300 400 500 600 0 Common control track PID control track End theory track (a) y/mm x/mm x/mm z/mm 150 50 −50 −100 −150300 350 400 450 500 550 660 0 100 (b) y/mm 250.10 250.08 250.06 250.04 250.00 250.02 249.98 249.96 249.94 249.92 600 550 500 450 400 350 300 z/mm x/mm (c) 250.10 250.08 250.06 250.04 250.02 250.00 249.98 249.96 249.94 249.92−150 −100 −50 0 20 100 150 z/mm y/mm (d) Common control track PID control track End theory track Common control track PID control track End theory track Common control track PID control track End theory track 图 7    进给速率为 15000 mm·min−1 时不同控制下末端的平面矩形跟踪轨迹对比图. （a）三维图；（b）x−y 平面投影；（c）x−z 平面投影；（d）y−z 平面投影 Fig.7    Comparison of the planar rectangular tracking tracks at ends under different controls at a feed rate of 15000 mm·min−1: (a) three-dimensional figure; (b) x−y plane projection; (c) x−z plane projection; (d) y−z plane projection 250.10 250.10 150 50 −50 −100 −150300 350 400 450 500 550 600 0 100 250.05 250.00 249.95 249.90 200 100 −100 −200 300 400 500 600 0 250.08 250.06 250.04 250.02 250.00 249.98 249.96 249.94 600 550 500 450 Common control track PID control track End theory track 400 350 300 z/mm x/mm (c) (a) (b) 250.10 250.08 250.06 250.04 250.02 250.00 249.98 249.96 249.94 −150 −100 −50 0 50 100 150 z/mm y/mm y/mm x/mm x/mm z/mm y/mm (d) Common control track PID control track End theory track Common control track PID control track End theory track Common control track PID control track End theory track 图 8    进给速率为 10000 mm·min−1 时不同控制下末端的平面矩形跟踪轨迹对比图. （a）三维图；（b）x−y 平面投影；（c）x−z 平面投影；（d）y−z 平面投影 Fig.8    Comparison of the planar rectangular tracking tracks at ends under different controls at a feed rate of 10000 mm·min−1: (a) three-dimensional figure; (b) x−y plane projection; (c) x−z plane projection; (d) y−z plane projection 崔旭东等： 面向六关节机器人的位置域控制 · 251 ·

252 工程科学学报，第44卷，第2期 到A,单位为mm;运动方向为逆时针方向，进给速 2019(11):54) 率F分别为15000和10000. [2] Zhang H X.Research on the development status and trend of 设定进给速率为15000，时域控制下末端的三 industrial robots at home and abroad.Electron World,2013(12):5 (张红霞.国内外工业机器人发展现状与趋势研究.电子世界， 维运动轨迹及其三个方向投影与加入位置域PD 2013(12):5) 控制算法模块后的情况对比如图7所示 [3] Zhang Y.Review of the development history of foreign industrial 设定进给速率为10000，时域控制下末端的三 robots.Robot Ind,2015(3):68 维运动轨迹及其三个方向投影与加入位置域PD (张宇.国外工业机器人发展历史回顾.机器人产业，2015(3)： 控制算法模块后的情况对比如图8所示 68) 通过图7、图8可以明显地看出，位置域PD控 [4] Yu D Q.Current situation and trend of industrial robots at home 制下的平面矩形轨迹具有更好的轮廓轨迹精度， and abroad.Popular Utili=Electr,2017(9):20 (余德泉.国内外工业机器人发展现状与趋势.大众用电， 亦证明了本文所提出的位置域PD控制系统，能够 2017(9):20) 有效提高六关节机器人末端的轮廓轨迹的精度. [5] Ostergaard E H.Future road of industrial robots.Office 4结论 Informatization,2015(11):15 (艾斯本·奥斯特加.工业机器人的未来之路.办公自动化， 本文以时间域PID控制原理为依据，基于机 2015(11):15) 器人动力学模型推导出多自由度串联关节型机器 [6]Astrom K J,Hagglund T.The future of PID control.Contro/Eng 人的位置域PD控制模型，并设计了基于六关节机 Practice,2001,9(11:1163 [7]Wang S P.Xie L.Li L P.et al.Covert attack technology of 器人位置域PD的实时控制的整体方案；紧接着展 EtherCAT based 7 degrees of freedom manipulator.Chin J Eng, 开了基于LinuxCNC系统的六关节机器人位置域 2020,42(12):1653 控制系统的研究，从位置域PD控制系统总体方 (汪世鹏，解仑，李连鹏，等.基于EtherCAT总线的七自由度机械 案、位置域PD控制算法模块以及位置域PD控制 臂的隐蔽攻击技术.工程科学学报，2020,42(12)：1653) 程序设计三个方面，将六关节机器人位置域PD控 [8] Barton K L,Bristow D A,Alleyne A G.Design of a linear time- 制算法在LinuxCNC控制系统中予以实现；在所搭 varying cross-coupled iterative leaming controller/008 American 建的六关节机器人实验平台中，通过相关实验的 Control Conference (ACC).Seattle,2008:3914 [9] 设计、数据采集及分析，验证了本文所研究的位置 Hu C X.Yao B.Wang Q F.Coordinated contouring controller design for an industrial biaxial linear motor driven 域PD控制算法，在提高六关节机器人末端轮廓轨 gantrylllEEElASME International Conference on Advanced 迹运动精度方面的有效性 Intelligent Mechatronics(AIM2009).Singapore,2009:1810 位置域控制算法，用在复杂的关节型机器人 [10]Ouyang P R,Dam T,Pano V.Cross-coupled PID control in 的实时控制中，其难点不在于如何实时确定关节 position domain for contour tracking.Roborica,2015,33(6):1351 轴的主从关系，而是在确定了主从关系后，如何实 [11]Mu H R.Analysis and research on motion control of industrial 时地构造以主动关节轴位置为自变量、各个从动 robots.Electron Technol Software Eng,2018(23):115 (牟海荣.工业机器人运动控制分析与研究.电子技术与软件工 关节轴位置为因变量的各自对应的函数.本文针 程，2018(23)：115) 对的是已知轮廓轨迹的方式，可以离线确定各个 [12]Yeh SS,Hsu P L.A new approach to bi-axial cross-coupled 指令运动轨迹段中的主、从运动关系及从动轴的 control//Proceedings of the 2000 IEEE International Conference 位置函数，从而简化了实验.如若真正地将位置域 on Control Applications.Anchorage,2000:168 控制方式，引入到多轴联动的复杂机械结构形式 [13]Ouyang P R,Dam T,Huang J,et al.Contour tracking control in 的机器人实时运动控制，则在运动轨迹代码的解 position domain.Mechatronics,2012,22(7):934 释、小线段轨迹拟合、速度规划阶段，实时确定 [14]Ouyang P R.Kang H M,Yue W H,et al.Revisiting hybrid five- bar mechanism:Position domain control application//2014 /EEE 主、从关节轴及其关系，尤其是如何实时构建从动 International Conference on Information and Automation (ICIA). 轴的位置函数方面，尚需做进一步深入研究 Hailar,2014:795 [15]Ouyang P R,Pano V,Tang J,et al.Nonlinear PD-type control in 参考文献 position domain/2016 IEEE 11th Conference on Industrial [Zhou J.Intelligent manufacturing is the main direction of "Made Electronics and Applications (ICIEA).Hefei,2016:2407 in China 2025".Enterprise Observer,2019(11):54 [16]Yue W H,Pano V,Ouyang P R,et al.Model-independent position (周济.智能制造是“中国制造2025”主攻方向.企业观察家， domain sliding mode control for contour tracking of robotic

到 A，单位为 mm；运动方向为逆时针方向，进给速 率 F 分别为 15000 和 10000. 设定进给速率为 15000，时域控制下末端的三 维运动轨迹及其三个方向投影与加入位置域 PD 控制算法模块后的情况对比如图 7 所示. 设定进给速率为 10000，时域控制下末端的三 维运动轨迹及其三个方向投影与加入位置域 PD 控制算法模块后的情况对比如图 8 所示. 通过图 7、图 8 可以明显地看出，位置域 PD 控 制下的平面矩形轨迹具有更好的轮廓轨迹精度， 亦证明了本文所提出的位置域 PD 控制系统，能够 有效提高六关节机器人末端的轮廓轨迹的精度. 4    结论 本文以时间域 PID 控制原理为依据，基于机 器人动力学模型推导出多自由度串联关节型机器 人的位置域 PD 控制模型，并设计了基于六关节机 器人位置域 PD 的实时控制的整体方案；紧接着展 开了基于 LinuxCNC 系统的六关节机器人位置域 控制系统的研究，从位置域 PD 控制系统总体方 案、位置域 PD 控制算法模块以及位置域 PD 控制 程序设计三个方面，将六关节机器人位置域 PD 控 制算法在 LinuxCNC 控制系统中予以实现；在所搭 建的六关节机器人实验平台中，通过相关实验的 设计、数据采集及分析，验证了本文所研究的位置 域 PD 控制算法，在提高六关节机器人末端轮廓轨 迹运动精度方面的有效性. 位置域控制算法，用在复杂的关节型机器人 的实时控制中，其难点不在于如何实时确定关节 轴的主从关系，而是在确定了主从关系后，如何实 时地构造以主动关节轴位置为自变量、各个从动 关节轴位置为因变量的各自对应的函数. 本文针 对的是已知轮廓轨迹的方式，可以离线确定各个 指令运动轨迹段中的主、从运动关系及从动轴的 位置函数，从而简化了实验. 如若真正地将位置域 控制方式，引入到多轴联动的复杂机械结构形式 的机器人实时运动控制，则在运动轨迹代码的解 释、小线段轨迹拟合、速度规划阶段，实时确定 主、从关节轴及其关系，尤其是如何实时构建从动 轴的位置函数方面，尚需做进一步深入研究. 参    考    文    献 Zhou J. Intelligent manufacturing is the main direction of “Made in China 2025”. Enterprise Observer, 2019（11）: 54 （周济. 智能制造是“中国制造2025”主攻方向. 企业观察家, [1] 2019（11）：54） Zhang  H  X.  Research  on  the  development  status  and  trend  of industrial robots at home and abroad. Electron World, 2013（12）: 5 （张红霞. 国内外工业机器人发展现状与趋势研究. 电子世界, 2013（12）：5） [2] Zhang Y. Review of the development history of foreign industrial robots. Robot Ind, 2015（3）: 68 （张宇. 国外工业机器人发展历史回顾. 机器人产业, 2015（3）： 68） [3] Yu D Q. Current situation and trend of industrial robots at home and abroad. Popular Utiliz Electr, 2017（9）: 20 （余德泉. 国内外工业机器人发展现状与趋势. 大众用电, 2017（9）：20） [4] Ostergaard  E  H.  Future  road  of  industrial  robots. Office Informatization, 2015（11）: 15 （艾斯本·奥斯特加. 工业机器人的未来之路. 办公自动化, 2015（11）：15） [5] Astrom K J, Hagglund T. The future of PID control. Control Eng Practice, 2001, 9（11）: 1163 [6] Wang  S  P,  Xie  L,  Li  L  P,  et  al.  Covert  attack  technology  of EtherCAT  based  7  degrees  of  freedom  manipulator. Chin J Eng, 2020, 42（12）: 1653 （汪世鹏, 解仑, 李连鹏, 等. 基于EtherCAT总线的七自由度机械 臂的隐蔽攻击技术. 工程科学学报, 2020, 42（12）：1653） [7] Barton K L, Bristow D A, Alleyne A G. Design of a linear time￾varying cross-coupled iterative learning controller//2008 American Control Conference (ACC). Seattle, 2008: 3914 [8] Hu  C  X,  Yao  B,  Wang  Q  F.  Coordinated  contouring  controller design  for  an  industrial  biaxial  linear  motor  driven gantry//IEEE/ASME International Conference on Advanced Intelligent Mechatronics (AIM 2009). Singapore, 2009: 1810 [9] Ouyang  P  R,  Dam  T,  Pano  V.  Cross-coupled  PID  control  in position domain for contour tracking. Robotica, 2015, 33（6）: 1351 [10] Mu  H  R.  Analysis  and  research  on  motion  control  of  industrial robots. Electron Technol Software Eng, 2018（23）: 115 （牟海荣. 工业机器人运动控制分析与研究. 电子技术与软件工 程, 2018（23）：115） [11] Yeh  S  S,  Hsu  P  L.  A  new  approach  to  bi-axial  cross-coupled control//Proceedings of the 2000 IEEE International Conference on Control Applications. Anchorage, 2000: 168 [12] Ouyang P R, Dam T, Huang J, et al. Contour tracking control in position domain. Mechatronics, 2012, 22（7）: 934 [13] Ouyang P R, Kang H M, Yue W H, et al. Revisiting hybrid five￾bar  mechanism:  Position  domain  control  application//2014 IEEE International Conference on Information and Automation (ICIA). Hailar, 2014: 795 [14] Ouyang P R, Pano V, Tang J, et al. Nonlinear PD-type control in position  domain//2016 IEEE 11th Conference on Industrial Electronics and Applications (ICIEA). Hefei, 2016: 2407 [15] Yue W H, Pano V, Ouyang P R, et al. Model-independent position domain  sliding  mode  control  for  contour  tracking  of  robotic [16] · 252 · 工程科学学报，第 44 卷，第 2 期