第三讲:三维空间的刚体运动
文章目录
本讲主要介绍 一个刚体在三维空间中如何运动的.
1. 旋转矩阵
1.1. 点和向量,坐标系
三维空间的刚体要考虑 位置 和 姿态 .
向量内积:
a ⃗ ⋅ b ⃗ = a ⃗ T b ⃗ = ∑ i = 1 3 a i b i = ∣ a ⃗ ∣ ∣ b ⃗ ∣ cos ⟨ a ⃗ , b ⃗ ⟩ \vec{a} \cdot \vec{b} = \vec{a} ^T \vec{b} = \sum_{i=1}^3a_ib_i = |\vec{a}||\vec{b}| \cos \langle \vec{a}, \vec{b} \rangle a⋅b=aTb=i=1∑3aibi=∣a∣∣b∣cos⟨a,b⟩
向量外积:
a ⃗ × b ⃗ = ∥ i ⃗ j ⃗ k ⃗ a 1 a 2 a 3 b 1 b 2 b 3 ∥ = [ a 2 b 3 − a 3 b 3 a 1 b 3 − a 3 b 1 a 1 b 2 − a 2 b 1 ] = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] b ⃗ \vec{a} \times \vec{b} = \begin{Vmatrix} \vec{i} & \vec{j} & \vec{k} \\ a_1 & a_2 & a_3 \\ b_1 & b_2 & b_3 \end{Vmatrix}= \begin{bmatrix} a_2b_3-a_3b_3 \\ a_1b_3-a_3b_1 \\ 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} \vec{b} a×b=∥∥∥∥∥∥ia1b1ja2b2ka3b3∥∥∥∥∥∥=⎣⎡a2b3−a3b3a1b3−a3b1a1b2−a2b1⎦⎤=⎣⎡0a3−a2−a30a1a2−a10⎦⎤b
反对称矩阵 A T = − A A^T = - A AT=−A , 在本书中将定义反对称矩阵为: a的反对称矩阵为 a^
1.2. 坐标系间的欧式变换
对于同一个向量 p ⃗ \vec{p} p, 它在世界坐标系和相机坐标系下的坐标是不同的. 在两个坐标系下的转变我们用转换矩阵 T T T来表示
一个欧式变换由 旋转 和 平移 两部分组成.
-
旋转:
假设某个单位正交基 ( e 1 , e 2 , e 3 ) (e_1,e_2,e_3) (e1,e2,e3)经过旋转变为了 ( e 1 , e 2 , , e 3 , ) (e_1^,e_2^,,e_3^, ) (e1,e2,,e3,), 对于向量 a ⃗ \vec{a} a :
[ e 1 T e 2 T e 3 T ] [ a 1 a 2 a 3 ] = [ e 1 , e 2 , e 3 , ] [ a 1 , a 2 , a 3 , ] \begin{bmatrix} e_1^T & e_2^T & e_3^T \end{bmatrix} \begin{bmatrix} a_1 \\ a_2 \\ a_3 \end{bmatrix}= \begin{bmatrix} e_1^, & e_2^, & e_3^, \end{bmatrix} \begin{bmatrix} a_1^, \\ a_2^, \\ a_3^, \end{bmatrix} [e1Te2Te3T]⎣⎡a1a2a3⎦⎤=[e1,e2,e3,]⎣⎡a1,a2,a3,⎦⎤
所以可得:
[ a 1 a 2 a 3 ] = [ e 1 T e 1 , e 1 T e 2 , e 1 T e 3 , e 2 T e 1 , e 2 T e 2 , e 2 T e 3 , e 3 T e 1 , e 3 T e 2 , e 3 T e 3 , ] [ a 1 , a 2 , a 2 , ] = R a , \begin{bmatrix} a_1 & a_2 & a_3 \end{bmatrix}= \begin{bmatrix} e_1^Te_1^, & e_1^Te_2^, & e_1^Te_3^, \\ e_2^Te_1^, & e_2^Te_2^, & e_2^Te_3^, \\ e_3^Te_1^, & e_3^Te_2^, & e_3^Te_3^, \\ \end{bmatrix} \begin{bmatrix} a_1^, & a_2^, & a_2^, \end{bmatrix} = Ra^, [a1a2a3]=⎣⎡e1Te1,e2Te1,e3Te1,e1Te2,e2Te2,e3Te2,e1Te3,e2Te3,e3Te3,⎦⎤[a1,a2,a2,]=Ra,
R R R 是一个旋转矩阵, 旋转矩阵的性质:
旋 转 矩 阵 R { R 是 正 交 矩 阵 { R − 1 = R T R T R = I R 的 行 列 式 为 1 旋转矩阵R \begin{cases} R是正交矩阵 \begin{cases} R^{-1} = R^T \\ R^TR = I \end{cases} \\ R的行列式为1 \end{cases} 旋转矩阵R⎩⎪⎨⎪⎧R是正交矩阵{R−1=RTRTR=IR的行列式为1
将n维空间的旋转矩阵表示为 S O ( n ) SO(n) SO(n)
-
平移:
将旋转和平移合并到一起:
a , = R a + t a^, = Ra +t a,=Ra+t
t t t表示平移向量
1.3. 变化矩阵与齐次坐标系
如果进行了两次线性变换:
b = R 1 a + t 1 , c = R 2 b + t 2 b=R_1a+t_1, \quad c=R_2b+t_2 b=R1a+t1,c=R2b+t2
那么从a到c的变换为:
c = R 2 ( R 1 a + t 1 ) + t 2 c=R_2(R_1a+t_1)+t_2 c=R2(R1a+t1)+t2
但是这样的变换太复杂,我们引入齐次坐标和变换矩阵的重写式:
[ a , 1 ] = [ R t 0 T 1 ] [ a 1 ] = T [ a 1 ] \begin{bmatrix} a^, \\1 \end{bmatrix}= \begin{bmatrix} R & t \\ 0^T & 1 \end{bmatrix} \begin{bmatrix} a \\ 1 \end{bmatrix} = T \begin{bmatrix} a \\ 1 \end{bmatrix} [a,1]=[R0Tt1][a1]=T[a1]
我们在一个三维向量的末尾加1,将其变为4维向量, 称其为 齐次坐标 . 我们将旋转和平移写在了一个矩阵里,使得这个关系变成了线性关系.
我们用 a ~ \tilde{a} a~ 表示 a a a 的齐次坐标.
齐次坐标: 乘以任意非零常数后仍表示同一坐标.
转化为非齐次坐标,就是乘以一个非零常数将坐标的最后以为变为1
那么;坐标a 到坐标 c的转变为:
c ~ = T 2 T 1 a ~ \tilde{c} =T_2T_1\tilde{a} c~=T2T1a~
在后面默认用 b = T a b=Ta b=Ta 表示其次方程.
关于上述的转换矩阵T, 又称为 特殊欧式群
S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } SE(3) = \left\{ T = \begin{bmatrix} R & t \\ 0^T & 1 \end{bmatrix} \in R^{4 \times 4} | R \in SO(3), t\in \Bbb{R}^{3} \right\} SE(3)={T=[R0Tt1]∈R4×4∣R∈SO(3),t∈R3}
一个欧式运动由
旋转和平移组成.
与 S O ( 3 ) SO(3) SO(3)一样,求解该矩阵的逆表示一个反向变换:
T − 1 = [ R T − R T t 0 T 1 ] T^{-1} = \begin{bmatrix} R^T & -R^Tt \\ 0^T & 1 \end{bmatrix} T−1=[RT0T−RTt1]
为了方便,在后面使用
Ta表示齐次坐标,使用Ra表示非齐次坐标.
2. 实践:Eigen
该部分有两个内容:
- 如何使用Eigen来表示矩阵,向量
- 旋转矩阵与变换矩阵的计算
安装Eigen:
sudo apt-get install libeigen3-dev
对eigen的实践使用:
转code/第三讲文件夹
3. 旋转向量和欧拉角
3.1. 旋转向量
回顾:
- SO(n)特殊正交群: S O ( n ) = { R ∈ R n × n ∣ R R T = I , d e t ( R ) = 1 } SO(n) = \{R \in R^{n \times n} | RR^T =I, det(R)=1 \} SO(n)={R∈Rn×n∣RRT=I,det(R)=1} . R为旋转矩阵.
- SE(n)特殊欧式群: $
SE(3) =
\left{
T =
\begin{bmatrix}
R & t \
0^T & 1
\end{bmatrix}
\in R^{4 \times 4} | R \in SO(3), t\in \Bbb{R}^{3}
\right}
$ ,T 为变换矩阵.
存在的问题:
- SO(3)的旋转矩阵R有9个量,SE(3)的变换矩阵有16个量表达了6个自由度的变换. 是否有更加紧凑的表达方式呢?
- R,T矩阵必须是正交矩阵,且行列式为1. 这些约束使得在估计或优化矩阵时变得困难.
因此,需要一种更紧凑的方法来描述旋转和平移.
旋转向量: 其方向与旋转轴一致, 而长度等于旋转角. 这种表示方法只需要一个三维向量就可以描述旋转.
一次变换可用三维向量旋转,3维向量平移,合并为一个6维向量表示.
实际上
旋转向量就是李代数
罗德里的公式(Rodrigues’s Formula)表明:
R = cos θ I + ( 1 − cos θ ) n n T + sin θ n ∧ R = \cos{\theta I} +(1-\cos{\theta})nn^T + \sin{\theta } n^{\wedge} R=cosθI+(1−cosθ)nnT+sinθn∧
符号 ^ 表示向量的反对称符号.
反之,可以计算旋转矩阵到旋转向量的转换. 对于转角 θ \theta θ :
t r ( R ) = cos θ t r ( I ) + ( 1 − cos θ ) t r ( n n T ) + sin θ t r ( n ∧ ) = 3 cos θ + ( 1 − cos θ ) = 1 + 2 cos θ \begin{aligned} tr(R) & = \cos\theta tr(I) + (1-\cos\theta) tr(nn^T) + \sin{\theta} tr(n^{\wedge})\\ & = 3\cos\theta +(1-\cos\theta) \\ & = 1+2\cos\theta \end{aligned} tr(R)=cosθtr(I)+(1−cosθ)tr(nnT)+sinθtr(n∧)=3cosθ+(1−cosθ)=1+2cosθ
因此:
θ = arccos ( t r ( R ) − 1 2 ) \theta=\arccos(\frac{tr(R)-1}{2}) θ=arccos(2tr(R)−1)
由于旋转轴上的向量在旋转后不发生变化,所以关于转轴n:
R
n
=
n
⇒
(
R
−
1
)
n
=
0
Rn = n \quad \Rightarrow \quad (R-1)n =0
Rn=n⇒(R−1)n=0
所以,转轴n是矩阵R特征值1对应的特征向量.
3.2. 欧拉角
将旋转分解为 X Y Z XYZ XYZ轴上的旋转. 著名的旋转顺序为 Z Y X ZYX ZYX,它被称为 r p y rpy rpy旋转,使用 [ r , p , y ] T [r,p,y]^T [r,p,y]T向量表示.
欧拉角的重大缺陷是会碰到著名的万向锁问题: 在俯仰角(绕Y轴旋转)为
±
9
0
∘
\pm90^\circ
±90∘时,第一次旋转和第三次旋转将使用同一个轴,使得系统丢失了一个自由度. 这被称为奇异性问题.
欧拉角不适用与插值与迭代,往往只用于人机交互中, 本书中很少使用欧拉角表示姿态,也不会在滤波和优化中使用欧拉角表示旋转.
4. 四元数
4.1. 四元数的定义
- 旋转矩阵用9个量描述3个自由度具有冗余性.
- 欧拉角和旋转向量是紧凑的,但是具有奇异性.
事实上, 找不到不带奇异性的三维向量描述方式
使用复数来表示:
- 复数集 C \Bbb{C} C表示平面上的向量
- 复数乘法表示复平面的旋转
有一种类似于复数的代数:四元数, 四元数是一种扩展的复数,它既是紧凑的,也没有奇异性. 缺点:不够直观,运算稍复杂.
一个四元数有一个实部和三个虚部:
q
=
q
0
+
q
1
i
+
q
2
j
+
q
3
k
q=q_0+q_1i+q_2j+q_3k
q=q0+q1i+q2j+q3k
i , j , k i,j,k i,j,k为其三个虚部,它们满足以下关系:
{ 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 \begin{cases} i^2=j^2=k^2=-1 \\ ij=k,ji=-k \\ jk=i,kj=-i \\ ki=j,ik=-j \end{cases} ⎩⎪⎪⎪⎨⎪⎪⎪⎧i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j
有时也使用一个标量和一个向量来表达四元数:
q = [ s , v ] , s = q 0 ∈ R , v = [ q 1 , q 2 , q 3 ] T ∈ R 3 {\bm q} =[s,{\bm v}],\quad s=q_0\in {\Bbb{R}}, \quad {\bm v} =[q_1,q_2,q_3]^T \in {\Bbb{R^3}} q=[s,v],s=q0∈R,v=[q1,q2,q3]T∈R3
s为四元数实部,v 为虚部,如果虚部为0,表示
实四元数;反之,实部为0表示虚四元数.
乘以i对应着旋转 18 0 ∘ 180^\circ 180∘,这样才能保证 i j = k ij=k ij=k的性质.而 i 2 = − 1 i^2=-1 i2=−1表明绕i旋转 36 0 ∘ 360^\circ 360∘后得到相反的东西.这个东西要旋转两周才会恢复原样.
某个旋转绕单位向量 n = [ n x , n y , n z ] T {\bm n}=[n_x,n_y,n_z]^T n=[nx,ny,nz]T进行了角度为 θ \theta θ的旋转,那么旋转的四元数形式为:
q = [ cos θ 2 , n x sin θ 2 , n y sin θ 2 , n z sin θ 2 ] T {\bm q}=[\cos \frac{\theta}{2},n_x\sin{\frac{\theta}{2}},n_y\sin{\frac{\theta}{2}},n_z\sin{\frac{\theta}{2}}]^T q=[cos2θ,nxsin2θ,nysin2θ,nzsin2θ]T
反之,也可从单位四元数中计算出对应旋转的夹角:
{ θ = 2 arccos q 0 [ n x , n y , n z ] T = [ q 1 , q 2 , q 3 ] T / sin θ 2 \begin{cases} \theta = 2\arccos q_0 \\ [n_x,n_y,n_z]^T = [q_1,q_2,q_3]^T/\sin \frac{\theta}{2} \end{cases} {θ=2arccosq0[nx,ny,nz]T=[q1,q2,q3]T/sin2θ
任意的旋转都可以由两个互为相反数的四元数表示
另外,取 θ = 0 \theta=0 θ=0: q 0 = [ ± 1 , 0 , 0 , 0 ] T {\bm q_0}=[\pm1,0,0,0]^T q0=[±1,0,0,0]T,得到一个没有旋转的四元数.
4.2. 四元数的运算
四元数和通常的复数一样可以进行四则运算,数乘,求逆,共轭等运算.
准备两个四元数:
q
a
=
s
a
+
x
a
i
+
y
a
j
+
z
a
k
,
q
a
=
s
b
+
x
b
i
+
y
b
j
+
z
b
k
{\bm q_a}=s_a+x_ai+y_aj+z_ak, \quad {\bm q_a} = s_b+x_bi+y_bj+z_bk
qa=sa+xai+yaj+zak,qa=sb+xbi+ybj+zbk
-
加减运算
q a ± q b = [ s a s b , v a ± v b ] {\bm q_a \pm q_b} = [s_a \rm s_b,\bm {v_a \pm v_b}] qa±qb=[sasb,va±vb]
-
乘法
q a , q b q_a,q_b qa,qb的每一项相乘后相加.
q a q b = s a s b − x a x b − y a y b − z a z b + ( s a x b + x a s b + y a z b − z a y b ) i + ( s a y b − x a z b + y a s b + z a x b ) j + ( s a z b + x a y b − y a x b + z a s b ) k \begin{aligned} \bm {q_aq_b} & = s_as_b -x_ax_b-y_ay_b-z_az_b \\ & +(s_ax_b+x_as_b+y_az_b-z_ay_b)i \\ & +(s_ay_b-x_az_b+y_as_b+z_ax_b)j \\ & +(s_az_b+x_ay_b-y_ax_b+z_as_b)k \end{aligned} qaqb=sasb−xaxb−yayb−zazb+(saxb+xasb+yazb−zayb)i+(sayb−xazb+yasb+zaxb)j+(sazb+xayb−yaxb+zasb)k如果写成向量形式并利用内外积运算:
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 ] \bm {q_aq_b} =[s_as_b-v_a^Tv_b,s_av_b+s_bv_a+v_a \times v_b] qaqb=[sasb−vaTvb,savb+sbva+va×vb] -
共轭
四元数的共轭是把虚部取相反数:
q a ∗ = s a − x a i − y a j − z a k = [ s a , v a ] {\bm q_a^*} = s_a-x_ai-y_aj-z_ak = [s_a,{\bm v_a}] qa∗=sa−xai−yaj−zak=[sa,va] -
模长
∣ ∣ q a ∣ ∣ = s a 2 + x a 2 + y a 2 + z a 2 ||\bm{q_a}||=\sqrt{s_a^2+x_a^2+y_a^2+z_a^2} ∣∣qa∣∣=sa2+xa2+ya2+za2
-
逆
q − 1 = q ∗ ∣ ∣ q ∣ ∣ 2 \bm{q^{-1}} =\frac{q^*}{||q||^2} q−1=∣∣q∣∣2q∗
按照此定义四元数和自己的逆的乘积为实四元数1
q q − 1 = q − 1 q = 1 qq^{-1} = q^{-1}q = 1 qq−1=q−1q=1如果q为单位四元数,其逆和共轭是同一个量
其乘积的逆有和矩阵相似的性质:
( q a q b ) − 1 = q b − 1 q a − 1 (q_aq_b)^{-1} = q_b^{-1}q_a^{-1} (qaqb)−1=qb−1qa−1 -
数乘与点乘
和向量相似,四元数可以与数相乘:
k q = [ k s , k v ] k\bm{q} = [ks,k\bm{v}] kq=[ks,kv]点乘: 两个四元数每个位置上的数值分别相乘.
q a q b = s a s b + x a x b + y a y b + z a z b \bm{q_aq_b} = s_as_b +x_ax_b+y_ay_b+z_az_b qaqb=sasb+xaxb+yayb+zazb
4.3. 用四元数表示旋转
假设一个三维点 p = [ x , y , z ] ∈ R 3 \bm{p}=[x,y,z] \in \Bbb{R}^3 p=[x,y,z]∈R3 ,以及轴角 n , θ \bm{n},\theta n,θ指定的旋转.
这里的 n n n是一个三维向量,表示旋转轴
首先,把三维空间点用一个虚四元数来表示:
p
=
[
0
,
x
,
y
,
z
]
=
[
0
,
v
]
\bm{p} = [0,x,y,z] = [0,\bm{v}]
p=[0,x,y,z]=[0,v]
然后,用四元数q表示这个旋转:
q
=
[
cos
θ
2
,
n
sin
θ
2
]
\bm{q} = [\cos \frac{\theta}{2},\bm{n}\sin \frac{\theta}{2}]
q=[cos2θ,nsin2θ]
最后,旋转后的
p
′
\bm{p^\prime}
p′:
p
′
=
q
p
q
−
1
\bm{p^\prime}=\bm{qpq}^{-1}
p′=qpq−1
4.4. 四元数到旋转矩阵的转换
假设四元数为
q
=
q
0
+
q
1
i
+
q
2
j
+
q
3
k
\bm{q}=q_0+q_1i+q_2j+q_3k
q=q0+q1i+q2j+q3k,对应旋转矩阵为:
R
=
[
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
]
\bm{R}= \begin{bmatrix} 1-2q_2^2-2q_3^2 & 2q_1q_2+2q_0q_3 & 2q_1q_3-2q_0q_2 \\ 2q_1q_2-2q_0q_3 & 1-2q_1^2-2q_3^2 & 2q_2q_3+2q_0q_1 \\ 2q_1q_3+2q_0q_2 & 2q_2q_3-2q_0q_1 & 1-2q_1^2-2q_2^2 \end{bmatrix}
R=⎣⎡1−2q22−2q322q1q2−2q0q32q1q3+2q0q22q1q2+2q0q31−2q12−2q322q2q3−2q0q12q1q3−2q0q22q2q3+2q0q11−2q12−2q22⎦⎤
由旋转矩阵转换到四元数:
q 0 = t r ( R ) + 1 2 , q 1 = m 23 − m 32 4 q 0 , q 2 = m 31 − m 13 4 q 0 , q 3 = m 12 − m 21 4 q 0 q_0 = \frac{\sqrt{tr(R)+1}}{2} ,\quad q_1 = \frac{m_{23}-m_{32}}{4q_0}, \quad q_2 = \frac{m_{31}-m_{13}}{4q_0}, \quad q_3 = \frac{m_{12}-m_{21}}{4q_0} q0=2tr(R)+1,q1=4q0m23−m32,q2=4q0m31−m13,q3=4q0m12−m21
一个 R \bm{R} R对应的四元数并不唯一.
5. 相似, 仿射, 射影变换
- 相似变换
T s = [ s R t 0 T 1 ] \bm{T}s = \begin{bmatrix} s\bm{R} & \bm{t} \\ \bm{0}^T & 1 \end{bmatrix} Ts=[sR0Tt1] - 仿射变换
T A = [ A t 0 T 1 ] \bm{T}_A = \begin{bmatrix} \bm{A} & \bm{t} \\ \bm{0}^T & 1 \end{bmatrix} TA=[A0Tt1] - 射影变换
T p = [ A t a T v ] \bm{T}_p = \begin{bmatrix} \bm{A} & \bm{t} \\ \bm{a}^T & v \end{bmatrix} Tp=[AaTtv]
总结几种变换的性质:
| 变换名称 | 矩阵形式 | 自由度 | 不变性质 |
|---|---|---|---|
| 欧式变换 | $\ \begin{bmatrix}\bm{R} & \bm{t} \ \bm{0}^T & 1 \end{bmatrix}\$ | 6 | 长度,夹角,面积 |
| 相似变换 | $\ \begin{bmatrix} s\bm{R} & \bm{t} \ \bm{0}^T & 1 \end{bmatrix}\$ | 7 | 体积比 |
| 仿射 | $\ \begin{bmatrix}\bm{A} & \bm{t} \ \bm{0}^T & 1 \end{bmatrix}\$ | 12 | 平行性,体积比 |
| 射影变换 | $\ \begin{bmatrix}\bm{A} & \bm{t} \ \bm{a}^T & v \end{bmatrix}\$ | 15 | 接触平面的相交和相切 |
从真实世界到相机照片的变换是射影变换
扫一扫
783
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
