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

Abstract

The present invention relates to a method for maintaining the formation of a USV formation based on passive positioning under communication-restricted conditions, and belongs to the field of unmanned surface vessel formations. The method comprises: establishing a circular USV formation positioning model under passive conditions based on a three-point triangulation method, adopting an improved least squares filtering algorithm, and introducing a distributed state estimation mechanism to enable the USV to achieve self-positioning; combining a leader-follower model with a virtual structure method, establishing a virtual leader-follower circular formation model under passive conditions, setting an overall formation virtual pilot structure when the USV formation moves, and guiding the formation movement; integrating the distributed state estimation mechanism into a distributed model predictive control strategy, so that each USV follows the trajectory of the USV corresponding to itself in the virtual pilot structure, and performing real-time rolling optimization control on its own trajectory at each time step. The present invention can enhance the robustness of formation control under communication-restricted scenarios.

Description

Technical Field

[0001] The present invention belongs to the field of unmanned surface vessel formation, and in particular relates to a USV formation maintaining method based on passive positioning under communication restricted conditions. Background Art

[0002] In complex marine environments, unmanned surface vehicles (USVs) have attracted widespread attention due to their small size, high maneuverability, and ability to perform missions in hazardous areas without causing casualties. In recent years, UUVs have gradually evolved into fleets of UUVs, which collaborate to complete complex tasks within complex environments. Coordinated control of these fleets is a key research focus. Because signal transmission is susceptible to interference from the external environment when UUV fleets perform rescue missions in disaster zones, and because communications may be delayed or interrupted during navigation or large-scale missions, research on passive control strategies for fleets under communication-restricted conditions is crucial for improving the collaborative control capabilities of these fleets.

[0003] The prerequisite for coordinated control of USV formations is accurate positioning. Positioning technologies are categorized into active and passive positioning. Active positioning uses signals transmitted and received reflected signals for positioning, offering all-weather and high-precision advantages. However, external signal interference can reduce positioning accuracy. Passive positioning, which does not transmit signals but only receives electromagnetic waves radiated by the target, is highly resistant to interference and is suitable for USV positioning under communication-restricted conditions. Types of formation passive positioning include time-of-day positioning and direction-finding cross-positioning. Time-of-day positioning requires precise signals, is computationally intensive, and has slow positioning speed, making it unsuitable for formation-coordinated passive positioning. Time-of-day positioning requires precise signals, is computationally intensive, and has slow positioning speed, making it unsuitable for formation-coordinated passive positioning. Therefore, time-of-day positioning is generally not used for formation-coordinated passive positioning. Multi-platform direction-finding cross-positioning, on the other hand, is easy to implement and provides rapid positioning, making it widely used in formation-coordinated control. These methods can effectively determine the relative positions of formation vessels. However, the direction-finding cross-positioning model has a large number of classifications for different position distributions, resulting in complex positioning calculations. As the formation size increases, the excessive computational effort can lead to delays in the formation decision-making process.

[0004] In the field of formation control, formation control methods primarily include leader-follower, behavioral, and virtual structure methods. Trajectory control methods primarily include proportional-integral control (PID) and model predictive control (MPC). In actual formation maintenance, a combination of these two methods is often required to ensure the entire formation maintains a stable relative formation and accurately follows a predetermined trajectory. PID control, however, is difficult to incorporate constraints, resulting in motion that violates ship kinematic principles. Existing methods achieve the formation and trajectory movement of a set robot formation, as well as the maintenance of the formation movement of multiple robots in complex environments with varying behavioral states. However, the leader-follower model is highly dependent on the leader, posing challenges to formation control in the event of a leader failure. While UAV control strategies based on virtual structure methods address this overreliance on the leader, they require coordinated control of the formation based on the leader's motion state under passive control conditions, while also ensuring dynamic formation maintenance in the event of a leader signal failure due to external interference. While virtual structure methods can address leader failure, they suffer from the inability to dynamically operate according to the leader's commands.

[0005] Centralized control places significant pressure on the navigator when processing formation information, while distributed predictive control (DMPC) provides predictive power and robust processing capabilities for multivariable systems. While centralized control places significant pressure on the navigator when processing formation information, distributed predictive control (DMPC), with its foresight and robust multivariable processing capabilities, is well-suited to alleviating the information processing and communication burdens of complex formation control tasks. Furthermore, by considering multiple constraints, it can prevent USV motion from violating ship dynamics principles and effectively adapt to the motion of unmanned vehicles. To enhance dynamic adaptability and efficient collaboration during formation control, state estimation methods, based on distributed predictive control, enable dynamic formation coordination and adjustment by sharing state information. Applying DMPC to trajectory control effectively limits the deviation between predicted and actual trajectories. However, this method significantly increases computational complexity due to the need to distribute signals from each distributed controller. The DMPC method has been applied to the field of robot communication. By using the DMPC strategy, the trajectory tracking error caused by communication interference has been reduced. However, the existing technology has a small number of robot formations, and only the effect of weak communication interference has been studied. There is no research on 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, it does not consider the situation where communication is interfered or interrupted. The L-BFGS-B algorithm can quickly update the model parameters during iteration, which helps to quickly adjust the control input of the unmanned vehicle 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 focuses on two types of external perturbations: environmental changes and communication perturbations. In terms of communication perturbations, research mainly focuses on mild perturbations, does not consider different degrees of perturbation or communication interruption, and pays limited attention to related constraints.

[0007] In summary, the navigator faces greater communication pressure and failure risk under communication-restricted conditions, which leads to difficulties in coordinated control. Summary of the Invention

[0008] The purpose of the present invention is to address the problem that unmanned surface vehicle (USV) formation control is unstable due to external interference and communication constraints during maritime rescue operations. This paper provides a USV formation maintenance method based on passive positioning under communication-constrained conditions, which can enhance the robustness of formation control in communication-constrained scenarios.

[0009] To achieve the above objectives, the technical solution of the present invention is: a USV formation maintenance method based on passive positioning under communication-restricted conditions, comprising:

[0010] A circular USV formation positioning model under passive conditions is established based on the three-point triangulation method, and an improved least squares filtering algorithm is used. At the same time, a distributed state estimation mechanism is introduced to enable the USV to achieve self-positioning.

[0011] Combining the leader-follower model with the virtual structure method, a virtual-leader-follower circular formation model under passive conditions is established. When the USV formation moves, a virtual pilot structure of the entire formation is set to guide the formation movement.

[0012] The distributed state estimation mechanism is integrated into the distributed model predictive control strategy, so that each USV follows the trajectory of its own USV in the virtual pilot structure and performs real-time rolling optimization control on its own trajectory at each time step.

[0013] Compared with the prior art, the present invention has the following beneficial effects:

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

[0015] (2) Improve communication efficiency and reduce costs: In the present invention, followers generally only need to synchronize with the state of the virtual leader, without having to 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 drone formations or self-driving car formations. In the traditional leader-follower approach, each follower needs to receive real-time commands or status information from the leader, which results in a heavy communication burden. As the scale of the system expands, the communication demand increases exponentially, which may lead to delays or data transmission bottlenecks.

[0016] (3) Fault tolerance: In this invention, since the virtual leader does not rely on a physical leader, it has higher fault tolerance. Even if the actual leader fails or cannot continue to perform the task, 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 when there is no backup leader or it is impossible to replace the leader, which may cause the formation to lose control or disband. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 This is a flow chart of the USV formation maintaining method of the present invention.

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

[0019] Figure 3 Schematic diagram for constructing the circumcircle.

[0020] Figure 4 Schematic diagram for calculating the standard equation of the first circumscribed circle.

[0021] Figure 5 Schematic diagram for calculating the standard equation of the second circumcircle.

[0022] Figure 6 Schematic diagram of the improved least squares method process.

[0023] Figure 7 Schematic diagram of the overall MPC optimization process.

[0024] Figure 8 It is the formation movement state. DETAILED DESCRIPTION

[0025] The technical solution of the present invention will be described in detail below with reference to the accompanying drawings.

[0026] The present invention provides a USV formation maintenance method based on passive positioning under communication-limited conditions, comprising:

[0027] A circular USV formation positioning model under passive conditions is established based on the three-point triangulation method, and an improved least squares filtering algorithm is used. At the same time, a distributed state estimation mechanism is introduced to enable the USV to achieve self-positioning.

[0028] Combining the leader-follower model with the virtual structure method, a virtual leader-follower circular formation model is established under passive conditions. When the USV formation moves, a virtual pilot structure of the entire formation is set to guide the formation movement.

[0029] The distributed state estimation mechanism is integrated into the distributed model predictive control strategy to ensure that each USV follows the trajectory of its own USV in the virtual pilot structure and performs real-time rolling optimization control on its own trajectory at each time step.

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

[0031] like Figure 1 As shown in the figure, the present invention provides a USV formation maintenance method based on passive positioning under communication limited conditions, which includes three steps:

[0032] The first step is to consider the influence of the passive environment. Under these conditions, a positioning model for each USV in the circular formation is established based on the inter-vessel angle. The impact of environmental noise on positioning accuracy is also taken into account. By setting a noise threshold to eliminate abnormal noise data, the least squares filtering algorithm is improved, and a distributed state estimation mechanism is introduced to enable the USV to achieve self-positioning. This model helps the USV accurately locate its own position relative to the USV at the center of the circle.

[0033] After achieving precise positioning, the next step is to address the issue of maintaining the formation's stability during movement. In addition to considering external noise interference, we also consider the possibility of more severe communication interruptions. Leveraging the strengths of the leader-follower and virtual navigator methods, we establish a virtual-pilot-follower model based on passive conditions. This establishes a virtual pilot structure for the entire formation during USV formation movement to guide the movement. This effectively prevents the formation from maintaining stability in the event of a leader failure due to communication interruptions.

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

[0035] 1. Circular USV formation positioning model

[0036] Common USV formations include linear, rectangular, V-shaped, and circular. A circular formation can effectively disperse mutual interference between individuals, enhance the overall stability of the formation, and facilitate expansion and contraction of the formation size. Considering the advantages of circular formations in dynamic adjustment and space utilization, this paper selects a circular formation as the ideal USV formation. A common number of nine vessels is selected, with eight evenly distributed around the circumference as follower USVs and one unmanned vehicle at the center of the circle as the center USV. Positive angles are counterclockwise, with 0° coinciding with the positive direction of the X-axis.

[0037] 1.1 Center USV:

[0038] The overall movement of the formation is led by the central USV, and the positions and movement states of the other unmanned boats will be dynamically adjusted according to the movement of the central USV. The initial position of the central USV is set to , the movement speed of the center USV is a constant vector ,in, is the speed of the center USV in the X-axis direction, is the velocity of the center USV in the Y-axis direction, then the position of the center USV at time k is expressed as follows:

[0039]

[0040] is the position of the center USV at time step k-1, is the discrete time step between two time steps;

[0041] 1.2 Follower USV:

[0042] The ideal position of each follower USV remains unchanged relative to the position of the center USV, as shown in the following equation:

[0043]

[0044] is the relative ideal position of the i-th follower USV in the formation at time k, Expressed as:

[0045]

[0046] With the center USV as the origin O, a rectangular coordinate system is established. is the angle between the i-th follower USV and the center USV. Ideally, the positions of all follower USVs in the formation are evenly distributed on a circular trajectory with the center USV as the center and R as the radius.

[0047] 1.3 Virtual Leader-Follower Circular Formation Model

[0048] Based on the characteristics of the leader-follower model and the virtual structure method, a leader-follower model is adopted for formation control, with a central USV leading and followers maintaining their relative positions to the leader. Trajectory prediction uses the real-time motion state of the central USV to establish a virtual formation model, predicting the future trajectories and positions of formation members. During trajectory tracking, each USV only follows the virtual formation's predicted future positions, while the future positions of each follower USV also form a virtual image. This method effectively overcomes the problem of leader failure during movement.

[0049] To predict trajectories, a virtual formation and navigator model was established based on the real-time motion status of the central USV. This model predicts the future trajectory and position of each formation member using the MPC control method. In the event of interference or signal interruption, the USV members can use MPC to predict their future trajectories and positions. When tracking the trajectory, each USV simply follows its own future position predicted by the virtual formation, and the future positions of each USV also constitute a virtual formation. The virtual overall formation can adjust the position of each virtual navigator using a virtual structure method, that is, maintaining the relative distance and azimuth between the virtual formation members and the virtual central USV. This method alleviates the problem of being unable to receive information instructions and respond to new movements when the signal is not smooth, effectively solves the problem of navigator failure during movement, and ensures the continuity of formation control.

[0050] The key geometric features of a 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 uniformly distributing the circumferential angle. The angular distribution of the ideal position is:

[0051]

[0052] is the angular position of the i-th follower USV in the circular formation.

[0053] like Figure 2 As shown, in this example, N is 8, that is, 8 USVs are evenly distributed around the circle as follower USVs, and 1 USV is located at the center of the circle as the center USV. The center USV is defined as The other two unmanned boats that sent signals and the follower USV are numbered and , let the number of the unmanned boat receiving the signal, that is, the follower USV, be , the error position is defined as . Assume that the circular formation The horizontal line is the X axis, The straight line is the Y axis. If there is no error, the angle between each adjacent unmanned boat is 45 degrees. 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.

[0054] 1.4 Unmanned Boat Power Model

[0055] In the present invention, each USV adopts a discrete-time dual integrator model. Considering the changes in position and speed, the state vector of the i-th USV at time k is defined as:

[0056]

[0057] in: and They represent the positions of the i-th follower USV in the X-axis and Y-axis directions at time step k respectively; and They represent the velocity components of the X-axis and Y-axis directions of the i-th follower USV at time step k;

[0058] The state update equation of the system dynamics model is as follows:

[0059]

[0060] The state transfer matrix A and control input matrix B are expressed as follows:

[0061]

[0062] The state transition matrix A represents the transition from the current state to the next state; is the system state at time step k, Indicates the system state at the next time step k+1. The control input matrix B represents the state change of the system affected by the external input. The acceleration state , They represent the acceleration of the i-th follower USV in the X-axis and Y-axis directions at time step k, respectively. The center USV motion is operated according to the state information set in advance, and the speed and acceleration parameters of the follower USV are optimized according to the distributed model predictive control strategy DMPC to achieve the formation control goal;

[0063] To ensure that the USV's motion is safe and complies with physical limitations, the following velocity and acceleration constraints are set:

[0064]

[0065] Where k is the time step, is the heading angle of the i-th follower USV at time step k, is the velocity state of the i-th follower USV at time step k, is the acceleration state of the i-th follower USV at time step k, is the coordinate of the ith follower USV in the X-axis direction in the coordinate system at time step k, is the Y-axis coordinate of the i-th follower USV 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, The maximum speed of the USV during the movement is set. 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 of the follower in the ideal state, Set the maximum speed of the USV during movement;

[0066] 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 trajectory tracking effect.

[0067] 2. USV positioning model establishment

[0068] 2.1 Model Assumptions

[0069] Assuming that USV signals are emitted by the central USV and any two follower USVs within the circle, the ideal position and trajectory are determined by the circle in which the three USVs are located, while the other USVs have position deviations or no deviations. Only the angle and azimuth information between the USV receiving the signal and the three USVs transmitting the signal is known. Under this premise, a relative position positioning model for the USV receiving the signal is established.

[0070] 2.2 Model establishment

[0071] In the rectangular coordinate system, the present invention proposes a three-station passive positioning mathematical model based on trigonometric functions and the circular angle theorem. By establishing two circumscribed circles of an ideal circle, the relative position coordinates of the USV receiving the signal are determined; the details are as follows:

[0072] like Figure 3-5 As shown, construct an ideal circular formation The two circumcircles of is the center USV number, and Number any two follower USVs that transmit signals, and define the USV numbers of the formation members to be located as , then the first circumcircle is and The circle composed of , according to the circular angle theorem The length between the points on the circumference , calculate the radius of the first circumscribed circle, and then calculate the coordinates of the center of the first circumscribed circle, that is The position coordinates of the second circumscribed circle are and The circumscribed circle is , For 、 Different follower USV numbers. and , the radius of the second circumscribed circle is obtained by the sine theorem, and the coordinates of the center of the second circumscribed circle are calculated, that is, The specific solution process is as follows:

[0073] set up and The length between , the corresponding central angle is , according to the double angle property of the circular angle theorem, the central angle of the same arc in the same circle is Equal to the circumference angle Right now twice of:

[0074]

[0075] for and , calculate the radius of the first circumscribed circle and Length:

[0076]

[0077] Set the coordinates of the center of the first circumscribed circle , the equation of the center of the first circumscribed circle and the standard equation of the circumscribed circle are as follows:

[0078]

[0079]

[0080] Based on the second circumscribed circle, find the radius of the second circumscribed circle , from the midline theorem, we know that for two points on the circumference and The midpoint F and the center of the circle Connection perpendicular to and bisect the line connecting them , based on the symmetry of the circle and the median theorem, we know ;

[0081] because , so , , from the sine theorem we know that:

[0082]

[0083] Let the coordinates of the center of the second circumscribed circle be , we get the equation of the center of the second circumscribed circle and the standard equation of the circumscribed circle as follows:

[0084]

[0085]

[0086] 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. The location information of another intersection That is, the coordinate information of the USV to be positioned with or without position deviation.

[0087] 2.3 Improved least squares filtering algorithm

[0088] Least Squares (LS) is a statistical method that estimates target parameters (such as position) by minimizing the squared error between observed values ​​and model predictions. In this paper, LS is used to estimate the two-dimensional relative position of the target based on noisy inter-vessel relative angle measurements acquired from three USVs.

[0089] A nonlinear least squares optimization algorithm (Adaptive LS) is used to minimize the sum of squares of all residuals. To address the LS method's sensitivity to outliers, this paper proposes an adaptive least squares method (Adaptive LS) for passive positioning. This method uses an adaptive threshold filtering mechanism to dynamically identify and eliminate anomalous residuals, thereby enhancing its resistance to outliers.

[0090] like Figure 6As shown in Figure 1, the adaptive least squares method introduces an adaptive threshold filtering step based on the fixed threshold used in the traditional least squares method. This dynamically adjusts the threshold to identify and eliminate abnormal residuals. This process statistically analyzes the residuals of the initial estimate to determine an adaptive threshold. Measurement points with large residuals are considered outliers and are eliminated from the estimation process. The overall process is as follows:

[0091] First, an initial estimate is performed, and a least squares filtering algorithm is used to perform a preliminary target position estimate to obtain an initial estimated value;

[0092] Secondly, the residual calculation is performed to calculate the residual under the initial estimate, that is, the difference between each angle measurement and the model prediction value;

[0093] Then determine the adaptive threshold: calculate the median of the residuals , calculate the median absolute deviation of the residuals , set the adaptive threshold to ,in is the threshold factor, which is used to adjust the filtering strictness and is selected according to the experimental results. The optimal adjustment range is between 1.2 and 1.8. Its value before automatic adjustment is 0. The overall algorithm determines a basic threshold through the median, and then automatically adjusts the threshold factor value according to the change of residual distribution to change the threshold, so that the rejection rate is always controlled in a reasonable range (5%-15%).

[0094] Then, the inlier screening is performed, and the measurement points with residuals less than the threshold are marked as inliers, and the rest are outliers;

[0095] Finally, a re-estimation is performed, and only the inlier data is used to perform the least squares optimization again to obtain the final target position estimate, and the outliers are screened out. If the number of inliers after screening is less than two, the initial estimation result is retained.

[0096] 3 Formation Control Strategy

[0097] 3.1 Distributed State Prediction Mechanism

[0098] The distributed state prediction strategy disperses the prediction task of the global system state to multiple nodes, allowing each node to independently predict the system state. At the same time, accurate state predictions are obtained globally through communication and information exchange, effectively reducing the computational pressure of a single node and having higher robustness.

[0099] Based on the characteristics of limited USV communication conditions, the present invention proposes a distributed state estimation strategy for formations under communication-limited conditions. The followers independently sample the motion states of the central USV at the most recent and different times, autonomously estimate the predicted information of the ideal position in the virtual structure within the communication signal transmission interval, and adjust the motion according to the dynamic model to maintain the stability of the formation. This method enables the followers to autonomously estimate and adjust their positions based on the last received state information, thereby enhancing the robustness of trajectory tracking during communication interruptions after external interference.

[0100] 3.2 Cost Function

[0101] The cost function is the core component of the distributed model predictive control (DMPC) strategy. The 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.

[0102] In the state prediction method, the ideal position is calculated based on the predicted center USV state, and the cost function Defined as:

[0103]

[0104] is the position of the i-th follower at the k-th time step, the ideal position of the follower USV Central USV status based on prediction , speed estimation , Ideal distance between follower USV and center USV , k time steps and the step size of each time step Perform the calculation:

[0105]

[0106] 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 status information, and updates its position according to the last known speed:

[0107]

[0108] is the estimated position of the center USV at time k, is the estimated position of the center USV in the X-axis direction at time k, is the estimated position of the center USV in the Y-axis direction at time k; is the estimated speed of the center USV; is the estimated velocity of the central USV in the X-axis direction at time k, The estimated velocity of the center USV in the Y direction at time step k; this prediction method allows the USVs to maintain the integrity of the formation even in the absence of real-time updates.

[0109] 3.3 DMPC Controller

[0110] Cost function in DMPC optimization process Used to evaluate control input sequences The pros and cons, is the total sum of squares of errors between the predicted state and the estimated ideal state at step i, is The input value of the sum of squared errors, j = 0, 1, 2, ..., t-1, the optimization goal is to find a set of control input sequences so that the total sum of squared errors Reach the minimum value, that is:

[0111] .

[0112] Figure 7 The overall MPC optimization process is demonstrated, with real-time adjustments to the USV's speed, acceleration, and position to ensure they do not exceed the set constraints. The introduction of a state prediction mechanism allows the formation to maintain stability and consistency even when signals are intermittently interrupted. The optimization process is as follows:

[0113] Every T seconds, the follower USV updates its state prediction based on the latest received central USV state information, setting the current follower USV state to , based on the last speed and other motion state information, the state of the center USV is estimated and predicted within the communication signal transmission interval, and the dynamic model is used to predict the next n steps and each time step. The cost function is used to calculate the sum of squared errors between the predicted state and the estimated ideal state at each step. , using the optimization algorithm L-BFGS-B to optimize the control input to minimize The control input sequence , apply the first control input in the optimized control input sequence The remaining input sequences are re-optimized in the next control interval, and finally, according to the virtual structure method, the ideal position of the follower USV itself is predicted by the ideal distance between the follower USV and the center USV in the ideal state.

[0114] 4 Experimental design

[0115] 4.1 Simulation Design

[0116] 4.1.1 Initial position and motion state of the central USV

[0117] The initial position of the center USV is set as the coordinate origin , whose initial velocity is a constant vector (Unit: m / s), , .

[0118] 4.1.2 Initial position error and motion state of follower USV

[0119] In order to simulate the actual situation during the formation control process, a certain initial error is introduced into the initial position of each USV formation. The initial error matrix is ​​set as , where each line It represents the initial position error of the i-th following USV in the X-axis and Y-axis directions. The following formula is the setting of the initial error in the simulation process.

[0120]

[0121] 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 row number represents the USV number. For example, the first row represents the error setting of USV-2 (i.e., the first follower USV). As the USV number increases, the error distance setting becomes larger. The purpose of this setting is to facilitate the comparison of how the effects of different control methods change with the initial error, which is conducive to demonstrating the performance of the control method.

[0122] Therefore, the following formula represents the initial actual position of the i-th follower USV:

[0123]

[0124] The initial velocity of all follower USVs is set to zero vector, which is as follows:

[0125]

[0126] 4.1.3 Formation motion state design

[0127] The formation dynamically follows the movement of the center unmanned boat. According to the movement state of the center unmanned boat, the present invention divides Figure 8 The formation shown in the figure is in uniform linear motion in a two-dimensional plane (e.g. Figure 8 a)), uniformly accelerated linear motion (such as Figure 8 b)), uniform speed steering (such as Figure 8 c)) and accelerating the steering (e.g. Figure 8 d) Reliability of four common motion state verification methods.

[0128] 4.1.4 Environmental Conditions Design

[0129] The present invention incorporates two types of external environmental factors. The first is random noise interference, which affects the accuracy of USV positioning. The second is communication interruption interference, which completely disrupts communication and prevents inter-vessel communication and positioning. During the measurement process, the angle data is affected by random noise, which follows a normal distribution. Furthermore, to simulate abnormal conditions in real applications, 10% of the measurement data is subjected to abnormal noise.

[0130] 4.2 Evaluation indicators

[0131] To evaluate the effectiveness of this method, we incorporated a distributed state prediction mechanism into the MPC control strategy. During signal updates, the position and velocity of the center USV are estimated. At each time step, the optimal acceleration for the current time step is calculated. During signal interruptions, the estimated values ​​are used, and the followers continue control using the estimated center position, calculating only one acceleration step at a time. This is known as "rolling optimization," and the effectiveness of this method is compared with that of a virtual leader method.

[0132] The present invention adopts the following performance indicators:

[0133] 1. Convergence time T. Compare the displacement error of each USV with the ideal position and reduce it to the preset threshold The faster convergence speed indicates that the control method is more efficient in adjusting the USV position to achieve the ideal state.

[0134] 2. Trajectory stability. The following formula represents the standard deviation of the displacement error of the follower USV within 10 seconds after convergence:

[0135]

[0136] represents the actual position of 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 USV motion near the ideal position is more stable, reflecting the ability of the control method in reducing oscillations and suppressing disturbances.

[0137] 3. Deviation Adjustment Ability. By calculating the rate at which the error decays over time, we can assess the effectiveness of the control method in correcting the initial deviation. The faster the error decays, the better the control method is at adjusting the deviation and the faster it can adjust the USV to the desired position.

[0138] The above are preferred embodiments of the present invention. Any changes made according to the technical solution of the present invention, as long as the resulting functions and effects do not exceed the scope of the technical solution of the present invention, shall fall within the scope of protection of the present invention.

Claims

1. A USV formation maintenance method based on passive positioning under communication-restricted conditions, characterized in that: include: A circular USV formation positioning model under passive conditions is established based on the three-point triangulation method, and an improved least squares filtering algorithm is used. At the same time, a distributed state estimation mechanism is introduced to enable the USV to achieve self-positioning. Combining the leader-follower model with the virtual structure method, a virtual-leader-follower circular formation model under passive conditions is established. When the USV formation moves, a virtual pilot structure of the entire formation is set to guide the formation movement. The distributed state estimation mechanism is integrated into the distributed model predictive control strategy, so that each USV follows the trajectory of its own USV in the virtual pilot structure and performs real-time rolling optimization control of 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, setting the current follower USV state to , based on the last speed and other motion state information, the state of the center USV is estimated and predicted within the communication signal transmission interval, and the dynamic model is used to predict the next n steps and each time step. The cost function is used to calculate the sum of squared errors between the predicted state and the estimated ideal state at each step. , using the optimization algorithm L-BFGS-B to optimize the control input to minimize The control input sequence , apply the first control input in the optimized control input sequence The remaining input sequences are re-optimized in the next control interval, and finally, according to the virtual structure method, the ideal position of the follower USV itself is predicted by the ideal distance between the follower USV and the center USV in the ideal state.

2. The USV formation maintaining method based on passive positioning under communication restricted conditions according to claim 1 is characterized in that: The circular USV formation positioning model includes a central USV and follower USVs evenly distributed around the central USV. The angle is positive in the counterclockwise direction, with 0° coinciding with the positive direction of the X-axis. Center USV: The initial position of the central USV is , the movement speed of the center USV is a constant vector ,in, is the speed of the center USV in the X-axis direction, is the velocity of the center USV in the Y-axis direction, then the position of the center USV at time k is expressed as follows: is the position of the center USV at time step k-1, is the discrete time step between two time steps; Follower USV: The ideal position of each follower USV remains unchanged relative to the position of the center USV, as shown in the following equation: is the relative ideal position of the i-th follower USV in the formation at time k, Expressed as: With the center USV as the origin O, a rectangular coordinate system is established. is the angle between the i-th follower USV and the center USV. Ideally, the positions of all follower USVs in the formation are evenly distributed on a circular trajectory with the center USV as the center and R as the radius.

3. The USV formation maintaining method based on passive positioning under communication restricted conditions according to claim 2 is characterized in that: In the virtual-leader-follower circular formation model, a leader-follower model is adopted during formation control, with the central USV leading and the follower USVs maintaining 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 trajectory and position of future formation members. When tracking the trajectory, each follower USV only follows the future position predicted by the virtual formation model, and the future position of each follower USV itself also constitutes a virtual image. In the virtual-leader-follower circular formation model, the number of follower USVs is set to N, and the ideal position of each USV in the circular USV formation model is determined by uniformly distributing the circumferential angle. 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 USV formation maintaining method based on passive positioning under communication restricted conditions according to claim 2 is characterized in that: The USVs in the circular USV formation positioning model all adopt the 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: in: and They represent the positions of the i-th follower USV in the X-axis and Y-axis directions at time step k respectively; and They represent the velocity components of the X-axis and Y-axis directions of the i-th follower USV at time step k; The state update equation of the system dynamics model is as follows: The state transfer matrix A and control input matrix B are expressed as follows: The state transition matrix A represents the transition from the current state to the next state; is the system state at time step k, Indicates the system state at the next time step k+1. The control input matrix B represents the state change of the system affected by the external input. The acceleration state , They represent the acceleration of the i-th follower USV in the X-axis and Y-axis directions at time step k, respectively. The center USV motion is operated according to the state information set in advance, and the speed and acceleration parameters of the follower USV are optimized according to the distributed model predictive control strategy DMPC to achieve the formation control goal; To ensure that the USV's motion is safe and complies with physical limitations, the following velocity and acceleration 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 of the i-th follower USV at time step k, is the acceleration state of the i-th follower USV at time step k, is the coordinate of the ith follower USV in the X-axis direction in the coordinate system at time step k, is the Y-axis coordinate of the i-th follower USV 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, The maximum speed of the USV during the movement is set. 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 of the follower in the ideal state, Set the maximum speed of the 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 trajectory tracking effect.

5. The USV formation maintaining method based on passive positioning under communication restricted conditions according to claim 2 is characterized in that: When the angle and azimuth information can be received, a circular USV formation positioning model is established under passive conditions based on the three-point triangulation method to determine the relative positions of the formation members. In addition, the actual positions of the formation members can be accurately calculated when there are different azimuth and distance deviations from the ideal position. The specific method is as follows: (1) Model assumptions Assuming that USV signals are emitted by the central USV and any two follower USVs within the circle, the ideal position and trajectory are determined by the circle in which the three USVs are located, while the other USVs have position deviations or no deviations. Only the angle and azimuth information between the USV receiving the signal and the three USVs transmitting the signal is known. Under this premise, a relative position positioning model for 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 circular angle theorem. By establishing two circumscribed circles of an ideal circle, the relative position coordinates of the USV receiving the signal are determined; the details are as follows: Constructing an ideal circular formation The two circumcircles of is the center USV number, and Number any two follower USVs that transmit signals, and define the USV numbers of the formation members to be located as , then the first circumcircle is and The circle composed of , according to the circular angle theorem The length between the points on the circumference , calculate the radius of the first circumscribed circle, and then calculate the coordinates of the center of the first circumscribed circle, that is The position coordinates of the second circumscribed circle are and The circumscribed circle is , For 、 Different follower USV numbers. and , the radius of the second circumscribed circle is obtained by the sine theorem, and the coordinates of the center of the second circumscribed circle are calculated, that is, The specific solution process is as follows: set up and The length between , the corresponding central angle is , according to the double angle property of the circular angle theorem, the central angle of the same arc in the same circle is Equal to the circumference angle Right now twice of: for and , calculate the radius of the first circumscribed circle and Length: Set the coordinates of the center of the first circumscribed circle , the equation of the center of the first circumscribed circle and the standard equation of the circumscribed circle are as follows: Based on the second circumscribed circle, find the radius of the second circumscribed circle , from the midline theorem, we know that for two points on the circumference and The midpoint F and the center of the circle Connection perpendicular to and bisect the line connecting them , based on the symmetry of the circle and the median theorem, we know ; because , so , , from the sine theorem we know that: Let the coordinates of the center of the second circumscribed circle be , we get the equation of the center of the second circumscribed circle and the standard equation of the circumscribed circle 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. The location information of another intersection That is, the coordinate information of the USV to be positioned with or without position deviation.

6. The USV formation maintaining method based on passive positioning under communication restricted conditions according to claim 1 is characterized in that: The improved least squares filtering algorithm introduces an adaptive threshold filtering step based on the fixed threshold adopted by the least squares filtering algorithm, and dynamically adjusts the threshold. The specific implementation of the improved least squares filtering algorithm is as follows: First, an initial estimate is performed, and a least squares filtering algorithm is used to perform a preliminary target position estimate to obtain an initial estimated value; Secondly, the residual calculation is performed to calculate the residual under the initial estimate, 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 , set the adaptive threshold to ,in is the threshold factor; Then, the inlier screening is performed, and the measurement points with residuals less than the threshold are marked as inliers, and the rest are outliers; Finally, a re-estimation is performed, and only the inlier data is used to perform the least squares optimization again to obtain the final target position estimate, and the outliers are screened out. If the number of inliers after screening is less than two, the initial estimation result is retained.

7. The method for maintaining USV formation based on passive positioning under communication-restricted conditions according to claim 1, characterized in that: The cost function is defined as follows: is the position of the i-th follower at the k-th time step, the ideal position of the follower USV Central USV status based on prediction , speed estimation , the ideal distance between the follower USV and the center USV , k time steps and the step size of each time step Perform the 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 status information, and updates its position according to the last known speed: is the estimated position of the center USV at time k, is the estimated position of the center USV in the X-axis direction at time k, is the estimated position of the center USV in the Y-axis direction at time k; is the estimated speed of the center USV; is the estimated velocity of the central USV in the X-axis direction at time k, The estimated velocity of the center USV in the Y-axis direction at time step k; The solution obtained For evaluating control input sequences The pros and cons, is the total sum of squares of errors between the predicted state and the estimated ideal state at step i, is The input value of the sum of squared errors, j = 0, 1, 2, ..., t-1, the optimization goal is to find a set of control input sequences so that the total sum of squared errors Reach the minimum value, that is: 。

Images