三维空间的刚体位姿
参考：《视觉SLAM十四讲》、 四元数和欧拉角
1. 点和向量
向量的内积描述的向量之间的投影关系
向量的外积
a
×
b
=
[
a
1
b
1
c
1
a
2
b
2
c
2
i
j
k
]
=
[
0
−
a
3
a
2
a
3
0
−
a
1
−
a
2
a
1
0
]
b
≜
a
∧
b
a × b =\left[\begin{matrix} a_1 & b_1 & c_1\\ a_2 & b_2 & c_2\\ i & j & k\\ \end{matrix}\right] = \left[\begin{matrix} 0 & -a_3 & a_2\\ a_3 & 0 & -a_1\\ -a_2 & a_1 & 0\\ \end{matrix}\right]b \triangleq a^\land b
a×b=⎣⎡a1a2ib1b2jc1c2k⎦⎤=⎣⎡0a3−a2−a30a1a2−a10⎦⎤b≜a∧b
大小
∣
a
∣
∣
b
∣
s
i
n
<
a
,
b
>
|a||b|sin<a,b>
∣a∣∣b∣sin<a,b>, 其中
∧
^\land
∧可以看做是反对称的符号
反对称矩阵
设A为n维方阵，若有A’=-A，则称矩阵A为反对称矩阵。对于反对称矩阵，它的主对角线上的元素全为零，而位于主对角线两侧对称的元反号。反对称矩阵具有很多良好的性质，如若A为反对称矩阵，则A’，λA均为反对称矩阵；若A,B均为反对称矩阵，则A±B也为反对称矩阵；设A为反对称矩阵，B为对称矩阵，则AB-BA为对称矩阵；奇数阶反对称矩阵的行列式必为0。反对称矩阵的特征值是0或纯虚数，并且对应于纯虚数的特征向量的实部和虚部形成的实向量等长且互相正交。
外积表示两个向量之间的旋转关系
2. 刚体在欧式空间的位姿
刚体在欧式空间(大部分属于欧式空间,非欧式空间一般涉及不到)中的运动使用旋转和平移来描述.
定义刚体运动,任取刚体上的两个向量,刚体运动前后(我自己定义的)
- 两个向量的长度不发生改变
- 两个向量之间的夹角不发生改变
2.1 坐标系间的欧式变换(旋转矩阵和位移向量)
设某个向量在坐标系
{
A
}
\left\{A\right\}
{A}和坐标系
{
B
}
\left\{B\right\}
{B}下的坐标分别是
(
a
1
,
a
2
,
a
3
)
(a_1, a_2, a_3)
(a1,a2,a3),
(
a
1
′
,
a
2
′
,
a
3
′
)
({a_1'},{a_2'},{a_3'})
(a1′,a2′,a3′), 坐标系
{
A
}
\left\{A\right\}
{A}和坐标系
{
B
}
\left\{B\right\}
{B}正交基坐标分别是
(
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′), 则有
[
e
1
,
e
2
,
e
3
]
[
a
1
a
2
a
3
]
=
[
e
1
′
,
e
2
′
,
e
3
′
]
[
a
1
′
a
2
′
a
3
′
]
[e_1,e_2,e_3]\left[\begin{matrix} a_1\\ a_2\\ a_3\\ \end{matrix}\right] = [e_1',e_2',e_3']\left[\begin{matrix} a_1'\\ a_2'\\ a_3'\\ \end{matrix}\right]
[e1,e2,e3]⎣⎡a1a2a3⎦⎤=[e1′,e2′,e3′]⎣⎡a1′a2′a3′⎦⎤
等式右边同时左乘
[
e
1
T
,
e
2
T
,
e
3
T
]
[e_1^T,e_2^T,e_3^T]
[e1T,e2T,e3T]可得
[
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
′
\left[\begin{matrix} a_1\\ a_2\\ a_3\\ \end{matrix}\right]=\left[\begin{matrix} 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{matrix}\right]\left[\begin{matrix} a_1'\\ a_2'\\ a_3'\\ \end{matrix}\right]=Ra'
⎣⎡a1a2a3⎦⎤=⎣⎡e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e2Te3′e3Te3′⎦⎤⎣⎡a1′a2′a3′⎦⎤=Ra′
上面的3*3的矩阵就是旋转矩阵
R
R
R.旋转矩阵有些特别的性质:它是行列式为1的正交矩阵
正交矩阵是逆为自身转置的矩阵
所以我们就定义旋转矩阵的群
S
O
(
3
)
=
{
R
∈
R
n
×
n
∣
R
R
T
=
I
,
d
e
t
(
R
)
=
1
}
SO(3) = \left\{R\in R^{n×n}|RR^T = I, det(R)=1\right\}
SO(3)={R∈Rn×n∣RRT=I,det(R)=1}
其中,3代表三维欧式空间的意思.
再加上平移的变换
t
t
t就得到了
a
′
=
R
a
+
t
a' = Ra + t
a′=Ra+t
2.2 齐次变换(4*4变换矩阵)
2.1节中讲的变换并不是齐次的,当进行两次变换就会出现
a
′
′
=
R
′
(
R
a
+
t
)
+
t
′
a'' = R'(Ra + t) + t'
a′′=R′(Ra+t)+t′
多次变换比较复杂,所以引入齐次的变换
[
a
′
1
]
=
[
R
t
0
T
1
]
[
a
1
]
≜
T
[
a
1
]
\left[\begin{matrix} a'\\ 1 \\ \end{matrix}\right] = \left[\begin{matrix} R & t\\ 0^T & 1\\ \end{matrix}\right] \left[\begin{matrix} a\\ 1\\ \end{matrix}\right] \triangleq T\left[\begin{matrix} a\\ 1\\ \end{matrix}\right]
[a′1]=[R0Tt1][a1]≜T[a1]
T T T就是我们需要的变换矩阵
除了 T T T还有
- 相似变换 T S T_S TS
- 仿射变换 T A T_A TA
- 射影变换 T P T_P TP
2.3 旋转向量
为什么要用旋转向量?
- 旋转矩阵 R R R有9个参数但是旋转只有三个自由度,所以信息是有冗余的
- 而且旋转矩阵必须是正交矩阵且行列式为1
综上我们再进行优化的时候使用旋转矩阵就十分的不方便, 所以就有了只有三个参数的旋转向量
- 其长度为旋转角
- 其方向与旋转轴一致
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^\land R=cosθI+(1−cosθ)nnT+sinθn∧
其中
n
n
n为旋转轴
θ
=
a
r
c
c
o
s
(
t
r
(
R
)
+
1
2
)
\theta = arccos(\frac{tr(R)+1}{2})
θ=arccos(2tr(R)+1)
旋转向量为
θ
n
\theta n
θn
2.4 欧拉角
旋转向量和旋转矩阵虽然可以描述刚体的旋转但是对于人来说是不直观的, 因此我们引入另外一种便于理解的方法欧拉角.
欧拉角有两种形式
- 一种是绕固定的坐标轴旋转假设开始两个坐标系重合，先将{B}绕{A}的X轴旋转𝛾，然后绕{A}的Y轴旋转𝛽，最后绕{A}的Z轴旋转𝛼，就能旋转到当前姿态,可以称其为X-Y-Z fixed angles或RPY角(Roll, Pitch, Yaw)。
- r(roll, 翻滚角) 绕旋转之后的X轴旋转的角度
- p(pitch 俯仰角)绕旋转之后的Y轴旋转的角度
- yaw(偏航角)绕物体的Z轴旋转得到偏航角
对应的旋转矩阵
R
X
Y
Z
(
γ
,
β
,
α
)
=
R
Z
(
α
)
R
Y
(
β
)
R
X
(
γ
)
=
[
c
α
c
β
c
α
s
β
s
γ
−
s
α
c
γ
c
α
s
β
c
γ
+
s
α
s
γ
s
α
c
β
s
α
s
β
s
γ
+
c
α
c
γ
s
α
s
β
c
γ
−
c
α
s
γ
−
s
β
c
β
s
γ
c
β
c
γ
]
R_{XYZ}(\gamma,\beta,\alpha)=R_Z(\alpha)R_Y(\beta)R_X(\gamma)=\begin{bmatrix} c\alpha c\beta & c\alpha s\beta s\gamma-s\alpha c\gamma & c\alpha s\beta c\gamma+s\alpha s\gamma\\ s\alpha c\beta & s\alpha s\beta s\gamma+c\alpha c\gamma & s\alpha s\beta c\gamma-c\alpha s\gamma\\ -s\beta& c\beta s\gamma & c\beta c\gamma \end{bmatrix}
RXYZ(γ,β,α)=RZ(α)RY(β)RX(γ)=⎣⎡cαcβsαcβ−sβcαsβsγ−sαcγsαsβsγ+cαcγcβsγcαsβcγ+sαsγsαsβcγ−cαsγcβcγ⎦⎤
- 另一种是绕自身坐标轴轴旋转:假设开始两个坐标系重合，先将{B}绕自身的Z轴旋转𝛼，然后绕Y轴旋转𝛽，最后绕X轴旋转𝛾，就能旋转到当前姿态。称其为Z-Y-X欧拉角，由于是绕自身坐标轴进行旋转.
R Z ′ Y ′ X ′ ( α , β , γ ) = R Z ( α ) R Y ( β ) R X ( γ ) = [ c α c β c α s β s γ − s α c γ c α s β c γ + s α s γ s α c β s α s β s γ + c α c γ s α s β c γ − c α s γ − s β c β s γ c β c γ ] R_{Z'Y'X'}(\alpha,\beta,\gamma)=R_Z(\alpha)R_Y(\beta)R_X(\gamma)=\begin{bmatrix} c\alpha c\beta & c\alpha s\beta s\gamma-s\alpha c\gamma & c\alpha s\beta c\gamma+s\alpha s\gamma\\ s\alpha c\beta & s\alpha s\beta s\gamma+c\alpha c\gamma & s\alpha s\beta c\gamma-c\alpha s\gamma\\ -s\beta& c\beta s\gamma & c\beta c\gamma \end{bmatrix} RZ′Y′X′(α,β,γ)=RZ(α)RY(β)RX(γ)=⎣⎡cαcβsαcβ−sβcαsβsγ−sαcγsαsβsγ+cαcγcβsγcαsβcγ+sαsγsαsβcγ−cαsγcβcγ⎦⎤
可以发现这两种描述方式得到的旋转矩阵是一样的，即绕固定坐标轴X-Y-Z旋转(𝛾,𝛽,𝛼)(γ,β,α)和绕自身坐标轴Z-Y-X旋转(𝛼,𝛽,𝛾)(α,β,γ)的最终结果一样，只是描述的方法有差别而已。
但是使用欧拉角进行描述会遇到万向锁问题, 当俯仰角为±90的时候,第一次旋转与第三次旋转使用同一个轴,所以会缺失一个自由度,也叫奇异性问题.
这里查阅资料发现RPY角的定义有说是X-Y-Z《机器人学》 熊有伦等 编著 机械工业出版社, 有说是Z-Y-X《视觉SLAM十四讲》
可以证明(我不会证明)使用三个实数来描述旋转总是不可避免的遇到奇异性问题.所以欧拉角只适合用于人机交互。
2.5 四元数
旋转矩阵九个参数具有冗余性, 欧拉角和旋转向量是紧凑的但是会遇到奇异性的问题, 所以我们使用四个数也就是四元数来表示物体的旋转.
绕坐标轴的多次旋转可以等效为绕某一转轴旋转一定的角度。假设等效旋转轴方向向量为
K
⃗
=
[
k
x
,
k
y
,
k
z
]
T
\vec{K}=[k_x,k_y,k_z]^T
K=[kx,ky,kz]T，等效旋转角为𝜃，则四元数
q
=
(
q
0
,
q
1
,
q
2
,
q
3
)
=
q
0
+
q
1
i
+
q
2
j
+
q
3
k
q=(q_0,q_1,q_2,q_3)=q_0+q_1 i+q_2 j+q_3 k
q=(q0,q1,q2,q3)=q0+q1i+q2j+q3k，其中：
q
1
=
k
x
⋅
s
i
n
θ
2
q
2
=
k
y
⋅
s
i
n
θ
2
q
3
=
k
z
⋅
s
i
n
θ
2
q
0
=
c
o
s
θ
2
q_1 = k_x \cdot sin \frac{\theta}{2}\\ q_2 = k_y \cdot sin \frac{\theta}{2}\\ q_3 = k_z \cdot sin \frac{\theta}{2}\\ q_0 = cos \frac{\theta}{2}
q1=kx⋅sin2θq2=ky⋅sin2θq3=kz⋅sin2θq0=cos2θ
且有 q 0 2 + q 1 2 + q 2 2 + q 3 2 = 1 q_0^2+q_1^2+q_2^2+q_3^2=1 q02+q12+q22+q32=1
相当于一个标量和三个向量, 所以也可以表示为 q = [ s , v ] , s = q 0 , v = [ q 1 , q 2 , q 3 ] q=[s,v], s=q_0, v=[q_1,q_2,q_3] q=[s,v],s=q0,v=[q1,q2,q3]
根据上面的对应关系我们可以求出旋转轴和旋转角
计算的时候因为向量和点都是三维的所以我们使用 p = [ 0 , x , y , z ] p=[0,x,y,z] p=[0,x,y,z]
则有
p
′
=
q
p
q
−
1
p' = qpq^{-1}
p′=qpq−1
3. 李群李代数
3.1 李群
定义:
S O ( 3 ) = { R ∈ R 3 × 3 ∣ R R T = I , d e t ( R ) = 1 } SO(3) = \left\{R\in R^{3×3}|RR^T = I, det(R)=1\right\} SO(3)={R∈R3×3∣RRT=I,det(R)=1}
S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R R T = I , d e t ( R ) = 1 } SE(3) = \left\{T = \left[\begin{matrix} R & t\\ 0^T & 1\\ \end{matrix}\right]\in R^{4×4}|RR^T = I, det(R)=1\right\} SE(3)={T=[R0Tt1]∈R4×4∣RRT=I,det(R)=1}
因为 R R R是有约束的,所以他们对加法是不封闭的,但是对于乘法是封闭的,像这种的叫做群
像 S O ( 3 ) SO(3) SO(3)和 S E ( 3 ) SE(3) SE(3)这样的称之为李群
2639
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
