张源 詹金林 韩冰 陈伟
摘要
为克服惯性导航系统（INS）的积累误差，提高误差的修正精度，提出了基于多天线北斗差分载波相位的北斗/惯性导航系统组合导航算法。该算法建立并线性化惯性导航系统（INS）和北斗导航系统（BDS）的状态方程和量测方程，对系统的运动状态参数应用自适应迭代扩展卡尔曼滤波（adaptive iterated extended Kakman filter，AIEKF）算法进行估计。仿真结果表明，自适应迭代扩展卡尔曼滤波算法能够提高INS/BDS组合导航系统的精度和抗干扰能力，验证了自适应迭代扩展卡尔曼滤波算法的有效性。
关键词
INS;BDS;组合导航;自适应卡尔曼滤波
中图分类号： U666.1 文献标识码： A
DOI：10.19694/j.cnki.issn2095-2457.2020.04.81
惯性导航系统（Inertial Navigation System， INS）和北斗卫星导航系统（Beidou Navigation Satellite System，BDS）是目前两种重要的舰船导航系统。惯性导航系统（INS）是自主导航系统，仅依靠自身就能进行连续的导航和定位，具有自主、隐蔽等特性，所获取舰船的运动信息完备，但其定位误差是积累的，随着时间的积累而不断增大[1]。北斗卫星导航系统（BDS）的定位精度系统与第3代GPS定位精度相当，具有观测时间短、定位连续、精度高、误差不随时间积累等优点，可提供覆盖全球的精准定位、导航和授时（Positioning， Navigation and Timing， PNT）服务[2]。由于两种系统所具备优势具有互补的特性，将两者结合起来构成更高精度的组合导航系统备受人们关注。通常将BDS导航信息和INS相应的导航信息进行比对，采用卡尔曼滤波器根据观测差值在线估计INS的各类误差并进行相应补偿。在以往文献中，大多将GPS输出的位移和速度信息输入到Kalman滤波器中，滤波输出的位移、速度和姿态信息又反馈回INS，修正INS的积累误差[3]。舰艇组合导航系统要求具备更高的系统精度和抗干扰能力，提高外部观测数据的信息数量和质量可以较好地提高INS各类误差的可观测性，而高精度北斗姿态测量系统可以提供载体的位置、速度、航向和姿态等信息。文中将高精度北斗姿态测量系统与INS进行组合，将BDS输出的位置、速度和姿态信息作为INS的外部修正信息，以获得综合性能更加优良的舰船组合导航系统。
标准的卡尔曼滤波器有一系列必要条件，这些条件包括：状态模型和量测模型均为线性，状态噪声和量测噪声均为零期望白噪声，状态噪声、量测噪声和状态三者之间不相关，初值或其统计特性（期望和协方差矩阵）已知。当满足这些条件时，滤波结果在无偏最小方差意义上是最优的;反之，如果不满足这些条件，滤波结果不是最优的。但在实际中，BDS的测量噪声会受到多种因素（如：多路径效应、内部噪声等）的影响发生变化，标准的卡尔曼滤波器由于其局限性，无法对上述的变化进行相应的检测和调整，导致量测噪声和状态噪声建模不准确，会出现严重的估计偏差。为解决此类问题，大量学者致力于自适应滤波算法的研究工作，通过实时调整模型参数和噪声统计特性优化算法的滤波精度。文献所报道的自适应滤波算法主要集中于基于新息估计的卡尔曼滤波器（IAE）[4]和多模型卡尔曼滤波器（MMAE）[5]设计。其中。基于新息估计的自适应滤波算法主要采用协方差匹配、输出相关技术和极大似然估计等思想[6]，如Sage-Husa自适应滤波算法可自适应噪声协方差矩阵[7]，Sasiadek根据新息的方差与均值自动调整加权扩展卡尔曼滤波器的权值[8]，Loebis通过诊断新息方差变化，对滤波器使用的系统噪声方差阵R的增量进行调整获取系统的稳定性能[5]，Zhang基于新息对系统噪声方差阵R进行系数调整[9]。仿真验证，上述方法不增加系统状态的维数，计算量低，同时获得较好的精度。
本文针对BDS观测误差较大变化的情况下，通过对实际新息的测量和计算，直接对卡尔曼滤波器的增益矩阵进行自适应修正，提高了滤波器的精度和鲁棒性。
1 自适应迭代扩展卡尔曼滤波算法（AIEKF）[10]
对于确定性系统，已知系统初始条件，通过求解系统的微分方程，就可以得到系统在未来任意时刻的准确状态。但实际中的系统在运行过程中必然受到各种干扰和噪声的影响，在其运行状态中产生各种误差。卡尔曼滤波器是组合导航系统数据处理中常用的最优估计算法，但只针对线性系统，要求状态方程和量测方程均为线性，采用状态空间法建立准确的状态方程和量测方程，在设定状态噪声与量测噪声均为零期望白噪声统计特性的基礎上，根据系统每一时刻的观测量，运用递推算法实现对系统状态的最优估计[10]。当组合导航的系统模型和观测模型确定时，解算精度较高。卡尔曼滤波器的增益矩阵K是调节加权权重的参数，其计算需要依赖于事先已知的状态转移矩阵Φ、量测矩阵H、系统噪声方差阵R和状态噪声协方差矩阵Q，并不依赖在线的实际观测量。在算法的实际运行中，状态转移矩阵Φ、量测矩阵H、状态噪声协方差矩阵Q和系统噪声方差阵R发生变化，K的计算必然会出现偏差，不再满足最优估计条件，滤波器将发散。有效提高K值在滤波过程的修正能力，是解决这一问题的根本途径。
新息是指滤波器的测量估计值与实际测量值之间的差值。当滤波器工作正常时，理想状态下的新息应为零均值白噪声序列。当状态估计由于建模误差而导致估计误差增大时，导致新息增大且统计特性复杂，此时，基于新息相应调整滤波器的增益、量测噪声和观测噪声的大小，可能会抑制状态估计误差的持续增大，提高状态估计的精度。
对于离散线性系统模型，其状态方程和量测方程如下：
式中，Xk为状态估计;Wk为系统噪声序列;Vk为量测噪声序列;Φk，k-1为tk-1时刻至tk时刻的一步转移矩阵;Гk，k-1为系统噪声驱动矩阵;Hk为量测矩阵。且Wk和Vk互不相关并满足：E[Wk]=0，Cov[Wk，Wj]=QKδkj，E[Vk]=0，Cov[Vk，Vj]=RKδkj。以新息方式列写标准卡尔曼滤波算法，如式（1）～（5）所示。
2 BDS/INS组合导航系统及系统模型
2.1 组合系统总体设计
INS/BDS组合导航系统以惯性导航系统（INS）作为组合导航系统的关键子系统，而北斗卫星导航系统（BDS）作为辅助子系统。通过组合，使用BDS输出的位置、速度和姿态等信息来修正陀螺漂移、加速度计偏差和初始失准角引起的INS位置、速度和姿态的误差，从而获得高精度的导航信息。图1为INS/BDS组合导航系统总体设计图，INS和BDS之间采取松散耦合方式进行组合，两个子系统独立工作，各自输出导航参数。这种组合方式采取位置、速度和姿态组合，将INS的误差方程作为组合导航系统的状态方程，将BDS和INS各自输出的位置、速度和姿态之差作为观测量，采取自适应迭代扩展卡尔曼滤波器对INS的位置误差、速度误差、姿态误差及惯性元器件误差进行最优估计，然后对INS进行输出反馈校正。在系统中，北斗导航姿态测量模块根据多天线北斗载波观测数据，建立北斗双差载波相位观测模型，利用系统输出信息辅助整周模糊度解算、周跳探测与修复，最终解算出整周模糊度，解算出高精度的舰船姿态信息。
这种组合方式的优点是可以估计出组合导航系统的速度误差和位置误差，由于BDS提供INS修正所必需的位置、速度、航向和纵摇6种信息，可以较好地提高INS误差状态的可观测性和可观测度。通过补偿能够大幅度地提升系统的定位精度。此外，该系统组合方式原理简单，工程实现容易，子系统独立工作，并具有系统冗余，在实际组合导航系统中应用较多。但是，在系统的实际应用中，多种因素会导致BDS的测量误差不稳定，所以相关的卡尔曼滤波器必须采取相关算法抑制BDS观测噪声增大的影响。本文中提出的AIE自适应算法对本组合系统效果进行改善。
2.2 系统误差模型
2.2.1 惯性导航系统系统误差模型
系统误差模型选取半解析式当地水平指北平台式惯性导航系统，采用东北天坐标系。选用定位误差、速度误差、平台误差角、陀螺漂移和加速度漂移误差作为惯性导航系统的状态变量。
式中：δL，δ？姿为惯导系统在纬度和经度上的定位误差;δve，δvn为惯导系统的东向速度误差和北向速度误差;φe，φn，φu为平台的东向、北向和方位误差角;？荦e，？荦n为东向和北向加速度计漂移误差;εe，εn，εu为东向、北向和方位陀螺仪漂移。
BINS=[02×12;05×7 I5×5;05×12]T，WINS=[01×7 W W W W W ]T，WINS中，W ，W 分别为东向、北向加速度计随机偏差，W W W 分别为东、北向和方位陀螺随机漂移，以上各值均设定为零均值、方差为QINS的白噪声。FINS为12×12的矩阵，如式（12），式中L为舰船所处位置的纬度，g为当地重力加速度;RM、RN分别为地球卯酉圈和子午圈半径;ωie为地球自转角速度。
2.2.2 组合系统量测方程
设定组合导航系统状态模型与惯导系统的误差模型一致，令惯性导航系统输出的载体姿态角的测量值分别为ψI=ψ+δψI、θI=θ+δθI、？酌I=？酌+δ？酌I，令BDS测量的姿态角分别为ψB=ψ+δψB、θB=θ+δθB;考虑到当前组合导航系统获得的INS的位置速度是由计算机运算数字发送实现，而舰船姿态角则依靠平台高精度的旋转变压器测量并通过高精度的数字固态发送实现，上述传输过程中的误差与INS的系统误差相比较小。所以为了简化对准时刻的系统观测方程，忽略INS的测量噪声，而将BDS姿态测量系统的各种误差归为组合系统的量测噪声。根据试验分析，假定VBDS是零均值，方差强度可变的BDS测量白噪声，RBDS为其正常情况下的噪声协方差。VBDS，WINS互不相关。得到组合导航系统的量测方程如下所示。
令载体坐标系为b系，地理坐标系为t系，平台坐标系为p系，设载体的航向角、俯仰角和横滚角分别为ψ、θ和？酌，由坐标变换理论可知C =C ×C ，得：
（15）
3 仿真结果分析
为验证方法的精度和鲁棒性，对INS/BDS组合导航系统进行计算机仿真测试，仿真测试工具采用MATLAB，仿真时间为3600s，采样时间为1s。
仿真条件如下所示：舰船航向ψ=45°，θ=1°，？酌=1°，Ve=Vn=5m/s，φ=32°N;INS参数如下：φe=φn=2'，φu=5'，？荦e=？荦n=（1×10-5）g，δφ0=δλ0=0.05"，δVe=δVn=0.1m/s，εe=εn=εu=（1×10-3）（°）/h;INS系統噪声方差（QINS）为：δ =δ =（1×10 g） ，δ =δ =δ =（5×10 （°）/h） ，δ =δ =（0.2m/s） ;BDS量测噪声方差（RBDS）为δ =（3'）2，δ =（30"）2，δ =δ =（0.01"）2，δ =δ =（0.2m/s）2。
对于标准卡尔曼滤波器，预先设定系统噪声，计算INS的各项误差，INS误差估计效果仿真曲线如图2、图3所示中灰线所示。而对于自适应迭代扩展卡尔曼滤波（AIEKF），根据sage开窗估计法，设定记忆窗口长度N=20，系统工作100秒之后，切换到新息自适应控制阶段，根据AIEKF算法适时改进卡尔曼增益K的计算，仿真曲线如INS误差估计效果仿真曲线如图2、图3所示中黑线所示。
从仿真结果对比中可以看出：标准的卡尔曼滤波器在BDS量测噪声发生变化时，由于滤波器鲁棒性较差，无法准确跟踪系统状态变化，系统噪声方差矩阵R无法适时调整，导致滤波器输出的位置误差、速度误差、平台误差角以及元件误差等各参数出现较大偏差。在同样的仿真条件下，当外部量测噪声变大时，AIEKF算法通过对新息计算，自适应调整滤波器的R值，使增益K自动修正，减弱了量测噪声对滤波值的影响，表现出较高的精度和抗干扰性。
从曲线中，我们可以看出采用了BDS姿态测量系统的INS/BDS组合导航系统可以较好地对各个参数进行相应估计，所以INS/BDS组合系统无论在初始对准还是动态组合条件下的INS参数误差能力都比仅采取位置或速度的系统优越。
4 结论
本文将AIEKF算法应用于INS/BDS舰艇用组合导航系统，通过对新息计算，自动根据量测噪声变化调节增益K，减弱了量测噪声对滤波值的影响。组合导航需要采取高精度BDS姿态测量系统为INS提供外部修正信息，将载体姿态信息引入系统，增加了系统的观测量种类，能够较好地克服由于BDS量测噪声的不稳定特性造成系统卡尔曼滤波器性能下降。仿真结果表明，相较于Kalman滤波器，AIEKF算法较好地提高了系统的精度和鲁棒性。
参考文献
[1]汤郡郡，胡伟，刘祥水，等.基于SINS/TAN/ADS/MCP的无人机组合导航系统[J].中国惯性技术学报，2018，26（1）：33-38.
TANG Junjun，HU Wei，LIU Xiangshui，et al.Unmanned aerial vehicle integrated navigation system based on SINS/TAN/ADS/MCP[J].Journal of Chinese Inertial Technology，2018，26（1）;33-38.
[2]蔡煊，王长林.基于抗差估计的BDS/ODO 组合列车定位方法[J].铁道科学与工程学报，2018.15（10）：2654-2660.
CAI Xuan， WAN Changlin，BDS/ODO integrated train positioning method based on robust estimation[J]. Journal of Railway Science and Engineering，2018.15（10） ：2654-2660.
[3]万德钧，房建成. 惯性导航初始对准.东南大学出版社P160-165.1998.10.
[4]A.H. Mohamed， K.P. Schwarz. Adaptive kalman filtering for INS/GPS. Journal of Geodesy， Page 193-203， 1999.
[5]D.Loebis， R. Sutton， J. Chudley， W. Naeem. Adaptive tuning of a kalman filter via fuzzy logic for an intelligent AUV navigation system， Page1-7， 2004.
[6]姜浩楠，蔡远利.带有噪声递推估计的自适应集合卡尔曼滤波[J].控制与决策，2018，33（9）：1567-1574.
JIANG Hao-nan，CAI Yuan-li.Adaptive ensemble Kalman filter with recursive noise estimation[J].Control and Decision， 2018，33（9）：1567-1574.
[7]付心如，孙伟，徐爱功，等.组合导航抗差自适应卡尔曼滤波[J].测绘科学，2018.43（1）：6-10.
FU Xinru，SUN Wei，XU Aigong，et al.Research on robust Kakman filter for integrated navigatuin[J]. Science of Surveying and Mapping[J]，2018.43（1）：6-10.
[8]J.Z. Sasiadek. Sensor fusion. Page203-228， Annual review in control， 2002.
[9]San-tong Zhang， Xue-ye Wei. Fuzzy adaptive kalman filtering for DR/GPS， Proceeding of the second international conference on machine learning and cybernetics， Xian， 2-5 November 2003， p2634-2637.
[10] 卞鸿巍， 金志华，王俊璞，等.组合导航系统新息自适应卡尔曼滤波算法[J]. 上海交通大学学报，2006，40（6）：1009-1014.
BIAN Hongwei，JIN Zhihua，WANG Junpu，et al. The Innovation-based Estimation Adaptive Kalman Filter Algorithm for INS/GPS Integrated Navigation System[J]. Journal of Shanghai Jiaotong University，2006，40（6）：1009-1014.
[11]R.E. Kalman. A new approach to linear filtering and prediction problems. Journal of Basic Engineering， Pages 35-46， 1960.
[12] 楊艳娟，卞鸿巍，田蔚风，金志华. 一种新的INS/GPS组合导航技术[J].中国惯性技术学报.2004.12（2）：23-26.
YANG Yan-juan，BIAN Hong-wei，TIAN Wei-feng，et al.A New INS/BDS Integrated Navigation Technique[J].Journal of Chinese Inertial Technology.2004.12（2）：23-26.
