A minimum energy trajectory planning method for unmanned surface vehicle

Abstract

The application provides a minimum energy consumption trajectory planning method for an unmanned surface vehicle, and relates to the technical field of unmanned surface vehicle motion planning, and comprises the following steps: constructing and updating a three-dimensional occupancy grid map of an environment in which the unmanned surface vehicle is currently located based on depth image information; constructing a signed Euclidean distance field; searching for a front path point based on the three-dimensional occupancy grid map by using an A-star algorithm based on ship kinematics to obtain a path point; fitting a curve to the path point by using a straight line to obtain a path curve; and solving an unconstrained optimization problem for speed, acceleration, obstacle avoidance distance and curve smoothness based on the Euclidean distance field to obtain an optimal obstacle avoidance trajectory. The application can plan a trajectory for the unmanned surface vehicle based on visual information, can avoid obstacles and collisions for any shape of obstacle in a complex environment in an open water area, and the unmanned surface vehicle can make real-time local planning according to the surrounding obstacle environment.

Description

Technical Field

[0001] This invention relates to the field of motion planning technology for unmanned surface vessels, and more particularly to a method for planning the trajectory of an unmanned surface vessel with minimum energy consumption. Background Technology

[0002] In recent years, the exploration, development, and utilization of the ocean have become a key development focus for coastal nations. Unmanned surface vessels (USVs), with their miniaturization, lightweight design, and autonomy, have become important tools for ocean exploration and development. The most important characteristic of USVs is their autonomous mission execution. Even without human intervention, USVs can navigate autonomously after determining their course and operational tasks, intelligently avoiding obstacles and thus reducing unnecessary and tedious work for humans.

[0003] As research on unmanned surface vessels (USVs) continues to deepen, many countries and experimental institutions have begun to extensively study trajectory planning for USVs, achieving significant progress and results. Based on different optimization objectives, USV trajectory planning can be broadly categorized into time-optimal trajectory planning, energy-optimal trajectory planning, and multi-objective optimization trajectory planning. Among these, multi-objective optimization trajectory planning has received considerable attention in recent years. Its goal is to select multiple optimization objectives to establish a multi-objective optimization model and introduce weight coefficients to adjust the optimization weights of each objective to obtain the optimal trajectory for the USV. Practical applications of trajectory planning cover a wide range of marine missions, such as exploration and development, territorial sea monitoring, and route planning.

[0004] Most existing methods for unmanned surface vessel (USV) trajectory planning rely on high-resolution maps or electronic nautical charts. However, these algorithms suffer from several problems: First, map construction often results in a simple grid map without further optimization to provide more information for obstacle avoidance, such as Euclidean distance and gradients. They also fail to address complex obstacles of arbitrary shapes. Second, the trajectory planning algorithms often employ simple graph search or fast random tree algorithms, neglecting to optimize speed, acceleration, and obstacle avoidance distance based on the environment. They also fail to consider the kinematics of moving objects, the need for minimum energy consumption, and smooth course design. Third, electronic nautical charts or high-resolution maps cannot provide real-time information on surrounding obstacles, hindering real-time updates and improvements to the USV's environment. This makes it difficult to avoid nearby obstacles and limits efficient trajectory planning due to the grid size limitations of electronic nautical charts. Summary of the Invention

[0005] In view of this, the purpose of this invention is to propose a trajectory planning method for minimizing the energy consumption of unmanned surface vessels. This method utilizes a signed Euclidean distance map constructed based on visual information, and on the basis of the Euclidean distance map, constructs an unconstrained optimization problem to solve for the optimal trajectory of the unmanned surface vessel, considering obstacle avoidance distance, velocity, acceleration, and curve smoothness.

[0006] The technical means employed in this invention are as follows:

[0007] A method for minimizing the energy consumption trajectory planning of an unmanned surface vessel includes the following steps:

[0008] Use a depth camera to collect depth image information of the environment in which the unmanned surface vessel is currently located, and build and update a three-dimensional occupancy grid map of the environment in which the unmanned surface vessel is currently located based on the depth image information.

[0009] A signed Euclidean distance field is constructed based on the three-dimensional occupied raster map using the Euclidean transformation algorithm;

[0010] The A* algorithm based on ship kinematics is used to search for front-end path points based on a 3D occupied grid map to obtain path points;

[0011] The path curve is obtained by fitting a straight line to the path points.

[0012] For the path curve, based on the Euclidean distance field, an unconstrained optimization problem is constructed for velocity, acceleration, obstacle avoidance distance, and curve smoothness;

[0013] Based on the Euclidean distance field, an unconstrained optimization problem for velocity, acceleration, obstacle avoidance distance, and curve smoothness is solved to obtain the optimal obstacle avoidance trajectory.

[0014] Furthermore, the steps for establishing the three-dimensional occupancy grid map are as follows:

[0015] Let the grid size be m i The probability that a grid cell is occupied is p(m i =1), the probability of a grid cell being empty is p(m) i =0), let the t-th observation be set as z. t z1, z2, z3, z4...z t The objective is to calculate the posterior probability of occupying a grid cell over t known observations, i.e., p(m i =1|z 1:t ) and p(m i =0|z 1:t From Bayes' theorem, we can obtain:

[0016]

[0017] According to the Markov assumption, the current observation is independent of previous observations, therefore:

[0018] p(z t |z 1:t-1 ,m i )=p(z t |m i (2)

[0019] The probabilities of a grid being occupied or empty are as follows:

[0020]

[0021] Let m be the grid i The t-th observation is denoted as l. t (m i The first t-1 observations are denoted as l. t-1 (m i Then, the above equation can be rearranged to obtain the following equation:

[0022]

[0023] p(m i |z t The inverse sensor model is p(z), which is then converted into a sensor model. t |m i The conversion formula is as follows:

[0024]

[0025] Substituting the sensor model into (4) yields:

[0026]

[0027] Where: l t (m i ) for grid m i The t-th observation, l t-1 (m i ) for grid m i The first t-1 observations, p(z) t |z 1:t-1 ,m i ),p(z t |m i ) is the grid m i Based on the observations from the previous 1 to t-1, the t-th observation value and the t-th observation value, p(m i |z t ), For the inverse sensor model under grid vacancy and grid occupation, p(z t |m i ), Sensor models for grid vacancy and grid occupation;

[0028] For a fixed sensor, the grid information updated each time is a fixed value. As the sensor data is continuously updated, then l t (m i As the t-th observation is continuously updated, the 3D occupied grid map within the camera range is updated on the t-th observation.

[0029] Furthermore, the construction formula for the Euclidean distance field is as follows:

[0030] D(p)=min{(po) 2 +f(o)} (7)

[0031] Where: D(p) is the distance between the free grid p and the nearest obstacle grid, o is the obstacle grid, and f(o) is the sampling function used to store the distance information for constructing a high-dimensional signed Euclidean distance field.

[0032] Furthermore, the method for obtaining the path points is as follows:

[0033] The three-dimensional positions of the starting point and the target point are converted to the index number of the raster map, and the cost of the first extended point is calculated.

[0034] The search expands the node control space sampling iteration loop, while simultaneously calculating the time elapsed so far in the computational planning;

[0035] Determine whether the expanded node is in the closed set and whether a collision has occurred. Calculate the loss value of the expanded node using the Pontryagin extreme value.

[0036] Determine whether the current temporary extension node is in the same grid as other temporary extension nodes of the current node;

[0037] If they are in the same grid, prune them; determine if the loss value of the current temporary expansion node is smaller than the contrast loss value of the same grid. If it is smaller than the contrast loss value of the same grid, update this grid node as the current temporary expansion node.

[0038] If no pruning is performed, first determine whether the current temporary expansion node appears in the open set. If it does not appear in the open set, add the temporary expansion node directly to the open set. If it exists in the open set but has not yet been expanded, compare the loss value of the current temporary expansion node with that of the corresponding grid. If the loss value is smaller, update the node in the grid.

[0039] Furthermore, in the step of fitting a straight line to the path points to obtain the path curve:

[0040] The variable D of the multi-segment trajectory is:

[0041]

[0042] The matrix A constructed from multiple trajectories is:

[0043]

[0044] The coefficient matrix P of the path curve i for:

[0045]

[0046] Wherein: T i The actual time between trajectories. Let m represent the start or end time of the trajectory, i be the i-th segment of the trajectory, k be the k-th order variable, A be the parameter matrix constructed from the position, velocity, and acceleration of the start and end points of multiple trajectory segments, D be the parameter matrix constructed from the position, velocity, and acceleration of all discrete points of the trajectory, and P be the position, velocity, and acceleration of the trajectory. in This is the nth parameter of the i-th segment.

[0047] Furthermore, the unconstrained optimization problem for velocity, acceleration, obstacle avoidance distance, and curve smoothness takes the following form:

[0048] minλ1f s +λ2f o +λ3(f v +f a (20)

[0049] Among them, regarding the curve smoothness f s The form of the construction problem and the Jacobian matrix are shown below:

[0050]

[0051] Regarding obstacle avoidance distance f o The form of the construction problem and the Jacobian matrix are shown below:

[0052]

[0053] Regarding velocity f v The form of the construction problem and the Jacobian matrix are shown below:

[0054]

[0055] Regarding acceleration f a The form of the construction problem and the Jacobian matrix are shown below:

[0056]

[0057] Where: λ1, λ2, and λ3 are the weighting coefficients corresponding to curve smoothness, obstacle avoidance distance, and acceleration, respectively, and D... f D p These are the fixed parameter matrix and the parameter matrix to be optimized, respectively. The fixed parameters include the position, velocity, and acceleration of the starting and ending points, while the parameters to be optimized are the position, velocity, and acceleration of each discrete path point between the starting and ending points. C t A, Q are the coefficient matrix parameters of the path curve during the calculation process. The symmetric matrix formed by multiplying the three is represented by R. FF ,R PF ,R FP ,R PP To represent, J s J o J v J a c represents the first derivative of the corresponding optimization variable. a c v c represents the penalty function constructed from the corresponding acceleration, velocity, and obstacle avoidance distance. p represents the first derivative of the penalty function constructed from acceleration, velocity, and obstacle avoidance distance. u (t),v u (t),a u (t) The obstacle avoidance distance, acceleration, and velocity of this trajectory at time t, G. a ,G,F,v u Let τ and δt be the intermediate variable matrix in the calculation process, respectively, and let τ and δt be the total time T of the i-th trajectory segment. i And discrete time intervals, where α is a coefficient parameter.

[0058] The present invention also provides a storage medium comprising a stored program, wherein, when the program is executed, any of the above-described unmanned surface vessel energy-minimum trajectory planning methods are performed.

[0059] The present invention also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes any of the above-described unmanned surface vessel energy-minimum trajectory planning methods through the computer program.

[0060] Compared with the prior art, the present invention has the following advantages:

[0061] First, compared with existing unmanned surface vessel motion planning algorithms, this invention plans the trajectory of unmanned surface vessels based on visual information, which can avoid obstacles of arbitrary shapes in complex open water environments. At the same time, the algorithm has good real-time performance, and the unmanned surface vessel can make real-time local planning based on the surrounding obstacle environment.

[0062] Second, compared with the existing motion planning methods based on A-satellite in unmanned surface vessels, this invention considers the kinematic model of the ship, incorporates the minimum turning radius of the unmanned surface vessel as a criterion in the path point search, and designs a back-end trajectory optimization part to optimize the turning angle of the unmanned surface vessel, thereby reducing energy consumption.

[0063] Third, compared with the traditional three-dimensional occupancy grid map as the map for motion planning, this invention constructs a signed Euclidean distance field, which provides more gradient and distance information for subsequent motion planning algorithms, and is beneficial to the optimization of unmanned surface vessel trajectories. Attached Figure Description

[0064] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0065] Figure 1 This is a flowchart of the present invention.

[0066] Figure 2 This is a schematic diagram of solving the gradient of the Euclidean distance field with signs according to the present invention.

[0067] Figure 3 This is a schematic diagram of the Euclidean distance field with symbols around the unmanned surface vessel constructed according to the present invention.

[0068] Figure 4 This is a schematic diagram of straight line fitting for the path points obtained in this invention.

[0069] Figure 5 This is a schematic diagram illustrating the autonomous navigation of a single unmanned surface vessel in a complex environment according to the present invention.

[0070] Figure 6 This is a schematic diagram illustrating trajectory planning for a single unmanned surface vessel in a simulation environment constructed using the Berlin algorithm, according to the present invention. Detailed Implementation

[0071] To enable those skilled in the art to better understand the present invention, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of the present invention.

[0072] It should be noted that the terms "first," "second," etc., in the specification, claims, and accompanying drawings of this invention are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate so that the embodiments of the invention described herein can be implemented in orders other than those illustrated or described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover a non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0073] This invention provides a trajectory planning method for unmanned surface vessels (USVs) that minimizes energy consumption. First, based on depth image information acquired by a depth camera, a 3D occupancy grid map of the USV's current environment is continuously updated. A signed Euclidean distance field is constructed on the occupancy grid map using the Euclidean transformation algorithm. Then, an A* algorithm based on ship kinematics is used to search for front-end path points. The obtained path points are fitted with straight lines, and gradients and distances are calculated using trilinear interpolation. Based on the straight-line trajectory, an unconstrained optimization problem is constructed regarding obstacle avoidance distance, velocity, acceleration, and curve smoothness. Solving this problem yields a smooth, obstacle-avoiding optimal trajectory. Finally, a simulation platform is built using the Berlin algorithm to conduct simulation experiments on trajectory planning for a single USV.

[0074] A. Establishment of a 3D Occupation Raster Map

[0075] Let the grid size be m i The probability that a grid cell is occupied is p(m i =1), the probability of a grid cell being empty is p(m) i =0). Let the t-th observation be set as z. t z1, z2, z3, z4...z t It can also be written as z 1:t The objective is to calculate the posterior probability of occupying a grid cell over t known observations, i.e., p(m i =1|z 1:t ) and p(m i =0|z 1:t From Bayes' theorem, we can obtain:

[0076]

[0077] According to the Markov assumption, the current observation is independent of previous observations, therefore:

[0078] p(z t |z1:t-1 ,m i )=p(z t |m i (2)

[0079] The probabilities of a grid being occupied or empty are as follows:

[0080]

[0081] Let m be the grid i The t-th observation is denoted as l. t (m i The first t-1 observations are denoted as l. t-1 (m i Then, the above equation can be rearranged to obtain the following equation:

[0082]

[0083] p(m i |z t The inverse sensor model is p(z), which is then converted into a sensor model. t |m i The conversion formula is as follows:

[0084]

[0085] Substituting the sensor model into (4) yields:

[0086]

[0087] Given that for a fixed sensor, the grid information updated each time is a fixed value, and this value is continuously updated as the sensor data is updated, then l t (m i As the t-th observation is continuously updated, the 3D occupied grid map within the camera range is updated on the t-th observation.

[0088] B. Construction of the Signed Euclidean Distance Field

[0089] Let D(p) be the distance between the free grid p and the nearest obstacle grid, and let the obstacle grid be o. Let f(o) be the sampling function used to store the distance information for constructing the high-dimensional signed Euclidean distance field. The construction formula is as follows:

[0090] D(p)=min{(po) 2 +f(o)} (7)

[0091] The obtained map is divided into grids with a fixed resolution. Grids containing obstacles are set to 0, and grids without obstacles are set to 1. First, ignoring the y and z axes, the shortest distance from each grid to an obstacle is calculated along the x-axis. If there are no obstacles, f(o) is positive infinity. Second, considering the x and y axes, each obstacle grid with a value is treated as an obstacle grid, and the distance from the grid with an infinite value to the nearest grid with a value is calculated again. The same process applies when considering the x, y, and z axes. The resulting distance field constructs a positive distance field only in the obstacle-free areas. To construct a negative distance field in the obstacle-containing areas, the negative distance from the obstacle grid to the nearest free grid needs to be calculated, using the same method. This yields a signed Euclidean distance map where the distance to the obstacle grid is negative and the distance to the free grid is positive. The distance from the trajectory point to the nearest obstacle is calculated using trilinear interpolation. The gradient along the x, y, and z axes is obtained by differentiating the resulting distance function. Let C be the distance between the center points of the eight known grid cells. 000 C 001 C 010 C 011 C 100 C 101 C 110 C 111 Let the distances between the interpolation points be a, b, c, d, e, f, and g, where g is relative to C. 000 The position offset is (t) x ,t y ,t z The formula for solving the distance using trilinear interpolation is as follows:

[0092]

[0093] Let the gradient of g along the x-axis, y-axis, and z-axis be relative to grad. x (g),grad y (g),grad z (g) The formula for solving the gradient based on other interpolation points is shown below:

[0094]

[0095] C. A* algorithm based on ship kinematics

[0096] The 3D positions of the starting and target points are converted to their index numbers on the raster map, and the cost of the first expansion point is calculated. An iterative sampling loop is then performed to search for expansion nodes in the control space (based on the ship's kinematics model, categorized as a. maximum left turn, b. maximum right turn, c. straight, d. 1 / 2 maximum left turn, e. 1 / 2 maximum right turn). Simultaneously, the time elapsed since the planning stage is calculated: Time elapsed since planning stage = current time - start time. Expansion nodes are checked to see if they are in a closed set and whether a collision has occurred. The loss value of the expansion node is calculated using the Pontryagin extreme value. The current temporary expansion node is checked against other temporary expansion nodes in the same raster; if so, pruning is performed. The loss value of the current temporary expansion node is checked against the comparative loss value of nodes in the same raster; if so, this raster node is updated as the current temporary expansion node. If no pruning is performed, the current temporary expansion node is first checked against an open set; if not, it is directly added to the open set. If the node exists in an open set but has not yet been expanded, compare the loss value of the currently temporarily expanded node with that of the corresponding grid. If the loss value is smaller, update the node in the grid. The underlying ship kinematics formula is shown below:

[0097]

[0098] Let the current state of a node be (x0, y0, ψ0). After expansion, the state of the node will be (x0, y0, ψ0). t y t , ψ t The formula for node expansion is shown below:

[0099]

[0100] Based on the cost function calculation of the Pontryagin extreme value, the velocity and position of the expansion point and endpoint are determined. The optimal cost is calculated with the goal of minimizing acceleration. J(T) is designed as the objective cost function, and α... μ β μ For coefficient variables, the state variables at the expansion point are set as (p μc v μc The endpoint state variable is set to (p) μg v μg The formula is as follows:

[0101]

[0102] The optimal cost J(T) is calculated using the above formula and serves as the basis for selecting path points from the expansion points.

[0103] D. Using straight lines to fit curves to path points

[0104] The above-mentioned series of points, p0, p1, p2, ..., p, obtained from star A based on ship kinematics, are set as p0, p1, p2, ..., p n This generates n trajectories. Let the mapping matrix of the i-th segment be A. i Given a velocity v c Given an acceleration a c Given time t c Given distance d c The actual distance between the trajectories is denoted as s. i The actual time between trajectories is denoted as T. i The rough formula for calculating the time of this trajectory segment is as follows:

[0105]

[0106] If s i <2d c The time is then calculated according to the following formula:

[0107]

[0108] If s i >2d c The time is then calculated according to the following formula:

[0109]

[0110] For the i-th segment of the trajectory, construct A i The matrix is ​​shown below:

[0111]

[0112] Let variables Let m represent the start or end time of the trajectory, i represent the i-th segment of the trajectory, and k represent the k-th order variable. Then, the variable D of the multiple trajectories is represented as follows:

[0113]

[0114] The matrix A constructed from multiple trajectories is shown below:

[0115]

[0116] Let P be the coefficient matrix of each trajectory segment. i And the expression for the i-th segment of the trajectory is as follows:

[0117]

[0118] The expression for the coefficient matrix P of all trajectories is calculated as follows:

[0119]

[0120] E. Constructing an unconstrained optimization problem for velocity, acceleration, obstacle avoidance distance, and curve smoothness. The form of the problem is as follows:

[0121] minλ1f s +λ2f o +λ3(f v +f a (20)

[0122] Regarding the curve smoothness f s The form of the construction problem and the Jacobian matrix are shown below:

[0123]

[0124] Regarding obstacle avoidance distance f o The form of the construction problem and the Jacobian matrix are shown below:

[0125]

[0126] Regarding velocity f v The form of the construction problem and the Jacobian matrix are shown below:

[0127]

[0128] Regarding acceleration f a The form of the construction problem and the Jacobian matrix are shown below:

[0129]

[0130] The above unconstrained optimization problem is solved to obtain the optimal trajectory.

[0131] like Figure 1 As shown, a method for minimizing the energy consumption trajectory planning of an unmanned surface vessel includes the following steps:

[0132] The first step is to construct a signed Euclidean distance field based on the collected visual information using a trilinear interpolation algorithm, such as... Figure 2 and Figure 3 As shown, based on the information provided by the distance field, the A* algorithm based on ship kinematics is used to search for the required path points at the front end. The optimal cost between the extended path point and the endpoint is calculated based on the Pontryagin extreme value principle to select the path point, and a straight line is fitted as shown. Figure 3 and Figure 4 As shown.

[0133] The second step involves constructing an unconstrained optimization problem based on the fitted straight line, considering velocity, acceleration, curve smoothness, and obstacle avoidance distance. Solving this problem yields the optimal trajectory. This provides information such as position, velocity, and yaw angle for subsequent motion control of the unmanned surface vessel. Figure 4 and Figure 5 The image shows the straight-line trajectory of the unmanned surface vessel during its journey, as well as the optimized black trajectory.

[0134] Before planning begins, the initial position of the unmanned surface vessel (USV) is set at the origin of the world coordinate system. A target location is randomly selected. Within its perception area, the USV constructs a signed Euclidean distance field and continuously performs collision detection on the planned trajectory. If an obstacle is detected, the trajectory is replanned at the current safe position to avoid it. When the distance to the target point is detected to be too close, trajectory planning stops, and the velocity upon reaching the destination approaches zero. By considering curve smoothness as an optimization objective, energy loss during the USV's movement is minimized.

[0135] The present invention also provides a storage medium comprising a stored program, wherein, when the program is executed, any of the above-described unmanned surface vessel energy-minimum trajectory planning methods are performed.

[0136] The present invention also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes any of the above-described unmanned surface vessel energy-minimum trajectory planning methods through the computer program.

[0137] The sequence numbers of the above embodiments of the present invention are for descriptive purposes only and do not represent the superiority or inferiority of the embodiments.

[0138] In the above embodiments of the present invention, the descriptions of each embodiment have different focuses. For parts not described in detail in a certain embodiment, please refer to the relevant descriptions of other embodiments.

[0139] In the several embodiments provided in this application, it should be understood that the disclosed technical content can be implemented in other ways. The device embodiments described above are merely illustrative; for example, the division of units can be a logical functional division, and in actual implementation, there may be other division methods. For instance, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the displayed or discussed mutual coupling, direct coupling, or communication connection may be through some interfaces; the indirect coupling or communication connection between units or modules may be electrical or other forms.

[0140] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0141] Furthermore, the functional units in the various embodiments of the present invention can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.

[0142] If the integrated unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, read-only memory (ROM), random access memory (RAM), portable hard drives, magnetic disks, or optical disks.

[0143] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for energy-minimized trajectory planning of an unmanned surface vehicle, characterized in that, The method comprises the following steps: Collecting depth image information of the current environment of the unmanned surface vehicle using a depth camera, and constructing and updating a three-dimensional occupancy grid map of the current environment of the unmanned surface vehicle based on the depth image information; Constructing a signed Euclidean distance field based on the three-dimensional occupancy grid map using a Euclidean conversion algorithm; Searching for a front-end path point based on the three-dimensional occupancy grid map using an A-star algorithm based on ship kinematics, and obtaining the path point; Fitting a curve to the path point using a straight line to obtain a path curve; For the path curve, constructing an unconstrained optimization problem for speed, acceleration, obstacle avoidance distance, and curve smoothness based on the Euclidean distance field; Solving the unconstrained optimization problem for speed, acceleration, obstacle avoidance distance, and curve smoothness based on the Euclidean distance field to obtain an optimal obstacle avoidance trajectory.

2. The method of claim 1, wherein, The three-dimensional occupancy grid map is constructed by: Let the grid be , the probability of the grid being occupied be , also denoted as , the probability of the grid being free be , also denoted as ; let the observation at the t-th time be denoted as , the target be to calculate the posterior probability of the occupied grid at the t-th time, i.e. , by the Bayes formula, we have: , , , ... .​ （1） According to the Markov assumption, the current observation is independent of the previous observations, so: （2） According to the Bayes formula, the probability that the grid is occupied or free is as follows: （3） Then the above formula can be arranged as: （4） Convert between the sensor model and the inverse sensor model, and the conversion formula is as follows: （5） The sensor model is brought into (4) to obtain: （6） wherein: is the first observation of the grid is the second observation of the grid is the previous observation of the grid is the second observation of the grid is the first observation of the grid is the second observation of the grid based on the previous observation is the inverse sensor model for grid free and grid occupied, is the sensor model for grid free and grid occupied;​​​​​ For fixed sensors, the grid information is a fixed value for each update, and as the sensor data is constantly updated, the grid information is constantly updated As the tth observation is constantly updated, the update of the three-dimensional occupancy grid map within the camera range at the tth observation is completed.

3. The method of claim 1, wherein, The construction formula of the Euclidean distance field is: （7） Wherein: D(p) is the distance of the free grid p from the nearest obstacle grid, o is the obstacle grid, and f(o) is a sampling function for storing distance information for constructing a high-dimensional signed Euclidean distance field.

4. The method of claim 1, wherein, The path point is obtained in the following manner: Convert the three-dimensional positions of the starting point and the target point to index numbers of the grid map, and calculate the heuristic value of the first expansion point; Search the expansion node control space sampling iterative loop, and calculate the time consumed for the planning so far; Determine whether the expansion node is in the closed set and whether a collision occurs, and calculate the expansion node loss value using the Pontryagin extremum; Determine whether the current temporary expansion node and other temporary expansion nodes of the current node are in the same grid; If they are in the same grid, pruning is performed; Determine whether the loss value of the current temporary expansion node is smaller than the comparison loss value of the same grid, and if it is smaller, update the grid node to the current temporary expansion node; If pruning is not performed, first determine whether the current temporary expansion node appears in the open set, and if it does not, directly add the temporary expansion node to the open set, and if it exists in the open set but has not been expanded, compare the loss value of the current temporary expansion node with the corresponding grid, and if it is smaller, update the node in the grid.

5. The method of claim 1, wherein, In the step of fitting a curve to the path point using a straight line to obtain a path curve, the variable D of the multi-segment trajectory is: The A matrix of the multi-segment trajectory construction is: （16） The form of the unconstrained optimization problem for speed, acceleration, obstacle avoidance distance, and curve smoothness is: （17） a coefficient matrix of the path curve is: （19） wherein: is the actual time between trajectories, is the position, velocity or acceleration variable, wherein m represents the start or end time of the trajectory, i is the i-th trajectory segment, k is the k-th order variable, A is a parameter matrix constructed from the position, velocity, acceleration at the start and end of the multi-segment trajectory, and D is a parameter matrix constructed from the position, velocity, acceleration at all discrete points of the trajectory, is the n-th parameter of the i-th segment.

6. The method of claim 5, wherein, The storage medium comprises a stored program, wherein the program, when executed, performs the unmanned surface vehicle energy consumption minimum trajectory planning method of any one of claims 1 to 6. （20） wherein, with respect to the curve smoothness The problem is formulated and the Jacobian matrix is as follows: （21） On the obstacle avoidance distance The problem is formulated and the Jacobian matrix is given as follows: （22） On the speed The form of the construction problem and the Jacobian matrix are as follows: （23） Regarding acceleration The form of the construction problem and the Jacobian matrix are as follows: （24） wherein: are the weight coefficients corresponding to the curve smoothness, obstacle avoidance distance, and acceleration, respectively, are the fixed and the to-be-optimized variable matrices, respectively, the fixed variables include the positions, velocities, and accelerations of the start point and the end point, and the to-be-optimized variables are the positions, velocities, and accelerations of the discrete path points between the start point and the end point, is the coefficient matrix variable of the path curve in the calculation process, and the symmetric matrix formed by the multiplication of the three is denoted by denotes the first-order derivative of the corresponding optimization variable, denotes the penalty function constructed by the corresponding acceleration, velocity, and obstacle avoidance distance, denotes the first-order derivative of the penalty function constructed by the acceleration, velocity, and obstacle avoidance distance, are the obstacle avoidance distance, acceleration, and velocity at time t, is an intermediate variable matrix in the calculation process, are the total time and the discrete time interval of the i-th trajectory, respectively, is a coefficient variable.​ 7. A storage medium, characterized by ​ 8. An electronic device comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that, The processor executes the computer program to perform the unmanned surface vehicle energy consumption minimal trajectory planning method of any one of claims 1 to 6.

Images