常见的机械臂：
a 一个6自由度串联机械臂。通用工业机械臂
它由一系列刚性连杆和转动关节组成，这种机械臂也是本文的主要讨论对象。

b 四个自由度的SCARA机器人，通常用于电路板装配
这种机器人在垂直方向是刚性的，而在水平面上是柔性的，因此非常适合平面作业任务。

c 直角坐标机器人，机械臂在一个高架导轨上移动
沿着高架轨道有一个或两个运动自由度，具有非常大的工作空间。

d 并联机械臂，末端执行器由6根并联的连杆驱动
所有连杆都是并行地与末端工具相连，这使得它具有诸多优势，例如所有电机都安装在基座上，结构刚性很强。

一个机械臂用它的末端执行器或工具来操作其作业对象，如图：

末端执行器可以是一个简单的两指或平行卡爪夹持器(a)，也可以是一个复杂的仿人机械手(b)，具有多个可弯曲的手指关节和一个对生拇指。

机器人运动学

1 描述一台机械臂（标准D-H参数法）

一个串联机械臂的整个链路是由一组被称为连杆的刚体组成的，这些连杆由关节(运动副)连接起来。每个关节有一个平移(滑动或移动副)或者转动(转动副)的自由度。关节的运动改变了其邻接连杆的相对角度和位置。

一个机器人的关节结构可以用一串字符来描述，例如Puma 560为“RRRRRR”,斯坦福臂为“RRPRRR”，其中每个字符代表相应关节的类型,R是转动副,P为移动副。迪拉维特( De-navit)和哈滕贝格(Hartenberg)于1955年提出了一种用于描述这种串联式链路上连杆和关节几何关系的系统方法,这就是如今熟知的D-H参数法。

有$N$个关节(编号从1到$N$)的机械臂具有$N+1个$连杆(编号从0到$N$)。连杆0是机械臂的基座，连杆$N$则固接末端执行器或者工具。关节$j$将连杆$j-1$连接到连杆$j$上,因此关节$j$带动连杆$j$。

连杆被看作一个刚体，它确定了其相邻两个关节轴线的空间关系。一个连杆由长度$a_i$和扭转角${\alpha }_i$两个参数来描述。

同样，一个关节也由两个参数来描述，其中连杆偏移$d_i$是指沿着关节轴线从一个连杆坐标系到另一个连杆坐标系的距离，而关节角${\theta }_i$则是指一个连杆相对于另一个连杆关于关节轴线的转动角度。

下图对这种参数法进行了图示说明。 { j } \{j\} {j}坐标系固接于连杆$j$的远端或末梢。关节$j$的轴线即为该坐标系的$z$轴。这些连杆和关节的参数即我们所熟知的D-H参数，总结于表7.1中。

---

红色和蓝色分别表示与连杆$i$和连杆$i+1$关联的所有参数。圆圈中的数字表示进行基本变换的顺序。$x_j$ 与 $z_{j-1}$和$z_j$的公法线平行； 如果 $z_{j-1}$和 $z_j$ 平行，则$d_i$可以选任意值，即可沿沿着z轴任意建立坐标。

---

按照约定,第一个关节，即关节1，连接连杆0和连杆1。连杆0为机器人的基座。通常来说第一个连杆有$d=\alpha =0$,然而我们可以设定$d>0$用来表示肩关节的高度超出基座。出基座。

在一个制造系统中,基座通常是固定在环境中的，然而它有可能安装在一个移动的基座上，例如一架航天飞机、一个水下机器人或者一辆卡车。

最后一个关节，即关节$N$,连接连杆$N-1$和连杆$N$。连杆$N$是机器人的工具,它的相关参数$d_N$，和 $a_N$,分别代表了工具的长度及其$x$轴的偏移量。如下图所示，这个工具通常指向$z$($a$即为$z$)轴。

从连杆坐标系行 { j − 1 } \{j-1\} {j−1}到坐标系 { j } \{j\} {j}行的变换被定义为基本旋转和平移，其形式如下:

它可以展开为齐次矩阵形式：

参数$a_i$和${\alpha }_i$始终是常量。对于转动关节${\theta }_i$是关节变量且$d_i$是常量，然而对于移动关节$d_i$是变量${\theta }_i$是常量，且${\alpha }_i$。在以后的很多公式中，我们将使用如下的广义关节坐标：

我们使用工具箱中的Link对象来描述一个机器人连杆，其创建如下：

L=Link([0,0.1,0.2,pi/2,0])

其中，输人入向量的元素按顺序分别是${\theta }_j,d_j,a_j,{\alpha }_j$。而可选的第5个元素${\sigma }_j$,则表明该关节是转动的( ${\sigma }_j=0$)还是移动的(${\sigma }_j=1$)，如果未指定该值则默认为转动关节。

该Link对象的显示值不仅给出了其运动学参数，还表示了其他的一些状态，例如它是一个转动关节(标签“R”)，以及使用了标准的D-H参数表达(标签“stdDH”)。

---

注意：
尽管赋给了$\theta$一个值，但并没有显示出来——这个值在运动学参数系列中仅仅作为一个占位者。$\theta$将被关节变量$q$所取代，因此它的值在Link对象中被忽略掉了。这个值将由Link对象进行管理。

---

https://blog.csdn.net/weixin_44378835/article/details/109319351

Link对象有许多的参数和方法。然而最常用的参数和方法将通过下面例子进行说明。当$q=0.5$时，${}^{j-1}{A}_j$表示的连杆变换如下:

L.A(0.5)

这是一个4X4阶的齐次变换矩阵。我们可以对不同连杆参数的值进行读取或更改。例如：

 L.RP

表明这个连杆是一个转动型。

一个连杆包含一个偏移量，该值将在计算连杆变换之前加到关节变量中：

L.offset=0.5;
L.A(0)

L.offset=0;
L.A(0)

2 正运动学

机械臂的正运动学通常表述为如下的函数形式：

它表明末端执行器的位姿是基于关节坐标的一个函数。若使用齐次变换，其表达式将是由每个连杆变换矩阵的简单乘积。对于一个$N$轴机械臂,有

对于任何一个串联机械臂，无论其关节的数量和类型如何，都可以计算出其正向运动学的解。

末端执行器的位姿共有6个自由度——3个移动自由度和3个旋转自由度。因此为了使末端执行器具有任意的位姿，机械臂通常具有6个关节或自由度。

2.1 两连杆机器人

上图二连杆机械手的D-H参数如下：

L(1)=Link([0,0,1,0]);
L(2)=Link([0,0,1,0]);
L

这个向量传递给构造函数SerialLink，可以建立一个二连杆对象：

two_link=SerialLink(L,'name','two link')

我们还可以复制一个SerialLink对象，并命名为bob。

bob=SerialLink(two_link,'name','bob')

现在可以让机械臂工作了。正向运动学的计算使用fkine方法。在这个例子中，$q_1=q_2=0$。

two_link.fkine([0,0])

该方法返回代表机器人第二个连杆坐标系位姿的齐次变换矩阵$T_2$。对于一个不同的位形，工具的位姿为：

two_link.fkine([pi/4,-pi/4])

画出机器人的图形

two_link.plot([0,0])

two_link.plot([pi/4,-pi/4])

2.2 六轴机器人

现实的任务空间由6个自由度：3个平移和3个转动。这就需要一个具有位形空间为6的机器人，即一个有6个关节的机器人。以Puma560为例子：

mdl_puma560
% 在工作区中创建一个名为p560的SerialLink对象
p560

注意：$a_j$和$d_j$都采用国际标准单位，这意味着正向运动学的平移部分也将使用国际标准呢单位。

指令"mdl_puma560"还在工作取中创建了大量关节坐标向量，它们代表一些典型的机器人位形：

 p560.plot(qz)

零角度时，末端工具默认指向上。

p560.plot(qr)

p560.plot(qs)

p560.plot(qn)

灵巧空间状态即远离奇异点。

机器人的正向运动学计算如下：

p560.fkine(qz)

返回一个对应末端执行器位姿的齐次变换$T_6$。机器人上连杆6坐标系 { } \{\} {}的原点被定义位最后三个关节轴的交点——这个点在机器人的腕关节内部。

通过以下命令可以定义一个工具变换，

p560.tool=transl([0,0,0.2])

从$T_6$坐标系变换到实际的工具顶端。在本例中工具顶端是沿这$T6$的$z$轴延伸200mm。此时，工具顶端（也称工具中心点或者TCP）的位姿变为：

p560.fkine(qz)

0.6318=0.4318+0.2

我们在定义一上述运动学变换时，认为机器人的基座是位于机器人结构内部的腰关节和肩关节轴线的交点上（机器人专家也倾向于用拟人化的术语描述机器人。我们使用像腰、肩、肘和腕这样的名称来描述一个串联机械臂。对于Puma 560机器人，这些名称分别对应于其关节1、2、3及4~6。）。Puma560机器人有一个30英寸高的底座结构。我们可以通过一个基座变换将机器人的原点从其内部转移到底座的下端部：

p560.base=transl([0,0,30*0.0254]) % 英寸被转化位米

现在，经过基座变换和工具变换后，正向运动学变为：

p560.fkine(qz)

1.3938=0.762+0.2+0.4318

q=[0 0 0 0 0 0;
   0 0.0365 -0.0365 0 0 0;
   0 0.2273 -0.2273 0 0 0;
   ];

其中每一行表示不同时间步下的关节坐标值，而每一列的数字则对应每个关节。此时再使用方法fkine：

T=p560.fkine(q);
about(T)

返回一个三维矩阵。其中前两维是一个4X4阶齐次变换矩阵，第三维是时间步。对应于q中第三行的关节坐标值的齐次变换矩阵是：

T(:,:,3)

3 逆运动学

正运动学：给定关节坐标以及可选工具和基座变换的条件下，确定末端执行器的位姿

逆运动学：给定期望的末端执行器位姿${\xi }_E$，求所需的关节坐标。

逆运动学写成函数形式如下：

一般这个函数的解是不唯一的，而且对于某种类型的机械臂甚至没有封闭形式的解存在，只能求出一个数值解。

3.1 封闭形式解（用于六轴机械臂）

对于一个六轴机器人，其逆运动学具有封闭形式解的一个必要条件是：三个腕部关节的轴相交于一点。这意味着腕关节的运动只改变末端执行器的姿态，而不改变其位置。这种机构称为球腕。几乎所有工业机器人都具有这样的关节。
继续以Puma560为例：

mdl_puma560

标准状态的关节坐标为：

qn

对应的末端执行器位姿是：

T=p560.fkine(qn)

Puma560是一种包含一个球关节的六轴机器人，因此我们使用``ikine6s方法来计算其逆运动学的封闭解。要实现位姿$T$，所需的关节坐标为：

qi=p560.ikine6s(T)

这个解与qn不同，但是

T=p560.fkine(qi)

qn和qi得到的末端执行器位姿是相同的。
将两者可视化观察它们的位形：

p560.plot(qn)

p560.plot(qi)

即逆解是不唯一的。该Puma机器人的肩关节相对于腰关节有个水平偏移，所以一组坐标解对应的机械臂在腰部的左侧，而另一组则对应在右侧，它们分别称为左手运动学位形和右手运动学位形。一般有8组不同的关节坐标值可以得到相同的末端执行器位姿，即逆解是不唯一的。

ikine6s返回的运动学位形由一个或多个字符标志控制：

如：右上

qi=p560.ikine6s(T,'ru')
p560.plot(qi)

由于关节转角上的机械限制以及连杆之间可能发生碰撞，并不是所有8中位形解在物理上都可以实现，有时甚至可能不存在可以实现的解。例如手臂长度不足：

p560.ikine6s(transl(3,0,0))

另外，由于奇异位型的存在，即关节轴线重合而减少了有效自由度，某个姿态也可能无法实现。Puma560有一个奇异位形，即当$q_5$为零时，关节4和关节6之间的轴线重合。在这种情况下，ikine6s只能保证$q_4+q_6$等于一个常值，但它们各自的数值确实任意的。

3.2 数值解

对于不具有6个关节和球形腕关节的机器人，我们需要使用一种迭代数值解。继续使用上一节的示例，我们使用ikine 方法来计算一般逆运动学解:

qn
T=p560.fkine(qn);
qi=p560.ikine(T,[0 0 3 0 0 0]) % 指定关节坐标初始值为[0 0 3 0 0 0]

T1=p560.fkine(qn)
T2=p560.fkine(qi)

qn与·qi不同，但它们都能得到相同的末端位姿。

这个通用数值解法的局限性是:它不像解析法能提供对手臂运动位形的明显控制，只有通过设定关节坐标初始值(默认为零)的隐含控制。

通用数值方法ikine要比解析方法ikine6s慢很多。然而，它也具有很大的优势，它能够求解处于奇异位形以及非6关节型的机械臂。

3.3 轨迹

机器人学中最常见的要求之一，是把机器人末端执行器平滑地从位姿A移动至位姿B。下面我们将讨论两种产生这种轨迹的方法，分别在关节空间中(根据关节变化而变化)的直线和在笛卡尔空间（根据末端执行器的坐标系变化而变化）中的直线。

这两种直线分别被称为关节空间运动和笛卡尔空间运动。

3.3.1 关节空间运动

我们考虑末端执行器在两个笛卡尔位姿之间移动：

T1=transl([0.4,0.2,0])*trotx(pi);
T2=transl([0.4,-0.2,0])*trotx(pi/2);

它们位于xy平面内，且末端执行器指向朝下。与这两个位姿对应的初始和最终关节坐标向量为：

q1=p560.ikine6s(T1);
q2=p560.ikine6s(T2);

而我们要求整个运动发生在2秒时间内，采样时间间隔为50微秒。

t=[0:0.02:2];

通过在$q_1$和$q_2$两个位形之间的平滑插值就能得到一个关节空间轨迹。我们可以用标量插补函数tpoly或lspb，再配合多轴驱动函数mtraj一起实现:

q=mtraj(@tpoly,q1,q2,t)
q=mtraj(@lspb,q1,q2,t)

结果得到一个50X6阶矩阵，每行对应一个采样时间点，每列对应一个关节值。

还有一个相当于具有tpoly插值的mtraj的函数jtraj

q=jtraj(q1,q2,t)

它还对多轴情况进行了优化，而且还允许使用额外参数来设置初始和最终速度。
我们可以通过可选输出参数，获得随时间变化的关节速度和加速度向量：

[q,qd,qdd]=jtraj(q1,q2,t);

SerialLink类中的jtraj方法为上述步骤提供了一个更简洁的表达方式：

q=p560.jtraj(T1,T2,t);

所得轨迹可以绘制出动画进行观察：

clc;
clear
close all
mdl_puma560
t=[0:0.05:2]';
T1=transl([0.4,0.2,0.])*trotx(pi);
T2=transl([0.4,-0.2,0])*trotx(pi/2);
q=p560.jtraj(T1,T2,t);
T=p560.fkine(q)
p=transl(T);
plot(p(:,1),p(:,2)) % 轨迹
view(45,30)
hold on
p560.plot(q)

绘制随时间变化的关节角：

qplot(t,q)

3.3.2 笛卡尔空间运动

关节空间运动是指定初始与末尾的位姿然后转化为关节角进行插值，所以机械臂工作时中间部分的位姿是由插值得到的关节角转换得到的，得到的轨迹不是直线。但对于许多应用，我们都需要笛卡尔空间中的直线运动，这称为笛卡尔运动。ctraj函数可以实现：

clc;
clear
close all
mdl_puma560
p560.tool=transl([0,0,0.5])  % 指定末端执行器长度0.5m
T1=transl([0.4,0.2,0])*trotx(pi);    % 初始位姿
T2=transl([0.4,-0.2,0])*trotx(pi/2); % 结束位姿
t=[0:0.05:2]';
Ts=ctraj(T1,T2,length(t));
p=transl(Ts);
plot(p(:,1),p(:,2));
view(45,30)
hold on
q=p560.ikine6s(Ts);  % 得到关节空间轨迹
p560.plot(q)

ctraj是直接对末端执行器的笛卡尔坐标系进行插值的，轨迹为一条直线。

绘制随时间变化的关节角：

qplot(t,q)

参考
《机器人学、机器视觉与控制——MATLAB算法基础》 作者 Peter Corke; 翻译：刘荣等