Matlab建立六自由度机器人模型
近期更新
【汇总】
【Matlab 六自由度机器人】系列文章汇总 \fcolorbox{green}{aqua}{【Matlab 六自由度机器人】系列文章汇总 } 【Matlab 六自由度机器人】系列文章汇总
【主线】
运 动 学 \color{red}运动学 运动学
动 力 学 \color{red}动力学 动力学
（待补充）
【补充说明】
- 关于灵活工作空间与可达工作空间的理解
- 关于改进型D-H参数(modified Denavit-Hartenberg)的详细建立步骤
- 关于旋转的参数化（欧拉角、姿态角、四元数）的相关问题
- 关于双变量函数atan2(x,y)的解释
- 关于机器人运动学反解的有关问题
前言
1.Matlab机器人工具箱
由于我们需要接触Matlab上的机器人工具箱，因此首先先放上下载工具箱的官网，不需要积分，直接从官网上下载，不用担心会出问题。https://petercorke.com/resources/downloads/
相关资源： 若发现无法正常登录该网址，可通过该资源进行下载机器人工具箱压缩包：Matlab机器人工具箱-robot-10.3.1及相关实例.
具体的安装方法已经有朋友说得很清楚了：Matlab机器人工具箱
2.研究对象—六自由度机器人
调出机器人的各连杆参数，根据各连杆的长度和结构可以得出机器人的D-H参数。
随着人工智能的发展，机器人也不断变换着形态出现在人们的眼里，有的表现为餐厅的服务型机器人、有的表现为智能汽车。无一不在昭示着机器人的多用途以及它的多功能，而最典型最基础的的莫过于工业机器人。
正文
一、D-H参数(Denavit–Hartenberg parameters)
机械臂的一个重要特征参数就是DH参数。
坐标系建立步骤：
- 确定Z轴 如果关节是旋转的，Z轴按右手定则大拇指指向为正方向，关节转角θ为关节变量。如果关节是移动的，Z轴定在沿直线运动方向的正方向，连杆偏移d为关节变量。
- 确定X轴 情况一：两关节Z轴既不平行也不相交，即呈异面直线时。则取两Z轴公垂线方向作为X轴方向。 情况二：两关节Z轴平行。此时，两Z轴之间有无数条公垂线，可挑选与前一关节的公垂线共线的一条公垂线。
情况三：两关节Z轴相交。则取两条Z轴的叉积方向作为X轴（叉积：向量积） - 确定Y轴 通过右手定则确定Y轴的方向。大拇指指向Z轴的方向，以x轴为准，逆时针旋转90°的方向为y轴的方向。
对于标准型和改进型，区别在于固连坐标系不同以及执行变换的的顺序不同。
1.标准型D-H参数(STD)
- 对于标准型来说，X轴的方向以当前的Z轴和前一个关节的Z轴（也就是Zi-1轴）的叉乘方向，右手定则：由Zi-1轴转向Z轴，大拇指方向即为X轴方向。也可以使用简便的方法，由Zi-1轴指向Z轴，公垂线方向即是X轴的方向。附上更加直观的视频：机器人学DH参数的最直观讲解机器人入门必备
- 建模步骤：
（ 1 ） 绕 z i − 1 轴 旋 转 θ i ， 使 得 x i − 1 和 x i 平 行 ； （1）绕z_{i-1}轴旋转θ_i，使得x_{i-1}和x_{i}平行； （1）绕zi−1轴旋转θi，使得xi−1和xi平行；
（ 2 ） 沿 z i − 1 轴 平 移 d i ， 使 得 x i − 1 和 x i 重 合 ； （2）沿z_{i-1}轴平移d_{i}，使得x_{i-1}和x_{i}重合； （2）沿zi−1轴平移di，使得xi−1和xi重合；
（ 3 ） 沿 x i 轴 平 移 a i ， 使 得 z i − 1 和 z i 重 合 ； （3）沿x_{i}轴平移a_{i}，使得z_{i-1}和z_{i}重合； （3）沿xi轴平移ai，使得zi−1和zi重合；
（ 4 ） 绕 x i 轴 旋 转 α i ， 使 得 z i − 1 和 z i 共 线 ； （4）绕x_{i}轴旋转α_{i}，使得z_{i-1}和z_{i}共线； （4）绕xi轴旋转αi，使得zi−1和zi共线； - STD-DH方法变换时四个参数相乘的顺序依次为 θ θ θ→ d d d→ a a a→ α α α
- 通过每个旋转和平移的步骤得到旋转矩阵
R o t ( z i − 1 , θ i ) Rot(z_{i-1},θ_i) Rot(zi−1,θi) = [ c o s θ i − s i n θ i 0 0 s i n θ i c o s θ i 0 0 0 0 1 0 0 0 0 1 ] \left[ \begin{matrix} cosθ_i & -sinθ_i & 0&0 \\ sinθ_i & cosθ_i & 0 &0\\ 0 & 0& 1& 0\\ 0 & 0& 0& 1 \end{matrix} \right] ⎣⎢⎢⎡cosθisinθi00−sinθicosθi0000100001⎦⎥⎥⎤
T r a n s ( z i − 1 ， d i ) = Trans(z_{i-1}，d_i)= Trans(zi−1，di)= [ 1 0 0 0 0 1 0 0 0 0 1 d i 0 0 0 1 ] \left[ \begin{matrix} 1&0&0&0 \\ 0&1&0 &0\\ 0&0&1&d_{i} \\ 0&0&0&1 \end{matrix} \right] ⎣⎢⎢⎡10000100001000di1⎦⎥⎥⎤
T r a n s ( x i ， a i ) = Trans(x_i，a_i)= Trans(xi，ai)= [ 1 0 0 a i 0 1 0 0 0 0 1 0 0 0 0 1 ] \left[ \begin{matrix} 1&0&0&a_{i} \\ 0&1&0 &0\\ 0&0&1&0 \\ 0&0&0&1 \end{matrix} \right] ⎣⎢⎢⎡100001000010ai001⎦⎥⎥⎤
R o t ( x i ， α i ) = Rot(x_i，α_i)= Rot(xi，αi)= [ 1 0 0 0 0 c o s α i − s i n α i 0 0 s i n α i c o s α i 0 0 0 0 1 ] \left[ \begin{matrix} 1& 0 & 0&0 \\ 0& cosα_i & -sinα_i &0\\ 0& sinα_i & cosα_i& 0\\ 0& 0& 0& 1 \end{matrix} \right] ⎣⎢⎢⎡10000cosαisinαi00−sinαicosαi00001⎦⎥⎥⎤
可 以 得 到 变 换 矩 阵 i − 1 T i = R o t ( z i − 1 , θ i ) × T r a n s ( z i − 1 ， d i ) × T r a n s ( x i ， a i ) × R o t ( x i ， α i ) 可以得到变换矩阵^{i-1}T_i =Rot(z_{i-1},θ_i)×Trans(z_{i-1}，d_i)×Trans(x_i，a_i)×Rot(x_i，α_i) 可以得到变换矩阵i−1Ti=Rot(zi−1,θi)×Trans(zi−1，di)×Trans(xi，ai)×Rot(xi，αi)
i − 1 T i = ^{i-1}T_i = i−1Ti= [ c o s θ i − s i n θ i c o s α i s i n θ i s i n α i a i c o s θ i s i n θ i c o s θ i c o s α i − c o s θ i s i n α i a i s i n θ i 0 s i n α i c o s α i d i 0 0 0 1 ] \left[ \begin{matrix} cosθ_i&-sinθ_icosα_i&sinθ_isinα_i&a_{i}cosθ_i \\ sinθ_i&cosθ_icosα_i&-cosθ_isinα_i&a_{i}sinθ_i\\ 0&sinα_i&cosα_i&d_{i} \\ 0&0&0&1 \end{matrix} \right] ⎣⎢⎢⎡cosθisinθi00−sinθicosαicosθicosαisinαi0sinθisinαi−cosθisinαicosαi0aicosθiaisinθidi1⎦⎥⎥⎤
2.改进型D-H参数(MOD)
- 对于改进型来说， X X X轴的方向以当前的Z轴和后一个关节的 Z Z Z轴（也就是 Z i + 1 Z_{i+1} Zi+1轴）的叉乘方向，使用右手定则：由Z轴转向 Z i + 1 Z_{i+1} Zi+1轴，大拇指方向即为 X X X轴方向。具体如何建立查看上述的建立步骤，关于 X X X轴确定的三种情况。
- 建模步骤：
（ 1 ） 绕 x i 轴 旋 转 α i ， 使 得 z i 和 z i + 1 共 线 ； （1）绕x_{i}轴旋转α_{i}，使得z_{i}和z_{i+1}共线； （1）绕xi轴旋转αi，使得zi和zi+1共线；
（ 2 ） 沿 x i 轴 平 移 a i ， 使 得 z i 和 z i + 1 重 合 （2）沿x_{i}轴平移a_{i}，使得z_{i}和z_{i+1}重合 （2）沿xi轴平移ai，使得zi和zi+1重合
（ 3 ） 绕 z i 轴 旋 转 θ i ， 使 得 x i − 1 和 x i 共 线 ； （3）绕z_{i}轴旋转θ_i，使得x_{i-1}和x_{i}共线； （3）绕zi轴旋转θi，使得xi−1和xi共线；
（ 4 ） 沿 z i 轴 平 移 d i ， 使 得 x i − 1 和 x i 重 合 ； （4）沿z_{i}轴平移d_{i}，使得x_{i-1}和x_{i}重合； （4）沿zi轴平移di，使得xi−1和xi重合； - MOD-DH方法变换时四个参数相乘的顺序依次为 α α α→ a a a→ θ θ θ→ d d d
- 参考正文1.4可得到变换矩阵
i − 1 T i = R o t ( x i ， α i − 1 ) × T r a n s ( x i ， a i − 1 ) × R o t ( z i , θ i ) × T r a n s ( z i ， d i ) ^{i-1}T_i =Rot(x_{i}，α_{i-1})×Trans(x_{i}，a_{i-1})×Rot(z_{i},θ_i)×Trans(z_{i}，d_i) i−1Ti=Rot(xi，αi−1)×Trans(xi，ai−1)×Rot(zi,θi)×Trans(zi，di)
i − 1 T i = ^{i-1}T_i = i−1Ti= [ c o s θ i − s i n θ i 0 a i − 1 s i n θ i c o s α i − 1 c o s θ i c o s α i − 1 − s i n α i − 1 − s i n α i − 1 d i s i n θ i s i n α i − 1 c o s θ i s i n α i − 1 c o s α i − 1 c o s α i − 1 d i 0 0 0 1 ] \left[ \begin{matrix} cosθ_i&-sinθ_i&0&a_{i-1} \\ sinθ_icosα_{i-1}&cosθ_icosα_{i-1}&-sinα_{i-1}&-sinα_{i-1}d_{i} \\ sinθ_isinα_{i-1}&cosθ_isinα_{i-1}&cosα_{i-1}&cosα_{i-1}d_{i} \\ 0&0&0&1 \end{matrix} \right] ⎣⎢⎢⎡cosθisinθicosαi−1sinθisinαi−10−sinθicosθicosαi−1cosθisinαi−100−sinαi−1cosαi−10ai−1−sinαi−1dicosαi−1di1⎦⎥⎥⎤
D-H参数是研究各类型机器人的必经之路，只有熟悉构建机器人的DH参数才能更好得对机器人进行正逆解的求解。
二、Link函数与SerialLink函数
Link函数包含与机器人关节和链接相关的所有信息，例如运动学参数，刚体惯性参数，电机和传输参数。
Link函数的各类参数如下：
| 运动学参数 | 含义 |
|---|---|
| theta | joint angle 关节转角 |
| d | link offset 连杆偏移 |
| a | link length 连杆长度 |
| alpha | link twist 连杆扭角 |
| jointtype | ‘R’ if revolute, ‘P’ if prismatic 旋转关节为R，移动关节为P |
| mdh | 0 if standard D&H, else 1 标准型为0，改进型为1 |
| offset | joint variable offset 关节变量偏移 |
| qlim | joint variable limits [min max] 关节变量极限[min max] |
| 动力学参数 | 含义 |
|---|---|
| m | link mass 连杆质量 |
| r | link COG wrt link coordinate frame 3x1 连杆相对于坐标系的质心位置 3x1位置矩阵 |
| I | link inertia matrix, symmetric 3x3, about link COG.连杆重心相对于坐标系的惯性矩阵 3x3旋转矩阵 |
| B | link viscous friction (motor referred) 连杆粘性摩擦力(以电机为基准) |
| Tc | link Coulomb friction 连杆库仑摩擦力 |
| G | gear ratio 齿轮比(传动比) |
| Jm | motor inertia (motor referred)电机惯量(以电机为基准) |
对于只需要把机器人模型建立出来，不考虑机器人的动力学参数的情况，那么只需要用到theta、d、a、alpha和offset等几个参数即可。
接下来使用Link函数对D-H参数表进行模型的建立，并通过使用SerialLink函数来将L1 L2 L3 L4 L5 L6连杆连接起来。
对于Link函数来说，无论是标准型还是改进型，参数的顺序都为：关节转角、连杆偏移、连杆长度、连杆扭角。
1.使用标准型D-H参数建立机器人模型
%% STD-DH参数
%定义连杆的D-H参数
%连杆偏移
d1 = 398;
d2 = -0.299;
d3 = 0;
d4 = 556.925;
d5 = 0;
d6 = 165;
%连杆长度
a1 = 168.3;
a2 = 650.979;
a3 = 156.240;
a4 = 0;
a5 = 0;
a6 = 0;
%连杆扭角
alpha1 = pi/2;
alpha2 = 0;
alpha3 = pi/2;
alpha4 = -pi/2;
alpha5 = pi/2;
alpha6 = 0;
%建立机器人模型
% theta d a alpha
L1=Link([0 d1 a1 alpha1]);
L2=Link([0 d2 a2 alpha2]);L2.offset = pi/2;
L3=Link([0 d3 a3 alpha3]);
L4=Link([0 d4 a4 alpha4]);
L5=Link([0 d5 a5 alpha5]);
L6=Link([0 d6 a6 alpha6]);
%限制机器人的关节空间
L1.qlim = [(-165/180)*pi,(165/180)*pi];
L2.qlim = [(-95/180)*pi, (70/180)*pi];
L3.qlim = [(-85/180)*pi, (95/180)*pi];
L4.qlim = [(-180/180)*pi,(180/180)*pi];
L5.qlim = [(-115/180)*pi,(115/180)*pi];
L6.qlim = [(-360/180)*pi,(360/180)*pi];
%连接连杆，机器人取名为myrobot
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','myrobot');
robot.plot([0,0,0,0,0,0]);%输出机器人模型，后面的六个角为输出时的theta姿态
robot.display();%打印出机器人D-H参数表
robot.teach;%展示机器人模型
hold on;
2.使用改进型D-H参数建立机器人模型
其中‘modified’的意思是该机器人使用改进型D-H参数表来建立模型。
%% MOD-DH参数
%定义连杆的D-H参数
%连杆偏移
d1 = 398;
d2 = -0.299;
d3 = 0;
d4 = 556.925;
d5 = 0;
d6 = 165;
%连杆长度
a1 = 0;
a2 = 168.3;
a3 = 650.979;
a4 = 156.240;
a5 = 0;
a6 = 0;
%连杆扭角
alpha1 = 0;
alpha2 = pi/2;
alpha3 = 0;
alpha4 = pi/2;
alpha5 = -pi/2;
alpha6 = pi/2;
%建立机器人模型
% theta d a alpha
L1=Link([0 d1 a1 alpha1 ],'modified');
L2=Link([0 d2 a2 alpha2 ],'modified');L2.offset = pi/2;
L3=Link([0 d3 a3 alpha3 ],'modified');
L4=Link([0 d4 a4 alpha4 ],'modified');
L5=Link([0 d5 a5 alpha5 ],'modified');
L6=Link([0 d6 a6 alpha6 ],'modified');
%限制机器人的关节空间
L1.qlim = [(-165/180)*pi,(165/180)*pi];
L2.qlim = [(-95/180)*pi, (70/180)*pi];
L3.qlim = [(-85/180)*pi, (95/180)*pi];
L4.qlim = [(-180/180)*pi,(180/180)*pi];
L5.qlim = [(-115/180)*pi,(115/180)*pi];
L6.qlim = [(-360/180)*pi,(360/180)*pi];
%连接连杆，机器人取名为myrobot
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','myrobot');
robot.plot([0,0,0,0,0,0]);%输出机器人模型，后面的六个角为输出时的theta姿态
robot.display();%打印出机器人D-H参数表
robot.teach;%展示机器人模型
hold on;
注解：L2.offset = pi/2的意思为第二根轴相对于第一根轴有90度的关节偏移量，也就是初始状态下连杆2对于连杆1来说有一个关节的偏置。
接下来看添加关节偏移量与不添加的区别：
添加入关节偏移量
不添加关节偏移量
可以看出关节偏置的添加与否对于机器人末端位置来说差别还是蛮大的