惯导系统模型及其仿真（五）

2.编程实现–位置解算

2.3 位置解算

惯性器件均以增量形式输出量测信息，同时，之前的姿态和速度解算也只能给出更新时间点上的信息，都是离散的形式。
所以，位置解也只有离散解，只能根据离散时间点上的速度、姿态、角增量、速度增量求取。
【注】：本编程实现中，位置更新周期 [tl−1,tl] [ t l − 1 , t l ] 与速度更新周期 [tm−1,tm] [ t m − 1 , t m ] 、姿态更新周期 [tk−1,tk] [ t k − 1 , t k ] 相同，均为6倍的采样时间间隔。

2.3.1 算法原理

位置解算的目标是求得 tl t l 时刻的经纬度信息，即， Ll,λl L l , λ l .
设 tl−1 t l − 1 时刻的经纬度信息已知，记为 L,λ L , λ
则，根据经纬度和位置矩阵的对应关系，可以写出位置矩阵 Cne C e n

Cn(l−1)e=⎡⎣⎢−sinλ−sinLcosλcosLcosλcosλ−sinLsinλcosLsinλ0cosLsinL⎤⎦⎥ C e n ( l − 1 ) = [ − s i n λ c o s λ 0 − s i n L c o s λ − s i n L s i n λ c o s L c o s L c o s λ c o s L s i n λ s i n L ]

假设，又已知 [tl−1,tl] [ t l − 1 , t l ] 时间段内坐标系 n(l−1) n ( l − 1 ) 到 n(l) n ( l ) 的旋转矢量 ξl ξ l
则， Cn(l−1)n(l)=I+(ξl×) C n ( l ) n ( l − 1 ) = I + ( ξ l × ) ，（此处和速度解算时一样取了近似，认为 ξl ξ l 是小量）
亦即，

Cn(l)n(l−1)=I−(ξl×) C n ( l − 1 ) n ( l ) = I − ( ξ l × )

这样， tl t l 时刻的位置矩阵为：

Cn(l)e=Cn(l)n(l−1)Cn(l−1)e C e n ( l ) = C n ( l − 1 ) n ( l ) C e n ( l − 1 )

最后，根据 经纬度和位置矩阵的对应关系，可以解出 tl t l 时刻的经纬度信息。

现在问题的关键落到， [tl−1,tl] [ t l − 1 , t l ] 坐标系 n(l−1) n ( l − 1 ) 到 n(l) n ( l ) 的旋转矢量 ξl ξ l 的求解上。

直观上可以想象， n(l−1) n ( l − 1 ) 到 n(l) n ( l ) 的旋转矢量 ξl ξ l ，就是 [tl−1,tl] [ t l − 1 , t l ] 内位置速率 ωen ω e n 的积分，即：

ξl=∫tltl−1ωnendt ξ l = ∫ t l − 1 t l ω e n n d t

• 其中，（非极区、选用地理坐标系作为导航坐标系）
  
  ωnen=⎡⎣⎢⎢⎢⎢−VNRM+hVERN+hVERN+htanL⎤⎦⎥⎥⎥⎥=F(t)Vn(t)=⎡⎣⎢⎢⎢01RN+h1RN+htanL−1RM+h00000⎤⎦⎥⎥⎥⎡⎣⎢VEVNVU⎤⎦⎥ ω e n n = [ − V N R M + h V E R N + h V E R N + h t a n L ] = F ( t ) V n ( t ) = [ 0 − 1 R M + h 0 1 R N + h 0 0 1 R N + h t a n L 0 0 ] [ V E V N V U ]
  
  且，认为在位置更新周期里，纬度变化不大，将 F(t) F ( t ) 以 [tl−1,tl] [ t l − 1 , t l ] 的中间时刻对应的纬度 L^(l−1,l)/2 L ^ ( l − 1 , l ) / 2 计算所得的常值矩阵 F^(l−1,l)/2 F ^ ( l − 1 , l ) / 2 作为其估计值。
  其中， L^(l−1,l)/2=L^l−1+12(L^l−1−L^l−2) L ^ ( l − 1 , l ) / 2 = L ^ l − 1 + 1 2 ( L ^ l − 1 − L ^ l − 2 ) ，即线性外推值。

这样，积分变为：

ξl=∫tltl−1ωnendt≈F^(l−1,l)/2∫tltl−1Vn(t)dt=F^(l−1,l)/2ΔRnl ξ l = ∫ t l − 1 t l ω e n n d t ≈ F ^ ( l − 1 , l ) / 2 ∫ t l − 1 t l V n ( t ) d t = F ^ ( l − 1 , l ) / 2 Δ R l n

现在问题的关键落到， ΔRnl Δ R l n 的求解上。

理论上，可以经过 M M 次速度更新( Tm T m 速度更新周期)后进行一次位置更新（计算量更小些），即

ΔRnl=∫tltl−1Vn(t)dt=∑m=1M∫tl−1+mTmtl−1+(m−1)TmVn(t)dt=∑m=1MΔRnm Δ R l n = ∫ t l − 1 t l V n ( t ) d t = ∑ m = 1 M ∫ t l − 1 + ( m − 1 ) T m t l − 1 + m T m V n ( t ) d t = ∑ m = 1 M Δ R m n

---

（1）.梯形积分

ΔRnm=Tm2(Vnm+Vnm−1) Δ R m n = T m 2 ( V m n + V m − 1 n )

本质：将 Vn(t) V n ( t ) 在 [tm−1,tm] [ t m − 1 , t m ] 随时间变化的曲线 拟合为 直线。
特点：简单但是精度不够
（2）. 高分辨率的算法
Vn(t) V n ( t ) 在 [tm−1,tm] [ t m − 1 , t m ] 随时间 t t 变化的真实曲线

Vn(t)=Vnm−1+ΔVsf(t)+ΔVg/cormt−tm−1Tm=Vnm−1+∫ttm−1Cn(l−1)b(τ)fb(τ)dτ+t−tm−1Tm∫tmtm−1[−(2ωnie+ωnen)×Vn+gn]dt V n ( t ) = V m − 1 n + Δ V s f ( t ) + Δ V g / c o r m t − t m − 1 T m = V m − 1 n + ∫ t m − 1 t C b ( τ ) n ( l − 1 ) f b ( τ ) d τ + t − t m − 1 T m ∫ t m − 1 t m [ − ( 2 ω i e n + ω e n n ) × V n + g n ] d t

将这条曲线在 [tm−1,tm] [ t m − 1 , t m ] 积分得到的就是 ΔRnm Δ R m n ，故：

ΔRnm=[Vnm−1+12ΔVg/corm]Tm+ΔRnsfm Δ R m n = [ V m − 1 n + 1 2 Δ V g / c o r m ] T m + Δ R s f m n

• 其中， ΔRnsfm=∫tmtm−1ΔVsf(t)dt=∫tmtm−1∫ttm−1Cn(l−1)b(τ)fb(τ)dτdt Δ R s f m n = ∫ t m − 1 t m Δ V s f ( t ) d t = ∫ t m − 1 t m ∫ t m − 1 t C b ( τ ) n ( l − 1 ) f b ( τ ) d τ d t

这里仅给出 ΔRnsfm Δ R s f m n 的积分结果，推导过程详见P301：

ΔRnsfm=Tm3[(ξl−1,m−1−ξl−1,m)×]ΔVn(l−1)sfm+Cn(m−1)n(l−1)Cn(l−1)b(m−1)ΔRb(m−1)sfm Δ R s f m n = T m 3 [ ( ξ l − 1 , m − 1 − ξ l − 1 , m ) × ] Δ V s f m n ( l − 1 ) + C n ( l − 1 ) n ( m − 1 ) C b ( m − 1 ) n ( l − 1 ) Δ R s f m b ( m − 1 )

• 其中， ΔVn(l−1)sfm=Cn(l−1)b(m)ΔVbm Δ V s f m n ( l − 1 ) = C b ( m ) n ( l − 1 ) Δ V m b
  Cn(l−1)b(m) C b ( m ) n ( l − 1 ) 是 tm t m 时刻的姿态阵， ΔVbm Δ V m b 是 [tm−1,tm] [ t m − 1 , t m ] 时间段内的速度增量。

• ΔRb(m−1)sfm=SbΔvm+ΔRbrotm+ΔRbscrlm Δ R s f m b ( m − 1 ) = S Δ v m b + Δ R r o t m b + Δ R s c r l m b
  (1). SbΔvm=∫tmtm−1ΔVb(t)dt=∫tmtm−1∫ttm−1fb(τ)dτdt S Δ v m b = ∫ t m − 1 t m Δ V b ( t ) d t = ∫ t m − 1 t m ∫ t m − 1 t f b ( τ ) d τ d t
  (2).P307（三子样算法）给出了 ΔRrotm Δ R r o t m
  (3).P310 （优化三子样算法）给出了 ΔRscrlm Δ R s c r l m

2.3.2 编程实现

写成两个函数：
（1）.在已知 ΔRnm Δ R m n 的前提下，求解出经纬度；
（2）.求解 ΔRnm Δ R m n

（1）.实现导航解算中经纬度的求解。
函数的输入参数：
tl−1 t l − 1 时刻位置解算得到的经纬度： L^l−1,λ^l−1 L ^ l − 1 , λ ^ l − 1 ；
tl−2 t l − 2 时刻位置解算得到的经纬度： L^l−2,λ^l−2 L ^ l − 2 , λ ^ l − 2 ；
[tl−1,tl] [ t l − 1 , t l ] 时间段内运载体的位置增量： ΔRnl Δ R l n ；
函数的输出参数：
tl t l 时刻位置解算得到的经纬度： L^l,λ^l L ^ l , λ ^ l ；

（2）.梯形积分（Trapezoidal integral）