INS 惯性推算

2023-12-17 20:27:19

4 SINS编排

4.1 速度更新

v k n = v k ? 1 n + △ v f , k n + △ v g / c o r , k n v_{k}^{n}=v_{k-1}^{n}+\triangle v_{f,k}^{n}+\triangle v_{g/cor,k}^{n} vkn?=vk?1n?+vf,kn?+vg/cor,kn?

  • 计算 △ v f , k n \triangle v_{f,k}^{n} vf,kn?:
    △ v f , k b ( k ? 1 ) = △ v f , k b + 1 2 △ θ k × △ v f , k b + 1 12 ( △ θ k ? 1 × △ v f , k b + △ v f , k ? 1 b × △ θ k ) \triangle v_{f,k}^{b(k-1)}=\triangle v_{f,k}^{b}+\frac{1}{2}\triangle\theta_{k}\times\triangle v_{f,k}^{b}+\frac{1}{12}(\triangle\theta_{k-1}\times\triangle v_{f,k}^{b}+\triangle v_{f,k-1}^{b}\times\triangle\theta_{k}) vf,kb(k?1)?=vf,kb?+21?θk?×vf,kb?+121?(θk?1?×vf,kb?+vf,k?1b?×θk?) △ v f , k n = [ I ? ( 0.5 ζ k × ) ] C b ( k ? 1 ) n ( k ? 1 ) △ v f , k b ( k ? 1 ) \triangle v_{f,k}^{n}=[I-(0.5\zeta_k\times)]C_{b(k-1)}^{n(k-1)}\triangle v_{f,k}^{b(k-1)} vf,kn?=[I?(0.5ζk?×)]Cb(k?1)n(k?1)?vf,kb(k?1)? 使用中点时刻的速度和位置信息 更新 ω i n n \omega_{in}^{n} ωinn?,得到 0.5 ζ k \zeta_k ζk?:
    ζ k = [ ω i e n + ω e n n ] k ? 1 / 2 △ t k \zeta_{k} = [\omega_{ie}^n+\omega_{en}^{n}]_{k-1/2}\triangle t_k ζk?=[ωien?+ωenn?]k?1/2?tk?其中 ζ k \zeta_{k} ζk?为坐标框架 n ( k ) n(k) n(k)相对 n ( k ? 1 ) n(k-1) n(k?1)的旋转向量; k ? 1 / 2 k-1/2 k?1/2表示区间 [ t k ? 1 , t k ] [t_{k-1}, t_k] [tk?1?,tk?]中值;时刻 t k t_k tk?的位置和速度未知,为了计算中点时刻的速度和位置,对其进行外推:
    ? 高程外推
    h k ? 1 / 2 = h k ? 1 ? v D , k ? 1 △ t k 2 h_{k-1/2} = h_{k-1} -\frac{v_{D,k-1}\triangle t_k}{2} hk?1/2?=hk?1??2vD,k?1?tk??
    经纬度外推
    可以通过如下方求得: q n ( k ? 1 / 2 ) e ( k ? 1 / 2 ) = q e ( k ? 1 ) e ( k ? 1 / 2 ) ? q n ( k ? 1 ) e ( k ? 1 ) ? q n ( k ? 1 / 2 ) n ( k ? 1 ) q_{n(k-1/2)}^{e(k-1/2)} =q_{e(k-1)}^{e(k-1/2)} \ast q_{n(k-1)}^{e(k-1)} \ast q_{n(k-1/2)}^{n(k-1)} qn(k?1/2)e(k?1/2)?=qe(k?1)e(k?1/2)??qn(k?1)e(k?1)??qn(k?1/2)n(k?1)?其中,
    q n ( k ? 1 / 2 ) n ( k ? 1 ) = [ c o s ∣ 0.5 ζ k ? 1 / 2 ∣ ? s i n ∣ 0.5 ζ k ? 1 / 2 ∣ 0.5 ζ k ? 1 / 2 0.5 ζ k ? 1 / 2 ] q e ( k ? 1 ) e ( k ? 1 / 2 ) = [ c o s ∣ 0.5 ? k ? 1 / 2 ∣ ? s i n ∣ 0.5 ? k ? 1 / 2 ∣ 0.5 ? k ? 1 / 2 0.5 ? k ? 1 / 2 ] q_{n(k-1/2)}^{n(k-1)}=\begin{bmatrix} cos|0.5\zeta_{k-1/2}| \\ -\frac{sin|0.5\zeta_{k-1/2}|}{0.5\zeta_{k-1/2}}0.5\zeta_{k-1/2}\end{bmatrix}\\ q_{e(k-1)}^{e(k-1/2)} = \begin{bmatrix} cos|0.5\epsilon_{k-1/2}| \\ -\frac{sin|0.5\epsilon_{k-1/2}|}{0.5\epsilon_{k-1/2}}0.5\epsilon_{k-1/2}\end{bmatrix} qn(k?1/2)n(k?1)?=[cos∣0.5ζk?1/2??0.5ζk?1/2?sin∣0.5ζk?1/2??0.5ζk?1/2??]qe(k?1)e(k?1/2)?=[cos∣0.5?k?1/2??0.5?k?1/2?sin∣0.5?k?1/2??0.5?k?1/2??] ζ k ? 1 / 2 = [ ω i e n + ω e n n ] k ? 1 △ t k 2 \zeta_{k-1/2} = [\omega_{ie}^n+\omega_{en}^{n}]_{k-1}\frac{\triangle t_k}{2} ζk?1/2?=[ωien?+ωenn?]k?1?2tk?? ? k ? 1 / 2 = ω i e e △ t k 2 \epsilon_{k-1/2} =\omega_{ie}^{e}\frac{\triangle t_k}{2} ?k?1/2?=ωiee?2tk??。最后单位化四元数 q n ( k ? 1 / 2 ) e ( k ? 1 / 2 ) q_{n(k-1/2)}^{e(k-1/2)} qn(k?1/2)e(k?1/2)?并得到对应的方向余弦矩阵,根据方向余弦矩阵求得此刻的经纬度值。
    速度外推
    v k ? 1 / 2 n = v k ? 1 n + △ v k ? 1 n 2 v_{k-1/2}^{n}=v_{k-1}^{n}+\frac{\triangle v_{k-1}^{n}}{2} vk?1/2n?=vk?1n?+2vk?1n??
  • 计算 △ v g / c o r , k n \triangle v_{g/cor,k}^{n} vg/cor,kn?:
    △ v g / c o r , k n = [ g n ? ( 2 ω i e n + ω e n n ) × v n ] k ? 1 / 2 △ t k \triangle v_{g/cor,k}^{n}=[g^n-(2\omega_{ie}^{n}+\omega_{en}^{n})\times v^n]_{k-1/2}\triangle t_k vg/cor,kn?=[gn?(2ωien?+ωenn?)×vn]k?1/2?tk?

4.2 位置更新

n系到e系的四元数 q n e q_{n}^{e} qne?包含经纬度信息,四元数更新如下:
q n ( k ) e ( k ) = q e ( k ? 1 ) e ( k ) ? q n ( k ? 1 ) e ( k ? 1 ) ? q n ( k ) n ( k ? 1 ) q_{n(k)}^{e(k)} = q_{e(k-1)}^{e(k)} \cdot q_{n(k-1)}^{e(k-1)} \cdot q_{n(k)}^{n(k-1)} qn(k)e(k)?=qe(k?1)e(k)??qn(k?1)e(k?1)??qn(k)n(k?1)?其中, q n ( k ) n ( k ? 1 ) = [ c o s ∣ 0.5 ζ k ∣ ? s i n ∣ 0.5 ζ k ∣ 0.5 ζ k 0.5 ζ k ] q e ( k ? 1 ) e ( k ) = [ c o s ∣ 0.5 ? k ∣ ? s i n ∣ 0.5 ? k ∣ 0.5 ? k 0.5 ? k ] q_{n(k)}^{n(k-1)}=\begin{bmatrix} cos|0.5\zeta_{k}| \\ -\frac{sin|0.5\zeta_{k}|}{0.5\zeta_{k}}0.5\zeta_{k}\end{bmatrix}\\ q_{e(k-1)}^{e(k)} = \begin{bmatrix} cos|0.5\epsilon_{k}| \\ -\frac{sin|0.5\epsilon_{k}|}{0.5\epsilon_{k}}0.5\epsilon_{k}\end{bmatrix} qn(k)n(k?1)?=[cos∣0.5ζk??0.5ζk?sin∣0.5ζk??0.5ζk??]qe(k?1)e(k)?=[cos∣0.5?k??0.5?k?sin∣0.5?k??0.5?k??]
? k = ω i e e △ t k \epsilon_{k} =\omega_{ie}^{e}\triangle t_k ?k?=ωiee?tk?:表示e(k)相对于e(k-1)的旋转向量。
ζ k ? 1 / 2 = [ ω i e n + ω e n n ] k ? 1 △ t k 2 \zeta_{k-1/2} = [\omega_{ie}^n+\omega_{en}^{n}]_{k-1}\frac{\triangle t_k}{2} ζk?1/2?=[ωien?+ωenn?]k?1?2tk??:这个量的计算需要位置和速度信息,首先根据更新后的速度,再次更新中点时刻的速度 v k ? 1 / 2 n = 1 2 ( v k n + v k ? 1 n ) v_{k-1/2}^{n}=\frac{1}{2}(v_{k}^{n}+v_{k-1}^{n}) vk?1/2n?=21?(vkn?+vk?1n?)
经纬度外推
单位化四元数 q n ( k ) e ( k ) q_{n(k)}^{e(k)} qn(k)e(k)?并得到对应的方向余弦矩阵,根据方向余弦矩阵求得此刻的经纬度值。
高程外推
h k = h k ? 1 ? v D , k ? 1 / 2 △ t k h_{k} = h_{k-1} -v_{D,k-1/2}\triangle t_k hk?=hk?1??vD,k?1/2?tk?

4.3 姿态更新

b系到n系的姿态 q b n q_{b}^{n} qbn?更新算法:
q b ( k ) n ( k ) = q n ( k ? 1 ) n ( k ) ? q b ( k ? 1 ) n ( k ? 1 ) ? q b ( k ) b ( k ? 1 ) q_{b(k)}^{n(k)} = q_{n(k-1)}^{n(k)}\cdot q_{b(k-1)}^{n(k-1)}\cdot q_{b(k)}^{b(k-1)} qb(k)n(k)?=qn(k?1)n(k)??qb(k?1)n(k?1)??qb(k)b(k?1)?
b系四元数 q b ( k ) b ( k ? 1 ) q_{b(k)}^{b(k-1)} qb(k)b(k?1)?更新如下:
q b ( k ) b ( k ? 1 ) = [ c o s ∣ 0.5 ? k ∣ ? s i n ∣ 0.5 ? k ∣ 0.5 ? k 0.5 ? k ] q_{b(k)}^{b(k-1)}=\begin{bmatrix} cos|0.5\phi_{k}| \\ -\frac{sin|0.5\phi_{k}|}{0.5\phi_{k}}0.5\phi_{k}\end{bmatrix} qb(k)b(k?1)?=[cos∣0.5?k??0.5?k?sin∣0.5?k??0.5?k??]b系的旋转向量计算方法为: ? k = △ θ k + 1 12 △ θ k ? 1 × △ θ k \phi_k=\triangle \theta_k+\frac{1}{12}\triangle \theta_{k-1}\times\triangle\theta_k ?k?=θk?+121?θk?1?×θk?
n系四元数 q n ( k ? 1 ) n ( k ) q_{n(k-1)}^{n(k)} qn(k?1)n(k)?更新如下: q n ( k ? 1 ) n ( k ) = [ c o s ∣ 0.5 ζ k ∣ ? s i n ∣ 0.5 ζ k ∣ 0.5 ζ k 0.5 ζ k ] q_{n(k-1)}^{n(k)}=\begin{bmatrix} cos|0.5\zeta_{k}| \\ -\frac{sin|0.5\zeta_{k}|}{0.5\zeta_{k}}0.5\zeta_{k}\end{bmatrix} qn(k?1)n(k)?=[cos∣0.5ζk??0.5ζk?sin∣0.5ζk??0.5ζk??]基于上边更新的位置信息,更新中点的位置,进一步更新 ζ k \zeta_{k} ζk?
最后,单位化四元数 q b ( k ) n ( k ) q_{b(k)}^{n(k)} qb(k)n(k)?,并得到对应的方向余弦矩阵,再有方向余弦阵计算欧拉角

X k / k ? 1 = F k / k ? 1 ? X k ? 1 \begin{aligned} \mathbf{X_{k/k-1}} = \mathbf{F_{k/k-1}} * \mathbf{X_{k-1}} \end{aligned} Xk/k?1?=Fk/k?1??Xk?1??

P k / k ? 1 = F k / k ? 1 ? P k ? 1 ? F k / k ? 1 T + 0.5 ? ( F k / k ? 1 ? G k / k ? 1 ? Q k ? 1 G k / k ? 1 T ? F k / k ? 1 T + G k / k ? 1 ? Q k ? 1 ? G k / k ? 1 T ) ? Δ t k \begin{aligned} \mathbf{P_{k/k-1}} &= \mathbf{F_{k/k-1}} * \mathbf{P_{k-1}} * \mathbf{F_{k/k-1}^T}+ 0.5 * (\mathbf{F_{k/k-1}} * \mathbf{G_{k/k-1}} * \mathbf{Q_{k-1}}\mathbf{G_{k/k-1}^T} \\ &* \mathbf{F_{k/k-1}^T} +\mathbf{G_{k/k-1}} * \mathbf{Q_{k-1}} * \mathbf{G_{k/k-1}^T}) * \Delta t_{k} \end{aligned} Pk/k?1??=Fk/k?1??Pk?1??Fk/k?1T?+0.5?(Fk/k?1??Gk/k?1??Qk?1?Gk/k?1T??Fk/k?1T?+Gk/k?1??Qk?1??Gk/k?1T?)?Δtk??

文章来源:https://blog.csdn.net/qq_40230900/article/details/135049068
本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。