机器人运动学（二）姿态的描述

旋转矩阵是描述坐标系的一种方式，那么还有没有其他描述方法呢？本文将会介绍以下内容：

• 欧拉角、等效轴角、旋转矩阵和四元数的定义及其优缺点
• Eigen库是如何完成这些转换的

一、旋转矩阵

旋转矩阵是一个 3 × 3 3\times3 3×3矩阵称为旋转矩阵。其一般形式为： R = [ n x o x a x n y o y a y n z o z a z ] R=\begin{bmatrix} n_x&o_x &a_x\\ n_y&o_y&a_y\\ n_z&o_z&a_z \end{bmatrix} R=⎣⎡​nx​ny​nz​​ox​oy​oz​​ax​ay​az​​⎦⎤​ 假如坐标系的姿态如下： R = [ 0.7071 − 0.5721 0.4156 0.7071 0.5721 − 0.4156 0 0.5878 0.8090 ] R=\begin{bmatrix} 0.7071 & -0.5721& 0.4156\\ 0.7071 & 0.5721 &-0.4156\\ 0 & 0.5878 & 0.8090\\ \end{bmatrix} R=⎣⎡​0.70710.70710​−0.57210.57210.5878​0.4156−0.41560.8090​⎦⎤​

对于一些比较特殊的旋转你可以直接看出其姿态，如：

• 绕 Z Z Z轴旋转 R z ( θ ) = [ cos ⁡ θ − sin ⁡ θ 0 sin ⁡ θ cos ⁡ θ 0 0 0 1 ] R_z(\theta)=\begin{bmatrix} \cos\theta & -\sin\theta & 0\\ \sin\theta & \cos\theta &0\\ 0 & 0 & 1\\ \end{bmatrix} Rz​(θ)=⎣⎡​cosθsinθ0​−sinθcosθ0​001​⎦⎤​
• 绕 Y Y Y轴旋转 R y ( θ ) = [ cos ⁡ θ 0 sin ⁡ θ 0 1 0 − sin ⁡ θ 0 cos ⁡ θ ] R_y(\theta)=\begin{bmatrix} \cos\theta &0& \sin\theta \\ 0& 1 &0\\ -\sin\theta & 0 & \cos\theta \\ \end{bmatrix} Ry​(θ)=⎣⎡​cosθ0−sinθ​010​sinθ0cosθ​⎦⎤​
• 绕 X X X轴旋转 R x ( θ ) = [ 1 0 0 0 cos ⁡ θ − sin ⁡ θ 0 sin ⁡ θ cos ⁡ θ ] R_x(\theta)=\begin{bmatrix} 1 & 0 & 0\\ 0 & \cos\theta &-\sin\theta\\ 0 & \sin\theta & \cos\theta\\ \end{bmatrix} Rx​(θ)=⎣⎡​100​0cosθsinθ​0−sinθcosθ​⎦⎤​

这一种表达方式非常不直观，看到这一组数字，你非常难以想象坐标系空间姿态。计算机非常喜欢这种形式，因为不就是一些加加减减除除吗，So easy! 最重要是，如果操作者想指定机器人末端姿态，需要繁琐的输入这九个元素的正交矩阵。

二、最直观的方法：绕主轴的旋转

绕主轴的旋转表示方法，按是否固定可以分为两种，一是不固定的欧拉角，二是固定角。要三个参量就能表示空间所有姿态的一种方法，结果是最直观的。欧拉角不是唯一的，只要是参考坐标系按照一定顺序依次进行三次绕主轴的旋转就到达空间中任意姿态。只要满足以下条件就可以保证能到达所有姿态：

• 所有旋转都是绕着参考轴的
• 三次旋转至少涉及两个不同主轴
• 同一个主轴不进行两次旋转

下面讲一下最常见:

• ZYX欧拉角 先绕当前坐标轴 { B } \{B\} {B}的 Z Z Z旋转 α \alpha α，再绕旋转后的Y轴旋转 β \beta β，最后再绕更新后的角度旋转 γ \gamma γ B A R Z ′ Y ′ X ′ = R Z ( α ) R Y ( β ) R X ( γ ) = [ c α − s α 0 s α c α 0 0 0 1 ] [ c β 0 s β 0 10 0 − s β 0 c β ] [ 1 0 0 0 c γ − s γ 0 s γ c γ ] = [ c α c β c α s β s γ − s α c γ c α s β c γ + s α s γ s α c β s α s β s γ + c α c γ s α s β c γ − c α s γ − s β c β s α c β c γ ] \begin{aligned} ^A_BR_{Z'Y'X'}&=R_Z(\alpha)R_Y(\beta)R_X(\gamma)\\ &=\begin{bmatrix} c\alpha &-s\alpha&0\\ s\alpha&c\alpha&0\\ 0&0&1 \end{bmatrix}\begin{bmatrix}c\beta&0&s\beta\\0&10&0\\-s\beta&0&c\beta\end{bmatrix}\begin{bmatrix}1&0&0\\0&c\gamma&-s\gamma\\0&s\gamma&c\gamma\end{bmatrix}\\ &=\begin{bmatrix} c\alpha c\beta&c\alpha s\beta s\gamma-s \alpha c\gamma&c\alpha s\beta c\gamma+s\alpha s\gamma\\s\alpha c\beta&s\alpha s\beta s\gamma+c\alpha c\gamma&s\alpha s\beta c\gamma-c\alpha s\gamma\\-s\beta & c\beta s\alpha&c\beta c\gamma \end{bmatrix} \end{aligned} BA​RZ′Y′X′​​=RZ​(α)RY​(β)RX​(γ)=⎣⎡​cαsα0​−sαcα0​001​⎦⎤​⎣⎡​cβ0−sβ​0100​sβ0cβ​⎦⎤​⎣⎡​100​0cγsγ​0−sγcγ​⎦⎤​=⎣⎡​cαcβsαcβ−sβ​cαsβsγ−sαcγsαsβsγ+cαcγcβsα​cαsβcγ+sαsγsαsβcγ−cαsγcβcγ​⎦⎤​​

与欧拉角对应的还有绕固定坐标系旋转的，一种另一种旋转规则，与欧拉角不同的是，他每次都是绕着参考主轴旋转的：

• X-Y-Z固定角 绕参考坐标系主轴 { A } \{A\} {A} X X X旋转

γ \gamma γ，再绕参考坐标系的主轴 Y Y Y旋转 β \beta β，最后绕参考坐标系主轴 Z Z Z旋转 α \alpha α B A R X Y Z ( γ , β , α ) = R Z ( α ) R Y ( β ) R X ( γ ) = [ c α − s α 0 s α c α 0 0 0 1 ] [ c β 0 s β 0 1 0 − s β 0 c β ] [ 1 0 0 0 c γ − s γ 0 s γ c γ ] = [ c α c β c α   s β s γ − s α c γ c α s β c γ + s α s γ s α c β s α s β s γ + c α c γ s α s β c γ − c α s γ − s β c β s γ c β c γ ] \begin{aligned} ^A_BR_{XYZ}(\gamma,\beta, \alpha) &=R_Z(\alpha)R_Y(\beta)R_X(\gamma)\\ &=\begin{bmatrix} c\alpha&-s\alpha &0\\s\alpha&c\alpha&0\\0&0&1 \end{bmatrix} \begin{bmatrix} c\beta&0&s\beta\\0&1&0\\-s \beta&0&c\beta \end{bmatrix} \begin{bmatrix} 1&0&0\\ 0&c\gamma&-s\gamma\\0&s\gamma&c\gamma \end{bmatrix}\\&= \begin{bmatrix} c\alpha c\beta &c\alpha\ s\beta s\gamma-s\alpha c\gamma &c\alpha s\beta c\gamma+s\alpha s\gamma \\ s\alpha c\beta &s\alpha s\beta s\gamma+c\alpha c\gamma &s\alpha s\beta c\gamma -c\alpha s\gamma \\ -s\beta&c\beta s\gamma&c\beta c\gamma \end{bmatrix} \end{aligned} BA​RXYZ​(γ,β,α)​=RZ​(α)RY​(β)RX​(γ)=⎣⎡​cαsα0​−sαcα0​001​⎦⎤​⎣⎡​cβ0−sβ​010​sβ0cβ​⎦⎤​⎣⎡​100​0cγsγ​0−sγcγ​⎦⎤​=⎣⎡​cαcβsαcβ−sβ​cα sβsγ−sαcγsαsβsγ+cαcγcβsγ​cαsβcγ+sαsγsαsβcγ−cαsγcβcγ​⎦⎤​​

注意：旋转顺序与矩阵乘法，先旋转的在右边，后旋转的在左边。

三、等效轴角

如果轴的方向是一般方向（不是主轴方向），任何姿态都可以通过选择适当的轴和角度来得到。 空间中的 { B } \{B\} {B}坐标系的姿态是这样表达的：

首先将坐标系 { B } \{B\} {B}和一个已知参考坐标系 { A } \{A\} {A}重合。将 { B } \{B\} {B}绕向量 A K ^ ^A\hat K AK^按右手定则旋转 θ \theta θ角。

向量 K ^ \hat K K^有时候被称为有限旋转的等效轴。 { B } \{B\} {B}相对于 { A } \{A\} {A}的一般姿态可用 B A R ( K ^ , θ ) ^A_BR(\hat K,\theta) BA​R(K^,θ)或者 R K ( θ ) R_K(\theta) RK​(θ)表示。

K ^ \hat K K^也是一个单位向量，如下： K ^ = [ k x k y k z ] \hat K=\begin{bmatrix}k_x\\k_y\\k_z\end{bmatrix} K^=⎣⎡​kx​ky​kz​​⎦⎤​

如果我们选择的是一个主轴，那么他就是我们常见的平面旋转矩阵；如果选择的是一般轴，那么等效旋转矩阵为： R K ( θ ) = [ k x k x v θ + c θ k x k y v θ − k z s θ k x k z v θ + k y s θ k x k y v θ + k z s θ k y k y v + c θ k y k z v θ − k x s θ k x k z v θ − k y s θ k y k z v θ + k x s θ k z k z v θ + c θ ] (1) R_K(\theta)=\begin{bmatrix} k_xk_xv\theta+c\theta&k_xk_yv\theta-k_zs\theta&k_xk_zv\theta+k_ys\theta\\ k_xk_yv\theta+k_zs\theta& k_yk_yv+c\theta&k_yk_zv\theta-k_xs\theta\\ k_xk_zv\theta-k_ys\theta&k_yk_zv\theta+k_xs\theta&k_zk_zv\theta+c\theta \end{bmatrix}\tag{1} RK​(θ)=⎣⎡​kx​kx​vθ+cθkx​ky​vθ+kz​sθkx​kz​vθ−ky​sθ​kx​ky​vθ−kz​sθky​ky​v+cθky​kz​vθ+kx​sθ​kx​kz​vθ+ky​sθky​kz​vθ−kx​sθkz​kz​vθ+cθ​⎦⎤​(1) (1)又被称为Rodrigues公式，对证明感兴趣的可以看这里。

四、欧拉参数

另一种姿态表示方法是通过四个数来表示的，称为欧拉参数。它和等效轴 k ^ = [ k x k y k z ] T \hat k=\begin{bmatrix}k_x&k_y&k _z\end{bmatrix}^T k^=[kx​​ky​​kz​​]T和旋转轴 θ \theta θ的等效轴角表示方法的关系为： ε 1 = k x sin ⁡ θ 2 ε 2 = k y sin ⁡ θ 2 ε 3 = k z sin ⁡ θ 2 ε 4 = cos ⁡ θ 2 \begin{aligned} \varepsilon_1&=k_x\sin\frac{\theta}2\\ \varepsilon_2&=k_y\sin\frac{\theta}2\\ \varepsilon_3&=k_z\sin\frac{\theta}2\\ \varepsilon_4&=\cos\frac{\theta}2\\ \end{aligned} ε1​ε2​ε3​ε4​​=kx​sin2θ​=ky​sin2θ​=kz​sin2θ​=cos2θ​​ 这四个参数平方和恒为1。 ε 1 2 + ε 2 2 + ε 3 2 + ε 4 2 = 1 \varepsilon_1^2+\varepsilon_2^2+\varepsilon_3^2+\varepsilon_4^2=1 ε12​+ε22​+ε32​+ε42​=1 一个姿态可以看作是四维空间单位超球面上的一点。单位四元数不等于欧拉参数矢量 4 × 1 4\times 1 4×1的矢量，但是我们还是将欧拉参数看做四元数。旋转矩阵和欧拉参数之间的关系是： R ε = [ 1 − 2 ε 2 2 − 2 ε 3 2 2 ( ε 1 ε 2 − ε 3 ε 4 ) 2 ( ε 1 ε 3 + ε 2 ε 4 ) 2 ( ε 1 ε 2 + ε 3 ε 4 ) 1 − 2 ε 1 2 − 2 ε 3 2 2 ( ε 2 ε 3 − ε 1 ε 4 ) 2 ( ε 1 ε 3 − ε 2 ε 4 ) 2 ( ε 2 ε 3 + ε 1 ε 4 ) 1 − ε 1 2 − ε 2 2 ] R\varepsilon=\begin{bmatrix} 1-2\varepsilon_2^2-2\varepsilon_3^2&2(\varepsilon_1\varepsilon_2-\varepsilon_3\varepsilon_4)&2(\varepsilon_1\varepsilon_3+\varepsilon_2\varepsilon_4)\\ 2(\varepsilon_1\varepsilon_2+\varepsilon_3\varepsilon_4)&1-2\varepsilon_1^2-2\varepsilon_3^2&2(\varepsilon_2\varepsilon_3-\varepsilon_1\varepsilon_4)\\ 2(\varepsilon_1\varepsilon_3-\varepsilon_2\varepsilon_4)&2(\varepsilon_2\varepsilon_3+\varepsilon_1\varepsilon_4)&1-\varepsilon_1^2-\varepsilon_2^2 \end{bmatrix} Rε=⎣⎡​1−2ε22​−2ε32​2(ε1​ε2​+ε3​ε4​)2(ε1​ε3​−ε2​ε4​)​2(ε1​ε2​−ε3​ε4​)1−2ε12​−2ε32​2(ε2​ε3​+ε1​ε4​)​2(ε1​ε3​+ε2​ε4​)2(ε2​ε3​−ε1​ε4​)1−ε12​−ε22​​⎦⎤​ 一个旋转矩阵的如何求得欧拉参数？ ε 1 = r 32 − r 23 4 ε 4 ε 2 = r 13 − r 31 4 ε 4 ε 3 = r 21 − r 12 4 ε 4 ε 4 = 1 2 1 + r 11 + r 22 + r 33 \begin{aligned} \varepsilon_1&=\frac{r_{32}-r_{23}}{4\varepsilon_4}\\ \varepsilon_2&=\frac{r_{13}-r_{31}}{4\varepsilon_4}\\ \varepsilon_3&=\frac{r_{21}-r_{12}}{4\varepsilon_4}\\ \varepsilon_4&=\frac{1}{2}\sqrt{1+r_{11}+r_{22}+r_{33}} \end{aligned} ε1​ε2​ε3​ε4​​=4ε4​r32​−r23​​=4ε4​r13​−r31​​=4ε4​r21​−r12​​=21​1+r11​+r22​+r33​ ​​ 从计算意义上说，旋转矩阵若是绕某个轴旋转 18 0 ∘ 180^{\circ} 180∘，上述分母将会为0。

20220505 修复了公式上的错误