?1?0 t = Trans(4,-3,7)w = ??0??0010000104??2??6??7??4??3??????? 7??3??10??????1??1??1?2. 如图所示为具有三个旋转关节的3R机械手，求末端机械手在基坐标系{x0,y0}下的运动学方程。
θ3L3y0L1θ2L2θ1Ox0解：建立如图1的参考坐标系，则
?c1?s1?sc10T1=?1?00??0000??c2?s00??,1T2=?2?010???01??0?s2c2000L1??c3?s00??,2T3=?3?010???01??0?s30L2?c300?? 010??001??c123?s0102T3=T1T2T3=?123?0??0?s123c123000L1c1?L2c12?0L1s1?L2s12??
?10?01?其中c123?cos??1??2??3?,s123?sin??1??2??3?.
3. 如图所示为平面内的两旋转关节机械手，已知机器人末端的坐标值{x,y}，试求其关节旋转变量θ1和θ2.
6
PyL1θ2L2θ1x解：如图所示，逆运动学有两组可能的
解。
第一组解：由几何关系得
x?L1cos?1?L2cos??1??2? （1） y?L1sin?1?L2sin??1??2? （2） （1）式平方加（2）式平方得
2x2?y2?L1?L22?2L1L2cos?2
2?x2?y2?L1??L22???2?arccos??
2L1L2???L2sin?2??y??1?arctan???arctan??
L?Lcos??x??122?2???x2?y2??L1??L22? 第二组解：由余弦定理， ??arccos?2L1L2?????'????
2??????y????arctan??
'12?x?4. 如图所示两自由度机械手在如图位置时（θ1= 0 , θ2=π/2），生成手爪力 FA = [ fx 0 ]T 或FB = [ 0 fy ]T。求对应的驱动力 τA和τB 。
7
yPτ2τ1?0?FB?f??y?FA?fx??0???L2xL1解：由关节角给出如下姿态：
?L2sin??1??2????L2?L2? ????0?L2cos??1??2????L1??L1sin?1?L2sin??1??2?J????L1cos?1?L2cos??1??2?由静力学公式??JTF
??L2L1??fx???L2fx??A?JFA????????Lf? ?L0?2??0??2x?T ?B?JTFB??21????????L20??fy??0?5. 如图所示的两自由度机械手，手部沿固定坐标系在手上X0轴正向以1.0m/s的速度移动，杆长l1=l2=0.5m。设在某时刻θ1=30°，θ2=-60°，求该时刻的关节速度。已知两自由度机械手速度雅可比矩阵为
??LL??0??L1fy???ls??lsJ??11212?l1c?1?l2c12y0l1-θ2l2x0?l2s12? ?l2c12?y3v3x3θ1O
??ls??ls解：因为：J??11212?l1c?1?l2c12
?l2s12? ?l2c12?8
因此，逆雅可比矩阵为：J?1?1l1l2s?2l2c12???lc??lc?11212l2s12? ??l1s?1?l2s12???J?1v，且v=[1, 0]T，即vX=1m/s，vY=0，因此 因为，?????11??????2??l1l2s?2?l2c12l2s12???1???lc??lc??0??ls??ls?1121211212?????c12?1rad/s=-2rad/s ?1l1s?20.5????2c?1c?12?4rad/sl2s?2l1s?2（算出最后结果2分）
因此，在该瞬时两关节的位置分别为， θ1=30°，θ2=-60°；速度分别为??1=-2rad/s，??2=4 rad/s；手部瞬时速度为1m/s。
sθ1=sinθ1式中：s12=sin(θ1+θ2)
??????6. 如图所示的三自由度机械手（两个旋转关节加一个平移关节，简称RPR机械手），求末端机械手的运动学方程。
L2d2L3θ3L1θ1
解：建立如图的坐标系，则各连杆的DH参数为：
转角?n 偏距dn 扭角?i?1 连杆 ?1 L1 1 0 0 d2 2 90° ?3 3 0 L2 由连杆齐次坐标变换递推公式 杆长ai?1 0 0 0 9
?s?i0ai?1??c?i?s?c??c?c??s??ds?ii?1ii?1i?1ii?1i?1? Ti???s?is?i?1c?is?i?1c?i?1dic?i?1???0001??有
?c?1?s?1?s?c?110?T?1?00?0?0故
00??1?000?1?，2T???01L1???01??00??c?3?s?0?1?d2?2?，T??33?0100???001??000?s?3c?30000?00?? 1L2??01??c?1c?3?s?c?130012?T?TTT?3123?s?3??0结果2分）
s?1?sin?1?c?1s?3?s?1s?3c?30s?1s?1L2?s?1d2??c?1?c?1L2?c?1d2??（写出最后
?0L1?01?式中：c?1?cos?1
??三连杆操作臂的逆运动学方程：
第一组解：由几何关系得
x?L2cos?2?L3cos(?2??3)（1） y?L2sin?2?L3sin(?2??3)（2） 将（1）式平方加（2）式平方得
2 x2?y2?L2?L23?2L2L3cos?3
由此式可推出
2?x2?y2?L2?2?L3?? ?3?arccos?? 2LL23???L3sin?3??y??? ?2?arctan ?arctan?????x??L2?L3cos?3?2第二组解：由余弦定理x2?y2?L22L3?2L2L3cos?，得
222?L2??-??y?''2L3?x?y ??arccos? ???arctan??????? 23?2222L2L3?x?????
10
7. 下面的坐标系矩阵B移动距离d=(5，2，6)T：
?0?1B???0??0
12?004??求该坐标系相对于参考坐标系的新位置。 0?16??001?0
11