Application of Kalman filter algorithm in ultra short baseline location of autonomous underwater vehicle

0 Introduction

At present, autonomous underwater vehicle has become an important platform for human to obtain Marine information, and has a broad development prospect. Autonomous underwater vehicle uses self-carried equipment such as acoustic transducer, geomagnetic sensor, inertial measurement, terrain matching device to achieve navigation. Due to the complexity of the Marine environment itself, the underwater acoustic location technology based on acoustic detection is one of the effective means to transmit and acquire underwater information, which is widely used in underwater robots.

Underwater acoustic positioning system can be divided into ultra-short baseline (usbl), short baseline (sbl) and long baseline (lbl) positioning systems according to the receiving array size [2]. The positioning system based on the ultra-short baseline utilizes the phase difference principle of the measurement signal for underwater positioning, in which the distance between the elements of the receiving array is usually less than or equal to half of the wave length, which makes the equipment more simple and easy to install, and at the same time has strong accuracy, adaptability and flexibility.

In this paper, the formation and positioning principle of the underwater acoustic positioning system based on the ultra-short baseline are deeply studied. On this basis, the energy loss and noise interference in the process of acoustic wave propagation are analyzed, and the measurement and motion equation based on Kalman filter algorithm is proposed. Finally, the simulation experiment is carried out.

1. Underwater acoustic positioning system

1.1 Ultra-short baseline underwater acoustic positioning system

In the underwater acoustic positioning system, the underwater acoustic positioning can be divided into ultra-short baseline, short baseline and long baseline according to the length of the baseline. Among them, the ultra-short baseline positioning uses the phase difference between the received signals between the elementary elements to realize the positioning of the underwater robot. In this technology, the distance between the elementary elements is no more than 1/2 wavelength, and the array length is usually in the order of centimeters or tens of centimeters.

In the underwater robot positioning system based on ultra-short baseline, an acoustic array composed of at least 3 transducers is usually installed on the surface hull, and the coordinate relationship between the ship and the acoustic array is precisely measured during installation. The distance between the transducers is generally several centimeters. During the positioning process, the transducer sends acoustic signals to the transponder installed on the autonomous underwater vehicle, and then measures the received signals of the transducer, and determines the position of the transponder by calculating the phase difference between the signals, and then determines the position of the underwater vehicle. The distance between the transducer and the underwater vehicle is determined by calculating the ship time of the acoustic wave and correcting the beam line of the acoustic velocity profile.

The combination of ultra short baseline positioning system and shipborne GPS system can effectively improve the accuracy of positioning of underwater vehicles. The GPS receiving device is placed on the surface ship, and the GPS system is used to measure the ship's position in real time. At the same time, the relative position and azimuth of the measuring ship and the underwater robot are measured synchronously, so as to calculate the accurate position of the underwater robot. The positioning system of underwater vehicle based on ultra-short baseline has the advantages of high cost performance, easy operation, easy installation and high measurement accuracy.

The working principle of the positioning system based on the ultra-short baseline is shown in Figure 1, where the receiving array is mounted on the bottom of the surface vessel and the transponder is mounted on the upper part of the underwater vehicle.

1.2 Positioning Principle

The positioning system based on ultra-short baseline is composed of receiving array, transponder and transmitting transducer. The receiving array and transmitting transducers are installed on the surface ship, and the transponder is installed on the underwater vehicle. The transmitter transmitter transmits acoustic pulses to the transponder. When the transponder receives the acoustic pulses, the response pulses will be sent. When the receiving array receives the response pulses, the phase difference in the X and Y directions will be measured. Then the position of the underwater robot in the plane coordinate system and its underwater depth [3] are obtained.

The positioning principle based on phase difference is as follows: assume that 5 receiving transducers are installed at equal intervals in a plane with consistent characteristics and vertical and orthogonal positions, and each receiving transducer is used as a primitive. A receiving transducer is placed at the coordinate origin O, and the other four receiving transducers Xa, Xb, Ya and Yb are placed at the position d/2 away from the origin O. The transponder placed on the underwater vehicle is P, and the distance between P and the coordinate origin O is S. The distance between P and other receiving transducers is SXa, SXb, SYa and SYb. Energy exchanger pulse signal, when the transponder received the pulse signal after the transmission should answer the signal, in each element respectively received the response signal, can root according to Xa, Xb signal phase difference between OP and X axis Angle θX, according to Ya, Yb signal phase difference between OP and Y axis Angle θY. The mathematical model is:

Where ∆ is the signal phase difference between the basic element; f is the signal frequency.

2. Kalman filtering algorithm

2.1 Algorithm Principle

Kalman filtering algorithm is a kind of minimum linear variance estimation, which has the following characteristics:

1) Because Kalman filter algorithm uses state space square method to design filter in time domain and recursively, it is suitable for estimation of multidimensional stochastic system.

2) Kalman filtering algorithm uses the equation of state and the dynamic equation to describe the change rule of the estimator. The information of the estimator to be estimated is usually determined by the dynamic equation and excited white noise statistics method. Since the dynamic equation is known and the excitation white noise is stable, the estimator can present either a stationary state or a non-stationary state, which means that the Kalman filter algorithm is also applicable to non-stationary processes.

3) Kalman filter algorithm has two kinds of calculation methods: discrete type and continuous type, among which, discrete algorithm is also applicable to non-stationary process.

For the discrete control system, random linear differential equation is used to describe it, then the Kalman filter algorithm [4] is as follows:

1) Firstly, the system model is used to pre-test the next state of the system, as follows:

Where, A and B are system parameters; U (k) is the current system control quantity; X (k-1 k-1) is the predicted result of the previous state of the system.

Update covariance corresponding to the predicted state of the system as follows:

Where, P is covariance, A 'is the transpose of A, and Q is covariance of the system process.

2) Collect the measured value of the system state, and obtain the optimal estimated value of the system state according to the predicted value, as follows:

Where, Kg is Kalman gain.

In order to enable the algorithm to perform autoregressive operation, it is also necessary to update covariance corresponding to the K-state of the system, as follows:

2.2 Positioning equation of underwater vehicle

According to the noisy data obtained by array measurement, Kalman filter algorithm is used to optimize the positioning estimation of underwater robot, and the motion equation of underwater robot is constructed. The vector form is as follows:

Where, X (k) is the state vector of the underwater vehicle, which is composed of four parts: XA (k+1), XA (k+1), YA (k+1), YA (k+1), and is the component of the underwater vehicle's position in the x and y directions at the time k+1. And are the components of the AUV's velocity in x and y directions at time k + 1.

In the measurement process, the projection of the underwater vehicle in the x and y directions is and respectively. Then, the vector form of the measurement equation is:

3 Simulation experiment

In this paper, the underwater robot positioning algorithm based on Kalman filter is simulated on the Matlab platform. During the experiment, it is assumed that roll or pitch occurs on the measured ship. The experimental results are shown in Figure 2.

In Figure 2, the first point is the number of points whose measurement error is within the range of [0,1), and the NTH point is the number of points whose measurement error is within the range of [n-1, n). It can be seen from the experimental results that the error accounts for 75% of the total number within 15m. Therefore, the algorithm proposed in this paper has an obvious effect in reducing the error.

4 Conclusion

Under the background that ultra-short base underwater acoustic positioning technology is widely used in autonomous underwater vehicle positioning system, how to use Kalman filter algorithm to improve the positioning accuracy is the focus of this paper. In this paper, the feasibility of applying Kalman filter algorithm to the positioning system of the underwater vehicle based on the ultra-short base is studied, and the corresponding motion and measurement model is established. Finally, the simulation experiment is carried out, and the experimental results meet the expectations.

References:

[1] Li Ye, Chang Wentian, Wan Lei, et al. Research on adaptive Kalman filtering technology for underwater robot [J]. Journal of Intelligent Systems, 2006, 1(2): 44 -- 47.

[2] KAMGAR-PARSI B, ROSENBLUM I, PIPITONE F, et,  al. Toward an automated system for a correctly registered bathymetric chart [J]. IEEE J. Ocean Engineering, 1999(4):  314-325.

[3] VERMA, a. k. Variability index constant false alarm rate (VI  CFAR) for sonar target detection [J]. IEEE - International Conference on Signal processing, CNMIT, Anna University Chennai India, 2008,(4-6): 38 -- 141.