一个六自由度机构控制系统的实现

    摘 要： 叙述了一个六自由度机构伺服控制系统的基本结构、组成、控制原理、伺服控制算法等；介绍了TE5650A控制器模块；同时介绍了伺服控制系统的软件结构和功能，以及对由软件驱动的手动指令进给的机构抖动问题的解决方法

    关键词： 六自由度机构 伺服控制系统 控制算法 伺服控制器 系统软件 机构抖动  

      本文所述的六自由度机构是用来进行风洞外挂物可控轨迹实验的主要设备，它是外挂物模型的支撑和运动执行机构，由它来附带一个刚性物体，在一定空间范围内到达任意位置并形成任意姿态。它是由计算机控制的六个自由度相互独立运动的机电一体化装置，包括三个作直线运动的部件（轴向X、垂直方向Z、侧向Y）和三个作回转运动的部件（俯仰α、偏转β、滚转 γ）。各个自由度的示意图如图１所示。

    机构的每个自由度均由一个直流伺服电机拖动，其中轴向X、垂直方向Z、侧向Y采用滚珠丝杠传动，它们将电机的旋转运动转换为机构的直线运动；而俯仰α、偏转β、滚转γ三个自由度均采用谐波减速和齿轮传动相结合的传动，以形成各个姿态角的旋转运动。机构的每一个自由度都带有位置反馈环节和速度反馈环节以形成闭环系统，而用对位置变化量进行编码的差分增量式光电编码器产生控制器的位置和速度反馈信号。本文所述的六自由度机构控制系统对机构的各个自由度都能进行精确的运行和位置控制，同时使机构的各个自由度运行平稳。

    １ 控制系统的基本结构和功能

    1.1 控制系统组成

    该机构控制系统以一台工业PC机为核心，通过以太网与进行空间位置、姿态运算的上位机相连，接收上位机发给的各个自由度的行进指令，通过伺服控制器产生控制信号，然后经电机驱动器控制电机运行。其控制系统组成如图2所示。

    控制系统主要包括工控机、2个TE5650A伺服控制器、6个自由度的直流伺服电机、伺服驱动单元（PDC）及光电编码器、手动轻便操纵箱（MPG）和用于接受、控制机构运行状态的I/O接口板以及相应的外围控制线路。其中，TE5650A伺服控制器是实现机构控制的核心，每个TE5650A模块可以控制四个自由度的伺服电机，在本系统中，每个TE5650A模块控制三个自由度的伺服电机。第二个TE5650A模块除控制伺服电机外，还用于对手动轻便操纵箱的进给指令进行计数，以实现软件驱动的手动指令进给。直流伺服驱动器PDC将TE5650A的控制信号进行功率放大后输出给电机。

    1.2 控制系统原理

    各个自由度的控制回路均包含速度环和位置环两个闭环系统，其中速度闭环主要由伺服驱动器PDC和TE5650A控制器模块共同构成，而位置闭环则主要由TE5650A模块构成。系统中的速度和位置反馈信号都来自于一个差分增量式光电编码器，其速度值是由编码器的输出脉冲经过频率／电压变换后得到，而位置值通过对光电编码器的输出脉冲计数得到。各个自由度的控制原理如图3所示。

    TE5650A模块实现系统的闭环调节过程，它接受工控机发给的运行指令，控制各个自由度的整个运行过程。伺服控制器的控制算法为典型的PID算法，伺服控制器将当前运行轨迹过程中的目标位置与当前的实际位置相比较产生位置误差，进行PID调节。PID算法的示意图如图4所示，公式如下：

    其中，TPn为n时刻应当到达的目标位置；APn为n时刻的实际位置；∑En为n时刻的误差积分和；Kp、Ki、Kd为不同轴的系统传递函数经过计算和实验得出的PID控制参数。

    在本系统中，还采用带有速度前馈的PID算法（PIDVFF）。PIDVFF算法的示意图如图５所示，公式如下：

    其中，VEn为n时刻的速度误差；PCn为n时刻的位置误差PID算法的输出；TVn为n时刻的目标速度；AVn为n时刻的实际速度。KVFF为速度前馈系数，由用户根据实际情况设置。PIDVFF算法可以使得电机运行更加平稳。

    ２ TE5650A伺服控制器

    TE5650A是美国Technology 80公司的基于DSP技术的伺服控制器产品。它在硬件和软件上均采用了开放式的结构设计，在提供伺服控制功能的同时，也给使用和开发带来很大的方便。TE5650A模块采用PC/AT ISA总线形式，使用I/O映射方式与计算机打交道。TE5650A各个轴伺服闭环的最小控制周期为0.4ms，最大刷新频率为2.5kHz；TE5650A支持模拟信号和脉宽调制信号（PWM）两种输出控制信号。

    2.1 电机运行方式

    TE5650A伺服控制器支持两种基本类型的电机运行模式：速度方式和点到点方式。在速度方式下，可以设置电机的运行速度，让电机以恒定速度运行，直到发出指令让其停止或者改变其速度为止，其示意图如图6所示。在点到点方式下，由用户设置机构运行的目标位置和运行模式，使TE5650A控制电机平稳地运行到目标位置。点到点方式主要包括梯形和S曲线两种运行模式，其示意图分别如图7、图8所示。可以看出，S曲线运行模式的加速方式是逐渐加速，减速也是如此，它比梯形运行模式更加平稳。两种模式都需要设置目标位置、最大速率、加速度等参数，S曲线模式还需要设置加加速度参数（jerk），这个参数是指在速度变化过程中的加速度的变化率。

    2.2 TE5650A基本结构

    图9示出了TE5650A伺服控制器的基本结构。

    每个伺服轴通过增量式编码器的反馈来驱动各个轴运行到指定位置。运行轨迹发生器产生每个伺服轴在当前运行模式下的每个时刻的目标位置、速度、加速度等参量。数字滤波器用来实现PID和PIDVFF算法，以计算输出控制信号的大小。为了精确地同步各个轴的运动，TE5650A采用了双缓存技术来实现各个轴的参数和运行指令的同时发送。每个轴还有正、负限位及零点控制。计算机可以随时对TE5650A的运行状态进行监测。

    ３ PDC直流伺服驱动器

    直流伺服驱动器主要将TE5650A输出的控制信号进行功率放大和整形，以匹配不同的电机特性。同时，它还具有一级速度闭环控制，使系统运行更加平稳。直流伺服驱动器采用了北京宝伦机电技术公司的PDC系列驱动器，它采用了先进的智能型功率模块和混合集成电路，具有频带宽、响应速度快、调速范围广、位置与速度检测二合一等优点，具有良好的可靠性。

    ４ 控制系统软件的基本结构与功能

    TE5650A模块提供了多种形式的驱动程序，在Windows9598NT下，可以采用动态链接库（DLL）形式的驱动程序。它是一个函数集，对TE5650A模块的所有操作全都包含在这个函数集中，利用这些函数，可以编写适合于系统实际情况的应用程序，同时还可以利用TE5650A的许多强大的初级功能。驱动程序库对TE5650A模块的操作是进行直接的寄存器读写，这使得整个系统有很高的处理速度。

    控制系统软件是建立在TE5650A模块驱动程序之上的，它们之间的关系如图10所示。

    控制系统软件主要实现机构的实时状态显示、伺服系统的电源管理、TE5650A状态监测、机构故障联锁、故障解除、静态调试、以及通过软件驱动的手动操作；控制软件还具备接收上位机发给的网络控制命令以运行机构的网络控制功能；另外它还可以实现多个轴联动的动、静态调试功能。

    ５ 软件驱动的手动指令进给的机构抖动问题

    该机构可以通过手动轻便操纵箱进行各个自由度的人为手动的小范围内的位置和姿态调整。手动轻便操纵箱发出的进给信号为可以反映进给方向和速度的差分脉冲信号。为了减少整个系统的硬件开销，我们使用其中一个TE5650A模块剩余的一个伺服控制器，利用TE5650A对增量式编码输出脉冲的计数功能，将手动轻便操纵箱的手动进给脉冲接入该控制器的编码器信号输入端，从而实现手动进给量多少的输入，同时利用开关量进行轴选通，由工控机发出相应的运行指令，使机构运行。这样设计避免了用硬件实现手动进给时额外的硬件开销和控制线路设计。手动进给的硬件结构如图11所示。

    在执行手动时，由控制软件循环扫描TE5650A对手动进给脉冲的计数值并且清零，同时根据轴选通开关决定哪一个轴运行并且运行多少。由于系统软件的参与，一个连续的手动脉冲计数过程被程序的循环扫描过程分成了许多小段，如果TE5650A对每一个小段都执行点对点的运行方式，由于每个小段都有升、减速乃至停止的过程，因此机构运行在这种方式下显得不太平稳，有明显的抖动现象。由于α、β自由度机构的运动放大作用，这种现象更为明显。加快程序的循环扫描速度也不能解决这个问题。

    经过试验，我们发现如果在手动过程中每一个循环扫描小段采用TE5650A的速度运行方式，即将每一个小段的手动进给脉冲计数值乘以一个当量系数，用作TE5650A的执行速度值，机构运行就很平稳。即使手动进给的脉冲速度有快有慢，机构运行也只是反映手动速度的快慢，并且在速度变化过程中运行也很平稳。

    整个控制系统体积小、结构紧凑，具有较高的可靠性。机构的运行控制精度较高，可以达到±3×编码器脉冲当量以内，能够满足系统运行的需要。采用速度运行方式可以解决软件驱动的手动指令进给中的机构抖动问题。