?1?0 t = Trans(4,-3,7)w = ??0??0004??2??6??7??4?10?3???????
017??3??10??????001??1??1?
2. 如图所示为具有三个旋转关节的3R机械手，求末端机械手在基坐标系{x0,y0}下的运动学方程。
θ3L3y0L1θ2L2θ1Ox0
解：建立如图1的参考坐标系，则
?c1?s0T1=?1?0??0?s100??c2?sc100?1?,T2=?2?0010???001??0?s2c2000L1??c3?s00?2?,T=?310?3?0??01??0?s3c3000L2?00?? 10??01??c123?s1230T3=0T11T22T3=??0??0?s123c123000L1c1?L2c12?0L1s1?L2s12??
?10?01?其中c123?cos??1??2??3?,s123?sin??1??2??3?.
3. 如图所示为平面内的两旋转关节机械手，已知机器人末端的坐标值{x,y}，试求其关节旋转变量θ1和θ2.
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??
2LL?12??L2sin?2??y??1?arctan???arctan??
?x??L1?L2cos?2?2???x2?y2??L1??L22? 第二组解：由余弦定理， ??arccos?2LL??12???'????
2?'?1??????arctan?y?2??
?x?
4. 如图所示两自由度机械手在如图位置时（θ1= 0 , θ2=π/2），生成手爪力 FA = [ fx 0 ]T 或FB = [ 0 fy ]T。求对应的驱动力 τA和τB 。
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??L2L1??0??L1fy??B?JFB???f???? ???L20??y??0?T5. 如图所示的两自由度机械手，手部沿固定坐标系在手上X0轴正向以1.0m/s的速度移动，杆长l1=l2=0.5m。设在某时刻θ1=30°，θ2=-60°，求该时刻的关节速度。已知两自由度机械手速度雅可比矩阵为
??ls??lsJ??11212?l1c?1?l2c12y0l1-θ2l2x0?l2s12? l2c12??y3v3x3θ1O
??ls??ls解：因为：J??11212?l1c?1?l2c12?l2s12? ?l2c12?1l1l2s?2l2c12???lc??lc?11212l2s12?
?l1s?1?l2s12??因此，逆雅可比矩阵为：J?1?因为，??J?1v，且v=[1, 0]T，即vX=1m/s，vY=0，因此
??1?l2c12l2s12??1?1??????lc??lc??0??ls??lslls?????1121211212???122?2?c1?1?12?rad/s=-2rad/s
l1s?20.5?2??c?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 L2 3 0 由连杆齐次坐标变换递推公式 杆长ai?1 0 0 0