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(&＃39;d&＃39;, 0.340, &＃39;a&＃39;, 0, &＃39;alpha&＃39;, pi/2 ,&＃39;standard&＃39; );
L2 = Link(&＃39;d&＃39;, 0, &＃39;a&＃39;, 0.260, &＃39;alpha&＃39;, 0 ,&＃39;standard&＃39; );
L3 = Link(&＃39;d&＃39;, 0 , &＃39;a&＃39;, 0.025, &＃39;alpha&＃39;, pi/2 ,&＃39;standard&＃39; );
L4 = Link(&＃39;d&＃39;, 0.280, &＃39;a&＃39;, 0, &＃39;alpha&＃39;, -pi/2 ,&＃39;standard&＃39; );
L5 = Link(&＃39;d&＃39;, 0, &＃39;a&＃39;, 0, &＃39;alpha&＃39;, pi/2 ,&＃39;standard&＃39;);
L6 = Link(&＃39;d&＃39;, 0.072, &＃39;a&＃39;, 0, &＃39;alpha&＃39;, 0 ,&＃39;standard&＃39;);theta=[-pi/2,pi*4/3,pi/4,pi/4,pi/4,-pi/4];%关节角度
STA_MZ204=SerialLink([L1,L2,L3,L4,L5,L6],&＃39;name&＃39;,&＃39;MZ204-STA&＃39;); %SerialLink 类函数
A = fkine(STA_MZ204,theta);//正运动学求解
figure(1),STA_MZ204.teach(theta,&＃39;eul&＃39;);%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) ;%绕Ybeta_alpha = atan2(r23/sin(beta_beta),r13/sin(beta_beta));%先绕Zbeta_yeta = atan2(r32/sin(beta_beta),-r31/sin(beta_beta));%再绕zpos=[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;
elsetheta1(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组