A写成机器人正运动学方程B将已知的末端位姿矩阵与求得的正运动学方程建立等式关系C根据等式两端相应元素相等建立方程求解相应的关节变量D画出几何图形列出几何关系式