CN112683265B - 一种基于快速iss集员滤波的mimu/gps组合导航方法 - Google Patents

Abstract

本发明公开了一种基于快速ISS集员滤波的MIMU/GPS组合导航方法，以输入状态稳定的集员滤波算法为基础，通过寻求次优定界椭球解决方案，对参数的优化目标进行松弛，避免非线性方程的求解，降低算法复杂度，同时保证较高的估计精度，并进一步的证明了快速算法的输入状态稳定性。将快速算法用于MIMU/GPS组合导航试验，结果表明，本发明提出的导航方法有效的提高了系统的导航精度，同时保证了使用的实时性。

Description

一种基于快速ISS集员滤波的MIMU/GPS组合导航方法

技术领域

本发明属于导航技术领域，具体涉及一种基于快速ISS集员滤波的MIMU/GPS组合导航方法。

背景技术

惯性导航系统具有隐蔽性、自主性、宽频性以及信息全面等优势，在军用和民用领域均得到了广泛的应用。其缺点为导航误差随时间累积，特别是对于微型惯性测量单元(Miniature Inertial Measurement Unit，MIMU)这种精度相对较低的惯性系统而言，有必要通过组合导航的方式对导航误差进行校正。其中，应用最多的为MIMU/GPS组合导航。

组合导航需要高性能的导航滤波技术的支撑。目前，导航滤波通常采用概率化滤波方法来解决。这类方法解决了很多应用中的问题，但是，这些概率化方法对噪声的分布都有严格的要求，并要求其统计特性已知，导致这类方法存在一定的缺陷。为解决该问题，近年来，另一类滤波方法，集员估计方法正逐渐受到重视。该类方法只要求噪声未知但有界(Unknown But Bounded,UBB)，由于在实际系统中噪声的边界信息较之概率化假设来说更容易获取，可克服概率化滤波方法在应用中存在的局限性；此外，可以将不确定性包含于集合中进行运算，然后得到状态可行集以及边界的确定性描述。这使得该方法比传统估计方法更具鲁棒性，而且可以非常方便地与需要状态边界信息的控制方法(如鲁棒控制和最优控制等)进行结合，从而可用于后继的航迹规划和制导控制任务之中，以保证载体的高稳定性和高可靠性。

根据集合形状的不同，集员滤波算法可以分为椭球算法、基于区间分析的集员估计算法以及全对称多胞形、超平形体等算法。其中，由于椭球算法具有计算过程相对简单，可递推实现，对数据的处理比较灵活，较强的自适应性，估计结果可以被显式表达并且其边界光滑可导等优点，使其成为一种比较重要的集员滤波算法。椭球集员滤波算法最早由Schweppe提出，他给出了时间更新和量测更新过程中状态可行集的外包椭球族。而后，该类算法得到了迅速的发展，并被应用于并将其应用于室内移动机器人多传感器融合定位、多机器人观测信息融合、移动目标主动持续定位，车辆绝对定位等方面。

但是目前对椭球集员滤波算法本身的稳定性的研究不多，而稳定性对于算法在系统中的应用至关重要。Becis-Aubry提出了一种输入状态稳定(Input-State-Stable,ISS)集员估计算法，虽然该算法在估计精度、稳定性、选择性更新等方面具有优势，但由于在参数优化过程需要求解复杂的非线性方程，因此该算法的计算量较大，在组合导航这种实时性要求高的场合中应用受限。

发明内容

针对上述存在的问题，本发明提供一种基于快速ISS集员滤波的MIMU/GPS组合导航方法，通过寻求次优定界椭球解决方案，降低算法复杂度，同时保证算法的稳定性和较高的估计精度，并将其用于MIMU/GPS组合导航。

为了实现上述目的，本发明目所采用的技术解决方案为：

一种基于快速ISS集员滤波的MIMU/GPS组合导航方法，其特征在于，包括以下步骤：

步骤1：获取GPS卫星系统的位置、速度数据以及MIMU采集的陀螺仪和加速度数据，并利用MIMU采集的数据解算位置、速度、姿态信息；

步骤2：建立MIMU/GPS组合导航系统的离散化状态方程和量测方程：

其中，

和

分别为状态向量和观测向量，且状态向量为15维误差向量；观测向量为GPS位置、误差数据与MIMU解算位置、误差结果的差值；F_k为状态转移矩阵；Γ_k-1为系统噪声分配矩阵；H_k为量测阵；

和

分别为系统噪声和量测噪声，其中，系统噪声元素为各轴的陀螺仪白噪声和加速度计白噪声，量测噪声元素为GPS沿各轴向的距离、速度测量误差，所述这两种噪声属于如下的椭球集合：

其中，Q_k和R_k为已知的正定矩阵，表示噪声椭球集合的形状；

步骤3：采用输入状态稳定的集员滤波ISMF算法，更新椭球；

步骤4：利用基于松弛优化目标的FISMF算法进行参数的求解和状态估计，得到组合导航的误差估计值。

进一步地，步骤3所述的椭球更新分为状态更新阶段和量测更新阶段，不同阶段的椭球更新公式为：

步骤31：在状态更新阶段，更新椭球

其计算公式为：

σ_k|_k-1＝σ_k-1 (8)，

其中，σ_k-1作为系统李雅普诺夫函数的上界，与P_k-1共同决定了k-1时刻状态椭球的形状，p_k∈(0,+∞)可以用来优化椭球E_k|k-1的大小；

步骤32：在量测更新阶段，更新椭球

其计算公式为：

P_k＝(I_n-K_kH_k)P_k|k-1 (9)，

其中，滤波增益

q_k∈[0,+∞)，残差

进一步地，步骤4的具体操作步骤包括：

步骤41：将式(11)转换为：

其中，

而

则由

Cholesky分解得到，即

由式(13)可以得出，σ_k的上界为：

其中，

为

的最大特征值；

步骤42：以

为优化目标，通过式(14)得到参数q_k的最优值的计算公式为：

步骤43：初始化

P₀,σ₀，设定k←1；

步骤44：根据式，和来计算时间更新椭球

步骤45：如果

取P_k＝P_k|k-1，

σ_k＝σ_k|k-1；否则,根据式，和计算P_k，

σ_k，参数q_k根据式计算；

步骤46：令k←k+1并返回步骤44，直到程序终止。

本方法与现有技术相比，具有以下有益效果：

本发明从改进改进参数优化的角度出发，对输入-状态稳定的集员滤波算法进行了改进，在保持算法稳定性的前提下，提出了快速ISMF算法，选择次优的策略，通过最小化σ_k上界的来求取参数，避免了求解非线性方程，极大的提高了算法的效率，使之可以满足组合导航滤波的实时性要求。并且通过实验结果可以看出，本发明的快速算法在运算复杂度和估计精度方面取得了良好的平衡，导航精度显著优于Kalman滤波算法，同时时间消耗远小于ISMF算法，有效提高了MIMU/GPS组合导航的可靠性、精度和实时性。

附图说明

图1为本发明实施例中的松组合试验装置图；

图2(a)-(b)为实施例中两次试验中推车的运动轨迹；

图3为实施例中试验1的推车姿态角；

图4为实施例中试验1的推车位置信息；

图5为实施例中试验1的推车速度信息；

图6为实施例中试验2的推车姿态角；

图7为实施例中试验2的推车位置信息；

图8为实施例中试验2的推车速度信息。

具体实施方式

为了使本领域的普通技术人员能更好的理解本发明的技术方案，下面结合附图和实施例对本发明的技术方案做进一步的描述。

1、系统描述

考虑如下线性离散系统：

x_k＝F_k-1x_k-1+Γ_k-1w_k-1 (1)，

z_k＝H_kx_k+v_k (2)，

其中，

和

分别为状态向量和观测向量，状态向量为15维误差向量，观测向量为GPS位置、误差数据与MIMU解算位置、误差结果的差值；F_k为状态转移矩阵；Γ_k-1为系统噪声分配矩阵；H_k为量测阵；

和

分别为系统噪声和量测噪声，其中，系统噪声元素为各轴的陀螺仪白噪声和加速度计白噪声，观测噪声元素为GPS沿各轴向的距离、速度测量误差。

假设两种噪声属于如下的椭球集合：

其中，Q_k和R_k为已知的正定矩阵，它们定义了噪声椭球集合的形状；

初始状态属于下式所描述的椭球：

其中，

为椭球的中心，P₀为正定矩阵，它定义了椭球的形状，σ₀为大于0的实数；

式(5)中所描述的椭球通常简写为

并且上述所有矩阵的维数均由状态、观测向量及噪声的维数所确定；

2、输入状态稳定的集员滤波(ISMF)

IMSF算法于Kalman滤波类似，其过程分为状态更新和量测更新两个阶段：

第一个阶段：状态更新阶段的估计目标为状态更新椭球

更新计算公式为：

σ_k|k-1＝σ_k-1 (8)，

其中，p_k∈(0,+∞)可以用来优化椭球E_k|k-1的大小，其最优值通过最小化σ_{k|k- 1}P_k|k-1的迹得到。

第二个阶段：量测更新阶段的目标为量测更新椭球

更新计算公式为：

P_k＝(I_n-K_kH_k)P_k|k-1 (9)，

其中，

q_k∈[0,+∞)，

并且，q_k的最优值如下式取值：

其中，

为方程

的唯一正实根，

和

为由H_k、P_k|k-1、R_k和δ_k共同确定的常量。

由上可以看出，参数的求解需要求解非线性方程，对非线性方程的数值解算运算复杂度非常高，这必然对导航的实时性造成不利影响。考虑到算法的高复杂度是由参数的计算过程导致的，所以本发明从参数的优化过程出发，寻求新的方法，以避免非线性方程的求解。

3、基于松弛优化目标的快速ISMF算法(FISMF)

在ISMF算法中，参数的选择是通过最小化σ_k实现的，即该过程中需要求解非线性方程。所以本发明将最小化σ_k松弛为最小化σ_k的上界，以避免非线性方程的求解，从而减少算法复杂度，提高运算效率。

3.1算法推导

首先，将式转换为：

其中，

由式(13)可以得出，σ_k的上界为：

其中，

为

的最大特征值；

以此为基础，经过推导后得到改进后的参数求解过程为：

定理1假设

为σ_k的上界，给定式，以

为优化目标，参数q_k的最优值为：

证明：

根据式，

可以当做q_k的函数，其一阶、二阶导数分别为：

由于P_k|k-1正定，

非奇异，且H_k行满秩，所以

也正定，则

为正，所以

如果

则对所有q_k≥0，

这就意味着

是q_k在区间[0,+∞]的增函数，所以q_k取0时

最小；

如果

则

根据式，显然有

所以

是q_k在区间[0,+∞)的增函数，又

所以

时

最小，可以得到

证毕。

最终，根据上面的证明过程，得到基于松弛优化目标的FISMF算法的执行步骤如下：

Step 1：初始化

P₀,σ₀，设定k←1；

Step 2：根据式，和计算时间更新椭球

Step 3：如果

取P_k＝P_k|k-1，

σ_k＝σ_k|k-1；否则,根据式，和计算P_k，

σ_k，参数q_k根据式计算；

Step 4：令k←k+1并返回Step 2，直到算法终止。

3.2稳定性证明

为了验证优化目标松弛后的FISMF算法依然保持ISMF算法的稳定性，进行以下证明。

输入-状态稳定的具体定义为：

定义：对于系统，z_k＝f(z_k-1,u_k-1)如果存在连续ISS-李雅谱诺夫函数

则称该系统是输入-状态稳定的；

即就是：存在K∞函数μ₁和μ₂，使得对所有

满足μ₁(||z||)≤L(z)≤μ₂(||z||)；存在K∞函数μ₃和K函数χ，使得对所有

满足L(f(z,u))-L(z)≤-μ₃(||z||)+χ(||u||)。

定理2给定式和构成的系统，假设包含状态x_k的椭球

通过FISMF算法计算而得到，且

同时(F_k,H_k)一致可观，令：

则：

L_k为ISS-李雅谱诺夫函数且估计误差

是输入-状态稳定的。

证明：

首先，直接可以得到：

其中，λ(P_k)为矩阵P_k的特征值；

令

由于FISMF算法和ISMF算法更新过程完全相同，只是参数求解过程不同，所以同样可以得到：

L_k-L_k|k-1≤σ_k-σ_k|k-1 (19)，

所以有：

L_k-L_k-1≤L_k|k-1-L_k-1 (21)，

根据状态更新过程，可以得到：

将(22)代入(21)，可以得到：

对比ISS定义和式和式可以说明L_k为ISS-李雅谱诺夫函数且对该系统而言估计误差

是输入-状态稳定的。

证毕。

实施例

1、算例例证

本发明采用松组合模式进行MIMU/GPS组合导航试验，在松组合模式下，GPS和MIMU各自独自工作，然后通过适当的滤波算法对两种系统的数据进行融合，得到最终的导航结果。

本发明采用FISMF算法作为融合算法实现组合导航的误差状态估计，同时将Kalman滤波(KF)和ISMF算法作为对比。系统状态方程中的状态向量共15维，包括维度、经度、高度误差δL、δλ、δh，东向、北向、天向速度误差δV_E、δV_N、δV_U，数学平台姿态角误差δφ_E、δφ_N、δφ_U以及x、y、z轴上的陀螺漂移误差ε_bx、ε_by、ε_bz和加速度计漂移误差B_bx、B_by、B_bz：

x＝[δL,δλ,δh,δV_E,δV_N,δV_U,δφ_E,δφ_N,δφ_U,ε_bx,ε_by,ε_bz,B_bx,B_by,B_bz]^T (24)，

状态方程为：

上式中的系统噪声为w＝[w_gx,w_gy,w_gz,w_ax,w_ay,w_az]^T，并且其中的元素通常假设为x，y，z轴上的陀螺白噪声和加速度计白噪声，为了提高导航的精度和鲁棒性，本发明采用UBB噪声来描述其误差特性。且系统噪声分配矩阵为：

其中，

为由导航坐标系(当地地理坐标系)到载体坐标系的转换矩阵。

状态转移矩阵为

其中

F_w为由MIMU误差方程确定的系数矩阵。

量测方程为：

其中，L、λ、h、V_E、V_N、V_U分别为系统维度、经度、高度和东向、北向、天向速度，而下标I代表MIMU解算值，G代表GPS测量值；R_M、R_N分别为为载体位置对应的子午圈和卯酉圈的曲率半径；量测阵H为6×15维矩阵，其非零元素为H_1,1＝R_M，H_2,2＝R_N，H_3,3＝1，H_4,4＝1，H_5,5＝1，H_6,6＝1；观测噪声为v＝[n_N,n_E,n_U,n_νE,n_νN,n_νU]^T，其中的元素为GPS沿地理坐标系各轴向的距离、速度测量误差；将方程(26)(27)进行离散化，得到(1)(2)描述的离散系统方程，然后用FISMF、KF和ISMF算法进行处理。

系统采用的惯导为HX-IMU-M02惯组件，由MIMU和外部采集板两部分构成。MIMU集成了3轴MEMS陀螺、加速度计、磁强计，同时可以测量外部气压和温度，试验中应用MEMS陀螺和加速度计进行惯性导航解算。MEMS陀螺的零偏稳定性(Allan方差)为6°/h，全寿命周期和全温范围内零偏重复性为0.2°/h；加速度计的零偏稳定性为0.1mg，全寿命周期和全温范围内零偏重复性为16mg。外部采集板在组合导航中主要作用是接收外部GPS秒脉冲，同时标注MIMU输出数据的时间标志。

系统采用的GPS接收机为北斗星通的C200系列接收机，搭配高精度板卡BDM682，可实现1.5m(RMS)的平面单点定位精度和0.03m/s(RMS)的速度精度。实际上该接收机可覆盖北斗、GPS、GLONASS、Galileo等多种全球卫星导航系统(GlobalNavigation SatelliteSystem，GNSS)信号的接收。为检验算法性能，利用上述设备进行了推车试验，试验中，将MIMU和GPS接收机一同放置在试验平台上，GPS天线放置在推车上部，试验装置总体如附图1所示。

试验进行两次，第1次试验推车沿矩形轨迹行进，时间为1100s，第2次试验推车沿8字轨迹行进，时间为1500s，两次试验中推车的运动轨迹分别如附图2(a)和附图2(b)所示。

试验中系统的初始位置、速度误差分别设置为2m、0.1m/s，GPS数据更新率为1Hz，惯导系统数据更新率为200Hz。该系统可以使用信号接收器接收到的多GNSS系统对载体位置和速度进行解算，所以试验中将多系统联合解算的结果作为试验参考值。上位机通过USB接口接受GPS和MIMU数据并利用FISMF算法进行处理，得到组合导航的位置、速度、姿态误差，如附图3-8所示。作为对比，同时也使用ISMF算法和标准Kalman滤波器(KF)以及使用单个模型的联合滤波方法进行了组合导航解算。另外，表1和表2给出了使用不同算法得到的导航误差的统计特性。

表1试验1导航误差统计特性

表2试验2导航误差统计特性

由于推车试验只是在小范围的地面上运动，所以结果中忽略了天向速度和高度。同时，为方便比较，表中位置误差的统计结果用单位米(m)描述。

2、实验结论

通过上述验证过程可以看出，首先，无论是从图中的导航位置速度信息还是表中的误差统计特性来看，本发明提出的方法在MIMU/GPS组合导航系统中能正确导航解算，且两次试验中FISMF、ISMF算法与Kalman滤波相比都具有明显优势。如试验1中采用Kalman滤波导航的最大位置误差达到了10m，且图4显示误差有逐渐增大的趋势，利用FISMF和ISMF方法则将试验1的位置误差限制在了1.5m以内，这充分体现了ISS集员滤波的优势。但是，虽然采用ISMF算法的导航精度得到了大幅提高，其算法的复杂的也大幅增加，导致算法时间消耗增加幅度超过了两个数量级，这严重影响了该方法的适应性。而FISMF算法在提高导航算法精度的同时保证了算法的实时性。

其次，从表1-2中的统计结果可以看出，FISMF算法导航结果与ISMF算法相比虽然精度略有下降(个别参数提高)，但其运算效率与ISMF相比却得到了大幅提升，故而可以看出本发明提出的算法在运算精度和实时性上取得了良好的平衡。

本说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。尽管参照前述实施例对本发明专利进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (1)

1.一种基于快速ISS集员滤波的MIMU/GPS组合导航方法，其特征在于，包括以下步骤：

步骤1：获取GPS卫星系统的位置、速度数据以及MIMU采集的陀螺仪和加速度数据，并利用MIMU采集的数据解算位置、速度、姿态信息；

步骤2：建立MIMU/GPS组合导航系统的离散化状态方程和量测方程：

其中，

和

分别为状态向量和观测向量，且状态向量为15维误差向量；观测向量为GPS位置、误差数据与MIMU解算位置、误差结果的差值；F_k为状态转移矩阵；Γ_k-1为系统噪声分配矩阵；H_k为量测阵；

和

分别为系统噪声和量测噪声，其中，系统噪声元素为各轴的陀螺仪白噪声和加速度计白噪声，量测噪声元素为GPS沿各轴向的距离、速度测量误差，这两种噪声属于如下的椭球集合：

其中，Q_k和R_k为已知的正定矩阵，表示噪声椭球集合的形状；

步骤3：采用输入状态稳定的集员滤波ISMF算法，更新椭球；

步骤4：利用基于松弛优化目标的FISMF算法进行参数的求解和状态估计，得到组合导航的误差估计值；

步骤3所述的椭球更新分为状态更新阶段和量测更新阶段，不同阶段的椭球更新公式为：

步骤31：在状态更新阶段，更新椭球

其计算公式为：

σ_k|k-1＝σ_k-1 (8)，

其中，σ_k-1作为系统李雅普诺夫函数的上界，与P_k-1共同决定了k-1时刻状态椭球的形状，p_k∈(0,+∞)可以用来优化椭球ε_k|k-1的大小；

步骤32：在量测更新阶段，更新椭球

其计算公式为：

P_k＝(I_n-K_kH_k)P_k|k-1 (9)，

其中，滤波增益

残差

步骤4的具体操作步骤包括：

步骤41：将式(11)转换为：

其中，

而

则由

Cholesky分解得到，即

由式(13)可以得出，σ_k的上界为：

其中，

为

的最大特征值；

步骤42：以

为优化目标，通过式(14)得到参数q_k的最优值的计算公式为：

步骤43：初始化

P₀,σ₀，设定k←1；

步骤44：根据式(6)，(7)和(8)来计算时间更新椭球

步骤45：如果

取P_k＝P_k|k-1，

σ_k＝σ_k|k-1；否则,根据(9)，(10)和(11)计算P_k，

σ_k，参数q_k根据式(15)计算；

步骤46：令k←k+1并返回步骤44，直到程序终止。

Images