幾何学的手法に基づくロボットインバースキネマティクスソリューション-産業用ロボットの最初の3つの関節

産業用ロボットの逆運動学を解くために幾何学的方法を使用することは、空間内のロボットの結び目をいくつかの直交する平面の下で幾何学的解法に変換することであり、これは単純で直感的な効果があります。ただし、ロボットの空間位置や姿勢が複数の関節角度を持っている場合、幾何学的手法を用いた解法では十分な考慮が得られず、解法の結果が低下する可能性があります。しかし、その利点と直感性に基づいて、私はまだ私のソリューションの例をあなたと共有できると思います。産業用ロボットのストーブリを例にとると、ロボットの最初の3つの関節がロボットの位置を決定し、最後の3つの関節がロボットの姿勢を決定し、6番目の関節座標系がロボットの手首関節に確立されます。 。
最初の関節角度の解を次の図に示します。

対応する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

2番目のジョイントのソリューションを次の図に示します

。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;

3番目のジョイントの解は次のように示されます

。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);

上記は私の派生であり、ロボットツールボックスのコードを参照して書かれています。当面問題は見つかりませんでした。参照プロセス中に問題が見つかった場合は、私を批判して修正してください。