Five-degree polynomial trajectory generation system and method based on head and tail ship state input

Abstract

The invention discloses a quintic polynomial trajectory generation system and method based on head and tail ship state input, and belongs to the technical field of ship automatic driving and formation control. The system constructs a quintic polynomial trajectory function with time as a variable by acquiring position, speed and acceleration information of a head ship and a tail ship of a formation in real time and combining an environment interference dynamic compensation mechanism, and generates a smooth trajectory of an intermediate ship. The influence of interference such as water flow and wind speed on the states of the head ship and the tail ship is corrected through a self-adaptive weight algorithm, polynomial coefficients meeting boundary conditions are calculated, and acceleration continuity and trajectory feasibility are ensured. According to the scheme, the problems of sudden acceleration change, poor environmental adaptability of a geometric formation control method and the like caused by traditional cubic polynomial interpolation are solved, and the navigation safety, the energy consumption efficiency and the real-time response capability of the ship formation are remarkably improved.

Description

Technical Field

[0001] The present invention relates to the technical field of ship autonomous driving and formation control, and particularly to a quintic polynomial trajectory generation system and method based on the state input of the head and tail ships, which are applicable to the cooperative navigation control of scenarios such as cargo ship queues and unmanned boat clusters. Background Art

[0002] In the formation navigation of ships, the trajectory planning and control of the middle ships are crucial. At present, there are many defects in the commonly used trajectory planning methods:

[0003] Cubic polynomial interpolation method: This method can only ensure the continuity of speed. During the ship's movement, the acceleration will experience sudden changes. The sudden change in acceleration will cause the ship to generate inertial oscillations, which not only affects the stability of the ship's navigation but also increases the fatigue loss of the ship's structure. At the same time, the cubic polynomial interpolation method does not consider the real-time influence of environmental factors such as ocean currents on the states of the head and tail ships. In a complex ocean environment, it is difficult to accurately plan the trajectory of the middle ships.

[0004] Geometric formation control method: This method relies on a preset formation template and lacks adaptability to complex and changeable environments. When encountering sudden disturbances such as strong winds and rapid currents, global replanning is required, which will cause significant computational delays. In actual navigation, this delay may cause the ship to be unable to respond to environmental changes in a timely manner, thereby affecting the safety and efficiency of formation navigation.

[0005] Patent document CN202411695738.4 discloses a path planning method for unmanned boat autonomous berthing, which is characterized by including the following steps: Step S1: Grid the feasible space, where each grid is used as a node. Step S2: Set the berthing position as the starting point and the current position as the ending point, and search for the first path through the hybrid A* method; Step S3: Reverse the first path to obtain the actual path; Step S4: Perform real-time collision detection on the actual path and adjust the actual path based on the results of the collision detection.

[0006] Patent document CN202411169990.1 discloses an unmanned ship autonomous berthing and unberthing method and system based on vision-laser fusion, which relates to the field of image processing technology. It includes obtaining an image of the dock area and performing preprocessing to obtain the dock edge features, solving the initial attitude angle of the hull relative to the dock, and optimizing the initial attitude angle to obtain the optimal hull attitude estimation result; obtaining the three-dimensional point cloud data of the dock area through a lidar and performing preprocessing to obtain the dock plane image, calculating the initial displacement of the hull relative to the dock based on the dock plane equation, and optimizing the initial displacement to obtain the optimal hull position estimation result; inputting the obtained results into a pre-trained vision-laser fusion model to obtain the optimal ship pose fusion estimation result, and according to the preset target dock position, using the pre-constructed ship berthing and unberthing path planning model to obtain the optimal berthing and unberthing path curve, and converting the path curve into an executable motion trajectory. However, there are challenges in the high-precision estimation of ship pose information and path planning in the above method, especially in complex dock environments and obstacle distributions, and it is difficult for the existing technology to achieve efficient and safe trajectory positioning.

[0007] With the development of the ship transportation industry, the requirements for the safety, efficiency, and environmental adaptability of ship formation navigation are getting higher and higher. The existing trajectory planning methods can no longer meet these needs. Therefore, it is urgent to develop a new trajectory generation system. Summary of the Invention

[0008] The purpose of the present invention is to provide a quintic polynomial trajectory generation system based on the input of the states of the leading and trailing ships to solve the problems of trajectory smoothness and real-time control of the middle ship in ship formation navigation, and at the same time improve the robustness of the system to environmental interference.

[0009] To solve the above problems, the present invention provides a quintic polynomial trajectory generation method based on the input of the states of the leading and trailing ships, which is characterized by including the following steps:

[0010] S1: Obtain the motion state information of the leading and trailing ships in the formation within the time interval, including the position x (t) , speed v (t) , acceleration a (t) ;

[0011] S2: Construct a quintic polynomial trajectory function for the middle ship, and the quintic polynomial is expressed as:

[0012] x(t) = a5t 5 +a4t 4 +a3t 3 +a2t 2 +a1t + a0

[0013] where a0 to a5 are coefficients to be solved;

[0014] S3: Collect environmental interference data through sensors, including water flow velocity, direction, and wind speed;

[0015] S4: Dynamically correct the state parameters of the leading ship and the trailing ship based on the environmental interference data to generate compensated boundary conditions;

[0016] S5: Solve the coefficients of the fifth-degree polynomial according to the compensated boundary conditions of the leading and trailing ships;

[0017] S6: Calculate the planned trajectory of the middle ship based on the coefficients of the fifth-degree polynomial and calculate the propulsion force requirement based on the ship dynamics model.

[0018] Further, in step S4, based on the environmental interference data, dynamically correcting the state parameters of the leading ship and the trailing ship to generate compensated boundary conditions includes: measuring the water flow velocity μ current , direction θ current and wind speed ν wind in real time through on-board sensors, and superimposing an environmental interference compensation term on the basis of the ship position data x (t) :

[0019] x compensated (t) = x(t) + Δx(t)

[0020]

[0021] where is the ship's course angle to ensure that the compensation direction is aligned with the direction of the water flow; k is the gain coefficient, k = α·μ current +β·ν wind , α and β are empirical coefficients, and λ is the attenuation factor;

[0022] Use the influence of environmental interference to correct the boundary conditions of the leading and trailing ships:

[0023]

[0024] where x0 and x1 are the original positions of the leading and trailing ships, and x′0 and x′1 are the corrected positions.

[0025] Further, the adjustment rule of the gain coefficient k includes that when the actual trajectory deviation e(t) of the ship exceeds the threshold, an incremental adjustment is triggered:

[0026] k new = k old +γ·e(t), where γ is the learning rate.

[0027] Further, it is characterized in that the lower limit of λ is:

[0028] m is the ship mass and D is the damping coefficient.

[0029] Further, the λ attenuation factor uses the gradient descent method with the energy consumption as the optimization objective J = ∫τ 2 dt, where τ is the torque, and updates λ:

[0030] η is the step size.

[0031] Further, in step S5, according to the compensated head and tail ship boundary conditions, the coefficients of the quintic polynomial are specifically solved as follows:

[0032] Construct a system of linear equations including the corrected boundary conditions:

[0033]

[0034] Solve the six - variable system of equations in real - time to obtain the polynomial coefficients a0 - a5; t0 is the time when the state of the head ship is (x0, v0, a0), which is a known quantity; t1 is the time when the state of the tail ship is (x1, v1, a1), t1 = t0+(n - 1)*Δt, n represents the number of ships in this formation, Δt represents the time interval between formation ships, and Δt is a known condition.

[0035] According to the method described in claim 1, characterized in that, in step S6, based on the coefficients of the quintic polynomial, the planned trajectory of the intermediate ship is calculated, and the propulsion force requirement is calculated based on the ship dynamics model specifically as follows: According to the obtained polynomial coefficients a0 - a5; calculate the planned trajectory of the intermediate ship According to the ship dynamics model Calculate the propulsion force requirement, and respectively represent the linear velocity and linear acceleration, and F is the propulsion force.

[0036] Further, α = 0.3 and β = 0.1.

[0037] Further, γ is 0.05; η is 0.01.

[0038] On the other hand, the present invention also provides a quintic polynomial trajectory generation system based on the input of the head and tail ship states, characterized in that it includes a state acquisition module, an environment compensation module, an interpolation calculation module, and a control instruction output module. The state acquisition module is used to obtain the motion state information of the formation head ship and tail ship and environmental interference data; the environment compensation module is used to introduce an environmental interference compensation term to dynamically correct the state parameters of the head ship and tail ship; the interpolation calculation module is used to construct the quintic polynomial trajectory function of the intermediate ship and solve the polynomial coefficients according to the corrected boundary conditions; the control instruction output module is used to calculate the planned trajectory of the intermediate ship based on the coefficients of the quintic polynomial and calculate the propulsion force requirement based on the ship dynamics model.

[0039] Technological Innovation: For the first time, this invention integrates environmental information into the process of correcting ship status information and innovatively applies the fifth-degree polynomial to ship formation navigation control, achieving multi-ship collaborative control with continuous acceleration. This innovation breaks through the limitations of traditional methods and effectively improves the stability and safety of ship formation navigation.

[0040] Application Value: The system of this invention can be widely applied to various scenarios such as unmanned boats and freight fleets. Through precise trajectory planning and optimized control strategies, it can significantly reduce fuel consumption, lower operating costs, and improve economic benefits.

[0041] Compatibility: The system supports seamless docking with existing marine systems such as AIS and radar, facilitating upgrades and transformations based on existing ship equipment, and has good prospects for popularization and application. Description of the Drawings

[0042] Figure 1 It is an architecture diagram of a fifth-degree polynomial trajectory generation system based on the input of the bow and stern ship status; Detailed Implementation Modes

[0043] The following combines the drawings and embodiments to further describe the detailed implementation modes of the present invention in detail. The following embodiments are used to illustrate the present invention, but are not used to limit the scope of the present invention.

[0044] It should be understood that when used in this specification and the appended claims, the term "comprising" indicates the presence of described features, wholes, steps, operations, elements, and / or components, but does not exclude the presence or addition of one or more other features, wholes, steps, operations, elements, components, and / or groups.

[0045] To make the drawings concise, only the parts related to the present invention are schematically shown in each drawing, and they do not represent their actual structures as products. Additionally, to make the drawings concise and easy to understand, in some drawings, components with the same structure or function are only schematically shown for one of them, or only one of them is marked. In this article, "one" not only means "only this one" but also can mean "more than one" situation.

[0046] It should also be further understood that the term "and / or" used in the specification and appended claims of this application refers to any combination and all possible combinations of one or more of the associated listed items, and includes these combinations.

[0047] In the embodiments shown in the drawings, the indication of directions (such as up, down, left, right, front and back) is used to explain that the structures and movements of various components of the present invention are not absolute but relative. When these components are in the positions shown in the drawings, these explanations are appropriate. If the descriptions of the positions of these components change, then the indications of these directions also change accordingly.

[0048] In addition, in the description of the present application, the terms "first", "second", etc. are only used for distinguishing descriptions and cannot be construed as indicating or implying relative importance.

[0049] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will describe the specific embodiments of the present invention with reference to the drawings.

[0050] Figure 1 is the architecture diagram of the quintic polynomial trajectory generation system based on the input of the bow and stern ship states in this embodiment; it can be seen that it includes:

[0051] State acquisition module:

[0052] Ship state data acquisition: Use on-board sensors to obtain the GPS / INS data of the bow ship and the stern ship, including position, speed, acceleration (position x0, x1, speed v0, v1, acceleration a0, a1) and the ship's heading angle. These data can accurately reflect the real-time motion states of the bow and stern ships and provide basic information for subsequent trajectory planning.

[0053] Environmental interference data acquisition: Use on-board sensors to measure the water flow velocity μ current and direction θ current , wind speed ν wind and other environmental interference data in real time. The water flow and wind speed have a great impact on ship navigation. Accurately obtaining these data helps to more precisely plan the trajectory of the middle ship.

[0054] Environmental compensation module:

[0055] Dynamic correction model: Superimpose an environmental interference compensation term on the basis of the ship state data. The design of the compensation term ensures that the compensation direction is aligned with the water flow action direction, and based on the proportional control of the water flow velocity and the wind speed: Based on the environmental interference data, dynamically correct the state parameters of the bow ship and the stern ship to generate the compensated boundary conditions, including measuring the water flow velocity μ current , direction θ current and wind speed ν wind in real time through on-board sensors, and superimposing an environmental interference compensation term on the basis of the ship position data x (t) :

[0056] x compensated (t) = x(t) + Δx(t)

[0057]

[0058] Among them, is the ship's course angle, ensuring that the compensation direction is aligned with the water flow direction; k is the gain coefficient, and its magnitude affects the intensity of compensation;

[0059] k = α·μ current +β·ν wind , where α and β are empirical coefficients, and λ is the attenuation factor; used to adjust the persistence of compensation, etc. α and β are preset coefficients, with default values of α = 0.3 and β = 0.1; through this proportional control method, the gain coefficient can be adjusted in real time according to the changes in water flow speed and wind speed to adapt to different environmental conditions.

[0060] Parameter dynamic adjustment strategy:

[0061] Gain coefficient:

[0062] Adaptive rule: When the actual trajectory deviation of the ship exceeds the threshold, an incremental adjustment of the gain coefficient is triggered. The specific adjustment method is that when the actual trajectory deviation e(t) of the ship exceeds the threshold, the incremental adjustment is triggered: k new = k old +γ·e(t), where γ is the learning rate, with a default value of 0.05; this adaptive adjustment mechanism enables the system to respond in a timely manner when the ship has a trajectory deviation, enhancing the robustness of the system.

[0063] Attenuation factor:

[0064] Ship inertia matching: Set the lower limit of the attenuation factor according to the ship's mass and damping coefficient. The lower limit of λ is:

[0065] where m is the ship's mass and D is the damping coefficient. This can ensure that the attenuation factor always maintains a reasonable value during the ship's movement, avoiding over-compensation or under-compensation.

[0066] Online optimization: Use the gradient descent method with energy consumption as the optimization objective to update the attenuation factor. The specific gradient descent update formula is that the λ attenuation factor uses the gradient descent method with the energy consumption J = ∫τ 2 dt as the optimization objective, τ is the torque, and λ is updated:

[0067] where η is the step size, with a default value of 0.01.

[0068] Through online optimization, it is possible to reduce the energy consumption of the ship while ensuring the accuracy of trajectory planning.

[0069] Interpolation calculation module:

[0070] Boundary condition remapping: Considering the influence of environmental disturbances, the boundary conditions of the leading and trailing ships are corrected. Correct the boundary conditions of the leading and trailing ships using the influence of environmental disturbances:

[0071]

[0072] Where x0 and x1 are the original positions of the leading and trailing ships, and x′0 and x′1 are the corrected positions, ensuring that the boundary conditions are more in line with the actual navigation situation.

[0073] Real-time calculation of polynomial coefficients: Construct a system of linear equations containing the corrected boundary conditions

[0074] Real-time solve the six-variable system of equations to obtain the polynomial coefficients a0 to a5; t0 is the time when the state of the leading ship is (x0, v0, a0), which is a known quantity; t1 is the time when the state of the trailing ship is (x1, v1, a1), and t1 = t0 + (n - 1) * Δt, where n represents the number of ships in the formation, and Δt represents the time interval between ships in the formation, and Δt is a known condition. These coefficients determine the trajectory function of the intermediate ships, thus achieving precise trajectory planning.

[0075] Control instruction output module:

[0076] According to the polynomial coefficients a0 to a5 obtained by the interpolation calculation module, calculate the planned trajectory of the intermediate ships According to the ship dynamics model Calculate the propulsion force requirement, and represent the linear velocity and linear acceleration respectively, and F is the propulsion force. The propulsion force requirement calculated by this model can ensure that the ship sails stably along the planned trajectory.

[0077] Example 1: Formation navigation in a narrow waterway

[0078] The leading ship and the trailing ship obtain the real-time positions (x0, y0), (x1, y1) through GPS / INS, with speeds v0 = 2.5 m / s, v1 = 2.3 m / s, and accelerations a0 = 0.1 m / s 2 , a1 = 0.08 m / s 2 .

[0079] The water flow velocity μ is measured by radar current = 0.5 m / s, and the direction θ current = 30°; θ current = 4 m / s.

[0080] Environmental compensation

[0081] Calculate the lateral compensation amount: Correct the position of the leading ship

[0082] Construct a system of fifth-degree polynomial equations to obtain polynomial coefficients a0 to a5; generate the intermediate ship trajectory:

[0083] x(t) = 0.2t 5 -1.5t 4 +3t 3 +2t 2 +10t + 50 (t ∈ [0, 10s]);

[0084] Calculate the propulsion force requirement of the ship according to the solution information of the polynomial.

[0085] Finally, it should be noted that: the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit them. Although the present invention has been described in detail with reference to the above embodiments, those of ordinary skill in the art should understand that: modifications or equivalent replacements can still be made to the specific embodiments of the present invention, and any modifications or equivalent replacements that do not depart from the spirit and scope of the present invention should be covered by the protection scope of the claims of the present invention.

Claims

1. A quintic polynomial trajectory generation method based on the input of the bow and stern ship states, characterized in that, It includes the following steps: S1: Obtain the motion state information of the leading ship and the trailing ship in the time interval, including the position x (t) , the speed v (t) , the acceleration a (t) ; S2: Construct a quintic polynomial trajectory function for the middle ship, and the quintic polynomial is expressed as: x(t) = a5t 5 + a4t 4 + a3t 3 + a2t 2 + a1t + a0 where, a0 to a5 are coefficients to be solved; S3: Collect environmental interference data through sensors, including water flow velocity, direction and wind speed; S4: Based on the environmental interference data, dynamically correct the state parameters of the leading ship and the trailing ship to generate compensated boundary conditions; S5: Solve the quintic polynomial coefficients according to the compensated boundary conditions of the leading and trailing ships; S6: Calculate the planned trajectory of the middle ship based on the quintic polynomial coefficients, and calculate the propulsion force requirement based on the ship dynamics model.

2. The method according to claim 1, wherein The step S4 dynamically corrects the state parameters of the leading ship and the trailing ship based on the environmental interference data, and generates the compensated boundary conditions, including measuring the water flow velocity μ current , direction θ current and wind speed ν wind , and superimposing an environmental interference compensation term on the basis of the ship position data x (t) : x compensated (t) = x(t) + Δx(t) Among them, is the ship's course angle, ensuring that the compensation direction is aligned with the water flow direction; k is the gain coefficient, k = α·μ current +β·ν wind , α and β are empirical coefficients, and λ is the attenuation factor; Use the influence of environmental interference to correct the boundary conditions of the leading and trailing ships: where, x0, x1 are the original positions of the leading and trailing ships, and x′0, x′1 are the corrected positions.

3. The method according to claim 2, wherein The adjustment rule of the gain coefficient k includes that when the actual trajectory deviation e(t) of the ship exceeds the threshold, an incremental adjustment is triggered: k new = k old + γ·e(t), where γ is the learning rate.

4. The method according to claim 2, characterized in that The lower limit of λ is: where m is the ship mass and D is the damping coefficient.

5. The method according to claim 2, wherein The λ decay factor uses the gradient descent method with the energy consumption as the optimization objective of J = ∫τ 2 dt, where τ is the torque, and update λ: η is the step size.

6. The method according to claim 1, wherein In step S5, specifically solving the quintic polynomial coefficients according to the compensated boundary conditions of the leading and trailing ships is: Construct a linear equation system including the corrected boundary conditions: Solve the six-variable equation system in real time to obtain the polynomial coefficients a0~a5; t0 is the time when the state of the leading ship is (x0, v0, a0), which is a known quantity; t1 is the time when the state of the trailing ship is (x1, v1, a1), and t1 = t0 + (n - 1)*Δt, where n represents the number of ships in this formation, and Δt represents the time interval between formation ships, and Δt is a known condition.

7. The method according to claim 1, characterized in that In step S6, the planned trajectory of the intermediate ship is calculated based on the coefficients of the fifth-degree polynomial, and the propulsion force requirement is calculated based on the ship dynamics model. Specifically: according to the obtained polynomial coefficients a0 to a5, the planned trajectory of the intermediate ship is calculated. According to the ship dynamics model calculate the propulsion force requirement, and represent the linear velocity and linear acceleration respectively, and F is the propulsion force.

8. The method according to claim 2, characterized in that, α = 0.3 and β = 0.

1.

9. The method according to claim 3 or 5, characterized in that, γ is 0.05; η is 0.

01.

10. A quintic polynomial trajectory generation system based on the input of the bow and stern ship states, characterized in that, It includes a state acquisition module, an environmental compensation module, an interpolation calculation module and a control instruction output module. The state acquisition module is used to obtain the motion state information of the leading and trailing ships in the formation and environmental interference data; the environmental compensation module is used to introduce an environmental interference compensation term to dynamically correct the state parameters of the leading and trailing ships; the interpolation calculation module is used to construct a quintic polynomial trajectory function for the middle ship and solve the polynomial coefficients according to the corrected boundary conditions; the control instruction output module is used to calculate the planned trajectory of the middle ship based on the quintic polynomial coefficients and calculate the propulsion force requirement based on the ship dynamics model.

Images