坐标系与矩阵(5): Denavit-Hartenberg算法

上一篇我介绍了坐标系与矩阵的应用之一：ECEF与ENU坐标转换的相关的概念。本篇介绍坐标系在动力学中的应用场景，这里则涉及到Denavit-Hartenberg(DH) Algorithm。

在动力学中，比如人的胳膊就有好几个关节，且不同的关节有不同的旋转轴，如果是路飞的话，关节之间的长度还是不固定的。这里，每一个关节都存在一个自身坐标系，其中旋转可以是绕三个轴，平移则是沿着三个轴，每个坐标系存在6个自由度。问题就有点复杂了，每个人对每个关节可能会定义不同的坐标系方向，这会直接决定求解该问题的难度。DH算法则提供了一个一般性理论，且每一个关节只需要4个自由度。

DH满足四个规则：

• 每一个关节z轴和关键轴的方向相同
• x轴和当前关节的

和上一关节

垂直，如果不唯一，则方向从

到

• y轴由右手坐标系确定

轴必须和

相交

根据该规则，以上图为例，来确定每一个关节的坐标系。

(0)由左向右，确定第一个关节的

，这里

有多个选择，你选择一个正常的就可以，然后根据右手坐标系确定

(1)确定第二个关节的

，

有两个选择，这里选择向右，根据右手坐标系确定

(2)同理，依次确定第三个关节的

、

和

，第四个关节的的

、

和

(3)第四个关节的特殊点在于它有两个旋转轴，因此，我们需要在对其建立另一个坐标系，确定对应的

、

和

根据上图确定最后一个关节的坐标轴

(3)最后的终端称为tool，我们定义对应的approach vector

，sliding vector

，另一个轴则是normal vector

，确定对应的

、

和

如上，我们确定了每一个节点的坐标系，但这还不够，我们需要确定相邻坐标系之间的旋转和平移参数。参数计算规则如下：

1. 确定辅助点

位置，是轴

和轴

，如果没有相交，则是轴

和轴

的法线与轴

的交点

1. 计算

，是绕轴

从

到

的角度

1. 计算

，是从坐标系

的原点沿着轴

到

的距离

1. 计算

，是从

沿着

到坐标系

的原点的距离

1. 计算

，是绕轴

从

到

的角度

上图是从

到

的转换步骤（1-5）:

从

到

：

从

到

：

从

到

：

从

到

：

如上，我们首先确定了每个关节的坐标系，进而确定关节的四个参数，对应其四个自由度，这样，我们按照如下规则计算两个相邻关节之间的转换矩阵，该矩阵将

上的点

转为

上对应的点

：

我们把从

到

中相邻坐标系，可以建立一个参数表：

当我们需要将

上的点

转到

坐标系下的点

，对应的 转换矩阵为：

例子1

对上图建立每个关节的坐标系

例子2

对上图建立每个关节的坐标系

答案：

DH算法的介绍到此结束。下一篇是OpenGL中基础的模型视图投影矩阵。

参考资料：Motion and Manipulation

https://www.cs.uu.nl/docs/vakken/moma/2019.html