Underactuated unmanned surface vehicle safe formation tracking control method based on robust control obstacle

Abstract

The invention discloses an under-actuated unmanned surface vehicle safe formation tracking control method based on robust obstacle control. The method comprises the following steps: establishing a multi-unmanned ship group and performing real-time communication, and enabling each follower to follow a leader according to a formation configuration and to avoid an obstacle; a safe formation tracking control framework of the underactuated unmanned surface vehicle is established, during formation tracking, followers input state information into the control framework, processing is carried out according to a reference trajectory of a leader and real-time information of a current follower and an obstacle, and forward and steering control torque instructions are output to control the current follower. Obstacles are avoided and formation configuration is restored to continue following the leader. According to the method, the underactuation characteristic problem of the unmanned surface vehicle is solved, the worst case error generated when the neural network approaches the unknown dynamic state can be effectively compensated, it is ensured that the unmanned surface vehicle can strictly avoid collision with the obstacle in the complex environment, and meanwhile the formation tracking performance is optimally kept.

Description

TECHNICAL FIELD

[0001] The present application relates to a kind of unmanned ship safety formation tracking control method, specifically, specifically relates to a kind of underactuated unmanned ship safety formation tracking control method based on robust control barrier. BACKGROUND

[0002] In recent years, as a new intelligent water surface platform, unmanned ship has attracted wide attention in the field of ocean exploration. Compared with single unmanned ship, the system composed of multiple unmanned ships shows stronger flexibility, robustness and scalability when performing complex sea tasks. Among many cooperative scenarios, multi-unmanned ship formation tracking is a core technology, which requires multiple unmanned ships to track a predetermined trajectory while maintaining a specific geometric configuration, which has important application value in the fields of ocean resource exploration and cargo transportation.

[0003] However, existing research on unmanned ship formation control mostly focuses on formation keeping under specific communication or fault conditions, and less considers active safety obstacle avoidance in complex environments. In practical applications, ensuring safety during formation navigation is an unavoidable and challenging issue, especially in dense or unknown environments, where the risk of collision between unmanned ships and between unmanned ships and external obstacles is extremely high.

[0004] To address this safety issue, although some control methods based on state constraints have emerged, these methods have fundamental flaws when applied to unmanned ships: on the one hand, the design of existing methods usually relies on accurate system dynamics models and requires the control system to be fully driven; on the other hand, when unmanned ships are sailing, they are affected by wind, waves, currents and complex hydrodynamic forces, and their dynamics models have strong nonlinearity and time-varying uncertainty. Accurate modeling is almost impossible in engineering practice, and unmanned ships are typically underactuated systems. This contradiction between theoretical assumptions and physical reality leads to the application of existing constraint methods to such underactuated systems, which may produce no solution or incorrect control instructions due to model mismatch, and the safety constraints cannot be satisfied mathematically and physically, so they are essentially ineffective. Therefore, how to provide reliable safety guarantees for underactuated unmanned ships under model uncertainty is a technical problem that needs to be solved in the field of formation control. SUMMARY

[0005] In order to solve the problems in the background art, the application provides a kind of underactuated unmanned ship safety formation tracking control method based on robust control barrier. The application is a kind of robust formation tracking control method for underactuated multi-unmanned ship system in complex environment containing unknown dynamics and external obstacles, which can ensure safety, to stably maintain the preset formation and track the expected trajectory under the premise of ensuring that each unmanned ship strictly avoids collision with obstacles and neighboring ships, especially in the case of unknown unmanned ship dynamics model and external disturbance.

[0006] The technical solution adopted by the application is:

[0007] The underactuated unmanned ship safety formation tracking control method based on robust control barrier of the application comprises:

[0008] Step 1) Establish a multi-unmanned ship group including one leader and several followers. The unmanned ship as the leader sails according to the preset reference trajectory of the formation tracking task, each follower follows the leader and avoids obstacles according to the preset formation configuration, and the multi-unmanned ship group communicates in real time through a multi-ship communication network, and each unmanned ship transmits data according to its own communication range.

[0009] Step 2) Based on the kinematics and dynamics model of the unmanned ship, an underactuated unmanned ship safety formation tracking control framework is established, including a double-loop formation tracking controller, a radial basis neural network adaptive controller and a safety filter based on a robust control barrier function.

[0010] Step 3) When the multi-unmanned ship group is in formation tracking, each follower inputs its actual and the estimated state information of the leader of the adjacent unmanned ship into the underactuated unmanned ship safety formation tracking control framework, which simultaneously processes the reference trajectory of the leader, the real-time state information, the speed information of the current follower and the external obstacle information, and outputs safe forward control force instructions and steering control torque instructions that meet the physical constraints of the actuator to control the current follower to avoid external obstacles and restore the preset formation configuration to continue to follow the leader. The current follower feeds back the estimated state information of the leader to the adjacent follower through the multi-ship communication network in real time, realizes safe closed-loop control, and completes the formation tracking task until the formation tracking task is completed.

[0011] In step 2), the dual-loop formation tracking controller includes a cooperative leader estimator, a second-order filter, and a trajectory tracking control law connected in sequence. During processing, the underactuated unmanned surface vessel (USV) safe formation tracking control framework inputs the estimated leader state information from neighboring USVs into the cooperative leader estimator to obtain the estimated leader state in a distributed manner. It also inputs the leader's reference trajectory, processes it, and outputs the current follower's desired trajectory. After smoothing by the second-order filter, it generates the current follower's reference trajectory. The current follower's reference trajectory and its actual state information are input into the trajectory tracking control law, which then outputs the current follower's nominal forward velocity and nominal turning angular velocity, and inputs them respectively into the radial... In the radial basis neural network adaptive controller and safety filter, the safety filter optimizes the nominal control command by solving a quadratic programming problem. After processing, the radial basis neural network adaptive controller outputs the nominal forward control force of the current follower to the safety filter. The safety filter simultaneously receives information about external obstacles near the current follower, processes it, and outputs a safe forward control force command that satisfies the physical constraints of the actuator to control the current follower. At the same time, it outputs a steering angular velocity command to the radial basis neural network adaptive controller. The radial basis neural network adaptive controller simultaneously receives real-time speed information fed back by the current follower, processes it, outputs the nominal forward control force to the full filter, and outputs a steering control torque command to control the current follower.

[0012] In step 2), the cooperative leader estimator of the dual-ring formation tracking controller is specifically as follows:

[0013]

[0014] in, and The estimated position of leader l for the i-th follower. speed and heading angle The derivative of The estimated position of leader l for the j-th follower. The heading angle of the leader l estimated by the j-th follower; p l and φ l κ1, κ2, and κ3 represent the actual position and heading angle of the leader l in the reference trajectory, respectively; κ1, κ2, and κ3 represent the first, second, and third positive constant gains, respectively; a ij Let R be the communication topology connection weight between the i-th and j-th followers; R() is the rotation matrix; δ i-j Let δ be the expected formation offset between the i-th and j-th followers. i-j =δ i -δ j δ i and δ jb represents the formation bias of the i-th and j-th followers relative to the leader l position, respectively; i Let b be the connection weight between the i-th follower and the leader l. i When b = 1, the i-th follower directly obtains the leader's information; when b = 1, the i-th follower directly obtains the leader's information. i When p = 0, the i-th follower cannot directly obtain leader information; d,i Let be the expected trajectory of the i-th follower.

[0015] In step 2), the second-order filter of the dual-loop formation tracking controller is specifically as follows:

[0016]

[0017] Where, p r,i and Let q be the reference trajectory of the i-th follower and its derivative. r,i and Let be the reference velocity and its derivative of the i-th follower, respectively; γ1 is a filter constant greater than zero; p d,i Let φ be the expected trajectory of the i-th follower; r,i Let i be the reference heading angle of the i-th follower. The estimated heading angle of the leader l for the i-th follower.

[0018] In step 2), the trajectory tracking control law of the dual-ring formation tracking controller is as follows:

[0019]

[0020] k = k * ‖q r,i ||

[0021] Among them, u r,i and r r,i These are the nominal forward speed command and turning angular velocity command for the i-th follower, respectively; u d,i Let u be the speed of the reference trajectory of the i-th follower. d,i =‖q r,i ‖,q r,i Let c1, c2, c3, and k be the reference velocities of the i-th follower. * These represent the first, second, third, and fourth normal control gains, respectively, and k is the lateral error adjustment gain related to the reference speed; x e,i y e,i and φ e,i Let r be the tracking error in the forward and lateral directions, and the tracking error in the bow angle, respectively, in the hull coordinate system of the i-th follower; d,i Let be the heading angular velocity of the reference trajectory of the i-th follower. Let be the derivative of the reference heading angle of the i-th follower; σ is a geometric factor related to coordinate transformation. e,i To incorporate the combined error term of the heading angle error and lateral error of the i-th follower; ζ σ This is the nonlinear coupling compensation term in the dynamics of composite error; p is the rotation matrix from the hull coordinate system to the global coordinate system; i and φ i Let p be the actual position and heading angle of the i-th follower, respectively; r,i and φ r,i These are the reference trajectory and reference heading angle of the i-th follower, respectively.

[0022] In step 2), the radial basis function neural network adaptive controller is specifically as follows:

[0023]

[0024] in, τ is the nominal forward control force of the i-th follower; r,i For the steering control torque command of the i-th follower; m u,i and m r,i These are the forward and forward inertial parameters, respectively, including the added mass; Let be the derivative of the nominal forward speed command of the i-th follower; The derivative of the steering angular velocity command of the i-th follower output by the safety filter; and These are the known forward and bow dynamics terms, respectively; and These are the first and second weight estimation vectors for the i-th follower in a radial basis function neural network, respectively; Φ m,i Let Φ be a column vector containing n radial basis functions for the i-th follower, m∈{u,r}. m,i (χ m,i )=[φ m,1 (χ m,i ),φ m,2 (χ m,i ),...,φ m,n (χ m,i )] T , χ m,i Let φ be the input vector of the corresponding channel of the radial basis function neural network of the i-th follower. m,1 (χ m,i ), φ m,2 (χ m,i ), ..., φ m,n (χ m,ic1, c2, ..., cn are the 1st, 2nd, ..., nth radial basis functions in the column vector of the i-th follower; c4, c5, c6, and c7 are the fifth, sixth, seventh, and eighth positive constant control gains, respectively; u e,i Let u be the forward velocity tracking error of the i-th follower. e,i =u i -u r,i r e,i Let be the tracking error of the i-th follower's turning angular velocity. u i and r i Let u be the actual forward velocity and turning angular velocity of the i-th follower, respectively. r,i and These are the nominal forward speed command for the i-th follower and the steering angular velocity command output by the safety filter, respectively.

[0025] The weights of a radial basis function neural network are updated through its adaptive law.

[0026] In step 2), the safety filter based on the robust control barrier function is specifically as follows:

[0027]

[0028]

[0029] in, For optimization variables in a quadratic programming problem, and These are the forward control force command and steering angular velocity command for the i-th follower, respectively, output by the safety filter. The input is the nominal control command. and r r,i The nominal forward control force and steering angular velocity commands of the i-th follower are respectively input to the radial basis function neural network adaptive controller and the dual-loop formation tracking controller; H i and Ω i Let u be the matrix and vector corresponding to the safety constraints of the i-th follower; max and u min The upper and lower limits of the physical constraints of the unmanned surface vessel actuators are the optimization variables of the quadratic programming problem, respectively; α i,o Let m be the relative encounter angle between the i-th follower and the obstacle; u,i The forward inertial parameter includes the added mass; u i Let ψ be the actual forward velocity of the i-th follower; 1,i and ψ 2,i These are the first and second nonlinear terms containing system dynamics, respectively; α1() and α2() are the first and second extended nonlinear terms, respectively. Class function; h1,i and h 2,i Here, a represents the obstacle functions used to guarantee the safe distance and safe encounter angle for the i-th follower; max d represents the maximum deceleration. i,o Let be the distance between the i-th follower and the obstacle; d0 is the minimum safe distance; δ0 is a positive constant used to limit the encounter angle; Forward dynamics terms that are known; Φ is the first weight estimate vector for the i-th follower in a radial basis function neural network; u,i Let σ be the first type of column vector containing n radial basis functions for the i-th follower; 1,i For the i-th follower, there is a robust compensation term used to compensate for the approximation error of the neural network; This is the upper bound of the neural network weight estimation error; This is the upper bound of the neural network approximation error.

[0030] The present invention provides a robust control obstacle-based safe formation tracking control system for underactuated unmanned surface vessels, comprising:

[0031] The system building module is used to establish a safe formation tracking control framework for underactuated unmanned surface vessels, including a dual-loop formation tracking controller, a radial basis function neural network adaptive controller, and a safety filter based on a robust control obstacle function.

[0032] The tracking control module, when multiple unmanned surface vessels (USVs) are tracking each other in formation, inputs the actual state information of each follower and the estimated state information of the leader from adjacent USVs into the underactuated USV safe formation tracking control framework. The underactuated USV safe formation tracking control framework processes the leader's reference trajectory, the real-time state information, speed information, control torque of the current follower, and external obstacle information. After processing, it outputs forward control force commands and steering control torque commands to control the current follower to avoid external obstacles and restore the preset formation configuration to continue following the leader.

[0033] This invention addresses the challenges of underactuated unmanned surface vessels (USVs) with unknown dynamics. It designs a formation controller integrating cooperative leader estimation and dual-loop tracking control to generate nominal control commands that enable USVs to track a desired formation trajectory under partial communication conditions. Furthermore, to address collision safety during formation, this invention designs a robust control obstacle function-based safety filter. This filter optimally modifies the nominal commands by solving a quadratic programming problem, ensuring that USVs strictly avoid collisions within physical constraints. Specifically, this invention designs an adaptive dynamic controller based on a radial basis function neural network (RBN) to compensate for and suppress model uncertainties caused by unknown hydrodynamics and external environmental disturbances. The upper bound of the neural network's approximation error is explicitly incorporated as a robust compensation term into the constraints of the safety filter, theoretically guaranteeing effective safety avoidance even in the worst-case scenario with model uncertainties. Through these designs, a safety-critical formation tracking control system for underactuated multi-USV systems is ultimately achieved under the presence of model uncertainties and external disturbances.

[0034] The beneficial effects of this invention are:

[0035] 1. This invention decouples the complex coupled safety constraints of underactuated unmanned surface vessels (USVs) into linear constraints on forward force and yaw rate by designing a control obstacle function based on geometric transformation. This fundamentally solves the problem of the infeasibility of applying traditional constraint control methods to underactuated systems and provides an effective way to achieve reliable and safe control of USVs.

[0036] 2. This invention designs an adaptive controller based on radial basis function neural network to approximate and compensate for unknown nonlinear dynamics and external disturbances. In particular, a robust compensation term is innovatively introduced into the safety filter. This compensation term is used to define and offset the worst-case approximation error of the neural network, thereby theoretically ensuring that the collision avoidance safety guarantee remains strict and effective even when the model is severely uncertain. This solves the core defect of existing methods that cannot guarantee safety due to reliance on accurate models.

[0037] 3. The adaptive dynamics controller based on radial basis function neural networks designed in this invention enables unmanned surface vessels (USVs) to track the desired velocity commands generated by the formation kinematics controller and safety filter in real time and accurately, even in the presence of unknown nonlinear hydrodynamics and external environmental disturbances. This controller continuously optimizes network weights through online adaptive laws, effectively compensating for model uncertainties and significantly improving the tracking accuracy and robustness of the underlying velocity loop, thereby ensuring high-precision maintenance of the upper-level formation configuration and dynamic response performance.

[0038] 4. This invention systematically integrates a high-performance formation tracking controller and a robust safety filter into a unified framework. By treating safety constraints as hard conditions in a quadratic programming problem, the tracking control commands are modified to minimize their impact while ensuring safety. This achieves an optimal balance between obstacle avoidance and formation tracking performance, significantly improving the unmanned surface vessel's mission execution capability and the robustness of the overall control system in complex and uncertain environments. Attached Figure Description

[0039] Figure 1 This is a block diagram of the tracking control method of the present invention;

[0040] Figure 2 This is a simulation trajectory diagram of the unmanned surface vessel formation in the simulation experiment of this invention embodiment;

[0041] Figure 3 This is a graph showing the unmanned surface vessel (USV) formation tracking error according to an embodiment of the present invention. Figure 3 (a) is a graph showing the variation of the X-direction component of the tracking error over time. Figure 3 (b) is a graph showing the change of the Y-direction component of the tracking error over time.

[0042] Figure 4 This is a schematic diagram of the controller performance of unmanned surface vessel No. 1 in this embodiment of the invention, wherein, Figure 4 (a) shows the adaptive fitting effect of the radial basis function neural network on the system's uncertainty term. Figure 4 (b) is the control input curve of the unmanned surface vessel during the entire simulation process. Detailed Implementation

[0043] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention. Furthermore, the technical features involved in the various embodiments of this invention described below can be combined with each other as long as they do not conflict with each other.

[0044] The present invention provides a safe formation tracking control method for underactuated unmanned surface vessels based on robust control obstacles, as detailed below:

[0045] Step 1) Establish a multi-unmanned surface vessel (USV) group consisting of a leader and several followers. The USV acting as the leader navigates according to the preset reference trajectory of the formation tracking mission. Each follower follows the leader according to the preset formation configuration and avoids obstacles. The multi-USV group communicates in real time through a multi-vessel communication network, and each USV transmits data according to its own communication range.

[0046] Step 2) Establish a safe formation tracking control framework for underactuated unmanned surface vessels based on their kinematic and dynamic models, including a dual-loop formation tracking controller, a radial basis function neural network adaptive controller, and a safety filter based on a robust control obstacle function.

[0047] Step 3) When multiple unmanned surface vessels (USVs) are performing formation tracking, each follower inputs its own actual state information and the estimated state information of the leader from adjacent USVs into the underactuated USV safe formation tracking control framework. The underactuated USV safe formation tracking control framework processes the leader's reference trajectory, the real-time state information and speed information of the current follower, and the external obstacle information. After processing, it outputs safe forward control force commands and steering control torque commands that satisfy the physical constraints of the actuators to control the current follower to avoid external obstacles and restore the preset formation configuration to continue following the leader. The current follower feeds back its estimated state information of the leader to the adjacent followers in real time through the multi-vehicle communication network to achieve safe closed-loop control until the formation tracking task is completed.

[0048] The dual-loop formation tracking controller comprises a cooperative leader estimator, a second-order filter, and a trajectory tracking control law connected in sequence. During processing, the underactuated unmanned surface vessel (USV) safe formation tracking control framework inputs the estimated leader state information from neighboring USVs into the cooperative leader estimator to obtain the estimated leader state in a distributed manner. The system also inputs the leader's reference trajectory, which is then processed and outputs the current follower's desired trajectory. After smoothing by the second-order filter, the current follower's reference trajectory is generated. The current follower's reference trajectory and its actual state information are then input into the trajectory tracking control law, which outputs the current follower's nominal forward velocity and nominal turning angular velocity, respectively, and input into a radial basis function neural network. In the adaptive controller and safety filter, the safety filter optimizes the nominal control command by solving a quadratic programming problem. The radial basis function neural network adaptive controller processes the optimized command and outputs the nominal forward control force of the current follower to the safety filter. The safety filter also receives information about external obstacles near the current follower, processes it, and outputs a safe forward control force command that satisfies the physical constraints of the actuator to control the current follower. At the same time, it outputs a steering angular velocity command to the radial basis function neural network adaptive controller. The radial basis function neural network adaptive controller also receives real-time speed information from the current follower, processes it, outputs the nominal forward control force to the full filter, and outputs a steering control torque command to control the current follower.

[0049] The kinematic and dynamic models of the unmanned surface vessel are as follows:

[0050] a) Kinematic model of unmanned surface vessel:

[0051]

[0052] in, and Let be the derivatives of the x and y coordinates of the i-th unmanned surface vessel in the global coordinate system, and p be the position information of the i-th unmanned surface vessel in the global coordinate system. i =[x i ,y i ] T ;u i v i and r i Let ξ represent the forward velocity, lateral velocity, and bow roll rate of the i-th unmanned surface vessel in the ship's coordinate system, respectively, and let ξ represent the velocity information of the i-th unmanned surface vessel in the ship's coordinate system. i =[u i ,v i ,r i ] T ;φ i and Let be the heading angle of the i-th unmanned surface vessel in the global coordinate system and its derivative.

[0053] b) Dynamics model of unmanned surface vessel:

[0054]

[0055] Where, m u,i m v,i and m r,i These are the forward, lateral, and bow inertial parameters, respectively, which include the added mass. and These are the known forward, lateral, and bow dynamics terms, respectively; f′ u,i f′ v,i and f′ r,i These are the unknown forward, lateral, and bow nonlinear dynamic terms, respectively. and τ represents the derivatives of the forward velocity, lateral velocity, and bow roll rate of the i-th unmanned surface vessel in the ship's coordinate system, respectively; u,i and τ r,i These represent the forward control force and steering control torque of the i-th unmanned surface vessel, respectively; τ e,u τ e,v and τ e,r These are external environmental disturbances from the front, sides, and bow, respectively.

[0056] The cooperative leader estimator for the dual-ring formation tracking controller is as follows:

[0057]

[0058] in, and The estimated position of leader l for the i-th follower. speed and heading angle The derivative of The estimated position of leader l for the j-th follower. The heading angle of the leader l estimated by the j-th follower; p l and φ l κ1, κ2, and κ3 represent the actual position and heading angle of the leader l in the reference trajectory, respectively; κ1, κ2, and κ3 represent the first, second, and third positive constant gains, respectively; a ij Let R be the communication topology connection weight between the i-th and j-th followers; R() is the rotation matrix; δ i-j Let δ be the expected formation offset between the i-th and j-th followers. i-j =δ i -δ j δ i and δ j b represents the formation bias of the i-th and j-th followers relative to the leader l position, respectively; i Let b be the connection weight between the i-th follower and the leader l. i When b = 1, the i-th follower directly obtains the leader's information; when b = 1, the i-th follower directly obtains the leader's information. i When p = 0, the i-th follower cannot directly obtain leader information; d,i Let be the expected trajectory of the i-th follower.

[0059] The second-order filter of the dual-loop formation tracking controller is as follows:

[0060]

[0061] Where, p r,i and Let q be the reference trajectory of the i-th follower and its derivative. r,i and Let be the reference velocity and its derivative of the i-th follower, respectively; γ1 is a filter constant greater than zero; p d,i Let φ be the expected trajectory of the i-th follower; r,i Let i be the reference heading angle of the i-th follower. The estimated heading angle of the leader l for the i-th follower.

[0062] The trajectory tracking control law of the dual-loop formation tracking controller is as follows:

[0063]

[0064]

[0065] Among them, u r,i and rr,i These are the nominal forward speed command and turning angular velocity command for the i-th follower, respectively; u d,i Let the velocity of the reference trajectory of the i-th follower be denoted as . q r,i Let c1, c2, c3, and k be the reference velocities of the i-th follower. * These represent the first, second, third, and fourth normal control gains, respectively, and k is the lateral error adjustment gain related to the reference speed; x e,i y e,i and φ e,i Let r be the tracking error in the forward and lateral directions, and the tracking error in the bow angle, respectively, in the hull coordinate system of the i-th follower; d,i Let be the heading angular velocity of the reference trajectory of the i-th follower. Let be the derivative of the reference heading angle of the i-th follower; Geometric factors related to coordinate transformation, and These are the other two geometric factors; σ e,i To incorporate the combined error term of the heading angle error and lateral error of the i-th follower; ζ σ This is the nonlinear coupling compensation term in the dynamics of composite error; p is the rotation matrix from the hull coordinate system to the global coordinate system; i and φ i Let p be the actual position and heading angle of the i-th follower, respectively; r,i and φ r,i These are the reference trajectory and reference heading angle of the i-th follower, respectively; ζ x and ζ y These are the first and second kinematic coupling terms in the error dynamics, respectively; v d,i The lateral velocity of the reference trajectory of the i-th follower is set to 0.

[0066] The radial basis function neural network adaptive controller is as follows:

[0067]

[0068] in, τ is the nominal forward control force of the i-th follower; r,i For the steering control torque command of the i-th follower; m u,i and m r,i These are the forward and forward inertial parameters, respectively, including the added mass; Let be the derivative of the nominal forward speed command of the i-th follower; The derivative of the steering angular velocity command of the i-th follower output by the safety filter; and These are the known forward and bow dynamics terms, respectively; and These are the first and second weight estimation vectors for the i-th follower in a radial basis function neural network, respectively; Φ m,i Let m be a column vector containing n radial basis functions for the i-th follower, where m ∈ {u, r}. χ m,i Let be the input vector of the corresponding channel of the radial basis function neural network of the i-th follower. c1, c2, ..., cn are the 1st, 2nd, ..., nth radial basis functions in the column vector of the i-th follower; c4, c5, c6, and c7 are the fifth, sixth, seventh, and eighth positive constant control gains, respectively; u e,i Let u be the forward velocity tracking error of the i-th follower. e,i =u i -u r,i r e,i Let be the tracking error of the i-th follower's turning angular velocity. u i and r i Let u be the actual forward velocity and turning angular velocity of the i-th follower, respectively. r,i and These are the nominal forward speed command for the i-th follower and the steering angular velocity command output by the safety filter, respectively.

[0069] Each radial basis function φ m,k All are in the form of Gaussian functions, as follows:

[0070]

[0071] Where h is the number of neurons in the neural network; Let m be the neural network input vector for the corresponding channel, and m ∈ {u, r} be the input vector for the forward channel. Input vector of the steering channel c m,k b is the center point vector of the k-th radial basis function; m,k Let be the width parameter of the k-th radial basis function.

[0072] The weights of a radial basis function neural network are updated through its adaptive law.

[0073] The adaptive law of radial basis function neural networks is as follows:

[0074]

[0075] in, The update rate for the weight estimate of the i-th follower; γ mFor positive constants, adaptive gain; Proj(·) is the projection operator that ensures the weight estimate remains within a preset boundary; e m,i Let be the speed tracking error of the i-th follower.

[0076] The safety filter based on robust control barrier function is as follows:

[0077]

[0078]

[0079] Among them, u i * For optimization variables in a quadratic programming problem, and These are the forward control force command and steering angular velocity command of the i-th follower, respectively, output by the safety filter. The input is the nominal control command. and r r,i The nominal forward control force and steering angular velocity commands of the i-th follower are respectively input to the radial basis function neural network adaptive controller and the dual-loop formation tracking controller; H i and Ω i These are the matrix and vector corresponding to the safety constraints of the i-th follower, respectively; u max and u min The upper and lower limits of the physical constraints of the unmanned surface vessel actuators are the optimization variables of the quadratic programming problem, respectively; α i,o Let m be the relative encounter angle between the i-th follower and the obstacle; u,i The forward inertial parameter includes the added mass; u i Let ψ be the actual forward velocity of the i-th follower; 1,i and ψ 2,i These are the first and second nonlinear terms containing system dynamics, respectively; α1() and α2() are the first and second extended terms, respectively. Class function; h 1,i and h 2,i Here, a represents the obstacle functions used to guarantee the safe distance and safe encounter angle for the i-th follower; max d represents the maximum deceleration. i,o Let be the distance between the i-th follower and the obstacle; d0 is the minimum safe distance; δ0 is a positive constant used to limit the encounter angle; Forward dynamics terms that are known; Φ is the first weight estimate vector for the i-th follower in a radial basis function neural network; u,i Let σ be the first type of column vector containing n radial basis functions for the i-th follower; 1,i For the i-th follower, there is a robust compensation term used to compensate for the approximation error of the neural network; This is the upper bound of the neural network weight estimation error; This is the upper bound of the neural network approximation error.

[0080] This invention, through a novel robust control obstacle function design, not only solves the problem of underactuated characteristics of unmanned surface vessels (USVs), but also effectively compensates for the worst-case error generated when the neural network approximates unknown dynamics. This ensures that USVs can strictly avoid collisions with obstacles in complex environments with model uncertainty and external disturbances, while maintaining optimal formation tracking performance. This significantly improves the control robustness and safety of multi-USV systems in uncertain environments.

[0081] Finally, to verify the effectiveness of the method of the present invention, the control method proposed in this invention was simulated in the Matlab / Simulink environment.

[0082] During verification, a formation of three unmanned surface vessels (USVs) is set up. Their task is to form a pre-defined triangular formation and track a virtual leader. The expected formation offset δ of the three USVs relative to the leader is defined. i The settings are as follows:

[0083] δ1=[0,0] T δ2=[-4,-8] T δ3=[4,-8] T

[0084] The virtual leader starts from coordinates [-40m, 40m]. T Departure, with u l The unmanned surface vessels (USVs) travel along a pre-defined S-shaped path at a constant forward velocity of 0.8 m / s. The initial positions of the three USVs are set as follows: USV 1: initial position (-35m, 40m), initial bow angle 0 rad; USV 2: initial position (-50m, 45m), initial bow angle -0.2 rad; USV 3: initial position (-50m, 35m), initial bow angle 0.1 rad. All USVs have an initial velocity of zero. Two static circular obstacles are placed in the scene, centered at (-10m, -7m) and (-10m, 7m) respectively, with a safety detection radius of 8m.

[0085] In this embodiment, the unknown nonlinear hydrodynamic term f′ u,i ,f′ v,i ,f′ r,i It is specifically configured as a set of typical nonlinear functions to simulate real hydrodynamic effects:

[0086]

[0087] f ov,i =1 / 33.8(-25.8u) i r i -0.86v i -36.3|v i |v i +0.81|r i |v i +36.5|v i |r i )

[0088] f o r,i =1 / 2.76(-33.8u) i v i -1.1u i r i +25.8v i r i -1.9r i- 36.3|v i |v i +0.75|r i |r i )

[0089] External environmental interference τ e,i It is modeled as a first-order Gaussian-Markov process, by the formula Generate, τ e,i =w i w i Let Ω be the noise being modeled, where the parameters are set as Ω = diag(0.5, 0.5, 0.5), K = diag(1, 1, 0.5), and Υ. i It is Gaussian white noise with a mean of 0 and a variance of 4.

[0090] In this embodiment, the key parameters of the controller are uniformly set as follows: cooperative estimator parameters k1 = 1.0, κ2 = 1.0, κ3 = 1.0; kinematic controller parameters k * =0.2, c1=1.0, c2=0.5, c3=0.8; dynamic controller parameters c4=3.0, c5=0.5, c6=1.0, c7=0.5, γ u =8.0,γ r =10.0; Security filter parameters The actuator's physical constraint is the forward force τ. u ∈[-50N,300N], steering torque τ r ∈[-100Nm,100Nm].

[0091] In the specific implementation process, the simulation results are as follows: Figure 2 , Figure 3 and Figure 4As shown.

[0092] like Figure 2 The diagram shows the simulated trajectory of the unmanned surface vessel (USV) formation in an embodiment of the present invention. As can be observed from the diagram, after starting from their respective initial positions and attitudes, the three USVs can quickly and smoothly converge to the preset triangular formation configuration and stably track the S-shaped path of the virtual leader as a whole. Specifically, during the time interval T = 130s to 160s, when the formation approaches a narrow passage formed by two symmetrically distributed obstacles, it can be clearly observed that the three USVs actively change and compress their formation, successfully and safely passing through the area, and then automatically returning to the preset formation configuration. This trajectory diagram intuitively demonstrates that the method of the present invention can achieve stable formation tracking while possessing advanced, cooperative autonomous obstacle avoidance capabilities.

[0093] To evaluate the formation tracking performance of this invention, the formation tracking error e is first... p,i The definition is as follows: e p,i =p i -p l -R(φ l )δ i , where p i Let p be the actual position of the i-th unmanned surface vessel. l and φ l These represent the actual position of the leader and the heading angle, R(φ). l ) is the rotation matrix based on the leader heading angle, δ i Let be the desired formation offset of the i-th unmanned surface vessel. Figure 3 (a) and Figure 3 As shown in (b), the formation tracking error e described above is illustrated in an embodiment of the present invention. p,i The graphs show the changes in the X and Y components over time. As can be seen from the graphs, the tracking error is relatively large in the initial stage of the simulation, but with the intervention of the control law of this invention, the error rapidly decreases and converges to a small neighborhood centered at zero. It is worth noting that during the obstacle avoidance phase from T = 130 seconds to 160 seconds, the formation tracking error experiences a brief but controllable increase. This is because the safety filter of this invention is activated during this phase, temporarily switching the control priority from maintaining the highest-precision formation to safe obstacle avoidance, thus actively sacrificing some tracking accuracy to ensure absolute safety. After successfully navigating the obstacle area, the error quickly converges back to near zero. This process fully demonstrates the superiority of this invention in balancing safety and performance, proving the effectiveness and safety of the control framework implemented by this invention.

[0094] like Figure 4As shown, the internal performance of key modules in an embodiment of the present invention is illustrated, including the nonlinear fitting effect and the control input of the unmanned surface vessel. Specifically, as... Figure 4 As shown in (a), the adaptive fitting effect of the radial basis function neural network (RBN) on the system uncertainties in the No. 1 unmanned surface vessel is illustrated. The two curves in the figure represent the actual uncertainties in the system and the output of the RBN, respectively. It can be observed that the output of the neural network can quickly and accurately track the changing trend of the actual uncertainties. This result strongly demonstrates that the adaptive dynamic controller based on the RBN designed in this invention can effectively estimate and compensate for unknown dynamics in the system. Figure 4 As shown in (b), the control input curves of the No. 1 unmanned surface vessel are displayed throughout the simulation process. It can be seen from the figure that the forward thrust and steering torque curves of the unmanned surface vessel calculated by the method of the present invention remain smooth and continuous, and their amplitudes are always within a reasonable physical constraint range. This proves that the control solution generated by the method of the present invention is mild and engineering-realizable.

[0095] This invention also designs a robust obstacle control-based underactuated unmanned surface vessel (USV) safe formation tracking control system, including a system construction module and a tracking control module. The system construction module is used to establish the underactuated USV safe formation tracking control framework, including a dual-loop formation tracking controller, a radial basis function neural network adaptive controller, and a safety filter based on a robust obstacle control function. When multiple USVs are in formation tracking, the tracking control module inputs the actual state information of each follower and the estimated state information of the leader from adjacent USVs into the underactuated USV safe formation tracking control framework. The underactuated USV safe formation tracking control framework simultaneously processes the leader's reference trajectory, the real-time state information, velocity information, control torque of the current follower, and external obstacle information. After processing, it outputs forward control force commands and steering control torque commands to control the current follower to avoid external obstacles and restore the preset formation configuration to continue following the leader.

[0096] The above content is merely a technical concept of the present invention and should not be construed as limiting the scope of protection of the present invention. Any modifications made to the technical solution based on the technical concept proposed in this invention shall fall within the scope of protection of the claims of this invention.

Claims

1. A method for safe formation tracking control of underactuated unmanned surface vessels based on robust control obstacles, characterized in that, include: Step 1) Establish a multi-unmanned surface vessel group including a leader and several followers. The unmanned surface vessel acting as the leader navigates according to the preset reference trajectory of the formation tracking mission. Each follower follows the leader according to the preset formation configuration and avoids obstacles. The multi-unmanned surface vessel group communicates in real time through a multi-vehicle communication network. Step 2) Establish a safe formation tracking control framework for underactuated unmanned surface vessels, including a dual-loop formation tracking controller, a radial basis function neural network adaptive controller, and a safety filter based on a robust control obstacle function; Step 3) When multiple unmanned surface vessels (USVs) are performing formation tracking, each follower inputs its own actual state information and the estimated state information of the leader from adjacent USVs into the underactuated USV safe formation tracking control framework. The underactuated USV safe formation tracking control framework simultaneously processes the leader's reference trajectory, the real-time state information and speed information of the current follower, and the external obstacle information. After processing, it outputs forward control force commands and steering control torque commands to control the current follower to avoid external obstacles and restore the preset formation configuration to continue following the leader. The current follower feeds back its estimated state information of the leader to adjacent followers in real time through the multi-vehicle communication network to achieve safe closed-loop control until the formation tracking task is completed.

2. The method for safe formation tracking control of underactuated unmanned surface vessels based on robust control obstacles according to claim 1, characterized in that: In step 2), the dual-loop formation tracking controller includes a cooperative leader estimator, a second-order filter, and a trajectory tracking control law connected in sequence. During processing, the underactuated unmanned surface vessel (USV) safe formation tracking control framework inputs the estimated leader state information from neighboring USVs into the cooperative leader estimator, along with the leader's reference trajectory. After processing, the expected trajectory of the current follower is output. After smoothing by the second-order filter, the reference trajectory of the current follower is generated. The reference trajectory of the current follower and its actual state information are input into the trajectory tracking control law, which then outputs the nominal forward velocity and nominal... The steering angular velocity is input into the radial basis function neural network adaptive controller and the safety filter, respectively. After processing, the radial basis function neural network adaptive controller outputs the nominal forward control force of the current follower to the safety filter. The safety filter simultaneously receives information about external obstacles near the current follower, processes it, and outputs a forward control force command to control the current follower. At the same time, it outputs a steering angular velocity command to the radial basis function neural network adaptive controller. The radial basis function neural network adaptive controller simultaneously receives real-time speed information fed back by the current follower, processes it, outputs the nominal forward control force to the full filter, and outputs a steering control torque command to control the current follower.

3. The method for safe formation tracking control of underactuated unmanned surface vessels based on robust obstacle control according to claim 2, characterized in that: In step 2), the cooperative leader estimator of the dual-ring formation tracking controller is specifically as follows: in, and The estimated position of leader l for the i-th follower. speed and heading angle The derivative of The estimated position of leader l for the j-th follower. The heading angle of the leader l estimated by the j-th follower; p l and φ l κ1, κ2, and κ3 represent the actual position and heading angle of the leader l in the reference trajectory, respectively; κ1, κ2, and κ3 represent the first, second, and third positive constant gains, respectively; a ij Let R be the communication topology connection weight between the i-th and j-th followers; R() is the rotation matrix; δ i-j Let δ be the expected formation offset between the i-th and j-th followers. i-j =δ i -δ j δ i and δ j b represents the formation bias of the i-th and j-th followers relative to the leader l position, respectively; i Let b be the connection weight between the i-th follower and the leader l. i When b = 1, the i-th follower directly obtains the leader's information; when b = 1, the i-th follower directly obtains the leader's information. i When p = 0, the i-th follower cannot directly obtain leader information; d,i Let be the expected trajectory of the i-th follower.

4. The method for safe formation tracking control of underactuated unmanned surface vessels based on robust control obstacles according to claim 2, characterized in that: In step 2), the second-order filter of the dual-loop formation tracking controller is specifically as follows: Where, p r,i and Let q be the reference trajectory of the i-th follower and its derivative. r,i and Let be the reference velocity and its derivative of the i-th follower, respectively; γ1 is a filter constant greater than zero; p d,i Let φ be the expected trajectory of the i-th follower; r,i Let i be the reference heading angle of the i-th follower. The estimated heading angle of the leader l for the i-th follower.

5. The method for safe formation tracking control of underactuated unmanned surface vessels based on robust control obstacles according to claim 2, characterized in that: In step 2), the trajectory tracking control law of the dual-ring formation tracking controller is as follows: k=k * ‖q r,i ‖ Among them, u r,i and r r,i These are the nominal forward speed command and turning angular velocity command for the i-th follower, respectively; u d,i Let u be the speed of the reference trajectory of the i-th follower. d,i =‖q r,i ‖,q r,i Let c1, c2, c3, and k be the reference velocities of the i-th follower. * These represent the first, second, third, and fourth normal control gains, respectively, and k is the lateral error adjustment gain related to the reference speed; x e,i y e,i and φ e,i Let r be the tracking error in the forward and lateral directions, and the tracking error in the bow angle, respectively, in the hull coordinate system of the i-th follower; d,i Let be the heading angular velocity of the reference trajectory of the i-th follower. χ² is the derivative of the reference heading angle of the i-th follower; σ² is the geometric factor related to the coordinate transformation; e,i To incorporate the combined error term of the heading angle error and lateral error of the i-th follower; ζ σ This is a nonlinear coupling compensation term; p is the rotation matrix from the hull coordinate system to the global coordinate system; i and φ i Let p be the actual position and heading angle of the i-th follower, respectively; r,i and φ r,i These are the reference trajectory and reference heading angle of the i-th follower, respectively.

6. The method for safe formation tracking control of underactuated unmanned surface vessels based on robust control obstacles according to claim 1, characterized in that: In step 2), the radial basis function neural network adaptive controller is specifically as follows: in, τ is the nominal forward control force of the i-th follower; r,i For the steering control torque command of the i-th follower; m u,i and m r,i These are the forward and forward inertial parameters, respectively, including the added mass; Let be the derivative of the nominal forward speed command of the i-th follower; The derivative of the steering angular velocity command of the i-th follower output by the safety filter; and These are the known forward and bow dynamics terms, respectively; and These are the first and second weight estimation vectors for the i-th follower in a radial basis function neural network, respectively; Φ m,i Let Φ be a column vector containing n radial basis functions for the i-th follower, m∈{u,r}. m,i (χ m,i )=[φ m,1 (χ m,i ),φ m,2 (χ m,i ),...,φ m,n (χ m,i )] T , χ m,i Let φ be the input vector of the radial basis function neural network of the i-th follower. m,1 (χ m,i ), φ m,2 (χ m,i ), ..., φ m,n (χ m,i c1, c2, ..., cn are the 1st, 2nd, ..., nth radial basis functions in the column vector of the i-th follower; c4, c5, c6, and c7 are the fifth, sixth, seventh, and eighth positive constant control gains, respectively; u e,i Let u be the forward velocity tracking error of the i-th follower. e,i =u i -u r,i r e,i Let be the tracking error of the i-th follower's turning angular velocity. u i and r i Let u be the actual forward velocity and turning angular velocity of the i-th follower, respectively. r,i and These are the nominal forward speed command of the i-th follower and the turning angular velocity command output by the safety filter, respectively. The weights of a radial basis function neural network are updated through its adaptive law.

7. The method for safe formation tracking control of underactuated unmanned surface vessels based on robust control obstacles according to claim 1, characterized in that: In step 2), the safety filter based on the robust control barrier function is specifically as follows: in, For optimization variables in a quadratic programming problem, and These are the forward control force command and steering angular velocity command for the i-th follower, respectively, output by the safety filter. The input is the nominal control command. and r r,i The nominal forward control force and steering angular velocity commands of the i-th follower are respectively input to the radial basis function neural network adaptive controller and the dual-loop formation tracking controller; H i and Ω i Let u be the matrix and vector corresponding to the safety constraints of the i-th follower; max and u min The upper and lower limits of the physical constraints of the unmanned surface vessel actuators are the optimization variables of the quadratic programming problem, respectively; α i,o Let m be the relative encounter angle between the i-th follower and the obstacle; u,i The forward inertial parameter includes the added mass; u i Let ψ be the actual forward velocity of the i-th follower; 1,i and ψ 2,i These are the first and second nonlinear terms, respectively; α1() and α2() are the first and second extended nonlinear terms, respectively. Class function; h 1,i and h 2,i Here, a represents the obstacle functions used to guarantee the safe distance and safe encounter angle for the i-th follower; max d represents the maximum deceleration. i,o Let be the distance between the i-th follower and the obstacle; d0 is the minimum safe distance; δ0 is a positive constant used to limit the encounter angle; Forward dynamics terms that are known; Φ is the first weight estimate vector for the i-th follower in a radial basis function neural network; u,i Let σ be the first type of column vector containing n radial basis functions for the i-th follower; 1,i For the robust compensation term of the i-th follower; This is the upper bound of the neural network weight estimation error; This is the upper bound of the neural network approximation error.

8. A robust obstacle-based safe formation tracking control system for underactuated unmanned surface vessels, characterized in that, include: The system building module is used to establish a safe formation tracking control framework for underactuated unmanned surface vessels, including a dual-loop formation tracking controller, a radial basis function neural network adaptive controller, and a safety filter based on a robust control obstacle function. The tracking control module, when multiple unmanned surface vessels (USVs) are tracking each other in formation, inputs the actual state information of each follower and the estimated state information of the leader from adjacent USVs into the underactuated USV safe formation tracking control framework. The underactuated USV safe formation tracking control framework processes the leader's reference trajectory, the real-time state information, speed information, control torque of the current follower, and external obstacle information. After processing, it outputs forward control force commands and steering control torque commands to control the current follower to avoid external obstacles and restore the preset formation configuration to continue following the leader.

Images