求解得 a3c3?d4s3?k
p2x?p2y?p2z?a22?a23?d22?d24式中 k?
2a2以上方程中已经消去?2，因而可由三角代换求解?3
22?d4?k2 ?3=arctan2?a3,d4??arctan2k,?a3??式中正负号对应两种可能解。
（3）求?2
0?1为求解?2在矩阵方程1两边左乘逆变换T3得
0T?13??1,?2,?3?0T6=3T4??4?4T5??5?5T6??6?
s1c23?s1s23c10?s23?c2300?a2c3?a2s3???d2??1??nx?n?y?nz??0oxoyoz0axayaz0px?py??=3 （11） T6pz??1??c1c23??cs?123??s1??0令以上矩阵两边的元素分别对应相等可得 c1c23px?s1c23py?s23pz?a2c3?a3 ?c1s23px?s1s23py?c23px?a2s3?d4 联立求解得
??a3?a2c3?pz??c1px?s1py??a2s3?d4???s23?2pz2??c1px?s1py?? ???d4?a2s3?pz??c1px?s1py???a2c3?a3??c23?22???p?cp?spz1x1y?=arctan2???a3?a2c3?px??c1px?s1py??a2s3?d4?,??d4?a2s3?pz??c1px?s1py??a2c3?a3??于是可得到?2的四种可能解
==?2?23??3-arctan2???a3?a2c3?px??c1px?s1py??a2s3?d4?,??d4?a2s3?pz??c1px?s1py??a2c3?a3??22arctan2?a3,d4??arctan2k,?a3?d4?k2
s23和c23表达式的分母相等且为正，于是 ?23??2??3
??（4）求?4
由于式11的左边为已知，令两元素（1，3）和（3，3）分别对应相等则
axc1c23?ays1c23?azs23??c4s5
?axs1?ayc1?s4s5
只要s5?0即可求出s4来，即
?4?arctan?axs1?ayc1?axc1c23?ays1c23?azs23
但当s5?0时机械手处于奇异形位，在奇异形位时，可任意选取?4的值，在算出相应的?6。 （6） 求?5
0?1根据求出的?4可进一步求出?5，将式111两端左乘逆变换T4??1,?2,?3,?4?，
045?????,?,?,??=T41234T6T55T6??6? （4）
0?1因为上式的?1,?2,?3,?4均已解出，逆变换T4??1,?2,?3,?4?为 0?1?c1c23c4?s1s4??ccs0?11234T4??1,?2,?3,?4?=???c1s23?0?s1c23c4?c1s4?s1c23s4?s1s230?s23c4s23s4?c230?a2c3c4?d2s4?a3c4?a2c3s4?d2c4?a3s4??
?a2s3?d4?1?据矩阵两边元素分别对应相等可得
ax?c1c23c4?s1s4??ays1c23c4?c1s4?azs23c4??s5 ax??c1s23??ay??s1s23??az??c23??c5
??由此可得?5的封闭解
?5=arctan2?s5,c5? （7） 求?6
005???,?,?,?,?=T512345T6T6??6? ?1令矩阵方程两边对应元素分别对应相等
?nx?c1c23s4?s1c4??ny?s1c23s4?c1c4??nzs23s4?s6
nz??c1c23c4?s1s4?c5?c1s23s5??ny??s1c23c4?c1s4?c5?s1s23s5??nz?s23c4c5?c23s5??c6
从而可求出?6的封闭解 ?6?arctan2?s6,c6? 以上即为?的全部解 2.7机械手的雅克比矩阵 连杆 1 2 3 4 5 变量 ?1 ? ?d 0 0 0 a 0 cos? 0 1 0 0 0 sin? -1 0 1 -1 1 ?90? 0 ?2 ?3 ?4 ?5 90? ?90? 90? a2 a3 0 0 d4 0 6 ?6 0 ?0 0 1 0 机械手的操作速度与关节速度的线性变换称为机械手的雅克比矩阵，可以认识它从关节空间向操作空间运动速度的传动比 令机械手的运动方程为：x?x?q?
代表操作空间x与关节空间q之间的位移关系，将上式两边分别对时间求导即得出微分关系：
x?J?q?q
x称为末端操作空间的广义速度，q为关节速度，J?q?是6?n的偏导数矩
????阵，称为机械手的雅克比矩阵。
雅克比矩阵的第六列把关节6的运动变换为T6的坐标，然而关节6又作用于连杆5的坐标，可得微分坐标变换矩阵5T6。可得 Td6=0i?oj?0k T?6=0i?oj?0k
这两个矢量构成了雅克比矩阵的第六列。 对于第五列，其微分变换为4T6 Td5=0i?oj?0k T?5=0i?oj?0k
对于第四列采用矩阵3T6，于是有 Td4=0i?oj?0k T?4=0i?oj?0k
对于第三列其微分坐标变换矩阵为2T6，因为是旋转关节所以各元素 T=d3x6666666?nxpy?nypx???c3?c4c5c6?s4s6??s3s5s6??a3s3?c3d4???s3?c4c5c6?s4s6??c3s5c6??a3c3?s3d4?=
?oxpy?oypx????c3?c4c5c6?s4s6??s3s5s6??a3s3?c3d4????s3?c4c5c6?s4s6??c3s5c6??a3c3?s3d4?Td3z=?axpy?aypx=??c3c4s5?s5c5??a3s3?c3d4???s3c4s5?c3c5??a3c3?s3d4?
用同样的方法可求出本机械手雅克比矩阵的第二列和第一列。化简得 Td3x=d4?c4c5c6?s4s6??a3s5c6 Td3y??d4?c4c5s6?s4s6??a3s5s6 Td3z=d4c5s5?a3c5
用同样的方法可求出本机械手雅克比矩阵的第三列第二列和第一列。 第三列三个元素为 T?3x=s4c5c6?c4s6 T?3x=?s4c5s6?c4c6
T6d3y666666T6?3x=s4s5
第二列元素为
??a2s3?d4??c4c5c6?s4s6???a2c3?a3?s5s6????as?d??ccc?ss???ac?a?ss?2344564623356??T6?c4s5d4?a2c3c5?a2s3c4s5?a3c5? =?s4c5c6?c4s6??2?????s4c5s6?c4c6??s4s5????s4c5c6?c4s6??a2c2?s23d4?????scs?cc??ac?sd??4564622234???T6s4s5?a2c2?s23d4?? =???1??s23?c4c5c6?s4s6??c23s5s6??s23?c4c5s6?s4c6??c23s5c6????s23c4s5?c23c5??
2.8 机械手的动力学分析
机器人的动力学主要研究和分析作用在机械手上的力矩和力，通过建立机械手的动力学方程来确定质量、加速度和力矩、转动惯量等，计算出完成机器人运动时各驱动器所需的驱动力。通过动力学分析，可依据机器人的外部载荷计算出机器人的最大载荷，为机械手选择合适的驱动器。 多自由度、三维质量分布的工业机器人的动力学分析和建模主要采用拉格朗日力学方法来建立动力学方程，但是由于计算困难若不加以简化，很难用于机器人的实际控制 2.81拉格朗日——欧拉法
拉格朗日力学是基于能量项对系统变量及时间微分的方法。其定义 L=K?P
其中L是拉格朗日函数，K是系统动能，P是系统势能。 对于工业机器人，公式中的K为操作臂的总动能，P为操作臂的总势能。所以拉格朗日——欧拉方程
??d??L??L? ?i? ???dt?q?qi?i?i?1,2,???,n.其中L是拉格朗日函数，?i为系统广义力矩，qi为系统变量，
对于工业机器人，?i为在关节i处作用于系统以驱动杆件i的广义力矩，qi为操作臂的广义坐标。机械手各转动关节的转角、移动关节的位移可以通过电位计、编码器传感器测量。 2.82机械手的连杆速度
为了计算系统的动能，必须知道各关节的速度，计算出操作臂上各点的速度。如图所示连杆3上的P点的坐标为 0rp?0T33rp
所以P点的速度为
0
?3?0T3??3d0vp?rp???qj?rp ??dt?j?1?qj?????P点的加速度为
?3?0T3???30? ap?????qqj?rpi?j?1?????33?20T???33????qkql?rp ??k?1j?1?qk?qj?????同理对于任意连杆i上的位置为
0ri?0Tiiri
因此任意连杆上的速度为
o?i?oTI??ivi???qj?ri ?j?1?q?j????4.3机器人的动能
在获得连杆上各点的速度后，就能够计算连杆的动能了，假设ki是杆件i相对于基坐标系的动能，而dki是杆件上微元质量dm的动能，则
?ii???1iiTTdki?Trace???uijridmriUikqjqk? 4--12
?j?1k?1?2??因此，连杆i的动能可以通过对方程4-12的积分得到
?ii???1iiTTki??dki?Trace???uij?ridmridmUikqjqk?4-13
?j?1k?1?2??????