带拟人臂的球形臂运动学逆解

一、运动学逆解问题

已知机械臂相对基坐标系的变换矩阵，如图1.1所示，求解机械臂各个关节角度。

图1.1 已知条件 带球形腕的拟人臂构型以及相关的DH参数表如图1.2所示：

图1.2 带球形腕的拟人臂构型

二、求解思路

这里参考《机器人学 建模、规划与控制》这本书。
首先，总体的求解流程：

图2.1 带球形腕的拟人臂构型逆解求解总流程

2.1首先计算腕的位置Pw：

2.2求解（q1/q2/q3）

通过以上，得到4组（q1/q2/q3）解；

2.3求解（theta4/theta5/theta6）

通过以上，得到2组（theta4/theta5/theta6）解；

2.4 总结

通过以上的方法，可以得到2*4=8组解，通常通过计算距离当前机械臂关节角度最近的一组解来决定逆解使用哪组解。