【机器人学与计算机视觉基础】（一）位置与姿态描述 1 位姿的抽象符号表示

【机器人学与计算机视觉基础】（一）位置与姿态描述 2 二维空间位姿描述

（三）三维空间位姿描述

三维情况实际上是二维情况的延伸。我们在二维坐标系上增加一个额外的坐标轴,通常用$z$表示，它同时与$x$轴和$y$轴正交。$z$轴的方向服从右手规则：

与各坐标轴平行的单位向量记作$\hat{x}、\hat{y}$和$\hat{z}$，有：

$$\hat{z}=\hat{\mathbf{x}}\times \hat{\mathbf{y}},\hat{\mathbf{x}}=\hat{\mathbf{y}}\times \hat{\mathbf{z}};\hat{\mathbf{y}}=\hat{z}\times \hat{\mathbf{x}}$$

坐标系中的一个点$P$可用其$x,y$和$z$的坐标值$(x，y，z)$或者一个约束向量表示:
$$\mathbf{P}=x\hat{\mathbf{x}}+y\hat{\mathbf{y}}+z\hat{z}$$

上图展示了一个相对于参考坐标系 { A } \{A\} {A}的坐标系 { B } \{B\} {B}。可以清楚地看到, { B } \{B\} {B}是由 { A } \{A\} {A}原点通过向量$t=(x,y，z)$进行平移，然后再通过某种复杂方式进行了旋转得到的。正如二维情况一样,重点是我们要如何表示坐标系之间的方向。

与二维一样，我们的做法还是从相对于每个坐标系的任意一点$Р$出发，然后再确定${}^{A}P$和${}^{B}P$之间的关系。我们仍然从两方面考虑：旋转和平移。

3.1 三维空间姿态描述

欧拉旋转定理（奎伯斯）：任何两个独立的正交坐标系都可以通过一系列（不超过3次）相对于坐标轴的旋转联系起来，其中连续的两次旋转不能绕同一轴线。

---

围绕向量旋转：把你的右手绕在向量上并用拇指指向向量箭头的方向。你的其他四个手指卷曲的方向就是角度增加的方向。

---

首先考虑绕单个坐标轴的旋转。下图显示了一个右手坐标系，以及它绕不同坐标轴旋转不同角度的情形。

但绕坐标轴连续旋转就没那么简单了,如图下所示，

它演示了一个坐标系按不同的顺序旋转的情形。可以看到，虽然旋转角度相同,但最终得到的坐标系方向完全不同，它取决于旋转的顺序。旋转顺序是不可交换的。

数学家已经提炼出许多表示旋转的方法，我们将在后续部分讨论其中几种：正交旋转矩阵(Rxyz),欧拉和卡尔丹角(Euler)，横滚-俯仰-偏航角(RPY)，绕任意向量旋转（angvec），以及单位四元数(Q)。

其他：
P：平移
R：旋转
T：平移+旋转
plot：画图

下图中展示了各中表示各种方式之间的关系：

3.1.1 正交旋转矩阵

正如在二维情况下一样，我们可以用相对于参考坐标系的坐标轴单位向量表示它们所在坐标系的方向。每一个单位向量有3个元素，它们组成了3×3阶正交矩阵${}^A{R}_B$：
$$\left(\begin{array}{l}{}^{A}x\\ {}^{A}y\\ {}^{A}z\end{array}\right)={}^A{R}_B\left(\begin{array}{l}{}^{B}x\\ {}^{B}y\\ {}^{B}z\end{array}\right)$$

上式将一个相对于坐标系 { B } \{B\} {B}的向量旋转为相对于坐标系 { A } \{A\} {A}的向量。矩阵$R$属于特殊三维正交群。它具有标准正交矩阵的特性，如$R^T=R^{-1}$以及$det(R)=1$。

绕$x，y，z$轴旋转$\theta$角后的标准正交旋转矩阵可表示为：
$$R_x(\theta )=\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & -\sin \theta \\ 0& \sin \theta & \cos \theta \end{array}\right)$$

$$R_y(\theta )=\left(\begin{array}{ccc}\cos \theta & 0& \sin \theta \\ 0& 1& 0\\ -\sin \theta & 0& \cos \theta \end{array}\right)$$

$$R_z(\theta )=\left(\begin{array}{ccc}\cos \theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right)$$

工具箱（关注微信公众号二进制人工智能回复robot获取。）提供了一些函数来计算这些基本的旋转矩阵：
$R_x(\theta )$

R=rotx(pi/2);
trplot(R)

$R_y(\theta )$

R=roty(pi/2);
trplot(R)

$R_z(\theta )$

R=rotz(pi/2);
trplot(R)

旋转动画

tranimate(R)

旋转复合：先绕$x$旋转90度，再绕$y$旋转90度

R0=rotx(0);
R1=rotx(pi/2);
R2=roty(pi/2);
tranimate(R0,R1)
tranimate(R1,R1*R2)

颠倒旋转顺序

R0=rotx(0);
R1=roty(pi/2);
R2=rotx(pi/2);
tranimate(R0,R1)
tranimate(R1,R1*R2)

只显示结果：

trplot(rotx(pi/2)*roty(pi/2))
hold on
trplot(roty(pi/2)*rotx(pi/2),'color','r')

可以看出旋转的不可交换性。

正交矩阵有9个元素，但它们不是独立的。每一列都是单位长度，这提供了3个约束。列与列之间相互正交，又提供了另外3个约束1。9个元素加上6个约束，实际上只有3个独立的值。

3.1.2 三角度表示法

欧拉旋转定理要求绕3个轴依次旋转，但不能绕同一轴线连续旋转两次。旋转顺序分为欧拉式和卡尔式。

• 欧拉式：绕一个特定的轴重复旋转，但不是连续的$XYX,XZX,YXY,YZY,ZXZ,ZYZ$
• 卡尔式：绕三个不同轴旋转。$XYZ,XZY,YZX,YXZ,ZXY,ZYX$

通常，我们将以上12个序列均称为欧拉角。

ZYZ序列的欧拉角旋转：
$$R=R_z(\varphi )R_y(\theta )R_z(\psi )$$

它常用在航天和机械动力学中。欧拉角是一个三维向量$\Gamma (\varphi ,\theta ,\psi )$
例如，要计算$\Gamma =(0.1,0.2,0.3)$的等价旋转矩阵，我们可以这样写：

R=rotz(0.1)*roty(0.2)*rotz(0.3);

或采用更简便的方式：

R=eul2r(0.1, 0.2, 0.3) 
R =

    0.9021   -0.3836    0.1977
    0.3875    0.9216    0.0198
   -0.1898    0.0587    0.9801

上述问题的逆命题是要找到给定旋转矩阵的欧拉角，可采用以下函数：

gamma=tr2eul(R)
gamma =

    0.1000    0.2000    0.3000

但是，如果$\theta$为负时，如：

R=eul2r(0.1,-0.2,0.3)
R =

    0.9021   -0.3836   -0.1977
    0.3875    0.9216   -0.0198
    0.1898   -0.0587    0.9801

其反函数结果为：

tr2eul(R)
ans =

   -3.0416    0.2000   -2.8416

它返回了一个正的$\theta$值和两个与前一组欧拉角不同的$\varphi ,\psi$。但这组欧拉角对应的旋转矩阵仍与前一组是相同的：

eul2r(ans)
ans =

    0.9021   -0.3836    0.1977
    0.3875    0.9216    0.0198
   -0.1898    0.0587    0.9801

两组不同的欧拉角对应同一个旋转矩阵，说明从旋转矩阵到欧拉角的映射是不唯一的，而工具箱函数返回的角度$\theta$始终为正。

对于$\theta =0$的情况：

R=eul2r(0.1,0,0.3)
tr2eul(R)
R =

    0.9211   -0.3894         0
    0.3894    0.9211         0
         0         0    1.0000


ans =

         0         0    0.4000

反函数返回的角度值与原值不同。其实在这种情况下，从式子$$R=R_z(\varphi )R_y(\theta )R_z(\psi )$$
得出的旋转矩阵是：
$$R=R_z(\varphi )R_z(\psi )=R_z(\varphi +\psi )$$

由于$R_z=I$，所以可化简为只是$\varphi +\psi$的一个函数。对于逆运算而言，只能确定这个和的值。要想得到其中每个值，只能按惯例取$\varphi =0$。
$\theta =0$的情况下实际上是一个奇异点。

另一种广泛使用的旋转角顺序为$XYZ$（横滚(红色)-俯仰（紫色）-偏航角（绿色）），即：
$$R=R_x({\theta }_r)R_y({\theta }_p)R_z({\theta }_y)$$

其用于描述船舶、飞机和车辆的姿态时非常直观。

对于航空及地面车辆而言，一般$x$轴为前进的方向、$z$轴垂直向下，$y$轴指向右手方向，例如：

R = rpy2r(0.1, 0.2, 0.3) 
R =

    0.9363   -0.2896    0.1987
    0.3130    0.9447   -0.0978
   -0.1593    0.1538    0.9752

求逆：

gamma=tr2rpy(R)
gamma =

    0.1000    0.2000    0.3000

横滚-卧仰-偏航序列允许每个角度值有任意正负号，不会产生多解的情况。但它也有一个奇异点，即当${\theta }_p=\pm \pi /2$，不过这个点刚好在大多数车辆可能的姿态范围以外。

3.1.3 双向量表示法

对于关节臂式机器人，一般会在它的末端执行器上固联一个坐标系$E$，如上图所示。

通常情况下，工具的轴线为坐标系的$z$轴，并被称为接近向量，记为$\hat{a}=(a_x,a_y,a_z)$。对于某些应用来说，定义接近向量比定义欧拉角或横滚-俯仰-偏航角更为方便。

然而定义出$z$轴的方向还不足以表示完整坐标系——我们还需要确定$x$轴和$y$轴的方向。为了确定末端执行器的姿态，我们定义一个与接近向量正交的向量，它位于机器人手爪的两个手指之间，被称为姿态向量，记为$\hat{o}=(o_x,o_y,o_z)$。有了接近和姿态这两个单位向量，就足以完全确定旋转矩阵了：
$$\mathbf{R}=\left(\begin{array}{lll}{n}_x& o_x& a_x\\ n_y& o_y& a_y\\ n_z& o_z& a_z\end{array}\right)$$

因为剩下的一列$\hat{n}$可由两两正交得到，即$\hat{n}=\hat{o}\times \hat{a}$

即使两个向量$\hat{a}$和$\hat{o}$不是正交的，它们仍然能定义一个平面，而且计算出来的$\hat{n}$仍垂直于这个平面。在这种情况下，我们需要用${\hat{o}}^{\prime }=\hat{a}\times \hat{n}$来计算一个新的向量值，它仍然在$\hat{a}$和$\hat{o}$定义的平面中，只不过与$\hat{a}$和$\hat{n}$都正交。

使用工具箱函数，可以执行以下计算：

a=[1,0,0]';
o=[0,1,0]';
oa2r(o,a)
ans =

     0     0     1
     0     1     0
    -1     0     0

从上文可以知道，任何两个非平行的向量都足以定义一个坐标系。

3.1.4 绕任意向量旋转

对于空间中两个任意姿态的坐标系，总可以在空间里找到某个轴,使其中一个坐标系绕该轴旋转一个角度就能与另一个坐标系姿态重合。以先前使用过的一一个旋转为例：

R=rpy2r(0.1,0.2,0.3);

我们可以确定如下的一个角度和一个向量：

[theta,v]=tr2angvec(R)
theta =

    0.3816


v =

    0.3379    0.4807    0.8092

其中theta是旋转的角度大小，v即我们所说的任意向量。

这些信息实际上包含在$R$的特征值和特征向量中。使用eig可以求矩阵的特征值和特征向量：

[v,lambda]=eig(R)
v =

   0.6655             0.6655             0.3379          
  -0.1220 - 0.6079i  -0.1220 + 0.6079i   0.4807          
  -0.2054 + 0.3612i  -0.2054 - 0.3612i   0.8092          


lambda =

   0.9281 + 0.3724i        0                  0          
        0             0.9281 - 0.3724i        0          
        0                  0             1.0000  

一个正交旋转矩阵总有一个实特征值$\lambda =1$,以及一对共轭复特征值$\lambda =cos\theta 土isin\theta$,其中$\theta$代表旋转的角度。根据特征值和特征向量的定义有
$$Rv=\lambda v$$
其中，$v$是对应于$\lambda$的特征向量。当$\lambda =1$时：
$$Rv=v$$
这意味着对应的这个特征向量$v$是不随旋转发生改变的。这样的向量只有一个，而旋转就是以这个向量为轴发生的。上面的例子中第三个特征值为1，所以旋转轴应是矩阵$v$中的第三列。
反过来，使用罗德里格斯（Rodrigues）旋转矩阵，可以从角度和向量计算出相应的旋转矩阵：
$$R=I_{3\times 3}+sin\theta S(v)+(1-cos\theta )(v{v}^T-I_{3\times 3})$$
前面我们熟悉的一个绕$x$轴旋转$\pi /2$的例子，其旋转矩阵为：

R=angvec2r(pi/2,[1,0,0])
R =

    1.0000         0         0
         0    0.0000   -1.0000
         0    1.0000    0.0000

我们注意到,绕任意轴旋转这种表示法都可以参数化为4个数字: 3个代表旋转轴(即$v$的三个元素), 1个代表旋转角度（即$\theta$）。

然而，空间中的方向可以用一个只有两个独立参数的单位向量表示，因为第三个元素可以由下式计算:
$$v_3=\sqrt{1-v_1^2-v_2^2}$$

这样真正独立的参数只有3个。或者,我们也可以把单位向量和角度相乘,得到另外一种3个参数的表示方法$v_{\theta }$。虽然这些表示方法对于数据存储来说是最小和最高效的，但在分析研究时是存在问题的。其他学者还提出过多种变形，包括$vsin(theta/2)$和$vtan(0)$,但它们在$\theta =0$时都呈现病态。

3.1.5 单位四元数

四元数是复数的一种扩展，或叫超复数，记作一个标量加上一个向量：
$$\dot{q}=s+v=s+v_{1}i+v_{2}j+v_{3}k$$
其中$s\in R$，$v\in R^3$，正交复数$i,j$和$k$定义如下：
$$i^2=j^2=k^2=ijk=-1$$

我们将一个四元数表示为：
$$\dot{q}=s<v_1,v_2,v_3>$$

早期反对四元数的一个理由是其乘法不可交换，但正如我们在上面看到的,这种不可交换性正好符合坐标系旋转的情况。除去最初的争论不说，四元数以其格式优雅、功能强大、计算简单已被广泛应用于机器人、计算机视觉、计算机图形学以及航空航天惯性导航领域。

工具箱中用类Quaternion实现四元数如下：

q=Quaternion(rpy2tr(0.1,0.2,0.3))
q = 
 
0.98186 < 0.064071, 0.091158, 0.15344 >

为了描述坐标系的旋转，我们使用单位四元数。这些四元数为一个单位大小，即$|\dot{q}|=1$或$s^2+v_1^2+v_2^2+v_3^2=1$，例如：

q.norm

ans =

    1.0000

单位四元数具有一个特殊特性，它可以被看作是绕单位向量$\hat{n}$旋转了$\theta$角，该旋转与四元数组的关系为：
$$s=cos\frac{\theta }{2}，v=(sin\frac{\theta }{2})\hat{n}$$

Quaternion类可以重载一些标准方法和函数。四元数的乘法通过重载乘法运算符调用：

q*q
ans = 
 
0.92808 < 0.12582, 0.17901, 0.30131 >

如果将四元数表示为4元向量$\left(s,v_1,v_2,v_2\right)$,则四元数之间的乘积可表示为一个矩阵-向量积:
$$\dot{q}\oplus {\dot{q}}^{\prime }=\left(\begin{array}{rrrr}s& -v_1& -v_2& -v_3\\ v_1& s& -v_3& v_2\\ v_2& v_3& s& -v_1\\ v_3& -v_2& v_1& s\end{array}\right)\left(\begin{array}{c}{s}^{\prime }\\ v_1^{\prime }\\ v_2^{\prime }\\ v_3^{\prime }\end{array}\right)$$

两个正交旋转矩阵的复合需要进行27次乘法和18次加法，而通过四元数的形式只需要16次乘法和12次加法，这对嵌入式系统尤其重要。

求一个四元数的共轭为：

q.inv()
0.98186 < -0.064071, -0.091158, -0.15344 >

一个四元数乘以它的逆四元数为：

q*q.inv
ans = 
 
1 < 0, 0, 0 >

得出一个单位四元数，它代表一个无效旋转。

一个四元数可以用以下方式转化为一个正交旋转矩阵：

q.R
ans =

    0.9363   -0.2896    0.1987
    0.3130    0.9447   -0.0978
   -0.1593    0.1538    0.9752

我们也可以用以下函数绘制一个四元数所指的方向：

q.plot()

对于四元数：
$$\dot{q}=s+v=s+v_{1}i+v_{2}j+v_{3}k$$

其广义位姿是$\xi \sim q\in \mathbb{Q}$，且
$${\dot{q}}_1\oplus {\dot{q}}_2\mapsto s_1{s}_2-v_1\cdot v_2,<s_1{v}_2+s_2{v}_1+v_1\times v_2>$$
上式被称为四元数积或汉密尔顿积,并有
$$\ominus \dot{q}\mapsto {\dot{q}}^{-1}=s,<-v>$$

这是四元数的共辄。零位姿$0\to 1<0,0,0>$，为单位四元数。一个向量$\mathbf{v}\in {\mathbb{R}}^3$被旋转，表示为$\dot{q}\cdot \mathbf{v}\mapsto \dot{q}\dot{q}(\mathbf{v})\dot{q}-1$，其中$\dot{q}(v)=0$,$<v>$被称为纯四元数。

将一个三元向量传递给构造函数，将产生一个纯四元数：

Quaternion([1,2,3])
ans = 
 
0 < 1, 2, 3 >

其中的标量为0。使用重载的乘法运算符，一个向量可以被一个四元数旋转：

q*[1,0,0]'
ans =

    0.9363
    0.3130
   -0.1593

3.2 平移与旋转组合

现在我们回到三维空间中相对位姿的表示，即两个坐标系之间位置和姿态的变化。3.1介绍了几种不同的姿态表示法，现在我们需要将它们与平移变换结合，创造出一个完整的相对位姿表示方法。

描述三维空间相对位姿的两个实用方法是：四元数向量对和4×4齐次变换矩阵。

3.2.1 四元数向量对

对于向量-四元数的情况，有$\xi \sim (t,\stackrel{\circ }{q})$，其中$t\in {\mathbb{R}}^3$是坐标系原点相对于参考坐标系的笛卡儿位置，$q\in \mathbb{Q}$是坐标系相对于参考坐标系的姿态。

加法定义如下：

$${\xi }_1\oplus {\xi }_2=\left(t_1+{\dot{q}}_1\cdot t_2,q_1\oplus {\hat{q}}_2\right)$$

取负数为：
$$\ominus \xi =\left(-{\dot{q}}^{-1}\cdot t,{\dot{q}}^{-1}\right)$$

一个点坐标向量通过下式在坐标系之间变换:
$${}^X\mathbf{p}={}^X{\xi }_Y\cdot {}^Y\mathbf{p}=\dot{q}\cdot {}^Y\mathbf{p}+\mathbf{t}$$

3.2.2 4×4齐次变换矩阵

推导方法与二维情况相似，但因为增加了$z$轴，所以进行扩展：

$$\left(\begin{array}{c}{}^{A}x\\ {}^{A}y\\ {}^{A}z\\ 1\end{array}\right)=\left(\begin{array}{ll}{}^A{R}_B& t\\ 0_{1\times 3}& 1\end{array}\right)\left(\begin{array}{c}{}^{B}x\\ {}^{B}y\\ {}^{B}z\\ 1\end{array}\right)$$

坐标系原点之间的笛卡儿平移向量是$t$，姿态的变化由一个3×3正交子矩阵$R$表示，其余向量都表示成齐次形式,这样可以写成

$$\begin{array}{rl}{}^A\tilde{\mathbf{p}}& =\left(\begin{array}{cc}{}^A{\mathbf{R}}_B& \mathbf{t}\\ 0_{1\times 3}& 1\end{array}\right){}^B\tilde{\mathbf{p}}\\ & ={}^A{\mathbf{T}}_B{}^B\tilde{\mathbf{p}}\end{array}$$

${}^A{T}_B$是$4\times 4$阶齐次变换，属于特殊的三维欧几里得群，记作
$$T\in SE(3)\subset {\mathbb{R}}^{4\times 4}$$

相对位姿的一个具体表现形式是$\xi \sim T\in SE(3)$，
与二维类似地，

$${\mathbf{T}}_1\oplus {\mathbf{T}}_2\mapsto {\mathbf{T}}_1{\mathbf{T}}_2$$
$$T_1{T}_2=\left(\begin{array}{cc}{R}_1& t_1\\ 0_{1\times 3}& 1\end{array}\right)\left(\begin{array}{cc}{R}_2& t_2\\ 0_{1\times 3}& 1\end{array}\right)=\left(\begin{array}{cc}{R}_1{R}_2& t_1+R_1{t}_2\\ 0_{1\times 3}& 1\end{array}\right)$$
这是标准的矩阵乘法。

$$\xi \oplus 0=\xi \mapsto TI=T$$

其中$I$为单位矩阵。

$$\xi \ominus \xi =0\mapsto T{T}^{-1}=I$$

这意味着
$$\ominus \mathbf{T}\mapsto {\mathbf{T}}^{-1}$$
且：
$${\mathbf{T}}^{-1}={\left(\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0_{1\times 3}& 1\end{array}\right)}^{-1}=\left(\begin{array}{cc}{\mathbf{R}}^T& -{\mathbf{R}}^T\mathbf{t}\\ 0_{1\times 3}& 1\end{array}\right)$$

工具箱中有许多函数可以创建齐次转换，例如：

T= transl(1,0,0)* trotx(pi/2) * transl(0,1,0)
T =

    1.0000         0         0    1.0000
         0    0.0000   -1.0000    0.0000
         0    1.0000    0.0000    1.0000
         0         0         0    1.0000

函数transl创建了一个有平移但无旋转的相对位姿,而函数trotx则返回一个绕x轴旋转T/2的4×4齐次变换矩阵:旋转部分与rotx(pi/2)相同，平移部分为零。

我们可以将以上函数组合对应的坐标系变化描述如下:

• 首先沿着x轴方向前进一个单位长度
• 然后绕x轴旋转90°
• 接着再沿新的y轴，也就是原来的z轴前进一个单位长度。

所得矩阵的最后一列，表示了沿原坐标系的x轴和z轴各平移一个单位长度的结果。从最终位姿矩阵的姿态部分,则可以看出是绕x轴旋转的效果。

我们可以用如下函数绘制出相应的坐标系:

 trplot(T)

要提取矩阵$T$中的旋转矩阵部分，可用：

t2r(T)
ans =

    1.0000         0         0
         0    0.0000   -1.0000
         0    1.0000    0.0000

平移部分：

transl(T)

ans =

    1.0000
    0.0000
    1.0000

参考：

[1] 机器人学、机器视觉与控制：MATLAB算法基础；Peter Corke 著; 刘荣等译.

[2] Robotics，Vision and Control Fundamental Algorithms in MATLAB；Peter Corke

THE END.

感谢阅读。