基于强跟踪滤波的机器人运动轨迹控制系统设计

霍延军，崔 巍

(1.延安职业技术学院 人事处，陕西 延安 716000；2.延安大学 物理与电子信息学院，陕西 延安 716000)

0 引言

在工业生产中，机器人常常遇到起动或停止时手抖动、机器人末端执行器不能移动到指定位置等问题。这种问题的出现，大大提高了机器人机构损坏概率[1]。这就要求机器人系统在运动过程中必须遵循尽可能平稳的原则，避免位置突然转变而导致速度和加速度的改变[2]。若移动不稳定，则会增加机械零件磨损，振动和冲击机器人系统。为保证机器人能够到达指定位置，需提高整个机器人系统运行稳定性，因此，需先设计出运行轨迹，再借助仿真技术进行仿真分析以避免不良现象发生[3]。轨迹规划优劣，直接影响机器人操作效率。

以往采用抛物过渡法来控制该系统，在节点空间进行轨迹规划，计算简单[4]。但由于关节空间的轨迹规划路径不唯一，关节角度函数不具代表性，且运动轨迹易漏检；而空间路径规划方法严格要求运动路径和姿态瞬时变化规律为动作路径和姿态，必须在直角空间执行轨迹规划，用关节角函数来描述机器人运动轨迹，即机器人末端执行机构运动轨迹由关节变量直接确定[5]。但是，直角空间路径规划需要大量计算，导致控制范围过大。根据传统方法存在的问题，设计了基于强跟踪滤波的机器人运动轨迹控制系统。将强跟踪滤波方法应用于轨迹控制中，实验结果说明该系统设计的有效性。

1 系统总体架构设计

提出的基于强跟踪滤波的机器人轨迹规划方法，应用在机器人轨迹控制系统中，并在图1中得到具体描述。

图1 系统总体架构

由图1可知，该系统通过软硬件的协同工作，实现了伺服控制。使用轨迹规划器，将输出规划位置输入轴资源AXIS之中；资源轴是指控制并输出驱动报警和停止信号软、硬件组合[6]。控制脉冲经等效变换后送至电机控制器，再通过比较规划位置和实际接收脉冲信号，获取误差，及时调整，最终获得机器人轨迹[7]。

2 硬件结构设计

通过拉伸直流伺服电机提供机器人各关节的转矩，DC电机线性调速特性好，过载能力强[8]。采用三菱 IPM电机驱动，通过强大跟踪滤波控制整个系统。硬件结构如图2所示。

图2 硬件结构

根据指令和感知信息控制机器人，可完成某一特定动作或任务的轨迹控制[9]。该系统采用多CPU结构及分布式控制方式，分为上下位机控制系统两个层次[10]。上位机负责管理整个系统，下位机由许多单片机组成，单片机控制机器人关节动作，上下位机的CPU通过并行总线相连[11]。

2.1 PCI-485接口卡

上下位机与外部设备连接的总线就是PCI总线，具有即插即用优势，保证数据稳定、高效传输。在各种与计算机连接的设备中得到广泛应用[12]。考虑到 PCI总线协议复杂性，设计利用 PLX专用PCI9052接口控制 PCI总线，以实现 PCI上、下位机的通讯。整个数据传输系统的结构如图3所示。

图3 数据传输系统结构

MAX485卡插入计算机的 PCI插槽，主要包括专用 PCI芯片 EXRXR17D158，该芯片由整流控制器93LC46提供额外存储空间，并被连接到DART通道，SP485微分信号直接与DSPMAX485芯片控制卡连接，通过LF2407模块的DSP控制芯片串行通信，实现上位机与下位机之间数据传输[13]。

2.2 上下位机

上位机为普通台式计算机，安装了 Windows XP操作系统，主要分析机器人运动轨迹。其作用是向下位机发送各种任务中断信息，刺激下位机调用相应程序来完成操作任务[14]。

采用专用PCI-485板完成上下位机通讯，并将RS-485模块差分信号接入下位机之中，实现可靠数据传输。下位机采用TMS320LF2407A型号 DSP芯片，主要完成脉冲信号的输出、位置反馈信号的采集与处理。将输出脉冲调制 PWM连接到电机驱动端，要求增加光电隔离，其功能是隔离强弱电，驱动装置和马达连接。马达输出的扭矩由传动装置带动各个关节运动。

2.3 DSP控制器

DSP控制器实现了程序空间与数据空间的完全分离，并可同时获得指令和操作数，实现不同程序空间之间数据传输。TMS32OF240XDS具有独立的内部数据结构和程序总线结构的 CPU内核[15]。

程序总线分为6个16位总线，即：程序地址总线，它提供读写程序空间的地址；数据读地址总线，提供数据空间读地址；数据写地址总线，提供写数据空间地址；程序读总线是从程序空间向 CPU传输代码、直接操作信息；数据读总线是从数据空间向中央逻辑单元传输数据；数据写总线是从程序空间和数据空间传输数据。两者都是独立的地址总线，CPU能够在同一机器运行周期中同时读写数据。

TMS320f240xds流水线有4个不同阶段：指令获取、指令解码、操作数获取和指令执行。提取指令占用 PAB和 PRDB；解码指令不占用数据总线；获得操作数时会占用 DRAB和 DRDB；指令执行时会将执行结果写回数据内存，使指令运行速度大大提高。

2.4 光电隔离驱动器

选择CAN-TTLG型号单片机光隔离超远程驱动器，具有光速发射，光速接收和信号放大优势。由输入电信号驱动 LED 发射出一定波长的光，然后由光电检测器接收，产生光电流，然后放大输出。因为光电隔离的输入与输出是相互隔离的，所以电信号传输是单向性的，所以电绝缘性能好，抗干扰能力强。光电隔离驱动器输入端为低阻抗元件，工作于电流模式，所以共模抑制能力强。因此，将其作为终端隔离驱动器能够提高信噪比。

TXD、RXD通讯口要实现远距离通讯，通常采用RS422接口，以此收发控制信号。选择CA-TTLG光电隔离超远程驱动器，无需收发控制信号，可延伸单片机通讯距离，最长可达到10 km，便于单片机组网。单片机的 TXD和 RXD信号由 TTL级驱动电路驱动，而信号自接收抑制电路则保证了设备不会接收到自己发出的信号，CA总线通过光电隔离输出 CA信号。采用隔离电源模块，实现信号与电气的隔离，确保系统抗干扰性能。

2.5 直流电机

选用100 W综合交流AGV低压直流电动机，转换直流功率为机械能，或选用旋转电动机转换机械能。该电动机可以实现直流电与机械能的转换，可将电能转化为机械能，也可将机械能转化为电能。

直流电机结构如图4所示。

图4 直流电机结构

电源接口具有3个端子号，分别是PE/VCC/GD，其对应的端子定义为接地端子、电源正、电源负。对于PE端子号连接机柜大地，线缆截面积保持0.75 mm2以上；VCC/GD端子号主电源输入端，电压为直流30.5～38.5 V，这两个端子严格按照电源正负连接，严禁反接。

电机接口具有8个端子号，分别是DIR+、DIR-、PUL+、PUL-、ENA+、ENA-、ALM+、ALM-，其对应的端子定义为方向输入正、方向输入负、脉冲输入正、脉冲输入负、使能输入正、使能输入负、报警输出正、报警输出负。对于DIR+、DIR-端子号光耦隔离输入，最大输入电压为6 V，在输入同时应串联2.4 K电阻限流，并在脉冲信号停止至少5 μs之后再切换方向信号；对于PUL+、PUL-端子号最大输入频率为200 kHz，在信号切换完成5 μs之后，再输出脉冲信号；对于ENA+、ENA-端子号最大输入频率为1 kHz，当配置为高有效时，该端口无输入使能；当配置为低有效时，该端口有输入使能。在使能信号有效至少10 ms后，再输入脉冲指令；对于ALM+、ALM-端子号最大耐压为30 V，最大带负载能力为30 mA，设置为有报警输出和无报警输出。

3 系统软件部分实现

以VC++为面向对象的上层系统开发工具，完成控制界面程序基于MFC的API功能设计，结合MallControl卡，以及操作者与信息交互之间的关系，可促使系统执行相关任务。该系统集成了机械手运动控制，实现了对机器人运动动作的控制。

主机程序通过VC提供的MS通信控制，从端口以事件驱动方式获取数据。如果缓冲区收到字符，则会触发通讯任务。根据光电隔离驱动器的串行通信协议，通过串行通信控制接口，实现了事件响应功能。

如果参数 Rthreshold为0，缓冲区将收到字符，已分配给程序，没有产生通讯任务。如果参数 Rthreshold为1，缓冲区中将收到字符，每一个字符都会触发一项通讯任务。

系统软件包括主界面软件和连续 PT运动控制软件，主要接口接收目标姿态数据，可进行点到点运动和各轴状态检测。依据检测结果确定各关节的绝对位置，判断是否超出各关节的活动范围，并在主界面上实时显示实际位置和计划位置，便于用户及时作出决定。通过强跟踪滤波方法，对机器人运动控制进行重力补偿，保证各个关节之间非线性。

设机器人运动方程为：

(1)

e=sd-s

(2)

基于对话框的机器人轨迹规划软件，采用 Visual c++6.0的 MFC核心编写。计算机与机器人之间的通信可通过菜单命令和功能按钮完成，调用参数设置窗口，退出机器人控制系统，包括控制机器人姿态、读数等。基于强跟踪滤波方法在机器人运动轨迹控制步骤如图5所示。

图5 机器人运动轨迹控制步骤

因为系统是非线性的，状态转换矩阵不确定，所以要实现系统稳定运行，就要将强跟踪滤波应用其中，通过引入弱化因子，在线调整滤波增益，最大程度提取有效信息，根据提前预测轨迹，设计具体运行轨迹，由此完成系统软件部分设计。

4 实验分析

为验证基于强跟踪滤波的机器人运动轨迹控制系统的性能，设计仿真实验。采用MATLAB仿真软件作为实验平台，根据机器人的实际尺寸进行三维仿真，因为下位机和上位机之间的通信也是通过 VC进行的，所以通过 VC实现三维仿真，在 VC接口上直接进行实时验证，有利于整个系统调试。利用 VC++6.0开发平台，在 MFC的基础上，引入 OpenGL动态链接库，对系统的设计进行验证。

实验参数设置如表1所示。

表1 实验参数

在表1所示的实验参数下，以机器人抓取物品的过程为例，选取抓取物品曲线起始点、终止点和两点之间结点，共5个地址变量，如图6所示。

图6 机器人抓取物品曲线连接示意图

由图6可知：该机器人抓取物品的运动轨迹具有不规则性，因此针对这5个节点，分别采用抛物过渡法、空间路径规划方法设计的系统和基于强跟踪滤波控制系统对运动轨迹进行对比分析，结果如图7所示。

图7 三种系统运动轨迹对比结果

由图7可知：使用抛物过渡系统5个节点均不在实际机器人运动所在位置，误差较大；使用空间路径规划系统5个节点中有2个节点在实际机器人运动所在位置，其余3个与实际位置有所偏差；基于强跟踪滤波控制系统5个节点均在实际机器人运动所在位置，无偏差。

在此基础上测试采用不同控制系统机器人多次抓取物品的轨迹控制时间，得到轨迹控制耗时对比结果如图8所示。

图8 三种系统运动轨迹对比结果

通过图8可以看出，在10次抓取实验过程中，采用抛物过渡系统的机器人运动轨迹控制耗时平均为12.1s，采用空间路径规划系统的机器人运动轨迹控制耗时平均为7.5s，而采用所提基于强跟踪滤波的机器人运动轨迹控制系统的2.3。由此可见，所提方法能够快速对机器人运动信息做出采集，并及时规划最短运动轨迹，有效提升机器人的工作效率。

5 结束语

为了解决现有机器人运动轨迹控制系统的不足，提出了一种基于强跟踪滤波的机器人运动轨迹控制系统设计方案，并将该方案应用到机器人时间优化轨迹规划中，极大地提高了工作效率。针对机器人的实际工况，在强跟踪滤波技术支持下，给出了机器人运动轨迹，并用 Matlab在实验条件下进行了仿真分析，以验证系统设计的合理性。由验证结果可知，该系统实现了机器人运动轨迹的精准控制。

受到实验条件限制，系统设计仍存在不足之处，仍需后续继续完善。

1)所设计的轨迹控制系统仅仅是基于机器人最优时间进行轨迹优化的，对能量优化或时间-能量优化轨迹设计的进一步研究将是今后工作方向。

2)根据实验情况，可以考虑在该机器人机械臂的末端安装一个触觉传感器，以抓住较硬物体。对机器人运动姿态而言，应采用何种姿态对不同形状物体进行抓取，还有进一步考虑。

3)在实验研究基础上，该视觉反馈装置还可加入实现三维图像处理的爪子位置操纵器，并进一步研究基于机器人轨迹最佳控制方案。