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 期