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經過運動學反解，獲得各個關節的角度，並估算各個關節的角速度與角加速度的值，以及末端執行器的速度與加速度的大小。

代碼以下

%TrajectoryGeneration_p2p_Cartesian.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    
];


%在笛卡爾空間座標系內，根據梯形速度生成軌跡
% Cartesian trajectory between two poses
% tc = ctraj(T0, T1, n) is a Cartesian trajectory (4 4 n) from pose T0 to T1 with n
% points that follow a trapezoidal velocity profile along the path. The Cartesian trajectory
% is a homogeneous transform sequence and the last subscript being the point index, that
% is, T(:,:,i) is the i^th point along the path.

step = 40;
Tc = ctraj(p1,p2,step);

% 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.

% 顯示機器人姿態隨時間的變化
subplot(3,3,[1,4,7]);
q = zeros(step,6);
for i = 1:step
q(i,:) = robot.ikine(Tc(:,:,i));         
end
robot.plot(q);

qd = zeros(step-1,6);
for i = 2:step
    qd(i,1) = q(i,1)-q(i-1,1);
    qd(i,2) = q(i,2)-q(i-1,2);
    qd(i,3) = q(i,3)-q(i-1,3);
    qd(i,4) = q(i,4)-q(i-1,4);
    qd(i,5) = q(i,5)-q(i-1,5);
    qd(i,6) = q(i,6)-q(i-1,6);
end 

qdd = zeros(step-2,6);
for i = 2:step-1
    qdd(i,1) = qd(i,1)-qd(i-1,1);
    qdd(i,2) = qd(i,2)-qd(i-1,2);
    qdd(i,3) = qd(i,3)-qd(i-1,3);
    qdd(i,4) = qd(i,4)-qd(i-1,4);
    qdd(i,5) = qd(i,5)-qd(i-1,5);
    qdd(i,6) = qd(i,6)-qd(i-1,6);
end 
%顯示機器人關節運動狀態
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
plot3(Tc(1,4,i),Tc(2,4,i),Tc(3,4,i),'b.','MarkerSize',5);
end

%顯示末端執行器的速度
subplot(3,3,6);
hold on
grid on
title('末端執行器速度大小隨時間的變化');
vel_velocity = zeros(step,1);
for i=2:step
vel_velocity(i) = sqrt((Tc(1,4,i)-Tc(1,4,i-1))^2+(Tc(2,4,i)-Tc(2,4,i-1))^2+(Tc(3,4,i)-Tc(3,4,i-1))^2);
end
x = linspace(1,step,step);
plot(x,vel_velocity);

%顯示末端執行器的加速度
subplot(3,3,9);
hold on
grid on
title('末端執行器加速度大小隨時間的變化');
vel_acceleration= zeros(step-2,1);
for i=3:step
vel_acceleration(i-2) = sqrt((Tc(1,4,i)-Tc(1,4,i-1)-(Tc(1,4,i-1)-Tc(1,4,i-2))   )^2+( Tc(2,4,i)-Tc(2,4,i-1)-(Tc(2,4,i-1)-Tc(2,4,i-2)) )^2+( Tc(3,4,i)-Tc(3,4,i-1)-(Tc(3,4,i-1)-Tc(3,4,i-2)))^2);
end
x = linspace(1,step-2,step-2);
plot(x,vel_acceleration);

%EOF

效果以下：

從圖中解得的曲線證實：在笛卡爾座標系內作速度的梯形插補，其末端執行器的軌跡是一條直線，速度是一個梯形，即爲勻加速，勻速，勻減速過程。加速度在兩個值之間突變，機械臂存在振動和柔性衝擊。另外，關節空間內的曲線也出現尖銳的突變點，說明在這段範圍內機械臂的運動通過奇點附近，即便末端執行器的速度比較小，可是關節的角速度和角加速度卻急劇變化。