机器人学的学与思 [ 1 ] ：旋转矩阵与其指数表达形式

科学始终是不公道的，如果它不提出十个问题，也就永远不能解决一个问题。

—— 萧伯纳

0 说在前面

机器人学（Robotics） 是一门研究机器人设计、建造、运作、应用的科学。 其中，机器人学最经典的问题就是运动学、动力学和轨迹规划等，它们是研究机器人学的基础。

机器人系统的构成，如下图所示（以工业机械臂作为例子）：

其中，对几个术语做出解释说明：

• Planning 指的是规划阶段，举机械臂运动的例子来说，输入为机器臂末端期望到达的位置。
• Controller 指的是机器人的控制器，比如PID控制器，它会接收来自编码器的反馈。
• Motor 和 Encoder 一般是连接在一起的，所以在框图上表示为两个连接的方框。
• link指的是执行器（这里指 Motor）连接的机械结构。
• 系统的输出为机械臂末端实际到达的位置，我们要做的就是让 Output 和 Input 尽量接近。

如何知道机器人末端执行器上某个点运动后在空间坐标系下的位置？如何让机器人的执行器快速、稳定地到达我们想让它到达的点？这些都是机器人学所研究的内容。

阅读本文需要读者具有高等数学与线性代数基础，那么就让我们从刚体运动中开始走进机器人学吧！

1 刚体变换

在机器人学中，我们关心的并不是各质点的独立运动，而是一组质点的共同运动。刚体（rigid body） 为一个完全不变形体，我们可以定义其是一个任意两质点距离保持不变的集合。设 $p$p 和 $q$q 是刚体上任意两点，则不论刚体如何运动， $p$p 和 $q$q 一定满足：
$||p(t) - q(t) || = ||p(0) - p(0)|| = \text{a constant}$∣∣p(t)−q(t)∣∣=∣∣p(0)−p(0)∣∣=a constant
刚体运动（rigid motion） 即表示物体上任意两质点之间距离始终保持不变的连续运动，刚体运动可以由刚体变换（rigid body transformation） 来描述。

若变换 $g: \R^3 \rightarrow \R^3$g:R3→R3 ，满足以下条件，则其称为刚体变换：

• 长度不变：对于任意点 $p,q \in \R^3$p,q∈R3，均有 $||g(p) - g(q) || = ||p - q||$∣∣g(p)−g(q)∣∣=∣∣p−q∣∣
• 叉积不变：对于任意矢量 $v,w \in \R^3$v,w∈R3 ，均有 $g_*(v \times w) = g_*(v) \times g_*(w)$g∗​(v×w)=g∗​(v)×g∗​(w)

$g()$g() 和 $g_*()$g∗​() 可以分别理解为刚体变换作用在点或矢量上。 下面利用数学语言描述最简单的刚体变换：旋转。

2 旋转矩阵

2.1 旋转矩阵的定义

设$A$A 为惯性坐标系，$B$B 为与刚体固联的动坐标系，$x_{ab},y_{ab},z_{ab} \in \mathbb{R^3}$xab​,yab​,zab​∈R3 为坐标系 $B$B 中主坐标轴相对于坐标系 $A$A 的坐标。依次排列这三个坐标矢量，构成一个 $3 \times 3$3×3 的矩阵：
$R_{ab} = [x_{ab}\ y_{ab} \ z_{ab}]$Rab​=[xab​ yab​ zab​]
按这种方式建立的矩阵称为旋转矩阵（rotation matrix）。

旋转矩阵的下标 $ab$ab 可以理解为把 $B$B 坐标系（上的点或向量）表示在 $A$A 坐标系上，或者理解为 $B$B 坐标系相对于 $A$A 坐标系的位形。

如图所示，$q_b$qb​ 是刚体上 $q$q 点在 $B$B 坐标系下的表示，我们可以通过下式得到其在 $A$A 坐标系下的表示：
$q_a = R_{ab}q_b$qa​=Rab​qb​
可以形象地把旋转矩阵的作用理解为，将在 “ 动 ” 坐标系表示的点（或向量）表示在 “ 静 ” 坐标系下，得到不一样的坐标数值。

2.2 SO(3)

由于旋转矩阵 $R$R 列向量相互正交，且基为 1 ，所以旋转矩阵满足 $RR^T = R^TR = I,det \ R = 1$RRT=RTR=I,det R=1，将所有满足这两个性质的 $3 \times 3$3×3 矩阵的集合记作 $SO(3)$SO(3) ，符号 $SO$SO 是 special orthogonal （特殊正交）的缩写，有：
$SO(3) = \{R \in \mathbb{R^{3 \times 3}}:RR^T = I,det \ R = +1\}$SO(3)={R∈R3×3:RRT=I,det R=+1}
更一般地， $\mathbb{R}^{n \times n}$Rn×n 空间中的旋转矩阵可以定义为：
$SO(n) = \{R \in \mathbb{R^{n \times n}}:RR^T = I,det \ R = +1\}$SO(n)={R∈Rn×n:RRT=I,det R=+1}

2.3 群的性质

对于元素可以用算子 $\circ$∘ 构成二元运算的集合 $G$G ，如果满足：

• 封闭性：若 $g_1,g_2 \in G$g1​,g2​∈G ，则 $g_1 \circ g_2 \in G$g1​∘g2​∈G 。
• 单位性：对于任意的 $g \in G$g∈G ，一定存在一个单位元素 $e$e ，使得 $g \circ e = e\circ g = g$g∘e=e∘g=g 。
• 可逆性：对于任意的 $g \in G$g∈G ，一定存在唯一的逆 $g^{-1} \in G$g−1∈G，使得 $g \circ g^{-1} = g^{-1} \circ g = e$g∘g−1=g−1∘g=e 。
• 结合性：若 $g_1,g_2,g_3 \in G$g1​,g2​,g3​∈G ，则有 $(g_1 \circ g_2) \circ g_3 = g_1 \circ (g_2 \circ g_3) $ 。

则 $G$G 可称为群（group）。

$SO(3) \subset \mathbb{R}^{3 \times 3}$SO(3)⊂R3×3 就是满足矩阵相乘运算的群，称为三维旋转群，证明如下：

• 封闭性：设 $R_1,R_2 \in SO(3)$R1​,R2​∈SO(3) ，则有：

$R_1R_2(R_1R_2)^T = R_1R_2R_2^TR_1^T = R_1R_1^T = I$R1​R2​(R1​R2​)T=R1​R2​R2T​R1T​=R1​R1T​=I

$det(R_1R_2) = det(R_1)det(R_2) = +1$det(R1​R2​)=det(R1​)det(R2​)=+1

• 单位性：单位矩阵 $I$I 是单位元素。
• 可逆性：因为 $RR^T = R^TR = I$RRT=RTR=I ，且 $R^T \in \mathbb{R}^{3 \times 3}$RT∈R3×3 ，所以 $R$R 的逆是 $R^T$RT。
• 结合性：$R_1(R_2R_3) = (R_1R_2)R_3$R1​(R2​R3​)=(R1​R2​)R3​ 。

2.4 合成法则（链式法则）

如果坐标系 $C$C 相对于坐标系 $B$B 的位形为 $R_{bc}$Rbc​ ，坐标系 $B$B 相对于坐标系 $A$A 的位形为 $R_{ab}$Rab​ ，那么，坐标系 $C$C 相对于坐标系 $A$A 的位形为:
$R_{ac} = R_{ab}R_{bc}$Rac​=Rab​Rbc​
符合链式法则。可以理解为组合起来去掉中间桥接的字母下标。

当旋转矩阵的组合作用与一点（或向量）上，比如右乘 $R_{ab}R_{bc}$Rab​Rbc​ ，可以理解为先将在 $c$c 上表示的点（或向量）表示在 $b$b 上，再将其在 $b$b 上的表示表示到 $a$a 上。

2.5 旋转矩阵的引理

对于给定的 $R \in SO(3)$R∈SO(3) 和 $v,w \in \mathbb{R}^{3}$v,w∈R3 ，存在以下性质：
$R(v \times w) = (Rv) \times (Rw)$R(v×w)=(Rv)×(Rw)

$R(w)^{\wedge}R^T = (Rw)^{\wedge}$R(w)∧RT=(Rw)∧

其中，$\wedge$∧ 运算符将在下面定义。

3 旋转运动的参数化

3.1 反对称矩阵

设两矢量 $a,b \in \mathbb{R}^3$a,b∈R3 ，其值如下：
$a = \begin{bmatrix} a_1 \\ a_2 \\ a_3 \\ \end{bmatrix}$a=⎣⎡​a1​a2​a3​​⎦⎤​

$b = \begin{bmatrix} b_1 \\ b_2 \\ b_3 \\ \end{bmatrix}$b=⎣⎡​b1​b2​b3​​⎦⎤​

则这两个向量的叉积为：
$a \times b = \begin{bmatrix} a_2b_3 - a_3b_2 \\ a_3b_1 - a_1b_3 \\ a_1b_2 - a_2b_1 \\ \end{bmatrix}$a×b=⎣⎡​a2​b3​−a3​b2​a3​b1​−a1​b3​a1​b2​−a2​b1​​⎦⎤​
可得叉积运算是一个线性算子。若定义：
$(a)^\wedge = \begin{bmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \\ \end{bmatrix}$(a)∧=⎣⎡​0a3​−a2​​−a3​0a1​​a2​−a1​0​⎦⎤​
则：
$a \times b = (a)^\wedge b$a×b=(a)∧b
用符号 $\widehat{a}$a 代替 $(a)^\wedge$(a)∧ ，由定义可知， $\widehat{a}$a 是反对称矩阵，即满足 $\widehat{a}^T = - \widehat{a}$aT=−a ，所有 $3 \times 3$3×3 反对称矩阵的矢量空间记作 $so(3)$so(3) ，更一般地，$n \times n$n×n 反对称矩阵的矢量空间记作：
$so(n) = \{S \in \mathbb{R}^{n \times n}:S^T = -S\}$so(n)={S∈Rn×n:ST=−S}
我们将 $^\wedge$∧ 读作 “戴帽” 操作， $^\vee$∨ 读作 “去帽” 操作，其可以实现 $\mathbb{R}^3 \Leftrightarrow so(3)$R3⇔so(3) 的双射变换（即 “一一对应” ）。

3.2 指数表达

如果在三维空间中的物体绕 $w$w 轴旋转 $\theta$θ 角度，则其旋转矩阵可以通过以下式子计算：
$R(w,\theta) = e^{\widehat{w}\theta} = I + \widehat{w}sin\theta+\widehat{w}^2(1-cos\theta)$R(w,θ)=ewθ=I+wsinθ+w2(1−cosθ)
其中 $w \in \mathbb{R}^3,\theta\in \mathbb{R}$w∈R3,θ∈R ，且 $\widehat{w} \in so(3)$w∈so(3) 。特别要注意 $w$w 为单位矢量（如何得到这个式子将另作解释）。

由上式可得：

• $(w,\theta) \Rightarrow SO(3)$(w,θ)⇒SO(3) 是一个映射，符合 “ 多对一 ” 和满射。因为一个旋转矩阵对应不止一组 $(w,\theta)$(w,θ) ，且每一个旋转矩阵都能找到对应。

• 当 $\theta = 0$θ=0 时，$R = I$R=I ，有奇异点产生，可以理解为 $R = I$R=I 时对应无穷多组 $(w,\theta)$(w,θ) 。

4 其他表示法

4.1 欧拉角

4.1.1 绕坐标轴旋转的旋转矩阵

分别将绕 $x,y,z$x,y,z 轴旋转的旋转矩阵定义为基本旋转矩阵：
$R_x(\phi) = e^{{\widehat{x}}\phi} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & cos\phi & -sin\phi \\ 0 & sin\phi & cos\phi \\ \end{bmatrix}$Rx​(ϕ)=exϕ=⎣⎡​100​0cosϕsinϕ​0−sinϕcosϕ​⎦⎤​

$R_y(\beta) = e^{{\widehat{y}}\beta} = \begin{bmatrix} cos\beta & 0 & sin\beta \\ 0 &1 & 0 \\ -sin\beta & 0 & cos\beta \\ \end{bmatrix}$Ry​(β)=ey​β=⎣⎡​cosβ0−sinβ​010​sinβ0cosβ​⎦⎤​

$R_z(\alpha) = e^{{\widehat{z}}\alpha} = \begin{bmatrix} cos\alpha & -sin\alpha & 0 \\ sin\alpha & cos\alpha & 0 \\ 0 & 0 & 1 \\ \end{bmatrix}$Rz​(α)=ezα=⎣⎡​cosαsinα0​−sinαcosα0​001​⎦⎤​

这些式子通过指数表达式不难求出。需要注意的是，如果无特殊说明，旋转的角度皆指逆时针旋转。

4.1.2 欧拉角定义

设坐标系 $B$B 最初与坐标系 $A$A 重合，首先将 $B$B 系绕其 $z$z 轴旋转 $\alpha$α 角，然后绕 $B$B 系的新 $y$y 轴旋转 $\beta$β 角，最后绕 $B$B 系的新 $z$z 轴旋转 $\gamma$γ 角，得到的角度 $(\alpha,\beta,\gamma)$(α,β,γ) 称为关于 $ZY\!Z$ZYZ 的欧拉角，依旋转轴的不同，还有关于 $ZY\!X,Y\!Z\!X$ZYX,YZX 的欧拉角等。注意旋转顺序为字母表示的从右到左。这样可以得到 $B$B 系相对于 $A$A 系的旋转矩阵：
$R_{ab} = R_z(\alpha)R_y(\beta)R_z(\gamma)$Rab​=Rz​(α)Ry​(β)Rz​(γ)
这个矩阵同样可以理解为点在不同坐标系下的依次表示，表示的顺序和旋转的顺序相反。

对上式求逆，可以得到 $A$A 系相对于 $B$B 系的旋转矩阵：
$R_{ba} = R_z(-\gamma)R_y(-\beta)R_z(-\alpha)$Rba​=Rz​(−γ)Ry​(−β)Rz​(−α)
需要注意的是，使用欧拉角表示旋转，也会出现奇异点。

4.2 四元数

四元数（quafernions）是复数的一种推广，其可以利用四个数字来表示在三维空间上的旋转，而且利用这种表示方法没有奇异点产生。四元数的一般形式为：
$Q = q_0 + q_1i + q_2j + q_3k \quad q_i \in \R ,i=0,1,2,3$Q=q0​+q1​i+q2​j+q3​kqi​∈R,i=0,1,2,3
其中，$q_0$q0​ 为 $Q$Q 的标量部分，$q = (q_1,q_2,q_3)$q=(q1​,q2​,q3​) 为矢量部分。可以简洁地表示为 $Q = (q_0, q)$Q=(q0​,q) 。

其中，$i,j,k$i,j,k 是一种特殊的虚数， 对于 $i,j,k$i,j,k 本身的几何意义可以理解为一种旋转，其中$i$i旋转代表 X 轴与 Y 轴相交平面中 X 轴正向向 Y 轴正向的旋转，$j$j 旋转代表 Z 轴与 X 轴相交平面中 Z 轴正向向 X 轴正向的旋转，$k$k 旋转代表 Y 轴与 Z 轴相交平面中 Y 轴正向向 Z 轴正向的旋转。

由于四元数不是本系列文章研究的重点，所以不再详细描述，感兴趣的同学可以参考文末的链接。

5 参考文献

• 《a mathematical introduction to robotic manipulation》

• 四元数 - *