A method for trajectory planning of unmanned surface vessels under complex ocean current conditions

Abstract

This invention provides a trajectory planning method for unmanned surface vessels (USVs) under complex ocean current conditions, belonging to the field of unmanned control. The method includes: acquiring the USV's initial state information, final state information, size information, and kinematic parameters, as well as a multi-objective cost function; acquiring map information and ocean current information between the USV's starting and ending points; constructing a dynamic model of the USV considering ocean currents to obtain state constraints; constructing a coarse trajectory for the USV to obtain an initial solution vector and a navigation tunnel constraint; constructing boundary constraints based on the initial and final state information; constructing control input constraints based on the size information and kinematic parameters; and obtaining the optimal control sequence to complete the trajectory planning of the USV. This invention employs a two-layer trajectory planning algorithm based on numerical optimization and a trajectory planning method considering ocean currents to ensure the safe and efficient operation of the USV.

Description

Technical Field

[0001] This invention belongs to the field of unmanned control, specifically relating to a method for trajectory planning of unmanned surface vessels under complex ocean current conditions. Background Technology

[0002] With the continuous development of intelligent control theory and sensing equipment, and the constant updating of unmanned control technology, the application of unmanned surface vessels (USVs) in both civilian and military fields is increasing. Autonomous navigation is crucial for the successful completion of their missions. However, USVs are large and have poor maneuverability. When navigating waters around ports with rugged coastlines, they are further affected by ocean currents. The movement of fluids has a significant impact on ship motion. If the influence of ocean currents is not considered in trajectory planning, the ship may fail to follow the predetermined trajectory in real-world scenarios, posing a significant safety hazard.

[0003] Currently, trajectory planning for unmanned surface vessels (USVs) has received widespread attention and research. Existing USV trajectory algorithms have the following shortcomings: 1) Current USV trajectory planning methods mostly employ ant colony optimization, potential field methods, and search methods, neglecting the physical dynamics of the USV, resulting in difficult-to-track trajectories; 2) Although some methods consider the dynamics of the USV, they fail to account for the influence of ocean currents on its motion. However, ocean currents have a significant impact on USV motion during actual operation. Ignoring ocean currents during planning could potentially lead to safety issues. 3) Compared to path planning in open waters, environments with numerous reefs and islands, and waters around ports, obviously require more assistance from unmanned surface vessels (USVs). However, existing methods achieve collision avoidance by inflating obstacles, resulting in a significant loss of solution space and hindering USV navigation in narrow waters. 4) The trajectory planning process aims for high computational speed and high trajectory quality. However, some methods use a spatiotemporal separation approach to improve solution speed at the expense of trajectory quality; some methods use a spatiotemporal joint approach, but due to the collision avoidance requirement, the solution speed is slow, sacrificing high solution speed. Therefore, there is currently no method that can balance both aspects. Summary of the Invention

[0004] To address the problems in the existing technology, this invention proposes a trajectory planning method for unmanned surface vessels under complex ocean current conditions.

[0005] The technical solution adopted in this invention is as follows:

[0006] This invention discloses a trajectory planning method for unmanned surface vessels under complex ocean current conditions, comprising the following steps:

[0007] 1) Obtain the initial state information, final state information, size information and kinematic parameters of the unmanned surface vessel (USV), obtain the map information between the USV's starting point and the final point, as well as the ocean current information, and obtain the USV's multi-objective cost function;

[0008] 2) Construct an unmanned surface vessel (USV) dynamic model that considers ocean currents, and construct state constraints based on the USV dynamic model; construct a coarse trajectory of the USV, construct an initial solution vector based on the coarse trajectory, and then construct a tunnel constraint based on the initial solution vector; construct boundary constraints based on the initial state information and the final state information; construct control input constraints based on the size information and kinematic parameters.

[0009] 3) Obtain the optimal control sequence based on state constraints, initial solution vector, ship tunnel constraints, boundary value constraints, multi-objective cost function, and control input constraints to complete the trajectory planning of the unmanned surface vessel.

[0010] Furthermore, the dimensional information includes the unmanned surface vessel's hull length L, hull width W, and distance L from its center of gravity to the bow. f and the length L from the center of mass to the stern r The kinematic parameters include the minimum longitudinal velocity u of the unmanned surface vessel. min and maximum longitudinal velocity u max The ocean current information includes the location of the vortex center, the speed of movement, and the intensity coefficient of the ocean current; the initial state information includes the center of mass position, attitude angle, longitudinal velocity, lateral velocity, and yaw rate of the unmanned surface vessel at the initial moment; the final state information includes the center of mass position, attitude angle, longitudinal velocity, lateral velocity, and yaw rate of the unmanned surface vessel at the initial moment.

[0011] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0012] This invention employs a two-layer trajectory planning algorithm based on numerical optimization, consisting of an upper-layer solver and a lower-layer optimizer. During trajectory planning, the algorithm first obtains the initial and final state information of the unmanned surface vessel (USV) and a multi-objective cost function based on user requirements. Then, it acquires map information between the USV's starting and ending points, the USV's dimensions, kinematic parameters, and ocean current information based on the USV's perception module. Next, it constructs a matching ocean current model based on the ocean current information, and then constructs a dynamic model of the USV considering ocean currents based on the matching ocean current model. The map information, USV dimensions, kinematic parameters, and the matching ocean current model are input into the upper-layer solver. The upper-layer solver receives the information and starts, using the matching ocean current model to plan the trajectory. The flow model constructs the navigation environment, employs an improved hybrid A* search method to find a coarse trajectory, and constructs an initial solution vector based on this trajectory, which serves as the input to the lower-level optimizer. Based on the initial solution vector, a ship tunnel constraint is constructed; state constraints are constructed based on a ship dynamics model considering ocean currents; control input constraints are constructed based on the unmanned surface vessel's (USV) size information and kinematic parameters; boundary constraints and a multi-objective cost function are constructed based on the USV's initial and final state information. The lower-level optimizer obtains the boundary constraints, ship tunnel constraints, state constraints, control input constraints, and the initial solution vector, models the trajectory planning task as an optimal control problem, and integrates a nonlinear solver to solve the problem, outputting the optimal control sequence.

[0013] This invention designs an ocean current model and constructs a dynamic model of an unmanned surface vessel (USV) under the influence of ocean currents. While ensuring solution speed, it improves the accuracy of the solution results, overcoming the problem in existing technologies where insufficient consideration of environmental factors prevents the algorithm from being used in real-world scenarios. This enables the USV to navigate more smoothly and economically in real-world environments. The invention employs numerical optimization methods to optimize the trajectory. By designing a multi-objective function, it overcomes the problem of single planning objectives in existing technologies. Furthermore, by constructing a tunnel constraint, it overcomes the slow solution speed problem of existing technologies using the same method. Ultimately, this invention overcomes the problem in existing technologies that cannot simultaneously balance trajectory solution speed and solution quality, thereby ensuring the safe and efficient operation of the USV. Attached Figure Description

[0014] Figure 1 This is a diagram illustrating the architecture of a trajectory planning method that considers complex ocean current conditions.

[0015] Figure 2 This is a kinematic model diagram of a three-degree-of-freedom unmanned surface vessel;

[0016] Figure 3 This is a diagram showing the effect of ocean currents on the absolute speed of unmanned surface vessels;

[0017] Figure 4 It is a map of ocean currents generated randomly at a certain moment;

[0018] Figure 5 This is a diagram illustrating the multi-feature disk representation method for unmanned surface vessels;

[0019] Figure 6 It is a collision check diagram of the unmanned surface vessel and its environment;

[0020] Figure 7 This is a diagram of the forward expansion method of the search algorithm;

[0021] Figure 8 This is a diagram of the node collision detection method;

[0022] Figure 9 This is a schematic diagram of the triangular area method;

[0023] Figure 10 This is a schematic diagram of a navigation tunnel;

[0024] Figure 11 It is a point mass P j Schematic diagram of the ship tunnel (N) box =N sam =12);

[0025] Figure 12 This is a diagram illustrating the method for generating a rectangular region. Detailed Implementation

[0026] The present invention will be further described and illustrated below with reference to specific embodiments. The embodiments described are merely examples of the content of this disclosure and do not limit the scope of the invention. The technical features of each embodiment in the present invention can be combined accordingly, provided that there is no mutual conflict.

[0027] This invention proposes a trajectory planning method for surface unmanned surface vessels (USVs) considering complex ocean current conditions, based on navigation perception and ocean current prediction. A two-layer trajectory planning algorithm based on numerical optimization is designed: the upper layer quickly searches for feasible trajectories, while the lower layer optimizes and solves for high-quality trajectories. This method can fully consider environmental information during navigation, enabling rapid calculation of safe and comfortable trajectories conforming to the kinematic laws of USVs under complex terrain and ocean current conditions. The invention improves the search method by proposing a new simplified representation method for USVs based on the A* algorithm, incorporating the time dimension and dynamic influences into the search space, thus improving trajectory quality without affecting the solution speed. The impact of ocean currents on USV kinematics is studied in depth, and an ocean current model is designed to construct a dynamic model of USVs under the influence of ocean currents, improving the accuracy of the solution results while maintaining the solution speed. To address the slow solution speed caused by collision avoidance constraints, this invention designs a "ship tunnel constraint" method. Based on the original algorithm framework, the optimal control problem is modified, removing redundant collision avoidance constraints and improving the trajectory solution speed.

[0028] The trajectory planning algorithm of this invention achieves safe, efficient, energy-saving, and comfortable global trajectory planning from the initial state to the final state based on the unmanned surface vessel's navigation perception and ocean current prediction. The implementation process of the algorithm is as follows: Figure 1 As shown.

[0029] like Figure 1 As shown, the trajectory planning method adopts a two-layer architecture of decision-making plus planning to construct and solve the trajectory planning problem. The algorithm consists of an upper-layer solver and a lower-layer optimizer. The lower-layer optimizer models the trajectory planning task as an optimal control problem and integrates a nonlinear solver to solve the problem and output the optimal control sequence. The upper-layer solver provides the lower-layer optimizer with the initial solution vector, so that the lower-layer nonlinear programming solution process can "hot start" from the position of the high-quality solution vector.

[0030] The implementation process of this planning method is as follows:

[0031] (1) Based on user requirements, obtain the initial state information, final state information, and multi-objective function (i.e., multi-objective cost function) of the unmanned surface vessel (USV). Based on the USV's perception module, obtain the map information between the USV's starting point and ending point, the USV's size information, kinematic parameters, and ocean current information. Then, construct a matching ocean current model based on the ocean current information, and construct a ship dynamics model that considers ocean currents (i.e., an USV dynamics model that considers ocean currents) based on the matching ocean current model. Input the map information, the USV's size information, kinematic parameters, and the matching ocean current model into the upper-level solver.

[0032] (2) The upper-level solver obtains information and starts up. Based on the input information, it constructs the navigation environment using the matching ocean current model, searches for a rough trajectory using the improved hybrid A* search method, and constructs an initial solution vector based on the trajectory. The initial solution vector is used as the input of the lower-level optimizer.

[0033] (3) The lower-level optimizer establishes the optimal control proposition, specifically as follows: Based on the initial solution vector, it constructs the "ship tunnel constraint"; based on the ship dynamics model considering ocean currents, it constructs the state constraints (i.e., the system state constraints in the figure); based on the mechanical performance of the unmanned surface vessel (i.e., the size information and kinematic parameters of the unmanned surface vessel), it constructs the control input constraints; based on the user requirements (i.e., the initial state information and final state information of the unmanned surface vessel), it constructs the boundary constraints and multi-objective cost function.

[0034] (4) The lower-level optimizer discretizes the optimal control problem into a nonlinear programming (NLP) problem and initializes the NLP solution process using the initial solution vector input in step (2).

[0035] (5) The lower-level optimizer uses a nonlinear solver to obtain the optimal solution vector, obtains the numerical optimal solution of the optimal control problem, obtains the optimal control sequence, and outputs the optimal control sequence to the downstream control actuator to achieve autonomous navigation.

[0036] This invention proposes a dynamic model for unmanned surface vessels that considers the influence of water flow. It integrates the impact of ocean currents on unmanned vessels into dynamic constraints to ensure that the planned trajectory can be accurately tracked in an environment where ocean currents actually exist.

[0037] For unmanned surface vessels (USVs) navigating on the water, determining their attitude requires six independent coordinates. However, when modeling the trajectory planning task, we only need to consider the changes in the heading angle and the navigation trajectory, i.e., the motion of the vessel in the horizontal plane. Considering that the research object is a USV, unlike container ships, roll-on / roll-off ships, and high-speed warships with high centers of gravity, when it is navigating in the ocean, wind and waves and turning motion will not cause a large heel angle. Therefore, when constructing the problem, the effects of heave, pitch, and roll motion can be ignored, simplifying the six-degree-of-freedom USV motion into a three-degree-of-freedom USV motion problem.

[0038] When constructing the dynamic model of the unmanned surface vessel in this invention, the model follows the following assumptions:

[0039] (1) The unmanned surface vessel travels on a horizontal surface, ignoring the effects of heave, pitch and roll motions;

[0040] (2) Ignore the effects of wind and wave forces on the motion of the unmanned surface vessel;

[0041] (3) The unmanned surface vessel navigates in a uniform flow field;

[0042] (4) The unmanned surface vessel is a rigid body that is symmetrical on both sides and is small enough to be regarded as a point mass in the ocean;

[0043] (5) The unmanned surface vessel has no lateral thrusters and only generates power in the bow direction.

[0044] like Figure 2 As shown, the inertial coordinate system {n} = (x n ,y n ,z n (x) is an inertial coordinate system fixed to the Earth's surface, taken as the reference system, and x is defined as... n The axis points due east, y n The axis points due north, z n The axis points to the Earth's center, and the unit vector along each axis is denoted as . Body-fixed coordinate system {b}=(x b ,y b ,z b The origin is located at a designated point O within the unmanned surface vessel. b The attached coordinate system on the plane is defined as x b The axis points towards the bow, y b The axis points to the port side, z b The axis points to the keel, and the unit vector along each axis is denoted as . The body-fixed coordinate system {b} moves arbitrarily in space with the unmanned surface vessel and is a non-inertial coordinate system.

[0045] According to the unmanned surface vessel (USV) dynamics model, the motion of the USV in the inertial coordinate system {n} is constrained by the following set of differential equations:

[0046]

[0047] Where t is time, t∈[0,t] f The time t at which the unmanned surface vessel's motion terminates. f Let be the variables to be determined; θ(t) be the attitude angle of the unmanned surface vessel at time t; [x(t), y(t)] be the centroid of the unmanned surface vessel at time t; u(t) be the angle of the unmanned surface vessel relative to the water flow along x at time t. b The velocity of the axis (i.e., u(t) is the longitudinal velocity); v(t) is the velocity of the unmanned surface vessel relative to the water flow along the y-axis at time t. b The velocity of the axis (i.e., v(t)d3 is the lateral velocity); r(t) is the velocity of the unmanned ship orbiting z at time t. b The angular velocity of the axis rotation (i.e., r(t) is the yaw angular velocity); m1, m2, and m3 are inertial masses considering the added mass effect, all of which are preset; u cur_east [(x(t),y(t),t)] represents the ocean current velocity along x at time t. n Components of the axis; u cur_north[(x(t),y(t),t)] represents the ocean current velocity along y at time t. n The components of the axis; d1, d2, are the hydrodynamic damping coefficients for u(t), v(t), and r(t), respectively; τ u (t) represents the propulsion force of the unmanned surface vessel; τ r (t) represents the yaw moment of the unmanned surface vessel.

[0048] The derivation process of equation (1) is introduced below.

[0049] According to hypothesis (3), the unmanned surface vessel (USV) navigates in a uniform flow. The influence of the flow on a ship at sea depends on the nature of the flow. When navigating in a uniform flow, the flow does not generate power; it only causes the ship to drift kinematically with the flow, such as... Figure 3 As shown, it demonstrates the influence of ocean currents on the absolute speed of unmanned surface vessels. Based on geometric relationships, the first three terms of equation (1) can be derived.

[0050] According to the acceleration composition theorem of rigid body kinematics, "the acceleration of a point on a rigid body is equal to the geometric sum of the acceleration when moving with the pole and the acceleration when moving around the pole in a fixed position", the acceleration of the center of mass M of the unmanned surface vessel can be derived, and the dynamic equation can be established using Newton's second law.

[0051] like Figure 2 As shown above, it has been set Let {b} be the unit vector along each coordinate axis of the body-fixed coordinate system; M is the center of mass of the unmanned surface vessel; and the pole O is the point on the surface. b Coincides with the center of mass M; pole O b The speed is u and v represent the poles O, respectively. b speed In x b axis and y b The component on the axis; r is the unmanned surface vessel orbiting the pole O. b Angular velocity of rotation.

[0052] Pole O b speed It can be represented as:

[0053]

[0054] in, The speed at which the unmanned surface vessel travels along its bow; The speed at which the unmanned surface vessel travels along its starboard side;

[0055] Pole O b acceleration for:

[0056]

[0057] in, For the pole O b speed In x b The first derivative of the component on the axis; r is the unmanned vessel orbiting z. b Angular velocity of the axis of rotation;

[0058] From the pole O b Since the center of mass M coincides with the pole O, it can be deduced that the center of mass M revolves around the pole O. b Speed ​​of fixed-point motion for:

[0059]

[0060] Center of mass M around pole O b acceleration of a fixed point for:

[0061]

[0062] Therefore, the acceleration at the center of mass M for:

[0063]

[0064] Use a x a y They represent accelerations respectively. In x b With y b Components in the axial direction:

[0065]

[0066] use Let a represent the total external torque applied to the unmanned surface vessel. x a y And the pole O b When the mass coincides with the center of mass M, according to the dynamic-static method, "when a non-free mass system is in motion, the principal moment of the force system acting on it must be zero," we know that:

[0067]

[0068] The x of unmanned surface vessels b Total external force in the axial direction, The unmanned surface vessel was affected by y b Total external force in the axial direction:

[0069]

[0070] in, and These represent the effects of x on the unmanned surface vessel. b axial direction and y b Hydrodynamic forces in the axial direction; This indicates that the unmanned surface vessel is affected by z. b Hydrodynamic moment in the axial direction; These represent the effects of x on the unmanned surface vessel. b Axial direction, y b Hydrostatic force in the axial direction and z b Hydrostatic moment in the axial direction; These represent the effects of x on the unmanned surface vessel. b Axial direction, y b Wind force along the axis and z b Wind moment in the axial direction; This indicates that the unmanned surface vessel is at x b Axial direction, y b The main force in the axial direction and z b The reaction force of the yaw moment in the axial direction.

[0071] Under the conditions of assumptions (2), (3), and (5):

[0072]

[0073] Assuming that the hydrodynamic forces and moments on a rigid body can be linearly superimposed, the hydrodynamic forces can be derived using a fluid dynamic mass-damped-spring system as follows:

[0074]

[0075] Where, τ hyd M represents the hydrodynamic matrix acting on the unmanned surface vessel; A This represents the additional mass matrix generated by the unmanned surface vessel due to the inertia of the surrounding fluid during its motion; v r C represents the velocity matrix of the unmanned surface vessel relative to the water flow; A (v r D(v) represents the Coriolis force and centripetal force matrix caused by the added mass of the unmanned surface vessel, generated by the rotation of the body-fixed coordinate system {b} relative to the inertial coordinate system {n}; r This represents the total hydrodynamic damping matrix. Under the assumptions of this invention, it is specifically expressed as follows:

[0076]

[0077] In equation (15) These represent the unmanned surface vessel along x. b Axial direction, y b Acceleration in the axial direction and z bAngular acceleration along the axis; Equations (13) and (17) use the sign of the fluid dynamics derivative, for example, along x. (SNAME T. Nomenclature for treating the motion of a submerged body through a fluid[J].The Society of Naval Architects and Marine Engineers, Technical and Research Bulletin, 1950(1950):1-5.) b Acceleration in the axial direction The resulting hydrodynamic additional mass force X is represented as follows:

[0078]

[0079] Based on equations (9), (10), and (11), we can deduce that:

[0080]

[0081] According to Newton's second law and equations (7), (8) and (9), we can deduce that:

[0082]

[0083] Where, m g For the quality of the unmanned surface vessel, d1 = X u d2=Y v d3=N r By rearranging equation (20), the last three terms in equation (1) can be derived.

[0084] The formation of ocean currents is very complex. To accurately represent time-varying ocean current models, a large number of complex equations and data observation results are required. When studying the trajectory planning problem of unmanned surface vessels, it is necessary to comprehensively consider the accuracy of the calculation results and the real-time performance of the solution. Therefore, this invention relates to a simplified time-varying ocean current model that can still accurately describe the impact on the motion of unmanned surface vessels after simplifying the complex ocean current model. This model can be used to match ocean current information.

[0085] As mentioned earlier, under the dynamic model assumption (3), the absolute speed of the unmanned surface vessel (USV) in a uniform flow field is affected by the ocean current. Furthermore, in a uniform flow field, the current does not generate power for the hull, only causing the vessel to drift kinematically with the fluid. Therefore, the ocean current model to be constructed can only consider the velocity effect. To better reflect the influence of ocean currents on the motion of the USV, a method of superimposing randomly distributed vortex fields is chosen to simulate the ocean current velocity data. The vortex field formula is as follows:

[0086]

[0087] Where eddy{Q(t),a(t),f} is the vortex function; Q(t) is the vortex center; x Q (t), y Q (t) represent the vortex center Q(t) at x. n axis, y n The coordinates of the axis; x Q_init y Q_init These represent the initial positions of the vortex center; These represent the vortex center at x and x, respectively. n y n The velocity along the axial direction; a(t) represents the intensity of the vortex field, a x (t), a y (t) represent the vortex field along x, respectively. n y n Strength modulus in the axial direction; a x_init a y_init These represent the vortex field along x. n y n Initial strength coefficient in the axial direction; Then they respectively represent the vortex field along x n y n The rate of change of the intensity coefficient along the axial direction; s(x,y,t) is the distance between the current position and the vortex center; v x_cur v y_cur These represent ocean currents along x. n y n The magnitude of the axis velocity; f represents the rotation direction of the vortex field, and its sign determines the rotation direction of the vortex field. If f is positive, the rotation direction of the vortex field is clockwise, and if f is negative, the rotation direction of the vortex field is counterclockwise; sgn() is the sign function.

[0088] The ocean current formula is as follows:

[0089]

[0090] Where Γ represents the flow field generated by the vortex and N eddy This indicates the number of vortex fields; rand() represents a random function. The ocean current field consists of N... eddy It is formed by the superposition of multiple vortex fields.

[0091] The ocean current model established according to equation (22) is shown in the figure below. Figure 4 As shown, the direction of the arrow indicates the velocity direction of the ocean current at this point, and the size of the arrow indicates the magnitude of the current velocity.

[0092] Based on map information, the current position of the unmanned surface vessel (USV) is determined. Then, using ocean current maps and models, the location of the USV at that moment can be determined.

[0093] u cur_east [(x(t),y(t),t)] and u cur_north [(x(t),y(t),t)], then obtain u cur_east [(x(t),y(t),t)] and u cur_north Substituting [(x(t),y(t),t)] into the dynamics model of the unmanned surface vessel (USV) considering ocean currents, we obtain the dynamics model of the USV at the current location considering ocean currents. That is, the flow field generated by the vortex is the ocean current, so at each point (x,y), there is a corresponding vector, u, in the dynamics model. cur_east [(x(t),y(t),t)] and u cur_north [(x(t),y(t),t)] represents the components of this vector in the east and north directions.

[0094] When dealing with the problem of autonomous parking trajectory planning for vehicles, one or two feature disks are generally used to represent the vehicle based on its size and the parking space. However, considering the slender shape of unmanned surface vessels (USVs), it is not reasonable to represent them with one or two feature disks. Therefore, this invention proposes a multi-feature disk representation method for USVs.

[0095] like Figure 5 As shown, the hull of the unmanned surface vessel is covered by a rectangle of length L and width W, with its centroid at M(x,y). f With L r These represent the distances from the center of mass to the bow and stern, respectively. First, a square with side length W is taken at both the bow and stern, and the radius R of the feature disk is set. disc for:

[0096]

[0097] It can be deduced that the area not covered by the square has a length of L-2W in the longitudinal direction (the length of the unmanned surface vessel). Within this area, N can be arranged at equal intervals. disc -2 feature disks:

[0098]

[0099] Where, N disc The number of feature disks used to characterize the unmanned surface vessel; ceil() represents the rounding function; and the length Δl independently covered by each feature disk in the longitudinal direction is:

[0100]

[0101] The entire unmanned surface vessel can use N disc The system is covered by a set of feature disks, and any two adjacent feature disks overlap each other. Let the center of the i-th feature disk be denoted as .

[0102]

[0103] Where θ represents the attitude angle of the unmanned surface vessel; l i This represents the distance from the i-th feature disk to the centroid:

[0104]

[0105] Among them, L r This is the distance from the center of mass to the stern.

[0106] Before performing the search task, the upper-level solver first divides the two-dimensional planar region where the trajectory planning task is located into a grid, constructing a raster map. Based on the simplified representation method described above, the condition "the vehicle body does not collide with obstacles" can be transformed into "all feature disks do not collide with obstacles." This invention shrinks the feature disks towards the center by R... disc Expand all obstacles in the environment (R) disc The condition "all characteristic disks do not collide with obstacles" is transformed into "point mass P". i (i = 1, 2, ..., N) disc "None of them will collide with the expanded obstacles." Figure 6 As shown, it is possible to make particle P i (i = 1, 2, ..., N) disc A trajectory that does not collide in the puffed area must meet the collision avoidance constraints in the original scene.

[0107] This invention designs a four-dimensional hybrid A* global trajectory search algorithm that considers ocean currents. The upper-level solver uses this algorithm to find the initial solution vector, i.e., the initial solution, for the lower-level optimizer. This invention can obtain an initial solution vector with global optimality while ensuring the real-time computation of the upper-level solver, enabling the lower-level optimizer to "start" from a high-quality solution vector position when solving NLP problems.

[0108] The task of the search phase is to quickly search the state space of the unmanned surface vessel (USV) for a collision-free, coarse global trajectory that connects the initial and final states with minimal cost. Currently, the A* search algorithm widely used in academia and industry does not fully consider the ship's kinematic model. While the coarse path obtained by A* search can be used for decision-making, it is not conducive to subsequent NLP solutions. Therefore, based on A*, this invention proposes a four-dimensional hybrid A* search algorithm that considers ocean currents.

[0109] First, based on the aforementioned grid map, two dimensions, θ and t, are added to grid the continuous four-dimensional xy-θ-t state space. The vehicle state (the current state of the unmanned surface vessel) and ocean current are updated with the sampling time Δt to obtain a directed acyclic four-dimensional connected graph.

[0110] This will lead to an acyclic four-dimensional connected graph, starting node (starting trajectory point). start Termination Node (Termination Trajectory Point) end Input trajectory solver, starting from the starting node Node start We begin the traversal using a forward expansion approach. The forward expansion method is as follows, for example, the current node Node cur The unmanned surface vessel's attitude is [x cur ,y cur ,θ cur According to the dynamics model of unmanned surface vessels that takes into account ocean currents, the τ will be [value missing] per unit time. u ,τ r Sampling is a constant value τ u_cur ,τ r_cur , τ u For the propulsion of the unmanned surface vessel; τ r The yaw moment of the unmanned surface vessel; simulating the unmanned surface vessel's yaw rate per unit time [x cur ,y cur ,θ cur The initial pose of [τ] u_cur ,τ r_cur The control input propels the unmanned surface vessel (USV) forward. Under the assumption of uniform speed, the influence of the ocean current on the USV's motion is considered, thereby generating a navigation path that conforms to the kinematic laws of the USV under the influence of ocean currents. The pose at the end of the path corresponds to [x]. chi ,y chi ,θ chi Corresponding child node chi Furthermore, if τ u (t),τ r (t) is used as a control variable. On the one hand, its value will vary significantly for unmanned surface vessels with different inertial parameters; on the other hand, obtaining the next state requires a lot of time. Therefore, the longitudinal velocity (the unmanned surface vessel relative to the water flow along x) is chosen as the control variable. b The velocity of the axis u(t) and the yaw rate (unmanned surface vessel orbiting z) b The angular velocity of the axis rotation r(t) is used as the control variable, and only the simplified kinematic model shown in equation (28) is considered. To speed up the calculation, the lateral velocity of the unmanned surface vessel is ignored and the lateral velocity v(t) is set to 0.

[0111]

[0112] In the marine navigation environment, backward motion is generally not considered, and sufficient adjustment space needs to be left for control in engineering. Therefore, in u(t)∈{u min ,u max}, r(t)∈{r min ,r max Under the condition that u max The maximum value of the longitudinal velocity; u min The minimum longitudinal velocity; r max This represents the maximum yaw rate; r min The minimum yaw rate; set the forward extension mode list to:

[0113] {(u max 0.8r min ),(u max 0.5r min ),(u max 0.2r min ),(u max ,0),(u max 0.2r max (u) max 0.5r max (u) max 0.8r max )}

[0114] Expansion situation as follows Figure 7 As shown.

[0115] If a termination node is found during the expansion process end It is exactly the current node (current trajectory point) Node cur If the node is found to be expandable, the search is complete. Otherwise, select the node in the expandable point set that has the best chance of having a direct or indirect connection to the terminal node, and record the data during the expansion process. The data includes the total cost (Node.f), the historical cost (Node.g), the expected future cost (Node.h), the parent node index (Node.parent_idx), whether it is in the open list (Node.is_in_openlist), whether it is in the closed list (Node.is_in_closedlist), and the current pose (Node.x). cur Node.y cur Node.θ cur At the current moment, Node.t cur From which u and r are they derived? What is the flight path within the extended period? {(x)} i ,y i ,θ i ,t i |i = 0, 1, 2, ..., N f}, where N f N represents the number of points involved in the expansion process from the current point to the next point; that is, the entire process from the current point to the next point is a forward deduction process, which involves a series of points. f This refers to this series of points. For example, starting from (0, 0), expanding forward at a fixed speed and yaw angle, recording the data every 0.1 seconds, and then recording N. f One, arrived at (1, 1).

[0116] The total cost Node.f, historical cost Node.g, and expected future cost Node.h are used to determine whether the next expandable point is the most likely point in the set of expandable nodes to have a direct or indirect connection with the terminal node; that is, to find the expandable point with the minimum total cost Node.f. The historical cost function g penalizes undesirable trajectories, giving undesirable trajectory segments a greater historical cost value, thereby calculating a more reasonable and smoother trajectory. For example, considering that frequent steering operations increase sailing time and cause energy waste, this system sets the historical cost function g as follows:

[0117] Node chi .g = Node cur .g+s simu +μ|Node chi .r-Node cur .r| (29)

[0118] Among them, Node chi .g, Node cur .g represents the past historical cost of the child node (i.e., the next trajectory point, where each expandable point of the current trajectory point will be used as the next trajectory point for calculating past historical costs) and the current node (the current trajectory point), respectively. simu The step size represents the search range, μ represents the penalty factor for the steering operation, and Node represents the step size. chi .r, Node cur The .r represents the rotational angular velocity of the child node and the current node, respectively.

[0119] In addition, the expected future cost Node.h is calculated by the heuristic function h, which affects the search quality and efficiency. Setting a reasonable heuristic function h is beneficial for accurately estimating the shortest obstacle avoidance path length from child nodes to the terminal node. This invention uses two sub-functions h. nonholonomics with h collision_avoidance Let's define the heuristic function h:

[0120] h = max{h nonholonomics ,h collision_avoidance} (30)

[0121] Among them, h nonholonomics This represents the path length that conforms to the kinematic laws of unmanned surface vessels (USVs) while neglecting obstacle avoidance. By analyzing and comparing the kinematic laws of USVs with those of vehicles, this study uses the Reeds-Shepp curve to estimate its value; h collision_avoidance This represents the path length considering obstacle avoidance while ignoring kinematic laws, and is characterized by the larger of the path length obtained in a two-dimensional grid using the classic A* algorithm and the Manhattan distance.

[0122] The parent node index Node.parent_idx is used to record the parent node of the extended point and to determine whether the currently obtained sequence is the sequence of updated optimal trajectory points.

[0123] The open list Node.is_in_openlist and the closed list Node.is_in_closedlist are used to record whether the expansion point is an expandable point and whether it has been expanded before. If the expansion point is neither in the open list nor the closed list, it has never been expanded. If the expansion point is in the open list but not the closed list, it has been expanded but can be expanded again. If the expansion point is in the closed list, it cannot be expanded.

[0124] Current pose Node.x cur Node.y cur Node.θ cur At the current moment, Node.t cur This is used to record the status information of the expansion point, so that it can continue to expand forward.

[0125] The extended flight path {(x k ,y k ,θ k ,t k |k = 0, 1, 2, ..., N f The} is used for collision detection and trajectory output after finally obtaining the trajectory point sequence.

[0126] Repeat the above expansion operation until the termination node is found. Then, reverse the reading of the navigation trajectory within the expansion period of each trajectory point to obtain a complete trajectory (i.e., the rough trajectory of the unmanned surface vessel) {(x j ,y j ,θ j ,t j j = 0, 1, 2, ..., N A}, N A This represents the total number of trajectory points in a rough running trajectory; and the trajectory is resampled to obtain {(x m ,y m ,θm m = 0, 1, 2, ..., N sam} and the time T for the formation of the trajectory tst N sam The total number of trajectory points after resampling (i.e., assuming N) A If N is 200, then N sam (can be 100), where N is defined. sam +1 discrete nodes uniformly distributed over the estimated travel time range, with the longitudinal velocity {u} of the unmanned surface vessel already considered during resampling. m |m=0,1,2,...,N sam} and yaw rate {r m |m=0,1,2,...,N sam Considering that the lateral velocity is relatively small during the entire navigation process under normal circumstances, it is set to {v} during the initial value configuration. m ≡0|m=0,1,2,...,N sam}, and obtain {τ} based on the kinematic model of the unmanned surface vessel. u_m |m=0,1,2,…,N sam},{τ r_m |m=0,1,2,…N sam}, recombine the above sequence of configuration points into a vector {x} m ,y m ,θ m ,u m ,v m ,r m ,τ u_m ,τ r_m |m=0,1,2,…N sam}, estimated time T tst vector {x m ,y m ,θ m ,u m ,v m ,r m ,τ u_m ,τ r_m |m=0,1,2,…N sam These are collectively referred to as the initial solution vector.

[0127] This invention proposes a simplified method for collision avoidance constraints—the ship tunnel constraint method—which can remove redundant collision avoidance constraints in the optimal control problem, thereby improving the computation speed of the lower-level optimizer and ensuring the real-time solution of the planner.

[0128] Currently, the mainstream method for determining whether a point mass is within an obstacle region is the triangular area method, such as... Figure 9As shown, when the sum of the areas of the triangles formed by the particle and each side of the obstacle is greater than the area of ​​the obstacle, it indicates that the particle falls outside the obstacle region. However, this method introduces a large number of non-convex and nonlinear constraints, which is very detrimental to the rapid solution of the trajectory.

[0129]

[0130] in, For triangle ΔPQ n The area of ​​Q1; For ΔPQ k Q k+1 The area; for Q1Q2...Q n The area.

[0131] Therefore, this invention proposes a method that, using the aforementioned multi-feature disk representation method for unmanned surface vessels, constructs a dedicated navigation tunnel for each particle, simplifying collision avoidance constraints through "navigation tunnel constraints," thereby reducing the large number of nonlinear constraints present in NLP problems. Figure 10 As shown, if tunnel constraints are not used, both routes 1 and 2 satisfy the collision avoidance constraint conditions. However, when the given initial solution is homotopic with route 1, route 2 will not be considered during the optimization process. In this case, the constraint setting can obviously completely exclude route 2, thereby improving the solution speed. With the "ship tunnel constraint" method proposed in this invention, as long as the center of the feature disk at each time step is within its corresponding local range, it can be guaranteed that the unmanned surface vessel will not collide with obstacles.

[0132] The construction method for ship tunnels is as follows:

[0133] The vector {x} constructed using a four-dimensional hybrid A* global trajectory search algorithm that considers ocean currents m ,y m ,θ m ,u m ,v m ,r m ,τ u_m ,τ r_m |m=0,1,2,…N sam} reflects the rough trajectory of the unmanned surface vessel's center of mass, and takes the pose-related variable {x} as an example. m ,y m ,θ m |m=0,1,2,…N sam By combining equations (26) and (27), the center P of each characteristic disk representing the hull of the unmanned surface vessel can be determined. i (i = 1, 2, ..., N) discrough trajectory traj i (t)(i=1,2,...,N disc Based on this, P can be considered as... i (i = 1, 2, ..., N) disc Construct its corresponding ship tunnel.

[0134] For ease of description, we choose the center P of the i-th feature disk. i This section details the tunnel construction method using an example. First, P is determined based on the initial solution vector constructed by the upper-level solver. i rough trajectory traj i (t), assuming P i The tunnel is made up of N box It consists of N local polygonal regions. box =N sam Under these circumstances, the estimated total sailing time is T. tst The nth sampling time t n It can be defined as (T) tst ·n) / N box =(T tst ·n) / N sam Then at the nth sampling time, the center P of the feature disk i The location is:

[0135]

[0136] in, For the nth sampling time, particle P i x-axis coordinate; For the nth sampling time, particle P i y-axis coordinate; θ n The attitude angle of the unmanned surface vessel at the nth sampling time; x n For the nth sampling time, the x-axis coordinate of the unmanned surface vessel's center of mass; y n The y-axis coordinate of the unmanned surface vessel's center of mass is given at the nth sampling time.

[0137] Then, the mass P at each sampling time... i A local polygon, or local safe region, is constructed around the center. Considering that the purpose of setting this region is to improve the solution speed of NLP problems, this invention chooses to use a horizontally and vertically straight rectangle as the local safe region, such as... Figure 11 As shown.

[0138] This invention will use P at the nth sampling time. i (i.e. P) i_n The matrix region corresponding to ) is represented as SSC i_n This section introduces how to build an SSC. i_n First, initialize SSC. i_nAt point P i_n Place, regard it as Define a rectangle with length and width both equal to 0 at a given location, and define the expansion step size Δs and the maximum allowable unidirectional expansion length L. max Then, following the direction list [π / 2,π,3π / 2,2π], that is, in a counterclockwise direction along the four sides of the rectangle, it continuously expands outward with a constant step size Δs. During the expansion process, if the generated region does not overlap with the obstacle region in the expanded raster map, the current expansion is considered valid, and the current expanded region is merged into the rectangle SSC. i_n Conversely, if the expansion overlaps with an obstacle area, the current expansion is deemed invalid, the expansion area is abandoned, and expansion in this direction is no longer permitted. Furthermore, to avoid infinite expansion, when the length of any expansion direction exceeds L... max When this happens, expansion in this direction ceases, and the expansion process is as follows: Figure 12 As shown.

[0139] When the unfolding process stops, the obtained rectangle SSC will be... j_i Range is denoted as Therefore, the constraints for the ship tunnel are: Therefore, it is only necessary to ensure P. i By ensuring that the object falls within the corresponding matrix region at any discrete time, collision-free operation is guaranteed, eliminating the need for complex collision avoidance constraints. This construction operation (N) can be repeated. sam By repeating this process (+1), a series of SSCs can be obtained, and the combination of these SSCs constitutes the particle P. i The tunnel; among which, x i_n For the nth sampling time, particle P i The lower limit of the x-axis coordinate; For the nth sampling time, particle P i The upper limit of the x-axis coordinate; y i_n For the nth sampling time, particle P i The lower limit of the y-axis coordinate; For the nth sampling time, particle P i The upper limit of the y-axis coordinate.

[0140] Figure 1 The (system) state constraints are:

[0141] During the movement of an unmanned surface vessel (USV), it must always adhere to its basic motion laws, that is, it must satisfy system state constraints. Based on the kinematic model proposed in this invention, the system state constraints are obtained as follows:

[0142]

[0143] Wherein, the state vector With control vector for:

[0144]

[0145] Because the mechanical characteristics of unmanned surface vessels (USVs) impose certain constraints on their state and control variables, boundary conditions are required to describe the mechanical characteristics of their motion. Figure 1 The control input constraints are:

[0146]

[0147] Among them, v min v is the minimum lateral velocity of the unmanned surface vessel; max r represents the maximum lateral velocity of the unmanned surface vessel. min The minimum yaw rate r of the unmanned surface vessel. max τ represents the maximum yaw rate of the unmanned surface vessel. u_min τ represents the minimum propulsion force of the unmanned surface vessel. u_max τ represents the maximum propulsion force of the unmanned surface vessel; r_min τ is the minimum yaw moment of the unmanned surface vessel; r_max This represents the maximum yaw moment of the unmanned surface vessel.

[0148] Furthermore, due to the mechanical characteristics of the unmanned surface vessel (USV), it can only generate yaw moment during motion when its longitudinal velocity is non-zero. To describe this constraint, the following constraint is added:

[0149] |τ r (t)|≤|u(t)·λ|

[0150] Where λ>0 is the dimension conversion factor.

[0151] Figure 1 The boundary constraints in the code are:

[0152] The initial and final positions, motion states, and control variables of the unmanned surface vessel should meet the following constraints:

[0153]

[0154] Among them, [x init ,y init ,θ init ,u init ,v init ,r init ,τ u_init ,τ r_init This corresponds to the objective reality of the motion state at the current moment (t=0) recorded and calculated by the sensors. [x] final ,y final sin(θ) final),cos(θ final )]、[u x_final_min ,u y_final_min ]、[u x_final_max ,u y_final_max ] indicates at the termination time (t = t f The motion state constraints of x. init The x-axis coordinate of the unmanned surface vessel's center of mass in its initial state; y-axis coordinate of the unmanned surface vessel's center of mass. init The y-coordinate of the unmanned surface vessel's center of mass in its initial state; θ init The attitude angle of the unmanned surface vessel in its initial state; u init The longitudinal velocity of the unmanned surface vessel in its initial state; v init The lateral velocity of the unmanned surface vessel in its initial state; r init τ is the yaw rate of the unmanned surface vessel in its initial state. u_init The propulsion force of the unmanned surface vessel in its initial state; τ r_init The yaw moment of the unmanned surface vessel in its initial state; x final The x-axis coordinate of the unmanned surface vessel's center of mass when it reaches the final state; y-axis coordinate of the center of mass. init The y-axis coordinate of the unmanned surface vessel's center of mass when it reaches the final state; θ init The attitude angle of the unmanned surface vessel when it reaches the final state; u x_final_min The minimum value of the x-axis component of the lateral velocity of the unmanned surface vessel when it reaches the final state; u y_final_min The minimum value of the y-axis component of the unmanned surface vessel's lateral velocity at the final state; u x_final_max The maximum value of the x-axis component of the unmanned surface vessel's lateral velocity at the final state; u y_final_max The maximum value of the y-axis component of the lateral velocity of the unmanned surface vessel when it is in the final state.

[0155] During the process of solving the optimal control problem by the lower-level optimizer, there is more than one trajectory in the configuration space that can satisfy the above constraints. In order to obtain a satisfactory trajectory from among the many trajectories that satisfy the hard constraints, it is also necessary to design a cost function for screening high-quality trajectories. Considering efficiency, safety, and comfort factors, the multi-objective cost function is designed as follows:

[0156] J=w1·J1+w2·J2+w3·J3+w4·J4+w5·J5

[0157] Where J represents the multi-objective cost function; w1, w2, w3, w4, and w5 are all weight coefficients, and all are greater than 0; J1 represents the expected time for the unmanned surface vessel to complete its navigation mission as quickly as possible, J1 = t fJ2 indicates the expectation that the unmanned surface vessel (USV) can avoid the center of ocean currents and eddies as much as possible; J3 indicates the expectation that the USV can avoid obstacles such as reefs and islands as much as possible; J4 indicates the expectation that the USV's speed changes can be minimized and acceleration and deceleration operations can be minimized; J5 indicates the expectation that the USV's rudder turning speed can be minimized and rudder turning operations can be minimized.

[0158]

[0159] Where, N eddy Let c represent the number of vortex fields, assuming the center of the c-th vortex is... This is used to represent the distance of the unmanned surface vessel from the center of the c-th vortex. (x(t), y(t)) are the coordinates of the unmanned surface vessel's center of mass; μ c As a penalty coefficient, it represents the degree of punishment for an unmanned surface vessel approaching the center of a vortex.

[0160]

[0161] Where, N obs Let d represent the number of obstacles, and assume the geometric center of the d-th obstacle is y = 0. d obs_d (t) represents the distance of the unmanned surface vessel from the d-th obstacle. κ d As a penalty coefficient, it represents the degree of punishment for an unmanned surface vessel approaching an obstacle.

[0162]

[0163] Wherein, λ1 and λ2 represent penalty coefficients, indicating the degree of punishment for the two manipulation scenarios mentioned above.

[0164]

[0165] Wherein, λ3 and λ4 represent penalty coefficients, indicating the degree of punishment for the two manipulation scenarios mentioned above.

[0166] The method for trajectory planning of unmanned surface vessels under complex ocean current conditions of the present invention includes the following specific implementation steps:

[0167] Step 1: Receive the unmanned surface vessel's dimensions and the distance L from its center of gravity to the bow. f Length L from the center of mass to the stern r hull width W and maximum longitudinal speed u of the unmanned surface vessel max Minimum longitudinal velocity u min The system sends the data to the upper-level solver and the dynamic model of the unmanned surface vessel considering ocean currents, constructs control input constraints, and sends them to the lower-level optimizer.

[0168] Step 2: Receive the current navigation status parameter x of the unmanned surface vessel.init ,y init ,θ init and the target state parameter x final ,y final ,θ final The boundary constraints are then sent to the upper-level solver to construct the boundary constraints, which are then sent to the lower-level optimizer.

[0169] Step 3: Receive environmental map information and send it to the upper-level solver;

[0170] Step 4: Receive ocean current information, vortex center position x Q (t), y Q (t), speed of movement Strength coefficient a x_init a y_init Send the data to the ocean current model, construct the ocean current model using equation (22), and send it to the unmanned surface vessel dynamics model that considers ocean currents;

[0171] Step 5: Construct an unmanned surface vessel dynamics model considering ocean currents based on equation (1), construct system state constraints, and send them to the lower-level optimizer;

[0172] Step 6: The upper-level solver completes initialization based on the input information and configuration parameters, implements coarse trajectory planning using the four-dimensional hybrid A* global trajectory search algorithm that takes into account ocean currents, and constructs an initial solution vector based on the control parameter configuration file, which is then sent to the lower-level optimizer.

[0173] Step 7: The upper-level solver uses the initial solution vector, combined with equations (26) and (27), to obtain the rough trajectories traj of all feature disks. i (t)(i=1,2,…,N disc Input the rough trajectory into the SSCs solver;

[0174] Step 8: Solve using SSCs iteratively to obtain the tunnel for each coarse trajectory. And construct the ship tunnel constraints and send them to the lower-level optimizer;

[0175] Step 9: The lower-level optimizer configures a multi-objective cost function, and constructs an optimal control proposition by combining the input control constraints, boundary constraints, system state constraints, and ship tunnel constraints. It then transforms this into a nonlinear programming problem using a first-order explicit discretization method. The initial solution vector is input into the IPOPT solver to obtain the unmanned surface vessel's state and control variables, which are then sent to the downstream control actuator. The state variables include the unmanned surface vessel's center of mass position, attitude angle, longitudinal velocity, lateral velocity, and yaw rate. The control variables include the unmanned surface vessel's propulsion force and yaw moment.

[0176] The above-described embodiments are merely illustrative of several implementations of the present invention, and while the descriptions are specific and detailed, they should not be construed as limiting the scope of the present invention. Those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and these modifications and improvements all fall within the scope of protection of the present invention.

Claims

1. A method for trajectory planning of a surface unmanned vehicle in complex ocean current conditions, characterized in that, The method comprises the following steps: 1) obtaining initial state information, terminal state information, size information and kinematic parameters of the unmanned ship, obtaining map information and ocean current information between the starting point and the terminal point of the unmanned ship, and obtaining a multi-objective cost function of the unmanned ship; 2) constructing an unmanned ship dynamics model considering ocean currents, constructing state constraints according to the unmanned ship dynamics model, constructing a rough running trajectory of the unmanned ship, constructing an initial solution vector according to the rough running trajectory, and constructing a ship tunnel constraint according to the initial solution vector; constructing boundary value constraints according to the initial state information and the terminal state information; constructing control input constraints according to the size information and the kinematic parameters; 3) obtaining an optimal control sequence according to the state constraints, the initial solution vector, the ship tunnel constraint, the boundary value constraint, the multi-objective cost function and the control input constraint to complete the trajectory planning of the surface unmanned ship; In step 1), the dimensional information includes the unmanned surface vessel's hull length L, hull width W, and distance L from the bow to the center of gravity. f and the length L from the center of mass to the stern r The kinematic parameters include the minimum longitudinal velocity u of the unmanned surface vessel. min and maximum longitudinal velocity u max The ocean current information includes the location of the vortex center, the speed of movement, and the intensity coefficient of the ocean current; the initial state information includes the center of mass position, attitude angle, longitudinal velocity, lateral velocity, and yaw rate of the unmanned surface vessel at the initial moment; the final state information includes the center of mass position, attitude angle, longitudinal velocity, lateral velocity, and yaw rate of the unmanned surface vessel at the initial moment.

2. The method of claim 1, wherein, In step 2), the construction of the unmanned ship dynamics model considering ocean currents comprises: First, an ocean current model is constructed based on the ocean current information, and then the unmanned ship dynamics model is constructed according to the ocean current model, the map information, the size information and the kinematic parameters of the unmanned ship; The formula of the ocean current model is: wherein, Γ represents the flow field of vortex generation; and eddy represents the number of vortex fields; rand represents a random function; Q a (t) is the vortex center position of the a-th vortex field; a a (t) is the intensity coefficient of the a-th vortex field; f a is the rotation direction of the a-th vortex field; The formula of the unmanned ship dynamics model considering ocean currents is: where t is time, t ∈ [0, t f ], t f is the terminal time of the motion of the USV; [x(t), y(t)] is the mass center of the USV at time t; θ(t) is the attitude angle of the USV at time t; u(t) is the velocity of the USV along the x b axis relative to the water flow, i.e., the longitudinal velocity; x b is a coordinate axis of the body coordinate system with the origin at a preset point O b on the USV, the x b axis pointing to the bow of the USV; v(t) is the velocity of the USV along the y b axis relative to the water flow, i.e., the lateral velocity; y b is a coordinate axis of the body coordinate system with the origin at a preset point O b on the USV, the y b axis pointing to the starboard of the USV; r(t) is the angular velocity of the USV rotating around the z b axis, i.e., the yaw angular velocity; z b is a coordinate axis of the body coordinate system with the origin at a preset point O b on the USV, the z b axis pointing to the keel of the USV; u cur_east [(x(t), y(t), t)] is the component of the ocean current velocity along the x n axis at time t; x n is a coordinate axis of the inertial coordinate system fixed on the earth surface, the x n axis pointing to the due east; u cur_north [(x(t), y(t), t)] is the component of the ocean current velocity along the y n axis; y n is a coordinate axis of the inertial coordinate system fixed on the earth surface, the y n axis pointing to the due north; m1, m2 and m3 are inertial masses preset considering the additional mass effect; d1 is the hydrodynamic damping coefficient of u(t); d2 is the hydrodynamic damping coefficient of v(t); d3 is the hydrodynamic damping coefficient of r(t); τ u (t) is the propelling force of the USV at time t; τ r (t) is the yaw moment of the USV at time t.

3. The method of claim 2, wherein, In step 2), the construction of a rough running trajectory of the unmanned ship comprises: 2.1) constructing a grid map and inflating all obstacles in the grid map to obtain an inflated grid map; then adding θ and t dimensions in the inflated grid map to obtain a directed acyclic four-dimensional connected graph; 2.2) Obtain the start trajectory point Node of the unmanned vehicle navigation trajectory start , the end trajectory point Node end , start from the start trajectory point Node start , expand all the trajectory points on the unmanned vehicle navigation trajectory in a forward expansion manner, when expanding to the end trajectory point, read each trajectory point in reverse to obtain the rough running trajectory {(x j ,y j ,θ j ,t j )|j=0,1,2,...,N A} of the unmanned vehicle; wherein N A is the total number of trajectory points in the rough running trajectory.

4. The method of claim 3, wherein, In step 2.1), the method for obtaining the inflated grid map comprises: First, a rectangle with length L and width W is used to cover the unmanned ship body, and a square with side length W is taken at the bow and the stern, and a feature disc is constructed based on the square, and the radius R of the feature disc is set disc For Therefore, in the length direction of the unmanned ship, the length of the area not covered by the square is L-2W, and in this area, N disc - 2 feature discs, where N disc is the total number of feature discs to be placed on the unmanned surface vehicle; ceil() denotes the ceiling function; The length Δl covered by each feature disc independently in the length direction of the unmanned ship is: the center of the i-th characteristic disc is: wherein, l i denotes the length of the center of the i-th feature disc from the center of mass; The two-dimensional plane area where the unmanned ship is located when trajectory planning is performed is divided into a grid, a grid map is constructed, and all obstacles in the grid map are inflated R disc to obtain an inflated grid map.

5. The method of claim 4, wherein, Step 2.2) comprises: 2.2.1) obtaining a starting trajectory point and a terminal trajectory point of the unmanned ship navigation trajectory, and expanding the trajectory points from the starting trajectory point in a forward expansion manner; 2.2.2) Set u(t) and r(t) in the dynamics model of the USV as control variables, set τ u (t) and τ r (t) as constants, set v(t) as 0, and obtain the pose [x, y, θ, t] of the USV at different trajectory points; Under the condition that u(t) ∈ {u min ,u max} and r(t) ∈ {r min ,r max}, the list of expansion points of the forward expansion of the unmanned ship is set as: {(u max ,0.8r min )(u max ,0.5r min )(u max ,0.2r min )(u max ,0)(u max ,0.2r max )(u max ,0.5r max )(u max ,0.8r max )} where r min is the minimum value of the yaw rate; r max is the maximum value of the yaw rate. Each expansion point has total cost Node.f, past history cost Node.g, future expected cost Node.h, parent node index, whether in open list, whether in closed list, current pose, current time, what kind of u, r expansion, navigation trajectory {(x k ,y k ,θ k ,t k )|k=0,1,2,...,N f} in the expansion period, wherein N f is the number of points involved in the expansion process from the current expansion point to the next expansion point; The parent node index is used to record the parent node of the expansion point; the open list and the closed list are used to record whether the expansion point is an expandable node and whether it has been expanded; if the expansion point is neither in the open list nor in the closed list, it has not been expanded at all; if the expansion point is in the open list but not in the closed list, it has been expanded but can still be expanded; if the expansion point is in the closed list, it cannot be expanded; The total cost Node.f is the sum of the past history cost Node.g and the future expected cost Node.h, and the expandable point with the minimum total cost Node.f is selected as the next trajectory point when expanding the trajectory point; wherein, Node chi .g = Node cur .g + s simu + μ |Node chi .r - Node cur .r|, Node chi .g represents the past history cost of the next trajectory point, at this time, each expandable point of the current trajectory point will be calculated as the next trajectory point; Node cur .g represents the past history cost of the current trajectory point, s simu represents the step length of the search, μ represents the penalty factor of the steering operation, Node chi .r represents the rotation angular velocity of the next trajectory point, Node cur .r represents the rotation angular velocity of the current trajectory point; The future expected cost Node.h of the expansion point is calculated by the heuristic function h, h = max {h nonholonomics ,h collision_avoidance} wherein h nonholonomics represents the path length that complies with the kinematics law of the unmanned ship and ignores obstacle avoidance; h collision_avoidance represents the path length that considers obstacle avoidance and ignores the kinematics law If the termination track point is found to be the expandable node of the current track point during the expansion process, the forward expansion ends, and the sailing track in the track point expansion period is read reversely to obtain the rough running track of the unmanned ship {(x j ,y j ,θ j ,t j )|j=0,1,2,...,N A}.

6. The method of claim 4, wherein, In step 2), constructing an initial solution vector according to the rough running trajectory, and constructing a ship tunnel constraint according to the initial solution vector; comprising: The rough running track of the unmanned ship is resampled to obtain and the estimated time T is expanded to form tst N sam is the total number of the resampled track points; vectors {x m , y m , θ m , u m , v m , r m , τ u_m , τ r_m |m=0, 1, 2, …N sam} are obtained, and the estimated time T tst and the vectors {x m , y m , θ m , u m , v m , r m , τ u_m , τ r_m |m=0, 1, 2, …N sam} are collectively referred to as an initial solution vector. obtaining a rough trajectory of the feature disc center of the unmanned surface vehicle based on the initial solution vector, the center of the feature disc, and the length from the center of the feature disc to the centroid i (t) (i = 1, 2, …, N disc ); comprising: When the unmanned vehicle is moving, the center of the i-th feature disc is taken as the mass point P i ; then the position of the mass point P tst at the n-th sampling moment in the estimated time T i is: wherein, is the x-axis coordinate of the mass point P i at the n-th sampling time; is the y-axis coordinate of the mass point P i at the n-th sampling time; θ n is the attitude angle of the unmanned surface vehicle at the n-th sampling time; x n is the x-axis coordinate of the mass center of the unmanned surface vehicle at the n-th sampling time; y n is the y-axis coordinate of the mass center of the unmanned surface vehicle at the n-th sampling time; with the particle P at each sampling time i constructing local polygons SSC with the particle P at each sampling time i_j obtaining the ship tunnel constraint based on the obtained local polygons SSC i_j obtaining the ship tunnel constraint based on the obtained local polygons SSC 7. The method of claim 6, wherein, The particle P at each sampling time i locally polygonal SSCs i_j based on the obtained locally polygonal SSCs i_j obtaining a ship tunnel constraint; comprising: a) the particle P at the nth sampling time i initializes SSC i_n , and regards SSC i_n as a rectangle with both length and width being 0 at the point, and defines the expansion step Δs and the maximum allowed expansion length L in one direction max , then expands outward along the four edges of the rectangle in the counterclockwise direction with a constant step Δs; in the expansion process, if the generated region does not overlap with the region of obstacles in the expanded grid map, it is determined that the current expansion is valid, and the current expansion region is merged into the rectangle SSC i_n ; otherwise, if it overlaps with the obstacle region, it is determined that the current expansion is invalid, the current expansion region is abandoned, and the expansion in this direction is not continued; in order to avoid infinite expansion, when the length of expansion in any direction exceeds L max , the expansion in this direction is stopped. When the expansion is stopped, the obtained rectangular SSC i_n The range is denoted as Therefore, the row ship tunnel constraint at the nth sampling moment is Wherein, x i_n is the lower limit of the x-axis coordinate of the mass point P i at the nth sampling moment; is the upper limit of the x-axis coordinate of the mass point P i at the nth sampling moment; y i_n is the lower limit of the y-axis coordinate of the mass point P i at the nth sampling moment; is the upper limit of the y-axis coordinate of the mass point P i at the nth sampling moment; b) repeating step a) to obtain the ship tunnel constraint at each sampling time.

8. The method of claim 7, wherein, In step 1), the multi-objective cost function is: J = w1·J1 + w2·J2 + w3·J3 + w4·J4 + w5·J5 wherein J represents a multi-objective cost function; w1, w2, w3, w4 and w5 are all weight coefficients, and are all greater than 0; J1 represents the time that the unmanned ship is expected to complete the navigation task as soon as possible; J1=t f ; J2 represents the center of the ocean current vortex that the unmanned ship is expected to avoid as much as possible; J3 represents the obstacle that the unmanned ship is expected to avoid as much as possible; J4 represents the speed change of the unmanned ship that is expected to be as small as possible, and the acceleration and deceleration operation is expected to be as little as possible; J5 represents the rudder turning speed of the unmanned ship that is expected to be as small as possible, and the rudder turning operation is expected to be as little as possible; wherein the cth vortex center is denotes the distance of the unmanned vehicle's center of mass from the cth vortex center; (x(t),y(t)) is the coordinate of the unmanned vehicle's center of mass; μ c is a penalty coefficient, indicating the degree of punishment for the unmanned vehicle approaching the vortex center; where N obs denotes the number of obstacles; the geometric center of the dth obstacle is d obs_d (t) denotes the distance between the unmanned surface vehicle and the dth obstacle, κ d is a penalty coefficient, indicating the degree of punishment for the unmanned surface vehicle approaching the obstacle; Wherein, λ1 and λ2 represent penalty coefficients; Wherein, λ3 and λ4 represent penalty coefficients; In step 2), the state constraints are: The boundary value constraints are: wherein x init is the x-axis coordinate of the center of mass of the USV at the initial state; y init is the y-axis coordinate of the center of mass of the USV at the initial state; θ init is the attitude angle of the USV at the initial state; u init is the longitudinal velocity of the USV at the initial state; v init is the lateral velocity of the USV at the initial state; r init is the yaw angular velocity of the USV at the initial state; τ u_init is the propulsion force of the USV at the initial state; τ r_init is the yaw moment of the USV at the initial state; x final is the x-axis coordinate of the center of mass of the USV at the final state; y init is the y-axis coordinate of the center of mass of the USV at the final state; θ init is the attitude angle of the USV at the final state; u x_final_min is the minimum value of the x-axis component of the lateral velocity of the USV at the final state; u y_final_min is the minimum value of the y-axis component of the lateral velocity of the USV at the final state; u x_final_max is the maximum value of the x-axis component of the lateral velocity of the USV at the final state; u y_final_max is the maximum value of the y-axis component of the lateral velocity of the USV at the final state; The control input constraints are: where v min is a minimum value of the lateral velocity of the unmanned surface vehicle; v max is a maximum value of the lateral velocity of the unmanned surface vehicle; r min is a minimum value of the yaw rate of the unmanned surface vehicle r max is a maximum value of the yaw rate of the unmanned surface vehicle; τ u_min is a minimum value of the propulsion force of the unmanned surface vehicle; τ u_max is a maximum value of the propulsion force of the unmanned surface vehicle; τ r_min is a minimum value of the yaw moment of the unmanned surface vehicle; τ r_max is a maximum value of the yaw moment of the unmanned surface vehicle.

9. The method of claim 8, wherein, Step 3) comprises: The optimal control proposition is constructed in combination with a multi-target cost function, the control input constraint, the boundary value constraint, the state constraint and the ship tunnel constraint, a first-order explicit discrete method is used to transform the optimal control proposition into a nonlinear programming problem, the nonlinear programming problem and an initial solution vector are input into an IPOPT solver to obtain state quantities and control quantities of the unmanned ship; the state quantities include positions of a mass center of the unmanned ship, attitude angles, longitudinal velocities, lateral velocities and yaw angular velocities, and the control quantities include a propulsion force of the unmanned ship and a yaw moment of the unmanned ship.

Images