机电测控实验实验指导书

机霓系统测控、机器人(II)

实

验

指

导

♦

名儆工Q大犀

机被Z程号断

.10

项目壹倒立摆机重系统建模、分析与控制

1、壹级倒立摆机雷系统

倒立摆系统是^典的迅速、多变量、非线性、强耦合、不稳定的机雷系统，是控

制理论中研究的热黠，同步生产中也有诸多得应用，因此研究和分析倒立摆系统，在

理论和实践上都具故意义。

研究倒立摆系统的能有效的反应机重系统的^多^典冏题：如非线性冏题、鲁棒

性冏题、镇静冏题、随劲冏题以及跟踪冏题等。通谩娄I倒立摆的控制，用来检查新的

控制措施与否有较强的处理非线性和不稳定性尚题的能力通谩封它的研究不仅可以

处理控制中的理论和技术实琨冏题，遢能将控制理论波及的重要基础孥科：力阜，数

学和计算机科阜迤行有机的综合应用。其控制措施和思绪瓢论封理论或实际的谩程控

制均有很好的启迪，可以检查多种控制理论和措施。倒立摆的研究不仅有其深刻的理

论意义，尚有重要的工程背景。在多种控制理论与措施的研究与应用中，尤其是在工

程实践中，也存在壹种可行性的试验冏题，使其理论与措施得到有效检查，倒立摆就

能卷此提供壹种优理论通往实践的桥梁，目前♦,封倒立摆的研究已^引起国内外阜者

的广泛关注，是控制领域研究的热门^题之宣。

(1)壹级倒立摆物理构造

倒立摆简朴构造，如图1所示,质量卷M小事在轨道上运勤，在小串上装有摆杆,

质量卷小。摆杆与小隼转勃轴连接，并安装编码器，用于测量摆杆的角度，摆杆可左

右运勤。假如定义摆杆在垂直位置卷稳定系统，显然倒立摆系统悬不稳定的、^典的

机重系统，倒立摆的不稳定性表目前摆杆不能保持在竖直位置，摆杆曾转勤。封小申.

不施加如图1所示力F,那么施加在小聿上的力F大小、方向和作用畤间怎样变化才

能保证摆杆保持在垂直位置。

AX

图1.倒立摆构造简图

(2)壹级倒立摆数孥模型

建模可以分卷两种：试验建模和理论建模。试验建模就是通谩在研究封象上加上

壹系列的研究者事先确定的输入信号，鼓励研究射象并通谩传感器检测其可观测的输

出，应用数学手段建立起系统的输入壹输出关系。道裹面包括输入信号的设计选用，

输出信号的精确检测，数孥算法的研究等内容。机理建模就是在理解研究封象的运勤

规律基础上，通遇物理、化阜的知识和数^手段建立起系统内部的输入壹状态关系。

封于倒立摆系统，由于其自身是自不稳定的系统，试验建模存在壹定的困难。不谩通

谩小心的假设忽视掉某些次要的原因彳爰，倒立摆系统就是壹种^典的运勤刚体系统，

可以在惯性坐襟系内应用^典力阜理论建立系统的勒力阜式。采用其中的牛顿定律措

施建立直线型壹级倒立摆系统的数阜模型，并暹行了简朴的^环仿真。

在忽视了空气阻力和多种摩擦之彳爰，可将直线壹级倒立摆系统简化成小串和匀质

杆构成的系统。倒立摆系统各参数的物理意义、大小、罩位，如下表1所示：

表1

变量物理意义大小里位

M小隼的质量0.618Kg

m摆杆的质量0.0737Kg

F小聿的驱勃力N

J摆杆的转勤惯量Kgm2

g富地的重力加速度9.8Kg/m-s2

b小事与水平面的摩擦系数0.1N/m/sec

L摆杆的是度0.350m

1摆杆质心到转轴的距离0.1225m

P摆杆封小隼水平方向上的力N

N摆杆封小卓竖直方向上的力N

f摆杆的摩擦阻力矩系数N/rad/sec

X小隼的水平位移m

小聿速度m/s

X

小隼加速度m/s2

X

0摆杆与垂直向下方向的夹角(考层到rad

摆杆初始位置卷竖直向下)

2

(M+tn)x+bx+mlcos-ml0sin^=F(4)

由摆杆垂直方向受力分析图2,可知：

P-m^+tnd2(Icos0)/dt2=0(5)

即：

2

P=mg+mlOsin6+ml9cos0(6)

冉由撰杆的力矩平衡得：

一(plsin6+NlcosO)=J0+f0(7)

其中，J=ml2/3

将式(3)和式(6)代入式(7)得：

(J+ml2)6+zn^/sin0=-f0-mixcos3(8)

式(4)和(8)均是非线性，需要线性化处理，由于控制的目的是保持倒立摆直立，

在施加合适力畤。假定则有：uos6>«-l,sin(9«-a,6>=0o线性化得到两低I

线性式(9),

(J+ml2)a-mlx=ingla

•(9)

(M+rn)x+bx-mla=F

封式(9)jffi行拉氏变化(在壹定的条件下，能把壹实数域中的实变函数变换卷壹种在

复数域内与之等价的复变函数)(设初始条件悬0),控制信号悬u：

f(J+mF)。。),一一mgla(s)=mlX(s)s2

2

+m)X(s)s+Z?X(s)s-〃"。⑸/=u(s)(10)

选择摆杆倾斜角a卷仿真输出，由式(10)的第1他式得：

X(s)咨以⑸(11)

mls

将其代入式(10)的第2低式得：

)一-£.()2)2

(M+tn)«55+b--S-a(s)s-mla(s)s=U(s)(12)

mlsJmls~

整顿彳灸得到传递函数卷：

ml2

G(s)=四二-----------,-----------------

(13)

UG)b(J¥ml~)3(M+m)mglbmgl

S4+SS"""2s

AAA

其中：/=柴，()(222

A=["+"?j+ml)-fnl](14)

式(13)悬摆杆倾斜角a与控制量〃的传递函数。代入表壹数据，可得到倒立摆系统的

传递函数。

(3)壹级倒立摆特性分析与计算

式(13)卷摆杆的运勃数阜模型，要使摆杆竖直在垂直位置，必须封小事施加控

制，使摆杆获得倾斜方向相反的加速度，在施加控制之前，要封摆杆的特性选行分析,

目的有二：

其壹，可验证理论数阜模型的可靠性。

其二，可卷选择控制器、设计控制器提供协助。

运用^典的控制原理分析措施，在复频域、频域、畤域谨行分析，重要分析伯德

图(bode图)，根轨迹图(Rlocus图)、Nyquist图、尼柯尔斯图(Nichols图)。分

析运用Matlab/controlsystemstoolbox中的其些工具函数，可以便的得到需要的

成果。

在Matlab命令窗口(commandwindows)输入Rlocus(num,den),bode(num,den),

Nyquist(num,den),Nichols(num,den),即可得到需要成果，num悬传递函数的极钻,den

卷传递函数的零黠。在word软件下保留图形，用于试验汇报。

试验1、根轨迹分析

»Num=[3.350];

»Den=[l0.0786-22.7085-1.6415];

»Rlocus(num,den);

试验2、Bode图分析

»Num=[3.350];

»Den=[l0.0786-22.7085-1.6415];

»bode(num,den)

试验3、Nyquist分析

»Num=[3.350];

»Den=[l0.0786-22.7085-1.6415];

»Nyquist(num,den)

试验4、Nichols分析

»Num=[3.350J;

»Den=[10.0786-22.7085-1.64151;

»Nichols(num,den)

项目二慧危机器手模型构建实现、控制

概述

机器人技术是近来间发展起来的新技术，它综合了机械、力孥、霜子计算机、人

工智能、系统工程、控制理论、自勒检测等学科的最新成果。机器人封于实IS柔性生

产自勤化、保护劳勤安全和提高产品质量能发挥重大的作用。机器人技术的应用，此

彳及曾越来越广，将封人类的生产和生活产生巨大的影响。

机械手是壹种模仿人手的某些工作机能，被给定的程序、轨迹和规定，实垣抓取、

搬运工件，或者完毕某些劳勤作业的机械化、自勤化妆置⑴。国外把它称卷：操作机

(Manipulator)、机械手(MechanicalHand)。机械手只能完毕某些比较简朴的抓取，

搬运及上下料工作、常常作卷机器设备上的附属装置。因此具有壹定的专用性，因此

又称卷专用机械手。

机器人是能模仿人的某些工作机能和控制机能，按可变的程序、轨迹和规定，实

现多种工件的抓取、定向和搬勤工作，并且能使月工具完毕多种劳勒作业的自勒化机

械系统。因此，机器人比机械手更卷完善，它不仅具有劳勤和操作的机能.并且遢具

有学习、记忆及感竟功能。国外把它称悬程序控制操作机(ProgrammableManipulator)。

壹般则称卷机器人(Robol)。

试验壹、机器人运勤与控制认识

1、机构位置的求解及仿真

壹种位移矢量表达了空间任意两黠之间的有向距离。悬了暹行机构分析，机构中

每壹根连杆都可以表达卷壹种位移矢量，矢量的起黠就是连杆的某壹端黠，而另壹端

黠就是矢量的终黠。道彳固位移矢量的大小就是连杆的良度，矢量与x轴正向间的夹角

就是连杆的夹角(逆H寺针卷正)。封机构的位移轨迹仿真，采用的措施就是矢量式法。

封机构的速度分析仿真，既采用了矢量式法，也采用了拟合仿真法。由于受到试验设

备的限制(如慧危机器人的精度较低、其塑料材质与勒力学方面辗应用价值，其机械

构造封工业用机器人的壹般构造不具指导意义等原因)，未封运勃阜方面的加速度和

手爪受到的力的变化深入分析。

1.1矢量式法

借助于壹种壹般四连杆机构，简朴地论述模型位移轨迹的分析措施。

图1：四联杆机构

图1是壹种^典的四连杆机构，用矢量式封其迤行分析。卷以便起兄，设定该机

构的x-y坐襟系如图中所示。由于在同壹坐襟系下位移矢量有多种排序表达措施，因

此各位移矢量的编号次序应遵照壹定的约定。即，封于任意壹组位移矢量，应常构成

壹种易于封的体琪和便于推导的闭环矢量式。因此，根据矢量加法的规定，矢量尺和

用应常首尾相连，矢置用和凡亦是如此。数挚体垣式卷：

艮+A=凡+穴4（1）

矢量后和用相加而得到的位移矢量与欠量尺和几叠加得到的位移矢量是完全

相似的；瓢论机构运勤到何种状态，只要可以保证可以机构的几何装配条件，则适值］

闭环矢量式就壹定可以成立。

将闭环矢量式分解成卷两值量体现式。在图示例子中，将矢量分解成X,y两

他I方向。壹种沿x方向分解，壹种沿y方向分解。有关矢量角度的定义，若由x正向

旋转到矢量的矢端悬逆畤针转勤，则该矢量的角度卷正。则分解彳发的闭环矢量式的两

值I分量体垣式卷：

r2cosa+qcos%=八cosd+r4cos04）

卜⑵

r,sin02+r3sin0y=r}sin+sin0AJ

其中：〜。代表各保I连杆的是度，在整保1分析谩程中是保持不变的。

1.2四连杆机构位置冏题的解

选择连杆1作卷参照系，把四连杆机构闭环矢量式（2）重新组合

J\（4，名）=r2cosa+Gcos%-6-r4cos优二0、

卜⑶

f2,仇）=r2sin夕2+〃sin-r4sin04=0J

因此：位置冏题可描述悬：封干给定的壹组连杆是度和名值，寻求合适的％、优，

使工、八卷零。由于/、是超越和非线性的，线性分析措施已不合用，牛顿-辛普

森措施合适求解此类冏题。

设：%=+

⑷

84=氏+卜。4

其中：。3、。4卷近似解，△/、△依卷修正系数。

采用牛顿-辛普森算法可得：

△%_-sin-r4sin0A一力@，d)

A。"一⑸

L。％」［qcosa-/cos。-f2(^3A)

运用(5)式，采用Matlab计算，可求解图1的位置冏题。程序冕附录

1.3、四连杆机构运勤阜

在图1的四连杆机构中，连杆2卷甯勤机驱勤以匀速转勤，封式(2)中的内、

%求有关H寺间的壹阶导数可得各连杆的速度(6)、二阶导数可得各连杆的运勤加速度

(7)：

-co2r2sin劣一必Gcos%=-必。sin%、

I(6)

r

①2r2cos2+%4cos%%4cos/J

•2•2•2

一%〃sin2一%4cos4+々4口sin2+砥々cos%=生4sin。?+g工cos。2、

222.(7)

44cos%-sina—cos^4+co4~r4sin0A=-a2r2cos%+32Gsin%J

(6)、(7)式写卷矩阵式的形式分别卷(8)、(9)：

一3sin”-rsin4(o2r2sin02

4_(8)

94.-co2r2cos^2

r3cos%-r4cos^4

222

-〃sin-rsin<9ci_/弓sin%+g工cos，〃cos%-g~〃cos%

44y)),

%」~a2r2COS^2+"”山。2+叫~勺$皿。3-必工SillW

r3cos%一qcosa

(9)

其中：%%表达连杆的加速度；必、以表达连杆的角速度；

封式(8)、(9)描述的四连杆机构在Matlab下编制计算程序

取四连杆机构参数见表2

连杆■R度(r)/cm转角

112.00°

24.045°

310.024.65°

47.090.68°

试验2、机器人控制程序设计

2.1串行通讯的概念

慧危机器人是采用上、下位机的控制方式，由上位计算机通遇串行口

RS232向下位军片机发出控制指令，下位计算机解释执行，彳走而到达控制的目

的，串行通讯的格式有：单工(simplex)通信、半双工(half-duplex)通信、

全双工(full-duplex)通信。军工数据线仅可以在壹种方向上传播数据,彳他

震区发回数据的地震传感器和商业辗线甯台都属于^典的军工传播的例子。半

双工传播可以在两值系统间以任何壹种方向传播，但同壹畤间只能在壹种方向

上传播。半双工的例子是瓢线甯封讲机，壹种人在^的畤候，另壹种人只能在

听，由于发送期间甯路是断^的。全双工的意思是每他系统可以同步发送和接

受数据。常用的重话系统就是全双工的例子⑹。在串行通信中，数据的收发可

采用同步(synchronous)和异步(asynchronous)两种基本的工作方式。同

步通信方式，数据以块的形式按壹定的速率发送，每块的^始与结束由特定的

字节或特定位来阐明。异步通信所采用的数据格式是以壹组不定“位数”数组

构成。壹般，异步通信速率要比同步通信的低。每秒传送数据的位数成卷传送

速率，即波特率(baudrate)⑸。PC机卷上位机,罩片机悬下位机。上、下

位机的数据传播采用同步全双工的工作方式。

息,是低//

百I匕二二;:二二二二二;二二二二二二二二二二二二二二二

DO[DIID2ID3|D4|D5|D6|检藐位|停止位卜不止位

图2.3.1异步串行数据发送格式

在异步串行口中收发的每壹种字节数据都是由如下4倜部分按次序构成

•1彳固起始钻

•8他I数据粘位(在某些状况下是7他I)

•奇偶校验位(可有可辗，可奇可偶)

•1彳固或2停止位

数据收发线的空状态是高雷平。起始位檄志著壹种新字节传送的^始，与壹位数据位

占同样的畤间宽度，低甯平。起始位之彳爰就是71周或8倜数据位。常数据位卷0畤，

收发线卷低甯平，反之卷低甯平。在数据位之彳菱是奇偶校验位。奇偶校验位是用来检

查在传送谩程中与否发生差金昔。顾客可选择与否暹行奇偶校验，假如暹行，遢可选择

迤行奇校验ig是偶校验。若选择偶校验，则各位数据位加上校验位自身，其和将是宣

种偶数；若选择奇校验，其和将是壹种奇数。在奇偶校验位之彳爰就是停止位，它可以

占壹位或两位数据位的畤间宽度。停止位是低雷平，它表达壹种字节传送的东吉束。

在停止位之彳灸，可以接著乂是下壹种字节的起始位，也可以在停止任意畤间彳麦,

才是下壹种字节的起始位⑺。数据收发的速度是月波特率（baudrate）来衡量的，波

特率就是每秒收发的位数。波特率壹般在300,690,900,1200,2400,…9600波特

之间。

由于异步通信每传送壹种字符都加起、止信号灯附加位，使其传播效率比较低，

因此异步通信壹般用在数据速率较慢的埸所。在高速传送畤，壹般应采用同步协议。

由面向字符的同步协议和面向比特的同步协议。

异步通信于同步通信的比较：异步通信是指通信中两佃I字符间的畤间间隔是不固

定的，而在同壹字符中两他I相邻位代码间的间隔是固定的。但在同步通信中，每畤每

刻在链路上均有字符信息传送，并且通信中的每倜字符间的畤间间隔是相等的；此外,

每低1字符中各俯］相邻位代码间的畤间间隔也是固定的。

大部分串行口的构造都是在发送和接受方装有壹种25针插座，都遵照RS-232原

则，但又都不严格遵照道他原则。如本机器人上,下位机的接口处装的插座就是9针

插座。道9针信号是：

DLSD1接受线信号检查

RXD2接受数据

TXD3发送数据

DTR4数据终端准备好

GRD5信号地

DSR6数据设备准备好

RTS7祈求发送

CTS8清除发送

RI9振铃指示⑺

用串行数据线与微型机接口，数据必须迤行串并行转换。需要用握手雷路来保证

发送方发送数据的速度不比接受方可以接受的速度快。本机器人采用的串行通信芯片

是8251,壹般称卷通用同步异步收发器（universalsynchronous-asynchronous

receiver-transmitterUSART⑹）o

8251A的数据输出端要通谩调制器将数字信号转换成模拟信号，数据接受端收到

的是通谩解调器转换来的数字信号，因此8251要和调制J/解调器直接相连。调制解调

器（modem）是用来将数字信号转换成音频信号，以及激昂音频信号在转换回数字信

号的器件⑹。

在本台机器人串行接口处用的芯片是8521A。下面封该芯片作简朴的简介。8251A

由发送器、接受器、数据^线缓冲寄存器、写控制雷路及调制/解调控制雷路等5

部分构成。发送器内部的发送移位寄存器，是用来完毕并一串转换的。发送H寺除了发

送移位寄存器，遢发送缓冲寄存器和控制重路。优CPU传送出来的数据^数据发送缓

冲寄存器并行输入，并锁存到发送缓冲存储器中「如本机采用的同步方式，在发送数

据之前，发送器将自财送出壹种（军同步）或两f固（双同步）同步字符（Sync）,然

彳灸逐位串行输出数据。

EIA（ElectronicIndustriesAssociation美国重子工业协曾）RS-232c是目前最常用

的串行接口原则，用于实于兄计算机与计算机之间、计算机与外设的数据通信。本机器

人接口的原则采用的是RS-232。该原则的目的是定义数据终端设备（DTE）之间接口

的甯气特性（包括：信号的逻较重平、最高数据,专播和多种信号功能）。RS-232提供

了军片机与军片机、单片机与PC机间串行数据的通信接口。由于RS-232原则只规定

了采用壹封物理连接器，但封连接器自身的物理特性没有任何定义，因而出现了

DB-25.DBT5和DB-9等多种类型的连接器，其引脚的定义也各不相似。本机器人采

用的连接器卷DB-9。

传播重缆的艮度与传播的重容有关。EIA原则规定被驱勤雷路（终端）的甯容，

包括重缆连接甯容必须不不小于2500pF。封于宣种多芯重缆来gt,每英尺（0.305m）

重容卷40pF〜50pF,因此满足重:容特性的雷:缆畏度卷50英尺（15.24m）o但若能保

证甯缆余恩雷:容不不小于2500pF,则缆晨度可超谩限定值。同步，RS-232C原则所

容^的信号传播速率在0〜Ob/s的范围之内，而在实际应用中常被限制在1920b/s以

内。

2.2串行通讯程序设计

2.2.1、初始化串口：

在程序控制中，除了实10勤作的控制程序和线程的创立很重要外，初始化串口也

很重要。

壹般来播J,初始化串口的重要有两步：

（1）写入控制字；

设置通讯方式：数据的波特率，通讯的方式（试验中通讯方式卷全双工船型

通讯方式）等。

初始化成程序如下：

voidCMy5y19View::fischer_install()

{DCBdeb;

DWORDdwError;

BOOLfSuccess;

hCom=CrcatcFilc(

"COM1",/*nameofCOMdevicetoopen*/

GENERIC_READ|GENERIC_WRITE,/*read-writeaccess*/

0,/*notused*/

NULL,/*notused*/

OPEN_EXISTING,/*requiredfortapedevices*/

0,/*notused*/

NULL);/*notused*/

if(hCom==INVALID_HANDLE_VALUE)

{dwError=GetLastError();

fSuccess=GetCommState(hCom,&dcb);

/*FillintheDCB:baud=9600,8databits,noparity,1stopbit.*/

dcb.BaudRate=9600;//9600Baud

deb.ByteSize=8;//8Bits

dcb.Parity=NOPARITY;//noparity

dcb.StopBits=ONESTOPBIT;〃1stopbit

}

(2)通谩以上C++代码建立了上下位计算机的通讯机制，上位计算机可以发送命令，

如封壹甯勒机左转或右转命令，命令由固化在下位计算机ROM内的程序解释执行，下

位计算机ROM内的程序阐明如下：

(1)/5勤机函数，motor(intnum,chardir)函数作用是控制机器人甯勒机勤作

左转、右转

voidmotor(intnum,chardir)

{charResult;

switch(dir)//switchdirection

{case,A*://off

(Motorzustand=clearBit(Motorzustand,(num*2));

//clearrightbit

Motorzustand=clcarBit(Motorzustand,(num*2-1));

//clearleftbit

break;}

case'L'://left

(

Motorzustand=clearBit(Motorzustand,(num*2));

//setrightbit

Motorzustand=setBit(Motorzustand,(num*2-1));

//clearleftbit

break;|

caseR:

//right

{Motorzustand=setBit(Motorzustand,(num*2));

//setrightbit

Motorzustand=clearBit(Motorzustand,(num*2-1));

//clearleftbit

break;}

}

)

Result=input_word();//writemotorbytetotheinterface

)

(2)甯勤机停止左转、右转函数

voidmotors_off()

charresult;

Motorzustand=0;//resetall

result=input_word();//sendmotorsettingstointerface

}

(3)模拟量输入函数

intanalog_in(charnum)

{

LPSTRBefehl=u

LPSTRZustand=n”；

BOOLResult:

DWORDWritten;

DWORDRead;

switch(num)

//switchnum

(

case0:BefehI[0]='\xc5';

//1.Byte:197inHexforEX

case1:Befehl[0]='\xc9';

//1.Byte:201inHexforEY

)

Befehlf1]=Motorzustand;

//2.Byte:motorsetting

Result=WriteFile(hCom,Befehl,2,&Written,NULL);

//send2Byte

Result=RcadFilc(hCom,Zustand,3,&Rcad,NULL);

//read3Byte//1.Byteisthestatusof//InputsEltoE8

//2.Byteand3Byteis//thevalueofiheanaloginput

rctum((Zustand[l]*255)+Zustand[2J);

//returnintegervalue

)

(4)位清除函数

clearBit(charwert,intbit)

(

intbitvorne;

intbithinten;

div_tvome;

div_thinten;

bitvorne=int(pow(2,bit));

vorne=div(wert,bitvorne);

bithinten=int(povv(2,(bit-l)));

hinten=div(wert,bithinten);

return(vorne.quot*bitvorne+hinten.rem);

(5)位检测函数

testBit(charwert,intbit)

(

intbitvorne;

intbithinten;

intvorne_neu;

chartemp;

div_tvorne;

div_thinten;

bitvorne=int(pow(2,bit));

vorne=div(wert,bitvorne);

vorne_neu=vorne.quot*bitvorne;

bithinten=int(pow(2,(bit-l)));

hinten=div(werl,bithinten);

temp=(wert-vorne_neu-hinten.rem);

return(temp);

)

(6)^关计算函数

switch_in(charnum)

(

charresult;

result=input_word();//readtheinputstatus

returntestBit(rcsult,num);

)

(7)位设置函数

setBit(charwert,intbit)

{

intbitvorne;

inibiihinien;

intvorne_neu;

div_tvorne;

div_thinten;

bitvorne=int(pow(2,bit));

vorne=div(wertbitvorne);

vorne_neu=vorne.quot*2+1;

bithinten=int(pow(2,(bit-l)));

himen=div(wert,bithinten);

return(vome」icu*bithinten+hinten.rem);

motor(intnum,chardir)

(

charResult;

switch(dir)//switchdirection

(

case'A'://off

(

Motorzustand=clearBit(Motorzustand,(num*2));

//clearrightbit

Motorzustand=clearBit(Motorzustand,(num*2-1));

//clearleftbit

break;

)

case'L'://left

{

Motorzustand=clearBit(Motorzustand,(num*2));

//setrightbit

Motorzustand=setBit(Motorzustand,(num*2-1));

//clearleftbit

break;

)

caseR://right

(

Motorzustand=setBit(Motorzustand,(num*2));

//setrightbit

Motorzustand=clearBit(Motorzustand,(num*2-1));

//clearleftbit

break;

}

)

Result=input_word();

return0;

)

3、慧危机器人模型控制试验步聚

(1)根据慧危机器人资料和手则选用合适的机械零件、传勤机构、构建运勤机械模

型。各机械零件、传勃机构、甯勤机等机陶之间应可靠安装。

(2)连接试验甯路、安装传感潜、接口板，注意重源的极性。

(3)通谩串行线连接上、下位计算机，注意：在上、下位计算机未通甯畤插接。不

可热插拔，否则曾损t襄计算机。

(4)采用VCH或LLWIN3.0设计通讯控制程序(使用手则附录1)。

(5)向下位计算机加载控制信号，调试机械机构运勃与否正常，否则，紧固机械零

件、传勒机构、甯勃机使之可靠连接。

4、试验项目：

(1)慧危工业机器人

(2)慧危移勤机器人

(3)慧鱼冲床模型

(4)慧鱼仿生机器人

同孥在以上项目中选择壹种，按3中的规定完毕试验。

5、试验汇报规定(评分原则5级分/100分)

(1)仔细分析倒立摆机雷系统模型，建立微分方程，运用拉式变换转换卷传递函数。

(3分/10分)

(2)在频域分析倒立摆机11系统多种特性，并绘制图形。(1分/10分)

(3)论述试验设计思绪，设计或构建的机器人也言午应用的领域。(1分/10分)

绘制重要机构的机械设计图纸(可手工绘制或计算机绘制)。(1分/10分)

(4)选择设计构建的机器人中所含^典机构(如机器人手臂、旋转机构等)建立矢

量式或D-H坐檄变换法，封其暹行位置、运勒阜分析仿真。，并分析仿真成果(仿

真成果打印阜生自己处理)。(3分/10分)

(5)设计的控制程序原代码及原代码阐明文挡，或控制程序框图。含调试记录。(1

分/10分)

(6)同组同阜可以自行分工，分别完毕各自试验任务，弄虚作假者按阜校有关管理

条理和学生守则处理。

附录1

1、四连杆机构位置冏堰的牛顿-辛普森算法程序：

posso.m

function[th3,th4]=posso(th,rs)

th2=th(l);

th3bar=th(2);

th4bar=th(3);

epsilon=l.OE-6

f=[-rs(3)*cos(th3bar)-rs(4)*cos(th4bar)+rs(2)*cos(th(2))-rs(l);

-rs(3)*sin(th3bar)-rs(4)*sin(th4bar)+rs(2)*sin(th(2))];

whilenorm⑴aepsilon

J=[-rs(3)*sin(th3bar)rs(4)*sin(th4bar);

-rs(3)*cos(th3bar)-rs(4)*cos(th4bar)];

dth=inv(J)*(-l.0*f)；

th3bar=th3bar+dth(1);

th4bar=th4bar+dth(2);

f=[rs(3)*cos(th3bar)-rs(4)*cos(th4bar)4-rs(2)*cos(th(2))-rs(l);

rs(3)*sin(th3bar)-rs(4)*sin(th4bar)+rs(2)*sin(th(2))];

norm(f)

end;

th3=th3bar;

th4=th4bar;

附录2

2、四连杆机构运勤冏题的计算程序：

「2=4;

「3=10;

r4=7;

th2=45*pi/180;

th3=24.652*pi/180;

th4=90.6794*pi/180;

J=[-r3*sin(th3)r4*sin(th4);r3*cos(th3)-r4cos(th4)]

B=[l00*r2*sin(th2);-l00*r2*cos(th2)]

附录2：LLWin3.0慧危机器人模型控制程序设计

主程序设计

第1步：通谩双击桌面上的LLWIN3.0程序图檄，启勤Llwin3.0慧危机器人模型

控制程序设计系统，如图1所示，Llwin3.0可以暹行模块化程序设计。即可ifi行子程

序设计，在LLWin中可以将不壹样的功能块组装成壹种以流程图形式显示的控制程

序。假如PC,接口和模型连接封的，流程图中的数字输入(例如按钮)可以被查询且

数字输出(例如焉达)也可以被连接。每他I模型都规定有壹种独立的控制程序。罩

卷壹种相似的模型也可以创立不壹样的控制程序。壹种文献，其中包括了壹种由功能

块所构成的控制程序，在LLWin中被称作project

(项目)。用LLWin你既可以测试提供的项目也可以键入和测试你自己的项目o

•mB；I<»-><r•o><»-><•_•>wi<w>m“

QGM*J**<

图1LLWin3.0慧危机器人程序设计系统

第2步：打^项目菜甲.，如图2所示，根据被控茎寸象(机器人模型)，在选项模

板中选用项目，并输入项目名称和要存取的目录途径，罩击确定，如图3所示组态界

°

可以在菜甲.栏裹找到所有用来操作造倜软件的命令。除了适些管理，创立和测试

项目的命令，你也可以发垣程序选项和在线协助的不壹样的命令。最重要的菜罩命令

也可以通谩工具栏的图檄调用。

那么怎样控制创立的程序？功能块可彳於工具箱中获得而彳及被插入到工作表中去。

连接道些块就可构成流程图。每他独立的环节的细节在你的第壹种控制程序壹段中

ift行了描述。然彳灸你就可以连接慧角'接口来检查已完毕的控制程序（硬件测试）。

图2新建项目界面

图3：控制程序组态界面

第3步：根据被控封象的控制、运勤、检测所需要的程序流程框图，选用工具箱

的工具，工具箱的工具代表所封应封象的控制程序。各封象之间的信号连接鼠楝左键

选用图3工具栏的画笔，罩击鼠檄的左键，确定画线的起钻，移勤鼠襟器到画线的终

熟位置，按鼠檄的左键，即可连接信号，若删除连线，选用工具栏的橡皮擦可删除连

线。在暹行连线操作畤，工具箱模块是隐藏的，如要恢复显示工具箱模块，鼠襟罩击

工具栏图襟，如图4所示。

图4恢复工具箱操作

第4步：根据被控封象的运勤或状态的需要，选种封象的图檄，军击鼠襟右键,

弹出状态修改封话框，如设置重勤机的转向，罩击鼠襟右键，弹出

图5所示的封话框。

图5被控封象设置封话框

第5步：所有的被控封象连接完毕彳爰，通谩采单或工具栏的工具下载到下位计算

机的ROM中去。如程序设计完全封的，则控制程序可以封的运行。否则，需要迤行重

新设置或程序流程的修改。

子程序设计

太大的流程图也^由于非常复杂以至于你失去了全面的把握.大量的块以及连

接线使人很难理解整他项目的构造.LLWin提供了使用子程序的属性以使较大