Trajectory planning method for unmanned surface vehicle under complex ocean current condition

Abstract

The invention provides a trajectory planning method for an unmanned surface vehicle under a complex ocean current condition, and belongs to the field of unmanned control. Comprising the following steps: acquiring initial state information, end point state information, size information and kinematics parameters of an unmanned ship, and a multi-target cost function; acquiring map information and ocean current information between the starting point and the ending point of the unmanned ship; constructing an unmanned ship dynamic model considering ocean currents, and obtaining state constraints; constructing a rough moving trajectory of the unmanned ship, and obtaining an initial solution vector and a ship sailing tunnel constraint; constructing a boundary value constraint according to the initial state information and the end point state information; constructing a control input constraint according to the size information and the kinematics parameters; an optimal control sequence is obtained, and trajectory planning of the unmanned surface vehicle is completed. According to the method, a trajectory planning algorithm of a double-layer architecture based on numerical optimization is adopted, and a trajectory planning method considering ocean current is adopted, so that the unmanned ship can safely and efficiently run.

Description

Technical Field

[0001] The invention belongs to the field of unmanned control, and in particular relates to a trajectory planning method for an unmanned surface boat under complex ocean current conditions. Background Art

[0002] With the continuous development of intelligent control theory and sensor equipment, and the continuous updating of unmanned control technology, the application of unmanned surface vessels in civil and military fields is growing, and the autonomous navigation of unmanned surface vessels is the key to their successful completion of tasks. However, surface vessels themselves have large mass and poor maneuverability. When they need to pass through the waters around ports with rugged coastlines, they will be more affected by ocean currents in the marine environment. The movement of fluids has a greater impact on the movement of ships. If the impact of ocean currents on ships is not considered in trajectory planning, it is likely that the ship will not be able to sail according to the predetermined trajectory in actual scenarios, which will bring great safety risks.

[0003] At present, the trajectory planning of surface unmanned boats has received extensive attention and research. The existing trajectory algorithms of surface unmanned boats have the following main shortcomings: 1) The current trajectory planning methods of unmanned boats mostly use ant colony algorithms, potential field methods, search methods, etc., which do not consider the physical dynamics of the unmanned boats, and the planned trajectories are difficult to track; 2) Although some methods have considered the dynamics of unmanned boats, they do not consider the impact of ocean currents in the waters on the movement of unmanned boats. However, in actual operation, ocean currents have a great impact on the movement of unmanned boats. If the impact of ocean currents is not considered during planning, it is likely to cause accidents. 3) Compared with path planning in open waters, waters with dense reef islands and waters around ports obviously need the help of unmanned boats more. However, the existing methods achieve collision avoidance by expanding obstacles, which loses a lot of solution space and is not conducive to the navigation of unmanned boats in narrow waters. 4) The trajectory planning process pursues high computing speed and high trajectory quality. However, some methods use the idea of ​​time and space separation to improve the solution speed, sacrificing trajectory quality; some methods use the idea of ​​time and space combination, but due to the existence of collision avoidance requirements, the solution speed is slow, sacrificing high solution speed. Therefore, there is currently no method that can take both into account. Summary of the invention

[0004] In order to solve the problems in the prior art, the present invention proposes a trajectory planning method for an unmanned surface vehicle under complex ocean current conditions.

[0005] The technical solution adopted by the present invention is as follows:

[0006] The present invention discloses a method for planning the trajectory of an unmanned surface vehicle under complex ocean current conditions, comprising the following steps:

[0007] 1) Obtain the initial state information, terminal state information, size information and kinematic parameters of the unmanned boat, obtain the map information and ocean current information between the starting point and the terminal point of the unmanned boat, and obtain the multi-objective cost function of the unmanned boat;

[0008] 2) constructing a dynamic model of the unmanned boat considering ocean currents, and constructing state constraints based on the dynamic model of the unmanned boat; constructing a rough running trajectory of the unmanned boat, constructing an initial solution vector based on the rough running trajectory, and then constructing a ship tunnel constraint based on the initial solution vector; constructing a boundary value constraint based on the initial state information and the terminal state information; and constructing a control input constraint based on the size information and kinematic parameters;

[0009] 3) According to the state constraints, initial solution vector, navigation tunnel constraints, boundary value constraints, multi-objective cost function and control input constraints, the optimal control sequence is obtained to complete the trajectory planning of the surface unmanned vehicle.

[0010] Furthermore, the size information includes the length L of the unmanned boat, the width W of the boat, the length L from the center of mass 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 boat min and the maximum longitudinal velocity u max ; The ocean current information includes the vortex center position, moving speed and intensity coefficient of the ocean current; the initial state information includes the center of mass position, attitude angle, longitudinal speed, lateral speed and yaw angular velocity of the unmanned boat at the initial moment; the terminal state information includes the center of mass position, attitude angle, longitudinal speed, lateral speed and yaw angular velocity of the unmanned boat at the initial moment.

[0011] Compared with the prior art, the present invention has the following beneficial effects:

[0012] The present invention adopts a trajectory planning algorithm based on a two-layer architecture of numerical optimization, which is composed of an upper solver and a lower optimizer. When performing trajectory planning, the initial state information, the terminal state information and the multi-objective cost function of the unmanned boat are first obtained based on user needs, and the map information between the starting point and the terminal point of the unmanned boat, the size information and kinematic parameters of the unmanned boat and the ocean current information are obtained based on the perception module of the unmanned boat; then a matching ocean current model is constructed based on the ocean current information, and an unmanned boat dynamics model considering the ocean current is constructed based on the matching ocean current model; the map information, the size information and kinematic parameters of the unmanned boat and the matching ocean current model are input into the upper solver; the upper solver obtains information and starts, and according to the input information, the matching ocean current model is used to obtain the map information, the size information and kinematic parameters of the unmanned boat and the matching ocean current model. The navigation environment is constructed based on the flow model, and an improved hybrid A* search method is used to search for a rough trajectory. An initial solution vector is constructed based on the trajectory, and the initial solution vector is used as the input of the lower-level optimizer; based on the initial solution vector, a navigation tunnel constraint is constructed; based on the ship dynamics model considering ocean currents, a state constraint is constructed; based on the size information and kinematic parameters of the unmanned boat, a control input constraint is constructed; based on the initial state information and terminal state information of the unmanned boat, a boundary value constraint and a multi-objective cost function are constructed; the lower-level optimizer obtains the boundary value constraint, navigation tunnel constraint, state constraint, control input constraint and initial solution vector, models the trajectory planning task as an optimal control proposition, integrates a nonlinear solver to solve the proposition, and outputs the optimal control sequence.

[0013] The present invention designs an ocean current model and constructs a dynamic model of an unmanned boat under the influence of ocean currents. On the basis of ensuring the solution speed, the accuracy of the solution result is improved, and the problem that the algorithm cannot be used in actual scenarios due to the lack of consideration of environmental factors in the prior art is overcome, so that the unmanned boat can sail more smoothly and economically in the actual environment. The present invention uses a numerical optimization method to optimize the trajectory, overcomes the problem of single planning objectives in the prior art through the design of multi-objective functions, and overcomes the problem of slow solution speed in the existing technology using the same method through the construction of navigation tunnel constraints. Finally, the present invention overcomes the problem that the prior art cannot take into account both the trajectory solution speed and the solution quality, thereby ensuring that the unmanned boat can travel safely and efficiently. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] Figure 1 This is the architecture diagram of the trajectory planning method considering complex ocean current conditions;

[0015] Figure 2 It is a kinematic model diagram of a three-degree-of-freedom unmanned boat;

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

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

[0018] Figure 5 It is a diagram of the multi-feature disk representation method of an unmanned boat;

[0019] Figure 6 It is the collision check diagram between the unmanned boat and the environment;

[0020] Figure 7 It is a graph of the forward expansion method of the search algorithm;

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

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

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

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

[0025] Figure 12 It is a diagram of the rectangular area generation method. DETAILED DESCRIPTION

[0026] The present invention is further described and illustrated below in conjunction with specific embodiments. The embodiments are merely exemplary of the present disclosure and do not define the scope of limitation. The technical features of each embodiment of the present invention may be combined accordingly without conflicting with each other.

[0027] Based on the navigation perception and ocean current prediction of unmanned boats at sea, the present invention proposes a surface unmanned boat trajectory planning method that takes into account complex ocean current conditions, and designs a trajectory planning algorithm with a two-layer architecture based on numerical optimization. The upper layer quickly searches for feasible trajectories, and the lower layer optimizes and solves high-quality trajectories. The method can fully consider the environmental information during navigation, and realize the rapid calculation of safe and comfortable trajectories that conform to the kinematic laws of unmanned boats under the conditions of complex terrain and complex ocean currents. The present invention makes improvements on the search method. Based on the A* algorithm, a new simplified representation method for unmanned boats is proposed, and the time dimension and dynamic influence are included in the search space, which improves the quality of the trajectory without affecting the solution speed. The influence of ocean currents on the kinematics of unmanned boats is studied in depth, and an ocean current model is designed. The dynamic model of unmanned boats under the influence of ocean currents is constructed, and the accuracy of the solution results is improved on the basis of ensuring the solution speed. In view of the slow solution speed caused by collision avoidance constraints, the present invention designs a "ship tunnel constraint" method. Based on the original algorithm framework, the optimal control problem is transformed, the redundant collision avoidance constraints in the optimal control problem are removed, and the trajectory solution speed is improved.

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

[0029] like Figure 1 As shown in the figure, the trajectory planning method adopts a two-layer architecture scheme of decision plus planning to construct and solve the trajectory planning proposition. The algorithm consists of an upper-level solver and a lower-level optimizer. The lower-level optimizer models the trajectory planning task as an optimal control proposition, and integrates a nonlinear solver to solve the proposition and outputs the optimal control sequence. The upper-level solver provides the initial solution vector for the lower-level optimizer, so that the lower-level nonlinear planning solution process can be "hot started" from a high-quality solution vector position.

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

[0031] (1) Based on user needs, the initial state information, terminal state information and multi-objective function (i.e., multi-objective cost function) of the unmanned boat are obtained, and the map information between the starting point and the end point of the unmanned boat, the size information and kinematic parameters of the unmanned boat and the ocean current information are obtained based on the perception module of the unmanned boat; then, a matching ocean current model is constructed based on the ocean current information, and a ship dynamics model considering the ocean current is constructed based on the matching ocean current model (i.e., an unmanned boat dynamics model considering the ocean current); the map information, the size information and kinematic parameters of the unmanned boat and the matching ocean current model are input into the upper solver;

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

[0033] (3) The lower optimizer establishes the optimal control proposition, as follows: Based on the initial solution vector, a "ship tunnel constraint" is constructed; based on the ship dynamics model considering ocean currents, a state constraint (i.e., the system state constraint in the figure) is constructed; based on the mechanical properties of the unmanned boat (i.e., the size information and kinematic parameters of the unmanned boat), a control input constraint is constructed; based on user requirements (i.e., the initial state information and terminal state information of the unmanned boat), a boundary value constraint and a multi-objective cost function are constructed;

[0034] (4) The lower 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, obtain the numerical optimal solution to the optimal control problem, obtain the optimal control sequence, and output the optimal control sequence to the downstream control actuator to achieve autonomous navigation.

[0036] The present invention proposes a surface unmanned vehicle dynamics model that takes into account the influence of water currents, and integrates the influence of ocean currents on the unmanned vehicle into the dynamic constraints to ensure that the planned trajectory can be accurately tracked in an environment where real ocean currents exist.

[0037] For an unmanned boat sailing on the water surface, six independent coordinates are required to determine its position and posture. However, when modeling the trajectory planning task proposition, we only need to care about the changes in the heading angle and the navigation trajectory, that is, the movement of the ship in the horizontal plane. Considering that the research object is a surface unmanned boat, unlike container ships, roll-on / roll-off ships, high-speed warships and other ships with a high center of gravity, when it sails in the ocean, wind and wave interference and rotational motion will not cause a large roll angle. Therefore, when constructing the proposition, the influence of heave, pitch and roll motion can be ignored, and the six-degree-of-freedom unmanned boat motion can be simplified to a three-degree-of-freedom unmanned boat motion problem.

[0038] When constructing the dynamic model of the unmanned boat in the present invention, the model follows the following assumptions:

[0039] (1) The unmanned boat sails on a horizontal plane, ignoring the effects of heave, pitch and roll motions;

[0040] (2) Ignore the impact of wind and wave forces on the movement of unmanned boats;

[0041] (3) The unmanned boat sails in a uniform flow field;

[0042] (4) The unmanned boat is a bilaterally symmetrical rigid body and is small enough to be regarded as a point mass in the ocean;

[0043] (5) The unmanned boat has no lateral thrusters and only generates power along the bow direction

[0044] like Figure 2 As shown, the inertial coordinate system {n} = (x n ,y n ,z n ) is an inertial coordinate system fixed on the surface of the earth, taken as the reference system, and x is specified n The y axis points due east. n The axis points due north, z n The axis points to the center of the earth, and the unit vectors on each axis are recorded as Body-fixed coordinate system {b} = (x b ,y b ,z b ) is the origin located at a specified point O in the unmanned boat b The attached coordinate system on the b The y axis points to the bow. b The axis points to port, z b The axis points to the keel, and the unit vectors on each axis are recorded as The body-fixed coordinate system {b} moves arbitrarily in space with the unmanned boat and is a non-inertial coordinate system.

[0045] According to the unmanned boat dynamics model, the motion process of the unmanned boat in the inertial coordinate system {n} is constrained by the following differential equations:

[0046]

[0047] Where t is time, t∈[0,t f ], the end time of the unmanned boat movement t f is the variable to be determined; θ(t) is the attitude angle of the unmanned boat at time t; [x(t), y(t)] is the center of mass of the unmanned boat at time t; u(t) is the position of the unmanned boat 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 boat relative to the water flow along the y axis at time t. b The speed of the axis (i.e. v(t)d 3 is the lateral speed); r(t) is the speed at which the unmanned ship moves around z at time t. b Angular velocity of axis rotation (i.e. r(t) is the yaw angular velocity); m 1 , m 2 , m 3 is the inertial mass considering the additional mass effect, all of which are preset; u cur_east[(x(t), y(t), t)] is the component of the ocean current velocity along the x-axis at time t; u n cur_north [(x(t), y(t), t)] is the component of the ocean current velocity along the y-axis at time t; d n 1 , d 2 , are the hydrodynamic damping coefficients for u(t), v(t), and r(t) respectively; τ u (t) is the propulsion force of the unmanned boat; τ r (t) is the yaw moment of the unmanned boat.

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

[0049] According to assumption (3), the unmanned boat sails in a uniform flow. The influence of the flow on a ship sailing at sea depends on the nature of the flow. When sailing in a uniform flow, the flow does not generate power but only causes the ship to make kinematic drifts with the flow. As Figure 3 shown, which shows the influence of the ocean current on the absolute velocity of the unmanned boat. According to the geometric relationship, the first three terms of Equation (1) can be deduced.

[0050] According to the acceleration synthesis 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 respect to a pole and the acceleration when making a fixed-point motion around the pole", the acceleration of the center of mass M of the surface unmanned boat can be deduced, and the dynamic equation can be established using Newton's second law.

[0051] As Figure 2 shown, it has been set above that are the unit vectors along the coordinate axes of the body-fixed coordinate system {b}; M is the center of mass of the surface unmanned boat; the pole O b coincides with the center of mass M; the velocity of the pole O b is u, v respectively represent the velocity of the pole O b in the x b axis and the y b axis; r is the angular velocity of the surface unmanned boat rotating around the pole O b .

[0052] The velocity of the pole O b can be expressed as:

[0053]

[0054] Among them, is the velocity of the unmanned boat moving along the bow; is the velocity of the unmanned boat moving along the starboard;

[0055] ​​​​Pole O b 's acceleration is:

[0056]

[0057] wherein, is the velocity of pole O b 's first derivative of the component on the x axis; r is the angular velocity of the unmanned boat rotating around the z b axis; b axis;

[0058] Since the pole O b coincides with the center of mass M, it can be deduced that the velocity of the center of mass M moving as a fixed point around the pole O b is: is:

[0059]

[0060] The acceleration of the center of mass M moving as a fixed point around the pole O b is: is:

[0061]

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

[0063]

[0064] Using a x , a y to represent the components of the acceleration in the x b and y b axis directions respectively:

[0065]

[0066] Using to represent the total external moment applied to the unmanned boat. Given a x , a y and the pole O b 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", it can be known that:

[0067]

[0068] is the total external force on the unmanned boat in the x b axis direction, is the total external force on the unmanned boat in the y b axis direction:

[0069]

[0070] in, and They represent the unmanned boat being subjected to x b Axis direction and y b Hydrodynamic force in the axial direction; Indicates that the unmanned boat is under b Hydrodynamic moment in the axial direction; They represent the unmanned boat being subjected to x b Axis direction, y b The hydrostatic force in the axial direction and the z b Hydrostatic moment about the axis; They represent the unmanned boat being subjected to x b Axis direction, y b The wind force in the axial direction and z b Wind moment in the axial direction; It means that the unmanned boat is at x b Axis direction, y b The main force in the axial direction and z b The reaction force of the yaw moment in the axis direction.

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

[0072]

[0073] Assuming that the hydrodynamic forces and moments on the rigid body can be linearly superimposed, the hydrodynamic forces can be derived from the fluid dynamics mass-damper-spring system as follows:

[0074]

[0075] Among them, τ hyd represents the hydrodynamic matrix of the unmanned boat; M A represents the additional mass matrix generated by the inertia of the surrounding fluid when the unmanned boat moves; v r represents the velocity matrix of the unmanned boat relative to the water flow; C A (v r ) represents the Coriolis force and centripetal force matrix caused by the additional mass of the unmanned boat, which is generated by the rotation of the body-fixed coordinate system {b} relative to the inertial coordinate system {n}; D(v r ) represents the total hydrodynamic damping matrix. Under the premise of the present invention, it is specifically expressed as follows:

[0076]

[0077] Among them, in formula (15) They represent the unmanned boat along x b Axis direction, y b The acceleration in the z-axis direction and b Angular acceleration along the x-axis direction; SNAME (1950) (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.) is used in equations (13) and (17) to indicate the signs of the fluid dynamics derivatives. For example, b The acceleration in the axial direction The induced hydrodynamic additional mass force X is expressed as follows:

[0078]

[0079] According to formula (9), formula (10) and formula (11), we can deduce:

[0080]

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

[0082]

[0083] Among them, m g is the mass of the unmanned boat, d 1 =X u , d 2 =Y v , d 3 =N r , and rearrange equation (20), we can deduce the last three items in equation (1).

[0084] The formation of ocean currents is very complex. If you want to accurately represent the time-varying ocean current model, you need a large number of complex equations and data observation results. When studying the trajectory planning problem of unmanned boats, you need to comprehensively consider the accuracy of the calculation results and the real-time nature of the solution. Therefore, after simplifying the complex model of the ocean current, you can still accurately describe the impact on the movement of the unmanned boat. The present invention relates to a simplified time-varying ocean current model that can be used to match ocean current information.

[0085] As mentioned above, under the condition of the dynamic model assumption (3), the absolute navigation speed of the unmanned boat in the uniform flow field will be affected by the ocean current, and in the uniform flow field, the flow force does not generate power for the hull, but only causes the ship to drift kinematically with the fluid. Therefore, the ocean current model that needs to be constructed can only consider the speed effect. In order to better reflect the impact of ocean currents on the movement of unmanned boats, the method of superimposing randomly distributed vortex fields is selected 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) respectively represent the vortex center Q(t) at x n Axis, y n The coordinate of the axis; x Q_init ,y Q_init They represent the initial positions of the vortex center respectively; The vortex center is at x n ,y n The moving speed in the axial direction; a(t) represents the intensity of the vortex field, a x (t), a y (t) represents the vortex field along x n ,y n Strength coefficient in the axial direction; a x_init 、a y_init They represent the vortex field along x n ,y n Initial strength coefficient in the axial direction; Then they represent the vortex field along x n ,y n The speed of change of the intensity coefficient in the axial direction; s(x,y,t) is the distance between the current position and the vortex center; v x_cur 、v y_cur They represent the ocean currents along x n ,y n The magnitude of the axis velocity; f represents the direction of rotation of the vortex field, and its positive or negative value determines the direction of rotation of the vortex field. If f is positive, the vortex field rotates clockwise, and if f is negative, the vortex field rotates counterclockwise; sgn() is the sign function.

[0088] The ocean current formula is as follows:

[0089]

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

[0091] The ocean current model established according to formula (22) is shown as follows: Figure 4 As shown, the direction of the arrow indicates the speed direction of the ocean current at this moment, and the size of the arrow indicates the size of the flow speed.

[0092] The current position of the unmanned boat can be determined based on the map information. Then, based on the current position of the unmanned boat, the ocean current map and the ocean current model can be used to determine the current position of the unmanned boat.

[0093] u cur_east [(x(t),y(t),t)] and u cur_north [(x(t),y(t),t)], and then the obtained u cur_east [(x(t),y(t),t)] and u cur_north Substitute [(x(t), y(t), t)] into the unmanned boat dynamics model considering ocean currents to obtain the unmanned boat dynamics model considering ocean currents at the current position. That is, the flow field generated by the vortex is the ocean current, so at each point (x, y) there is a corresponding vector, and u in the dynamics model cur_east [(x(t),y(t),t)] and u cur_north [(x(t),y(t),t)] is the east and north components of this vector.

[0094] When dealing with the problem of autonomous parking trajectory planning for vehicles, one or two characteristic disks are generally used to represent the vehicle according to the size of the vehicle and the parking space. However, considering the slender shape of the unmanned boat, it is not reasonable to use one or two characteristic disks to represent it. Therefore, the present invention proposes a multi-characteristic disk representation method for the unmanned boat.

[0095] like Figure 5 As shown, a rectangle with a length of L and a width of W covers the hull of the unmanned boat, with the center of mass being M(x, y), L f With L r First, take a square with a side length of W at the bow and stern, and set the radius R of the characteristic disk to disc for:

[0096]

[0097] It can be inferred that the area not covered by the square is L-2W in the longitudinal direction (the length direction of the unmanned boat). In this area, N disc -2 characteristic discs:

[0098]

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

[0100]

[0101] The entire unmanned boat can be used N disc feature disks cover each other, and any two adjacent feature disks overlap each other. The center of the i-th feature disk is

[0102]

[0103] Where, θ represents the attitude angle of the unmanned boat; l i Represents the length of the i-th characteristic disk from the center of mass:

[0104]

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

[0106] Before performing the search task, the upper solver first divides the two-dimensional plane area where the trajectory planning task is located into grids and constructs a grid map. According to the simplified representation method mentioned above, the condition of "the vehicle body does not collide with obstacles" can be converted into "all characteristic disks do not collide with obstacles". The present invention shrinks the characteristic disk toward the center of the circle R disc , expand all obstacles in the environment R disc , transforming "all characteristic disks do not collide with obstacles" into "particle P i (i=1,2,…,N disc ) do not collide with the expanded obstacles. Figure 6 As shown, it is possible to make the particle P i (i=1,2,…,N disc ) Trajectories that do not collide in the puffed area must comply with the collision avoidance constraints in the original scene.

[0107] The present invention designs a four-dimensional hybrid A* global trajectory search algorithm that takes ocean currents into consideration. The upper solver uses the algorithm to solve the starting solution vector of the lower optimizer, i.e., the initial solution. The present invention can obtain an initial solution vector with global optimality under the premise of ensuring the real-time calculation of the upper solver, so that the lower optimizer can "start" from a high-quality solution vector position when solving the NLP problem.

[0108] The task of the search phase is to quickly search for a collision-free and cost-minimum rough global trajectory connecting the initial state and the terminal state in the state space of the unmanned boat. The A* search algorithm currently widely used in academia and industry does not fully consider the kinematic model of the ship. Although the rough path obtained by A* search can be used for decision-making, it is not conducive to the subsequent NLP solution. Therefore, based on A*, the present invention proposes a four-dimensional hybrid A* search algorithm that considers ocean currents.

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

[0110] The acyclic four-dimensional connected graph, the starting node (starting trajectory point) Node start , End Node (End Track Point) end Input trajectory solver, starting from the starting node Node start Start traversing by forward expansion. The forward expansion method is as follows. For example, the current node Node cur The position of the unmanned boat is [x cur ,y cur ,θ cur ], according to the unmanned boat dynamics model considering ocean currents, τ u ,τ r Sampling is a constant value τ u_cur ,τ r_cur , τ u is the propulsion force of the unmanned boat; τ r is the yaw moment of the unmanned boat; simulate the unmanned boat in unit time [x cur ,y cur ,θ cur ]’s initial pose, [τ u_cur ,τ r_cur ], and under the assumption of uniform speed, the influence of the ocean current on the motion of the unmanned boat is considered, so as to generate a navigation path that conforms to the kinematic law of the unmanned boat under the influence of the ocean current. The corresponding position at the end of the path is [x chi ,y chi ,θ chi ] Corresponding child node Node chi In addition, if τ u (t),τ r (t) is used as the control variable. On the one hand, for unmanned boats with different inertial parameters, its value will have a large deviation; on the other hand, it takes a lot of time to obtain the next state. Therefore, the longitudinal speed (the speed of the unmanned boat relative to the water flow along the x direction) is selected.b The velocity of the axis) u(t) and the yaw angular velocity (the unmanned ship rotates around z b The angular velocity of the axis rotation) r(t) is taken as the control variable, and only the simple kinematic model shown in equation (28) is considered. In order to speed up the calculation, the lateral velocity of the unmanned boat is ignored and the lateral velocity v(t) is set to 0.

[0111]

[0112] In the marine navigation environment, backward motion is basically not considered, and in engineering, sufficient adjustment space needs to be left for control. Therefore, in u(t)∈{u min ,u max},r(t)∈{r min ,r max} condition, where u max is the maximum longitudinal velocity; u min is the minimum value of longitudinal velocity; r max is the maximum value of the yaw angular velocity; r min is the minimum value of the yaw angular velocity; set the forward expansion 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 Figure 7 shown.

[0115] If the termination node Node is found during the expansion process end It happens to be the current node (current trajectory point) cur If there is an expandable node, the search is completed. Otherwise, the point in the expandable point set that is most likely to have a direct or indirect connection with the terminal node is selected, and the data during the expansion process is recorded, including the total cost Node.f, the past historical cost Node.g, the future expected 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 position Node.xcur 、Node.y cur 、Node.θ cur 、Current time Node.t cur , from which u, r is extended, the navigation trajectory during the extended period {(x i ,y i ,θ i ,t i )|i=0,1,2,…,N f}, where N f N is the number of points involved in the expansion process from the current expansion point to the next expansion point; that is, the entire process from the current point to the next expansion point is a forward deduction process, that is, there will be a series of points in this deduction process. f Refers to this series of points. For example, starting from (0, 0), moving forward at a fixed speed and yaw angle, recording every 0.1s, and then recording N f , and arrived at (1,1).

[0116] The total cost Node.f, the past history cost Node.g, and the future expected cost Node.h are used to determine whether the next expandable point is the point in the expandable node set that is most likely to have a direct or indirect connection with the terminal node, that is, to find the expandable point with the smallest total cost Node.f. The past history cost function g imposes a penalty on undesirable trajectories, so that undesirable trajectory fragments have a larger past history cost value, thereby calculating a more reasonable and smooth trajectory. For example, considering that frequent steering operations will increase navigation time and cause energy waste, this system sets the past history 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. At this time, each expandable point of the current trajectory point will be used as the next trajectory point to calculate the past historical cost) and the current node (the current trajectory point). s simu represents the search step size, μ represents the penalty factor for steering operation, and Node chi .r、Node cur .r represents the angular velocity of the child node and the current node respectively.

[0119] In addition, the future expected cost Node.h is calculated by the heuristic function h, which affects the search quality and efficiency. Setting a reasonable heuristic function h is conducive to accurately estimating the shortest obstacle avoidance path length from the child node to the terminal node. nonholonomics With h collision_avoidance To define the heuristic function h:

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

[0121] Among them, h nonholonomics It represents the length of the path that complies with the kinematic law of the unmanned boat and ignores obstacle avoidance. The kinematic law of the unmanned boat is analyzed and compared with the kinematic law of the vehicle. This study uses the Reeds-Shepp curve to estimate its value; h collision_avoidance It represents the path length that takes obstacle avoidance into account while ignoring the kinematic laws, and is represented by the larger value of the path length obtained by the classic A* algorithm in a two-dimensional grid and the Manhattan distance.

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

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

[0124] Current poseNode.x cur 、Node.y cur 、Node.θ cur 、Current time Node.t cur Used to record the status information of the extension point so that it can continue to expand forward.

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

[0126] Repeat the above expansion operation until the end node is found, and then read the navigation trajectory of each trajectory point in the expansion period in reverse to obtain a complete trajectory (i.e., the rough running trajectory of the unmanned boat) {(x j ,y j ,θ j ,t j )j=0,1,2,...,N A},N A is the total number of trajectory points in the 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 expanding the trajectory tst , N sam is the total number of trajectory points after resampling (assuming N A is 200, then N sam can be 100), where N is defined sam +1 discretized node evenly distributed over the estimated sailing time range. In the resampling process, the longitudinal velocity of the unmanned boat {u m |m=0,1,2,...,N sam} and yaw angular velocity {r m |m=0,1,2,...,N sam}, considering that in general, the lateral speed during the entire navigation process is small, it is set to {v m ≡0|m=0,1,2,...,N sam}, and according to the kinematic model of the unmanned boat, {τ u_m |m=0,1,2,…,N sam}, {τ r_m |m=0,1,2,…N sam}, recombining the above collocation point sequence 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} are collectively called the initial solution vector.

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

[0128] At present, the mainstream method for judging whether a particle is in the obstacle area is the triangular area method, such as Figure 9 As shown in , 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 area. However, this method will bring a large number of non-convex and nonlinear constraints, which is very unfavorable for the rapid solution of the trajectory.

[0129]

[0130] in, For triangle ΔPQ n Q 1 area; ΔPQ k Q k+1 area; for Q 1 Q 2 ...Q n area.

[0131] Therefore, the present invention proposes a method, which uses the multi-feature disk representation method of the unmanned boat proposed above to construct a dedicated navigation tunnel for each particle, that is, to simplify the collision avoidance constraint through the "navigation tunnel constraint", thereby reducing a large number of nonlinear constraints in the NLP problem. Figure 10 As shown in the figure, if the tunnel constraint is not adopted, both route 1 and route 2 satisfy the collision avoidance constraint conditions. However, when the given initial solution is of the same type as route 1, route 2 will not be considered in the optimization process. In this case, the setting of the constraint can obviously completely exclude route 2, thereby improving the solution speed. Through the "ship tunnel constraint" method proposed in the present invention, as long as the center of the characteristic disk at each moment is within its corresponding local range, it can be ensured that the unmanned boat does not collide with obstacles.

[0132] The construction method of the ship tunnel is as follows:

[0133] The vector {x 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 center of mass of the unmanned boat, and the variables {x m ,y m ,θ m |m=0,1,2,…N sam Combining equations (26) and (27), the center P of each characteristic disk used to represent the hull of the unmanned boat can be determined separately. i (i=1,2,…,N disc ) of the rough trajectory traj i (t)(i=1,2,...,N disc ). Based on this, P i (i=1,2,…,N disc ) to construct the corresponding ship tunnel.

[0134] For ease of description, we choose the center P of the i-th characteristic disk i The tunnel construction method is described in detail using the example. First, P is determined based on the initial solution vector constructed by the upper solver. i The rough trajectory traj i (t), assuming P i The tunnel is made of N box local polygonal regions, in N box =N sam In the case of 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 moment, the center of the characteristic disk P i The location is:

[0135]

[0136] in, is the nth sampling moment, particle P i The x-axis coordinate of is the nth sampling moment, particle P i The y-axis coordinate of n is the attitude angle of the unmanned boat at the nth sampling moment; x n is the x-axis coordinate of the center of mass of the unmanned boat at the nth sampling moment; n is the y-axis coordinate of the center of mass of the unmanned boat at the nth sampling moment.

[0137] Then, at each sampling moment, the particle P i A local polygon is constructed as the center, that is, a local safety area. Considering that the purpose of setting this area is to improve the speed of solving the NLP problem, the present invention chooses to use a horizontal and vertical rectangle as the local safety area, such as Figure 11 shown.

[0138] The present invention converts P at the nth sampling moment i (i.e. P i_n ) is represented by the matrix area corresponding to SSC i_n Now let’s introduce how to build SSC i_n First, initialize the SSC i_n At particle P i_n regard it as A rectangle with length and width of 0 at the position, and define the expansion step Δs and the maximum allowable expansion length L in one direction max Then, follow the direction list [π / 2,π,3π / 2,2π], that is, follow the four sides of the rectangle in a counterclockwise direction and continue to expand outward with a constant step size Δs. During the expansion process, if the generated area does not overlap with the obstacle area in the expanded grid map, the current expansion is considered valid and the current expanded area is merged into the rectangle SSC i_n On the contrary, if it overlaps with the obstacle area, the current expansion is considered invalid, the current expansion area is abandoned and no further expansion is performed in this direction; in addition, in order to avoid infinite expansion, when the length of any direction of expansion exceeds L max When , the expansion in this direction stops. The expansion process is as follows Figure 12 shown.

[0139] When the expansion process stops, the obtained rectangle SSC j_i The range is recorded as Therefore, the ship tunnel constraint is Therefore, we only need to ensure that P i As long as the matrix falls into the corresponding matrix area at any discrete moment, it can be guaranteed that no collision will occur, and no complex collision avoidance constraints are required. sam +1) times, we can get a series of SSCs, which are combined into particle P i tunnel; where x i_n is the nth sampling moment, particle P i The lower limit of the x-axis coordinate; is the nth sampling moment, particle P i The upper limit of the x-axis coordinate; y i_n is the nth sampling moment, particle P i The lower limit of the y-axis coordinate; is the nth sampling moment, particle Pi The upper limit of the y-axis coordinate.

[0140] Figure 1 The (system) state constraint in is:

[0141] During the movement of the unmanned boat, its basic movement law needs to be followed all the time, that is, the system state constraint needs to be satisfied. The system state constraint is obtained according to the kinematic model proposed by the present invention:

[0142]

[0143] Among them, the state vector and the control vector are:

[0144]

[0145] Due to the mechanical characteristics of the unmanned boat, there are certain limitations on its state and control variables. Therefore, it is necessary to describe the mechanical characteristics of the unmanned boat movement by applying boundary conditions. Therefore Figure 1 The control input constraint in is:

[0146]

[0147] Among them, v min is the minimum value of the lateral speed of the unmanned boat; v max is the maximum value of the lateral speed of the unmanned boat; r min is the minimum value of the yaw angular velocity of the unmanned boat r max is the maximum value of the yaw angular velocity of the unmanned boat; τ u_min is the minimum value of the propulsion force of the unmanned boat; τ u_max is the maximum value of the propulsion force of the unmanned boat; τ r_min is the minimum value of the yaw moment of the unmanned boat; τ r_max is the maximum value of the yaw moment of the unmanned boat.

[0148] In addition, affected by the mechanical characteristics of the unmanned boat, it can only generate a yaw moment when the longitudinal speed is non-zero during the movement. To describe this limitation, the following constraint condition is added:

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

[0150] Among them, λ > 0 is the dimension conversion factor.

[0151] Figure 1 The boundary value constraint in is:

[0152] The initial and final poses, movement states and control quantities of the surface unmanned boat should satisfy the following constraint conditions:

[0153]

[0154] Among them, [x init ,y init ,θ init ,u init ,v init ,r init ,τ u_init ,τ r_init ] corresponds to the objective actual state of motion at the current moment (t=0) recorded and calculated by the sensor. final ,y final ,sin(θ final ),cos(θ final )]、[u x_final_min ,u y_final_min ]、[u x_final_max ,u y_final_max ] indicates that at the end time (t = t f )'s motion state restrictions. That is, x init is the x-axis coordinate of the center of mass of the unmanned boat in the initial state; init is the y-axis coordinate of the center of mass of the unmanned boat in the initial state; θ init The attitude angle of the unmanned boat in the initial state; u init is the longitudinal velocity of the unmanned boat in the initial state; v init The lateral speed of the unmanned boat in the initial state; r init is the yaw angular velocity of the unmanned boat in the initial state; τ u_init The propulsion force of the unmanned boat in the initial state; τ r_init When it is the initial state, the yaw moment of the unmanned boat; final When it is the terminal state, the x-axis coordinate of the center of mass of the unmanned boat; y init When it is the terminal state, the y-axis coordinate of the center of mass of the unmanned boat; θ init When it is the terminal state, the attitude angle of the unmanned boat; u x_final_min When it is the terminal state, the minimum value of the x-axis component of the lateral velocity of the unmanned boat; u y_final_min When it is the terminal state, the minimum value of the y-axis component of the lateral velocity of the unmanned boat; u x_final_max When it is the terminal state, the maximum value of the x-axis component of the lateral velocity of the unmanned boat; u y_final_max The maximum value of the y-axis component of the lateral velocity of the unmanned boat when it is in the terminal state.

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

[0156] J=w 1 ·J 1 +w 2 ·J 2 +w 3 ·J 3 +w 4 ·J 4 +w 5 ·J 5

[0157] Where J represents the multi-objective cost function; w 1 、w 2 、w 3 、w 4 and w 5 are weight coefficients, and are all greater than 0; J 1 J expressed his hope that the unmanned boat could complete its navigation mission as soon as possible. 1 =t f ; J 2 He expressed the hope that the unmanned boat could avoid the center of the ocean current vortex as much as possible; J 3 He expressed the hope that the unmanned boat could avoid obstacles such as reefs and islands as much as possible; J 4 He expressed his expectation that the speed change of the unmanned boat could be as small as possible, and the acceleration and deceleration operations could be as few as possible; J 5 They expressed their hope that the unmanned boat's steering speed would be as slow as possible and the steering operations would be as few as possible.

[0158]

[0159] Among them, N eddy represents the number of vortex fields, assuming that the center of the cth vortex is It is used to indicate the distance between the unmanned boat and the center of the cth vortex. (x(t), y(t)) is the coordinate of the center of mass of the unmanned boat; μ c As a penalty coefficient, it indicates the degree of penalty for the unmanned boat approaching the center of the vortex.

[0160]

[0161] Among them, N obs represents the number of obstacles. Assume that the geometric center of the dth obstacle is d obs_d(t) represents the distance between the unmanned boat and the dth obstacle, κ d As a penalty coefficient, it indicates the degree of penalty for the unmanned boat approaching an obstacle.

[0162]

[0163] Among them, λ 1 With λ 2 Represents the penalty coefficient, which indicates the degree of penalty for the above two manipulation situations.

[0164]

[0165] Among them, λ 3 With λ 4 Represents the penalty coefficient, which indicates the degree of penalty for the above two manipulation situations.

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

[0167] Step 1: Receive the unmanned boat size information and the length L from the center of mass to the bow of the unmanned boat f , Length from center of mass to stern L r , hull width W and maximum longitudinal speed u of the unmanned boat max , minimum longitudinal speed u min etc., and sent them to the upper solver and the unmanned boat dynamics model considering the ocean current, to construct the control input constraints and send them to the lower optimizer;

[0168] Step 2: Receive the current navigation status parameter x of the unmanned boat init ,y init ,θ init And the target state parameter x final ,y final ,θ final , sent to the upper solver, construct boundary value constraints, and sent to the lower optimizer;

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

[0170] Step 4: Receive ocean current information and vortex center position x Q (t), y Q (t), moving speed Strength coefficient a x_init 、a y_init Etc. are sent to the ocean current model, the ocean current model is constructed using formula (22), and sent to the unmanned boat dynamics model considering the ocean current;

[0171] Step 5: According to formula (1), the unmanned boat dynamics model considering the ocean current is constructed, the system state constraints are constructed, and sent to the lower optimizer;

[0172] Step 6: The upper solver completes initialization based on the input information and configuration parameters, implements rough trajectory planning using the four-dimensional hybrid A* global trajectory search algorithm that considers ocean currents, and constructs an initial solution vector based on the control parameter configuration file and sends it to the lower optimizer;

[0173] Step 7: The upper solver uses the initial solution vector and combines equations (26) and (27) to obtain the rough trajectory traj of all characteristic disks i (t)(i=1,2,…,N disc ), input the rough trajectory into the SSCs solver;

[0174] Step 8: SSCs are solved cyclically to obtain the tunnel of each rough trajectory And construct the ship tunnel constraints and send them to the lower-level optimizer;

[0175] Step 9: The lower optimizer configures a multi-objective cost function, combines the input control input constraints, boundary value constraints, system state constraints, and navigation tunnel constraints to construct an optimal control proposition, uses a first-order explicit discrete method to transform it into a nonlinear programming problem, and inputs the initial solution vector into the IPOPT solver to obtain the state and control quantities of the unmanned boat, and sends them to the downstream control actuator. The state quantities include the position of the center of mass of the unmanned boat, attitude angle, longitudinal velocity, lateral velocity, and yaw angular velocity, and the control quantities include the propulsion force of the unmanned boat and the yaw moment of the unmanned boat.

[0176] The above-mentioned embodiments only express several implementation methods of the present invention, and the description is relatively specific and detailed, but it cannot be understood as limiting the scope of the present invention. For ordinary technicians in this field, several modifications and improvements can be made without departing from the concept of the present invention, which all belong to the protection scope of the present invention.

Claims

1. A method for trajectory planning of an unmanned surface vehicle under complex ocean current conditions, characterized in that: The following steps are involved: 1) Obtain the initial state information, terminal state information, size information and kinematic parameters of the unmanned boat, obtain the map information and ocean current information between the starting point and the terminal point of the unmanned boat, and obtain the multi-objective cost function of the unmanned boat; 2) constructing a dynamic model of the unmanned boat considering ocean currents, and constructing state constraints based on the dynamic model of the unmanned boat; constructing a rough running trajectory of the unmanned boat, constructing an initial solution vector based on the rough running trajectory, and then constructing a ship tunnel constraint based on the initial solution vector; constructing a boundary value constraint based on the initial state information and the terminal state information; and constructing a control input constraint based on the size information and kinematic parameters; 3) According to the state constraints, initial solution vector, navigation tunnel constraints, boundary value constraints, multi-objective cost function and control input constraints, the optimal control sequence is obtained to complete the trajectory planning of the surface unmanned vehicle.

2. The method for trajectory planning of an unmanned surface vehicle according to claim 1, characterized in that: In step 1), the size information includes the length L of the unmanned boat, the width W of the boat, the length L from the center of mass 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 boat min and the maximum longitudinal velocity u max ; The ocean current information includes the vortex center position, moving speed and intensity coefficient of the ocean current; the initial state information includes the center of mass position, attitude angle, longitudinal speed, lateral speed and yaw angular velocity of the unmanned boat at the initial moment; the terminal state information includes the center of mass position, attitude angle, longitudinal speed, lateral speed and yaw angular velocity of the unmanned boat at the initial moment.

3. The method for trajectory planning of an unmanned surface vehicle according to claim 2, characterized in that: In step 2), the construction of the unmanned boat dynamics model considering ocean currents includes: Firstly, an ocean current model is constructed based on the ocean current information, and then a dynamic model of the unmanned boat is constructed according to the ocean current model, map information, size information and kinematic parameters of the unmanned boat; The formula of the ocean current model is: Where Γ represents the flow field and generated by the vortex; N eddy represents the number of vortex fields; rand represents the random function; Q a (t) is the vortex center position of the ath vortex field; a a (t) is the intensity coefficient of the ath vortex field; f a is the rotation direction of the ath vortex field; The formula of the unmanned boat dynamics model considering ocean currents is: Where t is time, t∈[0,t f ], t f is the end time of the unmanned boat movement; [x(t), y(t)] is the center of mass of the unmanned boat at time t; θ(t) is the attitude angle of the unmanned boat at time t; u(t) is the position of the unmanned boat relative to the water flow along x at time t b The speed of the axis, that is, the longitudinal speed; x b The origin is located at a preset point O in the unmanned boat b The coordinate axis of the attached coordinate system, x b The axis points to the bow of the unmanned boat; v(t) is the time when the unmanned boat moves along the y axis relative to the water flow. b The speed of the axis, that is, the lateral speed; y b The origin is located at a preset point O in the unmanned boat b The coordinate axis of the attached coordinate system, y b The axis points to the starboard side of the unmanned boat; r(t) is the time when the unmanned boat revolves around z b The angular velocity of the axis rotation, i.e. the yaw angular velocity; z b The origin is located at a preset point O in the unmanned boat b The coordinate axis of the attached coordinate system, z b The axis points to the keel of the unmanned boat; cur_east [(x(t),y(t),t)] is the time when the ocean current velocity is along x n The component of the axis; x n is one axis of the inertial coordinate system fixed to the surface of the earth, x n The axis points due east; u cur_north [(x(t),y(t),t)] is the ocean current velocity along y n Component of the axis; y n is one axis of the inertial coordinate system fixed to the surface of the earth, y n The axis points to the north; m1, m2 and m3 are the preset inertial masses considering the added 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 propulsion force of the unmanned boat at time t; τ r (t) is the yaw moment of the unmanned boat at time t.

4. The method for trajectory planning of an unmanned surface vehicle according to claim 3, characterized in that: In step 2), the construction of a rough running track of the unmanned boat includes: 2.1) Construct a grid map and expand all obstacles in the grid map to obtain an expanded grid map; then add two dimensions, θ and t, to the expanded grid map to obtain a directed acyclic four-dimensional connected graph; 2.2) Get the starting point Node of the unmanned boat's navigation trajectory start , End track point Node end , from the starting trajectory point Node start Start to expand all the track points on the navigation track of the unmanned boat by forward expansion. When the track point is reached, read each track point in reverse to obtain the rough running track of the unmanned boat {(x j ,y j ,θ j ,t j )j=0,1,2,…,N A }; where N A is the total number of trajectory points in the rough running trajectory.

5. The method for trajectory planning of an unmanned surface vehicle according to claim 4, characterized in that: In step 2.1), the method for obtaining the expanded grid map includes: First, a rectangle with a length of L and a width of W is used to cover the hull of the unmanned boat. A square with a side length of W is taken at the bow and stern respectively, and a characteristic disk is constructed based on the square. The radius R of the characteristic disk is set disc for Therefore, in the length direction of the unmanned boat, the length of the area not covered by the square is L-2W. In this area, N disc - 2 characteristic discs, Among them, N disc is the total number of characteristic disks arranged on the unmanned boat; ceil() represents the upward rounding function; The length Δl that each characteristic disk independently covers in the length direction of the unmanned boat is: The center of the i-th characteristic disk for: Among them, l i represents the distance from the center of the i-th characteristic disk to the center of mass; The two-dimensional plane area where the unmanned boat is located during trajectory planning is divided into grids, a grid map is constructed, and all obstacles in the grid map are expanded R disc , and get the expanded grid map.

6. The method for trajectory planning of an unmanned surface vehicle according to claim 5, characterized in that: Step 2.2) includes: 2.2.1) Obtain the starting and ending trajectory points of the unmanned boat's navigation trajectory, and expand the trajectory points by forward expansion from the starting trajectory point; 2.2.2) Take u(t) and r(t) in the unmanned boat dynamics model as control variables, and set τ u (t) and τ r (t) is set to a constant, v(t) is set to 0, and the position and posture [x, y, θ, t] of the unmanned boat at different trajectory points are obtained; In u(t)∈{u min ,u max }, r(t)∈{r min ,r max }, the forward expansion point list of the unmanned boat is set to: {(in max ,0.8r min )(in max ,0.5r min )(in max ,0.2r min )(in max ,0)(in max ,0.2r max )(in max ,0.5r max )(in max ,0.8r max )} Among them, r min is the minimum value of the yaw angular velocity; r max is the maximum value of the yaw angular velocity; Each expansion point has the total cost Node.f, the past historical cost Node.g, the future expected cost Node.h, the parent node index, whether it is in the open list, whether it is in the closed list, the current position, the current time, the type of u, r from which it is expanded, and the navigation trajectory during the expansion period {(x k ,y k ,θ k ,t k )|k=0,1,2,…,N f }, where N f 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 extension point; the open list and the closed list are used to record whether the extension point is an extendable node and whether it has been extended before; if the extension point is neither in the open list nor in the closed list, it has not been extended at all; if the extension point is in the open list but not in the closed list, it has been extended but can be extended again; if the extension point is in the closed list, it cannot be extended; The total cost Node.f is the sum of the past historical cost Node.g and the future expected cost Node.h. When expanding the trajectory point, the expandable point with the smallest total cost Node.f is used as the next trajectory point; chi .g=Node cur .g+s simu +μNode chi .r-Node cur .r|,Node chi .g represents the past historical cost of the next trajectory point. At this time, each expandable point of the current trajectory point will be used as the next trajectory point to calculate the past historical cost; Node cur .g represents the past historical cost of the current trajectory point, s simu represents the search step size, μ represents the penalty factor for steering operation, and Node chi .r represents the angular velocity of the next trajectory point, Node cur .r represents the angular velocity of the current trajectory point; The future expected cost of the expansion point Node.h is calculated by the heuristic function h. h=max{h nonholonomics ,h collision_avoidance } Among them, h nonholonomics represents the path length that complies with the kinematic law of the unmanned boat and ignores obstacle avoidance; h collision_avoidance represents the path length considering obstacle avoidance and ignoring kinematic laws; If the end track point is found to be an expandable node of the current track point during the expansion process, the forward expansion ends, and the navigation track of each track point during the expansion period is read in reverse to obtain the rough running track of the unmanned boat {(x j ,y j ,θ j ,t j )j=0,1,2,…,N A }.

7. The method for trajectory planning of an unmanned surface vehicle according to claim 5, characterized in that: In step 2), an initial solution vector is constructed according to the rough running trajectory, and then a ship tunnel constraint is constructed according to the initial solution vector; including: The rough running trajectory of the unmanned boat is resampled to obtain And expand the formation Estimated time T tst , N sam is the total number of trajectory points after resampling; then obtain the 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 With the vector {x m ,y m ,θ m ,u m ,v m ,r m ,τ u_m ,τ r_m |m=0,1,2,…N sam } are collectively called the initial solution vector; Then, based on the initial solution vector, the center of the characteristic disk and the distance from the center of the characteristic disk to the center of mass, the rough trajectory traj of the center of the characteristic disk of the unmanned boat is obtained. i (t)(i=1,2,…,N disc ) which include: When the unmanned boat is moving, the center of the i-th characteristic disk is taken as the mass point P i ; then the estimated time T tst At the nth sampling moment in i The location is: in, is the nth sampling moment, particle P i The x-axis coordinate of is the nth sampling moment, particle P i The y-axis coordinate of n is the attitude angle of the unmanned boat at the nth sampling moment; x n is the x-axis coordinate of the center of mass of the unmanned boat at the nth sampling moment; n is the y-axis coordinate of the center of mass of the unmanned boat at the nth sampling moment; At each sampling moment, the particle P i Construct local polygon SSC for the center i_j , based on the obtained rectangular SSC j_i Get the ship tunnel constraint.

8. The method for trajectory planning of an unmanned surface vehicle according to claim 7, characterized in that: The particle P at each sampling moment i Construct local polygon SSC for the center i_j , based on the obtained rectangular SSC j_i Get the ship tunnel constraints; including: a) At the nth sampling moment, the particle P i Initialize SSC i_n , SSC i_n Seen as A rectangle with length and width of 0 at the position, and define the expansion step Δs and the maximum allowable expansion length L in one direction max Then, it expands outward along the four sides of the rectangle in a counterclockwise direction with a constant step size Δs. During the expansion process, if the generated area does not overlap with the area of ​​the obstacle in the expanded grid map, the current expansion is considered valid and the current expanded area is merged into the rectangle SSC. i_n On the contrary, if it overlaps with the obstacle area, the current expansion is considered invalid, the current expansion area is abandoned and no further expansion is performed in this direction; in order to avoid infinite expansion, when the length of any direction of expansion exceeds L max When , the expansion in this direction stops; When the expansion stops, the obtained rectangular SSC i_n The range is recorded as Therefore, the ship tunnel constraint at the nth sampling time is Among them, x i_n is the nth sampling moment, particle P i The lower limit of the x-axis coordinate; is the nth sampling moment, particle P i The upper limit of the x-axis coordinate; y i_n is the nth sampling moment, particle P i The lower limit of the y-axis coordinate; is the nth sampling moment, particle P i The upper limit of the y-axis coordinate; b) Repeat step a) to obtain the navigation tunnel constraints at each sampling moment.

9. The method for trajectory planning of an unmanned surface vehicle according to claim 8, characterized in that: Step 1), the multi-objective cost function is: J=w1·J1+w2·J2+w3·J3+w4·J4+w5·J5 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 time when the unmanned boat is expected to complete the navigation mission as soon as possible; J1 = t f ; J2 indicates that the unmanned boat is expected to avoid the center of the ocean current vortex as much as possible; J3 indicates that the unmanned boat is expected to avoid obstacles as much as possible; J4 indicates that the speed change of the unmanned boat is expected to be as small as possible, and the acceleration and deceleration operations are expected to be as few as possible; J5 indicates that the unmanned boat is expected to have the smallest steering speed and the least steering operations; Among them, the center of the cth vortex is represents the distance between the mass center of the unmanned boat and the center of the cth vortex; (x(t), y(t)) is the coordinate of the center of mass of the unmanned boat; μ c is the penalty coefficient, which indicates the degree of penalty for the unmanned boat approaching the center of the vortex; Among them, N obs represents the number of obstacles; the geometric center of the dth obstacle is d obs_d (t) represents the distance between the unmanned boat and the dth obstacle, κ d is the penalty coefficient, which indicates the degree of penalty for the unmanned boat approaching an obstacle; Among them, λ1 and λ2 both represent penalty coefficients; Among them, λ3 and λ4 represent penalty coefficients; In step 2), the state constraint is: The boundary value constraints are: Among them, x init is the x-axis coordinate of the center of mass of the unmanned boat in the initial state; init is the y-axis coordinate of the center of mass of the unmanned boat in the initial state; θ init The attitude angle of the unmanned boat in the initial state; u init is the longitudinal velocity of the unmanned boat in the initial state; v init The lateral speed of the unmanned boat in the initial state; r init is the yaw angular velocity of the unmanned boat in the initial state; τ u_init The propulsion force of the unmanned boat in the initial state; τ r_init When it is the initial state, the yaw moment of the unmanned boat; final When it is the terminal state, the x-axis coordinate of the center of mass of the unmanned boat; y init When it is the terminal state, the y-axis coordinate of the center of mass of the unmanned boat; θ init When it is the terminal state, the attitude angle of the unmanned boat; u x_final_min When it is the terminal state, the minimum value of the x-axis component of the lateral velocity of the unmanned boat; u y_final_min When it is the terminal state, the minimum value of the y-axis component of the lateral velocity of the unmanned boat; u x_final_max When it is the terminal state, the maximum value of the x-axis component of the lateral velocity of the unmanned boat; u y_final_max The maximum value of the y-axis component of the lateral velocity of the unmanned boat when it is in the terminal state; The control input constraints are: Among them, v min is the minimum lateral velocity of the unmanned boat; v max is the maximum lateral velocity of the unmanned boat; r min is the minimum yaw angular velocity r of the unmanned boat max is the maximum value of the yaw angular velocity of the unmanned boat; τ u_min is the minimum propulsion force of the unmanned boat; τ u_max is the maximum propulsion force of the unmanned boat; τ r_min is the minimum yaw moment of the unmanned boat; τ r_max is the maximum value of the yaw moment of the unmanned boat.

10. The method for trajectory planning of an unmanned surface vehicle according to claim 9, characterized in that: Step 3) includes: An optimal control proposition is constructed by combining a multi-objective cost function, the control input constraints, boundary value constraints, state constraints, and navigation tunnel constraints. The optimal control proposition is transformed into a nonlinear programming problem using a first-order explicit discrete method. The nonlinear programming problem and the initial solution vector are input into the IPOPT solver to obtain the state quantity and control quantity of the unmanned boat. The state quantity includes the position of the center of mass of the unmanned boat, the attitude angle, the longitudinal velocity, the lateral velocity, and the yaw angular velocity. The control quantity includes the propulsion force of the unmanned boat and the yaw moment of the unmanned boat.

Images