A multi-module ship finite time fast cooperative obstacle avoidance control method, program, device and storage medium

Abstract

The present application relates to a kind of multi-module ship finite time fast collaborative obstacle avoidance control method, program, equipment and storage medium.The present application considers the requirement of multi-module ship to formation speed when executing task, based on backstepping method and finite time instruction filter, finite time distributed formation controller is designed;Using function segmentation design idea, dynamic artificial potential field method is used to realize formation to avoid static obstacles, avoid dynamic obstacles and dynamic collision avoidance between module ships, and static obstacle avoidance, dynamic obstacle avoidance and dynamic collision avoidance control item in formation control rate are designed;While considering the influence of external interference, finite time disturbance observer is added to estimate external time-varying disturbance;In order to reduce module ship communication consumption and prolong controller update cycle, event trigger mechanism is introduced into the multi-module ship collaborative formation controller.The present application can ensure that multi-module ship formation is stable, safe and executes task under the condition of environmental disturbance.

Description

Technical Field

[0001] This invention belongs to the field of unmanned surface vessel (USV) formation motion control technology, specifically relating to a multi-module vessel finite-time rapid collaborative obstacle avoidance control method, program, device, and storage medium. Background Technology

[0002] Modular vessels are a type of fully driven surface vessel with dynamic positioning capabilities. Multiple modular vessels can be assembled into a Mobile Offshore Base (MOB) to move to a predetermined location to meet needs such as aircraft takeoff and landing, cargo transfer, and resupply. For the coordinated motion control of multiple modular vessels, obstacle avoidance is a prerequisite for successful mission completion. This requires considering not only static or dynamic obstacles in the sea area but also collision avoidance between modular vessels. Furthermore, modular vessels are affected by external disturbances such as wind, waves, and currents during navigation, which can impact the control accuracy and effectiveness of the controller to varying degrees. Additionally, in practical engineering, frequent controller updates can lead to actuator wear and tear. Summary of the Invention

[0003] The purpose of this invention is to provide a method, program, device, and storage medium for rapid collaborative obstacle avoidance control of multi-module ships within a limited time. This invention enables distributed control of dynamic collision avoidance and static obstacle avoidance problems of multi-module ships within a limited time, while introducing an event triggering mechanism to ensure safe, stable, and reliable system operation and improve the accuracy of path tracking control.

[0004] A finite-time rapid cooperative obstacle avoidance control method for multi-module ships includes the following steps:

[0005] Step 1: Determine the trigger interval T for execution control. The multi-module ship formation consists of n module ships. Construct a virtual pilot module ship and n real-world module ships as follower module ships. Treat each module ship as a node in a graph. The information interaction between module ships is represented by an edge connecting two nodes. Use graph G = (V, E) to represent the network communication relationship of the multi-module ship formation consisting of the virtual pilot module ship and the n real-world follower module ships.

[0006] Step 2: For each following module ship υ i Obtain its current pose information η i (t) and velocity information υ i (t), to obtain the position and velocity information of other modular ships and dynamic and static obstacles in the environment;

[0007] Step 3: For each following module ship i Select a suitable formation reference vector l i (t), according to li (t) Calculate the following module ship υ i The reference point η that forms the desired formation di (t); according to η di (t) and η i (t) Calculate the following module ship υ i Position tracking error vector z i1 (t);

[0008] Step 4: Based on υ i (t) and the output vector of the finite-time command filter at time tT Calculate the following modules of the ship. i velocity error vector z i2 (t);

[0009] Step 5: Obtain the data of each following module ship using a finite-time interference observer. i Velocity observation information according to With υ i (t) Calculate the following module ship υ i observation error w i (t), according to w i (t) Estimate the following module ships υ i Interference from finite-time interference observers

[0010] Step 6: Construct each following module ship. i The filtered compensation signal, according to z i1 (t) and z i2 (t) Calculate the following module ship υ i Position compensation error s i1 (t) and velocity compensation error s i2 (t);

[0011] Step 7: Calculate the following module ship υ i Formation control law τ i (t): The dynamic artificial potential field method is used to calculate the υ of each following module ship. i Formation collision avoidance control items Dynamic obstacle avoidance control items and static obstacle avoidance control items After weighting, it is added to the formation control law τ i In (t), the final formation control law is obtained.

[0012] Step 8: Each following module ship i Execution of formation control law If the multi-module ship formation has not completed the task until time t+T, step 9 is executed; otherwise, the control is stopped.

[0013] Step 9: Calculate the virtual control amount α i of the current time t i1 (t) as the input vector of the finite time command filter, and the output vector of the finite time command filter is i1 (t) as the input vector of the finite time command filter, and the output vector of the finite time command filter is Return to step 2.

[0014] Further, in step 1, V={υ1,υ2,...,υ n} is a node set, and E is a set of edges; an edge ε ij =(υ i ,υ j )∈E indicates that the follower module ship υ i can transmit information to the follower module ship υ j , and the follower module ship υ i is a neighbor of the follower module ship υ j , and the edge does not exist, indicating that the two nodes cannot transmit information; i,j=1,2,...,n.

[0015] The adjacency matrix A of G is a ij is the element in the i-th row and the j-th column of A, the diagonal element a ii =0, and the off-diagonal element a ij is the weight of the edge ε ij , a ij >0 indicates that the follower module ship υ i and the follower module ship υ j exist communication, otherwise a ij =0.

[0016] The degree matrix D of G is D=diag{d1,d2,...,d n},

[0017] In step 2, η i (t)=[x i (t),y i (t),ψ i (t)] T ,υ i (t)=[u i (t),v i (t),r i (t)] T ; wherein X i (t) =(x i (t),y i (t)) is the follower module ship υi the position at the current t moment, ψ i (t) is the following module ship υ i the heading angle at the current t moment.

[0018] Further, the reference point η di (t) of the desired formation in step 3

[0019] η di (t) = η i (t) + R(ψ i )l i (t)

[0020] wherein,

[0021] the position tracking error vector z i (t) of the following module ship υ i1 (t) is:

[0022]

[0023] wherein, a i0 is the weight coefficient of the following module ship υ i tracking the desired formation reference point η di (t);

[0024] the speed error vector z i (t) of the following module ship υ i2 (t) in step 4

[0025]

[0026] Further, the observation error w i (t) of the following module ship υ i (t) in step 5

[0027]

[0028] wherein, M i is the inertia matrix of the following module ship υ i containing the hydrodynamic added mass at the current t moment;

[0029]

[0030] wherein, m i is the mass of the following module ship υ i x gi is the vertical distance from the origin of the ship body coordinate system of the following module ship υ i to the center; i ​derivative of the additional mass force with respect to acceleration at the current time t; I zi (t) is the following module ship υ i derivative of the moment of inertia with respect to rotation at the current time t;

[0031] According to w i (t) estimate the finite-time disturbance observer disturbance of each following module ship υ i

[0032]

[0033] Where λ1, λ2∈R 3×3 is a positive definite diagonal matrix, 0.5≤δ1<1, δ2=2δ1-1.

[0034] Further, the step 6 of constructing the filter compensation signal of each following module ship υ i

[0035]

[0036] Where b i1 (t), b i2 (t) are the position error compensation and speed error compensation signals of the following module ship υ i , respectively, and b i1 (0)=0, b i2 (0)=0; k i1 , k i2 are normal numbers, and

[0037] The position compensation error s i (t) and the speed compensation error s i1 (t) of each following module ship υ i2

[0038] s i1 (t)=z i1 (t)-b i1 (t)

[0039] s i2 (t)=z i2 (t)-b i2 (t).

[0040] Further, the step 7 of the formation control law τ i (t) of each following module ship υ i

[0041]

[0042] Where D​​​​​i to follow the module ship υ i the damping matrix,

[0043]

[0044] represents the repulsive force F i of the static obstacle c to follow the module ship υ ric using dynamic artificial potential field method, is expressed as:

[0045]

[0046] where d ic = X i (t)-X c = [x i (t)-x c , y i (t)-y c ] T , i.e. the position coordinates of the module ship υ i minus the position coordinates of the static obstacle c; R imax represents the collision danger radius of the module ship υ i , R imin represents the collision radius of the module ship υ i , ||d ic ||≤R imin represents that the module ship υ i collides with the static obstacle c;

[0047] represents the repulsive force F j of the dynamic obstacle e or other module ship υ i in the multi-module ship formation to follow the module ship υ rij is expressed as:

[0048]

[0049] where d ij = X i (t)-X j (t) = [x i (t)-x j (t), y i (t)-y j (t)] T , let ψ ij = ψ j (t)-ψ i (t), η ij ∈(0,1);

[0050] Each static obstacle c is paired with the following module ship υ i repulsive force F ric By superimposing these values, we obtain the static obstacle avoidance control term. for:

[0051]

[0052] Each dynamic obstacle e will follow the module ship υ i repulsive force F rij By superimposing these values, a dynamic obstacle avoidance control term is obtained. for:

[0053]

[0054] Other follower module ships in the multi-follower module ship formation j For follow-up module ship υ i repulsive force F rij By superimposing these values, we obtain the formation collision avoidance control terms. for:

[0055]

[0056] Follow the modular ship i Final formation control law for:

[0057]

[0058] Where κ1, κ2, and κ3 are weighting coefficients.

[0059] Furthermore, in step 9, the following module ship υ i The virtual control quantity α at time t i1 (t) is:

[0060]

[0061] in, ι i1 γ is a positive constant, and 0 < γ < 1.

[0062] A computer device / equipment / system includes a memory, a processor, and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of the above-described multi-module ship finite-time rapid cooperative obstacle avoidance control method.

[0063] A computer-readable storage medium having a computer program / instructions stored thereon, which, when executed by a processor, implements the steps of the aforementioned multi-module ship finite-time rapid cooperative obstacle avoidance control method.

[0064] A computer program product comprises computer programs / instructions which, when executed by a processor, implement the steps of the multi-module ship finite time fast cooperative obstacle avoidance control method.

[0065] The present application has the advantages of:

[0066] The present application considers the requirement of multi-module ship for formation speed when performing a task, designs a finite time distributed formation controller based on backstepping method and finite time command filter, uses functional segmentation design idea, uses dynamic artificial potential field method to realize avoidance of static obstacles, avoidance of dynamic obstacles and dynamic collision avoidance between module ships, designs static obstacle avoidance, dynamic obstacle avoidance and dynamic collision avoidance control items in the formation control rate, and adds a finite time disturbance observer to estimate external time-varying disturbance considering the influence of external disturbance. BRIEF DESCRIPTION OF DRAWINGS

[0067] Figure 1 A schematic diagram for multi-module ship formation navigation in a dynamic environment.

[0068] Figure 2 A schematic diagram for virtual leader cooperative formation by introducing a formation reference point.

[0069] Figure 3 A schematic diagram for distributed formation of multi-module ships.

[0070] Figure 4 A control block diagram of the finite time command filter.

[0071] Figure 5 A schematic diagram of forces on the controlled object by the artificial potential field method.

[0072] Figure 6 A schematic diagram for collision avoidance of multi-module ship cooperative motion formation.

[0073] Figure 7 A schematic diagram for obstacle avoidance of module ships in the case of dynamic sudden obstacles.

[0074] Figure 8 A schematic diagram of multi-module ship formation motion trajectory (at t=320s, 390s, 410s, 600s) using the present application. DETAILED DESCRIPTION

[0075] The present application will be further described below in conjunction with the drawings.

[0076] A multi-module ship formation consists of n module ships. Each module ship is considered as a node in a graph. The information interaction between module ships is represented by an edge connecting two nodes. The graph G = (V, E) represents the network communication relationship of the multi-module ship formation consisting of a virtual pilot module ship and n real follower module ships. The virtual pilot module ship is used to provide the desired path and desired speed to each real follower module ship.

[0077] Where, V={υ1,υ2,...,υ n Let} be the set of nodes, and E be the set of edges; edge ε ij =(υ i ,υ j )∈E represents module ship υ i Capable of providing modular ships j Transmitting information, and the module ship υ i For modular ships j A neighbor of a node; if an edge does not exist, it means that information cannot be transmitted between the two nodes; i,j = 1,2,...,n;

[0078] The adjacency matrix of G a ij Let a be the element in the i-th row and j-th column of A, and let a be the diagonal element. ii =0, off-diagonal element a ij For edge ε ij The weight, a ij >0 indicates a modular ship υ i With modular ship j Communication exists, otherwise a ij =0;

[0079] The degree matrix D of G = diag{d1,d2,...,d n},

[0080] If all modular ships in the formation could directly receive control information from the virtual pilot ship, then the controllers of each modular ship could be designed to track the reference trajectory. However, due to communication distance limitations, some modular ships in the formation would not be able to directly receive control information from the virtual pilot ship. Figure 2 This is a schematic diagram of a multi-module ship formation under limited communication distance. In this case, we need to consider how to complete the formation task by using the status information of neighboring module ships. Therefore, we need to design the controller as a distributed formation controller.

[0081] The concept of formation reference point assumes that the virtual leader moves along a given reference trajectory, while each of the remaining follower module ships has a corresponding formation reference point. The purpose of the formation reference point is to apply a formation keeping control algorithm to handle the cooperative formation control problem of multiple module ships. To explicitly represent the relative position between the follower module ships and the virtual leader, we select a suitable formation reference vector l for each follower module ship i (t) to calculate the reference point η di (t) of each follower module ship forming the desired formation. The derived cooperative control law is used to make the formation reference point of each follower module ship infinitely close to the same ideal state point, ensuring that all module ships can complete the cooperative task. The schematic diagram of the scheme is shown in Figure 3 .

[0082] η di (t) = η i (t) + R(ψ i )l i (t)

[0083] wherein,

[0084] The trigger interval T for executing control is determined. For each module ship υ i , the current time self-position information η i (t) = [x i (t), y i (t), ψ i (t)] T and velocity information υ i (t) = [u i (t), v i (t), r i (t)] T are obtained, as well as the position and velocity information of other module ships and dynamic and static obstacles in the environment; wherein X i (t) = (x i (t), y i (t)) is the position of the module ship υ i at the current t time, and ψ i (t) is the heading angle of the module ship υ i at the current t time;

[0085] The velocity observation information i of each module ship υ i is obtained through the finite time disturbance observer The observation error w i (t) of each module ship υ i is calculated;

[0086]

[0087] Among them, M i For modular ships i The inertial matrix at time t, including hydrodynamic added mass;

[0088]

[0089] Where, m i For modular ships i quality; x gi For modular ships i The vertical distance from the origin of the ship's coordinate system to the center; For modular ships i The derivative of the additional mass force with respect to acceleration at the current time t; I zi (t) represents the modular ship υ i The moment of inertia at the current time t;

[0090] Estimate the υ of each module ship i Interference from finite-time interference observers

[0091]

[0092] Where λ1,λ2∈R 3×3 It is a positive definite diagonal matrix, 0.5≤δ1<1,δ2=2δ1-1;

[0093] Calculation module ship υ i Position tracking error vector z i1 (t) and velocity error vector z i2 (t);

[0094]

[0095] Among them, a i0 For modular ships i Tracking expected formation reference point η di The weighting coefficients of (t), a i0 The larger the value, the more likely it is to be a modular ship. i The more inclined to track the desired formation reference point, the better. i0 Smaller or zero indicates module ship υ i It primarily adjusts its position through interaction with other modular ships, rather than directly tracking the desired formation reference point;

[0096]

[0097] Among them, the virtual control quantity α at the previous trigger time, i.e., time tT. i1The output vector of the finite-time command filter is The control signal maintains a constant value between t-T and t

[0098] In order to eliminate the filtering error, a filtering compensation signal is designed:

[0099]

[0100] Wherein, b i1 (t), b i2 (t) are the position error compensation and speed error compensation signals of the module ship υ i , and b i1 (0) = 0, b i2 (0) = 0; k i1 , k i2 are normal numbers, and

[0101] The position compensation error s i (t) of the module ship υ i1 :

[0102] s i1 (t) = z i1 (t) - b i1 (t)

[0103] The virtual control quantity a i (t) of the module ship υ i1 :

[0104]

[0105] Wherein, ι i1 is a normal number, and γ is a normal number and 0 < γ < 1;

[0106] The speed compensation error s i (t) of the module ship υ i2 :

[0107] s i2 (t) = z i2 (t) - b i2 (t)

[0108] The formation control law τ i (t) of the module ship υ i :

[0109]

[0110] Wherein, D i is the module ship υi The damping matrix,

[0111]

[0112] like Figure 5 , Figure 6 and Figure 7 As shown, for each module ship υ i The dynamic artificial potential field method is used to calculate the υ of the module ship respectively. i Formation collision avoidance control items Dynamic obstacle avoidance control items and static obstacle avoidance control items After weighting, add it to the module ship υ i In the formation control law, the parameters of each module ship are obtained from the current time t until the trigger time t+T of the next control execution. i Formation control law

[0113]

[0114] Where κ1, κ2, and κ3 are weighting coefficients;

[0115] Static obstacle c for modular ship υ i repulsive force F ric Represented as:

[0116]

[0117] Where, d ic =X i (t)-X c =[x i (t)-x c ,y i (t)-y c ] T Modular ship i Subtract the position coordinates of the static obstacle c from the position coordinates of R; imax Represents modular ship υ i The collision danger radius, R imin Represents modular ship υ i The collision radius, ||d ic ||≤R imin Represents modular ship υ i Collision with static obstacle c;

[0118] Dynamic obstacles e or other modular ships in a multi-module ship formation υ j For modular ships i repulsive force F rij Represented as:

[0119]

[0120] where d ij = X i (t)-X j (t) = [x i (t)-x j (t), y i (t)-y j (t)] T Let ψ ij = ψ j (t)- ψ i (t), η ij ∈(0,1);

[0121] The repulsion force F i of each static obstacle c to the module ship υ ric is superimposed to obtain the static obstacle avoidance control term is:

[0122]

[0123] The repulsion force F i of each dynamic obstacle e to the module ship υ rij is superimposed to obtain the dynamic obstacle avoidance control term is:

[0124]

[0125] The repulsion force F j of other module ships υ i to the module ship υ rij is superimposed to obtain the formation collision avoidance control term is:

[0126]

[0127] The virtual control quantity α i1 (t) at the current time t is taken as the input vector of the finite time command filter, and the output vector of the finite time command filter is Each module ship υ i executes the formation control law until time t+T, if the multi-module ship formation has not completed the task, the above steps are repeated to execute the next control.

[0128] The above merely provides the preferred embodiments of the present application, and is not used to limit the present application. For those skilled in the art, the present application can have various modifications and changes. Any modifications, equivalent replacements, improvements, etc. made within the principles and technical scope of the present application shall fall into the scope of the present application.

Claims

1. A finite-time fast cooperative obstacle avoidance control method for a multi-module ship, characterized in that, comprising the steps of: Step 1: determine the trigger interval T of the execution control, the multi-module ship formation is composed of a virtual leading module ship, a real existing module ship as a following module ship; each module ship is regarded as a node in the figure, the information interaction between the module ships is represented by an edge connecting two nodes, and the network communication relationship of the multi-module ship formation composed of the virtual leading module ship and the real existing following module ship is represented by a graph . Step 2: For each following module ship To obtain its own pose information at the current moment. and speed information To obtain position and velocity information of other modular ships and dynamic and static obstacles in the environment; Step 3: For each following module ship Select a suitable formation reference vector ,according to Calculate the following module ships Reference points for forming the desired formation ;according to and Calculate each following module ship Position tracking error vector ; Step 4: According to With The output vector of the finite-time command filter at the moment , the speed error vector of each following module ship is calculated ; Step 5: Obtain information about each following module ship using a finite-time interference observer. Velocity observation information ,according to and Calculate each following module ship observation error ; ； wherein, Follow-up module ship Inertial matrix at current time t including hydrodynamic added mass; ； wherein mass of the following module ship ; vertical distance from the origin of the body coordinate system of the following module ship to the center; , , , , derivative of the additional mass force of the following module ship with respect to acceleration at the current time t; moment of inertia of the following module ship at the current time t; according to Estimate the number of accompanying module ships Interference from finite-time interference observers ; ； wherein is a positive definite diagonal matrix, ; Step 6: Constructing each following module ship of the filter compensation signal according to and calculating the position compensation error and the speed compensation error of each following module ship ; Step 7: Calculate the formation control law of each following module ship Step 8: Calculate the formation control law of each following module ship Step 9: Calculate the formation collision avoidance control term , dynamic obstacle avoidance control term and static obstacle avoidance control term of each following module ship respectively by using dynamic artificial potential field method, and add them to the formation control law after weighting to obtain the final formation control law ; Step 8: Each follower module ship Execute formation control law , until time, if the multi-module ship formation has not completed the task, execute step 9; otherwise, stop control; Step 9: Calculate each following module ship Current Virtual control variable at the moment , the input vector of the finite time command filter, the output vector of the finite time command filter , return to step 2.

2. The finite-time fast co-avoidance control method for multi-module ship according to claim 1, characterized in that: The step 1 in is a set of nodes, is a set of edges; edge represents a following module ship is capable of transmitting information to a following module ship and the following module ship is a neighbor of the following module ship does not exist if the two nodes cannot transmit information to each other; ; adjacency matrix of , is the element in the i-th row and j-th column of the diagonal elements , the off-diagonal elements is the weight of the edge , denotes that the following module ship is in communication with the following module ship , otherwise ; matrix of degrees , ; in step 2 , ; wherein, is the position of the following module ship at the current time t, is the heading angle of the following module ship at the current time t.

3. The finite-time fast co-avoidance control method for multi-module ship according to claim 2, characterized in that: The reference point of the desired formation in step 3 Is: ； wherein ; Following module ship a position tracking error vector is: ； wherein, Following module ship Tracking a desired formation reference point Weight coefficients; The step 4 following module ship The speed error vector is: 。 4. The finite-time fast co-avoidance control method for multi-module ship according to claim 3, characterized in that: The step 6 constructs each following module ship The filter compensation signal is: wherein are position error compensation and velocity error compensation signals, respectively, for the following module ship , and ; is a positive constant, , and ; ; The position compensation error of each follower module ship is the speed compensation error is : ； 。 5. The finite-time fast co-avoidance control method for multi-module ship according to claim 4, characterized in that: The step 7 of each following module ship The formation control law is: ； wherein a following module ship a damping matrix, ; ； Using the dynamic artificial potential field method, the static obstacle c is used for the following module ship. repulsive force Represented as: ； wherein i.e. the following module ship the position coordinates of the static obstacle c are subtracted from the position coordinates of the following module ship denotes the collision danger radius of the following module ship denotes the collision radius of the following module ship denotes the collision radius of the following module ship denotes the collision of the following module ship denotes the collision of the following module ship with the static obstacle c; Dynamic obstacle e or multiple following module ships in a formation of other following module ships For following module ships Repulsion Is expressed as: ； wherein , let , , , ; Each static obstacle c is paired with the following module ship. repulsive force By superimposing these values, we obtain the static obstacle avoidance control term. for: ； Each dynamic obstacle e pairs with a follower module ship repulsion force superimposed to get the dynamic obstacle avoidance control term is: ； To follow the other following module ships in the multi-following module ship formation To follow the other following module ships in the multi-following module ship formation To follow the other following module ships in the multi-following module ship formation To follow the other following module ships in the multi-following module ship formation To follow the other following module ships in the multi-following module ship formation ； Following module ship Final formation control law is: ； wherein are weight coefficients.

6. The finite-time fast co-avoidance control method for multi-module ship according to claim 4, characterized in that: The step 9 following module ship Currently Virtual control quantity at the moment Is: ； wherein , is a normal number, is a normal number and .

7. A computer device comprising a memory, a processor, and a computer program stored on the memory, wherein: the processor executes the computer program to implement the steps of the method of any one of claims 1 to 6.

8. A computer readable storage medium having stored thereon a computer program, characterized in that: the computer program, which when executed by the processor, implements the steps of the method of any one of claims 1 to 6.

9. A computer program product comprising computer instructions, characterized in that: the computer program, which when executed by the processor, implements the steps of the method of any one of claims 1 to 6. the computer program, which when executed by the processor, implements the steps of the method of any one of claims 1 to 6.

Images