基于几何法的机器人逆运动学求解--工业机器人前三个关节

利用几何法求解工业机器人逆运动学是将空间中的机器人结转换为几个正交平面下的几何法求解方法，就有简单，直观的效果。但是，对于机器人的一个空间位置姿态可能对于多种关节角的情况，采用几何法的求解可能会造成，考虑不充分，求解结果变少的情况。但基于它所具有的优点和直观性，我觉得还是可以和大家分享一下我的求解例子。我以工业机器人staubli为例，机器人的前三个关节确定机器人的位置，后三个关节确定机器人的姿态，将第六各关节坐标系建立在机器人的腕关节处。
第一关节角的求解如下图所示：

对应的matlab代码如下：
首先定义好机器人的姿态关系：

n1 = -1;   % 'l'
 n2 = -1;   % 'u'
 n4 = -1;   % 'n'
 if sol(1)==1%~isempty(strfind(configuration, 'l'))
     n1 = -1;
 end
 if sol(1)==2%~isempty(strfind(configuration, 'r'))
     n1 = 1;
 end
 if sol(2)==1%~isempty(strfind(configuration, 'u'))
     if n1 == 1
         n2 = 1;
     else
         n2 = -1;
     end
 end
 if sol(2)==2%~isempty(strfind(configuration, 'd'))
     if n1 == 1
         n2 = -1;
     else
         n2 = 1;
     end
 end
 if sol(3)==1%~isempty(strfind(configuration, 'n'))
     n4 = 1;
 end
 if sol(3)==2%~isempty(strfind(configuration, 'f'))
     n4 = -1;
 end

姿态描述如下：

theta1的逆解代码为：

 r = sqrt(Px^2 + Py^2);
 if sol(1) == 1
     theta(1) = atan2(Py,Px) + pi - asin(d3/r);
 else
     theta(1) = atan2(Py,Px) + asin(d3/r);
 end
 if theta(1)>pi
      theta(1) = theta(1)-2*pi;
 end

第二关节的求解如下图所示：

theta2的逆解代码为：

V114 = Px*cos(theta(1)) + Py*sin(theta(1))-a1;
r = sqrt(V114^2 + Pz^2);
Psi = acos((a2^2-d4^2-a3^2+V114^2+Pz^2)/(2.0*a2*r));
if ~isreal(Psi)
    theta = [];
else
    
    theta(2) = atan2(Pz,V114) + n2*Psi;

第三关节的求解如下图示：

theta3的逆解代码为：

num = cos(theta(2))*V114+sin(theta(2))*Pz-a2;
den = cos(theta(2))*Pz - sin(theta(2))*V114;
theta(3) = atan2(a3,d4) - atan2(num, den);

以上是我的推导以及参考机器人工具箱中的代码写的，暂时没有没有发现问题，如果大家在参考的过程中发现了问题，欢迎批评指正。