一种基于Huber方法的大失准角传递对准方法与流程

本发明涉及捷联惯导初始对准技术领域，特别是涉及一种基于huber方法的大失准角传递对准方法。

背景技术

惯性导航由于具有抗电磁干扰、隐蔽性好、信息全面、短期精度和稳定性高等诸多优点，成为舰船导航的一种重要方式。惯性导航系统利用陀螺仪测量运载体的角速度信息，利用加速度计测量运载体的比力信息，经过惯导解算得到运载体的速度、姿态以及位置等信息。惯性导航作为一种推算定位方式，在导航之前需要获得准确的初始信息，因此初始对准的速度与精度对惯导系统的性能有着重要影响。初始对准一般分为自对准和传递对准，罗经自对准要求静基座条件下进行，并且对准时间较长，而传递对准可以实现短时间高精度对准，能够满足武器系统等快速反应的需要。

在舰船传递对准的实际应用中，子惯导系统与主惯导系统在方位上往往存在一个大安装误差角，所以建立传递对准非线性数学模型更符合实际情况。并且舰船在海面航行时，受到外界复杂环境的影响，观测量中往往会存在非高斯强噪声甚至野值，此时基于l2范数最小的容积卡尔曼滤波可能会失效。为提高滤波算法的鲁棒性，国内许多学者进行了相应的研究。

陈雨等人在《一种混合h2/h∞滤波的传递对准算法》(发表于期刊《系统工程与电子技术》，2013年，第35卷，11期)一文中，将姿态噪声建模为能量有限噪声，将滤波问题转化为最小最大问题，设计了一种能处理观测噪声中既有高斯白噪声又有能量有限噪声的混合h2/h∞滤波算法，该算法比h∞滤波有更高的精度，比kalman滤波有更强的稳定性。高敬东等人在《基于huber的改进鲁棒滤波算法》(发表于期刊《系统仿真学报》，2014年，第26卷，08期)一文中，提出了一种利用新息卡方检测法预判断的鲁棒滤波算法，并通过一个简单线性模型的仿真验证了该算法可以抑制观测连续非高斯强噪声的影响，提高滤波精度及稳定性，具有良好的鲁棒性。张文杰等人在《基于huber的高阶容积卡尔曼跟踪算法》(物理学报，2016年，第65卷，08期)一文中，提出了一种新的基于huber的高阶容积卡尔曼滤波算法，在采用统计线性回归模型近似非线性量测模型的基础上,利用huberm估计算法实现状态的量测更新，通过仿真验证了该算法能够目标跟踪的精度，并且具有一定的鲁棒性。综合近年的文献，国内学者在提高滤波算法的鲁棒性方面进行了一些研究，但基本是基于简单的线性或者非线性模型来验证算法的有效性。针对传递对准这一实际问题，本发明设计了一种基于huber方法的大失准角传递对准方法，可用于实际观测噪声为非高斯噪声的情况，并且该方法利用容积卡尔曼滤波的优势，无需对非线性量测模型进行统计线性回归近似，传递对准的速度和精度比现有方法均有明显的提高。

技术实现要素：

本发明的目的在于提供一种可应用于舰船存在大方位失准角并且观测量中存在非高斯强噪声甚至野值情况下的快速高精度传递对准方法。

实现本发明目的的技术方案为：一种基于huber方法的大失准角传递对准方法，包括以下步骤：

步骤一：安装两套惯导系统，其中高精度的惯导系统作为主惯导，精度较低的惯导系统作为子惯导，完成主、子惯导系统的启动、预热准备；

步骤二：主、子惯导系统分别进行惯导解算，主惯导输出的速度、角速度和姿态信息传输给子惯导系统；

步骤三：考虑系统的非线性，建立舰船存在大失准角情况下的传递对准非线性状态方程和量测方程；

步骤四：利用本发明提出的基于huber方法的大失准角传递对准方法对主、子惯导间的安装误差角进行估计，完成传递对准。

在步骤四中，将基于huber方法的大失准角传递对准方法用于估计主、子惯导间的安装误差角，具体方式为：

(1)首先进行标准ckf时间更新获得状态预测值和状态预测协方差矩阵pk/k-1；

(2)之后基于第一步得到的和pk/k-1进行sigma点重采样，并在非线性观测方程中传递观测预测均值、自相关协方差以及互相关协方差；

(3)然后对残差进行随机解耦，并利用huber方法构造权矩阵对解耦后的残差进行加权平均；

(4)最后利用加权平均后的残差进行标准ckf量测更新得到当前时刻状态估计值和状态估计协方差矩阵pk。

与现有技术相比，本发明的有益效果是：

本发明在舰船存在大失准角的情况下，充分考虑到系统的非线性，建立非线性模型，并且针对观测量中存在非高斯强噪声和野值的情况，设计了一种基于huber方法的传递准方法，在标准ckf的基础上，利用huber方法构造权值函数对观测残差进行修正，以降低非高斯强噪声和野值的影响，有效地提高了滤波效果，从而提高了传递对准的精度。

附图说明

图1为本发明的具体流程图；

图2为matlab仿真得到的安装误差角估计误差曲线；

图3为matlab仿真得到的航向安装误差角估计误差曲线；

图4为50次蒙特卡罗仿真结果。

具体实施方式

下面结合附图1本发明的具体流程图对本发明进一步说明。

为了验证本发明的有效性，利用matlab对舰船大方位失准角传递对准模型进行仿真。

首先，建立速度加姿态匹配传递对准数学模型，具体如下：

忽略垂向通道，选取的状态变量为：

系统状态方程为：

其中，n为导航坐标系；m系为主惯导载体坐标系；s系为子惯导载体坐标系；为子惯导计算载体坐标系；δvⁿ为速度误差在导航坐标系的投影；为主惯导载体坐标系到子惯导载体坐标系的方向余弦矩阵；为主惯导载体坐标系到子惯导计算载体坐标系的方向余弦矩阵；为主惯导载体坐标系到导航坐标系的方向余弦矩阵；为子惯导测量的比力在其载体坐标系的投影；为地球自转角速度在导航坐标系的投影；为n系相对于地球坐标系的角速度在n系的投影；为s系和m系之间的安装误差角；为系和m系之间的量测失准角；为主惯导相对于导航坐标系的角速度在m系的投影；▽^s为加速度计常值漂移；wv为加速度计随机漂移；ε^s为陀螺常值漂移；为陀螺随机漂移。

选取主、子惯导间的速度误差δvⁿ、量测失准角以及角速度误差作为观测量：

量测方程为：

z＝h(x)+v

其中，v为系统的观测噪声。

然后，设计基于huber方法的容积卡尔曼滤波算法，具体如下：

对于如下一个离散非线性系统：

其中，xk为系统状态矢量；zk为观测矢量；wk为系统噪声矢量，vk为量测噪声矢量，均为零均值的高斯白噪声，且互不相关，即满足：

其中，qk为系统噪声序列的方差阵；rk为量测噪声序列的方差阵；δkj为克罗内克函数。

基于huber方法的容积卡尔曼滤波的具体实现步骤如下：

a.时间更新

假设k-1时刻的状态xk-1的统计特性已知，先对pk-1做cholesky分解：

计算容积点：

计算经系统状态方程传递后的容积点：

估计k时刻的状态预测值：

估计k时刻的状态预测协方差阵：

b.量测更新

对pk/k-1做cholesky分解：

计算容积点：

计算经系统量测方程传递后的容积点：

zi,k/k-1＝h(xi,k/k-1)i＝1,2…,2n

估计k时刻的量测预测值：

估计k时刻的量测预测协方差阵：

估计k时刻的一步预测互相关协方差阵：

对观测残差进行随机解耦：

利用huber方法构造权值矩阵，重构观测残差：

计算k时刻的状态估计值：

计算k时刻的状态误差协方差阵：

最后，仿真验证本发明的有效性，舰船在海中航行时会受到风浪影响，产生三轴摇摆运动，其数学模型为：

式中，ψ，θ，γ分别表示航向角、纵摇角和横摇角；ψm，θm，γm为摇摆角幅值；ωy，ωp，ωr为摇摆角频率；ti＝2π/ωi,(i＝y,p,r)为摇摆周期；为初始姿态角；k为初始航向。

仿真参数设置如下：

摇摆角幅值：ψm＝5°，θm＝15°，γm＝10°；

摇摆周期：ty＝8s，tp＝12s，tr＝6s；

初始姿态角：

初始航向：k＝30°；

初始纬度初始经度λ＝126.6705°；

误差角为：

陀螺常值漂移为εx＝εy＝εz＝0.01°/h，随机漂移为0.001°/h；

加速度计随机常值偏置为10^-4g，加速度计随机漂移为10^-5g；

舰船以10nmile/h的速度匀速直航；

滤波周期：0.05s；

观测噪声服从混合高斯分布：

观测量中随机加入野值。

滤波器初始条件，包括状态估计协方差阵p0、系统噪声方差阵q0及量测噪声方差阵r0，设定如下：

p0＝diag{(0.1m/s)²,(0.1m/s)²,(1°)²,(1°)²,(10°)²,(1°)²,(1°)²,(10°)²,(1×10^-4g0)²,(1×10^-4g0)²,(0.01°/h)²,(0.01°/h)²,(0.01°/h)²}

q0＝diag{(1×10^-5g0)²,(1×10^-5g0)²,(0.001°/h)²,(0.001°/h)²,(0.001°/h)²}

r0＝diag{(0.02m/s)²,(0.02m/s)²,(0.0004°)²,(0.0004°)²,(0.0004°)²}

仿真结果：

以上述仿真条件，利用ckf和本发明方法两种滤波算法估计安装误差角，仿真得到的结果如图2、图3和图4所示。

由图2和图3可以看出，在观测量中存在混合高斯噪声和野值的情况下，ckf滤波估计收敛速度变慢，滤波精度降低，而采用本发明方法，可以降低混合高斯噪声和野值的影响，误差估计曲线可以快速收敛，并且估计精度明显提高。由图4蒙特卡罗仿真结果可以看出，采用传统ckf滤波估计安装误差角，估计误差在5角分左右，而采用本发明方法，估计误差可达到2角分以下。综上所述，本发明提供的方法，可以在观测量中存在混合高斯噪声和野值的情况下，实现快速高精度对准。