一种基于级联自适应鲁棒联邦滤波的车载导航计算方法与流程

本发明属于车载导航技术领域，具体涉及一种基于级联自适应鲁棒联邦滤波的车载导航计算方法。

背景技术：

捷联式惯性导航系统(sins)、全球定位系统(gps)与车载里程计(odometer)的组合既弥补了相互的劣势，又充分利用了自身的优势，可以提供相对精确且全面的导航参数：姿态、速度与位置信息，因此被广泛应用于各种领域的车辆载体中，成为目前比较常用的一种车载导航方式。然而随着环境日益复杂，传统的基于sins/gps/od联邦组合的车载导航方式易受异常量测值和不确定系统先验信息的影响，滤波结果容易发散，导航精度下降。为了使导航系统具有更好的鲁棒性以及导航参数更准确，目前常用的方法有：基于多模型自适应估计(mmae)的方法、基于新息的自适应估计(iae)的方法、基于sage-husa的自适应估计方法以及抗差估计方法等。但以上几种方法或存在计算量大的缺点，或存在解决问题单一的不足，均不能很好的解决目前导航系统存在的问题。

近些年来，一些解决组合导航系统中系统先验信息不确定和量测异常值的方法被提出。文献(anoptimaladaptivekalmanfilter,j.geodesy,vol.80,no.4,pp.177-183,2006.)提出使用衰落因子和鲁棒估计减少动力学模型的影响，但是它的缺点是如果不能计算稳健的估计解，则不能形成衰落因子，并且由于粗线性近似，其性能在非线性情况下降低；文献(hybridkalmanfilter-fuzzylogicadaptivemultisensorydatafusionarchitectures,inproc.42^ndieeeconf.decis.control,hi,usa,dec.2003,pp.5215-5220.)提出了一种基于模糊逻辑的自适应算法，称为模糊卡尔曼滤波器，它能够识别异常测量并校正滤波结果，但是该方法在很大程度上依赖于人的经验，如果经验是未知或错误的，模糊方法将无效；文献(model-basedfiltersfor3-dpositioningofmarinemammalsusingahrsandgps-equippeduavs,ieeetrans.aerosp.electron.syst.,vol.51,no.4,pp.3307-3320,oct.2015)将两个卡尔曼滤波器插入多模型自适应估计框架中，以应对海洋模型在海洋哺乳动物定位中的未知参数问题，然而，这种方法的明显局限性是并行运行的几个卡尔曼滤波器增加了计算复杂度和处理时间；文献(自适应联邦滤波器在gps-ins-odometer组合导航的应用.测绘学报，2016,45(2)：157-163.)与文献(ppp/instightlycouplednavigationusingadaptivefederatedfilter.gpssolutions,2017,21(1):1-12.)均提出一种自适应因子与信息分配因子相结合的自适应联邦卡尔曼滤波结构，在一定程度上解决了传感器量测值异常的问题，但是它们只能达到信息分配调节的作用，当多个传感器同时产生异常值时效果不是很好。文献(smalluavlocalizationbasedstrongtrackingfiltersaugmentedwithinteractingmultiplemodel.appliedsciencesandtechnology(ibcast),201815thinternationalbhurbanconferenceon.ieee,2018:310-317.)使用交互式多模型的强跟踪滤波方法对小型无人机不同的运行状态进行估计，提高了无人机的鲁棒性与精度，但是计算量过大，运行复杂。文献(reaearchonunderwaterintegratednavigationsystembasedonsins/dvl/magnetometer/depth-sensor.oceans2017-aberdeen.ieee,2017:1-6.)提出利用sage-husa自适应滤波对未知或时变统计特性进行估计，但是不能对量测值野值有较好的滤波效果，提高精度不明显。

技术实现要素：

本发明的目的在于提供了一种基于级联自适应鲁棒联邦滤波的车载导航计算方法，克服多传感器组合的车载导航系统中系统先验信息不确定与量测值异常所造成的滤波发散甚至失效的问题，获得全局最优值，从而得到更加精确、鲁棒性更好的导航参数解。

本发明的目的是这样实现的：

一种基于级联自适应鲁棒联邦滤波的车载导航计算方法，具体的实现步骤为：

步骤1.建立sins的导航解算方程以及sins、gps和odometer的误差模型；

步骤2.以残差作为统计量构建三段式的降权函数，利用降权因子对含有量测异常值的量测噪声进行缩减；

步骤3.通过级联方式，在量测噪声r达到稳定的基础上，基于sage-husa自适应滤波器构建系统噪声估计器实时在线估计先验信息q，获得子系统的初步状态最优估计值；

步骤4.对两个导航子系统的最优估计值，根据信息分配原则进行全局融合，获得最终的最优估计值，然后进行反馈过程，重复步骤1至步骤4。

本发明还包括：

步骤2.1.根据观测值与预测观测值的差异，获得系统的残差：

vi＝zk-hkxk,k-1；

vi为观测量对应的残差向量，zk表示系统的m维观测向量；下角标k代表时刻，xk,k-1为时刻介于k与k-1之间的时系统的n维状态向量；

步骤2.2.建立基于残差统计量的三段式降权函数：

其中为vi的均方差，实际情况下，为vi的权倒数，方差因子k0的取值范围分别为1.5～2.5，ki的取值范围3.0～5.0。

所述的步骤2利用降权因子对含有量测异常值的量测噪声进行缩减具体为：原观测值的权为pi，则等加权根据降权因子对含有异常值误差的量测噪声缩减：

所述的步骤3中利用改进的sage-husa自适应滤波器构建系统噪声估计器实时在线估计先验信息q是基于线性离散系统数学模型；所述的线性离散系统为：

其中，xk表示系统k时刻的n维状态向量；zk表示系统k时刻的m维观测向量；φk,k-1为k-1时刻到k时刻的n×n维状态转移矩阵；hk为系统k时刻的m×n维观测矩阵，wk-1和vk为k时刻的系统噪声和量测噪声，其统计特性为：

其中，e{·}表示期望函数；qk和rk表示k时刻的过程噪声和量测噪声的协方差矩阵。

所述的步骤3中获得子系统的初步状态最优估计值具体过程为：

步骤3.1.当rk经过降权得到后，获得k时刻的滤波增益kk：

步骤3.1.1：计算k时刻的状态一步预测

步骤3.1.2：计算状态一步预测的误差协方差矩阵pk,k-1：

其中是由时变噪声统计估值器获得的；

步骤3.1.3：计算k时刻的滤波增益kk：

步骤3.2.获得k时刻的状态最优估计和k时刻的最优状态误差协方差矩阵pk；

pk＝[i-kkhk]pk,k-1

其中，与由以下时变噪声统计估值器获得：

式中dk-2＝(1-b)/(1-b^k-2)，b为遗忘因子，b的取值为0.95<b<0.99。

本发明的有益效果在于：本发明克服了多传感器组合的车载导航系统中系统先验信息不确定与量测值异常所造成的滤波发散甚至失效的问题，获得全局最优值，从而得到更加精确、鲁棒性更好的导航参数解，可以消除粗差所带来的影响，提高了车载导航系统的精度，通过对系统噪声的实时估计，结合异常值的降权处理，可以在提高滤波精度的同时，减小甚至消除粗差与系统噪声变化所带来的影响。

附图说明

图1是基于sins/gps/od组合的车载导航系统框图。

图2是车辆的行驶轨迹图。

图3是gps有粗差时，基于cfkf与carshfkf的车载导航系统的导航参数对比仿真结果。

图4是od有粗差时，基于cfkf与carshfkf的车载导航系统的导航参数对比仿真结果。

图5是gps与od有粗差且系统噪声不确定时，基于cfkf与carshfkf的车载导航系统的导航参数对比仿真结果。

图6是gps与od有粗差且系统噪声不确定时，基于cfkf与carshfkf算法的实测数据对比结果。

图7是仿真实验中仿真轨迹的参数设置。

图8是仿真实验中三种传感器的参数设置。

具体实施方式

下面结合附图对本发明做进一步的描述：

本发明的目的在于克服多传感器组合的车载导航系统中系统先验信息不确定与量测值异常所造成的滤波发散甚至失效的问题，提出一种基于级联自适应鲁棒联邦滤波的车载导航算法。该方法首先在联邦子系统中利用简化版的sage-husa自适应方法对系统的先验信息q进行在线实时估计，将修正后的系统噪声代入卡尔曼滤波系统中，然后根据统计量残差构建降权因子，对含有异常值影响的量测噪声r进行降权，将二者进行级联，即可获得子系统的最优估计值，然后将子系统的最优值代入主滤波器中进行最优融合，获得全局最优值，从而得到更加精确、鲁棒性更好的导航参数解。

实现本发明目的技术方案：

步骤1：建立sins的导航解算方程以及误差模型；

捷联式惯性导航系统的姿态、速度、位置解算过程如下所示：

(1)姿态算法

其中，表示姿态转移矩阵，表示角速率的反对称阵。

(2)速度算法

其中，为加速度计测量的比力在导航系的投影，为由载体运动和地球自转引起的哥氏加速度，为由载体运动引起的对地向心加速度，gⁿ为重力加速度，

(3)位置算法

其中

gⁿ＝[00-g]^t

捷联式惯性导航系统的误差方程如下所示：

(1)姿态误差方程

(2)速度误差方程

(3)位置误差方程

(4)陀螺误差方程

ε＝εb+wg

其中，εb表示常值漂移，wg表示白噪声。

(5)加速度计误差方程

其中，表示常值零偏，wa表示白噪声。

步骤2：以残差作为统计量构建三段式的降权函数，利用降权因子对含有量测异常值的量测噪声进行缩减；

步骤2.1：根据观测值与预测观测值的差异，获得系统的残差

vi＝zk-hkxk,k-1

步骤2.2：建立基于残差统计量的三段式降权函数

其中，vi为观测量对应的残差向量；为vi的均方差，实际情况下，为vi的权倒数；方差因子σ0根据求得；k0与ki的取值范围分别为1.5～2.5和3.0～5.0。

步骤2.3：根据降权函数对含有异常值误差的量测噪声缩减

假设原观测值的权为pi，则等加权

步骤3：通过级联方式，在量测噪声达到稳定的基础上，利用改进的sage-husa自适应滤波器构建系统噪声估计器实时在线估计从而获得子系统的初步状态最优估计值；

步骤3.1：当rk经过降权得到后，获得新的滤波增益

步骤3.2：获得局部最优估计值和协方差矩阵

pk＝[i-kkhk]pk,k-1

步骤3.3：构造系统噪声估计器，对噪声实时在线估计

式中dk-2＝(1-b)/(1-b^k-2)，b为遗忘因子，b的取值为0.95<b<0.99。

步骤4：对两个导航子系统的最优估计值进行全局融合，获得最终的最优估计值，再实行反馈过程，迭代进行。

步骤4.1：滤波初值信息设定。初始协方差矩阵pi,0和系统噪声协方差阵qi,0可以由组合系统初始值来确定，即：

其中，β为子系统的信息分配系数，qg,0这里取粗略值，后续进行估计更新

步骤4.2：时间更新。时间更新在各子滤波器之间独立进行

步骤4.3：量测更新。各子滤波器接收子系统量测信息并独立进行量测更新

pi,k＝(i-ki,khi,k)pi,k/k-1

由于主滤波器不存在量测值，故主滤波器没有量测更新。

步骤4.4：信息分配和全局最优化估计

步骤4.5：信息融合。联邦卡尔曼滤波核心算法即为：把各子滤波器的局部最优估计信息进行融合，从而得到全局最优估计。融合方法可以表述为：

步骤2中降权函数的构造过程如下：

其中，vi为观测量对应的残差向量；为vi的均方差，实际情况下，为vi的权倒数；方差因子σ0根据求得；k0与ki的取值范围分别为1.5～2.5和3.0～5.0。

步骤3中的过程描述如下：

线性离散系统如下：

其中，xk表示系统k时刻的n维状态向量；zk表示系统k时刻的m维观测向量；φk,k-1为k-1时刻到k时刻的n×n维状态转移矩阵；hk为系统k时刻的m×n维观测矩阵，wk-1和vk为k时刻的系统噪声和量测噪声，其统计特性如下所示：

e{wk}＝qk,cov{wkwi}＝qkδkj

e{vk}＝rk,cov{vkvi}＝rkδkj

其中，e{·}表示期望函数；qk和rk表示k时刻的过程噪声和量测噪声的协方差矩阵。

在上述线性离散系统数学模型的基础上，基于sage-husa的系统噪声估计器在线实时估计算法流程如下：

pk＝[i-kkhk]pk,k-1

其中，与由以下时变噪声统计估值器获得：

式中dk-2＝(1-b)/(1-b^k-2)，b为遗忘因子，b的取值为0.95<b<0.99。

上述中的第一个公式求得k时刻的状态一步预测第二个公式求取状态一步预测的误差协方差矩阵pk,k-1，其中的便是由时变噪声统计估值器获得的，第三个公式求取k时刻的滤波增益，其中的便是有降权函数获得的，第四个与第五个公式获得k时刻的状态最优估计第六个公式为k时刻的最优状态误差协方差矩阵pk。

仿真实验采用sins、gps与odometer三种导航传感器，仿真时长为510s，载体的三轴初始姿态角分别设置为：俯仰0°、横滚0°、航向10°，初始速度设置为：0，初始位置为：纬度45.779°、经度126.6705°、高度10m，仿真轨迹的参数设置见图7，仿真轨迹为矩形运动，如图2所示。三种传感器的参数设置见图8，gps与里程计的数据在ins的仿真数据基础上添加一定的误差完成。

从图3可看出，当gps在110s-111s过程中具有100m的位置误差，5m/s的速度误差时，传统的联邦滤波(cfkf)不能很好的解决粗差带来的影响，而本文提出的算法在保持的精度的同时，可以减小甚至消除粗差所带来的导航参数跳变。

从图4可看出，当od在210s-211s过程中具有5m/s的速度误差时，传统的联邦滤波(cfkf)不能很好的解决粗差带来的影响，而本文提出的算法在保持的精度的同时，可以减小甚至消除粗差所带来的导航参数跳变。

从图5可看出，当gps在110s-111s过程中具有100m的位置误差，5m/s的速度误差，od在210s-211s过程中具有5m/s的速度误差时，并且系统噪声在140s-150s发生了变化，设置为10q0，系统噪声的变化会影响异常值的处理过程，通过对系统噪声的实时估计，结合异常值的降权处理，可以在提高滤波精度的同时，减小甚至消除粗差与系统噪声变化所带来的影响。

从图6可看出，在实测数据过程中，当gps与od存在粗差，系统噪声不确定时，会引起滤波结果的跳变，同时使导航结果精度降低，而本文所提出的算法可以消除粗差所带来的影响，提高了车载导航系统的精度。