?c?1?s?0?11T=?0??0?c?2?s?1?22T=?0??0?c?3?s?2?33T=?0??0?1?03?eT=?0??0?s?1c?100?s?2c?200?s?3c?30000?00?? 10??01?0l1?00?? 10??01?0l2?00?? 10??01?00l3?100?? （4-1） 010??001?（4）末端执行器的运动方程为
0e231T=01T2T3TeT （4-2）
将式（4-1）代入式（4-2）即可求解运动学正解，给一组关节变量即可得到一个位姿，运动学正解关节变量与位姿变量是一一对应的关系。
?c123?s0?123eT=?0??0?s123c123000c123l1?c12l2?c1l1?0s123l3?s12l2?s1l1?? （4-3）
?10?01?式中c123=cos(?1??2??3)，s123=sin(?1??2??3)，c12=cos(?1??2)，
s12=sin(?1??2)。
6