CN108931249A

CN108931249A - 基于svd简化的卡尔曼滤波模型的导航方法及系统

Info

Publication number: CN108931249A
Application number: CN201810793901.9A
Authority: CN
Inventors: 陈光武; 杨菊花; 张琳婧; 邢东峰; 李鹏; 李文元; 石建强; 包成启
Original assignee: Lanzhou Jiaotong University
Current assignee: Lanzhou Jiaotong University
Priority date: 2018-07-18
Filing date: 2018-07-18
Publication date: 2018-12-04

Abstract

本发明公开了一种基于SVD简化的卡尔曼滤波模型的导航方法及系统，其中基于SVD简化的卡尔曼滤波模型的导航方法，包括：采集实时数据；数据解析：对采集的实时数据进行解析，得到解析数据；卡尔曼滤波模型的建立；应用SVD简化所述卡尔曼滤波模型，得到SVD简化的卡尔曼滤波模型；数据融合：基于所述SVD简化的卡尔曼滤波模型对所述解析数据进行融合，得到列车的实时数据。应用SVD(奇异值)简化卡尔曼滤波模型，根据所需精度选取阈值，确定非零奇异值的个数，将此作为可观测矩阵的秩，简化卡尔曼滤波的计算量，从而最大程度地保证原信号的完整度。

Description

基于SVD简化的卡尔曼滤波模型的导航方法及系统

技术领域

本发明涉及导航领域，具体地，涉及一种基于SVD简化的卡尔曼滤波模型的导航方法及系统。

背景技术

如今的导航早已不仅仅依赖于天基信息，天基、地基空间的态势感知系统各具特点，卫星导航系统是具有全天时、全天候探测的优势系统，惯性导航是不依赖外界环境具有自主性的系统，所以“天地一体”是导航领域内发展的主要思路，因此组合导航技术在导航行业的影响越来越大。组合导航技术不仅可以克服纯SINS(惯性导航)误差随时间积累的缺陷，在实际工程应用中，当遇到卫星信号失锁的情况下，组合导航也可以依靠SINS来实现导航定位，同时正确的GPS信息因为全天时、全天候的服务所以能够始终不间断地对SINS信息进行修正，如此组合导航系统可以保持较高的导航精度，为更可靠、更安全、更新速率更快的定位技术奠定基础。多信息融合的组合导航系统在众多领域内都有重大应用，包括高超、空天高动态飞行器、水下无人航行器、车辆的自主驾驶以及列车的高精度定位等等，其中的关键技术多信息融合是一大难点。

在惯性导航系统下发展组合导航系统的核心技术是多传感器的信息融合处理算法，而卡尔曼算法是信息融合中的一种常用算法，通常被称作最优估计，但是由于计算过程中的增益矩阵的维数过大使得难以在工程中达到理论中所说的最优估计。针对此，国内外研究者也作了许多研究，希望通过对卡尔曼模型的简化使得其在工程中的使用更接近理论中的最优。而在组合导航系统的应用中比较常用的做法有二，一为忽略较小的姿态四元数误差，将原滤波器简化为分别估计姿态四元数误差和估计速度、位置速度、位置误差的两个完全独立的降阶滤波器；或者根据整个状态空间的可观和可控去剔除滤波器中较低的状态因子。二为噪声等效思想，简化传感器的误差模型，比如对加速度计、陀螺仪等传感器的误差模型简化，以此降低卡尔曼滤波器的维数来降低计算量。

以上所述的模型简化都遵循一个原则，即最大程度地留原信号的主要信息，并在这个基础上尽量减小计算量，在工程中更易使用，而无论是忽略较小误差、剔除影响程度较低的状态因子或者是噪声的等效思想都一定程度的削弱了原信号的完整度。

发明内容

本发明的目的在于，针对上述问题，提出一种基于SVD简化的卡尔曼滤波模型的导航方法及系统，以实现至少部分的解决现有技术中存在的问题。

为实现上述目的，本发明采用的技术方案是：

一种基于SVD简化的卡尔曼滤波模型的导航方法，包括：

采集实时数据；

数据解析：对采集的实时数据进行解析，得到解析数据；

卡尔曼滤波模型的建立；

应用SVD简化所述卡尔曼滤波模型，得到SVD简化的卡尔曼滤波模型；

数据融合：基于所述SVD简化的卡尔曼滤波模型对所述解析数据进行融合，得到列车的实时数据。

优选的，所述采集实时数据中，所述实时数据包括：

陀螺仪数据、加速度计数据和GPS接收机数据。

优选的，所述GPS接收机数据包括：经度、纬度、高度、东向速度、北向速度和天向速度信息。

优选的，还包括：

数据输出：将所述列车的实时数据输出。

优选的，所述卡尔曼滤波模型的建立，包括:

建立状态方程；

建立量测方程。

优选的，所述建立状态方程包括：选取15维的状态变量；

所述建立量测方程包括：选取6维量测向量。

优选的，所述应用SVD简化所述卡尔曼滤波模型，包括：

应用SVD分解所述15维的状态变量；

基于SVD分解后得到的数据进行定义同维线性方程；

基于定义的同维线性方程改写协方差矩阵；

基于改写的协方差矩阵得到新的状态变量方程；

基于新的状态变量方程改写增益矩阵；

基于改写后的增益矩阵得到新的观测序列方程；

将所述6维量测向量代入所述新的观测序列方程，得到SVD简化的卡尔曼滤波模型。

优选的，所述数据融合，包括：

预估：建立时间更新来对当前状态进行先验估计，以此推算当前状态变量和误差协方差估计的值，得到预估值；

校正：利用量测更新建立起对当前状态校正后的后验估计，得到后验估计值；

基于所述预估值和所述后验估计值利用得到SVD简化的卡尔曼滤波模型进行迭代，得到最优估计值。

本发明还公开了一种基于SVD简化的卡尔曼滤波模型的导航系统，包括：

抗多径天线模块、GPS接收机、SINS惯性导航模块、数据融合模块和数据输出模块，所述抗多径天线模块与所述GPS接收机电连接，所述GPS接收机与所述数据融合模块电连接，所述SINS惯性导航模块与所述数据融合模块电连接，所述数据融合模块与所述数据输出模块电连接。

优选的，所述SINS惯性导航模块，包括惯性测量单元，所述惯性测量单元包含有信号的预处理电路。

本发明的技术方案具有以下有益效果：

本发明的技术方案，应用SVD(奇异值)简化卡尔曼滤波模型，因为计算总是存在误差的，而用奇异值分解法计算奇异值时，理论上为零的奇异值不可能为零，而是一个非常小的数，此时根据所需精度选取阈值，确定非零奇异值的个数，将此作为可观测矩阵的秩，简化卡尔曼滤波的计算量，从而最大程度地保证原信号的完整度。

同时还具有以下优点：

下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。

附图说明

图1为本发明实施例所述的基于SVD简化的卡尔曼滤波模型的导航方法的流程图；

图2为本发明实施例所述的基于SVD简化的卡尔曼滤波模型的导航系统的原理框图；

图3为本发明实施例所述的基于SVD简化的卡尔曼滤波模型的导航方法的计算过程框图；

图4为本发明实施例所述的基于SVD简化的卡尔曼滤波模型的框图；

图5为本发明实施例所述的基于SVD简化的卡尔曼滤波算法的框图。

具体实施方式

以下结合附图对本发明的优选实施例进行说明，应当理解，此处所描述的优选实施例仅用于说明和解释本发明，并不用于限定本发明。

如图1所示，一种基于SVD简化的卡尔曼滤波模型的导航方法，包括：

S101：采集实时数据；实时数据包括三轴(x,y,z)陀螺仪数据、三轴(x,y,z)加速度计数据和GPS接收机数据。

S102：数据解析：对采集的实时数据进行解析，得到解析数据，解析数据包括陀螺仪数据、加速度计数据和GPS接收机数据。

S103：卡尔曼滤波模型的建立；

S104：应用SVD简化卡尔曼滤波模型，得到SVD简化的卡尔曼滤波模型；

S105：数据融合：基于SVD简化的卡尔曼滤波模型对解析数据进行融合，得到列车的实时数据。

优选方案中，GPS接收机数据包括：经度、纬度、高度、东向速度、北向速度和天向速度信息。

优选的，还包括：S106：数据输出：将列车的实时数据输出。将列车的实时速度、位置作为最终要输出到数据输出板的信息。

优选的，卡尔曼滤波模型的建立，包括:

建立状态方程；

建立量测方程。

优选的，建立状态方程包括：选取15维的状态变量；

建立量测方程包括：选取6维量测向量。

1、建立状态方程。X_k＝φ_k,k-1X_k-1+Γ_k-1W_k-1，其中X是n维系统状态向量，φ为n×n维系统矩阵，Γ为n×r维系统噪声矩阵。本发明选取了15维的状态变量，X(t)＝[φ_e φ_n φ_uδv_e δv_n δv_u δ_x δ_y δ_z ε_bx ε_by ε_bz Δ_ax Δ_ay Δ_az]，其中，

15维的状态变量从左至右依次为3维数学平台失准角,单位为角分；载体的东向、北向和天向速度误差，单位米/秒；纬度误差、经度误差，单位弧度(rad)，高度误差，单位为米；3维陀螺常值漂移，单位度/小时；3维加速度计随机常值零偏，单位为ug。

e、n、u分别代表动态列车坐标轴的东向、北向和天向位置，状态变量分别对应坐标轴的平台角误差；列车运动速度的误差；位置误差，经纬度和高度；陀螺仪随机漂移；加速度计零偏值。

2、建立量测方程。观测模型为Z_k＝H_kX_k+V_k，观测序列Z_k的动态模型为Z(k+1)＝A(k)X(k)+ω(k)，其中Z为m维量测向量，H为m×n维量测矩阵，ω(k)为动态模型噪声，V_k为观测模型噪声，A(k)为状态转移矩阵。本发明选取了6维量测向量，取SINS和GNSS输出的位置和速度之差作为量测量，具体包括三维速度误差向量和三维位置误差向量。

3、卡尔曼滤波满足的统计特性。(1)卡尔曼滤波以估计的均方误差值最小为准则，即式中X作为系统的状态向量，是在忽略量测Z的情况下用卡尔曼迭代得到的最优估计值，是用非卡尔曼滤波得到的估计值，可以看出卡尔曼滤波作为最优估计，其均方误差必须小于其他滤波方式的均方误差才有意义。

(2)卡尔曼滤波是无偏估计，即估计误差的均值为零。

优选的，应用SVD简化卡尔曼滤波模型，包括：

应用SVD分解15维的状态变量；

基于SVD分解后得到的数据进行定义同维线性方程；

基于定义的同维线性方程改写协方差矩阵；

基于改写的协方差矩阵得到新的状态变量方程；

基于新的状态变量方程改写增益矩阵；

基于改写后的增益矩阵得到新的观测序列方程；

将6维量测向量代入新的观测序列方程，得到SVD简化的卡尔曼滤波模型。

应用SVD简化卡尔曼滤波模型具体的为：

对于一个m×n的矩阵M，设其秩为r，且m≥n，则存在一个m×n阶的正交矩阵U和一个n×n阶的正交矩阵V使得(1)式成立。

式中，∑＝diag(λ₁ λ₂ … λ_r),r≤n，且λ₁≥λ₂≥…≥λ_r＞0。通常在卡尔曼滤波算法中的观测数据都带有随机误差，即意味着矩阵为满秩，则(1)式又可表达为

令M＝S+W，则

∑1和∑2中的奇异值是按照降序排列的，因此(3)式和(4)式分别代表了矩阵M的主要成分和非主要成分，而要使得S尽可能地代表M，则W必须尽可能地小。将W看作是S的截差部分，而根据确定的允许截差，就可以唯一地确定∑1中奇异值的个数，这个个数就是有效表达数据矩阵的秩。

由上述卡尔曼模型建立的状态向量为X，则设为预定义的时不变均值状态向量，X'为含有较少维数的关于主要成分的向量，c是含有较少维数的关于次要成分的向量，定义同为线性变换的B和Z，将X和c分别与X'联系起来：

如此，原状态向量X的协方差阵P就可由舍去次要成分的向量X'的协方差阵P'和只含次要成分的向量c的协方差阵Δ来表示，即：

P＝BP'B^T+ZΔZ^T (6)，

设B^*、Z^*分别为变换B和Z的伪逆，伪逆定义为：B^*B＝I,Z^*Z＝I，其中I为单位阵，则式(5)可改写为：

将式(5)、(7)代入卡尔曼滤波器的动态观测模型中得到：

舍去次要部分得到：

进一步改写为：

X'(t+1)≈A'X'(t)+w'(t) (10)，

式中，A'＝B^*AB，

假定舍去误差对系统并无较大影响，则在卡尔曼滤波中，增益矩阵K还可表示为：

K＝PH^TR^-1 (11)，

将式(6)代入到式(11)中得到：

K＝BP'B^TH^TR^-1+ZΔZ^TH^TR^-1 (12)，

舍去次要部分得到：

K≈BP'B^TH^TR^-1 (13)，

将式(5)代入到卡尔曼滤波器的观测模型中得到：

令H'＝HB，舍去次要，则式(14)改写为

Y≈H'X'+v' (15)，

依据上述简化过程，(8)式舍去次要项

B^*Zc(t+1)-B^*AZc(t)＝0 (16)，

改写式(16)得到：

c(t+1)＝Z^*AZc(t) (17)，

则式(17)则是舍去部分的状态转移方程。在式(12)中，舍去的增益矩阵部分用K_ε表示为

K_ε＝ZΔZ^TH^TR^-1 (18)，

所得到的上述各主要部分的公式，从本质上只考虑了预报误差协方差阵中的主要部分，即存在下式：

P≈BP'B^T (19)，

而舍去部分用Px表示为：

Px＝ZΔZ^T (20)，

式中：Δ＝E[cc^T]，在由式(14)到(15)的过程中，舍去的部分用Yx表示为：

Yx＝HZc (21)，

综上，既得到了由主要部分表达的新的状态转移方程，而且得到了它们舍去部分的具体表达形式。

优选的，数据融合，包括：

基于预估值和后验估计值利用得到SVD简化的卡尔曼滤波模型进行迭代，得到最优估计值。

应用SVD简化卡尔曼滤波模型优点为确定系统可观测性的同时也能确定观测状态的可观程度。

应用SVD简化卡尔曼滤波模型优点为确定模型主要成分得到数据融合后的信息之外还能得到舍去误差的具体方程。

如图2所示，本发明还公开了一种基于SVD简化的卡尔曼滤波模型的导航系统，包括：

抗多径天线模块、GPS接收机、SINS惯性导航模块、数据融合模块和数据输出模块，抗多径天线模块与GPS接收机电连接，GPS接收机与数据融合模块电连接，SINS惯性导航模块与数据融合模块电连接，数据融合模块与数据输出模块电连接。

在一个具体的实施例中，抗多径天线模块、GPS接收机、SINS惯性导航模块和数据融合模块为两路，且两路均独立工作。

在一个具体的实施例中，数据输出模块电连接车载计算机，系统还包括安全电源模块分别为GPS接收机、SINS惯性导航模块、车载计算机提供电源。

抗多径天线模块可以提高GPS接收机的距离测量精度，使得整个系统具有一定的抗干扰性。

优选的，SINS惯性导航模块，包括惯性测量单元(IMU)，惯性测量单元包括三轴加速度计、三轴陀螺。

IMU中包含有信号的预处理电路，具体作用为对三轴速度计和三轴陀螺的信号进行预处理，包括剔除粗大误差和使数据平滑，相当于对传感器采集的实时数据进行实时滤波，IMU通过CPCI连接器将采集的数据输送至SINS惯性导航模块中的数据解算模块，IMU与数据解算模块采用RS232串行通信连接，数据解算模块的处理器型号为STM32F103ZET6。

SINS惯性导航模块中的数据解算模块，具体为利用三轴陀螺仪检测到的载体相对于惯性参考坐标系的角速度信号和三轴加计检测到的载体在惯性参考坐标系中的三轴加速度信号经过解算得到载体的速度、位置信息。

数据融合模块的功能包括GPS/SINS的信息融合和定位的数据修正，数据融合模块的处理器型号为TMS320F28335。

GPS接收机包括卫星天线、卫星数据采集板卡和无线传输模块，卫星数据采集板卡采用的是K700。无线传输模块型号为WG-8010。

GPS接收机输出的数据包括卫星RTK数据，卫星RTK数据包括：经度、纬度、高度、东向速度、北向速度和天向速度。数据融合模块与数据输出模块采用CPCI连接器，利用RS232串行通信连接。

数据输出模块的功能是将接收到的经过融合后的数据发送至车载计算机。数据输出模块采用的微处理器型号为STM32F103ZET6。

本发明所研究的基于SVD简化的卡尔曼滤波模型的组合导航系统，核心技术是融合不同冗余度的定位信息，提出了一种简化的卡尔曼滤波算法，具有一定的工程实用价值。系统中有两套完全相同的结构，且各自独立，输入包括惯性测量单元输出的角速率信息和加速度信息以及GPS接收机输出的经纬度、高度和东北天向速度信息。

本发明研究的基于SVD简化的卡尔曼滤波模型的组合导航系统如图2所示，系统模块包括：抗多径天线模块、GPS接收机、SINS惯性导航模块、数据融合模块和数据输出模块，抗多径天线模块与GPS接收机电连接，GPS接收机与数据融合模块电连接，SINS惯性导航模块与数据融合模块电连接，数据融合模块与数据输出模块电连接。

并在系统的试验中连接车载计算机。系统的各个模块工作独立，包括各部分数据的采集、解析和解算，并且分别都由安全电源供电。整个完整的系统包括两套子系统，两套子系统功能和性能都相同，是为了在实际环境中拥有更强的稳定性，以防止其中某套系统突然出问题而导致不必要的损失。

本发明所研究的基于SVD简化的卡尔曼滤波模型的导航方法，GPS接收机、SINS惯性导航模块和数据融合模块同时工作，此时GPS接收机接收到的是卫星RTK的高精度定位数据，然后使用SINS惯性导航模块进行解算，得到惯导计算后的速度与位置信息，将惯导的速度与位置信息与卫星RTK的速度与位置信息经过数据融合模块融合，得到惯导系统的位置、速度、姿态的修正值，与此同时，将解算得到最终的列车位置、姿态、速度信息由数据输出模块输出。

如图4所示，本发明所研究的基于SVD简化的卡尔曼滤波模型的导航方法,数据融合算法使用卡尔曼滤波。基于SVD简化的卡尔曼滤波模型思路为将卡尔曼滤波器的状态空间信息改写为用主次部分分别表达的形式，通过次要部分得到滤波器误差的状态转移公式，通过简化后的公式计算出少维数的协方差矩阵，此时得到的协方差矩阵只包含主要信息，最后通过状态转移矩阵将只包含主要信息的协方差矩阵转移到原始滤波器的增益矩阵上，降低增益矩阵的计算量以此达到简化卡尔曼滤波模型的目的。

如图5所示，本发明所研究的基于SVD简化的卡尔曼滤波模型的导航方法中的卡尔曼滤波模型的算法框图，卡尔曼滤波器主要包括两个过程：预估与校正。首先建立时间更新来对当前系统的状态进行先验估计，如图左边红框中的内容，以此推算当前状态变量值；然后利用量测更新建立起对当前状态校正后的后验估计，利用简化后的增益矩阵推算出误差协方差估计的值，如图右边红框中内容。依次进行迭代，最终得到所建立的组合导航数学模型的最优估计值。

本发明应用于车载导航系统，基于SVD简化的卡尔曼滤波模型的思路是，将卡尔曼滤波器的状态空间信息改写为用主次部分表达的形式，通过次要部分得到滤波器误差的状态转移公式，通过简化后的公式计算出少维数的协方差矩阵，此时得到的协方差矩阵只包含主要信息，最后通过状态转移矩阵将只包含主要信息的协方差矩阵转移到原始的增益矩阵上，以此达到简化卡尔曼滤波模型的目的。

本发明技术方案还具有以下优点：

1、本发明提出的基于SVD简化的卡尔曼滤波模型的导航系统能够在卫星信号异常的条件下提供较高精度的列车定位结果，并且由于多了多径抗干扰天线模块，使得系统能够一定程度地克服多径效应。

2、本发明提出的基于SVD简化的卡尔曼滤波模型的导航系统采用两套完全相同的系统，利用冗余信息提高组合导航在车载定位中的可靠性与安全性。并且各模块独立工作，具有一定抗干扰性。

3、本发明提出的基于SVD简化的卡尔曼滤波模型的导航方法可以实时融合不同冗余的多传感器数据，属于组合导航领域的核心技术。

4、本发明提出的基于SVD简化的卡尔曼滤波模型的导航方法，在确定系统可观测性的同时，也可确定系统可观测状态的可观程度。

5、本发明提出的基于SVD简化的卡尔曼滤波模型的导航方法是一种简化的滤波方案，给车载计算机的计算量减少了矩阵迭代，提高了组合导航系统的实时性。

最后应说明的是：以上所述仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims

1.一种基于SVD简化的卡尔曼滤波模型的导航方法，其特征在于，包括：

采集实时数据；

数据解析：对采集的实时数据进行解析，得到解析数据；

卡尔曼滤波模型的建立；

2.根据权利要求1所述的基于SVD简化的卡尔曼滤波模型的导航方法，其特征在于，所述采集实时数据中，所述实时数据包括：

陀螺仪数据、加速度计数据和GPS接收机数据。

3.根据权利要求2所述的基于SVD简化的卡尔曼滤波模型的导航方法，其特征在于，所述GPS接收机数据包括：经度、纬度、高度、东向速度、北向速度和天向速度信息。

4.根据权利要求1所述的基于SVD简化的卡尔曼滤波模型的导航方法，其特征在于，还包括：

数据输出：将所述列车的实时数据输出。

5.根据权利要求1所述的基于SVD简化的卡尔曼滤波模型的导航方法，其特征在于，所述卡尔曼滤波模型的建立，包括:

建立状态方程；

建立量测方程。

6.根据权利要求5所述的基于SVD简化的卡尔曼滤波模型的导航方法，其特征在于，

所述建立状态方程包括：选取15维的状态变量；

所述建立量测方程包括：选取6维量测向量。

7.根据权利要求6所述的基于SVD简化的卡尔曼滤波模型的导航方法，其特征在于，所述应用SVD简化所述卡尔曼滤波模型，包括：

应用SVD分解所述15维的状态变量；

基于SVD分解后得到的数据进行定义同维线性方程；

基于定义的同维线性方程改写协方差矩阵；

基于改写的协方差矩阵得到新的状态变量方程；

基于新的状态变量方程改写增益矩阵；

基于改写后的增益矩阵得到新的观测序列方程；

8.根据权利要求1所述的基于SVD简化的卡尔曼滤波模型的导航方法，其特征在于，所述数据融合，包括：

9.一种基于SVD简化的卡尔曼滤波模型的导航系统，其特征在于，运行权利要求1至8任一所述的方法，包括：

10.一种基于SVD简化的卡尔曼滤波模型的导航系统，其特征在于，所述SINS惯性导航模块，包括惯性测量单元，所述惯性测量单元包含有信号的预处理电路。