Underwater vehicle docking process position estimation system and method based on reinforcement learning

Abstract

The invention provides an underwater vehicle docking process position estimation system and method based on reinforcement learning. The problem of position deviation in the underwater vehicle docking process is solved by combining reinforcement learning and Kalman filtering. The reinforcement learning learns filtering gain from historical data including sensor measurement and estimation, and adaptive error compensation of position information in the docking process of the underwater vehicle is realized by using a good starting point provided by Kalman filtering and learnable characteristics of the reinforcement learning. The ultra-short baseline positioning information and the inertial navigation information are fused, the problems of inertial navigation errors accumulated along with time and ultra-short baseline data lag are solved by using the extended Kalman filter algorithm, and the positioning stability and real-time performance are improved. Through simulation verification, the method can effectively compensate the gain of the filter and accurately estimate the position of the underwater vehicle in the docking process.

Description

Technical Field

[0001] The present application belongs to the technical field of underwater vehicles, and specifically relates to a system and method for estimating the position of an underwater vehicle during docking based on reinforcement learning. Background Art

[0002] At present, underwater docking navigation mainly includes inertial navigation system and acoustic positioning system. Inertial navigation is a navigation parameter calculation system that uses gyroscopes and accelerometers as sensitive elements. The system establishes a navigation coordinate system based on the output of the gyroscope and calculates the speed and position of the vehicle by integrating the output of the accelerometer. Due to its small size and high accuracy, inertial navigation has gradually become the mainstream development direction, but it has the disadvantage of accumulating navigation positioning errors over time. Acoustic positioning systems include long baselines, short baselines and ultra-short baselines. Due to its advantages of long sound wave transmission distance and small attenuation loss, acoustic positioning technology has been fully developed. Ultra-short baseline positioning is an acoustic positioning method that has received more attention due to its outstanding positioning performance. It uses the time delay and phase difference of sound wave propagation to obtain the relative geometric relationship of autonomous underwater vehicles. However, traditional ultra-short baseline positioning has problems such as insufficient attitude calculation, slow signal update and positioning data delay when underwater vehicles are accurately docked. The single navigation mode has certain shortcomings in accuracy and reliability, and cannot meet the needs of underwater docking of vehicles. Therefore, the use of inertial navigation combined with Kalman filtering and ultra-short baseline positioning combined navigation can achieve complementary advantages and greatly meet the accuracy and reliability requirements of underwater vehicle docking. In order to fully realize the data fusion of combined navigation and obtain more accurate estimates, the main filtering methods currently include Kalman filtering, extended Kalman filtering and unscented Kalman filtering.

[0003] The Kalman filter is an optimal estimation algorithm that is linear, unbiased, and uses the minimum mean square error as a criterion, but performs poorly when dealing with nonlinear problems. The extended Kalman filter linearizes the nonlinear system by calculating the Jacobian matrix to obtain a more accurate estimate, while the unscented Kalman filter uses an unscented transformation and a set of fixed weighted sampling points to adapt the statistical characteristics of the sampling points passed by the nonlinear system so that it passes in a Gaussian form. However, the filter gain in these filtering algorithms determines the convergence of the estimation performance, and in practical applications it is usually necessary to manually adjust the filter gain to obtain the best estimate. Failure to adjust the filter gain may result in slow convergence or even divergence of the estimation performance.

[0004] Therefore, scholars have proposed many different methods to adjust the filter gain. Some literature proposes to identify the dynamic state and estimate the filter parameters online by performing fuzzy logic reasoning on the sensor's error signal and dynamic motion amplitude. Other scholars have proposed a two-step Kalman filter based on fuzzy logic variance matching. The filter consists of two sub-filters and adaptively adjusts the filter gain based on fuzzy logic variance matching. However, no matter how the filter is designed and adjusted, it is difficult for traditional methods to further improve the positioning accuracy of the integrated navigation system.

[0005] The development of machine learning has provided a new solution to this problem. At present, there are methods based on deep learning, which use deep learning to reduce the noise and bias of sensors to correct parameters to obtain more accurate attitude estimation. However, this method only removes the noise of a single inertial measurement sensor and cannot obtain accurate position estimation in a complex docking environment. A literature proposes a deep reinforcement learning estimator based on Lyapunov for state estimation of nonlinear random discrete systems, and achieves good estimation performance. However, this method uses reinforcement learning to train data from scratch, the sample efficiency during the training process is not high, and the training effect cannot be guaranteed. Therefore, it is necessary to study a more intelligent and adaptive positioning method for the docking process of underwater vehicles.

[0006] Through the analysis of the above background, it can be seen that the existing underwater positioning methods of aircraft can meet the needs of some underwater positioning applications. However, the underwater docking process puts forward higher requirements on positioning accuracy, and the existing filtering methods cannot effectively improve the positioning accuracy; on the other hand, the filter parameters have a great influence on the filtering accuracy, and accurate filter parameters can obtain relatively accurate positioning information. Therefore, it is necessary to improve the existing methods to improve their performance in the above tasks. Summary of the invention

[0007] The purpose of the present application is to overcome the defect of inaccurate filter gain estimation in the prior art.

[0008] In order to achieve the above-mentioned purpose, the present application proposes a position estimation system for underwater vehicle docking process based on reinforcement learning, the system comprises a sensor preprocessing module, a state prediction module, a state correction module and a reinforcement learning compensation module; wherein,

[0009] The sensor preprocessing module is used to align the raw data of the sensor at a certain time and filter out noise through a low-pass filter;

[0010] The state prediction module is used to predict the position of the underwater vehicle at the next moment based on the position of the underwater vehicle at the current moment in combination with the inertial navigation data;

[0011] The state correction module is used to correct the static error of the position of the underwater vehicle based on the sensor data received by the inertial navigation and USBL;

[0012] The reinforcement learning compensation module is built based on a deep neural network and is used to solve the extended Kalman filter gain that can optimize the performance index and output it to the state correction module.

[0013] As an improvement of the above system, the sensors involved in the system include inertial navigation and USBL; wherein,

[0014] The information of inertial navigation measurement is expressed as:

[0015]

[0016] Among them, σ1, σ2 are the inertial navigation measurement noise; v t is the speed of the underwater vehicle at time t; θ t is the heading angle of the underwater vehicle at time t;

[0017] The information measured by USBL is expressed as:

[0018]

[0019] Among them, L t represents the straight-line distance from the underwater vehicle position to the target point measured by USBL at time t; α t represents the angle between the straight-line distance measured by USBL at time t and the horizontal direction; σ3 and σ4 are the inertial navigation measurement noise;

[0020] The sensor preprocessing module uses the inertial navigation sampling time as a reference to align the data obtained by USBL to the first inertial navigation time before the data is received;

[0021] The low-pass filter is calculated as follows:

[0022] y[n]=bx[n]+(1-b)y[n-1]

[0023] Among them, y[n] represents the filter output signal at discrete time point n; x[n] represents the input signal at discrete time point n; b represents the filter coefficient, which is used to control the characteristics of the filter; y[n-1] represents the filter output signal at the previous time point n-1.

[0024] As an improvement of the above system, the state prediction module predicts the position of the aircraft at the next moment as follows:

[0025]

[0026] in, represents the speed information at time t+1 obtained by recursively calculating the speed information at time t; represents the heading angle information at time t+1 obtained by recursively calculating the heading angle information at time t; Represents the position information on the x-axis at time t The position information at time t+1 obtained by recursion; Represents the position information on the y-axis at time t The position information at time t+1 obtained recursively; Δt represents the time difference between time t+1 and time t.

[0027] As an improvement of the above system, the method of the state correction module to correct the static error of the position of the aircraft is as follows:

[0028] Based on the measured target position and the position of the docking point, calculate the predicted distance at time t+1 and azimuth

[0029]

[0030] Among them, P ux ,P uy Represents the fixed position information of the docking point, using sampled sensor measurements to correct the predicted range and azimuth;

[0031] Use USBL measurements to correct the predicted distance at time t+1 Azimuth speed and heading angle

[0032]

[0033] Among them, k 1,t+1 ,k 2,t+1 ,k 3,t+1 ,k 4,t+1 represents the Kalman filter gain at time t+1; L t+1 ,α t+1 ,v t+1 ,θ t+1 Represents the distance, azimuth, speed and heading angle measured by the sensor at time t+1;

[0034] Correct the current position using the corrected distance and azimuth and

[0035]

[0036] As an improvement of the above system, the reinforcement learning compensation module includes: an action network unit, a reward and punishment unit, an evaluation network unit and an experience replay pool; wherein,

[0037] The action network unit is composed of a deep neural network, receives a state as an input, and is used to calculate the extended Kalman gain value of the underwater vehicle after compensation by the reinforcement learning compensation module in the current state;

[0038] The reward and penalty unit is used to calculate the current instantaneous performance index of the underwater vehicle according to the current state and the compensated extended Kalman gain value calculated by the action network unit;

[0039] The evaluation network unit is composed of a deep neural network, which is trained by the historical experience data in the experience playback pool, and is used to evaluate the quality of the underwater vehicle's action in the current state, predict the comprehensive position error of the underwater vehicle in the future under the current state and current action conditions, and guide the action network unit to adjust the parameters used in the process of calculating the compensation gain according to the evaluation results;

[0040] The experience replay pool is used to periodically sample the current position information of the underwater vehicle, the Kalman gain compensation calculated by the action network unit, and the performance index obtained by the reward and penalty unit to form historical experience data.

[0041] As an improvement of the above system, the training process of the reinforcement learning compensation module includes:

[0042] Step 1: Initialize the deep neural network;

[0043] Step 2: The sensor obtains the current time information and obtains the estimated position information through the extended Kalman filter;

[0044] Step 3: The action network unit calculates the compensation gain based on the sensor information and the estimated position information;

[0045] Step 4: The reward and penalty unit calculates the performance index based on the sensor information, the extended Kalman estimated position information and the compensation gain;

[0046] Step 5: The experience playback pool collects the current estimated position information, compensation gain and performance indicators of the underwater vehicle to form historical experience data;

[0047] Step 6: The evaluation network unit evaluates the underwater vehicle's actions under the guidance of the experience replay pool;

[0048] Step 7: The evaluation network unit guides and adjusts the parameters of the action network unit;

[0049] Step 8: Update the deep neural network weights and estimated position information for the next training.

[0050] As an improvement of the above system, the one-step reward function of the reinforcement learning compensation module is used to measure the quality of the state-action pair in each training process, namely:

[0051]

[0052] Among them, r represents the reward value of the state-action pair, that is, the quality of the state-action pair after each training; represents the estimated residual at time t; x t represents the sensor measurement value at time t; represents the predicted value at time t; ||·|| 2 represents the 2-norm of the matrix;

[0053] Total Return R t is the sum of the reward function for one step:

[0054]

[0055] Among them, γ∈[0,1) represents the discount factor;

[0056] Value function of the reinforcement learning compensation module The definition is as follows:

[0057]

[0058] Among them, P t+1|t is the estimated residual transition probability; Indicates status Next select K t Strategy; K t represents the gain of the predicted value; δ∈[0,1) represents the constant discount factor.

[0059] As an improvement of the above system, the reinforcement learning compensation module uses an offline reinforcement learning algorithm to sample data in the experience replay pool during the training phase, and uses the offline generated data to train the action network unit and the evaluation network unit.

[0060] As an improvement of the above system, the action network unit is implemented by a neural network structure with a fully connected layer, and the input layer is the current state The hidden layer consists of two layers of networks, each layer contains 64 neurons, and the output layer is a single neuron; the Relu activation function is used between the input layer and the hidden layer, and between the hidden layer and the hidden layer, and the tanh activation function is used between the output layer and the hidden layer;

[0061] The evaluation network unit is implemented using a fully connected neural network structure, and the input layer consists of the current state The hidden layer is composed of two layers of networks, each layer contains 64 neurons, and the output layer is a single neuron. The Relu activation function is used between the input layer and the hidden layer, and between the hidden layer and the hidden layer, and the linear activation function is used between the output layer and the hidden layer.

[0062] The present application also provides a method for estimating the position of an underwater vehicle during docking based on reinforcement learning, which is implemented based on the above system and includes:

[0063] Step S1: The sensor preprocessing module preprocesses the measurement values ​​obtained by the inertial navigation and USBL sensors and removes noise through a low-pass filter;

[0064] Step S2: The state prediction module predicts the position of the underwater vehicle at the next moment based on the current position of the underwater vehicle and the inertial navigation data;

[0065] Step S3: The reinforcement learning compensation module solves the extended Kalman filter gain and outputs it to the state correction module;

[0066] Step S4: the state correction module updates the original filter gain using the extended Kalman filter gain;

[0067] Step S5: Repeat steps S2 to S4 to obtain the final estimated position of the underwater vehicle.

[0068] Compared with the prior art, the advantages of this application are:

[0069] This application integrates ultra-short baseline positioning information with inertial navigation information, and uses the extended Kalman filter algorithm to solve the problems of inertial navigation errors accumulated over time and ultra-short baseline data lag, and improves the stability and real-time performance of positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0070] Figure 1 Shown is the reinforcement learning training process of the extended Kalman filter;

[0071] Figure 2 Shown is a model of the underwater vehicle docking process;

[0072] Figure 3 Shown is the flow chart of reinforcement learning compensated extended Kalman filter;

[0073] Figure 4 Shown is the actor neural network diagram;

[0074] Figure 5 Shown is the critic neural network diagram;

[0075] Figure 6 The extended Kalman filter target tracking performance is shown;

[0076] Figure 7 Shown is the performance of extended Kalman filter target tracking improved by reinforcement learning;

[0077] Figure 8 Shown is the reward return curve during training. DETAILED DESCRIPTION

[0078] The technical solution of the present application is described in detail below with reference to the accompanying drawings.

[0079] The reinforcement learning-based underwater vehicle docking process position estimation system and method provided in the present application combines reinforcement learning with the Kalman filter to solve the problem of position deviation in the underwater navigation docking process. In the proposed system and method, reinforcement learning learns through the interaction between the agent and the environment, and has the ability to adapt to environmental changes. Reinforcement learning is used to train the filter gain in the Kalman filter and utilize the learnable characteristics of reinforcement learning. Kalman filtering provides a good starting point for reinforcement learning. First, the extended Kalman filter is used to fuse sensor information, and then the reinforcement learning training strategy is used to calculate the estimated residual of the fused data. The optimized extended Kalman filter is obtained by correcting the filter gain to correct the estimation error of the Kalman filter. The learned reinforcement learning strategy will adaptively adjust the filter gain value according to the residual size of the extended Kalman filter, thereby compensating for the filter estimation error. With the introduction of reinforcement learning, there is no need to manually adjust the filter parameters, and the reinforcement learning strategy can be used to adjust the filter gain in real time.

[0080] This application studies the close-range underwater docking process of underwater vehicles and their smooth attitude changes. The underwater vehicle only needs significant attitude changes when entering the water or in the initial stages of ascent and descent. During the close-range docking process, the depth difference between the underwater vehicle and the docking station is relatively small, so we assume that the vehicle and the docking station remain at the same horizontal depth throughout the process. In order to balance computational complexity and model accuracy, we only study the position information of the two-dimensional plane during the docking process. We used simplified inertial navigation and position calculation equations to simulate the movement of the underwater vehicle during the docking process, such as Figure 2 As shown. o is the starting point of the docking process. u is the position of the docking station. Since the attitude change of the underwater vehicle during the docking process is relatively stable, this application only considers the position and speed information.

[0081] Example 1

[0082] The present application provides a reinforcement learning-based underwater vehicle docking process position estimation system, which is based on the Kalman filter algorithm and includes: a sensor preprocessing module, a state prediction module, a state correction module and a reinforcement learning compensation module. The system is used to integrate multiple sensor information of the underwater vehicle and the dynamic characteristics of the underwater vehicle to estimate a stable and smooth vehicle position.

[0083] The position of the aircraft is expressed as (p x ,p y ), the heading angle of the spacecraft is denoted as θ and the speed is denoted as v.

[0084] 1. The sensor preprocessing module aligns the raw data of the sensor at all times and performs preliminary noise filtering through a low-pass filter.

[0085] The sensors involved in the present invention include inertial navigation and ultra-short baseline positioning system (USBL).

[0086] The information of inertial navigation measurement is expressed as:

[0087]

[0088] Wherein, a represents the acceleration measured by inertial navigation, ω represents the angular velocity measured by inertial navigation, σ1 and σ2 are the noises measured by inertial navigation. Since the present invention studies the uniform linear motion of underwater vehicles, a and ω are constant to 0.

[0089] According to the inertial navigation principle shown in formula (1), the simplified inertial navigation position calculation equation can be obtained as follows:

[0090]

[0091] In the equation, p x ,p y ,p x0 ,p y0 represents the position information and initial position of the underwater vehicle in the Cartesian coordinate system, v, v0 represents the speed and initial speed information of the vehicle's direction of travel, θ, θ0 represents the angle between the vehicle's direction of travel and the x-axis and the initial heading angle, t represents the measurement update time interval, ω1, ω2, ω3, ω4 represent the simulated inertial navigation measurement noise. This equation is a simplified navigation solution equation. Since the speed and position information of inertial navigation comes from integration, this process is accompanied by the coupling of sensor measurement noise. Due to the existence of integration, the positioning error of inertial navigation will continue to increase over time, that is, the error accumulation and position drift increase. Long-term use of inertial navigation will lead to large errors.

[0092] The information measured by USBL is expressed as:

[0093]

[0094] Where L represents the straight-line distance from the aircraft position to the target point measured by USBL, α represents the angle between the straight-line distance measured by USBL and the horizontal direction, and σ3 and σ4 are the inertial navigation measurement noises.

[0095] USBL mainly consists of a sound source transmitter and a rigid acoustic array installed on a ship. The sound source transmitter is located underwater and its position is known. It is usually used as an acoustic beacon, echo transponder or transducer. The transducer sends sound to the echo transponder, and the echo transponder returns the sound signal to the hydrophone. The slant range L and azimuth α can be obtained through the round-trip time delay and arrival angle information. It can be seen from formula (3) that the measurement principle of USBL can be expressed in a simplified two-dimensional plane using the following equation:

[0096]

[0097] Among them, P u =(P ux ,P uy ) represents the position of the docking station, and n1, n2 represent the measurement noise of the ultra-short baseline positioning. The ultra-short baseline positioning method is widely used in the underwater docking process. By using four hydrophones on the vehicle, they can provide very high positioning accuracy, with a close-range ranging accuracy of within a few centimeters and an attitude accuracy of within 1 degree. Although the ultra-short baseline positioning system can provide accurate positioning information during close-range navigation during the docking process, its positioning data update time is long and the data has a lag, which makes it inappropriate to directly use the ultra-short baseline positioning data for accurate close-range guidance and docking.

[0098] This application takes the inertial navigation sampling time as the benchmark, and the kth sampling time represents t k , the data obtained by USBL is aligned to the first inertial navigation moment before the data is received.

[0099] The low-pass filter is calculated as follows:

[0100]

[0101] Among them, y[n] represents the filter output signal at discrete time point n, x[n] represents the input signal at discrete time point n, b is the filter coefficient, which is used to control the characteristics of the filter, and y[n-1] represents the filter output signal at the previous time point n-1.

[0102] 2. The state prediction module predicts the position of the aircraft at the next moment based on the current position of the aircraft and the inertial navigation data, as follows:

[0103]

[0104] in, represents the predicted aircraft position information; Represents the speed information at time t; represents the speed information at time t+1 obtained by recursively calculating the speed information at time t; represents the heading angle at time t; represents the heading angle information at time t+1 obtained by recursively calculating the heading angle information at time t; Represents the position information on the x-axis at time t The position information at time t+1 obtained by recursion; Represents the position information on the y-axis at time t The position information at time t+1 obtained recursively; Δt represents the time difference between time t+1 and time t.

[0105] 3. The state correction module corrects the static error of the position of the aircraft according to the sensor data received by the inertial navigation and USBL, as follows:

[0106] Calculate the predicted distance based on the measured target position and the position of the docking point and azimuth

[0107]

[0108] Among them, P ux ,P uy Represents the fixed position information of the docking point. The predicted range and azimuth are corrected using sampled sensor measurements.

[0109] Use the USBL measurements to correct the predicted range and azimuth:

[0110]

[0111] Among them, k 1,t+1 ,k 2,t+1 ,k 3,t+1 ,k 4,t+1 Represents the Kalman filter gain at time point t+1. L t+1 ,α t+1 ,v t+1 ,θ t+1 Represents the sensor measurement value at time t+1.

[0112] Correct the current position using the corrected distance and azimuth:

[0113]

[0114] This application fuses ultra-short baseline positioning information with inertial navigation information, and uses the extended Kalman filter algorithm to solve the problem of inertial navigation errors accumulated over time and ultra-short baseline data lag, and improves the stability and real-time performance of positioning. The prediction equation of the Kalman filter can be obtained from the heading projection equation, as shown in formula (6). The update equation of the Kalman filter is shown in formulas (7) and (8). Finally, the corrected distance and azimuth are used to correct the current position, as shown in formula (9).

[0115] Extended Kalman filters are widely used in nonlinear state estimation to obtain a single accurate position information by combining the measurements of inertial navigation and ultra-short baseline positioning systems. The position of the underwater vehicle during docking is recursively estimated by performing state estimation and correction updates. However, extended Kalman filters also have certain limitations. First, the filter gains calculated using inaccurate noise covariance in the extended Kalman filter need to be adjusted in advance. Second, the noise covariance based on noisy measurements may cause position deviations during docking due to sudden changes in time.

[0116] In order to solve these problems, this application proposes a reinforcement learning method to correct the extended Kalman filter gain to obtain an accurate extended Kalman filter and obtain more accurate estimates during the docking process. The soft actor-critic (SAC) algorithm is used in reinforcement learning to train the gain of the position estimate used to compensate the extended Kalman filter.

[0117] The workflow of the extended Kalman filter with reinforcement learning compensation in this application is as follows Figure 3 As shown. First, inertial navigation and USBL sensors are used to obtain measurement values, and the extended Kalman filter uses the obtained measurement values ​​to predict the position. Since the performance of the filter depends largely on the filter gain. Using different filter gains, the estimated performance of the filter may vary significantly. Therefore, this application uses a reinforcement learning model and the residual of the sensor measurement value and the extended Kalman filter estimate value to train the extended Kalman filter gain to obtain a corrected Kalman filter gain, and then uses the trained filter gain to update the original filter gain and perform the extended Kalman filter again, repeating the extended Kalman prediction process and update process just now to obtain the final estimated value. Among them, the Kalman filter gain is trained using sensor measurement and estimation data, and the optimal reinforcement learning strategy is used to obtain the optimal filter gain to correct the sensor measurement and update the estimation information. In the reinforcement learning training process of the Kalman gain, the goal of this application is to use a deep neural network to learn and train strategies from the estimated error data that changes over time to obtain the optimal filter gain.

[0118] This application designs the strategy network and evaluation network required for reinforcement learning based on the above-mentioned aircraft kinematic model and extended Kalman filter.

[0119] 4. Reinforcement learning compensation module, used to solve an extended Kalman filter gain K that can optimize the performance index t (Used in the state correction module.) It specifically includes: action network unit, reward and punishment unit, evaluation network unit and experience replay pool.

[0120] An action network unit, which receives the state as input and is used to calculate the extended Kalman gain value of the underwater vehicle after compensation using the reinforcement learning compensation module in the current state;

[0121] The reward and penalty unit is used to calculate the current instantaneous performance index of the underwater vehicle according to the compensated extended Kalman gain value calculated by the current state and action network unit.

[0122] The evaluation network unit is trained by the historical experience data in the experience replay pool and is used to evaluate the quality of the underwater vehicle's actions in the current state. Its essence is to predict the comprehensive position error of the underwater vehicle in the future under the current state and current action conditions, and guide the action network unit to adjust the parameters used in the calculation of the compensation gain according to the evaluation results.

[0123] The experience replay pool is used to periodically sample the current position information of the underwater vehicle, the Kalman gain compensation calculated by the action network unit, and the performance indicators obtained by the reward and penalty unit to form historical experience data.

[0124] Both the action network unit and the evaluation network unit adopt the form of deep neural network, and the specific structure is as follows:

[0125] The action network unit is implemented using a fully connected neural network structure, and the input layer is the current state Its physical meaning is the estimated variable in formula (7); its hidden layer consists of two layers of networks, each layer contains 64 neurons, and the output layer is a single neuron. The relu activation function is used between the input layer and the hidden layer, and between the hidden layer and the hidden layer, and the tanh activation function is used between the output layer and the hidden layer.

[0126] The evaluation network unit is implemented using a fully connected neural network structure, whose input layer consists of the current state and the action K output by the action network t Spliced ​​together, It is obtained by subtracting the sensor measurement value from the extended Kalman estimator estimate; its hidden layer consists of two layers of networks, each layer contains 64 neurons, and the output layer is a single neuron. The relu activation function is used between the input layer and the hidden layer, and between the hidden layer and the hidden layer, and the linear activation function is used between the output layer and the hidden layer.

[0127] In the reinforcement learning compensation module, the estimated residual Represented as the sensor measurement value x t With the estimated observations The difference between in express The estimated variable in . The filter gain K t is through K is calculated by taking partial derivatives from the Kalman prediction and update equations. t It means [k 1,t ,k 2,t ,k 3,t ,k 4,t ] is the gain of the corresponding estimated variable in . The estimated residuals and gains can be expressed as a Markov decision process, where the randomness mainly comes from the sensor noise:

[0128]

[0129] Among them, the estimated residual is the state, the filter gain K t ∈A is the action sampled from the training policy, Represents the estimated transition probability. This application designs the estimated residual in the state space as a strategy The corrected estimated residual is obtained by training the strategy. Therefore, the position estimation problem can be formulated as a reinforcement learning problem, that is, learning a strategy π to train the filter gain K t to control the estimated residuals.

[0130] The performance index of the reinforcement learning compensation module is expressed as follows: The one-step reward function is used to measure the quality of the state-action pair in each training process, that is, (r represents the reward value for measuring the state-action pair, that is, the quality of the state-action pair after each training), where x t Represents the sensor measurement value in the state correction module, represents the estimated variable in formula (7). The total reward is the sum of the one-step reward function:

[0131]

[0132] The discount factor γ∈[0,1). The value function of the reinforcement learning compensator is defined as follows:

[0133]

[0134] in, is the transition probability of the estimated residual; φ∈[0,1) is a constant discount factor; Indicates status Next select K t The above indicators mean the long-term return of taking actions in the current state.

[0135] In reinforcement learning, the reward function is designed as described in formula (10), and the goal of reinforcement learning is to find the optimal strategy that minimizes the expected cumulative cost, and the cost measures the quality of the strategy as the value function. The value function is described in formula (11).

[0136] After designing the extended Kalman filter estimator and reinforcement learning reward function, we can design the deep reinforcement learning algorithm.

[0137] This application uses the Soft Actor Critic (SAC) algorithm in offline policy reinforcement learning. SAC uses a random strategy, which has certain advantages over deterministic strategies. In SAC, an entropy is added as a regularization term of the Q function to adjust the performance. The expression of the Q function is as follows:

[0138]

[0139] in, represents the entropy of the strategy; R t is the total reward value; In the actor-critic algorithm, a deep neural network (DNN) is used to approximate the "critic" and "actor", and β and φ are used to parameterize and Respectively and The DNN is composed of fully connected multilayer perceptrons with Relu function as activation function, and tanh function is used in input and output layers. Figure 4 and Figure 5 shown.

[0140] The reinforcement learning training process of the extended Kalman filter is as follows Figure 1As shown in the figure. During the training process, the extended Kalman filter continuously performs position estimation to collect data, and then stores the data in the experience replay area. After collecting a certain amount of data, a batch of data is randomly sampled from the experience replay area for policy evaluation and policy improvement. The improved data is applied to the state estimator to generate estimated data. When the Q function in the critic network converges, it means that the improved policy is reliable, that is, accurate filter gain and estimated residual are obtained.

[0141] The reinforcement learning compensation module uses an offline reinforcement learning algorithm (off-policy) to sample data in the experience pool during the training phase, and can use offline generated data to train the action network unit and the evaluation network unit. This allows the agent to better learn from historical data and improve strategies, thereby improving performance in reinforcement learning tasks.

[0142] The training process of the reinforcement learning compensation module includes the following steps:

[0143] (1) Initialize the neural network;

[0144] (2) The sensor obtains the current time information and obtains the estimated position information through the extended Kalman filter;

[0145] (3) The action network unit calculates the compensation gain based on the sensor information and the estimated information;

[0146] (4) the reward and penalty unit calculates the performance index based on the sensor information, the extended Kalman estimation information and the compensation gain;

[0147] (5) The current estimated position information, compensation gain and performance index of the underwater vehicle are stored in the experience playback pool to form historical experience data;

[0148] (6) The evaluation network unit evaluates the underwater vehicle's actions under the guidance of the experience pool;

[0149] (7) The evaluation network unit provides parameter guidance and adjustment to the action network unit;

[0150] (8) Update the neural network weights and estimated position information and proceed to the next training.

[0151] Example 2

[0152] The present application also provides a method for estimating the position of an underwater vehicle during docking based on reinforcement learning, which is implemented based on the above system and includes:

[0153] Step S1: The sensor preprocessing module preprocesses the measurement values ​​obtained by the inertial navigation and USBL sensors and removes noise through a low-pass filter;

[0154] Step S2: The state prediction module predicts the position of the underwater vehicle at the next moment based on the current position of the underwater vehicle and the inertial navigation data;

[0155] Step S3: The reinforcement learning compensation module solves the extended Kalman filter gain and outputs it to the state correction module;

[0156] Step S4: the state correction module updates the original filter gain using the extended Kalman filter gain;

[0157] Step S5: Repeat steps S2 to S4 to obtain the final estimated position of the underwater vehicle.

[0158] This application uses the Python-pytorch framework to simulate the algorithm. In the simulation, the sensor error parameters are set as follows:

[0159] Since the actual docking position data of the autonomous underwater robot is temporarily unavailable, a reference model is established using the above underwater vehicle kinematic model. This simulation experiment simulates the return docking process of the underwater vehicle on a two-dimensional plane, and improves the feasibility of the estimation performance by using verification reinforcement learning to train the Kalman filter gain at the algorithm level. In the experiment, the initial position of the underwater vehicle is set to [0,0], the fixed docking position is set to [410,17], the initial speed is set to 4m / s, and the initial heading angle is set to 27 degrees. The output frequency of USBL is 1Hz. The noise sampling distribution in the model is as follows: ω1~N(0,0.5), ω2~N(0,0.5), ω3~N(0,0.1), ω4~N(0,0.0025), ω5~N(0,5), ω6~N(0,0.002).

[0160] To ensure that the network structure is simple, easy to implement and train, the actor and critic neural network uses a fully connected network with a Relu function as the activation function, and uses the tanh function as the activation function of the input and output layers. The training process uses the Adam optimizer. 25 strategies were trained independently, and the strategy with the lowest verification error was selected. In the extended Kalman filter algorithm improved by reinforcement learning, the SAC algorithm is used to train the reinforcement learning strategy, and its hyperparameter settings are shown in Table 1:

[0161] Table 1 Hyperparameter settings

[0162]

[0163] Figure 6 and Figure 7The algorithm performance of the extended Kalman filter improved by reinforcement learning in integrated navigation is demonstrated. It can be seen that the extended Kalman filter algorithm based on reinforcement learning can quickly enter a stable state, and the maximum position error is smaller than that of the extended Kalman filter. The docking trajectory of the extended Kalman filter based on reinforcement learning is closer to the actual trajectory. The maximum position errors of the extended Kalman filter in the east and north directions are 10.4 meters and 14.2 meters respectively, while the maximum position errors of the extended Kalman filter based on reinforcement learning in these two directions are 2.1 meters and 8.3 meters respectively. It can be seen that the position accuracy in the east and north directions has been improved to a certain extent. The reward value curve obtained after 250 rounds of training using the reinforcement learning model is shown in the figure below. Figure 8 As shown in the figure. By analyzing the reward value curve, it can be concluded that the training process of the model converges quickly and reaches stability in about 60 rounds.

[0164] The present application may also provide a computer device, comprising: at least one processor, a memory, at least one network interface and a user interface. The various components in the device are coupled together through a bus system. It is understood that the bus system is used to achieve connection and communication between these components. In addition to the data bus, the bus system also includes a power bus, a control bus and a status signal bus.

[0165] The user interface may include a display, a keyboard or a pointing device, such as a mouse, a trackball, a touch pad or a touch screen.

[0166] It is understood that the memory in the embodiments disclosed in the present application may be a volatile memory or a non-volatile memory, or may include both volatile and non-volatile memories. Among them, the non-volatile memory may be a read-only memory (ROM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), an electrically erasable programmable read-only memory (EEPROM), or a flash memory. The volatile memory may be a random access memory (RAM), which is used as an external cache. By way of example and not limitation, many forms of RAM are available, such as static random access memory (SRAM), dynamic random access memory (DRAM), synchronous dynamic random access memory (SDRAM), double data rate synchronous dynamic random access memory (DDRSDRAM), enhanced synchronous dynamic random access memory (ESDRAM), synchronous link dynamic random access memory (SLDRAM), and direct memory bus random access memory (DRRAM). The memories described herein are intended to include, but are not limited to, these and any other suitable types of memories.

[0167] In some embodiments, the memory stores the following elements, executable modules or data structures, or a subset thereof, or an extended set thereof: an operating system and applications.

[0168] The operating system includes various system programs, such as a framework layer, a core library layer, a driver layer, etc., which are used to implement various basic services and process hardware-based tasks. The application includes various application programs, such as a media player (Media Player), a browser (Browser), etc., which are used to implement various application services. The program for implementing the method of the embodiment of the present disclosure can be included in the application.

[0169] In the above embodiment, the processor may also call a program or instruction stored in the memory, specifically, a program or instruction stored in an application program, and is used to:

[0170] Execute the steps of the above method.

[0171] The above method can be applied to a processor or implemented by a processor. The processor may be an integrated circuit chip with signal processing capabilities. In the implementation process, each step of the above method can be completed by an integrated logic circuit of hardware in the processor or an instruction in the form of software. The above processor may be a general processor, a digital signal processor (Digital Signal Processor, DSP), an application specific integrated circuit (Application Specific Integrated Circuit, ASIC), a field programmable gate array (Field Programmable Gate Array, FPGA) or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components. The above-disclosed methods, steps and logic block diagrams can be implemented or executed. The general processor may be a microprocessor or the processor may also be any conventional processor, etc. The steps of the above-disclosed method can be directly embodied as a hardware decoding processor to execute, or the hardware and software modules in the decoding processor are combined to execute. The software module can be located in a mature storage medium in the field such as a random access memory, a flash memory, a read-only memory, a programmable read-only memory or an electrically erasable programmable memory, a register, etc. The storage medium is located in the memory, and the processor reads the information in the memory and completes the steps of the above method in combination with its hardware.

[0172] It is understood that the embodiments described in the present application can be implemented by hardware, software, firmware, middleware, microcode or a combination thereof. For hardware implementation, the processing unit can be implemented in one or more application specific integrated circuits (ASIC), digital signal processors (DSP), digital signal processing devices (DSPD), programmable logic devices (PLD), field programmable gate arrays (FPGA), general-purpose processors, controllers, microcontrollers, microprocessors, other electronic units for performing the functions described in the present application or a combination thereof.

[0173] For software implementation, the technology of the present application can be implemented by executing the functional modules (such as procedures, functions, etc.) of the present application. The software code can be stored in a memory and executed by a processor. The memory can be implemented in the processor or outside the processor.

[0174] The present application may also provide a non-volatile storage medium for storing a computer program. When the computer program is executed by a processor, each step in the above method embodiment can be implemented.

[0175] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present application and are not intended to limit it. Although the present application is described in detail with reference to the embodiments, a person skilled in the art should understand that any modification or equivalent replacement of the technical solution of the present application does not depart from the spirit and scope of the technical solution of the present application and should be included in the scope of the claims of the present application.

Claims

1. A position estimation system for underwater vehicle docking process based on reinforcement learning, characterized in that: The system includes a sensor preprocessing module, a state prediction module, a state correction module and a reinforcement learning compensation module; wherein, The sensor preprocessing module is used to align the raw data of the sensor at a certain time and filter out noise through a low-pass filter; The state prediction module is used to predict the position of the underwater vehicle at the next moment based on the position of the underwater vehicle at the current moment in combination with the inertial navigation data; The state correction module is used to correct the static error of the position of the underwater vehicle based on the sensor data received by the inertial navigation and USBL; The reinforcement learning compensation module is built based on a deep neural network and is used to solve the extended Kalman filter gain that can optimize the performance index and output it to the state correction module.

2. The underwater vehicle docking process position estimation system based on reinforcement learning according to claim 1 is characterized in that: The sensors involved in the system include inertial navigation and USBL; among them, The information of inertial navigation measurement is expressed as: Among them, σ1, σ2 are the inertial navigation measurement noise; v t is the speed of the underwater vehicle at time t; θ t is the heading angle of the underwater vehicle at time t; The information measured by USBL is expressed as: Among them, L t represents the straight-line distance from the underwater vehicle position to the target point measured by USBL at time t; α t represents the angle between the straight-line distance measured by USBL at time t and the horizontal direction; σ3 and σ4 are the inertial navigation measurement noise; The sensor preprocessing module uses the inertial navigation sampling time as a reference to align the data obtained by USBL to the first inertial navigation time before the data is received; The low-pass filter is calculated as follows: y[n]=bx[n]+(1-b)y[n-1] Among them, y[n] represents the filter output signal at discrete time point n; x[n] represents the input signal at discrete time point n; b represents the filter coefficient, which is used to control the characteristics of the filter; y[n-1] represents the filter output signal at the previous time point n-1.

3. The underwater vehicle docking process position estimation system based on reinforcement learning according to claim 1, characterized in that: The state prediction module predicts the position of the aircraft at the next moment in the following manner: in, represents the speed information at time t+1 obtained by recursively calculating the speed information at time t; represents the heading angle information at time t+1 obtained by recursively calculating the heading angle information at time t; Represents the position information on the x-axis at time t The position information at time t+1 obtained by recursion; Represents the position information on the y-axis at time t The position information at time t+1 obtained recursively; Δt represents the time difference between time t+1 and time t.

4. The underwater vehicle docking process position estimation system based on reinforcement learning according to claim 3 is characterized in that: The method for the state correction module to correct the static error of the position of the aircraft is as follows: Based on the measured target position and the position of the docking point, calculate the predicted distance at time t+1 and azimuth Among them, P ux ,P uy Represents the fixed position information of the docking point, using sampled sensor measurements to correct the predicted range and azimuth; Use USBL measurements to correct the predicted distance at time t+1 Azimuth speed and heading angle Among them, k 1,t+1 ,k 2,t+1 ,k 3,t+1 ,k 4,t+1 represents the Kalman filter gain at time t+1; L t+1 ,α t+1 ,v t+1 ,θ t+1 Represents the distance, azimuth, speed and heading angle measured by the sensor at time t+1; Correct the current position using the corrected distance and azimuth and 5. The underwater vehicle docking process position estimation system based on reinforcement learning according to claim 1, characterized in that: The reinforcement learning compensation module includes: an action network unit, a reward and punishment unit, an evaluation network unit and an experience replay pool; wherein, The action network unit is composed of a deep neural network, receives a state as an input, and is used to calculate the extended Kalman gain value of the underwater vehicle after compensation by the reinforcement learning compensation module in the current state; The reward and penalty unit is used to calculate the current instantaneous performance index of the underwater vehicle according to the current state and the compensated extended Kalman gain value calculated by the action network unit; The evaluation network unit is composed of a deep neural network, which is trained by the historical experience data in the experience playback pool, and is used to evaluate the quality of the underwater vehicle's action in the current state, predict the comprehensive position error of the underwater vehicle in the future under the current state and current action conditions, and guide the action network unit to adjust the parameters used in the process of calculating the compensation gain according to the evaluation results; The experience replay pool is used to periodically sample the current position information of the underwater vehicle, the Kalman gain compensation calculated by the action network unit, and the performance index obtained by the reward and penalty unit to form historical experience data.

6. The underwater vehicle docking process position estimation system based on reinforcement learning according to claim 5 is characterized in that: The training process of the reinforcement learning compensation module includes: Step 1: Initialize the deep neural network; Step 2: The sensor obtains the current time information and obtains the estimated position information through the extended Kalman filter; Step 3: The action network unit calculates the compensation gain based on the sensor information and the estimated position information; Step 4: The reward and penalty unit calculates the performance index based on the sensor information, the extended Kalman estimated position information and the compensation gain; Step 5: The experience playback pool collects the current estimated position information, compensation gain and performance indicators of the underwater vehicle to form historical experience data; Step 6: The evaluation network unit evaluates the underwater vehicle's actions under the guidance of the experience replay pool; Step 7: The evaluation network unit guides and adjusts the parameters of the action network unit; Step 8: Update the deep neural network weights and estimated position information for the next training.

7. The underwater vehicle docking process position estimation system based on reinforcement learning according to claim 5, characterized in that: The one-step reward function of the reinforcement learning compensation module is used to measure the quality of the state-action pair during each training process, namely: Among them, r represents the reward value of the state-action pair, that is, the quality of the state-action pair after each training; represents the estimated residual at time t; x t represents the sensor measurement value at time t; represents the predicted value at time t; ||·|| 2 represents the 2-norm of the matrix; Total Return R t is the sum of the reward function for one step: Among them, γ∈[0,1) represents the discount factor; Value function of the reinforcement learning compensation module The definition is as follows: Among them, P t+1|t is the estimated residual transition probability; Indicates status Next select K t Strategy; K t represents the gain of the predicted value; δ∈[0,1) represents the constant discount factor.

8. The underwater vehicle docking process position estimation system based on reinforcement learning according to claim 5, characterized in that: The reinforcement learning compensation module uses an offline reinforcement learning algorithm to sample data in the experience replay pool during the training phase, and uses the offline generated data to train the action network unit and the evaluation network unit.

9. The underwater vehicle docking process position estimation system based on reinforcement learning according to claim 5, characterized in that: The action network unit is implemented using a fully connected layer neural network structure, and the input layer is the current state The hidden layer consists of two layers of networks, each layer contains 64 neurons, and the output layer is a single neuron; the Relu activation function is used between the input layer and the hidden layer, and between the hidden layer and the hidden layer, and the tanh activation function is used between the output layer and the hidden layer; The evaluation network unit is implemented using a fully connected neural network structure, and the input layer consists of the current state The hidden layer is composed of two layers of networks, each layer contains 64 neurons, and the output layer is a single neuron. The Relu activation function is used between the input layer and the hidden layer, and between the hidden layer and the hidden layer, and the linear activation function is used between the output layer and the hidden layer.

10. A method for estimating the position of an underwater vehicle during docking based on reinforcement learning, implemented based on the system of any one of claims 1 to 9, the method comprising: Step S1: The sensor preprocessing module preprocesses the measurement values ​​obtained by the inertial navigation and USBL sensors and removes noise through a low-pass filter; Step S2: The state prediction module predicts the position of the underwater vehicle at the next moment based on the current position of the underwater vehicle and the inertial navigation data; Step S3: The reinforcement learning compensation module solves the extended Kalman filter gain and outputs it to the state correction module; Step S4: the state correction module updates the original filter gain using the extended Kalman filter gain; Step S5: Repeat steps S2 to S4 to obtain the final estimated position of the underwater vehicle.

Images