六轴机器人matlab写运动学逆解函数（改进DH模型）

1.理论
本文采用的模型为之前博客“matlab机器人工具箱一般六轴的DH模型和改进DH模型建立与区别”里面的改进DH模型，参数不再重复给出。
基系与工具坐标系关系为：
$^{b} T_{0} \cdot （^{0} T_{1} \cdot^{1} T_{2} \cdot^{2} T_{3} \cdot^{3} T_{4} \cdot^{4} T_{5} \cdot^{5} T_{6} ） \cdot^{6} T_{e} =^{b} T_{e}$
将逆运动学问题简化为：
$^{0} T_{1} \cdot^{1} T_{2} \cdot^{2} T_{3} \cdot^{3} T_{4} \cdot^{4} T_{5} \cdot^{5} T_{6} =^{b} T_{0}^{- 1} \cdot^{b} T_{e} \cdot^{6} T_{e}^{- 1}$
2.转换为下式求解
$^{2} T_{3} \cdot^{3} T_{4} \cdot^{4} T_{5} =^{1} T_{2}^{- 1} \cdot^{0} T_{1}^{- 1} \cdot^{b} T_{0}^{- 1} \cdot^{b} T_{e} \cdot^{6} T_{e}^{- 1} \cdot^{5} T_{6}^{- 1}$
左边：

left = [ cos(theta3)*cos(theta4)*cos(theta5) - sin(theta3)*sin(theta5), - cos(theta5)*sin(theta3) - cos(theta3)*cos(theta4)*sin(theta5), cos(theta3)*sin(theta4), a2 + a3*cos(theta3) - d4*sin(theta3)] [ cos(theta3)*sin(theta5) + cos(theta4)*cos(theta5)*sin(theta3),   cos(theta3)*cos(theta5) - cos(theta4)*sin(theta3)*sin(theta5), sin(theta3)*sin(theta4),      d4*cos(theta3) + a3*sin(theta3)] [                                      -cos(theta5)*sin(theta4),                                         sin(theta4)*sin(theta5),             cos(theta4),                                    0] [                                                             0,                                                               0,                       0,                                    1]

右边：

right =  [ oz*sin(theta2)*sin(theta6) - nz*cos(theta6)*sin(theta2) + nx*cos(theta1)*cos(theta2)*cos(theta6) + ny*cos(theta2)*cos(theta6)*sin(theta1) - ox*cos(theta1)*cos(theta2)*sin(theta6) - oy*cos(theta2)*sin(theta1)*sin(theta6),   ax*cos(theta1)*cos(theta2) - az*sin(theta2) + ay*cos(theta2)*sin(theta1), oz*cos(theta6)*sin(theta2) + nz*sin(theta2)*sin(theta6) - ox*cos(theta1)*cos(theta2)*cos(theta6) - nx*cos(theta1)*cos(theta2)*sin(theta6) - oy*cos(theta2)*cos(theta6)*sin(theta1) - ny*cos(theta2)*sin(theta1)*sin(theta6), px*cos(theta1)*cos(theta2) - pz*sin(theta2) - a1*cos(theta2) + py*cos(theta2)*sin(theta1)] [ oz*cos(theta2)*sin(theta6) - nz*cos(theta2)*cos(theta6) - nx*cos(theta1)*cos(theta6)*sin(theta2) - ny*cos(theta6)*sin(theta1)*sin(theta2) + ox*cos(theta1)*sin(theta2)*sin(theta6) + oy*sin(theta1)*sin(theta2)*sin(theta6), - az*cos(theta2) - ax*cos(theta1)*sin(theta2) - ay*sin(theta1)*sin(theta2), oz*cos(theta2)*cos(theta6) + nz*cos(theta2)*sin(theta6) + ox*cos(theta1)*cos(theta6)*sin(theta2) + nx*cos(theta1)*sin(theta2)*sin(theta6) + oy*cos(theta6)*sin(theta1)*sin(theta2) + ny*sin(theta1)*sin(theta2)*sin(theta6), a1*sin(theta2) - pz*cos(theta2) - px*cos(theta1)*sin(theta2) - py*sin(theta1)*sin(theta2)] [                                                                                                           ny*cos(theta1)*cos(theta6) - nx*cos(theta6)*sin(theta1) - oy*cos(theta1)*sin(theta6) + ox*sin(theta1)*sin(theta6),                                            ay*cos(theta1) - ax*sin(theta1),                                                                                                           ox*cos(theta6)*sin(theta1) - ny*cos(theta1)*sin(theta6) - oy*cos(theta1)*cos(theta6) + nx*sin(theta1)*sin(theta6),                                                           py*cos(theta1) - px*sin(theta1)] [                                                                                                                                                                                                                           0,                                                                          0,                                                                                                                                                                                                                           0,                                                                                         1]

令左右两边相等，求出六个角。、
逆解顺序如下：

由于公式比较复杂我仅在此给出想法，各位可自行计算。
$θ_{1}$ ：两式第3行第4列。(本机器臂 $θ_{1}$ 解唯一)
$θ_{3}$ ：两式第1行第4列和第2行第4列。
$θ_{2}$ ：两式第1行第4列和第2行第4列。
$θ_{5}$ ：两式第1行第2列和第2行第2列。
$θ_{4}$ ：两式第1行第2列和第2行第2列。
$θ_{6}$ ：两式第3行第1列和第3行第3列。
3.MATLAB程序
分两个程序①主函数②function函数
①主函数

clear; clc; %建立机器人模型 %       theta    d           a        alpha     offset ML1=Link([0       0           0           0          0     ],'modified');  ML2=Link([0       0           0.180      -pi/2       0     ],'modified'); ML3=Link([0       0           0.600       0          0     ],'modified'); ML4=Link([0       0.630       0.130      -pi/2       0     ],'modified'); ML5=Link([0       0           0           pi/2       0     ],'modified'); ML6=Link([0       0           0          -pi/2       0     ],'modified'); modrobot=SerialLink([ML1 ML2 ML3 ML4 ML5 ML6],'name','modified'); modmyt06=mymodfkine(-pi/3,-pi/3,pi/3,-pi/4,pi/4,pi/6); modmyikine=mymodikine(modmyt06)

②function函数

function [ikine_theta]=mymodikine(Tbe) %    thetai   di     ai-1     alphai-1  MDH=[0   0       0         0;      0   0       0.180    -pi/2;      0   0       0.600     0;      0   0.630   0.130    -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);   px=Tbe(1,4); py=Tbe(2,4); pz=Tbe(3,4);  d4=MDH(4,2);a1=MDH(2,3);a2=MDH(3,3);a3=MDH(4,3);  %theta1  theta1=atan(py/px);  % %theta3  t1=a1-px*cos(theta1)-py*sin(theta1);  t2=t1^2+pz^2-a2^2-a3^2-d4^2;  theta31=-atan2(-a3,d4)+atan2(t2,(4*a2^2*(a3^2+d4^2)-t2^2)^0.5);  theta32=-atan2(-a3,d4)+atan2(-t2,(4*a2^2*(a3^2+d4^2)-t2^2)^0.5);  %theta2  m1=a2+a3*cos(theta31)-d4*sin(theta31);  n1=d4*cos(theta31)+a3*sin(theta31);  m2=a2+a3*cos(theta32)-d4*sin(theta32);  n2=d4*cos(theta32)+a3*sin(theta32);  theta21=atan2((t1*n1-pz*m1),(-t1*m1-pz*n1));  theta22=atan2((t1*n2-pz*m2),(-t1*m2-pz*n2));  %theta5  p1=ax*cos(theta1)*cos(theta21)+ay*cos(theta21)*sin(theta1)-az*sin(theta21);  q1=-ax*cos(theta1)*sin(theta21)-ay*sin(theta1)*sin(theta21)-az*cos(theta21);  p2=ax*cos(theta1)*cos(theta22)+ay*cos(theta22)*sin(theta1)-az*sin(theta22);  q2=-ax*cos(theta1)*sin(theta22)-ay*sin(theta1)*sin(theta22)-az*cos(theta22);  theta51=atan2((1-(q1*cos(theta31)-p1*sin(theta31))^2)^0.5,(q1*cos(theta31)-p1*sin(theta31)));  theta52=atan2(-(1-(q1*cos(theta31)-p1*sin(theta31))^2)^0.5,(q1*cos(theta31)-p1*sin(theta31)));  theta53=atan2((1-(q2*cos(theta32)-p2*sin(theta32))^2)^0.5,(q2*cos(theta32)-p2*sin(theta32)));  theta54=atan2(-(1-(q2*cos(theta32)-p2*sin(theta32))^2)^0.5,(q2*cos(theta32)-p2*sin(theta32)));  %theta4  if sin(theta51)==0      theta41=0;  else       theta41=atan2((ay*cos(theta1)-ax*sin(theta1)),-(p1*cos(theta31)+q1*sin(theta31)));  end  if sin(theta52)==0       theta42=0;  else       theta42=atan2((ay*cos(theta1)-ax*sin(theta1)),-(p1*cos(theta31)+q1*sin(theta31)));  end  if sin(theta53)==0       theta43=0;  else       theta43=atan2((ay*cos(theta1)-ax*sin(theta1)),-(p2*cos(theta32)+q2*sin(theta32)));  end  if sin(theta54)==0       theta44=0;  else       theta44=atan2((ay*cos(theta1)-ax*sin(theta1)),-(p2*cos(theta32)+q2*sin(theta32)));  end  %theta6  e=nx*sin(theta1)-ny*cos(theta1);  f=ox*sin(theta1)-oy*cos(theta1);  theta61=atan2((cos(theta41)*e-cos(theta51)*sin(theta41)*f),(cos(theta41)*f+cos(theta51)*sin(theta41)*e));  theta62=atan2((cos(theta42)*e-cos(theta52)*sin(theta42)*f),(cos(theta42)*f+cos(theta52)*sin(theta42)*e));  theta63=atan2((cos(theta43)*e-cos(theta53)*sin(theta43)*f),(cos(theta43)*f+cos(theta53)*sin(theta43)*e));  theta64=atan2((cos(theta44)*e-cos(theta54)*sin(theta44)*f),(cos(theta44)*f+cos(theta54)*sin(theta44)*e));  %%%%  ikine_theta=[theta1 theta21 theta31 theta41 theta51 theta61;               theta1 theta21 theta31 theta42 theta52 theta62;               theta1 theta22 theta32 theta43 theta53 theta63;               theta1 theta22 theta32 theta44 theta54 theta64];