3． 求解关节变量?3
由①式得：S3?(qxC2?qyS2?a2)/d4
由②式得：C3?(qxS2?qyC2)/d4
所以：
?3?atan2(qxC2?qyS2?a2,qxS2?qyC2)
?2有两组解，将其值代入上式，即可得到对应的两组?3值
这样就得到了两组?2?3的解，分别对应着大臂小臂的两个位形：
图4 手臂的两种位形
本课题中，根据手术时机器人大小臂的位置，选取第二组解位形。 第二组解位形所对应的?2是两组解中较小的一个，因此有：
当atan2????22qx?qy?k2,k???0时，
?22?2?atan2(qy,qx)?atan2???qx?qy?k2,k??
??当atan2????22qx?qy?k2,k???0时，
?22?2?atan2(qy,qx)?atan2??qx?qy?k2,k??
??4． 求解关节变量?5
式3左右两边同时左乘2T32?1??3?得：
T3??3?1T2??2?0T1?1?1?1?d1?0T5?3T4??4?4T5??5? ?d1?0T5?3T5
0?0??d4??1?2T3??3?1T2??2?0T1?1?1?1??S3?0??C3??0C30S3001000??nxC2?nyS2oxC2?oyS2axC2?ayS2qxC2?qyS2?a2??C4C5?S4?C4S5??nS?nC?oS?oC?aS?aC?qS?qC??SCC?SS0?x2y2x2y2x2y2??45445???x2y2?0??nzozazqz?d1??S50C5????1??000100??0令两边矩阵的（2，1）与（2，3）元素相等，得：
S4C5?nz
?S4S5?az
当S4?0时有：
S5??az/S4 C5?nz/S4
从而有：
?5?atan2(?az,nz)
5． 求解关节变量?4
已知：S4C5?nz， 当C5?0时
S4?nz/C5
由式两边的（2，2）元素相等得：C4?oz 即可求得：?4?atan2(nz/C5,oz) 结论：
通过对此机器人的正逆运动学分析，使我更好的理解了坐标变换、机器人路径规划基础的相关知识，为今后的学习和科研工作打下坚实的基础。