毕业设计（论文）报告纸

2.3.1运动学分析

Ⅰ运动学方程的正解

1.建立直角坐标系，如（图2.3）

图2.3直角坐标系

2.用D-H参数法建立坐标系转换矩阵，列出表2.1

表2.1三臂机械手结构参数表

杆件号 1 2 3

关节变量 θ

1

αi?1 0 0 0

ai?1 0 L1 L2

di 0 0 0

cosαi?1 1 1 0

sinαi?1 0 0 1

θ2 θ3

表中 杆件长度ai，杆件扭角αi，关节变量θi、di。 3.写出机械手臂变换矩阵和运动学方程 因为 i?1iT=rot（xi?1,?i?1）trans（xi?1,ai?1）trans（zi,di）rot（zi,?i） （2-1） - 11 - 毕业设计（论文）报告纸

所以

?cos?1?sin?1?sin?cos?110?=T?A11?00?0?0?cos?2?sin?21A?2T=2=?0??0?sin?2cos?200001000100?0?? （2-2） 0??1?l1?0?? （2-3） 0??1??cos?3?sin?32 3T=A3=??0??0?sin?3cos?3000l2?00?? （2-4） 10??01??c123?s0201?1233T=1T?2T?3T=a1?a2?a3=

?0??0式中：

c123=cos（θ1+θs123=sin（θ1+θc12=cos（θ1+θs12=sin（θ1+θc1=cos（θ1）； s1=sin（θ1）。

2?s123c123000l1c1?l2c12?0l1s1?l2s12?? （2-5）

?10?01?+θ

3）；

2+θ）；

3）；

22）；

Ⅱ运动学方程的逆解

已知此机械手臂位姿变换矩阵为

?nx?n0?y3T=?nz??0 oxoyoz0ayayaz0py?py?? （2-6） pz??1? - 12 - 毕业设计（论文）报告纸

1. 先求机械臂的θ1

根据公式

011T?1（?1）03T=3T （2-7）

得

?cos?1?sin?10?1?（?1）03T=1T?0??0?sin?1cos?10000100?0??0??1??nx?n?y?nz??0oxoyoz0ayayaz0py?py??= pz??1??c1nx?s1ny??sn?cn ?1x1y?nz?0?同时又因为

c1ox?s1oy?s1ox?c1oyoz0c1ax?s1ax?s1ax?c1axaz0c1px?s1px??s1px?c1py?? （2-8）

?pz?1??nx'ox'ay'?n'o'a'yy121?y3T=2T?3T=?nz'oz'az'?00?0py'?py'?? （2-9） pz'??1?式中

nx'=c2?c3-s2?s3 ny'=s2?c3+c2?s3 nz'=0

ox'=-c2?c3-s2?c3 oy'=-s2?s3+c2?c3 oz'=0 ax'=0 ay'=0 az'=1

- 13 - 毕业设计（论文）报告纸

px'=L2?c2+L1 py’=L2?s2 pz’=0 因为

1?1 0（?1）03T=3T （2-10） 1T

令该式中等号两端矩阵第一行第四列、第二行第四列对应元素相等，得

c1?px+s1?py=L2?c2+L1 （2-11） -s1?px+c1?py=L2?s2 （2-12）

作三角代换，令

px=ρsinφ py=ρcosφ

则

ρ= 代入（2-11）（2-12），有

px?py φ=arcTan22pypx ?2?l12?l22 c1sinφ+s1cosφ==X 2l1?由此得

?2?l12?l22sin（θ1+φ）= （2-13） 2l1?cos（θ1+φ）=1?1?X2 （2-14） 由（2-13）（2-14）得?

θ1+φ=arcTan所以

θ1=arcTanX1?X2X1?X2 -φ （2-15） - 14 - 毕业设计（论文）报告纸

2. 然后求θ

2、θ

3，其原理和上面计算θ1 的原理基本上是一样，都是通过矩阵中的相等关

系，通过一系列的数学及矩阵运算，求出相应的一个或者是几个解。

2.3.2动力学分析

首先计算任意连杆上的速度，再计算它的动能，然后推导势能，得到拉格朗日算子，进而对其微分，最后得到动力学方程。 1. 先求机器人臂上一点的速度

假定机器人的连杆Li上有一点ri，它再基座坐标中的位置为

r =Ti?ri （2-16）

其中

ri—此点在i坐标系中的位置矢量

Ti—i坐标系下相对于Oxyz的齐次变换矩阵 r —此点在基坐标系中的位置矢量 那么该点的速度为

i?Ti?drqj）ri （2-17） vi==（?dtj?1?qj其速度的平方为

T????ii???Tdr2????T?TiTirrririqiqk? （2-18） （）=r?r=Tr（）=Tr????qkdt???j?1k?1?qj?式中

Tr——方矩阵的迹的运算符号。 2. 然后求其动能

再连杆Li上的ri处，质量为dm的质点的动能为

?ii?TiT?Ti???1ririrjrk?= dk=dm Tr????qk2???j?1k?1?qj????1?ii?TiT?Ti?ridmri??qrjrk? （2-19） Tr????q2??k?j?1k?1j? - 15 -