1、基于 DSP 用插补法实现的运动轨迹跟踪控制（洛阳理工学院 电气工程与自动化系 河南洛阳 471023）姬宣德，蒋建虎摘要：本文以插补法为控制算法，利用 DSP 控制器实现了圆轨迹跟踪的运动控制。实验结果表明在误差范围内实现了对圆轨迹曲线的跟踪控制，并对其误差产生原因进行详细的分析。关键词：步进电机 DSP 插补法中图分类号：TP183；TM342 文献标识码：A0 绪论运动轨迹控制广泛应用于航天、军工、机器人控制、数控机床及其计算机辅助设计等领域的控制，随着电子技术、计算机技术、自动控制和精密测量技术的不断发展和应用，运动轨迹控制在各领域特别在制造业，正向高速度、高精度、多功能、智能化、开放
2、型以及高可靠性等方面迅速发展。悬挂运动轨迹控制系统是一个电机控制系统，控制物体在倾斜（仰角100 度）的板上运动在白色底板上，固定两滑轮，两个步进电机（固定在板上）通过穿过滑轮的吊绳控制物体（滑块）在板上运动，运动范围为 80cm100cm。物体（滑块）上固定有浅色画笔，以便运动时能在板上画出圆的运动轨迹；悬挂运动轨迹控制系统本质上是一个闭环控制的无位置传感器随动系统，必须实时计算在板上运动的物体（滑块）的当前位置（坐标）及与圆轨迹圆心的长度（圆的半径），而位置计算及半径长度模型算法比较复杂，因此需要运算速度快、处理能力强的控制器。TMS320LF2407ADSP 控制器是一款 16 位的微处
3、理器，作为电机控制专用芯片，极快的运算速度，25ns 的单指令周期为实时控制系统提供了良好的硬件基础。基于 C 语言的软件编程为实现复杂的位置计算及半径长度算法则提供了良好的软件基础。1 系统工作原理及控制思想1.1 系统工作原理DSP 控制器首先利用位置算法模型计算出物体（滑块）在 坐标系下的当前位置XY，然后计算出与圆形圆心 坐标的长度，判断出物体（滑块）在圆形轨迹),(fyx ),(0yx曲线位置（圆内或者圆外），并且计算出物体（滑块）坐标 在以圆形轨迹圆心为),(fyx坐标原点的 坐标系下的区间，根据以上信息生成控制信号经过 I/O 输出，由于2407ADSP 控制器属于低功耗控制器，
4、需要把 3.3V 的电平信号利用（SN74C04N）3.3/5.0V电压转换芯片转换成 5.0V 的电平信号，通过（ULN2803）功率驱动芯片驱动两个步进电机。这就是系统的工作原理。图 1 是悬挂运动控制系统的工作原理结构图。fR0当前位置是否在圆内3 . 3 V / 5 V电压转换功率放大驱动M 1M 2圆形半径及当前位置算法模型O / I当前位置所在坐标区间D S P 控制器图 1 控制系统工作原理结构图1.2 系统控制思想图 2 是系统的工作原理示意图。本系统利用插补算法来实现圆轨迹的跟踪，从图 2 可以看出，物体（滑块）位置 要么处在圆形轨迹曲线内，要么处在圆形轨迹曲线外，),(fy
5、x再根据（滑块）位置 在以圆形轨迹圆心为坐标原点的 坐标系下所处的区间，就),(fyx xy可以指令步进电机进行运动从而使移动物体（滑块）沿着圆形轨迹运动，实现圆轨迹的跟踪控制。例如，如图 2 所示，物体（滑块）位置 与圆形圆心 之间的长度),(fx),(0为 ， ，物体（滑块）在圆形轨迹之内；通过区间判断公式知道在 坐标系的fR0f xy2 区间，又由于是顺时针圆轨迹跟踪，这时应该使步进电机 1 收线使 缩短。这样，物体1L（滑块）在重力的作用下，以 B 点为圆心，以 为半径，做圆弧运动；一旦，物体（滑块）2L位置 处在圆形轨迹曲线外，则使步进电机 2 收线使 缩短。这样根据在不同的坐),(
6、fyx 2标区间及半径长度来确定步进电机的移动方式，从而实现龟形轨迹控制。为了避免控制位置不准确，两个步进电机在同一时刻不同时运行。的计算： ；fx32211cosLLxf 的计算：fyin14yf的计算：fR2020)()(fff xy的计算：fR这里， 都是已知量，而 可以通过公式 求出来。4321L、 1322arcosL2 硬件设计整个控制系统由 DSP 控制器、数码显示、键盘给定、SN74C04N（电压转换芯片） 、ULN2803（功率驱动芯片） 、步进电机组成。数码显示：用于显示物体（滑块）的当前位置；键盘：用于给定位置；SN74C04N：电压转换芯片，用于把 DSP 控制器输出的
7、 3.3v 电压转换成 5.0V 电压；电压转换芯片的引脚 为输入信号， 为输出4321A、 4321Y、信号。与 DSP 芯片的输出引脚 相连， 分别与功4321A、 3210P、 4321Y、率驱动芯片的 1、2、3、4 引脚相连作为输出信号。ULN2803：功率驱动芯片，用于放大DSP 控制器输出信号以驱动步进电机；功率驱动芯片是将输入 5V 电压转换为 12V 电压输出，以驱动步进电机动作。步进电机：作为执行机构，拖动物体（滑块）运动。3 软件设计软件设计由主程序和中断服务程序组成。主程序完成系统初始化设置、输出显示、检测给定输入、等待中断；初始化流程设置通用定时器控制寄存器T1/2C
8、ON并启动定时器；中断服务子程序任务就是首先计算滑块在 坐标系下的当前位置 ，接着计算出XY),(fyx的长度，然后算出 ，再利用区间判断公式判断出物体（滑块）位置 在 坐fRR ,fxy标系下的区间，根据 的正负和所在区间来决定滑块怎样运动，同时计算当前的 ，21L、以被下一个控制周期使用。等待下一次中断。中断服务子程序流程图如图4所示。 1L2L34 YXOfyfxyx),(f ),(0yx134BAC步进电机 1步进电机 2o0Rf图 2 系统的工作原理示意图D S PS N 7 4 C 04 N电压转换芯片1S N 7 4 C 04 N电压转换芯片2P 0P 1P 2P 3P 4P 5
9、P 6P 7数码显示键盘给定A 3A 1A 2A 4A 3A 1A 2A 4U L N 2 8 0 3功率驱动芯片1U L N 2 8 0 3功率驱动芯片2Y 3Y 1Y 2Y 4M 1M 2CABDCABDY 3Y 1Y 2Y 412341234图 3 控制系统硬件组成原理图4 实验结果分析及结论图 5 是利用此系统跟踪圆轨迹的结果实验图。从图中可以看出，在可以误差范围内，实现了对圆轨迹曲线的跟踪。存在误差的原因分析如下：1：物体（滑块）在下降过程中，受重力及棉线弹性的影响，使得实际下降长度大于理想计算长度；2：物体（滑块）在上升过程中，受重力及棉线弹性的影响，使得实际上升长度小于理想计算长
10、度；3：步进电机在缠绕过程中，缠绕半径的大小会随放线的长度而改变；4：由于本系统是一个闭环控制的无位置传感器随动系统，使得无法校正悬挂物体实际位置与理想位置之间的偏差。现场保护计算滑块在 X Y 坐标下位置 （ x f ， y f ）S e c t o r = 1 ?M 1 - d r o pL 1 = L 1 + s t e pM 2 - r i s eL 2 = L 2 - s t e pM 1 - r i s eL 1 = L 1 - s t e pM 2 - d r o pL 2 = L 2 + s t e pS e c t o r = 2 ?S e c t o r = 3 ?YNNY
11、NY判断滑块在 x y 坐标系下的区间 s e c t o r2020)()(fffRfR0判断滑块在 x y 坐标系下的区间 s e c t o rS e c t o r = 1 ?M 2 - r i s eL 2 = L 2 - s t e pM 1 - r i s eL 1 = L 1 - s t e pM 2 - d r o pL 2 = L 2 + s t e pM 1 - d r o pL 1 = L 1 + s t e pS e c t o r = 2 ?S e c t o r = 3 ?YNNYNY现场返回NY图 4 中断服务子程序流程图图 5 控制系统实验结果图参考文献：1
12、陈伯时. 电力拖动自动控制系统. 北京：机械工业出版社，2004；2 韩安太. DSP控制器原理及其在运动控制系统中的应用. 北京：清华大学出版社，2003；3 刘和平. TMS320LF240XDSPC语言开发与应用. 北京：航空航天大学出版社，2003；4 张 勇. 电机拖动及控制. 北京：机械工业出版社，2003；5 程 明. 微特电机及系统. 北京：中国电力出版社，2004；The realization of trajectory tracking control based on DSP Proccessor using the interpolation methodJi Xua
13、nde，Jiang Jianhu(Department of Electrical Engineering and Automation Luoyang Institute of Science and Technology HeNan Luoyang 471023)Abstract: This article has realized the motion orbit control of the roundpath tracks, Using extrapolation as a control arithmetic and using DSP as controller. The laboratory result indicated that the system is effectual. And the reason of error production is analyzed.Keywords: STEP MOTOR DSP INTERPOLATION METHOD姓名:姬宣德出生年:1980.10性别：男工作单位：洛阳理工学院 西校区 气工程与自动化系联系地址：洛阳理工学院 西校区 气工程与自动化系邮编：471023职务职称：讲师主要研究方向：电力电子与电力传动联系电话：15937920964电子信箱等：jxd_
