多自由度机械臂控制算法设计..

摘要

机器人是一种能够进行编程并在自动控制下执行某些操作和移动作业任务的机械装置。而机械臂作为机器人最主要的执行机构，是一个十分复杂的多输入多输出非线性系统,它具有时变、强耦合和非线性的动力学特征，因其控制的复杂性引起了相关从业人员的广泛关注。随着时代的进步，像军事制造、工业生产、日常生活及教育娱乐等各个领域对机器臂控制技术应用需求逐渐加大，从而使得设计一套工作空间大，运动灵活的多自由度机器臂尤为重要。

机械手臂运行轨迹追踪控制技术有包括：adaptive control(自适应控制)、smvsc(滑模变结构控制)、Robust adaptive control(鲁棒自适应控制)、Fuzzy adaptive(模糊自适应）等四大类。本文主要运用模糊PID控制设计二自由度机械臂控制算法，该控制方法具有模糊控制灵活和适应性强的优点，也具有经典PID控制精度高的特点。

本文围绕二自由度机械臂控制算法设计，首先建立二自由度关节型机械臂的数学模型，即二自由度机械臂输入驱动力矢量和输出转动角度矢量之间的函数关系。然后运用模糊PID控制设计一套机械臂轨迹规划算法，能够根据使用者的作业任务要求，求出二自由度机械臂终端执行器的轨迹。并研究如何对于给定的系统设计出PID控制器，实现控制系统的输出对参考输入跟踪，以及对扰动输入响应具有较小的振幅，且能够衰减到零即无稳态误差。最后给出了基于MATLAB/SIMULINK软件的案例分析,阐释模糊PID控制算法行之有效性。

关键词:多自由度，机械臂，PID算法控制，数学模型

Abstract

Robot is a kind of programming and perform certain operations and mobile task mechanism in automatic control. And robot arm as the main executive body, is a very complex multi input and multi output nonlinear system, it has a time-varying, strong coupling and nonlinear dynamic characteristics, due to the complexity of the control caused wide attention of practitioners. With the progress of the times, like military manufacturing, industrial production, daily life and entertainment, education and other fields of a robot arm control technology application requirements gradually increase, from the design a large working space, the flexible movement of the multi degree of freedom robot arm is particularly important.

Mechanical arm trajectory tracking control technology including: adaptive control, SMVSC, robust adaptive control, fuzzy adaptive etc.. In this paper, the use of fuzzy PID control design for two degree of freedom manipulator control algorithm, the control method with fuzzy control of a flexible and adaptable advantages, also has the classic PID control the characteristics of high precision.

This paper focuses on the design of control algorithm of two degrees of freedom manipulator, a mathematical model of two-DOF Manipulator, namely two-DOF Manipulator driving force and rotation angle between the output function. Then use the control to design a manipulator trajectory planning arithmetic of fuzzy PID method, according to the user's task requirements and for two degrees of freedom manipulator end effector trajectory. And study how to design a system for PID controller is given, to achieve the output of the control system of the reference input tracking, and disturbance input response amplitude is smaller, and can decay to zero no steady state error. Finally, a case analysis based on MATLAB/SIMULINK software is presented to illustrate the effective of PID fuzzy control algorithm.

Key Words: Multi degree of freedom, manipulator, control, PID algorithm, mathematical mode

目录

摘要.............................................................................................................................. I Abstract........................................................................................................................ II 1 绪论 ......................................................................................................................... III

1.1 本文研究的目的与意义 (1)

1.2 机械臂控制算法研究现状 (1)

1.3 本论文的工作总结 (3)

2 机械臂控制系统概述 (4)

2.1 二自由度机械臂的动力学控制模型 (4)

2.2 二自由度机械臂运动学正解 (5)

2.3 二自由度机械臂运动学中的反解 (7)

2.3.1 运动学反解的不唯一性 (8)

2.3.2 在运动学反解中出现的个别情形 (9)

3 控制算法设计 (10)

3.1 PID的概述 (10)

3.2 模糊PID控制 (12)

3.3 二自由度运动路径规划 (13)

3.3.1 直角坐标空间中规划算法 (13)

3.3.2 直线插补和圆弧插补算法 (16)

3.4 模糊PID控制算法设计 (17)

4 算例分析 (20)

4.1 二自由度机械臂PID模糊控制器的建模 (20)

4.1.1 模糊PID控制 (20)

4.1.2 PID模糊建模 (21)

4.1.3 PID模糊控制 (22)

4.2 设计模糊控制器规则以及其仿真分析 (22)

4.2.1 模糊控制器规则 (22)

4.2.2 PID参数的模糊整定 (25)

4.3 仿真结果分析 (30)

总结 (32)

致谢 (33)

参考文献 (34)

1 绪论

1.1 本文研究的目的与意义

机器人是一种能够进行编程并在自动控制下执行某些操作和移动作业任务

的机械装置。机械臂作为机器人最主要的执行机构，对它的研究越来越受到工程技术人员的关注。它涉及的学科有材料科学、控制技术、传感器技术、计算机技术、微电子技术、通讯技术、人工智能、仿生学等等很多学科。

一个机械臂系统主要包括机械、硬件和软件、算法这四个部分。到具体设计需要考虑结构设计、控制系统设计、运动学分析、动力学分析、轨迹规划研究、路径规划研究、运动学动力学仿真等部分。对于一套轻便型机械臂的研发，需要把各个部分紧密联系，互相协调设计。随着时代的进步，机器臂技术的应用越来越普及，已逐渐渗透到军事、航天、医疗、日常生活及教育娱乐等各个领域。目前实际应用的绝大多数机器臂都是固定在基座上的，它们只能固定在某一位置上进行操作，因而其应用范围多限于工业生产中的重复性工作。于是实际生产生活中迫切需要一种活动空间大，能适用于各种复杂环境和任务的可移动机器人。由于移动机器人工作空间大、运动灵活等优点，对它们的研究也是越来越多，但是这种机器人很多都是实现移动的，并没有可控制的手臂，所以没有抓取物体的功能。为了让移动机器人能够完成简单的作业，在它上面安装两只轻型服务型机械臂显的尤其必要。

1.2 机械臂控制算法研究现状

最早的机械臂是1962年美国联合公司制造的名为Unimate的机械臂，该系统的设计参照坦克塔台，其应用于将一些配件传送到生产线，其控制系统是一个大型计算机[7]。在文献[8]中，Liu Xinjun等人对二自由度并联机器人的运动学和动力学进行建模分析，并对二自由度并联机器人的综合性能做了深刻探讨，并提出新的研究方法。Kim J Y对二自由度五连杆机械臂进行运动学的研究分析[9]。在对机械臂实现控制时，控制器的设计过程采用无模型的控制思想，例如可以通

过神经网络滑模变结构控制(NN-SMVSC)等方法实现，即能够保障控制器的效率，同时，又能避免惯量矩阵或逆矩阵的计算[10]。

在文献[11]中，Su考虑了在执行器约束的情况下，针对两关节机械臂，研究了机械臂的全局输出反馈整定问题，并结合PID控制与补偿措施方法，通过Lyapunov方法证明了闭环系统的全局渐近稳定性。

Liuzzo等[I2]提出了一种不依赖于模型的控制器，该控制器在对机械臂实现控制时，仅依赖于输入的周期信号值及机械臂的动力学的常数界限。通过对每个关节的输入信号进行傅立叶级数展开，判断控制系统的全局稳定性和局部稳定性。当给定的输入信号的傅立叶级数展开有界时，可以获得全局稳定和局部指数稳定的误差动力学，使跟踪轨迹的误差达到任意精度。同时，自适应PID瓜控制器通过辨识输入信号的傅立叶系数来学习输入信号。

Purwar等[13]超出了Chebyshev自适应神经元控制器，该控制器的设计主要考虑了机械臂执行器的约束，并估计系统负载变化、未知非线性LJ及带干扰的输入力矩等结构化或非结构化的不确定性。该方法设计的控制器避免了速度信号通过位置微分获得而容易掺入噪声的缺点，只依赖于关节位置信息，能够实现神经网络逼近与动态滤波器的融合。

文献[14]主要针对系统在具有结构不确定性和非结构不确定性时，提出一种模糊自适应控制方法，并对机械臂的轨迹跟踪控制进行实验。当机械臂的关节速度不可测时，引入速度观测器，设计了自适应输出反馈控制器。通过研究机械臂的动力学特性，并对不确定性项进行解稱，就能够降低模糊控制器模糊规则的数目，简化控制器。当机械臂的关节速度可测时，通过设计全状态模糊自适应反馈控制器，可保证系统闭环动力学稳定。

国内的许多单位自从上世纪80年代开始，也开始重点研究机械臂的控制系统。例如，清华大学、浙江大学、沈阳自动化研究所及上海交通大学等学校或研究所在国家高新技术计划自动化领域智能机器人专题中做了很好的带头作用，且收获颇多[15-16]，促进了国内对机械臂的研究。

在文献[17]中，王启明和汪劲松对二自由度的并联机器人操作臂进行了运动学和动力学建模，并进行仿真。在文献[18]中，北京工业大学的刘善增对平面二自由度并联机器人进行了动力学设计研究，但是并没有对并联机器人作控制研

究。陈国栋等[19]，在对机器人的研究中，采用经过滤波器作用的位置误差信号作为反馈信号，并用＂参考速度＂来替代非线性部分输出端的实际速度信号，仿真结果中取得了满意的跟踪效果，该文献的缺点是该方法必须基于精确的动力学模型。目前国内已提出很多抑制抖振的方法[20]，例如，滑模控制通常与模糊控制、神经网络控制等方法相结合，来消除机械臂控制输入的抖振现象。新的滑模面及控制方式也被不断提出并应用到机械臂轨迹跟踪控制领域。

1.3 本文主要工作内容

本论文第二章主要就是构建二自由度机械臂动力学控制模型，首先详细分析二阶机械臂运动学解，定义各个目标变量，然后寻找输入机械臂驱动力矩矢量和输出机械臂转动矢量的数学约束关系。第三章主要就是介绍二自由度机械臂控制算法设计，详细介绍了PID控制和模糊PID控制的基本理论，比较二者优缺点，以及各自适宜的情况和约束条件。第四章主要就是叙述基于MATLAB/SIMULINK软件的案例分析,在Simulink仿真环境下搭建完整控制系统得到给定轨迹曲线、跟踪轨迹曲线、以及误差轨迹曲线。对仿真结果和误差来源进行分析，得出结论，并阐释模糊PID控制方法的有效性和可行性。

2 机械臂控制系统概述

因为机械臂随着自由度个数的增大，控制系统的复杂程度加大，但是其基本

原理类似，所以本文主要以二自由度机械臂为例阐释多自由度的基本原理，

2.1 二自由度机械臂的动力学控制模型

机械臂的运动方式在运动空间上来分一般而言有两大类，即关节型和直角

型。各个关节的的运动直接取决于它的运动坐标，把全部的关节变量设为一个关

节矢量，全部的关节矢量组成的集合则为它的关节空间。通过控制所有关节的移

动来控制机器人的移动便为关节空间运动模式。机械臂终端对象的具体地点和角

度一般在直角坐标空间中表现出来，这种模式就是称为直角坐标运动模式，在这

种运动模式中，机械臂用户规定的任务是由机械臂末端对象在直角坐标空间中的

移动来实现的。

就操作者而言，直角坐标空间更为人们接受，故而在直角坐标空间中进行对

机械臂的操作。因此我们就要在关节坐标和直角坐标之间建立一一对应的数学约

束关系。换言之，如知道机械臂每一个关节的坐标常数，就需要求解它的终端在

直角坐标空间中的具体位置坐标，这个被叫做为运动学正解;反之，如知道它末

端在直角坐标空间的坐标就需要求解各个运动关节的坐标参数，这个则被称为运

动学反向解。

设],...,,[21n q q q q =为机械臂在关节坐标空间中的变量， ...],,,[θz y x x =为它

的终端对象在直角坐标空间中的具体位置坐标，那么它们两个之间的关系为:

0),(=q x F (2.1)

上式是一个隐式方程，若能够从中求解出:

)(q f x = (2.2)

那么直角坐标变量由关节坐标变量来定量表达，就能够得到机械臂的运动学正

解。通常而言，可以从式(2.1)得到它的的惟一正解，得到的正解为:

)(x g q = （2.3）

然而一般情况下，我们不容易获得它的运动学反解，是因为它的反解通常就是多

解，所以在现实情况下，通常运用几何机械臂的运动学解。

2.2 二自由度机械臂运动学正解

已知:关节1连杆长度1l ，关节的值为1q (如下图所示);关节2连杆长度2l ，

关节的值为2q (角度如下图所示)

求解:记下图中关节连杆末端对象固定点的直角空间坐标:],[y x x =

图2.1 机械臂运动学的正解

机械臂操作端的位置方程为:

???++=++=)sin(sin )cos(cos 21211

21211q q l q l P q q l q l P y x （2.4） ()????????=???????

?21θθθ J P P y x （2.5） 上述方程的Jacobian 矩阵()θJ 为

()()()()()??????++++-+--=21221211

21221211cos cos cos sin sin sin θθθθθθθθθθθl l l l l l J 机械臂的动力学方程为

()()()τθG θθN θθM =++, （2.6） 其中，[]T 21ττ=τ为总的驱动力矩矢量，[]T 21θθ=θ为转动角度矢量；

()θM 为转动惯量矩阵，()

θθ

N , 为Coriolis 离心转矩，()θG 为重力矢量。()θM

、

()θθ

N , 、()θG 的数学表达式如下： ()()()()()θJ θJ θM T 3223223221

cos 2cos 2cos m a a a a a a a +??????+++=θθθ （2.7）

()()()()()()θθθJ θJ θθN ,,T 2122222122m sian a sian a +??????+-=θθθθθθ （2.8）

()()()()g θJ θG T 21521514cos cos cos m a a a +??????+++=θθθθθ （2.9）

其中 []

T 21212251211422232122212222211181.90 ,10 ,432.0 ,36.11,19.15 ,2

1 ,2131 , ,3131=======+===++=g kg m m l l kg m kg m gl m a gl m gl m a l m a l l m a l m l m l m a 定义状态变量

[][]4321T 2124132211,,,,x x x x x x x x x x ======X X θθθθ （2.10） 设

()()()()()[]2

1 ,,, ,τττθθθθθθθ==--==u G N E M D （2.11）

则机械臂模型的状态方程可以写成 ()()()()()∑+=????????????+??????????=????????????--i

i u x g x f u u x D x E x D x x x

x x x 021******** （2.12）

系统输出方程为

()()()()()()??

????++++=??????=??????=??????==21211212112121sin sin cos cos x x l x l x x l x l x h x h P P x x h y x X h X y （2.13）

2.3 二自由度机械臂运动学中的反解

已知:记关节1连杆长度为1l ，关节2连杆长度为2l ，关节连杆末端的点在平

面坐标空间的坐标为：],[y x x =

求解:记关节值为1q (角度如下图所示)，关节值为2q (角度如下图所示)。

图2.2 机械臂运动学的反解

如上图所示，在不考虑奇异点的情况下，得到如下公式:

22y x r += （2.14） 2

1212132cos l l r l l -+=? （2.15） 323cos 1sin ?-=? （2.16） r

l 321sin sin ?=? （2.17） 1

212112c o s rl r l l ++=? （2.18） )cos ,(sin 2tan 111??=?a （2.19） ),(2tan 2x y a =? (2.20)

))cos (),sin ((2tan 11113q l x q l y a --=? (2.21)

求解可知运动学的反解计算公式:

???-?=?-?=1

42121q q q （2.22） ),tan(y x a 是用来计算y x 的反正切值(单位为弧度)的数学函数。从得知的运动学

反解上来看，可以获得是非线性系统。

2.3.1 运动学反解的不唯一性

图2.3 机械臂的两个反解

它的运动学反解一般多解，会存在有好几组关节变量终端在用户要求的位置

点。图2-3很好的反应二自由度机械臂会在工作空间中运动时有两组运动学反解

这一情况。

综上所述，在实际操作中从反解中的多解问题找到一组最优解是一个重要问

题。通常情形下可以用优化算法来处理这种多组反解问题。当设置此算法的优化

准则时，可以选取的优化准则有两种情况:

①当没有障碍物的时候，就用“最短”原理。由于机械臂的动作是持续存在

的而非间断的，可以根据之前机械臂运动反解获得的关节坐标值，能够得到机械

臂关节运动最短的解确定为反解，所以使之保持连续。

②当有障碍物的时候，沿“最短”轨迹原则会发生碰撞，那么就综合选用机

械臂的有障碍路径轨迹，算法等方法来得到反解。

2.3.2 在运动学反解中出现的个别情形

当目标点位置),(y x 在机械臂运动空间范围之外，就用下面的公式判断: 2

12222212)(l l y x l l +-+=? （2.7） 如果满足： ε+>?1 （2.8） 就能够判断特定地点在机械臂的运动空间范畴之外，换句话就是说运动学反解无

解，式（2.8）ε为一无穷小正分数，譬如选取

8

10-=ε （2.9） 有一个特殊情况就是目标位置正好落在机械臂工作空间边界上，在这种情况

下（奇异位置），机械臂的移动情况就变得差了，控制的轴移动也许会发生跳变

的情况。这种情况下奇异情况的判别条件为:

)1(ε--

图2.4 反解中的特殊情况

根据以上可得到在特殊情况的反解为:

???==0)

,(2tan 2

1q x y a q （2.11）

3. 控制算法设计

3.1 PID 的概述

从ziegler 还有Nichols 提出PID 参数整定概念起，把PID 控制器的自动和手

动整定的思想理念用在诸多科学技术之上。并且PID 控制为目前最常用的方法，

被用于很多反馈控制亦或是其不大的变形控制。PID 调节器跟它的优化型在工业

的控制中最普遍。迄今为止，百分之八十四的依旧是单纯的PID 调节器，然而

优化型就包括在其中的就在百分之九十以上。

PID 控制器作为使用最广泛的控制器，为微分、比例、积分并联控制器。PID

控制器的数学模型可以用下式表示: ])()(1)([)(?++

=dt

t de T dt t e T t e Kp t u d i （3.1） 其中:)(t u 一控制器的输出

)(t e 一控制器输入，即误差信号。

Kp 一控制器的比例系数。 Ti 一控制器的积分时间。

Td 一控制器的微分时间。

在PID 控制器中，其数学模型由微分（D ）、比例（P ）、积分（I ）三部分

组成。这三部分分别是:

①比例部分数学式表示如下:

)(t e Kp ? （3.2）

控制器的功能随着偏差的出现而出现，并且让其偏差朝向减小的方向变动。

控制功能的程度由比例系数Kp 决定。随着比例系数增大，那么过渡过程就会减

短，伴随着控制结果的稳态误差也减小；然而Kp 愈大，其超调量愈大，产生振

荡的可能性加大，致使动态性能变坏的劣势，更有甚者会令其闭环系统不稳定。

为了达到过渡时间少和稳态误差小的良好实验结果，比例系数Kp 的择取是一定要非常合适。

②积分部分数学表达式表示如下式所示: ?dt t e T K i p

)( (3.3)

根据式（3.3）可知，只要存在误差，控制功能一直积累，输出控制量是很难去消除误差当存在误差的时候。可知积分部分的功能是可以消除系统的误差。但因为它具有滞后性，会使得积分控制功能很强将使系统超调不能够往变小的方向变化，其动态机能变弱，甚至会造成闭环系统不稳定的情况。Ti 对积分部分有着极大的控制能力，Ti 愈大积分功能愈弱。这种情况下对于变小系统超调有着极大的优势，过渡过程很难产生振荡。却会使得消除误差所需时间增长。当Ti 愈小积分功能愈强。这种情况下系统过渡过程中会产生振荡，使得消除误差所需的时间减少。

③微分部分数学表达式表示如下: dt

t de Td kp )(? （3.4） 微分部分能够有效掌控误差的变动趋向，加大其控制功能能使系统更快地反应，震荡不变大，系统的稳定性增强，其劣势就是降低了系统抵制外界扰动的功能。微分时间Td 长短决定微分部分的功能强弱。Td 愈大它抑制)(t e 变化的能力愈强；否则，Td 愈小它反抗)(t e 变化的能力愈弱。在数字控制的计算机系统中，计算机PID 控制算法程序可以达到计算机数字控制器的掌控。通常而言，这种控制系统就是一种采样数据系统。并且在处理数字信号的问题中，均需要用数值计算来无限逼近。

因此，PID 控制规律的实现，一定要数值逼近的方法。若只有很短的采样周期，则利用积分被求和取代，差商取代微分，让PID 算法使其离散化。表述连续时间PID 算法的微分方程，替换成表述离散时间PID 算法的差分方程式，便是数字PID 位置型算式，即式（3.5）：

])1()()()([)(0Ts k e k e Td i e Ti Ts k e Kp k u k i -+++=∑= （3.5）

式中:

)(k u 一 k 采样周期时的输出

)(k e 一 k 采样周期时的误差

Ts 一采样周期

其中 Ti Ts K K P

I = （3.6） Ts Td K K P

D = 即为

)]1()([)()()(0-+++=∑=k e k e K i e K k e K k u D k i I P （3.7）

其中Kp 为比例系数，KI 为积分系数，KD 为微分系数

3.2 模糊PID 控制

在常规的二维模糊控制器中，其输入变量是偏差和输入变量的变化量。所以通常而言，就把这种控制器认为拥有Fuzzy 比例和微分两种控制功能，但是缺乏Fuzzy 积分控制功能。然而，线性控制理论的积分控制功能是抵消稳定偏差，可是动态运行反应缓慢；比例控制功能动态响应快速；然而高的稳态精度以及快速的动态响应可由比例积分控制功能来获取。所以模糊控制器中增添了PI (PID )控制策略，从而组成PI Fuzzy -(或PID )复合型控制，除了让动静态性均能有可观的提高外，也有有动态响应的速度快、超调小、稳扰动偏差小的优点。 确定PID 参数是PID 控制的重点部分，这种方法就是通过模糊控制来确定PID 参数，即利用误差变化率ec 和系统误差e 。在线修改PID 参数并且运用模糊控制规则。实现思想就是找到PID 各个参数与误差变化率ec 和误差e 之间的联系，从实验之中一直检测误差变化率ec 和误差e 。进行在线修改各个物理参数，从而可以符合在不同e 和ec 时满足对控制参数的不同用户要求。因此可以让控制对象有良

好的动性能、静态性能、计算量小，易于用单片机实现的多种优势等。其原理框图如图3.1所示：

图3.1 模糊PID控制算法流程图

3.3 二自由度运动路径规划

在得到机械臂的运动学解的情况下，当它向目标轨迹运动时，需要设计一下它的运动路径。这里有直角坐标空间中的和关节空间中的路径规划这两种路径规划算法。

3.3.1 直角坐标空间中规划算法

关节值是终极控制着机械臂终端的移动，要是可以设计关节空间中的轨迹，那么就能有效阻止发生雅可比矩阵的奇异所造成的速度不受控制的情况，还可以大大减少计算的时间。然而多数情况下，关节坐标空间跟直角坐标空间这两者非线性关系。因此能在关节空间中进行直接规划的就只能够是对路径没有要求的作业，，像是连续弧焊的对运动路径要求较高的作业，就一定只能在直角坐标空间中设计。之后便是将设计获得的直角坐标空间中的轨迹序列，利用所对应反解算法换算解得，如下图所示。

图3.2 路径规划控制流程图

有必要的话在用运动学求反解得到的关节变量空间序列来进行关节坐标空间中的路径插补。通常而言，运动控制器在执行运动轴伺服控制时，会进而对关节指定变量作插补，插补速率曲线采用的方式有两种，分别是梯形图形方式、S 形图形方式。

记在直角坐标空间中，二自由度机械臂终端对象路径空间曲线方程为:

??

???===)()()(t z z t y y t x x （3.8） 上式中，记x,y,z 分别表示机械臂在三维直角坐标空间中的位置坐标值，记t 为时间。为方便实际当中的控制，通常会选择弧长参数表示的曲线表达式，则知道起点的初始值为A s 的曲线函数为:

?????+=+=+=)()()()()()(s z s z z s y s y y s x s x x A A A （3.9） 上式中S 表示为弧长参数，由积分公式: ?++=1

0222)()()(t t dt t z t y t x s （3.10）

得到)(s t t =，然后代入式(2.12)就可以得到式(2.13)所示曲线函数。

记t ?为运动轨迹的更新时间，即每隔t ?时间就会有一个插值点生成， T 为轨迹期望的运动时间，那么所得轨迹插补序列的长度为:

t T

n ?= （3.11）

当弧长给出的速率移动曲线段，每个插补弧长的增量n s s /=?已知，就能够获得轨迹插补序列表达式:

??????+=?+=?+=)()()()()()(s k z s z z s k y s y y s k x s x x A k

A k A k ),...,1,0(n k = （3.12）

若在梯形速度曲线模式下（路径段是由减速、等速和加速构成），则使得加速度和减速度大小相等，加速和减速时间相同，如图2-6所示，取a t 为加速和减速的时间。便能够得到的路径长度表达式为:

????

??????<≤?+-+=?-<≤+?=?≤≤?=)0(;)(21)2(21)20(;21)()0(;)(212232221t t k t k a t T v at s t t T j at t j v s t t i t i a s a a a j a a j a j （3.13）

图3.3 梯形模式的路径规划

时间点不同时，遵循式子（3.13）路径的不同分段，将结果

i s 代入式(3.9),

就能解出轨迹插补位置后序列的坐标值。

在解出轨迹插补位置序列的坐标值后，会由于轨迹更新周期t ?短暂的原因

在实际操作可能使用相邻位置序列点的差分近似来进行轨迹插补速度。

3.3.2 直线插补和圆弧插补算法

在平面直角坐标系之中，二自由度机械臂只有Y X 、的两维参数值。直线插补和圆弧插补这两种方法技巧在运动控制系统中采用最广泛，一些繁杂曲面可以由直线段和圆弧段近似逼近。本文会给出直线插补和圆弧插补技术在二自由度机械臂平面直角坐标内的一个简单运用。

①直线插补算法

在平面二维直角坐标中，t ?为轨迹校正时间周期， T 为轨迹理想的的运动时间，),(00y x 为起始点， ),(11y x 为指定点。

在等速度规则之下，每一次插补弧长的进给量为22y x s ?+?=?，直线插补序列的长度为t T n ?=，对于直线规划，等弧长进给等效于等y ??、x 的进给，每一次插补Y X 、方向的变化量:

n y y y n x x x 01

01,-=?-=? (3.10)

直线位置插补序列为:

),...,1,0(,00n k y k y y x k x x k k =????+=?+= (3.11)

②圆弧插补算法

如图2-7所示，需要在平面X-Y 内作出一个从),(A A y x 到),(B B y x 、圆心角为的圆弧、r 为半径、θ以),(00y x 为圆心。求圆弧插补位置序列。