CN109556631B - 一种基于最小二乘的ins/gnss/偏振/地磁组合导航系统对准方法 - Google Patents

一种基于最小二乘的ins/gnss/偏振/地磁组合导航系统对准方法 Download PDF

Info

Publication number
CN109556631B
CN109556631B CN201811414200.6A CN201811414200A CN109556631B CN 109556631 B CN109556631 B CN 109556631B CN 201811414200 A CN201811414200 A CN 201811414200A CN 109556631 B CN109556631 B CN 109556631B
Authority
CN
China
Prior art keywords
ins
polarization
gnss
navigation system
measurement
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201811414200.6A
Other languages
English (en)
Other versions
CN109556631A (zh
Inventor
杜涛
田常正
星艳
曾云豪
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
North China University of Technology
Original Assignee
North China University of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by North China University of Technology filed Critical North China University of Technology
Priority to CN201811414200.6A priority Critical patent/CN109556631B/zh
Publication of CN109556631A publication Critical patent/CN109556631A/zh
Application granted granted Critical
Publication of CN109556631B publication Critical patent/CN109556631B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/18—Stabilised platforms, e.g. by gyroscope

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manufacturing & Machinery (AREA)
  • Navigation (AREA)
  • Measuring Magnetic Variables (AREA)

Abstract

本发明涉及一种基于最小二乘的INS/GNSS/偏振/地磁组合导航系统对准方法：选择初始对准的状态变量；根据偏振传感器输出的偏振方位角与INS输出的方位角匹配融合建立偏振量测方程；根据磁强计输出的航向角与INS输出的航向角建立地磁量测方程；根据GNSS输出的速度、位置信息与INS输出的速度、位置信息匹配融合建立误差量测方程；利用增广技术建立统一的INS/GNSS/偏振/地磁组合导航系统量测方程；利用最小二乘技术估计INS/GNSS/偏振/地磁组合导航系统的失准角、速度误差以及位置误差等物理量；对INS/GNSS/偏振/地磁组合导航系统姿态、速度及位置进行反馈校正，提高初始对准估计精度。

Description

一种基于最小二乘的INS/GNSS/偏振/地磁组合导航系统对准 方法
技术领域
本发明涉及一种基于最小二乘的INS/GNSS/偏振/地磁组合导航系统对准方法，可用于提高地球大气层内运载体(车辆、舰船和飞行器)的动基座初始对准精度，缩短初始对准时间。
背景技术
在进入导航状态前，导航系统需要进行初始对准，通过初始对准得知初始导航参数，包括姿态和速度。在动基座条件下，载体在运动过程中存在晃动、振动等多种形式的干扰。这些干扰因素会降低导航系统初始对准精度，延长对准时间。因此，动基座初始对准方法一直是组合导航技术领域的难点和热点问题。
目前，动基座初始对准方法主要包括两大类：一类是基于主/子惯性导航系统的传递对准方法；另一类是惯性导航系统与卫星、磁罗盘、星敏感器等导航设备进行组合初始对准方法。传递对准方法中，由于导弹载体存在结构挠性变形、主惯性导航系统与子惯性导航系统之间存在数据传输延迟等因素影响，初始对准精度及时间往往不能得到保证。而惯性导航系统与卫星、磁罗盘和星敏感器等导航设备进行组合初始对准方法，同样存在问题：其中，卫星在载体作机动时误差较大，且与惯性导航系统信号同步性不好；磁罗盘依赖于地磁场模型精度，特别是在地磁场异常的情况下，将出现较大误差，因此用于动基座初始对准效果并不理想；星敏感器可高精度估计载体姿态误差，但并不适用于在地球大气层内白昼时间段内的动基座初始对准。基于上述原因，惯性导航系统结合新型导航装置，设计更合适的信息匹配方式是动基座初始对准技术的发展趋势之一。
偏振光导航是一种仿生导航方式。太阳光线入射大气层时，大气中的粒子会使入射光线散射，造成太阳光线的偏振现象，沙蚁等生物可利用天空中的偏振光进行导航。根据仿生导航原理设计的偏振光传感器，可敏感到传感器量测方向的入射光最大偏振方向，输出偏振方位角。基于偏振方位角的偏振光导航具有自主性强、误差不随时间积累等优点，尤其适用于高精度的姿态确定。但偏振方位角对定位误差极不敏感，可达数十米。惯性导航系统的误差随时间积累，位置信息是发散的。因此偏振光传感器仅与惯性导航系统组合进行动基座初始对准会造成精度不高，甚至可能出现滤波结果发散的情况。
综上，目前常用的动基座初始对准方法存在对准精度低、对准时间长等缺点，而偏振光传感器、磁传感器对位置误差极不敏感，仅与惯性导航系统组合进行动基座初始对准效果同样不好。
现有技术中，与专利号为201310069511.4“一种SINS/GPS/偏振光组合导航系统的动基座初始对准方法”的发明专利相比，本发明有以下优点：
(1)所建立的组合导航系统模型不同，本发明加入新的传感器磁强计，通过融合新的传感器数据，可进一步提高初始对准的精度；
(2)所建立的偏振机理模型不同，本发明获得的偏振量测方程与专利号为201310069511.4“一种SINS/GPS/偏振光组合导航系统的动基座初始对准方法”的发明专利完全不同，后者所建立的偏振机理模型采用单个偏振光传感器进行构建，建立的模型复杂，而且存在除法，导致噪声不再满足高斯分布，在状态估计阶段会存在问题；本发明采用双偏振光传感器的设计，构建的模型简单，解决了单一传感器噪声分布不再是高斯分布的问题；
(3)采用的滤波方法不同，本发明采用最小二乘方法进行状态估计，该方法通过最小化误差的平方和寻找数据的最佳函数匹配，在工程中易于实现。
发明内容
本发明的技术解决问题是：克服现有方法的不足，提供一种精度高、对准快、多传感器融合的基于最小二乘的INS/GNSS/偏振/地磁组合导航系统对准方法。
本发明的技术解决方案为：一种基于最小二乘的INS/GNSS/偏振/地磁组合导航系统对准方法，其实现步骤如下：
(1)选择失准角、速度误差、位置误差、陀螺仪零偏、加速度计零偏以及磁航向角误差作为INS/GNSS/偏振/地磁组合导航系统的状态变量，其中，INS(Inertial NavigationSystem)为惯性导航系统，GNSS(Global Navigation Satellite System)为全球导航卫星系统；
(2)根据偏振传感器输出的偏振方位角与INS输出的方位角匹配融合建立偏振量测方程，得到基于偏振方位角的量测方程；
(3)根据磁强计输出的航向角与INS输出的航向角建立地磁量测方程，得到基于磁强计航向角误差的量测方程；
(4)根据GNSS输出的速度、位置信息与INS输出的速度、位置信息匹配融合建立速度误差与位置误差量测方程，得到基于速度误差与位置误差的量测方程；
(5)在步骤(2)、(3)和(4)建立的量测方程基础上，利用增广技术将上述3个步骤中的量测方程进行向量化，建立统一的INS/GNSS/偏振/地磁组合导航系统量测方程；
(6)根据步骤(5)建立的统一的INS/GNSS/偏振/地磁组合导航系统量测方程，利用最小二乘技术估计INS/GNSS/偏振/地磁组合导航系统未知的状态；
(7)根据步骤(6)中估计得到的失准角、速度误差以及位置误差对INS/GNSS/偏振/地磁组合导航系统进行反馈校正，方法为基于INS获取的姿态、速度及位置上减去步骤(6)中估计得到的失准角、速度误差及位置误差，获取经反馈校正后的INS/GNSS/偏振/地磁组合导航系统姿态、速度及位置。
所述步骤(2)中，根据偏振传感器输出的偏振方位角与INS输出的方位角匹配融合建立偏振量测方程为：
Zpolar＝H1X+υpolar
其中，
Figure BDA0001879080270000041
为INS/GNSS/偏振/地磁组合导航系统16维状态变量，φE，φN，φU分别为导航系，即n系下的东向失准角、北向失准角和天向失准角；δVE，δVN，δVU分别为导航系下的东向、北向和天向速度误差，这里导航系采用当地地理坐标系，即以载体的质心为原点，当地东向为地理系x轴，北向为y轴，天向为z轴，x轴，y轴和z轴满足右手坐标系。δλ，δL，δh分别为经度、纬度和高度误差；εx，εy，εz为载体坐标系，即b系下三个轴向的陀螺常值漂移，这里b系定义为，其原点O与载体质心重合，x轴正向与载体正向同向，y轴沿载体纵轴指左，z轴垂直xOy构成的平面指上，坐标系满足右手坐标系；
Figure BDA0001879080270000042
分别为b系下加速度常值漂移；
Figure BDA0001879080270000043
为磁强计的航向角测量误差；υpolar为偏振传感器的量测噪声；zpolar为偏振量测量，表示为：
Figure BDA0001879080270000044
其中，sm为单位太阳矢量在模块系m系下的表示，模块系为偏振传感器模块所在的坐标系，定义与载体坐标系基本相同，原点为模块质心，一般情况下与载体坐标系重合；
Figure BDA0001879080270000045
为计算导航系n′系到b系下的姿态转换矩阵，计算导航系n′系为实际导航过程用于计算导航解的导航坐标系，坐标原点与导航系统相同，x轴、y轴和z轴与理想导航系n系存在偏差角，该偏差角即为失准角；
Figure BDA0001879080270000046
为地球系e系到理想导航系n系下的姿态转换矩阵，地球系原点与地心重合，xe轴指向本初子午线，ze轴沿极轴方向指向北极，ye轴在赤道平面内与xe、ze轴构成右手系，理想导航系即为“东北天”坐标系，其原点为载体所在点，N轴指向北极方向，E轴指向东，U轴指向天空，满足右手坐标系；se为单位太阳矢量在e系下的表示。
H1为以偏振方位角的量测矩阵，具体表示为：
Figure BDA0001879080270000051
其中，
Figure BDA0001879080270000052
表示矢量
Figure BDA0001879080270000053
的反对称阵形式。
所述步骤(3)中，根据磁强计输出的航向角与INS输出的航向角建立地磁量测方程为：
zmag＝H2X+υmag
其中，H2＝[01×15 1]为基于磁强计航向角的量测矩阵，维度为1×16；zmag为磁强计误差量测量；υmag为磁强计量测噪声。
所述步骤(4)中，根据GNSS输出的速度、位置信息与INS输出的速度、位置信息匹配融合建立速度误差与位置误差量测方程为：
Figure BDA0001879080270000054
其中，zv，zp分别融合了INS与GNSS速度、位置信息的速度误差与位置误差的量测量；H3＝[03×3 I3×3 03×10]为速度误差量测矩阵，维度为3×16，I3×3为3×3的单位矩阵；H4＝[03×5 I3×3 03×8]为位置误差量测矩阵，维度为3×16；
Figure BDA0001879080270000055
为速度误差量测噪声向量，
Figure BDA0001879080270000056
分别为东北天三个方向的速度噪声；υp＝[υλ υL υh]T为位置误差量测噪声，υλ、υL、υh分别为经度、纬度及高度位置量测噪声；
所述步骤(5)中，在步骤(2)、(3)和(4)建立的量测方程基础上，利用增广技术将上述3个步骤中的量测方程进行向量化，建立统一的INS/GNSS/偏振/地磁组合导航系统量测方程为：
Z＝HX+V
其中，H为INS/GNSS/偏振/地磁组合导航系统统一的量测矩阵，具体表示为：
Figure BDA0001879080270000061
Z为全部量测向量，具体表示为：
Figure BDA0001879080270000062
其中，vGNSS为GNSS测量的速度矢量，vINS为INS测量的速度矢量；pGNSS为GNSS测量的位置矢量，pINS为INS测量的位置矢量。
V为总量测噪声，具体表示为：
Figure BDA0001879080270000063
所述步骤(6)中，根据步骤(5)建立的统一的INS/GNSS/偏振/地磁组合导航系统量测方程，利用最小二乘技术估计INS/GNSS/偏振/地磁组合导航系统未知的状态，所设计的最小二乘滤波内容如下：
第n次测量后INS/GNSS/偏振/地磁组合导航系统量测方程为：
Zn＝HnX+Vn
其中，Zn为第n次量测的测量值，Hn为第n次INS/GNSS/偏振/地磁组合导航系统量测矩阵，其加权最小二乘估计为：
Figure BDA0001879080270000064
其中，Rn为第n次INS/GNSS/偏振/地磁组合导航系统测量噪声矩阵，
Figure BDA0001879080270000065
为第n次估计值；
估计误差方差为：
Figure BDA0001879080270000066
其中，Pn为误差方差矩阵，
Figure BDA0001879080270000067
为第n次测量真实值X与估计值
Figure BDA0001879080270000068
之差；
综合上述两式，得：
Figure BDA0001879080270000071
当得到一个新的测量Zn+1时，有：
Zn+1＝Hn+1X+Vn+1
新的加权最小二乘估计为：
Figure BDA0001879080270000072
把新的测量噪声加入原本的测量噪声矩阵中，此时噪声矩阵的逆矩阵
Figure BDA0001879080270000073
应为对角矩阵：
Figure BDA0001879080270000074
记
Figure BDA0001879080270000075
展开
Figure BDA0001879080270000076
得：
Figure BDA0001879080270000077
此时，n+1时刻误差方差矩阵为：
Figure BDA0001879080270000078
令中间量
Figure BDA0001879080270000079
则n+1时刻状态估计值为：
Figure BDA00018790802700000710
所述步骤(7)中，根据步骤(6)中估计得到的失准角、速度误差以及位置误差对INS/GNSS/偏振/地磁组合导航系统进行反馈校正，其方法为：
(1)姿态校正方法
首先计算n′系至n系的转换矩阵：
Figure BDA00018790802700000711
则校正后的姿态矩阵
Figure BDA00018790802700000712
为：
Figure BDA00018790802700000713
(2)速度校正方法
设校正后的东向、北向和天向速度为VadE、VadN、
Figure BDA00018790802700000714
则速度校正表示为：
Figure BDA0001879080270000081
其中，
Figure BDA0001879080270000082
分别为INS输出的东向、北向和天向速度；
(3)位置校正方法
设校正后的纬度、经度和高度分别为Lad、λad、had，则位置校正表示为：
Figure BDA0001879080270000083
其中，LINS、λINS、hINS分别为INS输出的纬度、经度和高度信息。
本发明与现有技术相比的优点在于：本发明将偏振光传感器、磁强计与INS、GNSS相结合，融合了多种传感器，使数据量变得丰富，可提高载体初始对准的精度。其中，偏振光传感器输出的偏振方位角不随时间积累，且具有高度自主性，具备高精度姿态校正能力；磁强计具有简单、快速、自主性好等优点，并且其功耗低，产生磁航向角可与偏振方位角中的航向角相冗，提高系统鲁棒性；GNSS提供速度、位置信息，弥补了偏振方位角信息对位置误差不敏感及惯性导航系统下速度、位置误差随时间积累并发散的缺点，缩短对准时间。
附图说明
图1为本发明的设计流程图；
图2为双偏振传感器坐标关系图。
具体实施方式
下面结合附图及实施例对本发明进行详细说明。
如图1所示，本发明具体实施步骤如下：
第一步，选择INS/GNSS/偏振/地磁组合导航系统的状态向量：
Figure BDA0001879080270000084
其中，φE，φN，φU分别为东向、北向和天向失准角；δVE，δVN，δVU分别为东向、北向和天向速度误差；δλ，δL，δh分别为经度、纬度和高度误差；εx，εy，εz为载体坐标系即b系下陀螺常值漂移，载体坐标系原点O与载体质心重合，x轴正向与载体正向同向，y轴沿载体纵轴指左，z轴垂直xOy构成的平面指上，整个坐标系满足右手坐标系；
Figure BDA0001879080270000091
分别为b系下加速度常值漂移；
Figure BDA0001879080270000092
为磁强计的航向角测量误差。
第二步，建立以偏振方位角为观测量的量测方程：
首先，根据偏振方位角定义可知，瑞利散射后的产生的偏振光与太阳矢量在模块系下的投影垂直，故有：
Figure BDA0001879080270000093
其中，
Figure BDA0001879080270000094
为偏振方位角，可由标定好的六通道偏振传感器计算出来：
Figure BDA0001879080270000095
其中，
Figure BDA0001879080270000096
为标定后的传感器参数，包括安装误差和尺度因子等。
Figure BDA0001879080270000097
Figure BDA0001879080270000098
为基于六通道仿生偏振传感器的对数处理后的量测输出。
综合式(2)、(3)，可得：
Figure BDA0001879080270000099
然后，确定太阳矢量在模块系与地球系下的坐标转换关系。太阳矢量在模块系下的向量sm表示为：
Figure BDA00018790802700000910
同样，太阳矢量在地球系下的向量se表示为：
Figure BDA00018790802700000911
其中，λe和ψe为地球系下的高度角和方位角。根据天文学可知，在已知载体所处地理位置及当地时间的情况下，可计算出上述两个角度。在INS/GNSS/偏振/地磁组合导航系统中，偏振光传感器所需的时间信息由GNSS提供，位置信息由经过校正后的惯性导航系统位置信息提供。
因为(5)、(6)为太阳矢量在不同坐标系下的描述，故可建立坐标转换：
Figure BDA0001879080270000101
其中，
Figure BDA0001879080270000102
为载体坐标系b系与模块系m系的转换关系，即为偏振传感器和载体之间的安装关系，为固定常值，可在对准前标定补偿，这里假定其为单位矩阵，载体坐标系原点O与载体质心重合，x轴正向与载体正向同向，y轴沿载体纵轴指左，z轴垂直xOy构成的平面指上，整个坐标系满足右手坐标系，模块系为偏振传感器模块所在的坐标系，原点为模块质心，定义与载体坐标系相同。
Figure BDA0001879080270000103
为地球系e系到理想导航系n系下的姿态转换矩阵，地球系原点与地心重合，xe轴指向本初子午线，ze轴沿极轴方向指向北极，ye轴在赤道平面内与xe、ze轴构成右手系，理想导航系即为“东北天”坐标系，其原点为载体所在点，N轴指向北极方向，E轴指向东，U轴指向天空，满足右手坐标系。
Figure BDA0001879080270000104
可根据当地地理纬度L和经度λ计算而得，具体如下：
Figure BDA0001879080270000105
姿态矩阵
Figure BDA0001879080270000106
实际上存在计算误差，这里用n′表示计算导航系，n系和n′系之间存在的姿态误差实际上就是失准角。针对小失准角情形时，可得：
Figure BDA0001879080270000107
其中，φ×为失准角的反对称矩阵形式：
Figure BDA0001879080270000108
其中，φi(i＝E,N,U)分别为东向、北向和天向失准角。
联立式(7)、(9)，并且令
Figure BDA0001879080270000109
可得：
Figure BDA00018790802700001010
针对式(11)，令等式右侧只与失准角有关，则有：
Figure BDA00018790802700001011
若A，B均为3×1向量，根据反对称阵叉乘性质有：
A×B＝-B×A (13)
根据上述性质，式(12)可写为：
Figure BDA0001879080270000111
即：
Figure BDA0001879080270000112
然后，考虑两组互相垂直朝向的偏振传感器，如图2所示：P1、P2分别为两垂直布置的偏振光传感器，α0表示其安装角度，这里
Figure BDA0001879080270000113
S为太阳矢量点；θi(i＝1,2)为太阳光射入第i个传感器的散射角；φi(i＝1,2)为第i个传感器测得的偏振方位角；
Figure BDA0001879080270000114
为相对于第i个传感器的太阳方位角。
记球面角
Figure BDA0001879080270000115
如图所示，根据球面三角形ΔP1P2S的正弦定理和余弦定理可得：
Figure BDA0001879080270000116
Figure BDA0001879080270000117
其中，
Figure BDA0001879080270000118
根据上述两式计算可得：
Figure BDA0001879080270000119
下面有两组偏振传感器在各自的模块系下可计算得到φ1和φ2，根据太阳矢量在模块系下的不同位置可以给出偏振方位角与球面角的计算关系式，可得：
Figure BDA00018790802700001110
考虑到太阳高度角和散射角的关系可得在模块系下的太阳高度角为：
Figure BDA0001879080270000121
记偏振方位角为
Figure BDA0001879080270000122
则可得以偏振方位角为观测量的量测方程：
zpolar＝H1X+υpolar (21)
其中，zpolar为偏振光传感器输出的偏振方位角；υpolar为偏振光传感器的量测噪声；H1为以偏振方位角的量测矩阵，具体表示为：
Figure BDA0001879080270000123
其中，
Figure BDA0001879080270000124
表示矢量
Figure BDA0001879080270000125
的反对称阵形式。
第三步，建立以磁强计航向角测量误差为观测量的量测方程。
首先对磁强计航向角量测误差进行分析。当系统固定在载体上后，航向角测量结果会受到周围铁磁物质的影响，干扰磁场由硬铁磁场和软铁磁场两部分组成，产生罗差。通常罗差的表现形式有由硬铁磁场产生的半圆罗差，由软铁磁场产生的圆周罗差和象限罗差，可建立罗差方程：
δφM＝A+BsinφM+CcosφM+Dsin2φM+Ecos2φM (23)
φ＝φM-δφM (24)
其中，δφM为罗差；φ为载体的实际航向；φM为传感器指示的航向；A为圆周落差；BsinφM+CcosφM为半圆罗差；Dsin2φM+Ecos2φM为象限罗差；另外，传感器的安装误差也可归于圆周罗差中。
由罗差原理分析及磁航向罗差公式可知，进行磁航向补偿即是要针对具体的环境测出磁航向误差公式中的误差系数A，B，C，D和E。基于最小二乘法的计算步骤为：首先，采用8位置误差测量方法，即在航向角360度范围内，取角距相隔45度的8个试验点进行磁航向误差测试，以此获得8组数据；然后，根据最小二乘原理，求出罗差系数A，B，C，D和E；最后，在得到补偿系数后，进行磁航向补偿运算，这样就可以得到较精确的航向角。
在得到航向角测量误差之后，建立以磁强计航向角测量误差为观测量的方程：
zmag＝H2X+υmag (25)
其中，H2＝[01×15 1]为基于磁强计航向角的量测矩阵，维度为1×16；zmag为磁强计误差量测量；υmag为磁强计量测噪声。
第四步，建立以速度误差、位置误差为观测量的量测方程。
Figure BDA0001879080270000131
其中，zv，zp分别融合了INS与GNSS速度、位置信息的速度误差与位置误差的量测量；H3＝[03×3 I3×3 03×10]为速度误差量测矩阵，维度为3×16，I3×3为3×3的单位矩阵；H4＝[03×5 I3×3 03×8]为位置误差量测矩阵，维度为3×16；
Figure BDA0001879080270000132
为速度误差量测噪声向量，
Figure BDA0001879080270000133
分别为东北天三个方向的速度噪声；υp＝[υλ υL υh]T为位置误差量测噪声，υλ、υL、υh分别为经度、纬度及高度位置量测噪声。
第五步，在步骤二、三和四建立的量测方程基础上，利用增广技术将上述3个步骤中的量测方程进行向量化，建立统一的INS/GNSS/偏振/地磁组合导航系统量测方程：
Z＝HX+V (27)
其中，H为INS/GNSS/偏振/地磁组合导航系统统一的量测矩阵，具体表示为：
Figure BDA0001879080270000134
Z为全部量测向量，具体表示为：
Figure BDA0001879080270000135
其中，vGNSS为GNSS测量的速度矢量，vINS为INS测量的速度矢量；pGNSS为GNSS测量的位置矢量，pINS为INS测量的位置矢量。
V为全部量测噪声，具体表示为：
Figure BDA0001879080270000141
第六步，根据第五步建立的统一的INS/GNSS/偏振/地磁组合导航系统量测方程，利用最小二乘技术估计INS/GNSS/偏振/地磁组合导航系统未知的状态：
第n次测量后INS/GNSS/偏振/地磁组合导航系统量测方程为：
Zn＝HnX+Vn (31)
其中，Zn为第n次量测的测量值，Hn为第n次INS/GNSS/偏振/地磁组合导航系统量测矩阵，其加权最小二乘估计为：
Figure BDA0001879080270000142
其中，Rn为第n次INS/GNSS/偏振/地磁组合导航系统测量噪声矩阵，
Figure BDA0001879080270000143
为第n次估计值；
估计误差方差为：
Figure BDA0001879080270000144
其中，Pn为误差方差矩阵，
Figure BDA0001879080270000145
为第n次测量真实值X与估计值
Figure BDA0001879080270000146
之差；
综合上述两式，得：
Figure BDA0001879080270000147
当得到一个新的测量Zn+1时，有：
Zn+1＝Hn+1X+Vn+1 (35)
新的加权最小二乘估计为：
Figure BDA0001879080270000148
把新的测量噪声加入原本的测量噪声矩阵中，此时噪声矩阵的逆矩阵
Figure BDA0001879080270000149
应为对角矩阵：
Figure BDA00018790802700001410
记
Figure BDA0001879080270000151
展开
Figure BDA0001879080270000152
得：
Figure BDA0001879080270000153
此时，n+1时刻误差方差矩阵为：
Figure BDA0001879080270000154
令中间量
Figure BDA0001879080270000155
则n+1时刻状态估计值为：
Figure BDA0001879080270000156
第七步，根据步骤六中估计得到的失准角、速度误差以及位置误差对INS/GNSS/偏振/地磁组合导航系统进行反馈校正，设最小二乘法估计的INS/GNSS/偏振/地磁组合导航系统状态变量为：
Figure BDA0001879080270000157
则姿态校正、速度校正以及位置校正方法如下所述：
(1)姿态校正方法
首先计算平台坐标系至真实地理坐标系的转换矩阵：
Figure BDA0001879080270000158
则校正后的姿态矩阵
Figure BDA0001879080270000159
为：
Figure BDA00018790802700001510
(2)速度校正方法
设校正后的东向、北向和天向速度为VadE、VadN、
Figure BDA00018790802700001511
则速度校正可表示为：
Figure BDA00018790802700001512
其中，
Figure BDA00018790802700001513
分别为INS输出的东向、北向和天向速度。
(3)位置校正方法
设校正后的纬度、经度和高度分别为Lad、λad、had，则位置校正可表示为：
Figure BDA0001879080270000161
其中，LINS、λINS、hINS分别为INS输出的纬度、经度和高度信息。
在导航系统进行反馈校正后，进入下一时刻导航计算过程，偏振光传感器、磁强计所需的时间信息可由GNSS实时提供，所需的位置信息由经过反馈校正后的惯性导航系统位置信息提供。
总之，本发明将偏振光传感器、磁强计与INS、GNSS相结合，融合了多种传感器，使数据量变得丰富，可提高初始对准的精度。此外，由于可通过偏振光传感器直接与失准角建立量测关系，使得失准角可被观测，可缩短对准时间，实现快速对准。
本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (5)

1.一种基于最小二乘的INS/GNSS/偏振/地磁组合导航系统对准方法，其特征在：于包括以下步骤：
(1)选择失准角、速度误差、位置误差、陀螺仪零偏、加速度计零偏以及磁航向角误差作为INS/GNSS/偏振/地磁组合导航系统的状态变量；
(2)根据偏振传感器输出的偏振方位角与INS输出的方位角匹配融合建立偏振量测方程，得到基于偏振方位角的量测方程；
(3)根据磁强计输出的航向角与INS输出的航向角建立地磁量测方程，得到基于磁强计航向角误差的量测方程；
(4)根据GNSS输出的速度、位置信息与INS输出的速度、位置信息匹配融合建立速度误差与位置误差量测方程，得到基于速度误差与位置误差的量测方程；
(5)在步骤(2)、(3)和(4)建立的量测方程基础上，利用增广技术将上述3个步骤中的量测方程进行向量化，建立统一的INS/GNSS/偏振/地磁组合导航系统量测方程；
(6)根据步骤(5)建立的统一的INS/GNSS/偏振/地磁组合导航系统量测方程，利用最小二乘技术估计INS/GNSS/偏振/地磁组合导航系统未知的状态；
(7)根据步骤(6)中估计得到的失准角、速度误差以及位置误差对INS/GNSS/偏振/地磁组合导航系统进行反馈校正，方法为基于INS获取的姿态、速度及位置上减去步骤(6)中估计得到的失准角、速度误差及位置误差，获取经反馈校正后的INS/GNSS/偏振/地磁组合导航系统姿态、速度及位置；
所述步骤(2)中，根据偏振传感器输出的偏振方位角与INS输出的方位角匹配融合建立偏振量测方程为：
zpolar＝H1X+υpolar
其中，
Figure FDA0002529477100000021
为INS/GNSS/偏振/地磁组合导航系统16维状态变量，φE，φN，φU分别为导航系，即n系下的东向失准角、北向失准角和天向失准角；δVE，δVN，δVU分别为导航系下的东向、北向和天向速度误差，这里导航系采用当地地理坐标系，即以载体的质心为原点，当地东向为地理系x轴，北向为y轴，天向为z轴，x轴，y轴和z轴满足右手坐标系；δλ，δL，δh分别为经度、纬度和高度误差；εx，εy，εz为载体坐标系，即b系下三个轴向的陀螺常值漂移，这里b系定义为，其原点O与载体质心重合，x轴正向与载体正向同向，y轴沿载体纵轴指左，z轴垂直xOy构成的平面指上，坐标系满足右手坐标系；
Figure FDA0002529477100000022
Figure FDA0002529477100000023
分别为b系下加速度常值漂移；
Figure FDA0002529477100000024
为磁强计的航向角测量误差；υpolar为偏振传感器的量测噪声；zpolar为偏振量测量，表示为：
Figure FDA0002529477100000025
其中，sm为单位太阳矢量在模块系m系下的表示，模块系为偏振传感器模块所在的坐标系，定义与载体坐标系基本相同，原点为模块质心，一般情况下与载体坐标系重合；
Figure FDA0002529477100000026
为计算导航系n′系到b系下的姿态转换矩阵，计算导航系n′系为实际导航过程用于计算导航解的导航坐标系，坐标原点与导航系统相同，x轴、y轴和z轴与理想导航系n系存在偏差角，该偏差角即为失准角；
Figure FDA0002529477100000027
为地球系e系到理想导航系n系下的姿态转换矩阵，地球系原点与地心重合，xe轴指向本初子午线，ze轴沿极轴方向指向北极，ye轴在赤道平面内与xe、ze轴构成右手系，理想导航系即为“东北天”坐标系，其原点为载体所在点，N轴指向北极方向，E轴指向东，U轴指向天空，满足右手坐标系；se为单位太阳矢量在e系下的表示；
H1是以偏振方位角的量测矩阵，具体表示为：
Figure FDA0002529477100000028
其中，
Figure FDA0002529477100000031
表示矢量
Figure FDA0002529477100000032
的反对称阵形式；
所述步骤(6)中，利用最小二乘技术估计INS/GNSS/偏振/地磁组合导航系统未知的状态具体为：所设计的最小二乘滤波具体如下：
第n次测量后INS/GNSS/偏振/地磁组合导航系统量测方程为：
Zn＝HnX+Vn
其中，Zn为第n次全部量测的测量值，Hn为第n次INS/GNSS/偏振/地磁组合导航系统统一的量测矩阵，Vn为n时刻噪声向量，其加权最小二乘估计为：
Figure FDA0002529477100000033
其中，Rn为第n次INS/GNSS/偏振/地磁组合导航系统测量噪声矩阵，
Figure FDA0002529477100000034
为第n次估计值；
估计误差方差为：
Figure FDA0002529477100000035
其中，Pn为误差方差矩阵，
Figure FDA0002529477100000036
为第n次测量真实值X与估计值
Figure FDA0002529477100000037
之差；
综合上述两式，得：
Figure FDA0002529477100000038
当得到一个新的测量Zn+1时，有：
Zn+1＝Hn+1X+Vn+1
新的加权最小二乘估计为：
Figure FDA0002529477100000039
把新的测量噪声加入原本的测量噪声矩阵中，此时噪声矩阵的逆矩阵
Figure FDA00025294771000000310
应为对角矩阵：
Figure FDA00025294771000000311
记
Figure FDA00025294771000000312
展开
Figure FDA00025294771000000313
得：
Figure FDA0002529477100000041
此时，n+1时刻误差方差矩阵为：
Figure FDA0002529477100000042
令中间量
Figure FDA0002529477100000043
则n+1时刻状态估计值为：
Figure FDA0002529477100000044
2.根据权利要求1所述的一种基于最小二乘的INS/GNSS/偏振/地磁组合导航系统对准方法，其特征在于：所述步骤(3)中，根据磁强计输出的航向角与INS输出的航向角建立地磁量测方程为：
Zmag＝H2X+υmag
其中，H2＝[01×15 1]为基于磁强计航向角的量测矩阵，维度为1×16，0m×n为m行n列全零向量；zmag为磁强计误差量测量；υmag为磁强计量测噪声。
3.根据权利要求2所述的一种基于最小二乘的INS/GNSS/偏振/地磁组合导航系统对准方法，其特征在于：所述步骤(4)中，根据GNSS输出的速度、位置信息与INS输出的速度、位置信息匹配融合建立速度误差与位置误差量测方程为：
Figure FDA0002529477100000045
其中，zv，zp分别融合了INS与GNSS速度、位置信息的速度误差与位置误差的量测量；H3＝[03×3 I3×3 03×10]为速度误差量测矩阵，维度为3×16，I3×3为3×3的单位矩阵；H4＝[03×6I3×3 03×7]为位置误差量测矩阵，维度为3×16；
Figure FDA0002529477100000046
为速度误差量测噪声向量，
Figure FDA0002529477100000047
分别为东北天三个方向的速度噪声；υp＝[υλ υL υh]T为位置误差量测噪声，υλ、υL、υh分别为经度、纬度及高度位置量测噪声。
4.根据权利要求3所述的一种基于最小二乘的INS/GNSS/偏振/地磁组合导航系统对准方法，其特征在于：所述步骤(5)中，建立统一的INS/GNSS/偏振/地磁组合导航系统量测方程为：
Z＝HX+V
其中，H为INS/GNSS/偏振/地磁组合导航系统统一的量测矩阵，具体表示为：
Figure FDA0002529477100000051
Z为全部量测向量，具体表示为：
Figure FDA0002529477100000052
其中，vGNSS为GNSS测量的速度矢量，vINS为INS测量的速度矢量；pGNSS为GNSS测量的位置矢量，pINS为INS测量的位置矢量；
V为总量测噪声，具体表示为：
Figure FDA0002529477100000053
5.根据权利要求1所述的一种基于最小二乘的INS/GNSS/偏振/地磁组合导航系统对准方法，其特征在于：所述步骤(7)中，反馈校正方法为：
(1)姿态校正方法
首先计算n′系至n系的转换矩阵：
Figure FDA0002529477100000054
则校正后的姿态矩阵
Figure FDA0002529477100000055
为：
Figure FDA0002529477100000056
(2)速度校正方法
校正后的东向、北向和天向速度为VadE、VadN、
Figure FDA0002529477100000061
则速度校正表示为：
Figure FDA0002529477100000062
其中，
Figure FDA0002529477100000063
分别为INS输出的东向、北向和天向速度；
(3)位置校正方法
校正后的纬度、经度和高度分别为Lad、λad、had，则位置校正表示为：
Figure FDA0002529477100000064
其中，LINS、λINS、hINS分别为INS输出的纬度、经度和高度信息。
CN201811414200.6A 2018-11-26 2018-11-26 一种基于最小二乘的ins/gnss/偏振/地磁组合导航系统对准方法 Active CN109556631B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811414200.6A CN109556631B (zh) 2018-11-26 2018-11-26 一种基于最小二乘的ins/gnss/偏振/地磁组合导航系统对准方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811414200.6A CN109556631B (zh) 2018-11-26 2018-11-26 一种基于最小二乘的ins/gnss/偏振/地磁组合导航系统对准方法

Publications (2)

Publication Number Publication Date
CN109556631A CN109556631A (zh) 2019-04-02
CN109556631B true CN109556631B (zh) 2020-07-24

Family

ID=65867472

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811414200.6A Active CN109556631B (zh) 2018-11-26 2018-11-26 一种基于最小二乘的ins/gnss/偏振/地磁组合导航系统对准方法

Country Status (1)

Country Link
CN (1) CN109556631B (zh)

Families Citing this family (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111912405A (zh) * 2019-05-10 2020-11-10 中国人民解放军火箭军工程大学 一种基于车载惯组与多普勒雷达的组合导航方法及系统
CN110375773B (zh) * 2019-07-29 2021-04-20 兰州交通大学 Mems惯导系统姿态初始化方法
CN112764414A (zh) * 2019-11-04 2021-05-07 北京京东乾石科技有限公司 数据处理方法、装置、系统、计算机可读存储介质
CN110672131B (zh) * 2019-11-19 2021-08-10 北方工业大学 一种大失准角下惯性/偏振光组合导航系统ukf对准方法
CN110672130B (zh) * 2019-11-19 2021-09-07 北方工业大学 一种大失准角下惯性/偏振光组合导航系统ekf对准方法
CN110887472B (zh) * 2019-12-09 2021-10-22 北京航空航天大学 一种偏振-地磁信息深度融合全自主姿态解算方法
CN111504306A (zh) * 2020-06-17 2020-08-07 哈尔滨工业大学 一种基于惯性导航的定位方法、装置及系统
CN112833917B (zh) * 2021-01-27 2022-09-16 北京航空航天大学 基于磁航向角和最小二乘法的三轴磁传感器标定方法
CN114018258B (zh) * 2021-11-05 2024-09-03 北京航空航天大学杭州创新研究院 基于偏振量测噪声方差自适应估计的仿生组合导航方法
CN113834484B (zh) * 2021-11-26 2022-03-08 北京航空航天大学 一种基于非瑞利散射模型误差的惯导/偏振组合导航方法

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108286966A (zh) * 2018-01-24 2018-07-17 北京航空航天大学 一种自适应多光谱偏振导航传感器及其定向方法

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN100487378C (zh) * 2007-01-10 2009-05-13 北京航空航天大学 一种sins/gps/磁罗盘组合导航系统的数据融合方法
FR2964476B1 (fr) * 2010-09-07 2012-10-05 Commissariat Energie Atomique Procede de calibration d'un appareil a fonctionnement atomique
CN102865881B (zh) * 2012-03-06 2014-12-31 武汉大学 一种惯性测量单元的快速标定方法
CN103217159B (zh) * 2013-03-06 2016-01-20 北京航空航天大学 一种sins/gps/偏振光组合导航系统建模及动基座初始对准方法
CN103323005B (zh) * 2013-03-06 2017-04-19 北京航空航天大学 一种sins/gps/偏振光组合导航系统多目标优化抗干扰滤波方法
CN104655152B (zh) * 2015-02-11 2017-06-27 北京航空航天大学 一种基于联邦滤波的机载分布式pos实时传递对准方法
CN105021188B (zh) * 2015-06-09 2018-08-21 北京航空航天大学 一种双模式仿生偏振/地磁辅助组合导航系统
CN106441372B (zh) * 2016-11-24 2019-09-17 北京航空航天大学 一种基于偏振与重力信息的静基座粗对准方法

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108286966A (zh) * 2018-01-24 2018-07-17 北京航空航天大学 一种自适应多光谱偏振导航传感器及其定向方法

Also Published As

Publication number Publication date
CN109556631A (zh) 2019-04-02

Similar Documents

Publication Publication Date Title
CN109556632B (zh) 一种基于卡尔曼滤波的ins/gnss/偏振/地磁组合导航对准方法
CN109556631B (zh) 一种基于最小二乘的ins/gnss/偏振/地磁组合导航系统对准方法
CN111947652B (zh) 一种适用于月球着陆器的惯性/视觉/天文/激光测距组合导航方法
CN102169184B (zh) 组合导航系统中测量双天线gps安装失准角的方法和装置
CN101344391B (zh) 基于全功能太阳罗盘的月球车位姿自主确定方法
CN110487301A (zh) 一种雷达辅助机载捷联惯性导航系统初始对准方法
CN103217159B (zh) 一种sins/gps/偏振光组合导航系统建模及动基座初始对准方法
CN104880191B (zh) 一种基于太阳矢量的偏振辅助导航方法
CN103674030B (zh) 基于天文姿态基准保持的垂线偏差动态测量装置和方法
CN106767787A (zh) 一种紧耦合gnss/ins组合导航装置
CN110672131B (zh) 一种大失准角下惯性/偏振光组合导航系统ukf对准方法
Chang et al. Strapdown inertial navigation system initial alignment based on modified process model
CN101788296A (zh) 一种sins/cns深组合导航系统及其实现方法
CN109708663B (zh) 基于空天飞机sins辅助的星敏感器在线标定方法
CN110672130B (zh) 一种大失准角下惯性/偏振光组合导航系统ekf对准方法
CN108562305B (zh) 一种惯性/天文深组合导航系统安装误差五位置快速粗标定方法
CN105910623B (zh) 利用磁强计辅助gnss/mins紧组合系统进行航向校正的方法
CN108151765B (zh) 一种在线实时估计补偿磁强计误差的定位测姿方法
CN109000639B (zh) 乘性误差四元数地磁张量场辅助陀螺的姿态估计方法及装置
Dou et al. A novel polarized skylight navigation model for bionic navigation with marginalized unscented Kalman filter
Mahmoud et al. Integrated INS/GPS navigation system
Giorgi Attitude determination
Zhang et al. RETRACTED: An improved RISS-GPS ship navigation approach via azimuth updates and magnetometer-calibration technology
Zhang et al. Analysis of key technologies in geomagnetic navigation
Jin et al. A dual-antenna heading determination method for single-frequency GNSS antennas with large phase pattern variations

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant