基于两级dsp的并联装备开放式运动控制卡及控制方法

专利名称：基于两级dsp的并联装备开放式运动控制卡及控制方法
技术领域：
本发明涉及一种基于两级DSP的并联装备开放式运动控制卡及控制方法，属于开放式数控系统技术。
背景技术：
并联机构是一组有两个或两个以上的分支机构并联而成的机构。它的特点是所有分支机构可同时接受驱动器输入，而最终共同给出输出，在机构学上属于多路闭环机械系统。以并联机构作为传动进给机构的数控机床、机器人操作机以及由此组成的制造单元统称为并联构型装备，是近年来发展起来的一种有着广阔应用前景的新型制造装备。
并联构型装备的最大特点在于其结构简单而控制复杂，由于在笛卡尔空间中的运动是关节空间各伺服轴运动的非线性映射(又称虚实映射)，即使是笛卡尔坐标单轴上的匀速直线运动也要由关节空间内几个伺服轴同时非线性运动共同合成，因此在进行运动控制时，必须先将事先给定的机构末端位姿以及速度信息通过运动学逆解运算转换为关节空间各伺服轴的控制指令，才能驱动并联机构实现期望运动。
目前的商用控制卡都是基于笛卡尔空间坐标系设计的，其在操作空间内的运动控制量与伺服电机的控制量之间是简单直接的线性对应关系，不能直接应用于并联机构的控制，因此建造并联机构控制系统普遍采用在“PC机十传统运动控制卡”的结构基础上进行二次开发的方法以工控机作为基本平台，负责运动学模型运算、轨迹规划以及人机交互、系统管理等实时性要求不高但计算量大的任务，运动控制卡仅完成对伺服单元的实时控制。这类系统虽然结构比较简单，容易实现，但实践上难以满足对并联机构的运动轨迹高速、高精度控制。主要原因如下1.并联构型装备采用的插补策略与传统串联机构有重大区别。以“PC+传统运动控制卡”为硬件平台的并联机构控制系统采用的是二次插补策略，即粗插补和细插补分别由PC机和控制卡分开进行。由于运动控制卡本身对插补周期的限制，为了保证并联机构的控制精度，就必须限制其进给速度，因而难以实现并联装备高速高精度的要求；2.由于对并联机构的运动控制必须由PC机与控制卡共同协调完成，在两个CPU之间频繁进行大量数据交换极易形成通讯瓶颈，进而影响整个系统的高速运行；
3.并联构型装备具有非线性、强耦合、时变的动力学特性，原则上应针对不同构型特点采用参数可变的智能控制算法以提高其动态性能。然而由于目前的运动控制卡大多只提供系统接口的开放，其核心控制策略仍是封闭的，用户无法更改其内部控制算法，导致并联装备的工作性能难以进一步提高。
由此可见，由于在控制原理上存在较大差异，现有传统的运动控制卡并不完全适合于并联机构的控制特点，而且不同构型的并联装备结构和尺度参数不尽相同，用户应能很方便地对运动学模型及控制策略进行修改，这要求控制系统有极高的灵活性和开放性，因此，针对并联机构的特点研制具有开放式体系结构的新型运动控制卡，对推动并联构型装备的实用化、产业化进程具有十分重要的意义。

发明内容
本发明的目的在于提供一种基于两级DSP的并联装备开放式运动控制卡及控制方法。该系统具有更强的实时性和更快的处理速度；实现运动轨迹的直接插补，满足并联构型装备高速高精度的控制需要。
为达到上述目的，本发明是通过下述技术方案加以实现的，一种基于两级DSP的并联装备开放式运动控制卡，其特征在于控制卡采用多CPU并行处理的两级结构，将控制卡的任务调度、系统监控和伺服控制功能分级处理控制级由一个高端的数字信号处理器(DSP)构成中央处理单元，作为指挥、协调整个系统工作的核心；执行级由两片以上DSP构成并行处理的电机控制单元，每个执行级的DSP分别负责1～2个伺服驱动单元的直接控制，伺服单元由交流伺服驱动器和伺服电机组成；中央处理单元和执行级的电机控制单元通过现场可编程门阵列(FPGA)芯片构造的通讯接口逻辑电路相连接，在先进先出寄存器(FIFO)和同步逻辑控制信号的控制下实现各单元间的高速数据传输和协调同步。
采用上述控制卡实现控制方法其过程为1.首先根据插补周期，由中央处理CPU将操作空间中的轨迹段在时间轴上进行细化，并根据系统加速度要求，插入若干速度过渡段，一次性完成插补工作；2.中央处理CPU完成插补运算后，将得到的笛卡尔空间离散点的包括位置、速度、加速度的大量数据以运动控制指令序列的形式写入FIFO，通过FPGA的同步控制逻辑保证不同的电机控制单元能够同步读取控制指令。
3.控制级CPU调用运动学逆解模型解算出关节空间轴电机的位置、速度控制量，与电机实际位置反馈信号进行比较得到位置误差值，通过系统设定的控制算法处理器和数/模转换器(DAC)转换为±10V的模拟电压速度控制信号，输出到交流伺服驱动器，实现对电机运动状态的位置、速度双闭环控制。
本发明的优越性在于针对并联机构的运动控制特点，采用了多CPU并行处理的两级控制结构，该结构的优势在于通过将任务合理分配，将总体决策管理与具体执行任务分离、非实时性与实时性任务分离，可以最大限度地发挥各子系统的运行效率，提高整个系统的计算速度，而且PC机不再参与实时控制，减轻了与PC间的数据通讯压力；由于计算性能提高，在轨迹插补时实现了粗精插补合一的的直接插补方法，使系统插补周期大大缩短，从而有效提高并联机构的进给速度和定位精度；采用了完全开放式的控制结构，支持运动控制策略的用户定制，允许用户根据不同机构的特点和控制需要应用各种复杂的智能控制算法，能使系统获得更好的动态性能，从而满足并联构型装备用于高速高精度控制的需要。


图1为本发明的运动控制卡整体结构原理框2本发明的运动控制卡DSP间通讯控制接口原理框3为本发明的轨迹控制方法原理框4为本发明的控制卡系统软件结构框5为本发明的控制卡控制的三自由度并联机构的结构示意中1为伺服电机，2为连架从动臂，3为主动臂，4为支架，5为从动臂，6为动平台，7为手爪，8为进给机构。
具体实施例方式
下面结合附图对本发明加以说明。
由图1所示为本控制卡的原理框图，控制卡采用了多CPU并行处理的两级控制结构，控制级选用TI公司的DSP芯片TMS320VC5402作为中央处理单元。TMS320C5402能够达到100MIPS的指令处理速度，拥有较多的外围电路接口和较好的数据处理能力。中央处理CPU实现系统的整体控制，包括任务调度、控制决策、故障处理、误差补偿，以及操作空间运动轨迹的计算，如运动曲线的轨迹插补和速度控制。由于中央处理CPU不参与电机的具体控制，只需完成运算并将结果传送到电机控制CPU，不需要进行外围接口控制，故控制卡中采用了TMS320VC5402的最小系统电路作为基本电路结构。
执行级选用两片以上的DSP芯片TMS320LF2407A作为电机控制单元的核心处理器，其基本电路与当前通用运动控制卡的基本电路结构相同。为了保证对多轴电机处理的快速性和精确性，每个电机控制CPU只负责控制1～2轴电机，保证每个伺服轴具有30μS以下的刷新时间和可控的、复杂的电机加减速控制能力。外围电路主要包括64K*16bit的外部程序存储器SRAM(61LV6416)、64K*16Bit的外部数据存储器SRAM(61LV6416)和仿真口(JTAG)等。
控制卡采用了多CPU并行处理的两级控制结构，为解决各CPU之间的数据通讯与协调同步问题，中央处理单元与电机控制单元通过FPGA构造的通讯接口逻辑电路相连接。利用FPGA的资源，构造多路结构相同的双向FIFO，每路与一个电机控制CPU对应。中央处理CPU完成插补运算后，将得到的笛卡尔空间离散点的位置、速度、加速度等大量数据以运动控制指令序列的形式写入FIFO，由FPGA的控制逻辑保证不同的电机控制单元能够同步读取控制指令。电机控制CPU读取FIFO中的控制指令后，调用运动学逆解模型进行反演，生成关节空间轴电机的位置、速度控制量，与实际位置反馈信号进行比较得到位置误差值，通过系统设定的控制算法处理器和数/模转换器(DAC)将信号转换为±10V的模拟电压速度控制信号，输出到交流伺服驱动器实现对电机运动状态的位置、速度双闭环控制。
控制卡采集增量式脉冲编码器作为位置反馈信号，脉冲编码盘输出+A、-A、+B、-B、+Z、-Z三组差分形式的方波脉冲，脉冲编码器产生的信号经控制卡的DS26LS32芯片差分整形后得到整形后的A、A-、B、B-、Z信号。通过A相与B相的相位关系可判别电机的旋转方向，利用系统中FPGA的部分资源实现对反馈信号的倍频/鉴相/计数的功能，构成位置信号反馈回路。控制卡的数/模转换接口采用D/A转换芯片DAC7724提供四通道12位并行输入、电压型输出；模/数转换接口选用四通道12位并行输出、电压差分输入的A/D转换芯片ADS7864，可用于电机转矩和转速等其它模拟信号的反馈输入。
与PC机组成系统时，PC只需实现用户操作界面和基本轨迹规划等实时性要求不高的任务，这样就大大减少了工控机和运动控制卡之间的通讯数据量和对实时性能力的要求，从而有效避免了PC与控制卡间形成数据通讯瓶颈的问题。考虑到PCI接口具有较高的数据传输速度和兼容性，使用PCI总线可充分利用工控机的控制功能和系统资源，本控制卡的PC通讯接口选用了PCI接口，采用当前比较成熟的PCI接口芯片PXI9052设计控制卡的通讯接口，并通过双端口RAM(DPRAM)实现PC机和控制卡之间的数据通讯。DPRAM选用Integrated DeviceTechnology，Inc.公司的高速8K×16DUAL-PORT STATIC RAM IDT7025。
由于并联机构的末端执行器在笛卡尔空间的运动是关节空间各伺服轴运动的非线性映射，因此折算到关节空间的等效负载是耦合的，且是位形的函数，因此在运动过程中应动态调节各控制器的PID参数。针对并联机构这种非线性、强耦合、时变的动力学特征，系统采用变参数的PID控制算法。为了使系统具有良好的开放性和可扩展性，为用户保留了在线编程接口，允许用户对系统的逻辑功能或程序进行动态修改。用户可根据并联机构的结构特点和不同应用场合下的控制要求自行定制运动学模型以及控制算法，包括各种复杂的智能控制算法。用户程序首先须经过汇编、连接生成DSP能够识别的机器代码，再通过为用户保留的在线编程接口下载到闪速存储器(FLASH)中，在系统复位时由DSP片内掩膜ROM中固化的引导装载(Boot loader)程序，采用并行加载方法将用户算法程序从外部引导到高速RAM中，保证其高速运行。选择适当的控制算法能够进一步改善整个系统的动态特性。
为实现控制卡系统内核完全开放，在硬件设计中采用了可重构器件FPGA构造逻辑接口和位置反馈电路。FPGA的主要优点就在于具有非常强的灵活性，其内部的具体逻辑功能可以根据需要动态配置，对电路结构的修改和维护很方便。系统采用这种构造方式实现硬件功能软件化的过程，使运动控制卡内部结构开放，极大增强了系统的开放性。
图5示出了3自由度并联机构的三维实体造型。该机构包括机架、动平台和两个结构相同的支链，各支链含两个平行四边形。安装在机架上的伺服电机分别带动主动臂转动，进而可实现动平台的二自由度平动。在与机构运动平面垂直的方向串接一进给机构，以伺服电机+滚珠丝杠传动，构成3自由度并联机构。
采用本发明的控制卡，实现对上述机构的控制过程如下(如图3所示)(1)由中央处理CPU离散控制轨迹根据设定的速度和插补周期，将运动轨迹在“时间轴”上离散成为操作空间中的点序，并根据设定的加速度确定需要插入的过渡段。将计算结果以运动指令序列的形式(离散点位置、速度、加速度)通过FIFO发送到相应的执行级CPU；(2)由执行级CPU进行位置与速度反演执行级CPU分别调用位置、姿态和速度加速度逆解模型，对插补后的离散点序列进行反演，得到关节空间中的相应离散点序，即各伺服轴电机的位置、速度控制量；(3)由执行级CPU进行变参数位置PID控制伺服电机的给定位置与电机实际位置反馈信号进行比较得到位置误差值，通过变参数位置PID控制器和数/模转换器(DAC)转换为±10V的模拟电压速度控制信号，输出到交流伺服驱动器，实现对电机运动状态的位置、速度双闭环控制。各电机控制单元间通过FPGA的同步控制实现协调同步，驱动伺服电机使机构末端走出希望轨迹。
权利要求
1.一种基于两级DSP的并联装备开放式运动控制卡，其特征在于，该控制卡采用多CPU并行处理的两级结构，将控制卡的任务调度、系统监控和伺服控制功能分级处理控制级由一个高端的数字信号处理器构成中央处理单元，作为指挥、协调整个系统工作的核心；执行级由两片以上DSP构成并行处理的电机控制单元，每个执行级的DSP分别负责1～2个伺服驱动单元的直接控制，伺服单元由交流伺服驱动器和伺服电机组成；中央处理单元和执行级的电机控制单元通过现场可编程门阵列芯片构造的通讯接口逻辑电路相连接，在先进先出寄存器和同步逻辑控制信号的控制下实现各单元间的高速数据传输和协调同步。
2.一种采用按权利要求1所述的运动控制卡基于两级DSP的控制并联装备的方法，其特征在于包括以下过程1)首先根据插补周期，由中央处理CPU将操作空间中的轨迹段在时间轴上进行细化，并根据系统加速度要求，插入若干速度过渡段，一次性完成插补工作；2)中央处理CPU完成插补运算后，将得到的笛卡尔空间离散点的包括位置、速度、加速度的大量数据以运动控制指令序列的形式写入先进先出寄存器，通过现场可编程门阵列的同步控制逻辑保证不同的电机控制单元能够同步读取控制指令；3)控制级CPU调用运动学逆解模型解算出关节空间轴电机的位置、速度控制量，与电机实际位置反馈信号进行比较得到位置误差值，通过系统设定的控制算法处理器和数/模转换器转换为±10伏的模拟电压速度控制信号，输出到交流伺服驱动器，实现对电机运动状态的位置、速度双闭环控制。
全文摘要
本发明公开了一种基于两级DSP的并联装备开放式运动控制卡及控制方法。该系统采用了多CPU并行处理的两级控制结构，将系统的总体控制和电机的具体控制功能分离，由控制级CPU作为控制卡的核心，控制和协调整个系统的操作；采用可编程逻辑器件FPGA构造通讯接口，将控制级的中央处理单元和执行级的电机控制单元与FPGA芯片相连接，通过FPGA控制FIFO和同步状态信号实现数据的高速传输和电机同步控制；控制方法包括中央处理CPU离散控制轨迹，由执行级CPU进行位置与速度反演，然后通过变参数位置PID控制伺服电机。本发明的控制卡具有开放性好、处理速度快、控制能力强、实时性高等特点，适合控制具有非线性、强耦合、时变特点的并联构型装备。
文档编号G05B15/02GK1808319SQ20051012232
公开日2006年7月26日 申请日期2005年12月13日 优先权日2005年12月13日
发明者倪雁冰, 王辉, 李可 申请人:天津大学