機械臂機器人——（4）Robotics Toolbox機器人仿真

文章目錄

• • RoboticsToolbox基礎用法
  • • （1）二維空間位姿描述
    • • 二維空間位姿變換示例
    • （2）三維空間位姿描述
    • • 正交旋轉矩陣
      • 三維空間位姿變換示例
  • 2.三角度表示法
  • • （1）歐拉角
    • • 示例：歐拉角與旋轉矩陣的互相轉化
    • （2）RPY角
    • • 示例：RPY角與旋轉矩陣的互相轉化
    • （3）雙向量角表示法
    • • 示例：雙向量轉化為旋轉矩陣
    • （4）向量和旋轉角表示法
    • • 示例：向量旋轉角與旋轉矩陣的互相轉化
    • （5）機關四元數表示法
    • • 示例：四元數用法
  • 3.RoboticsToolbox建立機器人模型
  • • （1）建立機器人模型函數
    • • Link類函數
      • Seriallink類函數
    • （2）DH參數
    • • 标準DH參數
      • 改進DH參數
    • （3）改進DH參數，建立機器人模型步驟
    • • 示例
    • （4）标準DH參數模組化
    • （5）标準DH和改進DH的差別
  • 4.正逆運動學、軌迹規劃
  • • （1）正運動學
    • • puma560機械臂正運動學示例
    • （2）逆運動學
    • • puma560機械臂逆運動學示例
    • （3）軌迹規劃
    • • 示例：sawyer機器人軌迹規劃
  • 5.速度和靜力
  • • （1）雅可比矩陣
    • （2）分解速度運動控制

RoboticsToolbox基礎用法

（1）二維空間位姿描述

二維空間位姿變換示例

T1 = SE2(1,3,30,"deg");
trplot2(T1,"frame","1","color","b");
axis([0 5 0 5]);
T2=trans12(3, 4);
hold on;
trplot2(T2,"frame","2","color","r");
           

運作效果：

（2）三維空間位姿描述

正交旋轉矩陣

三維空間位姿變換示例

R1=rotx(30,"deg")*roty(50,"deg"); %繞x軸旋轉30°，再繞y軸旋轉50°
trplot(R1,"frame","A", "color", "b"); %畫出旋轉矩陣R1
tranimate(R1,"frame","A", "color", "b"); %将R1的變換做成動畫
R2=roty(50,"deg")*rotx(30,"deg"); %繞y軸旋轉50°，再繞x軸旋轉30°
hold on
trplot(R2,"frame","B", "color", "r"); %畫出旋轉矩陣R2
tranimate(R2,"frame","B", "color", "r"); %将R2的變換做成動畫
           

運作效果：

R1和R2是兩個完全不同的旋轉矩陣，說明R1和R2具有不可交性。

2.三角度表示法

（1）歐拉角

歐拉角是在

示例：歐拉角與旋轉矩陣的互相轉化

R3=rotz(0.1)*roty(0.2)*rotz(0.3); % 構造旋轉矩陣R3
R4=eul2r(0.1,0.2,0.3); % 歐拉角轉化為旋轉矩陣
eul=tr2eul(R3); % 旋轉矩陣轉化為歐拉角
           

（2）RPY角

RPY角是在固定坐标系A下，以固定的XYZ軸作為旋轉的基準。三個角分别為Row（回轉），Pitch（俯仰）和Yaw（偏轉），可以用右手坐标系表示，食指為Row，中指為Pitch，大拇指為Yaw。

示例：RPY角與旋轉矩陣的互相轉化

R5=rotz(0.3)*roty(0.2)*rotx(0.1); % 構造旋轉矩陣R5
R6=rpy2r(0.3,0.2,0.1); % rpy角轉化為旋轉矩陣
eul=tr2rpy(R5); % 旋轉矩陣轉化為rpy角
           

（3）雙向量角表示法

示例：雙向量轉化為旋轉矩陣

a=[1 0 0]';
o=[0 1 0]';
R7=oa2r(o,a); % 将雙向量o,a轉化為旋轉矩陣R7
           

（4）向量和旋轉角表示法

示例：向量旋轉角與旋轉矩陣的互相轉化

（5）機關四元數表示法

示例：四元數用法

s=0.98335;
v=[0.034271, 0.10602, 0.14357];
Q=UnitQuaternion(s,v); % 組成四元數
q=Q.inv(); % 求共轭
Q.display(); % 列印出四元數
Q.plot(); % 畫出出四元數
Q.animate(); % 動畫展示四元數
TT=Q.T; % 制作齊次變換矩陣
RR=Q.R; % 制作旋轉矩陣
rpy=Q.torpy(); % 轉換成rpy角
eul=Q.toeul(); % 轉換成eul角
           

3.RoboticsToolbox建立機器人模型

（1）建立機器人模型函數

Link類函數

Seriallink類函數

（2）DH參數

标準DH參數

改進DH參數

（3）改進DH參數，建立機器人模型步驟

示例

clc;
clear;
% 定義各個連杆以及關節類型，預設為轉動關節
%           theta      d        a        alpha
%               連杆偏距參數d 連杆長度參數 關節偏角參數alpha
L1=Link([     0        0        0          0], 'modified'); % [四個DH參數], options
L2=Link([    -pi/2   0.1925,   0.081     -pi/2], 'modified');
L3=Link([     0       0.4       0        -pi/2], 'modified');
L4=Link([     0      0.1685,    0        -pi/2], 'modified');
L5=Link([     0      0.4,       0         pi/2], 'modified');
L6=Link([     0      0.1363     0         pi/2], 'modified');
L7=Link([     0      0.13375    0        -pi/2], 'modified');
% 
b=isrevolute(L1);
robot=SerialLink([L1,L2,L3,L4,L5,L6,L7]); % 将七個連杆組成機械臂
robot.name='modified sawyer';
robot.display(); % 展示出
view(3); % 解決robot.teach()和plot的索引超出報錯
robot.teach();
robot.plot([0 -pi/2 0 0 0 0 0]);
           

（4）标準DH參數模組化

clc;
clear;
% 定義各個連杆以及關節類型，預設為轉動關節
%           theta      d        a        alpha
%               連杆偏距參數d 連杆長度參數 關節偏角參數alpha
L1=Link([     0        0        0          0], 'standard'); % [四個DH參數], options
L2=Link([    -pi/2   0.1925,   0.081     -pi/2], 'standard');
L3=Link([     0       0.4       0        -pi/2], 'standard');
L4=Link([     0      0.1685,    0        -pi/2], 'standard');
L5=Link([     0      0.4,       0         pi/2], 'standard');
L6=Link([     0      0.1363     0         pi/2], 'standard');
L7=Link([     0      0.13375    0        -pi/2], 'standard');
% 
b=isrevolute(L1);
robot=SerialLink([L1,L2,L3,L4,L5,L6,L7]); % 将七個連杆組成機械臂
robot.name='standard sawyer';
robot.display(); % 展示出
view(3); % 解決robot.teach()和plot的索引超出報錯
robot.teach();
robot.plot([0 -pi/2 0 0 0 0 0]);
           

（5）标準DH和改進DH的差別

4.正逆運動學、軌迹規劃

（1）正運動學

puma560機械臂正運動學示例

mdl_puma560; % 加載puma560模型
qz    % 零角度
qr    % 就緒狀态，機械臂甚至且垂直
qs    % 伸展狀态，機械臂伸直且水準
qn    % 标準狀态，機械臂處于靈巧工作狀态
view(3);
p560.plot(qn);
T=p560.fkine(qn);
           

（2）逆運動學

puma560機械臂逆運動學示例

（3）軌迹規劃

示例：sawyer機器人軌迹規劃

clc;
clear;
% 定義各個連杆以及關節類型，預設為轉動關節
%           theta      d        a        alpha
%               連杆偏距參數d 連杆長度參數 關節偏角參數alpha
L1=Link([     0        0        0          0], 'modified'); % [四個DH參數], options
L2=Link([    -pi/2   0.1925,   0.081     -pi/2], 'modified');
L3=Link([     0       0.4       0        -pi/2], 'modified');
L4=Link([     0      0.1685,    0        -pi/2], 'modified');
L5=Link([     0      0.4,       0         pi/2], 'modified');
L6=Link([     0      0.1363     0         pi/2], 'modified');
L7=Link([     0      0.13375    0        -pi/2], 'modified');
% 
b=isrevolute(L1);
robot=SerialLink([L1,L2,L3,L4,L5,L6,L7],'name','FANUC'); % 将七個連杆組成機械臂
robot.name='modified sawyer';
robot.display(); % 展示出

init_ang=[0 0 0 0 0 0 0];
targ_ang=[pi/4,-pi/3,pi/5,pi/2,-pi/4,pi/2,pi/3];
step=200;
[q,qd,qdd]=jtraj(init_ang,targ_ang,step); %關節空間規劃軌迹，得到機器人末端運動的[位置，速度，加速度]
T0=robot.fkine(init_ang); % 正運動學解算
Tf=robot.fkine(targ_ang);
subplot(2,4,3); i=1:7; plot(q(:,i)); title("位置"); grid on;
subplot(2,4,4); i=1:7; plot(qd(:,i)); title("速度"); grid on;
subplot(2,4,7); i=1:7; plot(qdd(:,i)); title("加速度"); grid on;

Tc=ctraj(T0,Tf,step);
Tjtraj=transl(Tc);
subplot(2,4,8); plot2(Tjtraj, 'r');
title('p1到p2直線軌迹'); grid on;
subplot(2,4,[1,2,5,6]);
plot3(Tjtraj(:,1),Tjtraj(:,2),Tjtraj(:,3),"b"); grid on;
hold on;
view(3); % 解決robot.teach()和plot的索引超出報錯
qq=robot.ikine(Tc);
robot.plot(qq);
           

5.速度和靜力

（1）雅可比矩陣

（2）分解速度運動控制