[发明专利]适用于位姿测量卡尔曼滤波的滤波器更新信息筛选方法

说明书

本发明涉及一种适用于位姿测量卡尔曼滤波的滤波器更新信息筛选方法，包括：持续获取并存储带有时间戳信息的IMU测量数据、IMU解算结果和外部位姿观测结果；选取最新获取的和临近的两帧外部位姿观测结果，按照时间戳最接近再选取出两帧IMU解算结果；根据外部位姿观测结果和IMU解算结果求取相对偏差；相对偏差不超出偏差阈值范围，则将最新获取外部位姿观测结果作为外部观测量，进行卡尔曼滤波器状态向量的更新；利用更新的对应时刻的卡尔曼滤波器状态向量和该时刻之后已存储的IMU测量数据重新进行捷联惯导解算，更新位姿测量结果。本发明提升位姿测量结果准确性，既保证有效的观测数据及时接入滤波器，又避免不合理的观测数据对滤波器的破坏。

技术领域

本发明涉及位姿测量和卡尔曼滤波技术领域，尤其涉及一种适用于位姿测量卡尔曼滤波的滤波器更新信息筛选方法。

背景技术

由于惯性器件的自身特性，纯惯性位姿测量会随着传感器漂移而产生累积误差。在惯性系中，通过由GNSS、重力与磁强计、视觉、雷达等外部位姿传感器提供位姿观测信息，通过设计卡尔曼滤波器，实时修正IMU零偏以及积分误差，得到准确的测姿结果。而外部位姿传感器提供的外部观测更新数据可能由于环境因素干扰存在异常输出，如果不加甄别直接引入滤波器中可能造成滤波器发散。同时，在恶劣应用环境下，可能无法保证准时获取观测信息，观测信息中断一段时间重新恢复时，需要对观测信息的准确性进行判定。基于上述应用条件，需要建立适当的约束条件以提升位姿测量结果的准确性，即能保证有效的观测数据能够及时接入滤波器，又能避免不合理的观测数据对滤波器的破坏。

发明内容

鉴于上述的分析，本发明旨在提供一种适用于位姿测量卡尔曼滤波的滤波器更新信息筛选方法，提升位姿测量结果的准确性。

本发明提供的技术方案是：

本发明公开了一种适用于位姿测量卡尔曼滤波的滤波器更新信息筛选方法，包括以下步骤：

在进行位姿测量的过程中，持续获取并存储带有时间戳信息的IMU测量数据、IMU解算结果和外部位姿传感器的外部位姿观测结果；

选取最新获取的和与之时间戳临近的共两帧外部位姿观测结果；并按照时间戳信息最接近要求，再选取出两帧对应的IMU解算结果；

根据两帧外部位姿观测结果和对应的两帧IMU解算结果求取外部位姿测量与惯性测量的相对偏差；

判断述相对偏差是否超出偏差阈值范围；不超出，则将最新获取的外部位姿观测结果作为卡尔曼滤波的外部观测量，进行该时刻卡尔曼滤波器状态向量的更新；

利用更新的卡尔曼滤波器状态向量和该时刻之后已存储的IMU测量数据重新进行捷联惯导解算，更新位姿测量结果。

进一步地，所述偏差阈值范围为[-2RMSD_k,2RMSD_k]；RMSD_k为均方根偏差基准；

所述其中，D_i，i＝k-n-1,…,k-1，为k时刻前一定时间内，n组有效的相对偏差；

当相对偏差D_k∈[-2RMSD_k,2RMSD_k]时，则将k时刻获取的外部位姿观测结果作为卡尔曼滤波的外部观测量，进行该时刻卡尔曼滤波器状态向量的更新，同时，所述D_k计入k时刻的均方根偏差基准的更新；否则，所述外部位姿观测结果作为无效数据丢弃不用，所述D_k不计入均方根偏差基准。

进一步地，所述相对偏差为根据相邻两帧外部位姿观测结果计算的相对位姿变换矩阵；为与所述两帧相邻外部位姿观测结果时刻最接近的两帧IMU解算结果计算的相对位姿变换矩阵。

进一步地，所述位姿测量为基于双IMU差分测量的被测物体相对于运动载体的相对位姿；