2023, Vol. 45
2. 华中光电技术研究所，湖北 武汉 430223
2. Huazhong Istitute of Electro-opotics, Wuhan 430223, China
天文导航系统（以下简称天导）具有隐蔽性好、导航误差不随时间发散等优点，但其定位精度不高；而惯性导航系统（以下简称惯导）自主性强、短时导航精度高、导航信息完备，但其误差随时间积累，因此采用天导综合校正惯导可实现优势互补，提升长航时导航精度。
在天导与惯导组合过程中，影响组合效果的主要因素有天导信息质量和组合策略选择[1-2]。
天导信息质量提升方面，国内外学者主要在天文导航误差分析[3]、星光测量性能提升[4]、星光动态跟踪能力提升[5]等方面有所研究。部分学者从理论和试验方面分析了天文导航星体跟踪器和惯性水平基准性能精度对天文导航误差的影响[6]。
在天导和惯导组合策略方面，主要聚焦于天文测星与惯性水平基准组合层面[7]，尚未有天导设备和惯导设备组合导航技术体系化研究。在天文测星与惯性水平基准组合层面方面，国内外学者针对采用不同的观测信息展开了较多研究。熊智等[8]以天导方位角和高度角为观测信息，提出了基于天文角度观测信息的机载惯性/天文组合滤波算法，在单星观测条件下也能获得较好的定位精度。黄远等[9]基于星光单位矢量、光轴指向、姿态角、角速度、姿态矩阵、四元数误差和误差四元数等7种不同的信息作为观测信息，分析了不同组合模式对定姿结果的影响。
天导和惯导组合导航算法，目前以经典的线性kalman滤波为主[10]。然而，天导在测星导航解算后对位置误差的修正作用会使其出现“误差回降”现象，这导致采用基于经典Kalman滤波的天文/惯性组合方法估计惯导误差产生较大波动，且对传感器误差的估计存在常值偏差。
针对上述问题，本文围绕天导设备和惯导设备组合导航应用背景，以抗差Kalman为基础提出基于天导位置的惯导综合校正技术，充分利用天导位置观测信息的同时，能有效避免天导位置“误差回降”带来的不利影响。
1 天导/惯导组合导航技术原理天文/惯性组合模型以Kalman滤波为基础，以系统误差为状态信息建立状态方程，以位置为量测信息建立量测方程。
天文/惯性组合原理框架如图1所示。
|
图 1 天文/惯性位置组合技术原理 Fig. 1 Principles of astronomical inertial position combination technology |
组合系统状态选择姿态误差、速度误差、位置误差、惯性传感器误差。状态向量为：
| $ {\boldsymbol{X}} = {\left[ {\begin{array}{*{20}{c}} {{{\phi }^{\text{T}}}}&{{{({\mathbf{\delta }}{{\boldsymbol{v}}^n})}^{\text{T}}}}&{{{({\boldsymbol{\delta p}})}^{\text{T}}}}&{{{({{\mathbf{\varepsilon }}^b})}^{\text{T}}}}&{{{({{\nabla }^b})}^{\text{T}}}} \end{array}} \right]^{\text{T}}} 。$ | (1) |
1）状态方程
组合系统状态方程如下：
| $ \left\{ \begin{gathered} {\dot \phi } = {{\boldsymbol{M}}_{aa}}{\phi } + {{\boldsymbol{M}}_{av}}{\boldsymbol{\delta }}{{\boldsymbol{v}}^n} + {{\boldsymbol{M}}_{ap}}{\boldsymbol{\delta p}} - {\boldsymbol{C}}_b^n{{\boldsymbol{\varepsilon }}^b}，\\ {\boldsymbol{\delta }}{{{\boldsymbol{\dot v}}}^n} = {{\boldsymbol{M}}_{va}}{\phi } + {{\boldsymbol{M}}_{vv}}{\boldsymbol{\delta }}{{\boldsymbol{v}}^n} + {{\boldsymbol{M}}_{vp}}{\boldsymbol{\delta p}} + {\boldsymbol{C}}_b^n{{\nabla }^b}，\\ {\boldsymbol{\delta \dot p}} = {{\boldsymbol{M}}_{pv}}{\boldsymbol{\delta v}} + {{\boldsymbol{M}}_{pp}}{\boldsymbol{\delta p}}，\\ {{{\boldsymbol{\dot \varepsilon }}}^b}{\text{ = }}0，\\ {{{\dot \nabla }}^b}{\text{ = }}0 。\\ \end{gathered} \right. $ | (2) |
其中：
| $ \begin{array}{l}{M}_{pp}=\left[\begin{array}{ccc}0& 0& -{v}_{N}/{R}_{Mh}^{2}\\ {v}_{E}\mathrm{sec}L\mathrm{tan}L/{R}_{Nh}& 0& -{v}_{E}\mathrm{sec}L/{R}_{Nh}^{2}\\ 0& 0& 0\end{array}\right]，\\ {M}_{1}=\left[\begin{array}{ccc}0& 0& 0\\ -{\omega }_{ie}\mathrm{sin}L& 0& 0\\ {\omega }_{ie}\mathrm{cos}L& 0& 0\end{array}\right]，\\ {M}_{2}=\left[\begin{array}{ccc}0& -1/{R}_{Mh}& 0\\ 1/{R}_{Nh}& 0& 0\\ \mathrm{tan}L/{R}_{Nh}& 0& 0\end{array}\right]，\\ {M}_{3}=\left[\begin{array}{ccc}0& 0& {v}_{N}/{R}_{Mh}^{2}\\ 0& 0& -{v}_{E}/{R}_{Nh}^{2}\\ {v}_{E}{\mathrm{sec}}^{2}L/{R}_{Nh}& 0& -{v}_{E}\mathrm{tan}L/{R}_{Nh}^{2}\end{array}\right]，\\ {M}_{4}=\left[\begin{array}{ccc}0& 0 & 0\\ 0& 0& 0\\ -{g}_{e}sin2L(\beta -4{\beta }_{1}\mathrm{cos}2L)& 0& {\beta }_{2}\end{array}\right]，\\ {M}_{aa}=-({\omega }_{in}^{n}{\bf{\times}} )\text{，}{M}_{av}={M}_{2}，\\ {M}_{ap}={M}_{1}+{M}_{3}\text{，}{M}_{va}=({f}_{sf}^{n}{\bf{\times}} )，\\ {M}_{vv}=-\left[({v}^{n}{\bf{\times}} ){M}_{2}+(2{\omega }_{ie}^{n}+{\omega }_{en}^{n}){\bf{\times}} \right]，\\ {M}_{vp}=({v}^{n}{\bf{\times}} )(2{M}_{1}+{M}_{3}+{M}_{4})，\\ {M}_{pv}=\left[\begin{array}{ccc}0& 1/{R}_{Mh}& 0\\ \mathrm{sec}L/{R}_{Nh}& 0& 0\\ 0& 0& 1\end{array}\right]。\end{array} $ | (3) |
离散化后，与式（2）对应的状态矩阵为：
| $\begin{aligned} &{{\boldsymbol{\varPhi }}_{k/k - 1}} = {\boldsymbol{I}} +\\ &{\left[ \begin{gathered} \begin{array}{*{20}{c}} {{{\boldsymbol{M}}_{aa}}}&{{{\boldsymbol{M}}_{av}}}&{{{\boldsymbol{M}}_{ap}}}&{ - {\boldsymbol{C}}_b^n}&{{{0}_{3 \times 3}}}&{{{0}_{3 \times 4}}} \\ {{{\boldsymbol{M}}_{va}}}&{{{\boldsymbol{M}}_{vv}}}&{{{\boldsymbol{M}}_{vp}}}&{{{0}_{3 \times 3}}}&{{\boldsymbol{C}}_b^n}&{{{0}_{3 \times 4}}} \\ {{{0}_{3 \times 3}}}&{{{\boldsymbol{M}}_{pv}}}&{{{\boldsymbol{M}}_{pp}}}&{{{0}_{3 \times 3}}}&{{{0}_{3 \times 3}}}&{{{0}_{3 \times 4}}} \end{array} \\ \begin{array}{*{20}{c}} {}&{}&{}&{}&{}&{} \end{array}{{0}_{6 \times 15}} \\ \end{gathered} \right]_{k - 1}} \cdot {T_s} 。\end{aligned}$ | (4) |
2）量测方程
以位置作为量测信息，则天文/惯性信息组合系统量测方程为：
| $ \begin{gathered} {\boldsymbol{Z}} = \delta {\boldsymbol{p}} = {\boldsymbol{H}} \cdot {\boldsymbol{X}} \\ {\boldsymbol{H}} = {\left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{0}}_{3 \times 6}}}&{\boldsymbol{I}}&{{{\boldsymbol{0}}_{3 \times 6}}} \end{array}} \right]^{\rm{T}}} 。\\ \end{gathered} $ | (5) |
抗差Kalman滤波与常规Kalman滤波在递推形式上完全一致，两者的差异仅体现在观测模型中量测噪声的方差采用等价方差进行替换。目前，常用的等价权函数有丹麦法、Huber法，IGG(Ⅰ～Ⅲ）系列方案等。采用IGG-Ⅲ方案，其等价权可表示为：
| $ {\bar p_i} = \left\{ {\begin{array}{*{20}{l}} {{{\bar p}_i}}，{\left| {{{\tilde v}_i}} \right| \leqslant {k_0}}，\\ {{p_i}\dfrac{{{k_0}}}{{\left| {{{\tilde v}_i}} \right|}}{{\left( {\dfrac{{{k_1} - \left| {{{\tilde v}_i}} \right|}}{{{k_1} - {k_0}}}} \right)}^2}}，{{k_0} < \left| {{{\tilde v}_i}} \right| \leqslant {k_1}} ，\\ 0，{\left| {{{\tilde v}_i}} \right| > {k_1}} 。\end{array}} \right. $ | (6) |
式中：
抗差Kalman滤波通过调节观测噪声协方差矩阵调整Kalman滤波的增益。当天导位置观测值中含有粗差时，利用抗差最小二乘算法构造等价权矩阵的方法构造观测值的等价协方差矩阵，在Kalman滤波中，观测噪声方差的增大意味着相应观测值增益的减小。因此，可以通过滤波增益矩阵抑制粗差对系统的影响。当观测值中不存在粗差时，观测值的权因子αi =1（i = 1, 2, …n），此时抗差Kalman滤波转变为常规Kalman滤波。
3 试验与分析 3.1 试验条件实际天导设备在测星导航解算后，会对间隔期内的定位误差进行修正，此时天导位置误差由于修正作用而存在“跳变”现象。因此，本文在天导测星导航解算后的位置数据基础上，叠加周期性波动位置误差，模拟天导位置数据，并在第2.2 h处，叠加位置突变误差。
试验中惯导数据通过仿真生成，惯导仿真参数设置如下：陀螺零偏为0.003 °/h，加速度计零偏为10 μg，初始水平姿态误差为10′，初始航向误差为0.3′，惯导转动调制周期为240 s。
3.2 试验分析天导设备位置测星修正时存在“误差回降”现象。根据天导和惯导仿真数据，对比经典Kalman滤波和抗差Kalman滤波下的天导和惯导组合导航对惯导误差的估计效果。
1）经典 Kalman 滤波估计效果
采用基于经典Kalman的天文/惯性组合导航算法，航向、速度、位置、陀螺以及加速度计误差及其估计残差曲线如图2～图9所示。
|
图 2 航向误差估计曲线 Fig. 2 Heading error estimation curve |
|
图 3 航向误差估计残差曲线 Fig. 3 Residual curve of heading error estimation |
|
图 4 速度误差估计曲线 Fig. 4 Speed error estimation curve |
|
图 5 速度误差估计残差曲线 Fig. 5 Residual curve of velocity error estimation |
|
图 6 位置误差估计曲线 Fig. 6 Position error estimation curve |
|
图 7 位置误差估计残差曲线 Fig. 7 Position error estimation residual curve |
|
图 8 陀螺误差估计曲线 Fig. 8 Gyro error estimation curve |
图9中，第2.2 h处天导位置信息由于“误差回降”现象，导致天文/惯性组合后的航向误差、速度误差、位置误差和传感器误差均产生较大波动。其中，在“误差回降”后，惯导相对天导的航向误差、速度误差和位置误差可恢复至稳定高精度估计状态，而传感器误差估计效果被完全破坏。
|
图 9 加速度计误差估计曲线 Fig. 9 Accelerometer error estimation curve |
出现上述现象，主要是因为以天导位置作Kalman滤波量测信息时，位置量测信息“跳变”等效为量测信息叠加初始常值，从而影响了采用经典Kalam滤波进行天文/惯性组合时的滤波稳定性，导致滤波效果较差。
2）抗差Kalman滤波估计效果
采用基于抗差Kalman的天文/惯性组合导航算法，航向、速度、位置、陀螺以及加速度计误差及其估计残差曲线如图10～图17所示。
|
图 10 航向误差估计曲线 Fig. 10 Heading error estimation curve |
|
图 11 航向误差估计残差曲线 Fig. 11 Residual curve of heading error estimation |
|
图 12 速度误差估计曲线 Fig. 12 Speed error estimation curve |
|
图 13 速度误差估计残差曲线 Fig. 13 Residual curve of velocity error estimation |
|
图 14 位置误差估计曲线 Fig. 14 Position error estimation curve |
|
图 15 位置误差估计残差曲线 Fig. 15 Position error estimation residual curve |
|
图 16 陀螺误差估计曲线 Fig. 16 Gyro error estimation curve |
由图17可以看出，以天导位置信息作量测出现“跳变”时，采用基于抗差Kalman的天文/惯性组合导航方法可较好地估计惯导误差，且惯导相对天导位置误差接近于0，即惯导位置误差极限精度与天导位置误差一致。
|
图 17 加速度计误差估计曲线 Fig. 17 Accelerometet error estimation curve |
相比于经典Kalman滤波算法，抗差Kalman 滤波在处理包含粗差的数据时具备更加优良的鲁棒性，能给予更准确的变形估计，从而在天导位置量测信息出现“跳变”时，仍保持较好的估计效果。
4 结 语本文提出了一种基于抗差Kalman的天导/惯导组合导航技术。在天导位置量测信息出现“误差回降”现象时，基于经典Kalman滤波的天文/惯性组合导航方法对惯导误差的估计均产生较大波动，且传感器误差估计存在常值偏差。相比基于经典Kalman滤波的天文/惯性组合导航方法，本文方法对惯导误差的估计仍能保持较好的估计效果。本文大幅抑制了天导位置“误差回降”对惯导综合校正的不利影响，提升了天导综合校正惯导的效果，具有较强的工程应用价值。
| [1] |
周磊, 张锐, 樊建文. 单星跟踪的组合天文定位算法[J]. 导航定位学报, 2016, (2): 42−46. ZHOU Lei, ZHANG Rui, FAN Jian-wen. Acombined celestial positioning algorithm based on single star tracking[J]. Journal of Navigation and Positioning, 2016, (2): 42−46. |
| [2] |
王振华, 段滔, 李临, 等. 基于CCD星体跟踪器的舰船位置估计算法研究[J]. 计算机与数字工程, 2022, (5): 5−10. WANG Zhen-hua, DUAN Tao, LI Lin, et al. Research of ships position estimation algorithm nased on CCD star tracker[J].Computer and Digital Engineering, 2022, (5): 5−10. |
| [3] |
连月勇, 张超, 詹银虎, 等. 基于鱼眼相机的天文导航误差分析[C]// 第五届中国卫星导航学术年会论文集-S9 组合导航与导航新方法, 2014.
|
| [4] |
申功勋, 秦世光, 常庆之, 等. 天文-惯性组合式导航系统及星体跟踪器研制[J]. 航空学报, 1981, (1).
|
| [5] |
屈蔷. 机载捷联惯性/天文组合导航系统关键技术研究[D]. 南京: 南京航空航天大学, 2012.
|
| [6] |
He Yun-sheng, WANG qiang. A new autonomous celestial navigation method for deep space exploration based on square-root unscented kalman filter[C]// 中国宇航学会深空探测技术专业委员会第七届学术年会论文集. 2010.
|
| [7] |
周罡, 唐建博, 邹志. 一种高精度的天文导航姿态与航向解算模型[J]. 舰船科学技术, 2009, (5): 72−75. ZHOU Gang, TANG Jian-bo, ZOU Zhi. An high precision calculation model of attitude and course about celestia lnavigation[J]. Ship and Science and Technology, 2009, (5): 72−75. |
| [8] |
熊智, 刘建业, 郁丰, 等. 基于天文角度观测的机载惯性/天文组合滤波算法研究[J]. 宇航学报, 2010, (2).
|
| [9] |
黄远, 王可东, 刘宝. 机动天基平台惯性/天文导航组合模式研究[J]. 红外与激光工程, 2012, (6): 1622−1628. HUANG Yuan, WANG Ke-dong, LIU Bao. INS/CNS integration schemes for a maneuvering spacecraft[J]. Infrared and Laser Engineering, 2012, (6): 1622−1628. |
| [10] |
杨明喜. 关于惯性/天文组合导航系统算法的研究[D]. 厦门: 厦门大学, 2019.
|