本文中定义旋转正方向是指沿轴从正往负看，逆时针为正。
1.绕轴旋转公式
1.1 绕x轴旋转 ϕ \phi ϕ角度
(1)
R
x
(
ϕ
)
=
[
x
′
y
′
z
′
]
=
[
1
0
0
0
c
o
s
ϕ
s
i
n
ϕ
0
−
s
i
n
ϕ
c
o
s
ϕ
]
[
x
y
z
]
R_x(\phi)=\left[ \begin{matrix}x'\\ y'\\ z'\end{matrix} \right]= \left[ \begin{matrix} 1 & 0 & 0 \\ 0 & cos\phi & sin\phi \\ 0 & -sin\phi & cos\phi \end{matrix} \right] \left[ \begin{matrix} x\\ y\\ z \end{matrix} \right]\tag{1}
Rx(ϕ)=⎣⎡x′y′z′⎦⎤=⎣⎡1000cosϕ−sinϕ0sinϕcosϕ⎦⎤⎣⎡xyz⎦⎤(1)
其中，
[
x
,
y
,
z
]
[ \begin{matrix}x,y,z\end{matrix}]
[x,y,z]表示旋转
θ
\theta
θ之前坐标的坐标，而
[
x
′
,
y
′
,
z
′
]
[ \begin{matrix}x',y',z'\end{matrix}]
[x′,y′,z′]表示旋转之后的坐标，同理可以得到其他两轴的旋转公式。
1.2 绕y轴旋转 θ \theta θ
(2) R y ( θ ) = [ x ′ y ′ z ′ ] = [ c o s θ 0 − s i n θ 0 1 0 s i n θ 0 c o s θ ] [ x y z ] R_y(\theta)=\left[ \begin{matrix} x'\\ y'\\ z' \end{matrix} \right]= \left[ \begin{matrix} cos\theta& 0& -sin\theta \\ 0& 1& 0 \\ sin\theta& 0& cos\theta \end{matrix} \right] \left[ \begin{matrix} x\\ y\\ z \end{matrix} \right]\tag{2} Ry(θ)=⎣⎡x′y′z′⎦⎤=⎣⎡cosθ0sinθ010−sinθ0cosθ⎦⎤⎣⎡xyz⎦⎤(2)
1.3 绕z轴旋转 ψ \psi ψ
(3) R z ( ψ ) [ x ′ y ′ z ′ ] = [ c o s ψ s i n ψ 0 − s i n ψ c o s ψ 0 0 0 1 ] [ x y z ] R_z(\psi)\left[ \begin{matrix} x'\\ y'\\ z' \end{matrix} \right]= \left[ \begin{matrix} cos\psi& sin\psi& 0 \\ -sin\psi& cos\psi& 0 \\ 0& 0& 1 \end{matrix} \right] \left[ \begin{matrix} x\\ y\\ z \end{matrix} \right]\tag{3} Rz(ψ)⎣⎡x′y′z′⎦⎤=⎣⎡cosψ−sinψ0sinψcosψ0001⎦⎤⎣⎡xyz⎦⎤(3)
1.4 北东地坐标系转换到机体转换矩阵
R
N
E
D
2
b
=
R
x
(
ϕ
)
R
y
(
θ
)
R
z
(
ψ
)
=
R_{NED2b}=R_x(\phi)R_y(\theta)R_z(\psi)=
RNED2b=Rx(ϕ)Ry(θ)Rz(ψ)=
(4)
[
c
o
s
(
θ
)
c
o
s
(
ψ
)
c
o
s
(
θ
)
s
i
n
(
ψ
)
−
s
i
n
(
θ
)
c
o
s
(
ψ
)
s
i
n
(
θ
)
s
i
n
(
p
h
i
)
−
s
i
n
(
ψ
)
c
o
s
(
ϕ
)
s
i
n
(
ψ
)
s
i
n
(
θ
)
s
i
n
(
ϕ
)
+
c
o
s
(
ψ
)
c
o
s
(
ϕ
)
c
o
s
(
θ
)
s
i
n
(
ϕ
)
s
i
n
(
ψ
)
s
i
n
(
ϕ
)
+
c
o
s
(
p
s
i
)
s
i
n
(
θ
)
c
o
s
(
ϕ
)
s
i
n
(
ψ
)
s
i
n
(
θ
)
c
o
s
(
ϕ
)
−
c
o
s
(
ψ
)
s
i
n
(
ϕ
)
c
o
s
(
θ
)
c
o
s
(
ϕ
)
]
\left[ \begin{matrix} cos(\theta)cos(\psi) & cos(\theta)sin(\psi) & -sin(\theta)\\ cos(\psi)sin(\theta)sin(phi)-sin(\psi)cos(\phi) & sin(\psi)sin(\theta)sin(\phi)+cos(\psi)cos(\phi) & cos(\theta)sin(\phi)\\ sin(\psi)sin(\phi)+cos(psi)sin(\theta)cos(\phi) & sin(\psi)sin(\theta)cos(\phi)-cos(\psi)sin(\phi) & cos(\theta)cos(\phi) \end{matrix} \right]\tag{4}
⎣⎡cos(θ)cos(ψ)cos(ψ)sin(θ)sin(phi)−sin(ψ)cos(ϕ)sin(ψ)sin(ϕ)+cos(psi)sin(θ)cos(ϕ)cos(θ)sin(ψ)sin(ψ)sin(θ)sin(ϕ)+cos(ψ)cos(ϕ)sin(ψ)sin(θ)cos(ϕ)−cos(ψ)sin(ϕ)−sin(θ)cos(θ)sin(ϕ)cos(θ)cos(ϕ)⎦⎤(4)
(5)
[
X
b
Y
b
Z
b
]
=
R
N
E
D
2
b
[
X
N
E
D
Y
N
E
D
Z
N
E
D
]
\left[ \begin{matrix} X_{b}\\Y_{b}\\Z_{b} \end{matrix} \right]=R_{NED2b} \left[ \begin{matrix} X_{NED}\\Y_{NED}\\Z_{NED} \end{matrix} \right]\tag{5}
⎣⎡XbYbZb⎦⎤=RNED2b⎣⎡XNEDYNEDZNED⎦⎤(5)
2.角速度转换公式
根据北东地坐标系旋转到机体坐标系的顺序可知
(5)
[
p
q
r
]
=
[
1
0
0
]
ϕ
˙
+
R
x
(
ϕ
)
[
0
1
0
]
θ
˙
+
R
x
(
ϕ
)
R
y
(
θ
)
[
0
0
1
]
ψ
˙
=
[
1
0
−
s
i
n
θ
0
c
o
s
ϕ
c
o
s
θ
s
i
n
ϕ
0
−
s
i
n
ϕ
c
o
s
θ
c
o
s
ϕ
]
[
ϕ
˙
θ
˙
ψ
˙
]
\left[ \begin{matrix} p\\q\\r \end{matrix} \right]= \left[ \begin{matrix} 1\\0\\0 \end{matrix} \right] \dot{\phi}+ R_x(\phi)\left[\begin{matrix}0\\1\\0 \end{matrix} \right]\dot{\theta}+ R_x(\phi)R_y(\theta) \left[\begin{matrix}0\\0\\1 \end{matrix} \right]\dot{\psi} = \left[ \begin{matrix} 1 & 0 & -sin\theta \\ 0 & cos\phi & cos\theta sin\phi \\ 0 & -sin\phi & cos\theta cos\phi \end{matrix} \right] \left[ \begin{matrix} \dot{\phi}\\\dot{\theta}\\\dot{\psi} \end{matrix} \right]\tag{5}
⎣⎡pqr⎦⎤=⎣⎡100⎦⎤ϕ˙+Rx(ϕ)⎣⎡010⎦⎤θ˙+Rx(ϕ)Ry(θ)⎣⎡001⎦⎤ψ˙=⎣⎡1000cosϕ−sinϕ−sinθcosθsinϕcosθcosϕ⎦⎤⎣⎡ϕ˙θ˙ψ˙⎦⎤(5)
根据矩阵转置可求得
[
ϕ
˙
θ
˙
ψ
˙
]
=
[
1
t
a
n
θ
s
i
n
ϕ
t
a
n
θ
c
o
s
ϕ
0
c
o
s
ϕ
−
s
i
n
ϕ
0
−
s
i
n
ϕ
c
o
s
θ
c
o
s
ϕ
c
o
s
θ
]
[
p
q
r
]
\left[ \begin{matrix} \dot{\phi}\\\dot{\theta}\\\dot{\psi} \end{matrix} \right] = \left[ \begin{matrix} 1 & tan\theta sin\phi & tan\theta cos\phi \\ 0 & cos\phi & -sin\phi \\ 0 & \frac{-sin\phi}{cos\theta} & \frac{cos\phi}{cos\theta} \end{matrix} \right] \left[ \begin{matrix} p\\q\\r \end{matrix} \right]
⎣⎡ϕ˙θ˙ψ˙⎦⎤=⎣⎡100tanθsinϕcosϕcosθ−sinϕtanθcosϕ−sinϕcosθcosϕ⎦⎤⎣⎡pqr⎦⎤
262
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
