Water surface ship occupation control method considering initial terminal constraint

Abstract

The invention relates to a water surface ship occupation control method considering initial terminal constraint, which comprises the following steps of: 1, setting a pilot ship T to be immovable, enabling a following ship M to do relative motion on the pilot ship T, and obtaining a relative course line of the following ship M occupying array position maneuvering according to a connecting line of an initial array position point and a new array position point of the following ship M; based on the relative course line, establishing a following ship M speed equation and an array position equation; and 2, defining the trajectory of the following ship M as a first arc-tangent segment-second arc form, and calculating the trajectory considering the head and tail terminal constraint by adopting a cyclic search method from the zero moment T = T0. According to the method, the relative position relation between the following ship M and the pilot ship T can be reasonably and effectively expressed, and finally the feasible track and occupation time of the pilot array position of the pilot ship T occupied by the following ship M are obtained.

Description

Technical Field

[0001] The present invention belongs to the field of surface ship control, and particularly relates to a surface ship occupancy control strategy considering initial and terminal constraints. Background Art

[0002] The multi-surface ship navigation technology has been applied to operation tasks such as chart surveying and hydrographic exploration. Compared with a single surface ship, multi-surface ship navigation control has certain advantages in improving operation capabilities, enhancing work efficiency, and ensuring the overall robustness and fault tolerance of the system. The multi-surface ship cooperative motion control method is the key to realizing the cooperative operation of surface ships. In response to the need for each surface ship to effectively respond due to the complex and changeable task requirements, it is necessary to form and maintain the ability of surface ships to form and maintain a preset fixed formation and sail normally along a specified route under the action of a suitable control strategy.

[0003] There has been much research on multi-surface ship control technology at home and abroad. The commonly used formation control methods can be divided into the leader-follower method, virtual structure method, behavior-based method, graph-based method, etc. Among them, the leader-follower method means that one or more individuals in the formation structure are selected as the leader ship T to generate the reference trajectory of the entire formation, and the remaining individuals are used as the follower ships M to follow the leader ship T according to the ideal distance and (or) angle maintained with the leader ship T, occupy the positions designated by the leader ship T, and thus form a specific formation. When the motion state of the leader ship T changes, the follower ship M needs to adjust its navigation trajectory in a timely manner. During the transformation process of the formation, it is necessary to achieve a smooth switch between multi-unmanned ship formations, that is, to smoothly switch from one formation to another. In previous studies, less consideration has been given to the constraint conditions of the course adjustment due to the maneuverability of the unmanned surface ship itself, which will affect the actual trajectory during the position transformation process. Summary of the Invention

[0004] In view of the above technical problems, the present invention proposes a surface ship occupancy control method considering initial and terminal constraints.

[0005] The above object of the present invention is achieved by the following technical solutions:

[0006] A surface ship occupancy control method considering initial and terminal constraints includes the following steps:

[0007] Step 1: Set the leader ship T to be stationary, and the follower ship M makes relative motion with respect to the leader ship T. The relative course line for the follower ship M to occupy the position maneuver is obtained from the line connecting the initial position point and the new position point of the follower ship M; based on this relative course line, a speed equation and a position equation of the follower ship M are established;

[0008] Step 2: Define the trajectory line of the following ship M as the form of the first circular arc - tangent line segment - the second circular arc. Starting from the zero time T = T0, use the method of cyclic search to calculate the trajectory line considering the head and tail terminal constraints.

[0009] Moreover, in Step 1, the velocity equation and the position equation are established as follows:

[0010]

[0011]

[0012] where, V m is the motion velocity vector of the following ship M, C m is the course angle of the following ship M, V t is the motion velocity vector of the leading ship T, C t is the course angle of the leading ship T, C rm is the course angle of the following ship M relative to the leading ship T, B0 is the angle between the initial motion direction of the leading ship T and the line connecting the initial positions of the following ship M and the leading ship T, D0 is the distance between the leading ship T and the following ship M at the initial position, V rm is the navigation speed of the following ship M relative to the leading ship T, Δt is the navigation time, B1 is the angle between the leading ship T and the following ship M at the target position, D1 is the distance between the leading ship T and the following ship M at the target position;

[0013] In the position equation, B0, D0, B1, and D1 are known, and we can obtain:

[0014]

[0015] Substitute the obtained C rm into the velocity equation. V m 、C t 、V t are known, so we can solve for C m and Δt respectively as:

[0016]

[0017] or

[0018]

[0019] Moreover, in Step 2, the trajectory length of the first circular arc is represented by L1, the trajectory length of the tangent line segment is represented by L2, and the trajectory length of the second circular arc is represented by L3; then the steps to calculate the trajectory line considering the head and tail terminal constraints are as follows:

[0020] 2.1. Calculate the position, course angle, and turning center position of the end of the follower ship M based on the initial position of the leading ship T, the spacing distance, azimuth, and end course angle of the follower ship M, as well as the turning radius of the follower ship M. At the same time, calculate the sailing trajectory length L3 of the second arc;

[0021] 2.2. Calculate the position, course angle, and turning center position of the bow of the follower ship M based on the initial formation position, initial course angle, and turning radius of the follower ship M;

[0022] 2.3. Traverse different θ1 and θ2, and calculate L1, L2, and L3, where θ1 is the turning angle of the follower ship M at the initial end, and θ2 is the turning angle of the follower ship M at the terminal end;

[0023] 2.4. Determine whether L3 / Vt is equal to If so, find the feasible solution, record θ1 and θ2, and draw the trajectory of the follower ship M; if not, T = T0 + 1, use the current position of the leading ship T as the initial position, and loop to execute steps 2.1 - 2.4.

[0024] Advantages and positive effects of the present invention:

[0025] This patent addresses the formation control problem of leader - follower unmanned surface vessels, considering the factors of the initial course angle and end course angle of the follower, and studies the influence of the turning characteristics of the follower ship M on the trajectory. By constructing a formation equation and iteratively calculating the trajectory considering the turning characteristics of the follower ship M, the simulation results show that this method can reasonably and effectively express the relative position relationship between the follower ship M and the leading ship T, and finally obtain the feasible trajectory and occupancy time for the follower ship M to occupy the leading formation position of the leading ship T. Description of the Drawings

[0026] Figure 1 is a schematic diagram of the coordinate system;

[0027] Figure 2 is an intuitive diagram of the follower ship M occupying the formation position of the leading ship T at a constant speed;

[0028] Figure 3 is a position relationship diagram of the follower ship M occupying the leading ship T;

[0029] Figure 4 is an intuitive diagram of the follower ship M occupying the formation position at a constant speed considering the head - tail end constraints;

[0030] Figure 5 is a variable relationship diagram of the follower ship M occupying the formation position at a constant speed considering the head - tail end constraints;

[0031] Figure 6 is a trajectory comparison diagram formed by the embodiments of the present invention. Detailed Embodiment

[0032] The structure of the present invention will be further described below in conjunction with the accompanying drawings and through embodiments. It should be noted that this embodiment is narrative rather than restrictive.

[0033] The coordinate system adopted by the present invention is as Figure 1 shown, the ship-fixed coordinate system o-xyz and the earth coordinate system O-XYZ. Among them, xyz represents the x-axis, y-axis, and z-axis in the ship-fixed coordinate system, and XYZ represents the X-axis, Y-axis, and Z-axis in the earth coordinate system.

[0034] The intuitive diagram of the following ship M occupying the position of the leading ship T at a constant speed is as Figure 2 shown. It is assumed that the leading ship T sails at a course C t and a speed V t . The following ship M is located at point M0 with the initial positions B0 and D0. Now, it goes to occupy the new positions B1 and D1 at a speed Vm. Among them, B0 is the angle between the initial movement direction of the leading ship T and the line connecting the initial positions of the following ship M and the leading ship T, and D0 is the initial distance in the corresponding state; B1 is the angle between the leading ship T and the following ship M at the target position, and D1 is the distance in the corresponding state. V t is the motion velocity vector of the leading ship T, and V m is the motion velocity vector of the following ship M. C t is the course angle of the leading ship T, and C m is the course angle of the following ship M.

[0035] According to the principle of relative motion, it is assumed that ship T is stationary and ship M makes relative motion to ship T, as Figure 3 shown. Then, as long as the relative course line adopted by ship M passes through the new position point to be occupied, after a certain period of time, ship M can occupy the new position. Ship M can occupy the new position point by taking an approach-to-meeting maneuver to the new position point. Thus, it can be determined that the line connecting the initial position point and the new position point of ship M is the relative course line for occupying the position maneuver.

[0036] Establish a velocity equation, as shown in equation (1); establish a position equation, as shown in equation (2):

[0037]

[0038]

[0039] Among them, V m is the motion velocity vector of the following ship M, C m is the course angle of the following ship M, V t is the motion velocity vector of the leading ship T, C t is the course angle of the leading ship T, and C rmLet θ be the course angle of the follower ship M relative to the leading ship T, B0 be the angle between the initial movement direction of the leading ship T and the line connecting the initial positions of the leading ship T and the follower ship M, D0 be the distance between the leading ship T and the follower ship M at the initial position, and V rm be the navigation speed of the follower ship M relative to the leading ship T, Δt be the navigation time, B1 be the angle between the leading ship T and the follower ship M at the target position, and D1 be the distance between the leading ship T and the follower ship M at the target position.

[0040] Solve the position equation to find C rm and V rm ·Δt.

[0041] In the position equation, B0, D0, B1, and D1 are known, and C

[0042]

[0043] Solve the speed equation to find C m and Δt.

[0044] Substitute the obtained C rm into the speed equation. Since V m , C t , and V t are known, C m and Δt can be solved respectively as follows:

[0045]

[0046] Or

[0047]

[0048] Among them, Δt generally takes a smaller value as the time required to occupy the position, and the corresponding C m is used as the course to occupy the position.

[0049] Since the bow direction of ship M may be any initial direction and end direction, therefore, the intuitive diagram of constant-speed position occupation considering the bow and stern direction constraints is as shown in Figure 4 Figure.

[0050] Define the trajectory line of the follower ship M as the form of the first arc - tangent segment - second arc. Starting from the zero moment T = T0, adopt the method of cyclic search, and calculate the trajectory line considering the bow and stern terminal constraints through the following steps:

[0051] Step 1: According to the initial position of the leading ship T, the position occupation distance, azimuth, end course angle of the follower ship M, and the turning radius of the follower ship M, calculate the position, course angle, and turning center position of the end of the follower ship M, and at the same time calculate the navigation trajectory length L3;

[0052] Step 2: Calculate the position of the bow of the following ship M, the course angle, and the position of the turning center according to the initial position, the initial course angle, and the turning radius of the following ship M.

[0053] Step 3: Traverse different θ1 and θ2, and calculate L1, L2, and L3, where θ1 is the turning angle of the following ship M at the initial end, and θ2 is the turning angle of the following ship M at the terminal end.

[0054] Step 4: Determine whether L3 / Vt is equal to If so, find the feasible solution, record θ1 and θ2, and draw the trajectory of the following ship M; if not, T = T0 + 1, use the current position of the leading ship T as the initial position, and enter Step 1.

[0055] This patent verifies the effectiveness of the algorithm through a typical example. Select unmanned surface vessels with the same speed and turning radius as the research object, and study the feasibility of the track planned by the algorithm proposed in the patent under the initial position, the initial course angle of the following ship M, and the terminal course angle.

[0056] Example:

[0057] The speed of the following ship M is 13.0 knots, the initial course of the following ship M is 90 degrees, the speed of the leading ship T is 8.3 knots, the course of the leading ship T is 0 degrees, the initial position is 90 degrees, the initial distance is 25 meters. In the target state, the position of the following ship M and the leading ship T is 62 degrees, the leading distance is 15.4 meters, the turning radius is 9m, and the final course of the following ship M is the same as that of the leading ship T, which is 90 degrees. The simulation results are shown in Figure 6 . In the figure, the red solid line represents the trajectory of the leading ship T, and the green solid line represents the trajectory of the following ship M. It can be seen from the figure that the trajectory of the following ship M considering the bow and stern terminal constraints consists of an arc + a line segment + an arc, with a length of about 46.74m and a sailing duration of 7s, while the trajectory of the following ship M without considering the bow and stern terminal constraints is a line segment with a length of 27m and a sailing duration of 4s. The trajectories of the following ship M and the leading ship T considering the bow and stern terminal constraints are very different from those without considering the bow and stern terminal constraints. The trajectory considering the terminal constraints is more in line with the actual turning process of the unmanned boat, and the final course can be adjusted to be the same as that of the leading ship T, and the time for constant-speed station-keeping can be predicted more accurately.

[0058] Although the embodiments and drawings of the present invention are disclosed for illustrative purposes, those skilled in the art can understand that various substitutions, changes, and modifications are possible without departing from the spirit of the present invention and the appended claims. Therefore, the scope of the present invention is not limited to the content disclosed in the embodiments and drawings.

Claims

1. A water surface ship occupancy control method considering initial and terminal constraints, characterized in that: It includes the following steps: Step 1: Set the leading ship T stationary, and let the following ship M make relative motion with respect to the leading ship T. The relative course line for the following ship M to occupy the formation position maneuver is obtained from the line connecting the initial formation position point and the new formation position point of the following ship M; based on this relative course line, establish the speed equation and formation position equation of the following ship M. Step 2: Define the trajectory line of the following ship M in the form of a first circular arc - tangent segment - second circular arc. Starting from the zero moment T = T0, use the cyclic search method to calculate the trajectory line considering the head and tail terminal constraints.

2. The method for controlling the occupancy of a surface ship considering initial and terminal constraints according to claim 1, characterized in that: In Step 1, the establishment of the speed equation and formation position equation is as follows: Among them, V m is the motion velocity vector of the following ship M, C m is the course angle of the following ship M, V t is the motion velocity vector of the leading ship T, C t is the course angle of the leading ship T, C rm is the course angle of the following ship M relative to the leading ship T, B0 is the angle between the initial motion direction of the leading ship T and the line connecting the initial positions of the following ship M and the leading ship T, D0 is the distance between the leading ship T and the following ship M at the initial position, V rm is the navigation speed of the following ship M relative to the leading ship T, Δt is the navigation time, B1 is the angle between the leading ship T and the following ship M at the target position, D1 is the distance between the leading ship T and the following ship M at the target position; In the formation position equation, B0, D0, B1, and D1 are known, and we can obtain: Substitute the obtained C rm into the velocity equation, V m , C t , V t are known, so C m and Δt can be solved respectively as follows:

3. The method for controlling the occupancy of a surface ship considering the initial terminal constraint according to claim 1, wherein: In Step 2, the trajectory length of the first circular arc is represented by L1, the trajectory length of the tangent segment is represented by L2, and the trajectory length of the second circular arc is represented by L3; then the steps to calculate the trajectory line considering the head and tail terminal constraints are as follows: 2.

1. According to the initial position of the leading ship T, the formation position distance, azimuth, terminal course angle of the following ship M, and the turning radius of the following ship M, calculate the position, course angle, and turning center position of the terminal of the following ship M, and at the same time calculate the navigation trajectory length L3 of the second circular arc. 2.

2. According to the initial formation position and initial course angle of the following ship M and the turning radius of the following ship M, calculate the position, course angle, and turning center position of the head end of the following ship M. 2.

3. Traverse different θ1 and θ2, and calculate L1, L2, and L3, where, θ1 is the turning angle of the following ship M at the initial end, and θ2 is the turning angle of the following ship M at the terminal end. 2.

4. Determine whether L3 / Vt is equal to If so, find a feasible solution, record θ1 and θ2, and plot the trajectory of the following ship M; if not, T = T0 + 1, use the current position of the leading ship T as the initial position, and loop through steps 2.1 - 2.4.

Images