一种基于最小二乘的INS/GNSS/偏振/地磁组合导航系统对准方法与流程

本发明涉及一种基于最小二乘的ins/gnss/偏振/地磁组合导航系统对准方法，可用于提高地球大气层内运载体(车辆、舰船和飞行器)的动基座初始对准精度，缩短初始对准时间。

背景技术：

在进入导航状态前，导航系统需要进行初始对准，通过初始对准得知初始导航参数，包括姿态和速度。在动基座条件下，载体在运动过程中存在晃动、振动等多种形式的干扰。这些干扰因素会降低导航系统初始对准精度，延长对准时间。因此，动基座初始对准方法一直是组合导航技术领域的难点和热点问题。

目前，动基座初始对准方法主要包括两大类：一类是基于主/子惯性导航系统的传递对准方法；另一类是惯性导航系统与卫星、磁罗盘、星敏感器等导航设备进行组合初始对准方法。传递对准方法中，由于导弹载体存在结构挠性变形、主惯性导航系统与子惯性导航系统之间存在数据传输延迟等因素影响，初始对准精度及时间往往不能得到保证。而惯性导航系统与卫星、磁罗盘和星敏感器等导航设备进行组合初始对准方法，同样存在问题：其中，卫星在载体作机动时误差较大，且与惯性导航系统信号同步性不好；磁罗盘依赖于地磁场模型精度，特别是在地磁场异常的情况下，将出现较大误差，因此用于动基座初始对准效果并不理想；星敏感器可高精度估计载体姿态误差，但并不适用于在地球大气层内白昼时间段内的动基座初始对准。基于上述原因，惯性导航系统结合新型导航装置，设计更合适的信息匹配方式是动基座初始对准技术的发展趋势之一。

偏振光导航是一种仿生导航方式。太阳光线入射大气层时，大气中的粒子会使入射光线散射，造成太阳光线的偏振现象，沙蚁等生物可利用天空中的偏振光进行导航。根据仿生导航原理设计的偏振光传感器，可敏感到传感器量测方向的入射光最大偏振方向，输出偏振方位角。基于偏振方位角的偏振光导航具有自主性强、误差不随时间积累等优点，尤其适用于高精度的姿态确定。但偏振方位角对定位误差极不敏感，可达数十米。惯性导航系统的误差随时间积累，位置信息是发散的。因此偏振光传感器仅与惯性导航系统组合进行动基座初始对准会造成精度不高，甚至可能出现滤波结果发散的情况。

综上，目前常用的动基座初始对准方法存在对准精度低、对准时间长等缺点，而偏振光传感器、磁传感器对位置误差极不敏感，仅与惯性导航系统组合进行动基座初始对准效果同样不好。

现有技术中，与专利号为201310069511.4“一种sins/gps/偏振光组合导航系统的动基座初始对准方法”的发明专利相比，本发明有以下优点：

(1)所建立的组合导航系统模型不同，本发明加入新的传感器磁强计，通过融合新的传感器数据，可进一步提高初始对准的精度；

(2)所建立的偏振机理模型不同，本发明获得的偏振量测方程与专利号为201310069511.4“一种sins/gps/偏振光组合导航系统的动基座初始对准方法”的发明专利完全不同，后者所建立的偏振机理模型采用单个偏振光传感器进行构建，建立的模型复杂，而且存在除法，导致噪声不再满足高斯分布，在状态估计阶段会存在问题；本发明采用双偏振光传感器的设计，构建的模型简单，解决了单一传感器噪声分布不再是高斯分布的问题；

(3)采用的滤波方法不同，本发明采用最小二乘方法进行状态估计，该方法通过最小化误差的平方和寻找数据的最佳函数匹配，在工程中易于实现。

技术实现要素：

本发明的技术解决问题是：克服现有方法的不足，提供一种精度高、对准快、多传感器融合的基于最小二乘的ins/gnss/偏振/地磁组合导航系统对准方法。

本发明的技术解决方案为：一种基于最小二乘的ins/gnss/偏振/地磁组合导航系统对准方法，其实现步骤如下：

(1)选择失准角、速度误差、位置误差、陀螺仪零偏、加速度计零偏以及磁航向角误差作为ins/gnss/偏振/地磁组合导航系统的状态变量，其中，ins(inertialnavigationsystem)为惯性导航系统，gnss(globalnavigationsatellitesystem)为全球导航卫星系统；

(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

其中，为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构成的平面指上，坐标系满足右手坐标系；分别为b系下加速度常值漂移；为磁强计的航向角测量误差；υpolar为偏振传感器的量测噪声；zpolar为偏振量测量，表示为：

其中，s^m为单位太阳矢量在模块系m系下的表示，模块系为偏振传感器模块所在的坐标系，定义与载体坐标系基本相同，原点为模块质心，一般情况下与载体坐标系重合；为计算导航系n′系到b系下的姿态转换矩阵，计算导航系n′系为实际导航过程用于计算导航解的导航坐标系，坐标原点与导航系统相同，x轴、y轴和z轴与理想导航系n系存在偏差角，该偏差角即为失准角；为地球系e系到理想导航系n系下的姿态转换矩阵，地球系原点与地心重合，xe轴指向本初子午线，ze轴沿极轴方向指向北极，ye轴在赤道平面内与xe、ze轴构成右手系，理想导航系即为“东北天”坐标系，其原点为载体所在点，n轴指向北极方向，e轴指向东，u轴指向天空，满足右手坐标系；s^e为单位太阳矢量在e系下的表示。

h1为以偏振方位角的量测矩阵，具体表示为：

其中，表示矢量的反对称阵形式。

所述步骤(3)中，根据磁强计输出的航向角与ins输出的航向角建立地磁量测方程为：

zmag＝h2x+υmag

其中，h2＝[01×151]为基于磁强计航向角的量测矩阵，维度为1×16；zmag为磁强计误差量测量；υmag为磁强计量测噪声。

所述步骤(4)中，根据gnss输出的速度、位置信息与ins输出的速度、位置信息匹配融合建立速度误差与位置误差量测方程为：

其中，zv，zp分别融合了ins与gnss速度、位置信息的速度误差与位置误差的量测量；h3＝[03×3i3×303×10]为速度误差量测矩阵，维度为3×16，i3×3为3×3的单位矩阵；h4＝[03×5i3×303×8]为位置误差量测矩阵，维度为3×16；为速度误差量测噪声向量，分别为东北天三个方向的速度噪声；υp＝[υλυlυh]^t为位置误差量测噪声，υλ、υl、υh分别为经度、纬度及高度位置量测噪声；

所述步骤(5)中，在步骤(2)、(3)和(4)建立的量测方程基础上，利用增广技术将上述3个步骤中的量测方程进行向量化，建立统一的ins/gnss/偏振/地磁组合导航系统量测方程为：

z＝hx+v

其中，h为ins/gnss/偏振/地磁组合导航系统统一的量测矩阵，具体表示为：

z为全部量测向量，具体表示为：

其中，vgnss为gnss测量的速度矢量，vins为ins测量的速度矢量；pgnss为gnss测量的位置矢量，pins为ins测量的位置矢量。

v为总量测噪声，具体表示为：

所述步骤(6)中，根据步骤(5)建立的统一的ins/gnss/偏振/地磁组合导航系统量测方程，利用最小二乘技术估计ins/gnss/偏振/地磁组合导航系统未知的状态，所设计的最小二乘滤波内容如下：

第n次测量后ins/gnss/偏振/地磁组合导航系统量测方程为：

zn＝hnx+vn

其中，zn为第n次量测的测量值，hn为第n次ins/gnss/偏振/地磁组合导航系统量测矩阵，其加权最小二乘估计为：

其中，rn为第n次ins/gnss/偏振/地磁组合导航系统测量噪声矩阵，为第n次估计值；

估计误差方差为：

其中，pn为误差方差矩阵，为第n次测量真实值x与估计值之差；

综合上述两式，得：

当得到一个新的测量zn+1时，有：

zn+1＝hn+1x+vn+1

新的加权最小二乘估计为：

把新的测量噪声加入原本的测量噪声矩阵中，此时噪声矩阵的逆矩阵应为对角矩阵：

记展开得：

此时，n+1时刻误差方差矩阵为：

令中间量则n+1时刻状态估计值为：

所述步骤(7)中，根据步骤(6)中估计得到的失准角、速度误差以及位置误差对ins/gnss/偏振/地磁组合导航系统进行反馈校正，其方法为：

(1)姿态校正方法

首先计算n′系至n系的转换矩阵：

则校正后的姿态矩阵为：

(2)速度校正方法

设校正后的东向、北向和天向速度为vade、vadn、则速度校正表示为：

其中，分别为ins输出的东向、北向和天向速度；

(3)位置校正方法

设校正后的纬度、经度和高度分别为lad、λad、had，则位置校正表示为：

其中，lins、λins、hins分别为ins输出的纬度、经度和高度信息。

本发明与现有技术相比的优点在于：本发明将偏振光传感器、磁强计与ins、gnss相结合，融合了多种传感器，使数据量变得丰富，可提高载体初始对准的精度。其中，偏振光传感器输出的偏振方位角不随时间积累，且具有高度自主性，具备高精度姿态校正能力；磁强计具有简单、快速、自主性好等优点，并且其功耗低，产生磁航向角可与偏振方位角中的航向角相冗，提高系统鲁棒性；gnss提供速度、位置信息，弥补了偏振方位角信息对位置误差不敏感及惯性导航系统下速度、位置误差随时间积累并发散的缺点，缩短对准时间。

附图说明

图1为本发明的设计流程图；

图2为双偏振传感器坐标关系图。

具体实施方式

下面结合附图及实施例对本发明进行详细说明。

如图1所示，本发明具体实施步骤如下：

第一步，选择ins/gnss/偏振/地磁组合导航系统的状态向量：

其中，φe，φn，φu分别为东向、北向和天向失准角；δve，δvn，δvu分别为东向、北向和天向速度误差；δλ，δl，δh分别为经度、纬度和高度误差；εx，εy，εz为载体坐标系即b系下陀螺常值漂移，载体坐标系原点o与载体质心重合，x轴正向与载体正向同向，y轴沿载体纵轴指左，z轴垂直xoy构成的平面指上，整个坐标系满足右手坐标系；分别为b系下加速度常值漂移；为磁强计的航向角测量误差。

第二步，建立以偏振方位角为观测量的量测方程：

首先，根据偏振方位角定义可知，瑞利散射后的产生的偏振光与太阳矢量在模块系下的投影垂直，故有：

其中，为偏振方位角，可由标定好的六通道偏振传感器计算出来：

其中，为标定后的传感器参数，包括安装误差和尺度因子等。为基于六通道仿生偏振传感器的对数处理后的量测输出。

综合式(2)、(3)，可得：

然后，确定太阳矢量在模块系与地球系下的坐标转换关系。太阳矢量在模块系下的向量s^m表示为：

同样，太阳矢量在地球系下的向量s^e表示为：

其中，λ^e和ψ^e为地球系下的高度角和方位角。根据天文学可知，在已知载体所处地理位置及当地时间的情况下，可计算出上述两个角度。在ins/gnss/偏振/地磁组合导航系统中，偏振光传感器所需的时间信息由gnss提供，位置信息由经过校正后的惯性导航系统位置信息提供。

因为(5)、(6)为太阳矢量在不同坐标系下的描述，故可建立坐标转换：

其中，为载体坐标系b系与模块系m系的转换关系，即为偏振传感器和载体之间的安装关系，为固定常值，可在对准前标定补偿，这里假定其为单位矩阵，载体坐标系原点o与载体质心重合，x轴正向与载体正向同向，y轴沿载体纵轴指左，z轴垂直xoy构成的平面指上，整个坐标系满足右手坐标系，模块系为偏振传感器模块所在的坐标系，原点为模块质心，定义与载体坐标系相同。为地球系e系到理想导航系n系下的姿态转换矩阵，地球系原点与地心重合，xe轴指向本初子午线，ze轴沿极轴方向指向北极，ye轴在赤道平面内与xe、ze轴构成右手系，理想导航系即为“东北天”坐标系，其原点为载体所在点，n轴指向北极方向，e轴指向东，u轴指向天空，满足右手坐标系。可根据当地地理纬度l和经度λ计算而得，具体如下：

姿态矩阵实际上存在计算误差，这里用n′表示计算导航系，n系和n′系之间存在的姿态误差实际上就是失准角。针对小失准角情形时，可得：

其中，φ^×为失准角的反对称矩阵形式：

其中，φi(i＝e,n,u)分别为东向、北向和天向失准角。

联立式(7)、(9)，并且令可得：

针对式(11)，令等式右侧只与失准角有关，则有：

若a，b均为3×1向量，根据反对称阵叉乘性质有：

a×b＝-b×a(13)

根据上述性质，式(12)可写为：

即：

然后，考虑两组互相垂直朝向的偏振传感器，如图2所示：p1、p2分别为两垂直布置的偏振光传感器，α0表示其安装角度，这里s为太阳矢量点；θi(i＝1,2)为太阳光射入第i个传感器的散射角；φi(i＝1,2)为第i个传感器测得的偏振方位角；为相对于第i个传感器的太阳方位角。

记球面角如图所示，根据球面三角形δp1p2s的正弦定理和余弦定理可得：

其中，根据上述两式计算可得：

下面有两组偏振传感器在各自的模块系下可计算得到φ1和φ2，根据太阳矢量在模块系下的不同位置可以给出偏振方位角与球面角的计算关系式，可得：

考虑到太阳高度角和散射角的关系可得在模块系下的太阳高度角为：

记偏振方位角为则可得以偏振方位角为观测量的量测方程：

zpolar＝h1x+υpolar(21)

其中，zpolar为偏振光传感器输出的偏振方位角；υpolar为偏振光传感器的量测噪声；h1为以偏振方位角的量测矩阵，具体表示为：

其中，表示矢量的反对称阵形式。

第三步，建立以磁强计航向角测量误差为观测量的量测方程。

首先对磁强计航向角量测误差进行分析。当系统固定在载体上后，航向角测量结果会受到周围铁磁物质的影响，干扰磁场由硬铁磁场和软铁磁场两部分组成，产生罗差。通常罗差的表现形式有由硬铁磁场产生的半圆罗差，由软铁磁场产生的圆周罗差和象限罗差，可建立罗差方程：

δφ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×151]为基于磁强计航向角的量测矩阵，维度为1×16；zmag为磁强计误差量测量；υmag为磁强计量测噪声。

第四步，建立以速度误差、位置误差为观测量的量测方程。

其中，zv，zp分别融合了ins与gnss速度、位置信息的速度误差与位置误差的量测量；h3＝[03×3i3×303×10]为速度误差量测矩阵，维度为3×16，i3×3为3×3的单位矩阵；h4＝[03×5i3×303×8]为位置误差量测矩阵，维度为3×16；为速度误差量测噪声向量，分别为东北天三个方向的速度噪声；υp＝[υλυlυh]^t为位置误差量测噪声，υλ、υl、υh分别为经度、纬度及高度位置量测噪声。

第五步，在步骤二、三和四建立的量测方程基础上，利用增广技术将上述3个步骤中的量测方程进行向量化，建立统一的ins/gnss/偏振/地磁组合导航系统量测方程：

z＝hx+v(27)

其中，h为ins/gnss/偏振/地磁组合导航系统统一的量测矩阵，具体表示为：

z为全部量测向量，具体表示为：

其中，vgnss为gnss测量的速度矢量，vins为ins测量的速度矢量；pgnss为gnss测量的位置矢量，pins为ins测量的位置矢量。

v为全部量测噪声，具体表示为：

第六步，根据第五步建立的统一的ins/gnss/偏振/地磁组合导航系统量测方程，利用最小二乘技术估计ins/gnss/偏振/地磁组合导航系统未知的状态：

第n次测量后ins/gnss/偏振/地磁组合导航系统量测方程为：

zn＝hnx+vn(31)

其中，zn为第n次量测的测量值，hn为第n次ins/gnss/偏振/地磁组合导航系统量测矩阵，其加权最小二乘估计为：

其中，rn为第n次ins/gnss/偏振/地磁组合导航系统测量噪声矩阵，为第n次估计值；

估计误差方差为：

其中，pn为误差方差矩阵，为第n次测量真实值x与估计值之差；

综合上述两式，得：

当得到一个新的测量zn+1时，有：

zn+1＝hn+1x+vn+1(35)

新的加权最小二乘估计为：

把新的测量噪声加入原本的测量噪声矩阵中，此时噪声矩阵的逆矩阵应为对角矩阵：

记展开得：

此时，n+1时刻误差方差矩阵为：

令中间量则n+1时刻状态估计值为：

第七步，根据步骤六中估计得到的失准角、速度误差以及位置误差对ins/gnss/偏振/地磁组合导航系统进行反馈校正，设最小二乘法估计的ins/gnss/偏振/地磁组合导航系统状态变量为：

则姿态校正、速度校正以及位置校正方法如下所述：

(1)姿态校正方法

首先计算平台坐标系至真实地理坐标系的转换矩阵：

则校正后的姿态矩阵为：

(2)速度校正方法

设校正后的东向、北向和天向速度为vade、vadn、则速度校正可表示为：

其中，分别为ins输出的东向、北向和天向速度。

(3)位置校正方法

设校正后的纬度、经度和高度分别为lad、λad、had，则位置校正可表示为：

其中，lins、λins、hins分别为ins输出的纬度、经度和高度信息。

在导航系统进行反馈校正后，进入下一时刻导航计算过程，偏振光传感器、磁强计所需的时间信息可由gnss实时提供，所需的位置信息由经过反馈校正后的惯性导航系统位置信息提供。

总之，本发明将偏振光传感器、磁强计与ins、gnss相结合，融合了多种传感器，使数据量变得丰富，可提高初始对准的精度。此外，由于可通过偏振光传感器直接与失准角建立量测关系，使得失准角可被观测，可缩短对准时间，实现快速对准。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。