在求解六轴机器人逆解问题中，需要已知机器人末端位姿，再进行逆解运算得出关节角。末端姿态如下图所示。机器人后 3 个关节可以看成一个球铰，其球心在腕点 Pw，末杆绕球铰转动即可实现后 3 个关节的运动． 此时引入“工作球”的概念，在 该 机 械 臂 工 作 空 间 内 设 立 直 角 坐 标 系S( Oxyz) ，Pw( x1，y1，z1) 为工作空间中某一工作点，以该工作点作为球心，以末端杆长为半径，所作的球为“工作球”．图中圆球为机器人末端工作球，球体均布点上的坐标系 z 轴与球心与均布点的连线重合，方向为由球心指向均布点，可求得姿态矩阵。
文献中所提到的方法如上，有大神会求吗？
最后还要用matlab实现，我哭了
