机电测控实验实验指导书.doc

机电系统测控、机器人（II）

实
验
指
导
书

安徽工业大学
机械工程学院
2023．10

项目一 倒立摆机电系统建模、分析与控制
1、一级倒立摆机电系统
倒立摆系统是典型的快速、多变量、非线性、强耦合、不稳定的机电系统，是控制理论中研究的热点，同时生产中也有很多得应用，因此研究和分析倒立摆系统，在理论和实践上都具故意义。
研究倒立摆系统的能有效的反映机电系统的许多典型问题：如非线性问题、鲁棒性问题、镇定问题、随动问题以及跟踪问题等。通过对倒立摆的控制，用来检查新的控制方法是否有较强的解决非线性和不稳定性问题的能力通过对它的研究不仅可以解决控制中的理论和技术实现问题，还能将控制理论涉及的重要基础学科：力学，数学和计算机科学进行有机的综合应用。其控制方法和思绪无论对理论或实际的过程控制都有很好的启迪，可以检查多种控制理论和方法。倒立摆的研究不仅有其深刻的理论意义，尚有重要的工程背景。在多种控制理论与方法的研究与应用中，特别是在工程实践中，也存在一种可行性的实验问题，使其理论与方法得到有效检查，倒立摆就能为此提供一个从理论通往实践的桥梁，目前，对倒立摆的研究已经引起国内外学者的广泛关注，是控制领域研究的热门课题之一。
（1） 一级倒立摆物理结构
倒立摆简朴结构，如图1所示,质量为小车在轨道上运动，在小车上装有摆杆，质量为。摆杆与小车转动轴连接，并安装编码器，用于测量摆杆的角度，摆杆可左右运动。假如定义摆杆在垂直位置为稳定系统，显然倒立摆系统为不稳定的、典型的机电系统，倒立摆的不稳定性表现在摆杆不能保持在竖直位置，摆杆会转动。对小车不施加如图1所示力，那么施加在小车上的力大小、方向和作用时间如何变化才干保证摆杆保持在垂直位置。

图1. 倒立摆结构简图

（2） 一级倒立摆数学模型
建模可以分为两种：实验建模和理论建模。实验建模就是通过在研究对象上加上一系列的研究者事先拟定的输入信号，激励研究对象并通过传感器检测其可观测的输出，应用数学手段建立起系统的输入一输出关系。这里面涉及输入信号的设计选取，输出信号的精确检测，数学算法的研究等内容。机理建模就是在了解研究对象的运动规律基础上，通过物理、化学的知识和数学手段建立起系统内部的输入一状态关系。对于倒立摆系统，由于其自身是自不稳定的系统，实验建模存在一定的困难。但是通过小心的假设忽略掉一些次要的因素后，倒立摆系统就是一个典型的运动刚体系统，可以在惯性坐标系内应用经典力学理论建立系统的动力学式。采用其中的牛顿定律方法建立直线型一级倒立摆系统的数学模型，并进行了简朴的开环仿真。
在忽略了空气阻力和各种摩擦之后，可将直线一级倒立摆系统简化成小车和匀质杆组成的系统。倒立摆系统各参数的物理意义、大小、单位，如下表1所示：
表1
变量
物理意义
大小
单位

小车的质量
0.618
Kg

摆杆的质量
0.0737
Kg
F
小车的驱动力

N
J
摆杆的转动惯量

Kg·m2
g
本地的重力加速度
9.8
Kg/m·s2
b
小车与水平面的摩擦系数
0.1
N/m/sec
L
摆杆的长度
0.350
m
l
摆杆质心到转轴的距离
0.1225
m
P
摆杆对小车水平方向上的力

N
N
摆杆对小车竖直方向上的力

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

N/rad/sec
x
小车的水平位移

m

小车速度

m/s

小车加速度

m/s2
θ
摆杆与垂直向下方向的夹角(考虑到摆杆初始位置为竖直向下)

rad

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

rad/s

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

rad/s2
K
编码器参数

α
摆杆与垂直向上方向的夹角()

rad

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

rad/s

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

rad/s2
表1.倒立摆系统各参数的物理意义、大小、单位
在忽略了空气阻力，各种摩擦之后，可以将直线一级倒立摆系统抽象成小车和匀质杆组成的系统。运用牛顿第二定律对小车进行受力分析，一方面对小车进行受力分析如图2所示，摆杆的受力如图3所示。

图2.小车受力图 图3.摆杆受力图
小车与地面摩擦力

（1）
小车水平方向受力：

（2）
由摆杆水平受力得：
（3）
即：
将式（3）代入式（2）得：

（4）
由摆杆垂直方向受力分析图2，可知：

（5）
即：

（6）
再由摆杆的力矩平衡得：

（7）
其中，
将式（3）和式（6）代入式（7）得：

（8）
式（4）和（8）均是非线性，需要线性化解决，由于控制的目的是保持倒立摆直立，在施加合适力时。假定1rad则有：。线性化得到两个线性式(9)，

（9）
对式(9)进行拉氏变化（在一定的条件下，能把一实数域中的实变函数变换为一个在复数域内与之等价的复变函数）(设初始条件为0），控制信号为u：
（10）
选择摆杆倾斜角为仿真输出，由式(10)的第1个式得：

（11）
将其代入式(10)的第2个式得：

（12）
整理后得到传递函数为：

（13）
其中：
(14)
式(13)为摆杆倾斜角与控制量的传递函数。代入表一数据，可得到倒立摆系统的传递函数。
（3）一级倒立摆特性分析与计算
式（13）为摆杆的运动数学模型，要使摆杆竖直在垂直位置，必须对小车施加控制，使摆杆获得倾斜方向相反的加速度，在施加控制之前，要对摆杆的特性进行分析，目的有二：
其一，可验证理论数学模型的可靠性。
其二，可为选择控制器、设计控制器提供帮助。
运用经典的控制原理分析方法，在复频域、频域、时域进行分析，重要分析伯德图（bode图），根轨迹图（Rlocus图）、Nyquist图、尼柯尔斯图（Nichols图）。分析运用Matlab/control systems toolbox中的一些工具函数，可方便的得到需要的结果。
在Matlab命令窗口（command windows）输入Rlocus(num,den)，bode(num,den), Nyquist(num,den), Nichols(num,den),即可得到需要结果，num为传递函数的极点，den为传递函数的零点。在word软件下保存图形，用于实验报告。
实验1、 根轨迹分析
>> Num=[3.35 0 ];
>>Den=[1 0.0786 -22.7085 -1.6415];
>> Rlocus(num,den);
实验2、 Bode图分析
>> Num=[3.35 0 ];
>>Den=[1 0.0786 -22.7085 -1.6415];
>>bode(num,den)
实验3、 Nyquist分析
>> Num=[3.35 0 ];
>>Den=[1 0.0786 -22.7085 -1.6415];
>> Nyquist(num,den)
实验4、 Nichols分析
>> Num=[3.35 0 ];
>>Den=[1 0.0786 -22.7085 -1.6415];
>>Nichols(num,den)

项目二 慧鱼机器手模型构建实现、控制
概述
机器人技术是最近2023间发展起来的新技术，它综合了机械、力学、电子计算机、人工智能、系统工程、控制理论、自动检测等学科的最新成果。机器人对于实现柔性生产自动化、保护劳动安全和提高产品质量能发挥重大的作用。机器人技术的应用，此后会越来越广，将对人类的生产和生活产生巨大的影响。
机械手是一种模仿人手的某些工作机能，被给定的程序、轨迹和规定，实现抓取、搬运工件，或者完毕某些劳动作业的机械化、自动化装置[1]。国外把它称为：操作机(Manipulator) 、机械手(Mechanical Hand )。机械手只能完毕一些比较简朴的抓取，搬运及上下料工作、经常作为机器设备上的附属装置。因此具有一定的专用性，所以又称为专用机械手。
机器人是能模仿人的某些工作机能和控制机能，按可变的程序、轨迹和规定，实现多种工件的抓取、定向和搬动工作，并且能使用工具完毕多种劳动作业的自动化机械系统。因此，机器人比机械手更为完善，它不仅具有劳动和操作的机能．并且还具有学习、记忆及感觉功能。国外把它称为程序控制操作机(Programmable Manipulator)。通常则称为机器人(Robot)。
实验一、机器人运动与控制结识
1、机构位置的求解及仿真
一个位移矢量表达了空间任意两点之间的有向距离。为了进行机构分析，机构中每一根连杆都可以表达为一个位移矢量，矢量的起点就是连杆的某一端点，而另一端点就是矢量的终点。这个位移矢量的大小就是连杆的长度，矢量与x轴正向间的夹角就是连杆的夹角（逆时针为正）。对机构的位移轨迹仿真，采用的方法就是矢量式法。对机构的速度分析仿真，既采用了矢量式法，也采用了拟合仿真法。由于受到实验设备的限制（如慧鱼机器人的精度较低、其塑料材质与动力学方面无应用价值，其机械结构对工业用机器人的一般结构不具指导意义等因素），未对运动学方面的加速度和手爪受到的力的变化进一步分析。
1．1矢量式法
借助于一个一般四连杆机构， 简朴地阐述模型位移轨迹的分析方法。

图1：四联杆机构
图 1是一个典型的四连杆机构，用矢量式对其进行分析。为方便起见，设定该机构的x-y坐标系如图中所示。由于在同一坐标系下位移矢量有多种排序表达方法，所以各位移矢量的编号顺序应遵循一定的约定。即，对于任意一组位移矢量，应当构成一个易于对的表达和便于推导的闭环矢量式。因此，根据矢量加法的规定，矢量和应当首尾相连，矢量和亦是如此。数学表达式为：
+=+ （1）
矢量和相加而得到的位移矢量与矢量和叠加得到的位移矢量是完全相同的；无论机构运动到何种状态，只要可以保证可以机构的几何装配条件，则这个闭环矢量式就一定可以成立。
将闭环矢量式分解成为两个标量表达式。在图示例子中，将矢量分解成x，y两个方向。一个沿x方向分解，一个沿y方向分解。关于矢量角度的定义，若由x正向旋转到矢量的矢端为逆时针转动，则该矢量的角度为正。则分解后的闭环矢量式的两个分量表达式为：
cos+cos=cos+cos
(2)
sin+sin=sin+sin
其中： ～代表各个连杆的长度，在整个分析过程中是保持不变的。
1．2四连杆机构位置问题的解
选择连杆1作为参照系，把四连杆机构闭环矢量式（2）重新组合
=cos+cos--cos=0
(3)
=sin+sin-sin=0
所以：位置问题可描述为：对于给定的一组连杆长度和值，寻求合适的、，使、为零。由于、是超越和非线性的，线性分析方法已不合用，牛顿-辛普森方法合适求解这类问题。
设：=
(4)

其中：、为近似解，、为修正系数。
采用牛顿-辛普森算法可得:
= (5)
运用（5）式，采用Matlab计算，可求解图1的位置问题 。 程序见附录
1．3、四连杆机构运动学
在图1的四连杆机构中，连杆2为电动机驱动以匀速转动，对式（2）中的、求关于时间的一阶导数可得各连杆的速度（6）、二阶导数可得各连杆的运动加速度（7）：
-sin-cos= -sin
（6）
cos+cos=cos

（7）

（6）、（7）式写为矩阵式的形式分别为（8）、（9）：
= （8）
= （9）
其中： 表达连杆的加速度; 、表达连杆的角速度；
对式（8）、（9）描述的四连杆机构在Matlab下编制计算程序
取四连杆机构参数见表2
连杆
长度（r）/cm
转角
1
12.0
00
2
4.0
450
3
10.0
24.650
4
7.0
90.680
实验2、机器人控制程序设计
2．1串行通讯的概念
慧鱼机器人是采用上、下位机的控制方式，由上位计算机通过串行口RS232向下位单片机发出控制指令，下位计算机解释执行，从而达成控制的目的，串行通讯的格式有：单工（simplex）通信、半双工（half-duplex）通信、全双工（full-duplex）通信。单工数据线仅可以在一个方向上传输数据。从震区发回数据的地震传感器和商业无线电台都属于典型的单工传输的例子。半双工传输可以在两个系统间以任何一个方向传输，但同一时间只能在一个方向上传输。半双工的例子是无线电对讲机，一个人在说的时候，另一个人只能在听，由于发送期间电路是断开的。全双工的意思是每个系统可以同时发送和接受数据。常用的电话系统就是全双工的例子[6]。在串行通信中，数据的收发可采用同步（synchronous）和异步（asynchronous）两种基本的工作方式。同步通信方式，数据以块的形式按一定的速率发送，每块的开始与结束由特定的字节或特定位来说明。异步通信所采用的数据格式是以一组不定“位数”数组组成。通常，异步通信速率要比同步通信的低。每秒传送数据的位数成为传送速率，即波特率（baud rate）[5]。PC机为上位机，单片机为下位机。上、下位机的数据传输采用同步全双工的工作方式。

在异步串行口中收发的每一个字节数据都是由以下4个部分按顺序组成
l 1个起始点
l 8个数据点位（在某些情况下是7个）
l 奇偶校验位（可有可无，可奇可偶）
l 1个或2个停止位
数据收发线的空状态是高电平。起始位标志着一个新字节传送的开始，与一位数据位占同样的时间宽度，低电平。起始位之后就是7个或8个数据位。当数据位为0时，收发线为低电平，反之为低电平。在数据位之后是奇偶校验位。奇偶校验位是用来检查在传送过程中是否发生差错。用户可选择是否进行奇偶校验，假如进行，还可选择进行奇校验还是偶校验。若选择偶校验，则各位数据位加上校验位自身，其和将是一个偶数；若选择奇校验，其和将是一个奇数。在奇偶校验位之后就是停止位，它可以占一位或两位数据位的时间宽度。停止位是低电平，它表达一个字节传送的结束。
在停止位之后，可以接着又是下一个字节的起始位，也可以在停顿任意长时间后，才是下一个字节的起始位[7]。数据收发的速度是用波特率（baud rate）来衡量的,波特率就是每秒收发的位数。波特率一般在300，600，900，1200，2400，…9600波特之间。
由于异步通信每传送一个字符都加起、止信号灯附加位，使其传输效率比较低，因此异步通信一般用在数据速率较慢的场合。在高速传送时，一般应采用同步协议。由面向字符的同步协议和面向比特的同步协议。
异步通信于同步通信的比较：异步通信是指通信中两个字符间的时间间隔是不固定的，而在同一字符中两个相邻位代码间的间隔是固定的。但在同步通信中，每时每刻在链路上都有字符信息传送，并且通信中的每个字符间的时间间隔是相等的；此外，每个字符中各个相邻位代码间的时间间隔也是固定的。
大部分串行口的结构都是在发送和接受方装有一个25针插座，都遵照RS-232标准，但又都不严格遵循这个标准。如本机器人上/下位机的接口处装的插座就是9针插座。这9针信号是：
DLSD 1 接受线信号检查
RXD 2 接受数据
TXD 3 发送数据
DTR 4 数据终端准备好
GRD 5 信号地
DSR 6 数据设备准备好
RTS 7 请求发送
CTS 8 清除发送
RI 9 振铃指示[7]
用串行数据线与微型机接口，数据必须进行串并行转换。需要用握手电路来保证发送方发送数据的速度不比接受方可以接受的速度快。本机器人采用的串行通信芯片是8251，通常称为通用同步异步收发器（universal synchronous-asynchronous receiver-transmitter USART[6] ）。
8251A的数据输出端要通过调制器将数字信号转换成模拟信号，数据接受端收到的是通过解调器转换来的数字信号，所以8251要和调制/解调器直接相连。调制解调器（modem）是用来将数字信号转换成音频信号，以及激昂音频信号在转换回数字信号的器件[6]。
在本台机器人串行接口处用的芯片是8521A。下面对该芯片作简朴的介绍。8251A由发送器、接受器、数据总线缓冲寄存器、读/写控制电路及调制/解调控制电路等5部分组成。发送器内部的发送移位寄存器，是用来完毕并→串转换的。发送时除了发送移位寄存器，还发送缓冲寄存器和控制电路。从CPU传送出来的数据经数据发送缓冲寄存器并行输入，并锁存到发送缓冲存储器中。如本机采用的同步方式，在发送数据之前，发送器将自动送出一个（单同步）或两个（双同步）同步字符（Sync），然后逐位串行输出数据。
EIA（Electronic Industries Association 美国电子工业协会）RS-232C是目前最常用的串行接口标准，用于实现计算机与计算机之间、计算机与外设的数据通信。本机器人接口的标准采用的是RS-232。该标准的目的是定义数据终端设备（DTE）之间接口的电气特性（涉及：信号的逻辑电平、最高数据传输和各种信号功能）。RS-232提供了单片机与单片机、单片机与PC机间串行数据的通信接口。由于RS-232标准只规定了采用一对物理连接器，但对连接器自身的物理特性没有任何定义，因而出现了DB-25、DB-15和DB-9等各种类型的连接器，其引脚的定义也各不相同。本机器人采用的连接器为DB-9。
传输电缆的长度与传输的电容有关。EIA 标准规定被驱动电路（终端）的电容，涉及电缆连接电容必须小于2500pF。对于一个多芯电缆来说，每英尺（0.305m）电容为40pF～50pF，所以满足电容特性的电缆长度为50英尺（15.24m）。但若能保证电缆总电容小于2500pF，则电缆长度可超过限定值。同时，RS-232C标准所允许的信号传输速率在0～20230b/s的范围之内，而在实际应用中常被限制在1920b/s以内。
2．2串行通讯程序设计
2．2．1、初始化串口：
在程序控制中，除了实现动作的控制程序和线程的创建很重要外，初始化串口也很重要。
一般来说，初始化串口的重要有两步：
（1）写入控制字；
设立通讯方式：数据的波特率，通讯的方式（实验中通讯方式为全双工船型通讯方式）等。
初始化成程序如下：
void CMy5y19View::fischer_install()
{DCB dcb;
DWORD dwError;
BOOL fSuccess;
hCom = CreateFile(
"COM1", /* name of COM device to open */
GENERIC_READ | GENERIC_WRITE, /* read-write access */
0, /* not used */
NULL, /* not used */
OPEN_EXISTING, /* required for tape devices */
0, /* not used */
NULL); /* not used */
if (hCom == INVALID_HANDLE_VALUE)
{dwError = GetLastError();
}
fSuccess = GetCommState(hCom, &dcb);
/* Fill in the DCB: baud=9600, 8 data bits, no parity, 1 stop bit. */
dcb.BaudRate = 9600; // 9600 Baud
dcb.ByteSize = 8; // 8 Bits
dcb.Parity = NOPARITY; // no parity
dcb.StopBits = ONESTOPBIT; // 1 stopbit
}
(2) 通过以上C++代码建立了上下位计算机的通讯机制，上位计算机可以发送命令，如对一电动机左转或右转命令，命令由固化在下位计算机ROM内的程序解释执行，下位计算机ROM内的程序说明如下：
(1)电动机函数，motor(int num, char dir)函数作用是控制机器人电动机动作左转、右转
void motor(int num, char dir)
{char Result;
switch (dir) // switch direction
{case 'A': // off
{Motorzustand = clearBit(Motorzustand, (num * 2));
// clear right bit
Motorzustand = clearBit(Motorzustand, (num * 2 - 1));
// clear left bit
break; }
case 'L': // left
{
Motorzustand = clearBit(Motorzustand, (num * 2));
// set right bit
Motorzustand = setBit(Motorzustand, (num * 2 - 1));
// clear left bit
break; }
case 'R':
// right
{Motorzustand = setBit(Motorzustand, (num * 2)); // set right bit
Motorzustand = clearBit(Motorzustand, (num * 2 - 1));
// clear left bit
break; }
}
}
Result = input_word(); // write motorbyte to the interface
}
（2）电动机停止左转、右转函数
void motors_off( )
{
char result;
Motorzustand = 0; // reset all
result = input_word( ); // send motor settings to interface
}
（3）模拟量输入函数
int analog_in(char num )
{
LPSTR Befehl=" ";
LPSTR Zustand=" ";
BOOL Result;
DWORD Written;
DWORD Read;
switch (num)
// switch num
{
case 0: Befehl[0]='\xc5';
// 1. Byte : 197 in Hex for EX
case 1: Befehl[0]='\xc9';
// 1. Byte : 201 in Hex for EY
}
Befehl[1]=Motorzustand;
// 2. Byte : motor setting
Result = WriteFile(hCom,Befehl,2,&Written,NULL);
// send 2 Byte
Result = ReadFile(hCom,Zustand,3,&Read,NULL);
// read 3 Byte// 1. Byte is the status of //Inputs E1 to E8
// 2. Byte and 3 Byte is //the value of the analog input
return((Zustand[1] * 255) + Zustand[2]);
// return integer value
}
（4）位清除函数
clearBit(char wert, int bit)
{
int bitvorne;
int bithinten;
div_t vorne;
div_t hinten;
bitvorne = int(pow(2,bit));
vorne = div(wert,bitvorne);
bithinten = int(pow(2,(bit-1)));
hinten = div(wert,bithinten);
return (vorne.quot * bitvorne + hinten.rem);
}
（5）位检测函数
testBit(char wert, int bit)
{
int bitvorne;
int bithinten;
int vorne_neu;
char temp;
div_t vorne;
div_t hinten;
bitvorne = int(pow(2,bit));
vorne = div(wert,bitvorne);
vorne_neu=vorne.quot*bitvorne;
bithinten = int(pow(2,(bit-1)));
hinten = div(wert,bithinten);
temp=(wert-vorne_neu - hinten.rem);
return (temp);
}
（6）开关计算函数
switch_in(char num)
{
char result;
result = input_word(); // read the input status
return testBit(result, num);
}
（7）位设立函数
setBit(char wert, int bit)
{
int bitvorne;
int bithinten;
int vorne_neu;
div_t vorne;
div_t hinten;
bitvorne = int(pow(2,bit));
vorne = div(wert,bitvorne);
vorne_neu=vorne.quot*2+1;
bithinten = int(pow(2,(bit-1)));
hinten = div(wert,bithinten);
return (vorne_neu * bithinten + hinten.rem);
}
motor(int num, char dir)
{
char Result;
switch (dir) // switch direction
{
case 'A': // off
{
Motorzustand = clearBit(Motorzustand, (num * 2));
// clear right bit
Motorzustand = clearBit(Motorzustand, (num * 2 - 1));
// clear left bit
break;
}
case 'L': // left
{
Motorzustand = clearBit(Motorzustand, (num * 2));
// set right bit
Motorzustand = setBit(Motorzustand, (num * 2 - 1));
// clear left bit
break;
}
case 'R': // right
{
Motorzustand = setBit(Motorzustand, (num * 2));
// set right bit
Motorzustand = clearBit(Motorzustand, (num * 2 - 1));
// clear left bit
break;
}
}
Result = input_word();
return 0 ;
}
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分/10分）
（6） 同组同学可以自行分工，分别完毕各自实验任务，弄虚作假者按学校有关管理条理和学生守则解决。

附录1
1、四连杆机构位置问题的牛顿-辛普森算法程序：
posso.m
function [th3,th4]=posso(th,rs)
th2=th(1);
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(1);
-rs(3)*sin(th3bar)-rs(4)*sin(th4bar)+rs(2)*sin(th(2))];
while norm(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=th4bar+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、四连杆机构运动问题的计算程序：
r2=4;
r3=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=[100*r2*sin(th2);-100*r2*cos(th2)]
附录2：LLWin3.0慧鱼机器人模型控制程序设计
主程序设计
第1步：通过双击桌面上的LLWIN3.0程序图标，启动Llwin3.0慧鱼机器人模型控制程序设计系统，如图1所示，Llwin3.0可以进行模块化程序设计。即可进行子程序设计，在LLWin 中可以将不同的功能块组装成一个以流程图形式显示的控制程序。假如PC，接口和模型连接对的，流程图中的数字输入（比如按钮）可以被查询且数字输出（比如马达）也可以被连接 。每个模型都规定有一个独立的控制程序。单为一个相同的模型也可以创建不同的控制程序。一个文献，其中包含了一个由功能块所组成的控制程序，在LLWin中被称作project
（项目）。用LLWin你既可以测试提供的项目也可以键入和测试你自己的项目。

图1 LLWin3.0慧鱼机器人程序设计系统
第2步：打开项目菜单，如图2所示，根据被控对象（机器人模型），在选项模板中选取项目，并输入项目名称和要存取的目录途径，单击拟定，如图3所示组态界面。
可以在菜单栏里找到所有用来操作这个软件的命令。除了这些管理，创建和测试项目的命令，你也可以发现程序选项和在线帮助的不同的命令。最重要的菜单命令也可以通过工具栏的图标调用。
那么如何控制创建的程序? 功能块 可从 工具箱 中获得而后被插入到工作表中去。连接这些块就可构成流程图 。每个