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

工程科学学报 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 ·