【机器人学】2-1.六自由度机器人运动学逆解【附MATLAB机器人逆解代码】

        通过上一篇文章​​​​​​六自由度机器人正解已经获得了机器人各连杆间的转换关系。当知道各个连杆的转角1、2、3、4、5、6时可以求出末端执行器的位姿，这个过程称为正向运动学。这篇文章将讨论逆向运动学，即，给定末端执行器的位姿，求解各连杆的转角。在实际生活中逆向运动学更为实用。

现已知末端姿态矩阵和机器人的运动学模型，求解1、2、3、4、5、6。

已知六轴机器人的D-H参数如下所示：

如果不清楚何为改进的DH参数，可以看我的上一篇博客。【机器人学】1-1.六自由度机器人运动学正解 【附MATLAB代码】

	关节1	关节2	关节3	关节4	关节5	关节6
	0	90	0	0	-90	90
a	0	0	425	393	0	0
d	160.7	0	0	113.3	99	93.6
	0	90	0	-90	0	0
	0	0	0	0	0	0

选用改进型的D-H参数，各矩阵分别如下所示：

设

        其中

        如果你的机械臂D-H参数结构和我的一致，以上解析解你可以直接使用，如果参数相差不大，可以跟着我的计算思路推导一下你自己的逆解方程，相信你也可以得到正确的结果。

        MATLAB仿真验证

function R=AxisAngle_RotMat(Rxyz)
    theta=(Rxyz(1)^2+Rxyz(2)^2+Rxyz(3)^2)^0.5;
    if(abs(theta)