6軸機械臂（擬人臂+球形腕（三軸相交於一點））正逆運動學求解

1、DH座標下機械臂參數

theta=[pi/10,pi/2,pi/4,-pi/4,pi/4,-pi/8];%關節角度
a=[0, 0.260, 0.025,   0,    0,   0];%連桿長度
d=[0,  0,     0,  0.280,  0, 0.072];%偏移距離

alpha=[pi/2, 0, pi/2, -pi/2 , pi/2, 0];%RBR型6軸機械臂

使用MATLAB的機器人工具箱函數可以畫出該機械臂在指定關節角下的姿態，並且給出末端位置和姿態角，代碼如下：

​
L1 = Link('d', 0.340,      'a', 0,        'alpha', pi/2 ,'standard' );
L2 = Link('d', 0,          'a', 0.260,   'alpha',   0   ,'standard' );
L3 = Link('d', 0 ,         'a', 0.025,   'alpha', pi/2 ,'standard' );
L4 = Link('d', 0.280,      'a', 0,        'alpha',  -pi/2 ,'standard' );
L5 = Link('d', 0,          'a', 0,        'alpha', pi/2 ,'standard');
L6 = Link('d', 0.072,      'a', 0,        'alpha',   0   ,'standard');

theta=[-pi/2,pi*4/3,pi/4,pi/4,pi/4,-pi/4];%關節角度
STA_MZ204=SerialLink([L1,L2,L3,L4,L5,L6],'name','MZ204-STA');   %SerialLink 類函數
A = fkine(STA_MZ204,theta);//正運動學求解
figure(1),STA_MZ204.teach(theta,'eul');%eul爲以歐拉角顯示方向

效果如下：

二、正運動學求解（MATLAB代碼實現）

1、DH座標系下的T矩陣計算

 T=[cos(theta),-sin(theta)*cos(alpha),sin(theta)*sin(alpha),a*cos(theta);
        sin(theta),cos(theta)*cos(alpha),-cos(theta)*sin(alpha),a*sin(theta);
        0,sin(alpha),cos(alpha),d;
        0,0,0,1];

上面是一個軸的變換矩陣，那麼6軸機械臂的變換矩陣T06=T1*T2*T3*T4*T5*T6

2、根據T06矩陣求解末端位置和方向角，方向角爲歐拉角（繞ZYZ軸旋轉）和方向角（繞XYZ軸旋轉）的求解方法不同

末端位置:px=T06(1,4),py=T06(2,4),pz=T06(3,4)

繞ZYZ軸旋轉求解：

     r11 = T(1,1);
     r21 = T(2,1);
     r31 = T(3,1);
     r12 = T(1,2);
     r22 = T(2,2);
     r32 = T(3,2);
     r13 = T(1,3);
     r23 = T(2,3);
     r33 = T(3,3);
     px = T(1,4);
     py = T(2,4);
     pz = T(3,4);
    beta_beta = atan2(sqrt(r31*r31+r32*r32),r33) ;%繞Y
    beta_alpha = atan2(r23/sin(beta_beta),r13/sin(beta_beta));%先繞Z
    beta_yeta = atan2(r32/sin(beta_beta),-r31/sin(beta_beta));%再繞z
    pos=[px,py,pz,beta_alpha,beta_beta,beta_yeta];

繞XYZ軸旋轉求解：

     nx = T(1,1);
     ny = T(2,1);
     nz = T(3,1);
%      ox = T(1,2);
%      oy = T(2,2);
     oz = T(3,2);
%      ax = T(1,3);
%      ay = T(2,3);
     az = T(3,3);
     px = T(1,4);
     py = T(2,4);
     pz = T(3,4);
    beta_y = atan2(nz,sqrt(nx*nx+ny*ny)) ;
    beta_z = atan2(ny/cos(beta_y),nx/cos(beta_y));
    beta_x = atan2(oz/cos(beta_y),az/cos(beta_y));
    pos=[px,py,pz,beta_x,beta_y,beta_z];

三、逆運動學求解

逆運動學求解是由給定的末端姿態（位置+方向）求出各個關節的角度，下面將詳細敘述求解步驟

1、以ZYZ軸爲例，根據末端姿態pos（px,py,pz,theta_a,theta_b,theta_y）求解腕部位置（pwx,pwy,pwz），即三軸交點的位置。

這裏要注意的一點是我令d1爲0，如果你的機械臂d1不爲0的話，你可以將d1等效爲0求解，只需要將pz減去d1之後計算即可

 pwx=px-cos(theta_a)*sin(theta_b)*d6;
 pwy=py-sin(theta_a)*sin(theta_b)*d6;
 pwz=pz-cos(theta_b)*d6;

2、根據腕部位置可以求解出前三個關節的角度，因爲腕部的位置是由前三個軸決定的，與後三個軸無關，腕部位置等於第三個軸的末端位置，（令a3=d4，而d4=0）具體可以參考“機器人學建模規劃與控制”這本書,不過這種方法只適用於機械臂的a3=0。當a3不爲0時，我還是建議使用數學推導，根據具體情況來求解

      上面的說法讓你有些糊塗的話，我們可以直接用數學推導來說明，當你令d6=0時，機械臂的末端位置便是腕部位置，計算出d6=0時的px,py,pz如下：

px =d4*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2)) + a2*cos(theta1)*cos(theta2) + a3*cos(theta1)*cos(theta2)*cos(theta3) - a3*cos(theta1)*sin(theta2)*sin(theta3)
 
 
py =d4*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2)) + a2*cos(theta2)*sin(theta1) + a3*cos(theta2)*cos(theta3)*sin(theta1) - a3*sin(theta1)*sin(theta2)*sin(theta3)
 
 
pz =a2*sin(theta2) - d4*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)) + a3*cos(theta2)*sin(theta3) + a3*cos(theta3)*sin(theta2)
 

從上面的px,py,pz計算結果我們可以發現，他只與theta1,theta2,theta3相關

我們對其進行化簡令cos(theta2)=c2,cos(theta2+theta3)=c23得

pwx=px=d4c1s23+c1(a2c2+a3c23)
pwy=py=d4s1s23+s1(a2c2+a3c23)
pwz=pz=a2s2+a3s23-d4c23

從上面可以推出前三個關節的角度theta3,theta2,theta1,主要是一些三角函數推導計算，就不詳細說明了，直接上程序

pos2(1)爲pwx,pos2(2)爲pwy,pos2(3)爲pwz
%關節角3求解
m3 = (pos2(1)^2+pos2(2)^2+pos2(3)^2-a(2)^2-a(3)^2-d(4)^2)/(2*a(2))
n3 = sqrt(d(4)^2+a(3)^2)
theta3(1,1) = atan2(m3/n3,sqrt(1-m3^2/n3^2))-atan2(a(3),d(4));
theta3(1,2) = atan2(m3/n3,-sqrt(1-m3^2/n3^2))-atan2(a(3),d(4));

%關節角2求解
m2(1,1:2) = a(2)+a(3)*cos(theta3)+d(4)*sin(theta3)%m2(1)--theta3(1),m2(2)--theta3(2)
n2(1,1:2) = -a(3)*sin(theta3)+d(4)*cos(theta3)%n2(1)--theta3(1),n2(2)--theta3(2)

theta2(1,1:2)=atan2(n2,m2)+atan2(pos2(3),sqrt(n2.*n2+m2.*m2-pos2(3)*pos2(3)));%theta2(1,1)--theta3(1,1),theta2(1,2)--theta3(1,2)
theta2(1,3:4)=atan2(n2,m2)+atan2(pos2(3),-sqrt(n2.*n2+m2.*m2-pos2(3)*pos2(3)));%theta2(1,3)--theta3(1,1),theta2(1,4)--theta3(1,2)
test2=sqrt(n2.*n2+m2.*m2-pos2(3)*pos2(3))
test3=atan2(pos2(3),-sqrt(n2.*n2+m2.*m2-pos2(3)*pos2(3)))
%關節角1求解
theta1(1,1)=atan2(pos2(2),pos2(1));

if(pos2(2)>=0)
    theta1(1,2)=atan2(pos2(2),pos2(1))-pi; 
else
    theta1(1,2)=atan2(pos2(2),pos2(1))+pi;   
end

3、根據計算出來的theta1,theta2,theta3,我們可以計算出R03，同時根據位姿我們可以計算出R06，那麼就可以求解出R36，

R03是T03=T01*T12*T23的一部分（前三行，三列，表示方向變換部分），同理R06也是如此。

下面給出R03的計算代碼

R03(1,1)=cos(theta1).*cos(theta2).*cos(theta3) - cos(theta1).*sin(theta2).*sin(theta3);
R03(2,1)=cos(theta2).*cos(theta3).*sin(theta1) - sin(theta1).*sin(theta2).*sin(theta3);
R03(3,1)=cos(theta2).*sin(theta3) + cos(theta3).*sin(theta2);
    
     
R03(1,2)= sin(theta1);
R03(2,2)=-cos(theta1);
R03(3,2)=0;
   
     
R03(1,3)=cos(theta1).*cos(theta2).*sin(theta3) + cos(theta1).*cos(theta3).*sin(theta2);
R03(2,3)=cos(theta2).*sin(theta1).*sin(theta3) + cos(theta3).*sin(theta1).*sin(theta2);
R03(3,3)=sin(theta2).*sin(theta3) - cos(theta2).*cos(theta3);

根據末端姿態pos計算R06的代碼如下

 R06(1,1)=cos(pos(6))*cos(pos(5))*cos(pos(4))-sin(pos(4))*sin(pos(6));
 R06(2,1)=cos(pos(6))*cos(pos(5))*sin(pos(4))+cos(pos(4))*sin(pos(6));
 R06(3,1)=-sin(pos(5))*cos(pos(6));

 R06(1,2)=-sin(pos(6))*cos(pos(5))*cos(pos(4))-cos(pos(6))*sin(pos(4));
 R06(2,2)=-sin(pos(6))*cos(pos(5))*sin(pos(4))+cos(pos(6))*cos(pos(4));
 R06(3,2)=sin(pos(5))*sin(pos(6));
 
 R06(1,3)=cos(pos(4))*sin(pos(5));
 R06(2,3)=sin(pos(4))*sin(pos(5));
 R06(3,3)=cos(pos(5));
   

因爲R06=R03*R36;則R36=INV(R03)*R06,在MATLAB裏可以用R36=R03/R06來計算。

接下來講如何根據R36求解theta4,theta5,theta6

首先我們看一下R36的數學表達式

     R36(1,1)=cos(theta4)*cos(theta5)*cos(theta6) - sin(theta4)*sin(theta6);
     R36(2,1)=cos(theta4)*sin(theta6) + cos(theta5)*cos(theta6)*sin(theta4);
     R36(3,1)= -cos(theta6)*sin(theta5);
    
     
     R36(1,2)= - cos(theta6)*sin(theta4) - cos(theta4)*cos(theta5)*sin(theta6);
     R36(2,2)= cos(theta4)*cos(theta6) - cos(theta5)*sin(theta4)*sin(theta6);
     R36(3,2)= sin(theta5)*sin(theta6);
   
     
     R36(1,3)=cos(theta4)*sin(theta5);
     R36(2,3)=sin(theta4)*sin(theta5);
     R36(3,3)=cos(theta5);

根據R36的表達式我們便可以求出theta4,theta5,theta6

具體代碼如下：

    %theta5(0,pi),
    theta4(1,i)=atan2(R36(2,3),R36(1,3));
    theta5(1,i)=atan2(sqrt(R36(2,3)^2+R36(1,3)^2),R36(3,3));
    theta6(1,i)=atan2(R36(3,2),-R36(3,1));

    %theta5(-pi,0),
    theta4(2,i)=atan2(-R36(2,3),-R36(1,3));
    theta5(2,i)=atan2(-sqrt(R36(2,3)^2+R36(1,3)^2),R36(3,3));
    theta6(2,i)=atan2(-R36(3,2),R36(3,1));

 4、注意theta1,theta2,theta3的解一共有四組，分別是

        //theta1(1,1)，theta2(1,1)，theta3(1,1)
        //theta1(1,1)，theta2(1,2)，theta3(1,2)
        //theta1(1,2)，theta2(1,3)，theta3(1,1)
        //theta1(1,2)，theta2(1,4)，theta3(1,2)
    每一組解對應的theta4,theta5,theta6有兩組解

  所以逆運動學的解一共有8組