当前位置:首页 >> 信息与通信 >>

新一代DSP 多轴运动控制器的运动规划及控制算法研究


学校编号: 10384 学 号:200029012

分类号: U D C:

密级:

硕 士 学 位 论 文

新一代 DSP 多轴运动控制器的运动规划 及控制算法研究
刘广超

指导老师姓名:周学才 单 位 名 称:厦门大学 士

教授 机电工程



申请学位级别:硕 专 业

名 称:测试计量技术及仪器

论文提交日期:2003 年 5 月 论文答辩日期:2003 年 5 月 学位授予单位:厦 门 大 学 学位授予日期:2003 年 月

答辩委员会主席: 评 阅 人: 二 00 三年四月

厦门大学硕士论文





新一代 DSP 多轴运动控制器的运动规划及控制算法研究
研究生:刘广超 指导老师:周学才教授





传统运动控制系统一般采用多片单片机并行处理的封闭式结构, 其控制软件的兼容 性、系统整体的容错性和可靠性差,且不具备重构(Reconfiguration)和网络功能。这 种结构已无法适应制造业市场的变化与竞争,也不能满足现代制造业向信息化和敏捷制 造模式发展的需要。针对这些弊端,研究和开发具有开放式结构的高性能运动控制器已 成为目前运动控制领域里的一个新的热点。和封闭式结构相比,开放式结构运动控制器 的重要特点之一是它采用了基于 PC(PC-based)的双 CPU(中央处理器)结构。此时, PC 机承担非实时任务的处理;实时任务(Real Time Tasks)则可采用具有高速浮点运算 功能的 DSP(Digital Signal Processor)来处理。这个特点支持客户根据控制任务的具体 要求来重新创建各种高级控制算法、 操作界面等, 满足特别要求的应用程序则可在线 (On line)下载到下位机(DSP)上运行,从而使传统运动控制技术在速度和精度以及可靠 性、兼容性方面的技术瓶颈迎刃而解。开放式结构运动控制器在日本被认为将带来第三 次工业革命;在美国则被誉为新一代(Next Generation)运动控制器。 运动规划(Motion Plan)特别是多轴运动规划是新一代 DSP 多轴运动控制技术中 的关键技术, 它在一定程度上决定了运动控制系统的性能。 本文在前人研究的基础之上, 对新一代 DSP 多轴运动控制器的运动规划及控制算法进行了系统、深入的研究,并根据 客户实际运动控制的需求,建立了平面和空间各种曲线类型高速高精度的轨迹规划算法 以及控制算法。这些算法均在国产 MCT8000 系列运动控制器上进行了验证,结果是有 效和正确的。本文主要工作如下: 一.为避免速度或加速度突变而造成的对机械传动部分的冲击,采用了双曲线和抛 物线的速度分配模式来规划运动的加减速过程,从而获得了更理想的加减速效果。 二.对平面和空间的圆弧、椭圆弧运动,先按一维直线的速度、加速度分配方式进 行插补计算,然后利用给定的几何信息和空间的旋转矩阵,将得到的运动参数分配到各 轴做联合运动,实现平稳快速高精度的弧线运动。其中由于椭圆弧长求解涉及椭圆积分 的问题,所以本文另辟蹊径,用一种压缩圆的方法使椭圆弧的规划算法与圆弧的规划算 法在运算量上保持在同一个数量级内,从而满足了实时插补的需要。 三.探讨了三次样条曲线运动规划的插补模型和算法,不但丰富了该轨迹规划器的 运动类型,避免了用直线和圆弧逼近复杂曲线时过渡点的切线不连续现象,还可以更圆 滑地实现工件轮廓曲线的加工。 四. 将速度、 加速度前馈引入到经典的 PID 算法模型中, 减小了位置跟踪滞后误差,
-I-

厦门大学硕士论文





进一步提高了系统的动态响应性能。 五. 完成了所提出的算法在企业等离子切割机运动控制系统以及实验室运动控制平 台上的实际运行实验,取得了良好的控制效果。 本论文内容的安排如下: 第一章是前言部分,主要介绍了当前运动控制系统的发展现况和基本概念; 第二章探讨了运动控制领域里的两个基本问题——运动规划和运动控制的一些概 念性问题; 第三章建立了一维乃至 n 维的直线运动,二维和三维的圆弧、椭圆弧运动规划的数 学模型,以及各种不同约束条件下的三次样条曲线运动的数学模型; 第四章讨论了以上各种运动轨迹规划数学模型的具体算法实现, 并建立了一系列具 体的运动轨迹规划函数,构成了新型轨迹规划器的软体基础; 第五章研究了经典的 PID 控制算法, 并在此基础上探讨了速度和加速度前馈对于系 统性能的影响; 第六章利用计算机和数学工具软件对实现的轨迹规划算法进行仿真模拟以验证其 有效性,并列举了其在切割机行业中的一个典型的应用; 第七章以该新型轨迹规划器为基础搭建实验平台, 并在此平台上验证该轨迹规划器 的优越性能; 第八章对本文工作进行归纳, 并提出本课题所关注领域的后续研究热点和亟待解决 的问题。

关键词:运动控制

轨迹规划

运动插补

DSP 控制技术

切割机控制系统

- II -

厦门大学硕士论文





Path Planning & Motion Control Algorithms for the Next Generation Multi-Axes Motion Controller with DSP
Applicant: Supervisor: Guangchao Liu Prof. Xuecai Zhou

Abstract
Traditional motion control systems are of closed architecture by adopting multi single chip processors worked in parallel manner and lead to the poor compatibility and reliability of the control systems. The open architecture motion controller, on the other hand, is of PC-based and worked on a two CPU bases in which one CPU like PC is used to deal with the non real time tasks and another CPU like DSP is for real time tasks only. The crucial features of the open architecture motion control systems are high speed, high precision and high flexibility. By using these kind of motion control systems, users may write their own specific graphic interface and advanced control algorithms as well. The specific control algorithms can then be downloaded into DSP on the fly through the communication kernel. These advantages tend to be a good answer to the bottleneck of the traditional motion control technology in poor precision and reliability when worked at high speed. Because of this, the DSP based multi-axes motion controllers are thought to bring the third industrial revolution in Japan, and recognized to be the new or next generation motion controllers in the United States.

Motion Plan, especially the multi-axes Motion plan serves as the key technique in the new generation control system as mentioned above. The good performance of the motion control system is partially based on this key technology. This thesis presents a systemic and deep study on both the path-planning and control algorithms for the multi-axes motion controllers. To demonstrate the correctness and validity, several experiments are also presented. The summary of the presented work is listed bellow.

Firstly, dual sinusoidal and parabolic interpolation patterns are considered into the algorithms for the target system to achieve smooth motion without impact.

Secondly, to implement the arc and ellipse motion in the two or three-dimensional space, the one-dimensional interpolation is first computed by taking the arc length along the arc or ellipse trajectory as variable. The interpolation outputs are then transformed to the space with the aid of rotational tensor. Particularly, for the purpose to realize the real time ellipse motion, a new method
- III -

厦门大学硕士论文





called “compression –circle” has been developed.

Thirdly, the spline interpolation method is studied. With this motion style, the profile of a work piece will be smoother, and the impact in manufacturing procedure can be cut down into a very small level.

Fourth, in order to achieve high stability, classic PID arithmetic has been studied. The velocity and acceleration forward controls are taken into account in the algorithms developed.

Finally, several experiments were carried out to demonstrate the correctness and validity of the developed algorithms in the thesis. The arrangement of the thesis is as follows. The thesis begins with the introduction in Chapter 1. In chapter 2, two fundamental issues, including motion plan and motion control, were discussed. Chapter 3 presented the formulations of the interpolation of all the cases including line, arc and ellipse paths in from one to n-dimensional spaces. A series of the path planning functions were developed with ANSI C language in Chapter 4. Chapter 5 discusses the classic PID control

algorithm, and the velocity and acceleration forward controls are also considered to achieve better results. In Chapter 6, simulation was carried out with the aid of computer and mathematic software to validate the developed algorithms. Moreover, a real world application in cutting machine was illustrated. In Chapter 7, a laboratorial system was set up to verify the new system. The

conclusions are made at the end of the thesis.

[Key words] motion control, path plan, interpolation algorithm, DSP, ellipse interpolation, cutting machine

- IV -

厦门大学硕士论文






第一章
§1.1 §1.2 §1.3 §1.4 §1.5



前 言 ……………………………………………1
运动控制系统的发展现况………………………………………1 运动轨迹规划简介………………………………………………2 运动控制策略……………………………………………………3 本论文的主要研究内容…………………………………………3 小结………………………………………………………………3

第二章

运动控制中的基本问题 …………………………5
运动规划…………………………………………………………5 §2.1.1 运动规划的概念…………………………………………5 §2.1.2 运动规划中的精度控制…………………………………5

§2.1

§2.2

运动控制…………………………………………………………6

§2.2.1 运动控制算法……………………………………………6 §2.2.2 运动控制的稳定性………………………………………8 §2.3 小结………………………………………………………………9

第三章

运动轨迹规划的数学模型………………………10
直线轨迹规划 …………………………………………………10 §3.1.1 一维的情况 ……………………………………………10 §3.1.2 多维的情况 ……………………………………………10

§3.1

§3.2 §3.3

圆弧轨迹规划 …………………………………………………12 椭圆轨迹规划 …………………………………………………13

§3.3.1 椭圆所在平面内的二维规划 …………………………13 §3.3.2 空间矢量分析 …………………………………………16 §3.4 三次样条曲线的轨迹规划 ……………………………………17 §3.4.1 三次样条函数的建立及求解 …………………………17 §3.4.2 三次样条曲线插补算法的数学模型 …………………21 §3.5 小结 ……………………………………………………………24

第四章

运动轨迹规划算法实现…………………………25
总体规划 ………………………………………………………25
-V-

§4.1

厦门大学硕士论文





§4.2

直线轨迹规划的算法实现 ……………………………………29

§4.2.1 一维坐标直线运动轨迹插补函数 ……………………29 §4.2.2 n 维坐标直线运动轨迹插补函数 ……………………34 §4.3 圆弧轨迹规划的算法实现 ……………………………………38 §4.3.1 二维圆弧运动轨迹插补函数 …………………………38 §4.3.2 三维圆弧运动轨迹插补函数 …………………………41 §4.4 椭圆轨迹规划的算法实现 ……………………………………44 §4.4.1 二维椭圆弧运动轨迹插补函数 ………………………44 §4.4.2 三维椭圆弧运动轨迹插补函数 ………………………47 §4.5 Spline 线轨迹规划的算法实现 ………………………………48 ……………………………………………………………53 §4.6 小结

第五章 运动控制算法………………………………………54
§5.1 常规 PID 算法 …………………………………………………54 PID 控制原理…………………………………………54 数字 PID 控制算法 …………………………………55 PID 参数的整定………………………………………59 复合 PID 控制器的实现 实践与仿真 ……………………………66 §5.1.1 §5.1.2 §5.1.3 §5.2 §5.2.1 §5.2.2 §5.4 小结

带有速度和加速度前馈的算法 ………………………………65 …………………………………………67

§5.3 其他高级算法简介

……………………………………………67

……………………………………………………………68

第六章 仿真及应用…………………………………………69
§6.1 运动规划的仿真结果 …………………………………………69 §6.1.1 函数 line_move_1(一维) ……………………………69 §6.1.2 函数 line_move_n(n 维) ……………………………70 §6.1.3 函数 arc_move_2D(二维)……………………………73 §6.1.4 函数 arc_move_3D(三维)……………………………75 §6.1.5 函数 ellipse_move_2D(二维) ………………………77 §6.1.6 函数 ellipse_move_3D(三维) ………………………80 §6.1.7 混合运动仿真 §6.1.8 样条曲线函数组 §6.2 …………………………………………82 ………………………………………83

工业应用实例 …………………………………………………85
- VI -

厦门大学硕士论文





§6.2.1 数控切割机控制系统 §6.2.2 其他应用 §6.3 小结

………………………………85

……………………………………………92

………………………………………………………92

第七章 实验 ………………………………………………94
§7.1 实验系统简介…………………………………………………94 §7.1.1 应用方案………………………………………………94 §7.1.2 硬件平台搭建…………………………………………95 §7.1.3 人机界面………………………………………………96 §7.2 §7.3 实验结果与分析………………………………………………98 小结 …………………………………………………………100

第八章 结论与展望 ………………………………………101 致 谢 …………………………………………………103 …………………………………………………104

参考文献

- VII -

厦门大学硕士论文

图 形 列 表

图形列表
图 2-1-1 图 2-2-1 图 2-2-2 图 3-1-1 图 3-1-2 图 3-2-1 图 3-3-1 图 3-3-2 图 3-3-3 图 3-4-1 图 3-4-2 图 4-1-1 图 4-2-1 图 4-2-2 图 4-2-3 图 4-2-4 图 4-3-1 图 4-4-1 图 4-5-1 图 5-1-1 图 5-1-2 图 5-1-3 图 5-1-4 图 5-1-5 图 5-1-6 图 5-1-7 图 5-1-8 图 5-1-9 轮廓误差定义图………………………………………………………………6 单位阶跃响应曲线图…………………………………………………………7 S 平面到 Z 平面的映射图……………………………………………………9 一维直线连续轨迹规划………………………………………………………10 空间矢量的投影图……………………………………………………………11 三维空间圆弧轨迹的规划……………………………………………………12 椭圆规划之切线拟合法………………………………………………………13 椭圆规划之压缩圆法…………………………………………………………15 三维空间椭圆弧轨迹的规划…………………………………………………16 三次样条曲线规划微分图……………………………………………………22 样条曲线插补精度分析图……………………………………………………23 公共插补函数算法流程图……………………………………………………27 一维坐标直线运动算法流程图………………………………………………30 MCT8000 采用的加-速控制规律 ……………………………………………32 加减速控制方式算法流程图…………………………………………………32 n 维坐标直线运动算法流程图 ………………………………………………36 二维圆弧运动算法流程图……………………………………………………40 二维椭圆弧运动算法流程图…………………………………………………46 Spline 曲线运动算法流程图 …………………………………………………48 模拟 PID 控制系统原理图……………………………………………………54 数字 PID 控制器的结构图……………………………………………………56 位置式 PID 控制系统…………………………………………………………56 位置式 PID 算法流程图………………………………………………………57 增量式 PID 控制示意图………………………………………………………57 增量式控制算法流程图………………………………………………………58 手动 PID 参数调节界面图……………………………………………………59 K P 对动态性能的影响比较图 ………………………………………………60

K P 对动态性能的影响分离图 ………………………………………………61 图 5-1-10 K I 对动态性能的影响图 ……………………………………………………62 图 5-1-11 K D 对动态性能的影响图 ……………………………………………………64
图 5-1-12 图 5-2-1 图 5-2-2 图 5-2-3 图 6-1-1 图 6-1-2 图 6-1-3 图 6-1-4 图 6-1-5 图 6-1-6 图 6-1-7 阶跃响应曲线…………………………………………………………………65 带速度和加速度前馈的 PID 控制器原理框图………………………………66 改进算法前位置环阶跃响应…………………………………………………67 改进算法后位置环阶跃响应…………………………………………………67 一维直线运动仿真图…………………………………………………………70 n 维直线运动仿真图 …………………………………………………………72 二维圆弧仿真图………………………………………………………………74 三维圆弧仿真图………………………………………………………………77 二维椭圆弧仿真图……………………………………………………………79 三维椭圆弧仿真图……………………………………………………………82 二维混合运动仿真图…………………………………………………………82
- VIII -

厦门大学硕士论文

图 形 列 表

图 6-1-8 图 6-1-9 图 6-1-10 图 6-2-1 图 6-2-2 图 6-2-3 图 6-2-4 图 6-2-5 图 7-1-1 图 7-1-2 图 7-1-3 图 7-1-4 图 7-1-5 图 7-1-6 图 7-2-1 图 7-2-2 图 7-2-3

三维混合运动仿真图…………………………………………………………83 四种三次样条曲线的对比图…………………………………………………84 三次样条曲线仿真图…………………………………………………………85 切割机数控系统组成图………………………………………………………87 切割机软体工作机理图………………………………………………………88 切割机人机界面………………………………………………………………88 DSP 主控程序和中断调用模块………………………………………………89 切割机完成的零件图样 ……………………………………………………92 本地应用方案 ………………………………………………………………94 网际应用方案 ………………………………………………………………95 典型运动控制系统的组成 …………………………………………………95 实验平台全景图 ……………………………………………………………96 服务器程序界面 ……………………………………………………………97 客户端程序界面 ……………………………………………………………97 摄像头及照明灯控制图 ……………………………………………………98 高速同步实验图 ……………………………………………………………99 二维平台运动演示图 ………………………………………………………100

- IX -

厦门大学硕士论文

第一章 前 言

第一章:前言
1.1 运动控制系统的发展现况 运动控制 (Motion Control)是在电驱动技术研究的基础上,随着科学技术的发展而 形成的一门综合性多学科的交叉技术。随着微电子技术、电力电子技术以及微处理器控 制技术的发展,运动控制中高性能的要求能够得以实现。近几年来,逐步成为了一个新 的国际研究热点。 而运动控制系统是指以各类电机-传感器作业系统作为主控对象的控制器,包括控 制硬件及软件。随着科技进步,传统的运动控制系统由于其本身的特性局限已不能满足 现代工业和社会发展的要求[67]。主要体现在以下几个方面: 封闭式结构:提供给用户的只是特定环境下的特定功能的目标作业,控制系统的控 制算法和底层接口等对用户是一个"黑箱"。此结构不便于对系统资源的共享和利用,因 而造价高。 控制软件的兼容性差:软件的结构依赖于处理器硬件,难以在不同的系统间移植, 同时也不便于软件的升级或更新。 容错性和可靠性差: 由于采用多片单片机的并行结构, 使得系统的通讯和同步困难, 数据相关性复杂。 缺少网络功能:不便于构造大系统,实现多系统协调以及远程和漫游功能。 针对传统运动控制系统的这些缺陷,研究和开发具有开放式结构的高性能运动控制 器是当前运动控制领域的一个重要发展方向,越来越得到世界各国学者的广泛关注[69]。 在国际上,运动控制技术的研究是从 80 年代后期开始的。1989 年,美国政府为了 减少大型制造设备(数控机床、加工中心等)对日本控制器(FANUC,OMRON)的依 赖性,就成立了“国家制造科学中心” ,制定了下一代机器控制器 ( Next Generation Machine Controller ,简称NGC)的研究计划[68],目标是开发能用于 CAD/CAM 和多传 感器信息集成的商业化机床控制环境软件平台。该计划研究的结果就形成了有助于工业 运动控制器产品标准化的开放式系统结构标准规范 (NGC SOSAS)。随着 NGC SOSAS 的完成,一些发达国家的工业和研究部门也相继制定了开放式运动控制技术的研究计 划。 如欧共体为提高欧洲机床生产商和控制器开发商在世界市场上的竞争力, 1990 年制 定了联合研究计划 Open System Architechure for Control Within Automation (OSACA), 其 目的是制定一个独立于硬件开发商的能适用于大多数工业控制器的开放式体系结构规 范,以提高控制系统的柔性,减少用户OEM制造过程中的培训、开发和维护费用。日 本的六大公司 (Toshiba、Toyada、Mazak 机床公司、日本 IBM、三菱、SML 电气公司 ) 于 1995 年也联合制定了“控制器开放式系统环境”的研究计划,目的是为工厂自动化 设备和控制器的制造提供一个开放性的体系结构规范[66]。
-1-

厦门大学硕士论文

第一章 前 言

在日本,开放式运动控制器则被认为将带来第三次工业革命,并预测其应用的普遍 性将与目前广泛应用的 PLC 类似。 在美国,运动控制器产品的应用则遍及从数控机床、大规模集成电路焊接设备、磁 盘驱动、包装机械、纺织机械到木材加工设备等多个领域。可以说,只要有伺服电机应 用的场合都会用到运动控制器。发展到今天,美国一些著名的运动控制器制造商如 DeltaTau,Galil,DMC,Arrotech,Tech80 等早已蜚声全球。因此开放式运动控制器在 美国被誉为新一代的工业控制器 [68] 。美国还专门成立了美国运动控制器工程师协会 (AIME),由此也不难看出运动控制技术的重要性。 中国对开放式多轴运动控制技术的研究虽然刚刚起步,但发展迅速。几年前还是一 片空白,现在已经崛起了几家拥有独立知识产权的专门从事运动控制产品研发的高新技 术公司, 象深圳的摩信科技、 固高科技等。 其中摩信科技有限公司在中国率先推出的 “基 于网络技术的开放式结构高性能 DSP 运动控制系统”MCT8000 系列产品,是我国在开 放式结构控制系统方面取得重要成果的一个体现。这个成果的产生,表明中国也有了自 己的开放式运动控制系统技术。这种既可充分利用现有丰富的 PC 软、硬件资源,又具 有运动控制所需的实时性和可靠性的控制系统,是改造传统制造业的有利武器。而它的 开放式结构使系统可为许多高科技产品的开发,如“开放式高性能的数控系统”“基于 、 网络的开放式机器人控制器”“柔性自动化生产线”等,提供可靠的软、硬件平台。 、 此项高新技术在我国转化为成熟的产品,实现了巨大的技术跨越,彻底打破了发达 国家对运动控制技术的垄断,为我国对开放式运动控制技术的深一步研究和相关产品的 开发奠定了基础。随着开放式运动控制技术的推广和应用,必将使我国重大装备的系统 集成水平上一个台阶, 并带来巨大的社会和经济效益, 让我们看到了中国制造业的希望。 1.2 运动轨迹规划简介 所谓轨迹,是数控对象在运动过程中的位移、速度和加速度。而轨迹规划是根据作 业任务的要求,计算预期的运动轨迹。轨迹规划器可以使编程简化,只要求用户输入有 关路径和轨迹的若干约束和简单描述,其他复杂的细节计算由规划器完成。 在数控系统中,为了保证机床在起动或停止时不产生冲击、失步、超程或振荡,必 须设计专门的加-减速控制规律,驱使加给电动机的输入(脉冲频率或电压)按照这个 规律变化,从而使机床在各种加工作业的情况下都能快速准确地停留在给定的位置上。 这种控制称为加减速控制[21]。 对于连续切削的数控机床,其进给速度不仅直接影响到加工零件的表面粗糙度和精 度,而且与刀具和机床的寿命以及生产效率有密切关系。对于不同材料的零件,需根据 切削力、表面粗糙度和精度的要求,考虑加工工具、加工方式和条件,选择合适的进给 速度。进给速度的控制方法和所采用的插补算法相关。 插补运算是数控系统根据输入的基本数据 (如直线的起点和终点,圆弧的起点、终
-2-

厦门大学硕士论文

第一章 前 言

点和圆心,进给速度,刀具参数等),在轮廓起点和终点之间,计算出若干中间点的坐标 值,将工件轮廓描述出来[18]。由于每个中间点计算所需的时间影响系统的进给速度,而 插补中间点的精度又影响到加工精度,因此,寻求简单、能满足精度要求和实时性的插 补算法已成为目前设计高性能数控系统的关键。 1.3 运动控制策略 控制策略的设计目标是:1 )减少扰动和参数变化的影响;2 )获得期望的稳态精度和 输入输出的动态特性;3)通过对负载力矩以及对跟踪偏差的控制来达到上述目的[35]。 动态响应的快速性,稳态跟踪的高精度以及行为的鲁棒性是运动控制应用中的主要 性能要求。它们的获得,不仅意味着要有执行机构和仪器方面的先进技术,而且重要的 是合适的控制策略的应用。PID 控制是最早发展起来且目前在工业过程控制中依然是应 用最为广泛的控制策略之一,即便在科技发达的日本,PID 控制的使用率在 84.5%以上, 这是因为 PID 控制器结构简单,且综合了关于系统过去 (I)、现在 (P)和未来 (D)三 方面信息,对动态过程无需太多的预先知识,鲁棒性强,控制效果一般令人满意[29]。 根据PID控制策略开发的为广大工程技术人员所熟知按偏差的比例、积分和微分 进行控制的调节器是目前计算机控制领域应用最为广泛的调节器,它的结构简单,参数 易于调整,在长期的应用中积累了丰富的经验,本文所述的轨迹规划器中的控制算法就 是建立在 PID 控制算法之上的。 在此基础上本文还将探讨速度和加速度前馈对于系统整体性能的影响。 1.4 本文的主要研究内容 本文共分八章。第一章是前言部分,主要介绍了当前运动控制系统的发展现况和基 本概念;第二章探讨了运动控制领域里的两个基本问题——运动规划和运动控制的一些 概念性问题;第三章建立了一维乃至 n 维的直线运动,二维和三维的圆弧、椭圆弧运动 规划的数学模型,以及各种不同约束条件下的三次样条曲线运动的数学模型;第四章讨 论了以上各种运动轨迹数学模型的具体算法实现,并建立了一系列具体的轨迹规划函 数,构成了新型轨迹规划器的软体基础;第五章研究了经典的 PID 控制算法,并在此基 础上探讨了速度和加速度前馈对于系统性能的影响;第六章利用计算机和数学工具软件 对实现的轨迹规划算法进行仿真模拟以验证其有效性,并列举了其在切割机行业中的一 个典型的应用;第七章以该新型轨迹规划器为基础搭建了实验平台,并在此平台上验证 该轨迹规划器的优越性能;第八章对本文工作进行了归纳,并提出了本课题所关注领域 的后续研究热点和亟待解决问题。 1.5 小结 作为自动控制技术里的核心部分,运动控制技术已经成为机加工行业甚至是整个数 控领域里的关键技术,从而在一些发达国家优先发展起来。而研究新一代开放式结构的 高性能多轴运动控制系统又是当前运动控制技术的热点所在。
-3-

厦门大学硕士论文

第一章 前 言

运动控制技术的核心问题是运动轨迹规划算法的优化和控制策略的改进。 本文在研究轨迹规划算法和控制算法的基础上建立了一套切实可行的 DSP 多轴运 动控制器的轨迹规划系统,并应用于国产 MCT8000 系列运动控制器中,从而为构造高 速、高精度和高可靠性的新一代数控系统提供了解决方案。

-4-

厦门大学硕士论文

第二章 问 题 阐 述

第二章:运动控制中的基本问题
运动控制涉及两个基本问题,一是运动规划,二是控制算法。运动规划是在给定的 路径端点之间插入用于控制的中间点序列从而实现沿给定路径的平稳运动。运动控制则 主要解决如何控制目标系统准确跟踪指令轨迹的问题。即对于给定的指令轨迹,选择适 当的控制算法和参数,产生输出,控制目标实时、准确地跟踪给定的指令轨迹。 2.1 运动规划 运动规划通常又称运动插补。即在给定的路径端点之间插入用于控制的中间点序列 或轨迹, 以实现沿给定路径的平稳运动。 运动规划的输入为路径端点, 输出为指令轨迹。 目前运动控制领域里常用的运动规划算法有以下三种: a.逐点比较插补法:从给定的轨迹起点出发,每进给一步都要与给定轨迹上的坐标 值进行比较,决定下一步的进给朝着减少偏差的方向运动,直至到达轨迹的终点。 b.数字积分法(DDA 法) :利用对速度分量的积分的方法来分配脉冲,控制目标沿给 定的路径平稳移动。其插补速度较快,输出脉冲均匀,易于实现多坐标联动[28]。 c.时间分割法:又叫数据采样插补法,是把加工一段直线或圆弧的整段时间细分为 许多相等的时间间隔,称为单位时间间隔(或插补周期),每经过一个单位时间间隔就进 行一次插补计算,算出在这一时间间隔内各坐标轴的进给量,边计算,边加工,直至加 工结束。其插补速度更快,对于复杂多维曲线的运动规划尤其有利。 综合以上常见的插补算法,时间分割法更能满足系统高速高精度的要求,所以本文 的后续章节将对该算法展开详尽的阐述。 2.1.2 运动规划中的精度控制 利用时间分割法进行轨迹规划时,其插补点数据的精确程度将直接影响到运动控制 的效果,所以在构造各种规划曲线的算法模型时要考虑到可能出现的轨迹误差并尽可能 地减小甚至规避之。 对于直线插补来讲,插补所形成的轮廓步长子线段(即增长段)与给定的直线重合, 不会造成轨迹误差;而在圆弧及其他复杂曲线的插补中,因要用切线或弦线来逼近插补 曲线,因而不可避免地会带来轮廓误差[6]。以常见的圆弧插补为例,其轮廓误差的定义 如下图所示。 2.1.1 运动规划的概念

-5-

厦门大学硕士论文

第二章 问 题 阐 述

图 2-1-1 轮廓误差定义图 如上图所示,A 和 B 为曲线顶点某邻域内的两个相邻插补点, AB = dL 。 于是有

ε = R R 2 (dL 2 ) 2 ,所以 dL = 8Rε 4ε 2
(2-2-1) 于是在顶点的某个邻域内要求

dL ≤ 2 2 Rε ε 2
(2-2-2)



此处的 dL 就是在给定的轮廓精度为ε时所允许的最大的单个采样周期内的插补精度, 再进一步得到该轨迹规划的精度控制公式

Vmax ≤ 2 R[a tan 2( 2 Rε ε 2 , R ε )] / t
式中 Vmax 为运动允许的保证插补轮廓精度的最大的移动速度值, R 为圆弧半径,atan2 为计算机反正切函数,Δt 为插补周期。 其他类型的曲线运动的精度控制问题与以上所阐述的类似,只是往往更复杂一些, 可由插补点曲率半径的值推导得到符合要求的最大速度值以保证运动轨迹规划过程中 的精度要求。 2.2 运动控制 运动控制主要解决如何控制目标系统准确跟踪指令轨迹的问题。即对于给定的指令 轨迹,选择适当的控制算法和参数,产生输出,控制目标实时、准确地描述给定的指令 轨迹。 2.2.1 运动控制算法 时域中评价一个系统的动态性能,通常以系统对单位阶跃输入信号的暂态响应为依 据。这时系统的暂态响应曲线称为单位阶跃响应或单位过渡响应,典型的响应曲线如下 图所示。为了评价系统的动态性能,规定指标如下:
-6-

厦门大学硕士论文

第二章 问 题 阐 述

图 2-2-1 单位阶跃响应曲线图 1) 延迟时间 t d :指输出响应第一次到达稳态值 50%所需要的时间。 2) 上升时间 t r :指输出响应从稳态值的 10%上升到 90%所需的时间。对有振荡的 系统则取从零到第一次达到稳态值所需的时间。 3) 峰值时间 t p :指输出响应超过稳态值而到达第一个峰值所需的时间。 4) 调节时间 t s :指输出量 y (t ) 和稳态值 y (∞) 之间的偏差达到允许范围(一般取 2%或 5%)以后不再超过此值所需的最短时间。 5) 最大超调量 σ p % :指暂态过程中输出响应的最大值超过稳态值的百分数。即

σ p% =

[ y (t p ) y (∞)] y (∞ )

× 100%

6) 稳态误差 e ss :指系统输出实际值与希望值之差。 在上述几项指标中,峰值时间 t p 、上升时间 t r 和延迟时间 t d 均表征系统响应初始 阶段的快慢;调节时间 t s 表征过渡过程(暂态过程)的持续时间,从整体上反映了系统 的快速性;而超调量 σ p % 标志暂态过程的稳定性;稳态误差 e ss 反映系统复现输入信号 的最终精度[54]。
-7-

厦门大学硕士论文

第二章 问 题 阐 述

好的运动控制算法就是要通过选择合适的控制规律或策略来使系统具有低超调,短 调节时间和小稳态误差。 PID 控制算法是迄今为止最通用的控制方法, 其控制模型简单、 鲁棒性好、可靠性高,但常规的 PID 控制对于非线形、时变不确定系统无法达到理想的 控制效果[62]。 因此除了要对常规的 PID 算法进行积分项和微分项的改进外, 还要考虑引 入速度和加速度前馈来达到减小位置跟踪滞后误差,从而在一定程度上提高位置控制精 度。 另外,随着现代控制理论研究和应用的深入和发展,各种先进的新型智能 PID 控制 算法应运而生,为控制复杂无规则的系统开辟了新的途径。 2.2.2 运动控制的稳定性 一个控制系统,不管是连续的还是离散的,一旦受到外界或内部的扰动,都会偏离 原来的平衡工作状态。在扰动消失后,能否回到原来的平衡工作状态,对于系统能否正 常工作是非常重要的,这个问题就是系统的稳定性问题。如果系统受到外界的扰动后, 偏离了原来的平衡状态,当扰动去掉后,系统又能逐渐恢复到原来的平衡状态,则称系 统是稳定的,否则,系统就是不稳定的。系统的稳定性是系统的固有特性,它与扰动的 形式无关,而只取决于系统本身的结构参数[35]。 那么如何判定一个系统的稳定性呢?这里给出系统稳定性的判据。 对于连续系统,也即在 S 平面内,系统稳定的充分必要条件是:系统全部的闭环特 征根或全部极点均落在 S 平面的左半平面内。再看 S 平面与 Z 平面的变换关系。根据 z 变换的定义,复变量 s 到复变量 z 的变换关系为:

z = e sT
其中,T 为采样周期。以 s = σ + jω 代入上式

z = e (σ + jω )T = e σT e jωT = eαT ∠ωT =| z | ∠θ
式中, | z |= e
αT

, θ = ωT = 2π

ω 2π ,ωs = 。 ωs T

根据以上关系,当 ω 在 ∞到 + ∞ 之间变化时,z 的角度也从 ∞到 + ∞ 变化,两个复 平面的映射关系如下图所示。

-8-

厦门大学硕士论文

第二章 问 题 阐 述

图 2-2-2

S 平面到 Z 平面的映射图

于是,可以得到以下结论: 1.S 的虚轴在 Z 平面的映射为以原点为圆心的无穷个相重叠的单位圆的圆周。 2.S 的左半平面映射到 Z 平面上是单位圆的内部区域。 由此得到离散系统的稳定的充分必要条件:系统闭环传递函数 φ (z ) 的全部极点或 者说系统的特征方程的根均位于 Z 平面的单位圆的内部。当有极点位于单位圆上时,系 统处于临界状态,在工程上也认为是不稳定的。 综上述,要研究系统的稳定性,需求出闭环系统 z 传递函数的所有特征根并判断其 是否全部落于单位圆内,这对于低阶系统还算实用,但对于高阶系统就会很困难,所以 寻求不必求解系统特征根就能确定系统是否稳定的判据就显得很有意义了。在这方面, 已经有很多人作出了卓有成效的工作, 比较常见的有: 朱利—阿斯特隆姆 (Jurry-Astrom) 稳定判据,劳斯—霍尔维茨(Routh-Hurwith)稳定判据等。这里不再详细展开。 2.3 小结 高速度、高精度、高效率和高可靠性是现代数控高性能化的四项关键指标。高速度 要求数控系统具有高速计算,快速插补,超高速通信和主轴超高速运转能力。其中快速 插补运算技术涉及不断提高直线、圆弧插补运算性能,赋予插补器曲面直接插补计算功 能,实现复杂形状的曲面零件微小区段的逼近,设置专用函数发生器进行样条、极坐标、 渐开线、指数函数及正弦函数等高速插补运算。高精度要求数控系统体现高分辨率位置 检测和数字伺服控制技术。数字式伺服控制技术的特点在于应用现代控制理论,可在插 补前(后) 采用加减速控制,可使加工轮廓误差减至最小。高效率要求数控系统具备主 轴控制的高性能伺服技术、自动检测技术和自动运转的起停控制技术,以便于实现主轴 无级调速和位置控制。高可靠性则要求数控系统具有抗干扰和对环境的适应能力,此外 还要具备自诊断和故障自动清除的能力。所有上述性能的获取都是直接或间接地依靠运 动规划算法的优化和更优运动控制策略的选择。

-9-

厦门大学硕士论文

第三章 运动轨迹规划的数学模型

第三章:运动轨迹规划的数学模型
本章节将建立一维乃至 n 维的直线轨迹规划的数学模型,二维和三维的圆弧轨迹规 划的数学模型,二维和三维的椭圆弧轨迹规划的数学模型,同时还将讨论三次样条曲线 轨迹规划数学模型的建立。 3.1 直线轨迹规划 3.1.1 一维的情况 图 3-1-1 为一维直线连续轨迹规划的示意图。图中 Vs 表示第 i 个运动段起点的速 度,Vm 表示第 i 个运动段的目标速度,Ve 则表示第 i 个运动段终点的速度;Ts 为由 Vs 到 Vm 的过渡时间,Tm 为恒速时间,Te 为从 Vm 到 Ve 的过渡时间。又设 Td 为采样周期, Pi-1,Pi 分别为第 i 个运动段的起始和终了位置,a,V,P 分别表示轨迹规划器的加速度, 速度和位置输出。 应用等间隔采样和微积分原理,不难写出:

as , a = 0, a , e

0 ≤ T < Ts T s≤ T <T s+Tm Ts + Tm ≤ T < Ts + Tm + Te

(3-1-1) 图 3-1-1 一维直线连续轨迹规划

式中 a s = (Vm Vs ) / Ts , a e = (Ve Vm ) / Te 。

Vs + asT , V = Vm , V + a (T T T ), e s m m

0 ≤ T < Ts T s≤ T <T s+Tm Ts + Tm ≤ T < Ts + Tm + Te
(3-1-2)

Ps + VsT + asT 2 / 2, 0 ≤ T < Ts P = Ps + Ss + Vm (T Ts ), T s≤ T <T s+Tm 2 P + S + S + V (T T T ) + a (T T T ) / 2, T + T ≤ T < T + T + T s m m s m e s m s m s m e s
(3-1-3) 式中 S s = (Vm + Vs )Ts / 2 , S m = Pi Pi 1 S s (Ve + Vm )Te / 2 。 3.1.2 多维的情况 以三维空间直线为例,首先按一维的直线插补得到每个插补周期内前进的量,然后 按照直线在各个坐标轴的投影定理将插补出的运动参数(位移,速度,加速度)分配到

- 10 -

厦门大学硕士论文

第三章 运动轨迹规划的数学模型

各个轴上。

图 3-1-2 空间矢量的投影图 终点为 ( xe , ye , z e ) , 某周期内该运动沿直线方向 设直线运动的起点为 ( xs , y s , z s ) , 运动了 L 的距离,速度为 V,加速度为 a,即时运动点坐标为 P ( x, y, z ) 。则该时刻各 运动轴上的运动参数分量为:

xe x s x = xs + L * ( disp ) ye y s ) y = ys + L * ( disp ze z s z = z s + L * ( disp )
其中 disp = 同样的,

(3-1-4)

( xe x s ) 2 + ( y e y s ) 2 + ( z e z s ) 2 。

xe x s Vx = V * ( disp ) ye y s ) V y = V * ( disp ze z s Vz = V * ( disp )

(3-1-5)

- 11 -

厦门大学硕士论文

第三章 运动轨迹规划的数学模型

xe x s a x = a * ( disp ) ye y s ) a y = a * ( disp z zs ) az = a * ( e disp

(3-1-6)

式中,V x ,V y ,V z 分别为该时刻运动速度 V 在各个坐标轴上的投影; a x , a y , a z 则 分别为该时刻运动加速度 a 在各个坐标轴上的投影。 3.2 圆弧轨迹规划 在很多应用场合,比如弧焊机器人和数控机床,需要控制目标工具在空间沿圆弧运 动。本节简要介绍 MCT8000 运动轨迹规划器中三维空间圆弧轨迹的规划算法。 设给定圆弧的圆心为 p c ,运动起始和终止位置分别为 p s , p e 。又设 P ,v ,a 分别为轨迹规划器在 t 时刻的输出位置,速度和加速度向量。ξ 为通过圆心且垂直于给 定圆弧所在的平面的单位矢量。θ 为任意时刻插补点与起始点的之间的圆弧对应的圆心 角,P 点的速度和加速度的大小分别为 v p 和 a p ,参照图 3-2-1,可以写出 t 时刻轨迹规 划器的输出:

图 3-2-1 三维空间圆弧轨迹的规划

al =

( Ps Pc )e ξ (θ +π / 2 ) ( Ps Pc )e ξ (θ +π / 2 )

ap

(3-2-1)

- 12 -

厦门大学硕士论文

第三章 运动轨迹规划的数学模型

v=

( Ps Pc )e ξ (θ +π / 2 ) ( Ps Pc )e ξ (θ +π / 2 )

vp

(3-2-2)

P = Pc + ( Ps Pc )e ξθ
其中 e 为空间的旋转张量。其矩阵表达形式为
ξθ

(3-2-3)

eξθ

ξ x 2Vθ + Cθ = ξ xξ yVθ + ξ z Sθ ξ xξ zVθ ξ y Sθ

ξ xξ zVθ ξ z Sθ ξ xξ zVθ + ξ y Sθ ξ y 2Vθ + Cθ ξ yξ zVθ ξ z Sθ ξ yξ zVθ + ξ z Sθ ξ z 2Vθ + Cθ

式中,

Vθ = verSθ = 1 cosθ ; Sθ = sin θ ; Cθ = cosθ 。

ξ x , ξ y , ξ z 分别为单位法向矢量 ξ 在各个坐标轴上的投影;
3.3 椭圆轨迹规划[64]
3.3.1 3.3.1.1 椭圆所在平面内的二维规划 切线拟合法

图 3-3-1 椭圆规划之切线拟合法
- 13 -

厦门大学硕士论文

第三章 运动轨迹规划的数学模型

如图 3-3-1 所示, p s 为起始点 , P 为椭圆上任意点, p e 为终止点 ,DT 为采样 周期,L 为第一个采样周期走的步长 , β 为 P 点速度及切线加速度的方向角,θ - θ 0 为 P 点转过的角度,h0、m0、H0、h1、m1 如上图所示。 取L =

1 a( DT ) 2 (a 为加速度),并利用其作为划分椭圆弧的单元,对每一小段弧所 2
xx0 yy 0 + 2 = 1; a2 b

对应的中心角进行积累即为实时的中心角。 设 p s (x0,y0),则过 p s 点的椭圆切线 T 的方程: 则中心到切线 T 的距离: 0 = H

a 2b 2 a y 0 + b x0
4 2 4 2

; 所以 h0 =

L* H0

ρ0

; 0 = m

L2 h0 ;
2

dθ 0 = arctan

h0 ;故 θ 1 = θ 0 + dθ 0 ; ρ 0 m0

ρ1 =

a 2b 2 ; a 2 sin 2 θ 1 + b 2 cos 2 θ 1

x1 = ρ1 * cosθ 1 ; y1 = ρ1 * sin θ 1 ;

b 2 * x1 β 1 = 3π + arctan( 2 ); a * y1
依次类推: H i =

a 2b 2 a y i + b xi
4 2 4 2 2

;所以 hi =

L * Hi

ρi



mi = ± L2 hi (1、3 象限取正,2、4 象限取负) ;
dθ i = arctan


hi ρ i mi

; (3-3-1)

θ i +1 = θ i + dθ i ; ρ i +1 =
a 2b 2 ; a 2 sin 2 θ i +1 + b 2 cos 2 θ i +1
- 14 -

(3-3-2)

厦门大学硕士论文

第三章 运动轨迹规划的数学模型

xi +1 = ρ i +1 * cosθ i +1 ; y i +1 = ρ i +1 * sin θ i +1



β i +1 = kπ + arctan(

b 2 * xi +1 ) ( k=3,4,5, 具 体 由 运 动 点 所 处 的 象 限 决 定 ) ; a 2 * y i +1
(3-3-3)

由此得到任意采样时间的角度 θ 、 β 及矢径长 ρ 。 3.3.1.2 压缩圆法 先在椭圆所在的平面内对椭圆进行二维规划。 R 为虚拟圆的半径,a ,b 分别为所规划椭圆的长短轴,M ( m x , m y )为起始点,N ( n x , n y )为终止点,P(x,y)为椭圆上任意点, 所示:

θ , θ 0 , β 所对应的角如图 3-3-2

图 3-3-2 椭圆规划之压缩圆法 应用此法,只需将椭圆上的任意点 M、N 对应的虚拟圆上的点 M’,N’的坐标求出, 然后按圆弧进行轨迹规划,将得到的坐标压缩转化为对应的椭圆上的点即可。 已知椭圆上的起始点 M( m x , m y ), 由
' mx = mx

终止点 N( n x , n y )以及椭圆方程:

x2 y2 + = 1, a2 b2




m 'y = m y *

a b



' nx = nx ,

n 'y = n y *

a b

- 15 -

厦门大学硕士论文

第三章 运动轨迹规划的数学模型
'
'

得到虚拟圆上对应的起始点 M’( m x , m

y

)与终止点 N’( n x , n

'

'

y

), 然后就将椭圆弧的规

划问题转化为圆弧的规划,可得到虚拟圆上任意点 P’(x’,y’)的坐标,之后再由公式

x = x' , y = y' *

b ,得到相应的椭圆弧上的点 P(x,y)的坐标。即而由 a
; (3-3-4)

ρ = x2 + y2

θ = a tan( y / x)
β = a tan(



(3-3-5) (3-3-6)

b2 * x ) ; a2 * y

得到任意采样时间的角度 θ 、 β 及矢径长 ρ 。 3.3.2 空间矢量分析

设给定椭圆弧的椭圆中心为 p c ,运动起始位置为 p s 。又设 P,V,a 分别为轨迹规 划器在 t 时刻的输出位置,速度和加速度向量。ξ 为通过椭圆中心,且垂直于椭圆平面 的单位向量,P 点的速度和加速度的大小分别为 v p 和 a p ,

ρ 为 t 时刻对应的椭圆矢径

长度,θ 为任意时刻插补点与起始点的之间的圆弧对应的圆心角,θ 0 为起始点所对应的 中心角,如图 3-3-1、图 3-3-2 中所示,参照图 3-3-3,可以写出 t 时刻轨迹规划器的 输出:

图 3-3-3 三维空间椭圆弧轨迹的规划
- 16 -

厦门大学硕士论文

第三章 运动轨迹规划的数学模型

al =

( Ps Pc )e ξ ( β θ 0 ) ap Ps Pc

(3-3-7)

( Ps Pc )e ξ ( β θ 0 ) v= vp Ps Pc P = Pc + ( Ps Pc )e ξ (θ θ 0 ) ρ Ps Pc

(3-3-8)

(3-3-9)

故只需由公式(3-3-1)、(3-3-2)、(3-3-3)或(3-3-4)、(3-3-5)、(3-3-6)求出实时的 θ 角、

β 角及对应的失径大小 ρ ,代入公式(3-3-7)、(3-3-8)、(3-3-9)可得任意 t 时刻的位移、
速度及切向加速度的矢量值。 3.4 三次样条曲线的轨迹规划 3.4.1 三次样条函数的建立及求解 3.4.1.1 三次样条函数的定义 设 {( x k , y k )}k =0 有 n+1 个点,其中 a = x 0 < x1 < Λ < x n = b 。如果存在 n 个三次
n

多项式 s k (x ) ,系数为 s k , 0 , s k ,1 , s k , 2 , s k ,3 ,满足如下性质: I. s ( x) = s k ( x) = s k , 0 + s k ,1 ( x x k ) + s k , 2 ( x x k ) + s k ,3 ( x x k )
2 3

x ∈ [ x k , x k +1 ]且k = 0,Λ , n 1
II. s ( x k ) = y k , III s k ( x k +1 ) = s k +1 ( x k +1 ) , IV s k ( x k +1 ) = s
' ' k +1

k = 0, Λ , n k = 0, Λ , n 2 k = 0, Λ , n 2 k = 0, Λ , n 2

( x k +1 ) ,

V

s '' k ( x k +1 ) = s '' k +1 ( x k +1 ) ,

则称函数 s (x) 为三次样条函数。 其中性质 I 描述了分段三次多项式构成的 s (x) ; 性质 II 描述了给定数据点集的分段 三次插值; 性质 III 和性质 IV 保证分段三次多项式函数是一个光滑连续曲线; 性质 V 保
- 17 -

厦门大学硕士论文

第三章 运动轨迹规划的数学模型

证了函数的二阶导数也是连续的。 3.4.1.2 三次样条函数的构建 设 s (x) 是待求的三次样条曲线,则其可以表示为分段三次多项式函数。首先,为了 便于推导,我们定义以下三个变量:

hk = x k +1 x k , d k = ( y k +1 y k ) /( x k 1 x k ) ,

m k = s '' ( x k ) ,
因为 s (x) 是分段三次多项式,所以其二阶导数在区间 [ x0,x n ] 内是分段线形的,根据 线形 Lagrange 插值公式,可以写出:

s '' ( x k ) = s '' ( x k )

x xk x x k +1 + s '' ( x k +1 ) x k +1 x k x k x k +1

(3-4-1)

将 hk 及 m k 代入(3-4-1)

s '' (xk ) =

m mk (xk +1 x) + k +1 (x xk ) , x ∈[xk , xk+1 ],k = 0,1,2,...,n 1 (3-4-2) hk hk

对上式两边积分,并根据定义, s k ( x k ) = y k , s k ( x k +1 ) = y k +1 得:

s k ( x) =

mk m y m h ( x k +1 x) 3 + k +1 ( x x k ) 3 + ( k k k )( x k +1 x) 6hk 6 hk hk 6

y m h + ( k +1 k +1 k )( x x k ) hk 6

(3-4-3)

对式(3-4-3)两边求导并将 x k 代入式中分别表示 s ' k 1 ( x k ) 及 s ' k ( x k ) ,再根据一阶导 数连续,即 s ' k 1 ( x k ) = s ' k ( x k ) ,化简后最终得到下式:

hk 1 m k 1 + 2(hk 1 + hk )m k + hk mk +1 = 6(d k d k 1 ) ,
为方便求解,令

(3-4-4)

λ k = hk (h

k 1

+ hk )

, ρ k = 1 λ k , 则式(3-4-4)表示为

- 18 -

厦门大学硕士论文

第三章 运动轨迹规划的数学模型

ρ K mk 1 + 2mk + λk mk +1 = 6

d k d k 1 , k = 0,1,...n hk 1 + hk

(3-4-5)

此时,共有 n+1 个未知量 m k ,却仅有 n-1 个方程,所以我们还要给定 m0 和 m n 才能求 解。线形方程组如下:



2

λ1
2

0

0 0

Λ Λ Λ Ο 2

ρ2
0 Μ 0 0

λ2
2 Ο 0 0

ρ3
Μ Λ Λ

λ3
Ο

ρn2
0

ρn1

d1 d0 ρ1m0 6 0 m1 h0 + h1 d2 d1 0 m2 6 h1 + h2 0 m3 = Μ Μ Μ dn2 dn3 6 λn2 mn2 hn3 + hn2 m d d 2 n1 n1 n2 λn1mn 6 h +h n2 n1

(3-4-6)

利用式(3-4-6)求出 m k 的值代入式(3-4-3)即可得三次样条曲线的分段函数表达式。 或者代入下式:

sk ( x) = sk ,0 + sk ,1 ( x xk ) + sk ,2 ( x xk ) 2 + sk ,3 ( x xk ) 3 , x ∈ [ xk , xk +1 ] (3-4-7)
其中

s k , 0 = y k , s k ,1 = d k
3.4.1.3 分类求解 (1) 追赶法求解

hk m m mk (2mk + mk +1 ) , s k , 2 = k , s k , 2 = k +1 6hk 6 2



取一般情况,同时为便于阐述,将式(3-4-6)简写为



b1

λ1
b2

0

0 0

Λ Λ Λ Ο bn 2

ρ2
0 Μ 0 0

λ2
b3 Ο 0 0

ρ3
Μ Λ Λ

λ3
Ο

ρ n2
0

ρ n 1

0 m1 D1 0 m2 D2 0 m3 D3 = Μ Μ Μ λn 2 mn 2 Dn 2 bn 1 mn 1 Dn 1

(3-4-8)

再分解为下式(3-4-9)
- 19 -

厦门大学硕士论文

第三章 运动轨迹规划的数学模型

p1 = b1 λ 其中, qi = i (i = 1, Λ , n 1) pi pi = bi ρ i qi 1 (i = 2,Λ , n)
令 Λ Λ Λ Ο 1 0 0 m1 Z 1 0 m2 Z 2 0 m3 Z 3 = Μ Μ Μ q n 2 mn 2 Z n 2 1 mn 1 Z n 1

(3-4-10)

1 0 0 Μ 0 0

q1 1 0 Μ Λ Λ

0 q2 1 Ο 0 0

0 0 q3 Ο 0 0

(3-4-11)

代入(3-4-9)



p1

0 p2

0 0 p3 Ο 0 0

0 0 0 Ο

Λ Λ Λ Ο p n2

ρ2
0 Μ 0 0

ρ3
Μ Λ Λ

ρ n2
0

ρ n 1

0 Z 1 D1 0 Z 2 D2 0 Z 3 D3 = Μ Μ Μ 0 Z n 2 Dn 2 p n 1 Z n 1 Dn 1

(3-4-12)

于是有

D Z1 = 1 p1 Di ρ i Z i 1 Z i = (i = 2, Λ , n 1) pi

(3-4-13)

- 20 -

厦门大学硕士论文

第三章 运动轨迹规划的数学模型

进而得到

m n 1 = Z n 1 mi = Z i q i mi +1 (i = n 2, Λ ,1)

(3-4-14)

至此,所求得的 m k ( k = 1, Λ , n 1) 与给定的 m 0 , m n 一起就得到了用来构建三次 样条分段函数完整的 m k ( k = 0, Λ , n) 值,下个小节将讨论 m 0 、 m n 的取值,也就是三 次样条函数的分类和边界条件的给定。 (2) 分类 针对三次样条端点约束的不同,此处讨论以下四种情况[57]: 1)压紧(Clamped)样条: 给定端点处的一阶导数值 s ( x0 ) 及 s ( x n ) 。对应的
' '

m0 =

m m 3 3 (d 0 s ' ( x0 )) 1 , m n = ( s ' ( x n ) d n 1 ) n 1 。 h0 hn 1 2 2
'' ''

2)自然(Natural)样条: 端点处的二阶导数值 s ( x0 ) = 0 及 s ( x n ) = 0 。即

m0 = m n = 0 。
3)抛物线终结(Parabolically Terminated)样条: 在区间 [ x 0 , x1 ] 内 s ( x) = 0 ,
' ''

且在区间 [ x n 1 , x n ] 内 s ( x) = 0 。此时 m0 = m1 , m n = m n 1 。
' ''

4)端点曲率调整(End-point Curvature-adjusted)样条: 给定端点处的二阶导 数值 s ( x 0 ) 及 s ( x n ) ,即 m0 和 m n 的值。 以上不同的端点约束均可构成唯一的三次样条曲线, 将各自对应的 m0 和 m n 的值代 入式(3-4-6),再利用追赶法即可求得其余的 m k 值, 其中k = 1, Λ , n 1 。 3.4.2 三次样条曲线插补算法的数学模型 3.4.2.1 插补原理 为了最大程度地接近实际的三次样条曲线的轨迹,此处用一段误差允许范围内的等 弦来逼近实际的曲线,即在每一个运动周期 dt 内,通过插补计算,得到适当的 x、y 方 向的进给量 x 、 y ,使每个周期内用来逼近对应弧长的弦的长度为一误差允许范围
- 21 '' ''

厦门大学硕士论文

第三章 运动轨迹规划的数学模型

内的定值。 如图 3-4-1,设在一个周期 dt 内,当前点为 A( x n , y n ),目标点为 B 或 C,记为 ( x n +1 , y n +1 ),插补精度为 dL ,误差允许为 δ ,对应曲线的多项式函数表达式为 f (x) , 则根据微分线段的弧长公式可知,

图 3-4-1 三次样条曲线规划微分图

dL = 1 + ( f ' ( x n )) 2 dx0 ,


(3-4-15)

dx 0 =
于是

1 1 + ( f ' ( x n )) 2

dL ,

(3-4-16)

x n +1 = x n + dx0 = x n +

1 1 + ( f ' ( x n )) 2

dL ,

(3-4-17)

将 x n +1 代入曲线方程 f (x) 得 y n +1 的值 ,并令

E =| ( x n +1 x n ) 2 + ( y n +1 y n ) 2 dL | ,

(3-4-18)

若 E < δ ,则 B 点即为所求,记坐标为 ( x n +1 , y n +1 ) ,否则,记弦 AB 的斜率为 K AB ,

- 22 -

厦门大学硕士论文

第三章 运动轨迹规划的数学模型



dx1 =

1 1 + ( K AB ) 2

dL ,

(3-4-19)

x n +1 = x n + dx1 = x n +

1 1 + ( K AB )
2

dL



(3-4-20)

求得相应的 y n +1 后再次代入式(3-4-18)求 E 值,若 E < δ ,则 C 点为所求,记 坐标为 ( x n +1 , y n +1 ),否则,依此法继续求解,直至 E 值落在允许的误差范围 δ 内。 式(3-4-16)到式(3-4-20)即为单个插补周期内的目标点坐标( x n +1 , y n +1 )的递推 计算公式, 得到目标点坐标后, 可方便的计算出在该插补周期 dt 内两坐标轴上的位移增 量:

x = x n +1 x n , y = y n +1 y n ,

(3-4-21) (3-4-22)

利用式(3-4-21)和式(3-4-22)再计算得到合适的脉冲数并分别加载到两轴的控 制电机驱动器中。当下一个插补周期到来时,重复以上的算法步骤,直至规划结束到达 曲线的末端,实现三次样条曲线的运动轨迹。 3.4.2.2 插补精度分析 在每个插补点,y 轴坐标由样条函数直接计算而得,因而插补点上没有位置误差, 然而,由于用弦线来逼近弧,不可避免地会带来轮廓误差ε,如图 3-4-2 所示。

图 3-4-2 样条曲线插补精度分析图

- 23 -

厦门大学硕士论文

第三章 运动轨迹规划的数学模型

在样条曲线的各个顶点处的曲率之最大值设为 K ,说明曲线在此处弯曲的最为厉 害,因而插补精度 dL 一定时,在顶点的一个邻域内轮廓误差ε最大。 由于 dL 如上图所示, 和 B 为曲线顶点某邻域内的两个相邻插补点,AB = dL 。 A 和曲率半径 ρ 相比很小,近似认为 A 和 B 也在曲率圆上。于是有

ε = ρ ρ 2 (dL 2 ) 2 ,所以 dL = 8 ρε 4ε 2
于是在顶点的某个邻域内有

(3-4-23)

dL ≤

8ε 4ε 2 K



(3-4-24)

此式说明当轮廓误差ε给定时, 样条曲线各顶点曲率 K 越小, 则允许的插补精度 dL 就越大,反之亦然。所以插补精度 dL 的给定与三次样条曲线的形状有关。 3.5 小结 本章节建立了各种常用的平面和空间曲线的数学模型。 首先用梯形、双曲线和抛物线三种不同的速度分配模式来规划一维直线运动的加减 速过程,从而取得了理想的加减速效果,避免了由于速度或加速度突变而造成的对系统 硬件的冲击。多维直线运动的轨迹规划以一维直线运动的轨迹规划为基础,先转化为一 维插补并得到该采样时刻的运动参数, 然后按照坐标投影法则分配到各个维度 (电机轴) 上,从而实现多电机联动,完成多维直线的运动规划。 平面和空间的圆弧和椭圆弧运动也是按着一维直线的速度、加速度分配方式来进行 插补,然后利用给定的几何信息和空间的旋转矩阵将得到的运动参数分配到各轴做联合 运动, 实现平稳快速高精度的弧线运动。 其中由于椭圆弧长求解涉及到椭圆积分的问题, 所以本文另辟蹊径,用一种压缩圆的方法使椭圆弧的规划算法与圆弧的规划算法在运算 量上保持在同一个数量级内,从而满足了实时插补的需要。 本文还探讨了三次样条曲线运动规划的插补算法模型,不仅丰富了该轨迹规划器的 运动类型,而且避免了用直线和圆弧逼近复杂曲线时过渡点的切线不连续现象,从而可 以更好、更圆滑的实现工件轮廓曲线的加工。

- 24 -

厦门大学硕士论文

第四章 运动轨迹规划算法实现

第四章:运动轨迹规划算法实现
本章节将根据以上直线、圆弧、椭圆弧以及三次样条曲线的算法模型在国产 MCT8000 系列 DSP 运动控制卡上具体实现其程序模块,使该运动控制板可以控制伺 服或步进电机做以上各种类型曲线的运动插补。 4.1 总体规划 国产 MCT8000 运动控制卡的轨迹规划属于数据采样插补法,或称为时间分割法。 插补运算是数控系统根据输入的基本数据 (如直线的起点和终点,圆弧的起点、 终点和 圆心,进给速度,刀具参数等),在轮廓起点和终点之间,计算出若干中间点的坐标值, 通过计算,将工件轮廓描述出来。它尤其适合于闭环和半闭环以直流或交流电机为执 行机构的位置采样控制系统。这种方法是把加工一段直线或圆弧的整段时间细分为许 多相等的时间间隔,称为单位时间间隔(或插补周期)。每经过一个单位时间间隔就进 行一次插补计算,算出在这一时间间隔内各坐标轴的进给量,边计算,边加工,直至 加工终点。 在加工某一直线段或圆弧段时,先通过控制加速度来设计速度轨迹,然后通过速 度计算,将进给速度分割成单位时间间隔的插补进给量(或称为轮廓步长),又称为一 次插补进给量。根据刀具运动轨迹与各坐标轴的几何关系,就可求出各轴在一个插补 周期内的插补进给量,按时间间隔以增量形式给各轴送出一个插补增量,通过驱动部 分使机床完成预定轨迹的加工。 为了丰富和完善该板卡所能完成的运动类型, 本文将建立以下运动插补规划函数, 分别对应直线的一维和 n 维、圆弧的二维和三维、椭圆弧的二维和三维的轨迹规划。 具体函数名如下: 直线规划:line_move_1 (一维) 圆弧规划:arc_move_2D(二维) 椭圆弧规划:ellipse_move_2D(二维) 此外还有三次样条曲线的轨迹规划函数组: SplinePara(),dx_next(),spline_move()。 有了以上轨迹规划函数组及其之间的组合, MCT8000 运动控制板卡即可在生产实 践中通过编程控制实现各种复杂运动,满足客户需求。 下边是部分常量及公共变量和函数的定义与说明 ◆常量定义 #define #define #define MCT_NO_ERROR MCT_AXIS_UNMAP MCT_INVALID_AXIS
- 25 -

line_move_n arc_move_3D

(n 维) (三维)

ellipse_move_3D(三维)

0 -100 -300

//成功返回 //轴没有被映射 //系统不存在的轴

厦门大学硕士论文

第四章 运动轨迹规划算法实现

#define #define #define 太小 #define #define #define #define #define #define #define #define int

MCT_MOTION_AFTER MCT_MOTION_BEFORE MCT_LASTVEL_TOOHIGH MCT_ZERO_ACCELERATION MCT_ZERO_VELOCITY MCT_ZERO_DISPLACEMENT MCT_ZERO_SAMPLINGPERIOD MCT_ZERO_ANGLE MCT_INVALID_ARC MCT_INVALID_VECTOR MCT_ZERO_VECTOR *axis //运动轴数组。 //圆弧起点。 //圆弧的圆心。

-900 -1000 -1200 -1300 -1400 -1500 -1600 -1700 -1800 -1900 -2000

//采样点在运动结束点之后 //采样点在运动起始点之前 //某段起点速度太大或段长度 //最大加速度为 0 //最大稳定速度为 0 //段起点与段终点重合 //采样周期为 0 //旋转角度为 0 //圆弧半径为 0 //法向量不垂直圆弧面 //法向量为 0

◆公共变量定义

float *start_pos float *center_pos float *vector float angle int ccw_dir float start_vel float steady_vel float end_vel float allow_accel float sampling_period int pattern

//圆弧所在平面的法向向量。 //旋转角度。 //旋转方向。 //起点速度 //稳定速度 //结束点速度 //最大加速度 //采样周期 /*轨迹规划模式。目前提供的轨迹规划模式有: 轨迹规划模式 MCT_PATTERN_TERRACE MCT_PATTERN_ TRIANGLE MCT_PATTERN_ PARABOLA 说明 梯形轨迹规划 三角函数双 S 曲线轨迹规划 抛物线双 S 曲线轨迹规划*/

float sampling_time float *point_para ◆公共插补函数 函数原型:

//采样时间 //采样点数据

int trajectory_interpolation ( float start_pos, float end_pos, float start_vel,float
- 26 -

厦门大学硕士论文

第四章 运动轨迹规划算法实现

steady_vel, float end_vel, float allow_accel, float sampling_period, float sampling_time, float *point_para ); 功能: 根据插补类型的不同将各种运动类型转化为一维的插补计算,得到每个周期内采 样点的运动参数。 返回值: 出错代码: MCT_NO_ERROR , MCT_MOTION_BEFORE, MCT_MOTION_AFTER 输入参数: start_pos ,end_pos,start_vel,steady_vel,end_vel,allow_accel, sampling_period, sampling_time。 输出参数: *point_para 算法实现: ◆ 流程图

图 4-1-1 公共插补函数算法流程图 ◆ 具体步骤:(以梯形插补为例说明) STEP 1 分配时间段——加速段、匀速段及减速段和各段对应的速度加速度;
- 27 -

厦门大学硕士论文

第四章 运动轨迹规划算法实现

l = (float)fabs( end_pos - start_pos ); 距离

//总的距离

s = ( steady_vel + start_vel ) * ts / 2 + (steady_vel +end_vel)* te / 2;//加减速过程的 ts = fabs( steady_vel - start_vel ) / allow_accel; //加速时间 te = steady_vel / allow_accel; tm =( l - s ) / steady_vel; as = ( steady_vel - start_vel ) / ts; ae = -(steady_vel-end_vel)/ te; 度; if( sampling_time < ts + MCT_CALC_EPS ) { *point_para = start_pos + start_vel * sampling_time + as * sampling_time * sampling_time / 2; *( point_para + 1 ) = start_vel + as * sampling_time; *( point_para + 2 ) = as; } else if( sampling_time < ts + tm + MCT_CALC_EPS ) { *point_para = start_pos + ( start_vel + steady_vel ) * ts / 2 + steady_vel * ( sampling_time - ts ); *( point_para + 1 ) = steady_vel; *( point_para + 2 ) = 0; } else if( sampling_time < ts + tm + te + MCT_CALC_EPS ) //减速区 { *point_para = start_pos + ( start_vel - steady_vel ) * ts / 2 + steady_vel * sampling_time+ae * ( sampling_time - ts - tm ) * ( sampling_time - ts - tm ) / 2; *( point_para + 1 ) = steady_vel + ae * ( sampling_time - ts - tm ); *( point_para + 2 ) = ae; } STEP 3 若出错,返回相应的代码,插补停止。否则返回插补结果(位置、速度、加
- 28 -

//减速时间 //匀速时间 //调整后加速区加速度 //调整后减速区加速度

steady_vel = ( 2 * l - start_vel * ts ) / ( ts + 2 * tm + te ); //调整后的匀速阶段的速度

STEP 2 根据不同的运动阶段代入相应的运动公式计算当前时刻的位移、速度、加速 //加速区

//匀速区

厦门大学硕士论文

第四章 运动轨迹规划算法实现

速度) 。 当运动完成时,返回-900 以表示插补结束。 if( sampling_time> ts + tm + te - MCT_CALC_EPS ) error = -900; 同时,浮点型指针*point_para 存储了该插补点的运动参数,函数返回时通过该形 参将数据带回,供轨迹规划函数使用。 4.2 4.2.1 直线轨迹规划的算法实现 一维坐标直线运动轨迹插补函数 line_move_1。

◆函数说明 函数原型: int line_move_1 ( int axis, float start_pos, float end_pos, float start_vel, float steady_vel, float end_vel, float allow_accel, float sampling_period, int pattern, float sampling_time, float*point_para ) 功能: 一维坐标直线运动轨迹插补函数。 返回值: 出错代码 MCT_NO_ERROR MCT_INVALID_AXIS MCT_MOTION_AFTER MCT_ZERO_VELOCITY MCT_ZERO_DISPLACEMENT 输入参数: axis, start_pos,end_pos,start_vel ,steady_vel,end_vel, allow_accel ,sampling_period ,pattern,sampling_time 。 输出参数: *point_para 应用说明: axis 必须大于等于 0 并且不能大于系统初始化的轴数, 否则程序返回出错代码 MCT_INVALID_AXIS。同时,axis 对应的插补运动坐标轴已设置了基本控制参数,否 则程序返回出错代码 MCT_AXIS_UNMAP。steady_vel 不能等于零,否则程序返回出 错代码 MCT_ZERO_VELOCITY。allow_accel 不能等于零,否则程序返回出错代码 MCT_ZERO_ACCELERATION。start_pos 与 end_pos 之间的距离不能等于零,否则程 序返回出错代码 MCT_ZERO_DISPLACEMENT,sampling_period 不能等于零,否则 程序返回出错代码 MCT_ZERO_SAMPLINGPERIOD, pattern 必须是上面定义的模式,
- 29 -

MCT_AXIS_UNMAP MCT_MOTION_BEFORE MCT_LASTVEL_TOOHIGH MCT_ZERO_ACCELERATION MCT_ZERO_SAMPLINGPERIOD

厦门大学硕士论文

第四章 运动轨迹规划算法实现

否则程序使用系统默认的抛物线双 S 曲线轨迹规划模式。 警告 : (start_vel * start_vel)/ allow_accel 要 适 当 , 不 能 太 大 , 否 则 程 序 返 回 MCT_LASTVEL_TOOHIGH,插补停止。 ◆ 流程图

图 4-2-1 一维坐标直线运动算法流程图 ◆ 具体算法 STEP 1:输入进行插补运算所需参数,插补运动的坐标轴,起点位置绝对坐标,终点 位置绝对坐标,起点速度,最大允许速度,终点速度,最大允许加速度,采样周期, 轨迹规划模式,插补点时间。 STEP 2:产生一系列动态变量。 int error; float pi; float disp;
- 30 -

厦门大学硕士论文

第四章 运动轨迹规划算法实现

float disv; float max_accel; float max_velocity; STEP 3:判断插补轴号是否有效、是否被影射。若出错返回错误代码 if( axis >= 0 && axis < mct_max_axis ) { /* Check if the coordinate axis is mapped? */ if( ( mct_axis_chnls + axis )->axis_map == MCT_AXIS_MAP ) {…} else error = MCT_AXIS_UNMAP; } else error = MCT_INVALID_AXIS; return error; STEP 4:准备插补用的距离、速度、加速度值。 sampling_period = (float)fabs( sampling_period ); disp = (float)fabs( end_pos - start_pos ); steady_vel = (float)fabs( steady_vel ); allow_accel = (float)fabs( allow_accel ); max_accel = ( mct_axis_chnls + axis )->axis_max_accel; allow_accel = max_accel; end_vel = (float)fabs( end_vel ); STEP 5:根据轨迹规划模式计算分配各段的速度、加速度值后代入公共插补函数 *trajectory_interpolation[pattern] 计算该插补周期内的位置、速度和加速度并通过数组 point_para 是返回插补点参数(位置、速度、加速度) 。 国产 MCT8000 运动控制卡的轨迹规划器支持匀加减速、三角函数双 S 加减速和 抛物线双 S 加减速三种加减速控制方式,参见下图。由图 4-2-2 可知,在初速度、最 大加速度和最大速度相同的情况下,匀加减速控制方式在开始加减速和加减速结束处 速度有突变。这对机械部分是不利的,易引起抖动和振动;三角函数双 S 加减速和抛 物线双 S 加减速控制方式不存在速度突变,过渡平稳圆滑。抛物线双 S 加减速控制方 式比三角函数双 S 加减速控制方式要更平稳圆滑。
- 31 -

厦门大学硕士论文

第四章 运动轨迹规划算法实现

图 4-2-2

MCT8000 采用的加-速控制规律

T = 匀加-减速;S = 正弦加-减速;P = 抛物线加-减速 此模块的程序流程图如下:

图 4-2-3 加减速控制方式算法流程图 代码实现如下:
- 32 -

厦门大学硕士论文

第四章 运动轨迹规划算法实现

switch( pattern ) { /*梯形加减速时速度分配*/ case MCT_PLANPATH_LADDER: disv = (float)fabs( ( steady_vel * steady_vel – start_vel * start_vel )/ ( 2 * allow_accel ) ); if( end_vel <= MCT_CALC_EPS ) disv += (float)fabs( steady_vel * steady_vel / ( 2 * allow_accel ) ); if( disv > disp ) { disv = (float)sqrt( start_vel * start_vel + 2 * allow_accel * disp ); if( end_vel <= MCT_CALC_EPS ) steady_vel = disv / (float)sqrt( 2 ); else if( disv > steady_vel ) steady_vel = (float)sqrt( start_vel * start_vel – 2 * allow_accel * disp ); } break; /*三角函数双 S 加减速时速度分配*/ case MCT_PLANPATH_TRIGONAL: disv = (float)fabs( pi * ( steady_vel * steady_vel - start_vel * start_vel ) / ( 4 * allow_accel ) ); if( end_vel <= MCT_CALC_EPS ) disv += (float)fabs( pi * steady_vel * steady_vel / ( 4 * allow_accel ) ); if( disv > disp ) { disv = (float)sqrt( start_vel * start_vel + 4 * allow_accel * disp / pi ); if( end_vel <= MCT_CALC_EPS ) steady_vel = disv / (float)sqrt( 2 ); else if( disv > steady_vel ) steady_vel = (float)sqrt( start_vel * start_vel – 4 * allow_accel * disp / pi );
- 33 -

厦门大学硕士论文

第四章 运动轨迹规划算法实现

} break; /*抛物线加减速时的速度分配*/ default: pattern = MCT_PLANPATH_PARABOLA; disv = (float)fabs( ( steady_vel * steady_vel – start_vel * start_vel ) / allow_accel ); if( end_vel <= MCT_CALC_EPS ) disv += (float)fabs( steady_vel * steady_vel / allow_accel ); if( disv > disp ) { disv = (float)sqrt( start_vel * start_vel + allow_accel * disp ); if( end_vel <= MCT_CALC_EPS ) steady_vel = disv / (float)sqrt( 2 ); else if( disv > steady_vel ) steady_vel = (float)sqrt( start_vel * start_vel – allow_accel * disp ); } } /*代入公共插补函数*/ error = trajectory_interpolation ( start_pos, end_pos,(float)fabs( start_vel ), steady_vel, end_vel, allow_accel, sampling_period, sampling_time, point_para ); 其中 point_para 是返回插补点参数(位置、速度、加速度)的数组。 由于是一维坐标直线运动轨迹插补,所以 point_para 直接返回,无需再作处理。 STEP 5:返回采样点的运动参数及错误代码。 4.2.2 n 维坐标直线运动轨迹插补函数 line_move_n。 ◆ 函数说明 函数原型: int line_move_n ( int n_axis, int *axis, float *start_pos, float *end_pos, float start_vel, float steady_vel, float end_vel, float allow_accel, float sampling_period, int pattern, float sampling_time, float *point_para ) 功能: n 维坐标直线运动轨迹插补函数。 返回值:出错代码:
- 34 -

厦门大学硕士论文

第四章 运动轨迹规划算法实现

MCT_NO_ERROR 或 0:成功返回 MCT_AXIS_UNMAP 或-100:轴没有被映射 MCT_INVALID_AXIS 或-300:系统不存在的轴 MCT_MOTION_BEFORE 或-1000:采样点在运动起始点之前 MCT_MOTION_AFTER 或-900:采样点在运动结束点之后 MCT_LASTVEL_TOOHIGH 或-1200:起始点速度太大或位移太小 MCT_ZERO_VELOCITY 或-1400:最大稳定速度为 0 MCT_ZERO_ACCELERATION 或-1300:最大加速度为 0 MCT_ZERO_DISPLACEMENT 或-1500:起点与终点重合 MCT_ZERO_SAMPLINGPERIOD 或-1600:采样周期为 0 输入参数: int n_axis int *axis float *start_pos float *end_pos float start_vel float steady_vel float end_vel float allow_accel float sampling_period int pattern float sampling_time 输出参数: float *point_para 应用说明: axis 各坐标轴必须大于等于 0 并且不能大于系统初始化的轴数, 否则程序返回出 错代码 MCT_INVALID_AXIS。同时,axis 各坐标轴对应的插补运动坐标轴已设置了 基本控制参数,否则程序返回出错代码 MCT_AXIS_UNMAP。steady_vel 不能等于零, 否则程序返回出错代码 MCT_ZERO_VELOCITY。allow_accel 不能等于零,否则程序 返回出错代码 MCT_ZERO_ACCELERATION。 start_pos 与 end_pos 之间的距离不能等 于零,否则程序返回出错代码 MCT_ZERO_DISPLACEMENT,sampling_period 不能 等于零,否则程序返回出错代码 MCT_ZERO_SAMPLINGPERIOD,pattern 必须是上 面定义的模式,否则程序使用系统默认的抛物线双 S 曲线轨迹规划模式。 警告:
- 35 -

插补运动的坐标轴数。 插补运动的各坐标轴。 起点位置各坐标轴绝对坐标。 终点位置各坐标轴绝对坐标。 起点速度。 最大允许速度。 终点速度。 最大允许加速度。 采样周期。 轨迹规划模式。 插补点时间。 返回插补点各坐标轴参数(位置、速度、加速度) 。

厦门大学硕士论文

第四章 运动轨迹规划算法实现

数组 axis、start_pos 和 end_pos 的维数不能小于 n_axis,否则程序发生致命错误。 (start_vel * start_vel )/ allow_accel 的 结 果 要 适 当 , 不 能 太 大 , 否 则 程 序 返 回 MCT_LASTVEL_TOOHIGH,插补停止。 ◆ 流程图

图 4-2-4 ◆ 具体算法

n 维坐标直线运动算法流程图

STEP 1, STEP 2 ,STEP 3 同 4.2.1 中所述。 STEP 4:准备插补用的距离、速度、加速度值。
- 36 -

厦门大学硕士论文

第四章 运动轨迹规划算法实现

以下变量的取得与 4.2.1 相同, sampling_period = (float)fabs( sampling_period ); steady_vel = (float)fabs( steady_vel ); allow_accel = (float)fabs( allow_accel ); max_accel = ( mct_axis_chnls + axis )->axis_max_accel; allow_accel = max_accel; end_vel = (float)fabs( end_vel ); 但由于是多维运动,距离的计算由函数 line_displacement 来实现: disp = line_displacement(n_axis, start_pos, end_pos ); 该函数的实现代码为: float line_displacement( int axes, float *start_pos, float *end_pos ) { int i; float displacement = 0; for( i = 0; i < axes; i++ ) displacement += ( end_pos[i] - start_pos[i] ) * ( end_pos[i] - start_pos[i] ); displacement = (float)sqrt( displacement ); return displacement; } STEP 5:同 4.2.1 中所述。 STEP 6:将 STEP 5 得到的插补后的运动参数按比例分配到各轴上。 本文通过设计函数 line_axis_value()来实现,其实现代码如下: void line_axis_value( int n_axis, float *start_pos, float *end_pos, float displacement, float *path, float *point_para ) { int i; for( i = 0; i < n_axis; i++ ) { *( point_para + 3 * i ) = start_pos[i] + path[0] * ( end_pos[i] – start_pos[i] )/ displacement; *( point_para + 3 * i + 1 ) = path[1] * ( end_pos[i] – start_pos[i] ) / displacement; *( point_para + 3 * i + 2 ) = path[2] * ( end_pos[i] – start_pos[i] ) / displacement;
- 37 -

厦门大学硕士论文

第四章 运动轨迹规划算法实现

} } STEP 6:返回采样点的运动参数及错误代码。 4.3 4.3.1 圆弧轨迹规划的算法实现 二维圆弧运动轨迹插补函数 arc_move_2D。

◆ 函数说明 函数原型: int arc_move_2D ( int *axis, float *start_pos, float *center_pos, float angle,int ccw_dir, float start_vel, float steady_vel, float end_vel, float allow_accel, float sampling_period, int pattern, float sampling_time, float *point_para ) 功能: 二维圆弧运动轨迹插补函数。 返回值: 出错代码: MCT_NO_ERROR 或 0:成功返回 MCT_AXIS_UNMAP 或-100:轴没有被映射 MCT_INVALID_AXIS 或-300:系统不存在的轴 MCT_INVALID_ARC 或-1800:圆弧半径为 0 MCT_MOTION_BEFORE 或-1000:采样点在运动起始点之前 MCT_MOTION_AFTER 或-900:采样点在运动结束点之后 MCT_ZERO_ANGLE 或-1700:旋转角度为 0 MCT_ZERO_VELOCITY 或-1400:最大稳定速度为 0 MCT_LASTVEL_TOOHIGH 或-1200:起始点速度太大或位移太小 MCT_ZERO_ACCELERATION 或-1300:最大加速度为 0 MCT_ZERO_SAMPLINGPERIOD 或-1600:采样周期为 0 输入参数: int *axis float *start_pos float *center_pos float angle int ccw_dir float start_vel float steady_vel float end_vel 运动轴数组 起点位置 圆心位置 旋转角度 旋转方向 起始速度 稳定速度 结束点速度
- 38 -

厦门大学硕士论文

第四章 运动轨迹规划算法实现

float allow_accel float sampling_period int pattern float sampling_time 输出参数: *point_para 应用说明:

最大加速度 采样周期 轨迹规划模式。 采样时间 采样点数据

axis 数组中前 2 个元素的任何一个元素的值不能大于等于系统硬件最大轴数 MCT_MAX_AXIS,同时,axis 数组中前 2 个元素的值不能相等,否则程序返回出错 代码 MCT_INVALID_AXIS。axis 数组中前 2 个元素所代表的轴必须映射过,否则返 回 MCT_AXIS_UNMAP;start_pos 点到 center_pos 点的距离(半径)不能为零,否则 程序返回出错代码 MCT_INVALID_ARC。steady_vel 不能等于零,否则程序返回出错 代码 MCT_ZERO_VELOCITY。allow_accel 不能等于零,否则程序返回出错代码 MCT_ZERO_ACCELERATION。angle 不能为 0,否则返回 MCT_ZERO_ANGLE; start_vel 不能太大,否则返回 MCT_LASTVEL_TOOHIGH;ccw_dir 的取值为任何整 数范围的值,在处理时 ccw_dir != 0,为逆时针旋转;否则圆为顺时针旋转; sampling_period 不能为 0,否则返回 MCT_ZERO_SAMPLINGPERIOD。 警告: 数组 axis 的维数不能小于 2,否则程序发生致命错误。 ( start_vel * start_vel ) / allow_accel 要 适 当 , 不 能 太 大 , 否 则 程 序 返 回 MCT_LASTVEL_TOOHIGH,插补停止。 ◆ 流程图

- 39 -

厦门大学硕士论文

第四章 运动轨迹规划算法实现

图 4-3-1 ◆ 具体算法

二维圆弧运动算法流程图

STEP 1:输入进行插补运算所需参数,插补运动的坐标轴,起点位置绝对坐标,圆心 坐标,旋转角度,旋转方向,起点速度,最大允许速度,终点速度,最大允许加速度, 采样周期,轨迹规划模式,插补点时间。 STEP 2:产生一系列动态变量。 int error; float pi; float disp; float disv; float radio; float axis_x; \\错误编号 \\3.1415926 \\插补曲线的长度 \\分配速度加速度用 \\采样点的矢径长 \\采样点的 x 坐标
- 40 -

厦门大学硕士论文

第四章 运动轨迹规划算法实现

float axis_y; float value_cos; float value_sin; float max_accel;

\\采样点的 y 坐标 \\记录插补点的与起始点对应中心角的余弦值 \\记录插补点的与起始点对应中心角的正弦值 \\最大允许加速度 //用来记录插补点的运动参数

float max_velocity; \\最大允许速度 float path[3] = { 0, 0, 0 }; STEP 3 同 4.2.1 中所述。 STEP 4:准备插补用的距离、速度、加速度值。 以下变量的取得与 4.2.1 相同, sampling_period = (float)fabs( sampling_period ); steady_vel = (float)fabs( steady_vel ); allow_accel = (float)fabs( allow_accel ); max_accel = ( mct_axis_chnls + axis )->axis_max_accel; allow_accel = max_accel; end_vel = (float)fabs( end_vel ); 但由于是圆弧运动,距离的计算由圆心角与半径的乘积来得到: disp = (float)fabs( angle ) * radio; STEP 5:同 4.2.1 中所述。 STEP 6:将 STEP 5 中插补得到的数据进行齐次变换,得到采样点的运动参数。 axis_x = start_pos[0] - center_pos[0]; axis_y = start_pos[1] - center_pos[1]; value_cos = (float)cos( path[0] / radio ); value_sin = (float)sin( path[0] / radio ); *( point_para ) = center_pos[0] + axis_x * value_cos + axis_y * value_sin; *( point_para + 1 ) = ( axis_y * value_cos - axis_x * value_sin ) * path[1] / radio; *( point_para + 2 ) = ( axis_y * value_cos - axis_x * value_sin ) * path[2] / radio ( axis_x * value_cos + axis_y * value_sin ) * path[1] * path[1] / ( radio * radio ); *( point_para + 3 ) = center_pos[1] - axis_x * value_sin + axis_y * value_cos; *( point_para + 4 ) = -( axis_x * value_cos + axis_y * value_sin ) * path[1] / radio; *( point_para + 5 ) = -( axis_x * value_cos + axis_y * value_sin ) * path[2] / radio ( axis_x *value_sin + axis_y * value_cos ) * path[1] * path[1] / ( radio * radio ); STEP 7:返回采样点的运动参数及错误代码。 4.3.2 三维圆弧运动轨迹插补函数 arc_move_3D。
- 41 -

/*以上变量均为局部变量*/

厦门大学硕士论文

第四章 运动轨迹规划算法实现

◆ 函数说明 函数原型: int arc_move_3D ( int *axis, float *start_pos, float *center_pos, float *vector, float angle, int ccw_dir, float start_vel, float steady_vel,float end_vel, float allow_accel, float sampling_period, int pattern,float sampling_time, float *point_para ); 功能: 三维圆弧运动轨迹插补函数。 返回值: 出错代码: MCT_NO_ERROR 或 0:成功返回 MCT_AXIS_UNMAP 或-100:轴没有被映射 MCT_INVALID_AXIS 或-300:系统不存在的轴 MCT_INVALID_ARC 或-1800:圆弧半径为 0 MCT_INVALID_VECTOR 或 -1900:法向量不垂直圆弧面 MCT_MOTION_BEFORE 或-1000:采样点在运动起始点之前 MCT_MOTION_AFTER 或-900:采样点在运动结束点之后 MCT_ZERO_ANGLE 或-1700:旋转角度为 0 MCT_ZERO_VECTOR 或-2000:法向量为 0 MCT_ZERO_VELOCITY 或-1400:最大稳定速度为 0 MCT_LASTVEL_TOOHIGH 或-1200:起始点速度太大或位移太小 MCT_ZERO_ACCELERATION 或-1300:最大加速度为 0 MCT_ZERO_SAMPLINGPERIOD 或-1600:采样周期为 0 输入参数: int *axis float *start_pos float *center_pos float angle int ccw_dir float start_vel float steady_vel float end_vel float allow_accel float sampling_period int pattern float sampling_time 运动轴数组 起点位置 圆心位置 旋转角度 旋转方向 起始速度 稳定速度 结束点速度 最大加速度 采样周期 轨迹规划模式 采样时间
- 42 -

厦门大学硕士论文

第四章 运动轨迹规划算法实现

输出参数: float *point_para 应用说明: axis 数组中前 3 个元素的任何一个元素的值不能大于等于系统硬件最大轴数 MCT_MAX_AXIS,同时,axis 数组中前 3 个元素的任何两个元素的值不能相等,否 则程序返回出错代码 MCT_INVALID_AXIS。axis 数组中前 3 个元素所代表的轴必须 映射过,否则返回 MCT_AXIS_UNMAP;start_pos 点到 center_pos 点的距离(半径) 不能为零,否则程序返回出错代码 MCT_INVALID_ARC。steady_vel 不能等于零,否 则程序返回出错代码 MCT_ZERO_VELOCITY。allow_accel 不能等于零,否则程序返 回 出 错 代 码 MCT_ZERO_ACCELERATION 。 angle 不 能 为 0 , 否 则 返 回 MCT_ZERO_ANGLE;start_vel 不能太大,否则返回 MCT_LASTVEL_TOOHIGH; ccw_dir 的取值为任何整数范围的值,在处理时 ccw_dir != 0,为逆时针旋转;否则圆 为顺时针旋转; sampling_period 不能为 0, 否则返回 MCT_ZERO_SAMPLINGPERIOD。 vector 不能为零,否则返回 MCT_ZERO_VECTOR ,同时必须与圆心到当前点的向量 垂直,否则程序返回出错代码 MCT_INVALID_VECTOR。 警告: 数组 axis、start_pos、center_pos 和 vector 的维数不能小于 3,否则程序发生致命 错 误 。 start_vel * start_vel ) / allow_accel 要 适 当 , 不 能 太 大 , 否 则 程 序 返 回 ( MCT_LASTVEL_TOOHIGH,插补停止。 ◆ 流程图 如图 4-3-1 所示。 ◆ 具体算法 STEP 1:同 4.3.1; STEP 2:产生一系列动态变量。较之 4.3.1,多产生以下变量: float axis_z; float matrix[6] = { 0, 0, 0, 0, 0, 0 }; STEP 3,STEP 4 ,STEP 5 同 4.3.1 中所述。 STEP 6:将 STEP 5 中插补得到的数据进行齐次变换,得到采样点的运动参数。 axis_x = start_pos[0] - center_pos[0]; axis_y = start_pos[1] - center_pos[1]; axis_z = start_pos[2] - center_pos[2]; value_cos = (float)cos( path[0] / radio ); value_sin = (float)sin( path[0] / radio ); matrix[0] = axis_x * ( vector[0] * vector[0] * ( 1 - value_cos ) + value_cos ) +
- 43 -

采样点数据

厦门大学硕士论文

第四章 运动轨迹规划算法实现

axis_y * ( vector[0] * vector[1] * ( 1 - value_cos ) + vector[2] * value_sin ) + axis_z * ( vector[0] * vector[2] * ( 1 - value_cos ) - vector[1] * value_sin ); matrix[1] = axis_x * ( vector[0] * vector[0] * ( 1 + value_sin ) - value_sin ) + axis_y * ( vector[0] * vector[1] * ( 1 + value_sin ) + vector[2] * value_cos ) + axis_z * ( vector[0] * vector[2] * ( 1 + value_sin ) - vector[1] * value_cos ); matrix[2] = axis_x * ( vector[0] * vector[1] * ( 1 - value_cos ) - vector[2] * value_sin ) +axis_y * ( vector[1] * vector[1] * ( 1 - value_cos ) + value_cos ) + axis_z * ( vector[1] * vector[2] * ( 1 - value_cos ) + vector[0] * value_sin ); matrix[3] = axis_x * ( vector[0] * vector[1] * ( 1 + value_sin ) - vector[2] * value_cos ) +axis_y * ( vector[1] * vector[1] * ( 1 + value_sin ) - value_sin ) + axis_z * ( vector[1] * vector[2] * ( 1 + value_sin ) + vector[0] * value_cos ); matrix[4] = axis_x * ( vector[0] * vector[2] * ( 1 - value_cos ) + vector[1] * value_sin ) +axis_y * ( vector[1] * vector[2] * ( 1 - value_cos ) - vector[0] * value_sin ) + axis_z * ( vector[2] * vector[2] * ( 1 - value_cos ) + value_cos ); matrix[5] = axis_x * ( vector[0] * vector[2] * ( 1 + value_sin ) + vector[1] * value_cos ) +axis_y * ( vector[1] * vector[2] * ( 1 + value_sin ) - vector[0] * value_cos ) + axis_z * ( vector[2] * vector[2] * ( 1 + value_sin ) - value_sin ); *( point_para ) = center_pos[0] + matrix[0]; *( point_para + 1 ) = matrix[1] * path[1] / radio; *( point_para + 2 ) = matrix[1] * path[2] / radio - matrix[0] * path[1] *path[1] / ( radio * radio ); *( point_para + 3 ) = center_pos[1] + matrix[2]; *( point_para + 4 ) = matrix[3] * path[1] / radio; *( point_para + 5 ) = matrix[3] * path[2] / radio - matrix[2] * path[1] * path[1] / ( radio * radio ); *( point_para + 6 ) = center_pos[2] + matrix[4]; *( point_para + 7 ) = matrix[5] * path[1] / radio; *( point_para + 8 ) = matrix[5] * path[2] / radio - matrix[4] * path[1] * path[1] / ( radio * radio ); STEP 7:返回采样点的运动参数及错误代码。 4.4 4.4.1 椭圆轨迹规划的算法实现 二维椭圆弧运动轨迹插补函数 ellipse_move_2D。

◆ 函数说明 函数原型:
- 44 -

厦门大学硕士论文

第四章 运动轨迹规划算法实现

int ellipse_move_2D( int *axes, float *start_pos, float *center_pos, float a,float b, float angle, int ccw_dir, float start_vel, float steady_vel, float end_vel, float allow_accel, float sampling_period, int pattern,float sampling_time, float *point_para ) 功能: 椭圆二维坐标轨迹规划. 返回值:错误代码 MCT_NO_ERROR MCT_AXIS_UNMAP MCT_INVALID_AXIS MCT_INVALID_ARC MCT_MOTION_BEFORE MCT_ZERO_ANGLE MCT_ZERO_VELOCITY MCT_LASTVEL_TOOHIGH MCT_ZERO_ACCELERATION 或 0:成功返回 或-100:轴没有被映射 或-300:系统不存在的轴 或-1800:圆弧半径为 0 或-1000:采样点在运动起始点之前 或-1700:旋转角度为 0 或-1400:最大稳定速度为 0 -> the displacement is too closing 或-1300:最大加速度为 0 -3000 长轴或短轴参数不正确 输入参数: float *center_pos float float *axes 输出参数: *point_para 应用说明: axes 数组中前 2 个元素的任何一个元素的值不能大于等于系统硬件最大轴数 MCT_MAX_AXIS,同时,axes 数组中前 2 个元素的值不能相等,否则程序返回出错 代码 MCT_INVALID_AXIS。axes 数组中前 2 个元素所代表的轴必须映射过,否则返 回 MCT_AXIS_UNMAP;现行位置点到 center_pos 点的距离(半径)不能为零,否则 程序返回出错代码 MCT_INVALID_ARC,并且不能大于长半轴,也不能小于短半轴, 否 则 返 回 错 误 码 -3000 。 vel 不 能 等 于 零 , 否 则 程 序 返 回 出 错 代 码 MCT_ZERO_VELOCITY 。 accel 不 能 等 于 零 , 否 则 程 序 返 回 出 错 代 码 MCT_ZERO_ACCELERATION 。 rot_angle 不 能 为 0 , 否 则 程 序 返 回 出 错 代 码
- 45 -

MCT_ZERO_SAMPLINGPERIOD 或-1600:采样周期为 0

-> 椭圆中心坐标 -> 长半轴 -> 短半轴 ccw_dir steady_vel allow_accel pattern

a b angle

另外还有

采样点数据

厦门大学硕士论文

第四章 运动轨迹规划算法实现

MCT_ZERO_ANGLE。dir 的取值为任何整数范围的值,在处理时 dir != 0,为逆时针 旋转;否则椭圆为顺时针旋转。 ◆ 流程图:

图 4-4-1

二维椭圆弧运动算法流程图

◆ 算法实现: STEP 1:同 4.3.1,只是多了椭圆的长短轴两个参数; STEP 2:产生一系列动态变量。多产生一些变量用来记录椭圆的特征; STEP 3:判断轴参数是否有效,同 4.3.1 中所述; STEP 4:将椭圆起始位置和终止位置的特征数据记录下来后,转变成以长轴为直径的 圆的插补,出错则返回相应的错误号并终止插补。 STEP 5:将 STEP 4 中得到的插补数据按比例压缩到椭圆弧上,得到采样点的运动参
- 46 -

厦门大学硕士论文

第四章 运动轨迹规划算法实现

数; STEP 6:返回采样点的运动参数及错误代码。 4.4.2 三维椭圆弧运动轨迹插补函数 ellipse_move_3D。 ◆ 函数说明 函数原型: int ellipse_move_3D( int *axes, float *start_pos, float *center_pos, float *vector,float a,float b, float angle, int ccw_dir, float start_vel, float steady_vel, float end_vel, float allow_accel, float sampling_period, int pattern,float sampling_time, float *point_para ) 功能 : 三维椭圆轨迹(从现行位置开始)规划. 返回值:错误代码 MCT_NO_ERROR MCT_AXIS_UNMAP MCT_INVALID_AXIS MCT_INVALID_ARC MCT_INVALID_VECTOR MCT_MOTION_BEFORE MCT_ZERO_ANGLE MCT_ZERO_VECTOR MCT_ZERO_VELOCITY MCT_ZERO_ACCELERATION 输入参数: *axis *center_pos *vector (椭圆圆弧所在平面的法向向量) b(椭圆短半轴) allow_accel 采样点数据 angle pattern ccw_dir 或 0:成功返回 或-100:轴没有被映射 或-300:系统不存在的轴 或-1800:圆弧半径为 0 或 –1900:法向量不垂直圆弧面 或-1000:采样点在运动起始点之前 或-1700:旋转角度为 0 或-2000:法向量为 0 或-1400:最大稳定速度为 0 或-1600:采样周期为 0 -3000 长轴或短轴参数不正确

a(椭圆长半轴) steady_vel 输出参数: *point_para 应用说明:

axes 数组中前 2 个元素的任何一个元素的值不能大于等于系统硬件最大轴数 MCT_MAX_AXIS,同时,axes 数组中前 2 个元素的值不能相等,否则程序返回出错 代码 MCT_INVALID_AXIS。axes 数组中前 2 个元素所代表的轴必须映射过,否则返 回 MCT_AXIS_UNMAP;现行位置点到 center_pos 点的距离(半径)不能为零,否则 程序返回出错代码 MCT_INVALID_ARC, 并且不能大于长半轴,也不能小于短半轴,
- 47 -

厦门大学硕士论文

第四章 运动轨迹规划算法实现

否 则 返 回 错 误 码 -3000 。 vel 不 能 等 于 零 , 否 则 程 序 返 回 出 错 代 码 MCT_ZERO_VELOCITY 。 accel 不 能 等 于 零 , 否 则 程 序 返 回 出 错 代 码 MCT_ZERO_ACCELERATION 。 rot_angle 不 能 为 0 , 否 则 程 序 返 回 出 错 代 码 MCT_ZERO_ANGLE。dir 的取值为任何整数范围的值,在处理时 dir != 0,为逆时针 旋转;否则圆为顺时针旋转。 ◆ 流程图: 如图 4-4-1。 ◆ 具体算法 STEP 1:同 4.4.1,除了椭圆的长短轴外,还多了一个垂直于空间椭圆的法向向量; STEP 2, STEP 3,STEP 4 同 4.4.1, STEP 5:将 STEP 4 中得到的插补数据按比例压缩到椭圆弧上,得到平面状态时采样 点的运动参数; STEP 6:将 STEP 5 中插补得到的数据进行空间的齐次变换,得到空间采样点的运动 参数。 STEP 7:返回采样点的运动参数及错误代码。 4.5 Spline 线轨迹规划的算法实现 三次样条曲线是根据用户输入的一系列离散点而形成的,由于其特殊性,所以对 应的算法实现也作了相应的改动,具体流程和算法如下: ◆ 流程图:

图 4-5-1

Spline 曲线运动算法流程图
- 48 -

厦门大学硕士论文

第四章 运动轨迹规划算法实现

◆ 算法实现: STEP 1 构造相应的三次样条曲线; 该步骤由函数 SplinePara()实现,其原型为: int SplinePara(int segCount,int style,float *fx,float *points, float *h, float *m); 1) 初始化一系列变量; int error=0; int i; double *D = (double *)malloc(sizeof(double)*segCount); double *d = (double *)malloc(sizeof(double)*segCount); double *lamda = (double *)malloc(sizeof(double)*segCount); double *rou = (double *)malloc(sizeof(double)*segCount); double *p = (double *)malloc(sizeof(double)*segCount); double *q = (double *)malloc(sizeof(double)*segCount); double *z = (double *)malloc(sizeof(double)*segCount); double *b = (double *)malloc(sizeof(double)*segCount); 2) 计算参数数组 h[i],d[i] ,b[i],lamda[i],rou[i],D[i]; for(i=0;i<segCount;i++) { h[i]=points[2*(i+1)]-points[2*i]; d[i]=(points[2*(i+1)+1]-points[2*i+1])/h[i]; b[i]=TWO; if(i>0) { lamda[i]=h[i]/(h[i-1]+h[i]); rou[i]=1-lamda[i]; D[i]=6*(d[i]-d[i-1])/(h[i-1]+h[i]); } } 3)根据样条类型用追赶法计算 m[i]; switch(style) { case 0: m[0]=0.0; m[segCount]=0.0;
- 49 -

//h[i],i->[0,n-1],共 n 个; //d[i],i->[0,n-1],共 n 个; //b[i],i->[0,n-1],共 n 个;

//i->[1,n-1],共 n-1 个; //i->[1,n-1],共 n-1 个; //i->[1,n-1],共 n-1 个;

//为自然样条时

厦门大学硕士论文

第四章 运动轨迹规划算法实现

i=1; D[i]=6*(d[i]-d[i-1])/(h[i-1]+h[i])-rou[i]*m[i-1]; i=segCount-1; D[i]=6*(d[i]-d[i-1])/(h[i-1]+h[i])-lamda[i]*m[i+1]; break; case 1: m[0]=*fx; m[segCount]=*(fx+1); i=1; D[i]=6*(d[i]-d[i-1])/(h[i-1]+h[i])-rou[i]*m[i-1]; i=segCount-1; D[i]=6*(d[i]-d[i-1])/(h[i-1]+h[i])-lamda[i]*m[i+1]; break; case 2: i=1; b[i]=TWO-rou[i]/2; D[i]=6*(d[i]-d[i-1])/(h[i-1]+h[i])-3*rou[i]*(d[i-1]-*fx)/h[i-1]; i=segCount-1; b[i]=TWO-lamda[i]/2; D[i]=6*(d[i]-d[i-1])/(h[i-1]+h[i])-3*lamda[i]*(*(fx+1)-d[i])/h[i]; break; case 3: i=1; b[i]=TWO+rou[i]; i=segCount-1; b[i]=TWO+lamda[i]; break; default: return -100; break; } for(i=1;i<segCount;i++) { if(I ==1) p[i]=b[i];
- 50 -

//为端点曲率调整样条时

//为压紧三次样条时

//为抛物线终结样条时

//-100 –>没有选择样条种类

厦门大学硕士论文

第四章 运动轨迹规划算法实现

else q[i]=lamda[i]/p[i]; }

p[i]=b[i]-rou[i]*q[i-1];

for(i=1;i<segCount;i++) { if(i==1) else } m[segCount-1]=z[segCount-1]; for(i=segCount-2;i>0;i--) { m[i]=z[i]-q[i]*m[i+1]; } switch(style) { case 0:break; case 1:break; case 2: //自然样条 //端点曲率调整样条 //压紧三次样条 //得到 m[i]的值 z[i]=D[i]/p[i]; z[i]=(D[i]-rou[i]*z[i-1])/p[i];

m[0]=3*(d[0]-*fx)/h[0]-m[1]/2; m[segCount]=3*(*(fx+1)-d[segCount-1])/h[segCount-1]-m[segCount-1]/2; break; case 3: m[0]=m[1]; m[segCount]=m[segCount-1]; break; default:break; } 4)输出 m[i]; STEP 2 根据给定的插补精度计算横坐标的进给量; 该步骤由函数 dx_next()实现,其实现代码为: float dx_next(int segCount,float x,float *points,float *h,float *m,float precision) { float yd1,dx; int segNum=0;
- 51 -

//抛物线终结样条

厦门大学硕士论文

第四章 运动轨迹规划算法实现

int k; float Sk0,Sk1,Sk2,Sk3,dk,w; while(x>=points[2*segNum]) { segNum++; if(segNum>segCount) { } k=segNum-1; dk=(points[2*(k+1)+1]-points[2*k+1])/h[k]; Sk0=points[2*k+1]; Sk1=dk-h[k]*(2*m[k]+m[k+1])/6; Sk2=m[k]/2; Sk3=(m[k+1]-m[k])/(6*h[k]); w=x-points[2*k]; yd1=3*Sk3*w*w+2*Sk2*w+Sk1; dx=1/sqrt(1+yd1*yd1)*precision; return dx; } STEP 3 由三次样条曲线的方程得到纵坐标的当前值。 该步骤由函数 spline_move(()实现,其实现代码为: int { float y; int segNum=0; int k; float Sk0,Sk1,Sk2,Sk3,dk,w; if(x-points[2*segCount]>0.0001) while(x>=points[2*segNum]) { segNum++; if(segNum>segCount) { }
- 52 -

segNum-=1;

break;

}

spline_move(int segCount,float x,float *points,float *h,float *m,float *point_para)

return -900;

segNum-=1;

break;

}

厦门大学硕士论文

第四章 运动轨迹规划算法实现

k=segNum-1; dk=(points[2*(k+1)+1]-points[2*k+1])/h[k]; Sk0=points[2*k+1]; Sk1=dk-h[k]*(2*m[k]+m[k+1])/6; Sk2=m[k]/2; Sk3=(m[k+1]-m[k])/(6*h[k]); w=x-points[2*k]; y=((Sk3*w+Sk2)*w+Sk1)*w+points[2*k+1]; *point_para=x; *(point_para+3)=y; return 0; } STEP 4 返回采样点的坐标参数。 4.6 小结 本章节的工作是建立在前一章的基础之上的。将其所阐述的各种曲线的算法模型 用标准的 C 语言程序实现, 并根据 DSP 的特点进行了优化, 形成了一系列基本运动函 数,可以直接在 DSP 的中断程序中调用并完成实时的插补计算。下边是所建立的新型 轨迹规划器的主要函数体: ◆ 一维直线轨迹规划函数:line_move_1 ; ◆ n 维直线轨迹规划函数:line_move_n ; ◆ 二维圆弧轨迹规划函数:arc_move_2D ; ◆ 三维圆弧轨迹规划函数:arc_move_3D ; ◆ 二维椭圆弧轨迹规划函数:ellipse_move_2D ; ◆ 三维椭圆弧轨迹规划函数:ellipse_move_3D ; ◆ 三次样条曲线的轨迹规划函数组:SplinePara(),dx_next(),spline_move() 。 有了这一系列轨迹规划函数以及由它们相互组合而形成的混合运动函数,轨迹规划器 就可以完成平面或空间的各种图形的运动轨迹规划,从而为复杂数控系统和多自由度 机器手的操作奠定了基础。

- 53 -

厦门大学硕士论文

第五章 运动控制算法

第五章:运动控制算法
PID 控制是最早发展起来的控制策略之一,由于其控制算法简单、鲁棒性好和可 靠性高,被广泛应用于各种工业过程控制,但是仅仅依靠位置式或增量式 PID 控制器 有时还不能满足系统高速高精度和高稳定性的要求,所以本章又从速度和加速度前馈 角度对控制算法作了必要的改进 。 5.1 常规数字 PID 控制 数字 PID 是在生产过程中发展起来的一种普遍采用的控制方法,在冶金、机械、 化工和自动控制等诸多领域里都有广泛应用。 5.1.1 PID 控制原理 在模拟控制系统中,PID 控制是最常用的控制规律,其原理由下图所示:

图 5-1-1 模拟 PID 控制系统原理图 此处 PID 控制器是一种线形控制器, 它根据给定值 r(t)和实际输出值 c(t)构成控制偏差:

e(t ) = r (t ) c(t )
为:

(5-1-1-1)

将偏差的比例、积分、微分通过线形组合构成控制量,对被控对象进行控制,其规律

u (t ) = K P [e(t ) +

1 TI

∫ e(t )dt +
0

t

TD de(t ) ] dt

(5-1-1-2)

或者可以写成传递函数形式:

G ( s) =

U ( s) 1 = K P (1 + + TD s ) E (s) TI s

(5-1-1-3)

其中: K P ——比例系数; TI ——积分时间常数; TD ——微分时间常数。 PID 控制器各校正环节的作用如下[62]: ◆ 比例环节:即时成比例的反映控制系统的偏差信号 e(t ) ,偏差一旦产生,控 制器立即产生控制作用,以减少偏差,但比例控制不能消除稳态误差, K P 的 增大会引起系统的不稳定。 ◆ 积分环节:主要用于消除静差,提高系统的无差度。积分作用的强弱取决于 积分时间常数 TI , TI 越大,积分作用愈弱,反之则愈强。而且只要有足够的
- 54 -

厦门大学硕士论文

第五章 运动控制算法

时间,积分控制能完全消除误差,但积分作用太强会使系统超调加大,甚至 使系统出现振荡。 ◆ 微分环节:能反映偏差信号的变化趋势(变化速率) ,并能在偏差信号值变得 太大之前,在系统中引入一个有效的早期修正信号,从而加快系统的动作速 度,减少调节时间,从而改善系统的动态性能。所以微分作用可以减少超调 量,克服振荡,使系统的稳定性提高。 5.1.2 数字 PID 控制算法 在 MCT8000 系列运动控制板中,使用位置式 PID 控制算法来完成对饲服电机的 控制,增量式 PID 控制算法则用来控制步进电机作业系统。下边分别讨论此两种控制 算法。 5.1.2.1 位置式 PID 控制算法 由于计算机控制是一种采样控制,它只能根据采样时刻的偏差值计算控制量,所 以要对连续系统的控制模型进行离散化处理。具体可作如下代换:

t = kT (k = 0,1,2,...) t k ∫0 e(t )dt = T ∑ e( j ) j =0 de(t ) e(k ) e(k 1) dt = T
散的 PID 表达式:

(5-1-2-1)

T 为采样周期,必须足够小才能保证系统的精度。将上式代入(5-1-1-2)可得离

u (k ) = K P {e(k ) +


T TI

∑ e( j ) +
j =0

k

TD [e(k ) e(k 1)]} T

(5-1-2-2)

u ( k ) = K P e( k ) + K I ∑ e( j ) + K D [e( k ) e( k 1)]
j =0

k

(5-1-2-3)

其中,

k

——采样序号,k=0,1,2,….;

e(k ) ——第 k 次采样的偏差值; e(k 1) ——第(k-1)次采样的偏差值; u (k ) ——第 k 次采样时调节器的输出。
K P ——比例系数 ; K I ——积分系数, K I =

K PT

TI



- 55 -

厦门大学硕士论文

第五章 运动控制算法

K D ——微分系数, K D =
由 z 变换的性质

K P TD

T



z (e(k 1)) = z 1 E ( z )
z[∑ e( j )] =
j =0 k

E( z) (1 z 1 )

式(5-1-2-3)的 z 变换式为

U ( z) = K P E( z) + K I

E ( z) + K D [ E ( z ) z 1 E ( z )] , 1 z 1

(5-1-2-4)

其数字 PID 控制器的 z 传递函数为

G( z) =

KI U ( z) = KP + + K D [1 z 1 ] , E( z) 1 z 1

(5-1-2-5)

数字 PID 控制器的结构图如下图所示

图 5-1-2 数字 PID 控制器的结构图 由于计算机输出的 u(k)直接去控制执行机构,u(k)的值与执行机构的位置是一一 对应的,所以该算法称位置式 PID 控制算法。其控制系统示意图如下:

图 5-1-3 位置式 PID 控制系统 算法流程图:

- 56 -

厦门大学硕士论文

第五章 运动控制算法

图 5-1-4 位置式 PID 算法流程图 此种算法的输出 u(k)对应执行机构的实际位置,所以有 u(k)大幅度变化而导致执 行机构的大幅度变化(飞车) ,造成重大生产事故。所以编制控制程序时一定要加倍小 心。 5.1.2.2 增量式 PID 控制算法 当执行机构需要的是控制量的增量(例如驱动步进电机)时,可由式(5-1-2-3) 导出提供增量的 PID 控制算式。据递推原理可得:
k 1 j =0

u (k 1) = K P e(k 1) + K I ∑ e( j ) + K D [e(k 1) e(k 2)] ,
用式(5-1-2-3)与式(5-1-2-6)相减得:

(5-1-2-6)

u (k ) = K P e(k ) + K I e(k ) + K D [e(k ) e(k 1)] , 其中, e( k ) = e( k ) e( k 1) 。
式(5-1-2-7)即为增量式 PID 控制算法。其控制示意图如下:

(5-1-2-7)

图 5-1-5 增量式 PID 控制示意图
- 57 -

厦门大学硕士论文

第五章 运动控制算法

式(5-1-2-7)进一步改写为:

u (k ) = Ae(k ) Be(k 1) + Ce(k 2)
其中, A = K P (1 +

(5-1-2-8)

T TD + ) ; TI T TD ) ; T


B = K P (1 + 2 C = KP TD T

都是与采样周期、比例系数、积分时间常数以及微分时间常数有关的系数。由于一般 的计算机控制系统采用恒定的采样周期 T,所以一旦确定了 K P , K I , K D ,只要使 用前后三次测量值的偏差即可求出控制增量 u (k ) 。 算法流程图:

图 5-1-6 增量式控制算法流程图
- 58 -

厦门大学硕士论文

第五章 运动控制算法

采用该算法时,计算机输出的控制增量 u (k ) 对应的是本次执行机构位置的增 量。与位置式控制算法相比,增量式算法有如下优点: (1) 由于输出的仅仅是增量, 所以误动作时影响小, 必要时用逻辑判断的方法 去掉。 (2) 手动/自动切换时冲击小,便于实现无扰动切换。此外,当计算机发生故 障时,由于输出通道或执行机构具有锁存的作用,故仍能保持原值。 (3) 算式中不需要累加。控制增量 u (k ) 的确定,仅与最近 k 次的采样值有 关,所以较容易通过加权处理获得比较好的控制效果。 但增量式也有其不足之处:积分截断效应大,有静态误差;溢出的影响大。因此 在选择时不可一概而论,一般认为以晶闸管作为执行器或控制精度要求高的系统中采 用位置式控制算法,而在以步进电机或电动阀门作为执行器的系统中,可采用增量式 控制算法。 5.1.3 PID 参数的整定 PID 控制器虽然有诸多优点, 但其参数整定的繁琐过程一直困扰着工程技术人员, 研究 PID 参数的自整定技术具有十分重大的工程实践意义。 为了方便研究 PID 参数对 控制效果的影响, 本人用 VC6.0 和 MCT8000 自带的软件包开发了一个可手动调节 PID 参数的小程序,界面如下图所示:

图 5-1-7 手动 PID 参数调节界面图 本小节主要是研究 PID 参数的整定,所以对该界面不作详细介绍,只说明一点: 该界面可方便地在线修改轴号、采样周期 T、 K P 、 K I 、 K D 等参数。 5.1.3.1 PID 调节器参数对控制性能的影响 ◆ 比例控制参数 K P 对系统性能的影响
- 59 -

厦门大学硕士论文

第五章 运动控制算法

(1)对动态性能的影响:比例控制参数 K P 加大,使系统的动作灵敏,动作加快,

K P 偏大,振荡次数加多,调节时间加长。当 K P 太大时,系统会趋于不稳定。若 K P 太小,又会使系统的动作缓慢。下图比较了 K P 对系统动态性能的影响。 为了比较方便,将 K P 变化时整定的效果曲线罗列在一起。前 300 个周期电机轴
离开零点到一个指定的振幅(此处为 5 度)处,300 个采样周期后回归到零点位置。

K P = 0.11; 0.08; 0.05; 0.02(从上至下排列)

图 5-1-8 K P 对动态性能的影响比较图 为了将各条曲线清晰的分离开来, 再将 K P 取不同值时的整定曲线分别罗列如下:

K P =0.11(偏大)

K P =0.096

- 60 -

厦门大学硕士论文

第五章 运动控制算法

K P =0.080

K P =0.050

K P =0.020(偏小)

图 5-1-9 K P 对动态性能的影响分离图 (2)对稳态性能的影响:加大比例控制系数 K P ,在系统稳定的情况下可以减少 稳态误差 e ss ,提高控制精度。但是加大 K P 只是减小静态误差 e ss ,不能完全消除。 ◆ 积分控制参数 TI 对系统性能的影响 (1)对动态性能的影响:积分控制参数 TI 通常使系统的稳定性下降, TI 太小系 统将不稳定, TI 偏小,振荡次数较多。 TI 太大,对系统性能的影响减小。当 TI 合适 时,过渡特性较为理想。

- 61 -

厦门大学硕士论文

第五章 运动控制算法

注意到 K I =

K PT

TI

,在 K P 一定时, K I 与 TI 成反比。

实际效果图如下:

K I =2.4( TI 偏小)

K I =0.01( TI 偏大)

K I =0.2( TI 适中)

图 5-1-10 K I 对动态性能的影响图 (2)对稳态性能的影响:积分控制参数可以消除系统的稳态误差,提高控制系统 的控制精度。但是若 TI 太大时,积分作用太弱,以至于不能减小稳态误差。 ◆ 微分控制参数 TD 对系统性能的影响 微分控制通常与比例控制或积分控制联合使用,构成 PD 或 PID 控制。微分控制 可以改善动态特性,如超调量减小,调节时间缩短,允许加大比例控制,使稳态误差 减小,提高控制精度。当 TD 偏大或偏小时调节的时间都会变长,只有 TD 大小合适时
- 62 -

厦门大学硕士论文

第五章 运动控制算法

才能得到理想的过渡曲线。 同样,注意到 K D =

K P TD

T

, K D 与 TD 成正比。

K D =0.0000003(太小)

K D =0.00003(偏小)

K D =0.001(适中)

K D =0.003(适中)

- 63 -

厦门大学硕士论文

第五章 运动控制算法

K D =0.007(偏大)

图 5-1-11 K D 对动态性能的影响图 ◆ 控制规律的选择 PID 控制参数相互独立,参数的整定较为方便,且 PID 的算法简单,计算工作量 小,易实现多回路控制。但使用中要根据对象特性、负载特性等合理选择控制规律以 达到最佳的控制效果。 对于一阶惯性对象,负荷变化不大,工艺要求不高,可采用比例控制。 对于一阶惯性与纯滞后环节串联的对象,负荷变化不大,要求控制精度较高,可 采用比例积分控制。 对于纯滞后时间较大,负荷变化也较大,控制性能要求高的场合,可采用 PID 控 制。 对于二阶以上惯性与纯滞后环节串联的对象,负荷变化较大,控制性能要求也较 高的情况下,应采用串级控制,前馈——反馈,前馈——串级或纯滞后补偿控制[37]。 5.1.3.2 采样周期 T 的选择 采样周期 T 在微机控制系统中是一个重要参数。香农采样定理给出了采样频率的 下限,据此采样频率应满足

ω s > 2ω m
其中 ω m 为原来信号的最高频率。从控制性能来考虑,采样频率应尽可能的高,但采 样频率越高对计算机的运行速度和存储能力的要求也相应的提高。所以采样周期的选 择应遵循以下原则: (1) 作用于系统的扰动信号频率越高,则要求采样频率越高,采样周期越小; (2) 测量控制回路数越多,则采样周期 T 越长; (3) 控制用计算机的字长越长,计算速度越快,A/D 和 D/A 转化速度越快, 采样频率可越高,采样周期 T 可越小。 (4) 若控制算法复杂,则采样周期不宜太小。
- 64 -

厦门大学硕士论文

第五章 运动控制算法

5.1.3.3

PID 参数的自整定

一般来说,自整定大致分为二种手段:一种是基于现代控制理论,这种方法要求 具有控制对象数学模型的完备知识。然而,在过程控制领域中,控制对象往往有很多 模棱两可的部分,因而,制作其教学模型比较困难。另一种是基于古典控制理论,这 种方法的根本出发点是只需在P,PI,PID控制装置功能范围内的模型,其余看 不见的部分即使稍存差异,也不会对控制产生多大影响。尤其是这种方法中的 Ziegler-Nichols 是临界比例度法,只要求出控制对象的 2 个变量 (比例增益与周期 ), 便可决定最合适的PID参数[25]。下边来介绍 Ziegler-Nichols 整定法。 受控对象大多可近似用一阶惯性环节加延迟环节来表示,其传递函数为:

G ( s ) = Ke τs (Ts + 1)
对于典型的 PID 控制器

(5-1-3-1)

GC ( s ) = K P (1 +

1 + TD s ) TI s

有 Ziegler- Nichols 整定公式:

1.2T KP = Kτ TI = 2τ T = 0.5τ D
实际应用时 ,通常根据阶跃响应 曲线 (图 5-1-12 ), 人工测量出 K、 T、 τ参数,然后按式(5-1-3-2 )计算

(5-1-3-2)

K P 、TI 、TD 。用计算机进行控制计
算时,一是可以用模式识别的方法识 别出这些特证参数;一是可用曲线拟 合的方法将阶跃响应数据拟合成近似 的一阶惯性加纯延迟环节的模型。 图 5-1-12 阶跃响应曲线 5.2 带有速度前馈和加速度前馈的控制算法 本章节将在传统 PID 控制的基础上, PID 位置控制器进行改进, 对 设计带速度加 速度前馈的数字 PID 调节器与数字阶式滤波器构成的复合控制器,并将进行仿真研 究。
- 65 -

厦门大学硕士论文

第五章 运动控制算法

5.2.1 复合 PID 控制器的实现 把原来的位置前馈改进为速度前馈和加速度前馈,并在前向通路中增加了一个数 字阶式滤波器。速度前馈的作用是减小微分增益或者测速发电机环路阻尼所引起的跟 踪误差。它将一个与电机要求的转速成比例的量增加到控制输出上。惯性所带来的跟 踪误差与加速度成正比,因此它可以由加速度前馈来补偿。加速度前馈将一个与电机 期望的加速度成比例的量增加到控制输出上。其原理框图如下所示。

图 5-2-1 带速度和加速度前馈的 PID 控制器原理框图 复合 PID 调节器控制算法如下式 (5-2-1) :
n1 Dac(n) = K p P [e(n) + Kvff *Vc (n) + Kaff * Ca (n) + Ki ∑e( j)] Kd * P2 * Av (n) 1 j =0

式中,Dac(n) 为伺服周期 n 内的输出命令, 它被转换为一定的电压输出 ;P 为 1 位置放大系数 ; P2 为速度环放大系数; e(n) 为伺服周期 n 内的跟随误差;Vc (n) 为 伺服周期 n 内的指令速度 ; C a (n) 为伺服周期 n 内的指令加速度,即每个伺服周期 最后两个指令位置的差值 ; 伺服周期 n 内的实际速度。 数字阶式滤波器是一个防谐振 (带阻 )滤波器 ,用于抵消共振的影响。 如何设置数 字阶式滤波器有很多不同的基本定律。在一般情况下可以设置一个轻阻尼的带阻滤波 器 (其中心频率约为共振频率的 90 %)和一个大阻尼的带通滤波器 (其频率要高于共 振频率 ),这是为了减小滤波器本身的高频增益。 数字阶式滤波器的形式为:

∑ e( j ) 为伺服周期 n 内的跟随误差的积分 ; A (n) 为
j =0 v

n 1

N ( z ) 1 + N 1 z 1 + N 2 z 2 = D( z ) 1 + D1 z 1 + D2 z 2
- 66 -

厦门大学硕士论文

第五章 运动控制算法

式中 ,分子 N(z)为带阻滤波器; 分母 D(z)为带通滤波器。 一个简单的带通或带阻 滤波器可仅由两个参数来标识。但不同的分析途径使用的参数不同。通常使用的参数 有: 1. 自然频率 ω n ,为无阻尼时的中心频率 , ω n = ω d / 1 b ;
2

2. 阻尼频率 ω d ,为有阻尼时的中心频率 , ω d = ω n / 1 b 。
2

3. 阻尼系数 b,其范围由 0 (无阻尼 )到 1 (临界阻尼 ),b=1 / 2 Q; 4. Q 因子,其范围由 1 (临界阻尼 )到无穷 (无阻尼 ),Q=1 / 2 b。 在 PID 附加算法内执行数字阶式滤波器之前,应当把 PID 参数调节至获得最简 单的性能。采用复合 控制后,位置跟踪滞后误差可减小至零,从而大幅度提高位置控制精度。 5.2.2 实践与仿真 改进后的控制可以显著提高系统的动态响应特性,理论上可以使跟随误差减小到 零。前馈增益、PID 参数以及数字阶式滤波器的参数均可根据电机运动特性的改变而 在线调节和修改。 本实验选用 SEM 公司的直流饲服电机(额定转速 N=2 0 0 0 r/ m,光电编码器分 辨率 pf=4000 脉冲 / round,四倍频)和台湾仕彰自动化机械有限公司的 DC 饲服马达 驱动器。 还是使用作者自行开发的 PID 手工整定界面来进行测试并指定振幅为 1.0 度, 改进算法前后的位置环阶跃响应曲线如下图所示。

图 5-2-2 改进算法前位置环阶跃响应

图 5-2-3 改进算法后位置环阶跃响应

实验仿真结果说明:改进前,系统存在超调量,调整时间为 25 个周期;改进后, 系统无超调,系统的调整时间 7 个周期。这表明了所设计的复合位置控制器可以实现 对位置指令的快速无超调跟踪,且稳态无静差,满足高精度伺服系统控制的要求。 5.3 其它高级算法简介 随着现代控制理论(诸如智能控制、自适应模糊控制和神经网络技术等)研究和
- 67 -

厦门大学硕士论文

第五章 运动控制算法

应用的发展与深入,为控制复杂无规则系统开辟了新途径。各种新型的 PID 控制器也 应运而生,主要有: ◆ 自适应 PID 控制: 是将自适应控制思想与常规 PID 控制器相结合的控制方式, 兼有二者的优点。首先,它是自适应控制器,可以自动辨识被控过程参数、自动整定 控制器参数、能够适应被控过程参数的变化等一系列优点;其次,它又有常规 PID 控 制器的结构简单、鲁棒性好、可靠性高、为现场工作人员所熟悉等优点。 ◆ 智能 PID 控制是:智能控制与常规 PID 控制相结合的产物。智能控制是一类 无需人的干预就能独立驱动其目标的自动控制,所以智能 PID 控制具有不依赖系统精 确数学模型的特点,对系统的参数变化有较好的鲁棒性。 ◆ 模糊 PID 控制:模糊控制器是一种近年来发展起来的新型控制器,其优点是 不要求掌握受控对象精确的数学模型,而根据人工控制规则组织控制决策表,然后由 该表决定控制量的大小。所以将模糊控制与 PID 控制二者结合起来,可以扬长避短, 既有模糊控制灵活而适应性强的优点,又有 PID 控制精度高的特点,对复杂控制系统 和高精度饲服系统具有良好的控制效果。 ◆ 神经网络 PID 控制:PID 控制要取得好的控制效果,就必须通过调整好比例、 积分和微分三种控制作用在形成控制量中相互配合又相互制约的关系,这种关系不一 定是简单的“线形组合” ,从变化无穷的非线形组合中可以找出最佳的关系。神经网络 所具有的任意非线形组合能力,可以通过对系统性能的学习来实现最佳组合的 PID 控 制。 ◆ 预测 PID 控制:预测控制不需要被控对象精确的数学模型,利用数字计算机 的计算能力实行在线的滚动优化计算,从而取得好的综合性效果。工程应用中,该算 法所选参数与工程指标的联系不够紧密,这一点和常规 PID 控制有很强的互补性。因 此二者结合的预测 PID 控制可以取得更好的平衡控制性能。 这些先进的控制算法对工业中经常会碰到的大滞后、时变、非线性等复杂无规则 系统的控制开辟了新的途径[62]。 5.4 小结 数字 PID 控制器的算法简单、鲁棒性好和可靠性高,所以在今后相当长的一段时 间内还将是工程应用领域内的宠儿,而且由于在计算机控制系统中,PID 的控制规律 是用计算机程序来实现的,因此它的灵活性很大。一些原来在模拟 PID 控制器中无法 实现的问题,在引入计算机以后就可以得到解决。本章节在详细介绍了常规的 PID 控 制算法和分析了其各个主要参数对控制系统整体性能的影响后,又引入了速度和加速 度前馈来构成复合式的 PID 控制器,经过实验检验,确实可减小位置跟踪滞后误差, 从而在一定程度上大幅提高了位置控制精度。

- 68 -

厦门大学硕士论文

第六章 仿真及应用

第六章:仿真及应用
6.1 运动规划的仿真结果 本节将对第四章实现的典型的运动轨迹规划函数在 PC 机和数学工具 MATLAB 中 进行模拟仿真。其中包括直线运动轨迹规划函数 line_move_1(一维)和 line_move_n (n 维) ;圆弧运动轨迹规划函数 arc_move_2D(二维)和 arc_move_3D(三维) ;椭 圆弧运动轨迹规划函数 ellipse_move_2D(二维)和 ellipse_move_3D(三维)以及以 上各种线型混合运动函数。 另外,三次样条曲线的轨迹规划函数组: SplinePara(),dx_next(),spline_move() 也在普通 INTEL 系列 PC 机上做了模拟测试。 6.1.1 函数 line_move_1(一维) 终点 60 加速度 30 起始速度 0 采样周期 2(毫秒) 稳定速度 20 插补方式 梯形
◆ 输入参数:

起始点 -10 终止速度 0
◆ 规划描述:

该运动沿任一轴从-10 点运动到 60 点。
◆ 测试代码:

//The application of line_move_1 int axisno=0; float start_pos=-10.0; float end_pos= 60.0; float start_vel=0.0; float steady_vel=20.0; float end_vel=0.0; float allow_accel=30.0; int pattern=0; float point_para[3]={0,0,0}; int error=MCT_NO_ERROR; void isr_t0 () { if( error == MCT_NO_ERROR ) {
- 69 -

//must initialize

厦门大学硕士论文

第六章 仿真及应用

tm += DT; //one-dimensional path plan error = line_move_1 ( axisno, start_pos, end_pos, start_vel, end_vel, allow_accel, DT, pattern, tm, point_para ); } dc_move_axis ( 0, point_para[0],point_para[1], point_para[2],DT ); }
◆ 仿真图:

steady_vel,

位移—时间

速度—时间

加速度—时间

图 6-1-1 一维直线运动仿真图
◆ 结论:模拟结果与期望的一致。

6.1.2

函数 line_move_n(n 维) 终点 (20,30,50) 加速度 30 维数 3 插补方式 梯形 起始速度 0 采样周期 2(毫秒) 稳定速度 20

◆ 输入参数:

起始点 (0,0,0) 终止速度 0
◆ 规划描述:

在三维空间中由点(0,0,0)直线运动到点(20,30,50)。
◆ 测试代码:

#define DIMENSION 3 int pattern=2; float DT = 0.0002; float tm = 0; int error = MCT_NO_ERROR;
- 70 -

厦门大学硕士论文

第六章 仿真及应用

float start_point[] = { 0.0, 0.0 ,0.0}; float end_point[]= { 20.0,30,50}; int axisno[] = { 0, 1, 2}; float path[9] = {0,0,0,0,0,0,0,0,0}; //must initialize float start_vel=0.0; float end_vel=0.0; float steady_vel=20; float allow_accel=30; int i=0; void isr_t0 () { if(error==MCT_NO_ERROR) { tm+=DT; //n-dimensional path plan error=line_move_n (DIMENSION,axisno,start_point,end_point, start_vel, steady_vel,end_vel,allow_accel,DT, pattern, tm, path ); } for(i=0;i<DIMENSION;i++) dc_move_axis ( axisno[i], path[i*3], path[i*3+1], path[i*3+2], DT ); }
◆ 仿真图:

三坐标图

- 71 -

厦门大学硕士论文

第六章 仿真及应用

各轴位移-时间图: T-X 图 T-Y 图 T-Z 图

各轴速度-时间图: T-Xv 图 T-Yv 图 T-Zv 图

各轴加速度-时间图: T-Xa 图 T-Ya 图 T-Za 图

图 6-1-2
◆ 结论:模拟结果与期望的一致。

n 维直线运动仿真图

- 72 -

厦门大学硕士论文

第六章 仿真及应用

6.1.3

函数 arc_move_2D(二维) 圆心 (50,50) 加速度 180 旋转角度 360 插补方式 梯形 起始速度 0 采样周期 2(毫秒) 稳定速度 60 方向 1

◆ 输入参数:

起始点 (20,30) 终止速度 0
◆ 规划描述:

该动作将规划一个完整的圆,由点(20,30)以点(50,50)为圆心逆时针运动一周回 到起始点。
◆ 测试代码:

#define DIMENSION 2; int pattern=0; float DT = 0.002; float tm = 0; int error = MCT_NO_ERROR; float start_point_c[]={20.0,30.0}; float center_point[] = {50.0,50.0}; int axisno[] = { 0, 1}; float path[6] = {0,0,0,0,0,0,0,0,0}; //must initialize float angle=(3.1415926*2); int ccw_dir=0; float start_vel=0.0; float end_vel=0.0; float steady_vel=60; float allow_accel=180; int i=0; void isr_t0 () { if(error==MCT_NO_ERROR) { tm+=DT; error=arc_move_2D (axisno,start_point_c,center_point,angle,ccw_dir, start_vel,steady_vel,end_vel,allow_accel,DT,pattern,tm,path); }
- 73 -

厦门大学硕士论文

第六章 仿真及应用

for(i=0;i<DIMENSION;i++) dc_move_axis ( axisno[i], path[i*3], path[i*3+1], path[i*3+2], DT ); }
◆ 仿真图:

坐标图及各轴位移-时间图: 坐标图 X-T 图 Y-T 图

各轴速度-时间图: Xv-T 图 Yv-T 图

各轴加速度-时间图: Xa-T 图 Ya-T 图

图 6-1-3 二维圆弧仿真图
- 74 -

厦门大学硕士论文

第六章 仿真及应用

◆ 结论:模拟结果与期望的一致。

6.1.4

函数 arc_move_3D(三维) 圆心 (50,50,50) 加速度 80 旋转角度 270 插补方式 梯形 起始速度 0 采样周期 2(毫秒) 稳定速度 40 轴矢量 (1,1,-2) 方向 1

◆ 输入参数:

起始点 (0,0,0) 终止速度 0
◆ 规划描述:

在空间坐标系中由点(0,0,0)出发,以点(50,50,50)为圆心,以向量(1,1,-2)为轴, 逆时针旋转 270 度停止。
◆ 测试代码:

#define DIMENSION 3 int pattern=0; float DT = 0.002; float tm = 0; int error = MCT_NO_ERROR; float start_point[]={0.0, 0.0, 0.0}; float center_point[] = {50.0,50.0,50.0}; int axisno[] = { 0, 1, 2}; float path[9] = {0,0,0,0,0,0,0,0,0}; //must initialize float angle=(4.71); int ccw_dir=1; float start_vel=0.0; float end_vel=0.0; float steady_vel=40; float allow_accel=80; float normal[]={1,1,-2}; int i=0; void isr_t0 () { if(error==MCT_NO_ERROR) { tm+=DT; //three-dimensional arc path plan
- 75 -

厦门大学硕士论文

第六章 仿真及应用

error=arc_move_3D ( axisno , start_ point, center_ point, normal ,angle, ccw_dir,start_vel,steady_vel,end_vel,allow_accel,DT,pattern,tm,path); } for(i=0;i<DIMENSION;i++) dc_move_axis ( axisno[i], path[i*3], path[i*3+1], path[i*3+2], DT ); }
◆ 仿真图:

笛卡儿坐标图

各轴位移-时间图: X-T Y-T Z-T

各轴速度-时间图: Xv-T Yv-T Zv-T

- 76 -

厦门大学硕士论文

第六章 仿真及应用

各轴加速度-时间图: Xa-T Ya-T Za-T

图 6-1-4 三维圆弧仿真图
◆ 结论:模拟结果与期望的一致。

6.1.5

函数 ellipse_move_2D(二维) 椭圆中心 (60,80) 终止速度 0 长半轴 50 加速度 60 短半轴 30 插补方式 梯形 旋转角度 360 采样周期 2(毫秒) 起始速度 0 方向 0

◆ 输入参数:

起始点 (90,120) 稳定速度 30
◆ 规划描述:

在平面上从点(90,120)出发,以点(60,80)为中心,走一个长半轴为 50,短半轴为 30 的 顺时针方向的椭圆。
◆ 测试代码:

#define DIMENSION 2; int pattern=0; float DT = 0.002; float tm = 0; int error = MCT_NO_ERROR; float start_point_c[]={90.0,120.0}; float center_point[] = {60.0,80.0}; float a=50; float b=30; int axisno[] = { 0, 1}; float path[6] = {0,0,0,0,0,0,0,0,0}; //must initialize float angle=(3.1415926*2); int ccw_dir=0;
- 77 -

厦门大学硕士论文

第六章 仿真及应用

float start_vel=0.0; float end_vel=0.0; float steady_vel=30; float allow_accel=60; void isr_t0 () { if(error==MCT_NO_ERROR) { tm+=DT; //two-dimensional arc path plan error=ellipse_move_2D(axisno,start_point_c,center_a,b,point,angle,ccw_dir, start_vel,steady_vel,end_vel,allow_accel,DT,pattern,tm,path); } for(int i=0;i<DIMENSION;i++) dc_move_axis ( axisno[i], path[i*3], path[i*3+1], path[i*3+2], DT ); }
◆ 仿真图:

笛卡儿坐标图

各轴位移-时间图: X-T 图
- 78 -

Y-T 图

厦门大学硕士论文

第六章 仿真及应用

各轴速度-时间图: Xv-T 图 Yv-T 图

各轴加速度-时间图: Xa-T 图 Ya-T 图

图 6-1-5 二维椭圆弧仿真图
◆ 结论:模拟结果与期望的一致。

- 79 -

厦门大学硕士论文

第六章 仿真及应用

6.1.6

函数 ellipse_move_3D(三维) 椭圆中心 (0,50,0) 终止速度 0 长半轴 50 加速度 80 短半轴 30 插补方式 梯形 旋转角度 360 采样周期 2(毫秒) 起始速度 0 轴矢量 (1,0,0) 方向 0

◆ 输入参数:

起始点 (0,0,0) 稳定速度 40
◆ 规划描述:

在空间中由原点出发,以点(0,50,0)为中心,以向量(1,0,0)为轴,走一个长半轴为 50,短半轴为 30 的顺时针方向的空间椭圆弧。
◆ 测试代码:

int pattern=0; float DT = 0.002; float tm = 0; int error = MCT_NO_ERROR; float start_point[]={0.0, 0.0, 0.0}; float center_point[] = {0.0, 50.0, 0.0}; float a=50; float b=50; int axisno[] = { 0, 1, 2}; float path[9] = {0,0,0,0,0,0,0,0,0}; //must initialize float angle=(3.14159*2); int ccw_dir=0; float start_vel=0.0; float end_vel=0.0; float steady_vel=40; float allow_accel=80; float normal[]={1,0,0}; void isr_t0 () { if(error==MCT_NO_ERROR) { tm+=DT; //three-dimensional arc path plan error=ellipse_move_3D ( axisno , start_ point, center_ point,a,b, normal ,
- 80 -

厦门大学硕士论文

第六章 仿真及应用

angle,ccw_dir,start_vel,steady_vel,end_vel,allow_accel,DT,pattern,tm,path); } for(int i=0;i<3;i++) dc_move_axis ( axisno[i], path[i*3], path[i*3+1], path[i*3+2], DT ); }
◆仿真图:

笛卡儿坐标图

各轴位移-时间图: X-T 图 Y-T 图 Z-T 图

各轴速度-时间图: Xv-T 图 Yv-T 图 Zv-T 图

- 81 -

厦门大学硕士论文

第六章 仿真及应用

各轴加速度-时间图: Xa-T 图 Ya-T 图 Za-T 图

图 6-1-6 三维椭圆弧仿真图
◆ 结论:模拟结果与期望的一致。

6.1.7 混合运动仿真。 第一幅图(二维混合运动仿真图) : 从点(0,0)开始,以(1,0)为圆心,按圆弧逆时针转动 180 度到(2,0),再直线运动到 (5,0),再以(0,0)为中心,顺时针椭圆弧运动到(0,4) ,再直线运动到(0,0) 。 二维仿真图

图 6-1-7 二维混合运动仿真图 第二幅图(三维混合运动仿真图) : 第一段从点(0,0,0)开始,以(5,0,0)为圆心,绕向量(0,0,1) 按圆弧顺时针转动 360, 第二段从点(0,0,0)开始,以(5,0,0)为圆心,绕向量(0,1,0) 按圆弧顺时针转动 360 度, 第三段从点(0,0,0)以(5,0,0)为中心,绕向量(0,1,0)逆时针运动 360 度。

三维仿真图
- 82 -

厦门大学硕士论文

第六章 仿真及应用

图 6-1-8 三维混合运动仿真图 6.1.8 样条曲线函数组。

给定一系列离散点: (-9.0610,0.4915)(-7.1638,3.4014) , , (-5.0610,0.4915)( 2.1677,1.0027) , , ( 5.0610,0.4915)( 6.1638,3.4014) , 。 分别对四种不同的三次样条曲线进行轨迹规划的计算机仿真模拟。
◆ 测试代码。

#define N 6 float DT=0.001; int error; float x=0.0; float hk[N]; float mk[N]; int points[2*(N+1)]={ -9.0610 , 0.4915, -7.1638 , 3.4014,-5.0610 , 0.49152.1677,1.0027,5.061,0.4915,6.1638,3.4014}; float path[6]={0,0,0,0,0,0}; int i; int style=0;//0-自然样条;1-夹紧样条;2-抛物线终结样条;3-端点曲率调整 样条。 float fxk[2]={0,0}; float tm=0.0;

- 83 -

厦门大学硕士论文

第六章 仿真及应用

float vel=1; int firstFlag=1; void isr_t0 () { if(firstFlag==1) { tm=0.0; error=SplinePara(N,style,fxk,points,hk,mk); firstFlag =0; error=MCT_NO_ERROR return; } if(error==MCT_NO_ERROR) { tm+=DT; x += dx_next(*seg_num,x,pointsk,hk,mk,vel*DT); error=spline_move(*seg_num,x,pointsk,hk,mk,path); } dc_move_axis(0,path[0],path[1],path[2],DT); dc_move_axis(1,path[3],path[4],path[5],DT); }
◆ 四种不同端点约束下的三次样条曲线的对比

图 6-1-9 四种三次样条曲线的对比图
◆ 运动规划实现后实时仿真轨迹显示。
- 84 -

厦门大学硕士论文

第六章 仿真及应用

为了便于观察和比较每个周期内的插补步长,绘图时特意将绘制原点置于图象顶 端。 自然样条的规划实现 压紧三次样条的规划实现

抛物线终结样条的规划实现

端点曲率调整样条的规划实现

图 6-1-10 三次样条曲线仿真图 6.2 应用实例 该轨迹规划器应用于深圳摩信公司的 MCT8000 系列多轴运动控制卡后,推出不 久就成功的应用于数个自动化控制项目,象云南昆明威恒生的摄象机控制系统,深圳 强华 PCB 钻铣床控制系统, 深圳大族激光钻孔机等等, 并在第二届高交会上崭露头脚, 成功地控制了 SCARA 机器人作空间的复杂曲线轨迹的运动演示,得到了业内专家的 高度评价。现在以作者本人作为软件负责人参与的深圳坚达切割机控制系统为例来说 明该轨迹规划器的优越性。 6.2.1 6.2.1.1 数控切割机控制系统[65] 引言

切割机行业是机加工的一个重要领域。人们认识到利用先进的数控切割技术,不 但可以保证产品的加工质量,同时也使得企业的制造成本大幅下降,缩短产品的生产 周期。智能化精密切割已成为行业发展的趋势。我国的数控切割行业发展较晚,以前 由于认识和传统观念的原因,一直存在重焊接轻切割的观念。八十年代以前,板材下 料大多是以手工或半自动小车下料为主。八十年代以后人们越来越认识到先进的数控 切割设备是保证焊接产品质量的关键。因此切割行业也得到了很大的发展。现在我国
- 85 -

厦门大学硕士论文

第六章 仿真及应用

数控切割制造业已趋向成熟,应用领域相对较宽,如航天、航空、机械、纺织、石油、 化工、船舶、交通、铁路等,需求量呈逐年递增趋势。目前产品仍以火焰和普通等离 子切割系统为主,今后数控精细等离子切割设备会得到较快的发展。 当前中国市场应用于切割设备行业的控制系统主要以进口设备为主。如西班牙的 FAGOR、美国的 EDGE、德国的伊萨、梅萨公司自行生产的控制系统。此类控制系统 的性能比较稳定但价位较高,使用该类设备的数控生产厂家主要为上海伊萨公司、昆 山梅萨公司、上海十一所和北京百惠公司。深圳市摩信科技有限公司与深圳坚达机械 厂合力开发的这套切割机系统是国内切割机行业的一次新尝试,其运动控制系统采用 了基于 DSP(Digital Signal Processor)的运动控制卡, 使切割精度和速度以及可靠性均较 传统的控制系统有了质的飞跃。以下将分别阐述系统硬件结构和软件重要功能模块的 实现。 6.2.1.2 系统硬件结构及软件工作机理 考虑到该切割机系统对切割过程中的速度、精度以及操作过程中实时的轨迹跟踪 和断点记忆、重复切割、电机的同步驱动等均有较高要求,所以选用 MCT8000 多轴 运动控制卡来作为控制系统的核心部件。


MCT8000 多轴运动控制卡简介 深圳摩信科技公司的 MCT8000 系列智能运动控制器包括主控制板,接口板以及

控制软件等, 是新一代开放式结构高性能可编程运动控制器。 CPU 采用美国德州仪 其 器的 TMS320C31 40MHz DSP, 可通过 ISA 或 PCI 标准总线或 USB 高速接口与主控机 连接。主控机与控制器之间采用双向高速 FIFO 进行通讯,可提供 2-8 轴的高速,高精 度伺服控制,主控机可选用任何 PC 系列微机。和传统的运动控制器产品相比, MCT8000 系列产品具有开放式结构,高速、高精度,网际在线控制,多轴同步控制, 可重构性,高集成度,高可靠性和安全性等特点。 MCT8000 在设计上采用了先进的软硬件重构技术, 根据控制对象的不同可对系统 进行在线设置和重构,或通过软件升级硬件。能够根据客户的不同要求,可对控制器 的硬件模块进行最佳调整,配以相应的 BIOS,使用户可以对 MCT8000 的 I/O 口直接 操作。 MCT8000 提供了一整套运动控制函数,包括 BIOS 函数库(直接操作 MCT8000 的所有 I/O) 、基本运动函数库(1~3 维的运动插补和运动控制函数) 、扩展运动函数库 (机器人运动学模型和智能 PLC 代码翻译器等)和网际在线控制器(基于 TCP/IP 的 点对点安全通讯协议) 。利用 MCT8000 提供的软件系统和图形化的开发平台,可大大 地缩短切割机控制系统的开发周期。


切割机系统的硬件结构 整个切割机系统的组成如图 6-2-1 所示,主要包括了控制面板、工业计算机、多
- 86 -

厦门大学硕士论文

第六章 仿真及应用

轴运动控制卡及其接口板、电源板、伺服放大器和电机、继电器和电磁阀以及一些安 全和保护电路。其中工业计算机作为硬件平台,处理切割机控制中的非实时任务,实 时任务由 MCT8000 运动控制卡处理。而电源板、伺服放大器和电机、继电器、电磁 阀则作为系统的执行机构。另外考虑到工人操作的方便和人身、系统的安全,提供了 控制面板及安全保护电路。

图 6-2-1 切割机数控系统组成图



软件工作机理 本系统的实时控制部分是由基于高性能 DSP 的运动控制卡完成, 通过自带的基本

运动函数库(MCTBMCL)和基本输入/输出操作系统(MCTBIOS) ,完成实时的运动 轨迹规划和对 MCT8000 的 I/O 口的直接操作。另外还提供了专门的通讯函数库 (MCTHOST.DLL)实现 PC 机与 DSP 之间的通讯功能,所以 PC 机只用于完成非实 时控制部分,故对 PC 机操作系统的实时性要求不高,可选用 Windows 95/98/NT/2000 等流行的软件平台开发友好的用户操作界面。 系统软体的依存关系如图 6-2-2。 用户的 应用程序可在 Visual C/C++的环境下开发, 通过 MCT8000 提供的程序下载器下载到运 动控制板的内存中运行。运动控制指令和参数以及控制板所有输入输出接口的状态均 可通过与通讯模块进行交互来获得。

- 87 -

厦门大学硕士论文

第六章 仿真及应用

人机交互应用程序
(软件接口,逻辑控制,错误处理)

MCTHOST.DLL
(通讯模块)

DSP 主控程序
(参数设置,中断启动,I/O 状态检测) 图 6-2-2 切割机软体工作机理图

6.2.1.3

用户应用软件开发及各种控制功能的实现

本切割机控制系统的人机界面采用 VISUAL C++6.0 语言开发,DSP 主控程序则 用 C 语言实现, 配合摩信科技提供的通讯函数模块及运动函数库、 基本输入/输出函数 库,可以使用户在友好的人机界面中方便地操作切割机系统,完成各种加工指令。
◆人机界面功能介绍

本软件的用户人机界面如图 6-2-3 所示,在操作过程中有全面的提示信息,易学 易用。

图 6-2-3 切割机人机界面

其实现的主要功能有:与套料软件接口(G 指令) ,零件图形预览与无级缩放,零 件图形旋转及镜向,切割路径快速预演示,自动校正,故障监控及显示,实时切割轨
- 88 -

厦门大学硕士论文

第六章 仿真及应用

迹动态跟踪及控制指令跟踪,X、Y 坐标实时显示,断点记忆,重复切割,切割时间、 快移时间、工作时间累计等。
◆ DSP 主控程序

MCT8000 运动控制卡的运行机制, 是按预先设定的中断时间周期地执行各种运动 控制指令。DSP 主控程序在前台监控运动控制程序的运行状态,在后台响应用户的实 时控制命令,保证底层控制环节的正常运行。在本文所阐述的切割机系统中,DSP 主 控程序主要完成实时的运动插补运算和对 I/O 口的监控和操作,同时也负责和 PC 机 通信。这种双 CPU 的并行结构大大提高了系统的可靠性。DSP 主控程序和中断调用 模块的工作原理如图 6-2-4 所示。

图 6-2-4 DSP 主控程序和中断调用模块

◆ 重要功能模块的实现

由于本平台采用 G 代码作为加工指令,所以 G 代码的翻译及段号的管理便成为重 要的一环,另外,切割过程中的暂停、继续、返回以及实时加减速也是本平台的技术 特色,下面将就这些功能模块的具体实现方法进行阐述。 1) 加工文件段号的管理 本加工平台的运动指令主要由 G00、G01、G02、G03 组成,分别对应快速定位、直 线切割、顺时针圆弧切割、逆时针圆弧切割。MCT8000F4 运动控制卡的基本运动函数 库中提供的连续混合运动函数 mix_move_2_CP 主要针对二维的直线、圆弧的混合多段 运动。这样上下位机程序就有了协调工作的基础。在每一次具体的运动过程中,上位 机不断地检索下位机主控程序的执行情况,一旦真实的运动段发生变化,上位机对应 的加工 G 指令文件也就相应地向前或向后推进一行,保持一致性,从而实现段号的管 理。 2) G 代码的翻译 平台读取套料软件生成的 G 指令文件并将其生成加工指令序列。直线在生成加工 指令时起始点容易确定,圆弧在只给定半径参数时其圆心的确定就会有一些问题,因 为每一段弦在半径给定时都会对应两个圆心,这时可以利用矢量旋转来确定圆心,方 法如下:
- 89 -

厦门大学硕士论文

第六章 仿真及应用

I

当加工指令为 G03 且是优弧情况或加工指令为 G02 且是劣弧情况时,

| PsPe | cos α 1 = 2R

, sin α 1 =

(2 R) 2 (| PsPe |) 2 , 2R

α 1 =| a tan 2(sin α 1 , cos α 1 ) | ,
II

Pc1 =

PsPe | PsPe |

E α1 * R

(1)

当加工指令为 G03 且是劣弧情况或加工指令为 G02 且是优弧情况时,

| PsPe | cos α 2 = 2R

, sin α 2 =

(2 R) 2 (| PsPe |) 2 , 2R

α 2 = | a tan 2(sin α 2 , cos α 2 ) | ,

Pc 2 =

PsPe | PsPe |

E α2 * R

(2)

其中 R 为给定圆弧半径的大小,优劣弧则可根据 R 前的符号判定:正号为劣弧, 负号为优弧; a tan 2 为计算机语言中提供的专用反正切函数; E ∈ IR 换张量;另外 G 代码中 G02 表示顺时针转动,G03 表示逆时针转动。 3)切割过程中暂停、继续、返回功能的实现 当上位机程序发出暂停命令后,下位机的主控程序将执行板卡自带的运动控函数 event_stop 来逐步的停止各运动轴, 从而实现暂停功能。 同时上位机完成对当前段号 的记录。当继续命令到来时,上位机将从当前的段号开始向下位机的主控程序传送运 动参数,从而恢复运动,实现继续功能。返回功能也依赖于段号的管理,与继续不同 的是它从当前段号开始向前搜索 G 指令文件并形成相应的加工数据。在继续和返回的 运动数据形成时,得到各轴的当前位置是必须的,可以借助基本运动控制函数 get_position 来实现,但是当该运动段为圆弧时,还须计算该段由当前点到终止点的
- 90 3*3

,为旋转变

厦门大学硕士论文

第六章 仿真及应用

角度,其原理和公式如下:

1 1 ∠PsPcPe = 2a tan 2( | PsPe |, R 2 (| PsPe |) 2 ) ; 2 4 所以欲求角在优弧情况下: α = 2 * π ∠PsPcPe ; 同样在劣弧情况下: α = ∠PsPcPe ;
其中,Ps 的坐标可由基本运动控制函数库中的 get_position 得到,Pe 的坐标则由 G 代码文件给定。 4) 实时加减速的实现 实时加减速的实现用到了时间压缩的原理。在下位机的主控程序中,插补周期 Dt 给定后插补时间 Tm 会在每个周期自动增加 Dt 来满足轨迹规划的需要。此时我们可以 用一个系数 K 乘以该插补周期 Dt 后再加给插补时间 Tm,从而达到使规划值压缩相应 倍数的目的,得到的实际效果就是每个插补周期 Dt 内实际移动的距离是原来的 K 倍, 实现速度的实时变化。 在 DSP 主控程序中实现如下: void isr_t0() { if(err=MCT_NO_ERROR) { err=mix_move_2_CP(axisno,seg_num,start_pos,position,patter, steady_vel,allow_accel,DT,pattern,tm,path); Tm+=K*Dt; } } 6.2.1.4 工业应用 该切割机控制系统是深圳坚达机械有限公司委托研究项目, 现已投入生产和使用, 证明比现行的切割机控制系统有更高的性能价格比。下图 6-2-5 所示的一些图样就是
- 91 -

厦门大学硕士论文

第六章 仿真及应用

在该系统平台上成功切割出的零件图形,切面光滑整齐,完全符合加工要求。同时由 于该切割平台易学易用,与套料软件的接口完善,所以极大提高了加工效率,并且减 少了对板材的消耗。

图 6-2-5 切割机完成的零件图样

6.2.1.5 结束语 本小节阐述了一种新型的基于 DSP 运动控制的切割机系统, 具有友好的人机交互 界面、 完善的套料软件接口和高速高精度的特性, 可作为现行切割机的升级换代产品。 我国的切割机行业与发达国家同行相比仍有很大的差距,国外同行的产品涉及面 广泛,而且有一整套与生产和检测相关的标准。但是随着我国运动控制水平的提高和 对核心技术掌握的不断深入,相信不久的将来,我国会在切割机领域达到国际先进水 平,为社会主义经济的腾飞和科技的发展注入崭新的活力。 6.2.2 其他应用 以该轨迹规划器为基础的运动控制系统的应用例子还有很多,下边再简单列举几 个来说明该系统的一些技术指标。 PCB 钻铣床控制器(2-6 头) :该项目是深圳摩信公司和深圳强华公司联合开发的 全闭环控制加工系统,用 VC++6.0 开发的友好的人机界面,可实现加工预览,实时监 控,自动换刀等诸多功能,操作方便安全。这里着重指出的是其运动控制系统的技术 指标:行进速度可达到 25m/min,定位精度为 ± 0.005 mm,重复精度为 0.0025 mm。 该项目产品曾因作为第二届高新技术交流会的展品而引起广泛关注。 机器人控制器:本控制系统是以日本的 Scara 机器人为控制对象,以 MCT8000 系列运动控制卡作为控制系统的核心建立起来的, 除了可以达到 PCB 钻铣床的定位精 度外,该机器人可以完成空间的直线、圆弧甚至椭圆弧的轨迹规划,以及以上几种曲 线的混合运动规划。这样,该机器人就具备了空间的弧焊、切割、装配等能力,具有 极大的应用潜力。该系统也曾作为作为第二届高新技术交流会的展品,并给国内外的 业内同行作了演示。 还有很多成功的应用案例这里就不再一一赘述了。凡此种种,表明该轨迹规划系 统是先进且实用的, 并不比国外同类产品逊色, 具有极大的推广价值和开发应用潜力。 6.3 小结 本章节首先对前边章节所建立的一系列运动轨迹规划函数进行了 MATLAB 仿真,
- 92 -

厦门大学硕士论文

第六章 仿真及应用

其中包括直线运动轨迹规划函数 line_move_1(一维)和 line_move_n(n 维) ;圆弧运 动轨迹规划函数 arc_move_2D(二维)和 arc_move_3D(三维) ;椭圆弧运动轨迹规 划函数 ellipse_move_2D(二维)和 ellipse_move_3D(三维)以及以上各种线型混合 运动函数。此外,三次样条曲线的轨迹规划函数组: SplinePara(),dx_next(), spline_move()也在普通 INTEL 系列 PC 机上做了模拟测试。通过以上的仿真和测试, 验证了本论文所阐述的运动规划算法的有效性。之后又列举了该轨迹规划器投入实际 生产应用后的一个典型实例——深圳坚达切割机控制系统,其友好的人机交互界面、 完善的套料软件接口和高速高精度的特性,完全可作为现行切割机系统的升级换代产 品。 最后列举了 PCB 钻铣床控制器和机器人控制器借以说明该系统所能达到的技术指 标和性能参数。

- 93 -

厦门大学硕士论文

第七章 实



第七章:实验
该新型轨迹规划器一经推出,即应用于深圳市摩信科技有限公司在中国率先推出的 “基于网络技术的开放式结构高性能 DSP 运动控制系统”MCT8000 系列产品中,打破 了发达国家对运动控制技术的垄断,为我国对开放式运动控制技术的深一步研究和相关 产品的开发奠定了基础。为了简要介绍该运动控制板卡的常规应用模式和实际检验新型 轨迹规划器的有效性,本文设计了一个综合的实验平台来测试前面章节所阐述的内容。 7.1 实验系统简介 本实验选用 SEM 公司的直流饲服电机(额定转速 N=2 0 0 0 r/ m,光电编码器分辨 率 pf=4000 脉冲 / round,四倍频)4 台(两台用于高速同步实验,两台用于二维教学平 台)和台湾仕彰自动化机械有限公司的 DC 饲服马达驱动器,以及教研室自行研制的教 学平台的机械本体; 监控设备选用宏基摄像头和 ABE 的云台; 另外搭配插有 MCT8000F4 运动控制板卡并外接了其接口板的通用 PC 机一台和局域网环境;操作系统采用视窗 WIN2000。软件的开发则选用 VISUALC++6.0 和 MCT8000 板卡自带的软件包。 7.1.1 应用方案 本地应用方案:

Pc 主机与摩信 科技软件系统

MCT8000 硬 件系统 驱 动 单 元 MCT8000 控制器主板 MCT8000-IO 接口板

二 维 平 台 同 步 电机

PC 机母板

摄像头照 明灯

图 7-1-1 本地应用方案

- 94 -

厦门大学硕士论文

第七章 实



网际应用方案:
以太网(局域网或因特网)

各种计算机 终端设施

各种本地 应用方案

图 7-1-2 网际应用方案 7.1.2 硬件平台的搭建 下图是伺服电机控制的一个典型应用方案,在本实验所搭建的平台中共接有四个同 样的伺服电机及其伺服放大装置。

图 7-1-3 典型运动控制系统的组成 按上图所示的控制方式,本实验用了四个 DAC 输出分别用来控制四台电机,另外分配 四个 DO 输出来控制饲服放大器的使能。其中两台做高速同步实验,两台做二维运动平 台的驱动电机。 另外,利用 MCT8000F4 丰富的接口资源,外接了监控系统,用一个 DO 口来开启 照明灯,四个 DO 口分别对应云台的左右转动和俯仰的功能,一个 DO 口对应自动监控 功能。 整个实验系统的图示如下:

- 95 -

厦门大学硕士论文

第七章 实



图 7-1-4 实验平台全景图 7.1.3 人机界面 为了突现该运动控制卡的开放性特点,本系统特意用 WINSOCKET2.0 编写了一个 简单的可以远程聊天和网络控制的演示程序,由服务器和客户端两部分组成。其中服务 器程序运行在装有 MCT8000F4 板卡的通用 PC 机上,客户端程序则可以运行在任何一 台连接在因特网上的计算机终端。由于本程序通讯协议采用了基于面向连接的 TCP 协 议,所以其速度较快且不易丢失数据包,远程控制性能稳定。 ◆ 服务器程序 该模块主要功能有:1.负责客户段的请求任务;2.进行实时的系统广播;3.调度客户 端的控制权限;4.与下位的 DSP 芯片即时通讯,实现实时控制功能;5.通讯错误和控制 系统错误处理。 由于该模块同时肩负面向因特网的通讯和面向 DSP 板卡的通讯两层任务,所以负 担较重,在此将其登陆客户端数量控制在 100 人以内,但在同一时间只有一人可获得对 本地所有实验设施进行远程操作的权限。 其界面如下: (可隐藏界面并用 CTRL+SHIFT+J 唤醒)

- 96 -

厦门大学硕士论文

第七章 实



图 7-1-5 服务器程序界面 ◆ 客户端程序 该程序主要由以下功能模块组成: 1.即时聊天模块;2.远程监控模块;3.二维平台远程控制模块;4.高速同步电机控 制模块;5.自动更新和链接模块。其界面如下图所示:

图 7-1-6 客户端程序界面
- 97 -

厦门大学硕士论文

第七章 实



即时聊天模块可以同已登陆的所有客户保持即时的网上交流;远程监控模块则借助 于微软的网络会议程序 NETMEETING 来传输影像,MCT8000F4 控制板则负责摄像头 动作的控制和照明灯的开闭,其操作界面的四个箭头分别对应摄像头的四种运动,非常 直观;二维平台的运动模块可以完成远端的直线和圆弧的运动演示;而高速同步模块可 在远端在线设置速度、加速度来完成对同步性能的测试。 7.2 实验结果与分析

功能一:摄像头及照明灯控制 ◆ 实验现象描述

图 7-2-1 摄像头及照明灯控制图

该功能主要是为了远程的监控。通过 NETMEETING 程序将远端影像抓取回来后,若 发现摄像头关注焦点并非所需,可以即时通过网络调整到合适的角度来进行观测。四个 箭头分别对应摄像头的四种运动:左转、右转、抬头和低头,当鼠标按下时,动作开始 执行,抬起时马上终止。另外“自动运行”复选框被选中后电机将自动在一个角度内来 回摆动取像。如果光线不够,则可选中“照明灯”项以打开照明灯。 ◆ 实验结果分析 实时监控的反复实验验证了网络通讯和对 DSP 板卡通讯的可靠性,此功能对于将来 实现现场无人加工具有很积极的意义。

- 98 -

厦门大学硕士论文

第七章 实



功能二:电机同步运转控制 ◆ 实验现象描述

图 7-2-2 高速同步实验图 在确定获得控制权的情况下,可在设置了速度、加速度后点击“执行”按钮,弹出 “高速同步正在执行” 对话框, 此时可从视频口看到电机的运转情况。 两台中间衔着 2B 软铅芯的同步电机将逐步加速并最终达到 4000 转/分钟;正向旋转一定圈数后逐渐停止 再反向加速至 4000 转/分钟,保持高速运转一段时间后减速停止。铅芯保持不断。 ◆ 实验结果分析 此实验主要是验证轨迹规划算法和运动控制算法(PID 控制算法)的有效性,即系 统所要求的高速、高精度和高稳定性。在同步电机反向和高速运转时铅芯不断说明了该 控制算法调节时间短、超调小、稳态误差小,满足高性能运动控制系统的要求。 功能三:二维教学平台控制 ◆ 实验现象描述 同样,在确定获得控制权的情况下,点击平台控制区域的“远程实现”按钮,弹出 “执行中”对话框,此时可从视频窗口中看到二维运动平台的运动状况。下图是正在规 划中的一个齿轮图样。
- 99 -

厦门大学硕士论文

第七章 实



图 7-2-3 二维平台运动演示图 ◆ 实验结果分析 该实验表明本文所提供的新型轨迹规划器是实用且先进的,可以走任意用直线、圆 弧、椭圆弧甚至样条曲线混合而成的图样。这就为以本轨迹规划器为依托的运动控制系 统实现与 CAD、CAM 等计算机辅助设计或制造软件提供了可能的接口,为制造业全面 走向自动化迈出崭新一步。 7.3 小结 通过本实验所搭建的平台,全面测试了该新型轨迹规划器的开放性特点、运动规划 函数的先进和丰富性,以及运动控制算法的实用性。同时也介绍了基于该轨迹规划器的 MCT8000 系列多轴运动控制板卡的本地和网际应用方案,展现了其良好的兼容性,强 大的控制功能以及开发、操作的快捷方便等特性。

- 100 -

厦门大学硕士论文

第八章 结论与展望

第八章:结论与展望
运动规划(Motion Plan)和控制策略是新一代 DSP 多轴运动控制技术的核心技术, 它在一定程度上决定了运动控制系统的性能。本文在前人研究的基础之上,结合开放式 结构运动控制器的特点及其工业应用的实际需求,对新一代 DSP 多轴运动控制器的运 动规划及控制算法进行了系统、深入的理论和实验研究,取得了一些新进展。现将研究 结果归纳如下: 一.为避免速度或加速度突变而造成的对机械传动部分的冲击,建立了三种可选的 加减速速度模式(Profile) ,即梯形(Trapezoidal) ,双 S(Sinusoidal)和双 P(Parabolic) 。 实验表明:就目标系统的平稳运动而言,采用梯形模式较好;双 S 模式更好;双 P 模式 最好。但就 PTP(Point to Point)控制的过渡过程而言,梯形模式最短,且伴随一定的 冲击;双 S 模式较长,且没有冲击;双 P 模式最长,也没有冲击。实际应用中应当综合 考虑平稳性与速度要求来确定采用哪种模式。 二.对平面和空间的圆弧、椭圆弧运动规划,选取弧长、切向速度和切向加速度作 为插补变量,然后利用空间微分几何,将得到的运动参数映射到各坐标轴做联合运动, 从而实现平稳、快速、高精度的弧线运动。此法的重要特点之一是插补计算过程的判据 简单,速度快,精度高。在椭圆弧运动规划的过程中碰到了椭圆积分问题,本文另辟蹊 径,用一种压缩圆的方法使椭圆弧的规划算法与圆弧的规划算法在运算量上保持在同一 个数量级内,从而满足了实时插补的需要。 三.探讨了三次样条曲线运动规划的插补模型和算法,不但丰富了该轨迹规划器的 运动类型,避免了用直线和圆弧逼近复杂曲线时过渡点的切线不连续现象,还可以更圆 滑地实现工件轮廓曲线的加工。 四. 将速度、 加速度前馈引入到经典的 PID 算法模型中, 减小了位置跟踪滞后误差, 进一步提高了系统的动态性能。 五. 完成了所提出的算法在企业等离子切割机运动控制系统以及实验室运动控制平 台上的实际运行实验,取得了良好的控制效果。实验结果表明:用 DSP 作为运动轨迹 规划的插补计算芯片大大提高了运算的速度和精度,同时也极大提高了同步运动的精确 性和空间的多轴协同工作的能力[63]。 而这一点对于三维的数控加工和多自由度机器手的 操作尤为重要;PID 控制作为工程中最广泛应用的控制策略,其鲁棒性、可靠性以及操 作的便捷性可以满足一般数控系统的要求[29]; 速度前馈和加速度前馈可以在一定程度上 减小甚至消除跟随误差,从而改善系统性能。 综上所述,运动规划和控制策略是新一代 DSP 多轴运动控制技术的核心技术,它在 一定程度上决定了运动控制系统的性能。理论研究和实验均表明,本文所建立的运动规 划算法和控制策略可以用于新一代 DSP 多轴运动控制器,构成高速、高精度运动控制系
- 101 -

厦门大学硕士论文

第八章 结论与展望

统。注意到由于多轴运动控制器的目标系统可以是复杂的机器人或数控系统,实际应用 时,还有许多问题需要做进一步深入研究。例如对机器人的运动规划,除了本文研究的 情况外,还要结合机器人的运动学,才能得出机器人的运动规划器。此外,由于机器人 系统是一个藕合的强非线性系统,为了获得优良的动态性能,必须考虑其他高级运动控 制策略,例如自适应 PID 控制算法。这些都是本文的后续研究课题。

- 102 -

厦门大学硕士论文









本论文是在厦门大学周学才教授的悉心指导下完成的。在三年的研究生学习、科 研和生活中,周老师为我倾注了大量的心血,感激之情难以言表。本人在论文中取得 的每一点进步,无不凝聚着恩师的心血与汗水,他不但给我提供了一个绝佳的科研和 实践环境,而且还在生活上给予了细致入微的关怀,在此谨向他表达衷心的感谢和深 深的敬意。 特别感谢教研室陈新老师对我一如既往的关怀。无论在深圳的公司还是在厦门大 学,陈老师都给予了本人亲人般无微不至的照顾。不但帮我排解了众多工作和生活中 的困难,还在潜移默化中教会了我许多做人的道理和处世的方式,替我营造了一个和 谐的周边环境,使我得以专注于科研和学习。 还要感谢黄元庆教授在这三年的学习生活中对我一贯的支持和特别的关照,以及 在最后评审工作中对我学习成果的肯定和对我个人事业的鼓励。 感谢华南理工大学郑时雄教授,厦门大学陈朝教授,孙道恒教授,郭隐彪教授以 及刘守教授在我论文改进过程中提出的有益建议。 同时感谢深圳摩信公司原技术研发部门的同事康工、程工、邢工、马工等,他们 都是行业内的技术精英,那段共事的日子是充盈和令人兴奋的,加速了本人在该行业 内的技术积累过程,论文中的一部分内容是大家共同努力的成果。还要感谢公司市场 部的黄增主任,他教会了我市场的运营规则和一些管理方面的理念,使我的收获不仅 仅局限在技术领域内。还要感谢坚达切割机项目小组的彭工、黄工、孙工以及小石头 (张琦) ,大家在一起度过了一段艰辛且难忘的日子。 最后,感谢厦大武术队的小师妹夏丽君、孟梦、王惠敏在论文的完成过程中帮我 校正文稿和下载资料,感谢来自瑞典的朋友 MICHAELA 帮我订正英文摘要部分,感 谢室友马海阳在 VC 编程调试中的有益探讨,还感谢教研室学弟学妹在搭建实验平台 中的共同努力。此外,还得到了其他老师和同学的帮助,在此一并向他们致以诚挚的 谢意。

作者发表的与本课题有关的学术论文
1. 2. 空间椭圆弧轨迹规划的实现方法,机器人技术与应用,2002 年第 3 期
MCT8000 运动控制卡在切割行业中的应用,制造业自动化,2003 年第 3 期

- 103 -

厦门大学硕士论文

参 考 文 献

参 考 文 献
[1]秦忆,周永鹏等,现代交流伺服系统 [M],武汉 :华中理工大学出版社,1 995 [2]郭庆鼎,王成元 ,交流伺服系统 [M],北京 :机械工业出版社,1 994 [3]Tung E D, Toth izuka M, High-speed end thilling using a feed forward control architecture[J], Journal of Manufacturing Science and Engineering, 1 996, [4]刘乐善主编,微型计算机接口技术及应用,华中理工大学出版社,2000 [5]L in Faa-jeng,Real-time IP positioning controller design with torque feed-forward control for PM synchronousmotor[J], IEEE Trans, 1 997, 44(3 ):3 98-40 7 [6]朱心雄,自由曲线曲面造型技术[M],北京:科学出版社,2000, [7]Shiller Z,Motion Planning for Mars Rover[A],First workshop on Robot Motion and Control,Kiekrz,Poland,1999, [8]钱东海、马毅潇、赵锡芳,双臂机器人时间最优轨迹规划研究[J],机器人,1999 [9] Ye Bosheng et al,A High Speed Interpolation Algorithm for a Third-order B-spline Curve in CNC Systems, China Mechanical Engineering[J],1998,9(3) [10] Jean-Claude Latombe, Robot Motion Planning[M], Kluwer Acadmic Publishers, Boston, 1991 [11] 吴为民、冯培恩,机器人相平面高效建模方法[J],机器人,2000,22(2) :102-107 [12] deBoor,C,On Calculating with B-Spline,J,Approx, Theory,1972,6:50-62, [13]Kahn M E,Roth B, The Near minimum Time Control of open Loop Articulated Kinematic Chains, ASME Trans, Journal of Dynamic Systems, Measurement, Control1 and 971,93 (3 )164-172 [14]Kim B K,Shin K G, An Efficient Minimum time Robot Path Planning Under Realistic Conditions, Procof1 984American Cortrol Conference,296-303 [15]L in C S,Chang Po Rong and L uh JY S,Formulation and Optimization of Cubic Polynomial Joint Trajectories for Industrial Robots,IEEE Trans,on Automatic Control,Vol AC- 2 8,No,12,1983 , [16]Paul R P, Manipulator Cartesian Path Control,IEEE Trans,Systems,Man,and Cyberneties,Vol SMC - 9,No,11,702-711 , [17]Fu K S,L ee C S G,etc,Robotics:Control,Sensing,Vision,and Intelligence,Mc Graw Gill BookCompany,1 987, [18]Chang Y H,Tee T T,On Line Approximate Cartesian Path Planning for Robot Manipulators,IEEETrans, on Systems,Man &Cyberneties,1992,2 2 (3 ) [19]Chen Y C, Solving Robot Trajectory Planning Problems with Uniform Cubic B splines, Optimal Control Applications &Methods,1991,12 (4):247-262
- 104 -

厦门大学硕士论文

参 考 文 献

[20]Xu Xiangrong, Xiangfeng, Method for Trajectory Planning of Robot Manipulators, Ma A Modelling,Measurement &Control,B,AMSE Press,1994,54(3 ):33-36 [21]徐向荣,马香峰 ,机器人运动轨迹规划分析与算法 ,机器人,1988 [22]恽为民,席裕庚 ,基于遗传算法的机器人关节空间最优运动规划。机器人,1995 [23]Xu Xiangrong, Trajectory planning of Robot Manipulators in Cartesian Space, Proc, of Ninth World Congress on the Theory of Machines and Mechanisms,Italy1 995 [24]谢元旦, 吴胜, PID继电自整定几种算法的比较 [J], 自动化与仪器仪表, 1993, (2 ):2 0 -2 4, [25]陈福祥, 杨芝雄, PID调节器自整定的PM法及公式推导[J], 自动化学报, 1993, 1 9(6 ):736-740, [26 ]绪方胜彦 (日 ),现代控制工程 [M],北京 :科学出版社, 1987, 52 -1 40 , [27 ]孙增圻 ,计算机控制理论及应用 [M],北京 :清华大学出版社, 1989 [28]曹承志编著,微型计算机控制新技术,机械工业出版社,2001 [29]陶永华,新型 PID 控制及其应用 [M],北京:机械工业出版社,1998,56-80 [30]张洪润,实用自动控制 ,四川 :科学技术出版社, 1990 , 212-320 [31]常健生,检测与转换技术 ,北京 :机械工业出版社, 1 981 ,339-347 [32]方康玲等,微型计算机控制系统分析与设计 ,北京 :科学出版社,1992,98-111 [33]王锦标等,过程计算机控制 ,北京 :清华大学出版社,1992,23-42 [34]王永初 ,专家系统智能调节器的发展评述 [J],自动化仪表,1993,13 (1 ):1-4, [35]叶向前,夏春明,专家 PID 自整定算法及其应用[J],自动化仪表,1993 [36]倪忠远,赵耿,PID AGC 的现状 [J],工业仪表与自动化装置,1996 (6 ):1 3-1 9, [37]赵有洁,国外公司研制的自整定PID控制器及其自整定方法[J], 工业仪表与自 动化装置,1995 [38]杨彬彦,ATloopPID 自整定软件包的开发与应用 [J],化工自动化及仪表, 1995, [39]田立俭 ,编制曲线型零件程序的新方法 ,机床,1991 (8):3 5-3 8 [40]徐志刚,范燕新 ,计算机辅助非圆曲线的数控编程 ,机床,1991 (6):1 9-2 1 [41]李恩林 ,插补原理 ,北京 :机械工业出版社,1984, 1 1 0-1 88 [42]朱国力,段正澄等 ,基于圆心角分割的椭圆插补算法研究,机械工业自动化,1996 [43]褚秀萍,彭锡文,数控陶瓷成形中的椭圆插补方法 ,组合机床与自动化加工技术, 1 996(4):3 7-3 9 [44]谭伟明,胡赤兵等,斜率累积直线插补原理及其应用 ,甘肃工业大学学报,1997 [45]David Rathbun,“Preliminary Pluto Open Loop Slew Performance”,JPL Interoffice Memo IOM 434-94-205,May 1994, [46]E,Ali and E, Zafiriou, “Optimization-based tuning of nonlinear model predictive control with state estimation”,Process Control,2 (3),1993,
- 105 -

厦门大学硕士论文

参 考 文 献

[47]C, Garcia and M, E, Morari, “Internal Model Control: Unifying review and some new A results”,Ind,Eng,Chem,Process Dev,21:308-323,1982, [48]Prett D, Garcia C, and Morari M, E, “Model Predictive Control: Theory and Practice-A Survey”,Automatica,25,1989, [49]Seereeram S, Divelbiss A, Wen J, “A Global Approach to Kinematic Path Planning and for Robots with Holonomic and Non-holonomic Constraints”,In Proceedings of the 1993 IMA Workshop on Robotics,Minneapolis,MN,January 1993, [50]Lizarralde F, Wen J, “Attitude Control without Angular Velocity Measurements: and T, A Passivity Approach”,To appear:IEEE Transactions on Automatic Control,1996, [51]Seereeram S,and Wen J,T,“A Global Approach to Path Planning for Redundant Manipulators”,IEEE Transactions on Robotic and Automation,11(2),February 1995, [52]Divelbiss A , and Wen J , T , “Non-holonomic Motion Planning with Inequality Constraints”, In Proceedings of the 1994 IEEE Conference on Robotics and Automation, San Diego,CA,May 1994, [53]Divelbiss A, and Wen J,T,“Trajectory –Tracking Control of a Car-Trailer System”, To appear:IEEE Transaction on Automatic Control,1996 [54]李友善主编,自动控制原理,国防工业出版社,1981 [55]秦寿康编著,最优化理论和方法,电子工业出版社,1986 [56]秦寿康,张正方,最优控制,电子工业出版社,1984 [57]张志勇等著,精通 Matlab5.3 版,北京航空航天大学出版社,2000,8 [58]冯纯伯,田玉平等著,鲁棒控制系统设计,东南大学出版社,1995,12 [59]徐昕,李涛等著, Matlab 工具箱应用指南-控制工程篇,电子工业出版社,2000,5 [60]孙世贤,黄圳圭主编, 理论力学教程,长沙:国防科技大学出版社,1997 [61]俞忠原,陈一民编著, 工业过程控制计算机系统,北京理工大学出版社,1995 [62]陶永华主编,新型 PID 控制及其应用,机械工业出版社,2002 [63]谢万德,基于 DSP 的多轴运动控制器的研究, 浙江大学硕士学位论文,2002 [64]刘广超,周学才,空间椭圆弧轨迹规划的实现方法,机器人技术与应用,2002 [65]刘广超,周学才,MCT8000 运动控制卡在切割行业中的应用,制造业自动化,2003 [66]闫砺锋,运动控制技术研究及运动控制板卡开发,四川大学硕士学位论文,2001 [67]陈文泉, 刘永钦, PC-Based 运动控制技术之现况与发展, 台交大工研院机械所, 2001 [68 范大鹏等,方兴未艾的开放式运动控制技术,组合机床与自动化加工技术,1999 [69]周学才,陈新等,新一代开放式结构智能 DSP 运动控制系统,厦门科技,2000

- 106 -


相关文章:
运动控制技术
充分利用 DSP 的计算能力,进行复杂的运动规划、高速...算法实现、 BODE 图的绘制和数学模型的建立做了研究...构建运动控制器,选择配置,如控制轴数、控制方式、 ...
基于STM32与CPLD双轴运动控制器的设计与实现
运动控制器已经从单片机或专用芯片(ASIC)作为核心处理器,发展到了以 DSP/ARM ...充分利用高速处理器的计算能力,进行复杂的运动规划、高速实时多轴插补、误差补偿,...
多轴机械手控制系统的研究_图文
为 Z ,每 一都有同步带带动各方向上的运动...机械手控制系统通常是单片机或者 DSP 等微控制芯片为...3.2 轨迹规划算法研究 针对有效工作区中直线 CD ...
文献综述
交流不调速的格局;二十世纪后期, 交流调速兴起, 运动控制系统进入了一个全新的...充分利用 DSP 的计算 能力,进行复杂的运动规划、高速实时多轴插补、误差补偿和...
电力电子运动控制
高速、 高 精度始终是运动控制技术追求的目标。充分利用 DSP 的计算能力,进行复杂的运动规划、 高速实时多轴插补、误差补偿和更复杂的运动学、动力学计算,使得运动...
开放式多轴运动控制系统开发
基于计算机标准总线的运动控制器 基于计算机标准总线的运动控制器 这种运动控制器大都采用 DSP 或微机芯片作为 CPU,可完成运动规划、高速实时插 补、伺服滤波控制和...
基于ARM+DSP的小型地面移动机器人控制系统
基于ARM+DSP的小型地面移动机器人控制系统_电子/电路...总线和 I/O 通道分别传送给运动规划层和运动控制层...+多轴运动 控制卡、多机主从分布控制方式等多种...
通用运动控制技术现状、发展及其应用
充分利用 DSP 的计算能力,进行复杂的运动规划、高速实时多轴插补、误差补偿和更...一代控制器)研究计划”,该计划首先提出了开放体系结构控 制器的概念,这个计划...
DSP控制器原理 复习大纲
DSP控制器原理 复习大纲_工学_高等教育_教育专区。dps的部分复习知识 15-16-1 DSP 控制器原理 复习大纲 答案 2016-1-4 一、DSP 控制器基本概念题 1. DSP ...
DSP控制器原理 复习大纲 答案2016-1-4
DSP控制器原理 复习大纲 答案2016-1-4_工学_高等教育_教育专区。一、DSP 控制器基本概念题 1. DSP 控制器与其他微处理器的主要区别是什么? 答:外设硬件丰富...
更多相关标签:
dsp控制器原理及应用 | dsp控制器 | dsp运动控制器 | 电机控制器 mcu dsp | svg控制器算法 | pid控制器算法 | 运动控制器算法 | 无功补偿控制器算法 |