A trajectory planning method for unmanned vessels based on sampling optimization

Abstract

The present invention provides a trajectory planning method for an unmanned vessel based on sampling optimization, relating to the technical field of unmanned vessel motion planning. The method comprises the following steps: constructing and updating a three-dimensional occupancy grid map of the unmanned vessel's current location in real time based on binocular image information; forming an octree map based on the leaf nodes and root nodes; searching the octree map for path points that avoid obstacles to obtain path points; obtaining a path curve; sampling the path curve for lateral and longitudinal trajectories according to the Freinet coordinate system; constructing evaluation functions for braking time, lateral distance from the trajectory, and speed, respectively, and obtaining evaluation function values for braking time, lateral distance from the trajectory, and speed; and selecting the trajectory with the smallest evaluation function value as the optimal trajectory. The present invention constructs the octree map using an obstacle expansion method and performs trajectory planning on the optimized octree map, thereby improving trajectory safety.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned vessel motion planning, and in particular to an unmanned vessel trajectory planning method based on sampling optimization. Background Art

[0002] With the advancement of communications and autonomous navigation technologies, unmanned vessels, as intelligent surface platforms, are gradually expanding their application into various civil and military fields. In the military, unmanned vessels, equipped with various sensor modules, can perform tasks such as minesweeping and maritime surveys. Trajectory planning technology is crucial for these tasks. During autonomous navigation, unmanned vessels detect and autonomously avoid unknown obstacles, then continue on their designated route. Therefore, even without human intervention, unmanned vessels can intelligently avoid obstacles, helping people perform tasks that are dangerous or unsuitable for manual operation.

[0003] With the continuous deepening of research in the field of unmanned vessels, many related institutions and laboratories have made significant progress and achievements, especially in trajectory planning for unmanned vessels. The task of trajectory planning is to calculate a collision-free, executable trajectory (including path and velocity information) to ensure that the unmanned vessel can safely navigate from its starting point to its destination while minimizing time and energy loss. This problem is essentially a multi-objective mathematical optimization problem. By optimizing relevant variables such as velocity, acceleration, and curvature, the optimal trajectory is obtained for the unmanned vessel to execute, ensuring the safety and stability of the unmanned vessel during autonomous navigation.

[0004] Existing trajectory planning methods for unmanned vessels (UAVs) mostly rely on static environmental maps or satellite maps of global ocean information. These existing UAV trajectory planning methods suffer from the following problems: First, sensors only collect information about surrounding obstacles and generate an updated three-dimensional occupancy grid map, which is then used as environmental modeling for path planning. No further processing of obstacle information, such as expansion of obstacles, is performed, making the generated trajectory safe. Second, the planning algorithm utilizes a grid-based graph search algorithm. The search speed depends on the grid density. Larger grid distances result in faster search speeds but lower map accuracy. Smaller grid distances increase map accuracy but require more storage space, significantly increasing search time. Third, high-definition maps cannot provide real-time updates of surrounding obstacles. As the UAV navigates, it cannot perceive obstacles within a small area in a timely manner, making it difficult to quickly avoid obstacles at close range. Summary of the Invention

[0005] In view of this, the purpose of the present invention is to propose an unmanned ship trajectory planning method based on sampling optimization, which constructs an octree map by expanding obstacles on the basis of a three-dimensional occupied grid map, and uses the octree map as a local environment model to solve the technical problem that the existing unmanned ship trajectory planning method is based on a grid map and cannot meet the speed and accuracy requirements at the same time.

[0006] The technical means adopted in the present invention are as follows:

[0007] A trajectory planning method for an unmanned vessel based on sampling optimization includes the following steps:

[0008] The binocular camera collects binocular image information, and constructs and updates the three-dimensional occupancy grid map of the unmanned ship in real time based on the binocular image information;

[0009] Taking a grid in the three-dimensional occupied grid map as a leaf node, expanding the grid and forming a root node together with surrounding expanded grids, and forming an octree map based on the leaf nodes and the root node;

[0010] A heuristic progressively optimal fast random search tree algorithm is used on the octree map to search for path points that avoid obstacles, thereby obtaining path points;

[0011] Performing curve fitting on the path points based on a cubic B-spline curve with the distance as a parameter to obtain a path curve;

[0012] The obtained path curve is decomposed into horizontal and vertical trajectories according to the Freinet coordinate system, and the horizontal and vertical trajectories are sampled respectively;

[0013] Constructing evaluation functions on braking time, lateral distance from the track, and speed for the lateral trajectory and the longitudinal trajectory respectively, and obtaining evaluation function values on braking time, lateral distance from the track, and speed respectively;

[0014] The trajectory with the smallest evaluation function value is selected as the optimal trajectory.

[0015] Furthermore, the three-dimensional occupancy grid map is constructed by:

[0016] The grid map stores information about whether it is occupied, with 1 indicating it is occupied and 0 indicating it is blank. The posterior probability of the t-th observation represents the grid's occupied information. Let i , the t-th observation is recorded as l t (o i ), the first t-1 observations are recorded as l t-1 (o i ), the three-dimensional occupancy grid map formula is as follows:

[0017]

[0018] As sensor data is continuously updated, t (o i ) is constantly updated, when l t (o i ) is greater than 1, it indicates that the grid is occupied, and less than 0, it indicates that the grid is idle.

[0019] Furthermore, the formula of the octree map is:

[0020]

[0021] Among them, x exindex ,y exindex ,z exindex is the expanded leaf node coordinate index value, x index ,y index ,z index is the root node coordinate index value, x maxindex ,y maxindex ,z maxindex The maximum coordinate index value.

[0022] Furthermore, the specific steps of using the heuristic progressively optimal fast random search tree algorithm to search for path points that avoid obstacles are as follows:

[0023] Given a complete map, the starting and ending points are given, and a random sampling phase begins. Based on the sampled results, the nearest node in the search tree is found. A new node is then expanded. A circle with a certain radius is drawn with the new node as the center. The cost of each node within the circle from the starting point and the sum of the costs from the nodes within the circle to the new node are calculated. The minimum cost is selected as the cost of the new node, and the corresponding node becomes the parent node of the new node. Once a feasible path is obtained, the sampling area is set to an ellipse with the starting and ending points as the foci. As the path length decreases, the elliptical area is reduced, shortening the time to reach the path point, resulting in a series of obstacle avoidance path points from the starting point to the end point.

[0024] Furthermore, the formula of the path curve is:

[0025] y i =a i +b i (xx i )+c i (xx i ) 2 +d i (xx i ) 3

[0026] Among them, y iis an axis variable of curve fitting, a i ,b i ,c i ,d i are the coefficient variables of curve fitting, x,x i are the parameter variables for curve fitting.

[0027] Furthermore, in the decomposition of the lateral trajectory:

[0028] The formula for sampling the lateral trajectory is:

[0029] d(t)=c d0 +c d1 t+c d2 t 2 +c d3 t 3 +c d4 t 4 +c d5 t 5

[0030] Where, d(t) is the lateral trajectory, c d0 ,c d1 ,c d2 ,c d3 ,c d4 ,c d5 is the trajectory coefficient variable, t is the time,

[0031] The formula of the horizontal scoring function is as follows:

[0032]

[0033] Among them, J t (d(t)) is the minimum energy consumption of the lateral trajectory, To integrate the third-order derivative of the lateral trajectory d(t) from t0 to t1, C d is the evaluation function of the lateral trajectory, k j ,k t ,k d is the weight coefficient corresponding to the optimization variable, T is the braking time, and d1 is the set lateral distance.

[0034] Furthermore, in the decomposition of the longitudinal trajectory:

[0035] The formula for sampling the longitudinal trajectory is:

[0036] s(t)=c s0 +c s1 t+c s2 t 2 +c s3 t 3 +cs4 t 4

[0037] Among them, s(t) is the longitudinal trajectory, c s0 ,c s1 ,c s2 ,c s3 ,c s4 ,c s5 is the coefficient variable of the longitudinal trajectory, t is the time,

[0038] The formula of the vertical scoring function is as follows:

[0039]

[0040] Among them, J t (s(t)) is the minimum energy consumption of the longitudinal trajectory, To integrate the third-order derivative of the longitudinal trajectory s(t) from t0 to t1, C s is the evaluation function of the longitudinal trajectory, k j ,k t ,k s is the weight coefficient corresponding to the optimization variable, T is the braking time, is the velocity value of the longitudinal trajectory, is the speed change of the set longitudinal trajectory.

[0041] The present invention also provides a storage medium, which includes a stored program, wherein when the program is run, any of the above-mentioned unmanned ship trajectory planning methods based on sampling optimization is executed.

[0042] The present invention also provides an electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that the processor executes any of the above-mentioned unmanned ship trajectory planning methods based on sampling optimization through the operation of the computer program.

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

[0044] First, compared with the existing unmanned ship trajectory planning algorithm, the present invention is based on a three-dimensional occupancy grid map, constructs an octree map by the obstacle expansion method, and performs trajectory planning on the optimized octree map, thereby improving the safety of the trajectory.

[0045] Second, compared with the existing planning method based on fast search random tree in unmanned ships, the present invention designs a penalty term for maintaining distance from the reference centerline for back-end trajectory optimization, taking into account the factor of minimum energy consumption, so that the generated trajectory is smoother when turning and has smaller acceleration, while avoiding obstacles and getting as close to the reference centerline as possible.

[0046] Third, compared with the existing planning method based on electronic chart information, this method is based on visual information, better reflects the information of surrounding obstacles, has better real-time performance, and is more conducive to the unmanned ship to avoid obstacles in a small range through local planning during navigation. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative labor.

[0048] Figure 1 Flowchart of the present invention.

[0049] Figure 2 Schematic diagram of an octree map constructed based on a three-dimensional occupancy grid map according to the present invention.

[0050] Figure 3 Schematic diagram of the heuristic progressively optimal fast random search tree algorithm of the present invention.

[0051] Figure 4 Schematic diagram of cubic B-spline curve fitting for the path points obtained in the present invention.

[0052] Figure 5 This is a schematic diagram of a single unmanned vessel of the present invention performing autonomous navigation based on the optimal trajectory sampled in a complex environment.

[0053] Figure 6 This is a schematic diagram of trajectory planning of a single unmanned vessel in a simulation environment constructed using the Perlin algorithm. DETAILED DESCRIPTION

[0054] In order to enable those skilled in the art to better understand the solutions of the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the embodiments described are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts should fall within the scope of protection of the present invention.

[0055] It should be noted that the terms "first", "second", etc. in the description and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that the numbers used in this way can be interchanged where appropriate, so that the embodiments of the present invention described herein can be implemented in an order other than those illustrated or described herein. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions. For example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0056] The present invention provides a trajectory planning method for an unmanned ship based on sampling optimization.

[0057] The following steps are involved:

[0058] A. Establishment of 3D occupancy grid map

[0059] The grid occupancy map stores information about whether it is occupied. Assume that 1 represents occupied and 0 represents blank. The posterior probability of the t-th observation represents the grid occupancy information. Suppose that for grid o i , the t-th observation is recorded as l t (o i ), the first t-1 observations are recorded as l t-1 (o i ), then the above formula can be rearranged into the following formula:

[0060]

[0061] As sensor data is continuously updated, t (o i ) is constantly updated. t (o i ) is greater than 1, it indicates that the grid is occupied, and less than 0, it indicates that the grid is idle.

[0062] B. Construction of octree map

[0063] Set the maximum bounds of the map (x max ,y max ,z max ), the minimum boundary is (x min ,y min ,z min ). The maximum index value of the map grid is (x maxindex ,y maxindex ,z maxindex ), the map resolution is set to r, and the maximum map index value calculation formula is as follows:

[0064]

[0065] For any map coordinate (x, y, z), the corresponding index value (x index ,y index ,z index )The calculation formula is as follows:

[0066]

[0067] The corresponding address of each map coordinate is set to addr i , the calculation formula is as follows:

[0068] addr i =x index y maxindex z maxindex +y index z maxindex +z index (4)

[0069] The corresponding grid can be accessed by accessing the corresponding address of the coordinate. For a certain occupied grid map coordinate (x, y, z), set the grid to be expanded to (x exindex ,y exindex ,z exindex ), after expansion, the grid after expansion is set to be occupied. The calculation formula for the grid to be expanded is as follows:

[0070]

[0071] At this point, the octree map is constructed.

[0072] C. Heuristic asymptotically optimal fast random search tree algorithm

[0073] Given a complete map, a starting point and an end point are given. A random sampling phase begins. Based on the sampled result, the nearest node in the tree is found and then expanded to obtain a new node. A circle with a certain radius is drawn with the new node as the center. The cost of each node within the circle from the starting point and the sum of the costs from the nodes within the circle to the new node are calculated. The minimum cost is selected as the cost of the new node, and the corresponding node becomes the parent node of the new node. Once a feasible path is obtained, the sampling area is set to an ellipse with the starting and end points as the foci. As the path length decreases, the ellipse is reduced, shortening the time to obtain a path point. This algorithm generates a series of obstacle avoidance path points from the starting point to the end point.

[0074] D. Use cubic B-spline curve for curve fitting

[0075] The above heuristic asymptotically optimal fast random search tree algorithm obtains a series of path points as p0, p1, p2, ..., p n , thus generating n segments of path curves, the horizontal and vertical coordinates of each segment of the path curve are set as (x i ,y i ), and assume that the unknown parameters of each path curve are (a i ,b i ,c i ,d i ), then the expression of the n-segment path curve is as follows:

[0076]

[0077] At the connection point of the i-th segment and the i+1-th path curve, x i At , the continuity constraint is satisfied, and the formula is as follows:

[0078]

[0079] For a multi-segment path curve, let the step length between each two points be h i , then h i The expression is as follows:

[0080] h i =x i+1 -x i (8)

[0081] For a multi-segment path curve, the A matrix constructed according to the step size is as follows:

[0082]

[0083] The intermediate variable matrix M is constructed as follows:

[0084]

[0085] The parameter matrix of each path curve is obtained by solving the following formula:

[0086]

[0087] The parameter formula for solving each path curve is as follows:

[0088]

[0089] The path curve is obtained at x i ≤x≤x i+1 Within the range, the curve of each path is obtained as follows:

[0090] y i =ai +b i (xx i )+c i (xx i ) 2 +d i (xx i ) 3 (13)

[0091] E. Sample the horizontal and vertical trajectories according to the Freinet coordinate system and design a scoring function to select the optimal trajectory

[0092] E1. Conversion between Freinet coordinate system and Cartesian coordinate system

[0093] The Freinet coordinate system takes the location of the unmanned vessel as the origin, the direction along the reference line as the s-axis, and the direction perpendicular to the reference line as the d-axis. The coordinates (x f ,y f ), the formula is as follows:

[0094]

[0095] Among them (a ix ,a iy ,b ix ,b iy ,c ix ,c iy ,d ix ,d iy ), let the coordinates in the Cartesian coordinate system be (x d ,y d ), the angle between the s-axis direction of the unmanned ship and the x-axis direction of the Cartesian coordinate system is set to θ r , the conversion formula between Freinet coordinate system and Cartesian coordinate system is as follows:

[0096]

[0097] E2. Design of lateral trajectory sampling and scoring function

[0098] The formula for sampling the lateral trajectory is:

[0099] d(t)=c d0 +c d1 t+c d2 t 2 +c d3 t 3 +c d4 t 4 +c d5 t 5 (16)

[0100] Sampling a period of time t0 t1, setting the initial lateral displacement, lateral velocity, and lateral acceleration to d(t0), Set the lateral displacement, lateral velocity, and lateral acceleration at the end of the trajectory to be d(t1), The expressions for the relevant variables are as follows:

[0101]

[0102] Assuming t0 = 0, the expressions of some coefficient variables are as follows:

[0103]

[0104] Let T = t1 - t0, and solve for the remaining coefficients. The formula is as follows:

[0105]

[0106] Since t1 is the sampling time, which is given by humans, the remaining coefficient parameter c can be obtained by solving the above matrix d3 ,c d4 ,c d5 For lateral trajectories, the sampling targets can be:

[0107]

[0108] By defining the minimum lateral displacement and the maximum lateral displacement, that is, (d min ,d max ), as well as the minimum sampling time and the maximum sampling time, that is, (T min ,T max ) to determine the sampling range of lateral displacement and time, and use ΔT and Δd to determine the sampling density of the two. At the same time, according to the minimum energy consumption J t (d(t)), braking time T, distance from the track d1 design scoring function C d , the relevant formula is as follows:

[0109]

[0110] By adjusting the weight coefficient k j ,k t ,k d To select trajectories with different requirements and obtain the optimal lateral trajectory.

[0111] E3. Design of longitudinal trajectory sampling and scoring function

[0112] The formula for sampling the longitudinal trajectory is:

[0113] s(t)=cs0 +c s1 t+c s2 t 2 +c s3 t 3 +c s4 t 4 (twenty two)

[0114] Sampling a period of time t0 t1, setting the initial longitudinal velocity and longitudinal acceleration respectively Set the longitudinal velocity and longitudinal acceleration at the end of the trajectory to be The expressions for the relevant variables are as follows:

[0115]

[0116] Assuming t0 = 0, the expressions of some coefficient variables are as follows:

[0117]

[0118] Let T = t1 - t0, and solve for the remaining coefficients. The formula is as follows:

[0119]

[0120] Since t1 is the sampling time, which is given by humans, the remaining coefficient parameter c can be obtained by solving the above matrix s3 ,c s4 . Assume that the speed sampling interval is For longitudinal trajectories, the sampling targets can be:

[0121]

[0122] By defining the minimum longitudinal velocity and the maximum longitudinal velocity, that is, And the minimum sampling time and the maximum sampling time, that is (T min ,T max ) to determine the sampling range, using ΔT and To determine the sampling density. At the same time, according to the minimum energy consumption J t (s(t)), braking time T, speed maintenance item design scoring function C s , the relevant formula is as follows:

[0123]

[0124] By adjusting the weight coefficient k j ,k t ,k s To select trajectories with different requirements and obtain the optimal longitudinal trajectory. Finally, the overall evaluation function is designed as follows:

[0125] C total =k lat C d +k lon C s (28)

[0126] C total is the total cost function value, by adjusting the weight parameter k lat ,k lon To adjust the longitudinal and lateral trajectory penalties, select C total The smallest trajectory is taken as the optimal trajectory, and then the optimal trajectory in the Cartesian coordinate system is obtained by formulas (14) and (15).

[0127] The present invention can be summarized into the following steps: First, based on the constructed three-dimensional occupancy grid map, the obstacle expansion is performed to construct an octree map, such as Figure 2 As shown in , based on the map information provided by the octree map, a heuristic progressive optimal fast random search tree algorithm is used to obtain the path points that avoid obstacles, as shown in Figure 3 As shown in the figure, the algorithm sets the sampling area to an ellipse to sample and select path points, and uses a cubic B-spline curve to fit the obtained path points with distance as the variable. Figure 4 shown.

[0128] In the second step, based on the B-spline path, the transverse and longitudinal trajectories are sampled in the Freinet coordinate system, and an evaluation function is constructed to calculate the cost of each trajectory. The transverse and longitudinal trajectories with the minimum cost are selected and converted into trajectories in the Cartesian coordinate system, and this trajectory is used as the optimal trajectory for the unmanned ship to finally travel. The implementation process and the sampling trajectory during the movement are shown in the figure. Figure 5 and Figure 6 shown.

[0129] When planning the UAV's trajectory, the initial pose is estimated using visual mapping through inter-frame matching. Random target points are selected on the simulation platform, and the UAV moves along the optimized trajectory. As the vehicle moves away from the destination, the longitudinal trajectory is sampled at a constant speed at the end of the trajectory. The motion process generally adheres to the principle of minimum energy consumption, meaning that the jerk is close to zero. When the vehicle is close to the destination, the longitudinal trajectory is sampled at a speed of zero at the end of the trajectory, and the vehicle finally reaches the destination.

[0130] The present invention also provides a storage medium, which includes a stored program, wherein when the program is run, the above-mentioned unmanned ship trajectory planning method based on sampling optimization is executed.

[0131] The present invention also provides an electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the above-mentioned unmanned ship trajectory planning method based on sampling optimization through the operation of the computer program.

[0132] The serial numbers of the above embodiments of the present invention are for description only and do not represent the advantages or disadvantages of the embodiments.

[0133] In the above embodiments of the present invention, the description of each embodiment has its own focus. For parts that are not described in detail in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

[0134] In the several embodiments provided in this application, it should be understood that the disclosed technical content can be implemented in other ways. Among them, the device embodiments described above are only exemplary. For example, the division of the units can be a logical function division. In actual implementation, there may be other division methods, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of units or modules, which can be electrical or other forms.

[0135] The units described as separate components may or may not be physically separate, and 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 may be selected according to actual needs to achieve the purpose of the present embodiment.

[0136] In addition, the functional units in the various embodiments of the present invention may be integrated into a single processing unit, each unit may exist physically separately, or two or more units may be integrated into a single unit. The aforementioned integrated units may be implemented in the form of hardware or software functional units.

[0137] If the integrated unit is implemented in the form of 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, 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. The computer software product is stored in a storage medium and includes several instructions for enabling a computer device (which can be a personal computer, server or network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: U disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), mobile hard disk, magnetic disk or optical disk, etc., various media that can store program code.

[0138] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the above embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements 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 trajectory planning method for an unmanned ship based on sampling optimization, characterized in that: The steps include: The binocular camera collects binocular image information, and constructs and updates the three-dimensional occupancy grid map of the unmanned ship in real time based on the binocular image information; Taking a grid in the three-dimensional occupied grid map as a leaf node, expanding the grid and forming a root node together with surrounding expanded grids, and forming an octree map based on the leaf nodes and the root node; A heuristic progressively optimal fast random search tree algorithm is used on the octree map to search for path points that avoid obstacles, thereby obtaining path points; Performing curve fitting on the path points based on a cubic B-spline curve with the distance as a parameter to obtain a path curve; The obtained path curve is decomposed into horizontal and vertical trajectories according to the Freinet coordinate system, and the horizontal and vertical trajectories are sampled respectively; In the decomposition of the lateral trajectory: The formula for sampling the lateral trajectory is: in, is the horizontal trajectory, is the trajectory coefficient variable, For time, The formula of the horizontal scoring function is as follows: in, is the minimum energy consumption of the lateral trajectory, For Time-dependent lateral trajectory Integrate the third-order derivative of is the evaluation function of the lateral trajectory, is the weight coefficient corresponding to the optimization variable, is the braking time, is the set horizontal distance; In the decomposition of the longitudinal trajectory: The formula for sampling the longitudinal trajectory is: in, is the longitudinal trajectory, is the coefficient variable of the longitudinal trajectory, For time, The formula of the vertical scoring function is as follows: in, is the minimum energy consumption of the longitudinal trajectory, For Longitudinal trajectory in time Integrate the third-order derivative of is the evaluation function of the longitudinal trajectory, is the weight coefficient corresponding to the optimization variable, is the braking time, is the velocity value of the longitudinal trajectory, is the speed change of the set longitudinal trajectory; Constructing evaluation functions on braking time, lateral distance from the track, and speed for the lateral trajectory and the longitudinal trajectory respectively, and obtaining evaluation function values on braking time, lateral distance from the track, and speed respectively; The trajectory with the smallest evaluation function value is selected as the optimal trajectory.

2. The unmanned ship trajectory planning method based on sampling optimization according to claim 1 is characterized in that: The method for constructing the three-dimensional occupancy grid map is: The grid map stores information about whether it is occupied, with 1 indicating it is occupied and 0 indicating it is blank. The posterior probability of the t-th observation represents the grid's occupied information. Let , the t-th observation is recorded as , the first t-1 observations are recorded as , then the three-dimensional occupancy grid map formula is as follows: As sensor data is continuously updated, Continuously updated, When the value is greater than 1, it indicates that the grid is occupied, and when it is less than 0, it indicates that the grid is idle.

3. The unmanned vessel trajectory planning method based on sampling optimization according to claim 1, characterized in that: The formula for the octree map is: in, is the expanded leaf node coordinate index value, is the root node coordinate index value, The maximum coordinate index value.

4. The unmanned vessel trajectory planning method based on sampling optimization according to claim 1, characterized in that: The specific steps of using the heuristic progressively optimal fast random search tree algorithm to search for path points that avoid obstacles are as follows: Under the premise of knowing the complete map, given the starting point and end point, enter the random sampling phase, find the nearest node in the tree obtained by sampling, and then expand to obtain a new node. With the new node as the center, draw a circle with a certain radius, calculate the cost of the nodes in the starting circle and the sum of the costs from the nodes in the circle to the new node one by one, select the minimum cost as the cost of the new node, and the corresponding node as the parent node of the new node until a feasible path is obtained. Then, set the sampling area to an ellipse with the starting point and end point as the focus. As the path length continues to shorten, the elliptical area is reduced to shorten the time to obtain the path point, and a series of obstacle avoidance path points from the starting point to the end point are obtained.

5. The unmanned vessel trajectory planning method based on sampling optimization according to claim 1, characterized in that: The formula of the path curve is: in, is an axis variable of curve fitting, is the coefficient variable of curve fitting, are the parameter variables for curve fitting.

6. A storage medium, characterized in that The storage medium includes a stored program, wherein when the program is run, the unmanned ship trajectory planning method based on sampling optimization according to any one of claims 1 to 5 is executed.

7. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: The processor executes the unmanned ship trajectory planning method based on sampling optimization according to any one of claims 1 to 5 through the operation of the computer program.

Images