1. 理論知識
理論知識請參考：
- 機器人學導論++（原書第3版）_（美）HLHN+J.CRAIG著++貟超等譯
- 機器人學課程講義（丁燁）
- 機器人學課程講義（趙言正）
2. Matlab Robotics Toolbox安裝
上官網:
http://petercorke.com/wordpress/toolboxes/robotics-toolbox
Download RTB-10.3.1 mltbx format (23.2 MB) in MATLAB toolbox format (.mltbx)
將down下來的文件放到一般放untitled.m所在的文件夾內。打開MATLAB運行，顯示安裝完成即可。
不要下zip，里面的東西各種缺失並且亂七八糟，很難配。
該工具箱內的說明書是robot.pdf
也可查閱 “機器人工具箱簡介.ppt”
3. 機器人建模
本仿真程序仿照fanuc_M20ia機器人進行建模。
3.1 利用DH矩陣建立機器人模型（modified）
經測繪，用如下代碼建立DH矩陣
使用robot.teach（）函數，進行機器人示教
% 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
效果如下：
3.2 機器人參數設定
在做仿真計算時，需要設定各個關節的運動學與動力學參數
質量屬性可以在SolidWorks中指定材質后，在“評估-質量屬性”中查看
運動學參數：
% 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
效果如下：
從圖中解得的曲線證明：在笛卡爾坐標系內做速度的梯形插補，其末端執行器的軌跡是一條直線，速度是一個梯形，即為勻加速，勻速，勻減速過程。加速度在兩個值之間突變，機械臂存在振動和柔性沖擊。另外，關節空間內的曲線也出現尖銳的突變點，說明在這段范圍內機械臂的運動經過奇點附近，即使末端執行器的速度比較小，但是關節的角速度和角加速度卻急劇變化。
8. 關節控制
