MATLAB机器人工具箱【1】——建模+正逆运动学+雅克比矩阵

MATLAB机器人工具箱【1】—— 机械臂建模+正逆运动学+雅克比矩阵

  • 1. 二维空间位姿描述
  • 2. 三维空间位姿描述
  • 3. 建立机器人模型
    • 3.1 Link 类
    • 3.2 SerialLink 类
    • 3.3 建立机器人模型(展示)
  • 4. 正逆运动学+雅克比矩阵
    • 4.1 求解正运动学方程
    • 4.2 正运动学方程正确性验证
    • 4.3 求解逆运动学方程
    • 4.4 逆运动学方程正确性验证
    • 4.5 雅克比矩阵
  • 5. 参考资料

本文在参考B站up主刘海涛大佬的视频分享基础上,结合自己学习的机器人学知识,利用MATLAB机器人工具箱加深理解和运用,机器人工具箱安装教程见Robotic toolbox 工具箱的安装和初步了解,安装了工具箱后就可以用来愉快的学习啦。

1. 二维空间位姿描述

MATLAB机器人工具箱【1】——建模+正逆运动学+雅克比矩阵_第1张图片

2. 三维空间位姿描述

MATLAB机器人工具箱【1】——建模+正逆运动学+雅克比矩阵_第2张图片

3. 建立机器人模型

根据自己的机器人模型实际参数采用标准DH法或者改进DH法建模,得到DH参数表,然后利用工具箱建模,下面是建模时必须要用到的两个类函数。

3.1 Link 类

MATLAB机器人工具箱【1】——建模+正逆运动学+雅克比矩阵_第3张图片

3.2 SerialLink 类

MATLAB机器人工具箱【1】——建模+正逆运动学+雅克比矩阵_第4张图片

3.3 建立机器人模型(展示)

关于puma560的DH参数表有很多版本,可以参考PUMA560机器人D-H参数和改进DH参数

这里以这一版本为例【和《机器人学》——蔡自兴版本保持一致】:
MATLAB机器人工具箱【1】——建模+正逆运动学+雅克比矩阵_第5张图片

%%改进D-H模型
%       关节角   连杆偏距    连杆长度  连杆转角   
%       theta(i) d(i)        a(i-1)   alpha(i-1)  offset
SL1=Link([0      0           0        0           0     ],'modified');
SL2=Link([0      0.14909     0        -pi/2       0     ],'modified');
SL3=Link([0      0           0.4318   0           0     ],'modified');
SL4=Link([0      0.43307     0.02032  -pi/2       0     ],'modified');
SL5=Link([0      0           0        pi/2        0     ],'modified');
SL6=Link([0      0           0        -pi/2       0     ],'modified');
p560=SerialLink([SL1 SL2 SL3 SL4 SL5 SL6],'name','puma560');%SerialLink 类函数

%通过手动输入各个连杆转角,模型会自动运动到相应位置
%方法一:
% p560.teach([-pi/2 -pi/2 0 0 0 0]);
%方法二:
p560.plot([-pi/2 -pi/2 0 0 0 0]);

MATLAB机器人工具箱【1】——建模+正逆运动学+雅克比矩阵_第6张图片

在MATLAB的机器人工具箱中提供了puma560机械臂模型,运行 mdl_puma560 命令,会发现自动生成四个1*6的关节角矩阵,他们分别代表以下状态:
MATLAB机器人工具箱【1】——建模+正逆运动学+雅克比矩阵_第7张图片

4. 正逆运动学+雅克比矩阵

MATLAB机器人工具箱【1】——建模+正逆运动学+雅克比矩阵_第8张图片
MATLAB机器人工具箱【1】——建模+正逆运动学+雅克比矩阵_第9张图片

4.1 求解正运动学方程

MATLAB机器人工具箱【1】——建模+正逆运动学+雅克比矩阵_第10张图片
【小技巧:相邻连杆变换矩阵第一列第三个元素如果是零的话,说明选择的是标准DH建模法,否则(就像上图所示)是改进DH法】

自己编写的正运动学函数:

function T06 = puma560_fk(theta1,theta2,theta3,theta4,theta5,theta6)

    a2 = 0.4318;
    a3 = 0.02032;
    d2 = 0.14909;
    d4 = 0.43307;
    
    T01 = [cos(theta1)  -sin(theta1)    0   0;
           sin(theta1)  cos(theta1)     0   0;
           0            0               1   0;
           0            0               0   1];
       
    T12 = [cos(theta2)  -sin(theta2)    0   0;
           0            0               1   d2;
           -sin(theta2) -cos(theta2)    0   0;
           0            0               0   1];
       
    T23 = [cos(theta3)  -sin(theta3)    0   a2;
           sin(theta3)  cos(theta3)     0   0;
           0            0               1   0;
           0            0               0   1];
       
    T34 = [cos(theta4)  -sin(theta4)    0   a3;
           0            0               1   d4;
           -sin(theta4) -cos(theta4)    0   0;
           0            0               0   1];
       
    T45 = [cos(theta5)  -sin(theta5)    0   0;
           0            0               -1  0;
           sin(theta5)  cos(theta5)     0   0;
           0            0               0   1];
       
    T56 = [cos(theta6)  -sin(theta6)    0   0;
           0            0               -1  0;
           -sin(theta6) -cos(theta6)    0   0;
           0            0               0   1];
       
    T06 = T01*T12*T23*T34*T45*T56;
end

求解自己的正运动学公式:

%求正运动学公式
T06 = puma560_fk(theta1,theta2,theta3,theta4,theta5,theta6)

结果:

T06 =
 
[   sin(theta6)*(cos(theta4)*sin(theta1) + sin(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) + cos(theta6)*(cos(theta5)*(sin(theta1)*sin(theta4) - cos(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) - sin(theta5)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2))), cos(theta6)*(cos(theta4)*sin(theta1) + sin(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) - sin(theta6)*(cos(theta5)*(sin(theta1)*sin(theta4) - cos(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) - sin(theta5)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2))), sin(theta5)*(sin(theta1)*sin(theta4) - cos(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) + cos(theta5)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2)), (2159*cos(theta1)*cos(theta2))/5000 - (14909*sin(theta1))/100000 - (127*cos(theta1)*sin(theta2)*sin(theta3))/6250 + (127*cos(theta1)*cos(theta2)*cos(theta3))/6250 - (43307*cos(theta1)*cos(theta2)*sin(theta3))/100000 - (43307*cos(theta1)*cos(theta3)*sin(theta2))/100000]
[ - sin(theta6)*(cos(theta1)*cos(theta4) - sin(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))) - cos(theta6)*(cos(theta5)*(cos(theta1)*sin(theta4) + cos(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))) + sin(theta5)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2))), sin(theta6)*(cos(theta5)*(cos(theta1)*sin(theta4) + cos(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))) + sin(theta5)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2))) - cos(theta6)*(cos(theta1)*cos(theta4) - sin(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))), cos(theta5)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2)) - sin(theta5)*(cos(theta1)*sin(theta4) + cos(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))), (14909*cos(theta1))/100000 + (2159*cos(theta2)*sin(theta1))/5000 - (43307*cos(theta2)*sin(theta1)*sin(theta3))/100000 - (43307*cos(theta3)*sin(theta1)*sin(theta2))/100000 - (127*sin(theta1)*sin(theta2)*sin(theta3))/6250 + (127*cos(theta2)*cos(theta3)*sin(theta1))/6250]
[                                                                                                                                   sin(theta4)*sin(theta6)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)) - cos(theta6)*(sin(theta5)*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)) + cos(theta4)*cos(theta5)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2))),                                                                                                                                 sin(theta6)*(sin(theta5)*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)) + cos(theta4)*cos(theta5)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2))) + cos(theta6)*sin(theta4)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)),                                                                             cos(theta5)*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)) - cos(theta4)*sin(theta5)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)),                                                                                          (43307*sin(theta2)*sin(theta3))/100000 - (43307*cos(theta2)*cos(theta3))/100000 - (127*cos(theta2)*sin(theta3))/6250 - (127*cos(theta3)*sin(theta2))/6250 - (2159*sin(theta2))/5000]
[                                                                                                                                                                                                                                                                                                                                                                           0,                                                                                                                                                                                                                                                                                                                                                                         0,                                                                                                                                                                                                                         0,                                                                                                                                                                                                                                                                            1]

化简:

>> simplify(T06)
 
ans =
 
[   sin(theta6)*(cos(theta4)*sin(theta1) + sin(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) + cos(theta6)*(cos(theta5)*(sin(theta1)*sin(theta4) - cos(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) - sin(theta5)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2))), cos(theta6)*(cos(theta4)*sin(theta1) + sin(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) - sin(theta6)*(cos(theta5)*(sin(theta1)*sin(theta4) - cos(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) - sin(theta5)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2))), sin(theta5)*(sin(theta1)*sin(theta4) - cos(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) + cos(theta5)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2)), (2159*cos(theta1)*cos(theta2))/5000 - (14909*sin(theta1))/100000 - (127*cos(theta1)*sin(theta2)*sin(theta3))/6250 + (127*cos(theta1)*cos(theta2)*cos(theta3))/6250 - (43307*cos(theta1)*cos(theta2)*sin(theta3))/100000 - (43307*cos(theta1)*cos(theta3)*sin(theta2))/100000]
[ - sin(theta6)*(cos(theta1)*cos(theta4) - sin(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))) - cos(theta6)*(cos(theta5)*(cos(theta1)*sin(theta4) + cos(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))) + sin(theta5)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2))), sin(theta6)*(cos(theta5)*(cos(theta1)*sin(theta4) + cos(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))) + sin(theta5)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2))) - cos(theta6)*(cos(theta1)*cos(theta4) - sin(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))), cos(theta5)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2)) - sin(theta5)*(cos(theta1)*sin(theta4) + cos(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))), (14909*cos(theta1))/100000 + (2159*cos(theta2)*sin(theta1))/5000 - (43307*cos(theta2)*sin(theta1)*sin(theta3))/100000 - (43307*cos(theta3)*sin(theta1)*sin(theta2))/100000 - (127*sin(theta1)*sin(theta2)*sin(theta3))/6250 + (127*cos(theta2)*cos(theta3)*sin(theta1))/6250]
[                                                                                                                                                                                                                                sin(theta2 + theta3)*sin(theta4)*sin(theta6) - cos(theta6)*(cos(theta2 + theta3)*sin(theta5) + sin(theta2 + theta3)*cos(theta4)*cos(theta5)),                                                                                                                                                                                                                              sin(theta6)*(cos(theta2 + theta3)*sin(theta5) + sin(theta2 + theta3)*cos(theta4)*cos(theta5)) + sin(theta2 + theta3)*cos(theta6)*sin(theta4),                                                                                                                                           cos(theta2 + theta3)*cos(theta5) - sin(theta2 + theta3)*cos(theta4)*sin(theta5),                                                                                                                                                                                    - (2159*sin(theta2))/5000 - (127*116537^(1/2)*cos(theta2 + theta3 - atan(16/341)))/100000]
[                                                                                                                                                                                                                                                                                                                                                                           0,                                                                                                                                                                                                                                                                                                                                                                         0,                                                                                                                                                                                                                         0,                                                                                                                                                                                                                                                                            1]

正运动学方程一般写成这样:
MATLAB机器人工具箱【1】——建模+正逆运动学+雅克比矩阵_第11张图片
简洁表达成这样:
MATLAB机器人工具箱【1】——建模+正逆运动学+雅克比矩阵_第12张图片

4.2 正运动学方程正确性验证

验证自己编写的正运动学函数:

% 用自己编写的正运动学函数求解
T06 = puma560_fk(-pi/2,-pi/2,0,0,0,0)
% 用机器人工具箱提供的函数求解
T0_6 = p560.fkine([-pi/2 -pi/2 0 0 0 0])

对比运算结果,正运动学无误:

T06 =

    0.0000   -1.0000   -0.0000    0.1491
   -0.0000   -0.0000    1.0000   -0.4331
    1.0000         0    0.0000    0.4521
         0         0         0    1.0000

 

T0_6 = 
         0        -1         0    0.1491
         0         0        -1   -0.4331
         1         0         0    0.4521
         0         0         0         1

4.3 求解逆运动学方程

MATLAB机器人工具箱【1】——建模+正逆运动学+雅克比矩阵_第13张图片
求解按照《机器人学—蔡自兴》的步骤结合MATLAB就可以了,现在根据求解出来的各个关节角将逆运动学求解封装成函数:

function theta_Med=puma560_ik(T)
% a--连杆长度,d--连杆偏移量
	a2=0.4318;
    a3=0.02032;
    d2=0.14909;
    d4=0.43307;
    
     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);
    
% 为方便计算,定义的m系列向量
% 求解关节角1
	theta1_1 = atan2(py,px)-atan2(d2,sqrt(px^2+py^2-d2^2));
	theta1_2 = atan2(py,px)-atan2(d2,-sqrt(px^2+py^2-d2^2));
% 求解关节角3
	m3_1 = (px^2+py^2+pz^2-a2^2-a3^2-d2^2-d4^2)/(2*a2);
	theta3_1 = atan2(a3,d4)-atan2(m3_1,sqrt(a3^2+d4^2-m3_1^2));
	theta3_2 = atan2(a3,d4)-atan2(m3_1,-sqrt(a3^2+d4^2-m3_1^2));
% 求解关节角2
    ms2_1 = -((a3+a2*cos(theta3_1))*pz)+(cos(theta1_1)*px+sin(theta1_1)*py)*(a2*sin(theta3_1)-d4);
    mc2_1 = (-d4+a2*sin(theta3_1))*pz+(cos(theta1_1)*px+sin(theta1_1)*py)*(a2*cos(theta3_1)+a3);
    theta23_1 = atan2(ms2_1,mc2_1);
    theta2_1 = theta23_1 - theta3_1;
    
    ms2_2 = -((a3+a2*cos(theta3_1))*pz)+(cos(theta1_2)*px+sin(theta1_2)*py)*(a2*sin(theta3_1)-d4);
    mc2_2 = (-d4+a2*sin(theta3_1))*pz+(cos(theta1_2)*px+sin(theta1_2)*py)*(a2*cos(theta3_1)+a3);
    theta23_2 = atan2(ms2_2,mc2_2);
    theta2_2 = theta23_2 - theta3_1;
    
    ms2_3 = -((a3+a2*cos(theta3_2))*pz)+(cos(theta1_1)*px+sin(theta1_1)*py)*(a2*sin(theta3_2)-d4);
    mc2_3 = (-d4+a2*sin(theta3_2))*pz+(cos(theta1_1)*px+sin(theta1_1)*py)*(a2*cos(theta3_2)+a3);
    theta23_3 = atan2(ms2_3,mc2_3);
    theta2_3 = theta23_3 - theta3_2;

    ms2_4 = -((a3+a2*cos(theta3_2))*pz)+(cos(theta1_2)*px+sin(theta1_2)*py)*(a2*sin(theta3_2)-d4);
    mc2_4 = (-d4+a2*sin(theta3_2))*pz+(cos(theta1_2)*px+sin(theta1_2)*py)*(a2*cos(theta3_2)+a3);
    theta23_4 = atan2(ms2_4,mc2_4);
    theta2_4 = theta23_4 - theta3_2;    
% 求解关节角4
    ms4_1=-ax*sin(theta1_1)+ay*cos(theta1_1);
    mc4_1=-ax*cos(theta1_1)*cos(theta23_1)-ay*sin(theta1_1)*cos(theta23_1)+az*sin(theta23_1);
	theta4_1=atan2(ms4_1,mc4_1);
	
	ms4_2=-ax*sin(theta1_2)+ay*cos(theta1_2);
    mc4_2=-ax*cos(theta1_2)*cos(theta23_2)-ay*sin(theta1_2)*cos(theta23_2)+az*sin(theta23_2);
	theta4_2=atan2(ms4_2,mc4_2);
	
	ms4_3=-ax*sin(theta1_1)+ay*cos(theta1_1);
    mc4_3=-ax*cos(theta1_1)*cos(theta23_3)-ay*sin(theta1_1)*cos(theta23_3)+az*sin(theta23_3);
	theta4_3=atan2(ms4_3,mc4_3);
	
	ms4_4=-ax*sin(theta1_2)+ay*cos(theta1_2);
    mc4_4=-ax*cos(theta1_2)*cos(theta23_4)-ay*sin(theta1_2)*cos(theta23_4)+az*sin(theta23_4);
	theta4_4=atan2(ms4_4,mc4_4);
% 求解关节角5
	ms5_1=-ax*(cos(theta1_1)*cos(theta23_1)*cos(theta4_1)+sin(theta1_1)*sin(theta4_1))-ay*(sin(theta1_1)*cos(theta23_1)*cos(theta4_1)-cos(theta1_1)*sin(theta4_1))+az*(sin(theta23_1)*cos(theta4_1));
    mc5_1= ax*(-cos(theta1_1)*sin(theta23_1))+ay*(-sin(theta1_1)*sin(theta23_1))+az*(-cos(theta23_1));
    theta5_1=atan2(ms5_1,mc5_1);
    
	ms5_2=-ax*(cos(theta1_2)*cos(theta23_2)*cos(theta4_2)+sin(theta1_2)*sin(theta4_2))-ay*(sin(theta1_2)*cos(theta23_2)*cos(theta4_2)-cos(theta1_2)*sin(theta4_2))+az*(sin(theta23_2)*cos(theta4_2));
    mc5_2= ax*(-cos(theta1_2)*sin(theta23_2))+ay*(-sin(theta1_2)*sin(theta23_2))+az*(-cos(theta23_2));
    theta5_2=atan2(ms5_2,mc5_2);
  
	ms5_3=-ax*(cos(theta1_1)*cos(theta23_3)*cos(theta4_3)+sin(theta1_1)*sin(theta4_3))-ay*(sin(theta1_1)*cos(theta23_3)*cos(theta4_3)-cos(theta1_1)*sin(theta4_3))+az*(sin(theta23_3)*cos(theta4_3));
    mc5_3= ax*(-cos(theta1_1)*sin(theta23_3))+ay*(-sin(theta1_1)*sin(theta23_3))+az*(-cos(theta23_3));
    theta5_3=atan2(ms5_3,mc5_3);
    
	ms5_4=-ax*(cos(theta1_2)*cos(theta23_4)*cos(theta4_4)+sin(theta1_2)*sin(theta4_4))-ay*(sin(theta1_2)*cos(theta23_4)*cos(theta4_4)-cos(theta1_2)*sin(theta4_4))+az*(sin(theta23_4)*cos(theta4_4));
    mc5_4= ax*(-cos(theta1_2)*sin(theta23_4))+ay*(-sin(theta1_2)*sin(theta23_4))+az*(-cos(theta23_4));
    theta5_4=atan2(ms5_4,mc5_4);
% 求解关节角6
	ms6_1=-nx*(cos(theta1_1)*cos(theta23_1)*sin(theta4_1)-sin(theta1_1)*cos(theta4_1))-ny*(sin(theta1_1)*cos(theta23_1)*sin(theta4_1)+cos(theta1_1)*cos(theta4_1))+nz*(sin(theta23_1)*sin(theta4_1));
    mc6_1= nx*(cos(theta1_1)*cos(theta23_1)*cos(theta4_1)+sin(theta1_1)*sin(theta4_1))*cos(theta5_1)-nx*cos(theta1_1)*sin(theta23_1)*sin(theta4_1)+ny*(sin(theta1_1)*cos(theta23_1)*cos(theta4_1)+cos(theta1_1)*sin(theta4_1))*cos(theta5_1)-ny*sin(theta1_1)*sin(theta23_1)*sin(theta5_1)-nz*(sin(theta23_1)*cos(theta4_1)*cos(theta5_1)+cos(theta23_1)*sin(theta5_1));
	theta6_1=atan2(ms6_1,mc6_1);
	
	ms6_2=-nx*(cos(theta1_2)*cos(theta23_2)*sin(theta4_2)-sin(theta1_2)*cos(theta4_2))-ny*(sin(theta1_2)*cos(theta23_2)*sin(theta4_2)+cos(theta1_2)*cos(theta4_2))+nz*(sin(theta23_2)*sin(theta4_2));
    mc6_2= nx*(cos(theta1_2)*cos(theta23_2)*cos(theta4_2)+sin(theta1_2)*sin(theta4_2))*cos(theta5_2)-nx*cos(theta1_2)*sin(theta23_2)*sin(theta4_2)+ny*(sin(theta1_2)*cos(theta23_2)*cos(theta4_2)+cos(theta1_2)*sin(theta4_2))*cos(theta5_2)-ny*sin(theta1_2)*sin(theta23_2)*sin(theta5_2)-nz*(sin(theta23_2)*cos(theta4_2)*cos(theta5_2)+cos(theta23_2)*sin(theta5_2));
	theta6_2=atan2(ms6_2,mc6_2);
	
	ms6_3=-nx*(cos(theta1_1)*cos(theta23_3)*sin(theta4_3)-sin(theta1_1)*cos(theta4_3))-ny*(sin(theta1_1)*cos(theta23_3)*sin(theta4_3)+cos(theta1_1)*cos(theta4_3))+nz*(sin(theta23_3)*sin(theta4_3));
    mc6_3= nx*(cos(theta1_1)*cos(theta23_3)*cos(theta4_3)+sin(theta1_1)*sin(theta4_3))*cos(theta5_3)-nx*cos(theta1_1)*sin(theta23_3)*sin(theta4_3)+ny*(sin(theta1_1)*cos(theta23_3)*cos(theta4_3)+cos(theta1_1)*sin(theta4_3))*cos(theta5_3)-ny*sin(theta1_1)*sin(theta23_3)*sin(theta5_3)-nz*(sin(theta23_3)*cos(theta4_3)*cos(theta5_3)+cos(theta23_3)*sin(theta5_3));
	theta6_3=atan2(ms6_3,mc6_3);
	
	ms6_4=-nx*(cos(theta1_2)*cos(theta23_4)*sin(theta4_4)-sin(theta1_2)*cos(theta4_4))-ny*(sin(theta1_1)*cos(theta23_4)*sin(theta4_4)+cos(theta1_2)*cos(theta4_4))+nz*(sin(theta23_4)*sin(theta4_4));
    mc6_4= nx*(cos(theta1_2)*cos(theta23_4)*cos(theta4_4)+sin(theta1_2)*sin(theta4_4))*cos(theta5_4)-nx*cos(theta1_2)*sin(theta23_4)*sin(theta4_4)+ny*(sin(theta1_2)*cos(theta23_4)*cos(theta4_4)+cos(theta1_2)*sin(theta4_4))*cos(theta5_1)-ny*sin(theta1_2)*sin(theta23_4)*sin(theta5_4)-nz*(sin(theta23_4)*cos(theta4_4)*cos(theta5_4)+cos(theta23_4)*sin(theta5_4));
	theta6_4=atan2(ms6_4,mc6_4);
	
	
% 整理得到4组运动学非奇异逆解
	theta_Med_1 = [ 
                   theta1_1,theta2_1,theta3_1,theta4_1,theta5_1,theta6_1;
 				   theta1_2,theta2_2,theta3_1,theta4_2,theta5_2,theta6_2;
 				   theta1_1,theta2_3,theta3_2,theta4_3,theta5_3,theta6_3;
 				   theta1_2,theta2_4,theta3_2,theta4_4,theta5_4,theta6_4;
                 ];
% 将操作关节‘翻转’可得到另外4组解
theta_Med_2 = ...
    [ 
      theta1_1,theta2_1,theta3_1,theta4_1+pi,-theta5_1,theta6_1+pi;
 	  theta1_2,theta2_2,theta3_1,theta4_2+pi,-theta5_2,theta6_2+pi;
 	  theta1_1,theta2_3,theta3_2,theta4_3+pi,-theta5_3,theta6_3+pi;
 	  theta1_2,theta2_4,theta3_2,theta4_4+pi,-theta5_4,theta6_4+pi;
      ];
  theta_Med=[theta_Med_1;theta_Med_2];         
end

4.4 逆运动学方程正确性验证

%用自己编写的逆运动学函数求解
T06 = kinematics([-pi/2 -pi/2 0 0 0 0])
theta_med = puma560_ik(T06)

8组逆运动学解

theta_med =

   -1.5708   -1.5708   -0.0000         0         0         0
   -4.0493   -3.0986   -0.0000    2.4780    1.6047   -0.0265
   -1.5708   -0.0430   -3.0478         0    1.5201         0
   -4.0493    4.7124   -3.0478    1.6901    0.6687    1.4192
   -1.5708   -1.5708   -0.0000    3.1416         0    3.1416
   -4.0493   -3.0986   -0.0000    5.6196   -1.6047    3.1151
   -1.5708   -0.0430   -3.0478    3.1416   -1.5201    3.1416
   -4.0493    4.7124   -3.0478    4.8317   -0.6687    4.5608

其中第一组解就是我想要的结果:

   -1.5708   -1.5708   -0.0000         0         0         0

4.5 雅克比矩阵

机器人雅克比矩阵可以联系机器人末端操作空间速度和关节空间速度,已知关节空间速度,利用雅克比矩阵,可以得到操作空间的速度,再结合逆过程,就可以实现速度模式控制机器人,从而初步实现平滑运动。
在这里插入图片描述
公式中,v 和 w 是笛卡尔空间中的速度和角速度,并且 q(dot) 是一个关节速度向量。

%练习使用雅克比矩阵求解函数
qn = [0 0.7854 3.1416 0 0.7854 0];
%jacob0()求解的是将关节速度映射到世界坐标系中的末端执行器空间速度
p560.jacob0(qn)
%jaconb()求解的是将关节速度映射到工具坐标系中的末端执行器空间速度
p560.jacobe(qn)

结果:

jacob0 =

   -0.1491    0.0153    0.3206         0         0         0
    0.5972    0.0000    0.0000         0         0         0
   -0.0000   -0.5972   -0.2919         0         0         0
   -0.0000         0         0    0.7071         0    1.0000
         0    1.0000    1.0000    0.0000    1.0000    0.0000
    1.0000    0.0000    0.0000    0.7071    0.0000   -0.0000


jacobn =

   -0.0000   -0.5972   -0.2919         0         0         0
   -0.5972    0.0000    0.0000         0         0         0
   -0.1491    0.0153    0.3206         0         0         0
    1.0000         0         0    0.7071         0         0
   -0.0000   -1.0000   -1.0000   -0.0000   -1.0000         0
   -0.0000    0.0000    0.0000    0.7071    0.0000    1.0000

5. 参考资料

  • 基于Matlab的PUMA 560运动学与轨迹规划仿真
  • 博士论文《协作机器人零力控制与碰撞检测技术研究_陈赛旋》,有关于正逆运动学求解的详细推导过程
  • 论文《六自由度模块化机械臂的逆运动学分析》

你可能感兴趣的:(MATLAB机器人工具箱,matlab)