INS/GNSS组合导航（七）-SINS的微分方程的推导

（三）中对SINS的机械编排进行了初步可行性的介绍，并未对机械编排进行原理性介绍。那么在详细介绍机械编排之前，需要先对SINS的微分方程进行详细的推导。

无论是机械编排，还是后面误差方程的建立，SINS的微分方程都是其重要的基础内容。那么本文在基于严恭敏《惯性导航》的基础上，对SINS中常见的微分方程进行推导。

（二）中对INS常用的坐标系统及转换进行了介绍，同时介绍了姿态的三种表示方法，以及三种方法之间的相互转换。

1. 姿态的三种表示方法

欧拉角法
欧拉角法对应于载体坐标系的三个旋转角（roll， pitch， yaw）
方向余弦矩阵法
方向余弦矩阵（DCM）又被称为“坐标变换矩阵”，用于将矢量投影从一个坐标系变换到另一个坐标系中。
四元素法
四元数法是指有一个实数单位1和三个虚数单位i，j，k组成并具有如下形式实元的数。

下面对姿态微分方程的推导是也是基于这三种方法进行推导。

2. 姿态微分方程推导

欧拉角微分方程

欧拉角的旋转方式有很多种，常用的是ZYX旋转。这里的旋转指的是欧拉角的定义方式上的旋转。如下图所示，即为ZYX旋转。

上面三次旋转都对应一个坐标转换矩阵，这里分别为RψR_\psiRψ、RθR_\thetaRθ、RϕR_\phiRϕ。其具体形式不在这里给出，感兴趣的可以自行搜索。

欧拉角微分方程的推导就对应着上面三次旋转。
1）第一次旋转
绕原始坐标系Z轴旋转产生ψ\psiψ角，产生的旋转角速度在旋转后的临时坐标系1中的表示是绕Z轴方向，因此在1系中的表示为：ω^1=[00ψ˙]\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=[−ψ˙sinθθ˙ψ˙cosθ]\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=[ϕ˙00]+Rϕω^2=[10−sinθ0cosϕsinϕcosθ0−sinϕcosϕcosθ][ϕ˙θ˙ψ˙]\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=[10−sinθ0cosϕsinϕcosθ0−sinϕcosϕcosθ][ϕ˙θ˙ψ˙]\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=[ωnb,xbωnb,ybωnb,zb]\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。即Rnb=Rψ⋅Rθ⋅Rϕ=[cosθcosψ−cosϕsinψ+sinϕsinθcosψsinϕsinψ+cosϕsinθcosψcosθsinψcosψcosϕ+sinψsinθsinϕ−cosψsinϕ+sinψsinθcosϕ−sinθsinϕcosθcosθcosϕ]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ϕ⎦⎤
当三个欧拉角为小角度的时候，由极限知识可得：Ryx=[1−ψxyθxyψxy1−ϕxy−θxyϕxy1]=I+(v×),v=[ϕxyθxyψxy]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⎦⎤
另外，需要注意的是转置存在如下关系：
Cb(tk)b(tj)=I−(Δθb(tk)b(tj)×),Cb(tj)b(tk)=I+(Δθb(tk)b(tj)×)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为空间向量，由哥氏定理：
drdt∣n=drdt∣b+ωnb×r\frac{dr}{dt}|_n=\frac{dr}{dt}|_b+\omega_{nb}\times rdtdr∣n=dtdr∣b+ωnb×r
（2）上式向b系投影：
drdt∣nb=drdt∣bb+(ωnb×r)b\frac{dr}{dt}|^b_n=\frac{dr}{dt}|^b_b+(\omega_{nb}\times r)^bdtdr∣nb=dtdr∣bb+(ωnb×r)b
（3）由于：
drdt∣bb=r˙b\frac{dr}{dt}|^b_b=\dot r^bdtdr∣bb=r˙b
(ωnb×r)b=ωnbb×rb(\omega_{nb} \times r)^b=\omega^b_{nb}\times r^b(ωnb×r)b=ωnbb×rb
（4）代入到（2）式得：
r˙b=drdt∣nb−ωnbb×rb\dot r^b=\frac{dr}{dt}|^b_n-\omega^b_{nb}\times r^br˙b=dtdr∣nb−ωnbb×rb
（5）由于rb=Cnbrnr^b=C^b_nr^nrb=Cnbrn，对其两边求导：
r˙b=C˙nbrn+cnbr˙n=C˙nbrn+Cnbdrdt∣nn\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_nr˙b=C˙nbrn+cnbr˙n=C˙nbrn+Cnbdtdr∣nn
（6）上式中Cnbdrdt∣nnC^b_n\frac{dr}{dt}|^n_nCnbdtdr∣nn即位drdt∣nb\frac{dr}{dt}|^b_ndtdr∣nb，所以上式变为：
r˙b=C˙nbrn+drdt∣nb\dot r^b=\dot C^b_n r^n+\frac{dr}{dt}|^b_nr˙b=C˙nbrn+dtdr∣nb
（7）式与（4）式对比可得：
C˙nb=−ωnbb×rb=−ωnbb×Cnbrn=−ωnbbkCnbrn\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^nC˙nb=−ωnbb×rb=−ωnbb×Cnbrn=−ωnbbkCnbrn
（8）最终得：
C˙nb=−ωnbbkCnb\dot C^b_n=-\omega^{bk}_{nb}C^b_nC˙nb=−ωnbbkCnb
转置得：
C˙bn=Cbnωnbbk\dot C^n_b=C^n_b\omega^{bk}_{nb}C˙bn=Cbnωnbbk
其中，ωnbbk\omega^{bk}_{nb}ωnbbk为旋转欧拉角向量的反对称矩阵。
四元素微分方程
（1）定义b系到n系的四元素为Q，则用欧拉角表示为
Q=cosθ2+μRsinθ2Q=cos\frac{\theta}{2}+\mu^Rsin\frac{\theta}{2}Q=cos2θ+μRsin2θ
（2）对上式两边求导：
dQdt=−θ˙2sinθ2+μRθ˙2cosθ2+sinθ2dμRdt\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μRdt=CbRdμbdt+ωRbR×μR\frac{d\mu^R}{dt}=C^R_b\frac{d\mu^b}{dt}+\omega^R_{Rb}\times \mu^RdtdμR=CbRdtdμb+ωRbR×μR
（4）其中：dμbdt=0\frac{d\mu^b}{dt}=0dtdμb=0，且ωRbR=θ˙μR\omega^R_{Rb}=\dot \theta \mu^RωRbR=θ˙μR,故dμRdt=0\frac{d\mu^R}{dt}=0dtdμR=0
（5）则（2）式变为：
dQdt=−θ˙2sinθ2+μRθ˙2cosθ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^R2θ˙μR得：
θ˙2μR⊗Q=θ˙2μR⊗(cosθ2+μRsinθ2)=θ˙2μRcosθ2+μR⊗μRsinθ2=θ˙2μRcosθ2−sinθ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）相比即有：
dQdt=θ˙2μR⊗Q=12ωRbR⊗Q=12Q⊗ωRbb=12WQ\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}WQdtdQ=2θ˙μR⊗Q=21ωRbR⊗Q=21Q⊗ωRbb=21WQ
上式即为四元素微分方程。其中w为欧拉角向量的反对称矩阵。
速度微分方程

地速：drdt∣e=ve\frac{dr}{dt}|_e=vedtdr∣e=ve
哥氏方程：drdt∣a=drdtb+ωab×r\frac{dr}{dt}|_a=\frac{dr}{dt}_b+\omega_{ab}\times rdtdr∣a=dtdrb+ωab×r
导航方程：d2rdt2∣i=f+g\frac{d^2r}{d t^2}|_i=f+gdt2d2r∣i=f+g

（1）由哥氏方程和地速定义（i 系）：
drdt∣i=drdt∣e+ωie×r=ve+ωie×r\frac{dr}{dt}|_i=\frac{dr}{dt}|_e+\omega_{ie}\times r=v_e+\omega_{ie}\times rdtdr∣i=dtdr∣e+ωie×r=ve+ωie×r
（2）在i系下求导：
d2rdt2=dvedt∣i+ddt(ωie×r)∣i\frac{d^2 r}{dt^2}=\frac{dv_e}{dt}|_i+\frac{d}{dt}(\omega_{ie}\times r)|_idt2d2r=dtdve∣i+dtd(ωie×r)∣i
（3）利用导航方程整理可得：
dvedt∣i=f+g−dωiedt∣i×r−ωie×drdt∣i=f+g−ωie×(ve+ωie×r)=f−ωie×ve+(g−ωie×(ωie×r))=f−ωie×ve+gl\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_ldtdve∣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系：
=Cbifb−ωiei×vei+gli=C^i_bf^b-\omega^i_{ie}\times v^i_e + g^i_l=Cbifb−ωiei×vei+gli
上式即为速度微分方程。
位置微分方程
在INS中位置是由速度确定，因此其微分方方程也较为简单，在各种书籍论文中常用的位置微分方程如下所示：
r˙n=[ϕ˙λ˙h˙]=[1RM+h0001(RN+h)cosϕ000−1][vNvEvD]=D−1vn\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^nr˙n=⎣⎡ϕ˙λ˙h˙⎦⎤=⎣⎡RM+h1000(RN+h)cosϕ1000−1⎦⎤⎣⎡vNvEvD⎦⎤=D−1vn