Multi-AUV (Autonomous Underwater Vehicle) cooperative positioning optimization method and device based on time sequence factor network, and computer medium

Abstract

The invention discloses a multi-AUV (Autonomous Underwater Vehicle) cooperative positioning optimization method and device based on a time sequence factor network, and a computer medium. The method comprises the following steps: constructing a multi-AUV factor network model; initializing a node corresponding to initial time in the multi-AUV factor network model; according to an observation information source of the AUV, updating the multi-AUV factor network model; according to a preset time sequence window range, performing forward and backward message transmission updating in the updated multi-AUV factor network model in the current time sequence window; and performing weighted fusion on estimation results of forward and backward message passing in the updated multi-AUV factor network model in the current time sequence window to obtain estimation of the navigation state of each AUV at the current moment. According to the method, the real-time navigation positioning estimation precision of the multi-AUV collaborative navigation system is effectively improved, the effect is obvious when the sensor fails for a short time, and the complex and changeable underwater environment and task requirements of an AUV cluster can be effectively met.

Description

Technical Field

[0001] The present invention relates to the field of navigation and positioning technology, and more specifically, to a multi-AUV collaborative positioning optimization method, device and computer medium based on a temporal factor network. Background Art

[0002] In recent years, with the breakthrough of single AUV (Autonomous Underwater Vehicle, AUV) navigation technology and the wide application of cooperative navigation technology in the fields of multi-robot positioning on land and in the air, wireless sensor network positioning, satellite positioning, etc., multi-AUV cooperative navigation technology based on underwater acoustic communication network has emerged and become an important research direction in the field of underwater robots. However, the multi-AUV cooperative navigation system based on inertial base is a typical nonlinear system, accompanied by strong coupling and uncertainty. The measurement mechanism, seawater medium and working environment will also bring about the characteristics of heterogeneity, regionality and stage, low update frequency, non-complete dependence that is easily disturbed, delay uncertainty and accuracy variation of various navigation information sources. The above characteristics require that the multi-source information fusion filter has a plug-and-play open structure and the flexibility and scalability of real-time reorganization. In view of the problem of fusion of heterogeneous and heterogeneous information sources, the industry generally adopts the factor graph model framework to realize the plug-and-play and system reconstruction of multiple heterogeneous and heterogeneous information sources.

[0003] However, due to the complex underwater working environment and cost constraints of the AUV cluster, the amount of data available for underwater real-time navigation and positioning is small, the information quality is poor, the solution time is limited, and the short-term failure of the available auxiliary sensor equipment / system will restrict the improvement of real-time navigation accuracy. In response to the problem of real-time navigation and positioning, the industry generally adopts the mobile time window technology, adds the latest observation data to the real-time navigation solution, and uses the sliding time window to determine a reasonable time span, that is, intercepts the historical observation information source involved in the navigation solution and the start and end time of the estimated object, and optimally smoothes the real-time fusion result to obtain a more accurate real-time navigation result. In addition, the fused error estimate and the stored data in the window can be used to catch up and correct the SINS navigation error, while improving the navigation solution accuracy and occupying the least computer resources. The sliding window is usually combined with the Kalman filter algorithm and its various improved methods to achieve the purpose of optimal smoothing of the navigation state in the window. This type of method has been proven and partially applied in single-AUV integrated navigation systems, but when expanded to multi-AUV parallel collaborative navigation systems, it is very likely to have problems such as insufficient adaptability and state correlation, which limits its widespread application in multi-AUV collaborative positioning. Summary of the invention

[0004] One of the purposes of the present invention is to provide a multi-AUV collaborative positioning optimization method based on a timing factor network to solve the technical problems of low real-time navigation accuracy and poor stability of multi-AUV parallel collaborative navigation systems in the prior art; the second purpose of the present invention is to provide a multi-AUV collaborative positioning optimization device based on a timing factor network; the third purpose of the present invention is to provide a computer medium.

[0005] In order to solve the above technical problems, the technical solution of the present invention is as follows:

[0006] A first aspect of the present invention provides a multi-AUV collaborative positioning optimization method based on a temporal factor network, comprising the following steps:

[0007] Constructing a multi-AUV factor network model, wherein the multi-AUV factor network model includes nodes abstracted from the navigation information of each AUV and the relative observation information between the AUVs, which are connected in chronological order;

[0008] Initialize and set the nodes corresponding to the initial time in the multi-AUV factor network model;

[0009] According to the observation information source of the AUV, updating the multi-AUV factor network model;

[0010] According to a preset timing window range, forward and backward message passing updates are performed in the updated multi-AUV factor network model within the current timing window;

[0011] The estimation results of the nodes in the updated multi-AUV factor network model through forward and backward message passing within the current time window are weightedly fused to obtain the estimation of the navigation state of each AUV at the current moment.

[0012] Furthermore, each AUV is equipped with:

[0013] Autonomous navigation sensors, including strapdown inertial navigation system SINS, Doppler velocimeter DVL, magnetic compass MCP, terrain matching positioning system TAN and pressure depth gauge DM, are used to obtain the AUV's own navigation information;

[0014] Hydroacoustic detection equipment, used to obtain relative observation information between AUVs, including relative distance and relative orientation;

[0015] Hydroacoustic communication equipment, used to exchange information with other AUVs.

[0016] Furthermore, the multi-AUV factor network model includes:

[0017] The AUV’s own navigation information obtained by the strapdown inertial navigation system SINS, Doppler velocimeter DVL, magnetic compass MCP, terrain matching positioning system TAN and pressure depth gauge DM corresponds to the factor nodes f IMU 、f DVL 、f MCP 、f TAN and f DM ;

[0018] The relative distance, relative azimuth, and relative distance + relative azimuth measurement signals obtained by the underwater acoustic detection equipment correspond to the factor nodes f rela_dis 、f rela_Ang and f rela_Dis+Ang ;

[0019] The attitude error, velocity error, position error, gyroscope and accelerometer constant deviation of a single AUV are taken as state quantities, corresponding to the variable node of the AUV, the variable node of the AUV at each moment and the factor node f DVL 、f MCP 、f TAN and f DM The variable nodes corresponding to the same AUV at different times are connected in chronological order through the factor nodes corresponding to the current time f IMU and the deviation factor node f Bias Connection, the variable nodes corresponding to different AUVs at the same time are connected through the corresponding factor nodes f rela_dis 、f rela_Ang and f rela_Dis+Ang connect.

[0020] Further, the nodes corresponding to the initial time in the multi-AUV factor network model are initialized, including the variable nodes corresponding to the initial time and the prior factor nodes f prior , where, for Gaussian distribution, the prior factor node f prior With mean μ υ and variance∑ υ The way given is:

[0021]

[0022] Where d(·) represents the corresponding cost function; is the squared Mahalanobis distance, Σ is the noise covariance matrix;

[0023] Further, according to the observation information source of the AUV, the multi-AUV factor network model is updated, including:

[0024] If in t k The IMU measurement information of the strapdown inertial navigation system SINS is received at all times. and the current variable node x k To predict the variable node at the next moment Updated navigation solutions for each AUV platform:

[0025]

[0026] Where h(·) represents the IMU state transfer function;

[0027] If in t g Always receive other measurement information besides IMU measurement information, define and add new variable nodes and factor nodes f IMU , Deviation factor node f Bias and observation information factor nodes.

[0028] Further, according to the preset timing window range, before performing the forward and backward message transmission update in the updated multi-AUV factor network model in the current timing window, it also includes:

[0029] If the new observation information exceeds the preset time series window range, it slides forward and generates a new window, and at the same time, the navigation result estimated in the previous window is used as the prior estimate of the current window.

[0030] Further, according to a preset timing window range, the sum-product algorithm is used in the updated multi-AUV factor network model within the current timing window to perform forward and backward message passing updates.

[0031] Furthermore, the estimation results of the nodes in the updated multi-AUV factor network model through forward and backward message passing within the current time series window are weighted and fused to obtain the estimation of the navigation state of each AUV at the current moment:

[0032]

[0033] In the formula, a Gaussian distribution is described by the mean vector m and the variance matrix V. If the mean vector in the forward message of a variable node x is expressed as The variance matrix is ​​expressed as The mean vector in the backward message is expressed as The variance matrix is ​​expressed as

[0034] The second aspect of the present invention provides a multi-AUV collaborative positioning optimization device based on a temporal factor network, comprising:

[0035] A construction module, wherein the construction module is used to construct a multi-AUV factor network model, wherein the multi-AUV factor network model includes nodes abstracted from the navigation information of each AUV and the relative observation information between the AUVs, which are connected in chronological order;

[0036] An initialization module, the initialization module is used to initialize and set the nodes corresponding to the initial time in the multi-AUV factor network model;

[0037] A first updating module, wherein the first updating module updates the multi-AUV factor network model according to the observation information source of the AUV;

[0038] A second updating module, wherein the second updating module performs forward and backward message passing updates in the updated multi-AUV factor network model within the current timing window according to a preset timing window range;

[0039] The estimation module performs weighted fusion on the estimation results of the nodes by forward and backward message passing in the updated multi-AUV factor network model within the current time window to obtain an estimation of the navigation state of each AUV at the current moment.

[0040] The third aspect of the present invention provides a computer medium, characterized in that a computer program is stored on the computer medium, and when the computer program is executed by a processor, the multi-AUV collaborative positioning optimization method based on a temporal factor network is implemented.

[0041] Compared with the prior art, the technical solution of the present invention has the following beneficial effects:

[0042] Aiming at the high-precision real-time navigation requirements of multi-AUV collaborative navigation and positioning, the present invention proposes a multi-AUV collaborative positioning optimization method based on a temporal factor network. Compared with the conventional factor graph real-time navigation and positioning method and the existing sliding window distributed extended Kalman filter method, the method proposed in the present invention can effectively improve the real-time navigation and positioning estimation accuracy of the multi-AUV collaborative navigation system, and the effect is obvious when the sensor fails for a short time, and can effectively cope with the complex and changeable underwater environment and mission requirements of the AUV cluster. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] Figure 1 A schematic diagram of a flow chart of a multi-AUV collaborative positioning optimization method based on a temporal factor network provided in an embodiment of the present invention;

[0044] Figure 2 A schematic diagram of the structure of a multi-AUV factor network model provided by an embodiment of the present invention;

[0045] Figure 3A schematic diagram of bidirectional message transmission of a multi-AUV factor network model provided in an embodiment of the present invention;

[0046] Figure 4 A position error comparison curve diagram of the AUV1 provided in an embodiment of the present invention;

[0047] Figure 5 A position error comparison curve diagram of the AUV2 provided in an embodiment of the present invention;

[0048] Figure 6 A position error comparison curve diagram of the AUV3 provided in an embodiment of the present invention; DETAILED DESCRIPTION

[0049] The drawings are for illustrative purposes only and should not be construed as limiting the present patent;

[0050] In order to better illustrate the present embodiment, some parts in the drawings may be omitted, enlarged or reduced, and do not represent the size of the actual product;

[0051] It is understandable to those skilled in the art that some well-known structures and their descriptions may be omitted in the drawings.

[0052] The technical solution of the present invention is further described below in conjunction with the accompanying drawings and embodiments.

[0053] Example 1

[0054] This embodiment provides a multi-AUV collaborative positioning optimization method based on a temporal factor network. Figure 1 As shown, the following steps are included:

[0055] Constructing a multi-AUV factor network model, wherein the multi-AUV factor network model includes nodes abstracted from the navigation information of each AUV and the relative observation information between the AUVs, which are connected in chronological order;

[0056] Initialize and set the nodes corresponding to the initial time in the multi-AUV factor network model;

[0057] According to the observation information source of the AUV, updating the multi-AUV factor network model;

[0058] According to a preset timing window range, forward and backward message passing updates are performed in the updated multi-AUV factor network model within the current timing window;

[0059] The estimation results of the nodes in the updated multi-AUV factor network model through forward and backward message passing within the current time window are weightedly fused to obtain an estimate of the navigation state of each AUV at the current moment.

[0060] In a further embodiment, each AUV is equipped with:

[0061] Autonomous navigation sensors, including strapdown inertial navigation system SINS, Doppler velocimeter DVL, magnetic compass MCP, terrain matching positioning system TAN and pressure depth gauge DM, are used to obtain the AUV's own navigation information;

[0062] Hydroacoustic detection equipment, used to obtain relative observation information between AUVs, including relative distance and relative orientation;

[0063] Hydroacoustic communication equipment, used to exchange information with other AUVs.

[0064] The present invention uses the factor network model framework to fuse the AUV's own navigation information and the relative observation information between AUVs. Each AUV senses its own motion state through internal sensors according to its motion model, and deduce its position estimation and corresponding covariance matrix (self-navigation state information) under the factor network model framework; when the AUV senses the surrounding AUVs and communicates with them, the relative observation information between them is obtained through communication and detection equipment, and all available relative observation information is split into each AUV platform node, and the observation information of each AUV itself and the relative observation information between AUVs are fused through the message passing algorithm, so as to predict and update the navigation state of each AUV in the AUV cluster.

[0065] In a further embodiment, the multi-AUV factor network model, such as Figure 2 As shown, including:

[0066] The AUV’s own navigation information obtained by the strapdown inertial navigation system SINS, Doppler velocimeter DVL, magnetic compass MCP, terrain matching positioning system TAN and pressure depth gauge DM corresponds to the factor nodes f IMU 、f DVL 、f MCP 、f TAN and f DM ;

[0067] The relative distance, relative azimuth, and relative distance + relative azimuth measurement signals obtained by the underwater acoustic detection equipment correspond to the factor nodes f rela_dis 、f rela_Ang and f rela_Dis+Ang ;

[0068] The attitude error, velocity error, position error, gyroscope and accelerometer constant deviation of a single AUV are used as state quantities, corresponding to the variable nodes of the AUV, that is:

[0069] x=[(φ n ) T (δvn ) T (δp n ) T (ε b ) T (▽ b ) T ] T

[0070] The variable node and factor node f of the AUV at each moment DVL 、f MCP 、f TAN and f DM The variable nodes corresponding to the same AUV at different times are connected in chronological order through the factor nodes corresponding to the current time f IMU and the deviation factor node f Bias Connection, the variable nodes corresponding to different AUVs at the same time are connected through the corresponding factor nodes f rela_dis 、f rela_Ang and f rela_Dis+Ang connect.

[0071] In a further embodiment, the nodes corresponding to the initial time in the multi-AUV factor network model are initialized, including initializing the variable nodes corresponding to the initial time and the prior factor nodes f prior , where, for Gaussian distribution, the prior factor node f prior With mean μ υ and variance Σ υ The way given is:

[0072]

[0073] Where d(·) represents the corresponding cost function; is the squared Mahalanobis distance, Σ is the noise covariance matrix;

[0074] In a further embodiment, the multi-AUV factor network model is updated according to the observation information source of the AUV, including:

[0075] If in t k The IMU measurement information of the strapdown inertial navigation system SINS is received at all times. and the current variable node x k To predict the variable node at the next moment Updated navigation solutions for each AUV platform:

[0076]

[0077] Where h(·) represents the IMU state transfer function;

[0078] If in t g Always receive other measurement information besides IMU measurement information, define and add new variable nodes and factor nodes f IMU , Deviation factor node f Bias and observation information factor nodes;

[0079] Among them, define and add a new IMU factor node f IMU and IMU bias factor node f Bias That is, multiple IMU information and inertial device deviation information between two adjacent self-measurement information and two adjacent relative measurement information are integrated into one factor node:

[0080]

[0081] Where g(·) is the transfer function of the IMU inertial device bias.

[0082] Define and add new observation information factor nodes, such as in t j Receive measurement information from DVL at all times Define and add a new DVL factor node f DVL as follows:

[0083]

[0084] Among them, Σ j is the DVL measurement noise covariance matrix.

[0085] Similarly, when receiving measurement information other than IMU measurement information such as MCP, TAN, DM, etc. at other times, define and add new factor nodes f MCP 、f TAN 、f DM For details, please refer to the representation method of DVL factor node.

[0086] In t k Time AUV i AUV observed j When only their relative distance information is obtained through external sensors, a new relative distance factor node f is defined and added rela_Dis as follows:

[0087]

[0088] Among them, ξ k is the relative distance measurement noise covariance matrix. This factor node connects two corresponding variable nodes and

[0089] In t l Time AUVi AUV observed j When only their relative orientation information is obtained through external sensors, a new relative distance factor node f is defined and added rela_Ang as follows:

[0090]

[0091] Among them, l is the relative distance measurement noise covariance matrix. This factor node connects two corresponding variable nodes and

[0092] In t m Time AUV i AUV observed j When the relative distance and relative orientation information are obtained simultaneously through external sensors, a new relative distance + orientation factor node f is defined and added rela_Dis+Ang as follows:

[0093]

[0094] Among them, m is the relative distance measurement noise covariance matrix. This factor node connects two corresponding variable nodes and

[0095] The above steps are in no particular order, reflecting the plug-and-play flexibility and scalability of the factor network model.

[0096] In a further embodiment, the factor network model is a typical time series model, and the historical state under the framework of the factor network model can also become an estimated object, which has the ability of data smoothing. Therefore, the present invention uses a dynamic time series window to optimize it. The size defined by the time series window boundary is called the window length, and the amplitude of the boundary change is called the sliding step. The determination of the time series window length and the sliding step is generally determined by artificial setting, which simply sets an assumed value or empirical value and lacks a certain theoretical basis. The sliding rules of the time series window include fixed step length, variable step length, fixed window length, and variable window length.

[0097] Smoothing in real-time navigation is essentially the use of historical information to constrain current information. When the motion state changes, the correlation between historical information and current information is small. Generally, a sliding rule with a fixed step length and a variable window length is adopted to ensure that the algorithm can adaptively adjust the output rate of the estimated value according to the dynamic performance of the carrier; for tasks that require real-time response, such as emergency obstacle avoidance or target tracking, a shorter window length and step length should be selected to reduce calculation delays and increase response speed. Therefore, in the application scenario of multi-AUV multi-source fusion positioning of the patent of this invention, the dynamic selection of the timing window can ensure that the algorithm can be adaptively adjusted according to the real-time state and task requirements of the AUV, thereby improving positioning accuracy and algorithm efficiency.

[0098] The time series window length is set to w, the step length is set to d, and the time series window is continuously updated to optimize the calculation of the variable nodes in the box. The range of the time series window is set according to the number of variable nodes, and the time series window only contains part of all variable nodes and factor nodes. Figure 2 This is a schematic diagram of the principle of the multi-AUV collaborative positioning optimization algorithm based on the temporal factor network, taking the i-th AUV platform, window length 3, and step length 1 as an example. The specific analysis is as follows:

[0099] The left edge of the timing window n is t f At time t, the right boundary is j moment, which involves three variable nodes: t f Time, t g Time and t j Variable node at time Nine factor nodes: two f introduced by IMU measurement information IMU Factor node and its corresponding two deviation nodes f Bias , by t f Time, t g Time and t j The self-measurement information at the moment and the relative measurement information introduced by f DVL 、f MCP 、f TAN 、f rela_Dis 、f rela_Ang Factor node; at the next moment, the time series window slides rightward by 1 to become window n+1, and the left boundary is t g , the right boundary is t l , which involves three variable nodes: t g Time, t j Time and t l Variable node at time Seven factor nodes: two f introduced by IMU measurement information IMU Factor node and its corresponding two deviation nodes f Bias , by tg Time, t j Time and t l The self-measurement information at the moment and the relative measurement information introduced by f TAN 、f rela_Dis 、f rela_Ang Factor node; at the next moment, the time series window slides rightward by 1 to become window n+2, and the left boundary is t j , the right boundary is t q , which involves three variable nodes: t j Time, t l Time and t q Variable node at time Five factor nodes: two f introduced by IMU measurement information IMU Factor node and its corresponding two deviation nodes f Bias , by t j The relative position information at the time of the introduction of f rela_Ang Factor node.

[0100] The windowed range in the temporal factor network model is the range in which the message passing algorithm is used to estimate the state variable nodes. The optimal estimation problem for multi-AUV collaborative positioning optimization within the above temporal window can be expressed as follows:

[0101] The optimal estimation of the navigation state of the i-th AUV platform in the time window n is:

[0102]

[0103] The optimal estimation of the navigation state of the i-th AUV platform in the time window n+1:

[0104]

[0105] The optimal estimation of the navigation state of the i-th AUV platform in the time window n+2:

[0106]

[0107] In the factor network model, the scope of state estimation using the message passing algorithm can be the entire factor network model, but the ergodic estimation method makes the algorithm's computational complexity increase continuously during operation, and the early observations do not significantly improve the accuracy of the latest state variable estimation. Therefore, by adding a time window to the optimization scope of the factor network model, that is, limiting the scope of the variable nodes and factor nodes involved in the optimization, the algorithm's computational complexity can be effectively reduced and the efficiency of the algorithm can be improved.

[0108] In a further embodiment, according to a preset timing window range, before performing forward and backward message transmission updates in the updated multi-AUV factor network model within the current timing window, it also includes:

[0109] If the new observation information exceeds the preset time series window range, it slides forward and generates a new window, and at the same time, the navigation result estimated in the previous window is used as the prior estimate of the current window.

[0110] In a further embodiment, according to a preset timing window range, a sum-product algorithm is used in the updated multi-AUV factor network model within the current timing window to perform forward and backward message passing updates.

[0111] In a further embodiment, in the temporal factor network model, considering the real-time and high-precision navigation requirements of multi-AUV collaborative navigation and positioning, the present invention adopts the sum-product algorithm to perform bidirectional message transmission and update in the factor network model to obtain the optimal estimate of the navigation state of each AUV platform.

[0112] The linear discrete state space model of multi-AUV cooperative navigation system can be described as:

[0113]

[0114] In the formula, is the discrete state vector of the i-th AUV at time k; and are AUV at time k i With AUV j Discrete quantity measurement and measurement noise when encountering; AUV from time k-1 to time k i One-step state transfer matrix of ; is the AUV at time k i The discrete system noise input matrix of ; is the AUV at time k i The discrete system noise.

[0115] According to the linear discrete state space equation of the multi-AUV collaborative navigation system, the detailed decomposition of the factor network model and the schematic diagram of bidirectional message transmission are drawn as follows: Figure 3 In the figure, the black solid arrow indicates the forward message propagation direction, and the red dotted arrow indicates the backward message propagation direction.

[0116] Assume that all messages in the factor network model obey the multidimensional Gaussian probability density distribution. Define the time series window interval as [W k-w ,W k ]. Among them, W k-w is the starting data of the timing window, W kis the end data of the timing window. In the initial time period, no solution is performed when the timing window range is not met. When the timing window size is met, the forward message passing and backward message passing of the factor network model are performed within the timing window. By weighted combination of the navigation state estimation results of the two, the smooth optimal estimation of the navigation state of each AUV platform at the current moment is achieved. The solution result is:

[0117]

[0118]

[0119] In the formula, a Gaussian distribution is described by the mean vector m and the variance matrix V. If the mean vector in the forward message of a variable node x is expressed as The variance matrix is ​​expressed as The mean vector in the backward message is expressed as The variance matrix is ​​expressed as

[0120] In order to verify the effectiveness and reliability of the multi-AUV collaborative positioning optimization method based on the timing factor network of the present invention, a numerical simulation experiment was designed on the Matlab software platform taking the network topology composed of three AUVs as an example, and the distributed Kalman filter algorithm (DEKF) based on sliding windows and the factor graph algorithm were introduced as comparison methods. In order to further illustrate the effectiveness of the method proposed in the present invention when the sensor fails, the DVL failure interval of AUV1 is set to 150s~350s, and the TAN failure interval is set to 200s~400s; each navigation device of AUV2 is always valid during the entire simulation time period; the DVL failure interval of AUV3 is 200s~300s, the TAN failure interval is 400s~500s, and the failure interval of the underwater acoustic communication equipment is 850s~900s as follows. The position error comparison curve of AUV1, AUV2, and AUV3 is shown in the figure. Figure 4 , Figure 5 , Figure 6 shown.

[0121] During the entire simulation time period, the multi-AUV collaborative positioning optimization algorithm based on the timing factor network, the distributed extended Kalman filter algorithm based on the sliding window, and the factor graph algorithm proposed in the present invention can maintain a high navigation accuracy, which fully verifies the effectiveness and reliability of the method proposed in the present invention. The multi-AUV collaborative positioning optimization algorithm based on the timing factor network proposed in the present invention has a navigation accuracy comparable to that of the distributed extended Kalman filter algorithm based on the sliding window, which can play a good smoothing role on the overall navigation settlement results, and can greatly reduce the impact of sensor failure on the navigation system. For example, in the 200s to 400s time period of AUV1, the latitude error of the factor graph real-time filtering algorithm reached about 30m, and the longitude error reached about 60m, while the multi-AUV collaborative positioning optimization algorithm based on the timing factor network and the distributed extended Kalman filter algorithm based on the sliding window had significantly improved positioning errors in this time period. Moreover, the multi-AUV collaborative positioning optimization algorithm based on the timing factor network proposed in the present invention has high flexibility, scalability and low algorithm complexity, and therefore has strong feasibility and effectiveness.

[0122] Example 2

[0123] Based on Example 1, this embodiment provides a multi-AUV collaborative positioning optimization device based on a temporal factor network, including:

[0124] A construction module, wherein the construction module is used to construct a multi-AUV factor network model, wherein the multi-AUV factor network model includes nodes abstracted from the navigation information of each AUV and the relative observation information between the AUVs, which are connected in chronological order;

[0125] An initialization module, the initialization module is used to initialize and set the nodes corresponding to the initial time in the multi-AUV factor network model;

[0126] A first updating module, wherein the first updating module updates the multi-AUV factor network model according to the observation information source of the AUV;

[0127] A second updating module, wherein the second updating module performs forward and backward message passing updates in the updated multi-AUV factor network model within the current timing window according to a preset timing window range;

[0128] The estimation module performs weighted fusion on the estimation results of the nodes by forward and backward message passing in the updated multi-AUV factor network model within the current time window to obtain an estimation of the navigation state of each AUV at the current moment.

[0129] Example 3

[0130] This embodiment provides a computer medium, on which a computer program is stored. When the computer program is executed by a processor, the multi-AUV collaborative positioning optimization method based on a temporal factor network described in Example 1 is implemented.

[0131] The same or similar reference numerals correspond to the same or similar components;

[0132] The terms used in the drawings to describe positional relationships are only used for illustrative purposes and should not be construed as limiting this patent;

[0133] Obviously, the above embodiments of the present invention are merely examples for clearly illustrating the present invention, and are not intended to limit the embodiments of the present invention. For those skilled in the art, other different forms of changes or modifications can be made based on the above description. It is not necessary and impossible to list all the embodiments here. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the claims of the present invention.

Claims

1. A multi-AUV collaborative positioning optimization method based on a temporal factor network, characterized in that: The following steps are involved: Constructing a multi-AUV factor network model, wherein the multi-AUV factor network model includes nodes abstracted from the navigation information of each AUV and the relative observation information between the AUVs, which are connected in chronological order; Initialize and set the nodes corresponding to the initial time in the multi-AUV factor network model; According to the observation information source of the AUV, updating the multi-AUV factor network model; According to a preset timing window range, forward and backward message passing updates are performed in the updated multi-AUV factor network model within the current timing window; The estimation results of the nodes in the updated multi-AUV factor network model through forward and backward message passing within the current time window are weightedly fused to obtain the estimation of the navigation state of each AUV at the current moment.

2. The multi-AUV collaborative positioning optimization method based on temporal factor network according to claim 1 is characterized in that: Each AUV is equipped with: Autonomous navigation sensors, including strapdown inertial navigation system SINS, Doppler velocimeter DVL, magnetic compass MCP, terrain matching positioning system TAN and pressure depth gauge DM, are used to obtain the AUV's own navigation information; Hydroacoustic detection equipment, used to obtain relative observation information between AUVs, including relative distance and relative orientation; Hydroacoustic communication equipment, used to exchange information with other AUVs.

3. The multi-AUV collaborative positioning optimization method based on temporal factor network according to claim 2 is characterized in that: The multi-AUV factor network model includes: The AUV’s own navigation information obtained by the strapdown inertial navigation system SINS, Doppler velocimeter DVL, magnetic compass MCP, terrain matching positioning system TAN and pressure depth gauge DM corresponds to the factor nodes f IMU 、f DVL 、f MCP 、f TAN and f DM ; The relative distance, relative azimuth, and relative distance + relative azimuth measurement signals obtained by the underwater acoustic detection equipment correspond to the factor nodes f rela_dis 、f rela_Ang and f rela_Dis+Ang ; The attitude error, velocity error, position error, gyroscope and accelerometer constant deviation of a single AUV are taken as state quantities, corresponding to the variable node of the AUV, the variable node of the AUV at each moment and the factor node f DVL 、f MCP 、f TAN and f DM The variable nodes corresponding to the same AUV at different times are connected in chronological order through the factor nodes corresponding to the current time f IMU and the deviation factor node f Bias Connection, the variable nodes corresponding to different AUVs at the same time are connected through the corresponding factor nodes f rela_dis 、f rela_Ang and f rela_Dis+Ang connect.

4. The multi-AUV collaborative positioning optimization method based on temporal factor network according to claim 3 is characterized in that: Initialize and set the nodes corresponding to the initial time in the multi-AUV factor network model, including the variable node corresponding to the initial time and the unary prior factor node f associated with the variable v prior , where, for Gaussian distribution, the prior factor node f prior With mean μ υ and variance∑ υ The way given is: Where d(·) represents the corresponding cost function; is the squared Mahalanobis distance, Σ is the noise covariance matrix; 5. The multi-AUV collaborative positioning optimization method based on temporal factor network according to claim 4 is characterized in that: According to the observation information source of the AUV, the multi-AUV factor network model is updated, including: If in t k The IMU measurement information of the strapdown inertial navigation system SINS is received at all times. and the current variable node x k To predict the variable node at the next moment Updated navigation solutions for each AUV platform: Where h(·) represents the IMU state transfer function; If in t g Always receive other measurement information besides IMU measurement information, define and add new variable nodes and factor nodes f IMU , Deviation factor node f Bias and observation information factor nodes.

6. The multi-AUV collaborative positioning optimization method based on temporal factor network according to claim 5 is characterized in that: According to the preset timing window range, before performing forward and backward message transmission updates in the updated multi-AUV factor network model within the current timing window, it also includes: If the new observation information exceeds the preset time series window range, it slides forward and generates a new window, and at the same time, the navigation result estimated in the previous window is used as the prior estimate of the current window.

7. The multi-AUV collaborative positioning optimization method based on temporal factor network according to claim 6 is characterized in that: According to the preset timing window range, the sum-product algorithm is used to perform forward and backward message passing updates in the updated multi-AUV factor network model within the current timing window.

8. The multi-AUV collaborative positioning optimization method based on temporal factor network according to claim 7 is characterized in that: The estimated results of the nodes in the updated multi-AUV factor network model by forward and backward message passing within the current time series window are weighted fused to obtain the estimation of the navigation state of each AUV at the current moment: In the formula, a Gaussian distribution is described by the mean vector m and the variance matrix V. If the mean vector in the forward message of a variable node x is expressed as The variance matrix is ​​expressed as The mean vector in the backward message is expressed as The variance matrix is ​​expressed as 9. A multi-AUV collaborative positioning optimization device based on a temporal factor network, characterized in that: include: A construction module, wherein the construction module is used to construct a multi-AUV factor network model, wherein the multi-AUV factor network model includes nodes abstracted from the navigation information of each AUV and the relative observation information between the AUVs, which are connected in chronological order; An initialization module, the initialization module is used to initialize and set the nodes corresponding to the initial time in the multi-AUV factor network model; A first updating module, wherein the first updating module updates the multi-AUV factor network model according to the observation information source of the AUV; A second updating module, wherein the second updating module performs forward and backward message passing updates in the updated multi-AUV factor network model within the current timing window according to a preset timing window range; The estimation module performs weighted fusion on the estimation results of the nodes by forward and backward message passing in the updated multi-AUV factor network model within the current time window to obtain an estimation of the navigation state of each AUV at the current moment.

10. A computer medium, characterized in that The computer medium stores a computer program, and when the computer program is executed by the processor, the multi-AUV collaborative positioning optimization method based on the timing factor network described in any one of claims 1 to 8 is implemented.

Images