第6章机电控制原理及应用

机器运动控制的指令

生成与实现葛为民天津理工学院机械工程学院机械电子工程2023/1/311TianjinUniversityofTechnology

本章重点讨论机器运动控制原理与分类、指令生成方法以及轨迹控制的实现问题。所介绍的三种指令生成技术可用于开环系统、闭环系统以及多轴控制系统。其中，数字微分分析器用于增量控制；直线插补器可在设定点之间产生常速度变化指令；圆弧插补器可在圆周上生成一系列设定点；三次多项式样条函数轨迹规划器可以对加速度和速度受控的多轴机器方便地生成复杂的运动轨迹。最后，给出了典型系统实例。

2023/1/312TianjinUniversityofTechnology6．1运动控制原理与分类

凡是能进行自动化作业(包括加工、检测、装配及运输)的机器，都是多轴的机电一体化设备。最常见的有数控(NC)机床、工业机器人、自动测量机以及自主式自动导引车等。随着计算机数控(CNC)系统的应用，这些自动化机器的运动路径或轨迹都是可编程的，有关的运动指令都是由计算机生成的。只有当机器的各个轴沿着预定路径协调运动时，由计算机程序编制的作业才能顺利完成。因此，机器运动轨迹规划及控制是机电一体化设备的一个重要组成部分。它的优劣反映了机器的智能化程度。2023/1/313TianjinUniversityofTechnology数控机床的运动控制原理

图6-l为一种数控立铣床的信息流图。NC零件程序由穿孔纸带或磁带，或者人工操作键盘输入，经过计算机处理后，区分为工艺数据和几何数据。工艺数据由主轴速度、工具选择以及冷却液等开关功能组成。几何数据经过粗、精两次插补计算，生成运动指令信号，输入驱动工作台移动的伺服系统。只要伺服系统工作正常，数控机床就能加工出所要求的零件形状。2023/1/314TianjinUniversityofTechnology数控机床的信息流图2023/1/315TianjinUniversityofTechnology插补:就是产生一系列固定长度的步距，近似不能直接到达的几何图形，使得工具路径的最大偏差保持在一个步长以内。数控机床采用了插补器生成运动控制指令。

粗插补:采用离线近似算法，预先在整个高阶曲线上选择间隔的点列，将曲线分割为区段。精插补:在粗插补的基础上,对每一区段采用简单的直线或圆弧，或者三次多项式样条函数作近似计算，并把它们输入数控机床运动控制器实现。2023/1/316TianjinUniversityofTechnology

点位控制:将工具或零件(如机床主轴或工作台)由源点运动到规定的目标点，以便在该点进行加工作业。

数控机床根据所要完成的加工作业任务的不同，其运动控制分为两种类型：点位控制和连续路径控制，如表6-1所列。表中列出了运动控制类型、对应的路径以及应用场合。

连续路径控制:同时控制每一个轴的位置和速度，使得它们同步协调到达目标点。

2023/1/317TianjinUniversityofTechnology表6-1运动控制类型

2023/1/318TianjinUniversityofTechnology工业机器人运动控制方案

点位控制机器人:

通常用于物料搬运作业，即在一个地点抓取物体，输送到另一个地点放下。它们可以在一些不同的编程位置停留，但是，无法确定编程点之间的具体路径或轨迹。工业机器人按照控制水平可以分为三种不同的类型：

连续路径控制机器人:主要用于喷漆和弧焊，其特征是“示教”。它预先由人工将机械手沿着期望轨迹移动并记录整个运动，所获得的完整轨迹按照时间顺序点列储存在计算机中，然后再回放出来控制机器人运动。

控制路径机器人：通用工业机器人都属于这一类。轨迹是根据预先定义并存入存储器中的某些中间点由控制计算机生成的。2023/1/319TianjinUniversityofTechnology

图6-2圆柱型工业机器人运动控制

2023/1/3110TianjinUniversityofTechnology

参看图6-2，假设圆柱型工业机器人在水平面内工作，只考虑它的第二和第三连杆的运动。操作工具安装在手爪的中心，并与xy平面垂直。沿着z轴方向进给工具到目标位置。然后，沿着给定工作路径完成技术操作；或者，在路径的给定目标点上，完成材料处理或装配作业等。由于该机械手具有旋转和位移两个连杆。令q1为旋转连杆坐标，q3为位移运动坐标。注意，q2是z轴坐标，在这里为常量，图中没有表示。机械手的正向运动学方程：2023/1/3111TianjinUniversityofTechnology将q1和q3的任意两个数值转换为xy平面中的直角坐标，可以表示为

如果q1和q3是变化量，并且它们的速度值为和，那么，沿着x，y轴的瞬时速度值为此正向运动学方程定义了由机械手连杆坐标计算空间直角坐标系中的运动轨迹。2023/1/3112TianjinUniversityofTechnology

由方程(6-1)和方程(6-2)，可求得由空间直角坐标计算机械手各个连杆坐标的运动方程，即机械手的逆向运动学方程：2023/1/3113TianjinUniversityofTechnology①根据程序编译或机器人示教，可以获得运动路径各个区段的源点和目标点的坐标。例如，对于图6-2中的第一段直线，两端点的坐标分别为(xA，yA)和(xB，yB)；②通过轨迹规划器——专用计算机软件，给出希望运动路径或轨迹的中间点坐标(x，y)和每一点的速度值；③对于路径中每一点(x，y)，应用逆向运动方程决定相应的ql，q3，，值。总结以上结果，机器人运动控制方案可以采用方块图表示：

根据以上分析，机器人运动控制的一般方案如下：

2023/1/3114TianjinUniversityofTechnology图6-3运动控制的基本原理方案

2023/1/3115TianjinUniversityofTechnology

数字微分分析(DDA)法是由计算机软件生成参考脉冲序列的一种方法。它以数字量(整数)表达加速度、速度及位置坐标，采用求和公式进行加速度到速度和速度到位置的近似数值积分，从而产生增量式运动控制指令。DDA法在简易数控系统中不仅能应用于点位控制，也可完成双轴或多轴系统的连续路径控制。

下面，先讨论两种速度分布情况的DDA原理，然后把它们推广应用到直线和圆弧运动控制指令的生成。6．2数字微分分析器2023/1/3116TianjinUniversityofTechnology6．2．1常速度分布情况

假定速度v为常量，那么，位置函数p(t)可以表示为令N是n的四舍五入取整，而n为

于是，可写出等价的差分方程：P和V分别是p和v的整数。进一步，如果将速度V的量纲转换为单位采样周期的位置增量，那么，上式中的T恒等于1。因而，式(6-12)可以改写为2023/1/3117TianjinUniversityofTechnology

这样，只要采用一个时钟和两个寄存器，就可以生成常速度分布运动控制指令。其中，位置寄存器P存放P(tk)数据，速度寄存器V存放V数据。每当时钟信号电平由0变到1，V中的内容便累加到P中一次。P寄存器的溢出脉冲可作为控制步进电动机的参考脉冲。每一个参考脉冲代表一个步进角或基本长度单位(BLU)。当时钟频率固定时，输出参考脉冲速率由V的数值决定。图6-5表示了由DDA法生成常速度分布运动控制指令的原理图。2023/1/3118TianjinUniversityofTechnology

图6-5常速度分布运动控制指令生成原理图(a)常速度分布图；(b)运动控制指令生成原理

DDA法在计算机中实现时，必须采用整型变量算法，因为整数运算速度快，可产生高速率参考脉冲。这样，P(tk)，V以及N都必须用二进制整数表示。2023/1/3119TianjinUniversityofTechnology

例6-1根据下列已知信息，设计常速度分布指令信号的生成方案：总的希望转角＝1000脉冲希望速度＝51脉冲／s

时钟频率＝10kHzP和V寄存器字长＝16位带符号小数解：V寄存器中的希望值为式中，已将“Hz”转换为“加／s”。单位“脉冲／加”表示每一时钟周期的脉冲数，即V寄存器的内容向P寄存器每做一次加法所增加的脉冲数。2023/1/3120TianjinUniversityofTechnology

将这个数转换为带符号16位二进制小数：v＝0.005l＝167.1168×2－15脉冲／加。经取整

V＝167×2－15＝000000010100111＝0.0050964356脉冲／加应注意，V≠v，其误差是由于在寄存器中必须用固定位数表示v所造成的。根据这个V值，希望的时钟周期数(或输出1000个脉冲所要求的加法次数)为

经过四舍五入取整后，实际使用的时钟信号由“0”到“1”的转换次数

N＝196216加2023/1/3121TianjinUniversityofTechnology

这样，可保证实际得到的脉冲数VN＝1000，满足题意要求。实现时，P寄存器的初始内容为0，V寄存器中存入000000010100111。时钟信号由“0”到“1”每转换一次，V寄存器中的数加到P寄存器中一次。加到第197次时，P寄存器将产生第一个溢出脉冲输出，直到加了l96216次后，便完成1000个溢出脉冲输出。2023/1/3122TianjinUniversityofTechnology6．2．2常加速度分布情况

考虑到电动机加速度的限制，经常需要采用常加速度分布运动控制指令，如图6-6所示。图6-6(a)表示等加速度分布图，先由静止状态加速到步进电动机额定速度，维持等速运行，然后又等减速到停止位置。在等加速度分布情况下，数字微分分析器的计算公式如下：

式中，P，V，A分别为位置、速度及加速度。速度V的量纲为单位采样间隔(即采样周期)的位置增量。加速度A的量纲是单位采样间隔的V的增量。2023/1/3123TianjinUniversityofTechnology

图6-6常加速度分布运动控制指令生成原理图(a)等加速度分布图；(b)等加速度分布运动指令的生成原理

2023/1/3124TianjinUniversityofTechnology

图6-6(b)是生成等加速度分布运动指令的原理，由一个时钟和三个寄存器组成。每当时钟信号由“0”到“1”的转换瞬间，寄存器V中的内容加到寄存器P，同时，寄存器A中的内容加到寄存器V。P和V的初始值都为零，A在t0时刻的初始值置正的最大值。在t1时刻，V达到它的极限值。这里，t1与寄存器A的内容有关。在等加速段，寄存器P的溢出速率是匀速增长的。在加速段终点，寄存器A置0。在常速度段终点t2时刻，A置负的最大值，并且继续进行加法，直至t3时刻速度为零。加速度寄存器A、速度寄存器V以及总的步进脉冲数可由速度分布图决定。给定源点和目标点位置坐标、容许速度及容许加速度，三段时间关系式如下

2023/1/3125TianjinUniversityofTechnology式中：ax——容许加速度；vx——容许速度；x0，xf——源点和目标点位置坐标。如果已知时钟频率为f(Hz)，则每一段的加法次数为

令Ni是ni取整，那么，总的运动时间区间的加法次数为

2023/1/3126TianjinUniversityofTechnology

例6-2

生成下列等加速度分布指令信号：由零初始速度加速0.1s达到20000脉冲／min，然后恒速度20000脉冲/min维持2s。最后，减速0.1s到停止位置。减速度与初始加速度相等。假设时钟信号频率为l000Hz，寄存器字长为16位带符号小数。希望在产生该速度分布时，只改变A寄存器的内容。解：求解的问题是决定A寄存器中设置的希望值，以及每一段由设置信号引起的加法次数。

第一段0≤t≤0.1s即al＝3.3333×10-3＝109.2266667×2－15脉冲／加2截断取整后，Al＝109×2－15＝0.00000000110110212脉冲／加2

2023/1/3127TianjinUniversityofTechnology0.1s时间间隔内，加法次数为N1＝0.1×l000＝100加l00次加法后，实际达到的速度为

V1＝N1Al＝10900×2－15脉冲/加＝0.326416脉冲/加＝19958.496脉冲/min

第二段0.1≤t≤2.1sA2＝0，N2＝2×l000＝2000加第三段2.1≤t≤2.2sA2＝－Al＝－109×2-15脉冲/加2，N3＝N1＝100加这样，所有三段的A值及加法运算次数N分别为

Al＝109×2－15，A2＝0，A3＝-109×2－15N1＝l00，N2＝2000，N3＝1002023/1/3128TianjinUniversityofTechnology6．2．3直线运动

图6-7表示了常速度直线运动控制情况。常速度向量V的运动方程可表示为

2023/1/3129TianjinUniversityofTechnology

图6-7常速度直线运动(a)直线运动分布；(b)直线运动控制指令生成原理

2023/1/3130TianjinUniversityofTechnology

例6-4

x和y步进电动机驱动xy定位工作台，二者的位置分辨率为10脉冲／mm。希望以1000mm／min的速度产生与x轴成30°角的直线运动，距离为500mm。采用16位带符号寄存器，时钟频率为1000Hz。解

要求时钟周期数(即加法次数，简记为“加”)为

因此，x和y的速度分别为

2023/1/3131TianjinUniversityofTechnology

采用常速度分布实现直线运动控制指令时，只要1个时钟和4个寄存器(x轴和y轴各两个)，其中，两个作为速度寄存器，另外两个作为位置寄存器。速度寄存器的内容设为Vx和Vy。位置寄存器的初值设为0。时钟信号每由0到1转换一次，速度寄存器的内容向位置寄存器累加一次。位置寄存器的溢出脉冲就是步进电动机的参考脉冲指令。

2023/1/3132TianjinUniversityofTechnology6．2．4圆周运动

在xy平面中生成圆运动是一种比较复杂的情况，其中一根轴的运动速度正比于另一根轴的位移。图6-8表示了在xy平面上生成圆周运动的有关变量的定义。

图6-8圆周运动的生成2023/1/3133TianjinUniversityofTechnology根据图6-9所示的圆周运动，假设角速度＝常数，那么，可得下列方程：圆弧线速度xy直角坐标位置x，y轴上的速度

根据x，y轴的速度表达式(6-29)和(6-30)、可写出等价的差分表达式:

2023/1/3134TianjinUniversityofTechnology

在实现时，x每前进一步，必须将这一步与的乘积加到y速度寄存器；y每前进一步，则将其与

的乘积从x速度寄存器中减去。由于每一步的Δx和Δy的值为±1，所以乘运算是没有必要的,只需要向相应的速度寄存器直接加/减。注意，的量纲为单位采样间隔的弧度增量角。同时，位置寄存器每一步都按下列关系式改变其内容：2023/1/3135TianjinUniversityofTechnology式中，速度vx，vy的量纲为单位采样间隔的位置增量。在运算方程(6-33)和(6-34)时，每一步，只需要将速度寄存器的内容向位置寄存器相加一次即可。

根据以上原理，生成圆周运动控制指令的方法如图6-9所示。x轴和y轴都由三个寄存器组成：加速度寄存器分别存放-和+，速度寄存器分别存放Vx和Vy，位置寄存器分别存放x和y。2023/1/3136TianjinUniversityofTechnology

每当时钟信号由0到1转换一次时，速度寄存器的内容就向位置寄存器累加一次，加速度寄存器的内容就向速度寄存器累加一次。若位置寄存器有溢出脉冲时，则溢出脉冲作为步进电动机的控制指令脉冲。

图6-9圆周运动控制指令生成方法

2023/1/3137TianjinUniversityofTechnology例6-4

假设欲产生半径为50mm的45°圆弧，沿圆弧的步进速率为1000mm／min位置分辨率为10脉冲／mm，寄存器为16位带符号小数，时钟频率为1000Hz，初始角θi为0°。设计产生回运动指令所需的所有寄存器的初值和加法次数。

解

(1)圆周运动角速度

(2)初始位置坐标2023/1/3138TianjinUniversityofTechnology(3)各个寄存器的初始值

注意，这里pxi＝0脉冲是由于寄存器只保留一步的余数(即一个脉冲的小数部分)。最后，为了生成45°圆弧，还必须计算时钟由“0”到“1”的转换次数或加法次数。2023/1/3139TianjinUniversityofTechnology6．3直线与圆弧插补器

对于一个复杂的轮廓图形往往先通过直接函数计算法(粗插补器)，由位置函数方程产生一系列参考点，然后，在相邻两参考点之间利用直线或圆弧进行精插补。例如，图6-10表示了采用端面铣刀直接铣出大型齿轮的例子。根据输入参数Ps，PE，R及φ，通过函数计算在渐开线上产生参考点Pl，P2，…，PE。然后通过精插补产生位置坐标xi，yi，i＝1，2，…，N。2023/1/3140TianjinUniversityofTechnology

图6-10生成渐开线的粗精插补方法2023/1/3141TianjinUniversityofTechnology6．3．1直线插补器单轴情况

通常，被插补的轨迹函数由粗插补器生成的一组参考点存放在存储器中，直线插补器作为精插补产生中间点。其中，2023/1/3142TianjinUniversityofTechnology两轴情况

假设在xy平面上有一段直线，其起始点坐标为(x0，y0)，终点坐标为(xf，yf)，切割的线速度为v，采样周期为T，则完成该段直线插补所需的采样周期数N为其中，n＝1，2，…，N；初始条件为(x0，y0)；直线插补的递推公式可以表示为2023/1/3143TianjinUniversityofTechnology6．3．2圆弧插补器

在圆弧插补器中，圆以参量方程表示为

图6-10圆弧插补器原理

2023/1/3144TianjinUniversityofTechnology一次递推算法

参考图6-10，Pn+l点的坐标可用Pn的坐标(即xn＝Rcosβ，yn＝Rsinβ)表示如下：

在给定α的条件下，圆弧插补的参考点可用递推公式求解。

2023/1/3145TianjinUniversityofTechnology二次递推算法

1、在方程(6-51)的第二等式右边加／减2、在方程（6-52）的等式右边加/减得：2023/1/3146TianjinUniversityofTechnology

方程组（6-53）和（6-54），以及上面方程组是两组二次递推圆插补器。两组公式具有同等结果。只要前两点（x0，y0）和（x1，y1）在圆上，不管cosα或者sinα是否存在计算误差，（xn，yn），n=0，1，2，…，N，都在同一个圆上。实现时，先由初始点（x0，y0）一次递推计算出第一点（x1，y1）；然后，采用二次递推公式计算出其余的参考点。同理还可以得到另外一组公式：2023/1/3147TianjinUniversityofTechnology6．4样条函数轨迹规划器

样条函数通常是指多项式函数。“样条”这一名词来源于造船工业的仿样。三次多项式样条函数可以用来在计算机内存中细密地储存指令输入，以及实时产生复杂的机器运动控制指令序列。它可以用于点位运动控制，也可以用于连续路径控制，以及多轴系统的协调运动控制。2023/1/3148TianjinUniversityofTechnology6．4．1点位运动控制

一切机器的传动系统都具有有限的速度和加速度。点位运动控制是在容许加速度条件下，尽可能以最大速度由源坐标位置运动到目的坐标位置，对于两点之间的轨迹是没有精度要求的。在许多定位系统中，为了精密控制和可靠工作，控制加速度是关键。此外，一般的机器都是多轴的，所有的运动轴都必须同时启动同时停止，即完成协调运动。在直角坐标系中的协调运动将产生直线运动轨迹。2023/1/3149TianjinUniversityofTechnology

下面，集中讨论利用三次多项式样条函数，生成点位控制指令的设计方法。（1）单轴运动的情况

假设机器的x轴由t0时刻的x0运动到tf时刻的xf，轴的速度在运动前后都为零。机器运动时，由起始点x0加速，在运动到中点时达到某最大速度，然后立即开始减速，抵达xf时停止。采用三次多项式样条函数，路径表示为对t微分一次，得速度表达式为2023/1/3150TianjinUniversityofTechnology位置和速度必须满足下列边界条件，即这样就可以求得：

三次多项式样条函数点位控制路径指令为

抛物线速度分布为

2023/1/3151TianjinUniversityofTechnology

线性加速度为

路径、速度以及加速度时间曲线如图6-17所示。由图可以看出，采用三次多项式样条函数的点位控制，其最大速度和最大加速度分别为2023/1/3152TianjinUniversityofTechnology图6-11路径、速度以及加速度时间曲线

2023/1/3153TianjinUniversityofTechnology

假设vmax和amax分别为控制系统的最大容许跟踪速度和最大容许跟踪加速度，那么，给定起始点坐标z0和终点坐标zf，所对应的最小运动时间区间分别为在实际运动时，为了保证速度和加速度都不超过容许值，最小运动时间应选用二者中较长的一个，即2023/1/3154TianjinUniversityofTechnology（2）多轴系统的运动

假设有n个自由度，分别有n个起始坐标ζ0i，n个终点坐标ζfi，n个容许最大速度vimax以及n个容许最大加速度aimax。这里，i＝1，2，…，n。那么，对于每个轴，均可计算出2023/1/3155TianjinUniversityofTechnology

为了保证所有各轴的速度和加速度都不超过容许值，容许协调运动的最短时间区间应为

进一步，引入归一化时间τ=（t－t0）/(tf－t0)，0≤τ≤1。代入方程(6-30)，可得2023/1/3156TianjinUniversityofTechnology例6-5

考虑xy平面上的直线运动路径，源点坐标为(6，-3)，终点坐标为(2，-1)假设两轴只有加速度限制：ax=ay=±2。试设计三次多项式样条函数点位控制指令。

解

由于只有相同的加速度限制，两轴运动的距离分别为Δx＝|6—2|＝4，Δy＝|—3一(一1)|＝2，且Δx＞Δy，所以协调运动是先设计x轴，然后利用下列直线方程变换出y轴的控制指令，即2023/1/3157TianjinUniversityofTechnology6．4．2连续路径控制

令符号，i=0，1，…，N，是空间轨迹上给定参考点的位置和速度向量。对于第i段ti－1≤t≤ti，引入相对时间变量，则并且，将通过i一1和i点的三次多项式样条函数表示为2023/1/3158TianjinUniversityofTechnology则边界条件为2023/1/3159TianjinUniversityofTechnology例6-6

假设给定xy平面上的轨迹如图6-12所示。该轨迹共分为4段，每段的边界条件列于表6-2中。试采用三次多项式样条函数生成控制指令。

图6-12xy平面上的轨迹

2023/1/3160TianjinUniversityofTechnology表6-2例6-6边界条件

解

第一段0≤t≤5s2023/1/3161TianjinUniversityofTechnology第二段5s≤t≤15s

第三段15s≤t≤30s

第四段30s≤t≤40s

2023/1/3162TianjinUniversityofTechnology6．4．3轨迹控制的实现

根据三次多项式样条函数运动指令，实现轨迹控制的系统方案如图6-l3所示。

图6-13实现轨迹控制的系统方案

2023/1/3163TianjinUniversityofTechnology为了控制沿轨迹的运动速度，可以通过改变点的间隔(tk+1－tk)来实现。为了进一步缩短插补运算时间，可以将三次多项式函数拟合改为简单的直线插补法。为了缩小拟合误差，直线插补的间隔应该选得更小。

2023/1/3164TianjinUniversityofTechnology6．5典型系统举例

作为轨迹规划和控制的实例，在这一节中，集中讨论一种试验用的自主式自动导引车。自主式自动导引车实质上是一种轮式移动机器人，通过编程可以任意改变运动轨迹。自主导引方式是一种不依赖外界环境的、最具有通用性和环境适应性的一种导航方式。其中，轮距法是一种非常简单而价廉的方法。它利用小车左右侧安装的位置检测装置，如光电编码器，检测左右轮转过的角度，确定小车的位置和方向。

2023/1/3165TianjinUniversityofTechnology6．5．1总体结构方案

试验小车分机械本体和电控系统两部分。机械本体结构如图6-14所示。车身尺寸660mm(长)×540mm(宽)×430mm(高)。驱动轮(前轮)和小脚轮(后轮)皆为硬橡胶制品。驱动轮半径r＝638mm，齿轮减速比为3.6。驱动轮采用闭环直流伺服系统控制。反馈元件光电编码器又是轮距法中的轮距测量装置。左右轮伺服系统的原理结构如图6-15所示。2023/1/3166TianjinUniversityofTechnology

图6-14自主式自动导引车原理结构

2023/1/3167TianjinUniversityofTechnology图6-15左右轮伺服系统的原理结构

伺服系统的控制器采用8098单片机。上位机采用8088CPU进行轨迹规划和运动学逆运算。上下位机之间通信由双口RAM完成。车载控制器的计算机结构如图6-16所示。车载控制器的主要功能是：根据行车命令或环境模型，规划其自身的运动路径，控制两个独立驱动轮的转角和转速，使小车跟踪规划路径运动。

2023/1/3168TianjinUniversityofTechnology图6-16车载控制器的计算机结构

2023/1/3169TianjinUniversityofTechnology6．5．2运动学方程

小车的运动学正问题

这就是小车的定位问题，即由左右轮运动速度vL(t)和vR(t)确定小车的空间位置坐标(x(t)，y(t)和方向θ(t)。参考图6-17所示小车结构参数和几何位置，oxy表示地面固定坐标系，CR是两个驱动轮的中点。GP是小车中心线上的一点，定义为小车的参考点或导引点。该点的运动轨迹就是小车的运动轨迹。θ是描述车体方位的参数。一般选择GP与CR重合，即D＝0。GP与CR重合可使运动学方程简化。2023/1/3170TianjinUniversityofTechnology图6-17小车结构参数和几何位置

小车在oxy直角坐标系中的位置和方向可以表示如下：

2023/1/3171TianjinUniversityofTechnology小车的运动学逆问题

这就是小车的轨迹跟踪问题，即给定小车的运动轨迹，求解左右轮伺服系统的控制指令。当小车在地面上作无滑动运动时，小车左右轮的中心点CR既是小车的轨迹曲率中心又是小车的速度瞬心。

2023/1/3172TianjinUniversityofTechnology6．5．3轨迹规划与控制指令的生成

根据上述理论，为了使小车平稳移动和轨迹规划方便，采用小车沿直线→过渡曲线→另一直线移动的轨迹，并保证整个轨迹曲率半径连续。采用三次曲线作为过渡曲线，既可以实现曲率半径连续的轨迹规划，又可以由轨迹轮廓和最小曲率半径唯一确定。设小车先由直线Γ1前进，然后经过过渡曲线b1b2，再沿直线Γ2移动，如图6-18所示。

2023/1/3173TianjinUniversityofTechnology图6-18导引车运动轨迹图

每一段弧长的曲率半径Ri和对应的圆心角δi：

2023/1/3174TianjinUniversityofTechnology

在每一小段弧上，导引车左右驱动轮的速度分别为:

在每一个Δt内，左右驱动轮伺服系统的控制指令应为:

式中，符号Kb为光电编码器的脉冲当量，mm／脉冲。

这样，{(SLi，SRi)，i＝0，1，2，…}就形成了导引车控制指令序列。2023/1/3175TianjinUniversityofTechnology6．5．4上下位机软件编程

上位机8088编程

上位机扩大了8路优先中断板，可将外部的8路中断进行优先权排队后，从STD—BUS上向8088申