（三）中对SINS的机械编排进行了初步可行性的介绍，并未对机械编排进行原理性介绍。那么在详细介绍机械编排之前，需要先对SINS的微分方程进行详细的推导。
无论是机械编排，还是后面误差方程的建立，SINS的微分方程都是其重要的基础内容。那么本文在基于严恭敏《惯性导航》的基础上，对SINS中常见的微分方程进行推导。
（二）中对INS常用的坐标系统及转换进行了介绍，同时介绍了姿态的三种表示方法，以及三种方法之间的相互转换。
下面对姿态微分方程的推导是也是基于这三种方法进行推导。
欧拉角微分方程
欧拉角的旋转方式有很多种，常用的是ZYX旋转。这里的旋转指的是欧拉角的定义方式上的旋转。如下图所示，即为ZYX旋转。
上面三次旋转都对应一个坐标转换矩阵，这里分别为
R
ψ
R_\psi
Rψ、
R
θ
R_\theta
Rθ、
R
ϕ
R_\phi
Rϕ。其具体形式不在这里给出，感兴趣的可以自行搜索。
欧拉角微分方程的推导就对应着上面三次旋转。
1）第一次旋转
绕原始坐标系Z轴旋转产生
ψ
\psi
ψ角，产生的旋转角速度在旋转后的临时坐标系1中的表示是绕Z轴方向，因此在1系中的表示为：
ω
^
1
=
[
0
0
ψ
˙
]
\hat \omega_1=\begin{bmatrix} 0 \\ 0 \\ \dot \psi\end{bmatrix}
ω^1=⎣⎡00ψ˙⎦⎤
2）第二次旋转
绕中间坐标系1系Y轴旋转
θ
\theta
θ角，产生的旋转角速度在旋转后的临时坐标系2中的表示也是绕Y轴方向， 因此在2系中很好表示，但是旋转也改变了第一次旋转角速度的表示。因此前两次旋转在2系中的表示为：
ω
^
2
=
[
0
θ
˙
0
]
+
R
θ
ω
^
1
=
[
−
ψ
˙
s
i
n
θ
θ
˙
ψ
˙
c
o
s
θ
]
\hat \omega_2=\begin{bmatrix} 0 \\ \dot \theta \\ 0\end{bmatrix}+R_\theta \hat \omega_1=\begin{bmatrix} -\dot \psi sin \theta \\ \dot \theta \\ \dot \psi cos \theta\end{bmatrix}
ω^2=⎣⎡0θ˙0⎦⎤+Rθω^1=⎣⎡−ψ˙sinθθ˙ψ˙cosθ⎦⎤
3）第三次旋转
绕中间坐标系2系的X轴旋转
ϕ
\phi
ϕ角，产生的旋转角速度在旋转后的最终坐标系中的表示也是绕X轴方向，但是同样改变了前两次旋转角速度的表示，三次旋转在最终坐标系中的表示为：
ω
^
b
=
[
ϕ
˙
0
0
]
+
R
ϕ
ω
^
2
=
[
1
0
−
s
i
n
θ
0
c
o
s
ϕ
s
i
n
ϕ
c
o
s
θ
0
−
s
i
n
ϕ
c
o
s
ϕ
c
o
s
θ
]
[
ϕ
˙
θ
˙
ψ
˙
]
\hat \omega_b=\begin{bmatrix} \dot \phi \\ 0 \\ 0\end{bmatrix}+R_\phi \hat \omega_2=\begin{bmatrix} 1&0&-sin\theta \\ 0&cos\phi&sin\phi cos\theta \\ 0 & -sin\phi & cos\phi cos\theta \end{bmatrix}\begin{bmatrix} \dot \phi \\ \dot \theta \\ \dot \psi\end{bmatrix}
ω^b=⎣⎡ϕ˙00⎦⎤+Rϕω^2=⎣⎡1000cosϕ−sinϕ−sinθsinϕcosθcosϕcosθ⎦⎤⎣⎡ϕ˙θ˙ψ˙⎦⎤
即
ω
^
b
=
[
1
0
−
s
i
n
θ
0
c
o
s
ϕ
s
i
n
ϕ
c
o
s
θ
0
−
s
i
n
ϕ
c
o
s
ϕ
c
o
s
θ
]
[
ϕ
˙
θ
˙
ψ
˙
]
\hat \omega_b=\begin{bmatrix} 1&0&-sin\theta \\ 0&cos\phi&sin\phi cos\theta \\ 0 & -sin\phi & cos\phi cos\theta \end{bmatrix}\begin{bmatrix} \dot \phi \\ \dot \theta \\ \dot \psi\end{bmatrix}
ω^b=⎣⎡1000cosϕ−sinϕ−sinθsinϕcosθcosϕcosθ⎦⎤⎣⎡ϕ˙θ˙ψ˙⎦⎤
其中
ω
^
b
=
[
ω
n
b
,
x
b
ω
n
b
,
y
b
ω
n
b
,
z
b
]
\hat \omega_b =\begin{bmatrix}\omega^b_{nb,x} \\ \omega^b_{nb,y} \\ \omega^b_{nb,z} \end{bmatrix}
ω^b=⎣⎡ωnb,xbωnb,ybωnb,zb⎦⎤
对矩阵求逆即可得到欧拉角微分方程。欧拉角微分方程在惯导和组合导航中用的不多。
方向余弦矩阵微分方程
欧拉角和DCM之间是可以相互转换的。上述欧拉角三次旋转是对应的旋转矩阵相乘就是DCM。即
R
n
b
=
R
ψ
⋅
R
θ
⋅
R
ϕ
=
[
c
o
s
θ
c
o
s
ψ
−
c
o
s
ϕ
s
i
n
ψ
+
s
i
n
ϕ
s
i
n
θ
c
o
s
ψ
s
i
n
ϕ
s
i
n
ψ
+
c
o
s
ϕ
s
i
n
θ
c
o
s
ψ
c
o
s
θ
s
i
n
ψ
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
θ
s
i
n
ϕ
c
o
s
θ
c
o
s
θ
c
o
s
ϕ
]
R^b_n=R_\psi \sdot R_\theta \sdot R_\phi=\begin{bmatrix} cos\theta cos\psi & -cos\phi sin\psi+sin\phi sin\theta cos\psi & sin\phi sin\psi+cos\phi sin\theta cos\psi \\ cos\theta sin\psi & cos\psi cos\phi + sin\psi sin\theta sin\phi & -cos\psi sin\phi + sin\psi sin\theta cos\phi \\ -sin\theta & sin\phi cos\theta & cos\theta cos\phi \end{bmatrix}
Rnb=Rψ⋅Rθ⋅Rϕ=⎣⎡cosθcosψcosθsinψ−sinθ−cosϕsinψ+sinϕsinθcosψcosψcosϕ+sinψsinθsinϕsinϕcosθsinϕsinψ+cosϕsinθcosψ−cosψsinϕ+sinψsinθcosϕcosθcosϕ⎦⎤
当三个欧拉角为小角度的时候，由极限知识可得：
R
y
x
=
[
1
−
ψ
x
y
θ
x
y
ψ
x
y
1
−
ϕ
x
y
−
θ
x
y
ϕ
x
y
1
]
=
I
+
(
v
×
)
,
v
=
[
ϕ
x
y
θ
x
y
ψ
x
y
]
R^x_y=\begin{bmatrix} 1 & -\psi_{xy} & \theta_{xy} \\ \psi_{xy} & 1 & -\phi_{xy} \\ -\theta_{xy} & \phi_{xy} & 1\end{bmatrix}= I +(v \times), v=\begin{bmatrix} \phi_{xy} \\ \theta_{xy} \\ \psi_{xy} \end{bmatrix}
Ryx=⎣⎡1ψxy−θxy−ψxy1ϕxyθxy−ϕxy1⎦⎤=I+(v×),v=⎣⎡ϕxyθxyψxy⎦⎤
另外，需要注意的是转置存在如下关系：
C
b
(
t
k
)
b
(
t
j
)
=
I
−
(
Δ
θ
b
(
t
k
)
b
(
t
j
)
×
)
,
C
b
(
t
j
)
b
(
t
k
)
=
I
+
(
Δ
θ
b
(
t
k
)
b
(
t
j
)
×
)
C^{b(t_j)}_{b(t_k)}=I-(\Delta \theta_{b(t_k)b(t_j)} \times), C^{b(t_k)}_{b(t_j)}=I+(\Delta \theta_{b(t_k)b(t_j)}\times)
Cb(tk)b(tj)=I−(Δθb(tk)b(tj)×),Cb(tj)b(tk)=I+(Δθb(tk)b(tj)×)
（1）设r为空间向量，由哥氏定理：
d
r
d
t
∣
n
=
d
r
d
t
∣
b
+
ω
n
b
×
r
\frac{dr}{dt}|_n=\frac{dr}{dt}|_b+\omega_{nb}\times r
dtdr∣n=dtdr∣b+ωnb×r
（2）上式向b系投影：
d
r
d
t
∣
n
b
=
d
r
d
t
∣
b
b
+
(
ω
n
b
×
r
)
b
\frac{dr}{dt}|^b_n=\frac{dr}{dt}|^b_b+(\omega_{nb}\times r)^b
dtdr∣nb=dtdr∣bb+(ωnb×r)b
（3）由于：
d
r
d
t
∣
b
b
=
r
˙
b
\frac{dr}{dt}|^b_b=\dot r^b
dtdr∣bb=r˙b
(
ω
n
b
×
r
)
b
=
ω
n
b
b
×
r
b
(\omega_{nb} \times r)^b=\omega^b_{nb}\times r^b
(ωnb×r)b=ωnbb×rb
（4）代入到（2）式得：
r
˙
b
=
d
r
d
t
∣
n
b
−
ω
n
b
b
×
r
b
\dot r^b=\frac{dr}{dt}|^b_n-\omega^b_{nb}\times r^b
r˙b=dtdr∣nb−ωnbb×rb
（5）由于
r
b
=
C
n
b
r
n
r^b=C^b_nr^n
rb=Cnbrn，对其两边求导：
r
˙
b
=
C
˙
n
b
r
n
+
c
n
b
r
˙
n
=
C
˙
n
b
r
n
+
C
n
b
d
r
d
t
∣
n
n
\dot r^b=\dot C^b_n r^n+c^b_n \dot r^n=\dot C^b_n r^n+C^b_n\frac{dr}{dt}|^n_n
r˙b=C˙nbrn+cnbr˙n=C˙nbrn+Cnbdtdr∣nn
（6）上式中
C
n
b
d
r
d
t
∣
n
n
C^b_n\frac{dr}{dt}|^n_n
Cnbdtdr∣nn即位
d
r
d
t
∣
n
b
\frac{dr}{dt}|^b_n
dtdr∣nb，所以上式变为：
r
˙
b
=
C
˙
n
b
r
n
+
d
r
d
t
∣
n
b
\dot r^b=\dot C^b_n r^n+\frac{dr}{dt}|^b_n
r˙b=C˙nbrn+dtdr∣nb
（7）式与（4）式对比可得：
C
˙
n
b
=
−
ω
n
b
b
×
r
b
=
−
ω
n
b
b
×
C
n
b
r
n
=
−
ω
n
b
b
k
C
n
b
r
n
\dot C^b_n=-\omega^b_{nb}\times r^b=-\omega^b_{nb}\times C^b_n r^n=-\omega^{bk}_{nb}C^b_nr^n
C˙nb=−ωnbb×rb=−ωnbb×Cnbrn=−ωnbbkCnbrn
（8）最终得：
C
˙
n
b
=
−
ω
n
b
b
k
C
n
b
\dot C^b_n=-\omega^{bk}_{nb}C^b_n
C˙nb=−ωnbbkCnb
转置得：
C
˙
b
n
=
C
b
n
ω
n
b
b
k
\dot C^n_b=C^n_b\omega^{bk}_{nb}
C˙bn=Cbnωnbbk
其中，
ω
n
b
b
k
\omega^{bk}_{nb}
ωnbbk为旋转欧拉角向量的反对称矩阵。
四元素微分方程
（1）定义b系到n系的四元素为Q，则用欧拉角表示为
Q
=
c
o
s
θ
2
+
μ
R
s
i
n
θ
2
Q=cos\frac{\theta}{2}+\mu^Rsin\frac{\theta}{2}
Q=cos2θ+μRsin2θ
（2）对上式两边求导：
d
Q
d
t
=
−
θ
˙
2
s
i
n
θ
2
+
μ
R
θ
˙
2
c
o
s
θ
2
+
s
i
n
θ
2
d
μ
R
d
t
\frac{dQ}{dt}=-\frac{\dot \theta}{2}sin \frac{\theta}{2}+\mu^R\frac{\dot \theta}{2}cos\frac{\theta}{2}+sin\frac{\theta}{2}\frac{d\mu^R}{dt}
dtdQ=−2θ˙sin2θ+μR2θ˙cos2θ+sin2θdtdμR
（3）由哥氏定理：
d
μ
R
d
t
=
C
b
R
d
μ
b
d
t
+
ω
R
b
R
×
μ
R
\frac{d\mu^R}{dt}=C^R_b\frac{d\mu^b}{dt}+\omega^R_{Rb}\times \mu^R
dtdμR=CbRdtdμb+ωRbR×μR
（4）其中：
d
μ
b
d
t
=
0
\frac{d\mu^b}{dt}=0
dtdμb=0，且
ω
R
b
R
=
θ
˙
μ
R
\omega^R_{Rb}=\dot \theta \mu^R
ωRbR=θ˙μR,故
d
μ
R
d
t
=
0
\frac{d\mu^R}{dt}=0
dtdμR=0
（5）则（2）式变为：
d
Q
d
t
=
−
θ
˙
2
s
i
n
θ
2
+
μ
R
θ
˙
2
c
o
s
θ
2
\frac{dQ}{dt}=-\frac{\dot \theta}{2}sin \frac{\theta}{2}+\mu^R\frac{\dot \theta}{2}cos\frac{\theta}{2}
dtdQ=−2θ˙sin2θ+μR2θ˙cos2θ
（6）对（1）式两边乘
θ
˙
2
μ
R
\frac{\dot \theta}{2}\mu^R
2θ˙μR得：
θ
˙
2
μ
R
⊗
Q
=
θ
˙
2
μ
R
⊗
(
c
o
s
θ
2
+
μ
R
s
i
n
θ
2
)
=
θ
˙
2
μ
R
c
o
s
θ
2
+
μ
R
⊗
μ
R
s
i
n
θ
2
=
θ
˙
2
μ
R
c
o
s
θ
2
−
s
i
n
θ
2
\frac{\dot\theta}{2}\mu^R\otimes Q=\frac{\dot\theta}{2}\mu^R\otimes( cos\frac{\theta}{2}+\mu^Rsin\frac{\theta}{2})=\frac{\dot\theta}{2}\mu^R cos\frac{\theta}{2}+\mu^R \otimes \mu^R sin\frac{\theta}{2}=\frac{\dot \theta}{2} \mu^Rcos\frac{\theta}{2}-sin\frac{\theta}{2}
2θ˙μR⊗Q=2θ˙μR⊗(cos2θ+μRsin2θ)=2θ˙μRcos2θ+μR⊗μRsin2θ=2θ˙μRcos2θ−sin2θ
（7） 则（5）与（6）相比即有：
d
Q
d
t
=
θ
˙
2
μ
R
⊗
Q
=
1
2
ω
R
b
R
⊗
Q
=
1
2
Q
⊗
ω
R
b
b
=
1
2
W
Q
\frac{dQ}{dt}=\frac{\dot \theta}{2} \mu^R \otimes Q=\frac{1}{2}\omega^R_{Rb}\otimes Q=\frac{1}{2}Q\otimes \omega^b_{Rb}=\frac{1}{2}WQ
dtdQ=2θ˙μR⊗Q=21ωRbR⊗Q=21Q⊗ωRbb=21WQ
上式即为四元素微分方程。其中w为欧拉角向量的反对称矩阵。
速度微分方程
（1）由哥氏方程和地速定义（i 系）：
d
r
d
t
∣
i
=
d
r
d
t
∣
e
+
ω
i
e
×
r
=
v
e
+
ω
i
e
×
r
\frac{dr}{dt}|_i=\frac{dr}{dt}|_e+\omega_{ie}\times r=v_e+\omega_{ie}\times r
dtdr∣i=dtdr∣e+ωie×r=ve+ωie×r
（2）在i系下求导：
d
2
r
d
t
2
=
d
v
e
d
t
∣
i
+
d
d
t
(
ω
i
e
×
r
)
∣
i
\frac{d^2 r}{dt^2}=\frac{dv_e}{dt}|_i+\frac{d}{dt}(\omega_{ie}\times r)|_i
dt2d2r=dtdve∣i+dtd(ωie×r)∣i
（3）利用导航方程整理可得：
d
v
e
d
t
∣
i
=
f
+
g
−
d
ω
i
e
d
t
∣
i
×
r
−
ω
i
e
×
d
r
d
t
∣
i
=
f
+
g
−
ω
i
e
×
(
v
e
+
ω
i
e
×
r
)
=
f
−
ω
i
e
×
v
e
+
(
g
−
ω
i
e
×
(
ω
i
e
×
r
)
)
=
f
−
ω
i
e
×
v
e
+
g
l
\frac{dv_e}{dt}|_i=f+g-\frac{d\omega_{ie}}{dt}|_i\times r-\omega_{ie}\times \frac{dr}{dt}|_i=f+g-\omega_{ie}\times(v_e+\omega_{ie}\times r)=f-\omega_{ie}\times v_e+(g-\omega_{ie}\times(\omega_{ie}\times r))=f-\omega_{ie}\times v_e+g_l
dtdve∣i=f+g−dtdωie∣i×r−ωie×dtdr∣i=f+g−ωie×(ve+ωie×r)=f−ωie×ve+(g−ωie×(ωie×r))=f−ωie×ve+gl
投影到i系：
=
C
b
i
f
b
−
ω
i
e
i
×
v
e
i
+
g
l
i
=C^i_bf^b-\omega^i_{ie}\times v^i_e + g^i_l
=Cbifb−ωiei×vei+gli
上式即为速度微分方程。
位置微分方程
在INS中位置是由速度确定，因此其微分方方程也较为简单，在各种书籍论文中常用的位置微分方程如下所示：
r
˙
n
=
[
ϕ
˙
λ
˙
h
˙
]
=
[
1
R
M
+
h
0
0
0
1
(
R
N
+
h
)
c
o
s
ϕ
0
0
0
−
1
]
[
v
N
v
E
v
D
]
=
D
−
1
v
n
\dot r^n=\begin{bmatrix} \dot \phi \\ \dot \lambda \\ \dot h\end{bmatrix}=\begin{bmatrix} \frac{1}{R_M+h} & 0 & 0 \\ 0 & \frac{1}{(R_N+h)cos\phi}&0 \\ 0 & 0 & -1\end{bmatrix}\begin{bmatrix}v_N \\ v_E \\ v_D\end{bmatrix}=D^{-1}v^n
r˙n=⎣⎡ϕ˙λ˙h˙⎦⎤=⎣⎡RM+h1000(RN+h)cosϕ1000−1⎦⎤⎣⎡vNvEvD⎦⎤=D−1vn
