一种基于统计相似度量的组合导航鲁棒滤波方法
阅读说明：本技术 一种基于统计相似度量的组合导航鲁棒滤波方法 (Combined navigation robust filtering method based on statistical similarity measurement ) 是由 徐博 郭瑜 赵玉新 吴磊 胡俊苗 陈崇 王连钊 李盛新 王潇雨 王朝阳 于 2021-09-23 设计创作，主要内容包括：本发明属于非理想条件下的组合导航技术领域,具体涉及一种基于统计相似度量的组合导航鲁棒滤波方法。本发明考虑SINS/DVL紧组合系统中正常的DVL波束测量信息和大误差的波束测量信息同时出现的情形,针对目前现有的组合导航系统鲁棒滤波器对量测信息处理粗糙,导致正常量测信息易丢失的问题,基于多维量测方程分解和统计相似度量提出了新的鲁棒滤波方法。本发明在将SINS/DVL紧组合导航系统的多维量测方程进行分解的同时,引入统计相似度量(SSM)理论,协助每个波束的测量噪声方差在大测量误差出现时完成各自的自适应更新,最终保证每个波束测量信息处理的独立性。本发明可用于非理想条件下的水下航行器组合导航领域。(The invention belongs to the technical field of integrated navigation under non-ideal conditions, and particularly relates to an integrated navigation robust filtering method based on statistical similarity measurement. The invention provides a novel robust filtering method based on multi-dimensional measurement equation decomposition and statistical similarity measurement, which considers the situation that normal DVL beam measurement information and large-error beam measurement information in an SINS/DVL tight combination system occur simultaneously and aims at solving the problem that the conventional robust filter of the combined navigation system processes the measurement information coarsely to cause easy loss of the normal measurement information. The invention decomposes the multidimensional measurement equation of the SINS/DVL tight combination navigation system, and introduces a Statistic Similarity Measurement (SSM) theory to assist the measurement noise variance of each beam to complete respective self-adaptive updating when a large measurement error occurs, thereby finally ensuring the independence of the measurement information processing of each beam. The invention can be used in the field of underwater vehicle integrated navigation under non-ideal conditions.)
技术领域
本发明属于非理想条件下的组合导航技术领域，具体涉及一种基于统计相似度量的组合导航鲁棒滤波方法。
背景技术
捷联惯导/多普勒测速计程仪(SINS/DVL)组合导航系统是目前水下航行器最常用的组合导航方法之一。其中紧组合方式可以利用DVL的原始波束测量信息，容错性好，逐渐被广泛使用。DVL采用多普勒频移原理，通过计算向水底发射的声波频率和接受的反射声波频率之差可以测量载体沿声波波束方向的速度。但是在一些复杂情况下，DVL会出现较大的测量误差，例如，DVL发射的声波波束受海洋生物遮挡无法到达海底，或者当水下航行器驶过海沟时，航行器与海床之间的距离超出DVL声波波束范围，亦或者海底存在强吸波材料(如淤泥等)，发出的声波波束不能反射回来。理论上，当DVL的某一个波束出现大的测量误差时，紧组合系统的滤波算法需要能够自动降低对该波束错误信息的利用率。但是目前组合导航系统所使用的鲁棒滤波器多是对所有量测信息的利用率进行统一的调节，对于SINS/DVL紧组合导航系统这种典型的多维量测系统来说，降低对单个大测量误差的波束信息的利用率可能会导致其他正常的波束测量信息在系统中没有被充分使用，从而丢失有用信息。
因此，需要发明在不影响导航系统对正常波束信息利用的情况下降低含有大测量误差的波束信息影响的鲁棒滤波方法。
发明内容
本发明的目的在于提供一种基于统计相似度量的组合导航鲁棒滤波方法。
本发明的目的通过如下技术方案来实现：包括以下步骤：
步骤1：定义坐标系；
定义载体坐标系采用b表示，地心地固坐标系用e表示，“东-北-天”地理坐标系作为导航系采用n表示，惯性坐标系用i表示，DVL框架坐标系用d表示；
定义不同坐标系下的速度：
其中，表示捷联惯导系统在b系中的速度矢量；表示n系内捷联惯导系统的速度矢量；表示捷联惯导系统在d系中的速度矢量；表示d系中DVL波束的速度矢量；
步骤2：建立SINS/DVL紧组合导航系统状态方程；
选取SINS的误差作为状态，包括失准角φ、速度误差δVn、位置误差δp、陀螺常值漂移εb和加速度计常值零偏状态方程如下：
其中，εb表示陀螺常值漂移矢量；是陀螺随机漂移矢量；是加速度计常值零偏矢量；是加速度计随机零偏矢量；φ表示由俯仰、横滚和偏航失准角组成的失准角矢量；是方向余弦矩阵，表示从b系到n系的变换；表示b系下b系相对于i系的角速率矢量，对应的计算误差为 n＝[vE vN vh]T是n系中的载体相对于地面的速度矢量，对应的误差矢量是δvn；fb是加速度计的输出；δgn是重力误差向量在n系中的投影；δp表示由纬度误差δL、经度误差δλ和高度误差δh组成的位置误差矢量；RM和RN分别表示子午线圆和本初垂直圆的曲率半径；
步骤3：建立SINS/DVL紧组合导航系统量测方程，并分解；
量测方程：
Z＝HX+v
其中，表示测量噪声；I3×3是三维单位矩阵；[·×]表示向量的叉乘运算；α是波束和载体之间的水平角；设置为0°或45°；
DVL各个波束的测量噪声通常情况下是不相关的，其量测噪声协方差阵R＝E[vvT]是一个对角阵，因此将量测方程进行等效分解：
其中，Hj表示量测矩阵的第j行；vj是相应的测量噪声，不同的DVL波束的测量噪声不相关，这里是将DVL的四个波束测量值假设为由不同的传感器分别进行测量获得；
步骤4：基于统计相似度量构建状态估计的代价函数；
其中，和分别表示状态一步预测结果和一步预测协方差阵；N(X；Xk+1,Pk+1)表示关于X的高斯分布，均值是Xk+1，方差是Pk+1；相似度函数fx(t)选择为fx(t)＝-0.5t；相似度函数fz(t)选为ω是自由度参数，m是量测维数，m＝1；q＝4；
步骤5：应用高斯-牛顿迭代求取代价函数的最优值；
步骤5.1：初始化迭代次数t＝0，设定迭代初值为
步骤5.2：计算滤波增益
其中， 和相当于用来调节一步预测协方差阵和量测噪声方差的辅助参数；
步骤5.3：计算后验状态
步骤5.4：计算估计误差协方差阵
步骤5.5：判断是否满足若不满足，则令t＝t+1，返回步骤5.2；否则，令输出SINS的误差X；
步骤6：将估计的SINS的误差X反馈给SINS，进行修正；
根据估计的SINS的误差X，获取失准角φ、速度误差δVn、位置误差δp；对于SINS解算出的姿态阵速度位置姿态误差的反馈修正方法为：
速度误差δVn和位置误差δp直接在SINS的输出中直接减去即可：
本发明的有益效果在于：
本发明考虑SINS/DVL紧组合系统中正常的DVL波束测量信息和大误差的波束测量信息同时出现的情形，针对目前现有的组合导航系统鲁棒滤波器对量测信息处理粗糙，导致正常量测信息易丢失的问题，基于多维量测方程分解和统计相似度量提出了新的鲁棒滤波方法。本发明在将SINS/DVL紧组合导航系统的多维量测方程进行分解的同时，引入统计相似度量(SSM)理论，协助每个波束的测量噪声方差在大测量误差出现时完成各自的自适应更新，最终保证每个波束测量信息处理的独立性。本发明可用于非理想条件下的水下航行器组合导航领域。
附图说明
图1为SINS/DVL紧组合导航系统结构框图。
图2为不同坐标系下速度关系图。
图3为湖试实验轨迹图。
图4为速度误差和定位误差比较图。
图5为DVL波束测量值与辅助参数变化对比图。
图6为本发明的实施流程图。
具体实施方式
下面结合附图对本发明做进一步描述。
本发明的目的在于，针对SINS/DVL紧组合导航系统，提供一种可以对每个DVL波束的测量噪声方差进行单独调节的鲁棒滤波方法，即当某一量测出现较大误差时可以自动增大滤波器中所对应的测量噪声方差阵，而其他正常量测的测量噪声方差阵几乎不进行调节。
步骤一：建立SINS/DVL紧组合导航系统模型；
首先定义常用坐标系，载体坐标系采用b表示，地心地固坐标系用e表示，“东-北-天”地理坐标系作为导航系采用n表示，惯性坐标系用i表示，DVL框架坐标系用d表示。
图1描述了SINS/DVL紧组合导航系统的结构。SINS作为一种主要的导航设备，提供速度、姿态和位置信息。作为辅助导航设备，DVL提供四维测量矢量，即DVL在四个波束方向上相对于海底的速度。将SINS解转化为d坐标系下的速度与DVL波束速度的差值作为导航滤波器的观测量。然后对状态进行估计并反馈给SINS，以减小SINS的导航误差。
在本发明中，假设SINS与DVL之间的安装角误差和DVL的标度因数在设备安装完成后已经进行了标定和补偿。残余的安装角误差和标度因数误差可以忽略不计。因此，本发明只选取SINS的误差作为状态，包括失准角φ、速度误差δVn、位置误差δp、陀螺常值漂移εb和加速度计常值零偏状态方程如下：
式中εb表示陀螺常值漂移矢量，是陀螺随机漂移矢量。是加速度计常值零偏矢量 是加速度计随机零偏矢量。φ表示由俯仰、横滚和偏航失准角组成的失准角矢量。是方向余弦矩阵，表示从b系到n系的变换。表示b系下b系相对于i系的角速率矢量，对应的计算误差为vn＝[vE vN vh]T是n系中的载体相对于地面的速度矢量，对应的误差矢量是δvn。fb是加速度计的输出。δgn是重力误差向量在n系中的投影。δp表示由纬度误差δL、经度误差δλ和高度误差δh组成的位置误差矢量。RM和RN分别表示子午线圆和本初垂直圆的曲率半径。
本发明在引入测量方程之前，定义了不同坐标系下的速度，以四波束配置DVL为例，如图2所示。是捷联惯导系统在b系中的速度矢量。表示n系内捷联惯导系统的速度矢量。表示捷联惯导系统在d系中的速度矢量。表示d系中DVL波束的速度矢量。
捷联惯导系统在导航系下的速度与d系下的DVL波束速度之间的关系如下
其中α是波束和载体之间的水平角。通常，设置为0°或45°。
DVL波束测量值建模如下：
其中表示DVL的测量噪声。将捷联惯导系统的速度输出转换到d系的结果为
式中，I3×3是三维单位矩阵。[·×]表示向量的叉乘运算。
联立(19)和(20)可以得到量测方程
Z＝HX+v (22)
表示测量噪声。
步骤二：分解SINS/DVL紧组合导航系统的多维量测方程；
DVL各个波束的测量噪声通常情况下是不相关的，其量测噪声协方差阵R＝E[vvT]是一个对角阵。因此在本发明中可以将量测方程(22)进行等效分解。
其中Hj表示量测矩阵的第j行。vj是相应的测量噪声，不同的DVL波束的测量噪声不相关。这里是将DVL的四个波束测量值假设为由不同的传感器分别进行测量获得。
步骤三：引入统计相似度量概念，并基于统计相似度量构建状态估计的代价函数；
步骤3.1：引入统计相似度量(SSM)
统计相似度度量s(a,b)可以用来表示两个随机向量a和b之间的相似度，定义如下：
s(a,b)＝E[f(||a-b||2)]＝∫∫f(||a-b||2)p(a,b)dadb (25)
式中，||·||表示欧氏范数，p(a,b)表示随机向量a和b之间的联合概率密度。f(·)表示相似度函数，并且满足以下三个条件。
a)f(·)是在定义域[0，+∞)内的连续函数。
b)f(·)是单调递减函数，即
c)f(·)的二阶导数是非负的，即
统计相似度度量符合相似度度量的通常定义。随机向量之间的相似性越高，统计相似度度量就越大。选择f(t)为f(t)＝-t时，SSM表示不同随机向量之间的负均方误差。当选择f(t)为时，SSM表示不同随机向量之间的相关熵。通过选择不同的相似函数，可以实现不同的SSM。
步骤3.2：建立鲁棒代价函数
首先假设系统模型为
Xk+1＝fk(Xk)+nk (26)
式中，k表示时刻，Xk+1表示一个p维状态。表示第j个量测，fk(·)和表示状态转移函数和测量函数。nk和表示过程噪声和量测噪声。Qk和表示过程噪声标称协方差阵和第j个量测噪声标称协方差阵，对于本发明所介绍的SINS/DVL紧组合导航系统来说，q为4。
标准卡尔曼滤波(KF)本质上是状态预测和量测信息的加权，以获得状态的最优估计，它的加权最小二乘代价函数反映了状态和预测状态之间以及量测和预测量测之间的均方误差。而本发明则构造了反映状态与预测状态、量测与预测量测之间SSM的代价函数。
式中，和表示状态一步预测结果和一步预测协方差阵，和表示量测噪声方差和一步预测协方差阵的均方根分解。
为了便于求解(28)所示的代价函数，将后验分布近似为高斯分布并利用詹森(Jensen)不等式求解上式中代价函数的下界，将其作为一个新的代价函数。
式中，N(X；Xk+1,Pk+1)表示关于X的高斯分布，均值是Xk+1，方差是Pk+1。
当测量方程为非线性时，可以采用sigma点变换，如无迹变换规则或三次球面径向容积规则进行近似求解。当测量方程为线性时，(31)可写成
步骤四：应用高斯-牛顿迭代求取代价函数的最优值；
求解代价函数极值，通常情况下可以采用对代价函数求导，令导数为零的方法去计算。
由于式(33)为非线性方程，因此在本发明中使用高斯-牛顿迭代方法对(33)进行求解。
根据高斯-牛顿迭代法，对于非线性方程式(33)，解的数值更新过程为
式中，上标t表示第t次迭代。迭代初值设为表示代价函数J(Xk+1,Pk+1)的近似海森(Hessian)矩阵。
其中，
整理得
上式中，滤波增益为
其中和相当于用来调节一步预测协方差阵和量测噪声方差的辅助参数。
估计误差为
由此可以直接得到估计误差协方差阵
通常情况下，对于SINS/DVL紧组合导航系统，陀螺和加速度计的随机漂移构成的过程噪声是具有精确协方差的高斯噪声。因此，相似度函数fx(t)选择为fx(t)＝-0.5t，即采用负均方误差来度量状态与预测状态之间的相似程度。而DVL波束测量噪声则可能是由野值引起的非高斯噪声，因此相似度函数fz(t)选为ω是自由度参数，m是量测维数，在本发明中为1。
步骤五：将估计的导航误差反馈给捷联惯导。
通过鲁棒卡尔曼滤波器，可以得到估计的失准角φ，速度误差δVn和位置误差δp，假设捷联惯导纯惯性解算出的姿态阵、速度、位置分别为由于采用的是反馈校正去修正捷联惯导，惯导的误差量时刻都保持为小量，因此姿态误差的反馈修正方法为
速度误差δVn和位置误差δp直接在捷联惯导的输出中直接减去即可。
步骤六：使用湖试实验数据对所提出算法进行验证。
图3表示一次湖试实验的航行轨迹。实验中以自研捷联惯导和DVL作为待测试设备，GPS接收机和法国进口PHINS惯导组合作为基准。
图4表示所提出算法和已有的鲁棒滤波算法应用在SINS/DVL紧组合系统中的速度误差和水平定位误差。其中，RSTKF表示已有的基于Student’s t分布建模的鲁棒卡尔曼滤波器。MCKF表示已有的基于最大相关熵准则的卡尔曼滤波器。SSMKF表示已有的基于统计相似度量的卡尔曼滤波器，SSMKF与本发明所提出的鲁棒卡尔曼滤波器最大的区别在于，SSMKF只使用一个辅助参数去对所有量测的利用率做统一调节，而本发明提出的鲁棒卡尔曼滤波器则是使用多个辅助参数分别调节不同量测分量的利用率。从图4中可以看出，所提出算法的定位误差和速度误差均最小，和基于相同统计相似度量准则的SSMKF相比，所提算法的估计精度明显更好。
图5则表示实验中一段DVL波束测量值和每个波束对应的辅助参数变化情况。从图中可以看出，当出现大离群点时，DVL波束所对应的辅助参数会明显减小，从而使得波束测量噪声方差增大，降低大离群点量测在估计结果中所占的比重。而正常的DVL波束对应的辅助参数不会发生明显变化，使得所提出算法可以在降低错误量测影响的同时保证正常测量的使用。
本发明考虑SINS/DVL紧组合系统中正常的DVL波束测量信息和大误差的波束测量信息同时出现的情形，针对目前现有的组合导航系统鲁棒滤波器对量测信息处理粗糙，导致正常量测信息易丢失的问题，基于多维量测方程分解和统计相似度量提出了新的鲁棒滤波方法。
本发明在将SINS/DVL紧组合导航系统的多维量测方程进行分解的同时，引入统计相似度量(SSM)理论，协助每个波束的测量噪声方差在大测量误差出现时完成各自的自适应更新，最终保证每个波束测量信息处理的独立性。本发明可用于非理想条件下的水下航行器组合导航领域。
本发明的主要优点体现在：
(1)本发明将SINS/DVL紧组合导航系统的量测方程进行分解，并利用统计相似度量测概念建立代价函数，提高了组合导航系统鲁棒信息融合方法在非正常量测出现的情况下对正常量测信息的利用效率；
(2)本发明对非线性代价函数进行迭代求解，降低了数值计算误差；
(3)本发明在建立代价函数和对其进行求解时均将系统考虑为非线性系统，因此本发明所提出的鲁棒信息融合方法拥有着广阔的适用范围，既可用于线性系统，也可以用于非线性系统。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。