CN105783909A - 一种基于ukf和pso-svm的hov混合水下导航定位方法 - Google Patents

Abstract

本发明公开一种基于UKF和PSO-SVM的HOV混合导航定位方法。将HOV海试中获取的HOV的长基线系统位置数据和航迹推算得到的位置数据，利用UKF进行融合得到可靠的HOV位置信息。同时利用融合和航迹推算得到的位置信息，作为PSO-SVM位置估算模型的训练集和测试集对模型进行训练和测试；在长基线系统短暂失效，不能获得HOV数据时，利用航迹推算得到的位置信息和推算时间作为已训练好的PSO-SVM模型的输入，输出实时补偿的HOV的位置信息，从而在长基线系统失效的情况下获得精确的HOV位置信息。本发明能够提高HOV的导航精度，提高导航系统的鲁棒性。

Description

一种基于UKF和PSO-SVM的HOV混合水下导航定位方法

技术领域

本发明涉及水下导航定位方法，具体的说是一种基于无色卡尔曼滤波(UKF,UnscentedKalmanFilter)和粒子群算法(PSO,ParticleSwarmOptimization)参数寻优的支持向量机(SVM,SupportVectorMachine)的水下机器人混合水下导航定位方法。

背景技术

随着海洋开发事业的迅速发展，水下施工和建设项目越来越多，对于水下作用手段的性能也越来越高。由于水下机器人能够在水下进行观察、摄影、打捞和施工作业，因此在海洋开发中得到广泛的应用。而水下机器人位置信息的准确度决定着水下机器人能否很好完成预期的任务。

导航定位问题是航海、航天课题中非常重要的内容之一。对于深海载人潜水器，由于条件限制，导航定位问题更加突出重要。

HOV中装有多普勒计程仪和运动传感器组成的惯性导航系统，利用航迹推算方法(DR，DeadReckoning)，是一种自主导航方法，在大多数水下潜水器中得到了广泛的应用，但是惯性导航系统中的传感器存在误差，导致产生累计位置误差，导致载人潜水器在控制时会发生差错。

HOV在海试时同时使用长基线定位系统提供位置信息，长基线定位系统是一种声学传感器。现在也被广泛的应用的潜水器的航行中，并且能够提供的很高的位置精度。但是，长基线定位系统存在多野值和更新频率不稳定的缺点。

发明内容

为克服航迹推算和长基线导航定位系统的不足和缺点，同时利用二者的优点，本发明要解决的技术问题是提供一种提高载人潜水器导航系统精度和可靠性的基于UKF和PSO-SVM的HOV混合水下导航定位方法。同时，长基线导航定位系统可能发生短暂失效，不能更新HOV位置信息的问题，为解决这种问题，本发明又提出PSO-SVM位置误差估计模型方法，来更正补偿航迹推算的误差，从而获得精确的位置信息。

本发明为实现上述目的所采用的技术方案是：一种基于UKF和PSO-SVM的HOV混合水下导航定位方法，包括以下步骤：

在长基线定位系统正常工作时，将HOV海试中得到的长基线位置信息和航迹推算位置信息，利用UKF进行融合，得到HOV导航位置信息；

在长基线定位系统失效时，UKF融合算法失效，此时将航迹推算位置信息和推算时间作为PSO-SVM位置误差估算模型的输入，模型输出实时预测的HOV位置补偿信息，从而在长基线定位系统失效的情况下获得HOV实时位置信息。

所述利用UKF进行融合，是将长基线位置信息和航迹推算得到的位置信息，在一阶动态模型下，滤波融合得到更加可靠的HOV的导航位置信息；

所述一阶动态模型的状态方程为：

$\left\{\begin{array}{c}{x}_{k+1}=x_k+[\sin (j_k+N_1^v)\×(u_k+N_2^v)+\cos (j_k+N_1^v)\×(u_k+N_3^v)]\\ h_{k+1}=h_k+[\cos (j_k+N_1^v)\×(u_k+N_2^v)-\sin (j_k+N_1^v)\×(u_k+N_3^v)]\\ j_{k+1}=j_k+(r_k+N_4^v)\×\mathrm{Dt}\\ u_{k+1}=v_{k+1}+N_2^v\\ v_{k+1}=v_{k+1}+N_3^v\\ r_{k+1}=r_{k+1}+N_4^v\end{array}\right.$

上式中，x_k为航迹推算的得到的HOV东向位置，h_k为航迹推算得到的HOV北向位置，j_k为HOV艏向角，u_k为HOV前向速度，v_k为HOV横向速度，r_k为HOV艏向角速率，Dt为采样时间，为过程噪声(i＝1,2,3,4)；

观测方程定义为：

$y=\left[\begin{array}{c}{x}_k\\ h_k\end{array}\right]+N_5^v$

上式中，x_k为长基线定位系统测得的HOV东向位置，h_k为长基线定位系统测得的HOV北向位置，为过程噪声。

所述滤波融合通过以下过程实现：

步骤1，初始化：

$\left\{\begin{array}{c}{\stackrel{\‾}{x}}_0=E\left[x_0\right]\\ P_0=E\left[(x_0-{\stackrel{\‾}{x}}_0){(x_0-{\stackrel{\‾}{x}}_0)}^T\right]\end{array}\right.$

步骤2，产生sigma点:

${\mathrm{\χ}}_{k-1}=[{\stackrel{\‾}{x}}_{k-1},{\stackrel{\‾}{x}}_{k-1}+\sqrt{(n+\mathrm{\λ})P_{k-1}},{\stackrel{\‾}{x}}_{k-1}-\sqrt{(n-\mathrm{\λ})P_{k-1}}]$

步骤3，时间更新：

$\left\{\begin{array}{c}{\mathrm{\χ}}_{k|k-1}^{*}=f({\mathrm{\χ}}_{k-1},u_k)\\ {\stackrel{\‾}{x}}_{k|k-1}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{w}_i^m{x}_{i.k|k-1}^{*}\\ P_{k|k-1}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{w}_i^c({\mathrm{\χ}}_{i,k|k-1}^{*}-{\stackrel{\_}{x}}_{k|k-1}){({\mathrm{\χ}}_{i.k|k-1}^{*}-{\stackrel{\‾}{x}}_{k|k-1})}^T+Q^v\\ {\mathrm{\χ}}_{k|k-1}=[{\stackrel{\‾}{x}}_{k|k-1},{\stackrel{\‾}{x}}_{k|k-1}+\sqrt{(n+\mathrm{\λ})P_{k|k-1}},{\stackrel{\‾}{x}}_{k|k-1}-\sqrt{(n+\mathrm{\λ})P_{k|k-1}}]\\ {\mathrm{\γ}}_{k|k-1}=h\left({\mathrm{\χ}}_{k|k-1}\right)\\ {\stackrel{\‾}{x}}_{k|k-1}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{w}_i^m{\mathrm{\γ}}_{i,k|k-1}\end{array}\right.$

步骤4，测量更新：

$\left\{\begin{array}{c}{P}_{{\stackrel{\‾}{y}}_k{\stackrel{\‾}{y}}_k}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{w}_i^c({\mathrm{\γ}}_{i.k|k-1}^{*}-{\stackrel{\‾}{y}}_{k|k-1}){({\mathrm{\γ}}_{i.k|k-1}^{*}-{\stackrel{\‾}{y}}_{k|k-1})}^T+Q^n\\ P_{{\stackrel{\‾}{x}}_k{\stackrel{\‾}{y}}_k}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{w}_i^c({\mathrm{\χ}}_{i.k|k-1}^{*}-{\stackrel{\‾}{x}}_{k|k-1}){({\mathrm{\γ}}_{i.k|k-1}^{*}-{\stackrel{\‾}{y}}_{k|k-1})}^T\\ P_{{\stackrel{\‾}{x}}_k{\stackrel{\‾}{y}}_k}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{w}_i^c({\mathrm{\χ}}_{i.k|k-1}^{*}-{\stackrel{\‾}{x}}_{k|k-1}){({\mathrm{\γ}}_{i.k|k-1}^{*}-{\stackrel{\‾}{y}}_{k|k-1})}^T\\ K_k=P_{{\stackrel{\‾}{x}}_k{\stackrel{\‾}{y}}_k}{P}_{{\stackrel{\‾}{y}}_k{\stackrel{\‾}{y}}_k}^{-1}\\ \stackrel{\‾}{x}={\stackrel{\‾}{x}}_{k|k-1}+K_k(y_k-{\stackrel{\‾}{y}}_{k|k-1})\end{array}\right.$

循环步骤2到步骤4，直到完成；

其中，x₀初始状态量，P₀初始协方差矩阵，P_k分别为随机向量x的最有预测和协方差，χ_i(i＝1,...,2l)为sigma点，λ＝n(α²-1)，α为控制sigma点分布的常数。Q^v表示过程噪声，Qⁿ为测量噪声，γ_i用来近似非线性函数分布，β为非负常数，其作用是使变换后的方差含有部分的高阶信息，y_k为时刻k的测量值，为y_k的最优预测。K_k为卡尔曼增益，为互协方差矩阵，为新息的协方差矩阵，为权系数。

所述长基线位置信息是长基线定位系统获得的HOV的位置信息，包括经度和纬度信息。

所述航迹推算位置信息是利用航迹推算系统传感器数据推算得到的HOV的位置信息，包括经度和纬度信息。

所述航迹推算系统传感器数据包括多普勒计程仪测量的HOV横向、前向速度信息；运动传感器器测得的HOV的航向角信息；GPS测量得到的HOV初始经度、纬度信息。

所述PSO-SVM位置误差估算模型的建立过程为：

(1)读入HOV位置样本数据集并对样本集进行预处理；

(2)PSO初始化：初始化算法参数，随机初始化例子并形成粒子群体，随机生成各粒子的初始速度，将每个粒子的pbest设置当前位置，gbest设置为群体中最好粒子的当前位置；

(3)用训练集训练SVM，用适应度函数计算每个粒子的适应度值f_MAPE根据粒子的适应度值更新P_i和P_g，若某粒子当前适应度优于pbest则pbest被当前位置替代；若所有粒子的当前最优适应度优于gbest则gbest被当前最优位置替代；

(4)按照下式进行进化：

v_i+1＝f(v_i,x_i)

x_i+1＝g(x_i,v_i)

式中，v_i为粒子速度，x_i为粒子位置；若未达到模型建立过程的结束条件，则返回步骤(2)；否则输出最优解，PSO-SVM位置误差估计模型建立；模型建立结束条件是达到最大迭代次数G_max或最优解在预设迭代次数内停滞不再变化。

本发明相对现有技术具有如下优点及效果：

(1)本发明实现方便，可以再现有的载人潜水器上使用，不需要对载人潜水器的硬件系统进行任何的改动。该发明可以很方便的嵌入现有载人潜水器的控制系统中，对其而言，仅仅是增加计算量而已，并不会有其他影响。

(2)本发明是根据惯性导航系统和长基线导航定位系统，采用UKF滤波方法，实时融合估计HOV的最优位置信息，提高载人潜水器的导航精度。

(3)本发明采用SVM原理，在小样本情况中具有很好的表现，可以减少控制系统的计算量，提高实时性。

(4)本发明采用PSO原理，可以自动对SVM核函数的核参数进行寻优，提高了算法的鲁棒性、适应能力。

(5)本发明在长基线系统在线、可用时采用UKF滤波融合方法，在长基线系统不可获得更新信息时采用PSO-SVM误差估计模型对航迹推算方法进行修正补偿，得到精确位置信息，是一个成熟度和完整度较高的使用方法。

附图说明

图1为本发明算法中UKF滤波融合算法的实现流程图；

图2为本发明算法的PSO-SVM位置误差模型建立的流程图；

图3为本发明混合导航定位方法的流程框图；

图4为本发明算法中SVM的原理图；

图5为UKF滤波融合后的航迹图与航迹推算和长基线导航定位航迹图对比图；

图6为PSO-SVM位置误差估计模型修正前、后的HOV东向位置误差信息对比图；

图7为PSO-SVM位置误差估计模型修正前、后的HOV北向位置误差信息对比图。

具体实施方式

下面结合附图及实施例对本发明做进一步的详细说明。

参照附图1，为本发明方案的UKF滤波融合的流程图。在长基线导航定位系统在线、可用时，HOV导航系统可以同时获得航迹推算位置信息和长基线导航定位系统位置信息，采用UKF滤波估计算法，可以得到一个更加精确、可靠的HOV位置信息。

对航迹推算和长基线定位系统进行UKF滤波融合的实现过程：

建立一阶动态模型，状态方程定义为：

$\left\{\begin{array}{c}{x}_{k+1}=x_k+[\sin (j_k+N_1^v)\×(u_k+N_2^v)+\cos (j_k+N_1^v)\×(u_k+N_3^v)]\\ h_{k+1}=h_k+[\cos (j_k+N_1^v)\×(u_k+N_2^v)-\sin (j_k+N_1^v)\×(u_k+N_3^v)]\\ j_{k+1}=j_k+(r_k+N_4^v)\×\mathrm{Dt}\\ u_{k+1}=v_{k+1}+N_2^v\\ v_{k+1}=v_{k+1}+N_3^v\\ r_{k+1}=r_{k+1}+N_4^v\end{array}\right.$

上式中，x_k为航迹推算的得到的HOV东向位置，h_k为航迹推算得到的HOV北向位置，j_k为HOV艏向角，u_k为HOV前向速度，v_k为HOV横向速度，r_k为HOV艏向角速率，Dt为采样时间，为过程噪声(i＝1,2,3,4)。

观测方程定义为：

$y=\left[\begin{array}{c}{x}_k\\ h_k\end{array}\right]+N_5^v$

上式中，x_k为长基线定位系统测得的HOV东向位置，h_k为长基线定位系统测得的HOV北向位置，为过程噪声。

UKF滤波的实现过程：

步骤1，初始化:

$\left\{\begin{array}{c}{\stackrel{\‾}{x}}_0=E\left[x_0\right]\\ P_0=E\left[(x_0-{\stackrel{\‾}{x}}_0){(x_0-{\stackrel{\‾}{x}}_0)}^T\right]\end{array}\right.$

步骤2，产生sigma点:

${\mathrm{\χ}}_{k-1}=[{\stackrel{\‾}{x}}_{k-1},{\stackrel{\‾}{x}}_{k-1}+\sqrt{(n+\mathrm{\λ})P_{k-1}},{\stackrel{\‾}{x}}_{k-1}-\sqrt{(n-\mathrm{\λ})P_{k-1}}]$

步骤3，时间更新：

$\left\{\begin{array}{c}{\mathrm{\χ}}_{k|k-1}^{*}=f({\mathrm{\χ}}_{k-1},u_k)\\ {\stackrel{\‾}{x}}_{k|k-1}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{w}_i^m{x}_{i.k|k-1}^{*}\\ P_{k|k-1}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{w}_i^c({\mathrm{\χ}}_{i,k|k-1}^{*}-{\stackrel{\_}{x}}_{k|k-1}){({\mathrm{\χ}}_{i.k|k-1}^{*}-{\stackrel{\‾}{x}}_{k|k-1})}^T+Q^v\\ {\mathrm{\χ}}_{k|k-1}=[{\stackrel{\‾}{x}}_{k|k-1},{\stackrel{\‾}{x}}_{k|k-1}+\sqrt{(n+\mathrm{\λ})P_{k|k-1}},{\stackrel{\‾}{x}}_{k|k-1}-\sqrt{(n+\mathrm{\λ})P_{k|k-1}}]\\ {\mathrm{\γ}}_{k|k-1}=h\left({\mathrm{\χ}}_{k|k-1}\right)\\ {\stackrel{\‾}{x}}_{k|k-1}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{w}_i^m{\mathrm{\γ}}_{i,k|k-1}\end{array}\right.$

步骤4，测量更新：

$\left\{\begin{array}{c}{P}_{{\stackrel{\‾}{y}}_k{\stackrel{\‾}{y}}_k}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{w}_i^c({\mathrm{\γ}}_{i.k|k-1}^{*}-{\stackrel{\‾}{y}}_{k|k-1}){({\mathrm{\γ}}_{i.k|k-1}^{*}-{\stackrel{\‾}{y}}_{k|k-1})}^T+Q^n\\ P_{{\stackrel{\‾}{x}}_k{\stackrel{\‾}{y}}_k}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{w}_i^c({\mathrm{\χ}}_{i.k|k-1}^{*}-{\stackrel{\‾}{x}}_{k|k-1}){({\mathrm{\γ}}_{i.k|k-1}^{*}-{\stackrel{\‾}{y}}_{k|k-1})}^T\\ P_{{\stackrel{\‾}{x}}_k{\stackrel{\‾}{y}}_k}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{w}_i^c({\mathrm{\χ}}_{i.k|k-1}^{*}-{\stackrel{\‾}{x}}_{k|k-1}){({\mathrm{\γ}}_{i.k|k-1}^{*}-{\stackrel{\‾}{y}}_{k|k-1})}^T\\ K_k=P_{{\stackrel{\‾}{x}}_k{\stackrel{\‾}{y}}_k}{P}_{{\stackrel{\‾}{y}}_k{\stackrel{\‾}{y}}_k}^{-1}\\ \stackrel{\‾}{x}={\stackrel{\‾}{x}}_{k|k-1}+K_k(y_k-{\stackrel{\‾}{y}}_{k|k-1})\end{array}\right.$

循环步骤2到步骤4，直到完成。

非线性系统的无色变换：

无色变换是UKF滤波算法实现的基础。无色变换(UT，UnscentedTransform)的基础原理是用采样点的分布来近似随机变量的概率分布，由上一时刻被估计量由被估计量的“先验”均值和方差，产生一批离散的与被估计量具有相同概率统计特性的采样点，成为sigma点，再根据经过非线性方程传递后的sigma点，生成“后验”的均值和方差。

已知非线性函数y＝f(x)，其中x为随机向量，它的均值和方差分别是x和P_x，求y的均值和方差P_y，其无色变换的步骤如下：

sigma点的生成

根据随机向量x的均值和方差P_x，可以构造一组关于x对称求分布于其附近的离散sigma点，可以记成χ_i(i＝1,2,3,...,2l)。我们可以用这一组sigma点近似表示随机向量x的近似分布，具体如下：

$\left\{\begin{array}{c}{\mathrm{\χ}}_o=\stackrel{\‾}{x}\\ {\mathrm{\χ}}_i={(\stackrel{\‾}{x}+\sqrt{(1+\mathrm{\λ})P_x})}_i,i=1,...,l\\ {\mathrm{\χ}}_i={(\stackrel{\‾}{x}-\sqrt{(1+\mathrm{\λ})P_x})}_{i-1},i=l+1,...,2l\end{array}\right.$

上式中，l为状态和噪声组成的扩展状态的维数，λ＝n(α²-1)，α为控制sigma点分布的常数，用这一组sigma点近似表示随即向量的分布，sigma点具有与x相同的均值和方差。

计算非线性函数γ_i＝g(χ_i)i＝0,1,...,2l

用γ_i近似非线性函数的分布。

计算非线性函数y的均值和方差P_y

$\left\{\begin{array}{c}\stackrel{\‾}{y}=\underset{i=0}{\overset{2l}{\mathrm{\Σ}}}{w}_i^m{\mathrm{\γ}}_i\\ P_y=\underset{i=0}{\overset{2l}{\mathrm{\Σ}}}{w}_i^c({\mathrm{\γ}}_i-\stackrel{\‾}{y}){({\mathrm{\γ}}_i-\stackrel{\‾}{y})}^T\end{array}\right.$

上式中，为权系数，且

$\left\{\begin{array}{c}{w}_o^m=\frac{\mathrm{\λ}}{\mathrm{\λ}+1}\\ w_o^c=\frac{\mathrm{\λ}}{\mathrm{\λ}+1}+(1-{\mathrm{\α}}^2+\mathrm{\β})\\ w_i^m=w_i^c=\frac{1}{2(1+\mathrm{\λ})},i=1,...,2l\end{array}\right.$

上式中，β是非负常数，它能使变换后的方差含有部分的高阶信息。

参照附图2，为本发明方案的PSO-SVM位置误差估计模型建立的流程图。在使用UKF滤波融合得到HOV导航信息的同时，航迹推算的HOV位置信息和推算时间序列作为PSO-SVM位置误差估计模型的输入，将航迹推算HOV位置信息和UKF滤波融合后的位置信息的差值作为PSO-SVM训练的目标集，然后选取RBF核函数，采用PSO参数寻优算法，确定参数C和利用选取的输入变量，核函数以及模型参数，对PSO-SVM模型进行训练学习，得到航迹推算经度、纬度位置误差估计模型。图3为混合导航定位方法的流程框图。

图4是SVM的原理图。支持向量回归机的基本原理就是求下式优化函数的解：

$\min \frac{1}{2}\underset{i,j=1}{\overset{l}{\mathrm{\Σ}}}({\mathrm{\α}}_i-{\mathrm{\α}}_i^{*})({\mathrm{\α}}_j-{\mathrm{\α}}_j^{*})k(x_i,x_j)+\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{\mathrm{\α}}_i(\mathrm{\ϵ}-y_i)+\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{\mathrm{\α}}_i(\mathrm{\ϵ}+y_i)$

$s.t.\left\{\begin{array}{c}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}({\mathrm{\α}}_i-{\mathrm{\α}}_i^{*})=0\\ {\mathrm{\α}}_i,{\mathrm{\α}}_i^{*}\∈[0,C]\end{array}\right.$

式中α_i，为拉格朗日乘子；

C为惩罚因子；

ε为不敏感系数；

m为样本数

根据拉格朗日函数的极值满足条件和最优化函数可以得到基于SVM的HOV的位置误差估计模型，

支持向量回归机中的核函数本发明方案选择了RBF核函数，其函数式为:

$K(x,x_i)=\exp (-\frac{{\left|\right|x-x_i\left|\right|}^2}{2{\mathrm{\δ}}^2})$

为了提高PSO-SVM位置误差模型的性能，本方案选用PSO参数寻优算法对模型中的核函数宽度δ和惩罚因子C进行寻优。

本发明方案选择平均绝对误差(MAPE，meanabsolutepercentageerror)作为适应度函数，其形式如下：

$f_{\mathrm{MAPE}}=\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\frac{|y_i-{\hat{y}}_i|}{y_i}$

上式中，是训练样本的个数；y_i是实际值；是预测值；f_MAPE是适应度值。

PSO-SVM位置误差估计模型建立过程如下：

(1)读入HOV位置样本数据集并对样本集进行预处理

(2)PSO初始化。初始化算法参数，随机初始化例子并形成粒子群体，随机生成各粒子的初始速度，将每个粒子的pbest设置当前位置，gbest设置为群体中最好粒子的当前位置。

(3)用训练集训练SVM，用适应度函数计算每个粒子的适应度值f_MAPE根据粒子的适应度值更新P_i和P_g。若某粒子当前适应度优于pbest则pbest被当前位置替代；若所有粒子的当前最优适应度优于gbest则gbest被当前最优位置替代。

(4)按照下式进行进化：

v_i+1＝f(v_i,x_i)

x_i+1＝g(x_i,v_i)

若未达到模型建立过程的结束条件，则返回(2)；否则输出最优解，PSO-SVM位置误差估计模型建立。

模型建立结束条件是达到最大迭代次数G_max或最优解在一定迭代次数内停滞不再变化。

根据上述建立的PSO-SVM位置误差估计模型，将航迹推算得到的HOV位置信息和推算时间序列作为PSO-SVM位置误差估计模型的输入，输出即为航迹推算经度、纬度的实时误差，将航迹推算的实时误差补偿到航迹推算经度、纬度，就可以在长基线定位系统不更新信息时同样得到精确的HOV位置导航信息。

本发明的效果通过以下实验加以验证。

为了验证被发明方案的有效性，我们采用了“蛟龙号”某次海试数据作为方案验证的数据样本。图5是本发明方案中将航迹推算和长基线导航定位系统，利用UKF滤波融合后的“蛟龙号”航迹图对比.从图中可以看出航迹推算随着时间的进行会产生累积误差，长基线导航定位系统具有多野值的缺点，经过UKF滤波融合以后，可以看出HOV航迹图中航迹曲线变得平滑和精确。

图6，图7是利用PSO-SVM位置误差模型对航迹推算HOV位置信息进行补偿后的东向和北向位置误差对比。对比补偿以前，利用误差模型补偿后的航迹推算位置信息误差已经减小到可接收范围内。综上，本发明方案一种基于UKF和PSO-SVM的HOV混合水下导航定位方法是一种可以很大提高HOV导航系统精度和可靠性的综合导航算法。

Claims (8)

1.一种基于UKF和PSO-SVM的HOV混合水下导航定位方法，其特征在于，包括以下步骤：

在长基线定位系统正常工作时，将HOV海试中得到的长基线位置信息和航迹推算位置信息，利用UKF进行融合，得到HOV导航位置信息；

在长基线定位系统失效时，UKF融合算法失效，此时将航迹推算位置信息和推算时间作为PSO-SVM位置误差估算模型的输入，模型输出实时预测的HOV位置补偿信息，从而在长基线定位系统失效的情况下获得HOV实时位置信息。

2.根据权利要求1所述的一种基于UKF和PSO-SVM的HOV混合水下导航定位方法，其特征在于，所述利用UKF进行融合，是将长基线位置信息和航迹推算得到的位置信息，在一阶动态模型下，滤波融合得到更加可靠的HOV的导航位置信息。

3.根据权利要求2所述的一种基于UKF和PSO-SVM的HOV混合水下导航定位方法，其特征在于，所述一阶动态模型的状态方程为：

上式中，x_k为航迹推算的得到的HOV东向位置，h_k为航迹推算得到的HOV北向位置，j_k为HOV艏向角，u_k为HOV前向速度，v_k为HOV横向速度，r_k为HOV艏向角速率，Dt为采样时间，为过程噪声(i＝1,2,3,4)；

观测方程定义为：

上式中，x_k为长基线定位系统测得的HOV东向位置，h_k为长基线定位系统测得的HOV北向位置，为过程噪声。

4.根据权利要求2所述的一种基于UKF和PSO-SVM的HOV混合水下导航定位方法，其特征在于，所述滤波融合通过以下过程实现：

步骤1，初始化：

步骤2，产生sigma点:

步骤3，时间更新：

步骤4，测量更新：

循环步骤2到步骤4，直到完成；

其中，x₀初始状态量，P₀初始协方差矩阵，P_k分别为随机向量x的最有预测和协方差，χ_i(i＝1,...,2l)为sigma点，λ＝n(α²-1)，α为控制sigma点分布的常数，Q^v表示过程噪声，Qⁿ为测量噪声，γ_i用来近似非线性函数分布，β为非负常数，其作用是使变换后的方差含有部分的高阶信息，y_k为时刻k的测量值，为y_k的最优预测，K_k为卡尔曼增益，为互协方差矩阵，为新息的协方差矩阵，为权系数。

5.根据权利要求1所述的一种基于UKF和PSO-SVM的HOV混合水下导航定位方法，其特征在于，所述长基线位置信息是长基线定位系统获得的HOV的位置信息，包括经度和纬度信息。

6.根据权利要求1所述的一种基于UKF和PSO-SVM的HOV混合水下导航定位方法，其特征在于，所述航迹推算位置信息是利用航迹推算系统传感器数据推算得到的HOV的位置信息，包括经度和纬度信息。

7.根据权利要求6所述的一种基于UKF和PSO-SVM的HOV混合水下导航定位方法，其特征在于，所述航迹推算系统传感器数据包括多普勒计程仪测量的HOV横向、前向速度信息；运动传感器器测得的HOV的航向角信息；GPS测量得到的HOV初始经度、纬度信息。

8.根据权利要求1所述的一种基于UKF和PSO-SVM的HOV混合水下导航定位方法，其特征在于，所述PSO-SVM位置误差估算模型的建立过程为：

(1)读入HOV位置样本数据集并对样本集进行预处理；

(2)PSO初始化：初始化算法参数，随机初始化例子并形成粒子群体，随机生成各粒子的初始速度，将每个粒子的pbest设置当前位置，gbest设置为群体中最好粒子的当前位置；

(3)用训练集训练SVM，用适应度函数计算每个粒子的适应度值f_MAPE根据粒子的适应度值更新P_i和P_g，若某粒子当前适应度优于pbest则pbest被当前位置替代；若所有粒子的当前最优适应度优于gbest则gbest被当前最优位置替代；

(4)按照下式进行进化：

v_i+1＝f(v_i,x_i)

x_i+1＝g(x_i,v_i)

式中，v_i为粒子速度，x_i为粒子位置；若未达到模型建立过程的结束条件，则返回步骤(2)；否则输出最优解，PSO-SVM位置误差估计模型建立；模型建立结束条件是达到最大迭代次数G_max或最优解在预设迭代次数内停滞不再变化。