Matlab Robotics Toolbox 仿真计算：Kinematics, Dynamics, Trajectory Generation

时间 2019-12-14

标签 matlab robotics toolbox 仿真计算 kinematics dynamics trajectory generation 栏目 MATLAB 繁體版

原文原文链接

1. 理论知识

理论知识请参考：算法

机器人学导论++（原书第3版）_（美）HLHN+J.CRAIG著++贠超等译
机器人学课程讲义（丁烨）
机器人学课程讲义（赵言正）

2. Matlab Robotics Toolbox安装

上官网:
http://petercorke.com/wordpress/toolboxes/robotics-toolboxexpress

Download RTB-10.3.1 mltbx format (23.2 MB) in MATLAB toolbox format (.mltbx)
将down下来的文件放到通常放untitled.m所在的文件夹内。打开MATLAB运行，显示安装完成便可。wordpress

不要下zip，里面的东西各类缺失而且乱七八糟，很难配。函数

该工具箱内的说明书是robot.pdf
也可查阅 “机器人工具箱简介.ppt”工具

3. 机器人建模

本仿真程序仿照fanuc_M20ia机器人进行建模。ui

3.1 利用DH矩阵创建机器人模型（modified）

经测绘，用以下代码创建DH矩阵
使用robot.teach（）函数，进行机器人示教spa

% RobotTeach.m

clc;
%             theta   d         a        alpha     offset
ML1 =  Link([ 0,      0.4967,   0,       0,        0], 'modified');
ML2 =  Link([ -pi/2,  -0.18804, 0.2,     3*pi/2,   0], 'modified');
ML3 =  Link([ 0,      0.17248,  0.79876, 0 ,       0], 'modified');
ML4 =  Link([ 0,      0.98557,  0.25126, 3*pi/2,   0], 'modified');
ML5 =  Link([ 0,      0,        0,       pi/2 ,    0], 'modified');
ML6 =  Link([ 0,      0,        0,       pi/2 ,    0], 'modified');

robot = SerialLink([ML1 ML2 ML3 ML4 ML5 ML6],'name','Fanuc M20ia');
robot.teach(); %能够自由拖动的关节角度

% EOF

效果以下：
code

3.2 机器人参数设定

在作仿真计算时，须要设定各个关节的运动学与动力学参数
质量属性能够在SolidWorks中指定材质后，在“评估-质量属性”中查看orm

运动学参数：blog

%  theta    关节角度
%  d        连杆偏移量
%  a        连杆长度
%  alpha    连杆扭角
%  sigma    旋转关节为0，移动关节为1
%  mdh      标准的D&H为0，不然为1
%  offset   关节变量偏移量
%  qlim     关节变量范围[min max]

动力学参数：

%  m        连杆质量
%  r        连杆相对于坐标系的质心位置3x1
%  I        连杆的惯性矩阵（关于连杆重心）3x3
%  B        粘性摩擦力(对于电机)1x1或2x1
%  Tc       库仑摩擦力1x1或2x1

电机和传动参数：

%  G        齿轮传动比
%  Jm       电机惯性矩(对于电机)

完整的机器人建模代码

clear;
clc;
%             theta   d         a        alpha     offset
ML1 =  Link([ 0,      0.4967,   0,       0,        0], 'modified');
ML2 =  Link([ -pi/2,  -0.18804, 0.2,     3*pi/2,   0], 'modified');
ML3 =  Link([ 0,      0.17248,  0.79876, 0 ,       0], 'modified');
ML4 =  Link([ 0,      0.98557,  0.25126, 3*pi/2,   0], 'modified');
ML5 =  Link([ 0,      0,        0,       pi/2 ,    0], 'modified');
ML6 =  Link([ 0,      0,        0,       pi/2 ,    0], 'modified');

%配置机器人参数
ML1.m = 20.8;
ML2.m = 17.4;
ML3.m = 4.8;
ML4.m = 0.82;
ML5.m = 0.34;
ML6.m = 0.09;

ML1.r = [ 0    0    0 ];
ML2.r = [ -0.3638  0.006    0.2275];
ML3.r = [ -0.0203  -0.0141  0.070];
ML4.r = [ 0    0.019    0];
ML5.r = [ 0    0        0];
ML6.r = [ 0    0        0.032];

ML1.I = [  0         0.35         0         0         0         0];
ML2.I = [  0.13      0.524        0.539     0         0         0];
ML3.I = [  0.066     0.086        0.0125    0         0         0];
ML4.I = [  1.8e-3  1.3e-3  1.8e-3  0         0         0];
ML5.I = [  0.3e-3  0.4e-3  0.3e-3   0         0         0];
ML6.I = [  0.15e-3 0.15e-3 0.04e-3  0         0         0];

ML1.Jm =  200e-6;
ML2.Jm =  200e-6;
ML3.Jm =  200e-6;
ML4.Jm =  33e-6;
ML5.Jm =  33e-6;
ML6.Jm =  33e-6;

ML1.G =  -62.6111;
ML2.G =  107.815;
ML3.G =  -53.7063;
ML4.G =  76.0364;
ML5.G =  71.923;
ML6.G =  76.686;

% viscous friction (motor referenced)
ML1.B =   1.48e-3;
ML2.B =   0.817e-3;
ML3.B =   1.38e-3;
ML4.B =   71.2e-6;
ML5.B =   82.6e-6;
ML6.B =   36.7e-6;

% Coulomb friction (motor referenced)
ML1.Tc = [ 0.395        -0.435];
ML2.Tc = [ 0.126        -0.071];
ML3.Tc = [ 0.132        -0.105];
ML4.Tc = [ 11.2e-3      -16.9e-3];
ML5.Tc = [ 9.26e-3      -14.5e-3];
ML6.Tc = [ 3.96e-3      -10.5e-3];

robot=SerialLink([ML1 ML2 ML3 ML4 ML5 ML6],'name','Fanuc M20ia');% 注意：这句话最后写，否则会报错

4. 正向运动学与机器人工做空间的求取

4.1 正向运动学

串联链式操做器的正向运动学问题，是在给定全部关节位置和全部连杆几何参数的状况下，求取末端相对于基座的位置和姿态。
末端执行器相对于基座的变换矩阵

(_6^0)T=(_1^0)T(_2^1)T(_3^2)T(_4^3)T(_5^4)T(_6^5)T

该变换矩阵是关于6个关节变量θ_i的函数，在给定一组θ下，机器人末端连杆在笛卡尔坐标系里的位置和姿态均可以计算获得。

4.2 采用蒙特卡洛法对机器人的工做空间进行仿真分析

机器人末端执行器可以到达的空间位置点的集合构成了其工做空间范围。如今采用蒙特卡洛法对机器人的工做空间进行分析。蒙特卡洛法是一种借助于随机抽样来解决数学问题的数值方法，具体求解步骤以下：
1）在机器人正运动学方程中，能够获得末端执行器在参考坐标系中相对基坐标系的位置向量。
2）根据机器人关节变量取值范围，在MATLAB中生成各关节变量随机值。
θi=θimin+（θimax-θimin）×RAND（N,1）
3）将全部关节变量的随机值代入运动学方程的位置向量中从而获得由随机点构成的云图，就构成了机器人的蒙特卡洛工做空间。

代码以下：

%ShowWorkspace.m

clear;
clc;
%             theta   d         a        alpha     offset
ML1 =  Link([ 0,      0.4967,   0,       0,        0], 'modified');
ML2 =  Link([ -pi/2,  -0.18804, 0.2,     3*pi/2,   0], 'modified');
ML3 =  Link([ 0,      0.17248,  0.79876, 0 ,       0], 'modified');
ML4 =  Link([ 0,      0.98557,  0.25126, 3*pi/2,   0], 'modified');
ML5 =  Link([ 0,      0,        0,       pi/2 ,    0], 'modified');
ML6 =  Link([ 0,      0,        0,       pi/2 ,    0], 'modified');

robot = SerialLink([ML1 ML2 ML3 ML4 ML5 ML6],'name','Fanuc M20ia');
robot.plot([0,0,0,0,0,0]);
hold on;

N=3000;                                              %随机次数
theta1min = -180/180*pi; theta1max = 180/180*pi;
theta2min = -180/180*pi; theta2max = 0/180*pi;
theta3min = -90/180*pi;  theta3max = 90/180*pi;
theta4min = -180/180*pi; theta4max = 180/180*pi;
theta5min = -135/180*pi; theta5max = 135/180*pi;
theta6min = -180/180*pi; theta6max = 180/180*pi;

theta1=theta1min+(theta1max-theta1min)*rand(N,1); %关节1限制
theta2=theta2min+(theta2max-theta2min)*rand(N,1); %关节2限制
theta3=theta3min+(theta3max-theta3min)*rand(N,1); %关节3限制
theta4=theta4min+(theta4max-theta4min)*rand(N,1); %关节4限制
theta5=theta5min+(theta5max-theta5min)*rand(N,1); %关节5限制
theta6=theta6min+(theta6max-theta6min)*rand(N,1); %关节6限制

for n=1:N
q = zeros(1,6);
q(1) = theta1(n);
q(2) = theta2(n);
q(3) = theta3(n);
q(4) = theta4(n);
q(5) = theta5(n);
q(6) = theta6(n);
modmyt06 = robot.fkine(q);
plot3(modmyt06.t(1),modmyt06.t(2),modmyt06.t(3),'b.','MarkerSize',0.5);
end

%EOF

效果以下图

点的疏密程度表明了机械臂末端的执行器出如今这个点的几率大小。

5. 逆向运动学

从ShowWorkspace.m的运行结果中提取两个末端执行器的位姿
注意：modmyt06中有四个向量t，n，o，a
T = [ n o a t ]
[ 0 0 0 1 ]

逆向运动学的理论知识参见机器人学课本。对于6自由度球腕关节的机械臂，必有解析解，能够用ikine6s（）反解，可是必须是标准DH描述下
因此采用ikine（）迭代法求解这两个位姿的逆向角度

%给定末端执行器的初始位置
p1 =[
0.617222144    0.465154659    -0.634561241    -0.254420286
-0.727874557   0.031367208    -0.684992502    -1.182407321
-0.298723039   0.884673523    0.357934776     -0.488241553
0              0              0               1
];

%给定末端执行器的终止位置
p2 = [
-0.504697849    -0.863267623    -0.007006569    0.664185871
-0.599843651    0.356504321     -0.716304589    -0.35718173
0.620860432     -0.357314539    -0.697752567    2.106929688
0               0               0               1    
];


%利用运动学反解ikine求解各关节转角
% Inverse kinematics by optimization without joint limits
% q = R.ikine(T) are the joint coordinates (1 N) corresponding to the robot end-effector
% pose T which is an SE3 object or homogenenous transform matrix (4 4), and N is the
% number of robot joints.

init_ang = robot.ikine(p1);%使用运动学迭代反解的算法计算获得初始的关节角度
targ_ang = robot.ikine(p2);%使用运动学迭代反解的算法计算获得目标关节角度

6. 正向动力学与逆向动力学

6.1 正向动力学

已知关节力肯定机械臂运动

qdd=p560.accel(qn,qz,ones(1,6)); %给定位置qn，速度qz，力矩ones(1,6 )，求加速度

6.2 逆向动力学

已知各个关节的角度，角速度和角加速度，以及各机械臂的运动参数，求取各关节的力矩

代码以下：

%Dynamics.m

%（...）机器人动力学建模部分略去

%给定末端执行器的初始位置
p1 =[
0.617222144    0.465154659    -0.634561241    -0.254420286
-0.727874557   0.031367208    -0.684992502    -1.182407321
-0.298723039   0.884673523    0.357934776     -0.488241553
0              0              0               1
];

%给定末端执行器的终止位置
p2 = [
-0.504697849    -0.863267623    -0.007006569    0.664185871
-0.599843651    0.356504321     -0.716304589    -0.35718173
0.620860432     -0.357314539    -0.697752567    2.106929688
0               0               0               1    
];

init_ang = robot.ikine(p1);%使用运动学迭代反解的算法计算获得初始的关节角度
targ_ang = robot.ikine(p2);%使用运动学迭代反解的算法计算获得目标关节角度

step=40;
[q,qd,qdd] = jtraj(init_ang, targ_ang, step);%关节空间内的S曲线插补法，q qd qdd分别为各个关节的角度，角速度和角加速度


% 已知关节的角度、角速度、角加速度等信息，求各关节所需提供的力矩
% Inverse dynamics
% tau = R.rne(q, qd, qdd, options) is the joint torque required for the robot R to achieve
% the specified joint position q (1 N), velocity qd (1 N) and acceleration qdd (1 N),
% where N is the number of robot joints.

W = [0 0 -20*9.8 20*9.8*0.2 0 0]; %外力负载

tau = robot.rne(q,qd,qdd,'fext',W);

i=1:6;
plot(tau(:,i));
xlabel('Time (s)');
ylabel('Joint torque (Nm)');
title('各关节力矩随时间的变化');
grid on;

%EOF

效果以下：

7. 轨迹生成

轨迹规划是根据做业任务要求事先规定机器人的操做顺序和动做过程，轨迹规划分为关节空间和笛卡尔空间轨迹规划。

肯定末端操做器的初始位置和目标位置
根据逆运动学求出各关节的初始角度和目标角度
估计规划，求出各关节的角度变化曲线
进行运动控制，使机器人按照轨迹规划结果运动

7.1 关节空间轨迹规划

思路：给定起始点p1，目标点p2, 利用运动学反解的获得q1，q2，在关节空间内利用五次多项式作插补，获得[q, qd, qdd]，再经过运动学正解，获得末端执行器的位置，线速度与角速度的值

代码以下

%TrajectoryGeneration_p2p_lnksps.m

clear;
clc;
%             theta   d         a        alpha     offset
ML1 =  Link([ 0,      0.4967,   0,       0,        0], 'modified');
ML2 =  Link([ -pi/2,  -0.18804, 0.2,     3*pi/2,   0], 'modified');
ML3 =  Link([ 0,      0.17248,  0.79876, 0 ,       0], 'modified');
ML4 =  Link([ 0,      0.98557,  0.25126, 3*pi/2,   0], 'modified');
ML5 =  Link([ 0,      0,        0,       pi/2 ,    0], 'modified');
ML6 =  Link([ 0,      0,        0,       pi/2 ,    0], 'modified');
robot = SerialLink([ML1 ML2 ML3 ML4 ML5 ML6],'name','Fanuc M20ia');

%给定末端执行器的初始位置
p1 =[
0.617222144    0.465154659    -0.634561241    -0.254420286
-0.727874557   0.031367208    -0.684992502    -1.182407321
-0.298723039   0.884673523    0.357934776     -0.488241553
0              0              0               1
];

%给定末端执行器的终止位置
p2 = [
-0.504697849    -0.863267623    -0.007006569    0.664185871
-0.599843651    0.356504321     -0.716304589    -0.35718173
0.620860432     -0.357314539    -0.697752567    2.106929688
0               0               0               1    
];


%利用运动学反解ikine求解各关节转角
% Inverse kinematics by optimization without joint limits
% q = R.ikine(T) are the joint coordinates (1 N) corresponding to the robot end-effector
% pose T which is an SE3 object or homogenenous transform matrix (4 4), and N is the
% number of robot joints.

init_ang = robot.ikine(p1);%使用运动学迭代反解的算法计算获得初始的关节角度
targ_ang = robot.ikine(p2);%使用运动学迭代反解的算法计算获得目标关节角度

%利用五次多项式计算关节速度和加速度
% Compute a joint space trajectory
% [q,qd,qdd] = jtraj(q0, qf, m) is a joint space trajectory q (m N) where the joint
% coordinates vary from q0 (1 N) to qf (1 N). A quintic (5th order) polynomial is used
% with default zero boundary conditions for velocity and acceleration. Time is assumed
% to vary from 0 to 1 in m steps. Joint velocity and acceleration can be optionally returned
% as qd (m N) and qdd (m N) respectively. The trajectory q, qd and qdd are m N
% matrices, with one row per time step, and one column per joint.
step=40;
[q,qd,qdd] = jtraj(init_ang, targ_ang, step);

% 显示机器人姿态随时间的变化
subplot(3,3,[1,4,7]);
robot.plot(q); 

%显示机器人关节运动状态
subplot(3,3,2);
i=1:6;
plot(q(:,i));
title('初始位置      各关节角度随时间的变化      目标位置');
grid on;
subplot(3,3,5);
i=1:6;
plot(qd(:,i));
title('各关节角速度随时间的变化');
grid on;
subplot(3,3,8);
i=1:6;
plot(qdd(:,i));
title('各关节角加速度随时间的变化');
grid on;

%显示末端执行器的位置
subplot(3,3,3);
hold on
grid on
title('末端执行器在三维空间中的位置变化');
for i=1:step
position = robot.fkine(q(i,:));
plot3(position.t(1),position.t(2),position.t(3),'b.','MarkerSize',5);
end

%显示末端执行器的线速度与角速度
% Jacobian in world coordinates
% j0 = R.jacob0(q, options) is the Jacobian matrix (6 N) for the robot in pose q (1 N),
% and N is the number of robot joints. The manipulator Jacobian matrix maps joint
% velocity to end-effector spatial velocity V = j0*QD expressed in the world-coordinate frame.
subplot(3,3,6);
hold on
grid on
title('末端执行器速度大小随时间的变化');
vel = zeros(step,6);
vel_velocity = zeros(step,1);
vel_angular_velocity = zeros(step,1);
for i=1:step
vel(i,:) = robot.jacob0(q(i,:))*qd(i,:)';
vel_velocity(i) = sqrt(vel(i,1)^2+vel(i,2)^2+vel(i,3)^2);
vel_angular_velocity(i) = sqrt(vel(i,4)^2+vel(i,5)^2+vel(i,3)^6);
end
x = linspace(1,step,step);
plot(x,vel_velocity);

subplot(3,3,9);
hold on
grid on
title('末端执行器角速度大小随时间的变化');
x = linspace(1,step,step);
plot(x,vel_angular_velocity);

%EOF

效果以下

从图中解得的曲线证实：在关节空间内进行五次多项式插补获得的机械臂关节运动学曲线较为平滑，并且末端执行器的速度和角速度随时间的变化都比较均匀而平滑。

7.2 笛卡尔空间轨迹规划

思路：给定起始点p1，目标点p2, 利用梯形速度插补获得末端坐标系位姿矩阵Tc随时间的变化，再对每个Tc经过运动学反解，获得各个关节的角度，并估算各个关节的角速度与角加速度的值，以及末端执行器的速度与加速度的大小。