刚体运动和坐标变换-1

刚体运动和坐标变换-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}

• 共轭：

• 逆：

用四元数表示旋转

假设有一个三维空间点

\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}

从而：

注意到，

\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