Path planning method for multi-tug autonomous formation berthing of target ship

Abstract

The invention discloses a path planning method for multi-tug autonomous formation berthing of a target ship, and the method comprises the steps: S1, constructing a single-ship berthing planning strategy for assisting tugs according to an issued preset berthing task; s2, a virtual guide ship is defined, the initial position coordinates of the virtual guide ship are obtained according to the ship positions of the multiple assisting tugs, and the virtual guide ship is guided to be berthed to the served ship based on a single ship berthing planning strategy; and a formation path planning strategy of the multiple assistant tugs is constructed based on the virtual guide ship, so that path planning of the multiple tugs for autonomous formation berthing of the target ship is realized. The problems that current port tugboat operation mainly depends on artificial experience operation, in the path planning facing the tugboat to berth a target ship, especially in the multi-assisting tugboat cooperative work process, the requirement for berthing a served ship at the preset angle, speed and position cannot be accurately and effectively met, and assisted tugboat cooperative formation operation cannot be achieved are solved. And the operation precision and efficiency are low.

Description

Technical Field

[0001] The present invention relates to the technical field of ship path planning and berthing control, and in particular to a path planning method for a multi-tugboat autonomous formation to berth a target ship. Background Art

[0002] As port throughput continues to rise, tugboats, as important auxiliary tools for port operations, shoulder great responsibilities and pressures. Large ships have poor maneuverability and cannot dock at ports on their own, so they usually need to apply for tugboat assistance. Tugboats need to provide escort, push, tow and other services according to the requirements of the target ship to control the target ship's posture and avoid damage to personnel, hulls, other ships, port equipment, etc. during berthing.

[0003] The advancement of science and technology has driven the shipbuilding industry to move rapidly towards intelligence, and unmanned boats have been widely used in a variety of application scenarios. Compared with traditional ships, maritime autonomous surface ships (MASS) have more significant advantages in safety, economy, convenience and environmental protection, and have broad research and application prospects. In addition, tugboats have unique advantages in the development process of intelligence, such as complete hydrological, meteorological and environmental information, strong port infrastructure support, efficient emergency response capabilities, and superior safety measures. These factors together have laid a solid foundation for the development of unmanned tugboats.

[0004] At present, the research on tugboats mainly focuses on two major directions: escort and assisted berthing. Among them, escort means that the tugboat follows the target ship at a preset position until the target ship enters the berth. Most scholars regard this research as a path tracking problem for the escort position; assisted berthing means that the tugboat performs operations such as steering, turning, turning around, and changing speed on the target ship, and assists the target ship in making decisions such as turning and changing speed to help it enter the berth more safely and efficiently. In this research direction, the tugboat and the target ship are usually described as a coupled system, and the assisted berthing goal is achieved through a collaborative control method. The future intelligent tugboat operation can cover three stages: escort-berthing-assisted operation. In this process, the tugboat first follows the target ship at the escort position, and then chooses an appropriate time to berth the target ship. After the cable is completed, it enters the auxiliary operation stage until the target ship safely enters the berth.

[0005] At present, the research on escort and auxiliary berthing operations has received widespread attention. However, there are relatively few studies on berthing behavior, and the current port tugboat operations mainly rely on manual experience. In the path planning for the tugboat to berth the target ship, especially in the collaborative work of multiple assisting tugboats, it is impossible to accurately and effectively meet the requirements of berthing the served ship at a preset angle, speed, and position to realize the coordinated formation operation of the assisting tugboats, resulting in low operation accuracy and efficiency. In view of the current economic growth trend, future ports will inevitably show a larger throughput, and the pressure on tugboat operations will also increase accordingly. Summary of the invention

[0006] The present invention provides a path planning method for a multi-tugboat autonomous fleet to berth a target ship, so as to overcome the above technical problems.

[0007] In order to achieve the above object, the technical solution of the present invention is:

[0008] A path planning method for a multi-tugboat autonomous fleet to berth a target ship specifically comprises the following steps:

[0009] S1: According to the preset berthing tasks issued, a single-vessel berthing planning strategy for assisting tugboats is established;

[0010] The specific steps include:

[0011] S11: Define and obtain berthing position control line based on the served ship;

[0012] S12: obtaining a direction vector of the assisting tugboat relative to the side angle of the served ship in a stationary state, and obtaining a current desired heading of the assisting tugboat according to the berthing position control line;

[0013] S13: taking the current desired heading as the relative motion heading, and acquiring the turning azimuth vector of the assisting tugboat according to the current speed of the assisting tugboat;

[0014] and obtaining the berthing guidance force of the served ship based on the steering azimuth vector;

[0015] S14: taking the direction of the berthing guidance force as the desired heading of the assisting tugboat at the current moment, and updating and obtaining the desired heading at the next moment based on the constructed steering constraint;

[0016] S15: Construct a speed gradient planning model for assisting tugboats;

[0017] Based on the speed gradient planning model, the ship position of the assisting tugboat at the next moment is obtained according to the expected course at the next moment;

[0018] S2: Obtain the positions, headings and speeds of multiple assisting tugboats;

[0019] Define a virtual guide ship, obtain the initial position coordinates of the virtual guide ship according to the positions of multiple assisting tugboats, and guide it to berth to the served ship based on the single-ship berthing planning strategy;

[0020] A formation path planning strategy for multiple assisting tugboats is constructed based on the virtual guide ship to realize the path planning of multiple tugboats autonomously forming a formation to berth the target ship.

[0021] Furthermore, the berthing position control line in S11 is the straight line where the resultant direction of the navigation speed vector of the served ship TS and the berthing speed vector is located;

[0022] And the vector expressions of the navigation speed vector and the berthing speed vector are

[0023]

[0024] Where: u ts Indicates the speed of TS; Indicates the heading of TS; u ap represents the berthing target speed of TS; θ ap Indicates the berthing target angle; v ts represents the navigation speed vector; v ap represents the berthing velocity vector.

[0025] Furthermore, the S12 specifically includes the following steps:

[0026] S120: Define the direction vector of the berthing position control line as v ls The direction vector of the assisting tugboat relative to the side angle of the served ship at rest is v to , and v ls Denoted as (x ls ,y ls ), v to Denoted as (x to ,y to );

[0027] S121: Get v to With v ls The vector angle θ between 1 , the formula for obtaining it is

[0028]

[0029] v ls ·v to =x ls x to +y ls y to (5)

[0030]

[0031]

[0032] Where: ||v to || represents v to The modulus length of ||v ls || represents v ls The module length;

[0033] S122: Obtain the rotation angle θ of the direction vector of the served ship TS relative to the side angle of the assisting tugboat OS according to step S121 r , whose expression is

[0034]

[0035] Where: LC represents the berthing position control line;

[0036] S123: According to the rotation angle θ r Get the rotation matrix R(θ r ), the expression is

[0037]

[0038] And according to the rotation matrix R(θ r ) obtain the current desired heading of the assisting tugboat;

[0039] And the expression of the current desired heading vector of the assisting tugboat is

[0040]

[0041] Where: v co Indicates the assisting tug's current desired heading.

[0042] Furthermore, the S13 specifically includes the following steps:

[0043] S131: Taking the current desired heading vector as the relative motion heading, the expression is:

[0044] v co =v to -v ts (11)

[0045] S132: According to the navigation speed vector v ts Position of the assisting tugboat os Get the center of the circle o to assist the tugboat's speed u os As the radius, define and obtain the relative motion vector circle O r ;

[0046] And the relative motion vector circle O r The formula for obtaining the center of the circle is

[0047] o=p os -v ts (12)

[0048] Assume that the position of the assisting tugboat is p os As v co The starting point of the relative motion vector circle O r Any point on v co The pending end point is determined, and v is confirmed according to the preset vector direction vector d co The end point;

[0049] Let the preset vector direction vector d = (a, b), where a and b represent the horizontal component and the vertical component of the vector direction vector d respectively, then the formula for obtaining it is:

[0050]

[0051] S133: Obtaining an unknown parameter equation of the ray in the direction of the current expected heading vector according to the preset vector direction vector;

[0052] And the unknown parameter equation of the direction ray P(n) of the current desired heading vector is

[0053] P(n)=P os +n·d=(x os +an,y os +bn) (14)

[0054] Where: n represents the parameter to be solved and n≥0;

[0055] S134: Define relative motion vector circle O r The standard equation is

[0056] (xh) 2 +(yk) 2 =r 2 (15)

[0057] Where: h, k represent the coordinates of the center of the relative motion vector circle; r represents the radius;

[0058] Substituting formula (14) into (15) we can obtain

[0059] (x os +an-h) 2 +(y os +bn-k) 2 =r 2 (16)

[0060] And rewrite (16) as a quadratic equation with respect to n as

[0061] (a 2 +b 2 ) 2 +2(a(x os -h)+b(y os -k))n+((x os -h) 2 +(y os -k) 2 -r 2 )=0(17)

[0062] Simplify (17) and obtain the simplified formula:

[0063] A=a 2 +b 2 (18)

[0064] B=2(a(x os -h)+b(y os -k))(19)

[0065] C=(x os -h) 2 +(y os -k) 2 -r 2 (20)

[0066] An 2 +Bn+C=0(21)

[0067] S135: Rooting the simplified formula using a quadratic equation to obtain a parameter solution;

[0068] The expression for finding the roots of the quadratic equation is

[0069]

[0070] Determine the final current desired heading vector direction ray based on the parameter solution;

[0071] S136: Based on the final current desired heading vector direction ray, the turning azimuth vector of the assisting tugboat is obtained according to the current speed of the assisting tugboat, and its expression is:

[0072] v co =P(n)-p os (twenty three)

[0073] v to =v co +v ts (twenty four)

[0074] Where: v to represents the steering direction vector;

[0075] S137: Acquire the berthing guidance force of the served ship based on the steering azimuth vector, and the berthing guidance force of the served ship is

[0076] F m =k m ·v to (25)

[0077] Where: F m represents the berthing guidance force; k m Represents the position guidance gravity gain coefficient.

[0078] Furthermore, the expression of the steering constraint constructed in S14 is

[0079]

[0080] Where: Indicates F m The side angle relative to the assisting tug OS, with positive values ​​for starboard and negative values ​​for port; θ c represents the steering constraint angle; Δθ represents the steering angle; represents the heading of the assisting tug OS at time t; represents the heading of the assisting tug OS at time t+1.

[0081] Furthermore, the S15 specifically includes the following steps:

[0082] S151: Construct a speed gradient planning model for the assisting tugboat, and the expression of the speed gradient planning model is:

[0083]

[0084] Where: u m Indicates the maximum speed; d (os,ts) Indicates the distance between the assisting tugboat and the served vessel; d c Indicates the distance threshold; u t Indicates the desired heading; represents the speed of the assisting tugboat OS at time t; u c Indicates the speed change threshold; Δu indicates the speed change value; represents the speed of the assisting tug OS at time t+1;

[0085] S152: Based on the speed gradient planning model, the position of the assisting tugboat at the next moment is obtained according to the expected heading at the next moment. Its expression is:

[0086]

[0087] Where: represents the position of the assisting tug OS at time t; represents the position of the assisting tug OS at time t+1.

[0088] Further, the S2 specifically includes the following steps:

[0089] S21: Obtain the positions, headings and speeds of multiple assisting tugboats;

[0090] A virtual guide ship is defined, and the initial position coordinates of the virtual guide ship are obtained according to the positions of the multiple assisting tugboats;

[0091] And the formula for obtaining the initial position coordinates of the virtual guide ship is:

[0092]

[0093] Where: x i ,y i represents the position coordinates of each assisting tugboat; p c represents the coordinate centers of multiple assisting tugboats, k sd represents the guidance coefficient; p v Indicates the initial position coordinates of the virtual guide ship;

[0094] Based on the single-ship berthing planning strategy, it guides the ship to berth to the served ship and obtains the virtual berthing point;

[0095] S22: Construct a formation path planning strategy for multiple assisting tugboats based on a virtual guide ship to achieve path planning for multiple tugboats to autonomously form a formation to berth a target ship;

[0096] The formation path planning strategy of the plurality of assisting tugboats specifically includes the following steps:

[0097] S221: Construct formation planning principles to achieve coordinated formation of multiple assisting tugboats;

[0098] The formation planning principle is as follows:

[0099] A berthing reference line including multiple berthing points is defined by a virtual berthing point, and each berthing point is coded in sequence from near to far in the direction from the bow to the stern of the served ship to obtain a berthing point sequence list;

[0100] The berthing reference line is a straight line passing through the virtual berthing point and parallel to the side of the served vessel;

[0101] S222: Acquire the real-time positions of the plurality of assisting tugboats to obtain the relative distance of each assisting tugboat relative to the berthing reference line;

[0102] And arrange the relative distances in descending order to obtain a real-time distance sequence table;

[0103] Allocate berthing point locations to each assisting tugboat according to the real-time distance sequence table;

[0104] The rules for allocating berthing points are as follows:

[0105] Based on the berthing point position order from near to far in the berthing point sequence table, berthing positions, i.e., berthing points, are allocated to the tugboat sorting results in the real-time distance sequence table in turn;

[0106] S223: After obtaining the assigned position, obtaining the direction vector of each assisting tugboat relative to the corresponding berthing point to obtain the position guidance gravity of the assisting tugboat;

[0107] And the expression of position-oriented gravity is

[0108] F p =k p ·v p (34)

[0109] Where: k p represents the position-oriented gain coefficient; v p F represents the direction vector of the assisting tugboat relative to the corresponding berthing point; p Indicates position-oriented gravity;

[0110] Based on the position guidance gravity, the dynamic guidance force of the formation is obtained according to the position distance between each assisting tugboat and the corresponding berthing point, and the expression of the dynamic guidance force of the formation is:

[0111]

[0112] Where: F mp represents the dynamic guiding force of the formation; represents the distance between the assisting tugboat and the corresponding berthing point, i represents the number of the assisting tugboat, j represents the number of the berthing point, k represents the distance between the assisting tugboat and the corresponding berthing point, and dm represents the dynamic guidance coefficient of the formation guidance force; k dp It represents the dynamic guidance coefficient of the formation position guidance force;

[0113] S224: constructing a dynamic gain function of the repulsive force between any two assisting tugboats in the assisting tugboat formation;

[0114] And the expression of the repulsive force dynamic gain function is

[0115]

[0116] vec r =vec 1 -vec 2 (38)

[0117] Where: vec rrepresents the relative motion velocity vector between any two assisting tugboats; θ represents the velocity vector between any two assisting tugboats and vec r The angle of Indicates the distance between two ships; d s Indicates the safe distance boundary; The vector representing the direction from one assisting tugboat to another assisting tugboat; F s represents the safety guiding force; k v Represents the relative motion repulsion factor; vec 1 ,vec 2 represents the velocity vectors of the two assisting tugboats;

[0118] S225: According to the formation dynamic guidance force and the repulsive force dynamic gain function, the formation berthing guidance force is obtained; and the expression of the formation berthing guidance force is:

[0119] F t =k mp ·F mp +k s ·F s (39)

[0120] Where: k mp represents the dynamic guidance gain coefficient; k s Indicates the safety guidance gain factor; F ι represents the formation planning force, i.e., the formation berthing guidance force; v i represents the OS of the i-th assisting tugboat i The velocity vector of

[0121] Based on the formation berthing guidance force, according to formula (26) we can get

[0122]

[0123] And according to formula (40) combined with formula (27), the position update of the assisting tugboat formation is realized;

[0124] S226: Obtain the distance between the expected position of each assisting tugboat and the current position of the assisting tugboat in the assisting tugboat formation, so as to obtain the distance d between each assisting tugboat and the served ship. h , whose expression is

[0125]

[0126] Where: d(p i ,p os ) represents the distance between the expected position of each assisting tugboat and the current position of the assisting tugboat; p i represents the i-th assisting tugboat; represents the side angle of the i-th assisting tugboat;

[0127] And rewriting formula (28) according to formula (41) yields

[0128]

[0129] According to formula (42) combined with formulas (29) to (30), the speed update of the assisting tugboat formation is realized, thereby realizing the path planning of the multi-tugboat autonomous formation berthing the target ship.

[0130] Beneficial effect: The present invention provides a path planning method for a multi-tugboat autonomous formation berthing a target ship, by constructing a single-ship berthing planning strategy for an assisting tugboat, and according to the issued preset berthing task, the path planning task of the single-ship berthing is realized, by defining a virtual guide ship, and obtaining the initial position coordinates of the virtual guide ship according to the ship positions of multiple assisting tugboats, and guiding it to berth to the served ship based on the single-ship berthing planning strategy, and by introducing the formation dynamic guiding force and repulsive force dynamic gain function, a formation path planning strategy for multiple assisting tugboats is constructed to realize the path planning of the multi-tugboat autonomous formation berthing a target ship, which solves the problem that the current port tugboat operation mainly relies on manual experience operation, and in the path planning for the tugboat berthing target ship, especially in the collaborative work process of multiple assisting tugboats, it is impossible to accurately and effectively meet the preset angle, speed, and position to berth the served ship to realize the assisting tugboat collaborative formation operation, resulting in low operation accuracy and efficiency, which greatly reduces the operation pressure and difficulty of the tugboat, reduces the manpower input, and improves the intelligence level and operation accuracy of the path planning of the multi-tugboat autonomous formation berthing the target ship. BRIEF DESCRIPTION OF THE DRAWINGS

[0131] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0132] Figure 1 It is a flow chart of a path planning method for a multi-tugboat autonomous fleet to berth a target ship according to the present invention;

[0133] Figure 2 This is a schematic diagram of the definition of the berthing position control line in this embodiment;

[0134] Figure 3 This is a schematic diagram of the berthing task guidance method in this embodiment;

[0135] Figure 4 This is a schematic diagram of the formation task planning in this embodiment;

[0136] Figure 5Schematic diagram of formation planning in this embodiment;

[0137] Figure 6 Schematic diagram of formation safety planning in this embodiment;

[0138] Figure 7 The schematic diagram of the target contact point calculation of the OS in this embodiment;

[0139] Figure 8 This is a system architecture diagram of the path planning method for a multi-tugboat autonomous fleet berthing a target ship in this embodiment;

[0140] Fig. 9 This is a simulation diagram of a single ship berthing simulation experiment in this embodiment;

[0141] Fig.10 This is a simulation diagram of the formation berthing simulation experiment in this embodiment. DETAILED DESCRIPTION

[0142] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0143] This embodiment provides a path planning method for a multi-tugboat autonomous fleet to berth a target ship, such as Figure 1 and Figure 8 As shown, the specific steps include:

[0144] S1: According to the preset berthing task issued, a single-vessel berthing planning strategy for assisting tugboats is constructed, which specifically includes the following steps:

[0145] This embodiment is aimed at path planning for tugboats to berth target ships, and assists tugboats in berthing the served ships at preset angles, speeds, and positions. Aiming at the three-objective coupling planning problem, this embodiment proposes the concept of berthing position control line LC, and designs a guidance method, namely, single-ship berthing planning strategy, based on the berthing position control line LC and the ship motion characteristics.

[0146] S11: Define and obtain berthing position control line based on the served ship;

[0147] Specifically, Figure 2 As shown, the berthing position control line in S11 is the straight line where the resultant direction of the navigation speed vector and the berthing speed vector of the served ship TS is located;

[0148] And the vector expressions of the navigation speed vector and the berthing speed vector are

[0149]

[0150] Where: u ts Indicates the speed of TS; Indicates the heading of TS; u ap represents the berthing target speed of TS; θ ap Indicates the berthing target angle. In this embodiment, the assisting tugboat OS is assumed to berth from the side of the served ship TS. If it berths from the starboard side, θ is subtracted. ap , otherwise add θ ap ;v ts represents the navigation speed vector; v ap represents the berthing velocity vector;

[0151] like Figure 2 As shown, the berthing position control line LC is composed of v ts and -v ap In conclusion, if the assisting tugboat OS is just above the LC, by adjusting the course and speed of the assisting tugboat OS to accurately match the target values, the assisting tugboat OS can be stably kept on the LC until berthing the served ship TS.

[0152] S12: obtaining a direction vector of the assisting tugboat relative to the side angle of the served ship in a stationary state, and obtaining a current desired heading of the assisting tugboat according to the berthing position control line;

[0153] like Figure 3 The figure shows how to control the ship position on LC and adjust the navigation state at the same time to achieve the purpose of accurate berthing. In this embodiment, although the ship position is controlled on the berthing position control line LC and the navigation state is adjusted at the same time, accurate berthing can be achieved, but how to achieve this goal has become another problem. Usually, the berthing operation of the tugboat is carried out at a low speed. If the ship position of the OS is controlled on LC too early, it will be at u ap Slowly approaching TS will reduce the operation efficiency; the most ideal state is that OS contacts TS at the same time as it reaches LC, and the heading and speed are also the target values; based on the ship's motion characteristics, the ship cannot make a large angle turn in a short time, and the heading that needs to be adjusted from the starting state to the berthing state should be gradually achieved during the entire voyage. Without considering the sway, the bow direction is the same as the track direction, so the above-mentioned heading problem needs to be solved by the following methods:

[0154] The specific steps include:

[0155] S120: Assuming that TS is stationary, the optimal track line of OS at the end of berthing should be tangent to LC, and the direction vector of the berthing position control line is defined as v ls The direction vector of the assisting tugboat relative to the side angle of the served ship at rest is v to , and v ls Denoted as (x ls ,y ls ), v to Denoted as (x to ,y to ), where x ls ,y ls Respectively represent the vector direction vector v ls The horizontal vector and the vertical vector, x to ,y to Respectively represent the vector direction vector v to The horizontal vector and the vertical vector of ;

[0156] S121: Get v to With v ls The vector angle θ between 1 , the formula for obtaining it is

[0157]

[0158] v ls ·v to =x ls x to +y ls y to (5)

[0159]

[0160] Where: ||v to || represents v to The modulus length of ||v ls || represents v ls The modulus length; v ls ·v to Indicates v to With v ls The dot product of

[0161] S122: Obtain the rotation angle θ of the direction vector of the served ship TS relative to the side angle of the assisting tugboat OS according to step S121 r , whose expression is

[0162]

[0163] Where: LC represents the berthing position control line;

[0164] S123: According to the rotation angle θr Get the rotation matrix R(θ r ), the expression is

[0165]

[0166] And according to the rotation matrix R(θ r ) obtain the current desired heading of the assisting tugboat;

[0167] And the expression of the current desired heading vector of the assisting tugboat is

[0168]

[0169] Where: v co Indicates the assisting tug's current desired course;

[0170] S13: If the served ship TS has a speed, in order to ensure the effectiveness and applicability of the design, the current expected heading is used as the relative motion heading, and the steering azimuth vector of the assisting tugboat is obtained according to the current speed of the assisting tugboat, and the berthing guidance force of the served ship is obtained based on the steering azimuth vector;

[0171] The specific steps include:

[0172] S131: Taking the current desired heading vector as the relative motion heading, the expression is:

[0173] v co =v to -v ts (11)

[0174] S132: According to the navigation speed vector v ts Position of the assisting tugboat os Get the center of the circle o to assist the tugboat's speed u os As the radius, define and obtain the relative motion vector circle O r ;

[0175] And the relative motion vector circle O r The formula for obtaining the center of the circle is

[0176] o=p os -v ts (12)

[0177] Assume that the position of the assisting tugboat is p os As v co The starting point of the relative motion vector circle O r Any point on v co The pending end point is determined, and v is confirmed according to the preset vector direction vector d co The end point;

[0178] Let the preset vector direction vector d = (a, b), where a and b represent the horizontal component and the vertical component of the vector direction vector d respectively, then the formula for obtaining it is:

[0179]

[0180] S133: Obtaining an unknown parameter equation of the ray in the direction of the current expected heading vector according to the preset vector direction vector;

[0181] And the unknown parameter equation of the direction ray P(n) of the current desired heading vector is

[0182] P(n)=P os +n·d=(x os +an,y os +bn) (14)

[0183] Where: n represents the parameter to be solved and n≥0;

[0184] S134: Define relative motion vector circle O r The standard equation is

[0185] (xh) 2 +(yk) 2 =r 2 (15)

[0186] Where: h, k represent the coordinates of the center of the relative motion vector circle; r represents the radius;

[0187] Substituting formula (14) into (15) we can obtain

[0188] (x os +an-h) 2 +(y os +bn-k) 2 =r 2 (16)

[0189] And rewrite (16) as a quadratic equation with respect to n as

[0190] (a 2 +b 2 ) 2 +2(a(x os -h)+b(y os -k))n+((x os -h) 2 +(y os -k) 2 -r 2 )=0 (17)

[0191] Simplify (17) and obtain the simplified formula:

[0192] A=a 2 +b 2 (18)

[0193] B=2(a(x os -h)+b(y os -k)) (19)

[0194] C=(x os -h) 2 +(y os -k) 2 -r 2 (20)

[0195] An 2 +Bn+C=0 (21)

[0196] S135: Rooting the simplified formula using a quadratic equation to obtain a parameter solution;

[0197] The expression for finding the roots of the quadratic equation is

[0198]

[0199] Determine the final current desired heading vector direction ray based on the parameter solution;

[0200] In this embodiment, v co As the relative motion heading is shown in formula (11), it is necessary to obtain the direction that the OS should turn to according to the current speed state of the OS to ensure that v co It can meet the berthing guidance requirements. In the previous design, this embodiment only constrains v co The direction of the relative motion vector circle O is defined by the vector summation result. r , place OS's ship position p os Follow the vector -v ts Translate to get the center of the circle o, O r The radius r is the speed u of OS os ; If you set p os v co The starting point of O r Every point on the co The end point of v is obtained by using the preset vector direction in this embodiment. co The end point, in simple terms, is through O r and v co The direction of the ray can determine v co The module, in v co If the modulus and direction of are determined, v can be obtained to, which will serve as the basis for the ship to turn when performing the berthing task. The specific formula derivation is shown in formulas (12)-(22). In addition, formula (22) is the n corresponding to the intersection of the straight line and the circle. If n≥0 is satisfied, it is the n corresponding to the intersection of the ray and the circle. Before solving n, this embodiment needs to use the discriminant formula D=B 2 -4AC determines whether there is an intersection. If D>0, there are two real solutions, that is, the line and the circle have two intersections; if D=0, there is one real solution, that is, the line and the circle are tangent; if D<0, there is no real solution, that is, the line and the circle have no intersection. After obtaining n, the solution of n<0 can be eliminated. If n has two solutions, take the larger value, then P(n)=(x os +an,y os +bn) is the intersection of the ray and the circle. At this time, we can get v to and the corresponding berthing guidance force F m As shown in equations (23) and (24), v to It will be used as the output of the mission planning layer to guide the mission in the subsequent heading and speed planning;

[0201] S136: Based on the final current desired heading vector direction ray, the turning azimuth vector of the assisting tugboat is obtained according to the current speed of the assisting tugboat, and its expression is:

[0202] v co =P(n)-p os (twenty three)

[0203] v to =v co +v ts (twenty four)

[0204] Where: v to represents the steering direction vector;

[0205] S137: Acquire the berthing guidance force of the served ship based on the steering azimuth vector, and the berthing guidance force of the served ship is

[0206] F m =k m ·v to (25)

[0207] Where: F m represents the berthing guidance force; k m represents the position-oriented gravity gain coefficient;

[0208] S14: taking the direction of the berthing guidance force as the desired heading of the assisting tugboat at the current moment, and updating and obtaining the desired heading at the next moment based on the constructed steering constraint;

[0209] The expression of the steering constraint constructed is

[0210]

[0211] Where: Indicates F m The side angle relative to the assisting tug OS, with positive values ​​for starboard and negative values ​​for port; θ c represents the steering constraint angle; Δθ represents the steering angle; represents the heading of the assisting tug OS at time t; represents the heading of the assisting tug OS at time t+1;

[0212] In this embodiment, F m Afterwards, the direction is taken as the desired heading of the OS. The OS will adjust to the desired heading at every moment. In addition, this embodiment sets a steering constraint, that is, the steering amplitude of the OS at every moment shall not exceed a certain limit value. The calculation formulas are shown in (26) and (27);

[0213] S15: Construct a speed gradient planning model for assisting tugboats;

[0214] Based on the speed gradient planning model, the ship position of the assisting tugboat at the next moment is obtained according to the expected course at the next moment;

[0215] The specific steps include:

[0216] S151: Construct a speed gradient planning model for the assisting tugboat, and the expression of the speed gradient planning model is:

[0217]

[0218]

[0219] Where: u m Indicates the maximum speed; d (os,ts) Indicates the distance between the assisting tugboat and the served vessel; d c Indicates the distance threshold; u t Indicates the desired heading; represents the speed of the assisting tugboat OS at time t; u c Indicates the speed change threshold; Δu indicates the speed change value; represents the speed of the assisting tug OS at time t+1;

[0220] S152: Based on the speed gradient planning model, the position of the assisting tugboat at the next moment is obtained according to the expected heading at the next moment. Its expression is:

[0221]

[0222] Where: represents the position of the assisting tug OS at time t; represents the position of the assisting tug OS at time t+1;

[0223] In terms of speed planning, this embodiment designs a speed gradient planning model for assisting the tugboat. The model first calculates the distance between the two ships. If the distance is less than the preset distance threshold, gradient planning is performed between the maximum speed and the target speed to determine the expected speed. After obtaining the expected speed, the OS will make a speed change decision based on the speed. However, the speed change amplitude is also subject to certain constraints. The calculation formulas are shown in (28) to (30). After updating the heading speed, the ship position at a new moment can be calculated on this basis. The calculation formula is shown in (31).

[0224] S2: Obtain the positions, headings and speeds of multiple assisting tugboats;

[0225] Define a virtual guide ship, obtain the initial position coordinates of the virtual guide ship according to the positions of multiple assisting tugboats, and guide it to berth to the served ship based on the single-ship berthing planning strategy;

[0226] A formation path planning strategy for multiple assisting tugboats is constructed based on the virtual guide ship to realize the path planning of multiple tugboats autonomously forming a formation to berth the target ship.

[0227] The specific steps include:

[0228] S21: Obtain the positions, headings and speeds of multiple assisting tugboats;

[0229] A virtual guide ship is defined, and the initial position coordinates of the virtual guide ship are obtained according to the positions of the multiple assisting tugboats;

[0230] And the formula for obtaining the initial position coordinates of the virtual guide ship is:

[0231]

[0232] Where: x i ,y i represents the position coordinates of each assisting tugboat; p c represents the coordinate centers of multiple assisting tugboats, k sd represents the guidance coefficient; p v Indicates the initial position coordinates of the virtual guide ship;

[0233] Based on the single-ship berthing planning strategy, it guides the ship to berth to the served ship and obtains the virtual berthing point;

[0234] The core idea of ​​fleet planning in this embodiment is to regard all berthed ships as a unified whole, then plan their tasks, and guide each ship based on the planning results; through this method, it is possible to ensure that the fleet maintains a high degree of coordination and consistency when performing tasks. This embodiment defines a virtual guide ship OS v , whose initial coordinates are p v Located at the mean of the coordinates of each ship and p ts Between, and OS v The initial ship position is set at p c With p ts , so that it can effectively guide the formation group in the initial stage, and the calculation is shown in equations (32) to (33); for example, Figure 4 The four assisting tugboats need to berth at berthing points 1-4 respectively, but there is no need to constrain the fixed order. This embodiment is based on the single-ship berthing planning strategy. v The state of task planning is performed, that is, OS is obtained through task planning v After the task orientation, use formula (25) to obtain F m And boot the OS v berthing;

[0235] S22: Construct a formation path planning strategy for multiple assisting tugboats based on a virtual guide ship to achieve path planning for multiple tugboats to autonomously form a formation to berth a target ship;

[0236] The formation path planning strategy of the plurality of assisting tugboats specifically includes the following steps:

[0237] S221: Construct formation planning principles to achieve coordinated formation of multiple assisting tugboats;

[0238] The formation planning principle is as follows:

[0239] A berthing reference line including multiple berthing points is defined by a virtual berthing point, and each berthing point is coded in sequence from near to far in the direction from the bow to the stern of the served ship to obtain a berthing point sequence list;

[0240] The berthing reference line is a straight line passing through the virtual berthing point and parallel to the side of the served vessel;

[0241] S222: Acquire the real-time positions of the plurality of assisting tugboats to obtain the relative distance of each assisting tugboat relative to the berthing reference line;

[0242] And arrange the relative distances in descending order to obtain a real-time distance sequence table;

[0243] Allocate berthing point locations to each assisting tugboat according to the real-time distance sequence table;

[0244] The rules for allocating berthing points are as follows:

[0245] Based on the order of berthing point positions from near to far in the berthing point sequence table, berthing positions, i.e. berthing points, are allocated to the tugboat sorting results in the real-time distance sequence table in turn.

[0246] In a further example, the allocation rule for the position allocation of the berthing point is:

[0247] Tugboats ranked higher in the real-time distance sequence list (e.g., the one farthest from the berthing reference line) are preferentially assigned to the position closest to the bow of the served vessel in the berthing point sequence list;

[0248] In contrast, the tugboats ranked later in the real-time distance sequence list (those closest to the berthing baseline) are sequentially assigned to positions farther from the bow of the served ship in the berthing point sequence list. This allocation method ensures the efficiency and safety of the berthing process, enables each assisting tugboat to be reasonably distributed based on its distance from the berthing baseline, and optimizes the entire berthing operation process.

[0249] like Figure 5 As shown in the figure, in the study of the coordinated berthing of a large ship by multiple tugboats in this embodiment, the berthing of an individual tugboat in advance may adversely interfere with the task execution of the subsequent tugboats; therefore, it is very important to ensure the synchronization of the berthing of the tugboats, and ideally all tugboats should be berthed at the same time as much as possible; in order to meet this requirement, the group formation must be flexibly adjusted during the formation navigation. In the mission scenario studied in this embodiment, all tugboats need to berth on the same side of the target large ship TS, and the group formation should be based on OS v The four tugboats are arranged at the center according to the preset positions 1-4, while ensuring that the formation is parallel to the bow direction of the TS. In order to enhance the dynamic adaptability of the method of this embodiment, the four tugboats do not need to be strictly distributed at fixed positions in sequence. On the contrary, their arrangement order can be flexibly adjusted according to the real-time position distribution of each tugboat, and the overall formation presents an OS v The tugboats are arranged in a straight line based on the TS heading. On this basis, the embodiment pre-establishes a corresponding coordinate system according to the heading of the TS. By sorting the longitudinal values ​​of the coordinates of each tugboat, the specific positions in the formation can be more reasonably allocated. This dynamic adjustment rule not only improves the overall coordination of the formation, but also significantly enhances the adaptability of the system in complex environments.

[0250] S223: After obtaining the assigned position, obtaining the direction vector of each assisting tugboat relative to the corresponding berthing point to obtain the position guidance gravity of the assisting tugboat;

[0251] And the expression of position-oriented gravity is

[0252] F p =k p ·vp (34)

[0253] Where: k p represents the position-oriented gain coefficient; v p F represents the direction vector of the assisting tugboat relative to the corresponding berthing point; p Indicates position-oriented gravity;

[0254] Based on the position guidance gravity, the dynamic guidance force of the formation is obtained according to the position distance between each assisting tugboat and the corresponding berthing point, and the expression of the dynamic guidance force of the formation is:

[0255]

[0256] Where: F mp represents the dynamic guiding force of the formation; represents the distance between the assisting tugboat and the corresponding berthing point, i represents the number of the assisting tugboat, j represents the number of the berthing point, k represents the distance between the assisting tugboat and the corresponding berthing point, and dm represents the dynamic guidance coefficient of the formation guidance force; k dp It represents the dynamic guidance coefficient of the formation position guidance force;

[0257] After obtaining the assigned position, this embodiment can obtain the desired position guidance force, as shown in formula (34); in order to ensure the stability of the formation, when each ship approaches the target position, it is necessary to dynamically adjust its heading to the expected direction under the task guidance. This process should be dynamically corrected according to the real-time distance between each ship and the target position to meet the dual needs of the formation task and formation maintenance, as shown in formula (35). Through the above method, the formation can flexibly respond in a complex dynamic environment and achieve stable and efficient formation navigation;

[0258] S224: constructing a dynamic gain function of the repulsive force between any two assisting tugboats in the assisting tugboat formation;

[0259] And the expression of the repulsive force dynamic gain function is

[0260]

[0261] vec r =vec 1 -vec 2 (38)

[0262] Where: vec r represents the relative motion velocity vector between any two assisting tugboats; θ represents the velocity vector between any two assisting tugboats and vec r The angle of Indicates the distance between two ships; d s Indicates the safe distance boundary; The vector representing the direction from one assisting tugboat to another assisting tugboat; F s represents the safety guiding force; k v Represents the relative motion repulsion factor; vec 1 ,vec 2 represents the velocity vectors of the two assisting tugboats; in addition to the relative motion, the collision avoidance repulsion dynamic gain function of this embodiment also needs to consider the distance between the two ships, weakening the influence of the distance between the two ships and designing a safe distance boundary. If the distance between the ships is less than the safe distance boundary, a very large repulsion constant factor k will be generated. v , to avoid ships being too close together;

[0263] like Figure 6 As shown, this embodiment designs a collision avoidance repulsion dynamic gain function, and by introducing the traditional APF framework, improves the stability, safety, and coordination of the close-range collision avoidance decisions of each assisting tugboat in the formation; in the process of multi-ship formation collaborative operation, it is very important to ensure internal safety; due to factors such as task planning and formation planning, each ship needs to adjust its position frequently. In this process, if the coordination is not proper, it is very likely to cause collisions between ships within the formation, thereby affecting the smooth progress of the task and the overall stability of the formation. Therefore, each ship must take effective measures to ensure that it maintains an appropriate safety distance from other ships in the formation to minimize the risk of collision and ensure the safe operation of the formation;

[0264] In formation navigation, due to the high density of ships, small margin for collision avoidance, and ships usually traveling in the same direction with a small spacing, the traditional collision avoidance principle of "early, big, wide, and clear" is no longer applicable. Therefore, the collision avoidance algorithm needs to pay more attention to the analysis of the relative motion state, reduce the weight of the ship spacing, and achieve safe collision avoidance in a high-density, same-direction navigation environment through fine dynamic adjustment and path planning, to ensure the overall stability and efficient operation of the formation; in the field of ship collision avoidance algorithm research, APF is widely used due to its strong real-time performance; however, the classic APF often oscillates and is unstable when dealing with close-range collision avoidance, which may affect the collision avoidance effect and navigation safety. In order to overcome this problem, this embodiment is based on the classic APF framework and innovatively designs a collision avoidance repulsion dynamic gain function, which incorporates factors such as ship spacing and relative motion state into the decision-making process, aiming to improve the safety and coordination of formation navigation;

[0265] S225: According to the formation dynamic guidance force and the repulsive force dynamic gain function, the formation berthing guidance force is obtained; and the formation berthing guidance force F t The expression is

[0266] F t =k mp ·F mp +k s ·Fs (39)

[0267] Where: k mp represents the dynamic guidance gain coefficient; k s Indicates the safety guidance gain factor; F t represents the formation planning force, i.e., the formation berthing guidance force; v i represents the OS of the i-th assisting tugboat i The velocity vector of

[0268] Based on the formation berthing guidance force, according to formula (26) we can get

[0269]

[0270] And according to formula (40) combined with formula (27), the position update of the assisting tugboat formation is realized;

[0271] S226: Obtain the distance between the expected position of each assisting tugboat and the current position of the assisting tugboat in the assisting tugboat formation, so as to obtain the distance d between each assisting tugboat and the served ship. h , whose expression is

[0272]

[0273] Where: d(p i ,p os ) represents the distance between the expected position of each assisting tugboat and the current position of the assisting tugboat; p i represents the i-th assisting tugboat; represents the side angle of the i-th assisting tugboat;

[0274] And rewriting formula (28) according to formula (41) yields

[0275]

[0276] According to formula (42) combined with formulas (29) to (30), the speed update of the assisting tugboat formation is realized, thereby realizing the path planning of the multi-tugboat autonomous formation berthing the target ship.

[0277] The course planning of each assisting tugboat in the formation in this embodiment is similar to the course planning of a single ship. It is necessary to replace F in formula (26) with m Replaced with the resultant force obtained from the formation planning layer, i.e. F t , the specific calculation formula is shown in equation (39) and equation (40); through heading planning, each ship can move towards the desired position in the formation planning. However, this method cannot ensure that the ship remains at the desired position for a long time, and it is easy to cross the desired position, resulting in poor formation stability. In this regard, this embodiment innovatively designs a speed planning method for formation stability, namely OSv The speed of the tugboat is still calculated according to equations (28) to (30). The speed planning of the remaining ships needs to include the heading, position and expected position into the calculation formula to avoid the i-th assisting tugboat OS i Beyond its expected position, the specific formulas for the expected speed planning of each assisting tugboat are as follows: (41) to (42), and the speed change and position update formulas of the assisting tugboat are the same as those for the single ship attitude planning.

[0278] This embodiment also includes the principle of actually solving the contact points of the OS based on the berthing operation:

[0279] like Figure 7 As shown, during the berthing experiment, the ship has volume and cannot be regarded as a mass point. In this embodiment, the length of the OS ship is 38m, the width of the ship is 10m; the bow is a semicircular arc with a diameter of 10m, and the center of the circle is p h ;p os is the OS coordinate, located in the center of the hull, and can be obtained by formula (43): h ; In this embodiment, the contact point of OS is obtained by solving the relative motion state of the two ships; θ s is the heading angle between the two ships; θ c is θ s The complementary angle of c Then we can get P t As shown in formula (44), according to the current motion state of the two ships, the future contact point of OS is P t , at this time θ s It is not a preset berthing angle. s Replace with the preset value, the required P t It is the target contact point of OS. When planning tasks and formations, P t As coordinates instead of p os ;

[0280]

[0281] Where: Indicates the OS heading, and Figure 7 OS wants to berth to the starboard side of TS, so it needs to subtract θ c , otherwise add θ c .

[0282] This embodiment also includes simulation experiment results: Fig. 9The figure shows a single-ship berthing simulation experiment diagram, which plots the trajectories of the two ships and the historical positions. In this experiment, the OS is located on the port side of the TS, and is planned to dock at point 5 of the TS at an absolute speed of 2m / s and a relative angle of 10°. The TS should maintain direction and speed as planned. During the experiment, the TS speed was maintained at 1.5m / s. In order to verify the robustness of the proposed planning algorithm in wind and wave environments, it randomly turned 0.5 degrees per second. It can be seen that the TS has a slight turn throughout the entire navigation procedure, which is in line with the actual berthing operation environment. From the partial enlarged view of the berthing moment, it can be seen that the berthing effect is excellent. The high accuracy of single-ship berthing provides support for the accuracy of subsequent formation berthing.

[0283] like Fig.10 The figure shows the simulation diagram of the formation berthing experiment, with TS, OS1-4, OS v Tracks and historical positions of OS v It is a virtual guide ship, guided by the single-ship berthing algorithm to berth at point 9 of TS. 1-4 Affected by OS v It can be seen that the four ships achieved coordinated and orderly berthing, and the entire process strictly followed the safety principles. In the partial enlarged picture, it can be clearly seen that OS v It docked precisely at point 9, while the remaining ships automatically adjusted their berthing order according to real-time conditions and docked at points 1-4 in turn.

[0284] In summary, the present embodiment constructs a single-ship berthing planning strategy for assisting tugboats, and realizes the path planning task of single-ship berthing according to the preset berthing task issued, defines a virtual guide ship, obtains the initial position coordinates of the virtual guide ship according to the ship positions of multiple assisting tugboats, and guides it to berth to the served ship based on the single-ship berthing planning strategy, and constructs a formation path planning strategy for multiple assisting tugboats by introducing the formation dynamic guide force and repulsion dynamic gain function to realize the path planning of multiple tugboats autonomously forming a formation to berth a target ship, which solves the problem that the current port tugboat operation mainly relies on manual experience operation, and in the path planning for the tugboat berthing target ship, especially in the collaborative work process of multiple assisting tugboats, it is impossible to accurately and effectively meet the problem of berthing the served ship at a preset angle, speed, and position to realize the assisting tugboat collaborative formation operation, resulting in low operation accuracy and efficiency, which greatly reduces the operation pressure and difficulty of the tugboat, reduces manpower investment, and improves the intelligence level and operation accuracy of the path planning of multiple tugboats autonomously forming a formation to berth a target ship.

[0285] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A path planning method for a multi-tugboat autonomous fleet to berth a target ship, characterized in that: The specific steps include: S1: According to the preset berthing tasks issued, a single-vessel berthing planning strategy for assisting tugboats is established; The specific steps include: S11: Define and obtain berthing position control lines based on the served vessel; S12: obtaining a direction vector of the assisting tugboat relative to the side angle of the served ship in a stationary state, and obtaining a current desired heading of the assisting tugboat according to the berthing position control line; S13: taking the current desired heading as the relative motion heading, and acquiring the turning azimuth vector of the assisting tugboat according to the current speed of the assisting tugboat; and obtaining the berthing guidance force of the served ship based on the steering azimuth vector; S14: taking the direction of the berthing guidance force as the desired heading of the assisting tugboat at the current moment, and updating and obtaining the desired heading at the next moment based on the constructed steering constraint; S15: Construct a speed gradient planning model for assisting tugboats; Based on the speed gradient planning model, the ship position of the assisting tugboat at the next moment is obtained according to the expected course at the next moment; S2: Obtain the positions, headings and speeds of multiple assisting tugboats; Define a virtual guide ship, obtain the initial position coordinates of the virtual guide ship according to the positions of multiple assisting tugboats, and guide it to berth to the served ship based on the single-ship berthing planning strategy; A formation path planning strategy for multiple assisting tugboats is constructed based on the virtual guide ship to realize the path planning of multiple tugboats autonomously forming a formation to berth the target ship.

2. A path planning method for a multi-tugboat autonomous fleet berthing a target ship according to claim 1, characterized in that: The berthing position control line in S11 is the straight line where the resultant direction of the navigation speed vector and the berthing speed vector of the served ship TS is located; And the vector expressions of the navigation speed vector and the berthing speed vector are Where: u ts Indicates the speed of TS; Indicates the heading of TS; u ap represents the berthing target speed of TS; θ ap Indicates the berthing target angle; v ts represents the navigation speed vector; v ap represents the berthing velocity vector.

3. A path planning method for a multi-tugboat autonomous fleet berthing a target ship according to claim 2, characterized in that: The S12 specifically includes the following steps: S120: Define the direction vector of the berthing position control line as v ls The direction vector of the assisting tugboat relative to the side angle of the served ship at rest is v to , and v ls Denoted as (x ls ,y ls ), v to Denoted as (x to ,y to ); S121: Get v to With v ls The vector angle θ1 between them is obtained by: v ls ·v to =x ls x to +y ls and to (5) Where: ||v to || represents v to The modulus length of ||v ls || represents v ls The module length; S122: Obtain the rotation angle θ of the direction vector of the served ship TS relative to the side angle of the assisting tugboat OS according to step S121 r , whose expression is Where: LC represents the berthing position control line; S123: According to the rotation angle θ r Get the rotation matrix R(θ r ), the expression is And according to the rotation matrix R(θ r ) obtain the current desired heading of the assisting tugboat; And the expression of the current desired heading vector of the assisting tugboat is Where: v co Indicates the assisting tug's current desired heading.

4. A path planning method for a multi-tugboat autonomous fleet berthing a target ship according to claim 3, characterized in that: The S13 specifically includes the following steps: S131: Taking the current desired heading vector as the relative motion heading, the expression of which is v co =v to -v ts (11) S132: According to the navigation speed vector v ts Position of the assisting tugboat os Get the center of the circle o to assist the tugboat's speed u os As the radius, define and obtain the relative motion vector circle O r ; And the relative motion vector circle O r The formula for obtaining the center of the circle is o=p os -v ts (12) Assume that the position of the tugboat that will assist is p os As v co The starting point of the relative motion vector circle O r Any point on v co The pending end point is determined, and v is confirmed according to the preset vector direction vector d co The end point; Let the preset vector direction vector d = (a, b), where a and b represent the horizontal component and the vertical component of the vector direction vector d respectively, then the formula for obtaining it is: S133: Obtaining an unknown parameter equation of the ray in the direction of the current expected heading vector according to the preset vector direction vector; And the unknown parameter equation of the direction ray P(n) of the current desired heading vector is P(n)=P os +n·d=(x os +an,y os +bn) (14) Where: n represents the parameter to be solved and n≥0; S134: Define relative motion vector circle O r The standard equation is (x-h) 2 +(y-k) 2 =r 2 (15) Where: h, k represent the coordinates of the center of the relative motion vector circle; r represents the radius; Substituting formula (14) into (15) we can obtain (x os +an-h) 2 +(y os +bn-k) 2 =r 2 (16) And rewrite (16) as a quadratic equation with respect to n as (a 2 +b 2 )n 2 +2(a(x os -h)+b(y os -k))n+((x os -h) 2 +(y os -k) 2 -r 2 )=0 (17) Simplify (17) and obtain the simplified formula: A=a 2 +b 2 (18) B=2(a(x os -h)+b(y os -k)) (19) C=(x os -h) 2 +(y os -k) 2 -r 2 (20) An 2 +Bn+C=0 (21) S135: Finding the roots of the quadratic equation for the simplified formula to obtain a parameter solution; The expression for finding the roots of the quadratic equation is Determine the final current desired heading vector direction ray based on the parameter solution; S136: Based on the final current desired heading vector direction ray, the turning azimuth vector of the assisting tugboat is obtained according to the current speed of the assisting tugboat, and its expression is v co =P(n)-p os (twenty three) v to =v co +v ts (24) Where: v to represents the steering direction vector; S137: Acquire the berthing guidance force of the served ship based on the steering azimuth vector, and the berthing guidance force of the served ship is F m =k m ·v to (25) Where: F m represents the berthing guidance force; k m Represents the position guidance gravity gain coefficient.

5. A path planning method for a multi-tugboat autonomous fleet berthing a target ship according to claim 4, characterized in that: The expression of the steering constraint constructed in S14 is Where: Indicates F m The side angle relative to the assisting tug OS, with positive values ​​for starboard and negative values ​​for port; θ c represents the steering constraint angle; Δθ represents the steering angle; represents the heading of the assisting tug OS at time t; represents the heading of the assisting tug OS at time t+1.

6. A path planning method for a multi-tugboat autonomous fleet berthing a target ship according to claim 5, characterized in that: The S15 specifically includes the following steps S151: Construct a speed gradient planning model for the assisting tugboat, and the expression of the speed gradient planning model is: Where: u m Indicates the maximum speed; d (os,ts) Indicates the distance between the assisting tugboat and the served vessel; d c Indicates the distance threshold; u t Indicates the desired heading; represents the speed of the assisting tugboat OS at time t; u c Indicates the speed change threshold; Δu represents the speed change value; represents the speed of the assisting tug OS at time t+1; S152: Based on the speed gradient planning model, the position of the assisting tugboat at the next moment is obtained according to the expected heading at the next moment. Its expression is: Where: represents the position of the assisting tug OS at time t; represents the position of the assisting tug OS at time t+1.

7. A path planning method for a multi-tugboat autonomous fleet berthing a target ship according to claim 6, characterized in that: The S2 specifically includes the following steps S21: Obtain the positions, headings and speeds of multiple assisting tugboats; A virtual guide ship is defined, and the initial position coordinates of the virtual guide ship are obtained according to the positions of the multiple assisting tugboats; And the formula for obtaining the initial position coordinates of the virtual guide ship is: Where: x i ,y i represents the position coordinates of each assisting tugboat; p c represents the coordinate centers of multiple assisting tugboats, k sd represents the guidance coefficient; p v Indicates the initial position coordinates of the virtual guide ship; Based on the single-ship berthing planning strategy, it guides the ship to berth to the served ship and obtains the virtual berthing point; S22: Construct a formation path planning strategy for multiple assisting tugboats based on a virtual guide ship to achieve path planning for multiple tugboats to autonomously form a formation to berth a target ship; The formation path planning strategy of the plurality of assisting tugboats specifically includes the following steps: S221: Construct formation planning principles to achieve coordinated formation of multiple assisting tugboats; The formation planning principle is as follows: A berthing reference line including multiple berthing points is defined by a virtual berthing point, and each berthing point is coded in sequence from near to far in the direction from the bow to the stern of the served ship to obtain a berthing point sequence list; The berthing reference line is a straight line passing through the virtual berthing point and parallel to the side of the served vessel; S222: Acquire the real-time positions of the plurality of assisting tugboats to obtain the relative distance of each assisting tugboat relative to the berthing reference line; And arrange the relative distances in descending order to obtain a real-time distance sequence table; Allocate berthing point locations to each assisting tugboat according to the real-time distance sequence table; The rules for allocating berthing points are as follows: Based on the berthing point position order from near to far in the berthing point sequence table, berthing positions, i.e., berthing points, are allocated to the tugboat sorting results in the real-time distance sequence table in turn; S223: After obtaining the assigned position, obtaining the direction vector of each assisting tugboat relative to the corresponding berthing point to obtain the position guidance gravity of the assisting tugboat; And the expression of position-oriented gravity is F p =k p ·v p (34) Where: k p represents the position-oriented gain coefficient; v p F represents the direction vector of the assisting tugboat relative to the corresponding berthing point; p Indicates position-oriented gravity; Based on the position guidance gravity, the dynamic guidance force of the formation is obtained according to the position distance between each assisting tugboat and the corresponding berthing point, and the expression of the dynamic guidance force of the formation is: Where: F mp represents the dynamic guiding force of the formation; represents the distance between the assisting tugboat and the corresponding berthing point, i represents the number of the assisting tugboat, j represents the number of the berthing point, k represents the distance between the assisting tugboat and the corresponding berthing point, and dm represents the dynamic guidance coefficient of the formation guidance force; k dp It represents the dynamic guidance coefficient of the formation position guidance force; S224: constructing a dynamic gain function of the repulsive force between any two assisting tugboats in the assisting tugboat formation; And the expression of the repulsive force dynamic gain function is thing r =vec1-vec2(38) Where: vec r represents the relative motion velocity vector between any two assisting tugboats; θ represents the velocity vector between any two assisting tugboats and vec r The angle of Indicates the distance between two ships; d s Indicates the safe distance boundary; The vector representing the direction from one assisting tugboat to another assisting tugboat; F s represents the safety guiding force; k v represents the relative motion repulsion factor; vec1, vec2 represent the velocity vectors of the two assisting tugboats; S225: Obtaining a formation berthing guidance force according to a formation dynamic guidance force and a repulsive force dynamic gain function; And the expression of the formation berthing guidance force is F ι =k mp ·F mp +k s ·F s (39) Where: k mp represents the dynamic guidance gain coefficient; k s Indicates the safety guidance gain factor; F ι represents the formation planning force, i.e., the formation berthing guidance force; v i represents the OS of the i-th assisting tugboat i The velocity vector of Based on the formation berthing guidance force, according to formula (26) we can get And according to formula (40) combined with formula (27), the position update of the assisting tugboat formation is realized; S226: Obtain the distance between the expected position of each assisting tugboat and the current position of the assisting tugboat in the assisting tugboat formation, so as to obtain the distance d between each assisting tugboat and the served ship. h , whose expression is Where: d(p i ,p os ) represents the distance between the expected position of each assisting tugboat and the current position of the assisting tugboat; p i represents the i-th assisting tugboat; represents the side angle of the i-th assisting tugboat; And rewriting formula (28) according to formula (41) yields According to formula (42) combined with formulas (29) to (30), the speed update of the assisting tugboat formation is realized, thereby realizing the path planning of the multi-tugboat autonomous formation berthing the target ship.

Images