一种基于扩展卡尔曼粒子滤波的导航方法及系统与流程

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

背景技术：

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

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

技术实现要素：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

建立状态方程；

建立观测方程。

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

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

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

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

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

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

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

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

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

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

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

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

可选地，还包括：

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

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

状态方程建立模块，用于建立状态方程；

观测方程建立模块，用于建立观测方程。

本发明提出的一种基于扩展卡尔曼粒子滤波的导航方法及系统，该方法包括：通过全球定位系统获取车辆的姿态信息；通过安装在车辆上的惯性测量单元获取所述车辆的行走信息；对所述姿态信息和所述行走信息进行扩展卡尔曼粒子滤波处理；计算得到所述车辆的位置信息，通过所述位置信息对所述车辆进行导航，通过车辆内的全球定位系统和惯性测量单元采集车辆的姿态信息和行走信息，并通过扩展卡尔曼粒子滤波来构造粒子的建议分布函数，实时融入最新观测值，提高了车辆位置信息的精确度和稳定性。

附图说明

图1为本发明实施例一提供的一种基于扩展卡尔曼粒子滤波的导航方法流程图；

图2为本发明实施例一提供的另一种基于扩展卡尔曼粒子滤波的导航方法流程图；

图3为图1中步骤s30的具体方法流程图；

图4为本发明实施例二提供的一种基于扩展卡尔曼粒子滤波的导航示范性结构框图；

图5为本发明实施例二提供的另一种基于扩展卡尔曼粒子滤波的导航示范性结构框图；

图6为图4中滤波处理模块示范性结构框图。

本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。

具体实施方式

应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

在后续的描述中，使用用于表示元件的诸如“模块”、“部件”或“单元”的后缀仅为了有利于本发明的说明，其本身并没有特定的意义。因此，"模块"与"部件"可以混合地使用。

实施例一

如图1所示，在本实施例中，一种基于扩展卡尔曼粒子滤波的导航方法，包括：

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

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

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

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

在本实施例中，通过车辆内的全球定位系统gps和惯性测量单元imu采集车辆的姿态信息和行走信息，并通过扩展卡尔曼粒子滤波来构造粒子的建议分布函数，实时融入最新观测值，提高了车辆位置信息的精确度和稳定性。

在本实施例中，该导航方法适用于室外导航，所述车辆包括：小型汽车、智能小车等，所述车辆上设置有用于安装全球定位系统的载体，如：pda、车载导航、手机、笔记本电脑、mp4及数码相机等。

在本实施例中，所述姿态信息包括：经度、维度、高度、时间和三轴速度。

在本实施例中，所述惯性测量单元包括三轴加速度计和三轴陀螺仪，所述行走信息包括由所述三轴加速度计测量得到的三轴比力信息和由所述三轴陀螺仪测量得到的三轴角速度，其中，三轴加速度计相互垂直安装，通过测量到的比力信息可以计算得到运动加速度；而通过三轴陀螺仪测量到的三轴角速度可以判别车辆的运动状态。

如图2所示，在本实施例中，所述步骤s30之前还包括：

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

s22、对补偿后的行走信息进行姿态解算，所述姿态解算包括：姿态更新、比力坐标转换和四元数更新；其中所述比力坐标转换是指将载体坐标转换到东北天坐标系，在东北天坐标系中，以x轴表示东，以y轴表示北，以z轴表示天；

s23、建立状态方程：

将状态量表示为：

其中，vevnvu表示载体东北天速度，λh表示经度、纬度、高度，φeφnφu表示姿态误差，dedndu表示三轴陀螺仪常值漂移，aeanau表示三轴加速度。

系统的力学编排方程式表示为：

其中，rn表示卯酉圈曲率半径，rm表示子午圈曲率半径。

姿态误差方程表示为：

由上述方程可以得到状态方程：

s24、建立观测方程：

选取gps输出的载体的三轴速度和经度纬度高度为观测信息，表示为：

则观测方程为：

zk＝h(xk,vk)。

通过上述的状态方程和观测方程，可以在滤波过程中实施融入最新观测值，提高了导航的精确度和稳定性。

如图3所示，在本实施例中，所述步骤s30包括：

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

其中，所述已知的概率密度函数表示为p(x0)，根据p(x0)分布采样得到权值为

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

其中，扩展卡尔曼滤波算法通过ekf(extendedkalmanfilter，扩展卡尔曼滤波器)来实现；

非线性状态转移函数的局部线性化函数如下：

非线性观测函数的局部线性化函数如下

其中，更新采样粒子包括时间更新和测量更新：

时间更新以函数表示为：

为第i个粒子在第k-1时刻的误差协方差矩阵，为第i个粒子在第k时刻的误差协方差矩阵一步预测值，qk-1为k-1时刻的过程噪声。

测量更新以函数表示为：

rk为观测噪声，为第i个粒子在t时刻的增益矩阵，yk为t时刻的观测向量，为第i个粒子在t时刻的误差协方差矩阵。

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

其中，对于k∈{1,2，...,m}，采样

重要性权值更新为：

权值归一化以函数表示为：

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

定义有效采样大小当neff＜nth时，从集合中根据重要性权值重新采样得到n个粒子集合重新分配权值

状态估计函数表示为：

采用本发明所述方法，与现有技术相比，解决了卡尔曼滤波不能适用于非线性的过程，而且提高了解算精度等。

实施例二

如图4所示，在本实施例中，一种基于扩展卡尔曼粒子滤波的导航系统，包括：

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

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

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

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

在本实施例中，通过车辆内的全球定位系统gps和惯性测量单元imu采集车辆的姿态信息和行走信息，并通过扩展卡尔曼粒子滤波来构造粒子的建议分布函数，实时融入最新观测值，提高了车辆位置信息的精确度和稳定性。

在本实施例中，该导航方法适用于室外导航，所述车辆包括：小型汽车、智能小车等，所述车辆上设置有用于安装全球定位系统的载体，如：pda、车载导航、手机、笔记本电脑、mp4及数码相机等。

在本实施例中，所述姿态信息包括：经度、维度、高度、时间和三轴速度。

在本实施例中，所述惯性测量单元包括三轴加速度计和三轴陀螺仪，所述行走信息包括由所述三轴加速度计测量得到的三轴比力信息和由所述三轴陀螺仪测量得到的三轴角速度，其中，三轴加速度计相互垂直安装，通过测量到的比力信息可以计算得到运动加速度；而通过三轴陀螺仪测量到的三轴角速度可以判别车辆的运动状态。

如图5所示，在本实施例中，一种基于扩展卡尔曼粒子滤波的导航系统还包括：

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

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

状态方程建立模块70，用于建立状态方程；

将状态量表示为：

其中，vevnvu表示载体东北天速度，λh表示经度、纬度、高度，φeφnφu表示姿态误差，dedndu表示三轴陀螺仪常值漂移，aeanau表示三轴加速度。

系统的力学编排方程式表示为：

其中，rn表示卯酉圈曲率半径，rm表示子午圈曲率半径。

姿态误差方程表示为：

由上述方程可以得到状态方程：

观测方程建立模块80，用于建立观测方程；

选取gps输出的载体的三轴速度和经度纬度高度为观测信息，表示为：

则观测方程为：

zk＝h(xk,vk)。

通过上述的状态方程和观测方程，可以在滤波过程中实施融入最新观测值，提高了导航的精确度和稳定性。

如图6所示，在本实施例中，所述滤波处理模块包括：

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

其中，所述已知的概率密度函数表示为p(x0)，根据p(x0)分布采样得到权值为

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

其中，扩展卡尔曼滤波算法通过ekf(extendedkalmanfilter，扩展卡尔曼滤波器)来实现；

非线性状态转移函数的局部线性化函数如下：

非线性观测函数的局部线性化函数如下

其中，更新采样粒子包括时间更新和测量更新：

时间更新以函数表示为：

为第i个粒子在第k-1时刻的误差协方差矩阵，为第i个粒子在第k时刻的误差协方差矩阵一步预测值，qk-1为k-1时刻的过程噪声。

测量更新以函数表示为：

rk为观测噪声，为第i个粒子在t时刻的增益矩阵，yk为t时刻的观测向量，为第i个粒子在t时刻的误差协方差矩阵。

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

其中，对于k∈{1,2，...,m}，采样

重要性权值更新为：

权值归一化以函数表示为：

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

定义有效采样大小当neff＜nth时，从集中根据重要性权值重新采样得到n个粒子集合重新分配权值

状态估计函数表示为：

采用本发明所述的系统，与现有技术相比，解决了卡尔曼滤波不能适用于非线性的过程，而且提高了解算精度等。

需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者装置不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者装置所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、方法、物品或者装置中还存在另外的相同要素。

上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述实施例方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质(如rom/ram、磁碟、光盘)中，包括若干指令用以使得一台终端设备(可以是手机，计算机，服务器，空调器，或者网络设备等)执行本发明各个实施例所述的方法。

以上仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。