刚体运动与位姿表示

位姿表示

  位置：如果在三维空间中，使用三个空间坐标表示：

$$ \boldsymbol{x}_n = [x, y, z] $$

  姿态：2D 姿态用一个旋转角$ \theta $表示。3D 姿态可用旋转矩阵、变换矩阵、欧拉角、四元数等表示。

旋转矩阵与变换矩阵

坐标系间的欧式变换

  坐标系间的欧式变换：同一个向量在各个坐标系下的长度和夹角都不会发生变化，如相机运动。一个欧式变换由平移和旋转组成。

  某个向量$ \boldsymbol{p} $在世界坐标系和相机坐标系中的坐标分别为$ \boldsymbol{p}_w $和$ \boldsymbol{p}_c $，通常在计算中需要转换在同一个坐标系中进行，二者存在转换关系用矩阵 T 表示。

• 世界坐标系：机器人运动过程中，世界坐标系固定不动，下图左侧坐标系。
• 相机坐标系：相机本身存在一个坐标系，随相机的运动而变化，下图右侧坐标系。

旋转矩阵

  2D 空间下对旋转矩阵的推导较为直观，由 2D 空间可引出 3D 空间下旋转矩阵的表示。

2D 空间下

  设某点在世界坐标系中的坐标下标为$ w $，对应在机器人坐标系中的坐标下标为$ r $，根据下图所示，可推导得到二者存在如下关系：

$$
\begin{cases}
x_{w}=x_{r} \cos \theta-y_{r} \sin \theta+t_{x}\\
y_{w}=x_{r} \sin \theta+y_{r} \cos \theta+t_{y}
\end{cases}
$$

  将上述关系改写成矩阵形式，其中$ \boldsymbol{R} $为旋转矩阵，$ \boldsymbol{t} $为平移矢量。

$$
\begin{aligned}
\boldsymbol{x}_{w} &=\boldsymbol{R} \boldsymbol{x}_{r}+\boldsymbol{t} \\
&=\begin{bmatrix}{\cos \theta} & {-\sin \theta} \\ {\sin \theta} & {\cos \theta}\end{bmatrix} \boldsymbol{x}_{r}+\begin{bmatrix}{l}{t_{x}} \\ {t_{y}}\end{bmatrix}
\end{aligned}
$$

3D 空间下

  变换前后坐标系的基分别为$ (e_1,e_2,e_3) $和$ (e_1',e_2',e_3') $，则同一个向量在两个坐标系下的坐标分别为$ [a_1,a_2,a_3 ]^T $和$ [a_1',a_2',a_3' ]^T $。根据坐标定义，有：

$$
\begin{bmatrix}e_{1}, e_{2}, e_{3}\end{bmatrix}
\begin{bmatrix} {a_1} \\ {a_2} \\ {a_3}\end{bmatrix}
=\begin{bmatrix}e_{1}^{\prime}, e_{2}^{\prime}, e_{3}^{\prime}\end{bmatrix}
\begin{bmatrix} {a_{1}^{\prime}} \\ {a_{2}^{\prime}} \\ {a_{3}^{\prime}}\end{bmatrix}
$$

$$
\begin{bmatrix} {a_{1}} \\ {a_{2}} \\ {a_{3}} \end{bmatrix}
=\begin{bmatrix} {e_{1}^{T} e_{1}^{\prime}} & {e_{1}^{T} e_{2}^{\prime}} & {e_{1}^{T} e_{3}^{\prime}} \\ {e_{2}^{T} e_{1}^{\prime}} & {e_{2}^{T} e_{2}^{\prime}} & {e_{2}^{T} e_{3}^{\prime}} \\ {e_{3}^{T} e_{1}^{\prime}} & {e_{3}^{T} e_{2}^{\prime}} & {e_{3}^{T} e_{3}^{\prime}}\end{bmatrix}
\begin{bmatrix} {a_{1}^{\prime}} \\ {a_{2}^{\prime}} \\ {a_{3}^{\prime}}\end{bmatrix}
\triangleq \boldsymbol{R} \boldsymbol{a}^{\prime}
$$

  旋转矩阵是行列式为 1 的正交矩阵（正交：$ \boldsymbol{R}\boldsymbol{R}^{T}=\boldsymbol{E} $），它的逆或转置代表了相反的旋转，定义旋转矩阵的集合（特殊正交群）。

$$
\boldsymbol{a}^{\prime}=\boldsymbol{R}^{-1} a=\boldsymbol{R}^{T} \boldsymbol{a}
$$

$$
SO(n)={ \boldsymbol{R} \in R^{n×n} | \boldsymbol{R} \boldsymbol{R}^{T}=\boldsymbol{I}, \det(\boldsymbol{R})=1}
$$

  同 2D 空间，使用旋转矩阵和平移向量表示变换如下（转置表示相反的旋转）。

$$
\boldsymbol{a}^{\prime}=\boldsymbol{R} \boldsymbol{a}+\boldsymbol{t}
$$

变换矩阵和齐次坐标

• 变换矩阵：利用旋转矩阵表示，如果进行多次变换，会进行多次计算，显得很复杂。引入变换矩阵和齐次坐标，$ \boldsymbol{T} $即为变换矩阵。

$$
\begin{bmatrix} {\boldsymbol{a}^{\prime}} \\ {1}\end{bmatrix}
=\begin{bmatrix} {\boldsymbol{R}} & {\boldsymbol{t}} \\ {\mathbf{0}^{T}} & {1}\end{bmatrix}
\begin{bmatrix}\boldsymbol{a} \\ {1}\end{bmatrix}
\triangleq \boldsymbol{T} \begin{bmatrix}\boldsymbol{a} \\ {1}\end{bmatrix}
$$

  两次变换可表示为（将向量默认记作齐次坐标方式）：

$$
\boldsymbol{b}=\boldsymbol{T}_{1} \boldsymbol{a}, \quad \boldsymbol{c}=\boldsymbol{T}_{2} \boldsymbol{b}=\boldsymbol{T}_{2} \boldsymbol{T}_{1} \boldsymbol{a}
$$

  定义变换矩阵的集合（特殊欧式群）：

$$
S E(3)= {\boldsymbol{T} \in R^{4 \times 4} | \boldsymbol{R} \in S O(3), \boldsymbol{t} \in R^{3} }
$$

• 齐次坐标：上述在一个三维向量的末尾加 1，变为四维向量，称为齐次坐标，是一种运算技巧。

旋转向量

  任意旋转都可用一个旋转轴和一个旋转角来表示。因此可以使用一个向量（旋转向量），其方向与旋转轴一致，而长度等于旋转角。即：

• 旋转轴：$ \boldsymbol{n} $
• 旋转角：$ \theta $
• 旋转向量：$ \theta \boldsymbol{n} $

欧拉角

• 欧拉角的提出：旋转向量并不直观，不清楚具体在朝哪个方向旋转。欧拉角使用了 3 个分离的转角，把一个旋转分解成 3 次绕不同轴的旋转。分解方式有许多种，即绕着$ X $、$ Y $、$ Z $三轴旋转的顺序不同，如先绕$ X $轴旋转，再绕$ Y $轴，最后绕$ Z $轴，就得到了一个$ XYZ $轴的旋转。

• 示例：最常用的欧拉角表达方式：把旋转分解成沿三个轴转动的量：偏航角-俯仰角-滚转角（$ yaw-pitch-roll $）。三次旋转如下：

  • 绕物体的$ Z $轴旋转，得到偏航角$ yaw $；
  • 绕旋转之后的$ Y $轴旋转，得到俯仰角$ pitch $；
  • 绕旋转之后的$ X $轴旋转，得到滚转角$ roll $。
    
• 缺点：会碰到著名的万向锁问题：在俯仰为 ±90° 时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度（由三次旋转变成了两次旋转），这被称为奇异性问题。此外，它也不易于插值和迭代。因此它在 SLAM 中用处有限。不过想验证自己算法输出的姿态是否有错时，转换成欧拉角可快速辨认结果是否有错。

四元数

定义

• 四元数：旋转矩阵用 9 个量描述 3 自由度的旋转，具有冗余性；欧拉角和旋转向量是紧凑的，但具有奇异性。四元数是一种扩展的复数，既是紧凑的，又没有奇异性。一个四元数拥有 1 个实部和 3 个虚部。

$$
\boldsymbol{q}=q_{0}+q_{1} i+q_{2} j+q_{3} k
$$

$$
\begin{cases}
{i^{2}=j^{2}=k^{2}=-1} \\ {i j=k, j i=-k} \\ {j k=i, k j=-i} \\ {k i=j, i k=-j}
\end{cases}
$$

  也可用 1 个标量和 1 个向量表示四元数如下，其中$ s $为实部，$ \boldsymbol{v} $为虚部。实四元数：虚部为 0；虚四元数：实部为 0。

$$
\boldsymbol{q}=[\boldsymbol{s}, \boldsymbol{v}], \quad \boldsymbol{s}=q_{0} \in R, \quad \boldsymbol{v}=\left[q_{1}, q_{2}, q_{3}\right]^{T} \in R^{3}
$$

• 四元数表示旋转：设空间中一个三维点$ \boldsymbol{p}=[0,x,y,z]=[0,\boldsymbol{v}] $（以虚四元数形式表示），以及一个由单位向量$ \boldsymbol{n}=[n_x,n_y,n_z ]^T $和$ \theta $确定的旋转。用四元数形式表示该旋转如下：

$$
\begin{aligned}
\boldsymbol{q} &=\begin{bmatrix}\cos \frac{\theta}{2}, \boldsymbol{n} \sin \frac{\theta}{2}\end{bmatrix} \\
&=\begin{bmatrix}\cos \frac{\theta}{2}, n_{x} \sin \frac{\theta}{2}, n_{y} \sin \frac{\theta}{2},n_{z} \sin \frac{\theta}{2}\end{bmatrix}^{T}
\end{aligned}
$$

  旋转后的点表示如下，该计算结果的实部为 0，虚部的 3 个分量表示旋转后 3D 点的坐标。

$$
\boldsymbol{p}^{\prime}=\boldsymbol{q} \boldsymbol{p} \boldsymbol{q}^{-\mathbf{1}}
$$

四元数运算

$$
\boldsymbol{q}_a = [s_a, \boldsymbol{v}_a]=s_a + x_ai + y_a j + z_a k
$$

$$
\boldsymbol{q}_b = [s_b, \boldsymbol{v}_b] = s_b + x_b i + y_b j + z_b k
$$

• 加法和减法

$$
\boldsymbol{q}_a ± \boldsymbol{q}_b = [{s}_a ± {s}_b,\boldsymbol{v}_a ± \boldsymbol{v}_b]
$$

• 乘法

$$
\begin{aligned} \boldsymbol{q}_{a} \boldsymbol{q}_{b}=& s_{a} s_{b}-x_{a} x_{b}-y_{a} y_{b}-z_{a} z_{b} \\ &+\left(s_{a} x_{b}+x_{a} s_{b}+y_{a} z_{b}-z_{a} y_{b}\right) i \\ &+\left(s_{a} y_{b}-x_{a} z_{b}+y_{a} s_{b}+z_{a} x_{b}\right) j \\ &+\left(s_{a} z_{b}+x_{a} y_{b}-y_{b} x_{a}+z_{a} s_{b}\right) k \end{aligned}
$$

$$
\boldsymbol{q}_{a} \boldsymbol{q}_{b}=\left[s_{a} s_{b}-\boldsymbol{v}_{a}^{T} \boldsymbol{v}_{b}, s_{a} \boldsymbol{v}_{b}+s_{b} \boldsymbol{v}_{a}+\boldsymbol{v}_{a} \times \boldsymbol{v}_{b}\right]
$$

• 共轭

$$
\boldsymbol{q}_{a}^{*}=s_{a}-x_{a} i-y_{a} j-z_{a} k=\left[s_{a},-\boldsymbol{v}_{a}\right]
$$

$$
\boldsymbol{q}^ \boldsymbol{q}
=\boldsymbol{q} \boldsymbol{q}^
=\begin{bmatrix}s_{a}^{2}+\boldsymbol{v}^{T} \boldsymbol{v}, \mathbf{0}\end{bmatrix}
$$

• 模长

$$
\left|\boldsymbol{q}_{a}\right|=\sqrt{s_{a}^{2}+x_{a}^{2}+y_{a}^{2}+z_{a}^{2}}
$$

$$
\left|\boldsymbol{q}_{a} \boldsymbol{q}_{b}\right|=\left|\boldsymbol{q}_{a}\right| \boldsymbol{q}_{b} |
$$

• 逆

$$
\boldsymbol{q}^{-1}=\boldsymbol{q}^{*} /|\boldsymbol{q}|^{2}
$$

$$
\boldsymbol{q} \boldsymbol{q}^{-1}=\boldsymbol{q}^{-1} \boldsymbol{q}=\mathbf{1}
$$

$$
\left(\boldsymbol{q}_{a} \boldsymbol{q}_{b}\right)^{-1}=\boldsymbol{q}_{b}^{-1} \boldsymbol{q}_{a}^{-1}
$$

• 数乘与点乘

$$
k \boldsymbol{q}=[k s, k \boldsymbol{v}]
$$

$$
\boldsymbol{q}_{a} \cdot \boldsymbol{q}_{b}=s_{a} s_{b}+x_{a} x_{b} +y_{a} y_{b} +z_{a} z_{b}
$$

三者相互转换

四元数与旋转向量

  四元数与旋转向量之间的相互转换：

$$
\begin{cases}
\theta &=2 \arccos q_{0} \\left[n_{x}, n_{y}, n_{z}\right]^{T} &=\left[q_{1}, q_{2}, q_{3}\right]^{T} / \sin \frac{\theta}{2}
\end{cases}
$$

旋转向量与旋转矩阵

• 旋转向量到旋转矩阵：由下式（罗德里格斯公式）表示，其中$˄$表示向量的反对称：

$$
\boldsymbol{R}=\cos \theta \boldsymbol{I}+(1-\cos \theta) \boldsymbol{n} \boldsymbol{n}^{\boldsymbol{T}}+\sin \theta \boldsymbol{n}^{\wedge}
$$

$$
\begin{aligned} \operatorname{tr}(\boldsymbol{R}) &=\cos \theta \operatorname{tr}(\boldsymbol{I})+(1-\cos \theta) \operatorname{tr}\left(\boldsymbol{n} \boldsymbol{n}^{T}\right)+\sin \theta \operatorname{tr}\left(\boldsymbol{n}^{\wedge}\right) \\ &=3 \cos \theta+(1-\cos \theta) \\ &=1+2 \cos \theta \end{aligned}
$$

• 旋转矩阵到旋转向量：（1）旋转角：通过上述第 2 式计算。

  （2）旋转轴：$ n $是$ \boldsymbol{R} $特征值为 1 对应的特征向量。求解下述方程（旋转轴上的向量在旋转后不发生变化），再归一化：

$$
\boldsymbol{R} \boldsymbol{n}=\boldsymbol{n}
$$

四元数与旋转矩阵

  基本思路：四元数转为旋转向量；再转换为旋转矩阵。

• 四元数到旋转矩阵：

$$
\boldsymbol{R}=\left[ \begin{array}{ccc}{1-2 q_{2}^{2}-2 q_{3}^{2}} & {2 q_{1} q_{2}+2 q_{0} q_{3}} & {2 q_{1} q_{3}-2 q_{0} q_{2}} \\ {2 q_{1} q_{2}-2 q_{0} q_{3}} & {1-2 q_{1}^{2}-2 q_{3}^{2}} & {2 q_{2} q_{3}+2 q_{0} q_{1}} \\ {2 q_{1} q_{3}+2 q_{0} q_{2}} & {2 q_{2} q_{3}-2 q_{0} q_{1}} & {1-2 q_{1}^{2}-2 q_{2}^{2}}\end{array}\right]
$$

• 旋转矩阵到四元数：设旋转矩阵为$ \boldsymbol{R}={m_{ij} },ij∈[1,2,3] $。一个旋转矩阵对应的四元数并不唯一，因为$ q $和$ -q $可表示同一旋转。

$$
\begin{cases}
{q_{0}=\frac{\sqrt{\operatorname{tr}(\boldsymbol{R})+1}}{2}} \\ {q_{1}=\frac{m_{23}-m_{32}}{4 q_{0}}} \\ {q_{2}=\frac{m_{31}-m_{13}}{4 q_{0}}} \\ {q_{3}=\frac{m_{12}-m_{21}}{4 q_{0}}}\end{cases}
$$