Collaborative navigation positioning method for multiple offshore unmanned systems based on layered relay nodes

Abstract

The invention belongs to the technical field of underwater navigation positioning. The invention provides a collaborative navigation and positioning method for multiple offshore unmanned systems based on layered relay nodes. According to the embodiment of the invention, the method comprises the steps: carrying out the hierarchical division of a marine unmanned system, selecting an optimal reference node combination through employing an improved particle swarm algorithm, and obtaining a first layer of reference nodes and a second layer of nodes; constructing a first distance measurement model and a second distance measurement model according to the first layer of reference nodes and the second layer of nodes, and constructing a relay node communication network of the whole cluster by combining broadband signals; and calculating a first error correction position and a second error correction position to realize collaborative navigation positioning of the offshore unmanned system cluster. The problems that due to the fact that the number of the slave offshore unmanned systems is increased, communication burden of the master offshore unmanned system is serious, communication network connectivity is poor, system observability is weak, the utilization rate of cluster position information is low, positioning errors of the slave offshore unmanned systems cannot be corrected, and cluster collaborative navigation positioning fails can be solved.

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 is intended to provide background or context for the technical solutions of the present disclosure stated in the claims. The description herein is not admitted to be prior art by virtue of being included in this section. Summary of the invention

[0007] The purpose of the embodiments of the present disclosure is to provide a collaborative navigation and positioning method for multiple maritime unmanned systems based on hierarchical relay nodes, thereby overcoming one or more problems caused by the limitations and defects of related technologies at least to a certain extent.

[0008] According to an embodiment of the present disclosure, a collaborative navigation and positioning method for multiple unmanned maritime systems based on hierarchical relay nodes is provided, the method comprising: 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.

[0009] Furthermore, the expression of the kinematic model is:

[0010] 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 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 forward composite velocity of the maritime unmanned system, For the The yaw rate of the unmanned marine system, is the sampling period.

[0011] Furthermore, the step of selecting the optimal reference node combination from the marine unmanned system using the particle swarm algorithm includes: 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 reference node dispersion and 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.

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

[0013] In the formula, is the maximum effective area, is the number of reference nodes in the first layer; Physical measurement error The expression is:

[0014] 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:

[0015] In the formula, For maximum and minimum normalization processing, is the first weight coefficient, is the second weight coefficient.

[0016] Furthermore, an adaptive mutation mechanism is introduced to perform a second update of the particle state according to the probability principle, including: When the mutation rate When , the particle state performs a secondary update of the position, which is expressed as follows:

[0017] 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.

[0018] Furthermore, 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 a 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:

[0019] 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:

[0020] in, is the speed measurement noise variance, is the angle measurement noise variance; The expression of the first distance measurement model between the main maritime unmanned system and the first-layer reference node is:

[0021] 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, The vertical coordinate of the main maritime unmanned system.

[0022] Furthermore, the expression of the second distance measurement model is:

[0023] in, For the The second layer nodes and The distance between the first-layer reference nodes;

[0024] in, For the The second layer nodes and The second delay between the first-layer reference nodes.

[0025] Further, 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.

[0026] Further, 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, the step of obtaining the first error correction position of the first layer reference node includes: 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.

[0027] Further, 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, the step of obtaining the second error-corrected position of the second-layer node 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.

[0028] The technical solution provided by the embodiments of the present disclosure may have the following beneficial effects: In an embodiment of the present disclosure, through the above-mentioned collaborative navigation and positioning method of multiple maritime unmanned systems based on hierarchical relay nodes, on the one hand, by hierarchically dividing the slave maritime unmanned systems and selecting the optimal reference node combination using the improved particle swarm algorithm, the first-layer reference nodes and the second-layer nodes are obtained; a first distance measurement model is constructed according to the relative position of the main maritime unmanned system and the first-layer reference nodes; a second distance measurement model is constructed according to the relative position of the first-layer reference nodes and the second-layer nodes; the construction of the relay node communication network of the overall cluster is realized through the first distance measurement model, the second distance measurement model and the broadband signal; the first error correction position of the first-layer reference node and the second error correction position of the second-layer node are calculated using the first distance measurement model and the second distance measurement model to realize the collaborative navigation and positioning of the maritime unmanned system cluster. On the other hand, it solves the problems of serious communication burden on the main maritime unmanned system, poor communication network connectivity, weak system observability, and low utilization of cluster position information caused by the increase in the number of slave maritime unmanned systems, which leads to the inability to correct the positioning error of the slave maritime unmanned system and the failure of cluster collaborative navigation and positioning. It provides a set of robust collaborative navigation and positioning solutions for the master-slave collaborative navigation system of multiple maritime unmanned systems, thereby improving the success rate of cluster mission execution of maritime unmanned systems. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] The accompanying drawings herein are incorporated into the specification and constitute a part of the specification, illustrate embodiments consistent with the present disclosure, and together with the specification are used to explain the principles of the present disclosure. Obviously, the accompanying drawings described below are only some embodiments of the present disclosure, and for ordinary technicians in this field, other accompanying drawings can be obtained based on these accompanying drawings without creative work.

[0030] Figure 1 A diagram showing the steps of a collaborative navigation and positioning method for multiple unmanned maritime systems based on hierarchical relay nodes in an exemplary embodiment of the present disclosure; Figure 2 A schematic diagram showing a hierarchical relay node communication network in an exemplary embodiment of the present disclosure; Figure 3 A flowchart of selecting the first layer reference nodes based on the particle swarm algorithm in an exemplary embodiment of the present disclosure is shown; Figure 4 A schematic diagram showing the positioning of the second layer nodes in an exemplary embodiment of the present disclosure is shown; Figure 5 A diagram showing an iterative process of selecting first-layer reference nodes based on a particle swarm algorithm in an exemplary embodiment of the present disclosure; Figure 6 A performance analysis diagram of second-layer node positioning when the mutation probability is 0.2 in an exemplary embodiment of the present disclosure is shown; Figure 7 A performance analysis diagram of second-layer node positioning when the mutation probability is 0.4 in an exemplary embodiment of the present disclosure is shown; Figure 8 A performance analysis diagram of second-layer node positioning when the mutation probability is 0.6 in an exemplary embodiment of the present disclosure is shown; Fig. 9 A performance analysis diagram of second-layer node positioning when the mutation probability is 0.8 in an exemplary embodiment of the present disclosure is shown. DETAILED DESCRIPTION

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

[0032] 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 figures represent the same or similar parts, and their repeated descriptions will be omitted. Some of the block diagrams shown in the accompanying drawings are functional entities and do not necessarily correspond to physically or logically independent entities.

[0033] This example implementation provides a collaborative navigation and positioning method for multiple unmanned marine systems based on hierarchical relay nodes. Figure 1 As shown in , the collaborative navigation and positioning method for multiple maritime unmanned systems based on hierarchical relay nodes may include: steps S101 to S107.

[0034] Step S101: constructing respective kinematic models based on the state information of the marine unmanned system cluster; wherein the marine unmanned system cluster includes a master marine unmanned system and a plurality of slave marine unmanned systems; Step S102: using a particle swarm algorithm to select an optimal reference node combination from the unmanned maritime systems; wherein the optimal reference node combination includes at least three unmanned maritime systems as first-layer reference nodes, and the remaining unmanned maritime systems as second-layer nodes; Step S103: constructing a first distance measurement model based on the respective kinematic models and according to the relative positions of the main unmanned marine system and each first-layer reference node; Step S104: constructing a second distance measurement model based on the respective kinematic models and the relative positions of the first-layer reference nodes and the second-layer nodes; Step S105: constructing a relay node communication network based on the first distance measurement model, the first distance measurement model and the broadband signal, and broadcasting the location information of the main maritime unmanned system to the first-layer reference node; Step S106: 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 times 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; Step S107: 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.

[0035] Through the above-mentioned collaborative navigation and positioning method of multiple marine unmanned systems based on hierarchical relay nodes, on the one hand, by hierarchically dividing the slave marine unmanned systems and selecting the optimal reference node combination using the improved particle swarm algorithm, the first layer reference nodes and the second layer nodes are obtained; the first distance measurement model is constructed according to the relative position of the master marine unmanned system and the first layer reference node; the second distance measurement model is constructed according to the relative position of the first layer reference node and the second layer node; the construction of the relay node communication network of the overall cluster is realized through the first distance measurement model, the second distance measurement model and the broadband signal; the first error correction position of the first layer reference node and the second error correction position of the second layer node are calculated using the first distance measurement model and the second distance measurement model to realize the collaborative navigation and positioning of the marine unmanned system cluster. On the other hand, it solves the problems of the failure of the slave marine unmanned system positioning error to be corrected due to the increase in the number of slave marine unmanned systems, the poor connectivity of the communication network, the weak observability of the system, and the low utilization rate of the cluster position information, and the cluster collaborative navigation and positioning failure; it provides a set of robust collaborative navigation and positioning solutions for the master-slave collaborative navigation system of multiple marine unmanned systems, and improves the success rate of the cluster task execution of the marine unmanned system.

[0036] Next, we will refer to Figures 1 to 9 The various steps of the collaborative navigation and positioning method for multiple unmanned maritime systems based on hierarchical relay nodes in this example implementation are described in more detail.

[0037] In step S101, the present invention only considers the overall performance of the system and treats the offshore unmanned system as a spatial node, without considering the influence of shape, pitch angle, etc., uses a depth meter to measure its depth, and establishes a two-dimensional kinematic model of the offshore unmanned system through coordinate transformation.

[0038]

[0039] in, and is the two-dimensional coordinate of the unmanned marine system, represents the yaw angle, The number of unmanned systems at sea. When the value is 0, it represents the main unmanned system at sea. When the value is not 0, it represents the slave unmanned system at sea. is the sampling period, is the forward synthesis speed, is the yaw angular velocity.

[0040] 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 the adaptive mutation mechanism, updating the particle state twice according to the probability principle, and improving the traversal and optimization capabilities of the particle swarm. The above operation has the effect of: the optimal first-layer reference node can be selected from the marine unmanned system (i.e., the first-layer reference node) in a variety of combinations of marine unmanned systems.

[0041] (1) The constraints include: reference node dispersion, maximum communication measurement distance and physical measurement error. The parameter settings of the three constraints are not only the content of optimization initialization, but also affect the final positioning accuracy. The setting principles and meanings can be subdivided into: Reference node dispersion : With the first-layer reference node as the center and the longest node transmission distance as the radius, determine the maximum effective area that can be covered. The reference nodes are required to be more dispersed, that is, the discreteness is larger, so as to ensure that the movement area of ​​the unmanned system at sea can be effectively covered. It is expressed as:

[0042] in, is the area of ​​the maximum effective region, is the number of reference nodes in the first layer.

[0043] Maximum communication measurement distance: There is relative movement between nodes, which causes the relative distance to change continuously. Adding the maximum communication distance constraint condition ensures the connectivity of the communication network.

[0044] Physical measurement error : When solving the positioning distance, even if the multipath effect is ignored, the error caused by physical measurement cannot be avoided, which is proportional to the sum of the squares of the distance differences from the second-layer nodes to the first-layer reference nodes.

[0045]

[0046] in, Determined by the ranging error variance, , , is the optimal reference node combination with sequence number The distance between the first layer reference node and the second layer node, is the distance between the first-layer reference node with sequence number 1 in the optimal reference node combination and the second-layer node to be located from the unmanned system at sea, and It can be obtained by solving the ranging equation by the least squares algorithm and matrix transformation, which is specifically explained as follows:

[0047] Through matrix transformation and least squares, we can get , and for Matrix diagonal elements.

[0048] Considering the above constraints, and Min-Max Normalization (MMN) is introduced to achieve data standardization. The processing method is as follows:

[0049] in, is the maximum value of the sample data, For the minimum value of sample data, linear weighting is used to establish a new fitness function (i.e., objective function) , and minimize it; among them,

[0050] (2) Adaptive mutation mechanism is used to solve the problem of slow convergence speed when searching for the first-layer reference node and the local optimal solution. The particle dimensional state is updated twice according to the probability principle to improve the traversal ability, convergence speed and optimization ability of the particle swarm. When , the particle state performs a secondary update of the position, which is expressed as follows:

[0051] in, and are the upper and lower bounds of the particle search range, The spatial dimension takes values ​​1 to 2, is the number of unmanned systems at sea from the first layer of reference nodes, is the round-up symbol, is the position of the particle after the adaptive mutation mechanism, by setting Size, can randomly change a particle's position in a certain dimension, increase It will increase the frequency of mutation operations and reduce This will reduce the frequency of mutation operations.

[0052] In step S103, the first distance measurement model is constructed by combining the marine unmanned system motion model (i.e., kinematic model) of step S101, and establishing the first distance measurement model by the relative positions between the master and slave marine unmanned systems. Gaussian white noise is used as the noise, and the system input model is as follows:

[0053] in, is the speed measurement value, is the measured value of yaw angular velocity; and is a Gaussian white noise with a mean of zero and a variance matrix of , the noise covariance is expressed as:

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

[0055] in, is the distance between the first-layer reference node and the main node maritime unmanned system, is the total number of first-layer reference nodes from unmanned maritime systems, Solve using the speed of sound and the first delay , interpreted as:

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

[0057] in, is the distance between the second-layer node and the first-layer reference node from the unmanned system at sea, which is calculated by the speed of sound With the second delay Obtained, interpreted as:

[0058] in, For the second layer The node and the first layer reference node The first delay between unmanned systems at sea.

[0059] In step S105, the relay node communication network of the maritime unmanned system cluster undertakes the task of transmitting information such as 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 node to complete the transmission of information 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 reference slave maritime unmanned system is damaged, other reference slave maritime unmanned systems of the first layer can continue to complete the information transmission and the positioning requirements of the second-layer slave maritime unmanned system. The specific working principle of the relay node communication network is the existing technology and will not be described in detail here.

[0060] In step S106, the positioning process of the unmanned marine system for the first-layer reference node is as follows: the sound speed and time delay at adjacent measurement moments are used to obtain the distance from the main unmanned marine system. After using the odometer and obtaining its own speed measurement, the observability analysis of the single main boat system based on Lie derivatives has explained this situation. The position estimation and error correction are completed through the distance measurement model and the least squares or filtering algorithm.

[0061] In step S107, the number of reference nodes in the first layer is not less than three, a two-dimensional motion model is established for the unmanned marine system, and position estimation and error correction are completed through the distance measurement model and the least squares or filtering algorithm. When it is necessary to locate a certain unmanned marine system, the level of the unmanned marine system is first determined, and the navigation information, identification number, waiting time, and transmission delay information of the positioning signal are received through the communication network.

[0062] In one embodiment, the nodes of the slave maritime unmanned system are divided into the first-layer reference node slave maritime unmanned system (the number is not less than 3) and the second-layer node slave maritime unmanned system (i.e., the second-layer node). The main maritime unmanned system is equipped with a high-precision INS to estimate its own position and corresponding motion state, the speedometer measures its own speed, and the hydroacoustic communicator is used to exchange command data with the first-layer reference node slave maritime unmanned system; the data processing module is used to output its own position and generate collaborative positioning messages; the slave maritime unmanned system is equipped with a low-precision INS to output its own motion state and position information, the hydroacoustic communicator is used for data interaction and message reception, and the data processing module is used to fuse positioning data. The cluster realizes information interaction through the relay node communication network, suppresses the accumulation of INS positioning errors, realizes the correction of its own errors, and then completes the cluster's task allocation, formation control, collaborative navigation and positioning and other functions.

[0063] In one embodiment, in combination Figure 2 and Figure 3 , explaining the hierarchical relay node communication network and the first-layer reference node selection process. The first-layer reference node is selected from the unmanned marine system. Figure 3The flowchart shown selects the optimal combination. The first-layer reference node, as a relay node, establishes information interaction between the main maritime unmanned system and the slave maritime unmanned system, which can not only share the communication burden for the main maritime unmanned system, but also play the role of a base station to provide high-precision positioning for the slave maritime unmanned system of the second-layer node. At first, the initial position of the maritime unmanned system cluster floating on the water surface is recorded. After diving, the first-layer reference node is selected according to the motion characteristics of the maritime unmanned system and the fitness function and boundary conditions established in step S102, and the information interaction and first distance measurement model between the main maritime unmanned system and the first-layer reference node are established. The relative position of the main maritime unmanned system to the first-layer reference node from the maritime unmanned system is obtained by using adjacent measurement moments, and the first-layer reference node from the maritime unmanned system is subsequently coordinated with the maritime unmanned system using INS and periodic correction information. Since the motion models between maritime unmanned systems are the same, the changes in their motion heading and speed with the increase of flight time are negligible, and the relative motion speed between maritime unmanned systems can be considered to be 0. The first-layer reference node can be regarded as a fixed base station, and a second distance measurement model with the second-layer node from the maritime unmanned system is established, thereby realizing the positioning of the second-layer node from the maritime unmanned system, and subsequently realizing information exchange to the main maritime unmanned system through the communication network.

[0064] Figure 3 The flowchart for selecting the first-layer reference nodes based on the particle swarm algorithm is as follows: first, the boundary conditions are determined to impose multi-dimensional restrictions on the particles, including the maximum communication measurement distance, the movement area of ​​the maritime unmanned system, and the maximum number of communication links between the first-layer reference nodes and the maritime unmanned system; second, the fitness function is established based on the comprehensive constraint conditions to adjust the node dispersion. Physical measurement error Perform linear weighted summation to obtain the fitness function ; Finally, an adaptive mutation mechanism is introduced to give particles a mutation probability, thereby improving the optimization and convergence capabilities.

[0065] The maximum communication distance takes into account transmission loss, sets a threshold for the received signal power, and comprehensively considers factors such as sound source level intensity, ambient noise intensity, and absorption loss; The movement area of ​​the unmanned marine system is set as a two-dimensional plane; The maximum number of communication links of the first-layer reference node slave maritime unmanned system is set to 4, including the communication link with the master maritime unmanned system, the communication link controlling the three slave maritime unmanned systems, and the number of slave maritime unmanned systems selected as the first-layer reference node in the slave maritime unmanned system cluster; The fitness function is established according to the MMN principle to achieve node decentralization Physical measurement error The magnitude of is unified, and the solution is When the first layer reference node is used as the center of the circle, the maximum communication distance is used as the radius to obtain the maximum effective area The size of; Solving the physical measurement error When the distance is observed from the unmanned maritime system within the maximum communication distance of the first-layer reference node from the unmanned maritime system, the physical measurement errors observed by the first-layer reference node from the unmanned maritime system are accumulated and summed to obtain ; The introduction of the adaptive mutation mechanism optimizes the particle position and velocity update formula. The traditional formula is:

[0066] in, represents the inertia weight, and represents the learning factor, Indicated in Uniformly distributed random values, both independent of each other, For the particle swarm particles. After introducing the mutation probability, it becomes:

[0067] The above two operations are aimed at achieving multi-objective optimization and improving particle optimization capabilities. A variety of optimal combinations can be listed, which can be adjusted and selected according to the actual project. After establishing the first-layer reference node from the unmanned marine system, the communication relationship and distance measurement model between the main unmanned marine system and it are established. According to the communication distance, the distance measurement model and communication relationship between the second-layer node from the unmanned marine system and the first-layer reference node from the unmanned marine system are established to realize the construction of the overall unmanned marine system cluster communication network.

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

[0069] In the formula, The nonlinear measurement equation for the state The Jacobian matrix of For the next moment, the state The Jacobian matrix of is the posterior information matrix, if the position of the main unmanned marine system at adjacent measurement moments is and , the first layer reference node The position of the unmanned system at sea is and And the distance observation and , then the observable matrix can be expressed as:

[0070] When the matrix is ​​full rank, the matrix determinant That is, the coordinates need to change at adjacent measurement moments. Due to the existence of the heading angle, the observability of the system can be guaranteed. The position coordinates of the first-layer reference nodes relative to the main maritime unmanned system can be obtained by solving the observation equation through the least squares or filtering algorithm. Correction information can be periodically transmitted through the communication network to achieve collaborative positioning.

[0071] In one embodiment, Figure 4 A schematic diagram of solving the positioning of the second-layer reference nodes from the position coordinates of the offshore unmanned system based on the first-layer reference nodes is shown. The number of observation equations is greater than the number of coordinate unknowns. The least squares or filtering algorithm can be used to solve the position, and correction information can be periodically transmitted through the communication network to achieve collaborative positioning.

[0072] In a specific embodiment, Figure 5 The iterative process diagram of selecting the first-layer reference nodes based on the particle swarm algorithm under different mutation probabilities is shown. Each maritime unmanned system is equivalent to a single node, and a m area, taking twelve nodes as an example. Three first-layer reference nodes are selected, and the remaining nine are used as the second-layer nodes to be located. For the unmanned marine system, the formula As the fitness function (select ), the number of particles is 50, the number of iterations is 200, and the inertia weight is The value is 0.8, the learning factor , Take 0.5, mutation probability Select 0.2, 0.4, 0.6, and 0.8. When the mutation probability is different, the number of iterations required to achieve the optimal fitness is also different. This is because when the mutation probability is low, it means that the frequency of particle population mutation operations is low, and the range of particle population position changes is relatively small; when the mutation probability is high, it means that the frequency of particle population mutation operations is high, and the range of particle population position changes is relatively large, and a larger traversal space can be searched in a shorter time. Among them, Figure 6 This is the performance analysis diagram of the second-layer node positioning when the mutation probability is 0.2; Figure 7 This is the performance analysis diagram of the second-layer node positioning when the mutation probability is 0.4; Figure 8 This is the performance analysis diagram of the second-layer node positioning when the mutation probability is 0.6; Fig. 9This is a performance analysis diagram for positioning the second-layer nodes when the mutation probability is 0.8.

[0073] Figure 6 , Figure 7 , Figure 8 and Fig. 9 The positioning performance analysis of the second-layer nodes under different mutation probabilities is shown. At the same time, the positioning performance under different mutation probabilities is compared by adding Gaussian white noise with a mean of 0 and a variance of 0.2 to simulate the positioning ranging error during the ranging positioning process. The positioning errors of the nine unmanned maritime systems under the same observation epoch are obtained by observing the unmanned maritime systems of the second-layer nodes to be positioned 300 times, and the weighted summation of the positioning errors is used to obtain the overall positioning performance analysis diagram.

[0074] Through the above-mentioned collaborative navigation and positioning method of multiple marine unmanned systems based on hierarchical relay nodes, on the one hand, by hierarchically dividing the slave marine unmanned systems and selecting the optimal reference node combination using the improved particle swarm algorithm, the first layer reference nodes and the second layer nodes are obtained; the first distance measurement model is constructed according to the relative position of the master marine unmanned system and the first layer reference node; the second distance measurement model is constructed according to the relative position of the first layer reference node and the second layer node; the construction of the relay node communication network of the overall cluster is realized through the first distance measurement model, the second distance measurement model and the broadband signal; the first error correction position of the first layer reference node and the second error correction position of the second layer node are calculated using the first distance measurement model and the second distance measurement model to realize the collaborative navigation and positioning of the marine unmanned system cluster. On the other hand, it solves the problems of the failure of the slave marine unmanned system positioning error to be corrected due to the increase in the number of slave marine unmanned systems, the poor connectivity of the communication network, the weak observability of the system, and the low utilization rate of the cluster position information, and the cluster collaborative navigation and positioning failure; it provides a set of robust collaborative navigation and positioning solutions for the master-slave collaborative navigation system of multiple marine unmanned systems, and improves the success rate of the cluster task execution of the marine unmanned system.

[0075] It should be understood that the terms "center", "longitudinal", "lateral", "length", "width", "thickness", "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "outside", "clockwise", "counterclockwise", etc. in the above description indicate orientations or positional relationships based on the orientations or positional relationships shown in the accompanying drawings, and are only for the convenience of describing the embodiments of the present disclosure and simplifying the description, and do not indicate or imply that the referred device or element must have a specific orientation, be constructed and operated in a specific orientation, and therefore should not be understood as a limitation on the embodiments of the present disclosure.

[0076] In addition, the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of the features. In the description of the embodiments of the present disclosure, the meaning of "multiple" is two or more, unless otherwise clearly and specifically defined.

[0077] In the embodiments of the present disclosure, unless otherwise clearly specified and limited, the terms "installed", "connected", "connected", "fixed" and the like should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection or an indirect connection through an intermediate medium, it can be the internal connection of two elements or the interaction relationship between two elements. For ordinary technicians in this field, the specific meanings of the above terms in the present disclosure can be understood according to specific circumstances.

[0078] In the embodiments of the present disclosure, unless otherwise clearly specified and limited, a first feature being "above" or "below" a second feature may include the first and second features being in direct contact, or may include the first and second features not being in direct contact but being in contact through another feature between them. Moreover, a first feature being "above", "above" and "above" a second feature includes the first feature being directly above and obliquely above the second feature, or simply indicates that the first feature is higher in level than the second feature. A first feature being "below", "below" and "below" a second feature includes the first feature being directly below and obliquely below the second feature, or simply indicates that the first feature is lower in level than the second feature.

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

[0080] Those skilled in the art will readily appreciate other embodiments of the present disclosure after considering the specification and practicing the invention disclosed herein. This application is intended to cover any modification, use or adaptation of the present disclosure, which follows the general principles of the present disclosure and includes common knowledge or customary techniques in the art that are not disclosed in the present disclosure. The specification and examples are intended to be exemplary only, and the true scope and spirit of the present disclosure are indicated 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.

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 unmanned maritime system, For the The yaw angle of the unmanned maritime system, For the The horizontal axis of the unmanned maritime 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 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.

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 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.

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: 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.

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: 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; The expression of the first distance measurement model between the main 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, The vertical coordinate of the main maritime unmanned system.

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 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.

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 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.

9. The collaborative navigation and positioning method for multiple unmanned marine systems based on hierarchical relay nodes according to claim 8, 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.

10. The collaborative navigation and positioning method for multiple unmanned marine systems based on hierarchical relay nodes according to claim 9, 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