上节建立了机器人的DH参数和正运动学方程，本节对该IRB2600机器人求逆过程进行逐步推导，包括所有的运算过程和Matlab代码。
对于六自由度的工业机械臂，一般存在八组逆解，即机器人到达笛卡尔空间坐标系中的任意一点都存在八种姿态（在不考虑关节限位的情况下）。工业机器人的逆运动学求解，常用的有数值解和解析解，其中数值解是采用差值（最小二乘法）或逼近的方式得到的一组近似解，通常计算量较大，对于冗余自由度机器人较为适用。而一般的工业机器人在设计的时候会满足pieper准则（后三轴的轴线交于一点：腕部），这种情况下采用解析法（反变换法）来求逆通常较为简单。
首先定义前文中相邻关节的齐次变换矩阵和变量：
syms c1 c2 c3 c4 c5 c6 s1 s2 s3 s4 s5 s6 a1 a2 a3 d1 d4 d6 nx ny nz ox oy oz ax ay az wx wy wz
T01=[c1,-s1,0,0; s1,c1,0,0; 0,0,1,d1; 0,0,0,1];
T12=[s2,c2,0,a1; 0,0,1,0; c2,-s2,0,0; 0,0,0,1];
T23=[c3,-s3,0,a2; s3,c3,0,0; 0,0,1,0; 0,0,0,1];
T34=[c4,-s4,0,a3; 0,0,1,d4; -s4,-c4,0,0; 0,0,0,1];
T45=[c5,-s5,0,0; 0,0,-1,0; s5,c5,0,0; 0,0,0,1];
T56=[c6,-s6,0,0; 0,0,1,0; -s6,-c6,0,0; 0,0,0,1];
T06=[nx,ox,ax,wx; ny,oy,ay,wy; nz,oz,az,wz; 0,0,0,1];然后连乘T01*T12*T23*T34*T45*T56得到一个超级长的变换矩阵，还是很容易化简的。
T01*T12*T23*T34*T45*T56
ans =
[c6*(s5*(c1*c2*c3 - c1*s2*s3) + c5*(s1*s4 + c4*(c1*c2*s3 + c1*c3*s2))) + s6*(c4*s1 - s4*(c1*c2*s3 + c1*c3*s2)), c6*(c4*s1 - s4*(c1*c2*s3 + c1*c3*s2)) - s6*(s5*(c1*c2*c3 - c1*s2*s3) + c5*(s1*s4 + c4*(c1*c2*s3 + c1*c3*s2))), c5*(c1*c2*c3 - c1*s2*s3) - s5*(s1*s4 + c4*(c1*c2*s3 + c1*c3*s2)), a1*c1 + a3*(c1*c2*s3 + c1*c3*s2) + d4*(c1*c2*c3 - c1*s2*s3) + a2*c1*s2]
[c6*(s5*(c2*c3*s1 - s1*s2*s3) - c5*(c1*s4 - c4*(c2*s1*s3 + c3*s1*s2))) - s6*(c1*c4 + s4*(c2*s1*s3 + c3*s1*s2)), - s6*(s5*(c2*c3*s1 - s1*s2*s3) - c5*(c1*s4 - c4*(c2*s1*s3 + c3*s1*s2))) - c6*(c1*c4 + s4*(c2*s1*s3 + c3*s1*s2)), c5*(c2*c3*s1 - s1*s2*s3) + s5*(c1*s4 - c4*(c2*s1*s3 + c3*s1*s2)), a1*s1 + a3*(c2*s1*s3 + c3*s1*s2) + d4*(c2*c3*s1 - s1*s2*s3) + a2*s1*s2]
[ - c6*(s5*(c2*s3 + c3*s2) - c4*c5*(c2*c3 - s2*s3)) - s4*s6*(c2*c3 - s2*s3), s6*(s5*(c2*s3 + c3*s2) - c4*c5*(c2*c3 - s2*s3)) - c6*s4*(c2*c3 - s2*s3), - c5*(c2*s3 + c3*s2) - c4*s5*(c2*c3 - s2*s3), d1 + a2*c2 + a3*(c2*c3 - s2*s3) - d4*(c2*s3 + c3*s2)]
[ 0, 0, 0, 1]
在刚体的位姿描述中可以得到T06=
因为六轴坐标系建立在腕部，将px py pz 用wx wy wz表示。
我们通过等式T06=T01*T12*T23*T34*T45*T56可以看出，上面Matlab求得的wx wy wz分别为
可以得到 θ1=atan2(wy,wx) ，同理通过上式可以求得机器人的前三个关节角度，具体推导过程见结尾附图。
后三个关节角度需要进行矩阵的反变换，将等式T06=T01*T12*T23*T34*T45*T56的两端同时乘以得T36=
T06=T34*T45*T56.
其中为
inv(T01*T12*T23)
ans =
[(c1*c2*s3 + c1*c3*s2)/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2), (c2*s1*s3 + c3*s1*s2)/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2), (c2*c3 - s2*s3)/(c2^2*c3^2 + c2^2*s3^2 + c3^2*s2^2 + s2^2*s3^2), -(c2*c3*d1 + a1*c2*s3 + a1*c3*s2 - d1*s2*s3 + a2*c2^2*c3 + a2*c3*s2^2)/(c2^2*c3^2 + c2^2*s3^2 + c3^2*s2^2 + s2^2*s3^2)]
[(c1*c2*c3 - c1*s2*s3)/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2), (s1*(c2*c3 - s2*s3))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2), -(c2*s3 + c3*s2)/(c2^2*c3^2 + c2^2*s3^2 + c3^2*s2^2 + s2^2*s3^2), (a2*s2^2*s3 - a1*c2*c3 + c2*d1*s3 + c3*d1*s2 + a1*s2*s3 + a2*c2^2*s3)/(c2^2*c3^2 + c2^2*s3^2 + c3^2*s2^2 + s2^2*s3^2)]
[ -s1/(c1^2 + s1^2), c1/(c1^2 + s1^2), 0, 0]
[ 0, 0, 0, 1]T06为
T06=[nx ox ax wx
ny oy ay wy
nz oz az wz
0 0 0 1]; 等式左边 T06得到一个超级超级长的矩阵，定义为A.
inv(T01*T12*T23)*T06
ans =
[(nx*(c1*c2*s3 + c1*c3*s2))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2) + (nz*(c2*c3 - s2*s3))/(c2^2*c3^2 + c2^2*s3^2 + c3^2*s2^2 + s2^2*s3^2) + (ny*(c2*s1*s3 + c3*s1*s2))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2), (ox*(c1*c2*s3 + c1*c3*s2))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2) + (oz*(c2*c3 - s2*s3))/(c2^2*c3^2 + c2^2*s3^2 + c3^2*s2^2 + s2^2*s3^2) + (oy*(c2*s1*s3 + c3*s1*s2))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2), (ax*(c1*c2*s3 + c1*c3*s2))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2) + (az*(c2*c3 - s2*s3))/(c2^2*c3^2 + c2^2*s3^2 + c3^2*s2^2 + s2^2*s3^2) + (ay*(c2*s1*s3 + c3*s1*s2))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2), (wx*(c1*c2*s3 + c1*c3*s2))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2) - (c2*c3*d1 + a1*c2*s3 + a1*c3*s2 - d1*s2*s3 + a2*c2^2*c3 + a2*c3*s2^2)/(c2^2*c3^2 + c2^2*s3^2 + c3^2*s2^2 + s2^2*s3^2) + (wz*(c2*c3 - s2*s3))/(c2^2*c3^2 + c2^2*s3^2 + c3^2*s2^2 + s2^2*s3^2) + (wy*(c2*s1*s3 + c3*s1*s2))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2)]
[ (nx*(c1*c2*c3 - c1*s2*s3))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2) - (nz*(c2*s3 + c3*s2))/(c2^2*c3^2 + c2^2*s3^2 + c3^2*s2^2 + s2^2*s3^2) + (ny*s1*(c2*c3 - s2*s3))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2), (ox*(c1*c2*c3 - c1*s2*s3))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2) - (oz*(c2*s3 + c3*s2))/(c2^2*c3^2 + c2^2*s3^2 + c3^2*s2^2 + s2^2*s3^2) + (oy*s1*(c2*c3 - s2*s3))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2), (ax*(c1*c2*c3 - c1*s2*s3))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2) - (az*(c2*s3 + c3*s2))/(c2^2*c3^2 + c2^2*s3^2 + c3^2*s2^2 + s2^2*s3^2) + (ay*s1*(c2*c3 - s2*s3))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2), (a2*s2^2*s3 - a1*c2*c3 + c2*d1*s3 + c3*d1*s2 + a1*s2*s3 + a2*c2^2*s3)/(c2^2*c3^2 + c2^2*s3^2 + c3^2*s2^2 + s2^2*s3^2) + (wx*(c1*c2*c3 - c1*s2*s3))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2) - (wz*(c2*s3 + c3*s2))/(c2^2*c3^2 + c2^2*s3^2 + c3^2*s2^2 + s2^2*s3^2) + (s1*wy*(c2*c3 - s2*s3))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2)]
[ (c1*ny)/(c1^2 + s1^2) - (nx*s1)/(c1^2 + s1^2), (c1*oy)/(c1^2 + s1^2) - (ox*s1)/(c1^2 + s1^2), (ay*c1)/(c1^2 + s1^2) - (ax*s1)/(c1^2 + s1^2), (c1*wy)/(c1^2 + s1^2) - (s1*wx)/(c1^2 + s1^2)]
[ 0, 0, 0, 1]然后等式右边的T34*T45*T56定义为B.
T34*T45*T56
ans =
[ c4*c5*c6 - s4*s6, - c6*s4 - c4*c5*s6, -c4*s5, a3]
[ c6*s5, -s5*s6, c5, d4]
[- c4*s6 - c5*c6*s4, c5*s4*s6 - c4*c6, s4*s5, 0]
[ 0, 0, 0, 1]令A(2,3)=B(2,3),化简求得θ5.
令A(1,3)=B(1,3),A(3,3)=B(3,3),化简求得θ4.
令A(2,1)=B(2,1),A(2,2)=B(2,2),化简求得θ6.
详细计算过程如下：
1、其中T12和T56的修改是因为在机器人的2轴和6轴均有一定角度的初始偏置，经计算验证得到的结论是：无论是否将DH参数中的初始偏置角度代入，均不影响逆解方程的结果。
2、对于θ2的角度计算用到了以下万能公式：
将计算得到的逆解通式输入Matlab:
function [ikine_t]=mymodikine(Tbe)
% ti di ai-1 alphai-1
MDH=[0 0.445 0 0;
0 0 0.150 -pi/2;
0 0 0.700 0;
0 0.795 0.115 -pi/2;
0 0 0 pi/2;
0 0 0 -pi/2];
nx=Tbe(1,1); ny=Tbe(2,1); nz=Tbe(3,1);
ox=Tbe(1,2); oy=Tbe(2,2); oz=Tbe(3,2);
ax=Tbe(1,3); ay=Tbe(2,3); az=Tbe(3,3);
wx=Tbe(1,4); wy=Tbe(2,4); wz=Tbe(3,4);
d1=0.445; d4=0.795;
a1=0.150; a2=0.700; a3=0.115;
%t1
t11=atan2(wy,wx);
t12=atan2(-wy,-wx);
%t2
k11=wx/cos(t11)-a1;
k12=wx/cos(t12)-a1;
k3=wz-d1;
K11=(k11*k11+k3*k3+a2*a2-d4*d4-a3*a3)/2/a2;
K12=(k12*k12+k3*k3+a2*a2-d4*d4-a3*a3)/2/a2;
t21=atan2(K11/(sqrt(k11*k11+k3*k3)),sqrt((k11*k11+k3*k3-K11*K11)/(k11*k11+k3*k3)))-atan2(k3,k11);
t22=atan2(K12/(sqrt(k12*k12+k3*k3)),sqrt((k12*k12+k3*k3-K12*K12)/(k12*k12+k3*k3)))-atan2(k3,k12);
t23=atan2(K11/(sqrt(k11*k11+k3*k3)),-sqrt((k11*k11+k3*k3-K11*K11)/(k11*k11+k3*k3)))-atan2(k3,k11);
t24=atan2(K12/(sqrt(k12*k12+k3*k3)),-sqrt((k12*k12+k3*k3-K12*K12)/(k12*k12+k3*k3)))-atan2(k3,k12);
%t3
A1=(a3*(k11-a2*sin(t21))-d4*(k3-a2*cos(t21)))/(d4*d4+a3*a3);
B1=(a3*(k3-a2*cos(t21))+d4*(k11-a2*sin(t21)))/(d4*d4+a3*a3);
A2=(a3*(k12-a2*sin(t22))-d4*(k3-a2*cos(t22)))/(d4*d4+a3*a3);
B2=(a3*(k3-a2*cos(t22))+d4*(k12-a2*sin(t22)))/(d4*d4+a3*a3);
A3=(a3*(k11-a2*sin(t23))-d4*(k3-a2*cos(t23)))/(d4*d4+a3*a3);
B3=(a3*(k3-a2*cos(t23))+d4*(k11-a2*sin(t23)))/(d4*d4+a3*a3);
A4=(a3*(k12-a2*sin(t24))-d4*(k3-a2*cos(t24)))/(d4*d4+a3*a3);
B4=(a3*(k3-a2*cos(t24))+d4*(k12-a2*sin(t24)))/(d4*d4+a3*a3);
t31=atan2(A1,B1)-t21;
t32=atan2(A2,B2)-t22;
t33=atan2(A3,B3)-t23;
t34=atan2(A4,B4)-t24;
%t5
r51=ax*cos(t11)*cos(t21+t31)-az*sin(t21+t31)+ay*sin(t11)*cos(t21+t31);
r52=ax*cos(t12)*cos(t22+t32)-az*sin(t22+t32)+ay*sin(t12)*cos(t22+t32);
r53=ax*cos(t11)*cos(t23+t33)-az*sin(t23+t33)+ay*sin(t11)*cos(t23+t33);
r54=ax*cos(t12)*cos(t24+t34)-az*sin(t24+t34)+ay*sin(t12)*cos(t24+t34);
t51=atan2(-sqrt(1-r51*r51),r51);
t52=atan2(-sqrt(1-r52*r52),r52);
t53=atan2(-sqrt(1-r53*r53),r53);
t54=atan2(-sqrt(1-r54*r54),r54);
t55=atan2(sqrt(1-r51*r51),r51);
t56=atan2(sqrt(1-r52*r52),r52);
t57=atan2(sqrt(1-r53*r53),r53);
t58=atan2(sqrt(1-r54*r54),r54);
%t4
t41=atan2(ax*sin(t11)-ay*cos(t11),ax*cos(t11)*sin(t21+t31)+az*cos(t21+t31)+ay*sin(t11)*sin(t21+t31));
t42=atan2(ax*sin(t12)-ay*cos(t12),ax*cos(t12)*sin(t22+t32)+az*cos(t22+t32)+ay*sin(t12)*sin(t22+t32));
t43=atan2(ax*sin(t11)-ay*cos(t11),ax*cos(t11)*sin(t23+t33)+az*cos(t23+t33)+ay*sin(t11)*sin(t23+t33));
t44=atan2(ax*sin(t12)-ay*cos(t12),ax*cos(t12)*sin(t24+t34)+az*cos(t24+t34)+ay*sin(t12)*sin(t24+t34));
t45=atan2(-(ax*sin(t11)-ay*cos(t11)),-(ax*cos(t11)*sin(t21+t31)+az*cos(t21+t31)+ay*sin(t11)*sin(t21+t31)));
t46=atan2(-(ax*sin(t12)-ay*cos(t12)),-(ax*cos(t12)*sin(t22+t32)+az*cos(t22+t32)+ay*sin(t12)*sin(t22+t32)));
t47=atan2(-(ax*sin(t11)-ay*cos(t11)),-(ax*cos(t11)*sin(t23+t33)+az*cos(t23+t33)+ay*sin(t11)*sin(t23+t33)));
t48=atan2(-(ax*sin(t12)-ay*cos(t12)),-(ax*cos(t12)*sin(t24+t34)+az*cos(t24+t34)+ay*sin(t12)*sin(t24+t34)));
%t6
t61=atan2(ox*cos(t11)*cos(t21+t31)-oz*sin(t21+t31)+oy*sin(t11)*cos(t21+t31),-(nx*cos(t11)*cos(t21+t31)-nz*sin(t21+t31)+ny*sin(t11)*cos(t21+t31)));
t62=atan2(ox*cos(t12)*cos(t22+t32)-oz*sin(t22+t32)+oy*sin(t12)*cos(t22+t32),-(nx*cos(t12)*cos(t22+t32)-nz*sin(t22+t32)+ny*sin(t12)*cos(t22+t32)));
t63=atan2(ox*cos(t11)*cos(t23+t33)-oz*sin(t23+t33)+oy*sin(t11)*cos(t23+t33),-(nx*cos(t11)*cos(t23+t33)-nz*sin(t23+t33)+ny*sin(t11)*cos(t23+t33)));
t64=atan2(ox*cos(t12)*cos(t24+t34)-oz*sin(t24+t34)+oy*sin(t12)*cos(t24+t34),-(nx*cos(t12)*cos(t24+t34)-nz*sin(t24+t34)+ny*sin(t12)*cos(t24+t34)));
t65=atan2(-(ox*cos(t11)*cos(t21+t31)-oz*sin(t21+t31)+oy*sin(t11)*cos(t21+t31)),(nx*cos(t11)*cos(t21+t31)-nz*sin(t21+t31)+ny*sin(t11)*cos(t21+t31)));
t66=atan2(-(ox*cos(t12)*cos(t22+t32)-oz*sin(t22+t32)+oy*sin(t12)*cos(t22+t32)),(nx*cos(t12)*cos(t22+t32)-nz*sin(t22+t32)+ny*sin(t12)*cos(t22+t32)));
t67=atan2(-(ox*cos(t11)*cos(t23+t33)-oz*sin(t23+t33)+oy*sin(t11)*cos(t23+t33)),(nx*cos(t11)*cos(t23+t33)-nz*sin(t23+t33)+ny*sin(t11)*cos(t23+t33)));
t68=atan2(-(ox*cos(t12)*cos(t24+t34)-oz*sin(t24+t34)+oy*sin(t12)*cos(t24+t34)),(nx*cos(t12)*cos(t24+t34)-nz*sin(t24+t34)+ny*sin(t12)*cos(t24+t34)));
ikine_t=[ t11 t21 t31 t41 t51 t61
t12 t22 t32 t42 t52 t62
t11 t23 t33 t43 t53 t63
t12 t24 t34 t44 t54 t64
t11 t21 t31 t45 t55 t65
t12 t22 t32 t46 t56 t66
t11 t23 t33 t47 t57 t67
t12 t24 t34 t48 t58 t68]*57.3;联合上一节的正运动学方程将各关节设置为（25°，3°，10°，-45°， -10°，120°），得到此时腕部的齐次变换矩阵为
theta1 = theta(1,1);
theta2 = theta(1,2);
theta3 = theta(1,3);
theta4 = theta(1,4);
theta5 = theta(1,5);
theta6 = theta(1,6);
% theta d a alpha
MDH=[theta1 0.445 0 0;
theta2-pi/2 0 0.150 -pi/2;
theta3 0 0.700 0;
theta4 0.795 0.115 -pi/2;
theta5 0 0 pi/2;
theta6-pi 0 0 -pi/2];
T01=[cos(MDH(1,1)) -sin(MDH(1,1)) 0 0;
sin(MDH(1,1)) cos(MDH(1,1)) 0 0;
0 0 1 MDH(1,2);
0 0 0 1];
T12=[cos(MDH(2,1)) -sin(MDH(2,1)) 0 MDH(2,3);
0 0 1 0;
-sin(MDH(2,1)) -cos(MDH(2,1)) 0 0;
0 0 0 1];
T23=[cos(MDH(3,1)) -sin(MDH(3,1)) 0 MDH(3,3);
sin(MDH(3,1)) cos(MDH(3,1)) 0 0;
0 0 1 0;
0 0 0 1];
T34=[cos(MDH(4,1)) -sin(MDH(4,1)) 0 MDH(4,3);
0 0 1 MDH(4,2);
-sin(MDH(4,1)) -cos(MDH(4,1)) 0 0
0 0 0 1];
T45=[cos(MDH(5,1)) -sin(MDH(5,1)) 0 0;
0 0 -1 0;
sin(MDH(5,1)) cos(MDH(5,1)) 0 0;
0 0 0 1];
T56=[cos(MDH(6,1)) -sin(MDH(6,1)) 0 0;
0 0 1 0;
-sin(MDH(6,1)) -cos(MDH(6,1)) 0 0;
0 0 0 1];
% 腕部到法兰
T6TCP=[1 0 0 0;
0 1 0 0;
0 0 1 0.085;
0 0 0 1];
% T06=T01*T12*T23*T34*T45*T56*T6TCP;
T06=T01*T12*T23*T34*T45*T56;
theta=[25*pi/180,3*pi/180,10*pi/180,-45*pi/180,-10*pi/180,120*pi/180];
T06
%//
%输出结果
T06 =
-0.5365 -0.0432 0.8428 0.8946
0.8097 0.2552 0.5285 0.4172
-0.2379 0.9659 -0.1019 1.0773
0 0 0 1.0000将求得的齐次变换矩阵代入逆解得到：
MDH=[0 0.445 0 0;
0 0 0.150 -pi/2;
0 0 0.700 0;
0 0.795 0.115 -pi/2;
0 0 0 pi/2;
0 0 0 -pi/2];
nx=Tbe(1,1); ny=Tbe(2,1); nz=Tbe(3,1);
ox=Tbe(1,2); oy=Tbe(2,2); oz=Tbe(3,2);
ax=Tbe(1,3); ay=Tbe(2,3); az=Tbe(3,3);
wx=Tbe(1,4); wy=Tbe(2,4); wz=Tbe(3,4);
d1=0.445; d4=0.795;
a1=0.150; a2=0.700; a3=0.115;
%t1
t11=atan2(wy,wx);
t12=atan2(-wy,-wx);
%t2
k11=wx/cos(t11)-a1;
k12=wx/cos(t12)-a1;
k3=wz-d1;
K11=(k11*k11+k3*k3+a2*a2-d4*d4-a3*a3)/2/a2;
K12=(k12*k12+k3*k3+a2*a2-d4*d4-a3*a3)/2/a2;
t21=atan2(K11/(sqrt(k11*k11+k3*k3)),sqrt((k11*k11+k3*k3-K11*K11)/(k11*k11+k3*k3)))-atan2(k3,k11);
t22=atan2(K12/(sqrt(k12*k12+k3*k3)),sqrt((k12*k12+k3*k3-K12*K12)/(k12*k12+k3*k3)))-atan2(k3,k12);
t23=atan2(K11/(sqrt(k11*k11+k3*k3)),-sqrt((k11*k11+k3*k3-K11*K11)/(k11*k11+k3*k3)))-atan2(k3,k11);
t24=atan2(K12/(sqrt(k12*k12+k3*k3)),-sqrt((k12*k12+k3*k3-K12*K12)/(k12*k12+k3*k3)))-atan2(k3,k12);
%t3
A1=(a3*(k11-a2*sin(t21))-d4*(k3-a2*cos(t21)))/(d4*d4+a3*a3);
B1=(a3*(k3-a2*cos(t21))+d4*(k11-a2*sin(t21)))/(d4*d4+a3*a3);
A2=(a3*(k12-a2*sin(t22))-d4*(k3-a2*cos(t22)))/(d4*d4+a3*a3);
B2=(a3*(k3-a2*cos(t22))+d4*(k12-a2*sin(t22)))/(d4*d4+a3*a3);
A3=(a3*(k11-a2*sin(t23))-d4*(k3-a2*cos(t23)))/(d4*d4+a3*a3);
B3=(a3*(k3-a2*cos(t23))+d4*(k11-a2*sin(t23)))/(d4*d4+a3*a3);
A4=(a3*(k12-a2*sin(t24))-d4*(k3-a2*cos(t24)))/(d4*d4+a3*a3);
B4=(a3*(k3-a2*cos(t24))+d4*(k12-a2*sin(t24)))/(d4*d4+a3*a3);
t31=atan2(A1,B1)-t21;
t32=atan2(A2,B2)-t22;
t33=atan2(A3,B3)-t23;
t34=atan2(A4,B4)-t24;
%t5
r51=ax*cos(t11)*cos(t21+t31)-az*sin(t21+t31)+ay*sin(t11)*cos(t21+t31);
r52=ax*cos(t12)*cos(t22+t32)-az*sin(t22+t32)+ay*sin(t12)*cos(t22+t32);
r53=ax*cos(t11)*cos(t23+t33)-az*sin(t23+t33)+ay*sin(t11)*cos(t23+t33);
r54=ax*cos(t12)*cos(t24+t34)-az*sin(t24+t34)+ay*sin(t12)*cos(t24+t34);
t51=atan2(-sqrt(1-r51*r51),r51);
t52=atan2(-sqrt(1-r52*r52),r52);
t53=atan2(-sqrt(1-r53*r53),r53);
t54=atan2(-sqrt(1-r54*r54),r54);
t55=atan2(sqrt(1-r51*r51),r51);
t56=atan2(sqrt(1-r52*r52),r52);
t57=atan2(sqrt(1-r53*r53),r53);
t58=atan2(sqrt(1-r54*r54),r54);
%t4
t41=atan2(ax*sin(t11)-ay*cos(t11),ax*cos(t11)*sin(t21+t31)+az*cos(t21+t31)+ay*sin(t11)*sin(t21+t31));
t42=atan2(ax*sin(t12)-ay*cos(t12),ax*cos(t12)*sin(t22+t32)+az*cos(t22+t32)+ay*sin(t12)*sin(t22+t32));
t43=atan2(ax*sin(t11)-ay*cos(t11),ax*cos(t11)*sin(t23+t33)+az*cos(t23+t33)+ay*sin(t11)*sin(t23+t33));
t44=atan2(ax*sin(t12)-ay*cos(t12),ax*cos(t12)*sin(t24+t34)+az*cos(t24+t34)+ay*sin(t12)*sin(t24+t34));
t45=atan2(-(ax*sin(t11)-ay*cos(t11)),-(ax*cos(t11)*sin(t21+t31)+az*cos(t21+t31)+ay*sin(t11)*sin(t21+t31)));
t46=atan2(-(ax*sin(t12)-ay*cos(t12)),-(ax*cos(t12)*sin(t22+t32)+az*cos(t22+t32)+ay*sin(t12)*sin(t22+t32)));
t47=atan2(-(ax*sin(t11)-ay*cos(t11)),-(ax*cos(t11)*sin(t23+t33)+az*cos(t23+t33)+ay*sin(t11)*sin(t23+t33)));
t48=atan2(-(ax*sin(t12)-ay*cos(t12)),-(ax*cos(t12)*sin(t24+t34)+az*cos(t24+t34)+ay*sin(t12)*sin(t24+t34)));
%t6
t61=atan2(ox*cos(t11)*cos(t21+t31)-oz*sin(t21+t31)+oy*sin(t11)*cos(t21+t31),-(nx*cos(t11)*cos(t21+t31)-nz*sin(t21+t31)+ny*sin(t11)*cos(t21+t31)));
t62=atan2(ox*cos(t12)*cos(t22+t32)-oz*sin(t22+t32)+oy*sin(t12)*cos(t22+t32),-(nx*cos(t12)*cos(t22+t32)-nz*sin(t22+t32)+ny*sin(t12)*cos(t22+t32)));
t63=atan2(ox*cos(t11)*cos(t23+t33)-oz*sin(t23+t33)+oy*sin(t11)*cos(t23+t33),-(nx*cos(t11)*cos(t23+t33)-nz*sin(t23+t33)+ny*sin(t11)*cos(t23+t33)));
t64=atan2(ox*cos(t12)*cos(t24+t34)-oz*sin(t24+t34)+oy*sin(t12)*cos(t24+t34),-(nx*cos(t12)*cos(t24+t34)-nz*sin(t24+t34)+ny*sin(t12)*cos(t24+t34)));
t65=atan2(-(ox*cos(t11)*cos(t21+t31)-oz*sin(t21+t31)+oy*sin(t11)*cos(t21+t31)),(nx*cos(t11)*cos(t21+t31)-nz*sin(t21+t31)+ny*sin(t11)*cos(t21+t31)));
t66=atan2(-(ox*cos(t12)*cos(t22+t32)-oz*sin(t22+t32)+oy*sin(t12)*cos(t22+t32)),(nx*cos(t12)*cos(t22+t32)-nz*sin(t22+t32)+ny*sin(t12)*cos(t22+t32)));
t67=atan2(-(ox*cos(t11)*cos(t23+t33)-oz*sin(t23+t33)+oy*sin(t11)*cos(t23+t33)),(nx*cos(t11)*cos(t23+t33)-nz*sin(t23+t33)+ny*sin(t11)*cos(t23+t33)));
t68=atan2(-(ox*cos(t12)*cos(t24+t34)-oz*sin(t24+t34)+oy*sin(t12)*cos(t24+t34)),(nx*cos(t12)*cos(t24+t34)-nz*sin(t24+t34)+ny*sin(t12)*cos(t24+t34)));
ikine_t=[ t11 t21 t31 t41 t51 t61
t12 t22 t32 t42 t52 t62
t11 t23 t33 t43 t53 t63
t12 t24 t34 t44 t54 t64
t11 t21 t31 t45 t55 t65
t12 t22 t32 t46 t56 t66
t11 t23 t33 t47 t57 t67
t12 t24 t34 t48 t58 t68]*57.3;
Tbe=[-0.5365 -0.0432 0.8428 0.8946
0.8097 0.2552 0.5285 0.4172
-0.2379 0.9659 -0.1019 1.0773
0 0 0 1.0000];
ikine_t
%///
%输出结果
ikine_t =
25.0040 2.9975 10.0005 -45.0108 -9.9961 120.0217
-155.0093 -93.3535 -21.4919 7.4515 -71.2102 -106.9829
25.0040 102.8794 -173.5506 -172.7646 -76.6684 -106.2506
-155.0093 -28.5017 -142.0582 25.0538 -16.8536 -128.6682
25.0040 2.9975 10.0005 135.0024 9.9961 -59.9916
-155.0093 -93.3535 -21.4919 -172.5617 71.2102 73.0304
25.0040 102.8794 -173.5506 7.2486 76.6684 73.7627
-155.0093 -28.5017 -142.0582 -154.9594 16.8536 51.3451
在RobotStudio中示教相同点位对比验证，各组轴配置与逆解的关节角度一致。
扫一扫
3万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
