[发明专利]一种卡尔曼滤波结合图优化的组合导航后处理方法及系统

说明书

一种卡尔曼滤波结合图优化的组合导航后处理方法及系统。本发明提供了组合导航后处理方法及系统，方法包括以下步骤：获取惯性导航系统输出的比力和角速度，利用惯性导航积分算法对比力和角速度进行积分，得到位置、速度和姿态航向角；对位置、速度和姿态航向角进行前向卡尔曼滤波，根据卡尔曼滤波过程中的数值或矩阵构建方程组；利用图优化方法求解方程组，获得惯性导航系统所有时刻状态误差；利用图优化方法计算得到的该状态误差对所述惯性导航系统进行导航解算，输出导航数据。该方法既能保障导航精度，又能保障处理速度。

技术领域

本发明属于导航技术领域，具体涉及一种卡尔曼滤波结合图优化的组合导航后处理方法及系统。

背景技术

现有的组合导航后处理方法大多数是采用正反向卡尔曼滤波，这种方法容易实现，但是工程应用有其局限性。图1为正向卡尔曼滤波的方法。

卡尔曼滤波由于采用隐式马尔科夫模型，每次迭代必须进行边缘概率密度求解，这样的话既会丢掉很多历史测量信息，又不能用后续的信息对历史的测量进行反向信息传播。因此传统的方法多采用反向卡尔曼滤波加逆向导航解算的方式来提高精度，参见图2，如果不采用这种方式的话，很难把后续的测量信息传递到前方的测量点。但是，由于卡尔曼滤波采用的是不停的求边缘概率密度的方式，由于模型的马尔科夫性质，即假设每个时刻的观测是独立的，如图1所示，Y₁的观测独立于Y₃，即Y₂的观测到来时，就会求第2时刻X₂的边缘概率密度，这样一来第3时刻的Y₃无法影响X₀,X₁,X₂,即使采用了反向滤波相邻较远的观测也很难相互影响。卡尔曼滤波只是一步线性无偏最优估计，没有反向的消息传播。

图优化技术是一门新兴的最优估计技术，采用图模型的方式进行建模，解大规模稀疏状态更新方程的方式进行计算，可以处理大规模的传感器数据。图优化进行问题处理的时候，需要重新建模，将状态空间模型转换为图模型。与卡尔曼滤波相反，图优化处理问题的时候，最希望做的是全局优化，不希望用求边缘概率密度求解的方式来去掉以前的观测量。因此，采用图优化做后处理解算，计算时间会非常长，因为图的规模一直在增长。对组合导航后处理来说，如果每个时刻都用图优化计算所有时刻的概率分布，再用最新时刻的概率分布得到反馈量，对系统进行反馈，计算时间会非常长，软件体验会非常不好。而且现有技术中也缺乏用图优化来针对惯性导航系统与其他传感器组合获得更高精度的具体方案。现有技术多是采用惯性导航系统预积分的方案，用惯导的短时精度来对视觉或者激光相邻帧的信息进行平滑处理。现有技术没有用图优化做高精度组合导航后处理提高精度的具体方案。现有技术关于图优化组合导航的方案都是采用状态估计模式，没有考虑状态反馈模式，即估计都是用因子图优化直接估计导航系统的速度位置姿态等状态，而不是估计速度位置姿态的误差。

因此，现有技术的卡尔曼滤波和图优化组合导航后处理方案，都有其局限性，一个精度不够，一个计算量过大，且没有考虑误差估计方案。

发明内容

针对现有技术中的缺陷，本发明提供一种卡尔曼滤波结合图优化的组合导航后处理方法及系统，既能保障导航精度，又能保障处理速度。

第一方面，一种卡尔曼滤波结合图优化的组合导航后处理方法，包括以下步骤：

获取惯性导航系统输出的比力和角速度，利用惯性导航积分算法对比力和角速度进行积分，得到位置、速度和姿态航向角；

对位置、速度和姿态航向角进行前向卡尔曼滤波，根据卡尔曼滤波过程中的数值或矩阵构建方程组；

利用图优化方法求解方程组，获得惯性导航系统所有时刻的状态误差；

利用图优化方法计算得到的状态误差对所述惯性导航系统进行导航解算，输出导航数据。

优选地，所述方程组为：