机械臂逆运动学八组逆解求解程序，MATLAB实现

对机械臂求逆解，以puma560为原型，该机械臂理论上每个姿态对应着八组逆解，本程序则将该机械臂的八组逆解全部求出，以函数形式调用。

function TH = niyundx_change（A1）

a2=431.8;a3=20.32;d2=149.09;d4=433.07;

nx=A1（11）; ox=A1（12）; ax=A1（13）; px=A1（14）;
ny=A1（21）; oy=A1（22）; ay=A1（23）; py=A1（24）;
nz=A1（31）; oz=A1（32）; az=A1（33）; pz=A1（34）;

th1（1） = atan2（pypx）-atan2（d2（sqrt（abs（px^2+py^2-d2^2））））;
th1（2） = atan2（pypx）-atan2（d2-（sqrt（abs（px^2+py^2-d2^2））））;

k = （px^2+py^2+pz^2-a2^2-a3^2-d2^2-d4^2）/（2*a2）;
th3（1） = atan2（a3d4）-atan2（k（sqrt（abs（a3^2+d4^2-k^2））））;
th3（2） = atan2（a3d4）-atan2（k-（sqrt（abs（a3^2+d4^2-k^2））））;

%th23（11）的两列，第一列指的是第几个th3第二列指的是第几个th1；
th23（11） = atan2（（（-a3-a2*cos（th3（1）））*pz + （cos（th1（1））*px+sin（th1（1））*py）*（a2*sin（th3（1））-...
    d4））（（-d4+a2*sin（th3（1）））*pz-（cos（th1（1））*px+sin（th1（1））*py）*（-a2*cos（th3（1））-a3）））;
th23（12） = atan2（（（-a3-a2*cos（th3（1）））*pz + （cos（th1（2））*px+sin（th1（2））*py）*（a2*sin（th3（1））-...
    d4））（（-d4+a2*sin（th3（1）））*pz-（cos（th1（2））*px+sin（th1（2））*py）*（-a2*cos（th3（1））-a3）））;
th23（21） = atan2（（（-a3-a2*cos（th3（2）））*pz + （cos（th1（1））*px+sin（th1（1））*py）*（a2*sin（th3（2））-...
    d4））（（-d4+a2*sin（th3（2）））*pz-（cos（th1（1））*px+sin（th1（1））*py）*（-a2*cos（th3（2））-a3）））;
th23（22） = atan2（（（-a3-a2*cos（th3（2）））*pz + （cos（th1（2））*px+sin（th1（2））*py）*（a2*sin（th3（2））-...
    d4））（（-d4+a2*sin（th3（2）））*pz-（cos（th1（2））*px+sin（th1（2））*py）*（-a2*cos（th3（2））-a3）））;

th2（11） = th23（11） - th3（1）;
th2（12） = th23（12） - th3（1）;
th2（21） = th23（21） - th3（2）;
th2（22） = th23（22） - th3（2）;

th4（11） = atan2（（-ax*sin（th1（1））+ay*cos（th1（1）））（-ax*cos（th1（1））*cos（th23（11））-...
    ay*sin（th1（1））*cos（th23（11））+az*sin（th23（11））））;
th4（12） = atan2（（-ax*sin（th1（2））+ay*cos（th1（2）））（-ax*cos（th1（2））*cos（th23（12））-...
    ay*sin（th1（2））*cos（th23（12））+az*sin（th23（12））））;
th4（21） = atan2（（-ax*sin（th1（1））+ay*cos（th1（1）））（-ax*cos（th1（1））*cos（th23（21））-...
    ay*sin（th1（1））*cos（th23（21））+az*sin（th23（21））））;
th4（22） = atan2（（-ax*sin（th1（2））+ay*cos（th1（2）））（-ax*cos（th1（2））*cos（th23（22））-...
    ay*sin（th1（2））*cos（th23（22））+az*sin（th23（22））））;

s5（11） = -（ax*（cos（th1（1））*cos（th23（11））*cos（th4（11））+sin（th1（1））*sin（th4（11）））+...
    ay*（sin（th1（1））*cos（th23（11））*cos（th4（11））-cos（th1（1））*sin（th4（11）））-...
    az*（sin（th23（11））*cos（th4（11））））;
c5（11） = ax*（-cos（th1（1））*sin（th23（11）））+ay*（-sin（th1（1））*sin（th23（11）））+...
    az*（-cos（th23（11）））;
s5（12） = -（ax*（cos（th1（2））*cos（th23（12））*cos（th4（12））+sin（th1（2））*sin（th4（12）））+...
    ay*（sin（th1（2））*cos（th23（12））*cos（th4（12））-cos（th1（2））*sin（th4（12）））-...
    az*（sin（th23（12））*cos（th4（12））））;
c5（12） = ax*（-cos（th1（2））*sin（th23（12）））+ay*（-sin（th1（2））*sin（th23（12）））+...
    az*（-cos（th23（12）））;
s5（21） = -（ax*（cos（th1（1））*cos（th23（21））*cos（th4（21））+sin（th1（1））*sin（th4（21）））+...
    ay*（sin（th1（1））*cos（th23（21））*cos（th4（21））-cos（th1（1））*sin（th4（21）））-...
    az*（sin（th23（21））*cos（th4（21））））;
c5（21） = ax*（-cos（th1（1））*sin（th23（21）））+ay*（-sin（th1（1））*sin（th23（21）））+...
    az*（-cos（th23（21）））;
s5（22） = -（

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2020-07-09 16:50  逆运动学\
     文件        7002  2020-07-09 16:34  逆运动学\niyundx_change.m