基于DSP与FPGA的机器人运动控制系统设计

2014-07-04游有鹏

机械与电子 2014年4期

李疆，游有鹏

（南京航空航天大学机电学院，江苏南京210016）

0 引言

随着电子技术和计算机技术的发展，机器人已经在众多领域得到了日益广泛的应用。作为机器人的核心部分，机器人控制器是影响机器人性能的关键部分之一。传统的机器人控制系统基本采用专用计算机和专用机器人语言、专用微处理器的封闭式控制系统结构。这种结构限制了机器人系统的扩展性和灵活性，不利于进一步的扩展和开发。而通用、可靠、开放和实时动态的开放式控制系统，已是机器人控制系统发展的必然趋势。

利用DSP和FPGA设计了开放式架构的运动控制系统。以PC机为依托，利用它的强大处理能力，完成一些非实时任务，并通过PCI总线与下位机核心处理器DSP进行数据交互。下位机运动控制卡对机械臂进行精插补规划和实时控制等任务，并在其中嵌入了智能控制算法，同时克服了封闭式系统可扩展性差的缺陷［1］。

1 硬件平台设计

1．1 主控单元

作为下位机运动控制卡，承担了工业机器人实时控制的任务，主要工作是进行速度规划、位置控制和上位机的通讯以及任务调度等。因此，为满足工业机器人运动控制系统的要求，选取TI公司DSP，型号为TMS320F28335。同时，为实现多路PWM输出和光电编码器的反馈，以完成高精度机械臂的位置控制，选取Altera公司cycloneⅡ型号的FPGA作为辅助CPU，处理与上位机的通讯、PWM输出、编码器反馈和I／O量控制等功能。机器人控制系统硬件由工控机和基于DSP的运动控制器组成，其硬件架构如图1所示。

图1 硬件架构

1．2 上下位机通信接口

基于整个系统通信的要求，必须综合考虑速度、开发复杂度和稳定性等因素，系统选择了PCI方式进行高速数据交换。它是一种高性能的32／64位数据／地址复用总线，通信速度最快可达133 MB／s，满足控制系统的速率要求。系统采用FPGA作为PCI总线目标设备接口控制器，实现了下位控制器与客户端之间的高速实时通信，采用中断防冲突方式，把FPGA中集成的双端口RAM作为上下交互数据区域，增强了系统的集成性。

1．3 电机驱动接口

面向机器人的高精度位置控制系统设计，采用交流伺服驱动器输入模拟量的方式完成电机的闭环控制。考虑到硬件成本、多通道控制和电压输出精度，系统采用PWM＋滤波的方式实现模拟电压的输出。借助FPGA时钟计数器和比较器，输出对应电压占空比的差分信号，经过运算放大电路后即可得到－10～＋10 V的电压。

1．4 编码器反馈信号

编码器具有三相六路信号XA＋／XA－、XB＋／XB－、XZ＋／XZ－。首先，经过滤波电路消除杂波，然后，输入差分接收芯片26LS32。输出的信号直接连接到74HC14整形器，它可以将缓慢变化的输入信号转换成清晰无抖动的输出信号，Z相零位信号则用于回零的检测。编码器反馈接收原理如图2所示。

图2 编码器反馈原理

1．5 开关量接口

FPGA的开关量控制借助了74HC595芯片，它通过移位操作接收／发送8位串行数据至8个引脚并行输出。考虑到电平匹配和保护电路，信号由FPGA输出后，也要经过光耦隔离和电平转换，再经由BTS716输出，以保障其输出具有足够的驱动能力。对于开关量输入信号，为增强它的抗干扰能力，I／O接口部分需要使用光耦隔离，并且信号输入FPGA后仍要进行滤波，才能供运动控制卡使用。开关量信号输入／输出原理如图3所示。

图3 开关量接口原理

2 控制系统软件设计

系统软件主要划分为3个层次，包括上位机软件、PCI驱动程序以及运动控制DSP软件。上位机软件作为人机交互部分，主要实现状态显示、轨迹规划、轨迹优化、示教再现和参数管理等功能。PCI驱动程序主要完成上位机软件与运动控制卡之间的数据交互，包括命令的发送接收。运动控制DSP软件负责机器人控制系统的实时控制部分，主要实现速度规划、位置控制、I／O控制和参数设置等功能［2］。控制系统总体架构如图4所示。

图4 系统软件架构

上位机采用Visual C＋＋6．0开发。状态显示用来监测机器人控制系统的运行状态，并根据监测结果进行跟踪处理。轨迹规划主要完成理论曲线的速度前瞻控制，确保了运动的平稳性；轨迹优化则是研究时间最小的轨迹规划算法，使机器人的作业效率达到最优。示教再现指机器人通过记录机器人被操作路径上的点坐标，实现轨迹复现的过程。参数管理用以设置各个关节的运动参数，包括速度、加速度和加加速度的限制等。

下位机DSP控制器借助TI公司官方软件CCS3．3进行开发。通讯模块包含程序解释和状态反馈。程序解释指DSP通过FPGA的双端口RAM下载上位机发送的指令，并按照相关协议译码，得到控制指令信息，状态反馈则是把下位控制器的相关信息发送给PC端，供用户监测。速度规划部分的作用是避免机器人的冲击、振动，并在尽量减少系统运算负荷的情况下，使得插补曲线平滑。位置控制任务完成了机器人平稳、高精度的控制，尤其在连续路径控制时，保证实际轨迹和目标曲线重合。

下位机的控制程序流程如图5所示。为了促使软件模块化，提高开发效率，软件采用了DSP／BIOS实时操作系统作为整个系统的控制核心，来完成整个控制系统的任务调度。前后台系统中，循环程序通过中断来触发处理事件，其他所有后台任务都被认为具有相同的执行优先级，这就导致那些实时性要求高的任务得不到及时调度，影响整个系统的运行效率。DSP／BIOS提供可抢占式线程调度，通过高优先级线程抢占低优先级线程，实现了线程资源的优化配置［3］。

图5 下位机程序流程

3 加减速规划

机器人的轨迹插补不仅是对位置的插补，也是对速度和加速度的插补［4］。机械臂的启停平稳性直接影响到整体的控制精度，不合理的速度曲线势必会造成电机的震荡。机械臂是大惯量的执行机构，加速度是电机力矩输出的直接结果，所以巨大、突变的加速度会给予系统过大的负担，会导致不能及时响应，加剧机械的磨损。

机器人的运动分为点位（PTP）运动和连续轨迹（CP）。点位运动的轨迹插补比较简单，只需要在规定的时间里面到达目标位置。连续轨迹在笛卡尔空间进行规划，插值的方法是把目标曲线上的点细化成一组带位置和速度信息的结点，然后映射到关节空间完成速度曲线规划［5］。

基于上述分析，采用了基于PVT速度规划的位置插补，速度和加速度曲线连续无跳变［6］。PVT插补模式，即给定位移、速度和时间，用户可以利用这些参数直接控制轨迹曲线的形状。位置、速度和时间关系为：

给定连续2个数据点的“位置（s1，s2）、速度（v1，v2）和时间（t1，t2）”参数，代入函数得：

求解此方程组可以得到a，b，c，d。因此，连续2个数据点的运动规律就可以确定下来。为方便解方程，特令t1＝0，解得：

也可推出速度、加速度、加加速度计算方程为：

根据上述2组计算方程即可得到3个量。将机器人轨迹规划所得的位移、速度和时间输入PVT插补器，即可求解出三次方程。最后，利用差分公式得到每个周期的插补位移。在给定的位移和时间条件下，选取合适的速度参数，可以得到类似S形曲线的效果，最终得到加减速控制如图6所示。

图6 PVT插补加减速控制

4 位置控制

机器人的位置控制，关键在于控制机器人的各个关节到达给定位置。对于多关节机器人，随着关节递增，对应负载电机会随着重力和机械惯性影响而变化，构成一个非线性强耦合系统，因此，传统PID控制器很难保证系统的动态响应品质，使系统性能下降。为此，引入模糊PID控制策略构建关节空间位置控制系统［7］，原理如图7所示。

图7 位置控制系统

由于关节位置是一个时变量，从而使被控对象的参数变化，相应也要改变控制器的参数以满足系统要求。模糊推理器以实际位置反馈和它的变化率作为输入，按照一定的模糊控制规则导出PID控制器参数KP，KI，KD，输入给PID控制环节运算。前馈环节是为了提高系统的跟踪控制能力。位置环的PID控制器输出和前馈通道的速度输出，合成后作为总输出并以速度控制指令输出。

这种位置控制策略的主要优势在于：不同的位置对应使用不同的PID参数，不需要精确的数学模型，而且能满足机器人位置控制系统的要求。