Formation dynamic rendezvous control method for unmanned surface vehicle and underwater robot

Abstract

The invention discloses a formation dynamic rendezvous control method for an unmanned ship and an underwater robot, and relates to the technical field of cooperative control, and the method comprises the following steps: S1, constructing a system model of the unmanned ship and the underwater robot; s2, generating a control target of the formation of the unmanned surface vehicle and the underwater robot according to the system model of the unmanned surface vehicle and the underwater robot; s3, setting a controller; s4, according to the controller, constructing a Lyapunov function, and generating an auxiliary control law; and S5, performing formation dynamic rendezvous control according to the controller provided with the auxiliary control law and the control target of the formation of the unmanned ship and the underwater robot. According to the method, the formation can be controlled to converge in an extremely short time, the dynamic rendezvous task can be effectively and rapidly completed, the motion trail is smooth, the formation is kept stable, the error fluctuation is small, and the precision is high.

Description

Technical Field

[0001] The present invention relates to the field of cooperative control technology, and in particular to a formation dynamic rendezvous control method for an unmanned boat and an underwater robot. Background Art

[0002] In recent decades, due to the increase in investment in ocean exploration, the development of ocean exploration technology has received great attention. Among the various ocean exploration tasks, there are many dangerous and complex tasks that cannot be completed by humans without the assistance of tools. Marine robots such as unmanned boats and underwater robots can provide a variety of detection methods for various tasks and are widely used in many fields. For example, they are widely used in seabed mapping, ocean measurement, confrontation operations, maritime rescue and environmental monitoring. However, due to the limitations of the robot's own endurance and carrying capacity, the efficiency of a single robot in performing tasks is greatly limited. One of the best ways to improve efficiency is multi-robot collaboration, including formations, clusters and rendezvous.

[0003] When performing large-scale maritime searches or long-term deep-sea missions, underwater robots usually need to return to ports or fixed sites for refueling and data transmission. Fixed-point static charging limits the working range of underwater robots and hinders the continuity of their work. In order to solve this problem, cooperation between underwater robots and unmanned boats as mobile energy supplies is essential. Therefore, in the process of dynamic charging and data transmission, the dynamic rendezvous between unmanned boats and underwater robots is the key to the problem. In the existing research on dynamic rendezvous, its research content mainly focuses on planning problems, and there are fewer studies involving control methods. In the research on rendezvous control, it is mainly oriented towards relatively simple linear systems and fixed-point rendezvous.

[0004] More and more studies have shown that model predictive control strategies can effectively handle the control problems of complex systems and have strong reliability and robustness. However, traditional model predictive control requires local linearization of the system model. However, when faced with nonlinear systems with strong coupling, such as unmanned boats and underwater robots, it is obviously inefficient to use local linearization to construct local auxiliary feedback control laws to optimize the system. Summary of the invention

[0005] In order to solve the above problems, the present invention proposes a formation dynamic rendezvous control method for an unmanned boat and an underwater robot.

[0006] The technical solution of the present invention is: a formation dynamic rendezvous control method of an unmanned boat and an underwater robot comprises the following steps:

[0007] S1. Build the system model of unmanned boat and underwater robot;

[0008] S2. Generate control targets for the formation of the unmanned boat and the underwater robot according to the system model of the unmanned boat and the underwater robot;

[0009] S3, setting controller;

[0010] S4. According to the controller, construct the Lyapunov function and generate the auxiliary control law;

[0011] S5. Perform dynamic rendezvous control of the formation according to the controller provided with the auxiliary control law and the control targets of the formation of the unmanned boat and the underwater robot.

[0012] Furthermore, S1 includes the following sub-steps:

[0013] S11. Construct the kinematic model and dynamic model of the unmanned boat;

[0014] S12. Construct kinematic and dynamic models of underwater robots;

[0015] S13. Obtain a system model of the unmanned boat and the underwater robot according to the kinematic model and dynamic model of the unmanned boat and the kinematic model and dynamic model of the underwater robot.

[0016] Furthermore, in S11, the expression of the kinematic model of the unmanned boat is:

[0017]

[0018] In the formula, R S represents the rotation matrix between the earth coordinate system and the unmanned boat hull coordinate system, ψ S represents the yaw angle of the unmanned boat, represents the first-order derivative of the position state vector of the unmanned boat with respect to time, ν S represents the velocity state vector of the unmanned boat;

[0019] In S11, the expression of the dynamic model of the unmanned boat is:

[0020]

[0021] Where M S represents the inertial mass matrix of the unmanned boat, represents the first-order derivative of the velocity state vector of the unmanned boat with respect to time, C S Denotes the Coriolis centripetal force matrix of the unmanned boat, D S represents the hydrodynamic damping coefficient matrix of the unmanned boat, τ S represents the first control input of the unmanned boat, τ wS Indicates the external disturbance to the unmanned boat.

[0022] Furthermore, in S12, the expression of the kinematic model of the underwater robot is:

[0023]

[0024] In the formula, J U represents the rotation matrix between the earth coordinate system and the underwater robot body coordinate system, η U represents the position state vector of the underwater robot, represents the first-order derivative of the underwater robot's position state vector with respect to time, ν U represents the velocity state vector of the underwater robot;

[0025] In S12, the expression of the dynamic model of the underwater robot is:

[0026]

[0027] Where M U represents the inertial mass matrix of the underwater robot, represents the first-order derivative of the velocity state vector of the underwater robot with respect to time, C U represents the Coriolis centripetal force matrix of the underwater robot, D U represents the fluid damping matrix of the underwater robot, g U represents the restoring force of buoyancy and gravity on the underwater robot, τ U represents the control input of the underwater robot, τ wU Represents the external environmental disturbance to which the underwater robot is subjected.

[0028] Furthermore, in S13, the expression of the system model of the unmanned boat and the underwater robot is:

[0029]

[0030] In the formula, It represents the first-order derivative of the position state of the unmanned boat with respect to time, R S represents the rotation matrix between the earth coordinate system and the unmanned boat hull coordinate system, ψ S represents the yaw angle of the unmanned boat, ν S represents the velocity state vector of the unmanned boat, f 1S Represents the first functional relationship of the unmanned boat in the system model, x 1S Indicates the position status of the unmanned boat, u 1S represents the speed control value of the unmanned boat, represents the first-order derivative of the position state of the underwater robot with respect to time, J U represents the rotation matrix between the earth coordinate system and the underwater robot body coordinate system, η U represents the position state vector of the underwater robot, νU represents the velocity state vector of the underwater robot, f 1U Represents the first functional relationship of the underwater robot in the system model, x 1U Indicates the position state of the underwater robot, u 1U represents the speed control value of the underwater robot, It represents the first-order derivative of the speed state of the unmanned boat with respect to time, M S represents the inertial mass matrix of the unmanned boat, τ S represents the first control input of the unmanned boat, τ wS represents the external environmental disturbance to the unmanned boat, C S Denotes the Coriolis centripetal force matrix of the unmanned boat, D S represents the hydrodynamic damping coefficient matrix of the unmanned boat, f 2S Represents the second functional relationship of the unmanned boat in the system model, x 2S Indicates the speed status of the unmanned boat. It represents the first-order derivative of the velocity state of the underwater robot with respect to time, M U represents the inertial mass matrix of the underwater robot, τ U represents the first control input of the underwater robot, τ wU represents the external environmental disturbance to the underwater robot, C U represents the Coriolis centripetal force matrix of the underwater robot, D U represents the fluid damping matrix of the underwater robot, g U represents the restoring force of buoyancy and gravity on the underwater robot, f 2U Represents the second functional relationship of the underwater robot in the system model, x 2U represents the speed state of the underwater robot, u 2S represents the second control input of the unmanned boat, u 2U Represents the second control input of the underwater robot.

[0031] Furthermore, in S2, the expression of the control target of the unmanned boat and underwater robot formation is:

[0032]

[0033] In the formula, represents the position error between the i-th underwater robot and the unmanned boat on the horizontal plane, represents the speed error between the i-th underwater robot and the unmanned boat on the horizontal plane, z Ui represents the position coordinate of the i-th underwater robot in the z direction, represents the expected trajectory tracking error of the unmanned boat, represents the position state vector of the i-th underwater robot in the horizontal direction, represents the velocity state vector of the i-th underwater robot in the horizontal direction, ν S represents the velocity state vector of the unmanned boat, η d represents the desired position state vector in the horizontal direction, η S Represents the position state vector of the unmanned boat.

[0034] Furthermore, in S3, the expression of the controller is:

[0035]

[0036] Where J represents the performance function of the controller, represents the predicted amount of unmanned boats, represents the predicted quantity of the underwater robot, S(δ) represents the piecewise function system of the sampling period δ, δ represents the sampling period, represents the prediction time domain, represents the state error of the unmanned boat, s represents the independent variable of the integrated term, H nS represents the first weighted matrix or the second weighted matrix corresponding to the unmanned boat, represents the predicted speed control quantity or predicted control input of the unmanned boat, R nS represents the third weighted matrix or the fourth weighted matrix corresponding to the unmanned boat, represents the terminal state error of the unmanned boat, P nS represents the fifth weighted matrix or the sixth weighted matrix corresponding to the unmanned boat, N represents the total number of underwater robots, represents the state error of the i-th underwater robot on the horizontal plane, H nU represents the first weighted matrix or the second weighted matrix corresponding to the underwater robot, represents the state error of the i-th underwater robot on the vertical plane, Z nU represents the third weighted matrix or the fourth weighted matrix corresponding to the underwater robot, represents the predicted speed control quantity or predicted control input of the ith underwater robot, R nU represents the fifth weighted matrix or the sixth weighted matrix corresponding to the underwater robot, represents the terminal state error of the i-th underwater robot on the horizontal plane, P nU represents the seventh weighted matrix or the eighth weighted matrix corresponding to the underwater robot, represents the terminal state error of the i-th underwater robot on the vertical plane, Q nU represents the ninth weighted matrix or the tenth weighted matrix corresponding to the underwater robot, represents the first-order derivative of the predicted state of the unmanned boat with respect to the independent variable s, f nS Represents the first functional relationship or the second functional relationship of the unmanned boat in the system model, represents the predicted state of the unmanned boat, represents the first-order derivative of the predicted state of the i-th underwater robot with respect to the independent variable s, f nU represents the first functional relationship or the second functional relationship of the underwater robot in the system model, represents the predicted state of the i-th underwater robot, Indicates the current state of the unmanned boat, x nS (t0) represents the current state of the unmanned boat, represents the current state of the i-th underwater robot, x nUi (t0) represents the current state of the i-th underwater robot, u nSmax Indicates the maximum speed control value or maximum control input of the unmanned boat, u nUmax represents the maximum speed control value or maximum control input of the ith underwater robot, V nS represents the outer loop Lyapunov function or inner loop Lyapunov function of the unmanned boat, x nS Indicates the state of the unmanned boat, h nS represents the outer loop auxiliary control law or inner loop auxiliary control law of the unmanned boat, V nUi represents the outer loop Lyapunov function or inner loop Lyapunov function of the i-th underwater robot, x nUi represents the state of the i-th underwater robot, h nUi represents the outer loop auxiliary control law or inner loop auxiliary control law of the i-th underwater robot, It represents the current speed control value or current control input of the unmanned boat, and t0 represents time.

[0037] Further, in S4, the Lyapunov function includes an outer loop Lyapunov function and an inner loop Lyapunov function of the unmanned boat and an outer loop Lyapunov function and an inner loop Lyapunov function of the underwater robot, and the auxiliary control law includes an outer loop auxiliary control law and an inner loop auxiliary control law of the unmanned boat and an outer loop auxiliary control law and an inner loop auxiliary control law of the underwater robot;

[0038] The outer loop Lyapunov function V of the unmanned boat 1S The expression is:

[0039]

[0040] In the formula, for The transpose of K oS represents the first positive definite diagonal matrix corresponding to the unmanned boat, represents the trajectory tracking error of the unmanned boat;

[0041] Outer loop auxiliary control law of unmanned boat h 1S The expression is:

[0042]

[0043] In the formula, For R S The inverse of R S represents the rotation matrix between the earth coordinate system and the unmanned boat hull coordinate system, ψ S represents the yaw angle of the unmanned boat, represents the first-order derivative of the desired position state vector in the horizontal direction with respect to time, K pS represents the second positive definite diagonal matrix corresponding to the unmanned boat, represents the trajectory tracking error of the unmanned boat;

[0044] The inner loop Lyapunov function V of the unmanned boat 2S The expression is:

[0045]

[0046] In the formula, for The transpose of K tS represents the third positive definite diagonal matrix corresponding to the unmanned boat, represents the speed tracking error of the unmanned boat;

[0047] Inner loop auxiliary control law of unmanned boat 2S The expression is:

[0048]

[0049] Where M S represents the inertial mass matrix of the unmanned boat, represents the first-order derivative of the desired velocity state vector in the horizontal direction with respect to time, K dS represents the fourth positive definite diagonal matrix corresponding to the unmanned boat, represents the speed tracking error of the unmanned boat, C S represents the Coriolis centripetal force matrix of the unmanned boat, ν S represents the velocity state vector of the unmanned boat, D S represents the hydrodynamic damping coefficient matrix of the unmanned boat, τ wS Indicates the external disturbance to which the unmanned boat is subjected;

[0050] The outer loop Lyapunov function V of the i-th underwater robot 1Ui The expression is:

[0051]

[0052] In the formula, express The transpose of K oU represents the first positive definite diagonal matrix corresponding to the underwater robot, represents the trajectory tracking error of the i-th underwater robot;

[0053] The outer loop auxiliary control law h of the i-th underwater robot 1Ui The expression is:

[0054]

[0055] In the formula, Indicates J U The inverse of J U represents the rotation matrix between the earth coordinate system and the underwater robot body coordinate system, η Ui represents the position state vector of the i-th underwater robot, represents the first-order derivative of the desired position state vector of the i-th underwater robot with respect to time, K pU represents the second positive definite diagonal matrix corresponding to the underwater robot, represents the trajectory tracking error of the i-th underwater robot;

[0056] The inner loop Lyapunov function V of the i-th underwater robot 2Ui The expression is:

[0057]

[0058] In the formula, express The transpose of K tU represents the third positive definite diagonal matrix corresponding to the underwater robot, represents the velocity tracking error of the i-th underwater robot;

[0059] The inner loop auxiliary control law h of the i-th underwater robot 2Ui The expression is:

[0060]

[0061] Where M U represents the inertial mass matrix of the underwater robot, represents the first-order derivative of the desired velocity state vector of the ith underwater robot with respect to time, K dU represents the fourth positive definite diagonal matrix corresponding to the underwater robot, represents the velocity tracking error of the i-th underwater robot, C Ui represents the Coriolis centripetal force matrix of the ith underwater robot, ν Ui represents the velocity state vector of the ith underwater robot, DUi represents the fluid damping matrix of the i-th underwater robot, g Ui represents the restoring force of buoyancy and gravity on the i-th underwater robot, η Ui represents the position state vector of the i-th underwater robot, τ wUi Represents the external environmental disturbance to which the i-th underwater robot is subjected.

[0062] Furthermore, in S5, the control objectives of the unmanned boat and the underwater robot formation are optimized to obtain the optimal solution sequence and complete the dynamic rendezvous control of the formation.

[0063] The beneficial effects of the present invention are as follows: the present invention designs a dynamic rendezvous control method for a formation of an unmanned boat and an underwater robot, and utilizes a contraction constraint formula to design an auxiliary control law to perform online optimization of the unmanned boat and underwater robot system, which can control the formation to converge in a very short time, and can effectively and quickly complete the dynamic rendezvous task, and the motion trajectory is relatively smooth, the formation remains stable, the error fluctuation is very small, and the accuracy is high. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] Figure 1 A flow chart of a formation dynamic rendezvous control method for an unmanned boat and an underwater robot;

[0065] Figure 2 It is a 3D space trajectory diagram of the unmanned boat-underwater robot formation in Example 1;

[0066] Figure 3 It is a projection diagram of the 3D space trajectory of the unmanned boat-underwater robot formation in Example 1 onto the 2D plane xy plane;

[0067] Figure 4 A position error diagram of the unmanned boat and the underwater robot in the formation in Example 1;

[0068] Figure 5 is a graph showing the depth of the underwater robot in the z-axis direction changing with time in Example 1;

[0069] Figure 6 is a control input curve diagram of the unmanned boat changing with time in Example 1;

[0070] Figure 7 A control input curve diagram of the underwater robot in Example 1 that changes with time;

[0071] Figure 8 It is a 3D space trajectory diagram of the unmanned boat-underwater robot formation in Example 2;

[0072] Fig. 9 It is a projection diagram of the 3D space trajectory of the unmanned boat-underwater robot formation in Example 2 onto the 2D plane xy plane;

[0073] Fig.10 A position error diagram of the unmanned boat and the underwater robot in the formation in Example 2;

[0074] Fig.11 is a graph showing the depth of the underwater robot in the z-axis direction versus time in Example 2;

[0075] Fig.12 is a control input curve diagram of the unmanned boat changing with time in the second embodiment;

[0076] Fig.13 This is a control input curve diagram of the underwater robot in Example 2 that changes with time. DETAILED DESCRIPTION

[0077] The embodiments of the present invention will be further described below in conjunction with the accompanying drawings.

[0078] like Figure 1 As shown, the present invention provides a formation dynamic rendezvous control method for an unmanned boat and an underwater robot, comprising the following steps:

[0079] S1. Build the system model of unmanned boat and underwater robot;

[0080] S2. Generate control targets for the formation of the unmanned boat and the underwater robot according to the system model of the unmanned boat and the underwater robot;

[0081] S3, setting controller;

[0082] S4. According to the controller, construct the Lyapunov function and generate the auxiliary control law;

[0083] S5. Perform dynamic rendezvous control of the formation according to the controller provided with the auxiliary control law and the control targets of the formation of the unmanned boat and the underwater robot.

[0084] In this embodiment of the present invention, S1 includes the following sub-steps:

[0085] S11. Construct the kinematic model and dynamic model of the unmanned boat;

[0086] S12. Construct kinematic and dynamic models of underwater robots;

[0087] S13. Obtain a system model of the unmanned boat and the underwater robot according to the kinematic model and dynamic model of the unmanned boat and the kinematic model and dynamic model of the underwater robot.

[0088] In the embodiment of the present invention, in S11, the kinematic model of the unmanned boat is expressed as:

[0089]

[0090] In the formula, R S represents the rotation matrix between the earth coordinate system and the unmanned boat hull coordinate system, ψ S represents the yaw angle of the unmanned boat, represents the first-order derivative of the position state vector of the unmanned boat with respect to time, ν S represents the velocity state vector of the unmanned boat;

[0091] In S11, the expression of the dynamic model of the unmanned boat is:

[0092]

[0093] Where M S represents the inertial mass matrix of the unmanned boat, represents the first-order derivative of the velocity state vector of the unmanned boat with respect to time, C S Denotes the Coriolis centripetal force matrix of the unmanned boat, D S represents the hydrodynamic damping coefficient matrix of the unmanned boat, τ S represents the first control input of the unmanned boat, τ wS Indicates the external disturbance to the unmanned boat.

[0094] In the embodiment of the present invention, in S12, the expression of the kinematic model of the underwater robot is:

[0095]

[0096] In the formula, J U represents the rotation matrix between the earth coordinate system and the underwater robot body coordinate system, η U represents the position state vector of the underwater robot, represents the first-order derivative of the underwater robot's position state vector with respect to time, ν U represents the velocity state vector of the underwater robot;

[0097] In S12, the expression of the dynamic model of the underwater robot is:

[0098]

[0099] Where M U represents the inertial mass matrix of the underwater robot, represents the first-order derivative of the velocity state vector of the underwater robot with respect to time, C U represents the Coriolis centripetal force matrix of the underwater robot, D U represents the fluid damping matrix of the underwater robot, g U represents the restoring force of buoyancy and gravity on the underwater robot, τ U represents the control input of the underwater robot, τwU Represents the external environmental disturbance to which the underwater robot is subjected.

[0100] In the embodiment of the present invention, in S13, the expression of the system model of the unmanned boat and the underwater robot is:

[0101]

[0102] In the formula, It represents the first-order derivative of the position state of the unmanned boat with respect to time, R S represents the rotation matrix between the earth coordinate system and the unmanned boat hull coordinate system, ψ S represents the yaw angle of the unmanned boat, ν S represents the velocity state vector of the unmanned boat, f 1S Represents the first functional relationship of the unmanned boat in the system model, x 1S Indicates the position status of the unmanned boat, u 1S represents the speed control value of the unmanned boat, represents the first-order derivative of the position state of the underwater robot with respect to time, J U represents the rotation matrix between the earth coordinate system and the underwater robot body coordinate system, η U represents the position state vector of the underwater robot, ν U represents the velocity state vector of the underwater robot, f 1U Represents the first functional relationship of the underwater robot in the system model, x 1U Indicates the position state of the underwater robot, u 1U represents the speed control value of the underwater robot, It represents the first-order derivative of the speed state of the unmanned boat with respect to time, M S represents the inertial mass matrix of the unmanned boat, τ S represents the first control input of the unmanned boat, τ wS represents the external environmental disturbance to the unmanned boat, C S Denotes the Coriolis centripetal force matrix of the unmanned boat, D S represents the hydrodynamic damping coefficient matrix of the unmanned boat, f 2S Represents the second functional relationship of the unmanned boat in the system model, x 2S Indicates the speed status of the unmanned boat. It represents the first-order derivative of the velocity state of the underwater robot with respect to time, M U represents the inertial mass matrix of the underwater robot, τ U represents the first control input of the underwater robot, τ wU represents the external environmental disturbance to the underwater robot, C U represents the Coriolis centripetal force matrix of the underwater robot, D U represents the fluid damping matrix of the underwater robot, gU represents the restoring force of buoyancy and gravity on the underwater robot, f 2U Represents the second functional relationship of the underwater robot in the system model, x 2U represents the speed state of the underwater robot, u 2S represents the second control input of the unmanned boat, u 2U Represents the second control input of the underwater robot.

[0103] In the embodiment of the present invention, in S2, the control target of the unmanned boat and the underwater robot formation is expressed as follows:

[0104] In the maritime search mission, the unmanned boat-underwater robot formation needs to sail in a fixed formation along a designated route. In this process, due to the limitation of the energy stored in the underwater robot, it is difficult for it to work continuously underwater for a long time. Dynamic charging is one of the best ways to solve this problem. It can effectively improve the search efficiency and avoid the time cost of shore-based or fixed-point charging. In this context, the unmanned boat is used as the leader of the formation, and it is assumed that it can communicate with each underwater robot in real time. Therefore, the formation is located on the desired trajectory (x d ,y d ) is given to the unmanned boat, the expected state of the system can be obtained by the following formula:

[0105]

[0106] Definition η d =[x d y d ψ d ] T is the desired position state vector in the horizontal direction, and defines ν d =u d v d r d ] T is the desired velocity state vector in the horizontal direction. d is the expected yaw angle of the unmanned boat, u d is the expected longitudinal velocity of the unmanned boat, v d is the expected sway speed of the unmanned boat, r d is the expected bow angular velocity of the unmanned boat, x d is the desired position in the x direction, y d is the desired position in the y direction, is the first-order derivative of the desired position in the x-direction with respect to time, is the first-order derivative of the desired position in the y direction with respect to time, is the second-order derivative of the desired position in the x-direction with respect to time, is the second-order derivative of the desired position in the y direction with respect to time.

[0107] Underwater robot as follower, definition is the relative expectation of the position and velocity state between the i-th underwater robot and the unmanned boat on the horizontal plane; is the expected position and velocity state of the i-th underwater robot on the vertical plane, and the formation is designed accordingly. xi , β yi , β zi , β θi , β ψi is the relative expected distance in the x direction, the relative expected distance in the y direction, the relative expected distance in the z direction, the relative expected angle in the pitch angle, and the relative expected angle in the bow angle between the i-th underwater robot and the unmanned boat. Define β 1i =β xi β yi β zi β θi β ψi ] T , known where β xi , β yi , β zi , β θi is a known parameter, β 1i , β 2i is the relative expectation of the position and velocity state between the i-th underwater robot and the unmanned boat, is the first-order derivative of the relative expectation of the position state between the i-th underwater robot and the unmanned boat with respect to time, is the first-order derivative of the relative expectation of the position state between the i-th underwater robot and the unmanned boat on the horizontal plane with respect to time, is the first-order derivative of the expected position state of the i-th underwater robot on the vertical plane with respect to time.

[0108] Since dynamic charging requires the unmanned boat and the underwater robot to remain relatively still, dynamic rendezvous control requires the underwater robot to actively change its original navigation trajectory so that it has the same speed and position as the unmanned boat; at the same time, the unmanned boat needs to cooperate with the underwater robot to complete the rendezvous and keep tracking the predetermined trajectory as much as possible. This goal can be expressed as:

[0109]

[0110] In the formula, represents the position error between the i-th underwater robot and the unmanned boat on the horizontal plane, represents the speed error between the i-th underwater robot and the unmanned boat on the horizontal plane, z Ui represents the position coordinate of the i-th underwater robot in the z direction, represents the expected trajectory tracking error of the unmanned boat, represents the position state vector of the i-th underwater robot in the horizontal direction, represents the velocity state vector of the i-th underwater robot in the horizontal direction, ν S represents the velocity state vector of the unmanned boat, η d represents the desired position state vector in the horizontal direction, η S Represents the position state vector of the unmanned boat.

[0111] In the embodiment of the present invention, in S3, the controller is designed as follows:

[0112] Define the following error variables:

[0113]

[0114] in, and They are the position state error of the unmanned boat, the speed state error of the unmanned boat, the predicted position state of the unmanned boat, and the predicted speed state of the unmanned boat; and are respectively the position state error of the i-th underwater robot on the horizontal plane, the speed state error of the i-th underwater robot on the horizontal plane, the predicted position state of the i-th underwater robot on the horizontal plane, and the predicted speed state of the i-th underwater robot on the horizontal plane; and are respectively the position state error of the i-th underwater robot on the vertical plane, the speed state error of the i-th underwater robot on the vertical plane, the predicted position state of the i-th underwater robot on the vertical plane, and the predicted speed state of the i-th underwater robot on the vertical plane, η d represents the desired position state vector in the horizontal direction, ν d represents the desired velocity state vector in the horizontal direction, represents the state error of the i-th underwater robot on the horizontal plane, represents the predicted state of the i-th underwater robot on the horizontal plane, represents the relative expectation of the state between the i-th underwater robot and the unmanned boat on the horizontal plane, represents the predicted state of the unmanned boat, represents the state error of the i-th underwater robot on the vertical plane, represents the predicted state of the i-th underwater robot on the vertical plane, Represents the expectation of the state of the i-th underwater robot on the vertical plane.

[0115] The expression of the controller is:

[0116]

[0117] Where J represents the performance function of the controller, represents the predicted amount of unmanned boats, represents the predicted quantity of the underwater robot, S(δ) represents the piecewise function system of the sampling period δ, δ represents the sampling period, represents the prediction time domain, represents the state error of the unmanned boat, s represents the independent variable of the integrated term, H nS represents the first weighted matrix or the second weighted matrix corresponding to the unmanned boat, represents the predicted speed control quantity or predicted control input of the unmanned boat, R nS represents the third weighted matrix or the fourth weighted matrix corresponding to the unmanned boat, represents the terminal state error of the unmanned boat, P nS represents the fifth weighted matrix or the sixth weighted matrix corresponding to the unmanned boat, N represents the total number of underwater robots, represents the state error of the i-th underwater robot on the horizontal plane, H nU represents the first weighted matrix or the second weighted matrix corresponding to the underwater robot, represents the state error of the i-th underwater robot on the vertical plane, Z nU represents the third weighted matrix or the fourth weighted matrix corresponding to the underwater robot, represents the predicted speed control quantity or predicted control input of the ith underwater robot, R nU represents the fifth weighted matrix or the sixth weighted matrix corresponding to the underwater robot, represents the terminal state error of the i-th underwater robot on the horizontal plane, P nU represents the seventh weighted matrix or the eighth weighted matrix corresponding to the underwater robot, represents the terminal state error of the i-th underwater robot on the vertical plane, Q nU represents the ninth weighted matrix or the tenth weighted matrix corresponding to the underwater robot, represents the first-order derivative of the predicted state of the unmanned boat with respect to the independent variable s, f nS Represents the first functional relationship or the second functional relationship of the unmanned boat in the system model, represents the predicted state of the unmanned boat, represents the first-order derivative of the predicted state of the i-th underwater robot with respect to the independent variable s, f nUrepresents the first functional relationship or the second functional relationship of the underwater robot in the system model, represents the predicted state of the i-th underwater robot, Indicates the current state of the unmanned boat, x nS (t0) represents the current state of the unmanned boat, represents the current state of the i-th underwater robot, x nUi (t0) represents the current state of the i-th underwater robot, u nSmax Indicates the maximum speed control value or maximum control input of the unmanned boat, u nUmax represents the maximum speed control value or maximum control input of the ith underwater robot, V nS represents the outer loop Lyapunov function or inner loop Lyapunov function of the unmanned boat, x nS Indicates the state of the unmanned boat, h nS represents the outer loop auxiliary control law or inner loop auxiliary control law of the unmanned boat, V nUi represents the outer loop Lyapunov function or inner loop Lyapunov function of the i-th underwater robot, x nUi represents the state of the i-th underwater robot, h nUi represents the outer loop auxiliary control law or inner loop auxiliary control law of the i-th underwater robot, It represents the current speed control value or current control input of the unmanned boat, and t0 represents time.

[0118] In the embodiment of the present invention, in S4, the Lyapunov function includes an outer loop Lyapunov function and an inner loop Lyapunov function of the unmanned boat and an outer loop Lyapunov function and an inner loop Lyapunov function of the underwater robot, and the auxiliary control law includes an outer loop auxiliary control law and an inner loop auxiliary control law of the unmanned boat and an outer loop auxiliary control law and an inner loop auxiliary control law of the underwater robot;

[0119] The outer loop Lyapunov function V of the unmanned boat 1S The expression is:

[0120]

[0121] In the formula, for The transpose of K oS represents the first positive definite diagonal matrix corresponding to the unmanned boat, represents the trajectory tracking error of the unmanned boat;

[0122] Outer loop auxiliary control law of unmanned boat h 1S The expression is:

[0123]

[0124] In the formula, For R S The inverse of R S represents the rotation matrix between the earth coordinate system and the unmanned boat hull coordinate system, ψ S represents the yaw angle of the unmanned boat, represents the first-order derivative of the desired position state vector in the horizontal direction with respect to time, K pS represents the second positive definite diagonal matrix corresponding to the unmanned boat, represents the trajectory tracking error of the unmanned boat;

[0125] The inner loop Lyapunov function V of the unmanned boat 2S The expression is:

[0126]

[0127] In the formula, for The transpose of K tS represents the third positive definite diagonal matrix corresponding to the unmanned boat, represents the speed tracking error of the unmanned boat;

[0128] Inner loop auxiliary control law of unmanned boat 2S The expression is:

[0129]

[0130] Where M S represents the inertial mass matrix of the unmanned boat, represents the first-order derivative of the desired velocity state vector in the horizontal direction with respect to time, K dS represents the fourth positive definite diagonal matrix corresponding to the unmanned boat, represents the speed tracking error of the unmanned boat, C S represents the Coriolis centripetal force matrix of the unmanned boat, ν S represents the velocity state vector of the unmanned boat, D S represents the hydrodynamic damping coefficient matrix of the unmanned boat, τ wS Indicates the external disturbance to which the unmanned boat is subjected;

[0131] The outer loop Lyapunov function V of the i-th underwater robot 1Ui The expression is:

[0132]

[0133] In the formula, express The transpose of K oU represents the first positive definite diagonal matrix corresponding to the underwater robot, represents the trajectory tracking error of the i-th underwater robot;

[0134] The outer loop auxiliary control law h of the i-th underwater robot 1Ui The expression is:

[0135]

[0136] In the formula, Indicates J U The inverse of J U represents the rotation matrix between the earth coordinate system and the underwater robot body coordinate system, η Ui represents the position state vector of the i-th underwater robot, represents the first-order derivative of the desired position state vector of the i-th underwater robot with respect to time, K pU represents the second positive definite diagonal matrix corresponding to the underwater robot, represents the trajectory tracking error of the i-th underwater robot;

[0137] The inner loop Lyapunov function V of the i-th underwater robot 2Ui The expression is:

[0138]

[0139] In the formula, express The transpose of K tU represents the third positive definite diagonal matrix corresponding to the underwater robot, represents the velocity tracking error of the i-th underwater robot;

[0140] The inner loop auxiliary control law h of the i-th underwater robot 2Ui The expression is:

[0141]

[0142] Where M U represents the inertial mass matrix of the underwater robot, represents the first-order derivative of the desired velocity state vector of the ith underwater robot with respect to time, K dU represents the fourth positive definite diagonal matrix corresponding to the underwater robot, represents the velocity tracking error of the i-th underwater robot, C Ui represents the Coriolis centripetal force matrix of the ith underwater robot, ν Ui represents the velocity state vector of the ith underwater robot, D Ui represents the fluid damping matrix of the i-th underwater robot, g Ui represents the restoring force of buoyancy and gravity on the i-th underwater robot, ηUi represents the position state vector of the i-th underwater robot, τ wUi Represents the external environmental disturbance to which the i-th underwater robot is subjected.

[0143] The closed-loop control structure of the system consists of an inner loop and an outer loop. The inner loop controller uses the actual control input to gradually make the actual speed approach the desired speed, thereby ensuring the consistent convergence of the speed tracking error. In essence, the inner loop adjusts the speed to meet the expected value, and the outer loop controller focuses on achieving precise positioning, jointly improving the overall tracking performance and stability of the formation system.

[0144] In the embodiment of the present invention, in S5, the control objectives of the unmanned boat and the underwater robot formation are optimized to obtain the optimal solution sequence, and the dynamic rendezvous control of the formation is completed.

[0145] The nonlinear model predictive control technology applied to the unmanned boat-underwater robot system has the following specific steps of the algorithm:

[0146] 1. For the unmanned boat-underwater robot system, initialize the design parameters such as weight matrix and sampling period.

[0147] 2. The current state of the unmanned boat x nS (t0) and the current state of underwater robot i and x nUi (t0) is sampled and input into the performance function J.

[0148] 3. Set the status of the unmanned boat x nS (t) = x nS (t0), the current state x of underwater robot i nUi (t) = x nUi (t0), solve the optimization problem, define and is the optimal (suboptimal) solution sequence obtained.

[0149] 4. In the current sampling period, and The first control quantity κ in each sequence nS (s) and κ nUi (s) as control input.

[0150] 5. Roll the time domain, set t = t + δ, and jump to step 2, where t represents the time parameter.

[0151] The following is a description of the specific embodiments.

[0152] The model parameters are set as follows:

[0153] η S =[x S yS ψ S ] T is the position state vector of the unmanned boat, x S is the position coordinate of the unmanned boat in the x-axis direction, y S is the position coordinate of the unmanned boat in the y-axis direction, ψ S is the yaw angle of the unmanned boat; ν S =u S v S r S ] T is the velocity state vector of the unmanned boat, u S is the longitudinal speed of the unmanned boat, v S is the sway speed of the unmanned boat, r S is the bow angular velocity of the unmanned boat; is the rotation matrix between the earth coordinate system and the unmanned boat hull coordinate system; is the inertial mass matrix of the unmanned boat; is the Coriolis centripetal force matrix of the unmanned boat; is the hydrodynamic damping coefficient matrix of the unmanned boat; τ S =[τ uS 0τ rS ] T is the control input of the unmanned boat; τ wS =[τ wuS τ wvS τ wrS ] T is the external environmental disturbance to the unmanned boat; η U =[x U y U z U θ U ψ U ] T is the position state vector of the underwater robot, x S is the position coordinate of the underwater robot in the x-axis direction, y S is the position coordinate of the underwater robot in the y-axis direction, z S is the position coordinate of the underwater robot in the z-axis direction, θ U is the pitch angle of the underwater robot, ψ U is the bow pitch angle of the underwater robot; ν U =[u U v U w U q U r U ] T is the velocity state vector of the underwater robot, u U is the longitudinal velocity of the underwater robot, v U is the lateral velocity of the underwater robot, wU is the vertical velocity of the underwater robot, q U is the pitch angular velocity of the underwater robot, r U is the bow angular velocity of the underwater robot; M is the rotation matrix between the earth coordinate system and the underwater robot body coordinate system; U =diag{m 11U ,m 22U ,m 33U ,m 44U ,m 55U} is the inertial mass matrix of the underwater robot; is the Coriolis centripetal force matrix of the underwater robot; D U (ν U ) = diag{d 11U ,d 22U ,d 33U ,d 44U ,d 55U} is the fluid damping matrix of the underwater robot; is the restoring force (torque) of the buoyancy and gravity acting on the underwater robot; τ U =τ uU τ vU τ wU τ qU τ rU ] T is the control input of the underwater robot; τ wU =τ wuU τ wvU τ wwU τ wqU τ wrU ] T is the external environmental disturbance to the underwater robot; x 1S =[x S y S ψ S ] T is the position state of the unmanned boat, x 2S =[u S v S r S ] T is the speed state of the unmanned boat, u 1S =v S is the speed control value of the unmanned boat, u 2S =τ S is the control input of the unmanned boat; x 1U =[x U y U z U θ U ψU ] T is the position state of the underwater robot, x 2U =[u U v U w U q U r U ] T is the speed state of the underwater robot, u 1U =v U is the speed control variable of the underwater robot, u 2U =τ U It is the control input of underwater robot.

[0154] The parameters are shown in Table 1.

[0155] Table 1

[0156]

[0157]

[0158] Among them, m 11S 、m 22S 、m 23S 、m 33S is the component of the inertial mass matrix of the unmanned boat, c 13S 、c 23S 、c 31S 、c 32S is the component of the Coriolis centripetal force matrix of the unmanned boat, d 11S d 22S d 23S d 32S d 33S is the component of the hydrodynamic damping coefficient matrix of the unmanned boat, m U is the mass of the underwater robot, I yU is the moment of inertia of the underwater robot about the y-axis, I zU is the moment of inertia of the underwater robot about the z-axis, X uU , Y vU , Z wU 、M qU 、N rU is the linear damping coefficient of the underwater robot, is the hydrodynamic coefficient of the underwater robot, X |u|uU , Y |v|vU , Z |w|wU 、M |q|qU 、N |r|rU is the second-order nonlinear damping coefficient of the underwater robot, m 11U 、m 22U 、m 33U 、m 44U 、m55U is the component of the inertial mass matrix of the underwater robot, c 14U 、c 15U 、c 25U 、c 34U 、c 41U 、c 43U 、c 51U 、c 52U is the component of the Coriolis centripetal force matrix of the underwater robot, d 11U d 22U d 33U d 44U d 55U It is the component of the hydrodynamic damping coefficient matrix of the underwater robot.

[0159] Embodiment 1: Conduct a simulation experiment.

[0160] In the first embodiment, the unmanned boat-underwater robot formation consists of an unmanned boat and an underwater robot. The expected trajectory is designed as (x d ,y d )=(5sin(0.1t+π / 4),-5cos(0.1t+π / 4), where, x d represents the expected position coordinate of the unmanned boat in the x-axis direction, y d represents the expected position coordinate of the unmanned boat in the y-axis direction, and t represents the time. The formation is set to β x =0, β y =0, β z =0, β θ =0, where β x represents the relative expected distance between the underwater robot and the unmanned boat in the x direction, β y represents the relative expected distance between the underwater robot and the unmanned boat in the y direction, β z represents the relative expected distance between the underwater robot and the unmanned boat in the z direction, β θ represents the relative expected angle between the underwater robot and the unmanned boat in pitch angle. Let the unmanned boat and the underwater robot start the rendezvous mission at t = 0. The initial state of the unmanned boat is set to [2-20000] T , the initial state of the underwater robot is set to [0-33000000000] T When no external disturbance is set, the system parameters are as follows: δ = 0.1, u 1S max =5,u 2S max =300,u 1Umax =5,u 2Umax =300, H 1S =P 1S =diag(10 3 ,10 3 ,10 2 ), H 2S =P 2S =diag(10,10,10), R 1S =R 2S =diag(10 -4 ,10 -4 ,10 -4 ), R 1U =R 2U =diag(10 -4 ,10 -4 ,10 -4 ,10 -4 ,10 -4 ), H 1U =P 1U =diag(10 3 ,10 3 ,10 2 ), H 2U =P 2U =diag(10,10,10), Z 1U =Q 1U =diag(10 3 ,10 2 ), Z 2U =Q 2U =diag(10,10),K oS =diag(1.2,1.2,1.2),K pS =diag(1,1,1),K tS =diag(0.8,0.8,0.8),K dS =diag(0.2,0.2,0.2),K oU =diag(1.2,1.2,1.2,1.2,1.2),K pU =diag(1,1,1,1,1), K tU =diag(0.8,0.8,0.8,0.8,0.8),K dU =diag(0.2,0.2,0.2,0.2,0.2).

[0161] Embodiment 2: Conduct a simulation experiment.

[0162] In the second embodiment, the unmanned boat-underwater robot formation consists of one unmanned boat and two underwater robots. The expected trajectory is designed as (x d ,y d0 = (10sin (0.1t + 0.2), 20sin (0.05t + 0.1). The formation is set to β x1 =-2sin(ψ d ), β y1 =2cos(ψ d ), β z1 =3, β θ1 =0, β x2 =2sin(ψ d ), β y2 =-2cos(ψ d ), β z2 =3, β θ2 =0. Among them, β x1 represents the relative expected distance between the underwater robot 1 and the unmanned boat in the x direction, β y1 represents the relative expected distance between the underwater robot 1 and the unmanned boat in the y direction, β z1 represents the relative expected distance between the underwater robot 1 and the unmanned boat in the z direction, β θ1 represents the relative expected angle between the underwater robot 1 and the unmanned boat in terms of pitch angle, ψ d represents the expected yaw angle of the unmanned boat, β x2 represents the relative expected distance between the underwater robot 2 and the unmanned boat in the x direction, β y2 represents the relative expected distance between the underwater robot 2 and the unmanned boat in the y direction, β z2 represents the relative expected distance between the underwater robot 2 and the unmanned boat in the z direction, β θ2 It represents the relative expected angle between the underwater robot 2 and the unmanned boat in pitch angle. Let the unmanned boat and the first underwater robot start the rendezvous mission at t=10, and the unmanned boat and the second underwater robot start the rendezvous mission at t=110. The initial state of the unmanned boat is set to [0.500000] T , the initial state of underwater robot 1 is set to [0.51.52.5000000000] T , the initial state of the second underwater robot is set to [2-0.53.5000000000] T The system parameters are set as follows: δ = 0.1, u 1S max =10,u 2Smax =400,u 1Umax =10,u 2Umax =800, H 1S =P 1S =diag(10 3 ,10 3 ,10 2), H 2S =P 2S =diag(10,10,10), R 1S =R 2S =diag(10 -4 ,10 -4 ,10 -4 ), R 1U =R 2U =diag(10 -4 ,10 -4 ,10 -4 ,10 -4 ,10 -4 ), H 1U =P 1U =diag(10 3 ,10 3 ,10 2 ), H 2U =P 2U =diag(10,10,10), Z 1U =Q 1U =diag(10 3 ,10 2 ), Z 2U =Q 2U =diag(10,10),K oS =diag(1.2,1.2,1.2),K pS =diag(1,1,1),K tS =diag(0.8,0.8,0.8),K dS =diag(0.2,0.2,0.2),K oU =diag(1.2,1.2,1.2,1.2,1.2),K pU =diag(1,1,1,1,1), K tU =diag(0.8,0.8,0.8,0.8,0.8),K dU =diag(0.2,0.2,0.2,0.2,0.2).

[0163] Time-varying external disturbances are simulated using trigonometric functions:

[0164]

[0165] In the formula, τ wS represents the external disturbance to the unmanned boat, τ wU Represents the external environmental disturbance to which the underwater robot is subjected.

[0166] Figure 2 This is a 3D spatial trajectory diagram of the unmanned boat-underwater robot formation in Example 1. Figure 3 for Figure 2 The projection of the spatial trajectory onto the 2D plane xy plane. Figure 4 : is the position error diagram of the unmanned boat and the underwater robot in the formation in Example 1, (Xe, Ye) is the coordinate error of their current position relative to their position in the formation, Xe is the error on the x coordinate, and Ye is the error on the y coordinate. Figure 5 This is a graph showing the depth of the underwater robot in the z-axis direction versus time in Example 1. Figure 6 and Figure 7 This is a control input curve diagram of the unmanned boat and underwater robot in Example 1 that changes over time. Figure 8 This is a 3D spatial trajectory diagram of the unmanned boat-underwater robot formation in Example 2. Fig. 9 for Figure 8 The projection of the spatial trajectory onto the 2D plane xy plane. Fig.10 : is the position error diagram of the unmanned boat and the underwater robot in the formation in Example 2, (Xe, Ye) is the coordinate error of their current position relative to their position in the formation, Xe is the error on the x coordinate, and Ye is the error on the y coordinate. Fig.11 This is a graph showing the depth of the underwater robot in the z-axis direction versus time in Example 2. Fig.12 and Fig.13 This is a control input curve diagram of the unmanned boat and underwater robot in Example 2 that changes with time.

[0167] By analyzing the data and images of Example 1 and Example 2, it can be seen that the unmanned boat-underwater robot formation can effectively and quickly complete the dynamic rendezvous task in the process of tracking the desired trajectory, and the motion trajectory is relatively smooth. The formation can quickly and effectively build a formation, and the formation remains stable during the formation navigation, the error can be controlled within a very small range, the fluctuation is very small, and the accuracy is high.

[0168] Those skilled in the art will appreciate that the embodiments described herein are intended to help readers understand the principles of the present invention, and should be understood that the protection scope of the present invention is not limited to such specific statements and embodiments. Those skilled in the art can make various other specific variations and combinations that do not deviate from the essence of the present invention based on the technical revelations disclosed by the present invention, and these variations and combinations are still within the protection scope of the present invention.

Claims

1. A formation dynamic rendezvous control method for an unmanned boat and an underwater robot, characterized in that: The following steps are involved: S1. Build the system model of unmanned boat and underwater robot; S2. Generate control targets for the formation of the unmanned boat and the underwater robot according to the system model of the unmanned boat and the underwater robot; S3, setting controller; S4. According to the controller, construct the Lyapunov function and generate the auxiliary control law; S5. Perform dynamic rendezvous control of the formation according to the controller provided with the auxiliary control law and the control targets of the formation of the unmanned boat and the underwater robot.

2. The formation dynamic rendezvous control method of an unmanned boat and an underwater robot according to claim 1, characterized in that: The S1 comprises the following sub-steps: S11. Construct the kinematic model and dynamic model of the unmanned boat; S12. Construct kinematic and dynamic models of underwater robots; S13. Obtain a system model of the unmanned boat and the underwater robot according to the kinematic model and dynamic model of the unmanned boat and the kinematic model and dynamic model of the underwater robot.

3. The formation dynamic rendezvous control method of an unmanned boat and an underwater robot according to claim 2, characterized in that: In S11, the kinematic model of the unmanned boat is expressed as: In the formula, R S represents the rotation matrix between the earth coordinate system and the unmanned boat hull coordinate system, ψ S represents the yaw angle of the unmanned boat, represents the first-order derivative of the position state vector of the unmanned boat with respect to time, ν S represents the velocity state vector of the unmanned boat; In S11, the expression of the dynamic model of the unmanned boat is: Where M S represents the inertial mass matrix of the unmanned boat, represents the first-order derivative of the velocity state vector of the unmanned boat with respect to time, C S Denotes the Coriolis centripetal force matrix of the unmanned boat, D S represents the hydrodynamic damping coefficient matrix of the unmanned boat, τ S represents the first control input of the unmanned boat, τ wS Indicates the external disturbance to the unmanned boat.

4. The formation dynamic rendezvous control method of an unmanned boat and an underwater robot according to claim 2, characterized in that: In S12, the expression of the kinematic model of the underwater robot is: In the formula, J U represents the rotation matrix between the earth coordinate system and the underwater robot body coordinate system, η U represents the position state vector of the underwater robot, represents the first-order derivative of the underwater robot's position state vector with respect to time, ν U represents the velocity state vector of the underwater robot; In S12, the expression of the dynamic model of the underwater robot is: Where M U represents the inertial mass matrix of the underwater robot, represents the first-order derivative of the velocity state vector of the underwater robot with respect to time, C U represents the Coriolis centripetal force matrix of the underwater robot, D U represents the fluid damping matrix of the underwater robot, g U represents the restoring force of buoyancy and gravity on the underwater robot, τ U represents the control input of the underwater robot, τ wU Represents the external environmental disturbance to which the underwater robot is subjected.

5. The formation dynamic rendezvous control method of an unmanned boat and an underwater robot according to claim 2, characterized in that: In S13, the expression of the system model of the unmanned boat and the underwater robot is: In the formula, It represents the first-order derivative of the position state of the unmanned boat with respect to time, R S represents the rotation matrix between the earth coordinate system and the unmanned boat hull coordinate system, ψ S represents the yaw angle of the unmanned boat, ν S represents the velocity state vector of the unmanned boat, f 1S Represents the first functional relationship of the unmanned boat in the system model, x 1S Indicates the position status of the unmanned boat, u 1S represents the speed control value of the unmanned boat, represents the first-order derivative of the position state of the underwater robot with respect to time, J U represents the rotation matrix between the earth coordinate system and the underwater robot body coordinate system, η U represents the position state vector of the underwater robot, ν U represents the velocity state vector of the underwater robot, f 1U Represents the first functional relationship of the underwater robot in the system model, x 1U Indicates the position state of the underwater robot, u 1U represents the speed control value of the underwater robot, It represents the first-order derivative of the speed state of the unmanned boat with respect to time, M S represents the inertial mass matrix of the unmanned boat, τ S represents the first control input of the unmanned boat, τ wS represents the external environmental disturbance to the unmanned boat, C S Denotes the Coriolis centripetal force matrix of the unmanned boat, D S represents the hydrodynamic damping coefficient matrix of the unmanned boat, f 2S Represents the second functional relationship of the unmanned boat in the system model, x 2S Indicates the speed status of the unmanned boat. It represents the first-order derivative of the velocity state of the underwater robot with respect to time, M U represents the inertial mass matrix of the underwater robot, τ U represents the first control input of the underwater robot, τ wU represents the external environmental disturbance to the underwater robot, C U represents the Coriolis centripetal force matrix of the underwater robot, D U represents the fluid damping matrix of the underwater robot, g U represents the restoring force of buoyancy and gravity on the underwater robot, f 2U Represents the second functional relationship of the underwater robot in the system model, x 2U represents the speed state of the underwater robot, u 2S represents the second control input of the unmanned boat, u 2U Represents the second control input of the underwater robot.

6. The formation dynamic rendezvous control method of an unmanned boat and an underwater robot according to claim 1, characterized in that: In S2, the control target of the unmanned boat and underwater robot formation is expressed as: In the formula, represents the position error between the i-th underwater robot and the unmanned boat on the horizontal plane, represents the speed error between the i-th underwater robot and the unmanned boat on the horizontal plane, z Ui represents the position coordinate of the i-th underwater robot in the z direction, represents the expected trajectory tracking error of the unmanned boat, represents the position state vector of the i-th underwater robot in the horizontal direction, represents the velocity state vector of the i-th underwater robot in the horizontal direction, ν S represents the velocity state vector of the unmanned boat, η d represents the desired position state vector in the horizontal direction, η S Represents the position state vector of the unmanned boat.

7. The formation dynamic rendezvous control method of an unmanned boat and an underwater robot according to claim 1, characterized in that: In S3, the expression of the controller is: st Where J represents the performance function of the controller, represents the predicted amount of unmanned boats, represents the predicted quantity of the underwater robot, S(δ) represents the piecewise function system of the sampling period δ, δ represents the sampling period, represents the prediction time domain, represents the state error of the unmanned boat, s represents the independent variable of the integrated term, H nS represents the first weighted matrix or the second weighted matrix corresponding to the unmanned boat, represents the predicted speed control quantity or predicted control input of the unmanned boat, R nS represents the third weighted matrix or the fourth weighted matrix corresponding to the unmanned boat, represents the terminal state error of the unmanned boat, P nS represents the fifth weighted matrix or the sixth weighted matrix corresponding to the unmanned boat, N represents the total number of underwater robots, represents the state error of the i-th underwater robot on the horizontal plane, H nU represents the first weighted matrix or the second weighted matrix corresponding to the underwater robot, represents the state error of the i-th underwater robot on the vertical plane, Z nU represents the third weighted matrix or the fourth weighted matrix corresponding to the underwater robot, represents the predicted speed control quantity or predicted control input of the ith underwater robot, R nU represents the fifth weighted matrix or the sixth weighted matrix corresponding to the underwater robot, represents the terminal state error of the i-th underwater robot on the horizontal plane, P nU represents the seventh weighted matrix or the eighth weighted matrix corresponding to the underwater robot, represents the terminal state error of the i-th underwater robot on the vertical plane, Q nU represents the ninth weighted matrix or the tenth weighted matrix corresponding to the underwater robot, represents the first-order derivative of the predicted state of the unmanned boat with respect to the independent variable s, f nS Represents the first functional relationship or the second functional relationship of the unmanned boat in the system model, represents the predicted state of the unmanned boat, represents the first-order derivative of the predicted state of the i-th underwater robot with respect to the independent variable s, f nU represents the first functional relationship or the second functional relationship of the underwater robot in the system model, represents the predicted state of the i-th underwater robot, Indicates the current state of the unmanned boat, x nS (t0) represents the current state of the unmanned boat, represents the current state of the i-th underwater robot, x nUi (t0) represents the current state of the i-th underwater robot, u nSmax Indicates the maximum speed control value or maximum control input of the unmanned boat, u nUmax represents the maximum speed control value or maximum control input of the ith underwater robot, V nS represents the outer loop Lyapunov function or inner loop Lyapunov function of the unmanned boat, x nS Indicates the state of the unmanned boat, h nS represents the outer loop auxiliary control law or inner loop auxiliary control law of the unmanned boat, V nUi represents the outer loop Lyapunov function or inner loop Lyapunov function of the i-th underwater robot, x nUi represents the state of the i-th underwater robot, h nUi represents the outer loop auxiliary control law or inner loop auxiliary control law of the i-th underwater robot, It represents the current speed control value or current control input of the unmanned boat, and t0 represents time.

8. The formation dynamic rendezvous control method of an unmanned boat and an underwater robot according to claim 1, characterized in that: In S4, the Lyapunov function includes an outer loop Lyapunov function and an inner loop Lyapunov function of the unmanned boat and an outer loop Lyapunov function and an inner loop Lyapunov function of the underwater robot, and the auxiliary control law includes an outer loop auxiliary control law and an inner loop auxiliary control law of the unmanned boat and an outer loop auxiliary control law and an inner loop auxiliary control law of the underwater robot; The outer loop Lyapunov function V of the unmanned boat 1S The expression is: In the formula, for The transpose of K oS represents the first positive definite diagonal matrix corresponding to the unmanned boat, represents the trajectory tracking error of the unmanned boat; The outer loop auxiliary control law of the unmanned boat h 1S The expression is: In the formula, For R S The inverse of R S represents the rotation matrix between the earth coordinate system and the unmanned boat hull coordinate system, ψ S represents the yaw angle of the unmanned boat, represents the first-order derivative of the desired position state vector in the horizontal direction with respect to time, K pS represents the second positive definite diagonal matrix corresponding to the unmanned boat, represents the trajectory tracking error of the unmanned boat; The inner loop Lyapunov function V of the unmanned boat 2S The expression is: In the formula, for The transpose of K tS represents the third positive definite diagonal matrix corresponding to the unmanned boat, represents the speed tracking error of the unmanned boat; The inner loop auxiliary control law of the unmanned boat h 2S The expression is: Where M S represents the inertial mass matrix of the unmanned boat, represents the first-order derivative of the desired velocity state vector in the horizontal direction with respect to time, K dS represents the fourth positive definite diagonal matrix corresponding to the unmanned boat, represents the speed tracking error of the unmanned boat, C S represents the Coriolis centripetal force matrix of the unmanned boat, ν S represents the velocity state vector of the unmanned boat, D S represents the hydrodynamic damping coefficient matrix of the unmanned boat, τ wS Indicates the external disturbance to which the unmanned boat is subjected; The outer loop Lyapunov function V of the i-th underwater robot 1Ui The expression is: In the formula, express The transpose of K oU represents the first positive definite diagonal matrix corresponding to the underwater robot, represents the trajectory tracking error of the i-th underwater robot; The outer loop auxiliary control law h of the i-th underwater robot 1Ui The expression is: In the formula, Indicates J U The inverse of J U represents the rotation matrix between the earth coordinate system and the underwater robot body coordinate system, η Ui represents the position state vector of the i-th underwater robot, represents the first-order derivative of the desired position state vector of the i-th underwater robot with respect to time, K pU represents the second positive definite diagonal matrix corresponding to the underwater robot, represents the trajectory tracking error of the i-th underwater robot; The inner loop Lyapunov function V of the i-th underwater robot 2Ui The expression is: In the formula, express The transpose of K tU represents the third positive definite diagonal matrix corresponding to the underwater robot, represents the velocity tracking error of the i-th underwater robot; The inner loop auxiliary control law h of the i-th underwater robot 2Ui The expression is: Where M U represents the inertial mass matrix of the underwater robot, represents the first-order derivative of the desired velocity state vector of the ith underwater robot with respect to time, K dU represents the fourth positive definite diagonal matrix corresponding to the underwater robot, represents the velocity tracking error of the i-th underwater robot, C Ui represents the Coriolis centripetal force matrix of the ith underwater robot, ν Ui represents the velocity state vector of the ith underwater robot, D Ui represents the fluid damping matrix of the i-th underwater robot, g Ui represents the restoring force of buoyancy and gravity on the i-th underwater robot, η Ui represents the position state vector of the i-th underwater robot, τ wUi Represents the external environmental disturbance to which the i-th underwater robot is subjected.

9. The formation dynamic rendezvous control method of an unmanned boat and an underwater robot according to claim 1, characterized in that: In S5, the control objectives of the unmanned boat and the underwater robot formation are optimized to obtain an optimal solution sequence, thereby completing the dynamic rendezvous control of the formation.

Images