A multi-ship cooperative collision avoidance control method based on safety-first type switching guidance
Patent Information
- Application Number
- CN202611391401.3
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2026-09-09
- Publication Date
- 2026-10-23
AI Technical Summary
[0005]2）现有的避碰控制算法尽管在避碰效果上呈现了良好的性能，但是控制信号并非最优
1、本发明提出一种安全关键型切换制导策略，该策略引入DVS对拓扑连接的成员进行引导，既有效规避刚性编队构型引发的跟踪偏差，又能在狭窄水域场景下优先保障局部航行安全；
Smart Images
Figure CN122963178A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of autonomous navigation application technology, and in particular to a multi-ship cooperative collision avoidance control method based on safety-priority switching guidance. Background Technology
[0002] In the maritime field, a path tracking control system consists of three subsystems: guidance, control, and navigation. The guidance system can automatically construct a reference signal based on the positional relationship between the ship's current attitude and the desired path; the control system can achieve effective convergence by stabilizing the error between the current attitude and the reference signal; and the navigation system can transmit the position and attitude information of the controlled object to the guidance and control systems through sensors.
[0003] A three-degree-of-freedom mathematical model of the USV is established using a separation method, the controller is designed using the backstepping method, and a radial basis function neural network is used to approximate the structural uncertainty of the model online.
[0004] Guidance and control play a crucial role in USV formation path tracking and collision avoidance maneuvers. Existing research primarily focuses on open water scenarios. However, maintaining safe navigation of USV formations in confined waters with limited space and numerous obstacles remains a challenging problem. Furthermore, guidance systems often rely on dividing the path into straight and turning segments for separate calculations and combining collision avoidance algorithms with the guidance system, simplifying controller design. However, in dynamically uncertain environments, excessive inter-channel transmission load can lead to system response lag. Integrating the collision avoidance algorithm into the controller can effectively address this problem, but it results in a loss of formation tracking performance during collision avoidance. Based on the above analysis, existing USV formation collision avoidance and path tracking control algorithms still have the following two shortcomings: 1) The LVS guidance law generates cumulative position and attitude errors at the switching point (straight segment-turning segment), which affects the accuracy and safety of the guidance path. Traditional parallel formations can affect the status of other topology-connected vessels during collision avoidance and turning. This is not the optimal formation in narrow waterways with limited maneuvering space.
[0005] 2) While existing collision avoidance control algorithms demonstrate good performance in collision avoidance, the control signals are not optimal. Furthermore, the combination of traditional obstacle control functions and quadratic optimization is often infeasible in confined water environments with multiple constraints. Therefore, the algorithm needs to be improved to consider the specific scenario. Summary of the Invention
[0006] This invention provides a multi-ship cooperative collision avoidance control method based on safety-priority switching guidance to overcome the above-mentioned technical problems.
[0007] To achieve the above objectives, the technical solution of the present invention is as follows: A multi-ship cooperative collision avoidance control method based on safety-priority switching guidance includes: S1: Establish a nonlinear mathematical model of a three-degree-of-freedom unmanned surface vessel and generate a globally smooth reference path based on the L1 virtual vessel; S2: Define the obstacle control function and determine the initial time of collision avoidance maneuver by solving a quadratic programming problem in real time; S3: Define a safety priority switching guidance, wherein the safety priority switching guidance is used to generate a DVS reference path based on the motion state of the preceding vessel frozen at the initial time when the initial time is triggered, and switch the guidance target of the following vessel from the real-time state of the preceding vessel to the DVS reference path; S4: Based on the global smooth reference path and the DVS reference path, construct a distributed error, and then design a nominal virtual control law; S5: Based on the control obstacle function, design the optimal collision-free heading virtual control law, and make real-time corrections to the nominal virtual control law to obtain the final virtual control law; S6: Design an adaptive robust dynamic control law, which includes a control law and an adaptive law; estimate the neural network weights and actuator gains online using the adaptive law, and generate the final control command from the control law, so that the unmanned vessel executes the final virtual control law to achieve collision avoidance control.
[0008] Furthermore, a nonlinear mathematical model of a three-degree-of-freedom unmanned surface vessel is established, and a globally smooth reference path is generated based on the L1 virtual vessel, including: A nonlinear mathematical model of a three-degree-of-freedom unmanned surface vessel is established, and its formula is as follows:
[0009]
[0010] in, and This represents the ship's position and heading angle in a fixed coordinate system. Forward velocity, drift velocity, and bow roll rate. For the uncertain parameters of the ship model, For unknown functions, For the time-varying disturbances experienced by the USV during forward movement, drift, and yaw; and These represent control inputs related to the main engine speed and rudder angle, respectively. and For time-varying actuator gain; Generate a globally smooth reference path based on the L1 virtual ship, including: Based on the L1 virtual ship, according to the preset waypoint information W 1, W 2,…, W n The formula for generating a globally smooth reference path is as follows:
[0011] in, and Given the desired position and heading angle of the leader L1VS, and These represent the reference forward speed and bow angular velocity of the L1 virtual ship, respectively. In the straight line segment In the middle, the angular velocity of the bow. For the curve segment angular velocity of the bow , ,in It is a connection arrive line segments, express direction and The angle between directions, This represents the turning acceleration at the waypoint. The turning radius is and This refers to the starting and ending positions of the steering maneuver.
[0012] Furthermore, a control obstacle function is defined, and the initial time for collision avoidance maneuvers is determined by solving a quadratic programming problem in real time, including: S21. Define the control barrier function as follows: The nonlinear mathematical model is rewritten as follows:
[0013] in, , , , The angular velocity of the bow; and This is the rewritten system matrix; Define a first control obstacle function and a second control obstacle function to quantify the distance between the unmanned surface vessel (USV) and static obstacles, and the distance between the USV and other USVs, respectively. Their formulas are as follows:
[0014] in, It is a two-dimensional state space. and Let the first control barrier function and the second control barrier function be represented respectively, where:
[0015]
[0016] In the formula, and For safety margin, For the location information of the obstacle, For the first Location information of the unmanned vessel For the safety radius of the USV, The safe radius of the obstacle. For the first The safe radius of a ship; and Let them be the static obstacle boundary and the inter-ship safety boundary represented by their zero level sets, respectively, starting at any point in time. Its satisfaction And simplify it to ; S22. Construct second-order CBF constraints to ensure the forward invariance of the safe set, ensuring... Its formula is:
[0017] in, and These are first-order CBF safety sets and second-order CBF safety sets, respectively. and For differentiable functions, their formulas are as follows:
[0018]
[0019] Along the system dynamics Taking the derivative, we get:
[0020] according to ,get
[0021] in, and The Lie derivative corresponding to the kinematic model; The exponentially based penalty function, used to adaptively adjust the convergence rate of the CBF constraint, is defined as follows:
[0022]
[0023]
[0024] in, , For positive integers, The distance between the unmanned vessel and the restricted area. For safe distance threshold, To adjust the parameters; An adaptive penalty function; S22. Continuously run a quadratic programming solver to minimize the difference between the optimal collision-free virtual control law and the nominal virtual control law, while satisfying the control obstacle function as a safety constraint. S23. When the difference between the optimal collision-free virtual control law output by the quadratic programming solver and the nominal virtual control law to be solved exceeds a preset threshold, the current time is recorded as the initial time of the collision avoidance maneuver. .
[0025] Furthermore, a safety-priority handover guidance system is defined, including: The safety-priority handover guidance is defined by the following formula:
[0026] in, Indicates the first The state parameters of the topological neighbors of an unmanned vessel. The reference path representing DVS, The initial time for steering and collision avoidance maneuvers, Indicates in Time of the first The status of an unmanned vessel.
[0027] This formula indicates that when At that time, each unmanned surface vessel in the formation followed the global path planned by the L1 virtual vessel for normal tracking. At that time, the first The reference path of the unmanned vessel is switched based on its topological neighbors. The DVS reference path generated by the motion state at each moment, and this path remains unchanged during collision avoidance: when At that time, the first The unmanned vessel's position was restored to its original planned trajectory; During the cornering phase, a guidance strategy is defined, and its formula is:
[0028] in, These represent the starting and ending positions of the steering maneuver, respectively, both determined offline based on preset waypoints. Indicates the first The time period during which the USV navigated the curve section; It is the first The motion state of the USV during this navigation phase.
[0029] Furthermore, based on the globally smooth reference path and the DVS reference path, a distributed error is constructed, and then a nominal virtual control law is designed, including: S41. Define the first based on the global smooth reference path and the DVS reference path. The distributed error of a USV is calculated using the following formula:
[0030] in, It is a formation structure. Represents relative formation structure. This indicates the connectivity relationship between the virtual ship and the USV. The weights are the adjacency matrix weights. S42. Define position error and yaw angle error, and their formulas are as follows:
[0031] in, For positional error, For yaw angle error, The desired yaw angle is given by the following formula:
[0032] in, This represents the positional error along the X-axis in the geodetic coordinate system. This represents the positional error along the Y-axis in the geodetic coordinate system. Function to represent positive and negative signs S43. The derivatives of position error and yaw angle error are given by the following formulas:
[0033] in, For the first The derivative of the USV's location information, As an intermediate variable, its expression is as follows:
[0034] in, For the first The set of all neighboring nodes of the USV. For the first A two-dimensional rotation matrix for a USV; S44. Design a nominal virtual control law based on the derivatives of position error and yaw angle error to stabilize... and Its formula is:
[0035] in, For the virtual control law of position error, For the yaw angle error virtual control law, , This represents the positive design constant.
[0036] Furthermore, based on the aforementioned control obstacle function, an optimal collision-free heading virtual control law is designed, and the nominal virtual control law is corrected in real time to obtain the final virtual control law, including: S51, Secure Feasible Set Transform into about Linear constraint form:
[0037] in, , The normal vector of the constraint boundary. These are normalized constraint boundary values; S52. Design the optimal collision-free heading virtual control law by solving the following quadratic programming problem:
[0038] in, For the optimal collision-free heading virtual control law, Let be the relaxation function. For the feasible set with relaxation parameters, its formula is:
[0039] in,
[0040] To compensate Variables; S53. Based on the nominal virtual control law and the optimal collision-free heading virtual control law, construct the final virtual control law, the formula of which is:
[0041]
[0042] in, The final virtual control law for the direction of movement. This is the final virtual control law for the heading direction.
[0043] Furthermore, an adaptive robust dynamic control law is designed, which includes a control law and an adaptive law, including: S61. Define the dynamic error, and its formula is:
[0044] in, For the dynamic error, its derivative is expressed as:
[0045] S62. The model uncertainty and external disturbances in the nonlinear mathematical model are approximated online using a radial basis function neural network, and the formula is as follows:
[0046] in, , For neural basis functions, For neural network vectors, The neural basis function vector; This refers to the error term in the neural network. satisfy ,in It is the upper bound function of neural basis functions; S63. Reconstruct the derivative expression of the dynamic error into a form that includes neural network approximation:
[0047] in, and For damping; For control input; S64. Define the unknown gain correlation term and perform adaptive estimation: Define the unknown gain correlation term as ; Its estimated value satisfies ; The damping term is adaptively estimated using the following formula:
[0048] S65. The formulas for the design control law and the adaptive law are as follows:
[0049]
[0050] in, This is an intermediate control law. Represented as positive design parameters, It is a positive number.
[0051] Beneficial effects: The multi-ship cooperative collision avoidance control method based on safety-priority switching guidance of the present invention has the following advantages: 1. This invention proposes a safety-critical switching guidance strategy, which introduces DVS to guide the members of the topology connection, effectively avoiding the tracking deviation caused by rigid formation configuration, and prioritizing local navigation safety in narrow water scenarios; 2. In the kinematics module, this invention constructs an optimal heading virtual control law and a control obstacle function to achieve collision avoidance control; it ensures the feasibility of solving the quadratic programming problem and can adaptively adjust the convergence rate of the control obstacle function constraints. This invention fulfills the practical navigation requirements in narrow waterways. Attached Figure Description
[0052] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.
[0053] Figure 1 A flowchart of a multi-ship cooperative collision avoidance control method based on safety-priority switching guidance provided by the present invention; Figure 2 This is a schematic diagram illustrating the collision avoidance principle of the present invention. Figure 3 This is the overall path planning diagram for collision avoidance control in this invention; Figure 4 This is a schematic diagram of the unmanned vessel convoy's navigation trajectory in a narrow waterway, as described in an embodiment of the present invention. Figure 5 This is a schematic diagram of the propeller and rudder input trajectories of an unmanned vessel formation in an embodiment of the present invention; Figure 6 This is a schematic diagram illustrating the positional and heading angle errors of each ship in the formation according to an embodiment of the present invention. Detailed Implementation
[0054] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.
[0055] This embodiment provides a multi-ship cooperative collision avoidance control method based on safety-priority switching guidance, such as... Figure 1 As shown, it includes: S1: Establish a nonlinear mathematical model of a three-degree-of-freedom unmanned surface vessel and generate a globally smooth reference path based on the L1 virtual vessel; S2: Define the obstacle control function and determine the initial time of collision avoidance maneuver by solving a quadratic programming problem in real time; S3: Define a safety priority switching guidance, wherein the safety priority switching guidance is used to generate a DVS reference path based on the motion state of the preceding vessel frozen at the initial time when the initial time is triggered, and switch the guidance target of the following vessel from the real-time state of the preceding vessel to the DVS reference path; S4: Based on the global smooth reference path and the DVS reference path, construct a distributed error, and then design a nominal virtual control law; S5: Based on the control obstacle function, design the optimal collision-free heading virtual control law, and make real-time corrections to the nominal virtual control law to obtain the final virtual control law; S6: Design an adaptive robust dynamic control law, which includes a control law and an adaptive law; estimate the neural network weights and actuator gains online using the adaptive law, and generate the final control command from the control law, so that the unmanned vessel executes the final virtual control law to achieve collision avoidance control.
[0056] In a specific embodiment, the scheme for establishing a nonlinear mathematical model of a three-degree-of-freedom unmanned surface vessel and generating a globally smooth reference path based on an L1 virtual vessel is as follows: A nonlinear mathematical model of a three-degree-of-freedom unmanned surface vessel is established, and its formula is as follows:
[0057]
[0058] in, and This represents the ship's position and heading angle in a fixed coordinate system. Forward velocity, drift velocity, and bow roll rate. For the uncertain parameters of the ship model, For unknown functions, For the time-varying disturbances experienced by the USV during forward movement, drift, and yaw; and These represent control inputs related to the main engine speed and rudder angle, respectively. and For time-varying actuator gain; Figure 2A schematic diagram of the guidance algorithm of this invention during collision avoidance is provided. In the diagram, RS represents the reference ship, the RS path is the global reference path during normal formation tracking (i.e., L1 virtual ship / original planned path), and DVS represents the dynamic virtual ship. The yellow ship shape and green solid line in the diagram represent the "DVS path". After collision avoidance is triggered, the reference path switches to one based on topological neighbors. The DVS reference path generated in real time is used to avoid fixed obstacles / no-go zones, etc. Figure 3 Taking a formation of three USVs (Unmanned Surface Vessels) as an example, the geometry and guidance relationships of the USVs during waypoint tracking and obstacle avoidance based on the L1 virtual ship (VS), reference ship (RS), and dynamic virtual ship (DVS) are illustrated, showing the change in guidance signals during waypoint turns. In the figure, the red dotted line represents the original path of the rigid formation. The blue dashed line represents the virtual path using the proposed algorithm. This path is generated based on the L1VS (L1 virtual ship) guidance framework, without considering any ship inertia or uncertainties (a virtual ship refers to an ideal hull that does not consider ship damping and inertia). [Figure...] W i-1 , W i and W i+1 The actual USV tracks the smooth waypoint path generated by RS or VS. When there are obstacle zones near the path or the original path is unsafe: switch to DVS dynamic virtual ship reference, allowing the reference trajectory to transition from the original red / disturbed path to the green waypoint, i.e., the DVS path, avoiding entering the obstacle zone. The underlying layer then uses L1 guidance angle, reference heading, speed, etc., to drive the actual USV to track this dynamic smooth reference.
[0059] Waypoint information W 1, W 2,…, W n Global navigation information is pre-defined for the mission guidance layer. Operators determine this information based on electronic charts before the mission begins. The primary task of L1VS is to plan a smooth reference path for the entire ship formation based on waypoint information; that is, to generate a globally smooth reference path based on the L1 virtual ship, including: Based on the L1 virtual ship, according to the preset waypoint information W 1, W 2,…, W n The formula for generating a globally smooth reference path is as follows:
[0060] in, and Given the desired position and heading angle of the leader L1VS, and These represent the reference forward speed and bow angular velocity of the L1 virtual ship, respectively. from Figure 3 It can be seen from the straight line segment In the middle, the angular velocity of the bow. For the curve segment angular velocity of the bow , ,in It is a connection arrive line segments, express direction and The angle between directions, This represents the turning acceleration at the waypoint. The turning radius is and These are the starting and ending positions for steering maneuvers. This guidance algorithm does not require computation time, thus eliminating accumulated errors during turning.
[0061] In a specific embodiment, the scheme for defining a control obstacle function and determining the initial time of collision avoidance maneuvers by solving a quadratic programming problem in real time is as follows: S21. Define the control barrier function as follows: The nonlinear mathematical model is rewritten as follows:
[0062] in, , , , The angular velocity of the bow; and This is the rewritten system matrix; Define a first control obstacle function and a second control obstacle function to quantify the distance between the unmanned surface vessel (USV) and static obstacles, and the distance between the USV and other USVs, respectively. Their formulas are as follows:
[0063] in, It is a two-dimensional state space. and Let the first control barrier function and the second control barrier function be represented respectively, where:
[0064]
[0065] In the formula, and For safety margin, For the location information of the obstacle, For the first Location information of the unmanned vessel For the safety radius of the USV, The safe radius of the obstacle. For the first The safe radius of a ship; and Let them be the static obstacle boundary and the inter-ship safety boundary represented by their zero level sets, respectively, starting at any point in time. Its satisfaction And simplify it to ; S22. Construct second-order CBF constraints to ensure the forward invariance of the safe set, ensuring... Its formula is:
[0066] in, and These are first-order CBF safety sets and second-order CBF safety sets, respectively. and For differentiable functions, their formulas are as follows:
[0067]
[0068] Along the system dynamics Taking the derivative, we get:
[0069] according to ,get
[0070] in, and The Lie derivative corresponding to the kinematic model; The exponentially based penalty function, used to adaptively adjust the convergence rate of the CBF constraint, is defined as follows:
[0071]
[0072]
[0073] in, , For positive integers, The distance between the unmanned vessel and the restricted area. For safe distance threshold, To adjust the parameters; An adaptive penalty function; S22. Continuously run a quadratic programming solver to minimize the difference between the optimal collision-free virtual control law and the nominal virtual control law, while satisfying the control obstacle function as a safety constraint. S23. When the difference between the optimal collision-free virtual control law output by the quadratic programming solver and the nominal virtual control law to be solved exceeds a preset threshold, the current time is recorded as the initial time of the collision avoidance maneuver. .
[0074] In a specific embodiment, a safety-priority switching guidance is defined. This safety-priority switching guidance is used to generate a DVS reference path based on the motion state of the preceding vessel frozen at the initial time when the initial time is triggered, and to switch the guidance target of the following vessel from the aforementioned real-time state of the preceding vessel to the DVS reference path. Unlike in open water, USV formations have limited maneuverability in confined waters, especially during turns and collision avoidance. Therefore, prioritizing safety in specific areas is unavoidable. To address this, a safety-priority switching guidance system is proposed to handle various situations. The formula is as follows:
[0075] in, Indicates the first The state parameters of the topological neighbors of an unmanned vessel. The reference path representing DVS, The initial time for steering and collision avoidance maneuvers, Indicates in Time of the first Status parameters of an unmanned vessel It is a positive number; This formula indicates that when At that time, each unmanned surface vessel in the formation followed the global path planned by the L1 virtual vessel for normal tracking. At that time, the first The reference path of the unmanned vessel is switched based on its topological neighbors. The DVS reference path generated by the motion state at each moment, and this path remains unchanged during collision avoidance: when At that time, the first The unmanned vessel's position was restored to its original planned trajectory; In waypoint-based formation control, overshoot during turning is the core cause of collision risk. Therefore, during the curve turning phase, a guidance strategy is defined, with the following formula:
[0076] in, These represent the starting and ending positions of the steering maneuver, respectively, both determined offline based on preset waypoints, such as... Figure 2 As shown, Indicates the first The time period during which the USV navigated the curve section; It is the first The motion state of the USV during this navigation phase.
[0077] In a specific embodiment, the scheme for constructing a distributed error based on the globally smooth reference path and the DVS reference path, and then designing the nominal virtual control law, is as follows: S41. To achieve coordinated USV formation maneuvering guided by the virtual pilot ship, each USV must have the ability to obtain its topological neighbor state information. Therefore, based on the global smooth reference path and the DVS reference path, the first... The distributed error of a USV is calculated using the following formula:
[0078] in, It is a formation structure. Represents relative formation structure. This indicates the connectivity relationship between the virtual ship and the USV. The weights are the adjacency matrix weights. S42. Define position error and yaw angle error, and their formulas are as follows:
[0079] in, For positional error, For yaw angle error, The desired yaw angle is given by the following formula:
[0080] in, This represents the positional error along the X-axis in the geodetic coordinate system. This represents the positional error along the Y-axis in the geodetic coordinate system. Function to represent positive and negative signs S43. The derivatives of position error and yaw angle error are given by the following formulas:
[0081] in, For the first The derivative of the USV's location information, As an intermediate variable, its expression is as follows:
[0082] in, For the first The set of all neighboring nodes of the USV. For the first A two-dimensional rotation matrix for a USV; S44. Design a nominal virtual control law based on the derivatives of position error and yaw angle error to stabilize... and Its formula is:
[0083] in, For the virtual control law of position error, For the yaw angle error virtual control law, , This represents the positive design constant.
[0084] In a specific embodiment, in narrow waterways, static and dynamic obstacles, the shoreline, and passing vessels collectively constitute the main factors threatening the navigation safety of the USA formation. To address this issue, based on the aforementioned obstacle control function, an optimal collision-free heading virtual control law is designed. The nominal virtual control law is then modified in real-time to obtain the final virtual control law: S51, Secure Feasible Set Transform into about Linear constraint form:
[0085] in, , The normal vector of the constraint boundary. These are normalized constraint boundary values; S52. Design the optimal collision-free heading virtual control law by solving the following quadratic programming problem:
[0086] in, For the optimal collision-free heading virtual control law, Let be the relaxation function. For the feasible set with relaxation parameters, its formula is:
[0087] in,
[0088] To compensate Variables; S53. Based on the nominal virtual control law and the optimal collision-free heading virtual control law, construct the final virtual control law, the formula of which is:
[0089]
[0090] in, The final virtual control law for the direction of movement. This is the final virtual control law for the heading direction.
[0091] In a specific embodiment, an adaptive robust dynamic control law is designed, which includes a control law and an adaptive law. The adaptive law is used to estimate the neural network weights and actuator gains online, and the control law generates the final control command, enabling the unmanned vessel to execute the final virtual control law. The collision avoidance control scheme is as follows: S61. Define the dynamic error, and its formula is:
[0092] in, For the dynamic error, its derivative is expressed as:
[0093] S62. The model uncertainty and external disturbances in the nonlinear mathematical model are approximated online using a radial basis function neural network, and the formula is as follows:
[0094] in, , For neural basis functions, For neural network vectors, The neural basis function vector; This refers to the error term in the neural network. satisfy ,in It is the upper bound function of neural basis functions; S63. Reconstruct the derivative expression of the dynamic error into a form that includes neural network approximation:
[0095] in, and For damping; For control input; S64. Define the unknown gain correlation term and perform adaptive estimation: Define the unknown gain correlation term as ; Its estimated value satisfies ; The damping term is adaptively estimated using the following formula:
[0096] S65. The formulas for the design control law and the adaptive law are as follows:
[0097]
[0098] in, This is an intermediate control law. Represented as positive design parameters, This is a positive constant. This technology can compensate for uncertainties online.
[0099] To verify the effectiveness of the algorithm proposed in this invention, a formation of three ships was used as the controlled object, with each ship being 38m long and having a displacement of 1.18m. A 105kg under-propulsion vessel. A digital simulation experiment was conducted to design a navigation mission in narrow waters within a marine convoy, with a planned route consisting of 4 waypoints. , , , The initial motion state of the corresponding unmanned surface vessel formation is as follows: , , .
[0100] Desired path tracking speed is . Figures 4-6 The results are shown in the MATLAB simulation on the platform, which simulates the formation of unmanned vessels in narrow waters under sea state 4. Figure 4 This diagram shows the trajectory of a USV formation performing a collision avoidance mission in narrow waters. USV1, USV2, and USV3 are three unmanned surface vessels (USVs) in the formation. Figure 4 It can be seen that the present invention is applicable to collision avoidance in extremely small operating environments, obstacle avoidance of fixed obstacles and obstacle avoidance of dynamic obstacles, and has demonstrated excellent results.
[0101] Figure 5 The control inputs for the formation are shown in the diagram. , and For propeller input, , and The magnified view of the rudder input shows the superiority of the invention. The gray area represents the active period of collision avoidance / safety constraints or path switching; the right side is a magnified view of the local steady state / dynamic state, indicating that the system eventually becomes stable and the steering correction is reasonable.
[0102] Figure 6 This indicates the positional error of each ship in the formation ( ) and attitude error ( The gray area represents the period of collision avoidance / DVS switching or active safety constraints; clearly, the error remains within a reasonable range, increases briefly during switching, and remains close to zero at other times, indicating that guidance and collision avoidance control are effective.
[0103] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.
Claims
1. A multi-ship cooperative collision avoidance control method based on safety-priority switching guidance, characterized in that, include: S1: Establish a nonlinear mathematical model of a three-degree-of-freedom unmanned surface vessel and generate a globally smooth reference path based on the L1 virtual vessel; S2: Define the obstacle control function and determine the initial time of collision avoidance maneuver by solving a quadratic programming problem in real time; S3: Define a safety priority switching guidance, wherein the safety priority switching guidance is used to generate a DVS reference path based on the motion state of the preceding vessel frozen at the initial time when the initial time is triggered, and switch the guidance target of the following vessel from the real-time state of the preceding vessel to the DVS reference path; S4: Based on the global smooth reference path and the DVS reference path, construct a distributed error, and then design a nominal virtual control law; S5: Based on the control obstacle function, design the optimal collision-free heading virtual control law, and make real-time corrections to the nominal virtual control law to obtain the final virtual control law; S6: Design an adaptive robust dynamic control law, which includes a control law and an adaptive law; estimate the neural network weights and actuator gains online using the adaptive law, and generate the final control command from the control law, so that the unmanned vessel executes the final virtual control law to achieve collision avoidance control.
2. The multi-ship cooperative collision avoidance control method based on safety-priority switching guidance according to claim 1, characterized in that, A nonlinear mathematical model of a three-degree-of-freedom unmanned surface vessel (USV) is established, and a globally smooth reference path is generated based on an L1 virtual vessel, including: A nonlinear mathematical model of a three-degree-of-freedom unmanned surface vessel is established, and its formula is as follows: in, and This represents the ship's position and heading angle in a fixed coordinate system. Forward velocity, drift velocity, and bow roll rate. For the uncertain parameters of the ship model, For unknown functions, For the time-varying disturbances experienced by the USV during forward movement, drift, and yaw; and These represent control inputs related to the main engine speed and rudder angle, respectively. and For time-varying actuator gain; Generate a globally smooth reference path based on the L1 virtual ship, including: Based on the L1 virtual ship, according to the preset waypoint information W 1, W 2,…, W n The formula for generating a globally smooth reference path is as follows: in, and Given the desired position and heading angle of the leader L1VS, and These represent the reference forward speed and bow angular velocity of the L1 virtual ship, respectively. In the straight line segment In the middle, the angular velocity of the bow. For the curve segment angular velocity of the bow , ,in It is a connection arrive line segments, express direction and The angle between directions, This represents the turning acceleration at the waypoint. The turning radius is and This refers to the starting and ending positions of the steering maneuver.
3. The multi-ship cooperative collision avoidance control method based on safety-priority switching guidance according to claim 2, characterized in that, Define a control obstacle function and determine the initial time for collision avoidance maneuvers by solving a quadratic programming problem in real time, including: S21. Define the control barrier function as follows: The nonlinear mathematical model is rewritten as follows: in, , , , The angular velocity of the bow; and This is the rewritten system matrix; Define a first control obstacle function and a second control obstacle function to quantify the distance between the unmanned surface vessel (USV) and static obstacles, and the distance between the USV and other USVs, respectively. Their formulas are as follows: in, It is a two-dimensional state space. and Let the first control barrier function and the second control barrier function be represented respectively, where: In the formula, and For safety margin, For the location information of the obstacle, For the first Location information of the unmanned vessel For the safety radius of the USV, The safe radius of the obstacle. For the first The safe radius of a ship; and Let them be the static obstacle boundary and the inter-ship safety boundary represented by their zero level sets, respectively, starting at any point in time. Its satisfaction And simplify it to ; S22. Construct second-order CBF constraints to ensure the forward invariance of the safe set, ensuring... Its formula is: in, and These are first-order CBF safety sets and second-order CBF safety sets, respectively. and For differentiable functions, their formulas are as follows: Along the system dynamics Taking the derivative, we get: according to ,get in, and The Lie derivative corresponding to the kinematic model; The exponentially based penalty function, used to adaptively adjust the convergence rate of the CBF constraint, is defined as follows: in, , For positive integers, The distance between the unmanned vessel and the restricted area. For safe distance threshold, To adjust the parameters; An adaptive penalty function; S22. Continuously run a quadratic programming solver to minimize the difference between the optimal collision-free virtual control law and the nominal virtual control law, while satisfying the control obstacle function as a safety constraint. S23. When the difference between the optimal collision-free virtual control law output by the quadratic programming solver and the nominal virtual control law to be solved exceeds a preset threshold, the current time is recorded as the initial time of the collision avoidance maneuver. .
4. The multi-ship cooperative collision avoidance control method based on safety-priority switching guidance according to claim 3, characterized in that, Define a safety-priority handover guidance system, including: The safety-priority handover guidance is defined by the following formula: in, Indicates the first The state parameters of the topological neighbors of an unmanned vessel. The reference path representing DVS, The initial time for steering and collision avoidance maneuvers, Indicates in Time of the first Status parameters of an unmanned vessel It is a positive number; This formula indicates that when At that time, each unmanned surface vessel in the formation followed the global path planned by the L1 virtual vessel for normal tracking. At that time, the first The reference path of the unmanned vessel is switched based on its topological neighbors. The DVS reference path generated by the motion state at each moment, and this path remains unchanged during collision avoidance: when At that time, the first The unmanned vessel's position was restored to its original planned trajectory; During the cornering phase, a guidance strategy is defined, and its formula is: in, These represent the starting and ending positions of the steering maneuver, respectively, both determined offline based on preset waypoints. Indicates the first The time period during which the USV navigated the curve section; It is the first The motion state of the USV during this navigation phase.
5. The multi-ship cooperative collision avoidance control method based on safety-priority switching guidance according to claim 4, characterized in that, Based on the global smooth reference path and the DVS reference path, a distributed error is constructed, and then a nominal virtual control law is designed, including: S41. Define the first based on the global smooth reference path and the DVS reference path. The distributed error of a USV is calculated using the following formula: in, It is a formation structure. Represents relative formation structure. This indicates the connectivity relationship between the virtual ship and the USV. The weights are the adjacency matrix weights. S42. Define position error and yaw angle error, and their formulas are as follows: in, For positional error, For yaw angle error, The desired yaw angle is given by the following formula: in, This represents the positional error along the X-axis in the geodetic coordinate system. This represents the positional error along the Y-axis in the geodetic coordinate system. Function to represent positive and negative signs S43. The derivatives of position error and yaw angle error are given by the following formulas: in, For the first The derivative of the USV's location information, As an intermediate variable, its expression is as follows: in, For the first The set of all neighboring nodes of the USV. For the first A two-dimensional rotation matrix for a USV; S44. Design a nominal virtual control law based on the derivatives of position error and yaw angle error to stabilize... and Its formula is: in, For the virtual control law of position error, For the yaw angle error virtual control law, , This represents the positive design constant.
6. The multi-ship cooperative collision avoidance control method based on safety-priority switching guidance according to claim 5, characterized in that, Based on the aforementioned control obstacle function, an optimal collision-free heading virtual control law is designed. The nominal virtual control law is then modified in real time to obtain the final virtual control law, including: S51, Secure Feasible Set Transform into about Linear constraint form: in, , The normal vector of the constraint boundary. These are normalized constraint boundary values; S52. Design the optimal collision-free heading virtual control law by solving the following quadratic programming problem: in, For the optimal collision-free heading virtual control law, Let be the relaxation function. For the feasible set with relaxation parameters, its formula is: in, To compensate Variables; S53. Based on the nominal virtual control law and the optimal collision-free heading virtual control law, construct the final virtual control law, the formula of which is: in, The final virtual control law for the direction of movement. This is the final virtual control law for the heading direction.
7. The multi-ship cooperative collision avoidance control method based on safety-priority switching guidance according to claim 6, characterized in that, Design an adaptive robust dynamic control law, which comprises a control law and an adaptive law, including: S61. Define the dynamic error, and its formula is: in, For the dynamic error, its derivative is expressed as: S62. The model uncertainty and external disturbances in the nonlinear mathematical model are approximated online using a radial basis function neural network, and the formula is as follows: in, , For neural basis functions, For neural network vectors, The neural basis function vector; This refers to the error term in the neural network. satisfy ,in It is the upper bound function of neural basis functions; S63. Reconstruct the derivative expression of the dynamic error into a form that includes neural network approximation: in, and For damping; For control input; S64. Define the unknown gain correlation term and perform adaptive estimation: Define the unknown gain correlation term as ; Its estimated value satisfies ; The damping term is adaptively estimated using the following formula: S65. The formulas for the design control law and the adaptive law are as follows: in, This is an intermediate control law. Represented as positive design parameters, It is a positive number.