刚体运动和坐标变换-1

作者: Mezereon | 来源:发表于2023-03-22 15:15 被阅读0次

刚体运动学（3）：欧拉角
刚体运动学（1）：基本定义和独立坐标
相机标定知识和深度数据映射彩色数据
刚体运动学（7）：矢量的变化率
角速度与线速度
刚体运动学（5）：欧拉刚性运动定理
ROS机器人的tf变换
刚体运动学（2）：正交变换
第三讲三维空间的刚体运动
坐标变换

刚体运动和坐标变换-1

基础代数

外积：

$\bf a$ 和 $\bf b$ 两个向量的外积代表一个垂直这两个向量的向量，大小为 $\bf |a||b|\sin\langle a, b\rangle$
$\textbf{a} \times \textbf{b} = \begin{Vmatrix} e_1 & e_2 & e_3\\ a_1 & a_2 & a_3\\ b_1 & b_2 & b_3 \end{Vmatrix} = \begin{bmatrix} a_2b_3 -a_3b_2\\ a_3b_1 -a_1b_3\\ a_1b_2 -a_2b_1 \end{bmatrix} = \begin{bmatrix} 0 & -a_3 & a_2\\ a_3 & 0 & -a_1\\ -a_2 & a_1 & 0 \end{bmatrix} \textbf{b} = \textbf{a}^\wedge \textbf{b}$
其中, $e_i$ 是互相正交的基底向量。

我们可以将外积的形式写成矩阵乘以向量的形式，即：a的反对称矩阵左乘b

反对称矩阵 $A$ ，满足 $A^T = -A$

欧式变换

两个坐标系之间的变换，可以被解释成旋转加上平移。

旋转矩阵 ：旋转矩阵可以表示向量的旋转，其本质是两个坐标系基底之间的内积构成的矩阵
$SO(n) = \{R\in \mathbb{R}^{n\times n}\vert RR^T=I, \det(R) = 1\}$
SO(n) 是特殊正交群，这个集合包含所有n维的旋转矩阵，行列式为1，并且都是正交矩阵。

正交矩阵，即 $A^{-1} = A^T$

平移可以用一个向量 $\bf t$ 来表示

整个欧式变换，可以理解成：
$\textbf a' = R\textbf a + \textbf t$
齐次坐标和变换矩阵

为了将平移和旋转融合成一个式子，我们将欧式变换写成如下形式：
$\begin{bmatrix} R & \bf t\\ 0 & 1 \end{bmatrix} \begin{bmatrix} \bf a\\ 1 \end{bmatrix} = T\begin{bmatrix} \bf a\\ 1 \end{bmatrix} = \begin{bmatrix} \bf a'\\ 1 \end{bmatrix}$
其中，我们扩展了向量 $\bf a$ 变成四维，称之为 齐次坐标，矩阵 $T$ 称之为 变换矩阵

同样的，变换矩阵构成的集合，称之为 特殊欧式群
$SE(n) = \Bigg \{T = \begin{bmatrix} R & \bf t\\ 0 & 1 \end{bmatrix} \in \mathbb R^{4\times 4} \vert R\in SO(3), \textbf t \in \mathbb R^3 \Bigg \}$
变换矩阵的逆，也可以简单求出，即：
$T^{-1} =\begin{bmatrix} R^T & -R^T\bf t\\ 0 & 1 \end{bmatrix}$
Rodrigues's Formula

Rodrigues's Formula 是将旋转矩阵 $R$ , 变换成旋转轴 $\textbf n \in \mathbb R^{3}$ 和旋转角 $\theta$ 的形式:
$R = (\cos\theta) \textbf I + (1 - \cos \theta)\textbf n\textbf n^T + (\sin\theta) \textbf n^{\wedge }$
更进一步地，我们可以使用旋转矩阵的迹，来计算旋转角：
$\tr(R) = 3\cos \theta + (1-\cos\theta) = 2\cos\theta + 1\\ \theta = \arccos(\frac{\tr(R) - 1}{2})$

四元数

旋转矩阵用9个变量来描述三个自由度的旋转，具有冗余性，由于我们找不到无歧义的三维旋转表示，我们引入四元素来进行旋转的表示

注意到复数的乘法，表示复平面上的旋转，比如我们对复向量乘一个虚数 $i$ ，就等于逆时针旋转90度。

比如，对于复数向量 $v = a + 0i$ , 代表实数轴上的一个向量

$v\cdot i = 0+ ai$ , 代表虚轴正方向的一个向量，即逆时针旋转90度

四元数可以表示为，一个实部 + 三个虚部：
$q = q_0 + q_1\textbf i + q_2\textbf j + q_3 \textbf k$
三个虚部满足：
$\textbf i^2 =\textbf j^2=\textbf k^2=-1\\ \textbf i \textbf j = \textbf k, \textbf j \textbf i =-\textbf k\\ \textbf j \textbf k = \textbf i, \textbf k \textbf j = -\textbf i\\ \textbf k \textbf i =\textbf j, \textbf i \textbf k = -\textbf j$
我们可以将四元数记作实部和虚部的向量表示，即：
$\textbf q = [s, \textbf v]^T$

四元数的运算

不妨记， $\textbf q_a = [s_a, \textbf v_a]^T, \textbf q_b = [s_b, \textbf v_b]^T$

其中, $\textbf v_a =x_a\textbf i + y_a \textbf j + z_a \textbf k, \textbf v_b = x_b\textbf i + y_b \textbf j + z_b \textbf k$

加减法： $\textbf q_a \pm \textbf q_b = [s_a\pm s_b, \textbf v_a\pm \textbf v_b]^T$
乘法: $\textbf q_a \textbf q_b = [s_as_b - \textbf v_a^T\textbf v_b, s_a \textbf v_b + s_b \textbf v_a + \textbf v_a\times \textbf v_b]$
模长： $\Vert \textbf q_a \Vert =\sqrt{s_a^2 + x_a^2 + y_a^2 + z_a^2}$
共轭： $q_a^* = [s_a, -\textbf v_a ]^T$
逆： $q_a^{-1} = q_a^* / \Vert q_a\Vert^2$

用四元数表示旋转

假设有一个三维空间点 $\textbf p = [x,y,z]\in \mathbb R^3$ , 和一个单位四元数 $\textbf q$ 指定的旋转，记旋转后的点为 $\textbf p'$ ，我们有矩阵描述：
$\textbf p' = R\textbf p$
我们将三维空间点，记成一个虚四元数，即：
$\textbf p = [0, x, y, z]^T = [0, \textbf v]^T$
则旋转后的点，可以被表示成：
$\textbf p' = \textbf q\textbf p\textbf q^{-1}$
这个点也是一个虚四元数

Proof:

假设旋转四元数为 $\textbf q = [s, a, b, c]^T = [s, \textbf v_q]$ , 可得
$\textbf q^{-1} = [s, -a, -b, -c]^T \cdot \frac{1}{s^2 + a^2 + b^2 + c^2}$
从而：
$\begin{aligned} \textbf q\textbf p\textbf q^{-1} & = [-\textbf v^T\textbf v_q, s\textbf v + \textbf v\times \textbf v_q]\cdot \frac{\textbf q^*}{\Vert \textbf q\Vert^2}\\ & =[-\textbf v^T\textbf v_q, s\textbf v + \textbf v\times \textbf v_q]\cdot [s, -\textbf v_q]\cdot \frac{1}{\Vert \textbf q\Vert^2}\\ & = [-\textbf v^T\textbf v_qs - (s\textbf v + \textbf v\times \textbf v_q)^T (-\textbf v_q), \cdots]\cdot \frac{1}{\Vert \textbf q\Vert^2} \end{aligned}$
注意到， $\textbf v \times \textbf v_q$ 的结果是和 $\textbf v$ 以及 $\textbf v_q$ 都垂直，所以 $\textbf v\times \textbf v_q \times -\textbf v_q = 0$
$-\textbf v^T\textbf v_q s - s\textbf v^T (-\textbf v_q) = 0$