机电测控实验实验指导书

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

实

验

指

导

书

Z儆工Q大孽

机械工程孽晚

2023.10

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

1、一级倒立摆机电系统

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

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

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

研究倒立摆系统的能有效的反映机电系统的许多典型问题：如非线性问题、鲁棒

性问题、镇定问题、随动问题以及跟踪问题等。通过对倒立摆的控制，用来检查新的

控制方法是否有较强的解决非线性和不稳定性问题的能力通过对它的研究不仅可以

解决控制中的理论和技术实现问题，还能将控制理论涉及的重要基础学科：力学，数

学和计算机科学进行有机的综合应用。其控制方法和思绪无论对理论或实际的过程控

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

论意义，尚有重要的工程背景。在多种控制理论与方法的研究与应用中，特别是在工

程实践中，也存在i种可行性的实验问题，使其理论与方法得到有效检查，倒立摆就

能为此提供一个从理论通往实践的桥梁，目前，对倒立摆的研究己经引起国内外学者

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

(1)一级倒立摆物理结构

倒立摆简朴结构，如图1所示，质量为何小车在轨道上运动，在小车上装有摆杆,

质量为〃入摆杆与小车转动轴连接，并安装编码的，用于测量摆杆的角度，摆杆可左

右运动。假如定义摆杆在垂直位置为稳定系统，显然倒立摆系统为不稳定的、典型的

机电系统，倒立摆的不稳定性表现在摆杆不能保持在竖直位置，摆杆会转动。对小车

不施加如图1所示力尸，那么施加在小车上的力尸大小、方向和作用时间如何变化才

O。

AX

图1.倒立摆结构简图

(2)一级倒立摆数学模型

建模可以分为两种：实验建模和理论建模。实验建模就是通过在研究对象上加上

一系列的研究者事先拟定的输入信号，激励研究对象并通过传感器检测其可观测的输

出，应用数学手段建立起系统的输入一输出关系。这里面涉及输入信号的设计选取，

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

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

对于倒立摆系统，由于其自身是自不稳定的系统，实验建模存在一定的困难。但是通

过小心的假设忽略掉一些次要的囚素后，倒立摆系统就是一个典型的运动刚体系统，

可以在惯性坐标系内应用经典力学理论建立系统的动力学式。采用其中的牛顿定律方

法建立直线型一级倒立摆系统的数学模型，并进行了简朴的开环仿真。

在忽略了空气阻力和各种摩擦之后，可将直线一级倒立摆系统简化成小车和匀质

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

表1

变量物理意义大小单位

M小车的质量0.618Kg

m摆杆的质量0.0737Kg

F小车的驱动力N

J摆杆的转动惯量Kg-rn2

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

摆杆初始位置为竖直向下）

摆杆与垂直向下方向的角速度rad/s

0

摆杆与垂直向下方向的角加速度rad/s2

e

K编码器参数

a摆杆与垂直向上方向的夹角rad

（。二9一万）

摆杆与垂直向上方向的角速度rad/s

a

摆杆与垂直向上方向的角加速度rad/s2

a

表1.倒立摆系统各参数的物理意义、大小、单位

在忽略了空气阻力，各种摩擦之后，可以将直线一级倒立摆系统抽象成小车和匀

质杆组成的系统。运用牛顿第二定律对小车进行受力分析，一方面对小车进行受力分

析如图2所示，摆杆的受力如图3所示。

图2.小车受力图图3.摆杆受力图

小车与地面摩擦力

(1)

fM=bx

小车水平方向受力：

F-N-hx=Mx(2)

由摆杆水平受力得：

N=fnd2(x+1sin0)/dr(3)

2

即：N=Mx+ml0cos^-ml0sin。

将式(3)代入式(2)得：

2

(M+m)x+bx+ml8CGS®—ml9sin^=F(4)

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

P-mgmd~{Icos9)Idr=0(5)

即：

2

P=mg+〃?/0sin。+ml0cos0(6)

再由摆杆的力矩平衡得：

-(p/sinMcos^)=J0+f0(7)

其中，J=ml2/3

将式（3）和式（6）代入式（7）得:

(J+m尸)0+mglsine=-f0-mlxcosG(8)

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

在施加合适力时。假定a«lrad则有：cos。r-1,sin夕r-。,。=0。线性化得到两个

线性式(9),

(J+ml21a—mlx=mgla

«(9)

(M+m)x+bx—mla-F

对式(9)进行拉氏变化(在一定的条件下，能把一实数域中的实变函数变换为一个在

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

222

J(J+ml)a(s)s-mgla(s)=mlX(s)s

[(M+m)X(s)s2+bX(s)s-mla(s)s2=U(s)(10)

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

X(s』("'/-)_4(s)(11)

mls

将其代入式(10)的第2个式得:

(M+M业总与()2)一与2

a55+ba(s)s-mla(s)s=U(s)(12)

mlsmls

整理后得到传递函数为:

ml2

一s

a(s)A

G(s)=(13)

4b(J¥mF)3(M+m)mglbmgl

西S+Ss2s

AAA

其中：j=+m\J+ntl2)-m2l2](14)

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

传递函数。

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

式(13)为摆杆的运动数学模型，要使摆杆竖直在垂直位置，必须对小车施加控

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

目的有二：

其一，可验证理论数学模型的可靠性。

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

运用经典的控制原理分析方法，在复频域、频域、时域进行分析，重要分析伯德

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

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

结果。

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

Nyquist(num,dcn),Nichols(num,dcn),即可得到需要结果，num为传递函数的极点，den

为传递函数的零点。在word软件下保存图形，用于实验报告。

实验1、根轨迹分析

»Num=[3.3501;

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

»Rlocus(num,den);

实验2、Bode图分析

>>Num=[3.350J;

»Den=[10.0786-22.7085-1.64151;

»bode(num,den)

实验3、Nyquist分析

»Num=[3.350];

»Den=[10.0786-22.7085-1.6415];

»Nyquist(num,den)

实验4、Nichols分析

»Num=[3.350];

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

»Nichols(num,den)

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

概述

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

机、人工智能、系统工程、控制理论、自动检测等学科的最新成果。机器人对于实现

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

用，此后会越来越广，将对人类的生产和生活产生巨大的影响。

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

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

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

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

又称为专用机械手。

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

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

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

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

通常则称为机器人(Robot)。

实验一、机器人运动与控制结识

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

一个位移矢量表达了空间任意两点之间的有向距离。为了进行机构分析，机构中

每一根连杆都可以表达为一个位移矢量，矢量的起点就是连杆的某一端点，而另一端

点就是矢量的终点。这个位移矢量的大小就是连杆的长度，矢量与X轴正向间的夹角

就是连杆的夹角（逆时针为正）。对机构的位移轨迹仿真，采用的方法就是矢量式法。

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

备的限制（如慧鱼机器人的精度较低、其塑料材质与动力学方面无应用价值，其机械

结构对工业用机器人的一般结构不具指导意义等因素），未对运动学方面的加速度和

手爪受到的力的变化进一步分析。

1.1矢量式法

图1是一个典型的四连杆机构，用矢量式对其进行分析。为方便起见，设定该机

构的x-y坐标系如图中所示。由于在同一坐标系下位移矢量有多种排序表达方法，所

以各位移矢量的编号顺序应遵循一定的约定。即，对于任意一组位移矢量，应当构成

一个易于对的表达和便于推导的闭环矢量式。因此，根据矢量加法的规定，矢量区和

凡应当首尾相连，矢置用和凡亦是如此。数学表达式为：

凡+用=兄+凡（1）

矢量R2和用相加而得到的位移矢量与矢量A和几叠加得到的位移矢量是完全

相同的；无论机构运动到何种状态，只要可以保证可以机构的几何装配条件，则这个

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

将闭环矢量式分解成为两个标量表达式。在图示例子中，将矢量分解成X,y两

个方向。一个沿x方向分解，一个沿y方向分解。关于矢量角度的定义，若由x正向

旋转到矢量的矢端为逆时针转动，则该矢量的角度为正。则分解后的闭环矢量式的两

个分量表达式为：

GCOS夕2+bCOS%=ACOS&+GCOSW、

r⑵

r,sin%+qsin%=qsin〃+。sin%

其中：r.〜心代表各个连杆的长度，在整个分析过程中是保持不变的。

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

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

力（4,%）=r2cos②+4cos%F-Gcos%=0、

r⑶

r

f2（fi\,%）=2sin.+弓sin0y-r4sin04=0

所以：位置问题可描述为：对于给定的一组连杆长度和％值，寻求合适的%、/，

使力、八为零。由于，、右是超越和非线性的，线性分析方法已不合用，牛顿-辛普

森方法合适求解这类问题。

设：％-仇十△夕3

(4)

名二小+△2

其中：必、内为近似解，△%、为修正系数。

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

△2_一〃sin®3—〃sin"-力（2，%）

-AG」4cos%-qcosW-力⑸网）

运用（5）式，采用Mallab计算，可求解图1的位置问题。程序见附录

1.3、四连杆机构运动学

在图1的四连杆机构中，连杆2为电动机驱动以匀速转动，对式（2）中的。3、

%求关于时间的一阶导数可得各连杆的速度(6)、二阶导数可得各连杆的运动加速度

(7)：

cos

-gr2sin%-03G4=-%Qsin94

(6)

a)2r2cos区+处qcos%=grAcos/

999

-a3r3sina-小工cosa+a4r4sin仇+co4~r4cos/=a2r2sin02+a)2~r2cos，、

»(7)

«37jCOs^3一例?弓sin6^3-a4r4cos^4+g%sin^4=-a2r2cos%+gXsin%

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

一Gsin/_qsin/co2r2sin8?

(8)

-corcosa

4c。沟-qcos/22

222

一叫sin4-与sinqa2r2sin^2+co2~r2cos^2+<y3qcosa-g〃cos%

-a2r2cos^2+g2Gsin^2十例〉sin4-g^sin/

Gcos%-r4cos^4

(9)

其中：%%表达连杆的加速度；①3、%表达连杆的角速度;

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

取四连杆机构参数见表2

连杆长度(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机为上位机，单片机为下位机。上、下

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

展是高

起始位DO[D1ID2ID3ID4|D5|D6|停止位卜不止位

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

在异步串行口中收发的每一个字节数据都是由以下4个部分按顺序组成

・1个起始点

・8个数据点位(在某些情况下是7个)

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

•1个或2个停止位

数据收发线的空状态是高电平。起始位标志着一个新字节传送的开始，与一位数据位

占同样的时间宽度，低电平。起始位之后就是7个或8个数据位。当数据位为0时，

收发线为低电平，反之为低电平。在数据位之后是奇偶校验位。奇偶校验位是用来检

查在传送过程中是否发生差错。用户可选择是否进行奇偶校验，假如进行，还可选择

进行奇校验还是偶校验。若选择偶校验，则各位数据位加上校验位自身，其和将是一

个偶数；若选择奇校验，其和将是一个奇数。在奇偶校验位之后就是停止位，它可以

占一位或两位数据位的时间宽度。停止位是低电平，它表达一个字节传送的结束。

在停止位之后，可以接着又是下一个字节的起始位，也可以在停顿任意长时间后,

才是下一个字节的起始位⑺。数据收发的速度是月波特率(baudrate)来衡量的，波

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

之间。

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

因此异步通信一般用在数据速率较慢的场合。在高速传送时，一般应采用同步协议。

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

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

定的，而在同一字符中两个相邻位代码间的间隔是固定的。但在同步通信中，每时每

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

每个字符中各个相邻位代码间的时间间隔也是固定的。

大部分串行口的结构都是在发送和接受方装有一个25针插座，都遵照RS-232标

准，但又都不严格遵循这个标准。如本机器人上/下位机的接口处装的插座就是9针

插座。这9针信号是：

DLSD1接受线信号检查

RXD2接受激据

TXD3发送数据

DTR4数据终端准备好

GRD5信号地

DSR6数据设备准备好

RTS7请求发送

CTS8清除发送

RI9振铃指示⑺

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

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

是8251,通常称为通用同步异步收发器（universalsynchronous-asynchronous

receiver-transmitterUSART161）。

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

的是通过解调器转换来的数字信号，所以8251要和调制/解调器直接相连。调制解调

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

号的器件⑹。

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

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

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

送移位寄存器，还发送缓冲寄存器和控制电路。从CPU传送出来的数据经数据发送缓

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

据之前，发送器将自动送出一个（单同步）或两个（双同步）同步字符（Sync）,然

后逐位串行输出数据。

E1A（ElectronicIndustriesAssociation美国电子工业协会）RS-232C是日前最常用

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

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

的电气特性（涉及：信号的逻辑电平、最高数据咕输和各种信号功能）。RS-232提供

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

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

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

用的连接器为DB-9。

传输电缆的长度与传输的电容有关。E1A标准规定被驱动电路（终端）的电容,

涉及电缆连接电容必须小于2500pFo对于一个多芯电缆来说，每英尺(0.305m)电

容为40pF〜50pF,所以满足电容特性的电缆长度为50英尺(15.24m)o但若能保证

电缆总电容小于2500pF,则电缆长度可超过限定值。同时，RS-232C标准所允许的

信号传输速率在。〜20230b/s的范围之内，而在实际应用中常被限制在1920b/s以内。

2.2串行通讯程序设计

2.2.1、初始化串口：

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

很重要。

一般来说，初始化串口的重要有两步：

(1)写入控制字；

设立通讯方式：数据的波特率，通讯的方式(实验中通讯方式为全双工船型

通讯方式)等。

初始化成程序如下：

voidCMy5y19View::fischer_install()

{DCBdeb;

DWORDdwError;

BOOLfSuccess;

hCom=CreateFile(

"COM1",/*nameofCOMdevicetoopen*/

GENERIC_READ|GENERIC_WRITE,/*read-writeaccess*/

(),/*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

dcb.ByteSize=8;//8Bits

dcb.Parity=NOPARITY;//noparity

dcb.StopBits=ONESTOPBIT;//1slopbit

}

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

如对一电动机左转或右转命令，命令由固化在下位计算机ROM内的程序解释执行，下

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

(1)电动机函数，motor(intnum,chardir：)函数作用是控制机器人电动机动作

左转、右转

voidmotor(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=clcarBit(Motorzustand,(num*2-1));

//clearleftbit

break;}

)

)

Result=input_word();//writemotorbytetotheinterface

)

(2)电动机停止左转、右转函数

voidmotors_off()

(

charresult;

Motorzustand=0;//resetall

result=input_word();//sendmotorsettingstointerface

)

(3)模拟量输入函数

inianalog_in(charnum)

LPSTRBefehl="

LPSTRZustand=n”；

BOOLResult;

DWORDWritten;

DWORDRead;

switch(num)

//switchnum

(

case0:Befehl[0]-\xc5';

//1.Byte:197inHexforEX

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

//1.Byte:201inHexforEY

)

Befehl[i]=Motorzustand;

//2.Byte:motorsetting

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

//send2Byte

Result=ReadFile(hCom,Zustand,3,&Read,NULL);

//read3Byte//1.Byteisthestatusof//InputsEltoE8

//2.Byteand3Byteis//thevalueoftheanaloginput

retum((Zustand[1]*255)+Zustand[2]);

//returnintegervalue

)

(4)位清除函数

clearBit(charwert,intbit)

intbitvorne;

intbithinten;

div_tvome;

div_thinten;

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

vorne=div(wert,bitvorne);

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

hinten=div(wert,bithinten);

return(vome.quot*bilvorne+hinten.rem);

)

(5)位检测函数

testBit(charwert,intbit)

(

intbilvorne;

intbithinten;

intvorne_neu;

chartemp;

div_tvorne;

div_thinten;

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

vorne=div(wert,bitvorne);

vorne_neu=vorne.quot5}:bitvorne;

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

hinten=div(wert,bithinten);

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

return(temp);

}

(6)开关计算函数

switch_in(charnum)

{

charresult;

result=input_word();//readtheinputstatus

returntestBit(result,num);

I

(7)位设立函数

setBit(charwcrt,intbit)

{

intbitvorne;

intbithinten;

intvorne_neu;

div_tvorne;

div_thinten;

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

vorne=div(wert,bitvorne);

vorne_neu=vorne.quot*2+1;

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

hinlen=div(werl,bithinten);

return(vorne_neu*bithinten+hinten.rem);

)

motor(intnum,chardir)

{

charResult;

switch(dir)//switchdirection

case'A'://off

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

//clearrightbit

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

//clearleftbit

break;

1

case'L'://left

(

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

//setrightbit

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

//clearleftbit

break;

1

caseR://right

(

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

//setrightbit

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

//clearleftbit

break;

1

}

Result=input_word（）;

return0;

）

3、慧鱼机器人模型控制实验步聚

（1）根据慧鱼机器人资料和手则选用合适的机械零件、传动机构、构建运动机械模

型。各机械零件、传动机构、电动机等机陶之间应可靠安装。

（2）连接实验电路、安装传感落、接口板，注意电源的极性。

（3）通过串行线连接上、下位计算机，注意：在上、下位计算机未通电时插接。不

可热插拔，否则会损坏计算机。

（4）采用VC++或LLWIN3.0设计通讯控制程序（使用手则附录1）。

（5）向下位计算机加载控制信号，调试机械机构运动是否正常，否则，紧固机械零

件、传动机构、电动机使之可靠连接。

4、实验项目:

（1）慧鱼工业机器人

（2）慧鱼移动机器人

（3）慧鱼冲床模型

（4）慧鱼仿生机器人

同学在以上项目中选择一个，按3中的规定完毕实验。

5、实验报告规定（评分标准5级分/100分）

（1）仔细分析倒立摆机电系统模型，建立微分方程，运用拉式变换转换为传递函数。

（3分/10分）

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

（3）叙述实验设计思绪，设计或构建的机器人也许应用的领域。（1分/10分）

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

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

量式或D-H坐标变换法，对其进行位置、运动学分析仿真。，并分析仿真结果（仿

真结果打印学生自己解决)。(3分/10分)

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

分分)

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

条理和学生守则解决。

附录1

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

posso.m

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

th2=th(l);

th3bar=th(2);

th4bar=th(3);

epsilon=1.0E-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(f)>epsilon

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

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

dth=inv(J)*(-1.0*f);

th3bar=th3bar+dth(1);

th4bar=th4bar4-dth(2);

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

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

norm(f)

end;

th3=th3bar;

th4=th4bar;

附录2

2、四连杆机构运动问题的计算程序：

1,2=4;

「3=10;

r4=7;

th2=45*pi/180;

th3=24.652*pi/!80;

th4=90.6794*pi/180;

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

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

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

主程序设计

第1步：通过双击桌面上的LLWIN3.0程序图标，启动Llwin3.0慧鱼机器人模型

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

序设计，在LLWin中可以将不同的功能块组装成一个以流程图形式显示的控制程序。

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

输出(比如马达)也可以被连接。每个模型都规定有一个独立的控制程序。单为一

个相同的模型也可以创建不同的控制程序。一个文献，其中包含了一个由功能块所组

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

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

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

第2步：打开项目菜单，如图2所示，根据被控对象（机器人模型），在选项模

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

面。

可以在菜单栏里找到所有用来操作这个软件的命令。除了这些管理，创建和测试

项目的命令，你也可以发现程序选项和在线帮助的不同的命令。最重要的菜单命令也

可以通过工具栏的图标调用。

那么如何控制创建的程序？功能块可从工具箱中获得而后被插入到工作表中去。

连接这些块就可构成流程图。每个独立的环节的细节在你的第一个控制程序一段中

进行了描述。然后你就可以连接慧鱼接口来检查己完毕的控制程序（硬件测试）。

图2新建项目界面

图3：控制程序组态界面

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

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

选用图3工具栏的画笔，单击鼠标的左键，拟定画线的起点，移动鼠标器到画线的终

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

线。在进行连线操作时，工具箱模块是隐藏的，如要恢复显示工具箱模块，鼠标单击

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

图4恢复工具箱操作

第4步：根据被控对象的运动或状态的需要，选种对象的图标，单击鼠标右键,

弹出状态修改对话框，如设立电动机的转向，单击鼠标右键，弹出

图5所示的对话框。

图5被控对象设立对话框

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

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

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

子程序设计