一种高精度运动控制器IP核设计与实现闫永志 王宏 杨志家刘鹏(中国科学院沈阳自动化研究所，辽宁 沈阳 110016)(中国科学院研究生院，北京 100039)摘 要：本文提出了一种运动控制器软IP的设计方案，该控制器可以控制4个轴的步进电机或数字伺服电机，可以进行各轴独立的定位控制、速度控制，也可任选2轴或3轴来进行直线、圆弧和位模式插补。

文中介绍了其系统结构、基本功能和插补算法。

设计最终形成软IP核，并在Xilinx公司的Vertex2系列FPGA 中予以实现和验证。

关键词：运动控制 插补 IP ASIC FPGA中图法分类号: TN4文献标识码：ADesign and Implementation of High Precision Motion Controller IPYongzhi Yan1,2 Hong Wang1 Zhijia Yang1Peng Liu11( Shenyang Institute of Automation , Chinese Academy of Sciences, Liaoning Shenyang, 110016) 2( Graduate School of the Chinese Academy of Sciences, Beijing, 100039)Abstract: This paper designs a motion controller soft IP, it can control 4 axes of either stepper motor or pulse type servo drivers for position, speed, and interpolation. Any 2 or 3 axes can be selected to perform linear, circular, and bit pattern interpolation. We describe structure, function and interpolation arithmetic of the motion controller. Finally, the montion controller soft IP is implemented and verified in Xilinx Vertex2 FPGA.Key words:motion control interpolation IP ASIC FPGA1引言随着计算机、控制理论、微电子等技术的迅速发展，运动控制技术取得了巨大的进步，已成为推动新的产业革命的关键技术。

简单地说，运动控制就是对机械运动部件的位置、速度等进行实时的控制管理，使其按照预期的运动轨迹和规定的运动参数进行运动[1]。

早期的运动控制技术主要是伴随着数控技术、机器人技术和工厂自动化技术发展而来的。

近年来，随着运动控制技术的不断进步和完善，运动控制器作为一个独立的工业自动化控制类产品，已经应用在越来越多的产业领域中。

目前基于PC(Personal Computer)总线的以DSP(Digital Signal Processing)或专用运动控制ASIC(Application Specific Integrated Circuit)作为核心的开放式运动控制技术已经成为主流。

将PC 机的信息处理能力和开放式的特点与运动控制器的运动轨迹控制能力有机地结合在一起，具有信息处理能力强、开放程度高、运动轨迹控制准确、通用性好的特点[2]。

本设计使用verilog硬件描述语言设计了一种带有通用的PC机总线接口的运动控制器软IP(Intellectual Property)核，并通过FPGA(Field Programmable Gate Array)予以实现和验证。

2系统结构和基本功能本设计是一个用于实现4轴运动控制的控制器，通过这个控制器可以控制由步进电机或由数字脉冲型伺服电机驱动的4个轴的位置、速度和插补。

该系统由5个部分组成，分别是(1)命令/数据处理模块(2)插补控制模块(3)4个轴的运动控制模块(4)中断信号发生模块(5)脉冲分配模块。

图1为该运动控制器的系统结构图。

该运动控制器的主要功能如下：(1)独立的四轴驱动：可以分别控制四个电机驱动轴的运动，四个轴的功能完全相同。

(2)驱动速度控制：驱动脉冲的输出速度可以从1PPS(Pulses Per Second)到4MPPS，每个驱动轴可以进行定速驱动、直线加/减速驱动、S曲线加/减速驱动。

(3)2轴/3轴直线插补驱动：可以选择4个轴中的任何2个或3个轴进行直线插补驱动。

(4)圆弧插补驱动：可以选择4个轴中的任何2个轴进行圆弧插补驱动。

(5)位模式插补驱动：可以选择4个轴中的任何2个或3个轴进行位模式插补驱动。

这种插补的数据由上位PC机进行计算，并将结果写入运动控制器，使其在预置的驱动速度下连续输出插补脉冲，这样可以产生任何形状的插补曲线。

(6)16位上位机总线：通过此接口与上位PC机进行数据交换。

Figure 1 Structure of the motion controller图1 系统结构3插补模块的设计该控制器是一个4轴运动控制器，它可以实现任意2轴或3轴的直线插补、任意2轴的圆弧插补和任意2轴或3轴的位模式插补。

插补模块的核心部分是直线和圆弧的插补算法的设计，本设计中采用的是一种改进的最小偏差算法，该算法在已有的最小偏差理论[3][4][5][6]上加以改进使偏差公式计算简单、插补精度更高。

3.1直线插补算法平面上第一象限内的任意直线，已知其起点和终点坐标，直线方程为y = kx的标准形式。

当k < 1时，对于直线上的点，其横坐标大于纵坐标。

按朝着偏差减小方向运动的原则，动点的进给只有两种情况：一种是沿x方向进给一步，另一种是沿x，y方向同时进给一步。

当k > 1时，对于直线上的点，其纵坐标大于横坐标。

因此，动点的进给也有两种情况：一种是沿y方向进给一步；另一种是沿x，y方向同时进给一步。

事实上，对于直线斜率k > 1的情况，可以通过将直线方程的x与y的位置互换，变换成x = 1/k×y的形式，使得直线斜率1/k < 1，所以只分析第一象限直线k < 1的情况。

如图2所示，直线OA在第一象限内，其方程y = kx，斜率k < 1。

设OA直线上与加工动点P i (x i , y i)相对应的点为P’i (x i , y’i)，则加工动点P i (x i , y i)与理想直线OA上对应点P’i (x i , y’i)的纵向偏差为△y = y i – y’i = y i – kx i。

Figure 2 Principle of linear interpolation arithmetic图2 直线插补算法原理令偏差判别函数F i =y i – kx i，则：(1)当F i = y i – kx i> 0时，动点P在直线OA的上方，则向x方向进给一步，有：x i+1 = x i + 1，y i+1 = y i，新偏差F i+1 = y i+1 – kx i+1 = y i – k (x i + 1) = F i – k；(2)当F i = y i – kx i≤ 0时，动点P在直线OA的下方，则向x，y方向同时进给一步，有：x i+1 = x i + 1，y i+1 = y i + 1，新偏差F i+1 = y i+1 – kx i+1 = y i + 1 – k (x i + 1) = F i + 1 – k；上面是第一象限直线k < 1时的插补算法。

对于k >1的直线，只需将上述算法中的x，y坐标互换即可。

至于其它象限的直线，通过适当的坐标变换同样可以实现插补运算。

在进行插补运算之前，首先要经过斜率判断，也就是对直线的终点坐标x e，y e的大小进行比较，以判断直线斜率是否小于1。

对于不小于1的情况，则要将直线方程进行坐标变换，然后才开始进行插补运算。

3.2圆弧插补算法考虑典型的第一象限逆圆弧(对于其它象限的情况可以通过相应得坐标变化得到)，如图3所示，圆心在原点，半径为R，起点为(x0, y0)，终点为(x e, y e)。

设第i步插补点为P i (x i, y i)，此时加工偏差公式为：F i = x i2 + y i2 – R2则第i + 1步可能的插补点有A (x i – 1, y i + 1)，B i(x i – 1, y i)，C (x i , y i + 1)。

它们与理想圆弧间的偏差函数分别为：F(A) = (x i – 1)2 + (y i + 1)2 – R2 = F i – 2x i + 2y i + 2F(B) = (x i – 1)2 + y i2 – R2 = F i – 2x i + 1F(C) = x i + (y i + 1)2 – R2=F i + 2y i + 1为选择三点中与理想圆弧偏差最小的点，取判别函数为：F i’ = F(A) = F i – 2x i + 2y i + 2当F i’ < 0时，点A在圆内，新插补点应为C点，即向y轴正方向进给一步。

新偏差及坐标为：F i+1 = F(C) = F i + 2y i + 1，x i+1 = x i ，y i+1 = y i + 1当F i’≥0时，点A在圆上或圆外，下一插补点应在A和B中选择。

为此，进—步取二次判别函数：F’’ = F(B) = F i – 2x i + 1若F i’’≥ 0，说明点B在圆上或圆外，则选B，为第i+1步插补点，即向x轴负方向进给一步。

新的偏差及坐标为：F i+1 = F i’’ ，x i+1 = x i – 1，y i+1 = y i若F i’’≤ 0，说明点B在圆内，则应选A，为第i + 1步插补点，即向x轴负方向和y轴正方向各进给一步：F i+1 = F i’ ，x i+1 = xi – 1，y i+1 = y i + 1对圆弧起点(x0, y0)，应有x02 + y02 = R2，从而偏差初始值为F0 = 0。

Figure 3 Principle of circular interpolation arithmetic图3 圆弧插补算法原理3.3位模式插补模块位模式插补驱动是把上位PC计算的插补数据以数据包的方式并行接收后以指定的驱动速度串行输出插补脉冲[7][8]。

图4表示位模式插补的第1轴正方向寄存器构成，BP1P寄存器是从上位PC接收位模式数据的16位寄存器，用来接收负方向位模式数据的是BP1M寄存器，其寄存器结构与图4相同。