6轴机械臂（拟人臂+球形腕（三轴相交于一点））正逆运动学求解

最新推荐文章于 2024-05-15 03:21:25 发布

乔慕宾

最新推荐文章于 2024-05-15 03:21:25 发布

阅读量6.6k

点赞数 6

分类专栏： 6轴机械臂正逆运动学 matlab 文章标签： 6轴机械臂正逆运动学求解 MATLAB代码实现

本文链接：https://blog.csdn.net/qq_33374294/article/details/90713520

版权

matlab 同时被 2 个专栏收录

6 篇文章 1 订阅

订阅专栏

6轴机械臂正逆运动学

1 篇文章 0 订阅

订阅专栏

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组

乔慕宾

关注

6
点赞
踩
62

收藏

觉得还不错? 一键收藏
1
评论
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轴机械臂使用...
复制链接

扫一扫