[发明专利]一种基于扩展卡尔曼粒子滤波的导航方法及系统

说明书

技术领域

本发明涉及导航技术领域，尤其涉及一种基于扩展卡尔曼粒子滤波的导航方法及系统。

背景技术

全球定位系统(GPS，Global Positioning System)是一种星基无线电导航系统，能够提供精确的三维位置、三维速度以及时间信息，但是在动态环境中具有可靠性低、易受干扰等缺点；惯性测量单元(IMU，Inertial measurement unit)具有自主、机动灵活和能够提供多种导航参数的优点，缺点是误差会随着时间迅速累计增长；对GPS和IMU的测量数据进行数据融合滤波时常用的算法有卡尔曼滤波、扩展卡尔曼滤波和粒子滤波等。

CN105588566A公开了一种基于蓝牙与MEMS融合的室内定位系统及方法，这篇专利是利用蓝牙与惯性测量相结合只能运用于室内而不能在室外空旷的范围内适用。CN104408315 A公开了一种基于SINS/GPS组合导航的卡尔曼滤波数值优化方法，这篇专利运用的是卡尔曼滤波方法，卡尔曼的模型适用于线性，而实际模型是非线性非高斯。由于粒子滤波不受非线性、非高斯问题的限制，适用于任何状态模型和测量模型，但是经典的粒子滤波存在一个主要问题是退化现象，在状态转移时没有包含最新的观测信息，随着时间的递增，最后粒子的权重只聚集到少数粒子上，其余的粒子只有微小的归一化权重，导致粒子集无法表达实际的后验概率分布。

发明内容

本发明的主要目的在于提出一种基于扩展卡尔曼粒子滤波的导航方法及系统，能够通过车辆内的全球定位系统和惯性测量单元采集车辆的姿态信息和行走信息，并通过扩展卡尔曼粒子滤波来构造粒子的建议分布函数，实时融入最新观测值，提高了车辆位置信息的精确度和稳定性。

为实现上述目的，本发明提供的一种基于扩展卡尔曼粒子滤波的导航方法，包括：

通过全球定位系统获取车辆的姿态信息；

通过安装在车辆上的惯性测量单元获取所述车辆的行走信息；

对所述姿态信息和所述行走信息进行扩展卡尔曼粒子滤波处理；

计算得到所述车辆的位置信息，通过所述位置信息对所述车辆进行导航。

可选地，所述姿态信息包括：经度、维度、高度、时间和三轴速度。

可选地，所述惯性测量单元包括三轴加速度计和三轴陀螺仪，所述行走信息包括由所述三轴加速度计测量得到的三轴比力信息和由所述三轴陀螺仪测量得到的三轴角速度。

可选地，所述对所述姿态信息和所述行走信息进行扩展卡尔曼粒子滤波处理包括：

根据已知的概率密度函数进行滤波初始化；

用扩展卡尔曼滤波算法更新采样粒子；

进行重要性采样和重要性权值计算；

根据所述重要性权值重新采样，重新分配权值，输出状态估计函数。

可选地，所述对所述姿态信息和所述行走信息进行扩展卡尔曼粒子滤波处理之前还包括：

对所述行走信息进行补偿；

对补偿后的行走信息进行姿态解算，所述姿态解算包括：姿态更新、比力坐标转换和四元数更新；

建立状态方程；

建立观测方程。

作为本发明的另一方面，提供的一种基于扩展卡尔曼粒子滤波的导航系统，包括：

第一获取模块，用于通过全球定位系统获取车辆的姿态信息；

第二获取模块，用于通过安装在车辆上的惯性测量单元获取所述车辆的行走信息；

滤波处理模块，用于对所述姿态信息和所述行走信息进行扩展卡尔曼粒子滤波处理；

导航模块，用于计算得到所述车辆的位置信息，通过所述位置信息对所述车辆进行导航。

可选地，所述姿态信息包括：经度、维度、高度、时间和三轴速度。

可选地，所述惯性测量单元包括三轴加速度计和三轴陀螺仪，所述行走信息包括由所述三轴加速度计测量得到的三轴比力信息和由所述三轴陀螺仪测量得到的三轴角速度。

可选地，所述滤波处理模块包括：

初始化单元，用于根据已知的概率密度函数进行滤波初始化；

更新单元，用于用扩展卡尔曼滤波算法更新采样粒子；

采样计算单元，用于进行重要性采样和重要性权值计算；

重采样单元，用于根据所述重要性权值重新采样，重新分配权值，输出状态估计函数。

可选地，还包括：

补偿模块，用于对所述行走信息进行补偿；

姿态解算模块，用于对补偿后的行走信息进行姿态解算，所述姿态解算包括：姿态更新、比力坐标转换和四元数更新；