文章目录
第三讲 三维空间刚体运动
3.1 旋转矩阵
外积的反对称矩阵写法
a
×
b
=
[
0
−
a
3
a
2
a
3
0
−
a
1
−
a
2
a
1
0
]
b
a\times b= \begin{bmatrix} 0 && -a_3 && a_2 \\ a_3 && 0 && -a_1 \\ -a_2 && a_1 && 0 \end{bmatrix} b
a×b=⎣⎡0a3−a2−a30a1a2−a10⎦⎤b
记作a^b,其中 ^是反对称符号。
向量的内外积即使在不谈论它们的坐标时也可以计算,例如内积可以通过长度和夹角来计算。
坐标系之间的欧式变换
两个坐标系之间的运动由旋转加上平移组成,称为刚体运动
对于两个坐标系下的两组坐标表示的同一个向量,有
[
e
1
e
2
e
3
]
[
a
1
a
2
a
3
]
=
[
e
1
′
e
2
′
e
3
′
]
[
a
1
′
a
2
′
a
3
′
]
\begin{bmatrix} e_1 && e_2 && e_3 \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}
[e1e2e3]⎣⎡a1a2a3⎦⎤=[e1′e2′e3′]⎣⎡a1′a2′a3′⎦⎤
左右两边同时左乘
[
e
1
e
2
e
3
]
T
\begin{bmatrix}e_1 && e_2 && e_3\end{bmatrix}^T
[e1e2e3]T得
[
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
3
′
]
=
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_3' \end{bmatrix}= Ra'
⎣⎡a1a2a3⎦⎤=⎣⎡e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e2Te3′e3Te3′⎦⎤⎣⎡a1′a2′a3′⎦⎤=Ra′
R的各分量是两个坐标系基的内积,由于基向量的长度为一,因此每个分量其实是基向量夹角的余弦值。
旋转矩阵的性质
行列式为1的正交矩阵,可以将n维旋转矩阵的集合定义如下:
S
O
(
n
)
=
{
R
∈
R
n
×
n
∣
R
R
T
=
I
,
det
(
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}
SO(n)称为特殊正交群
进而有
a
′
=
R
−
1
a
=
R
T
a
a'=R^{-1}a=R^Ta
a′=R−1a=RTa
考虑平移
a
′
=
R
a
+
t
a'=Ra+t
a′=Ra+t
考虑坐标系1,2,a在两个坐标系下的坐标分别为
a
1
a_1
a1,
a
2
a_2
a2:
a
1
=
R
12
a
2
+
t
12
a_1=R_{12}a_2+t_{12}
a1=R12a2+t12
其中,
R
12
R_{12}
R12表示把坐标系2的向量变换到坐标系1中。
t
12
t_{12}
t12对应的是坐标系1原点指向坐标系2原点的向量,在坐标系1下取的坐标。
注:
t
12
≠
t
21
t_{12}\neq t_{21}
t12=t21
变换矩阵与齐次坐标
存在的问题
变换关系不是线性关系
b
=
R
1
a
+
t
1
c
=
R
2
b
+
t
2
b=R_1a+t_1 \qquad c=R_2b+t_2
b=R1a+t1c=R2b+t2
则有
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]
T称为变换矩阵
特殊欧式群
S
E
(
3
)
=
{
T
=
[
R
t
0
T
1
]
∈
R
4
×
4
∣
R
∈
S
O
(
3
)
,
t
∈
R
3
}
SE(3)=\{T=\begin{bmatrix}R&&t\\0^T&&1\end{bmatrix}\in R^{4\times 4}|R\in SO(3),t\in R^3\}
SE(3)={T=[R0Tt1]∈R4×4∣R∈SO(3),t∈R3}
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]
用
T
12
T_{12}
T12表示从坐标系1到坐标系2的变换。
3.2 实践:Eigen
要点
1. 声明矩阵
Matrix<数据类型,行,列>;
//内置类型
//Vector3d实质上是Eigen::Matrix<double,3,1>
Vector3d v_3d;
//Matrix3d实质上是Eigen::Matrix<double,3,3>
Matrix3d matrix_33;
//动态大小的矩阵
Matrix<double,Dynamic,Dynamic> matrix_dynamic;
//更简单的
MatrixXd matrix_x;
2. 初始化操作
//初始化为零
Matrix3d::Zero();
//输入数据
//按行输入
matrix_23 << 1,2,3,4,5,6
3. 输出
//直接输出
cout << "..." << matrix_23 << endl;
//用()访问矩阵中的元素
for (int i = 0; i < 2 ; i++) {
for (int j = 0; j < 3 ; j++) cout << matrix_23(i,j) << "\t";
cout << endl;
}
4. 运算
//矩阵和向量相乘
//注意数据类型匹配,需要用.cast<数据类型>()进行显示转换
Matrix<double,2,1> result = matrix_23.cast<double>() * V_3d;
//随机数矩阵
matrix_33 = Matrix3d::Random();
/*
转置:.transpose()
各元素和:.sum()
迹:.trace()
逆:.inverse()
行列式:.determinant()
*/
//特征值
//实对称矩阵可以保证对角化成功
SelfAdjointEigenSolver<Matrix3d> eigen_solver(matrix_33.transpose()*matrix_33);
//特征值
eigen_solver.eigenvalues()
//特征向量
eigen_solver.eigenvectors()
//解方程
//1. 直接求逆
Matrix<double,MATRIX_SIZE,1> x = matrix_NN.inverse() * V_Nd;
//2. 矩阵分解
Matrix<double,MATRIX_SIZE,1> x = matrix_NN.colPivHouseHolderQr().solve(v_Nd);
//3. 对于正定矩阵,还可以用cholesky分解来解方程
Matrix<double,MATRIX_SIZE,1> x = matrix_NN.ldlt().solve(v_Nd)
5. 添加头文件
include_directories("/usr/include/eigen3")
由于Eigen库只有头文件,所以不需要再用target_link_libraries语句将程序链接到库上。
3.3 旋转向量和欧拉角
旋转向量
存在的问题
- SO(3)用9个量表达了3个自由度
SE(3)用16个量表达了6个自由度
存在冗余 - 旋转矩阵自身存在约束,会使得估计或优化变得更困难
解决方案
用一种方式紧凑地描述旋转和平移
- 任意旋转都可以用一个旋转轴和一个旋转角来刻画
可以使用一个向量,其方向与旋转轴一致,而长度等于旋转角。这种向量称为旋转向量。 - 对于变换矩阵,使用一个旋转向量和一个平移向量即可表达一次变换。这时的变量维数正好是六维。
罗德里格斯公式
R = c o s θ I + ( 1 − c o s θ ) n n T + s i n θ n ∧ R=cos\theta I + (1-cos\theta)nn^T+sin\theta n^\wedge R=cosθI+(1−cosθ)nnT+sinθn∧
θ \theta θ和n的推导
t
r
(
R
)
=
c
o
s
θ
t
r
(
I
)
+
(
1
−
c
o
s
θ
)
t
r
(
n
n
T
)
+
s
i
n
θ
t
r
(
n
⋅
)
=
3
c
o
s
θ
+
(
1
−
c
o
s
θ
)
=
1
+
2
c
o
s
θ
\begin{aligned} tr(R)&=cos\theta tr(I) + (1-cos\theta)tr(nn^T)+sin\theta tr(n^\cdot) \\ &=3cos\theta+(1-cos\theta) \\ &=1+2cos\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
θ=arccos2tr(R)−1
关于转轴n,旋转轴上的向量在旋转后不发生改变,即
R
n
=
n
Rn=n
Rn=n
因此,n是矩阵R特征值1对应的特征向量对应的单位向量。
即有两个转换公式
{
θ
=
arccos
t
r
(
R
)
−
1
2
R
n
=
n
\begin{cases} \theta=\arccos \frac{tr(R)-1}2 \\ Rn = n \end{cases}
{θ=arccos2tr(R)−1Rn=n
欧拉角
存在的问题
旋转矩阵和旋转向量不够直观。
解决方案
使用3个分离的转角,把一个旋转分解成3次绕不同轴的旋转。
假设一个刚体的前方为X轴,右侧为Y轴,上方为Z轴。ZYX转角相当于把任意旋转分解成以下3个轴上的转角:
- 绕物体的Z轴旋转,得到偏航角yaw
- 绕旋转之后的Y轴旋转,得到俯仰角pitch
- 绕旋转之后的X轴旋转,得到滚转角roll
此时,可以使用 [ r , p , y ] T [r,p,y]^T [r,p,y]T这样一个三维的向量描述任意旋转。
万向锁问题
俯仰角为 ± 9 0 ∘ \pm90^\circ ±90∘时,第一次旋转与第三次旋转将使用同一个轴,使得系统丢失了一个自由度。
也就是说,第三次旋转可以被包含进第一次旋转中,从而丢失了一个自由度。
从理论角度:
当
θ
=
π
2
\theta=\dfrac{\pi}{2}
θ=2π时,公式变为:
可以看到,改变
ϕ
\phi
ϕ和
ψ
\psi
ψ的效果相同,而旋转轴始终是z轴。
从旋转表示方式角度:
一次旋转应当由4个变量表示:3个角度和1个旋转顺序
若只用3个变量表示,在特定情况下会丢失旋转顺序。
这使得欧拉角不适用于插值和迭代,往往只用于人机交互中。
3.4 四元数
定义
一个四元数拥有一个实部和三个虚部
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
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
]
T
,
s
=
q
0
∈
R
,
v
=
[
q
1
,
q
2
,
q
3
]
∈
R
3
q=[s,v]^T,s=q_0\in R,v=[q_1,q_2,q_3]\in R^3
q=[s,v]T,s=q0∈R,v=[q1,q2,q3]∈R3
四元数的运算
现有两个四元数
q
a
,
q
b
q_a,q_b
qa,qb,它们的向量表示为
[
s
a
,
v
a
]
T
,
[
s
b
,
v
b
]
T
[s_a,v_a]^T,[s_b,v_b]^T
[sa,va]T,[sb,vb]T,或者原始四元数表示为
q
a
=
s
a
+
x
a
i
+
y
a
j
+
z
a
k
,
q
b
=
s
b
+
x
b
i
+
y
b
j
+
z
b
k
q_a=s_a+x_ai+y_aj+z_ak,\qquad q_b=s_b+x_bi+y_bj+z_bk
qa=sa+xai+yaj+zak,qb=sb+xbi+ybj+zbk
- 加法和减法
- 乘法
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} 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 ] T q_aq_b=[s_as_b-v_a^Tv_b,s_av_b+s_bv_a+v_a\times v_b]^T qaqb=[sasb−vaTvb,savb+sbva+va×vb]T
四元数乘法不可交换 - 模长
四元数的模长定义为
∥ q a ∥ = s a 2 + x a 2 + y a 2 + z a 2 \left\|q_a \right\|=\sqrt{s_a^2+x_a^2+y_a^2+z_a^2} ∥qa∥=sa2+xa2+ya2+za2
可以验证,两个四元数乘积的模即模的乘积。这使得单位四元数相乘后仍是单位四元数。 - 共轭
四元数的共轭是把虚部取成相反数:
q ∗ q = q q ∗ = [ s a 2 + v T v , 0 ] T q^*q=qq^*=[s_a^2+v^Tv,0]^T q∗q=qq∗=[sa2+vTv,0]T - 逆
q − 1 = q ∗ / ∥ q a ∥ 2 q^{-1}=q^*/\left\|q_a \right\|^2 q−1=q∗/∥qa∥2
进而有
q q − 1 = q − 1 q = 1 qq^{-1}=q^{-1}q=1 qq−1=q−1q=1
若q为单位四元数,则有 q − 1 = q ∗ q^{-1}=q^* q−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 - 数乘
用四元数表示旋转
- 把空间三维点用一个虚四元数来描述:
p = [ 0 , x , y , z ] T = [ 0 , v ] T p=[0,x,y,z]^T=[0,v]^T p=[0,x,y,z]T=[0,v]T
旋转后的p’可表示为这样的乘积:
p ′ = q p q − 1 p'=qpq^{-1} p′=qpq−1
p’的虚部即为旋转之后点的坐标
四元数到其他旋转表示的转换
设
q
=
[
s
,
v
]
q=[s,v]
q=[s,v],定义
q
+
=
[
s
−
v
T
v
s
I
+
v
⋅
]
,
q
∘
=
[
s
−
v
T
v
s
I
−
v
⋅
]
q^+=\begin{bmatrix}s&&-v^T\\v&&sI+v^\cdot\end{bmatrix}, \quad q^\circ=\begin{bmatrix}s&&-v^T\\v&&sI-v^\cdot\end{bmatrix}
q+=[sv−vTsI+v⋅],q∘=[sv−vTsI−v⋅]
易得
q
1
q
2
=
q
1
+
q
2
=
q
2
∘
q
1
q_1q_2=q_1^+q_2=q_2^\circ q_1
q1q2=q1+q2=q2∘q1
进而有
p
′
=
q
p
q
−
1
=
q
+
p
+
q
−
1
=
q
+
q
−
1
∘
p
p'=qpq^{-1}=q^+p^+q^{-1}=q^+{q^{-1}}^\circ p
p′=qpq−1=q+p+q−1=q+q−1∘p
即有
R
=
q
+
q
−
1
∘
=
[
1
0
0
T
v
v
T
+
s
2
I
+
2
s
v
⋅
+
(
v
⋅
)
2
]
R=q^+{q^{-1}}^\circ= \begin{bmatrix} 1 && 0 \\ 0^T && vv^T+s^2I+2sv^\cdot+(v^\cdot)^2 \end{bmatrix}
R=q+q−1∘=[10T0vvT+s2I+2sv⋅+(v⋅)2]
由于p是虚四元数,有
R
=
v
v
T
+
s
2
I
+
2
s
v
⋅
+
(
v
⋅
)
2
R=vv^T+s^2I+2sv^\cdot+(v^\cdot)^2
R=vvT+s2I+2sv⋅+(v⋅)2
进而有
t
r
(
R
)
=
4
s
2
−
1
tr(R)=4s^2-1
tr(R)=4s2−1
带入上文结论可得
θ
=
arccos
(
2
s
2
−
1
)
\theta=\arccos(2s^2-1)
θ=arccos(2s2−1)
即
θ
=
2
arccos
s
\theta=2\arccos s
θ=2arccoss
在四元数旋转公式中,用q的虚部代替p,可得该向量在旋转中不动,即构成旋转轴,则有
{
θ
=
2
arccos
q
0
[
n
x
,
n
y
,
n
z
]
T
=
[
q
1
,
q
2
,
q
3
]
/
s
i
n
(
θ
2
)
\begin{cases} \theta=2\arccos q_0 \\ [n_x,n_y,n_z]^T=[q_1,q_2,q_3]/sin(\frac{\theta}2) \end{cases}
{θ=2arccosq0[nx,ny,nz]T=[q1,q2,q3]/sin(2θ)
相似、仿射、射影变换
1. 相似变换
多了一个缩放自由度
T
S
=
[
s
R
t
0
T
1
]
三
维
相
似
变
换
的
集
合
又
叫
相
似
变
换
群
,
记
作
S
i
m
(
3
)
T_S= \begin{bmatrix} sR && t \\ 0^T && 1 \end{bmatrix} 三维相似变换的集合又叫相似变换群,记作Sim(3)
TS=[sR0Tt1]三维相似变换的集合又叫相似变换群,记作Sim(3)
2. 仿射变换
T
S
=
[
A
t
0
T
1
]
T_S= \begin{bmatrix} A && t \\ 0^T && 1 \end{bmatrix}
TS=[A0Tt1]
仿射变换只要求A是一个可逆矩阵,而不必是正交矩阵
3. 射影变换
T S = [ A t a T v ] T_S= \begin{bmatrix} A && t \\ a^T && v \end{bmatrix} TS=[AaTtv]
3.6 实践:Eigen几何模块
见书上代码