工业机器人作业

工业机器人课后作业

姓名:

班级:

学号:

2014年4月

第三章作业

1.初始时坐标系｛B｝与参考系｛A｝重合，现将｛B｝先绕ZB轴旋转6角，然后

再绕XB轴旋转。角，求转动后的｛B｝对于｛A｝的旋转矩阵

解：R=Rot(z,0)Rot(x,6)

cose—sing0](100

sin。cos00cos°-sin。

01J10。

0sin(pcos(p

cose-sin。cos9sinBsine、

sin。coscos-sin0cos。

0sin*cos夕,

2.下图a给出了摆放在坐标系中的两个相同的楔形物体。要求把它们重新

摆放在图b所示位置。

(1)用数字值给出两个描述重新摆置的变换序列，每个变换表示沿某个轴

平移或绕该轴旋转。在重置过程中，必须避免两楔形物体的碰撞。

解：(1)如图建立两个坐标系｛。内,4｝、MX2y2Z2｝分别固结在

两个楔形物体上，如下图

对楔块1进行的变换矩阵为：

7；=Rof(z,9(T)RR(x,90)；

对楔块2进行的变换矩阵为:

7；=Tra〃s(—3,0,9)Rof(Z,—90)77wis(0,5,0)R"(X,90)Rof(Z,180)

由matlab可以求出

0010、00-12

1000000

01000-109

000170001

⑵作图说明每个从右至左的变换序列。

解：

/

⑶作图说明每个从左至右的变换序列。

解：

3.求出类型2和类型3欧拉角表达的正逆运动学方程的解。

解：类型2的正运动学解

，cos°-sin。OYcos00sin^Ycos^-sin〃0、

R二sin。cos。0010sin-cos〃0

、00J1-sinO0cos®八00"

/cos°cos9cos〃-sin。sin〃-cos。cosOcos〃-sin0cos〃cos。sin6、

=sin^cos8cos,+cos。sini//一sin°cosOsini//+cos^cos“sin^sin0

。〃。

\-sinOcosi//sinsincos7

类?S3的正运动学解

'cos。一sin°OVcos60sin^V100、

R=sin。cos。00100cosy/-sin-

、001J1-sin。0cos。八0sin-cos”)

coscoscos^sinsin-sin^cosi//cos^sin^cos^+sin.sin-、

=sin°cos0sin°sin。sin收+cos^cos材sin(/)sinOcos夕-cos(/)sin〃

一。

\sincosOsin”cos^cosy//

类型2的逆运动学解

'%Ox(cos。sin。0］(cos60sin。］(cost//一sin〃0、

%44=一sin°cos。0010sin〃cos犷0

kooJ

、生q生,［一sin®0cos^)Ioo"

则

'cos。sin。[%q/、'COS。0sin，、“cos"-sin”0、

-sin^cos。a—010sini//cos〃0

o1Jlp.v?>

、°0【〃二qa-、一sin。0cos。.I001J

COS041

‘cos。6+sin/〃vcos敢4+sin00V+sin/a、.、（COS^COSI//-cos。sin少sin。'

一sin。"、+cos网=0一sin"、.+cos4q.-sin04+cos%7V=sin”cos〃0

n.oz71-sin9cos材一sinOsin夕cos。）

由（2,3）元素相等，则

-sin（/）ax+cos°Q）=0

得

「av~|

/.（/）=arctan-=arctan（«y,ax）

。的正负由定义知

由

-sin（/）ox+cos（/>oy=cos〃

-sin（/）nx+cos网=sin”

则

一sin。%+cos°〃,,

〃二arctan=arctan（-sin^nv+cos勿7、,,一sin+cos°q.）

-sin0q.+cos°ov

由

cos。/+sin0%,=sine

az=cos0

则

cos。％.+sin婀,

0=arctan=arctan（cos^v+sin（j）ciy,a_）

类型的逆运动学解

/3、

阳必应，cos°-sin。01（cosO0sin。、’100、

%4ay—sin。cos。00100cos——sin〃

、生q%I001J[一sin80cos。.、0sin〃cos夕,

则

cos。sin。0、‘％q。J'cos00sin。）00

-sin^cos。0ny°y%=0100cos-—sin”

00L"z>'-sin80cos。J1。sin〃cos”

cos勿（+sin（/>nycos+sin00Vcos血+sin°Q、.、cos6sin。sin〃sin&osy/、

一sin。％+cos由1V一sin血+cos。-sin（j）ax4-cos。。、=0cos”-sini//

o.【sin。cosOsin〃cosOcos%

由（2,1）元素相等，则

一sin（jm、4-cos。%=0

得

〃v

。=arctan=arctan（nv,nv）

。的正负由定义知

由cos。6.+sin（/）ny=cos6和%=一sin0

得

-n.

0=arctan=arctan（-〃N，cos°/\+sin°〃y）

cos°凡+sin（j）nx

又有

-sin（/）ox+cos°ov=cosy/

-sin（/）ax+cos（f）a、尸一sin〃

得

sin-cos。/

〃二arctan=arctan（sin^A-cos^av,-sin（j）ox+cos（j）oK）

—sin0q.+cos00V

4.求PUMA560机器人的逆运动学方程的解。

解：建立PUMA560机器人的D-H坐标系如图如下:

求各连杆变换矩阵如下:

c9\-s0}00c6,-帆00

第

sG00E001d

x|T.=2

z-0010L州-烟00

_0001_-0001_

-

c&-s030a2c4-烟0%

S33eg00o014

z24二

0010-烟-烟00

_0001_-0001_

C35-S3500叫-叫oo

00-10r0010

4T5r=

40

我也00-S06-C3600

00010001

求PUMA560机器人的逆运动学方程的解,则末端执行器的位姿已知，即

%%P」

ny0a

0TyPy

出0%P：其中/o,a,P为已知量;

0001

且有

%°x*Px

oT4.o4VPy

16。乐幻,⑸了小可）立（4）Z（幻立（4）

n.4出A

0001

则求关节变量夕7,92,93,04,95,。6的值。具体求解步骤如下：

1.求以

用逆变换°小他）左乘（式D两边：

°，】（硝°4］工©）7e）Z0）

s300n

lx%见px

0000&%%Py

一世cOx10%C%Pz

00010001

令矩阵方程两端的元素（2,4）对应相等，可得：

FPx+qPy=4

利用三角代换：Px="COS。Py=psin。

式中，P=NP；+P；；。=atan2（py,pj。带入可以得出ei的解:

«-（4/p）一

sin（。-4）=4/。；c0s（@_q）=土

i/jYI

=atan21--<

P\\P）

0}=atan2（py,p,）-qtan2@,±Jp；+P；-d；）

式中，正、负号对应于。I的两个可能解。

2.求93

在选定，的一个解之后，再令矩阵方程两端的元素（1,4）和（3,4）分别对

应相等，即得两方程：

C\PxPy=a3c23~d4s23+。2c2

~Px~〃3s23+d4c23+。2s2

结合式2与上式，消去％，可以求解得93为:

22

03-atan2（%,《）一〃tan2（女,±^tz3+-k）

式中正、负号对应4的两种可能解。

k=p：+p；+p；-s-d：

其中,

2%

3.求82

为求解求92,在矩阵方程（式1）两边左乘逆变换°写1：

。不（ace）ZJ小a）F@）Z0）

展开得

01。23防。23一%—Q2c3n%

x4Px

Q2s3

-。1$23-S]%一。23ny%%Py_3

Fq0%Oz%Pz

00010001

令矩阵方程两边的元素（1,4）和（2,4）分别对应相等可得:

C[C23Px+5]。23夕y一$23Pz—“2。3=”3

—323Px-SlS23Py-CTiPy+=4

联立求解得邑3和C23：

（一/一〃2。3）Pz+（GPx+51〃\，）（。253一。4）

s=----------------------------------------：------------------

<区+（「〃+"）2

_（一4+〃2s3）Pz—（G〃X+M〃y）（一42c3一〃3）

〃；+（。1〃+邑夕了

由上式得

。23=。2+。3=〃tan2[一(％+〃2c3)Pz+(GP\+S]P\)(〃2s3-4)，

(-4+)Pz+(qpx+S]Py)(%。3+〃3)]

根据4和4解的四种可能组合，由上式可以得到相应的四种可能值冬3,于

是可得到%的四种可能解：

式中，％取与,相对应的值。

4.求。4

因为式4-3的左边均为已知，令两边元素(1,3)和(3,3)分别对应相等，

则可得：

[axC\C23ayS\C23-/，23=4s5

1-4必+ayc]=s4s5

只要s5W0,便可求出e：

“二〃tan2(一。必+aycv-axc}c23+a.s23)

5.求05

据求出的4,可进一步解出四，将式1两端左乘逆变换°石,(a，4，4，4)

得：

=幻%0)

逆变换为：

qc23c4+町4S]C23c4一二久F3c4一。2c3c4+-43c4

02c3s4+4c4+43s4

-C]C2354+“FC23s4-C&S23s4

"3_4

一C|43-%%-C23

_0001

根据矩阵两边元素分别对应相等，可得:

&（C]C23c4+S|S4）+%G|C23c4-9、）一见（523c4）=一丛

%（一“23）+%（一网23）+生（-。23）二。5

由此得到05的解：

05=atan2（55,c5）

6.求。6

将式1改写沏岁他©，…，幻Z=Z0)

令上面矩阵方程两边元素分别对应相等可得：

—々•(CCsL-5£)一&(5«2354+qC4)+〃z(%S4)二§6

”」(qc23c4+册4)。5—qs23s5]+"y[($C23c4一。山尼—23s5】—“zl%。4c5+C23s5)=，6

从而可求出96的解：

4=atan2(S6,C6)

以上求解过程即是PUMA560机器人的逆运动学方程求解过程

至此，坏%%名、2、4都以求得。

5.对于下图所示三自由度机械手，其关节1与关节2轴线相交，

关节2与关节3轴线平行，各关节的正向转动角度如图标示，请建立该机械手

的D-H坐标系，并求其变换矩阵°A142,2A3。

坐标系建立如图:

关节转角杆长距离

扭角,

1000

4

2900L2

6

30L30

4

rclrl00、rc2一s200、rc3一s3013、

sicl0000-1LIs3c300

°4=?4=

0010=s2c2000010

J00J001，J00L

第四章作业

1、求图（a）所示的二连杆非平面机械臂的动力学方程。假设每个连杆的质量

为集中质量并处于连杆最外端；每个关节的粘性阻尼系数分别为bl,b20

解：

L=Ek-Ep

1•1•1•

2

=5g(/]GJ+—+/2cos^2)^J4--(Z2劣)?sing

I•i•i•

E.=5仿储4)2+5a[(4+4cos%)即2+]b式k%)2

则对于町来说，

ddLdLdEd

,出d技峋dA,

•••••・••

22

=mJ；4+m24(/1+12cos02)-2m20x(/t+12cos02)/2sin0202+〃+b2(l1+Z2cos02)0x

则对于胆来说

ddLdLdE.

E二---------------------------1------L

2dt地帆d在

•••2•

cosmcos

=叫/；%+?(4+4名)44sin02+i8k2+b2l；02

2、求图（b）所示具有分布质量的2自由度机器人的动力学方

程。

解：

（1）系统的动能:

1•2

Ek\=2/i^1

4=§/宿

x2=4G+0.5/2C12

%=/[S]+0.5/2S|2

%2=—，]$]a—0.5/>书(。|+仇)

y2=/£4-0.5/2C12(^I+^2)

对连杆2,其绕质心转动的动能为：

1]

E|+22

k2=-/2(^^)+]"“

.2.2

v；=X2+y2

2

=(/,+0.25Zf+/1/2C2)6»I+(0.25/；)6>2+(0.5/2+/1Z2C2)(91di

所以，系统的动能和势能为：

Ek=昂+%

1,~1••1

=5,仇+]/式61+仇）2+万网q

(//;+j]]・2]1•2ii1

——mJ；H—mJ112G)6i+—mJ：仇+—H—〃芍，0\Oi

622~~~\66~~)13322

Ep=Ep]+弓2="480-5/囚+m2g(4电+0.54512)

(3)Lagrange函数为:

L=E「Ep

•21

(-+—mJ.：+—mJ：+—ntylj2c2)仇02+1—+—0\02

2

一町gO.5/]S]+初2g(4M+0.5/24y12)

(4)求动力学方程：

q=

(I1->)I]\••••

I-niy+7/22)/；+〃%/：+112c2a+—ml^+—m/)/c仇一(m2〃2c2)仇仇

3222)22

.2

62+1；〃2|+〃?2)g/|C|+；1根2gLq

22

%

,21

2c2夕+万利g/2ci2

第七章作业

1.如图所示某工业机器人的双爪夹持器控制原理图。夹持器由直流电机

驱动，电机输出的旋转运动经齿轮传动带动两个手指。若每个手指的惯

量为J,线性摩擦阻尼系数为B,已知直流电动机的传递函数（输入为电

枢电压V,输出为电动机输出轴转矩Tm）为：

-1

V（s）L〃（s）+凡

其中，La,Ra分别为电枢绕组的电感和电阻。

（1）试证明以下等式并用系统参数表示K1,K2：

（2）利用（D的结果，画出以给定角％--------------6泸--------

图；

（3）如果采用比例控制器=七,）,求出闭环系统的特征方程式。并确定0,

是否存在极限最大值？为什么？

解：（1）证明这里是输入，q和a是输出。

建立手指传动系统的传递函数如下：

Tm=J'0m+B'0m+K0m

2

其中/=1,“+//好+j,B'^Bm+B/?J+B分别表示传动系统对传动轴的总转动

惯量、总粘滞摩擦系数；（和纥表示电动机转子转动惯量及粘滞摩擦系数，并

且由于总刚度较小往往假设K=0；且77=%为减速比。

对上式拉氏变换得

7；($)=(/*+雨屐(s)

则代入J'和夕，有

—（§）=（（，+J/+J）S2+（扁+B//+B）S）q“（5）

若不计电动机转子转动惯量及粘滞摩擦系数J,n和Bm,则可化简上式得

6=（（J/rf+J）s2+（3/T+B）s）»,（5）

即有

2

7

Q,（s）1+772

7；,,（5）-5（75+B）

由系统结构有为=%=但，则/（s）=a（s）=〃a（s）,故可以得出以下表达式

n

—(s)_1+77~即2J

s(Js+B)

咪

。2(5)=]+〃-

即

-s(Js+B)

可和(均是由系统参数〃决定的量。

（2）以给定角/为输入，以夕为输出的系统闭环方框图如下:

这里采用的是PID控制器。

（3）如果采用比例控制器（G=0,）,求出闭环系统的特征方程式为

32

LaJs+（RuJ+LaB）s+RaBs+K,,（&+&）=0

系统能够正常运行的一个条件就是系统是稳定的，则根据劳斯稳定性判据得出

满足系统稳定性的条件。

①劳斯数列：

,3LJR“B

52(&J+L.B)£,(4+()

s0

(RJ+二)

5°£(&