CN103033186B - 一种用于水下滑翔器的高精度组合导航定位方法 - Google Patents

Abstract

本发明公布了一种用于水下滑翔器的高精度组合导航定位方法，惯性测量单元IMU的输出经过粗处理和细处理后得到较高精度的数据，融合基于Runge-Kutta法(RK₄)的航位推算得到的数据，再经自适应卡尔曼AKF和无迹卡尔曼UKF二级滤波，滤波后反馈回IMU来校正IMU的累积误差，最终输出较准的位置速度姿态等信息。本发明用于水下滑翔器的自主导航，具有高精度、实时性、稳定性等优点，使得整体导航系统能长航时、高精度、低功耗稳定运行，能迅速准确地得到水下航行器当前位姿信息，同时为其提供航迹和位置参数。

Description

一种用于水下滑翔器的高精度组合导航定位方法

技术领域

本发明属于导航技术领域，涉及水下滑翔器的导航定位，为一种用于水下滑翔器的高精度组合导航定位方法，该方法具有高可靠性、高精度、高稳定性及实时性等特点，可实现水下滑翔器的准确定位和自主导航。

背景技术

对于水下滑翔器的定位与导航，为了达到低功耗、低成本、长航时的目的，要求导航传感器的数量尽量少，基于微型机电系统MEMS的惯性测量单元IMU以其体积小、重量轻、低功耗等优点，加之其不受外界干扰，能在短期内提供较高精度，被作为水下滑翔器的首选导航元件。然而基于MEMS的IMU本身存在一些固有器件误差，加上热噪声、电子干扰等，会造成非静态的随机误差漂移和MEMS器件的标度因子呈较严重的非线性。在运行期间，这些非线性误差会增长，基于MEMS的惯性导航系统INS的性能也会随之下降，在没有额外的校正时位置误差会累积增长。对于大多数水面导航，全球定位系统GPS已经是很成熟且可靠的技术，但在水下，GPS可能遇到失锁、信号阻断、多径效应问题。所以航位推算算法将对INS的误差校正起到至关重要的作用。

尽管卡尔曼滤波KF通过去除INS误差中线性部分来补偿系统输出，但KF是适用于由高斯白噪声所产生的线性误差状态模型，对基于MEMS的INS的非线性动态模型，它的效果并不佳。扩展卡尔曼滤波EKF通过去掉高次项强制将非线性模型近似成线性化模型。但这种线性化在高维非线性模型中将会导致滤波器的发散，效果较差。无迹卡尔曼滤波UKF从原理上克服了EKF的缺陷，在非线性系统中比EKF要优很多，但是UKF在计算非线性变换的随机变量统计性时会有较大计算量。

因水下滑翔器导航与定位要求低功耗、长航时、高精度、小体积等，所以制约了器件的选择，而目前已有的算法均有不足。如何结合水下滑翔器导航的背景，在数据处理、算法优化、滤波方式等方面进行算法设计以达到高精度、高实时性、长航时、高稳定性等，在国内外还没有太多报道。

发明内容

本发明要解决的问题是：水下滑翔器导航与定位要求低功耗、长航时、高精度、小体积等，所以制约了器件的选择，而目前已有的算法均有不足，需要一种用于水下滑翔器的导航定位方法，克服传统组合导航系统由于误差随时间累积而不能高精度长航时实时导航与定位的欠缺，在数据处理、算法优化、滤波方式等方面达到高精度、高实时性、长航时和高稳定性。

本发明的技术方案为：一种用于水下滑翔器的高精度组合导航定位方法，以惯性测量单元IMU为导航原件，包括以下步骤：

1）将惯性测量单元IMU的输出数据输入无限脉冲响应IIR低通滤波器，去除掉粗大噪声，完成数据的粗处理；

2）对粗处理后的数据进行细处理，所述细处理为计算粗处理后得到的数据的高斯分布均值和方差；

3）采用基于龙格－库塔法Runge-Kutta的航位推算DR，推算每个更新周期内滑翔器的位置、速度和姿态；

4）设置两级滤波系统，第一级滤波为自适应卡尔曼滤波AKF，第二级滤波是无迹卡尔曼滤波UKF，自适应卡尔曼滤波AKF通过自适应模糊控制调节滤波增益，通过自适应强跟踪的扩展卡尔曼滤波EKF预测均方误差，所述滤波增益与预测均方误差共同调节得到估计均方误差，将所述估计均方误差代入无迹卡尔曼滤波UKF，得到两级滤波系统，两级滤波系统对输入数据进行UKF滤波；

5）将步骤2）得到的高斯分布均值和方差，以及步骤3）得到的推算数据融合进行UKF滤波，由UKF估出的误差反馈回惯性测量单元IMU，校正惯性测量单元IMU的累积误差，维持惯性测量单元IMU的精度，输出准确的位置、速度和姿态信息，实现稳定的导航定位。

步骤3）的基于龙格－库塔法Runge-Kutta的航位推算DR具体为：设航位推算的采样周期为T，在采样周期内进行m次速度、位置和姿态的信息更新，每次更新的周期为采样前速度为v₀，在采样周期内每次更新的速度为v_j，j=1,2，....，m，采样后速度为v_m，则在时间t内更新的速度由式(b)递推：

$\left\{\begin{array}{c}{v}_{j+1}=v_j+\frac{t}{6}(k_1+{2k}_2+{2k}_3+k_4)\\ k_1=f(t,v_j)\\ k_2=f(t+\frac{1}{2}h,v_j+\frac{1}{2}{\mathrm{hk}}_1)\\ k_3=f(t+\frac{1}{2}h,v_j+\frac{1}{2}{\mathrm{hk}}_2)\\ k_4=f(t+h,v_j+{\mathrm{hk}}_3)\end{array}\right.---\left(b\right)$

推导得到速度v_j，然后再代入式(c)中：

$\left\{\begin{array}{c}{v}_e=v_j\cos \left(P\right)\sin \left(H\right)\\ v_n=v_j\cos \left(P\right)\cos \left(H\right)\\ v_u=v_j\sin \left(P\right)\end{array}---\left(c\right)\right.$

式中v_e,v_n,v_u分别表示推算后东北天三向速度，H,P,R分别表示航向角、俯仰角和横滚角，然后根据三个方向的速度再推算位移，这样在每个更新周期得到速度、位置和姿态信息。

步骤4）中第一级滤波自适应卡尔曼滤波AKF具体为：

a）卡尔曼方程中滤波增益方程为式中P_k/k-1为k时刻的理论预测均方误差，H_k为k时刻的理论量测矩阵，R_k为k时刻的理论量测噪声的方差矩阵；自适应卡尔曼滤波AKF采用模糊自适应算法调节滤波增益K_k，记 $S_k=H_k{P}_{k/k-1}{H}_k^T+R_k,$ $C_k=H_k^{\′}{P}_{k/k-1}^{\′}{H_k^{\′}}^T+R_k^{\′},$ 式中P′_k/k-1为k时刻的实际预测均方误差，H′_k为k时刻的实际量测矩阵，R′_k为k时刻的实际量测噪声的方差矩阵，S_k表示k时刻更新序列的理论协方差，C_k表示k时刻实际协方差，记delta_k=C_k-S_k，delta_k为模糊逻辑控制器的输入，R_k作为输出，如果delta_k不变则R_k维持不变；如果delta_k大于零则R_k减小；如果delta_k小于零则R_k增大，通过自适应调节R_k，自适应调节滤波增益 $K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1};$

b）自适应卡尔曼滤波AKF通过自适应强跟踪扩展卡尔曼滤波EKF来调节理论预测均方误差P_k/k-1，设自适应强跟踪EKF的状态估计为式中为状态一步预测，Φ_k，k-1为k-1到k时刻的系统一步转移矩阵，为k-1时刻的状态预测，K_k为滤波增益，Z_k为k时刻的量测向量，h(·)为观测函数，r_k为量测方程中带时变均值和协方差的高斯白噪声序列的均值；预测均方误差为式中Q_k-1为量测方程中带时变均值和协方差的高斯白噪声序列的协方差，λ_k-1为k-1时刻的自适应渐消因子，采用时变的自适应渐消因子对过去的数据进行渐消，以减弱陈旧数据对当前滤波值的影响，通过实时调整状态预测误差的协方差矩阵以及相应的增益矩阵来实现减弱影响这一目标，自适应渐消因子由下式确定：

${\mathrm{\&lambda;}}_k\left\{\begin{array}{cc}{\mathrm{\&lambda;}}_{0,k}& {\mathrm{\&lambda;}}_{0,k}\&GreaterEqual;1\\ 1& {\mathrm{\&lambda;}}_{0,k}<1\end{array}---\left(d\right)\right.$

其中λ_0,k=max{1，tr[N_k]/tr[M_k]}，

$N_k={\mathrm{\&Omega;}}_{0,k}-H_k{Q}_{k-1}{H}_k^T-l_k{R}_{k-1},$ $M_k=H_k{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T{H}_k^T,$ tr[·]表示求矩阵的迹；Ω_0，k表示新息方差阵：

${\mathrm{\&Omega;}}_{0,k}=\left\{\begin{array}{cc}{\mathrm{\&omega;}}_1{\mathrm{\&omega;}}_1^T& k=0\\ \frac{{\mathrm{\&rho;\&Omega;}}_{0,k-1}+{\mathrm{\&omega;}}_k{\mathrm{\&omega;}}_k^T}{1+\mathrm{\&rho;}}& k\&GreaterEqual;1\end{array}\right.$

ρ为遗忘因子，l_k≥1为弱化因子，取l_k=1-d(k)，d_k=(1-ρ)/(1-ρ^k+1)；ω₁、ω_k为残差序列；

c）由a）和b）更新得到的K_k和P_k/k-1共同调节，代入P_k/k=(I-K_kH_k)P_k/k-1，得到估计均方误差P_k/k，并实时自适应调节估计均方误差P_k/k。

步骤4）中第二级滤波无迹卡尔曼滤波UKF具体为：考虑驱动噪声对系统的影响，在UKF中进行状态扩维，加入量测噪声R_k、系统噪声Q_k及自适应卡尔曼滤波AKF得到的估计均方误差P_k/k进行状态扩维，组成新的协方差矩阵，进行扩维后的状态变量和协方差矩阵为：

状态变量： $X_k^a={\left[\begin{array}{ccc}{X}_k^T& W_k^T& V_k^R\end{array}\right]}^T$

式中X_k表示k时刻的系统状态向量，W_k表示k时刻的系统噪声，V_k为k时刻的量测噪声；

协方差矩阵： $P_k^a\left[\begin{array}{ccc}{P}_k& 0& 0\\ 0& Q_k& 0\\ 0& 0& R_k\end{array}\right]---\left(e\right)$

将步骤2）得到的高斯分布均值和方差，以及步骤3）DR得到的推算数据融合进行UKF滤波，利用初始状态估计，设定最初的2n+1个Sigma点，用过程模型变化这些Sigma点，即对状态变量进行U变换，通过UKF滤波计算出滤波增益、预测误差和估计均方误差信息，最终估出的速度、位置、姿态等误差反馈回IMU，校正IMU的累积误差。

本发明克服现有技术的不足，提供一种用于水下滑翔器的高精度组合导航定位方法，克服了传统组合导航系统由于误差随时间累积而不能高精度长航时实时导航与定位的欠缺，并采取多级数据处理、精准航位推算算法、智能自适应卡尔曼滤波、多级滤波等，完成了水下滑翔器高精度长航时、高实时性、稳定自主导航与定位。本发明与现有技术相比的优点在于：

(1)IIR低通滤波器能有效的去除原始数据中的粗大噪声,而且能根据实际情况调节滤波阶数及参数保证最佳性能，对于水下导航这种非线性环境比较适用；

(2)将IMU的输出数据求高斯分布的均值和方差，比直接求数据的均值和方差更贴近实际数据的输出，能更好得拟合实际数据输出模型，为后续的算法处理提供更准确的输入。

(3)基于RK4的航位推算法，能在每个更新阶段较准确地推算出其速度信息，从而得到位移。比传统的直接用经纬度来求位移误差大大减小；

(4)AKF用两步自适应来调节估计均方误差，而该误差是UKF中用到的关键参数，从而减少了因系统的不确定性及非线性导致的参数偏差。

(5)对于水下导航的特殊环境，UKF成为适用于该环境较佳的选择，将上述处理后的数据融合做差作为状态量和观测量，再结合调整后的参数，能很好地估计误差，反馈到IMU来校正误差，得到精确的速度、位置、姿态等信息，达到高精度导航与定位的目的。

附图说明

图1为航位推算速度分解示意图。

图2为本发明的多级滤波原理框图。

图3为本发明方法框图。

具体实施方式

本发明公布了一种用于水下滑翔器的高精度组合导航定位方法，涉及的内容包括惯性测量单元IMU的输出经过粗处理和细处理后得到较高精度数据；基于Runge-Kutta(RK₄)法的航位推算DR；分级滤波算法自适应卡尔曼AKF和无迹卡尔曼UKF；经两次处理后的IMU输出数据与DR输出融合作为UKF的输入，滤波后反馈回IMU来校正IMU的累积误差。

本发明包括以下步骤：

(1)IMU输出的原始数据带有比较大的噪声，通过IIR低通滤波器可以去除高频噪声。但截止频率不能过低，不然会滤掉有用信号导致实际滤波结果有偏差。IIR低通滤波器的设计为：

$s\left(K\right)=b_{0}r\left(K\right)+\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{b}_{i}r(K-i)-\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{a}_{i}s(K-i)---\left(a\right)$

式(a)中，r(K)是导航系统惯性传感器的输出信号序列，s(K)是低通滤波器的输出序列，即滤波结果，K为自然数，n表示IIR滤波器的阶数，以4阶IIR为例，参数b₀、a_i、b_i，i=1，....，4能使用不同的方法计算得到，如插值法等；根据实际情况选用不同阶数以达到理想效果。IIR低通滤波器的设计为公知技术，不再详述。

IIR滤波器能抑制高频信号，具有实时迭代结构，比模块处理的小波变换法计算量要小得多，和后续的基于卡尔曼的滤波方式组合在一起具有更简单的结构。该滤波器工作在频域范围，和小波变换不同，它更是关注于有用信号的提取，滤除比真实输出信号更高的粗大噪声。

(2)通过粗处理后，求处理后的数据的高斯分布均值和方差。粗处理后再求高斯分布均值和方差更接近实际噪声的分布，比用求平均数来求均值和方差要精确得多，提高了整个系统的精度。数据的概率密度为：均值μ表示绝大多数数据所处的值，方差σ表示绝大多数数据偏离均值的幅度值，作为后续滤波的重要参数能更准确的描述原始数据的特性。

(3)航位推算DR作为唯一辅助INS的导航方式，对整个系统的导航精度起着至关重要的作用。如图1，本发明用基于RK₄航位推算法准确推算每个更新周期内滑翔器的位置、速度、姿态等。设采样时间为T，在该周期内进行m次速度、位置、姿态等信息更新，每次更新的周期为采样前速度为v₀，在采样周期内每次更新的速度为v_j，j=1,2，....，m，采样后速度为v_m。则在时间t内更新的速度可按方程组(b)递推：

$\left\{\begin{array}{c}{v}_{j+1}=v_j+\frac{t}{6}(k_1+{2k}_2+{2k}_3+k_4)\\ k_1=f(t,v_j)\\ k_2=f(t+\frac{1}{2}h,v_j+\frac{1}{2}{\mathrm{hk}}_1)\\ k_3=f(t+\frac{1}{2}h,v_j+\frac{1}{2}{\mathrm{hk}}_2)\\ k_4=f(t+h,v_j+{\mathrm{hk}}_3)\end{array}\right.---\left(b\right)$

方程组(b)为由基于Runge-Kutta(RK₄)法计算速度，其中f()为RK₄函数，为本领域技术人员公知，不再详述。

推出速度v_j，然后再代入方程组(c)中：

$\left\{\begin{array}{c}{v}_e=v_j\cos \left(P\right)\sin \left(H\right)\\ v_n=v_j\cos \left(P\right)\cos \left(H\right)\\ v_u=v_j\sin \left(P\right)\end{array}---\left(c\right)\right.$

方程组(c)即为航位推算，式中v_e,v_n,v_u分别表示推算后东北天三向速度，H,P,R分别表示航向角、俯仰角和横滚角。然后根据三个方向的速度再推算位移。这样在每个更新周期能得到较准确的速度、位置和姿态信息，为后续的误差补偿奠定了基础。

(4)采用两级滤波算法。如图2，第一级滤波是AKF(自适应卡尔曼滤波)，第二级滤波是UKF(无迹卡尔曼滤波)。AKF调整参数分为两步，第一步采用模糊自适应算法调节增益K_k。记 $S_k=H_k{P}_{k/k-1}{H}_k^T+R_k,$ $C_k=H_k^{\&prime;}{P}_{k/k-1}^{\&prime;}{H_k^{\&prime;}}^T+R_k^{\&prime;},$ S_k表示更新序列的理论协方差，C_k表示实际协方差，式中P_k/k-1为k时刻的理论预测均方误差，H_k为k时刻的理论量测矩阵，R_k为k时刻的理论量测噪声的方差矩阵，P′_k-1为k时刻的实际预测均方误差，H′_k为k时刻的实际量测矩阵，R′_k为k时刻的实际量测噪声的方差矩阵。记delta_k=C_k-S_k。delta_k为模糊逻辑控制器的输入，R_k作为输出。如果delta_k基本不变则R_k维持不变；如果delta_k大于零则R_k减小；如果delta_k小于零则R_k增大。这样可以通过自适应调节R_k，达到自适应调节滤波增益的目的。

(5)AKF的第二步是通过自适应强跟踪EKF（扩展卡尔曼滤波）来调节预测均方误差P_k/k-1。状态估计为预测均方误差为 $P_{k/k-1}={\mathrm{\&lambda;}}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T+Q_{k-1}.$ 式中为状态一步预测，Φ_k，k-1为k-1时刻到k时刻的系统一步转移矩阵，为k-1时刻的状态预测，K_k为滤波增益，Z_k为k时刻的量测向量，h(·)为观测函数，r_k为量测方程中带时变均值和协方差的高斯白噪声序列的均值，Q_k-1为量测方程中带时变均值和协方差的高斯白噪声序列的协方差。λ_k-1≥1为k-1时刻的自适应渐消因子，采用时变的渐消因子对过去的数据进行渐消，减弱陈旧数据对当前滤波值的影响，通过实时调整状态预测误差的协方差矩阵以及相应的增益矩阵来实现这一目标。自适应渐消因子可由下式确定：

${\mathrm{\&lambda;}}_k\left\{\begin{array}{cc}{\mathrm{\&lambda;}}_{0,k}& {\mathrm{\&lambda;}}_{0,k}\&GreaterEqual;1\\ 1& {\mathrm{\&lambda;}}_{0,k}<1\end{array}---\left(d\right)\right.$

其中λ_0,k=max{1，tr[N_k]/tr[M_k]}， $N_k={\mathrm{\&Omega;}}_{0,k}-H_k{Q}_{k-1}{H}_k^T-l_k{R}_{k-1},$ $M_k=H_k{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T{H}_k^T,$ tr[·]表示求矩阵的迹；

Ω_0,k表示新息方差阵：

${\mathrm{\&Omega;}}_{0,k}=\left\{\begin{array}{cc}{\mathrm{\&omega;}}_1{\mathrm{\&omega;}}_1^T& k=0\\ \frac{{\mathrm{\&rho;\&Omega;}}_{0,k-1}+{\mathrm{\&omega;}}_k{\mathrm{\&omega;}}_k^T}{1+\mathrm{\&rho;}}& k\&GreaterEqual;1\end{array}\right.$

ρ为遗忘因子，l_k≥1为弱化因子，取l_k=1-d(k)，d_k=(1-ρ)/(1-ρ^k+1)，ω₁、ω_k为残差序列。

(6)由(5)调节得到的预测均方误差P_k/k-1的和(4)调节得到的滤波增益K_k代入P_k/k=(I-K_kH_k)P_k/k-1，得到估计均方误差P_k/k。考虑驱动噪声对系统的影响，在UKF中进行状态扩维，将噪声项添加到状态项中，将量测噪声R_k、系统噪声Q_k及步骤(5)中得到的估计均方误差P_k/k进行状态扩维，组成新的协方差矩阵，进行扩维后的状态变量和协方差矩阵为：

$X_k^a={\left[\begin{array}{ccc}{X}_k^T& W_k^T& V_k^R\end{array}\right]}^T$

式中X_k和前面一样，表示k时刻的系统状态向量，W_k表示k时刻的系统噪声，V_k为k时刻的量测噪声。

$P_k^a\left[\begin{array}{ccc}{P}_k& 0& 0\\ 0& Q_k& 0\\ 0& 0& R_k\end{array}\right]---\left(e\right)$

将DR输出的数据和IMU细处理后输出的数据融合进行UKF滤波，利用初始状态估计，设定最初的2n+1个Sigma点，用过程模型变化这些Sigma点，即对状态变量进行U变换。依次进行下述的计算：

计算预测估计值

计算预测协方差P(k+1/k)；

通过测量方程计算测量值Z_i(k+1/k)；

计算预测测量值

计算信息方差P_zz(k+1/k)；

计算和Z(k+1/k)的协方差P_xz(k+1/k)；

计算卡尔曼增益K(k+1)；

更新误差协方差P(k+1/k+1)；

更新状态

上述计算过程为公知的UKF通用算法流程，不再详述。

经过上述步骤，滤波增益、预测误差、估计均方误差等信息被计算出，最终估出的速度、位置、姿态等误差反馈回IMU，校正IMU的累积误差。

对于系统的状态模型和量测模型多是非线性的水下滑翔器导航系统，UKF摒弃了对非线性函数进行线性化的传统做法，采用卡尔曼线性滤波框架，对于一步预测方程，使用无迹变换来处理均值和协方差的非线性传递，采用非线性滤波的UKF方法，明显提高了导航精度。

下面结合附图对发明的技术方案进行详细说明：

如图3，IMU单元将三轴陀螺、三轴加速度计、三轴高性能磁力计集成与一体，该单元体积小，集成度高，功耗低，带有自校正和标定功能。滑翔器在水下运行时，IMU的输出带有大的噪声，先经过IIR低通滤波器粗处理过程滤掉高频噪声，截止频率不能选太低，否则有用信号将会被滤掉，可以通过实际调试来确定。滤波器的阶数根据实际情况调整，保证最好的滤波效果，随着滤波阶数的确定，构建了滤波器的整体框架，之后可以确定相关系数完成粗滤波过程。

之后，进行数据细处理，将输出的数据求其高斯分布的均值及方差，这种求法能更拟合实际的输出情况，所得的均值和方差更贴近实际值。

水下滑翔器除了IMU之外，没有GPS等辅助导航方式，惯性导航器件的漂移不断累积使得IMU的输出误差越来越大，此时用航位推算DR的方法来估计实际的速度、位置、姿态等信息。位置漂移直接决定滑翔器的定位精度，所以准确的推算位置至关重要。将一个采样周期根据具体需要划分成若干更新周期，这个划分数量要适宜，过多会造成计算量的过大，不利于实时更新，而且滑翔器滑翔速度本来比较缓慢，更新次数过多也没有必要；过少会造成累积误差比较大，精度会有所降低，可在实际实施时通过测试调整来确定。在每个更新周期内利用RK4来精确推算速度，再用姿态解算算出姿态，两者结合推出较准确的位置。

经过前述的过程后，要进入滤波阶段，水下滑翔器在较复杂的环境中，模型成非线性且多变，传统的卡尔曼滤波KF滤波在此情况下可能不能达到较好效果，本发明运用两级滤波进行，即自适应卡尔曼滤波AKF和无迹卡尔曼滤波UKF。AKF又分为两步，一步是用自适应模糊控制法调节滤波增益，一步是用自适应强跟踪EKF（扩展卡尔曼滤波）来调节预测均方误差。调节后的滤波增益和预测均方误差再共同作用更新估计均方误差。

UKF考虑了各噪声对系统的影响，将噪声项添加到状态项中，即状态扩维，并将调节后的均方误差、系统噪声、量测噪声合并，组成新的协方差矩阵。IMU输出和DR输出做差作为状态量和观测量进入UKF进行滤波，以卡尔曼线性滤波为框架，确定Sigma点的个数、位置和相应的权值，使用无迹变换来处理均值和协方差，对非线性函数的概率密度分布进行近似，用一系列确定样本来逼近状态的后验概率密度，推算出较精确的位置、速度、姿态等误差，反馈回IMU进行校正，最终输出高精度的位置姿态等信息。本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (3)

1.一种用于水下滑翔器的高精度组合导航定位方法，其特征是以惯性测量单元IMU为导航元件，包括以下步骤：

1)将惯性测量单元IMU的输出数据输入无限脉冲响应IIR低通滤波器，去除掉粗大噪声，完成数据的粗处理；

2)对粗处理后的数据进行细处理，所述细处理为计算粗处理后得到的数据的高斯分布均值和方差；

3)采用基于龙格－库塔法Runge-Kutta的航位推算DR，推算每个更新周期内滑翔器的位置、速度和姿态；

4)设置两级滤波系统，第一级滤波为自适应卡尔曼滤波AKF，第二级滤波是无迹卡尔曼滤波UKF，自适应卡尔曼滤波AKF通过自适应模糊控制调节滤波增益，通过自适应强跟踪的扩展卡尔曼滤波EKF预测均方误差，所述滤波增益与预测均方误差共同调节得到估计均方误差，将所述估计均方误差代入无迹卡尔曼滤波UKF，得到两级滤波系统，两级滤波系统对输入数据进行UKF滤波；

第一级滤波自适应卡尔曼滤波AKF具体为：

a)卡尔曼方程中滤波增益方程为式中P_k/k-1为k时刻的理论预测均方误差，H_k为k时刻的理论量测矩阵，R_k为k时刻的理论量测噪声的方差矩阵；自适应卡尔曼滤波AKF采用模糊自适应算法调节滤波增益K_k，记C_k＝H′_kP′_k/k-1H′^T _k+R′_k，式中P′_k/k-1为k时刻的实际预测均方误差，H′_k为k时刻的实际量测矩阵，R′_k为k时刻的实际量测噪声的方差矩阵，S_k表示k时刻更新序列的理论协方差，C_k表示k时刻实际协方差，记delta_k＝C_k-S_k，delta_k为模糊逻辑控制器的输入，R_k作为输出，如果delta_k不变则R_k维持不变；如果delta_k大于零则R_k减小；如果delta_k小于零则R_k增大，通过自适应调节R_k，自适应调节滤波增益 $K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1};$

b)自适应卡尔曼滤波AKF通过自适应强跟踪扩展卡尔曼滤波EKF来调节理论预测均方误差P_k/k-1，设自适应强跟踪EKF的状态估计为式中为状态一步预测，Φ_k,k-1为k-1到k时刻的系统一步转移矩阵，为k-1时刻的状态预测，K_k为滤波增益，Z_k为k时刻的量测向量，h(·)为观测函数，r_k为量测方程中带时变均值和协方差的高斯白噪声序列的均值；预测均方误差为式中Q_k-1为量测方程中带时变均值和协方差的高斯白噪声序列的协方差，λ_k-1为k-1时刻的自适应渐消因子，采用时变的自适应渐消因子对过去的数据进行渐消，以减弱陈旧数据对当前滤波值的影响，通过实时调整状态预测误差的协方差矩阵以及相应的增益矩阵来实现减弱影响这一目标，自适应渐消因子由下式确定：

${\mathrm{\&lambda;}}_k=\left\{\begin{array}{cc}{\mathrm{\&lambda;}}_{0,k}& {\mathrm{\&lambda;}}_{0,k}\&GreaterEqual;1\\ 1& {\mathrm{\&lambda;}}_{0,k}<1\end{array}\right.---\left(d\right)$

其中λ_0,k＝max{1,tr[N_k]/tr[M_k]}，

$N_k={\mathrm{\&Omega;}}_{0,k}-H_k{Q}_{k-1}{H}_k^T-l_k{R}_{k-1},M_k=H_k{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T{H}_k^T,$ tr[·]表示求矩阵的迹；Ω_0，k表示新息方差阵：

${\mathrm{\&Omega;}}_{0,k}=\left\{\begin{array}{cc}{\mathrm{\&omega;}}_1{\mathrm{\&omega;}}_1^T& k=0\\ \frac{\mathrm{\&rho;}{\mathrm{\&Omega;}}_{0,k-1}+{\mathrm{\&omega;}}_k{\mathrm{\&omega;}}_k^T}{1+\mathrm{\&rho;}}& k\&GreaterEqual;1\end{array}\right.$

ρ为遗忘因子，l_k≥1为弱化因子，取l_k＝1-d(k)，d_k＝(1-ρ)/(1-ρ^k+1)；ω₁、ω_k为残差序列；

c)由a)和b)更新得到的K_k和P_k/k-1共同调节，代入P_k/k＝(I-K_kH_k)P_k/k-1，得到估计均方误差P_k/k，并实时自适应调节估计均方误差P_k/k；

5)将步骤2)得到的高斯分布均值和方差，以及步骤3)得到的推算数据融合进行UKF滤波，由UKF估出的误差反馈回惯性测量单元IMU，校正惯性测量单元IMU的累积误差，维持惯性测量单元IMU的精度，输出准确的位置、速度和姿态信息，实现稳定的导航定位。

2.根据权利要求1所述的一种用于水下滑翔器的高精度组合导航定位方法，其特征是步骤3)的基于龙格－库塔法Runge-Kutta的航位推算DR具体为：设航位推算的采样周期为T，在采样周期内进行m次速度、位置和姿态的信息更新，每次更新的周期为采样前速度为v₀，在采样周期内每次更新的速度为v_j，j＝1,2,....,m，采样后速度为v_m，则在时间t内更新的速度由式(b)递推：

$\left\{\begin{array}{c}{v}_{j+1}=v_j+\frac{t}{6}(k_1+2k_2+2k_3+k_4)\\ k_1=f(t,v_j)\\ k_2=f(t+\frac{1}{2}h,v_j+\frac{1}{2}{\mathrm{hk}}_1)\\ k_3=f(t+\frac{1}{2}h,v_j+\frac{1}{2}{\mathrm{hk}}_2)\\ k_4=f(t+h,v_j+h{k}_3)\end{array}\right.---\left(b\right)$

推导得到速度v_j，然后再代入式(c)中：

$\left\{\begin{array}{c}{v}_e=v_j\cos \left(P\right)\sin \left(H\right)\\ v_n=v_j\cos \left(P\right)\cos \left(H\right)\\ v_u=v_j\sin \left(P\right)\end{array}\right.---\left(c\right)$

式中v_e,v_n,v_u分别表示推算后东北天三向速度，H,P分别表示航向角和俯仰角，然后根据三个方向的速度再推算位移，这样在每个更新周期得到速度、位置和姿态信息。

3.根据权利要求1所述的一种用于水下滑翔器的高精度组合导航定位方法，其特征是步骤4)中第二级滤波无迹卡尔曼滤波UKF具体为：考虑驱动噪声对系统的影响，在UKF中进行状态扩维，加入量测噪声R_k、系统噪声Q_k及自适应卡尔曼滤波AKF得到的估计均方误差P_k/k进行状态扩维，组成新的协方差矩阵，进行扩维后的状态变量和协方差矩阵为：

状态变量： $X_k^a={\left[\begin{array}{ccc}{X}_k^T& W_k^T& V_k^T\end{array}\right]}^T$

式中X_k表示k时刻的系统状态向量，W_k表示k时刻的系统噪声，V_k为k时刻的量测噪声；

协方差矩阵： $P_k^a=\left[\begin{array}{ccc}{P}_k& 0& 0\\ 0& Q_k& 0\\ 0& 0& R_k\end{array}\right]---\left(e\right)$

将步骤2)得到的高斯分布均值和方差，以及步骤3)DR得到的推算数据融合进行UKF滤波，利用初始状态估计，设定最初的2n+1个Sigma点，用过程模型变化这些Sigma点，即对状态变量进行U变换，通过UKF滤波计算出滤波增益、预测误差和估计均方误差信息，最终估出的速度、位置、姿态等误差反馈回IMU，校正IMU的累积误差。