aubo-i5机械臂（2）-逆运动学求解

目录

1、逆运动学求解理论推导

2、matlab实现逆运动学求解

3、逆运动学求解正确性验证

---

1、逆运动学求解理论推导

对于AUBO-I5机械臂而言，其具有六个自由度，其中三个确定末端位置信息，另外三个确定末端姿态信息。以基座坐标为参照，由第一章正运动学AUBO-I5末端相对于他的姿态信息可以用进行表示，参考式子如下：

更多理论与求解公式参考[1]曹继项. 基于视觉的AUBO机器人标定方法研究[D].长安大学,2018.

注：求解θ时应使用atan2(sinxθ,cosθ)函数，因为θ的范围在[-pi,pi],而其他反三角函数不符合，详情自己搜索，默认学过机器人的都懂。

2、matlab实现逆运动学求解

 此代码为一个完整的逆运动学求解函数，可直接调用。

function q = aubo_ikin(H_t2o)
% q 为输出，表示关节坐标；
% H_t2o 为输入，表示末端到基座的 4X4 齐次变换矩阵。

%%%！！！定义100为无效值，判断某一个关节角度无法计算得出正确解时填充100！！！%%
%%
%%机械臂的连杆长度和连杆偏置值%%
d1=0.0985;
d2=0.1215;
d5=0.1025;
d6=0.094;
a2 = 0.408;
a3 = 0.376;

nx =H_t2o(1,1);   ox=H_t2o(1,2);   ax=H_t2o(1,3);
ny =H_t2o(2,1);   oy=H_t2o(2,2);   ay=H_t2o(2,3);
nz =H_t2o(3,1);   oz=H_t2o(3,2);   az=H_t2o(3,3);
px=H_t2o(1,4);    py=H_t2o(2,4);   pz=H_t2o(3,4);%%读取输入矩阵的各个元素

Theta = nan(6,8);%%预先开辟数组存放各组解%%
%%
%%根据公式计算各个关节角度%%
%%%%计算theta1%%%%
%%theta11、theta12为最终解%%
A=(ax*d6-px)^2+(d6*ay-py)^2-(d2)^2;
if abs(A)<=1e-5
    A=abs(A);
end
if A>=0
    theta11=atan2(d6*ay-py,ax*d6-px)-atan2(d2,sqrt(A));
    theta12=atan2(d6*ay-py,ax*d6-px)-atan2(d2,-sqrt(A));%%得出theta1的两个解%%
else
    theta11=100;
    theta12=100;  %%%不符合条件，填充无效值100（下同）%%
end   
    Theta(1,1:4)=theta11;
    Theta(1,5:8)=theta12;  %%将theta1存放至数组Theta中%%
%%
%%%%计算theta5%%%%
%%theta51、theta52、theta53、theta54为最终解%%
c51=ax*sin(theta11)-ay*cos(theta11);%theta11
s51=(1-(ay)^2)*(cos(theta11))^2+(1-(ax)^2)*(sin(theta11))^2-2*sin(theta11)*cos(theta11)*(nx*ny+ox*oy);
if abs(s51)<=1e-5
    s51=abs(s51);
end
if s51>=0 && theta11 ~=100
    theta51=atan2(sqrt(s51),c51);
    theta52=atan2(-sqrt(s51),c51);%%使用theta11-theta51 & theta52
else
    theta51=100;
    theta52=100;  
end
    Theta(5,1:2)=theta51;
    Theta(5,3:4)=theta52;%%将theta5存放至数组theta%%
    
c52=ax*sin(theta12)-ay*cos(theta12);%theta12
s52=(1-(ay)^2)*(cos(theta12))^2+(1-(ax)^2)*(sin(theta12))^2-2*sin(theta12)*cos(theta12)*(nx*ny+ox*oy);
if abs(s52)<=1e-5
    s52=abs(s52);
end
if s52>=0 && theta12 ~=100
    theta53=atan2(sqrt(s52),c52);
    theta54=atan2(-sqrt(s52),c52);%%使用theta12-theta53 & theta54  
else
    theta53=100;
    theta54=100;
end  
    Theta(5,5:6)=theta53;
    Theta(5,7:8)=theta54;
%%
%%%%%计算theta6%%%%%%
%%theta61、theta62、theta63、theta64为最终解%%
if theta11~=100 
   c61=(nx*sin(theta11)-ny*cos(theta11))/(-sin(theta51));
   s61=(ox*sin(theta11)-oy*cos(theta11))/(sin(theta51));%theta11-theta51
   c62=(nx*sin(theta11)-ny*cos(theta11))/(-sin(theta52));
   s62=(ox*sin(theta11)-oy*cos(theta11))/(sin(theta52));%theta11-theta52
   theta61=atan2(s61,c61);%theta11-theta51-theta61
   theta62=atan2(s62,c62);%theta11-theta52-theta62
else
   theta61=100;
   theta62=100;
end
   Theta(6,1:2)=theta61;
   Theta(6,3:4)=theta62;

if theta12~=100
   c63=(nx*sin(theta12)-ny*cos(theta12))/(-sin(theta53));
   s63=(ox*sin(theta12)-oy*cos(theta12))/(sin(theta53));%theta12-theta53
   c64=(nx*sin(theta12)-ny*cos(theta12))/(-sin(theta54));
   s64=(ox*sin(theta12)-oy*cos(theta12))/(sin(theta54));%theta12-theta54
   theta63=atan2(s63,c63);%theta12-theta53-theta63
   theta64=atan2(s64,c64);%theta12-theta54-theta63
else
   theta63=100;
   theta64=100;
end
   Theta(6,5:6)=theta63;
   Theta(6,7:8)=theta64; 
%%
%%计算theta3%%
m31=-px*cos(theta11)-py*sin(theta11)+d5*(sin(theta61)*(ny*sin(theta11)+nx*cos(theta11))+cos(theta61)*(ox*cos(theta11)+oy*sin(theta11)))+d6*(ay*sin(theta11)+ax*cos(theta11));
n31=pz-d1-az*d6-d5*(oz*cos(theta61)+nz*sin(theta61));%theta11-theta61

m32=-px*cos(theta11)-py*sin(theta11)+d5*(sin(theta62)*(ny*sin(theta11)+nx*cos(theta11))+cos(theta62)*(ox*cos(theta11)+oy*sin(theta11)))+d6*(ay*sin(theta11)+ax*cos(theta11));
n32=pz-d1-az*d6-d5*(oz*cos(theta62)+nz*sin(theta62));%theta11-theta62

m33=-px*cos(theta12)-py*sin(theta12)+d5*(sin(theta63)*(ny*sin(theta12)+nx*cos(theta12))+cos(theta63)*(ox*cos(theta12)+oy*sin(theta12)))+d6*(ay*sin(theta12)+ax*cos(theta12));
n33=pz-d1-az*d6-d5*(oz*cos(theta63)+nz*sin(theta63));%theta12-theta63

m34=-px*cos(theta12)-py*sin(theta12)+d5*(sin(theta64)*(ny*sin(theta12)+nx*cos(theta12))+cos(theta64)*(ox*cos(theta12)+oy*sin(theta12)))+d6*(ay*sin(theta12)+ax*cos(theta12));
n34=pz-d1-az*d6-d5*(oz*cos(theta64)+nz*sin(theta64));%theta12-theta64

c31=(m31^2+n31^2-a3^2-a2^2)/(2*a3*a2);%theta11-theta51-theta61
s31=1-c31^2;
if abs(s31)<=1e-5
    s31=abs(s31);
end
if s31>=0 && theta11~=100
   theta31=atan2(sqrt(s31),c31);
   theta32=atan2(-sqrt(s31),c31);%theta11-theta51-theta61-theta31&32
else
   theta31=100;
   theta32=100;
end

c32=(m32^2+n32^2-a3^2-a2^2)/(2*a3*a2);%theta11-theta52-theta61
s32=1-c32^2;
if abs(s32)<=1e-5
    s32=abs(s32);
end
if s32>=0 && theta11~=100
    theta33=atan2(sqrt(s32),c32);
    theta34=atan2(-sqrt(s32),c32);%theta11-theta52-theta62-theta33&34
else
    theta33=100;
    theta34=100;
end

c33=(m33^2+n33^2-a3^2-a2^2)/(2*a3*a2);%theta12-theta53-theta63
s33=1-c33^2;
if abs(s33)<=1e-5
    s33=abs(s33);
end 
if s33>=0 && theta12~=100
    theta35=atan2(sqrt(s33),c33);
    theta36=atan2(-sqrt(s33),c33);%theta12-theta53-theta63-theta35&36
else
    theta35=100;
    theta36=100;
end

c34=(m34^2+n34^2-a3^2-a2^2)/(2*a3*a2);%theta12-theta54-theta63 
s34=1-c34^2;
if abs(s34)<=1e-5
    s34=abs(s34);
end
if s34>=0 && theta12~=100
    theta37=atan2(sqrt(s34),c34);
    theta38=atan2(-sqrt(s34),c34);%theta12-theta54-theta64-theta37&38
else
    theta37=100;
    theta38=100;
end
Theta(3,1)=theta31;
Theta(3,2)=theta32;
Theta(3,3)=theta33;
Theta(3,4)=theta34;
Theta(3,5)=theta35;
Theta(3,6)=theta36;
Theta(3,7)=theta37;
Theta(3,8)=theta38;

%%
%%计算theta2%%
theta2(6)=0; %预先开辟数组存放theta2

%theta31&32-theta21&22
for i=1:2
if  Theta(3,i)~=100
  m21=(a3*cos(Theta(3,i))+a2)^2+(a3*sin(Theta(3,i))^2);
  theta2(i)=atan2(((m31*(a3*cos(Theta(3,i))+a2)+n31*a3*sin(Theta(3,i)))/m21),(n31*(cos(Theta(3,i))*a3+a2)-m31*a3*sin(Theta(3,i)))/m21);
else
    theta2(i)=100;
end
    Theta(2,i)=theta2(i);
end

%theta33&34-theta23&24
for i=3:4
    if  Theta(3,i)~=100
       m23=(a3*cos(Theta(3,i))+a2)^2+(a3*sin(Theta(3,i))^2);
       theta2(i)=atan2(((m32*(a3*cos(Theta(3,i))+a2)+n32*sin(Theta(3,i))*a3)/m23),(n32*(cos(Theta(3,i))*a3+a2)-m32*a3*sin(Theta(3,i)))/m23);
    else
       theta2(i)=100;
    end
       Theta(2,i)=theta2(i);
end

%theta35&36-theta25&26
for i=5:6
    if Theta(3,i)~=100
      m25=(a3*cos(Theta(3,i))+a2)^2+(a3*sin(Theta(3,i))^2);
      theta2(i)=atan2(((m33*(a3*cos(Theta(3,i))+a2)+n33*sin(Theta(3,i))*a3)/m25),(n33*(cos(Theta(3,i))*a3+a2)-m33*a3*sin(Theta(3,i)))/m25);
    else
       theta2(i)=100;
    end
       Theta(2,i)=theta2(i);
end

%theta37%38-theta27&28
for i=7:8
    if Theta(3,i)~=100
       m27=(a3*cos(Theta(3,i))+a2)^2+(a3*sin(Theta(3,i))^2);
       theta2(i)=atan2(((m34*(a3*cos(Theta(3,i))+a2)+n34*sin(Theta(3,i))*a3)/m27),(n34*(cos(Theta(3,i))*a3+a2)-m34*a3*sin(Theta(3,i)))/m27);
    else
       theta2(i)=100;
    end
       Theta(2,i)=theta2(i);
end
%%
%%计算theta4%%
%theta11-theta61-theta31&33-theta21&22-theta41&42
theta4(6)=0;
for i=1:2
    if Theta(3,i)~=100 
       H41=-cos(theta61)*(ox*cos(theta11)+oy*sin(theta11))-sin(theta61)*(nx*cos(theta11)+ny*sin(theta11));
       I41=oz*cos(theta61)+nz*sin(theta61);
       theta4(i)=atan2((H41*cos(Theta(2,i)-Theta(3,i))-I41*sin(Theta(2,i)-Theta(3,i))),(I41*cos(Theta(2,i)-Theta(3,i))+H41*sin(Theta(2,i)-Theta(3,i))));
    else
        theta4(i)=100;
    end
    Theta(4,i)=theta4(i);
end

%theta11-theta62-theta33&34-theta23&24-theta43&44
for i=3:4
    if Theta(3,i)~=100 
       H42=-cos(theta62)*(ox*cos(theta11)+oy*sin(theta11))-sin(theta62)*(nx*cos(theta11)+ny*sin(theta11));
       I42=oz*cos(theta62)+nz*sin(theta62);
       theta4(i)=atan2((H42*cos(Theta(2,i)-Theta(3,i))-I42*sin(Theta(2,i)-Theta(3,i))),(I42*cos(Theta(2,i)-Theta(3,i))+H42*sin(Theta(2,i)-Theta(3,i))));
    else
        theta4(i)=100;
    end
    Theta(4,i)=theta4(i);
end

%theta12-theta63-theta35&36-theta25&26-theta45&46
for i=5:6
    if Theta(3,i)~=100 
       H43=-cos(theta63)*(ox*cos(theta12)+oy*sin(theta12))-sin(theta63)*(nx*cos(theta12)+ny*sin(theta12));
       I43=oz*cos(theta63)+nz*sin(theta63);
       theta4(i)=atan2((H43*cos(Theta(2,i)-Theta(3,i))-I43*sin(Theta(2,i)-Theta(3,i))),(I43*cos(Theta(2,i)-Theta(3,i))+H43*sin(Theta(2,i)-Theta(3,i))));
   else
        theta4(i)=100;
    end
    Theta(4,i)=theta4(i);
end

%theta12-theta64-theta37&38-theta27&28-theta47&theta48
for i=7:8
    if Theta(3,i)~=100 
       H44=-cos(theta64)*(ox*cos(theta12)+oy*sin(theta12))-sin(theta64)*(nx*cos(theta12)+ny*sin(theta12));
       I44=oz*cos(theta64)+nz*sin(theta64);
       theta4(i)=atan2((H44*cos(Theta(2,i)-Theta(3,i))-I44*sin(Theta(2,i)-Theta(3,i))),(I44*cos(Theta(2,i)-Theta(3,i))+H44*sin(Theta(2,i)-Theta(3,i))));
   else
        theta4(i)=100;
    end
    Theta(4,i)=theta4(i);
end
%%
%%%%%去除无效的解%%%%%
 M=8;
for i=1:6           %%循环找出每一行中无效值所在的列
    q6=Theta(i,1:M);
    x= find(abs(q6-100) <0.0001);
    long=length(x);
  if long>0         %%if判断是否存在多个无效值并for循环删除
    for L=1:long
    Theta(:,x(L))=[];
    M=M-1;
    x=x-1;
    end
  end
end
%%
%%根据角度变化的平方和最小理论寻找最优的一组解%%
Theta1=Theta';
[m,n]=size(Theta1);
q0=[pi,-pi/2,0,-pi/2,0,0];%零位角度
if m ==0
    q=fprintf('无解')
    return;
else
q5(m)=0;
for j=1:m   %%循环求每一组解角度变换的平方和
    q4=0;
    q3=Theta1(j,1:6);
    for k=1:6
    q4=(q3(k)-q0(k))^2+q4;   
    end
    q5(j)=q4;
end
   q5_min=min(q5); %%找出最小值并输出该组解
   x = find(abs(q5-q5_min) <0.0001);
   q=Theta1(x,1:6)
end
end

3、逆运动学求解正确性验证

  可随机生成一组解调用第一章的正运动学求出末端相对于基座的齐次变换矩阵T1再代入逆运动学https://blog.csdn.net/ACE_YOUNG/article/details/125595485?spm=1001.2014.3001.5501

求解，若求出的全部解调用正运动学求出的齐次变换矩阵都等于T1则正确。

注：需加上零位时关节角度，原因看第一章。

q1 = pi*rand(1,6); 
q0=[pi,-pi/2,0,-pi/2,0,0];%零位时关节角度
q=q1+q0;
[H_t2o,H_i]=aubo_fkin(DHtable,q);