正向运动学 (Forward Kinematics)
约 5834 个字 26 张图片 预计阅读时间 19 分钟
正向运动学 (Forward Kinematics) 的核心任务是：已知机器人所有关节的角度（或位移），计算其末端执行器在空间中的位置和姿态。 本章将介绍实现这一目标的核心工具——齐次变换矩阵和D-H参数法。
3.1 位置与姿态 (Position & Orientation)
位置与姿态的统一描述
Note
机械臂末端执行器上参考点 P 的位置可以用一个位置向量 \(\overrightarrow{OP}\) 来表示。这个向量综合了旋转和平移两个部分。
- 点 P 在本体坐标系 1 中的坐标为 \(^1\mathbf{r}\)。
- 坐标系 1 相对于坐标系 0 进行了旋转，由旋转矩阵 \(^0R_1\) 描述。
- 坐标系 1 的原点相对于坐标系 0 的原点发生了平移，由平移向量 \(^0\mathbf{d}\) 描述。
因此，点 P 在世界坐标系 0 中的位置 \(^0\mathbf{r}\) 为：
$$
\overrightarrow{OP} = {}^0\mathbf{r} = {}^0R_1 \ {}^1\mathbf{r} + {}^0\mathbf{d}
$$
展开为分量形式：
$$
\begin{bmatrix} X \ Y \ Z \end{bmatrix} =
\begin{bmatrix} r_{11} & r_{12} & r_{13} \ r_{21} & r_{22} & r_{23} \ r_{31} & r_{32} & r_{33} \end{bmatrix}
\begin{bmatrix} x \ y \ z \end{bmatrix} +
\begin{bmatrix} d_x \ d_y \ d_z \end{bmatrix}
$$
齐次变换
齐次变换 (Homogeneous Transformation)
我们已经知道，一个点 P 的位置可以表示为旋转和平移的组合：
$$
\overrightarrow{OP} = {}^0\mathbf{r} = {}^0\mathbf{d} + {}^0R_1 \ {}^1\mathbf{r}
$$
为了将这两种运算统一为单次的矩阵乘法，我们引入齐次变换 (Homogeneous Transformation)。
$$
\begin{bmatrix} ^0\mathbf{r} \ 1 \end{bmatrix} =
\begin{bmatrix} ^0R_1 & ^0\mathbf{d} \ \mathbf{0}_{1\times3} & 1 \end{bmatrix}
\begin{bmatrix} ^1\mathbf{r} \ 1 \end{bmatrix} =
{}^0T_1
\begin{bmatrix} ^1\mathbf{r} \ 1 \end{bmatrix}
$$
齐次变换矩阵 (Homogeneous Transformation Matrix)
这个 4x4 的矩阵 \(^0T_1\) 结构如下：
$$
^0T_1 = \left[ \begin{array}{ccc:c} r_{11} & r_{12} & r_{13} & d_x \ r_{21} & r_{22} & r_{23} & d_y \ r_{31} & r_{32} & r_{33} & d_z \ \hdashline 0 & 0 & 0 & 1 \end{array} \right]
$$
关于齐次坐标的说明
齐次坐标 (Homogeneous coordinates)
- 用一个 (n+1) 维向量来表示一个 n 维向量的方法，称为齐次表示法 (homogeneous representation)。
-
附加的元素 c 是一个缩放因子 (scale factor)。
$$
\mathbf{r} = \begin{bmatrix} x \ y \ z \end{bmatrix} \implies
\begin{bmatrix} c\mathbf{r} \ c \end{bmatrix} =
\begin{bmatrix} cx \ cy \ cz \ c \end{bmatrix} =
\begin{bmatrix} r_1 \ r_2 \ r_3 \ c \end{bmatrix}
$$ -
只要 \(c \ne 0\)，无论 \(c\) 如何变化，齐次坐标都表示同一个向量。
齐次变换矩阵的分解
齐次变换矩阵关联了坐标系 1 和 0 中的坐标。它通过两种基本变换来同时表示位姿 (pose) 和位置 (position) 信息：
-
旋转变换 (Rotation transformation):
$$
^0T_1 =
\begin{bmatrix} ^0R_1 & \mathbf{0}{3\times1} \ \mathbf{0}} & 1 \end{bmatrix
$$ -
平移变换 (Translation transformation):
$$
^0T_1 =
\begin{bmatrix} \mathbf{I}{3\times3} & ^1\mathbf{d} \ \mathbf{0}} & 1 \end{bmatrix
$$
连续变换 (Successive Transformation)
对于一个多连杆机器人，从基座到末端的总变换，可以通过依次连乘每个连杆之间的齐次变换矩阵得到。
假设有一个点，在坐标系 2 中的坐标是 \(^2\mathbf{r}\)，我们想求它在坐标系 0 中的坐标 \(^0\mathbf{r}\)。
- 从 frame 2 到 frame 1: \(^1\mathbf{r} = {}^1R_2 \ ^2\mathbf{r} + {}^1\mathbf{d}_2\)
- 从 frame 1 到 frame 0: \(^0\mathbf{r} = {}^0R_1 \ ^1\mathbf{r} + {}^0\mathbf{d}_1\)
将 1 式代入 2 式，得到：
$$
^0\mathbf{r} = {}^0R_1 ({}^1R_2 \ ^2\mathbf{r} + {}^1\mathbf{d}_2) + {}^0\mathbf{d}_1 = ({}^0R_1 {}^1R_2) \ ^2\mathbf{r} + ({}^0R_1 {}^1\mathbf{d}_2 + {}^0\mathbf{d}_1)
$$
使用齐次变换矩阵，这个复杂的过程就变成了简单的矩阵乘法：
$$
\begin{bmatrix} ^0\mathbf{r} \ 1 \end{bmatrix} =
\underbrace{
\begin{bmatrix} ^0R_1 & ^0\mathbf{d}1 \ \mathbf{0} & 1 \end{bmatrix}
}
\underbrace{
\begin{bmatrix} ^1R_2 & ^1\mathbf{d}2 \ \mathbf{0} & 1 \end{bmatrix}
}
\begin{bmatrix} ^2\mathbf{r} \ 1 \end{bmatrix}
$$
因此，总的变换矩阵为：
$$
^0T_2 = {}^0T_1 \ {}^1T_2
$$
对于一个 n 连杆的机器人，从末端 (frame n) 到基座 (frame 0) 的总变换为：
$$
^0\mathbf{r} = {}^0T_n \ ^n\mathbf{r} \quad \text{其中} \quad ^0T_n = {}^0T_1 \ {}^1T_2 \cdots {}^{n-1}T_n
$$
示例 EX 3-1-1
问题
找到刚体 B 在坐标系 G 中的位置和姿态。B 的运动顺序如下：
- 绕 X 轴 旋转 \(\alpha\) 角。
- 沿 X 轴 平移距离 \(a\)。
- 沿 Z 轴 平移距离 \(d\)。
- 绕 Z 轴 旋转 \(\theta\) 角。
解 ：
最后一个操作（绕Z轴转\(\theta\)）的矩阵 \(T_{Z,\theta}\) 在最左边，所有操作都是相对于固定参考系的。
$$
T = T_{Z,\theta} \ T_{Z,d} \ T_{X,a} \ T_{X,\alpha}
$$
$$
T =
\underbrace{ \begin{bmatrix} c\theta & -s\theta & 0 & 0 \ s\theta & c\theta & 0 & 0 \ 0 & 0 & 1 & 0 \ 0 & 0 & 0 & 1 \end{bmatrix} }{T}
\underbrace{ \begin{bmatrix} 1 & 0 & 0 & 0 \ 0 & 1 & 0 & 0 \ 0 & 0 & 1 & d \ 0 & 0 & 0 & 1 \end{bmatrix} }{T}
\underbrace{ \begin{bmatrix} 1 & 0 & 0 & a \ 0 & 1 & 0 & 0 \ 0 & 0 & 1 & 0 \ 0 & 0 & 0 & 1 \end{bmatrix} }{T}
\underbrace{ \begin{bmatrix} 1 & 0 & 0 & 0 \ 0 & c\alpha & s\alpha & 0 \ 0 & s\alpha & c\alpha & 0 \ 0 & 0 & 0 & 1 \end{bmatrix} }{T}
$$
示例 EX 3-1-2
问题
根据图中展示的坐标系，利用变换矩阵的性质，求出 \(^0T_i, \ ^{i-1}T_i, \ ^jT_i\)。
解:
通过观察图中各坐标系的位置和姿态关系，可以写出它们之间的变换矩阵。
-
从 Frame 0 到 Frame 1 (\(^0T_1\)):
$$
^0T_1 = \begin{bmatrix} -1 & 0 & 0 & 0 \ 0 & 0 & -1 & e+c \ 0 & -1 & 0 & a-d \ 0 & 0 & 0 & 1 \end{bmatrix}
$$ -
从 Frame 2 到 Frame 1 (\(^1T_2\)):
$$
^1T_2 = \begin{bmatrix} 0 & -1 & 0 & b \ 0 & 0 & -1 & a-d \ 1 & 0 & 0 & 0 \ 0 & 0 & 0 & 1 \end{bmatrix}
$$ -
从 Frame 0 到 Frame 2 (\(^0T_2 = {}^0T_1 \ {}^1T_2\)，求解无需用矩阵乘法):
$$
^0T_2 = \begin{bmatrix} 0 & 1 & 0 & -b \ -1 & 0 & 0 & e+c \ 0 & 0 & 1 & 0 \ 0 & 0 & 0 & 1 \end{bmatrix}
$$
逆齐次变换
逆齐次变换 (Inverse Homogeneous Transformation)
使用齐次变换矩阵的优点是简化运算，但代价是失去了正交性。因此，它的逆矩阵不等于它的转置矩阵 (\(T^{-1} \ne T^T\))。
齐次变换矩阵 \(^0T_1\) 的逆 \(^0T_1^{-1} = {}^1T_0\) 可以通过以下公式计算：
$$
^0T_1 = \begin{bmatrix} ^0R_1 & ^0\mathbf{d} \ \mathbf{0} & 1 \end{bmatrix}
\implies
({}^0T_1)^{-1} = {}^1T_0 =
\begin{bmatrix} ({}^0R_1)^{-1} & -({}^0R_1)^{-1} {}^0\mathbf{d} \ \mathbf{0} & 1 \end{bmatrix}
$$
因为旋转矩阵 \(R\) 是正交的，所以 \(R^{-1} = R^T\)。代入上式得到：
$$
{}^1T_0 =
\begin{bmatrix} ({}^0R_1)^T & -({}^0R_1)^T {}^0\mathbf{d} \ \mathbf{0} & 1 \end{bmatrix}
$$
示例 EX 3-1-3: SCARA 机器人正向运动学
问题
图中展示了一个 R||R||P (SCARA) 机器人。基座上有世界坐标系 G，连杆1上有坐标系 \(B_1\)，连杆3的末端有坐标系 \(B_2\)。求末端执行器的位姿。
解:
我们的目标是求总变换矩阵 \(^G T_{B2}\)。我们可以通过链式法则计算：\(^G T_{B2} = {}^G T_{B1} \cdot {}^{B1} T_{B2}\)。
-
计算 \(B_1 \to G\) 的变换矩阵 \(^G T_{B1}\):
- 旋转：绕 G 的 Z 轴旋转 \(\theta_1\)。
- 平移：沿 G 旋转后的新 X 轴平移 \(l_1\)。
$$
^G T_{B1} = \begin{bmatrix} \cos\theta_1 & -\sin\theta_1 & 0 & l_1 \cos\theta_1 \ \sin\theta_1 & \cos\theta_1 & 0 & l_1 \sin\theta_1 \ 0 & 0 & 1 & 0 \ 0 & 0 & 0 & 1 \end{bmatrix}
$$
-
计算 \(B_2 \to B_1\) 的变换矩阵 \(^{B1} T_{B2}\):
- 旋转：绕 \(B_1\) 的 Z 轴旋转 \(\theta_2\)。
- 平移：沿 \(B_1\) 旋转后的新 X 轴平移 \(l_2\)，沿 Z 轴平移 \(-h\)。
$$
^{B1} T_{B2} = \begin{bmatrix} \cos\theta_2 & -\sin\theta_2 & 0 & l_2 \cos\theta_2 \ \sin\theta_2 & \cos\theta_2 & 0 & l_2 \sin\theta_2 \ 0 & 0 & 1 & -h \ 0 & 0 & 0 & 1 \end{bmatrix}
$$
-
计算总变换矩阵 \(^G T_{B2}\):
$$
^G T_{B2} = {}^G T_{B1} \cdot {}^{B1} T_{B2} =
\begin{bmatrix}
c(\theta_1+\theta_2) & -s(\theta_1+\theta_2) & 0 & l_1c\theta_1 + l_2c(\theta_1+\theta_2) \
s(\theta_1+\theta_2) & c(\theta_1+\theta_2) & 0 & l_1s\theta_1 + l_2s(\theta_1+\theta_2) \
0 & 0 & 1 & -h \
0 & 0 & 0 & 1
\end{bmatrix}
$$- 姿态 (Pose): 从旋转部分可以看出，末端的 RPY 角为 \((0, 0, \theta_1+\theta_2)\)。
- 位置 (Position): 末端的位置向量为 \([l_1c\theta_1 + l_2c(\theta_1+\theta_2), \ l_1s\theta_1 + l_2s(\theta_1+\theta_2), \ -h]^T\)。
示例 EX 3-1-4: 绕任意轴旋转
问题
求一个绕不经过原点的任意轴 \(\mathbf{u}\) 旋转的齐次变换矩阵。
解:
这个问题可以通过一个三步法解决：
- 平移：将旋转轴 \(\mathbf{u}\) 平移，使其通过坐标系原点。这需要一个平移变换 \(T(-\mathbf{d})\)，其中 \(\mathbf{d}\) 是原点到旋转轴上某点的向量。
- 旋转：绕经过原点的轴 \(\mathbf{u}\) 进行旋转。变换为 \(T_u(\phi)\)。
- 平移回去：将旋转后的物体平移回原来的位置。变换为 \(T(\mathbf{d})\)。
总变换是这三个变换的复合。因为这些操作是相对于固定参考系进行的，所以使用前乘，最后的变换在最左边：
3.2 Denavit-Hartenberg 方法
3.2 Denavit-Hartenberg (D-H) 方法
- 串联关节机器人（机械臂）结构多样，这给控制带来了复杂性。
- 因此，有必要发展一种通用的方法来定义机械臂的几何结构，以便用统一的方式控制不同的机械臂。
- 最有用的方法之一就是所谓的 Denavit-Hartenberg (D-H) 标记法，它能以通用的方式描述机械臂的结构。
连杆的标记法 (Notations of Links)
- 一个有 \(n\) 个关节的机器人，将有 \(n\) 个可动连杆和 1 个固定的基座连杆。
- 连杆编号从 0（固定的基座）开始，到 1（第一个可动连杆），依次递增，直到 n（末端执行器连杆）。
- 连杆 i 通过 关节 i 连接到其前一个连杆 i-1 (近端)，并通过 关节 i+1 连接到其后一个连杆 i+1 (远端)。
D-H 参数
D-H 参数 (Denavit-Hartenberg Parameters)
解决正向运动学问题，就是找到连杆 i 和 连杆 i-1 之间的变换矩阵 \(T\) 的过程。
连杆 i 相对于连杆 i-1 的位姿由两个方面决定：旋转 和 机械部件的几何形状。
这两个方面被四个 D-H 参数 所量化：
- 旋转相关:
- 相对旋转角 \(\theta\) (关节角)
- 几何形状相关:
- 连杆距离 \(d\) (关节偏移)
- 连杆扭转 \(\alpha\) (连杆扭转角)
- 连杆长度 \(a\) (连杆长度)
D-H 步骤 1: 建立坐标系
使用这 4 个几何参数来确定变换矩阵 \(T\) 的过程，被称为 Denavit-Hartenberg (DH) 方法。
-
确定 \(z_i\) 轴:
- 对于旋转关节，\(z_i\) 轴与连杆 \(i\) 的远端关节轴（即关节 \(i+1\)） 对齐。
- 对于移动关节，\(z_i\) 轴与平移的方向对齐。
- \(z_i\) 轴的方向可以任选其一。
-
确定原点 \(o_i\) 和 \(x_i\) 轴:
-
\(x_i\) 轴 沿着 \(z_{i-1}\) 轴和 \(z_i\) 轴之间的公垂线，方向从 \(z_{i-1}\) 指向 \(z_i\)。
$$ x_i = \pm (z_{i-1} \times z_i) $$ -
原点 \(o_i\) 是 \(z_i\) 轴与上述公垂线的交点。
- 若\(z_{i-1}\) 和 \(z_i\) 平行，\(x_{i}\)从 \(o_{i-1}\) 指向 \(o_i\)
-
-
确定 \(y_i\) 轴:
- \(y_i\) 轴 由右手定则确定：
$$ y_i = z_i \times x_i $$
- \(y_i\) 轴 由右手定则确定：
-
夹具的处理方式：
步骤 2: 确定 D-H 参数
- 关节参数 (Joint Parameters) - 通常是变量
- 关节角 \(\theta_i\) (Joint angle): 绕着 \(z_{i-1}\) 轴，从 \(x_{i-1}\) 旋转到与 \(x_i\) 平行所需的角度。
- 关节距离 \(d_i\) (Joint distance): 沿着 \(z_{i-1}\) 轴，从 \(x_{i-1}\) 移动到 \(x_i\) 的距离。
- 连杆参数 (Link Parameters) - 通常是常量
- 连杆长度 \(a_i\) (Link length): 沿着 \(x_i\) 轴，从 \(z_{i-1}\) 移动到 \(z_i\) 的距离。
- 连杆扭转 \(\alpha_i\) (Link twist): 绕着 \(x_i\) 轴，从 \(z_{i-1}\) 旋转到与 \(z_i\) 平行所需的角度。
示例 Ex 3-2-1: 3R 机械臂
问题
为下图所示的 3R 机械臂填写 D-H 参数表。
解:
- 建立坐标系 (如图所示)。
- 提取D-H参数，填入表格。
| Frame No. (i) | 连杆扭转 (\(\alpha_i\)) | 连杆长度 (\(a_i\)) | 关节距离 (\(d_i\)) | 关节角 (\(\theta_i\)) |
|---|---|---|---|---|
| 1 | \(0\) | \(a\) | \(0\) | \(\theta_1\) |
| 2 | \(-90^\circ\) | \(b\) | \(0\) | \(\theta_2\) |
| 3 | \(0\) | \(0\) | \(0\) | \(\theta_3\) |
关于 D-H 参数的注意事项
- 初始位形 (Home Configuration)：当所有关节变量（如 \(\theta_i\)）都为零时，机器人的构型称为初始位形或静止位，这是机器人所有运动的参考基准。
- 非唯一性：D-H 坐标系不是唯一的。因为 \(z\) 轴的方向是任意的，并且当 \(z_{i-1}\) 和 \(z_i\) 相交时，\(x\) 轴的选择也是任意的。
- \(x_i\) 的选择：当大部分关节参数为零时，\(x_i\) 的方向可以选择一个更方便的参考系。
- 最佳静止位：最好的静止位姿是让尽可能多的坐标轴相互平行且共面。
示例 Ex 3-2-2: PUMA 200 机器人
问题
为 PUMA 200 机器人填写 D-H 参数表。
解:
同样地，先建立坐标系，然后提取参数。
| No. (i) | \(a_i\) | \(\alpha_i\) | \(d_i\) | \(\theta_i\) | 初始值 |
| :-----: | :---: | :---------: | :---: | :--------: | :-----------: |
| 1 | 0 | \(-90^\circ\) | 13 | \(\theta_1\) | |
| 2 | 8 | \(0\) | 0 | \(\theta_2\) | |
| 3 | 0 | \(90^\circ\) | -e | \(\theta_3\) | (\(90^\circ\)) |
| 4 | 0 | \(90^\circ\) | 8 | \(\theta_4\) | (\(180^\circ\)) |
| 5 | 0 | \(90^\circ\) | 0 | \(\theta_5\) | (\(90^\circ\)) |
| 6 | 0 | 0 | t | \(\theta_6\) | |
再次强调：D-H 坐标系的建立不是唯一的。
- 正向运动学是将运动学信息从机器人关节变量空间转换到笛卡尔坐标空间，为一组给定的关节变量寻找末端执行器的位置和姿态是正向运动学中的主要问题。
- 解决正向运动学问题是一个确定变换矩阵 \(^0T_i\) 的过程，用以描述连杆 \(i\) 在基连杆坐标系中的运动学信息。
- 通过使用 Denavit-Hartenberg 标记法和坐标系，我们有：
其中，矩阵的左上部分 \(\boldsymbol{R}_n\) 代表坐标系 \(B_n\) 的姿态，右上部分 \(\boldsymbol{d}_n\) 代表 \(o_n\) 的位置。
3.3 Transformation of Two Frames
连接到连杆 \((i)\) 和连杆 \((i+1)\) 的两个相邻坐标系之间的变换矩阵是正向运动学问题的基本组成部分。
最初，坐标系 \(B_i\) 与 \(B_{i-1}\) 重合。它在经过四个齐次变换步骤后成为当前状态。
\(B_{i-1}\) 是一个全局坐标系，\(B_i\) 是一个局部坐标系。
- \(B_{i-1}\) 沿 \(z_{i-1}\) 平移 \(d_i\) \(\rightarrow D_{z_{i-1}}(d_i)\)
- \(B_{i-1}\) 绕 \(z_{i-1}\) 旋转 \(\theta_i\) \(\rightarrow R_{z_{i-1}}(\theta_i)\)
- \(B_{i-1}\) 沿 \(x_i\) 平移 \(a_i\) \(\rightarrow D_{x_i}(a_i)\)
- \(B_{i-1}\) 绕 \(x_i\) 旋转 \(\alpha_i\) \(\rightarrow R_{x_i}(\alpha_i)\)
步骤1和2是可交换的，步骤3和4也是可交换的。
Transformation Matrix
通过四个齐次变换矩阵的前乘或后乘，可以获得从坐标系 \(B_{i-1}\) 到 \(B_i\) 的总变换。
Note
将这些矩阵相乘得到最终的标准 D-H 变换矩阵：
\[ \begin{align} ^{i-1}T_i &= D_{z_{i-1}}(d_i) R_{z_{i-1}}(\theta_i) D_{x_i}(a_i) R_{x_i}(\alpha_i)\\ &=\begin{bmatrix} c\theta_i & -c\alpha_i s\theta_i & s\alpha_i s\theta_i & a_i c\theta_i \\ s\theta_i & c\alpha_i c\theta_i & -s\alpha_i c\theta_i & a_i s\theta_i \\ 0 & s\alpha_i & c\alpha_i & d_i \\ 0 & 0 & 0 & 1 \end{bmatrix} \\ \\ ^{i-1}T_i = {^i}T_{i-1} &= \begin{bmatrix} c\theta_i & s\theta_i & 0 & -a_i \\ -c\alpha_i s\theta_i & c\alpha_i c\theta_i & s\alpha_i & -d_i s\alpha_i \\ s\alpha_i s\theta_i & -s\alpha_i c\theta_i & c\alpha_i & -d_i c\alpha_i \\ 0 & 0 & 0 & 1 \end{bmatrix} \end{align} \]
Solution to Forward Kinematics Problem
- 在坐标系 n 中的一个点 P 的位置
$$
\begin{bmatrix} \boldsymbol{r}_p \ 1 \end{bmatrix} = {^0T_n} \begin{bmatrix} {}^n\boldsymbol{r}_p \ 1 \end{bmatrix} = \begin{bmatrix} \boldsymbol{R}_n & \boldsymbol{d}_n \ \boldsymbol{0} & 1 \end{bmatrix} \begin{bmatrix} {}^n\boldsymbol{r}_p \ 1 \end{bmatrix} = \begin{bmatrix} \boldsymbol{R}_n {}^n\boldsymbol{r}_p + \boldsymbol{d}_n \ 1 \end{bmatrix}
$$
其中 \(^0T_n = {^0T_1}{^1T_2}\cdots{^{n-1}T_n}\)
- 附着 \(B_n\) 的末端执行器的位姿
$$
\boldsymbol{R}n =
\begin{bmatrix}
r \} & r_{12} & r_{13
r_{21} & r_{22} & r_{23} \
r_{31} & r_{32} & r_{33}
\end{bmatrix}
=
\begin{bmatrix}
n_x & s_x & a_x \
n_y & s_y & a_y \
n_z & s_z & a_z
\end{bmatrix}
$$
例1: 求解P点位置
题目：\(\alpha=\pi/6\)。找到末端点 P 的位置。
D-H 参数表:
| No. | \(a_i\) | \(\alpha_i\) | \(d_i\) | \(\theta_i\) |
|---|---|---|---|---|
| 1 | \(a\) | 0 | 0 | \(\theta_1\) |
| 2 | \(b\) | -90° | 0 | \(\theta_2\) |
| 3 | 0 | 0 | 0 | \(\theta_3\) |
变换矩阵:
$$\begin{align}
^0T_1 &= \begin{bmatrix} c\theta_1 & -s\theta_1 & 0 & ac\theta_1 \ s\theta_1 & c\theta_1 & 0 & as\theta_1 \ 0 & 0 & 1 & 0 \ 0 & 0 & 0 & 1 \end{bmatrix}, \
^1T_2 = \begin{bmatrix} c\theta_2 & -s\theta_2 & 0 & bc\theta_2 \ s\theta_2 & c\theta_2 & 0 & bs\theta_2 \ 0 & 0 & 1 & 0 \ 0 & 0 & 0 & 1 \end{bmatrix}, \
^2T_3 &= \begin{bmatrix} c\theta_3 & -s\theta_3 & 0 & 0 \ s\theta_3 & c\theta_3 & 0 & 0 \ 0 & 0 & 1 & 0 \ 0 & 0 & 0 & 1 \end{bmatrix}
\end{align}
$$
总变换矩阵 \(^0T_3\):
$$
\begin{align}
^0T_3 &= {^0T_1}{^1T_2}{^2T_3} \
&= \begin{bmatrix}
c(\theta_1+\theta_2)c(\theta_3) & -c(\theta_1+\theta_2)s(\theta_3) & -s(\theta_1+\theta_2) & bc(\theta_1+\theta_2)+ac(\theta_1) \
s(\theta_1+\theta_2)c(\theta_3) & -s(\theta_1+\theta_2)s(\theta_3) & c(\theta_1+\theta_2) & bs(\theta_1+\theta_2)+as(\theta_1) \
-s(\theta_3) & -c(\theta_3) & 0 & 0 \
0 & 0 & 0 & 1
\end{bmatrix}
\end{align}
$$
P点位置计算:
在坐标系3中，P点的位置向量是 \(^3r_p = [-e\cos\alpha, -e\sin\alpha, c, 1]^T\)。
例2：SCARA 机械臂正向运动学
D-H 参数表:
| No. | \(a_i\) | \(\alpha_i\) | \(d_i\) | \(\theta_i\) |
|---|---|---|---|---|
| 1 | \(l_1\) | 0 | 0 | \(\theta_1\) |
| 2 | \(l_2\) | 0 | 0 | \(\theta_2\) |
| 3 | 0 | 0 | 0 | \(\theta_3\) |
| 4 | 0 | -180° | \(d(d_0)\) | 0 |
变换矩阵:
$$
^0T_1 = \begin{bmatrix} c\theta_1 & -s\theta_1 & 0 & l_1c\theta_1 \ s\theta_1 & c\theta_1 & 0 & l_1s\theta_1 \ 0 & 0 & 1 & 0 \ 0 & 0 & 0 & 1 \end{bmatrix}, \
^1T_2 = \begin{bmatrix} c\theta_2 & -s\theta_2 & 0 & l_2c\theta_2 \ s\theta_2 & c\theta_2 & 0 & l_2s\theta_2 \ 0 & 0 & 1 & 0 \ 0 & 0 & 0 & 1 \end{bmatrix}
$$
$$
^2T_3 = \begin{bmatrix} c\theta_3 & -s\theta_3 & 0 & 0 \ s\theta_3 & c\theta_3 & 0 & 0 \ 0 & 0 & 1 & 0 \ 0 & 0 & 0 & 1 \end{bmatrix}, \
^3T_4 = \begin{bmatrix} 1 & 0 & 0 & 0 \ 0 & -1 & 0 & 0 \ 0 & 0 & -1 & d \ 0 & 0 & 0 & 1 \end{bmatrix}
$$
总变换矩阵 \(^0T_4\):
$$
\begin{align}
^0T_4 &= {^0T_1}{^1T_2}{^2T_3}{^3T_4} \
&= \begin{bmatrix}
c(\theta_1+\theta_2+\theta_3) & s(\theta_1+\theta_2+\theta_3) & 0 & l_1c\theta_1+l_2c(\theta_1+\theta_2) \
s(\theta_1+\theta_2+\theta_3) & -c(\theta_1+\theta_2+\theta_3) & 0 & l_1s\theta_1+l_2s(\theta_1+\theta_2) \
0 & 0 & -1 & d \
0 & 0 & 0 & 1
\end{bmatrix}
\end{align}
$$
例3：6R 机械臂正向运动学
D-H 参数表:
| No. | \(a_i\) | \(\alpha_i\) | \(d_i\) | \(\theta_i\) |
|---|---|---|---|---|
| 1 | 0 | 90° | \(l_1\) | \(\theta_1\)(90°) |
| 2 | \(l_2\) | 0 | \(d_2\) | \(\theta_2\) |
| 3 | 0 | 90° | 0 | \(\theta_3\)(90°) |
| 4 | 0 | -90° | \(l_3\) | \(\theta_4\) |
| 5 | 0 | 90° | 0 | \(\theta_5\) |
| 6 | 0 | 0 | 0 | \(\theta_6\) |
变换矩阵计算:
- 这里 \(c_i, s_i\) 分别代表 \(\cos\theta_i, \sin\theta_i\)，\(c_{23}, s_{23}\) 分别代表 \(\cos(\theta_2+\theta_3), \sin(\theta_2+\theta_3)\)。
最终变换矩阵:
$$
^0T_6 = {^0T_1}{^1T_2}{^2T_3}{^3T_4}{^4T_5}{^5T_6} = {^0T_3}{^3T_6}
$$
$$
^0T_7 = {^0T_3}{^3T_6}{^6T_7} = {^0T_6}{^6T_7}
$$
3.4 Non-standard DH Parameters
- 如果机构是一个闭链，其中基座二元连杆分别连接连杆1和连杆n，则标准DH参数将变得低效。
- 这会引起歧义，因为基座连杆将有两个不同的附加坐标系。
| 闭链 (Closed chain) | 开放链 (Open chain) |
|---|---|
| 3个活动连杆 (3 active links) | 3个活动连杆 (3 active links) |
| 4个关节 (4 joints) | 3个关节 (3 joints) |
为了应对闭链机构，可以通过改变附体坐标系的位置来修改标准DH参数。
- 标准 (Standard)
局部坐标系 \(i\) 建立在二元连杆的远端。 - 非标准 (Non-standard)
局部坐标系 \(i\) 建立在二元连杆的近端。
其定义：
- \(a_i\) 是沿 \(x_i\) 轴的 \(z_i\) 和 \(z_{i+1}\) 轴之间的距离。
- \(\alpha_{i-1}\) 是绕 \(x_{i-1}\) 轴从 \(z_{i-1}\) 轴到 \(z_i\) 轴的角度。
- \(d_i\) 是沿 \(z_i\) 轴的 \(x_{i-1}\) 和 \(x_i\) 轴之间的距离。
-
\(\theta_i\) 是绕 \(z_i\) 轴从 \(x_{i-1}\) 轴到 \(x_i\) 轴的角度。
-
连杆参数: \(\alpha_i, a_i\)
- 关节参数: \(\theta_i, d_i\)
变换步骤:
- \(B_{i-1}\) 绕 \(x_{i-1}\) 旋转 \(\alpha_{i-1}\)
- \(B_{i-1}\) 沿 \(x_{i-1}\) 平移 \(a_{i-1}\)
- \(B_{i-1}\) 沿 \(z_i\) 平移 \(d_i\)
- \(B_{i-1}\) 绕 \(z_i\) 旋转 \(\theta_i\)
Homogeneous Transformation Matrix (Non-standard)
通过四个齐次变换矩阵的前乘或后乘，可以获得从坐标系 \(B_{i-1}\) 到 \(B_i\) 的总变换。
$$
^{i-1}T_i = R_{x_{i-1}}(\alpha_{i-1}) D_{x_{i-1}}(a_{i-1}) D_{z_i}(d_i) R_{z_i}(\theta_i)
$$
- 优点:
非标准DH方法的一个优点是旋转 \(\theta_i\) 是围绕 \(z\) 轴进行的，并且关节编号与坐标编号相同。 - 缺点:
一个缺点是变换矩阵混合了 \(i-1\) 和 \(i\) 的索引。
Ex 3-4-1: 3R 机械臂的非标准 DH 参数
题目：为3R机器人臂填写非标准DH参数表。
| Frame No. | \(a_{i-1}\) | \(\alpha_{i-1}\) | \(d_i\) | \(\theta_i\) |
|---|---|---|---|---|
| 1 | 0 | 0 | 0 | \(\theta_1\) |
| 2 | a | 0 | 0 | \(\theta_2\) |
| 3 | b | -90° | 0 | \(\theta_3\) |
Ex 3-4-2: PUMA200 机器人的非标准 DH 参数
题目：为 PUMA200 机器人填写非标准 DH 参数表。
解:
| No. | \(a_{i-1}\) | \(\alpha_{i-1}\) | \(d_i\) | \(\theta_i\) |
|---|---|---|---|---|
| 1 | 0 | 0 | 0 | \(\theta_1\) |
| 2 | 0 | -90° | d | \(\theta_2\) |
| 3 | b | 0 | -a | \(\theta_3\) |
| 4 | 0 | -90° | 0 | \(\theta_4\)(-90°) |
| 5 | 0 | 90° | c | \(\theta_5\) |
| 6 | 0 | 90° | 0 | \(\theta_6\)(90°) |