如何在编程逻辑中将链接参数和角度（在运动学中）转换为变换矩阵？

我作为一名本科生正在从事机器人研究，并且我大部分时间都了解概念数学。但是，在实际实现代码以计算机器人的正向运动学时，我陷入了困境。我只是不了解找到的书或网站的解释方式。

我想计算XYZ角度给出的链路参数（Denavit-Hartenberg的参数），诸如以下：

$$\begin{array}{ccc} \bf{i} & \bf{\alpha_i-1} & \bf{a_i-1} & \bf{d_i} & \bf{\theta_i}\\ \\ 1 & 0 & 0 & 0 & \theta_1\\ 2 & -90^{\circ} & 0 & 0 & \theta_2\\ 3 & 0 & a_2 & d_3 & \theta_3\\ 4 & -90^{\circ} & a_3 & d_4 & \theta_4\\ 5 & 90^{\circ} & 0 & 0 & \theta_5\\ 6 & -90^{\circ} & 0 & 0 & \theta_6\\ \end{array}$$

我不明白如何将这个值表转换为获取，最后一个链接的笛卡尔位置和旋转所需的适当转换矩阵。从那里，我希望我能从阅读本书中找出XYZ角，但是任何帮助都将不胜感激。$^0T_N$

维基百科上DH页的DH Matrix部分提供了详细信息。

基本上，您想使用表中的信息来创建一组齐次转换矩阵。我们这样做是因为可以将齐次变换相乘以找到由一个或多个其他帧分隔的帧之间的关系。例如，表示从帧1到帧0 的转换，而表示从帧2到帧1的转换。通过将它们相乘，我们得到从帧2到帧0的转换，即。$^0T_1$$^1T_2$$^0T_2 = ^0T_1^1T_2$

创建每个变换的一种简单方法是为表中的每一列进行齐次变换或齐次旋转矩阵，并将它们相乘。例如，从1到0的转换（例如）是$^{i-1}T_i, i = 1$

$^0T_1 = Trans(d_1)*Rot(\theta_1)*Trans(a_2)*Rot(\alpha_2)$

哪里

$Trans(d_1) = \begin{bmatrix}1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & \bf{d_1 = 0} \\ 0 & 0 & 0 & 1 \end{bmatrix},$

$Rot(\theta_1) = \begin{bmatrix} \text{cos}(\bf{\theta_1}) & - \text{sin}(\bf{\theta_1}) & 0 & 0 \\ \text{sin}(\bf{\theta_1}) & \text{cos}(\bf{\theta_1}) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix},$

$Trans(a_2) = \begin{bmatrix} 1 & 0 & 0 & \bf{a_2 = 0} \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix},$

$Rot(\alpha_2) = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & \text{cos}(\bf{\alpha_2 = 0}) & -\text{sin}(\bf{\alpha_2 = 0}) & 0 \\ 0 & \text{sin}(\bf{\alpha_2 = 0}) & \text{cos}(\bf{\alpha_2 = 0}) & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$。

在这种情况下

$^0T1 = Rot(\theta_1)$。

一旦完成所有转换，就将它们相乘，例如

$^0T_N = ^0T_1*^1T_2...^{N-1}T_N$。

最后，您可以从均匀变换读取位移矢量（即）。同样，您可以从读出旋转矩阵以找到XYZ角。$^0T_N$$d = [^0T_{N,14}, ^0T_{N,24}, ^0T_{N,34}]^T$$^0T_N$