机器人学基础第二章正向运动学

首次发布: 2025-04-06

... 次访问

Theory

机械臂的正向运动学

2.1 机械臂的模型#

机械臂模型通常可以简化为一系列刚体通过关节连接而成的链式结构。其中刚体被称为连杆，将两个连杆相连接的是关节

arm

机械臂有两种结构，分别是开链和闭链，其中，开链有明确的首尾，而闭链的每一连杆都与两个及以上连杆有关节耦合

2.2 机构自由度#

构形：在任何时刻均能完全确定机器人形状的所有关节变量值
机构自由度 (DoF)：表达构形所需要的最少的实值关节变量的数目
关节自由度：用于描述关节所连接连杆的相对运动自由状况的数值

常见的关节及自由度有

旋转关节 (Revolute joint): 1 DoF，两个连杆绕一个轴相对转动
棱柱关节 (Prismatic joint): 1 DoF，两个刚体沿一直线相对平移
万向节 (Universial joint): 2 DoF，可以看作两个正交的旋转关节组合而成的二自由度关节
球关节 (Spherical joint): 3 DoF，允许两个连杆之间的相对自由转动

旋转关节	棱柱关节	万向节	球关节

机构的自由度可以通过 Grubler 公式计算。设 $N$ 表示连杆的数目（注意要包含固定的基座）， $J$ 是关节的数目， $m$ 是一个刚体的自由度（空间机构取 $6$ ，平面机构取 $3$ ）， $f_i$ 表示第 $i$ 个关节提供的自由度，则可以求出机构的自由度为

\text{DoF} = m(N - 1 - J) + \sum_{i=1}^J f_i

例如

平面四连杆机构， $m = 3$ ， $N = 4$ (含基座)， $J = 4$ ， $f_i = 1 \ \forall i$ ，可以求得 $\text{DoF} = 1$
特殊具有重叠关节的机构， $m = 3$ , $N = 8$ (基座只计一次), $J = 9$ (有两个关节重叠)， $f_i = 1$ ，可以求得 $\text{DoF} = 3$
Delta 机器人：这是一个闭链结构，上下两个平台，上平台为固定基座，同过三个臂连接下平台。每个臂通过三个旋转关节 (R) 连接上平台、一个连杆和一个平行四边形闭式链、下平台，平行四边形闭式链通过万向节 (S) 连接而成。于是 $N = 17$ , $J = 21$ , 9R $f_i = 1$ , 12S $f_i = 2$ ，于是 $\text{DoF} = 3$ ，即下平台的末端可在空间中自由挪动

平面四连杆机构	特殊具有重叠关节的机构	Delta 机器人

2.3 正向运动学的目标#

构形空间 $\mathbb{Q}$ ：由所有可能构形构成的取值空间，空间中任意点是一组关节变量的取值

正向运动学的目标就是在给定构形的条件下（给定一组关节变量的条件下），计算求出机械臂末端的位姿情况

T: \mathbb{Q} \rightarrow SE(3)

其中 $T$ 可以表示机械臂的设计所形成的从关节变量的构形空间到末端位姿的特殊欧几里得群（有时只考虑末端的位置坐标，此时是笛卡尔空间） 的映射关系。

\begin{aligned} q &= [q_1, q_2, \dots, q_n]^\top \in \mathbb{Q}\\ \ ^{S}_{E}T &= \begin{bmatrix} \ ^{S}_{E}R(q) & \ ^{S}_{}p_{Eorg}(q)\\ 0 & 1\end{bmatrix} \end{aligned}

这里 $\{E\}$ 代表末端的固连框架。

2.4 机械臂的描述#

这里只考虑简单模型，即使用的关节大部分是 1 DoF 的关节如旋转关节和棱柱关节。从运动学的角度，由于连杆的运动通常是绕着或沿着关节的轴方向，因此对轴的描述用沿轴线方向的直线即可。连杆则是确定两个相邻关节相对位置关系的一个刚体。

定义从固定基座开始的连杆编号和关节编号

固定基座编为连杆0，第一个运动连杆是连杆1，往后由此类推
连接连杆0和连杆1的关节是关节1，连接连杆1和连杆2的关节式关节2，由此类推

2.4.1 连杆的参量描述#

一个连杆的运动学特性可以由两个参数描述

连杆长度 (link length) $a_{i-1}$ : 两个相邻关节轴的公垂线长度，方向由是从 关节轴 $i-1$ 到 关节轴 $i$
连杆扭曲角 (link twist) $\alpha_{i-1}$ : 在以两个关节轴的公垂线为法线的平面上，两个关节轴投影的夹角（实际上可直接定义为两个轴线的异面角），方向是从 关节轴 $i-1$ 转到 关节轴 $i$

2.4.2 关节的参量描述#

一个关节的运动学特性也可以用两个参数描述

连杆偏移量 (link offset) $d_i$ : 在公共关节轴上，相邻两个连杆长度公垂线之间的距离，方向从 连杆 $i-1$ 的公垂线 到 连杆 $i$ 的公垂线
关节角 (joint angle) $\theta_i$ : 相邻两个连杆（的长度公垂线）绕公共关节轴旋转的夹角，方向从 连杆 $i-1$ 到 连杆 $i$

对于旋转关节， $\theta_i$ 是关节变量；对于棱柱关节， $d_i$ 是关节变量

连杆描述	关节描述

2.4.3 DH 参数#

上述的四个运动学参数即是 DH 参数，描述一个运动链，只需要给出全部的四变量即可

a_{i-1},\quad \alpha_{i-1},\quad d_i, \quad \theta_i

不过由于起始的基座作为连杆0是特殊情况，不存在关节轴0，因此不考虑 $d_0$ 和 $\theta_0$
如果关节1是旋转关节，则通常有 $\alpha_0$ = 0, $a_0 = 0$
同样地，由于最后一根连杆 $n$ 只有一个相连的关节，因此不存在关节 $n+1$ ，所以不必考虑 $a_n$ 和 $\alpha_n$

例如，对于平面 $3R$ 机械臂，有如下的 DH 参数列表

$i$	$\alpha_i$	$a_i$	$d_i$	$\theta_i$
0	$0$	$0$
1	$0$	$L_1$	$0$	$\theta_1$
2	$0$	$L_2$	$0$	$\theta_2$
3			$0$	$\theta_3$

平面3R

2.4.4 机械臂连杆固连框架#

为了更方便地描述机械臂的坐标运算性质，定义连杆 $i$ 的刚体固连框架为框架 $\{i\}$

框架 $\{i\}$ 的 Z 轴与关节轴 $i$ 轴线方向重合
框架 $\{i\}$ 的原点位于关节轴公垂线 $a_i$ 与关节轴 $i$ 的交点处
框架 $\{i\}$ 的 X 轴沿连杆关节轴公垂线 $a_i$ 方向（关节轴 $i$ 到关节轴 $i+1$ ）
框架 $\{i\}$ 的 Y 轴方向由右手定则确定

注意框架的选取未必唯一，例如

Z 轴方向有两个可选
若关节轴相交，X 轴的方向可选
若关节轴平行，原点位置可选

在定义了刚体固连框架的标准后，可以得到求解 DH 参数的一个坐标系方法

\begin{aligned} a_i &= \text{Proj}_{X_i}\overline{Z_i Z_{i+1}}\quad\text{沿着框架 \{i\}的X轴 }，\text{框架\{i\}Z轴到框架\{i+1\}Z轴的距离} \\ \alpha_i &= \text{Axis}_{X_i}\text{ ang}({Z_i, Z_{i+1}})\quad\text{绕着框架 \{i\}的X轴 }，\text{框架\{i\}Z轴到框架\{i+1\}Z轴的角度} \\ d_i &= \text{Proj}_{Z_i}\overline{X_{i-1} X_{i}}\quad\text{沿着框架 \{i\}的Z轴 }，\text{框架\{i-1\}X轴到框架\{i\}X轴的距离} \\ \theta_i &= \text{Axis}_{Z_i}\text{ ang}({X_{i-1}, X_{i}})\quad\text{绕着框架 \{i\}的Z轴 }，\text{框架\{i-1\}X轴到框架\{i\}X轴的角度} \\ \end{aligned}

连杆0上固连框架

为了便于描述，定义连杆0 的框架 $\{0\}$ 就是基座的固定框架 $\{B\}$ 。
此外，定义框架 $\{1\}$ 在关节1 的变量取0时，与框架 $\{0\}$ 重合

旋转关节的框架 $\{1\}$ 与 $\{0\}$	棱柱关节的框架 $\{1\}$ 与 $\{0\}$

2.5 连杆变换#

2.5.1 齐次变换矩阵#

相邻连杆固连框架之间的齐次变换 $^{i-1}_{i}T$ 可以看作由如下过程依次执行后得到的齐次变换

将框架 $\{i-1\}$ 绕其 X 轴旋转 $\alpha_i$
然后将新框架沿其X轴平移 $a_i$
然后再将新框架绕其 Z 轴旋转 $\theta_i$
再将新框架沿其 Z 轴平移 $d_i$

从而得到框架 $\{i\}$ 。基于上述过程，可以得到齐次变换矩阵为

\begin{aligned} \ ^{i-1}_{i}T &= R_{X}(\alpha_{i-1}) D_{X}(a_{i-1}) R_{Z}(\theta_i) D_{Z}(d_i)\\ &= \begin{bmatrix} 1 & 0 & 0 & a_{i-1}\\ 0 & \cos\alpha_{i-1} & -\sin\alpha_{i-1} & 0\\ 0 & \sin\alpha_{i-1} & \cos\alpha_{i-1} & 0 \\ 0 &0 &0 &1 \end{bmatrix} \begin{bmatrix} \cos\theta_{i} & -\sin\theta_{i} &0 &0\\ \sin\theta_{i} & \cos\theta_{i} &0 &0\\ 0 &0 &1 &d_i\\ 0 &0 &0 &1\end{bmatrix} \\ &= \begin{bmatrix}\cos\theta_{i} & -\sin\theta_{i} &0 &a_{i-1}\\ \sin\theta_{i}\cos\alpha_{i-1} & \cos\theta_{i}\cos\alpha_{i-1} &-\sin\alpha_{i-1} &-\sin\alpha_{i-1}d_i\\ \sin\theta_{i}\sin\alpha_{i-1} & \cos\theta_{i}\sin\alpha_{i-1} &\cos\alpha_{i-1} &\cos\alpha_{i-1}d_i\\ 0 &0 &0 & 1 \end{bmatrix} \end{aligned}

2.5.2 框架约束关系#

需要注意的是，这样设定的框架关系是要满足一定的约束关系的

垂线关系：框架 $\{i\}$ 的 Z 轴必须与框架 $\{i-1\}$ 的 X 轴垂直

Z_i \cdot X_{i-1} = 0

原点关系：框架 $\{i\}$ 的原点 $O_i$ 必须位于框架 $\{i-1\}$ 的X轴 $X_{i-1}$ 所在直线上，即

O_{i-1} = a_{i-1} X_{i-1} + d_i Z_i

2.5.3 构形到末端位姿的映射#

利用上述求得的齐次变换矩阵 $\ ^{i-1}_{i}T$ ，可以求得最后一个框架位姿为

\ ^{0}_{n}T(q) = \ ^{0}_{1}T(q_1) \ ^{1}_{2}T(q_2) \dots \ ^{n-1}_{n}T(q_n)

其中， $q_i = \begin{cases}\theta_i, &\text{revolute}\\ d_i, &\text{prismatic} \end{cases}$ 是关节 $i$ 的变量。引入最后一个框架 $\{n\}$ 到末端框架 $\{E\}$ 的齐次变换 $\ ^{n}_{E}T$ ，得到

\ ^{S}_{E}T = \ ^{S}_{0}T \ ^{0}_{n}T \ ^{n}_{E}T

这里空间框架 $\{S\}$ 常作为框架 $\{0\}$ .