A method, device, equipment and medium for planning navigation trajectory of unmanned ship

Abstract

The present application discloses a method, device, equipment and medium for unmanned ship navigation trajectory planning, which relates to the field of unmanned ship multi-target trajectory planning. The method comprises: determining an initial guess according to the starting point and terminal state of the unmanned ship navigation trajectory by utilizing an RRT* algorithm, adopting a gradient-based solver, solving a target planning problem model of the unmanned ship multi-target navigation trajectory planning according to the initial guess, and obtaining the optimal navigation trajectory of the unmanned ship. The target planning problem model of the unmanned ship multi-target navigation trajectory planning comprises a planning objective function and constraints, and the constraints comprise unmanned ship trajectory terminal constraints, unmanned ship physical limitation constraints, unmanned ship obstacle avoidance constraints and unmanned ship motion equation constraints, which solves the problem of low solution speed of existing solution methods and realizes an improvement in solution speed.

Description

Technical Field

[0001] The present application relates to the field of multi-target trajectory planning for unmanned ships, and in particular to a method, device, equipment and medium for planning the navigation trajectory of an unmanned ship. Background Art

[0002] Unmanned ships are an important product of the Fourth Industrial Revolution. They integrate a variety of advanced technologies in the fields of information communication, computers, unmanned driving, and control. Due to their advantages of high safety and low energy consumption, unmanned ships have broad application prospects and high development potential in maritime transportation, harbor patrols, water quality monitoring, maritime rescue, military operations, etc. The trajectory planning of unmanned ships is a key link in their completion of various tasks. A safe, reliable, and easy-to-track navigation trajectory is an important guarantee for the safe and efficient navigation of unmanned ships.

[0003] When planning the navigation trajectory of an unmanned ship, many scholars tend to model the trajectory planning problem as an optimal control problem, and then solve it through a gradient-based optimization solver to obtain the optimal trajectory of the unmanned ship.

[0004] However, the solution speed of the above-mentioned solution method is relatively low. Therefore, there is an urgent need for an unmanned ship navigation trajectory planning method with high solution speed. Summary of the invention

[0005] The purpose of this application is to provide a method, device, equipment and medium for unmanned ship navigation trajectory planning, which can use the RRT* algorithm to determine the initial guess of the unmanned ship's navigation trajectory, and solve the target planning problem model of the unmanned ship's multi-target navigation trajectory planning based on the initial guess to obtain the optimal navigation trajectory of the unmanned ship, thereby improving the solution speed.

[0006] To achieve the above objectives, this application provides the following solutions.

[0007] In a first aspect, the present application provides a method for planning a navigation trajectory of an unmanned ship, comprising the following steps.

[0008] The motion equation of the unmanned ship is established according to the state and control vector of the unmanned ship; the state includes position, heading angle, longitudinal surge velocity, transverse surge velocity and center of mass angular velocity; the control vector includes longitudinal thrust and rotational torque.

[0009] A target planning problem model for multi-objective navigation trajectory planning of an unmanned ship is established; the target planning problem model for multi-objective navigation trajectory planning of an unmanned ship includes a planning objective function and constraints; the constraints include unmanned ship trajectory terminal constraints, unmanned ship physical limitation constraints, unmanned ship obstacle avoidance constraints and unmanned ship motion equation constraints; the unmanned ship physical limitation constraints are limit constraints on heading angle, longitudinal surge velocity, lateral surge velocity, center of mass angular velocity, longitudinal thrust and torque; the unmanned ship motion equation constraints are determined by the unmanned ship motion equation.

[0010] Using the RRT* algorithm, the initial guess is determined based on the starting and terminal states of the unmanned ship's navigation trajectory.

[0011] A gradient-based solver is used to solve the target planning problem model of the multi-target navigation trajectory planning of the unmanned ship according to the initial guess, so as to obtain the optimal navigation trajectory of the unmanned ship.

[0012] In a second aspect, the present application provides an unmanned ship navigation trajectory planning device, comprising the following modules.

[0013] The motion equation establishment module is used to establish the motion equation of the unmanned ship according to the state and control vector of the unmanned ship; the state includes position, heading angle, longitudinal speed, lateral speed and center of mass angular velocity; the control vector includes longitudinal thrust and torque.

[0014] The target planning problem establishment module is used to: establish a target planning problem model for multi-target navigation trajectory planning of an unmanned ship; the target planning problem model for multi-target navigation trajectory planning of an unmanned ship includes a planning target function and constraints; the constraints include unmanned ship trajectory terminal constraints, unmanned ship physical limitation constraints, unmanned ship obstacle avoidance constraints and unmanned ship motion equation constraints; the unmanned ship physical limitation constraints are limit constraints on heading angle, longitudinal speed, lateral speed, center of mass angular velocity, longitudinal thrust and torque; the unmanned ship motion equation constraints are determined by the unmanned ship motion equation.

[0015] The initial guess determination module is used to: use the RRT* algorithm to determine the initial guess according to the starting point and terminal state of the unmanned ship's navigation trajectory.

[0016] The solution module is used to: use a gradient-based solver to solve the target planning problem model of the multi-target navigation trajectory planning of the unmanned ship according to the initial guess, so as to obtain the optimal navigation trajectory of the unmanned ship.

[0017] In a third aspect, the present application provides a computer device, comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the above-mentioned unmanned ship navigation trajectory planning method.

[0018] In a fourth aspect, the present application provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the above-mentioned unmanned ship navigation trajectory planning method.

[0019] According to the specific embodiments provided by the present application, the present application discloses the following technical effects: the present application provides a method, device, equipment and medium for unmanned ship navigation trajectory planning, by utilizing the RRT* algorithm, determining the initial guess according to the starting point and terminal state of the unmanned ship navigation trajectory, and using a gradient-based solver to solve the target planning problem model of the unmanned ship multi-target navigation trajectory planning according to the initial guess, and obtaining the optimal navigation trajectory of the unmanned ship, thereby solving the problem of low solution speed of existing solution methods and achieving an improvement in solution speed. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0021] Figure 1 This is an application environment diagram of an unmanned ship navigation trajectory planning method in one embodiment of the present application;

[0022] Figure 2 A schematic diagram of a flow chart of a method for planning a navigation trajectory of an unmanned ship provided in one embodiment of the present application;

[0023] Figure 3 A schematic diagram of a specific process of an unmanned ship navigation trajectory planning method provided in one embodiment of the present application;

[0024] Figure 4 A schematic diagram of the dynamics and kinematics modeling of an unmanned ship provided in one embodiment of the present application;

[0025] Figure 5 A schematic diagram of a circular model of an unmanned boat provided in an embodiment of the present application;

[0026] Figure 6 A schematic diagram of an obstacle expansion provided in an embodiment of the present application;

[0027] Figure 7 A schematic diagram of functional modules of an unmanned ship navigation trajectory planning device provided by another embodiment of the present application;

[0028] Figure 8 A schematic diagram of the structure of a computer device provided in one embodiment of the present application.

[0029] Description of symbols: terminal—102, server—104. DETAILED DESCRIPTION

[0030] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0031] In order to make the above-mentioned objects, features and advantages of the present application more obvious and easy to understand, the present application is further described in detail below with reference to the accompanying drawings and specific implementation methods.

[0032] The unmanned ship navigation trajectory planning method provided in the embodiment of the present application can be applied to Figure 1 In the application environment shown. Among them, the terminal 102 communicates with the server 104 through the network. The data storage system can store the data that the server 104 needs to process. The data storage system can be set up separately, integrated on the server 104, or placed on the cloud or other servers. The terminal 102 can send the starting point and terminal state of the unmanned ship's navigation trajectory to the server 104. After the server 104 receives the starting point and terminal state of the unmanned ship's navigation trajectory, for the starting point and terminal state of the unmanned ship's navigation trajectory, the server 104 uses the RRT* algorithm to determine the initial guess based on the starting point and terminal state of the unmanned ship's navigation trajectory, and uses a gradient-based solver to solve the target planning problem model of the unmanned ship's multi-objective navigation trajectory planning based on the initial guess to obtain the optimal navigation trajectory of the unmanned ship. The server 104 can feedback the obtained optimal navigation trajectory of the unmanned ship to the terminal 102. In addition, in some embodiments, the unmanned ship navigation trajectory planning method can also be implemented independently by the server 104 or the terminal 102. For example, the terminal 102 can directly plan the navigation trajectory for the starting point and terminal status of the unmanned ship navigation trajectory, or the server 104 can obtain the starting point and terminal status of the unmanned ship navigation trajectory from the data storage system, and plan the navigation trajectory for the starting point and terminal status of the unmanned ship navigation trajectory.

[0033] The terminal 102 may be, but is not limited to, various desktop computers, laptop computers, smart phones, and tablet computers. The server 104 may be implemented as an independent server or a server cluster consisting of multiple servers, or may be a cloud server.

[0034] In an exemplary embodiment, Figure 2As shown, a method for planning the navigation trajectory of an unmanned ship is provided. The method is executed by a computer device, and can be executed by a computer device such as a terminal or a server alone, or can be executed by a terminal and a server together. In the embodiment of the present application, the method is applied to Figure 1 The server 104 in the example is used for explanation, and the steps include the following steps 201 to 204.

[0035] Step 201: Establishing the motion equation of the unmanned ship according to the state and control vector of the unmanned ship; the state includes position, heading angle, surge velocity, sway velocity and center of mass angular velocity; the control vector includes longitudinal thrust and torque.

[0036] Step 202: Establish a target planning problem model for multi-objective navigation trajectory planning of an unmanned ship; the target planning problem model for multi-objective navigation trajectory planning of an unmanned ship includes a planning objective function and constraints; the constraints include unmanned ship trajectory terminal constraints, unmanned ship physical limitation constraints, unmanned ship obstacle avoidance constraints and unmanned ship motion equation constraints; the unmanned ship physical limitation constraints are limit constraints on heading angle, longitudinal surge velocity, transverse surge velocity, center of mass angular velocity, longitudinal thrust and torque; the unmanned ship motion equation constraints are determined by the unmanned ship motion equation.

[0037] Step 203: Using the RRT* algorithm, an initial guess is determined based on the starting point and terminal state of the unmanned ship's navigation trajectory.

[0038] Step 204: using a gradient-based solver to solve the target planning problem model of the multi-target navigation trajectory planning of the unmanned ship according to the initial guess, and obtaining the optimal navigation trajectory of the unmanned ship.

[0039] Implement the above steps 201 to 204, by using the RRT* algorithm, determine the initial guess according to the starting point and terminal state of the unmanned ship's navigation trajectory, and use a gradient-based solver to solve the target planning problem model of the unmanned ship's multi-objective navigation trajectory planning according to the initial guess, and obtain the optimal navigation trajectory of the unmanned ship, which solves the problem of low solution speed of the existing solution method and achieves an improvement in solution speed. The solution quality of the gradient-based optimization solver is heavily dependent on the initial guess. A nearly optimal or nearly feasible initial guess can effectively improve the solution speed of the gradient-based optimization solver. In the process of unmanned ship navigation trajectory planning, decision makers may have the need to optimize multiple objectives at the same time. Therefore, when optimizing and solving the unmanned ship's navigation trajectory, it is difficult to cope with the actual situation by optimizing only a single objective. Therefore, the present application can also optimize multiple objective functions at the same time to meet the decision maker's needs to optimize multiple objectives and obtain a safe, reliable, and easy-to-track unmanned ship navigation trajectory.

[0040] In another exemplary embodiment of the present application, Figure 3As shown, the following steps 301 to 306 are also included.

[0041] Step 301: Establish the motion equation of the unmanned ship in a rectangular coordinate system.

[0042] Step 302: The decision maker sets the endpoint status information.

[0043] The decision maker sets the terminal state that the unmanned ship intends to reach according to the mission requirements. Specifically, the terminal state of the unmanned ship can be expressed as d =[x d ,y d ,Ψ d ,u d ,v d ,r d ] indicates that x d ,y d ,Ψ d ,u d ,v d ,r d They are the lateral coordinate target value, longitudinal coordinate target value, heading angle target value, surge velocity target value, sway velocity target value and center of mass angular velocity target value at the terminal moment.

[0044] Step 303: The decision maker sets the optimization goal and its expected value.

[0045] The decision maker sets the objective function J to be optimized in the navigation trajectory planning process according to the needs 1 (X,U),J 2 (X,U),…,J n (X,U) and the corresponding expected value It should be noted that the expected value can be set according to actual needs during use.

[0046] Step 304: Establish a target planning problem model for multi-target navigation trajectory planning of the unmanned ship.

[0047] Step 305: The upper layer obtains an initial guess based on the RRT* algorithm.

[0048] Step 306: The lower layer uses a gradient-based solver to solve the optimal navigation trajectory of the unmanned ship.

[0049] Assuming that the mass distribution of the unmanned ship is uniform and symmetrical about the xz plane, and ignoring the wave vibration, swaying, and yaw caused by wind and current, the motion equation of the unmanned ship in the rectangular coordinate system is established, as shown in the following equation (1).

[0050]

[0051] Where X = [x, y, Ψ, u, v, r] T and U=[τu ,τ r ] T is the state and control vector of the unmanned ship, (x, y) represents the position of the unmanned ship; Ψ is the heading angle of the unmanned ship; u, v, r are the longitudinal velocity, transverse velocity and angular velocity of the center of mass of the unmanned ship respectively; τ u and τ r Represents longitudinal thrust and rotational moment respectively; m 1 、m 2 、m 3 is the inertia taking into account the added mass effect; d 1 d 2 d 3 is the hydrodynamic damping; the · above the symbol indicates the differential, e.g. represents the differentiation of x. Figure 4 As shown, C(x,y) is the center of mass of the unmanned ship.

[0052] In this embodiment, minimizing the task completion time and minimizing energy consumption are selected as the goals. The planning objective function in this application includes a first objective function and a second objective function; the first objective function is a function with the goal of minimizing the task completion time; the second objective function is a function with the goal of minimizing energy consumption. The constructed first objective function and the second objective function are shown in the following equations (2) and (3) respectively.

[0053] 1) The calculation formula of the first objective function with the goal of minimizing the task completion time is shown as follows.

[0054] min J 1 (X,U)=t N (2).

[0055] Among them, min J 1 (X,U) represents the first objective function; t N Indicates the terminal time, that is, the task completion time.

[0056] 2) The calculation formula of the second objective function with the goal of minimizing energy consumption is shown below.

[0057]

[0058] Among them, min J 2 (X,U) represents the second objective function; τ u and τ r Represent the longitudinal thrust and rotational torque respectively; τ u,max is the maximum longitudinal thrust of the unmanned ship; τ r,max is the maximum value of the rotational torque of the unmanned ship; t represents the time.

[0059] Next, the constraints are constructed, which include the unmanned ship trajectory terminal constraint, the unmanned ship physical limitation constraint, the unmanned ship obstacle avoidance constraint and the unmanned ship motion equation constraint. The unmanned ship trajectory terminal constraint is shown in the following formula (4).

[0060]

[0061] Among them, t N represents the terminal time, d represents the terminal state, x(t N ),y(t N ),Ψ(t N ),u(t N ),v(t N ),r(t N ) are respectively the actual values ​​of the transverse coordinate, the actual values ​​of the longitudinal coordinate, the actual values ​​of the heading angle, the actual values ​​of the longitudinal velocity, the actual values ​​of the sway velocity and the actual values ​​of the angular velocity of the center of mass at the terminal moment; x d ,y d ,Ψ d ,u d ,v d ,r d They are the lateral coordinate target value, longitudinal coordinate target value, heading angle target value, surge velocity target value, sway velocity target value and center of mass angular velocity target value at the terminal moment.

[0062] The physical constraints of the unmanned ship are shown in equation (5).

[0063]

[0064] Among them, min and max are the minimum and maximum values ​​of the heading angle of the unmanned ship; u min and u max are the minimum and maximum values ​​of the unmanned ship’s longitudinal speed; v min and v max are the minimum and maximum values ​​of the unmanned ship’s swaying speed, respectively; r min and r max are the minimum and maximum angular velocity of the center of mass of the unmanned ship; τ u,min and τ u,max are the minimum and maximum longitudinal thrust of the unmanned ship respectively; τ r,min and τ r,max are the minimum and maximum values ​​of the rotational torque of the unmanned ship; t N Indicates the terminal time.

[0065] The obstacle avoidance constraints of the unmanned ship are shown in equation (6).

[0066]

[0067] Among them, S Δ represents the area of ​​the triangle formed by three points, Represents triangle P i W jm W j1 The area of ​​triangle P i W jm W j1 For point P i , click W jm and point W j1 The triangle formed, Represents triangle P i W jl W jl+1 The area of ​​triangle P i W jl W jl+1 For point P i , click W jl and point W jl+1 The triangle formed by P i , i=1,2,3,4,5 are the vertices of the unmanned ship; W jl ,l=1,2,...,m is the vertex of the lth obstacle; represents the area of ​​the jth obstacle.

[0068] The target planning problem model of the multi-target navigation trajectory planning of the unmanned ship is specifically shown as follows:

[0069]

[0070] Where J represents the planning objective function; and is the intermediate parameter; J k (X,U) represents the kth objective function; They are and The weight coefficient of represents the expected value of the kth objective function.

[0071] It should be noted that the sum of the weight coefficients of all objective functions in the planning objective function is 1.

[0072] The specific process of determining the initial guess by the RRT* algorithm in step 203 is as follows: input map information, the number of nodes and step length of RRT*, the starting coordinates of the navigation trajectory and the terminal state to obtain the planned path. The map information includes the location of obstacles and the navigation area of ​​the unmanned ship. The terminal state includes the lateral coordinate target value, longitudinal coordinate target value, heading angle target value, sway speed target value, sway speed target value and center of mass angular velocity target value at the terminal moment. Figure 5 As shown in Figure 1, first, a circle with a center of C and a radius of R is used to cover the entire unmanned ship. Secondly, the obstacle is expanded outward with a length of R, as shown in Figure 1. Figure 6 As shown, the middle slash area represents an obstacle, and the blank area outside the slash area represents the expansion area. At this point, the unmanned ship can be regarded as a target point. Next, the RRT* algorithm is used to plan a shortest and passable path from the starting point to the end point for the target point to obtain a planned path. The planned path is an initial collision-free path from the current position to the set terminal. Finally, a number of track points are uniformly sampled at intervals of a set sampling period on the planned path, and the sampled track points are used as the initial guess of the lower-level gradient-based solver. The sampling period is set by the user according to needs and experience.

[0073] The specific process of step 204 is: input the initial guess obtained in step 203, and use a gradient-based solver to solve the target planning problem model of the unmanned ship's multi-target navigation trajectory planning to quickly obtain the optimal navigation trajectory of the unmanned ship.

[0074] This application establishes the unmanned ship motion equation in a rectangular coordinate system; the decision maker sets the terminal state information; the decision maker sets the optimization target and its expected value; the kinematic and dynamic constraints, terminal constraints, physical restrictions, obstacle avoidance constraints, etc. to be met in the unmanned ship trajectory planning are integrated into the unified overall framework of target planning, and a target planning problem model for the unmanned ship multi-target navigation trajectory planning is constructed; the upper layer obtains the initial guess based on the RRT* algorithm; the lower layer uses a gradient-based solver to solve the unmanned ship's optimal navigation trajectory. The present invention can solve the unmanned ship's multi-target navigation trajectory planning problem, quickly obtain a safe, reliable, and easy-to-track navigation trajectory, so that the unmanned ship can efficiently complete related tasks, and optimize multiple objective functions at the same time to meet the decision maker's needs to optimize multiple goals, so as to obtain a safe, reliable, and easy-to-track navigation trajectory for the unmanned ship, thereby providing important guarantees for the unmanned ship to complete its tasks.

[0075] The present application also provides an application scenario, which applies the above-mentioned unmanned ship navigation trajectory planning method. Specifically: the unmanned ship navigation trajectory planning method provided in this embodiment can be applied to the unmanned ship multi-target trajectory planning scenario. The unmanned ship multi-target trajectory planning scenario includes a target determination link and a multi-target trajectory planning link; the starting point and terminal state of the unmanned ship navigation trajectory enter the multi-target trajectory planning link from the target determination link to obtain the optimal navigation trajectory of the unmanned ship. The unmanned ship navigation trajectory planning method provided in this embodiment belongs to the multi-target trajectory planning link in the multi-target trajectory planning link.

[0076] Based on the same inventive concept, the embodiment of the present application also provides an unmanned ship navigation trajectory planning device for implementing the unmanned ship navigation trajectory planning method involved above. The implementation scheme for solving the problem provided by the device is similar to the implementation scheme recorded in the above method, so the specific limitations in one or more unmanned ship navigation trajectory planning device embodiments provided below can refer to the limitations of the unmanned ship navigation trajectory planning method above, and will not be repeated here.

[0077] In an exemplary embodiment, Figure 7 As shown, an unmanned ship navigation trajectory planning device is provided, including the following modules.

[0078] The motion equation establishment module T1 is used to establish the motion equation of the unmanned ship according to the state and control vector of the unmanned ship; the state includes position, heading angle, longitudinal speed, lateral speed and center of mass angular velocity; the control vector includes longitudinal thrust and rotational torque;

[0079] The target planning problem establishment module T2 is used to: establish a target planning problem model for multi-target navigation trajectory planning of an unmanned ship; the target planning problem model for multi-target navigation trajectory planning of an unmanned ship includes a planning target function and constraints; the constraints include unmanned ship trajectory terminal constraints, unmanned ship physical limitation constraints, unmanned ship obstacle avoidance constraints and unmanned ship motion equation constraints; the unmanned ship physical limitation constraints are limit constraints on heading angle, longitudinal speed, lateral speed, center of mass angular velocity, longitudinal thrust and torque; the unmanned ship motion equation constraints are determined by the unmanned ship motion equation;

[0080] The initial guess determination module T3 is used to: use the RRT* algorithm to determine the initial guess according to the starting point and terminal state of the unmanned ship's navigation trajectory.

[0081] The solving module T4 is used to: use a gradient-based solver to solve the target planning problem model of the multi-target navigation trajectory planning of the unmanned ship according to the initial guess, so as to obtain the optimal navigation trajectory of the unmanned ship.

[0082] As an optional implementation, the planning objective function includes a first objective function and a second objective function; the first objective function is a function that aims to minimize task completion time; and the second objective function is a function that aims to minimize energy consumption.

[0083] Among them, when implementing this implementation method, the target planning problem model of the unmanned ship's multi-target navigation trajectory planning is specifically shown in the following formula.

[0084]

[0085] Where J represents the planning objective function; and is the intermediate parameter; J k (X,U) represents the kth objective function; They are and The weight coefficient of represents the expected value of the kth objective function; (x, y) represents the position of the unmanned ship; Ψ is the heading angle of the unmanned ship; u, v, r are the longitudinal velocity, lateral velocity and angular velocity of the center of mass of the unmanned ship respectively; τ u and τ r Represents longitudinal thrust and rotational moment respectively; m 1 、m 2 、m 3 is the inertia taking into account the added mass effect; d 1 d 2 d 3 is the hydrodynamic damping; min and max are the minimum and maximum values ​​of the heading angle of the unmanned ship; u min and u max are the minimum and maximum values ​​of the unmanned ship’s longitudinal speed; v min and v max are the minimum and maximum values ​​of the unmanned ship’s swaying speed, respectively; r min and r max are the minimum and maximum angular velocity of the center of mass of the unmanned ship; τ u,min and τ u,max are the minimum and maximum longitudinal thrust of the unmanned ship respectively; τ r,min and τ r,max are the minimum and maximum values ​​of the rotational torque of the unmanned ship; t N represents the terminal time; x(t N ),y(t N ),Ψ(t N ),u(t N ),v(t N ),r(t N ) are respectively the actual values ​​of the transverse coordinate, the actual values ​​of the longitudinal coordinate, the actual values ​​of the heading angle, the actual values ​​of the longitudinal velocity, the actual values ​​of the sway velocity and the actual values ​​of the angular velocity of the center of mass at the terminal moment; x d ,y d ,Ψ d ,u d ,v d ,r d They are the target values ​​of the lateral coordinate, longitudinal coordinate, heading angle, sway velocity, sway velocity and center of mass angular velocity at the terminal moment. Represents triangle P i W jm Wj1 area; Represents triangle P i W jl W jl+1 The area of ​​​​P i , i=1,2,3,4,5 are the vertices of the unmanned ship; W jl ,l=1,2,...,m is the vertex of the lth obstacle; represents the area of ​​the jth obstacle.

[0086] The calculation formula of the second objective function is:

[0087] Among them, min J 2 (X,U) represents the second objective function; τ u and τ r Represent the longitudinal thrust and rotational torque respectively; τ u,max is the maximum longitudinal thrust of the unmanned ship; τ r,max is the maximum value of the unmanned ship's torque; t N represents the terminal time; t represents the time.

[0088] In an exemplary embodiment, a computer device is provided. The computer device may be a server or a terminal. The internal structure diagram thereof may be as follows: Figure 8 As shown. The computer device includes a processor, a memory, an input / output interface (Input / Output, referred to as I / O) and a communication interface. Among them, the processor, the memory and the input / output interface are connected through a system bus, and the communication interface is connected to the system bus through the input / output interface. Among them, the processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, a computer program and a database. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The database of the computer device is used to store the state and control vector of the unmanned ship. The input / output interface of the computer device is used to exchange information between the processor and an external device. The communication interface of the computer device is used to communicate with an external terminal through a network connection. When the computer program is executed by the processor, a method for planning the navigation trajectory of an unmanned ship is implemented.

[0089] Those skilled in the art will understand that Figure 8 The structure shown in the figure is only a block diagram of a part of the structure related to the solution of the present application, and does not constitute a limitation on the computer device to which the solution of the present application is applied. The specific computer device may include more or fewer components than those shown in the figure, or combine certain components, or have a different arrangement of components.

[0090] In an exemplary embodiment, a computer device is further provided, including a memory and a processor, wherein a computer program is stored in the memory, and the processor implements the steps in the above-mentioned method embodiments when executing the computer program.

[0091] In an exemplary embodiment, a computer-readable storage medium is provided, storing a computer program, and when the computer program is executed by a processor, the steps in the above method embodiments are implemented.

[0092] It should be noted that the user information (including but not limited to user device information, user personal information, etc.) and data (including but not limited to data used for analysis, stored data, displayed data, etc.) involved in this application are all information and data authorized by the user or fully authorized by all parties, and the collection, use and processing of relevant data must comply with relevant regulations.

[0093] Those of ordinary skill in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to the memory, database or other medium used in the embodiments provided in the present application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetic random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. By way of illustration and not limitation, RAM may be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM).

[0094] The database involved in each embodiment provided in this application may include at least one of a relational database and a non-relational database. The non-relational database may include a distributed database based on blockchain, etc., but is not limited thereto. The processor involved in each embodiment provided in this application may be a general-purpose processor, a central processing unit, a graphics processor, a digital signal processor, a programmable logic device, a data processing logic device based on quantum computing, etc., but is not limited thereto.

[0095] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0096] This article uses specific examples to illustrate the principles and implementation methods of this application. The description of the above embodiments is only used to help understand the method and core ideas of this application. At the same time, for those skilled in the art, according to the ideas of this application, there will be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as limiting this application.

Claims

1. A method for planning a navigation trajectory of an unmanned ship, characterized in that: The unmanned ship navigation trajectory planning method comprises: The motion equation of the unmanned ship is established according to the state and control vector of the unmanned ship; the state includes position, heading angle, longitudinal velocity, lateral velocity and center of mass angular velocity; the control vector includes longitudinal thrust and rotational torque; Establish a target planning problem model for unmanned ship multi-objective navigation trajectory planning; the target planning problem model for unmanned ship multi-objective navigation trajectory planning includes planning objective functions and constraints; the constraints include unmanned ship trajectory terminal constraints, unmanned ship physical limitation constraints, unmanned ship obstacle avoidance constraints and unmanned ship motion equation constraints; the unmanned ship physical limitation constraints are limit constraints for heading angle, longitudinal sway speed, transverse sway speed, center of mass angular velocity, longitudinal thrust and torque; the unmanned ship motion equation constraints are determined by the unmanned ship motion equation; the planning objective function includes a first objective function and a second objective function; the first objective function is a function with the goal of minimizing task completion time; the second objective function is a function with the goal of minimizing energy consumption; the target planning problem model for unmanned ship multi-objective navigation trajectory planning is as follows: P min ≤Ψ≤Ψ max you min ≤u≤u max v min ≤v≤v max r min ≤r≤r max t u,min ≤τ u ≤τ u,max t r,min ≤τ r ≤τ r,max x(t N )=x d ,y(t N )=y d ,Ψ(t N )=Ψ d u(t N )=u d ,v(t N )=v d ,r(t N )=r d Where J represents the planning objective function; and is the intermediate parameter; J k (X,U) represents the kth objective function; They are and The weight coefficient of represents the expected value of the kth objective function; (x, y) represents the position of the unmanned ship; Ψ is the heading angle of the unmanned ship; u, v, r are the longitudinal velocity, lateral velocity and angular velocity of the center of mass of the unmanned ship respectively; τ u and τ r Represent the longitudinal thrust and torque respectively; m1, m2, m3 are the inertia considering the added mass effect; d1, d2, d3 are the hydrodynamic damping; Ψ min and max are the minimum and maximum values ​​of the heading angle of the unmanned ship; u min and u max are the minimum and maximum values ​​of the unmanned ship’s longitudinal speed; v min and v max are the minimum and maximum values ​​of the unmanned ship’s swaying speed, respectively; r min and r max are the minimum and maximum angular velocity of the center of mass of the unmanned ship; τ u,min and τ u,max are the minimum and maximum longitudinal thrust of the unmanned ship respectively; τ r,min and τ r,max are the minimum and maximum values ​​of the rotational torque of the unmanned ship; t N represents the terminal time; x(t N ),y(t N ),Ψ(t N ),u(t N ),v(t N ),r(t N ) are respectively the actual values ​​of the transverse coordinate, the actual values ​​of the longitudinal coordinate, the actual values ​​of the heading angle, the actual values ​​of the longitudinal velocity, the actual values ​​of the sway velocity and the actual values ​​of the angular velocity of the center of mass at the terminal moment; x d ,y d ,Ψ d ,u d ,v d ,r d They are the target values ​​of the lateral coordinate, longitudinal coordinate, heading angle, sway velocity, sway velocity and center of mass angular velocity at the terminal moment. Represents triangle P i W jm W j1 area; Represents triangle P i W jl W jl+1 The area of ​​​​P i , i=1,2,3,4,5 are the vertices of the unmanned ship; W jl ,l=1,2,...,m is the vertex of the lth obstacle; S Oj represents the area of ​​the jth obstacle; The RRT* algorithm is used to determine the initial guess according to the starting point and terminal state of the unmanned ship's navigation trajectory, specifically including: using the RRT* algorithm to plan a shortest and passable path from the starting point to the end point for the target point to obtain a planned path, which is a collision-free initial path from the current position to the set terminal; uniformly sampling at intervals of a set sampling period on the planned path to obtain a number of track points, and using the sampled track points as the initial guess; A gradient-based solver is used to solve the target planning problem model of the multi-target navigation trajectory planning of the unmanned ship according to the initial guess, so as to obtain the optimal navigation trajectory of the unmanned ship.

2. The unmanned ship navigation trajectory planning method according to claim 1, characterized in that: The calculation formula of the second objective function is as follows: Wherein, min J2(X,U) represents the second objective function; τ u and τ r Represent the longitudinal thrust and rotational torque respectively; τ u,max is the maximum longitudinal thrust of the unmanned ship; τ r,max is the maximum value of the unmanned ship’s torque; t N represents the terminal time; t represents the time.

3. An unmanned ship navigation trajectory planning device, characterized in that: The unmanned ship navigation trajectory planning device comprises: The motion equation establishment module is used to establish the motion equation of the unmanned ship according to the state and control vector of the unmanned ship; the state includes position, heading angle, longitudinal speed, lateral speed and center of mass angular velocity; the control vector includes longitudinal thrust and rotational torque; A target planning problem establishment module is used to: establish a target planning problem model for unmanned ship multi-target navigation trajectory planning; the target planning problem model for unmanned ship multi-target navigation trajectory planning includes a planning target function and constraints; the constraints include unmanned ship trajectory terminal constraints, unmanned ship physical limitation constraints, unmanned ship obstacle avoidance constraints and unmanned ship motion equation constraints; the unmanned ship physical limitation constraints are limit constraints for heading angle, longitudinal speed, lateral speed, center of mass angular velocity, longitudinal thrust and torque; the unmanned ship motion equation constraints are determined by the unmanned ship motion equation; The initial guess determination module is used to: use the RRT* algorithm to determine the initial guess according to the starting point and terminal state of the unmanned ship's navigation trajectory, specifically including: using the RRT* algorithm to plan a shortest and passable path from the starting point to the end point for the target point to obtain a planned path, where the planned path is a collision-free initial path from the current position to the set terminal; uniformly sampling a number of track points at intervals of a set sampling period on the planned path, and using the sampled track points as the initial guess; the planning objective function includes a first objective function and a second objective function; the first objective function is a function with the goal of minimizing the task completion time; the second objective function is a function with the goal of minimizing energy consumption; the target planning problem model of the unmanned ship's multi-objective navigation trajectory planning is as follows: P min ≤Ψ≤Ψ max you min ≤u≤u max v min ≤v≤v max r min ≤r≤r max t u,min ≤τ u ≤τ u,max t r,min ≤τ r ≤τ r,max x(t N )=x d ,y(t N )=y d ,Ψ(t N )=Ψ d u(t N )=u d ,v(t N )=v d ,r(t N )=r d Where J represents the planning objective function; and is the intermediate parameter; J k (X,U) represents the kth objective function; They are and The weight coefficient of represents the expected value of the kth objective function; (x, y) represents the position of the unmanned ship; Ψ is the heading angle of the unmanned ship; u, v, r are the longitudinal velocity, lateral velocity and angular velocity of the center of mass of the unmanned ship respectively; τ u and τ r Represent the longitudinal thrust and torque respectively; m1, m2, m3 are the inertia considering the added mass effect; d1, d2, d3 are the hydrodynamic damping; Ψ min and max are the minimum and maximum values ​​of the heading angle of the unmanned ship; u min and u max are the minimum and maximum values ​​of the unmanned ship’s longitudinal speed; v min and v max are the minimum and maximum values ​​of the unmanned ship’s swaying speed, respectively; r min and r max are the minimum and maximum angular velocity of the center of mass of the unmanned ship; τ u,min and τ u,max are the minimum and maximum longitudinal thrust of the unmanned ship respectively; τ r,min and τ r,max are the minimum and maximum values ​​of the rotational torque of the unmanned ship; t N represents the terminal time; x(t N ),y(t N ),Ψ(t N ),u(t N ),v(t N ),r(t N ) are respectively the actual values ​​of the transverse coordinate, the actual values ​​of the longitudinal coordinate, the actual values ​​of the heading angle, the actual values ​​of the longitudinal velocity, the actual values ​​of the sway velocity and the actual values ​​of the angular velocity of the center of mass at the terminal moment; x d ,y d ,Ψ d ,u d ,v d ,r d They are the target values ​​of the lateral coordinate, longitudinal coordinate, heading angle, sway velocity, sway velocity and center of mass angular velocity at the terminal moment. Represents triangle P i W jm W j1 area; Represents triangle P i W jl W jl+1 The area of ​​​​P i , i=1,2,3,4,5 are the vertices of the unmanned ship; W jl ,l=1,2,...,m is the vertex of the lth obstacle; S Oj represents the area of ​​the jth obstacle; The solution module is used to: use a gradient-based solver to solve the target planning problem model of the multi-target navigation trajectory planning of the unmanned ship according to the initial guess, so as to obtain the optimal navigation trajectory of the unmanned ship.

4. The unmanned ship navigation trajectory planning device according to claim 3, characterized in that: The calculation formula of the second objective function is as follows: Wherein, min J2(X,U) represents the second objective function; τ u and τ r Represent the longitudinal thrust and rotational torque respectively; τ u,max is the maximum longitudinal thrust of the unmanned ship; τ r,max is the maximum value of the unmanned ship's torque; t N represents the terminal time; t represents the time.

5. A computer device comprising: A memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the unmanned ship navigation trajectory planning method according to any one of claims 1 to 2.

6. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the unmanned ship navigation trajectory planning method described in any one of claims 1-2 is implemented.

Images