机器人技术3运动学-2013

1、第三章第三章 机器人运动学机器人运动学 3.1 .1 齐次坐标及对象物的描述齐次坐标及对象物的描述 3.2 3.2 齐次变换及相关运算齐次变换及相关运算 3.3 连杆参数和连杆坐标系连杆参数和连杆坐标系 3.4 3.4 连杆变换和运动学方程连杆变换和运动学方程 3.5 3.5 机器人运动学方程建立及反解求法机器人运动学方程建立及反解求法 3.6 3.6 关节空间和操作空间关节空间和操作空间刚体位姿描述和齐次变换方法在空间刚体位姿描述和齐次变换方法在空间机构动力学、机器人控制算法、计算机构动力学、机器人控制算法、计算机图形学和视觉计算等方面得到广泛机图形学和视觉计算等方面得到广泛的应用。的应用。

2、刚体在运动中，其上任意两点之间的刚体在运动中，其上任意两点之间的距离保持不变，任意两矢量之间的夹距离保持不变，任意两矢量之间的夹角保持不变，而且矢量的叉积亦保持角保持不变，而且矢量的叉积亦保持不变。这种固有性质简称不变。这种固有性质简称“形体不变形体不变”性，作为刚性物体的数学抽象。性，作为刚性物体的数学抽象。 齐次坐标和齐次变换齐次坐标和齐次变换No.33.1 3.1 齐次坐标及对象物的描述齐次坐标及对象物的描述一、位置的描述（位置矢量）一、位置的描述（位置矢量）No.4二、方位的描述（旋转矩阵）二、方位的描述（旋转矩阵）3.1 3.1 齐次坐标及对象物的描述齐次坐标及对象物的描述No.5正

3、交条件01BABABABABABABABABABABABAxzzyyxzzyyxx1;1RRRABTABABBA3.1 3.1 齐次坐标及对象物的描述齐次坐标及对象物的描述No.6三、位姿的描述（固接坐标系） 坐标系B相对坐标系A的描述：表示位置时：旋转矩阵表示方位时：位置矢量IRAB00BApA0BApBpB3.1 3.1 齐次坐标及对象物的描述齐次坐标及对象物的描述No.7四、齐次坐标四、齐次坐标复合变换公式：复合变换公式：齐次变换公式：齐次变换公式：写成矩阵形式：写成矩阵形式：0BABABAppRp1100010ppRpBBAABApTpBABAA0BAppABpB3.1 3.1 齐次坐

4、标及对象物的描述齐次坐标及对象物的描述No.8 1、 点的齐次坐标点的齐次坐标齐次坐标与直角坐标关系为：齐次坐标与直角坐标关系为：xxx14yxx24zxx34x x x x1234, , ,3.1 3.1 齐次坐标及对象物的描述齐次坐标及对象物的描述No.9 1、 点的齐次坐标点的齐次坐标2、 矢量的齐次坐标矢量的齐次坐标kcjbiarTxxxxr4321axx14bxx24cxx343.1 3.1 齐次坐标及对象物的描述齐次坐标及对象物的描述No.103、关于齐次坐标的几点说明、关于齐次坐标的几点说明 空间某点p的直角坐标和齐次坐标分别为： 坐标原点的矢量矢量为： x,y,z三坐标轴的方向

5、矢量方向矢量分别为：是非零常数）（其中，和zyxzyxpzyxp1T1 , 0 , 0 , 0 TTT0 , 1 , 0 , 00 , 0 , 1 , 00 , 0 , 0 , 1，3.1 3.1 齐次坐标及对象物的描述齐次坐标及对象物的描述No.114、 矢量的齐次坐标运算公式矢量的齐次坐标运算公式43,2,1144iibbaaciiiTcccc4321cba3.1 3.1 齐次坐标及对象物的描述齐次坐标及对象物的描述No.12zyxzyxxyyxzxxzyzzyzzyyxxbbbaaakjibakbabajbabaibabababababababa示或者用下列行列式来表的平面垂直的矢量与此

6、两相乘矢量所决定两矢量的交积为另一个为一标量。的点积和规定两矢量a b aba ba ba b1 12 23 34 4a b abababababab abT2 33 23 11 31 22 14 44、 矢量的齐次坐标运算公式矢量的齐次坐标运算公式3.1 3.1 齐次坐标及对象物的描述齐次坐标及对象物的描述No.134、 矢量的齐次坐标运算公式矢量的齐次坐标运算公式3.1 3.1 齐次坐标及对象物的描述齐次坐标及对象物的描述No.14 1.表示坐标系表示坐标系 经过齐次变换经过齐次变换T而转换为而转换为坐标系坐标系 2.表示坐标系表示坐标系 相对于坐标系相对于坐标系 的位姿的位姿 3.表明在

7、三维空间中点齐次坐标由坐标系表明在三维空间中点齐次坐标由坐标系 向坐标系向坐标系 的映射的映射 4.运动算子运动算子 用位置矢量描述点平移用位置矢量描述点平移 用旋转矩用旋转矩 阵描述点转动方位关系阵描述点转动方位关系 5、齐次变换的物理意义、齐次变换的物理意义 B A A B A BTAB3.1 3.1 齐次坐标及对象物的描述齐次坐标及对象物的描述No.15 6、齐次变化矩阵的一般概念齐次变化矩阵的一般概念 描述刚体的空间运动 建立 基础坐标系OXYZ 动坐标系OXYZ与刚体固联 随刚体运动 两坐标系原点O ,O 两个坐标的关系写成矩阵形式 :x xyzPxcos( )cos( )cos(

8、)iiijiky xyzPycos( )cos( )cos( )jijjjkzxyzPzcos( )cos( )cos()kikjkkxyzPPPxyzxyz100011cos( )cos( )cos( )cos( )cos( )cos( )cos( ) cos( ) cos()iiijikjijjjkkikjkk3.1 3.1 齐次坐标及对象物的描述齐次坐标及对象物的描述No.16cos()cos()cos()cos()cos()cos()cos()cos()cos()iiijikjijjjkkikjkkPPPxyz0001PPPxyzT1每一列阵为三维矢量齐次坐标每一列阵为三维矢量齐次坐标

9、 前前3列列- 新坐标三坐标轴相对参考坐标系新坐标三坐标轴相对参考坐标系 方向方向余弦余弦两坐标系原点间向量两坐标系原点间向量- 齐次坐标齐次坐标3.1 3.1 齐次坐标及对象物的描述齐次坐标及对象物的描述No.17OOROOPOOTOOTcos( )cos( )cos()cos( )cos( )cos()cos()cos()cos()iiijikjijjjkkikjkkPPPxyz0001OOOORP01=3x3子阵子阵 二个坐标系之间的方向余弦方阵二个坐标系之间的方向余弦方阵 姿态矩阵或旋转矩阵姿态矩阵或旋转矩阵3 1列列 齐次变换中的平移量齐次变换中的平移量 坐标原点的相对位置坐标原点的

10、相对位置 即位置矢量即位置矢量坐标系的旋转和平移齐次变换坐标系的旋转和平移齐次变换3.1 3.1 齐次坐标及对象物的描述齐次坐标及对象物的描述No.18五、手爪坐标系 Z轴：接近矢量aY轴：方位矢量o X轴：法向矢量n原点：位置矢量p旋转矩阵旋转矩阵手爪位姿手爪位姿3.1 3.1 齐次坐标及对象物的描述齐次坐标及对象物的描述No.19一、平移的齐次变换一、平移的齐次变换 坐标系B与坐标系A：方位相同，原点不同点P在坐标系A和B中的描述有以下变换关系：0BABAppp3.2 齐次变换及相关运算齐次变换及相关运算No.203.2 齐次变换及相关运算齐次变换及相关运算 I - 为33单位阵。 AAB

11、B100010,001010001xyxyzzPPIPTrans P P PPzyxPPP,为对应于为对应于X,Y,Z的平移量。的平移量。No.213.2、 齐次变换的类型及相关运算齐次变换的类型及相关运算 绕三坐标轴的旋转矩阵Rot( , )cos( ) cos( )cos( )cos( ) cos( ) cos( )cos( ) cos( ) cos()xiiijikjijjjkkikjkk0000001二、旋转的齐次变换二、旋转的齐次变换No.223.2、 齐次变换的类型及相关运算齐次变换的类型及相关运算 绕三坐标轴的旋转矩阵Rot( , )cos( ) cos( )cos( )cos(

12、 ) cos( ) cos( )cos( ) cos( ) cos()xiiijikjijjjkkikjkk0000001Rot( , )cossinsincosx100000000001Rot( , )cossinsincosy 000100000001Rot( , )cossinsincosz000000100001二、旋转的齐次变换二、旋转的齐次变换No.23二、旋转的齐次变换二、旋转的齐次变换pRpBABA3.2 齐次变换的类型及相关运算齐次变换的类型及相关运算 坐标系B与坐标系A：原点相同，方位不同点P在坐标系A和B中的描述有以下变换关系：No.243.2、 齐次变换的类型及相关运算

13、齐次变换的类型及相关运算 绕任意轴旋转变换绕任意轴旋转变换 zyxzyxzyxzzzyyyxxxTBBAABBAABBABAAABzyxABkkkooonnnkonkonkonkRRzRRRzRRRRRkRRBAkzBAkkjkikkABkRR1000cossin0sincos,1固接，则和同向且分别与轴与旋转前重合，和是过原点的单位矢量。其中的方位，相对参考系表示坐标系令旋转变换通式旋转变换通式 A B A BK,RZ,RRAARBBNo.253.2、 齐次变换的类型及相关运算齐次变换的类型及相关运算 绕任意轴旋转变换绕任意轴旋转变换 旋转变换通式旋转变换通式特殊形式。绕坐标轴旋转的式，它概

14、括了前述三种此公式成为旋转变换通式中质，化简整理有：根据旋转矩阵的正交性cos1,VerscVerskkskVerskkskVerskkskVerskkcVerskkskVerskkskVerskkskVerskkcVerskkkRzzxzyyzxxyzyyzyxyxzzxyxxNo.263.2、 齐次变换的类型及相关运算齐次变换的类型及相关运算 绕任意轴旋转变换绕任意轴旋转变换 。，求旋转矩阵转动的轴线绕过原点重合，将坐标系原来与坐标系120,120313131kRkjikoBABAA例题：No.27解：。，求旋转矩阵转动将其代入旋转通式，得120,120313131010001100120

15、,23120cos112023120sin,21120cos31kRkjikkRVerskkkAAAzyx3.2、 齐次变换的类型及相关运算齐次变换的类型及相关运算 No.28等效转轴和等效转角等效转轴和等效转角3.2、 齐次变换的类型及相关运算齐次变换的类型及相关运算 旋转变换通式旋转变换通式特殊形式。绕坐标轴旋转的式，它概括了前述三种此公式成为旋转变换通式中质，化简整理有：根据旋转矩阵的正交性cos1,VerscVerskkskVerskkskVerskkskVerskkcVerskkskVerskkskVerskkskVerskkcVerskkkRzzxzyyzxxyzyyzyxyxzz

16、xyxxNo.29等效转轴和等效转角等效转轴和等效转角3.2、 齐次变换的类型及相关运算齐次变换的类型及相关运算 若有一变换Rot=noanoanoaxxxyyyzzz0000001 求此转动变换是绕着那一个轴线转了多少角度才得到的。 No.30等效转轴和等效转角等效转轴和等效转角3.2、 齐次变换的类型及相关运算齐次变换的类型及相关运算 sin2sin2sin21tan21sin222222xyzzxyyzxzyxxyzxyzxyzxyzzzzyyyxxxonknakaokaononnaaoonnaaoaonaonaonRk；，则设值和效转轴和等效转角由旋转变换通式求其等No.31(五五)、

17、等效转轴和等效转角、等效转轴和等效转角3.2、 齐次变换的类型及相关运算齐次变换的类型及相关运算 两点注意：两点注意：定。角很小时，转轴难以确病态情况：当的值不是唯一的。和多值性：. 2. 1kNo.32例题 。和转角的等效转轴求复合旋转矩阵kzRyRRAB90,90,3.2、 齐次变换的类型及相关运算齐次变换的类型及相关运算 No.33解： 90,90,120,3131311202123sincostan010001100zRyRkRkjikkjkikkRzyxAB3.2、 齐次变换的类型及相关运算齐次变换的类型及相关运算 如果连续变换是相对于当前系进行的，则依如果连续变换是相对于当前系进行

18、的，则依次右乘变换矩阵，如果连续变换是相对于基础次右乘变换矩阵，如果连续变换是相对于基础系进行的，则依次左乘变换矩阵。系进行的，则依次左乘变换矩阵。 对于一个连续的变换对于一个连续的变换 ，根据中所，根据中所得结论，对于变换可以有两种解释。第一种是得结论，对于变换可以有两种解释。第一种是相对于当前系，第一次变换为相对于当前系，第一次变换为 ，第二次变换，第二次变换为为 ，第三次变换为，第三次变换为 ；第二种解释是相对；第二种解释是相对于基础坐标系，第一次变换为于基础坐标系，第一次变换为 ，第二次变换，第二次变换为为 ，第三次变换为，第三次变换为 。CBATTTT ATBTCTATBTCTOOT

19、OOTOOT OOT1 0 00 1 00 0 10 0 01111PPPxyz1 0 00 1 00 0 10 0 01222PPPxyz1 0 00 1 00 0 10 0 01121212PPPPPPxxyyzz 连续变换等于各变换矩阵相乘；连续变换等于各变换矩阵相乘； 变换矩阵相乘无交换律，但在连变换矩阵相乘无交换律，但在连续平移时，各矩阵相乘与次序无关。续平移时，各矩阵相乘与次序无关。OOTOOTOOTTOO TOO ooooRotRot( ,)( ,)zxcossinsincos0 00 0001 0000 1100000000001cossinsincoscossincossin

20、sinsincoscoscossinsincos00000001 连续转动变换时，变换矩阵相乘次序不能更换连续转动变换时，变换矩阵相乘次序不能更换cossincossinsinsincoscoscossinsincos00000001No.393.2、 齐次变换的类型及相关运算齐次变换的类型及相关运算 No.41变换矩阵相乘变换矩阵相乘 相对运动坐标系变换顺序“从左向右”相对固定坐标系变换顺序“从右向左”从而定义复合变换则的描述为相对的描述为相对，已知和，对于给定的坐标系TTTpTTpTppTpTBCTABCBABCABACCBCABBABACBCBBCAB;.,3.2、 齐次变换的类型及相关

21、运算齐次变换的类型及相关运算 No.42例题 。中的描述坐标系求它在的描述为在坐标系。假设点和旋转矩阵单位。求位置矢量轴移动的并沿单位，轴移动的，再沿轴转的坐标系相对于重合，首先的初始位姿与已知坐标系pApBpRPyAxAzABABATBABBAAAA,0 , 7 , 36123003.2、 齐次变换的类型及相关运算齐次变换的类型及相关运算 No.43解：0562.13098.1106120562. 7902. 00612;1000866. 05 . 005 . 0866. 0100030cos30sin030sin30cos30,000BABABABAABBAABPpRpPzRRPR则得，分

22、别为和3.2、 齐次变换的类型及相关运算齐次变换的类型及相关运算 No.44例题试用齐次变换的方法求解上例中的试用齐次变换的方法求解上例中的 。 。中的描述坐标系求它在的描述为在坐标系。假设点和旋转矩阵单位。求位置矢量轴移动的并沿单位，轴移动的，再沿轴转的坐标系相对于重合，首先的初始位姿与已知坐标系pApBpRPyAxAzABABATBABBAAAA,0 , 7 , 3612300pA3.2、 齐次变换的类型及相关运算齐次变换的类型及相关运算 No.45解：10562.13098.1110731000010060866. 05 . 01205 . 0866. 0,1000010060866.

23、05 . 01205 . 0866. 01000100ppTppRTPRABABABAABABBAAB则得代入齐次变换公式，阵，可以得到齐次变换矩和位置矢量求得的旋转矩阵由例3.2、 齐次变换的类型及相关运算齐次变换的类型及相关运算 No.46 齐次变换矩阵也代表坐标平移和坐齐次变换矩阵也代表坐标平移和坐标旋转的复合，可以将其分解成两个标旋转的复合，可以将其分解成两个矩阵相乘的形式。矩阵相乘的形式。1000,1000,00033okRkRotpIpTranskRotpTransTABBABABAAB3.2、 齐次变换的类型及相关运算齐次变换的类型及相关运算 No.47例题 。位置单位。求运动后

24、的轴平移单位，最后沿平移轴，再沿轴轴转绕其运动轨迹如下：首先的原始位置是中，点在坐标系2161230,0 , 7 , 3pYXZppAATA3.2、 齐次变换的类型及相关运算齐次变换的类型及相关运算 No.48解：10562.13098.1110731000010060866. 05 . 01205 . 0866. 01073.1000010060866. 05 . 01205 . 0866. 01211由此得到运动后的位置；分别为和原始位置由题意可知，运动算子pTppTPTAAAA3.2、 齐次变换的类型及相关运算齐次变换的类型及相关运算 No.49四、欧拉角与四、欧拉角与RPY角角3.2、

25、 齐次变换的类型及相关运算齐次变换的类型及相关运算 1.1.绕固定轴绕固定轴x-y-zx-y-z旋转（旋转（RPYRPY角）角）YawxxPitchyyRollzzRPY角称为偏转轴旋转右手法则；绕轴角称为俯仰轴旋转指向船右舷；绕轴角称为回转轴旋转船的行驶方向；绕轴行时姿态的一种方法。角是描述船舶在海中航No.50四、欧拉角与四、欧拉角与RPY角角3.2、 齐次变换的类型及相关运算齐次变换的类型及相关运算 1.1.绕固定轴绕固定轴x-y-zx-y-z旋转（旋转（RPYRPY角）角） ccscssccssccssscssscsccsssccccssccssccsscxRyRzRRzyxABAAA

26、xyzABAAA000010010010000,则有角转绕角转绕角转绕重合与初始No.51四、欧拉角与四、欧拉角与RPY角角3.2、 齐次变换的类型及相关运算齐次变换的类型及相关运算 2. z-y-x2. z-y-x欧拉角欧拉角 ccscssccssccssscssscsccsssccccssccssccsscxRyRzRRxyzABBBBzyxABBBB000010010010000,则有角转绕角转绕角转绕重合与初始No.52四、欧拉角与四、欧拉角与RPY角角3.2、 齐次变换的类型及相关运算齐次变换的类型及相关运算 3 3、z-y-zz-y-z欧拉角欧拉角 csscsssccscssccc

27、sscsccssccccssccssccssczRyRzRRzyzABBBBzyxABBBB100000010010000,则有角转绕角转绕角转绕重合与初始No.53五、变换矩阵求逆五、变换矩阵求逆3.2、 齐次变换的类型及相关运算齐次变换的类型及相关运算 方法一：利用齐次变换矩阵的特点求逆方法一：利用齐次变换矩阵的特点求逆 1000,00000000001BATABTABBABATABBABAABBABABBABABABTABABBAABBABAABBAABpRRTpRpRpBBpppRpRRRpRpRTT由上面公式，可得。从而得到，的描述，故为的原点相对表示因为利用复合变换公式有有根据旋转

28、矩阵的正交性。和计算和等价于给定求对于给定的No.54五、变换矩阵求逆五、变换矩阵求逆3.2、 齐次变换的类型及相关运算齐次变换的类型及相关运算 方法二：直接对方法二：直接对4 44 4的齐次变换矩阵求逆的齐次变换矩阵求逆10001000,apaaaopooonpnnnTpaonpaonpaonTTTzyxzyxzyxBAzzzzyyyyxxxxABBAAB，则设。求给定的的描述被变换了的坐标系也就是参考坐标系对于的一种变换坐标系变回为原坐标系逆变换是由被变换了的实际上No.55多刚体系列：多刚体系列：A A AAn123,每一刚体上的坐标原点：每一刚体上的坐标原点：O O OOn123,坐标

29、系：坐标系： OOOOn123,各坐标系间变换矩阵：各坐标系间变换矩阵：TT.T,T,1-nn231201得变换方程式得变换方程式 : 00121 123T T T -T n-nnT 3.2、 齐次变换的类型及相关运算齐次变换的类型及相关运算 六、变换方程式六、变换方程式No.56用不同坐标系来描述机器人与环境的相对位姿关系用不同坐标系来描述机器人与环境的相对位姿关系 目标系工作站系工具系基座坐标系（基座框）GSTB3.2、 齐次变换的类型及相关运算齐次变换的类型及相关运算 六、变换方程式六、变换方程式 TTTTTTTTBTTBTSGBSGTGTSGBSBTGT11,如变换矩阵来表示可用其余的

30、上式中任一变换矩阵都的描述：相对于基坐标系工具系图）如图表示空间尺寸链（有向变换控制和规划的目标No.57例题3.2、 齐次变换的类型及相关运算齐次变换的类型及相关运算 No.58解：3.2、 齐次变换的类型及相关运算齐次变换的类型及相关运算 TTTTTTUBUPUBPUPBHB11000201010010100100000012100101011TUP100030102001910010009100501010011000201010010100THB机器人操作臂运动学机器人操作臂运动学No.60 引引 言言 *表示机械手杆件相对于基础坐标系的位姿方表示机械手杆件相对于基础坐标系的位姿方程程

31、-运动学方程。运动学方程。 *描述机械手杆件的位姿描述机械手杆件的位姿-建立直角坐标系建立直角坐标系 绝对坐标系绝对坐标系建立在地面的坐标系建立在地面的坐标系 机座坐标系机座坐标系建立在机器人机座上坐标系建立在机器人机座上坐标系(固定坐标系固定坐标系) 杆件坐标系杆件坐标系建立在机器人杆件上的坐标系活动坐标系建立在机器人杆件上的坐标系活动坐标系 末端坐标系末端坐标系建立在末端执行器上的坐标系建立在末端执行器上的坐标系 No.61X2Y2X3Y3123123Y1X1X0Y0Z4Y4No.62 3.3 3.3 连杆参数和连杆坐标系连杆参数和连杆坐标系 No.63 3.3 3.3 连杆参数和连杆坐标

32、系连杆参数和连杆坐标系 No.64 3.4 3.4 连杆变换和运动学方程连杆变换和运动学方程111111111100001iiiiiiiiiiiiiiiiiiicsacsccsd sTsssccd cNo.65 3.5 3.5 机器人运动学方程建立及反解求法机器人运动学方程建立及反解求法XHK5140换刀机械手换刀机械手No.66233100001000010001Td111101000000100001csscT343401000110000001adT3422343430100011000001adTT Td00123412340001xxxxyyyyzzzznoapnoapTT T T

33、Tnoap2212212220112033322103330001csascTsc2232421342222311234234334222202112333333221223333330001csa cd saadscscdTT T Taddscsc 3.5 3.5 机器人运动学方程建立及反解求法机器人运动学方程建立及反解求法No.6704040001RRRRTxTxTxTxRRRRRRTyTyTyTyTTRRRRTzTzTzTznoapnoapTT T Tnoap*43300110020100001ldlda 3.5 3.5 机器人运动学方程建立及反解求法机器人运动学方程建立及反解求法No.

34、68 3.5 3.5 机器人运动学方程建立及反解求法机器人运动学方程建立及反解求法No.69 3.5 3.5 机器人运动学方程建立及反解求法机器人运动学方程建立及反解求法No.70 3.5 3.5 机器人运动学方程建立及反解求法机器人运动学方程建立及反解求法No.71 3.5 3.5 机器人运动学方程建立及反解求法机器人运动学方程建立及反解求法No.72100001000000111101csscT100000100002222212csdscT100001000003323323csascT100000010000555545csscT656666000010000001T cssc1000

35、00100044434434csdascT 3.5 3.5 机器人运动学方程建立及反解求法机器人运动学方程建立及反解求法No.73100056453423120106zzzzyyyyxxxxpaonpaonpaonTTTTTTTnsc c cs sc s cz 234564623 56()()(654641652364654231scsccsssscssccccox)()(654641652364654231scscccssscsscccsoync cc c cs ss s cs s c cc sx1234564623 56145646()()ns cc c cs ss s cc s c cc

36、 sy1234564623 56145646()()osc c cs sc s sz234564623 5 6() 3.5 3.5 机器人运动学方程建立及反解求法机器人运动学方程建立及反解求法No.74100056453423120106zzzzyyyyxxxxpaonpaonpaonTTTTTTT12234233221)(sdsdcacacpx12234233221)(cdsdcacaspypa sa sd cz 3 2322423ac c c ss cs s sx 123452351 45()as c c ss cc s sy 123452351 45()as c sc cz2345235

37、 3.5 3.5 机器人运动学方程建立及反解求法机器人运动学方程建立及反解求法No.75 3.5 3.5 机器人运动学方程建立及反解求法机器人运动学方程建立及反解求法No.76正解和逆解 *正解-已知各杆结构参数和关节变量-求末端执行器的空间位姿 *逆解-已知执行器空间位姿 和各杆的结构参数-求关节变量-控制末端执行器达到指定位置和姿态nT0nT0 3.5 3.5 机器人运动学方程建立及反解求法机器人运动学方程建立及反解求法60101212323434545656TTTTTTTqqqqqq600001Tnoapnoapnoapxxxxyyyyzzzz 1011 60212323 434545 656TTTTTTTqqqqqqqqq236,)q()q()q()q()q()q(6560611012112312341345145TTTTTTT求q5和q6No.781000010000001111061101cssc)(TTnoapnoapnoapxxxxyyyyzzzz000110001111111111111111zzzzyxyxyxyxyxyxyxyxpaonpcpsacasocosncnspspcasacosocnsncPUMA560PUM