MATLAB机器人工具箱（二）机器人模型建立

理论基础（D-H）

此处理论基础取自《机器人导论》采用改进的Modifird DH参数模型
R代表转动副，P代表移动副
有N个关节（编号从1到N）机械臂具有N+1个连杆（编号从0到N）连杆0是机械臂基座，连杆N固连末端执行器或者工具，关节i将连杆i-1连接到连杆i上，因此关节i带动连杆i运动。

按照上述规定将连杆坐标系固连千连杆上时，连杆参数可以定义如下：

建立连杆坐标系步骤：
1找出各个关节轴，画出轴线的延长线
2找出关节轴i和i+1之间的公垂线或关节轴i和i+1的交点，以关节轴i和i+1的交点或公垂线与关节轴i的交点作为连杆坐标系的{i}原点
3规定z轴沿关节轴i的指向
4规定x轴沿公垂线指向，如果关节轴i和i+1相交，则规定x轴垂直于关节轴i和i+1所在的平面
5按右手定则确定Y轴
6当第一个关节变量为0时，规定坐标系{0}和{1}重合，对于坐标系{N}，原点和x方向可以任意选取，选取时，尽量使连杆参数为0.
从连杆坐标系{i-1}到坐标系{i}的变换被定义为基本旋转平移变换，可以写为：

也就是

一般表达式为：

Link

此部分内容取自机器人工具箱，摘取常用函数等内容翻译，有兴趣可移步作者官网自行下载，网站中的robot.pdf 可作为函数使用说明文档，以及《机器人学、视觉、控制》也可自行找资源下载

连杆对象包含与机器人关节和连杆相关的所有信息，如运动学参数（kinematics parameters）、刚体惯性参数（rigid-body inertial parameters）、电机（motor）和传动参数（transmission parameters）等。

信息显示方法

display : 打印并显示D-H参数矩阵
dyn : 显示动力学参数
type:关节类型：R 或者 P
char : 转化为字符串

Link 的操作类函数：

A ：关节变换矩阵
s=robot.A(J,q);返回一个44的从基座到J的变换矩阵，q是一个1N的关节变量
对于standardDH，是从J-1到J
对于modifiedDH，是从J到J+1
例如robot.A(3:6,q);是指关节3到关节6的变换矩阵，q是theta
friction : 摩擦力
nofriction :将摩擦参数设置为0
islimit：检查关节是否超过软极限
isrevolute : 检测关节是否为转动副
isprismatic : 检测关节是否为移动副
issym:检测关节+连杆是否有符号参数

运动学参数

theta：关节角 ,若没有定义默认为转动关节
d:连杆偏移 若没有默认为移动副
a:连杆长度 默认0
alpha:连杆扭角 默认0
jointtype:R表示转动，P表示移动
sigma: 默认0，旋转关节；1，移动关节
mdh: 默认为0，1
offset:关节变量偏移量
qlim:关节变量范围[min max]
T = robot.fkine(theta),%求正运动学
q = robot.ikine(T) %逆运动学
j0 = robot.jacob0(q, options),%求雅可比矩阵，在世界坐标系下
jn = robot.jacobn(q, options),%求雅可比矩阵，在末端操作器空间中
jdq = robot.jacob_dot(q, qd),%求雅可比矩阵的微分

动力学参数

m: 质量 1*1
r: 质心 3*1
I: 惯性张量 3*1, 6*1 or 3*3
B: 粘性摩擦 默认0
Tc: 库仑摩擦 11 or 21 默认[0 0]
G: 减速比 默认1
Jm: 转子惯量 默认0
tau = R.PAY(w, J),%根据末端负重w和雅可比矩阵j，求关节力
au = R.PAY(q, w, f),%根据末端负重w和关节变量为q雅可比矩阵，求关节力。f=0，世界坐标系。f=1，末端关节坐标系。
[wmax,J] = R.paycap(q, w, f, tlim),%求关节变量为q，有效负荷为w，关节能承受的参考力为tlim时，末端允许的最大力 wmax，和此时达到力极限的关节J
R.payload(m, p),%在末端关节坐标系下，坐标为p处，添加质量为m的负荷
SerialLink.dyn ,%返回动力学参数
tau = R.rne(q, qd, qdd),%逆动力学，达到预定的(q, qd, qdd)，所需要的力tau
tau = R.rne(q, qd, qdd, grav, fext),%逆动力学，达到预定的q, qd, qdd)，重力加速度为grav，末端受力为fext，各关节所需要的力tau
[T,q,qd] = R.fdyn(T, torqfun)%时间[0,T], 返回时间、位置、速度，关节初始位置和速度为0。关节上的力矩用户提供的函数提供：
TAU = TORQFUN(T, Q, QD)%力矩是时间、位置、速度的函数。
[ti,q,qd] = R.fdyn(T, torqfun, q0, qd0)

%例   定义连杆的三种方式
L = Link([0 1.2 0.3 pi/2]);
L = Link(’revolute’, ’d’, 1.2, ’a’, 0.3, ’alpha’, pi/2);
L = Revolute(’d’, 1.2, ’a’, 0.3, ’alpha’, pi/2);
L.alpha		%返回扭转角

L=Link(dh,options)%为一个连杆options指定运动学参数
dh = [THETA D A ALPHA SIGMA OFFSET]
%其中对于转动副 SIGMA =0,对于移动副SIGMA=0；OFFSET是用户关节变量与运动模型所使用的值之间的恒定位移。
dh = [THETA D A ALPHA];可省略默认为0

SerialLink类

plot 以图形形式显示机器人
teach 驱动机器人
isspherical 测试机器人是否有球腕关节
islimit 测试机器人是否抵达关节极限
fkine 正运动学求解
ikine6s 6旋转轴球腕关节机器人的逆向运动学求解
ikine3 3旋转轴机器人的逆向运动学求解
ikine 采用迭代方法的逆向运动学求解
jacob0 在世界坐标系描述的雅克比矩阵
jacobn 在工具坐标系描述的雅克比矩阵
maniplty 可操纵性度
jtraj 关节空间轨迹
accel 关节加速度
coriolis 关节柯氏力
dyn 显示连杆的动力学属性
fdyn 关节运动
friction 摩擦力
gravload 关节重力
inertia 关节惯性矩阵
nofriction 设置摩擦力为0
rne 关节的力/力矩
payload 在末端坐标系增加负载
perturb 随机扰动连杆的动力学参数
trchain 是一系列基本变换，用于描述串联机器人手臂的运动学。 字符串cchain包括多个形式为X（ARG）的令牌，其中X是Tx，Ty，Tz，Rx，Ry或Rz之一。 ARG是关节变量，角度或长度尺寸。

属性：

links 连杆向量(1xN)
gravity 重力的方向[gx gy gz]
base 机器人基座的位姿(4x4)
tool 机器人的工具变换矩阵[ T6 to tool tip] (4x4)
qlim 关节范围[qmin qmax] (Nx2)
offset 偏置(Nx1)
name 机器人名字（在图形中显示）
manuf 注释, 制造商名
comment 注释, 总评
plotopt options for plot() method (cell array)
n 关节数
config 机器人结构字符串, 例如 ‘RRRRRR’
mdh 运动学中约定的布尔数 (0=DH, 1=MDH)

建立机器人

首先用link建立连杆，再用SerialLink建立机器人关节连接，最后使用plot等显示机器人
L=Link([alpha,A,theta,D,sigma,offset],standard/modified)%输入向量元素分别是alha扭转角，A杆件长度，theta关节角，D横距，sigma关节类型（0旋转，1移动）offset关节变量偏移，standard为标准DH参数，modified为改进的DH参数

clear;
clc;
L1 = Link([0 0 0 pi/2]);
L2 = Link([0 0 0.5 pi/2]);
L3 = Link([0 0 0 pi/2]);
L4 = Link([0 1 0 -pi/2]);
L5 = Link([0 0 0 pi/2]);
L6 = Link([0 1 0 0]);
b=isrevolute(L1);  %Link 类函数，转动关节变量
robot=SerialLink([L1,L2,L3,L4,L5,L6]，‘name’,'VIPER7','comment','LL');   %SerialLink 类函数
%robot.name='VIPER7';
%robot.comment='LL';
robot.display();  %Link 类函数，显示建立的机器人DH参数
%通过手动输入各个连杆转角，模型会自动运动到相应位置
theta=[0 0 0 0 0 0];
robot.plot(theta);   %SerialLink 类函数

运行结果：

robot = 
 
VIPER7 (6 axis, RRRRRR, stdDH, fastRNE)                          
 LL;                                                             
+---+-----------+-----------+-----------+-----------+-----------+
| j |     theta |         d |         a |     alpha |    offset |
+---+-----------+-----------+-----------+-----------+-----------+
|  1|         q1|          0|          0|          0|          0|
|  2|         q2|          0|          0|          0|          0|
|  3|         q3|          0|      0.332|      1.571|          0|
|  4|         q4|          1|          0|     -1.571|          0|
|  5|         q5|          0|          0|      1.571|          0|
|  6|         q6|          1|          0|          0|          0|
+---+-----------+-----------+-----------+-----------+-----------+
                                                                 
grav =    0  base = 1  0  0  0   tool =  1  0  0  0              
          0         0  1  0  0           0  1  0  0              
       9.81         0  0  1  0           0  0  1  0              
                    0  0  0  1           0  0  0  1              
 

ans =

     0