USV formation keeping method based on passive positioning under communication limited condition

Abstract

The invention relates to a USV formation keeping method based on passive positioning under a communication limited condition, and belongs to the field of unmanned surface vessel formation. The method comprises the following steps: establishing a circular USV formation positioning model under a passive condition based on a three-point three-angle measurement method, and adopting an improved least square filtering algorithm and introducing a distributed state estimation mechanism to enable USV to realize self-positioning; a leader-follower model and a virtual structure method are combined, a virtual-leader-follower circular formation model is established based on a passive condition, and an overall formation virtual navigation structure is set during USV formation movement to guide formation movement; and a distributed state estimation mechanism is fused into a distributed model prediction control strategy, so that each USV follows the track of the USV corresponding to the USV in the virtual navigation structure, and real-time rolling optimization control is performed on the track of the USV in each time step. According to the invention, the robustness of formation control in a communication limited scene can be enhanced.

Description

Technical Field

[0001] The present invention belongs to the field of unmanned surface vehicle (USV) formations, and particularly relates to a method for maintaining the formation of USVs based on passive positioning under communication constraints. Background Art

[0002] In a complex marine environment, unmanned surface vehicles (USVs) have attracted wide attention due to their advantages such as small size, strong mobility, the ability to perform tasks in dangerous areas without causing casualties, etc. In recent years, unmanned boats have gradually developed towards the field of unmanned boat formations that cooperate to complete complex tasks in complex environments, and the cooperative control of formations has become the focus of research. Since the signal transmission of unmanned boat formations is vulnerable to external environmental interference during rescue missions in disaster areas, and communication may be delayed or interrupted during driving or large-scale mission execution, it is of great significance to study the passive control strategy of formations under communication constraints for improving the cooperative control ability of formations.

[0003] The prerequisite for the coordinated control of USV formations is accurate positioning. Positioning technologies are divided into active positioning and passive positioning. Active positioning locates by emitting signals and receiving reflections, and has the advantages of all-weather and high precision, but external signal interference will reduce the accuracy of its positioning. Passive positioning does not emit signals but only receives the electromagnetic waves radiated by the target, and has the characteristic of strong anti-interference ability, which is suitable for solving the USV positioning problem under communication constraints. The types of formation passive positioning include time difference of arrival (TDOA) positioning method and direction-finding cross-location method. The TDOA positioning method requires precise signals, has a large amount of calculation, and a slow positioning speed, and is not suitable for formation cooperative passive positioning. The TDOA positioning method requires precise signals, a large amount of calculation and a slow positioning speed, so this method is usually not adopted for formation cooperative passive positioning. The multi-platform direction-finding cross-location method is easy to implement and has a fast positioning speed, so it is widely used in formation cooperative control. The above methods can effectively realize the positioning of the relative positions between formation boats, but the direction-finding cross-location model has many classification cases for different position distributions, resulting in relatively complex positioning calculations, and as the formation scale increases, it will lead to an excessive amount of calculation and cause a delay in the formation decision-making process.

[0004] In the field of formation control, formation control methods mainly include the leader-follower method, the behavior method, and the virtual structure method, while trajectory control methods mainly include proportional-integral control (PID) and model predictive control (MPC), etc. In the actual formation-keeping process, it is often necessary to combine the two to ensure that the entire formation can not only maintain a stable relative formation but also accurately follow the predetermined trajectory. Among them, due to the difficulty of introducing constraint conditions in PID control, the situation where the motion does not conform to the ship kinematics principle is likely to occur. Existing methods have achieved the formation of the set formation of the robot formation and the trajectory movement, and have also achieved the formation movement process of multiple robots in different behavioral states in a complex environment. However, the leader-follower model highly depends on the leader, and it will cause problems in formation control when the leader fails. The UAV control strategy based on the virtual structure method, although solving the problem of over-reliance on the leader, under the passive control condition, it is necessary to achieve the coordinated control of the formation according to the motion state of the leader, and at the same time, it is necessary to achieve the dynamic maintenance of the formation when the leader signal fails caused by external interference. Although the virtual structure method can cope with the failure of the leader, there is a problem that it cannot perform dynamic operations according to the leader's instructions.

[0005] Centralized control brings great pressure to the navigator when processing formation information, while distributed model predictive control (DMPC) provides predictive and powerful processing capabilities for multivariable systems. Centralized control causes huge pressure on the leader when processing formation information, while distributed model predictive control (DMPC) has foresight and good processing capabilities for multivariable systems. It is suitable for reducing the information processing and communication pressure in complex formation control tasks. When considering various constraints, it can prevent situations such as the USV movement violating the principles of ship dynamics during the process and can better adapt to the movement of unmanned boats. To improve the dynamic adaptability to the environment and efficient cooperation during formation control, based on distributed model predictive control, the state estimation method can achieve formation dynamic cooperation and adjustment by sharing state information. Applying DMPC to the field of trajectory control can effectively limit the deviation between the predicted trajectory and the actual trajectory. However, this method greatly increases the computational complexity because it needs to send signals from each distributed controller. Applying the DMPC method to the field of robot communication reduces the trajectory tracking error caused by communication interference through the use of DMPC strategies. However, in the existing technology, the number of robot formations is small, and only the effect under weak communication interference has been studied, without studying how the DMPC algorithm copes with the situation of complete communication interruption. Further research has improved the control performance of DMPC and enhanced the stability of vehicle fleet control by introducing a synchronization mechanism under communication delay. However, the situation of communication being disturbed or interrupted has not been considered. The L-BFGS-B algorithm can quickly update model parameters during iteration, which helps to quickly adjust the control input of the unmanned boat in state estimation to adapt to environmental changes. This method may help solve the problems of low computational efficiency and high complexity of DMPC. The distributed state estimation method based on DMPC can help USVs adapt to environmental changes.

[0006] Existing DMPC research mainly focuses on two types of external disturbances: environmental changes and communication disturbances. In terms of communication disturbances, the research mainly focuses on minor disturbances, without considering different degrees of disturbances or communication interruptions, and the attention to relevant constraints is limited.

[0007] In summary, the leader faces great communication pressure and failure risks under communication constraints, resulting in difficult coordination control. Summary of the Invention

[0008] The purpose of the present invention is to provide a method for maintaining the formation of USV based on passive positioning under communication constraints to enhance the robustness of formation control in communication-constrained scenarios, aiming at the problem that the formation control of unmanned surface vehicles (USVs) in maritime rescue operations is unstable due to external disturbances and communication constraints.

[0009] To achieve the above object, the technical solution of the present invention is: A method for maintaining the formation of USV based on passive positioning under communication restricted conditions, including: Establish a circular USV formation positioning model under passive conditions based on the three-point triangulation method, and adopt an improved least squares filtering algorithm, while introducing a distributed state estimation mechanism to enable the USV to achieve its own positioning; Combine the leader-follower model with the virtual structure method to establish a virtual-leader-follower circular formation model under passive conditions, and set an overall formation virtual leader structure during the movement of the USV formation to guide the formation movement; Integrate the distributed state estimation mechanism into the distributed model predictive control strategy, so that each USV follows the trajectory of the USV corresponding to itself in the virtual leader structure, and perform real-time rolling optimization control on its own trajectory at each time step.

[0010] Compared with the prior art, the present invention has the following beneficial effects: (1) Flexibility and robustness: The present invention simplifies the control relationship between the leader and the followers by introducing a virtual leader. The virtual leader does not directly control each follower, but acts as an ideal target. The followers control their own behaviors according to the state of the virtual leader, usually without direct interaction with the leader. Therefore, even in the case of communication loss or interruption between the leader and some followers, the virtual leader-follower method can still ensure the stability and consistency of the formation. In contrast, in the traditional leader-follower method, the followers directly rely on the control commands of the leader, and the state or behavior of the leader directly affects the behaviors of all followers. If the leader fails or the communication is interrupted, the formation may become unstable or unable to maintain the required configuration.

[0011] (2) Improve communication efficiency and reduce costs: In the present invention, the followers usually only need to synchronize with the state of the virtual leader, rather than exchange real-time information with all other members including the leader. This reduces the communication complexity and bandwidth consumption within the system, which is particularly important for large-scale systems such as UAV formations or autonomous vehicle formations. In the traditional leader-follower method, each follower needs to receive real-time commands or state information from the leader, which will result in a heavy communication burden. As the system scale expands, the communication requirements increase exponentially, which may lead to delays or data transmission bottlenecks.

[0012] (3) Fault tolerance: In the present invention, since the virtual leader does not depend on a physically existing leader, it has higher fault tolerance. Even if the actual leader fails or is unable to continue performing tasks, the virtual leader can still maintain the stability of the formation for a period of time. In contrast, in the traditional leader-follower method, if the leader fails or malfunctions, the stability of the entire formation may be affected, especially in the absence of a backup leader or the inability to replace the leader, which may lead to the loss of control or dissolution of the formation. BRIEF DESCRIPTION OF THE DRAWINGS

[0013] Figure 1 It is a flowchart of the USV formation shape maintenance method of the present invention.

[0014] Figure 2 It is a circular formation model.

[0015] Figure 3 It is a schematic diagram of constructing a circumcircle.

[0016] Figure 4 It is a schematic diagram of calculating the standard equation of the first circumcircle.

[0017] Figure 5 It is a schematic diagram of calculating the standard equation of the second circumcircle.

[0018] Figure 6 It is a schematic diagram of the process of improving the least squares method.

[0019] Figure 7 It is a schematic diagram of the overall optimization process of MPC.

[0020] Figure 8 It is the formation motion state. DETAILED DESCRIPTION OF THE INVENTION

[0021] The technical solution of the present invention will be specifically described below in conjunction with the accompanying drawings.

[0022] The present invention provides a method for maintaining the formation shape of USVs based on passive positioning under communication constraints, including: Establish a circular USV formation positioning model under passive conditions based on the three-point triangulation method, and adopt an improved least squares filtering algorithm, and at the same time introduce a distributed state estimation mechanism to enable the USV to achieve its own positioning; Combine the leader-follower model with the virtual structure method, establish a virtual-leader-follower circular formation model under passive conditions, and set an overall formation virtual navigation structure during the movement of the USV formation to guide the formation movement; Incorporate the distributed state estimation mechanism into the distributed model predictive control strategy to enable each USV to follow the trajectory of the USV corresponding to itself in the virtual navigation structure and perform real-time rolling optimization control on its own trajectory at each time step.

[0023] The following is the specific implementation process of the present invention.

[0024] As Figure 1 shown, a method for maintaining the formation of USV formation based on passive positioning under communication-limited conditions of the present invention includes three steps: The first step is to establish a positioning model for each USV in the USV circular formation based on the angle between boats under this condition considering the influence of the passive environment, and considering the influence of environmental noise factors on the positioning accuracy. The least squares filtering algorithm is improved by setting a noise threshold to exclude abnormal noise data, and a distributed state estimation mechanism is introduced to enable the USV to achieve its own positioning. This model can help the USV achieve accurate positioning of the relative position between itself and the central USV.

[0025] After achieving precise positioning, the second step is to solve the problem of formation stability during the movement of the formation. On the basis of considering external noise interference, the external interference condition of more serious communication interruption is additionally introduced. Utilizing the advantages of the leader-follower method and the virtual leader method, a virtual-leader-follower model is established under passive conditions. When the USV formation is moving, a virtual leader structure of the overall formation is set to guide the formation movement, which can effectively prevent the situation of leader failure caused by communication interruption in the problem of maintaining the USV formation.

[0026] The third step is to introduce a distributed state prediction mechanism into the MPC. Each USV uses the MPC controller separately, follows the trajectory of the USV corresponding to itself in the virtual leader structure, and performs real-time rolling optimization control on its own trajectory at each time step. At the same time, under the noise interference and external interference of communication interruption introduced in the second step, the communication interruption time is increased, and the initial position error is set during initialization to further improve the trajectory stability of the USV formation tracking in four motion states and shorten the convergence speed of returning to the ideal position, realizing accurate tracking and maintenance of the USV formation under passive conditions.

[0027] 1. Circular USV formation positioning model Common formation shapes in the USV formation include linear, rectangular, V-shaped, and circular, etc. The circular formation can effectively disperse the mutual interference between individuals, enhance the overall stability of the formation, and is also convenient for expanding and contracting the formation scale. Considering the advantages of the circular formation in dynamic adjustment and space utilization, the present invention selects the circular formation as the ideal formation shape of the USV, selects the common number of 9 boats, where 8 boats are evenly distributed on the circumference as follower USVs, and 1 unmanned boat is located at the center as the central USV; the angle is positive in the counterclockwise direction, and 0° coincides with the positive direction of the X-axis.

[0028] 1.1 Central USV: The overall movement of the formation is led by the central USV, and the positions and motion states of the remaining unmanned boats will be dynamically adjusted according to the motion of the central USV. The initial position of the central USV is set as , and the motion speed of the central USV is a constant vector , where is the speed of the central USV in the X-axis direction, is the speed of the central USV in the Y-axis direction. Then the position of the central USV at time step k is expressed as the following formula: is the position of the central USV at time step k - 1, is the discrete time step between two time steps; 1.2 Follower USV: The ideal position of each follower USV remains unchanged relative to the position of the central USV, as shown in the following formula: is the relative ideal position of the i-th follower USV in the formation at time step k, is expressed as: Taking the central USV as the origin O, a rectangular coordinate system is established, is the angle between the i-th follower USV and the central USV. In the ideal state, the positions of all follower USVs in the formation are evenly distributed on a circular trajectory with the central USV as the center and R as the radius.

[0029] 1.3 Virtual-Leader-Follower Circular Formation Model Based on the characteristics of the leader-follower model and the virtual structure method, the leader-follower model is adopted in formation control, led by the central USV, and the followers maintain their relative positions with the leader. When predicting the trajectory, a virtual formation model is established based on the real-time motion state of the central USV to predict the trajectories and positions of future formation members. When tracking the trajectory, each USV only follows the future positions predicted by the virtual formation, and the future positions of each follower USV also form a virtual image; this method can effectively overcome the problem of the leader's failure during the motion process.

[0030] To predict the trajectory, a virtual formation and navigator model is established based on the real-time motion state of the central USV. This model predicts the future trajectories and positions of the members of the formation through the MPC control method. In the event of interference or signal interruption, the USV members can predict their future trajectories and positions through MPC. When tracking the trajectory, each USV only follows its own future position predicted by the virtual formation, and the future positions of each USV itself also form a virtual formation. The virtual overall formation can adjust the positions of the virtual navigators through the virtual structure method, that is, maintaining the relative distance and azimuth angle between the members of the virtual formation and the virtual central USV. This method alleviates the problem of being unable to receive information commands and make new responses to motion when the signal is not smooth, effectively solves the problem of the navigator failure during the motion process, and ensures the continuity of formation control.

[0031] The key geometric features of the circular formation include the formation radius and the number of USVs. Assuming the formation radius is R and the number of follower USVs is N, the ideal position of each USV in the circular formation can be determined by the uniform distribution of the circumferential angle, and the angular distribution of the ideal position is: is the angular position of the i-th follower USV in the circular formation.

[0032] As Figure 2 shown, in this example, N is 8, that is, 8 USVs are evenly distributed on the circumference as follower USVs, and 1 unmanned boat is located at the center as the central USV. The unmanned boat at the center of the circle, that is, the central USV, is numbered , and the numbers of the other two unmanned boats that emit signals and the follower USVs are and . Let the number of the unmanned boat that receives the signal, that is, the follower USV, be , and the error position is defined as . Let the horizontal line where is located in the circular formation be the X-axis, and the line where is located be the Y-axis. In the case of no error, the angle between adjacent unmanned boats on the circumference is 45°. The angle difference between each unmanned boat is , where K is the minimum number of unmanned boats between two unmanned boats, satisfying 0 ≤ K ≤ 3.

[0033] 1.4 Unmanned Boat Power Model In the present invention, each USV adopts a discrete-time double integrator model. Considering the changes in position and velocity, the state vector of the i-th USV at time k is defined as: Where: and respectively represent the positions of the $i$-th follower USV in the $X$-axis and $Y$-axis directions at time step $k$; and respectively represent the velocity components of the $i$-th follower USV in the $X$-axis and $Y$-axis directions at time step $k$; The state update equation of the system dynamics model is as follows: The state transition matrix $A$ and the control input matrix $B$ are expressed as follows: The state transition matrix $A$ represents the transition from the current state to the state at the next moment; is the system state at time step $k$, represents the system state at the next time step $k + 1$. The control input matrix $B$ represents the influence of external inputs on the state change of the system. The acceleration state variable , respectively represent the accelerations of the $i$-th follower USV in the $X$-axis and $Y$-axis directions at time step $k$. Among them, the motion of the central USV runs through the pre-set state information. The speed and acceleration parameters of the follower USVs are optimized according to the distributed model predictive control strategy DMPC to achieve the formation control goal; To ensure the motion safety of the USV and comply with physical limitations, the following speed and acceleration limit constraints are set: where $k$ is the time step, is the heading angle of the $i$-th follower USV at time step $k$, is the speed state variable of the $i$-th follower USV at time step $k$, is the acceleration state variable of the $i$-th follower USV at time step $k$, is the coordinate of the $i$-th follower USV in the $X$-axis direction in the coordinate system at time step $k$, is the coordinate of the $i$-th follower USV in the $Y$-axis direction in the coordinate system at time step $k$, is the ideal position coordinate of the $i$-th follower USV in the $X$-axis direction in the coordinate system, is the ideal position coordinate of the $i$-th follower USV in the $Y$-axis direction in the coordinate system, is the heading angle of the follower USV in the ideal state, is the maximum speed of the USV set during the motion, is the acceleration value of the follower USV in the ideal state, is the maximum acceleration of the USV set during the motion, is the speed value of the follower USV in the ideal state, The maximum speed of the set USV during movement; During the actual control process, if the speed or acceleration of the USV exceeds the limit, the actual speed of the USV will be limited to the maximum speed to ensure that its speed and acceleration are always within the allowable range, and the acceleration and deceleration will be adjusted according to the actual trajectory tracking effect.

[0034] 2. Establishment of USV positioning model 2.1 Model assumptions It is assumed that the USV signal is emitted by the central USV and any two follower USVs within the circumference. The ideal position and trajectory are determined by the circles where the three USVs are located. Other USVs may have position deviations or no deviations, and only the angular azimuth information between the USV receiving the signal and the three USVs emitting the signal is known. On this premise, a relative position positioning model of the USV receiving the signal is established; 2.2 Model establishment Based on trigonometric functions and the circumferential angle theorem in the rectangular coordinate system, the present invention proposes a three-station passive positioning mathematical model. By establishing two circumscribed circles of the ideal circle, the relative position coordinates of the USV receiving the signal are determined; specifically as follows: As Figures 3 - 5 shown, construct two circumscribed circles of the ideal circular formation ; is the number of the central USV, and are the numbers of any two follower USVs emitting signals, and the number of the USV of the formation member to be positioned is defined as , then the first circumscribed circle is the circle composed of and , and the center of the circle is . According to the circumferential angle theorem, based on the length between , connect the points on the circumference , calculate the radius of the first circumscribed circle, and thus calculate the coordinates of the center of the first circumscribed circle, that is, the position coordinates of ; the second circumscribed circle is the circumscribed circle composed of and , and the center of the circle is . is a follower USV number different from and . According to the relationship between and , the radius of the second circumscribed circle is obtained by the sine theorem, and thus the coordinates of the center of the second circumscribed circle are calculated, that is, the position of ; the specific solution process is as follows: Let and The length between is , and the corresponding central angle is , according to the double - angle property of the inscribed - angle theorem, in the same circle, the central angle subtended by the same arc is equal to the inscribed angle That is twice of: For and cases, calculate the radius of the first circumcircle and the length of : Let the center coordinates of the first circumcircle be , and the center equation of the first circumcircle and the standard equation of the circumcircle are as follows: According to the second circumcircle, find the radius of the second circumcircle. According to the median - line theorem, for the mid - point F of two points and on the circumference, the line connecting it with the center is perpendicular to and bisects the line connecting them . Based on the symmetry of the circle and the median - line theorem, it can be known that ; Since , so , . According to the sine theorem: Let the center coordinates of the second circumcircle be , and the center equation of the second circumcircle and the standard equation of the circumcircle are as follows: By simultaneously solving the standard equations of the two circumcircles, find the coordinates of the two intersection points. One of them is the position information of the follower USV, that is , and the other intersection point is the coordinate information of the USV to be located with or without position deviation.

[0035] 2.3 Improved least - squares filtering algorithm The Least Squares (LS) is a statistical method for estimating target parameters (such as position) by minimizing the squared error between the observed values and the model predicted values. In the present invention, LS is used to estimate the two-dimensional relative position of the target based on the noisy relative angle measurement data between boats obtained from three USVs.

[0036] The nonlinear least squares optimization algorithm (Least Squares, Adaptibe LS) is used to solve for the minimum of the sum of all squared residuals. To address the problem that the LS method is sensitive to outliers, the present invention proposes an adaptive least squares method (Adaptive LS) under passive positioning conditions. This method uses an adaptive threshold filtering mechanism to dynamically identify and remove abnormal residuals, thereby enhancing the resistance to outliers.

[0037] As Figure 6 shown, the adaptive least squares method introduces an adaptive threshold filtering step on the basis of the traditional least squares method that uses a fixed threshold, dynamically adjusts the threshold to identify and remove abnormal residuals. This process determines an adaptive threshold by statistically analyzing the residuals of the initial estimate, treats the measurement points with larger residuals as outliers, and removes them from the estimation process. The overall process is as follows: First, perform an initial estimate. Use the least squares filtering algorithm to perform a preliminary estimate of the target position to obtain an initial estimate value; Secondly, calculate the residuals. Calculate the residuals under the initial estimate, that is, the difference between each angle measurement and the model predicted value; Then determine the adaptive threshold: calculate the median of the residuals 、calculate the median absolute deviation of the residuals , and set the adaptive threshold as , where is the threshold factor, which adjusts the filtering strictness. According to the experimental results, the optimal adjustment interval of is between 1.2 - 1.8, and its value before automatic adjustment is 0. The overall algorithm determines a basic threshold through the median, and then automatically adjusts the value of the threshold factor according to the change in the residual distribution to change the threshold, so that the rejection rate is always controlled within a reasonable range (5% - 15%).

[0038] Next, perform inlier screening. Mark the measurement points with residuals less than the threshold as inliers, and the rest as outliers; Finally, perform a re - estimate. Only use the inlier data to re - perform least squares optimization to obtain the final estimate of the target position, screen out the outliers. If the number of inliers after screening is less than two, then retain the initial estimate result.

[0039] 3 Formation control strategy 3.1 Distributed State Prediction Mechanism The distributed state prediction strategy disperses the prediction task of the global system state to multiple nodes, enabling each node to independently predict the system state. Meanwhile, information is exchanged through communication to obtain an accurate state prediction globally, effectively reducing the computational pressure on a single node and featuring high robustness.

[0040] According to the characteristics of the communication constraint conditions of USVs, the present invention proposes a formation distributed state estimation strategy under communication constraint conditions. The follower independently samples the motion states of the central USV at the nearest and different moments, autonomously estimates the prediction information of the ideal position in the virtual structure within the communication signal transmission interval, and adjusts its motion according to the dynamic model to maintain the stability of the formation. This method enhances the robustness of trajectory tracking during communication interruption after being affected by external disturbances by enabling the follower to autonomously estimate and adjust its position based on the last received state information.

[0041] 3.2 Cost Function The cost function is a core component in the distributed model predictive control (DMPC) strategy. Distributed state prediction quantifies the error between the follower USV and the ideal position and uses the L - BFGS - B algorithm to guide the optimization of the control input.

[0042] In the state prediction method, the calculation of the ideal position is based on the predicted central USV state, and the cost function is defined as: is the position of the i - th follower at the k - th time step. The ideal position of the follower USV is based on the predicted central USV state , speed estimation , the ideal distance between the follower USV and the central USV , k time steps, and the step size of each time step for calculation: During the communication signal transmission interval, the follower USV uses the distributed prediction of the future position and speed of the central USV based on the last received central USV state information, and updates its position according to the previous known speed: is the estimated position of the central USV at time k, is the estimated position of the central USV in the X - axis direction at time k, is the estimated position of the central USV in the Y - axis direction at time k; Estimated speed of the central USV; Estimated speed of the central USV in the X-axis direction at time k, Estimated speed of the central USV in the Y-axis direction at time step k; This prediction method enables the USV to maintain the integrity of the formation even in the absence of real-time updates.

[0043] 3.3 DMPC Controller Cost function in the DMPC optimization process is used to evaluate the quality of the control input sequence where is the sum of the squared errors between the predicted state and the estimated ideal state at the i-th step, is the input value of the sum of squared errors at when j = 0, 1, 2, ……, t - 1. The optimization goal is to find a set of control input sequences that minimize the sum of the squared errors i.e.: .

[0044] Figure 7 Shows the overall optimization process of MPC, which adjusts the speed, acceleration, and position of the USV in real time to ensure that they do not exceed the set constraint limits. After introducing the state prediction mechanism, the stability and consistency of the formation can be maintained even in the case of intermittent signal transmission interruptions. The optimization process is as follows: Every T seconds, the follower USV updates its state prediction based on the latest received state information of the central USV. Set the current state of the follower USV as , estimate and predict the state of the central USV based on the last speed and other motion state information during the communication signal transmission interval, and predict the ideal position of the central USV for the next n steps and each time step and the total time step t. Calculate the sum of the squared errors between the predicted state and the estimated ideal state at each step , use the optimization algorithm L-BFGS-B to optimize the control input to solve for the control input sequence that minimizes , apply the first control input in the optimized control input sequence , and re-optimize the remaining input sequence in the next control interval. Finally, according to the virtual structure method, predict the ideal position of the follower USV itself based on the ideal distance between the follower USV itself and the central USV in the ideal state.

[0045]

[0045] 4 Experimental Design 4.1 Simulation Design 4.1.1 Initial Position and Motion State of the Central USV The initial position of the central USV is set as the coordinate origin , and its initial velocity is a constant vector (unit: m / s), , .

[0046] 4.1.2 Initial position error and motion state of the follower USV To simulate the actual situation in the formation control process, a certain initial error is introduced to the initial position of each USV formation. The initial error matrix is set as , where each row represents the initial position error of the i-th follower USV in the X-axis and Y-axis directions. The following formula is the setting of the initial error in the simulation process.

[0047] In the formula, the first column is the error distance from the ideal position in the x coordinate, the second column is the error distance setting from the ideal position in the y coordinate, and the number of rows represents the number of the USV. For example, the first row represents the error setting of USV-2 (i.e., the 1st follower USV), and the error distance setting increases as the number of the USV increases. The purpose of this setting is to facilitate comparing how the control method's effect changes with different initial errors and to show the performance of the control method.

[0048] Therefore, the following formula represents the initial actual position of the i-th follower USV:[[]] The initial velocity of all follower USVs is set as the zero vector, that is, the following formula:[[]] 4.1.3 Formation motion state design The formation dynamically follows the motion of the central unmanned boat. According to the motion state of the central unmanned boat, the present invention divides Figure 8 the formations shown in Figure 8 into four common motion state verification methods: uniform linear motion in the two-dimensional plane (such as Figure 8 a) in Figure 8 ), uniformly accelerated linear motion (such as Figure 8 b) in

[0049] 4.1.4 Environmental condition design The external environmental influence factors introduced in the present invention include two parts. The first part is random noise interference, which affects the positioning accuracy of the USV. The second part is communication interruption interference. In this case, the communication is completely interrupted, and inter-boat communication transmission cannot be carried out to achieve positioning. During the measurement process, the angular data is affected by random noise, and the noise follows a normal distribution. In addition, to simulate abnormal situations in actual applications, 10% of the measurement data contains abnormal noise.

[0050] 4.2 Evaluation Index To evaluate the effectiveness of this method, the present invention introduces a distributed state prediction mechanism into the MPC control strategy. When the signal is updated, the position and velocity of the central USV are estimated. At each time step, the optimal acceleration at the current time step is calculated. During the signal interruption, the estimated values are used, and the followers use the estimated central position to continue the control. And only the acceleration for one step is calculated each time, that is, "rolling optimization", and the method effect is compared with the virtual leader method.

[0051] The present invention adopts the following performance indicators: 1. Convergence time T. Compare the time required for the displacement error between each USV and the ideal position to drop below the preset threshold in meters. A faster convergence speed indicates that the control method is more efficient in adjusting the position of the USV to reach the ideal state.

[0052] 2. Trajectory stability. The following formula represents the standard deviation of the displacement error of the follower USV within 10 seconds after convergence: represents the actual position at the i-th time step, N represents the total number of time steps within 10s, is the ideal position of the USV at the i-th time step. A smaller error standard deviation indicates that the movement of the USV near the ideal position is more stable, reflecting the ability of the control method to reduce oscillations and suppress disturbances. 3. Deviation adjustment ability. By calculating the decay rate of the error over time, the effect of the control method in correcting the initial deviation can be evaluated. The faster the error decays, the better the control method performs in adjusting the deviation, and it can adjust the USV to the ideal position more quickly.

[0053] The above are the preferred embodiments of the present invention. All changes made according to the technical solution of the present invention, when the functions and effects generated do not exceed the scope of the technical solution of the present invention, shall fall within the protection scope of the present invention.

Claims

1. A method for maintaining the formation of USV formation based on passive positioning under communication-limited conditions, characterized in that, Including: Based on the three-point triangulation method, a circular USV formation positioning model under passive conditions is established, and an improved least-squares filtering algorithm is adopted. At the same time, a distributed state estimation mechanism is introduced to enable the USV to achieve its own positioning. Combining the leader-follower model and the virtual structure method, a virtual-leader-follower circular formation model under passive conditions is established. When the USV formation moves, a virtual leader structure of the overall formation is set to guide the formation movement. Integrate the distributed state estimation mechanism into the distributed model predictive control strategy, so that each USV follows the trajectory of the USV corresponding to itself in the virtual leader structure and performs real-time rolling optimization control on its own trajectory at each time step.

2. The method for maintaining the formation of USV based on passive positioning under communication-limited conditions according to claim 1, wherein The circular USV formation positioning model includes a central USV and follower USVs evenly distributed in a circle centered on the central USV; the angle is positive in the counterclockwise direction, and 0° coincides with the positive direction of the X-axis. Among them, Central USV: The initial position of the central USV is , and the moving speed of the central USV is a constant vector , where is the speed of the central USV in the X-axis direction, is the speed of the central USV in the Y-axis direction. Then the position of the central USV at time k is expressed as the following formula: is the position of the central USV at time step k-1, is the discrete time step between two time steps; Follower USV: The ideal position of each follower USV relative to the central USV remains unchanged, as shown in the following formula: is the relative ideal position of the $i$-th follower USV in the formation at time $k$, which is expressed as: Taking the central USV as the origin O, a rectangular coordinate system is established. is the angle between the i-th follower USV and the central USV. In the ideal state, the positions of all follower USVs in the formation are evenly distributed on a circular trajectory with the central USV as the center and a radius of R.

3. The method for maintaining the formation of USV formations based on passive positioning under communication-limited conditions according to claim 2, characterized in that, In the virtual-leader-follower circular formation model, the leader-follower model is adopted during formation control, led by the central USV, and the follower USVs maintain their relative positions with the central USV; when predicting the trajectory, a virtual formation model is established based on the real-time motion state of the central USV to predict the trajectories and positions of future formation members. When tracking the trajectory, each follower USV only follows the future positions predicted by the virtual formation model, and the future positions of each follower USV also form a virtual image; in the virtual-leader-follower circular formation model, it is assumed that the number of follower USVs is N, then the ideal position of each USV in the circular USV formation model is determined by uniform distribution of the circumferential angle, and the angular distribution of the ideal position is shown in the following formula: is the angular position of the i-th follower USV in the circular formation.

4. The method for maintaining the formation of USV formation based on passive positioning under communication restricted conditions according to claim 2, wherein The USVs in the circular USV formation positioning model all adopt a discrete-time double-integrator model. Considering the changes in position and velocity, the state vector of the i-th follower USV at time k is defined as: Wherein: and respectively represent the positions of the i-th follower USV in the X-axis and Y-axis directions at time step k; and respectively represent the velocity components of the i-th follower USV in the X-axis and Y-axis directions at time step k; The state update equation of the system dynamics model is as follows: The state transition matrix A and the control input matrix B are expressed as follows: The state transition matrix A represents the transition from the current state to the state at the next moment; is the system state at time step k, represents the system state at the next time step k + 1. The control input matrix B represents the state change of the system affected by external inputs. The acceleration state quantity , respectively represent the accelerations of the i-th follower USV in the X-axis and Y-axis directions at time step k. Among them, the motion of the central USV operates through pre-set state information. The speed and acceleration parameters of the follower USVs are optimized according to the distributed model predictive control strategy DMPC to achieve the formation control goal; To ensure the motion safety of the USV and comply with physical limitations, the following velocity and acceleration limit constraints are set: where k is the time step, is the heading angle of the i-th follower USV at time step k, is the velocity state variable of the i-th follower USV at time step k, is the acceleration state variable of the i-th follower USV at time step k, is the coordinate of the i-th follower USV in the X-axis direction in the coordinate system at time step k, is the coordinate of the i-th follower USV in the Y-axis direction in the coordinate system at time step k, is the ideal position coordinate of the i-th follower USV in the X-axis direction in the coordinate system, is the ideal position coordinate of the i-th follower USV in the Y-axis direction in the coordinate system, is the heading angle of the follower USV in the ideal state, is the maximum speed of the set USV during movement, is the acceleration value of the follower USV in the ideal state, is the maximum acceleration of the set USV during movement, is the speed value of the follower in the ideal state, The maximum speed of the set USV during movement; In the actual control process, if the velocity or acceleration of the USV exceeds the limit, the actual velocity of the USV is limited to the maximum speed to ensure that its velocity and acceleration are always within the allowable range, and the acceleration and deceleration are actually adjusted according to the trajectory tracking effect.

5. The method for maintaining the formation of USV formation based on passive positioning under communication restricted conditions according to claim 2, characterized in that, In the case of being able to receive the included angle azimuth information, a circular USV formation positioning model under passive conditions is established based on the three-point triangulation method to determine the relative positions between formation members. And when there are different azimuth and distance deviations of the formation members from the ideal positions, the actual positions of the formation members are accurately calculated. The specific method is as follows: (1) Model assumptions Suppose the USV signal is emitted by the central USV and any two follower USVs within the circumference. The ideal position and trajectory are determined by the circle where the three USVs are located. Other USVs may have position deviations or no deviations, and only the angular azimuth information between the USV receiving the signal and the three USVs emitting the signal is known. Under this premise, a relative position positioning model of the USV receiving the signal is established; (2)Model establishment In the rectangular coordinate system, a three-station passive positioning mathematical model is proposed based on trigonometric functions and the circumferential angle theorem. By establishing two circumscribed circles of the ideal circle, the relative position coordinates of the USV receiving the signal are determined; specifically as follows: Construct an ideal circular formation of two circumcircles, where is the central USV number, and are the numbers of any two follower USVs that emit signals, and define the USV number of the formation members to be located as , then the first circumcircle is the circle formed by and , with the center at . According to the inscribed angle theorem, based on the length between , connect the points on the circumference , calculate the radius of the first circumcircle, and thus calculate the coordinates of the center of the first circumcircle, that is, the position coordinates of . The second circumcircle is the circumcircle formed by and , with the center at , is a follower USV number different from and . According to the relationship between and , obtain the radius of the second circumcircle by the sine theorem, and thus calculate the coordinates of the center of the second circumcircle, that is, the position of . The specific solution process is as follows: Let and The length between them is The corresponding central angle is According to the double - angle property of the inscribed - angle theorem, in the same circle, the central angle subtended by the same arc is That is twice the inscribed angle: For and cases, calculate the radius of the first circumcircle and lengths: Set the center coordinates of the first circumcircle , and the following are the center equation of the first circumcircle and the standard equation of the circumcircle: According to the second circumcircle, find the radius of the second circumcircle , according to the median theorem, for two points on the circumference and the midpoint F of and the center of the circle the connection line is perpendicular to and bisects the connection line between them ; Since , so , , according to the sine theorem, we know that: Let the center coordinates of the second circumcircle be , and the center equation of the second circumcircle and the standard equation of the circumcircle are as follows: By combining the standard equations of the circumscribed circles of the two circumscribed circles, we can find the coordinates of the two intersection points, one of which is the follower USV, i.e. The location information of another intersection That is, the coordinate information of the USV to be positioned with or without position deviation.

6. The method for maintaining the formation of USV formation based on passive positioning under communication restricted conditions according to claim 1, wherein The improved least squares filtering algorithm, that is, on the basis of using a fixed threshold in the least squares filtering algorithm, an adaptive threshold filtering step is introduced to dynamically adjust the threshold. The specific implementation of the improved least squares filtering algorithm is as follows: First, perform an initial estimation. Use the least squares filtering algorithm to perform a preliminary target position estimation to obtain the initial estimation value; Secondly, calculate the residuals. Calculate the residuals under the initial estimation, that is, the difference between each angle measurement and the model prediction value; Then determine the adaptive threshold: calculate the median of the residuals , calculate the median absolute deviation of the residuals , and set the adaptive threshold to , where is the threshold factor; Then, perform inlier screening. Mark the measurement points with residuals less than the threshold as inliers, and the rest as outliers; Finally, perform a re-estimation. Only use the inlier data to re-optimize the least squares to obtain the final target position estimation, and screen out the outliers. If the number of inliers after screening is less than two, the initial estimation result is retained.

7. The method for maintaining the formation of USV formation based on passive positioning under communication restricted conditions according to claim 1, characterized in that, Integrate the distributed state estimation mechanism into the distributed model predictive control strategy to enable each USV to follow the trajectory of the USV corresponding to itself in the virtual leader structure and perform real-time rolling optimization control on its own trajectory at each time step. The specific implementation is as follows: Every T seconds, the follower USV updates its state prediction based on the latest received central USV state information, and sets the current state of the follower USV as , estimates the state of the predicted central USV based on the last speed and other motion state information within the communication signal transmission interval, and predicts the ideal position of the central USV for the next n steps and each time step and the total time step t. Calculate the sum of squared errors between the predicted state and the estimated ideal state for each step through the cost function , use the optimization algorithm L-BFGS-B to optimize the control input to solve the minimization of the control input sequence , apply the first control input in the optimized control input sequence , and re-optimize the remaining input sequence in the next control interval. Finally, according to the virtual structure method, predict the ideal position of the follower USV itself based on the ideal distance between the follower USV itself and the central USV in the ideal state.

8. The method for maintaining the formation of USV based on passive positioning under communication-limited conditions according to claim 7, wherein The cost function is defined as follows: is the position of the i-th follower at the k-th time step, and the ideal position of the follower USV Based on the predicted central USV state , speed estimation , the ideal distance between the follower USV and the central USV , k time steps, and the step size for each time step Perform the calculation: During the communication signal transmission interval, the follower USV uses the distributed prediction to predict the future position and speed of the central USV based on the last received state information of the central USV, and updates its position according to the last known speed: The estimated position of the central USV at time k, The estimated position of the central USV in the X-axis direction at time k, The estimated position of the central USV in the Y-axis direction at time k; The estimated velocity of the central USV; The estimated velocity of the central USV in the X-axis direction at time k, The estimated velocity of the central USV in the Y-axis direction at time step k; Obtained by solving For evaluating the control input sequence The advantages and disadvantages of Is the total sum of squared errors between the predicted state and the estimated ideal state at the i-th step Is at The input value of the sum of squared errors at that time, j = 0, 1, 2, ……, t - 1. The optimization goal is to find a set of control input sequences such that the total sum of squared errors Reaches the minimum value, that is: 。

Images