机器人学的数学基础

第3章机器人学的数学基础在机器人操作手工作时，我们需要在其特定三维工作空间中掌握各个物体之间的几何关系，这些物体包括操作手组成自身的各个活动杆件、底座、末端执行器、抓持工具、待抓取物体、障碍物等，它们之间的三维空间几何关系可用两个非常重要的特性来描述：位置和姿态。

3.1 位置和姿态表示为了精确描述各个连杆或物体之间的位置和姿态关系，我们首先定义一个固定的坐标系，并以它作为参考坐标系，所有静止或运动的物体就可以统一在同一个参考坐标系中进行比较。

该坐标系统通常被称为世界坐标系。

基于此共同的坐标系描述机器人自身及其周围物体，是机器人在三维空间中工作的基础。

通常，我们对每个物体或连杆都定义一个本体坐标系，又称局部坐标系，每个物体与附着在该物体上本体坐标系是相对静止的,即其相对位置和姿态是固定的。

因此，每个物体之间位置和姿态的关系就可以用它们自身的本体坐标系之间的位姿关系来确定了，本体坐标系原点之间的关系代表了它们的位置关系，本体坐标系各个坐标轴方向之间的关系代表了方位关系。

图3-1表示了机器人手臂及其周围物体在世界坐标系∑w中及各自本体坐标系中的位置和姿态。

zyxz∑W y z xx zzzzzz x yyx p z zy\图 3-1 机器人手臂及其周围物体的位置和姿态3.1.1 位置描述建立坐标系之后，三维空间中的任何一点都可以用一个具有三个分量的位置矢量来进行定位。

例如， 图3-1中立方体的质心p 在世界坐标系中的表示是：⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=wz wy wx w p p p p下标w 代表了世界坐标系，因为位置矢量p 在不同坐标系中数值表示不同。

以上就是典型的基于笛卡尔坐标系的三维空间位置矢量的描述方法。

当采用不同的坐标系表示时，会有不同的位置描述方法。

例如 基于圆柱坐标系的空间矢量表示方法，基于球坐标系的空间矢量表示方法等。

3.1.2 方位描述机器人手臂工作时，不但要考虑所抓取的物体的质心的位置，还要考虑空间中该物体的姿态，既方位。

空间中的物体，不但要考虑它的位置，还要考虑它的方位。

空间物体通常用自身本体坐标系的坐标原点表示位置，坐标轴的方向代表方位。

物体的相对位置和方位通常用它们各自的本体坐标系之间的关系来表示。

相对位置关系用局部坐标系的坐标原点之间的关系表示，方位关系用各自本体坐标系的坐标轴单位方向矢量之间的关系来表示。

常用的方位描述包括旋转矩阵表示法，固定角表示法，欧拉角表示法，等效轴角表示法，欧拉参数表示法等。

我们主要介绍旋转矩阵方位描述法，并简介固定角表示法，其它方法参考教科书。

A. 旋转矩阵描述法一个本体坐标系{B}相对于另一个参考坐标系{A}的姿态描述，用这个本体坐标系{B}的三个坐标轴的单位方向向量分别在参考坐标系的{A}三个坐标轴上的投影值，共9个投影分量所组成的矩阵（称作旋转矩阵）来表示两个坐标系之间的方位关系，这种方位描述方法称作旋转矩阵方位描述法。

具体解释如下： B Z A z B Y{A} {B}A YA XB X图 3-2 本体坐标系{B}在参考坐标系{A}中的方位描述如图3-2 所示，我们用 B B B Z Y X ˆ,ˆˆ,表示本体坐标系{B}的三个坐标轴的单位方向矢量，A A A Z Y X ˆ,ˆˆ,表示参考坐标系{A}的三个坐标轴的单位方向矢量。

则本体坐标系{B}的X 轴单位方向向量B X ˆ 在参考坐标系{A}中的表示即本体坐标系BX ˆ在参考坐标系{A}的三个坐标轴单位向量B B B Z Y X ˆ,ˆˆ,上的投影，我们用矢量：()()()⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡⋅⋅⋅=A B A B A B A B A B A B B A Z X COS Y X COS X X COS Z X Y X X X X ˆ,ˆˆ,ˆˆ,ˆˆˆˆˆˆˆˆ 来表示， 这三个投影值代表了本体坐标系{B}的X 轴与参考坐标系{A}的各个坐标轴的夹角的余弦值。

同样道理， 本体坐标系{B}的BB Z Y ˆ,ˆ 在参考坐标系{A}中都可以如上述表示，得出矢量：()()()⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡⋅⋅⋅=AB A B A B A B A B A B B A Z Y COS Y Y COS X Y COS Z Y Y Y X Y Y ˆ,ˆˆ,ˆˆ,ˆˆˆˆˆˆˆˆ, ()()()⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡⋅⋅⋅=A B A B A B A B A B A B B A Z Z COS Y Z COS X Z COS Z Z Y Z X Z Z ˆ,ˆˆ,ˆˆ,ˆˆˆˆˆˆˆˆ, 则，本体坐标系{B}在参考坐标系{A}中的方位描述可以用公式3-1表示,其中三行三列矩阵R A B 中的9个分量描述了了{B} 在{A}中的方位，我们称该矩阵为旋转矩阵。

旋转矩阵的每个分量代表了两个坐标系坐标轴方向夹角的余弦值。

[]⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡⋅⋅⋅⋅⋅⋅⋅⋅⋅==A B A B A B A B A B A B A B A B A B B A B AB A A B Z Z Z Z Z X Y Z Y Y Y X X Z X Y X X Z Y X R ˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆ 3-1我们从空间几何意义上知道，姿态变换后又反变换回去，最终姿态不变。

观察{B}相对于{A}的方位（描述R A B ）（图3-2），{A}相对于{B}的方位（描述R B A ）与之大小相等方向相反， 因此利用旋转矩阵描述的方位应满足如下关系：⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=⋅100010001R R A B B A 即： 1-=R R A B B A 3-2而根据公式3-1 旋转矩阵的定义，我们得到：[]⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡⋅⋅⋅⋅⋅⋅⋅⋅⋅==B A B A B A B A B A B A B A B A B A A B A BA B B A Z Z Z Z Z X Y Z Y Y Y X X Z X Y X X Z Y X R ˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆ 3-3观察矩阵3-3的第一行与矩阵3-1的第一列相同，第二行与第二列相同，第三行与第三列相同。

因此，我们得出：T A B B A R R = 3-4由公式 3-2 和3-4联立，我们得出了旋转矩阵的重要特性：T A B A B R R =-1 3-5即旋转矩阵的逆阵等于它的转置矩阵。

这就为求旋转矩阵的逆阵提供了一条简单的方法，直接转置就可以了。

利用旋转矩阵3-1的定义，我们可以得到旋转矩阵的特性：1. 旋转矩阵的行向量或列向量都是单位方向向量。

1===B A B A B A Z Y X2. 旋转矩阵的行向量或列向量彼此两两垂直。

0=⋅=⋅=⋅B A B A B A B A B A B A Z Y Z X Y X3. 旋转矩阵的9个分量中只有3个独立变量，六个约束。

而 特性1 2 3 与 公式3-5 是等效的。

（有兴趣的同学自己验证）。

因此，判断一个3*3矩阵是否是旋转矩阵的充分必要条件就是看公式3-5 是否成立。

例一 坐标系{B} 相对于坐标系{A}绕X 轴逆时针方向旋转30度，利用旋转矩阵的定义求{B} 相对于{A}旋转矩阵R A B和 {A} 相对于{B}旋转矩阵R B A 及它们之间的关系 。

解 ：A z {A}：实线B Z B Y {B}：虚线030A YA X ，B X图 3-3如图3-3所示，根据旋转矩阵的定义，根据旋转矩阵的定义和公式3-3，得到[]B A B A B AA B Z Y X R ,,=, 而 ⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=001B A X ，()()⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=5.0866.003030000SIN COS Y B A ，()()⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡-=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡-=866.05.003030000COS SIN Z B A 因此，{B}坐标系相对于{A}坐标系的旋转矩阵是：⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡-=866.05.005.0866.00001R A B 利用同样的道理，求得R B A 及它们之间的关系：T A BB A R R =⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡-=866.05.005.0866.00001通过例1的学习，我们熟悉并复习前面学到的旋转矩阵的定义和方法。

例二：判断下面的矩阵是不是有效的旋转矩阵？⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎣⎡--2102301023021 （1） ，⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎣⎡---2102301023021（2）， 解：利用旋转矩阵的特性1 和2 ，或者公式3-5 检验矩阵是不是有效的旋转矩阵，矩阵 1 中，第一列向量和第三列向量的点积是 23-，不是零，不符合旋转矩阵特性1 ，因此矩阵 1不是旋转矩阵。

矩阵2与它的转置矩阵相乘，得到： ⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎣⎡---2102301023021*⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎣⎡---2102301023021=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡100010001 满足公式3-5 旋转矩阵的条件，因此 矩阵2 是有效的旋转矩阵。

旋转矩阵的几何变换含义：从旋转矩阵的定义，我们知道它代表了局部坐标系{B}相对于参考坐标系{A}的方位（姿态）。

它还有其它的几何变换含义。

如图3-2 所示，假定{B}坐标系初始与参考坐标系{A}重合，然后经过旋转到达图示的方位， 设{B}坐标系相对于{A}坐标系的方位表示用旋转矩阵R A B 来表示。

那么， 如果{B}坐标系中存在相对于该坐标系静止的矢量⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=BZ BY BX B p p P P ，经过与坐标系{B}相同的角度旋转，它在局部坐标系{B}中的姿态与在{A}坐标系中相比，同样差了方位R A B，这可以利用坐标投影证明，公式3-6中，B A B A B A Z Y X ,, 分别是{B}坐标系的坐标轴单位矢量在{A}坐标系的坐标轴上的投影值，[][]⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=⋅⋅⋅=BZ BY BX B A BA B A B A BZ B A BY B A BX A P P P Z Y X Z P Y P X P P 3-6=B A B P R ⋅因此，我们得到如下公式：B A B A P R P ⋅= 3-7从公式中看出，矢量A P 是矢量在参考坐标系{A}中的表示，矢量B P 可以看成是局部坐标系{B}中的固定矢量，因此旋转矩阵也可以看成是对矢量进行旋转的旋转操作算子。

B 固定角方位描述法用滚动角，俯仰角和偏摆角来表示方位是航海或航空中常见的轮船及飞机的方位描述方法。

滚动角的定义是指绕固定参考坐标系的X 轴旋转的角度，俯仰角是指绕固定参考坐标系的Y 轴旋转的角度，偏摆角是指绕固定参考坐标系的Z 轴旋转的角度，表3-1 表示了固定角表示法中的常用表示。