Collaborative Navigation and Positioning Method for Multi-Marine Unmanned Systems Based on Hierarchical Relay Nodes

Abstract

This application belongs to the technical field of underwater navigation and positioning. This application provides a cooperative navigation and positioning method for multiple maritime unmanned systems based on hierarchical relay nodes. In the embodiments of the present disclosure, by hierarchically dividing the slave maritime unmanned systems, an improved particle swarm algorithm is used to select the optimal combination of reference nodes to obtain the first-layer reference nodes and the second-layer nodes; a first distance measurement model and a second distance measurement model are constructed based on the first-layer reference nodes and the second-layer nodes, and a relay node communication network for the overall cluster is constructed in combination with broadband signals; the first error-corrected position and the second error-corrected position are calculated to achieve the cooperative navigation and positioning of the maritime unmanned system cluster. It can solve problems such as the serious communication burden of the master maritime unmanned system, poor connectivity of the communication network, weak system observability, and low utilization rate of cluster position information caused by the increase in the number of slave maritime unmanned systems, resulting in the inability to correct the positioning errors of the slave maritime unmanned systems and the failure of cluster cooperative navigation and positioning.

Description

Technical Field

[0001] The disclosed embodiments relate to the field of underwater navigation and positioning technology, and in particular to a collaborative navigation and positioning method for multiple marine unmanned systems based on hierarchical relay nodes. Background Art

[0002] The complexity and variability of the marine hydrological environment and the continuous expansion of the ocean area make it difficult for a single marine unmanned system with a small number of sensors, a small sensing range, and a slow information processing rate to independently undertake and complete tasks with complex instructions and a lot of interactive information. The collaborative operation system of multiple marine unmanned systems with advantages such as high quality and high efficiency has become a research hotspot in the field of navigation and positioning of marine cluster systems, and has gradually been applied to long-duration and long-range fields such as marine patrols, marine mapping, marine resource exploration, and diving support.

[0003] For the collaborative positioning system, system observability is the ability to determine the position of the unmanned system at sea using observation information, and is also a prerequisite for realizing the collaborative navigation and positioning of the unmanned system cluster at sea. The master-slave cooperative positioning system of the unmanned system at sea with a single system as the pilot is a research trend. The master system is equipped with a high-precision inertial navigation system (INS), and the slave system is equipped with a low-precision INS and realizes information interaction with the master system through the underwater acoustic communication network, suppressing the accumulation of positioning errors and realizing the correction of its own errors, thereby completing the cluster's task allocation, formation control and collaborative navigation and positioning functions.

[0004] The traditional master-slave maritime unmanned system collaborative positioning system has the following problems: due to the harsh underwater communication conditions and special underwater environment, angle measurement technology is introduced to improve positioning accuracy. However, as the number of maritime unmanned systems in the cluster increases, the computing cost will continue to increase. Considering the cost, it is impossible to obtain an accurate position reference based on a single distance observation information; the slave maritime unmanned system has a strong dependence on the master maritime unmanned system. When a slave maritime unmanned system is far away from the master maritime unmanned system, not only will the ranging error caused by the propagation delay of the underwater acoustic channel affect the size of the correction error of the slave maritime unmanned system and the accuracy of the positioning result, but the acoustic equipment carried by the master maritime unmanned system may be limited in maximum range due to power limitation, resulting in information interaction failure. Therefore, it is extremely important to seek a technical means to ensure the observability of the master-slave maritime unmanned system collaborative positioning system and the connectivity of the underwater acoustic communication network.

[0005] Therefore, it is necessary to improve one or more problems existing in the above-mentioned related technical solutions.

[0006] It should be noted that this section aims to provide background or context for the technical solutions of the present disclosure stated in the claims. The descriptions herein are not admitted to be prior art merely because they are included in this section. Summary of the Invention

[0007] The object of the embodiments of the present disclosure is to provide a cooperative navigation and positioning method for a multi-sea unmanned system based on hierarchical relay nodes, so as to overcome at least to some extent one or more problems caused by the limitations and defects of related technologies.

[0008] According to the embodiments of the present disclosure, there is provided a cooperative navigation and positioning method for a multi-sea unmanned system based on hierarchical relay nodes, and the method includes:

[0009] Based on the state information of the sea unmanned system cluster, kinematic models of each are constructed; wherein, the sea unmanned system cluster includes a main sea unmanned system and a plurality of slave sea unmanned systems;

[0010] The particle swarm algorithm is used to select an optimal reference node combination from the slave sea unmanned systems; wherein, at least 3 slave sea unmanned systems in the optimal reference node combination serve as the first-layer reference nodes, and the remaining slave sea unmanned systems serve as the second-layer nodes;

[0011] Based on their respective kinematic models, according to the relative positions between the main sea unmanned system and each first-layer reference node, a first distance measurement model is constructed;

[0012] Based on their respective kinematic models, according to the relative positions between each first-layer reference node and each second-layer node, a second distance measurement model is constructed;

[0013] Based on the first distance measurement model, the first distance measurement model and the broadband signal, a relay node communication network is constructed, and the position information of the main sea unmanned system is broadcast to the first-layer reference nodes;

[0014] Based on the first distance measurement model, according to the position information of the main sea unmanned system, the sound speed at adjacent measurement times and the first time delay, the first error correction position of the first-layer reference nodes is obtained, and the first error correction position is broadcast to the second-layer nodes;

[0015] Based on the second distance measurement model, according to the error correction position of the first-layer reference nodes, the sound speed and the second time delay, the second error correction position of the second-layer nodes is obtained.

[0016] Further, the expression of the kinematic model is:

[0017]

[0018] Wherein, is the number of sea unmanned systems, is the serial number of the unmanned maritime system, indicating the main unmanned maritime system, and the rest are slave unmanned maritime systems, is the abscissa of the th unmanned maritime system, is the ordinate of the th unmanned maritime system, is the yaw angle of the th unmanned maritime system, is the abscissa of the th unmanned maritime system, is the ordinate of the th unmanned maritime system, is the yaw angle of the th unmanned maritime system, is the forward synthesis speed of the th unmanned maritime system, is the yaw angular velocity of the th unmanned maritime system, is the sampling period.

[0019] Furthermore, in the step of using the particle swarm optimization algorithm to select the optimal reference node combination from the slave unmanned maritime systems, it includes:

[0020] Determine the maximum communication measurement distance, the maximum number of communication links between the motion area of the unmanned maritime system and the first-layer reference nodes;

[0021] Based on the maximum communication measurement distance, the maximum number of communication links between the motion area of the unmanned maritime system and the first-layer reference nodes, obtain the reference node dispersion and physical measurement error;

[0022] Perform a linear weighted sum of the reference node dispersion and the physical measurement error to obtain the objective function of the particle swarm optimization algorithm;

[0023] Based on the initial position information of the unmanned maritime system cluster, obtain the ranging information of each slave unmanned maritime system;

[0024] Based on the ranging information and the objective function, use the particle swarm optimization algorithm to calculate the particle fitness, the individual optimal solution and the global optimal solution, and update the particle state;

[0025] Introduce an adaptive mutation mechanism, perform a secondary update on the particle state according to the probability principle, and traverse all slave unmanned maritime systems to obtain the optimal reference node combination.

[0026] Furthermore, the expression of the reference node dispersion is:

[0027]

[0028] In the formula, is the maximum effective area, is the number of first-layer reference nodes;

[0029] Physical measurement error has the following expression:

[0030]

[0031] In the formula, is the error between the first-layer reference node and the second-layer node on the abscissa, is the error between the first-layer reference node and the second-layer node on the ordinate, is the ranging variance, is the difference in the distances from the second-layer node to the first-layer reference node with serial number 2 and the first-layer reference node with serial number 1 in the optimal reference node combination, is the difference in the distances from the second-layer node to the first-layer reference node with serial number 3 and the first-layer reference node with serial number 1 in the optimal reference node combination; is the first diagonal element, is the second diagonal element;

[0032] Objective function has the following expression:

[0033]

[0034] In the formula, is the maximum-minimum normalization process, is the first weight coefficient, is the second weight coefficient.

[0035] Furthermore, when introducing the adaptive mutation mechanism and performing the secondary update of the particle state according to the probability principle, it includes:

[0036] When the mutation rate is satisfied, the particle state performs a secondary update of the position, which is expressed as follows:

[0037]

[0038] Among them, is a random number between, is the lower bound of the particle search range, is the upper bound of the particle search range, the spatial dimension takes values from 1 to 2, is the number of first-layer reference nodes, is the ceiling symbol, is the position of the particle after passing through the adaptive mutation mechanism.

[0039] Further, in the step of constructing the first distance measurement model based on the respective kinematic models according to the relative positions of the main maritime unmanned system and each first-layer reference node, it includes:

[0040] Based on the respective kinematic models, establish a first distance measurement model through the relative positions between the main maritime unmanned system and the slave maritime unmanned system; wherein, the input of the first distance measurement model is:

[0041]

[0042] Wherein, is the velocity measurement value, is the yaw angular velocity measurement value, represents the first Gaussian white noise with a mean of zero, represents the second Gaussian white noise with a mean of zero;

[0043] The variance matrix is and the noise covariance is:

[0044]

[0045] Wherein, is the velocity measurement noise variance, is the angle measurement noise variance;

[0046] The expression of the first distance measurement model between the main maritime unmanned system and the first-layer reference node is:

[0047]

[0048] Wherein, is the distance between the first-layer reference node and the main maritime unmanned system, is the total number of the first-layer reference nodes; and , is the speed of sound, is the first time delay between the first-layer reference node and the main maritime unmanned system, is the state transition function, is the measurement noise, is the abscissa of the main maritime unmanned system, is the ordinate of the main maritime unmanned system.

[0049] Further, the expression of the second distance measurement model is:

[0050]

[0051] Wherein, is the th second-layer node and the The distance between the first-layer reference nodes;

[0052]

[0053] Wherein, is the second delay between the th second-layer node and the

[0054] th first-layer reference node.

[0055] Furthermore, the relay node communication network includes:

[0056] The first communication links between the main maritime unmanned system and each first-layer reference node, and the second communication links between each first-layer reference node and each second-layer node; wherein,

[0057] The main maritime unmanned system broadcasts its location information to each first-layer reference node through the first communication link, and each first-layer reference node broadcasts its first error-corrected location to each second-layer node through the second communication link.

[0058] Furthermore, in the step of obtaining the first error-corrected location of the first-layer reference node based on the first distance measurement model according to the location information of the main maritime unmanned system, the sound speed at adjacent measurement times, and the first delay, it includes:

[0059] Based on the first distance measurement model, obtain the distance between the first-layer reference node and the main maritime unmanned system according to the sound speed and delay at adjacent measurement times;

[0060] According to the location information of the main maritime unmanned system and the distance between the first-layer reference node and the main maritime unmanned system, obtain the first error-corrected location of the first-layer reference node.

[0061] Furthermore, in the step of obtaining the second error-corrected location of the second-layer node based on the second distance measurement model according to the error-corrected location of the first-layer reference node, the sound speed, and the second delay, it includes:

[0062] Based on the second distance measurement model, obtain the distance between the second-layer reference node and the second-layer node according to the sound speed and the second delay;

[0063] The technical solutions provided by the embodiments of the present disclosure may include the following beneficial effects:

[0064] In the embodiments of the present disclosure, through the above-mentioned collaborative navigation and positioning method for multi-maritime unmanned systems based on hierarchical relay nodes, on the one hand, by hierarchically dividing the slave maritime unmanned systems and using an improved particle swarm algorithm to select the optimal reference node combination, the first-layer reference nodes and the second-layer nodes are obtained; a first distance measurement model is constructed according to the relative positions of the master maritime unmanned system and the first-layer reference nodes; a second distance measurement model is constructed according to the relative positions of the first-layer reference nodes and the second-layer nodes; a relay node communication network for the overall cluster is constructed through the first distance measurement model, the second distance measurement model, and broadband signals; the first error correction positions of the first-layer reference nodes and the second error correction positions of the second-layer nodes are calculated using the first distance measurement model and the second distance measurement model to achieve the collaborative navigation and positioning of the maritime unmanned system cluster. On the other hand, it solves the problems caused by the increase in the number of slave maritime unmanned systems, such as the serious communication burden on the master maritime unmanned system, poor connectivity of the communication network, weak system observability, and low utilization rate of cluster position information, resulting in the inability to correct the positioning errors of the slave maritime unmanned systems and the failure of cluster collaborative navigation and positioning; it provides a set of performance-robust collaborative navigation and positioning solutions for the master-slave collaborative navigation system of multi-maritime unmanned systems, and improves the success rate of cluster mission execution of maritime unmanned systems. BRIEF DESCRIPTION OF THE DRAWINGS

[0065] The accompanying drawings herein are incorporated into the specification and constitute a part of this specification, showing embodiments consistent with the present disclosure, and are used together with the specification to explain the principles of the present disclosure. Obviously, the accompanying drawings in the following description are only some embodiments of the present disclosure, and those of ordinary skill in the art can obtain other drawings without creative efforts based on these drawings.

[0066] Figure 1 A flowchart showing the steps of the collaborative navigation and positioning method for multi-maritime unmanned systems based on hierarchical relay nodes in an exemplary embodiment of the present disclosure;

[0067] Figure 2 A schematic diagram showing the communication network of hierarchical relay nodes in an exemplary embodiment of the present disclosure;

[0068] Figure 3 A flowchart showing the selection of the first-layer reference nodes based on the particle swarm algorithm in an exemplary embodiment of the present disclosure;

[0069] Figure 4 A schematic diagram showing the positioning of the second-layer nodes in an exemplary embodiment of the present disclosure;

[0070] Figure 5 A diagram showing the iterative process of selecting the first-layer reference nodes based on the particle swarm algorithm in an exemplary embodiment of the present disclosure;

[0071] Figure 6Shows the analysis diagram of the positioning performance of the second-layer nodes when the mutation probability is 0.2 in the exemplary embodiment of the present disclosure;

[0072] Figure 7 Shows the analysis diagram of the positioning performance of the second-layer nodes when the mutation probability is 0.4 in the exemplary embodiment of the present disclosure;

[0073] Figure 8 Shows the analysis diagram of the positioning performance of the second-layer nodes when the mutation probability is 0.6 in the exemplary embodiment of the present disclosure;

[0074] Figure 9 Shows the analysis diagram of the positioning performance of the second-layer nodes when the mutation probability is 0.8 in the exemplary embodiment of the present disclosure. Detailed implementation manners

[0075] Example embodiments will now be described more fully with reference to the accompanying drawings. However, the example embodiments can be implemented in various forms and should not be construed as limited to the examples set forth herein; rather, these embodiments are provided so that this disclosure will be more complete and comprehensive, and will fully convey the concept of the example embodiments to those skilled in the art. The features, structures, or characteristics described may be combined in any suitable manner in one or more embodiments.

[0076] In addition, the accompanying drawings are only schematic illustrations of the embodiments of the present disclosure and are not necessarily drawn to scale. The same reference numerals in the drawings denote the same or similar parts, and thus their repeated description will be omitted. Some of the block diagrams shown in the drawings are functional entities and do not necessarily correspond to physically or logically independent entities.

[0077] In this example embodiment, a cooperative navigation and positioning method for a multi-sea unmanned system based on hierarchical relay nodes is provided. Refer to Figure 1 As shown in, the cooperative navigation and positioning method for the multi-sea unmanned system based on hierarchical relay nodes may include: Step S101 to Step S107.

[0078] Step S101: Based on the state information of the sea unmanned system cluster, construct their respective kinematic models; wherein, the sea unmanned system cluster includes a main sea unmanned system and several slave sea unmanned systems;

[0079] Step S102: Use the particle swarm optimization algorithm to select an optimal reference node combination from the slave sea unmanned systems; wherein, at least 3 slave sea unmanned systems in the optimal reference node combination serve as the first-layer reference nodes, and the remaining slave sea unmanned systems serve as the second-layer nodes;

[0080] Step S103: Based on their respective kinematic models, construct a first distance measurement model according to the relative positions of the main sea unmanned system and each first-layer reference node;

[0081] Step S104: Based on their respective kinematic models, construct a second distance measurement model according to the relative positions of each first-layer reference node and each second-layer node.

[0082] Step S105: Based on the first distance measurement model, the first distance measurement model, and the broadband signal, construct a relay node communication network for the unmanned marine systems, and broadcast the position information of the master unmanned marine system to the first-layer reference nodes.

[0083] Step S106: Based on the first distance measurement model, obtain the first error-corrected position of the first-layer reference nodes according to the position information of the master unmanned marine system, the sound speed at adjacent measurement times, and the first time delay, and broadcast the first error-corrected position to the second-layer nodes.

[0084] Step S107: Based on the second distance measurement model, obtain the second error-corrected position of the second-layer nodes according to the error-corrected positions of the first-layer reference nodes, the sound speed, and the second time delay.

[0085] Through the above collaborative navigation and positioning method for multi-unmanned marine systems based on hierarchical relay nodes, on the one hand, by hierarchically dividing the slave unmanned marine systems and using an improved particle swarm algorithm to select the optimal reference node combination, the first-layer reference nodes and the second-layer nodes are obtained; a first distance measurement model is constructed according to the relative positions of the master unmanned marine system and the first-layer reference nodes; a second distance measurement model is constructed according to the relative positions of the first-layer reference nodes and the second-layer nodes; a relay node communication network for the entire cluster is constructed through the first distance measurement model, the second distance measurement model, and the broadband signal; the first error-corrected position of the first-layer reference nodes and the second error-corrected position of the second-layer nodes are calculated using the first distance measurement model and the second distance measurement model to achieve the collaborative navigation and positioning of the unmanned marine system cluster. On the other hand, it solves the problems caused by the increase in the number of slave unmanned marine systems, such as the serious communication burden on the master unmanned marine system, poor connectivity of the communication network, weak system observability, and low utilization rate of the cluster position information, resulting in the inability to correct the positioning errors of the slave unmanned marine systems and the failure of the cluster collaborative navigation and positioning; it provides a set of performance-robust collaborative navigation and positioning solutions for the master-slave collaborative navigation system of multi-unmanned marine systems, and improves the success rate of the cluster task execution of the unmanned marine systems.

[0086] Next, reference will be made to Figures 1 to 9 for a more detailed description of each step of the above collaborative navigation and positioning method for multi-unmanned marine systems based on hierarchical relay nodes in this exemplary embodiment.

[0087] In step S101, the invention of the present application only considers the overall performance of the system and treats the unmanned marine system as a spatial node, without considering the influence of form, pitch angle, etc. The depth is measured by a depth gauge, and a two-dimensional kinematic model of the unmanned marine system is established through coordinate transformation.

[0088]

[0089] Among them, and are the two-dimensional coordinates of the unmanned marine system, represents the yaw angle, is the number of unmanned marine systems. When the value is 0, it represents the main unmanned marine system, and when the value is not 0, it represents the slave unmanned marine system. is the sampling period, is the forward synthetic velocity, is the yaw angular velocity.

[0090] In step S102, the particle swarm algorithm is used to select the first-layer reference nodes. The algorithm includes: enriching the constraint conditions and constructing the objective function; introducing an adaptive mutation mechanism to perform a secondary update on the particle state according to the probability principle to improve the traversal and optimization capabilities of the particle swarm. The effect of the above operations is: the optimal first-layer reference node slave unmanned marine system (i.e., the first-layer reference node) can be found among various combinations of slave unmanned marine systems.

[0091] (1) Among them, the constraint conditions include: reference node dispersion, maximum communication measurement distance, and physical measurement error. The parameter settings of the three constraint conditions are not only the content of the optimization initialization but also affect the final positioning accuracy. The setting principles and meanings can be subdivided into:

[0092] Reference node dispersion : With the first-layer reference node as the center and the maximum transmission distance of the node as the radius, determine the maximum effective area that can be covered. It is required that the reference nodes are more dispersed, that is, the dispersion degree is greater, so as to ensure that the motion area of the unmanned marine system can be effectively covered. The reference node dispersion can be expressed as:

[0093]

[0094] Among them, is the area of the maximum effective area, is the number of the first-layer reference nodes.

[0095] Maximum communication measurement distance: There is relative motion between nodes, resulting in continuous changes in the relative distance. Adding the maximum communication distance constraint condition ensures the connectivity of the communication network.

[0096] Physical measurement error When solving the positioning distance, even if the influence of multipath effect is ignored, it is impossible to avoid the error caused by physical measurement, which is proportional to the sum of the squares of the distance differences from the second-layer nodes to the first-layer reference nodes.

[0097]

[0098] Among them, is determined by the variance of the ranging error, , , is the distance from the first-layer reference node with the serial number in the optimal reference node combination to the second-layer node, is the distance from the first-layer reference node with the serial number 1 in the optimal reference node combination to the second-layer node to be located from the maritime unmanned system, and can be obtained by solving the ranging equation through the least squares algorithm and matrix transformation. The specific explanation is as follows:

[0099]

[0100] Through matrix transformation and least squares, , and are the diagonal elements of the matrix.

[0101] Combining the above constraints, for and , the maximum-minimum normalization (Min-Max Normalization, MMN) is introduced to implement the standardization processing of the data. The processing method is as follows:

[0102]

[0103] Among them, is the maximum value of the sample data, is the minimum value of the sample data, and a new fitness function (i.e., the objective function) is established by linear weighting and minimized; among them, , and make it minimum; among them,

[0104]

[0105] (2) Adaptive mutation mechanism, which is used to solve the problem of slow convergence speed and local optimality of the solution when searching for the first-layer reference nodes. According to the probability principle, the particle dimension state is updated twice to improve the traversal ability, convergence speed and optimization ability of the particle swarm. When the mutation rate , the particle state is updated twice for the position, which is expressed as follows:

[0106]

[0107] Among them, and are the upper and lower bounds of the particle search range, takes values from 1 to 2 for the spatial dimension, is the number of the first-layer reference nodes from the maritime unmanned systems, is the ceiling symbol, is the position of the particle after passing through the adaptive mutation mechanism. By setting the size, the position of a certain dimension of a certain particle can be randomly changed. Increasing will increase the frequency of the mutation operation, while decreasing will decrease the frequency of the mutation operation.

[0108] In step S103, the process of constructing the first distance measurement model is as follows: Combining the motion model (i.e., the kinematic model) of the maritime unmanned system in step S101, a first distance measurement model is established based on the relative position between the master and slave maritime unmanned systems. Gaussian white noise is selected for the noise, and the system input model is as follows:

[0109]

[0110] Among them, is the velocity measurement value, is the yaw angular velocity measurement value; and are Gaussian white noises with a mean of zero, and the variance matrix is , and the noise covariance is expressed as:

[0111]

[0112] The distance measurement model between the master maritime unmanned system and the first-layer reference node's slave maritime unmanned systems is:

[0113]

[0114] Among them, is the distance between the first-layer reference node and the master node's maritime unmanned system, is the total number of the first-layer reference node's slave maritime unmanned systems, Solving uses the sound speed and the first time delay , which is interpreted as:

[0115]

[0116] In step S104, the distance measurement model between the second-layer nodes and the first-layer reference nodes is the same as that in step S103 and can be expressed as:

[0117]

[0118] Among them, is the distance between the second-layer node and the first-layer reference node from the maritime unmanned system, which is obtained through the sound speed and the second time delay and is interpreted as:

[0119]

[0120] Among them, is the first time delay between the th node in the second layer and the th node in the first-layer reference node from the maritime unmanned systems.

[0121] In step S105, the relay node communication network of the maritime unmanned system cluster undertakes the information transmission tasks of navigation information, identification number, waiting time, and positioning signal transmission delay. The slave maritime unmanned system of the first-layer reference node establishes a communication link with the slave maritime unmanned system of the second layer to complete the information transmission and the response to the positioning request broadcast. The number of first-layer reference nodes is not less than 3, which can ensure that when a certain reference slave maritime unmanned system is damaged, other first-layer reference slave maritime unmanned systems can continue to complete the information transmission and the positioning requirements of the second-layer slave maritime unmanned systems. The specific working principle of the relay node communication network is prior art and will not be elaborated in detail here.

[0122] In step S106, the positioning process for the slave maritime unmanned system of the first-layer reference node is as follows: the distance from the master maritime unmanned system is obtained through the sound speed and time delay at adjacent measurement times. After obtaining its own speed measurement using a log, the observability analysis of the single-master vessel system based on the Lie derivative has explained this situation, and the position estimation and error correction are completed through the distance measurement model and the least squares or filtering algorithm.

[0123] In step S107, the number of first-layer reference nodes is not less than three. A two-dimensional motion model is established for the maritime unmanned system, and the position estimation and error correction are completed through the distance measurement model and the least squares or filtering algorithm. When it is necessary to position a certain slave maritime unmanned system, first perform a hierarchical judgment on the slave maritime unmanned system, and receive the navigation information, identification number, waiting time, and positioning signal transmission delay information through the communication network.

[0124] In one embodiment, the unmanned maritime system nodes are divided into the first-layer reference node unmanned maritime systems (with a quantity of not less than 3) and the second-layer node unmanned maritime systems (i.e., the second-layer nodes). The main unmanned maritime system is equipped with a high-precision INS to estimate its own position and corresponding motion state, a velocimeter to measure its own speed, and an underwater acoustic communication machine for command data interaction with the first-layer reference node unmanned maritime systems; the data processing module is used to output its own position and generate collaborative positioning messages; the slave unmanned maritime systems are equipped with low-precision INSs to output their own motion states and position information, the underwater acoustic communication machines are used for data interaction and message reception, and the data processing module is used to fuse the positioning data. The cluster realizes information interaction through the relay node communication network, suppresses the accumulation of INS positioning errors, corrects its own errors, and then completes functions such as task allocation, formation control, and collaborative navigation and positioning of the cluster.

[0125] In one embodiment, in combination with Figure 2 and Figure 3 , the hierarchical relay node communication network and the selection process of the first-layer reference nodes are explained. The first-layer reference node unmanned maritime systems use the Figure 3 shown flowchart to select the optimal combination. The first-layer reference nodes, as relay nodes, establish information interaction between the main unmanned maritime system and the slave unmanned maritime systems, which can not only share the communication burden for the main unmanned maritime system but also provide high-precision positioning for the second-layer node unmanned maritime systems like a base station. Initially, the initial positions of the unmanned maritime system cluster floating on the water surface are recorded. After diving, the first-layer reference nodes are selected according to the motion characteristics of the unmanned maritime systems and the fitness function and boundary conditions established in step S102. The information interaction and the first distance measurement model between the main unmanned maritime system and the first-layer reference nodes are established. The relative positions of the main unmanned maritime system to the first-layer reference node unmanned maritime systems are obtained by using adjacent measurement times. Subsequently, the collaborative positioning of the first-layer reference node unmanned maritime systems is realized by using INS and periodic correction information. Since the motion models of the unmanned maritime systems are the same, the changes in their motion headings and speeds with the increase of navigation time can be ignored. That is, it can be considered that the relative motion speed between the slave unmanned maritime systems is 0. The first-layer reference nodes can be regarded as fixed base stations, and the second distance measurement model with the second-layer node unmanned maritime systems is established, thereby realizing the positioning of the second-layer node unmanned maritime systems. Subsequently, information interaction is realized through the communication network to the main unmanned maritime system.

[0126] Figure 3 is the flowchart for selecting the first-layer reference nodes based on the particle swarm algorithm. The specific process is as follows: First, the boundary conditions are determined to perform multi-dimensional restrictions on the particles, including the maximum communication measurement distance, the motion area of the unmanned maritime systems, and the maximum number of communication links of the first-layer reference node unmanned maritime systems; Second, the fitness function is established by comprehensively considering the constraint conditions to evaluate the node dispersion With physical measurement errors Perform linear weighted summation to obtain the fitness function ; Finally, introduce an adaptive mutation mechanism to assign a mutation probability to the particles, improving the optimization ability and convergence ability.

[0127] The maximum communication distance considers transmission loss, sets a threshold for the received signal power, and comprehensively considers factors such as sound source level intensity, ambient noise intensity, absorption loss, etc.;

[0128] The motion area of the unmanned marine system is set as a two-dimensional plane;

[0129] The number of communication links of the first-layer reference node from the unmanned marine system is set to 4 at most, including the communication link with the main unmanned marine system and the communication links controlling three slave unmanned marine systems. At the same time, it involves the number of slave unmanned marine systems selected as the first-layer reference node from the slave unmanned marine system cluster;

[0130] The establishment of the fitness function follows the MMN criterion to achieve the magnitude unification of node dispersion With physical measurement errors When solving , take the first-layer reference node as the center and the maximum communication distance as the radius to obtain the maximum effective area region When solving the physical measurement error , select the slave unmanned marine systems within the maximum communication distance range of the first-layer reference node from the slave unmanned marine system for distance observation, and then perform cumulative summation on the physical measurement errors observed by the first-layer reference node from the slave unmanned marine system to obtain ;

[0131] The introduction of the adaptive mutation mechanism optimizes the particle position and velocity update formulas. The traditional formulas are:

[0132]

[0133] Among them, represents the inertia weight, and represent the learning factors, represents a random value uniformly distributed in , and the two are independent of each other, is the th particle in the particle swarm. After introducing the mutation probability, it becomes:

[0134]

[0135] The purpose of the above two operations is to achieve multi-objective optimization and improve the particle optimization ability. Multiple optimal combinations can be listed and adjusted and selected according to the actual engineering situation. After establishing the first-layer reference node from the maritime unmanned system, the communication relationship and distance measurement model between the main maritime unmanned system and it are established. According to the communication distance, the distance measurement model and communication relationship between the second-layer node from the maritime unmanned system and the first-layer reference node from the maritime unmanned system are established to realize the construction of the overall maritime unmanned system cluster communication network.

[0136] Based on the main maritime unmanned system, the cooperative positioning of the first-layer reference node from the maritime unmanned system is realized. Since the observation information is one-dimensional distance measurement, to solve the two-dimensional position state of the system, at least two observations are required to obtain a unique solution to ensure the observability of the system. The system observation matrix can be expressed as:

[0137]

[0138] In the formula, is the Jacobian matrix of the non-linear measurement equation with respect to the state , is the Jacobian matrix of the state at the next moment, is the posterior information matrix. If the position of the main maritime unmanned system at adjacent measurement times is and , the position of the th slave maritime unmanned system in the first-layer reference node is and , as well as the distance observation values and , then the observability matrix can be expressed as:

[0139]

[0140] When this matrix is full rank, the determinant of the matrix , that is, the coordinates at adjacent measurement times need to change. Due to the existence of the course angle, the observability of the system can be guaranteed. Subsequently, the position coordinates of the first-layer reference node relative to the main maritime unmanned system can be obtained by solving the observation equation through the least squares or filtering algorithm, and the cooperative positioning can be realized by periodically transmitting correction information through the communication network.

[0141] In one embodiment, Figure 4 shows the positioning schematic diagram for solving the second-layer reference node from the maritime unmanned system based on the position coordinates of the first-layer reference node from the maritime unmanned system. The number of observation equations is greater than the number of coordinate unknowns, and the least squares or filtering algorithm can be used for position solution. The cooperative positioning can be realized by periodically transmitting correction information through the communication network.

[0142] In a specific embodiment,Figure 5 Shows the iterative process diagram of selecting the first-layer reference nodes based on the particle swarm algorithm under different mutation probabilities. Each unmanned marine system is equivalent to a single node, and an area of m is established. Taking twelve nodes as an example, three first-layer reference nodes are selected for location, and the remaining nine are used as the second-layer nodes to be located for unmanned marine systems. The formula is used as the fitness function (select ), the number of particles is 50, the number of iterations is selected as 200, the inertia weight takes the value of 0.8, and the learning factors and take 0.5. The mutation probability selects four cases of 0.2, 0.4, 0.6, and 0.8. When the mutation probability is different, the number of iterations required to reach the optimal fitness is also different. Because when the mutation probability is low, it means that the frequency of mutation operations in the particle population is low, and the range of changes in the positions of the particle population is relatively small; when the mutation probability is high, it means that the frequency of mutation operations in the particle population is high, and the range of changes in the positions of the particle population is relatively large, and a larger traversal space can be searched in a shorter time. Among them, Figure 6 is the analysis diagram of the positioning performance of the second-layer nodes when the mutation probability is 0.2; Figure 7 is the analysis diagram of the positioning performance of the second-layer nodes when the mutation probability is 0.4; Figure 8 is the analysis diagram of the positioning performance of the second-layer nodes when the mutation probability is 0.6; Figure 9 is the analysis diagram of the positioning performance of the second-layer nodes when the mutation probability is 0.8.

[0143] Figure 6 and Figure 7 and Figure 8 and Figure 9 show the analysis of the positioning performance of the second-layer nodes under different mutation probabilities. At the same time, by adding Gaussian white noise with a mean of 0 and a variance of 0.2 to the ranging and positioning process to simulate the ranging and positioning errors, the positioning performances under different mutation probabilities are compared. Through 300 observations of the second-layer nodes to be located for unmanned marine systems, the positioning errors of the nine unmanned marine systems at the same observation epoch are obtained, and the overall positioning performance analysis diagram is obtained by weighted summation of the positioning errors.

[0144] Through the above collaborative navigation and positioning method for multi-sea unmanned systems based on hierarchical relay nodes, on the one hand, by hierarchically dividing the slave sea unmanned systems and using the improved particle swarm algorithm to select the optimal reference node combination, the first-layer reference nodes and the second-layer nodes are obtained; a first distance measurement model is constructed according to the relative positions of the master sea unmanned system and the first-layer reference nodes; a second distance measurement model is constructed according to the relative positions of the first-layer reference nodes and the second-layer nodes; the relay node communication network of the overall cluster is constructed through the first distance measurement model, the second distance measurement model and the broadband signal; the first error correction positions of the first-layer reference nodes and the second error correction positions of the second-layer nodes are calculated by using the first distance measurement model and the second distance measurement model to achieve the collaborative navigation and positioning of the sea unmanned system cluster. On the other hand, it solves the problems such as the serious communication burden of the master sea unmanned system, poor connectivity of the communication network, weak system observability, and low utilization rate of the cluster position information caused by the increase in the number of slave sea unmanned systems, resulting in the inability to correct the positioning errors of the slave sea unmanned systems and the failure of the cluster collaborative navigation and positioning; it provides a set of performance-robust collaborative navigation and positioning solutions for the master-slave collaborative navigation system of multi-sea unmanned systems, and improves the success rate of the cluster task execution of the sea unmanned systems.

[0145] It should be understood that the orientation or positional relationship indicated by the terms "center", "longitudinal", "transverse", "length", "width", "thickness", "upper", "lower", "front", "rear", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", "clockwise", "counterclockwise", etc. in the above description is based on the orientation or positional relationship shown in the drawings, and is only for the convenience of describing the embodiments of the present disclosure and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation to the embodiments of the present disclosure.

[0146] In addition, the terms "first" and "second" are only used for descriptive purposes and cannot be understood as indicating or implying relative importance or implicitly specifying the quantity of the indicated technical features. Thus, the features defined with "first" and "second" may explicitly or implicitly include one or more of such features. In the description of the embodiments of the present disclosure, "a plurality" means two or more unless otherwise specifically defined.

[0147] In the embodiments of the present disclosure, unless otherwise clearly defined and limited, terms such as "install", "connect", "couple", "fix", etc. shall be understood in a broad sense. For example, it may be a fixed connection, a detachable connection, or integrated; it may be a mechanical connection or an electrical connection; it may be directly connected, or indirectly connected through an intermediate medium, and it may be the communication inside two elements or the interaction relationship between two elements. For those of ordinary skill in the art, the specific meanings of the above terms in the present disclosure can be understood according to specific circumstances.

[0148] In the embodiments of the present disclosure, unless otherwise clearly defined and limited, the first feature being "on" or "under" the second feature may include the direct contact between the first and second features, or may include the situation where the first and second features are not in direct contact but in contact through additional features therebetween. Moreover, the first feature being "above", "over" and "on top of" the second feature includes that the first feature is directly above and obliquely above the second feature, or merely means that the horizontal height of the first feature is higher than that of the second feature. The first feature being "under", "below" and "beneath" the second feature includes that the first feature is directly below and obliquely below the second feature, or merely means that the horizontal height of the first feature is lower than that of the second feature.

[0149] In the description of this specification, the descriptions with reference to terms such as "one embodiment", "some embodiments", "example", "specific example" or "some examples", etc. mean that the specific features, structures, materials or characteristics described in connection with the embodiment or example are included in at least one embodiment or example of the present disclosure. In this specification, the schematic descriptions of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in a suitable manner in any one or more embodiments or examples. In addition, those skilled in the art can combine and combine the different embodiments or examples described in this specification.

[0150] After considering the specification and practicing the invention disclosed herein, those skilled in the art will readily conceive of other implementations of the present disclosure. This application is intended to cover any variations, uses, or adaptations of the present disclosure, which follow the general principles of the present disclosure and include known common general knowledge or conventional technical means in the technical field not disclosed in the present disclosure. The specification and examples are only regarded as exemplary, and the true scope and spirit of the present disclosure are pointed out by the appended claims.

Claims

1. A collaborative navigation and positioning method for multiple unmanned marine systems based on hierarchical relay nodes, characterized in that: The method includes: Based on the state information of the maritime unmanned system cluster, the kinematic models of each are constructed; wherein the maritime unmanned system cluster includes a main maritime unmanned system and several slave maritime unmanned systems; The particle swarm algorithm is used to select the optimal reference node combination from the unmanned maritime system; wherein, the optimal reference node combination has at least 3 unmanned maritime systems as the first-layer reference nodes, and the rest of the unmanned maritime systems are used as the second-layer nodes; Based on the respective kinematic models, a first distance measurement model is constructed according to the relative positions of the main maritime unmanned system and each first-layer reference node; Based on the respective kinematic models, a second distance measurement model is constructed according to the relative positions of each first-layer reference node and each second-layer node; Based on the first distance measurement model, the first distance measurement model and the broadband signal, a relay node communication network is constructed, and the location information of the main maritime unmanned system is broadcast to the first layer reference node; Based on the first distance measurement model, according to the position information of the main unmanned marine system, the speed of sound at adjacent measurement moments and the first time delay, a first error correction position of the first layer reference node is obtained, and the first error correction position is broadcast to the second layer nodes; Based on the second distance measurement model, the second error-corrected position of the second-layer node is obtained according to the error-corrected position of the first-layer reference node, the speed of sound, and the second time delay; wherein, The steps of selecting the optimal reference node combination from the maritime unmanned system using the particle swarm algorithm include: Determine the maximum communication measurement distance, the maximum number of communication links between the maritime unmanned system movement area and the first-layer reference nodes; Based on the maximum communication measurement distance, the maximum number of communication links between the maritime unmanned system movement area and the first-layer reference node, the reference node dispersion and physical measurement error are obtained; The objective function of the particle swarm algorithm is obtained by linearly weighted summing the dispersion of reference nodes and the physical measurement error; Based on the initial position information of the maritime unmanned system cluster, obtain the ranging information of each maritime unmanned system; Based on the distance information and objective function, the particle swarm algorithm is used to calculate the particle fitness, individual optimal solution and global optimal solution, and update the particle state; An adaptive mutation mechanism is introduced to update the particle state for a second time according to the probability principle, and all unmanned systems at sea are traversed to obtain the optimal reference node combination.

2. The collaborative navigation and positioning method for multiple unmanned marine systems based on hierarchical relay nodes according to claim 1 is characterized in that: The expression of the kinematic model is: in, is the number of unmanned systems at sea, The serial number of the unmanned maritime system. It represents the main unmanned marine system, and the rest are slave unmanned marine systems. For the The horizontal axis of the unmanned maritime system, For the The vertical coordinate of the maritime unmanned system, For the The yaw angle of the unmanned maritime system, For the The horizontal axis of the maritime unmanned system, For the The vertical coordinate of the unmanned maritime system, For the The yaw angle of the unmanned maritime system, For the The forward composite velocity of the maritime unmanned system, For the The yaw rate of the unmanned marine system, is the sampling period.

3. The collaborative navigation and positioning method for multiple unmanned marine systems based on hierarchical relay nodes according to claim 2 is characterized in that: The expression of reference node dispersion is: In the formula, is the maximum effective area, is the number of reference nodes in the first layer; Physical measurement error The expression is: In the formula, is the error between the first layer reference node and the second layer node on the horizontal axis, is the error between the first layer reference node and the second layer node on the ordinate, is the ranging variance, is the difference between the distances of the first-layer reference node with sequence number 2 and the first-layer reference node with sequence number 1 from the second-layer node in the optimal reference node combination, The difference between the distances of the first-layer reference node with sequence number 3 and the first-layer reference node with sequence number 1 from the second-layer node in the optimal reference node combination; is the first diagonal element, is the second diagonal element; Objective Function The expression is: In the formula, For maximum and minimum normalization processing, is the first weight coefficient, is the second weight coefficient.

4. The collaborative navigation and positioning method for multiple unmanned marine systems based on hierarchical relay nodes according to claim 3 is characterized in that: The steps of introducing the adaptive mutation mechanism and performing secondary updates on the particle state according to the probability principle include: When the mutation rate When , the particle state performs a secondary update of the position, which is expressed as follows: in, for A random number between is the lower bound of the particle search range, is the upper bound of the particle search range, The spatial dimension takes values ​​1 to 2, is the number of reference nodes in the first layer, is the round-up symbol, is the position of the particle after the adaptive mutation mechanism.

5. The collaborative navigation and positioning method for multiple unmanned marine systems based on hierarchical relay nodes according to claim 4 is characterized in that: Based on the respective kinematic models and according to the relative positions of the main unmanned marine system and each first-layer reference node, the step of constructing the first distance measurement model includes: Based on their respective kinematic models, a first distance measurement model is established by calculating the relative positions between the master unmanned marine system and the slave unmanned marine system; wherein the input of the first distance measurement model is: in, is the speed measurement value, is the yaw angular velocity measurement value, represents the first Gaussian white noise with zero mean, represents the second Gaussian white noise with a mean of zero; The variance matrix is , the noise covariance is: in, is the speed measurement noise variance, is the angle measurement noise variance; when When , is the first-layer reference node in the slave maritime unmanned system, the expression of the first distance measurement model between the master maritime unmanned system and the first-layer reference node is: in, is the distance between the first-layer reference node and the main maritime unmanned system, is the total number of reference nodes in the first layer; and , is the speed of sound, is the first delay between the first-layer reference node and the master node of the maritime unmanned system, is the state transfer function, To measure the noise, is the horizontal coordinate of the main maritime unmanned system, is the ordinate of the main maritime unmanned system; when When , it is the second-layer node in the unmanned maritime system, then the expression of the second distance measurement model is: in, For the The second layer nodes and The distance between the first-layer reference nodes; in, For the The second layer nodes and The second delay between the first-layer reference nodes.

6. The collaborative navigation and positioning method for multiple unmanned marine systems based on hierarchical relay nodes according to claim 5 is characterized in that: The relay node communication network includes: a first communication link between the main unmanned marine system and each first-layer reference node, and a second communication link between each first-layer reference node and each second-layer node; wherein, The main unmanned marine system broadcasts its position information to each first-layer reference node via a first communication link, and each first-layer reference node broadcasts its first error-corrected position to each second-layer node via a second communication link.

7. The collaborative navigation and positioning method for multiple unmanned marine systems based on hierarchical relay nodes according to claim 6 is characterized in that: The step of obtaining a first error correction position of a first-layer reference node based on the first distance measurement model and according to the position information of the main unmanned marine system, the speed of sound at adjacent measurement moments, and the first time delay comprises: Based on the first distance measurement model, the distance between the first layer reference node and the main unmanned marine system is obtained according to the sound speed and time delay at adjacent measurement moments; According to the position information of the main marine unmanned system and the distance between the first-layer reference node and the main marine unmanned system, a first error-corrected position of the first-layer reference node is obtained.

8. The collaborative navigation and positioning method for multiple unmanned marine systems based on hierarchical relay nodes according to claim 7 is characterized in that: The step of obtaining a second error-corrected position of a second-layer node based on the second distance measurement model according to the error-corrected position of the first-layer reference node, the speed of sound, and the second time delay includes: Based on the second distance measurement model, the distance between the second layer reference node and the second layer node is obtained according to the speed of sound and the second time delay; According to the error-corrected position of the first-layer reference node, the distance between the second-layer reference node and the second-layer node, a second error-corrected position of the second-layer node is obtained.

Images