刚体运动以及坐标变换-1
文章目录
- 刚体运动和坐标变换-1
-
- 基础代数
- 欧式变换
- 四元数
- 用四元数表示旋转
刚体运动和坐标变换-1
基础代数
外积：
a \bf a a 和 b \bf b b 两个向量的外积代表一个垂直这两个向量的向量，大小为 ∣ a ∣ ∣ b ∣ sin ⟨ a , b ⟩ \bf |a||b|\sin\langle a, b\rangle ∣a∣∣b∣sin⟨a,b⟩
a × b = ∥ e 1 e 2 e 3 a 1 a 2 a 3 b 1 b 2 b 3 ∥ = [ a 2 b 3 − a 3 b 2 a 3 b 1 − a 1 b 3 a 1 b 2 − a 2 b 1 ] = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] b = a ∧ b \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} a×b= e1a1b1e2a2b2e3a3b3 = a2b3−a3b2a3b1−a1b3a1b2−a2b1 = 0a3−a2−a30a1a2−a10 b=a∧b
其中, e i e_i ei 是互相正交的基底向量。
我们可以将外积的形式写成矩阵乘以向量的形式，即：a的反对称矩阵左乘b
反对称矩阵 A A A ，满足 A T = − A A^T = -A AT=−A
欧式变换
两个坐标系之间的变换，可以被解释成旋转加上平移。
旋转矩阵 ：旋转矩阵可以表示向量的旋转，其本质是两个坐标系基底之间的内积构成的矩阵
S O ( n ) = { R ∈ R n × n ∣ R R T = I , det ( R ) = 1 } SO(n) = \{R\in \mathbb{R}^{n\times n}\vert RR^T=I, \det(R) = 1\} SO(n)={R∈Rn×n∣RRT=I,det(R)=1}
SO(n) 是特殊正交群， 这个集合包含所有n维的旋转矩阵，行列式为1，并且都是正交矩阵。
正交矩阵，即 A − 1 = A T A^{-1} = A^T A−1=AT
平移可以用一个向量 t \bf t t 来表示
整个欧式变换，可以理解成：
a ′ = R a + t \textbf a' = R\textbf a + \textbf t a′=Ra+t
齐次坐标和变换矩阵
为了将平移和旋转融合成一个式子，我们将欧式变换写成如下形式：
[ R t 0 1 ] [ a 1 ] = T [ a 1 ] = [ a ′ 1 ] \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} [R0t1][a1]=T[a1]=[a′1]
其中，我们扩展了向量 a \bf a a 变成四维，称之为 齐次坐标，矩阵 T T T 称之为 变换矩阵
同样的，变换矩阵构成的集合，称之为 特殊欧式群
S E ( n ) = { T = [ R t 0 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } 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 \} SE(n)={T=[R0t1]∈R4×4∣R∈SO(3),t∈R3}
变换矩阵的逆，也可以简单求出，即：
T − 1 = [ R T − R T t 0 1 ] T^{-1} =\begin{bmatrix} R^T & -R^T\bf t\\ 0 & 1 \end{bmatrix} T−1=[RT0−RTt1]
Rodrigues’s Formula
Rodrigues’s Formula 是将旋转矩阵 R R R, 变换成旋转轴 n ∈ R 3 \textbf n \in \mathbb R^{3} n∈R3 和旋转角 θ \theta θ 的形式:
R = ( cos θ ) I + ( 1 − cos θ ) n n T + ( sin θ ) n ∧ R = (\cos\theta) \textbf I + (1 - \cos \theta)\textbf n\textbf n^T + (\sin\theta) \textbf n^{\wedge } R=(cosθ)I+(1−cosθ)nnT+(sinθ)n∧
更进一步地，我们可以使用旋转矩阵的迹，来计算旋转角：
KaTeX parse error: Undefined control sequence: \tr at position 2: \̲t̲r̲(R) = 3\cos \th…
四元数
旋转矩阵用9个变量来描述三个自由度的旋转，具有冗余性，由于我们找不到无歧义的三维旋转表示，我们引入四元素来进行旋转的表示
注意到复数的乘法，表示复平面上的旋转，比如我们对复向量乘一个虚数 i i i，就等于逆时针旋转90度。
比如，对于复数向量 v = a + 0 i v = a + 0i v=a+0i, 代表实数轴上的一个向量
v ⋅ i = 0 + a i v\cdot i = 0+ ai v⋅i=0+ai, 代表虚轴正方向的一个向量，即逆时针旋转90度
四元数可以表示为，一个实部 + 三个虚部：
q = q 0 + q 1 i + q 2 j + q 3 k q = q_0 + q_1\textbf i + q_2\textbf j + q_3 \textbf k q=q0+q1i+q2j+q3k
三个虚部满足：
i 2 = j 2 = k 2 = − 1 ij = k , ji = − k jk = i , kj = − i ki = j , ik = − j \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 i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j
我们可以将四元数记作实部和虚部的向量表示，即：
q = [ s , v ] T \textbf q = [s, \textbf v]^T q=[s,v]T
四元数的运算
不妨记， q a = [ s a , v a ] T , q b = [ s b , v b ] T \textbf q_a = [s_a, \textbf v_a]^T, \textbf q_b = [s_b, \textbf v_b]^T qa=[sa,va]T,qb=[sb,vb]T
其中, v a = x a i + y a j + z a k , v b = x b i + y b j + z b k \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 va=xai+yaj+zak,vb=xbi+ybj+zbk
- 加减法： q a ± q b = [ s a ± s b , v a ± v b ] T \textbf q_a \pm \textbf q_b = [s_a\pm s_b, \textbf v_a\pm \textbf v_b]^T qa±qb=[sa±sb,va±vb]T
- 乘法: q a q b = [ s a s b − v a T v b , s a v b + s b v a + v a × v b ] \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] qaqb=[sasb−vaTvb,savb+sbva+va×vb]
- 模长： ∥ q a ∥ = s a 2 + x a 2 + y a 2 + z a 2 \Vert \textbf q_a \Vert =\sqrt{s_a^2 + x_a^2 + y_a^2 + z_a^2} ∥qa∥=sa2+xa2+ya2+za2
- 共轭： q a ∗ = [ s a , − v a ] T q_a^* = [s_a, -\textbf v_a ]^T qa∗=[sa,−va]T
- 逆： q a − 1 = q a ∗ / ∥ q a ∥ 2 q_a^{-1} = q_a^* / \Vert q_a\Vert^2 qa−1=qa∗/∥qa∥2
用四元数表示旋转
假设有一个三维空间点 p = [ x , y , z ] ∈ R 3 \textbf p = [x,y,z]\in \mathbb R^3 p=[x,y,z]∈R3, 和一个单位四元数 q \textbf q q 指定的旋转，记旋转后的点为 p ′ \textbf p' p′，我们有矩阵描述：
p ′ = R p \textbf p' = R\textbf p p′=Rp
我们将三维空间点，记成一个虚四元数，即：
p = [ 0 , x , y , z ] T = [ 0 , v ] T \textbf p = [0, x, y, z]^T = [0, \textbf v]^T p=[0,x,y,z]T=[0,v]T
则旋转后的点，可以被表示成：
p ′ = qp q − 1 \textbf p' = \textbf q\textbf p\textbf q^{-1} p′=qpq−1
这个点也是一个虚四元数
Proof:
假设旋转四元数为 q = [ s , a , b , c ] T = [ s , v q ] \textbf q = [s, a, b, c]^T = [s, \textbf v_q] q=[s,a,b,c]T=[s,vq], 可得
q − 1 = [ s , − a , − b , − c ] T ⋅ 1 s 2 + a 2 + b 2 + c 2 \textbf q^{-1} = [s, -a, -b, -c]^T \cdot \frac{1}{s^2 + a^2 + b^2 + c^2} q−1=[s,−a,−b,−c]T⋅s2+a2+b2+c21
从而：
qp q − 1 = [ − v T v q , s v + v × v q ] ⋅ q ∗ ∥ q ∥ 2 = [ − v T v q , s v + v × v q ] ⋅ [ s , − v q ] ⋅ 1 ∥ q ∥ 2 = [ − v T v q s − ( s v + v × v q ) T ( − v q ) , ⋯ ] ⋅ 1 ∥ q ∥ 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} qpq−1=[−vTvq,sv+v×vq]⋅∥q∥2q∗=[−vTvq,sv+v×vq]⋅[s,−vq]⋅∥q∥21=[−vTvqs−(sv+v×vq)T(−vq),⋯]⋅∥q∥21
注意到， v × v q \textbf v \times \textbf v_q v×vq 的结果是和 v \textbf v v 以及 v q \textbf v_q vq 都垂直，所以 v × v q × − v q = 0 \textbf v\times \textbf v_q \times -\textbf v_q = 0 v×vq×−vq=0
− v T v q s − s v T ( − v q ) = 0 -\textbf v^T\textbf v_q s - s\textbf v^T (-\textbf v_q) = 0 −vTvqs−svT(−vq)=0