CN102818567A

CN102818567A - AUV (autonomous underwater vehicle) integrated navigation method integrating Kalman filtering and particle filtering

Info

Publication number: CN102818567A
Application number: CN2012102805237A
Authority: CN
Inventors: 李建龙; 温国曦; 徐文
Original assignee: Zhejiang University ZJU
Current assignee: Zhejiang University ZJU
Priority date: 2012-08-08
Filing date: 2012-08-08
Publication date: 2012-12-12

Abstract

An AUV integrated navigation method combining Kalman filter and particle filter, including the following steps: 1) Data collection: use the global positioning system to obtain the initial position information of the AUV when it is on the water surface, use the Doppler log, electronic compass 2) Filter positioning: use the filter algorithm based on the combination of ensemble Kalman filter and particle filter to fuse the navigation information collected by the sensor, and estimate the position of the AUV at each moment and attitude change information to realize the global positioning of AUV. The invention provides an AUV combined navigation method combining ensemble Kalman filter and particle filter with improved precision.

Description

AUV Integrated Navigation Method Combining Ensemble Kalman Filter and Particle Filter

技术领域 technical field

本发明涉及海洋工程领域，尤其是一种自主水下航行器（Autonomous Underwater Vehicle，AUV）组合导航方法。The invention relates to the field of marine engineering, in particular to an integrated navigation method for an autonomous underwater vehicle (Autonomous Underwater Vehicle, AUV).

背景技术 Background technique

自主水下航行器是目前海洋工程领域技术发展的热点，在水下环境监测、近海石油工程作业、水下搜索与测绘、以及水雷对抗和实时战区警戒等军事领域获得越来越广泛的应用。导航技术是实现AUV实现自主航行的关键，由于AUV存在工作时间长、环境复杂、信息源少、隐蔽性要求高等工作特点，这给稳定、精确的AUV自主导航带来很大的困难和挑战。Autonomous underwater vehicles are currently a hotspot in the development of marine engineering technology, and have been widely used in military fields such as underwater environmental monitoring, offshore petroleum engineering operations, underwater search and mapping, mine countermeasures, and real-time theater alerts. Navigation technology is the key to realizing autonomous navigation of AUVs. Due to the characteristics of AUVs such as long working hours, complex environments, few information sources, and high requirements for concealment, this brings great difficulties and challenges to stable and accurate AUV autonomous navigation.

近年来国际上对于AUV导航技术的研究一直非常活跃。水下导航技术通常需要在精度、运动范围和自动化程度等之间进行平衡，概括起来主要有声学定位、地球物理导航及航位推算与惯性导航等多种类型的导航技术。In recent years, the international research on AUV navigation technology has been very active. Underwater navigation technology usually requires a balance between accuracy, range of motion, and degree of automation. In summary, there are various types of navigation technologies such as acoustic positioning, geophysical navigation, dead reckoning, and inertial navigation.

现有的AUV通常利用惯性导航系统（Inertial Navigation System，INS）采集的姿态角及加速度信息，三维电子罗盘（TCM）采集的姿态角信息以及多普勒计程仪（Doppler Velocity Log，DVL）采集的速度信息进行组合导航，常用的数据融合方法包括卡尔曼滤波（KalmanFilter，KF）扩展卡尔曼滤波（Extended Kalman Filter，EKF）、无味卡尔曼滤波（Unscented Kalman Filter，UKF）、粒子滤波（Particle Filter，PF）和集合卡尔曼滤波（Ensemble Kalman Filter，EnKF）等，其中扩展卡尔曼滤波和无味卡尔曼滤波的共同问题是在非线性/非高斯性较强时收敛性急剧下降甚至发散。而粒子滤波利用序贯蒙特卡洛方法的，对于非线性/非高斯问题具有较好的性能，但是在粒子滤波中如果最新的观测信息位于先验概率分布的尾部或者似然函数相比先验概率是峰化的，则会导致粒子选择的盲目性，降低了估计精度。Existing AUVs usually use the attitude angle and acceleration information collected by the inertial navigation system (Inertial Navigation System, INS), the attitude angle information collected by the three-dimensional electronic compass (TCM), and the Doppler Velocity Log (DVL) to collect The commonly used data fusion methods include Kalman filter (Kalman Filter, KF), extended Kalman filter (Extended Kalman Filter, EKF), tasteless Kalman filter (Unscented Kalman Filter, UKF), particle filter (Particle Filter , PF) and Ensemble Kalman Filter (Ensemble Kalman Filter, EnKF), etc., the common problem of extended Kalman filter and tasteless Kalman filter is that the convergence drops sharply or even diverges when the nonlinear/non-Gaussian is strong. The particle filter uses the sequential Monte Carlo method, which has better performance for nonlinear/non-Gaussian problems, but in the particle filter, if the latest observation information is located at the tail of the prior probability distribution or the likelihood function is compared with the prior The probability is peaked, which will lead to the blindness of particle selection and reduce the estimation accuracy.

发明内容 Contents of the invention

本发明的目的是提供一种基于集合卡尔曼滤波－粒子滤波相结合的自主水下航行器组合导航方法，以提高AUV导航算法的精度。The purpose of the present invention is to provide an autonomous underwater vehicle integrated navigation method based on the combination of ensemble Kalman filter and particle filter, so as to improve the accuracy of AUV navigation algorithm.

本发明解决其技术问题所采用的技术方案是：The technical solution adopted by the present invention to solve its technical problems is:

一种集合卡尔曼滤波－粒子滤波相结合的AUV组合导航方法，所述组合导航方法包括以下步骤：An AUV combined navigation method combining ensemble Kalman filtering and particle filtering, the combined navigation method comprising the following steps:

1）数据采集：获取AUV在水面时的初始位置信息，采集AUV的速度和姿态角信息；1) Data collection: Obtain the initial position information of the AUV when it is on the water surface, and collect the speed and attitude angle information of the AUV;

2）滤波定位：以步骤1）的初始位置信息、速度和姿态角信息定义状态向量、观测向量、状态模型方程和观测模型方程，滤波过程如下：2) Filter positioning: Define the state vector, observation vector, state model equation, and observation model equation with the initial position information, velocity, and attitude angle information in step 1. The filtering process is as follows:

2.1)根据初始条件生成初始粒子和它相应的权系数；2.1) Generate initial particles and their corresponding weight coefficients according to the initial conditions;

2.2)定义每个粒子背景集合

根据式(1)对背景集合进行预测，2.2) Define each particle background set

Predict the background set according to formula (1),

状态模型方程：X_k=f(X_k-1，w_k) (1)State model equation: X _k =f(X _k-1 , w _k ) (1)

其中X_k-1，X_k分别表示k-1和k时4的状态向量，w_k表示状态过程噪声，f(·)表示X_k-1和X_k之间的非线性关系；Among them, X _k-1 and X _k represent the state vectors of k-1 and k respectively, w _k represents the state process noise, and f( ) represents the nonlinear relationship between X _k-1 and X _k ;

计算背景集合的均值和协方差，并计算相应的卡尔曼增益：Compute the mean and covariance of the background set, and compute the corresponding Kalman gain:

${\overset{^^}{x x}}_{k k}^{((i i)),, b b} = = \frac{11}{n no} {Σ Σ}_{j j = = 11}^{n no} {x x}_{k k,, j j}^{((i i)),, b b} - - - - - - ((1212))$

${\overset{^^}{P P}}_{xh xh}^{((i i)),, k k} = = \frac{11}{n no - - 11} {Σ Σ}_{j j = = 11}^{n no} (({x x}_{k k,, j j}^{((i i)),, b b} - - {\overset{^^}{x x}}_{k k}^{((i i)),, b b})) {((h h (({x x}_{k k,, j j}^{((i i)),, b b})) - - h h (({\overset{^^}{x x}}_{k k}^{((i i)),, b b}))))}^{T T} - - - - - - ((1313))$

${\overset{^^}{P P}}_{hh hh}^{((i i)) k k} = = \frac{11}{n no - - 11} {Σ Σ}_{j j = = 11}^{n no} ((h h (({x x}_{k k,, j j}^{((i i)),, b b})) - - h h (({\overset{^^}{x x}}_{k k}^{((i i)),, b b})))) {((h h (({x x}_{k k,, j j}^{((i i)),, b b})) - - h h (({\overset{^^}{x x}}_{k k}^{((i i)),, b b}))))}^{T T} - - - - - - ((1414))$

${K K}_{k k}^{((i i))} = = {\overset{^^}{P P}}_{xh xh}^{((i i)),, k k} {(({\overset{^^}{P P}}_{hh hh}^{((i i)),, k k} + + {v v}_{k k}))}^{- - 11} - - - - - - ((1515))$

其中

为第i个粒子在k时刻背景集合的均值，为第i个粒子在k时刻背景集合第j个样本点的值，

为第i个粒子在k时刻背景集合的x与h(·)的协方差矩阵，

为第i个粒子在k时刻背景集合的h(·)与h(·)的协方差矩阵，

为第i个粒子在k时刻的卡尔曼增益；in

is the mean value of the background set of the i-th particle at time k, is the value of the i-th particle at the j-th sample point of the background set at time k,

is the covariance matrix of x and h( ) of the background set of the i-th particle at time k,

is the covariance matrix of h(·) and h(·) of the background set of the i-th particle at time k,

is the Kalman gain of the i-th particle at time k;

3)利用式(7)更新分析集合，3) Utilize equation (7) to update the analysis set,

${x x}_{k k,, i i}^{a a} = = {x x}_{k k,, i i}^{b b} + + {K K}_{k k} (({y the y}_{k k} - - h h (({x x}_{k k,, i i}^{b b})))),, i i = = 1,2 1,2,, . . . . . .,, n no - - - - - - ((77))$

其中y_k，i是以y_k为均值，v_k为方差的高斯分布的采样，

为第i个粒子在k时刻的分析集合，

为第i个粒子在k时刻的背景集合；where y _{k, i} is the sampling of Gaussian distribution with y _k as the mean and v _k as the variance,

is the analysis set of the i-th particle at time k,

is the background set of the i-th particle at time k;

并利用式(8)和(9)计算分析集合的均值和协方差：And use equations (8) and (9) to calculate the mean and covariance of the analysis set:

${\overset{^^}{x x}}_{k k}^{a a} = = \frac{11}{n no} {Σ Σ}_{i i = = 11}^{n no} {x x}_{k k,, i i}^{a a} - - - - - - ((88))$

${\overset{^^}{P P}}_{k k}^{a a} = = \frac{11}{n no - - 11} {Σ Σ}_{i i = = 11}^{n no} (({x x}_{k k,, i i}^{a a} - - {\overset{^^}{x x}}_{k k}^{a a})) {(({x x}_{k k,, i i}^{a a} - - {\overset{^^}{x x}}_{k k}^{a a}))}^{T T} - - - - - - ((99))$

其中

为k时刻的分析集合的均值，

为k时刻的分析集合第i个样本点的值，

为k时刻的分析集合的协方差；in

is the mean value of the analysis set at time k,

is the value of the i-th sample point of the analysis set at time k,

is the covariance of the analysis set at time k;

4)利用分析集合构造建议分布函数，采样得到新的粒子，并更新权系数：4) Use the analysis set to construct the suggested distribution function, sample new particles, and update the weight coefficient:

${\overset{^^}{x x}}_{k k}^{((i i))} ~ ~ q q (({x x}_{k k}^{((i i))} | | {x x}_{k k - - 11}^{((i i))},, {z z}_{k k})) = = N N (({\overset{^^}{x x}}_{k k}^{((i i)),, a a},, {\overset{^^}{P P}}_{k k}^{((i i))},, a a)) - - - - - - ((1616))$

${w w}_{k k}^{((i i))} = = {w w}_{k k - - 11}^{((i i))} \frac{p p (({z z}_{k k} | | {\overset{^^}{x x}}_{k k}^{((i i))})) p p (({\overset{^^}{x x}}_{k k}^{((i i))} | | {x x}_{k k - - 11}^{((i i))}))}{q q (({\overset{^^}{x x}}_{k k}^{((i i))} | | {x x}_{00 : : k k - - 11}^{((i i))},, {z z}_{11 : : k k}))} - - - - - - ((1717))$

5)判断是否存在退化现象，如果不存在退化现象则进行下一次循环，如果存在退化现象则先进行重采样，再进行下一次循环，直到滤波结束；5) Judging whether there is a degradation phenomenon, if there is no degradation phenomenon, the next cycle is performed, if there is a degradation phenomenon, resampling is performed first, and then the next cycle is performed until the end of the filtering;

滤波结束后估计得到AUV每一时刻的位置及姿态变化信息，实现对AUV的全局定位。After the filtering is completed, the position and attitude change information of the AUV at each moment is estimated to realize the global positioning of the AUV.

本发明的技术构思为：为了克服粒子滤波存在的问题，本发明利用集合卡尔曼滤波产生粒子滤波每一时刻的建议分布函数，提出了集合卡尔曼滤波-粒子滤波（EnKPF）相结合的AUV新型导航滤波算法，使得建议分布函数能够更接近真实的后验概率密度，通过粒子滤波和集合卡尔曼滤波两种滤波算法的优势互补，以提高AUV导航算法的精度。The technical idea of the present invention is: in order to overcome the problems existing in the particle filter, the present invention uses the ensemble Kalman filter to generate the suggested distribution function of the particle filter at each moment, and proposes a new type of AUV combining ensemble Kalman filter and particle filter (EnKPF). The navigation filtering algorithm makes the proposed distribution function closer to the real posterior probability density. The advantages of the two filtering algorithms, particle filtering and ensemble Kalman filtering, are complementary to improve the accuracy of the AUV navigation algorithm.

本发明的有益效果主要表现在：利用集合卡尔曼滤波产生粒子滤波每一时刻的建议分布函数，使得建议分布函数能够更接近真实的后验概率分布，通过粒子滤波和集合卡尔曼滤波两种滤波算法的优势互补，从而改善AUV导航算法的精度。The beneficial effects of the present invention are mainly manifested in: using the ensemble Kalman filter to generate the suggested distribution function for each moment of the particle filter, so that the suggested distribution function can be closer to the real posterior probability distribution, and through the particle filter and the ensemble Kalman filter The advantages of the algorithms complement each other, thereby improving the accuracy of the AUV navigation algorithm.

附图说明 Description of drawings

图1是本发明EnKPF方法的示意图。Figure 1 is a schematic diagram of the EnKPF method of the present invention.

图2PF算法流程图。Figure 2 PF algorithm flow chart.

图3是EnKF算法流程图。Figure 3 is a flowchart of the EnKF algorithm.

图4是EnKPF算法流程图。Figure 4 is a flowchart of the EnKPF algorithm.

图5是PF、EnKF及本发明EnKPF组合导航方法对AUV进行导航定位比较图，运动轨迹为AUV在水面上绕逆时针方向转过两圈，导航数据来源于AUV湖上实验。Fig. 5 is a comparison diagram of navigation and positioning of AUV by PF, EnKF and the combined navigation method of EnKPF of the present invention. The trajectory is that the AUV turns counterclockwise twice on the water surface, and the navigation data comes from the experiment on the AUV lake.

图6是PF、EnKF及本发明EnKPF组合导航方法对AUV进行导航定位比较图，运动轨迹为AUV在水面上做类似S型的运动，导航数据来源于AUV湖上实验。Fig. 6 is a comparison diagram of navigation and positioning of AUV by PF, EnKF and the combined navigation method of EnKPF of the present invention. The motion trajectory is that the AUV performs an S-like motion on the water surface, and the navigation data comes from the experiment on the AUV lake.

图7是PF、EnKF及本发明EnKPF组合导航方法对AUV进行导航定位比较图，运动轨迹为AUV先在水面上跑一段路程，然后进行下潜，在水下进行定深15m的运动，最后重新浮出水面，导航数据来源于AUV湖上实验。Fig. 7 is a comparison chart of navigation and positioning of AUV by PF, EnKF and the combined navigation method of EnKPF of the present invention. The motion track is that the AUV first runs for a distance on the water surface, then dives, and moves to a fixed depth of 15m underwater, and finally restarts. Surfaced, navigation data derived from AUV lake experiments.

具体实施方式 Detailed ways

下面结合附图对本发明作进一步描述。The present invention will be further described below in conjunction with the accompanying drawings.

参照图1～图7，一种集合卡尔曼滤波－粒子滤波相结合的AUV组合导航方法，所述组合导航方法包括以下步骤：Referring to Figures 1 to 7, an AUV integrated navigation method combining Kalman filter-particle filter, the integrated navigation method includes the following steps:

1）数据采集：利用全球定位系统获取AUV在水面时的初始位置信息，利用多普勒计程仪、电子罗盘等导航传感器采集AUV的速度和姿态角等信息；1) Data collection: Use the global positioning system to obtain the initial position information of the AUV when it is on the water surface, and use the Doppler log, electronic compass and other navigation sensors to collect information such as the speed and attitude angle of the AUV;

2）滤波定位：利用基于集合卡尔曼滤波－粒子滤波相结合的滤波算法将传感器采集到的导航信息进行融合，估计得到AUV每一时刻的位置及姿态变化信息，实现对AUV的全局定位。2) Filter positioning: use the filter algorithm based on the combination of ensemble Kalman filter and particle filter to fuse the navigation information collected by the sensor, estimate the position and attitude change information of the AUV at each moment, and realize the global positioning of the AUV.

传感器采集到的所述速度信息为前进速度信息、横向速度信息、垂直速度信息中的任一种或任几种。The speed information collected by the sensor is any one or more of forward speed information, lateral speed information, and vertical speed information.

传感器采集到的所述姿态角信息包括航偏角信息、横滚角信息、纵倾角信息中的任一种或任几种。The attitude angle information collected by the sensor includes any one or more of yaw angle information, roll angle information, and pitch angle information.

全球定位系统所获取自主水下机器人的初始位置信息为经度信息和纬度信息。The initial position information of the autonomous underwater robot acquired by the global positioning system is longitude information and latitude information.

本发明EnKPF组合导航方法的工作过程如图1所示。导航数据来源于两种途径，包括：（1）INS或者TCM采集的姿态角信息；（2）DVL采集的AUV速度信息。利用于集合卡尔曼滤波－粒子滤波相结合的滤波方法，结合AUV上安装的GPS记录的AUV初始位置信息，可以得到自主水下航行器每一时刻的位置信息和姿态角信息。以下为EnKPF具体实现方法。The working process of the EnKPF integrated navigation method of the present invention is shown in FIG. 1 . Navigation data comes from two ways, including: (1) attitude angle information collected by INS or TCM; (2) AUV speed information collected by DVL. Using the filtering method combined with ensemble Kalman filter and particle filter, combined with the initial position information of AUV recorded by GPS installed on the AUV, the position information and attitude angle information of the autonomous underwater vehicle at each moment can be obtained. The following is the specific implementation method of EnKPF.

对于某一非线性过程，它的状态方程和测量方程可以由以下方程描述：For a nonlinear process, its state equation and measurement equation can be described by the following equations:

观测模型方程：z_k=h(X_k,v_k) (2)Observation model equation: z _k =h(X _k ,v _k ) (2)

其中X_k-1，X_k分别表示k-1和k时刻的状态向量，w_k表示状态过程噪声，f(·)表示X_k-1和X_k之间的非线性关系，z_k表示k时刻的观测向量，v_k表示测量噪声，h(·)表示X_k和z_k之间的非线性关系。where X _k-1 and X _k represent the state vectors at k-1 and k time respectively, w _k represents the state process noise, f( ) represents the nonlinear relationship between X _k-1 and X _k , z _k represents k The observation vector at time, v _k represents the measurement noise, and h( ) represents the nonlinear relationship between X _k and z _k .

粒子滤波本质是采用序贯蒙特卡洛方法，其基本思想为从后验概率密度中独立的抽取N个采样点，通过加权求和，近似表示后验概率分布。它的流程如图2所示，它包括以下几个步骤：1)根据初始条件生成初始粒子和它相应的权系数；2)更新各个粒子状态向量；3)利用得到的各个状态向量更新值和该时刻的观测值相结合，更新各个粒子的权系数；4)判断是否存在退化现象，如果不存在退化现象则进行下一时刻状态向量及其权系数的更新，如果存在退化现象则先进行重采样，再进行下一时刻状态向量及其权系数的更新，直到滤波结束。The essence of the particle filter is to adopt the sequential Monte Carlo method. Its basic idea is to independently extract N sampling points from the posterior probability density, and approximate the posterior probability distribution through weighted summation. Its process is shown in Figure 2, which includes the following steps: 1) Generate initial particles and their corresponding weight coefficients according to the initial conditions; 2) Update the state vectors of each particle; 3) Utilize the obtained state vectors to update values and Combine the observed values at this moment to update the weight coefficients of each particle; 4) judge whether there is a degradation phenomenon, if there is no degradation phenomenon, update the state vector and its weight coefficient at the next moment, if there is a degradation phenomenon, first re- Sampling, and then update the state vector and its weight coefficient at the next moment until the end of filtering.

集合卡尔曼滤波的基本思想是初始化一组系统的状态采样作为背景集合，利用观测信息通过卡尔曼滤波对背景数据集中每个个体进行更新，得到分析集合。分析集合用来估计状态的真实均值和方差。通过系统模型传递分析集合，可以得到下一时刻的背景数据集。这样使用集合估计真实统计值，提高了估计精度。以下为集合卡尔曼滤波的算法步骤：The basic idea of ensemble Kalman filtering is to initialize a set of system state samples as a background set, and use observation information to update each individual in the background data set through Kalman filtering to obtain an analysis set. The analysis set is used to estimate the true mean and variance of the state. The background data set at the next moment can be obtained by passing the analysis set through the system model. In this way, the set is used to estimate the real statistical value, which improves the estimation accuracy. The following are the algorithm steps of ensemble Kalman filtering:

定义集合

为k时刻的背景集合，它由k-1时刻的分析集合

传递而来，n为集合样本数，采样均值和协方差可以由下式求得：define set

is the background set at time k, which consists of the analysis set at time k-1

Passed, n is the number of samples in the collection, and the sampling mean and covariance can be obtained by the following formula:

${\overset{^^}{x x}}_{k k}^{b b} = = \frac{11}{n no} {Σ Σ}_{i i = = 11}^{n no} {x x}_{k k,, i i}^{b b} - - - - - - ((33))$

${\overset{^^}{P P}}_{xh xh}^{k k} = = \frac{11}{n no - - 11} {Σ Σ}_{i i = = 11}^{n no} (({x x}_{k k,, i i}^{b b} - - {\overset{^^}{x x}}_{k k}^{b b})) {((h h (({x x}_{k k,, i i}^{b b})) - - h h (({\overset{^^}{x x}}_{k k}^{b b}))))}^{T T} - - - - - - ((44))$

${\overset{^^}{P P}}_{hh hh}^{k k} = = \frac{11}{n no - - 11} {Σ Σ}_{i i = = 11}^{n no} ((h h (({x x}_{k k,, i i}^{b b})) - - h h (({\overset{^^}{x x}}_{k k}^{b b})))) {((h h (({x x}_{k k,, i i}^{b b})) - - h h (({\overset{^^}{x x}}_{k k}^{b b}))))}^{T T} - - - - - - ((55))$

卡尔曼增益为：The Kalman gain is:

${K K}_{k k} = = {\overset{^^}{P P}}_{xh xh}^{k k} {(({\overset{^^}{P P}}_{hh hh}^{k k} + + {v v}_{k k}))}^{- - 11} - - - - - - ((66))$

式(6)中v_k表示k时刻的测量噪声。In formula (6), v _k represents the measurement noise at time k.

利用最新观测信息，可以对背景集合进行更新，得到k时刻的分析集合：Using the latest observation information, the background set can be updated to obtain the analysis set at time k:

式(7)中y_k，i是以y_k为均值，v_k为方差的高斯分布的采样，相应的，分析集合的均值和协方差为：In formula (7) _{, y k and i} are Gaussian distribution samples with y _k as the mean and v _k as the variance. Correspondingly, the mean and covariance of the analysis set are:

集合卡尔曼滤波－粒子滤波相结合的滤波方法就是结合了以上两种滤波方法，使用EnKF产生粒子滤波的建议分布函数，主要是通过传播后验分布的高斯估计以及将每个时刻的最近观测与之结合。换句话说，就是使用EnKF对如下后验概率密度进行递归估计：The filtering method combining ensemble Kalman filtering and particle filtering is to combine the above two filtering methods, and use EnKF to generate the suggested distribution function of particle filtering, mainly by propagating the Gaussian estimation of the posterior distribution and combining the latest observations at each moment with the combination. In other words, EnKF is used to recursively estimate the following posterior probability density:

$p p (({x x}_{k k} | | {z z}_{11 : : k k})) \approx \approx {p p}_{N N} (({x x}_{k k} | | {z z}_{11 : : k k})) = = N N (({\overset{^^}{x x}}_{k k},, {\overset{^^}{P P}}_{k k})) - - - - - - ((1010))$

在粒子滤波的框架中，对每个粒子使用一个独立的EnKF产生和传递高斯建议分布：In the framework of particle filtering, an independent EnKF is used for each particle to generate and pass a Gaussian proposal distribution:

$q q (({x x}_{k k}^{((i i))} | | {x x}_{11 : : k k - - 11}^{((i i))},, {z z}_{11 : : k k})) = = N N (({\overset{^^}{x x}}_{k k}^{((i i))},, {\overset{^^}{P P}}_{k k}^{((i i))})) - - - - - - ((1111))$

在k-1时刻，使用EnKF和最近观测信息对每个粒子的建议分布的均值和方差进行更新，在k时刻从这个分布中采样得到新的粒子。At time k − 1, the mean and variance of the proposed distribution for each particle are updated using EnKF and recent observation information, and new particles are sampled from this distribution at time k.

集合卡尔曼滤波－粒子滤波相结合的滤波方法的流程如图3所示，它可以概括为以下步骤：The flow of the filtering method combined with ensemble Kalman filter and particle filter is shown in Figure 3, which can be summarized as the following steps:

1)根据初始条件生成初始粒子和它相应的权系数；1) Generate initial particles and their corresponding weight coefficients according to the initial conditions;

2)定义每个粒子背景集合

根据式(1)对背景集合进行预测，用以下公式计算背景集合的均值和协方差，并计算相应的卡尔曼增益：2) Define each particle background set

Predict the background set according to formula (1), calculate the mean and covariance of the background set with the following formula, and calculate the corresponding Kalman gain:

其中

为第i个粒子在k时刻背景集合的均值，

为第i个粒子在k时刻背景集合第j个样本点的值，

为第i个粒子在k时刻背景集合的x与h(·)的协方差矩阵，

为第i个粒子在k时刻背景集合的h(·)与h(·)的协方差矩阵，

为第i个粒子在k时刻的卡尔曼增益，上标T表示转置运算；in

is the mean value of the background set of the i-th particle at time k,

is the value of the i-th particle at the j-th sample point of the background set at time k,

is the Kalman gain of the i-th particle at time k, and the superscript T represents the transpose operation;

3)利用式(7)更新分析集合，并利用式(8)和(9)计算分析集合的均值和协方差；3) Utilize equation (7) to update the analysis set, and use equations (8) and (9) to calculate the mean and covariance of the analysis set;

5)判断是否存在退化现象，如果不存在退化现象则进行下一次循环，如果存在退化现象则先进行重采样，再进行下一次循环，直到滤波结束。5) Judging whether there is a degradation phenomenon, if there is no degradation phenomenon, the next cycle is performed, if there is a degradation phenomenon, first resampling is performed, and then the next cycle is performed until the filtering ends.

采用EnKPF方法融合AUV各个传感器采集的AUV速度和姿态角信息，结合AUV上安装的GPS记录的AUV初始位置信息，可以得到自主水下航行器每一时刻的位置信息和姿态角信息。以下给出水平面内两维的组合导航例子。Using the EnKPF method to fuse the AUV speed and attitude angle information collected by each sensor of the AUV, combined with the AUV initial position information recorded by the GPS installed on the AUV, the position information and attitude angle information of the autonomous underwater vehicle at each moment can be obtained. An example of combined navigation in two dimensions in the horizontal plane is given below.

为对AUV进行定位，其状态向量如下式所示：In order to locate the AUV, its state vector is as follows:

x(k)=[p_x(k)p_y(k)v_x(k)v_y(k)θ(k)w(k)]^T （18）x(k)=[p _x (k)p _y (k)v _x (k)v _y (k)θ(k)w(k)] ^T (18)

其中p_x(k)和p_y(k)分别是在采样点k时刻AUV在x和y方向的水平位移。v_x(k)和v_y(k)分别是在采样点k时刻AUV的前进速度和横向速度。θ(k)是AUV在k时刻航偏角（与x轴的夹角）。w(k)是k时刻θ(n)的角加速度。where p _x (k) and p _y (k) are the horizontal displacements of the AUV in the x and y directions at sampling point k, respectively. v _x (k) and v _y (k) are the forward velocity and lateral velocity of the AUV at sampling point k, respectively. θ(k) is the yaw angle of the AUV at time k (the angle with the x-axis). w(k) is the angular acceleration of θ(n) at time k.

AUV的观测向量如下式所示The observation vector of AUV is shown in the following formula

z(k)=[v_x(k)v_y(k)θ(k)w(k)]^T （19）z(k)=[v _x (k)v _y (k)θ(k)w(k)] ^T (19)

其中前进速度v_x(k)和横向速度v_y(k)由多普勒计程仪采集。航偏角θ(k)可由电子罗盘采集。角加速度w(k)可由加速度计采集。Among them, the forward speed v _x (k) and the lateral speed v _y (k) are collected by the Doppler log. The yaw angle θ(k) can be collected by an electronic compass. The angular acceleration w(k) can be collected by an accelerometer.

根据运动方程，由式（18）可以得到具体的状态模型方程：According to the motion equation, the specific state model equation can be obtained from formula (18):

$x x ((k k)) = = [\begin{matrix} 11 & 00 & cos cos ((θ θ ((k k - - 11)))) ΔT ΔT & sin sin ((θ θ ((k k - - 11)))) ΔT Δ T & 00 & 00 \\ 00 & 11 & - - sin sin ((θ θ ((k k - - 11)))) ΔT ΔT & cos cos ((θ θ ((k k - - 11)))) ΔT Δ T & 00 & 00 \\ 00 & 00 & 11 & 00 & 00 & 00 \\ 00 & 00 & 00 & 11 & 00 & 00 \\ 00 & 00 & 00 & 00 & 11 & ΔT Δ T \\ 00 & 00 & 00 & 00 & 00 & 11 \end{matrix}] x x ((k k - - 11)) + + w w - - - - - - ((2020))$

由式（19）可以得到具体的测量模型方程：The specific measurement model equation can be obtained from formula (19):

$z z ((k k)) = = [\begin{matrix} 00 & 00 & 11 & 00 & 00 & 00 \\ 00 & 00 & 00 & 11 & 00 & 00 \\ 00 & 00 & 00 & 00 & 11 & 00 \\ 00 & 00 & 00 & 00 & 00 & 11 \end{matrix}] x x ((k k)) + + v v - - - - - - ((21 twenty one))$

其中△T为采样间隔，w和v分别为状态过程噪声和测量噪声。定义完状态向量、观测向量、状态模型方程和观测模型方程后，则可根据上述EnKPF滤波方法1）－5）的步骤预测更新AUV每一时刻的位置信息和姿态角信息。Where △T is the sampling interval, w and v are the state process noise and measurement noise respectively. After defining the state vector, observation vector, state model equation, and observation model equation, the position information and attitude angle information of the AUV at each moment can be predicted and updated according to the above-mentioned steps of EnKPF filtering method 1)-5).

实施例Example

实际应用中，AUV垂直维深度估计通常通过压力传感器直接估计深度信息，以下实施例利用AUV湖上试验导航数据进行二维水平面的定位算法分析。现场试验中，首先利用AUV上安装的GPS得到AUV的初始位置信息，再利用三维电子罗盘采集航偏角信息，惯性导航系统采集航偏角的角加速度信息，多普勒计程仪采集的AUV前进速度和横向速度信息，可以推算出AUV每一时刻的位置信息，得到AUV的运动轨迹。图5、图6和图7给出了利用本发明EnKPF方法进行AUV导航的三个航次数据处理结果，并与PF和EnKF方法进行了分析比较。In practical applications, AUV vertical dimension depth estimation usually directly estimates depth information through pressure sensors. The following examples use the AUV lake test navigation data to analyze the positioning algorithm of the two-dimensional horizontal plane. In the field test, first use the GPS installed on the AUV to obtain the initial position information of the AUV, then use the three-dimensional electronic compass to collect the yaw angle information, the inertial navigation system collects the angular acceleration information of the yaw angle, and the AUV yaw angle information collected by the Doppler log The forward speed and lateral speed information can be used to calculate the position information of the AUV at each moment and obtain the trajectory of the AUV. Fig. 5, Fig. 6 and Fig. 7 show the data processing results of three voyages of AUV navigation using the EnKPF method of the present invention, and analyze and compare with the PF and EnKF methods.

图5中，AUV在水面上绕逆时针方向转过了两圈，平均航行速度为1.8m/s，In Figure 5, the AUV has rotated counterclockwise twice on the water surface, with an average sailing speed of 1.8m/s.

轨迹8为AUV在水面由GPS得到的位置信息，轨迹9为利用PF方法得到的AUV每一时刻的位置信息，轨迹10为利用EnKF方法得到的AUV每一时刻的位置信息，轨迹11为利用本发明EnKPF法得到的AUV每一时刻的位置信息，点12为由GPS得到的AUV终点位置信息，点13为利用PF方法得到的AUV终点位置信息，点14为利用EnKF方法得到的AUV终点位置信息，点15为利用本发明EnKPF方法得到的AUV终点位置信息，由图5及表1可以看出利用本发明EnKPF方法得到的AUV终点位置信息最接近由GPS得到的AUV终点位置信息导航精度最高。Trajectory 8 is the position information of the AUV obtained by GPS on the water surface, trace 9 is the position information of the AUV at each moment obtained by the PF method, trace 10 is the position information of the AUV at each moment obtained by the EnKF method, and trace 11 is the position information of the AUV obtained by the method The position information of the AUV at each moment obtained by inventing the EnKPF method, point 12 is the AUV end position information obtained by GPS, point 13 is the AUV end position information obtained by the PF method, and point 14 is the AUV end position information obtained by the EnKF method , point 15 is the AUV terminal position information obtained by using the EnKPF method of the present invention. It can be seen from Fig. 5 and Table 1 that the AUV terminal position information obtained by the EnKPF method of the present invention is closest to the AUV terminal position information obtained by GPS and has the highest navigation accuracy.

图6中，AUV在水面上做类似S型的运动，平均航行速度为1.5m/s，轨迹16为AUV在水面由GPS得到的位置信息，轨迹17为利用PF方法得到的AUV每一时刻的位置信息，轨迹18为利用EnKF方法得到的AUV每一时刻的位置信息，轨迹19为利用本发明EnKPF方法得到的AUV每一时刻的位置信息，点20为由GPS得到的AUV终点位置信息，点21为利用PF方法得到的AUV终点位置信息，点22为利用EnKF方法得到的AUV终点位置信息，点23为利用本发明EnKPF方法得到的AUV终点位置信息，由图6及表2可以看出利用本发明EnKPF方法得到的AUV终点位置信息最接近由GPS得到的AUV终点位置信息导航精度最高。In Figure 6, the AUV moves in an S-like motion on the water surface, with an average sailing speed of 1.5m/s. Trajectory 16 is the position information of the AUV obtained by GPS on the water surface, and trajectory 17 is the AUV’s position at each moment obtained by using the PF method. Position information, track 18 is the position information at every moment of the AUV that utilizes the EnKF method to obtain, and track 19 is the position information at every moment of the AUV that utilizes the EnKPF method of the present invention to obtain, and point 20 is the AUV terminal position information that is obtained by GPS, point 21 is the AUV terminal position information that utilizes the PF method to obtain, point 22 is the AUV terminal position information that utilizes the EnKF method to obtain, and point 23 is the AUV terminal position information that utilizes the EnKPF method of the present invention to obtain, as can be seen from Fig. 6 and Table 2 The AUV terminal position information obtained by the EnKPF method of the present invention is closest to the AUV terminal position information obtained by GPS and has the highest navigation accuracy.

图7中，AUV在水面运行一段时间后进行下潜，平均航行速度为1.5m/s，定深15m航行，航行区域水深大于40m，轨迹24为AUV在水面由GPS得到的位置信息，图中路径中间部分AUV处于水下，因此没有GPS信息。轨迹25为利用PF方法得到的AUV每一时刻的位置信息，轨迹26为利用EnKF方法得到的AUV每一时刻的位置信息，轨迹27为利用本发明EnKPF方法得到的AUV每一时刻的位置信息，点28为由GPS得到的AUV最终上浮的位置信息，点29为利用PF方法得到的AUV最终上浮的位置信息，点30为利用EnKF方法得到的AUV最终上浮的位置信息，点31为利用本发明EnKPF方法得到的AUV最终上浮的位置信息，由图7及表3可以看出利用本发明EnKPF方法得到的AUV最终上浮的位置信息最接近由GPS得到的AUV最终上浮的位置信息导航精度最高。In Figure 7, the AUV dives after running on the water surface for a period of time, with an average navigation speed of 1.5m/s, sailing at a fixed depth of 15m, and the water depth in the navigation area is greater than 40m. Track 24 is the position information of the AUV on the water surface obtained by GPS, as shown in the figure The AUV in the middle part of the path is underwater, so there is no GPS information. Trajectory 25 is the position information of the AUV that utilizes the PF method to obtain at every moment, and trajectory 26 is the position information of the AUV that utilizes the EnKF method to obtain at each moment, and trajectory 27 is the position information at each moment of the AUV that utilizes the EnKPF method of the present invention to obtain, Point 28 is the final floating position information of the AUV obtained by GPS, point 29 is the final floating position information of the AUV obtained by the PF method, point 30 is the final floating position information of the AUV obtained by the EnKF method, and point 31 is the final floating position information of the AUV using the method of the present invention. The final floating position information of the AUV obtained by the EnKPF method can be seen from Figure 7 and Table 3. The final floating position information of the AUV obtained by the EnKPF method of the present invention is the closest to the final floating position information of the AUV obtained by GPS. The navigation accuracy is the highest.

滤波算法 Filtering Algorithm 最终位置绝对误差 Final position absolute error PF PF 1.5315m 1.5315m EnKF EnKF 1.2522m 1.2522m EnKPF EnKPF 0.9329m 0.9329m

表1Table 1

滤波算法 Filtering Algorithm 最终位置绝对误差 Final position absolute error PF PF 2.7958m 2.7958m EnKF EnKF 1.9554m 1.9554m EnKPF EnKPF 0.7726m 0.7726m

表2Table 2

滤波算法 Filtering Algorithm 最终位置绝对误差 Final position absolute error PF PF 28.6549m 28.6549m EnKF EnKF 26.9219m 26.9219m EnKPF EnKPF 10.6352m 10.6352m

表3table 3

上述实施例表明利用本发明集合卡尔曼滤波－粒子滤波相结合的自主水下航行器组合导航方法可提高组合导航系统的定位精度。The above embodiments show that the positioning accuracy of the integrated navigation system can be improved by using the integrated navigation method of the autonomous underwater vehicle combined with the Kalman filter and the particle filter of the present invention.

Claims

1. An AUV combined navigation method combining Kalman filter-particle filter, characterized in that: the combined navigation method comprises the following steps:

1) Data collection: Obtain the initial position information of the AUV when it is on the water surface, and collect the speed and attitude angle information of the AUV;

2) Filter positioning: Define the state vector, observation vector, state model equation, and observation model equation with the initial position information, velocity, and attitude angle information in step 1. The filtering process is as follows:

2.1) Generate initial particles and their corresponding weight coefficients according to the initial conditions;

2.2) Define each particle background set

Predict the background set according to formula (1),

State model equation: X _k =f(X _k-1 , w _k ) (1)

Among them, X _k-1 and X _k represent the state vectors at k-1 and k moments respectively, w _k represents the state process noise, and f( ) represents the nonlinear relationship between X _k-1 and X _k ;

Compute the mean and covariance of the background set, and compute the corresponding Kalman gain:

{\overset{^^}{x x}}_{k k}^{((i i)),, b b} = = \frac{11}{n no} {Σ Σ}_{j j = = 11}^{n no} {x x}_{k k,, j j}^{((i i)),, b b} - - - - - - ((1212))

{\overset{^^}{P P}}_{xh xh}^{((i i)),, k k} = = \frac{11}{n no - - 11} {Σ Σ}_{j j = = 11}^{n no} (({x x}_{k k,, j j}^{((i i)),, b b} - - {\overset{^^}{x x}}_{k k}^{((i i)),, b b})) {((h h (({x x}_{k k,, j j}^{((i i)),, b b})) - - h h (({\overset{^^}{x x}}_{k k}^{((i i)),, b b}))))}^{T T} - - - - - - ((1313))

{\overset{^^}{P P}}_{hh hh}^{((i i)) k k} = = \frac{11}{n no - - 11} {Σ Σ}_{j j = = 11}^{n no} ((h h (({x x}_{k k,, j j}^{((i i)),, b b})) - - h h (({\overset{^^}{x x}}_{k k}^{((i i)),, b b})))) {((h h (({x x}_{k k,, j j}^{((i i)),, b b})) - - h h (({\overset{^^}{x x}}_{k k}^{((i i)),, b b}))))}^{T T} - - - - - - ((1414))

{K K}_{k k}^{((i i))} = = {\overset{^^}{P P}}_{xh xh}^{((i i)),, k k} {(({\overset{^^}{P P}}_{hh hh}^{((i i)),, k k} + + {v v}_{k k}))}^{- - 11} - - - - - - ((1515))

in

is the mean value of the background set of the i-th particle at time k,

3) Utilize equation (7) to update the analysis set,

{x x}_{k k,, i i}^{a a} = = {x x}_{k k,, i i}^{b b} + + {K K}_{k k} (({y the y}_{k k} - - h h (({x x}_{k k,, i i}^{b b})))),, i i = = 1,2 1,2,, . . . . . .,, n no - - - - - - ((77))

where y _{k, i} is the sampling of Gaussian distribution with y _k as the mean and v _k as the variance,

is the analysis set of the i-th particle at time k,

is the background set of the i-th particle at time k;

And use equations (8) and (9) to calculate the mean and covariance of the analysis set:

{\overset{^^}{x x}}_{k k}^{a a} = = \frac{11}{n no} {Σ Σ}_{i i = = 11}^{n no} {x x}_{k k,, i i}^{a a} - - - - - - ((88))

{\overset{^^}{P P}}_{k k}^{a a} = = \frac{11}{n no - - 11} {Σ Σ}_{i i = = 11}^{n no} (({x x}_{k k,, i i}^{a a} - - {\overset{^^}{x x}}_{k k}^{a a})) {(({x x}_{k k,, i i}^{a a} - - {\overset{^^}{x x}}_{k k}^{a a}))}^{T T} - - - - - - ((99))

in is the mean value of the analysis set at time k,

is the value of the i-th sample point of the analysis set at time k, is the covariance of the analysis set at time k;

4) Use the analysis set to construct the suggested distribution function, sample new particles, and update the weight coefficient:

{\overset{^^}{x x}}_{k k}^{((i i))} ~ ~ q q (({x x}_{k k}^{((i i))} | | {x x}_{k k - - 11}^{((i i))},, {z z}_{k k})) = = N N (({\overset{^^}{x x}}_{k k}^{((i i)),, a a},, {\overset{^^}{P P}}_{k k}^{((i i))},, a a)) - - - - - - ((1616))

{w w}_{k k}^{((i i))} = = {w w}_{k k - - 11}^{((i i))} \frac{p p (({z z}_{k k} | | {\overset{^^}{x x}}_{k k}^{((i i))})) p p (({\overset{^^}{x x}}_{k k}^{((i i))} | | {x x}_{k k - - 11}^{((i i))}))}{q q (({\overset{^^}{x x}}_{k k}^{((i i))} | | {x x}_{00 : : k k - - 11}^{((i i))},, {z z}_{11 : : k k}))} - - - - - - ((1717))

5) Judging whether there is a degradation phenomenon, if there is no degradation phenomenon, perform the next cycle, if there is a degradation phenomenon, first perform resampling, and then perform the next cycle until the end of filtering;

After the filtering is completed, the position and attitude change information of the AUV at each moment is estimated to realize the global positioning of the AUV.