姿态解算
)
方向余弦矩阵
基本概念
方向余弦:
在坐标系R中设v为一个空间向量,在R坐标系下的投影为(vx,vy,vz)
与其x y z 轴分别成
α
β
γ
\alpha \beta \gamma
αβγ角度,则
c
o
s
α
,
c
o
s
β
,
c
o
s
γ
cos\alpha ,cos\beta,cos\gamma
cosα,cosβ,cosγ分别为v在三轴的方向余弦,大小分别等于|vx|,|vy|,|vz|。
方向余弦矩阵(direction cosine matrix)
在一个平面内对向量进行旋转相当于对坐标进行旋转,初始状态下令载体坐标系和参考坐标系完全重合。
方向余弦矩阵便是存放着一系列方向余弦,设r(x,y,z)为参考坐标系,O(X1,Y1,Z1)为载体坐标系,载体转动时,载体坐标系会相对于参考坐标系转动,把载体坐标系的三个轴当做三个单位向量(vx,vy,vz),每个载体坐标轴都可以在参考坐标系上找到三个对应的方向余弦。最终得到九个方向余弦,把九个方向余弦写成矩阵形式就是方向余弦矩阵。
而方向余弦矩阵的写法有两种,一种是载体坐标系相对于参考坐标系,一种是参考坐标系相对于载体坐标系。
C
0
r
=
[
c
11
c
12
c
13
c
21
c
22
c
23
c
31
c
32
c
33
]
或
C
r
0
=
[
c
11
c
21
c
31
c
12
c
22
c
32
c
13
c
23
c
33
]
C_0^r=\begin{bmatrix}\\ c_{11} c_{12}c_{13} \\ c_{21} c_{22} c_{23} \\ c_{31}c_{32}c_{33} \end{bmatrix}或C_r^0=\begin{bmatrix}\\ c_{11} c_{21}c_{31} \\ c_{12} c_{22} c_{32} \\ c_{13}c_{23}c_{33} \end{bmatrix}
C0r=⎣⎡c11c12c13c21c22c23c31c32c33⎦⎤或Cr0=⎣⎡c11c21c31c12c22c32c13c23c33⎦⎤
C_0^r表称O系对于r系的方向余弦矩阵,这两个矩阵互为转置矩阵。
C
12
=
c
o
s
(
x
r
.
x
0
)
C_{12}=cos(x_r.x_0)
C12=cos(xr.x0)载体坐标系上的$x_0相对于参考坐标系上的x_r的方向余弦,
C
12
=
c
o
s
(
x
r
,
y
0
)
同
理
C_{12}=cos(x_r,y_0)同理
C12=cos(xr,y0)同理
y
0
相
对
于
参
考
坐
标
系
上
x
r
的
方
向
余
弦
y_0相对于参考坐标系上x_r的方向余弦
y0相对于参考坐标系上xr的方向余弦
已知欧拉角求方向余弦
ψ
θ
γ
\psi \theta \gamma
ψθγ
[
c
o
s
γ
c
o
s
ψ
+
s
i
n
γ
s
i
n
ψ
s
i
n
θ
−
c
o
s
γ
s
i
n
ψ
s
i
n
θ
−
s
i
n
γ
c
o
s
θ
s
i
n
ψ
c
o
s
θ
c
o
s
ψ
c
o
s
θ
s
i
n
θ
s
i
n
γ
c
o
s
ψ
−
c
o
s
γ
s
i
n
ψ
s
i
n
θ
−
s
i
n
γ
s
i
n
ψ
−
c
o
s
γ
c
o
s
ψ
s
i
n
θ
c
o
s
γ
c
o
s
θ
]
\begin{bmatrix} cos \gamma cos\psi +sin\gamma sin\psi sin \theta& -cos\gamma sin\psi sin\theta & -sin\gamma cos\theta \\ sin\psi cos\theta&cos\psi cos\theta &sin\theta \\sin\gamma cos\psi -cos\gamma sin\psi sin\theta &-sin\gamma sin\psi -cos\gamma cos\psi sin\theta&cos\gamma cos\theta \end{bmatrix}
⎣⎡cosγcosψ+sinγsinψsinθsinψcosθsinγcosψ−cosγsinψsinθ−cosγsinψsinθcosψcosθ−sinγsinψ−cosγcosψsinθ−sinγcosθsinθcosγcosθ⎦⎤
已知方向余弦求欧拉角
方向余弦矩阵:
[
T
11
T
21
T
31
T
12
T
22
T
32
T
13
T
23
T
33
]
\begin{bmatrix} T_{11}&T_{21}&T_{31}\\T_{12}&T_{22}&T_{32} \\T_{13}&T_{23}&T_{33}\end{bmatrix}
⎣⎡T11T12T13T21T22T23T31T32T33⎦⎤
推出:
横滚角:
θ
=
a
r
c
s
i
n
T
32
\theta=arcsinT_{32}
θ=arcsinT32
俯仰角:
γ
=
a
r
c
t
a
n
(
−
T
32
T
33
)
\gamma=arctan(-\frac{T_{32}}{T_{33}})
γ=arctan(−T33T32)
偏航角:
ψ
=
a
r
c
t
a
n
(
T
12
T
22
)
\psi =arctan(\frac {T_{12}}{T_{22}})
ψ=arctan(T22T12)
四元数
基本概念
四元数是由实数和虚数组成的一组超复数,假设Q是一个四元数。
Q = w + xi + yj + zj
其中w表示四元数的实数大小,x,y,z表示虚数大小,实部单位为1,虚部单为为i,j,k。
四元数一般表示:
q
⃗
=
[
w
v
⃗
]
=
[
w
(
x
y
z
)
]
\vec{q}=[w \vec{v}]=[w (xyz)]
q=[wv]=[w(xyz)]
四元数表征姿态
其中w为实数部分,
v
⃗
\vec{v}
v为矢量部分,首先讨论如何把一个空间向量转换到四元数。
假设在三维空间中有一个点(x,y,z),拓展到四元数空间有
p
⃗
=
[
0
(
x
y
z
)
]
\vec{p}=[0(x y z)]
p=[0(xyz)]
p
⃗
\vec{p}
p就是三维空间中一点四元数的表示,而旋转是通过旋转四元数来实现的。现在假设空间中有一个向量
n
⃗
\vec{n}
n,
(
n
x
,
n
y
,
n
z
)
(n_x,n_y,n_z)
(nx,ny,nz),若用四元数来表示点p(x,y,z)绕向量
n
⃗
\vec{n}
n旋转
θ
\theta
θ角度后
p
,
p^,
p,
定义旋转四元数为
p
⃗
\vec{p}
p则对于本次旋转有:
p
⃗
=
[
c
o
s
\vec{p}=[cos
p=[cos
(
θ
2
)
(\frac{\theta}{2})
(2θ) sin
(
θ
2
)
(\frac{\theta}{2})
(2θ)
n
⃗
\vec{n}
n]=[cos
(
θ
2
)
(\frac{\theta}{2})
(2θ)
.
.
. sin
(
θ
2
)
(\frac{\theta}{2})
(2θ)
n
x
⃗
\vec{n_x}
nx
.
.
. sin
(
θ
2
)
(\frac{\theta}{2})
(2θ)
n
y
⃗
\vec{n_y}
ny
.
.
. sin
(
θ
2
)
(\frac{\theta}{2})
(2θ)
n
z
⃗
\vec{n_z}
nz ]
(其中.为分隔用)
其中
n x ⃗ \vec{n_x} nx | n y ⃗ \vec{n_y} ny | n z ⃗ \vec{n_z} nz |
---|---|---|
n x ⃗ \vec{n_x} nx= n x n_x nx*i | n y ⃗ \vec{n_y} ny= n y n_y ny*j | n z ⃗ \vec{n_z} nz= n z n_z nz*k |
若想得到旋转后的矩阵只需要执行以下式子
p , ⃗ = q ⃗ p ⃗ q ⃗ − 1 \vec{p^,}=\vec{q}\vec{p} \vec{q}^-1 p,=qpq−1
( q ⃗ − 1 为 q ⃗ 的 逆 矩 阵 ) (\vec{q}^-1为\vec{q}的逆矩阵) (q−1为q的逆矩阵)
想 要 推 导 下 面 的 公 式 需 要 了 解 两 个 概 念 想要推导下面的公式需要了解两个概念 想要推导下面的公式需要了解两个概念
<1四元数的共轭
四元数的共轭就是让四元数的向量部分取负,记作
p
⃗
\vec{p}
p=[w
v
⃗
\vec{v}
v]=[w -
v
⃗
\vec{v}
v]=[w (-x -y -z)]
四元数与他的共轭代表反向的角位移,因为相当于旋转轴反向
<2四元数的逆
四元数的逆就是他的共轭除以他的模
q
−
1
⃗
=
(
q
⃗
∗
)
(
∣
q
⃗
∣
)
\vec{q^-1} = \frac{(\vec{q}*)}{(|\vec{q}|)}
q−1=(∣q∣)(q∗)
一般使用单位四元数,所以他的逆和共轭是相等的
因为公式中涉及到四元数的乘法,在表示旋转时用叉乘,计算公式如下
[
w
1
(
x
1
y
1
z
1
)
]
[
w
2
(
x
2
y
2
z
2
)
]
=
[
w
1
w
2
−
x
1
x
2
−
y
1
y
2
−
z
1
z
2
(
w
1
x
2
+
x
1
w
2
+
z
1
y
2
−
y
1
z
2
w
1
y
2
+
y
1
w
2
+
x
1
z
2
−
z
1
x
2
w
1
z
2
+
z
1
w
2
+
y
1
x
2
−
x
1
y
2
)
]
[w_1 (x_1 y_1 z_1)][w_2 ( x_2 y_2 z_2)]=\begin{bmatrix} w_1w_2 -x_1x_2 -y_1y_2-z_1z_2\\ \begin{pmatrix}w_1x_2 + x_1w_2 +z_1y_2-y_1z_2 \\w_1y_2+y_1w_2+x_1z_2-z_1x_2 \\w_1z_2+z_1w_2+y_1x_2-x_1y_2 \end{pmatrix} \end{bmatrix}
[w1(x1y1z1)][w2(x2y2z2)]=⎣⎢⎢⎡w1w2−x1x2−y1y2−z1z2⎝⎛w1x2+x1w2+z1y2−y1z2w1y2+y1w2+x1z2−z1x2w1z2+z1w2+y1x2−x1y2⎠⎞⎦⎥⎥⎤
example:
假设空间中有一个点p(0,1,0)饶z轴逆时针旋转90度,求旋转后的
p
,
p^,
p,点。理论上
p
,
p^,
p,点为(-1,0,0)。
首先把p(0,1,0)拓展成一个四元数
p
⃗
\vec{p}
p=(0,0,j,0),然后定义旋转四元数,旋转轴为z轴化为单位向量(0,0,1),因为旋转角度为90度所以------
q
⃗
=
(
c
o
s
4
5
。
,
0
,
0
,
1
∗
s
i
n
4
5
。
k
)
=
(
2
2
,
0
,
0
,
2
2
k
)
\vec{q}=(cos45^。,0,0,1*sin45^。k)=(\frac{\sqrt 2}{2},0,0,\frac{\sqrt 2}{2}k)
q=(cos45。,0,0,1∗sin45。k)=(22,0,0,22k)
单位四元数的逆的共轭是相等的
q ⃗ − 1 = ( 2 2 , 0 , 0 , − 2 2 k ) \vec{q}^-1=(\frac{\sqrt 2}{2},0,0,-\frac{\sqrt 2}{2}k) q−1=(22,0,0,−22k)
由四元数叉乘计算公式
p
,
⃗
=
q
⃗
p
⃗
q
⃗
−
1
\vec{p^,}=\vec{q}\vec{p} \vec{q}^-1
p,=qpq−1 用上述乘法公式计算
q
⃗
p
⃗
=
(
0
,
2
2
k
j
,
2
2
j
,
0
)
\vec{q}\vec{p}=(0,\frac{\sqrt 2}{2}kj,\frac{\sqrt 2}{2}j,0)
qp=(0,22kj,22j,0)
k*j=-i再乘以
q
⃗
−
1
得
到
p
⃗
,
=
[
0
,
−
i
,
0
,
0
]
\vec{q}^{-1}得到\vec{p}^{,}=[0,-i,0,0]
q−1得到p,=[0,−i,0,0],带回去得到点[-1,0,0]即为旋转之后的坐标。
四元数与方向余弦
由前面的介绍知道了旋转四元数
q
=
c
o
s
θ
2
+
x
∗
s
i
n
θ
2
∗
i
+
y
∗
s
i
n
θ
2
∗
j
+
z
∗
s
i
n
θ
2
∗
k
q=cos\frac {\theta}{2}+x*sin\frac{\theta}{2}*i+y*sin\frac {\theta}{2}*j+z*sin\frac{\theta}{2}*k
q=cos2θ+x∗sin2θ∗i+y∗sin2θ∗j+z∗sin2θ∗k
这里令
q
0
=
c
o
s
θ
2
,
q
1
=
s
i
n
θ
2
,
q
2
=
s
i
n
θ
2
,
q
3
=
s
i
n
θ
2
,
q_0=cos\frac{\theta}{2},q_1=sin\frac{\theta} {2},q_2=sin\frac{\theta}{2},q_3=sin\frac{\theta}{2},
q0=cos2θ,q1=sin2θ,q2=sin2θ,q3=sin2θ,
q=
q
0
+
q
1
i
+
q
2
j
+
q
3
k
q_0+q_1i+q_2j+q_3k
q0+q1i+q2j+q3k四元数旋转的方向余弦矩阵公式如下:
[
x
b
y
b
z
b
]
=
[
q
0
2
+
q
1
2
−
q
2
2
−
q
3
2
2
(
q
1
q
2
+
q
0
q
3
)
2
(
q
1
q
3
−
q
0
q
2
)
2
(
q
1
q
2
−
q
0
q
3
)
q
0
2
−
q
1
2
+
q
2
2
−
q
3
2
2
(
q
0
q
1
+
q
2
q
3
)
2
(
q
1
q
2
−
q
0
q
3
)
2
(
q
2
q
3
−
q
0
q
1
)
q
0
2
−
q
1
2
−
q
2
2
+
q
3
2
]
⋅
[
x
n
y
n
z
n
]
\begin{bmatrix} x_b\\y_b\\z_b\end{bmatrix}=\begin{bmatrix}q_0^2+q_1^2-q_2^2-q_3^2& 2(q_1q_2+q_0q_3)&2(q_1q_3-q_0q_2) \\2(q_1q_2-q_0q_3)&q_0^2-q_1^2+q_2^2-q_3^2&2(q_0q_1+q_2q_3) \\2(q_1q_2-q_0q_3)&2(q_2q_3-q_0q_1)&q_0^2-q_1^2-q_2^2+q_3^2 \\ \end{bmatrix}\cdot\begin{bmatrix}x_n \\y_n \\z_n\end{bmatrix}
⎣⎡xbybzb⎦⎤=⎣⎡q02+q12−q22−q322(q1q2−q0q3)2(q1q2−q0q3)2(q1q2+q0q3)q02−q12+q22−q322(q2q3−q0q1)2(q1q3−q0q2)2(q0q1+q2q3)q02−q12−q22+q32⎦⎤⋅⎣⎡xnynzn⎦⎤
四
元
数
表
示
的
旋
转
矩
阵
公
式
四元数表示的旋转矩阵公式
四元数表示的旋转矩阵公式
姿态表示的三种方法
欧拉角
直观的欧拉角 横滚角roll 俯仰角pitch 偏航角 yaw
方向余弦矩阵
[ c o s γ c o s ψ + s i n γ s i n ψ s i n θ − c o s γ s i n ψ s i n θ − s i n γ c o s θ s i n ψ c o s θ c o s ψ c o s θ s i n θ s i n γ c o s ψ − c o s γ s i n ψ s i n θ − s i n γ s i n ψ − c o s γ c o s ψ s i n θ c o s γ c o s θ ] \begin{bmatrix} cos \gamma cos\psi +sin\gamma sin\psi sin \theta& -cos\gamma sin\psi sin\theta & -sin\gamma cos\theta \\ sin\psi cos\theta&cos\psi cos\theta &sin\theta \\sin\gamma cos\psi -cos\gamma sin\psi sin\theta &-sin\gamma sin\psi -cos\gamma cos\psi sin\theta&cos\gamma cos\theta \end{bmatrix} ⎣⎡cosγcosψ+sinγsinψsinθsinψcosθsinγcosψ−cosγsinψsinθ−cosγsinψsinθcosψcosθ−sinγsinψ−cosγcosψsinθ−sinγcosθsinθcosγcosθ⎦⎤
四元数
[ x b y b z b ] = [ q 0 2 + q 1 2 − q 2 2 − q 3 2 2 ( q 1 q 2 + q 0 q 3 ) 2 ( q 1 q 3 − q 0 q 2 ) 2 ( q 1 q 2 − q 0 q 3 ) q 0 2 − q 1 2 + q 2 2 − q 3 2 2 ( q 0 q 1 + q 2 q 3 ) 2 ( q 1 q 2 − q 0 q 3 ) 2 ( q 2 q 3 − q 0 q 1 ) q 0 2 − q 1 2 − q 2 2 + q 3 2 ] ⋅ [ x n y n z n ] \begin{bmatrix} x_b\\y_b\\z_b\end{bmatrix}=\begin{bmatrix}q_0^2+q_1^2-q_2^2-q_3^2& 2(q_1q_2+q_0q_3)&2(q_1q_3-q_0q_2) \\2(q_1q_2-q_0q_3)&q_0^2-q_1^2+q_2^2-q_3^2&2(q_0q_1+q_2q_3) \\2(q_1q_2-q_0q_3)&2(q_2q_3-q_0q_1)&q_0^2-q_1^2-q_2^2+q_3^2 \\ \end{bmatrix}\cdot\begin{bmatrix}x_n \\y_n \\z_n\end{bmatrix} ⎣⎡xbybzb⎦⎤=⎣⎡q02+q12−q22−q322(q1q2−q0q3)2(q1q2−q0q3)2(q1q2+q0q3)q02−q12+q22−q322(q2q3−q0q1)2(q1q3−q0q2)2(q0q1+q2q3)q02−q12−q22+q32⎦⎤⋅⎣⎡xnynzn⎦⎤