目录
1. Matlab机器人工具箱
2. 创建MDH单机械臂
3. 创建MDH双臂机器人
UR构型双臂
如何进行轨迹仿真
4. MDH-双臂机器人
1. Matlab机器人工具箱
官方网站Robotics Toolbox | Peter Corke
下载，使用Matlab打开安装即可
机械臂文档SerialLink
2. 创建MDH单机械臂
clear;
clc;
%建立机器人模型
% theta d a alpha offset
% L1=Link([0 0.4 0.025 pi/2 0 -pi/2 ]); %定义连杆的D-H参数
% L2=Link([pi/2 0 0.56 0 0 pi/2 ]);
% L3=Link([0 0 0.035 pi/2 0 0 ]);
% L4=Link([0 0.515 0 pi/2 0 0 ]);
% L5=Link([pi 0 0 pi/2 0 0 ]);
% L6=Link([0 0.08 0 0 0 0 ]);
% theta | d | a | alpha | type| offset |
L(1)=Link([0 -72 150 0 0 pi/2 ],'modified'); % 关节1这里的最后一个量偏置
L(2)=Link([0 0 22 pi/2 0 -pi/2 ],'modified');
L(3)=Link([0 0 285 0 0 -pi/2 ],'modified');
L(4)=Link([0 220 3.5 -pi/2 0 0 ],'modified');
robot=SerialLink(L,'name',''); %连接连杆，机器人取名manman
robot.plot([-pi/2,-10*pi/180,-12*pi/180,0]);%输出机器人模型，后面的角为输出时的theta姿态
其中，Link、Seriallink等函数可参考官方API说明SerialLink
参数解释
classdef Link < matlab.mixin.Copyable
properties
% kinematic parameters
theta % kinematic: link angle
d % kinematic: link offset
alpha % kinematic: link twist
a % kinematic: link length
jointtype % revolute='R', prismatic='P' -- should be an enum
mdh % standard DH=0, MDH=1
offset % joint coordinate offset
name % joint coordinate name
flip % joint moves in opposite direction
% dynamic parameters
m % dynamic: link mass
r % dynamic: position of COM with respect to link frame (3x1)
I % dynamic: inertia of link with respect to COM (3x3)
Jm % dynamic: motor inertia
B % dynamic: motor viscous friction (1x1 or 2x1)
Tc % dynamic: motor Coulomb friction (1x2 or 2x1)
G % dynamic: gear ratio
qlim % joint coordinate limits (2x1)
end
创建实例
% Examples::
% A standard Denavit-Hartenberg link
% L3 = Link('d', 0.15005, 'a', 0.0203, 'alpha', -pi/2);
% since 'theta' is not specified the joint is assumed to be revolute, and
% since the kinematic convention is not specified it is assumed 'standard'.
%
% Using the old syntax
% L3 = Link([ 0, 0.15005, 0.0203, -pi/2], 'standard');
% the flag 'standard' is not strictly necessary but adds clarity. Only 4 parameters
% are specified so sigma is assumed to be zero, ie. the joint is revolute.
%
% L3 = Link([ 0, 0.15005, 0.0203, -pi/2, 0], 'standard');
% the flag 'standard' is not strictly necessary but adds clarity. 5 parameters
% are specified and sigma is set to zero, ie. the joint is revolute.
%
% L3 = Link([ 0, 0.15005, 0.0203, -pi/2, 1], 'standard');
% the flag 'standard' is not strictly necessary but adds clarity. 5 parameters
% are specified and sigma is set to one, ie. the joint is prismatic.
%
% For a modified Denavit-Hartenberg revolute joint
% L3 = Link([ 0, 0.15005, 0.0203, -pi/2, 0], 'modified');
3. 创建MDH双臂机器人
参考知乎专栏：OpenRobotSL - 知乎、Matlab双臂机器人建模仿真 - 知乎
实际上，如果就把双臂中的每个臂当做单个的机械臂进行规划，那么用matlab进行双臂建模就没有太大必要，只需要对每个单臂进行单独规划即可。但是，如果涉及到双臂之间的协同轨迹规划，如上图所示，这时用matlab进行双臂建模仿真就会显得事半功倍。本文先只介绍双臂在matlab中的正运动学建模，后续会补充在matlab中如何进行双臂轨迹规划。
具体创建双臂机器人过程如下文所示，详细请移步该文章查看Matlab双臂机器人建模仿真 - 知乎
此时腰关节连杆坐标系就是基坐标系X0Y0Z0，此时的肩关节坐标系X2Y2Z2就是单臂机器人的基坐标系，然后就从肩关节坐标系开始建立单臂的DH坐标系，也可以认为是单独的腰关节连杆坐标系+单臂的DH坐标系，需要注意的是建立整体DH坐标系时腰关节与肩关节之间需要加-pi/2角度偏置，目的是将puma560构型的机械臂垂下去，下图中的虚线X2即为不加偏置的肩关节坐标系，实线X2即为加了偏置角度后的肩关节坐标系,d=肩宽/2。
对应的matlab正运动学仿真如下所示：
腰关节与肩关节之间也可以不加偏置角度，这样的话，两个坐标系之间的转换就可以画成如下所示。
对应的matlab正运动学仿真如下所示：
UR构型双臂
本文UR采用标准DH建模，建模过程全网可搜，标准DH坐标系建立如下。
分析上图，黑色坐标系X0Y0Z0是世界坐标系，也可认为这是腰关节坐标系，红色虚线坐标系X1Y1Z1是肩关节坐标系（也就是UR的基坐标系{0}），此时腰关节与肩关节不存在角度偏置，matlab仿真建模就是如下所示
亮蓝色实线坐标系X1Y1Z1是加了pi/2偏置角度后的肩关节坐标系，matlab建模仿真如下所示
如何进行轨迹仿真
进行轨迹仿真的最重要一点就是，如何将世界（全局坐标系）坐标系下（也为腰关节坐标系）的轨迹映射到肩关节坐标系（机械臂的基坐标系）下，弄明白这一点，使用matlab进行双臂轨迹算法验证就简单很多了~
以PUMA560腰关节与肩关节有-pi/2角度偏置的双臂构型为例，如下图所示，
世界（全局，也为腰关节）坐标系下的位姿表示为$^0_{n}T$，这就是我们算法验证时所给定的轨迹位姿，都是相对于世界坐标系的；现在的问题就是，将轨迹位姿映射到肩关节（机械臂基坐标系）下，也就是求$^2_{n}T$。这个就很简单了，即$^2_{n}T=inv(^0_{2}T)^0_{n}T$。$^0_{2}T$即为单臂基坐标系相对于腰关节坐标系的姿态变换矩阵，根据腰关节和肩关节之间的坐标变换，可以分析得出，腰关节坐标系先绕X1轴（自己的）旋转pi/2，再绕动轴Z1旋转-pi/2，最后再沿着动轴Z1移动d长度，故$^0_{2}T=trotx(90)trotz(-90)*transz(d)$。按照上述步骤即可将全局坐标系下的位姿映射到单臂局部坐标系下了。
4. MDH-双臂机器人
4轴双臂机器人MDH参数表格
代码示例
% 2021年9月9日
% 双臂飞行机器人，机械臂4Dof运动学模型及工作空间绘制
% haowanghk@gmail.com
clear all;
clc;
% theta | d | a | alpha | type| offset |
L(1)=Link([0 -0.072 0.150 0 0 pi/2 ],'modified'); % 关节1这里的最后一个量偏置
L(2)=Link([0 0 0.022 pi/2 0 -pi/2 ],'modified');
L(3)=Link([0 0 0.285 0 0 -pi/2 ],'modified');
L(4)=Link([0 0.22 0.0035 -pi/2 0 0 ],'modified');
% L(5)=Link([0 0 0 -pi/2 0 ],'modified');
% L(6)=Link([0 0 0 pi/2 0 ],'modified');
% 0.262
p560L=SerialLink(L,'name','LEFT');
p560L.tool=[0 -1 0 0;
1 0 0 0;
0 0 1 0 ;
0 0 0 1;];
R(1)=Link([0 -0.072 -0.15 0 0 pi/2 ],'modified'); % 关节1这里的最后一个量偏置
R(2)=Link([0 0 0.022 pi/2 0 -pi/2 ],'modified');
R(3)=Link([0 0 0.285 0 0 -pi/2 ],'modified');
R(4)=Link([0 0.22 0.0035 -pi/2 0 0 ],'modified');
% R(5)=Link([0 0 0 -pi/2 0 ],'modified');
% R(6)=Link([0 0 0 pi/2 0 ],'modified');
% 0.262
p560R=SerialLink(R,'name','RIGHT');
p560R.tool=[0 -1 0 0;
1 0 0 0;
0 0 1 0 ;
0 0 0 1;];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% platform
platform=SerialLink([0 0 0 0],'name','platform','modified');%虚拟腰部关节
platform.base=[1 0 0 0;
0 1 0 0;
0 0 1 0 ;
0 0 0 1;]; %基座高度
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% R
pR=SerialLink([platform,p560R],'name','R'); % 单独右臂模型，加装底座
pR.display();
view(3)
hold on
grid on
axis([-1.5, 1.5, -1.5, 1.5, -1.0, 1.5])
pR.plot([0 pi/4 pi/4 0 0]) % 第一个量固定为0，目的是为了模拟腰关节，左臂下同
hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% L
pL=SerialLink([platform,p560L],'name','L'); % 单独左臂模型，加装底座
pL.display();
pL.plot([0 -pi/4 pi/4 0 0])
hold on右臂连杆参数表格
左臂连杆参数表格
创建结果
示教
