Local planning method and device for unmanned ship and related components

Abstract

The invention discloses a local planning method and device for an unmanned ship and related components. The method comprises the following steps: acquiring a task route and a current position of the unmanned ship; obtaining a starting point and an ending point according to the task route and the current position, and determining a Frenet coordinate system according to the starting point and the ending point; sampling in time and space on the Frenet coordinate system to obtain sampling points; constructing a transverse planning function and a longitudinal planning function according to the sampling points, and obtaining a plurality of sampling tracks according to the transverse planning function and the longitudinal planning function; evaluating the plurality of sampling trajectories by using a trajectory evaluation function, and selecting the sampling trajectory with the minimum cost as a local route according to an evaluation result; and controlling the unmanned ship to travel according to the local route. According to the method, sampling is carried out in space and time, and the local route is obtained according to the surrounding environment information, so that the unmanned ship can better adapt to a complex water area environment, and the accuracy, adaptability and real-time performance of route planning are improved.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and in particular to a local planning method, device and related components of an unmanned ship. Background Art

[0002] With the continuous advancement of science and technology, unmanned driving technology has gradually been applied to various fields, among which the application of unmanned ships on the water is particularly prominent. Unmanned ships play an increasingly important role in water surface cleaning, environmental testing, rescue and other fields. However, traditional unmanned ship planning methods often have problems such as poor path planning accuracy, insufficient interactivity and poor real-time performance. Especially in complex water environments, if encountering other ships, water currents, obstacles, etc., unmanned ships cannot effectively avoid obstacles, so the autonomous and safe navigation of unmanned ships faces huge challenges. Summary of the invention

[0003] The purpose of the present invention is to provide a local planning method, device and related components for an unmanned ship, aiming to solve the problems of poor path planning accuracy, insufficient interactivity and poor real-time performance of traditional unmanned ship planning methods.

[0004] In a first aspect, an embodiment of the present invention provides a local planning method for an unmanned ship, comprising:

[0005] Obtain the mission route and the current position of the unmanned vessel;

[0006] Calculate the closest point to the unmanned ship on the mission route according to the current position, and use the closest point as the starting point;

[0007] Acquire an end point of the local route according to the starting point, and determine a Frenet coordinate system according to the starting point and the end point;

[0008] Sampling the motion trajectory of the unmanned ship in time and space in the Frenet coordinate system to obtain sampling points;

[0009] Constructing a lateral planning function and a longitudinal planning function according to the sampling points, and acquiring time information and space information according to the lateral planning function and the longitudinal planning function;

[0010] Converting the time information and the space information from the Frenet coordinate system to the Cartesian coordinate system to obtain a plurality of sampling trajectories;

[0011] The trajectory evaluation function is used to evaluate multiple sampling trajectories, and the sampling trajectory with the minimum cost is selected as the local route according to the evaluation results;

[0012] The unmanned boat is controlled to travel along the local route.

[0013] In a second aspect, an embodiment of the present invention provides a local planning device for an unmanned ship, comprising:

[0014] An acquisition unit, used to acquire the mission route and the current position of the unmanned ship;

[0015] A calculation unit, configured to calculate the closest point to the unmanned ship on the mission route according to the current position, and use the closest point as a starting point;

[0016] a determining unit, configured to obtain an end point of the local route according to the starting point, and determine a Frenet coordinate system according to the starting point and the end point;

[0017] A sampling unit, used for sampling the motion trajectory of the unmanned ship in time and space in the Frenet coordinate system to obtain sampling points;

[0018] A construction unit, configured to construct a lateral planning function and a longitudinal planning function according to the sampling points, and obtain time information and space information according to the lateral planning function and the longitudinal planning function;

[0019] A conversion unit, used to convert the time information and the space information from the Frenet coordinate system to the Cartesian coordinate system to obtain a plurality of sampling trajectories;

[0020] An evaluation unit, used for evaluating multiple sampling trajectories using a trajectory evaluation function, and selecting a sampling trajectory with the minimum cost as a local route according to the evaluation result;

[0021] A driving unit is used to control the unmanned boat to travel along the local route.

[0022] In a third aspect, an embodiment of the present invention further provides a computer device, which includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, the local planning method of the unmanned ship described in the first aspect is implemented.

[0023] In a fourth aspect, an embodiment of the present invention further provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the local planning method of the unmanned ship described in the first aspect above is implemented.

[0024] The present invention discloses a local planning method, device and related components of an unmanned ship. The method comprises: obtaining a task route and a current position of the unmanned ship; calculating the closest point to the unmanned ship on the task route according to the current position, and taking the closest point as a starting point; obtaining an end point of the local route according to the starting point, and determining a Frenet coordinate system according to the starting point and the end point; sampling the motion trajectory of the unmanned ship in time and space on the Frenet coordinate system to obtain sampling points; constructing a lateral planning function and a longitudinal planning function according to the sampling points, and obtaining time information and space information according to the lateral planning function and the longitudinal planning function; converting the time information and space information from the Frenet coordinate system to a Cartesian coordinate system to obtain multiple sampling trajectories; evaluating multiple sampling trajectories using a trajectory evaluation function, and selecting a sampling trajectory with the minimum cost as a local route according to the evaluation result; and controlling the unmanned ship to travel along the local route. The present invention enables the unmanned ship to better adapt to the complex water environment by sampling in space and time and obtaining a local route based on the surrounding environment information, thereby improving the accuracy, adaptability and real-time performance of path planning, enabling the unmanned ship to effectively avoid obstacles and improving the safety of the unmanned ship's automatic navigation. The embodiments of the present invention also provide a local planning device for an unmanned ship, a computer device and a readable storage medium, which have the above-mentioned beneficial effects and are not described in detail here. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0026] Figure 1 is a flow chart of a local planning method for an unmanned ship of this embodiment;

[0027] Figure 2 A schematic diagram of the mission route before and after smoothing;

[0028] Figure 3 A schematic diagram of the sampling trajectory at a certain time interval;

[0029] Figure 4 A schematic block diagram of a local planning device for an unmanned ship of this embodiment. DETAILED DESCRIPTION

[0030] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are 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 creative work are within the scope of protection of the present invention.

[0031] It should be understood that when used in this specification and the appended claims, the terms "include" and "comprises" indicate the presence of described features, integers, steps, operations, elements and / or components, but do not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components and / or combinations thereof.

[0032] It should also be understood that the terms used in this specification of the present invention are only for the purpose of describing specific embodiments and are not intended to limit the present invention. As used in the specification of the present invention and the appended claims, the singular forms "a", "an" and "the" are intended to include plural forms unless the context clearly indicates otherwise.

[0033] It should be further understood that the term "and / or" used in the present description and the appended claims refers to any and all possible combinations of one or more of the associated listed items, and includes these combinations.

[0034] See also Figure 1 The present invention provides a local planning method for an unmanned ship, comprising:

[0035] S101: Obtain the mission route and the current position of the unmanned ship;

[0036] In this embodiment, the task route can be obtained in the form of a starting point, a route, or any form that can be converted into these two forms. For the task form of the starting point, the task route of the starting point is obtained according to the area where the unmanned ship is located and the river information, and according to the A* algorithm or other global planning algorithms. The task route is obtained in a specific task, and the unmanned ship executes it once, and it is updated again after the corresponding task is completed.

[0037] Among them, the acquired mission route is often not smooth, so after acquiring the mission route and the current position of the unmanned ship, it includes:

[0038] The task route is smoothed using a B-spline curve. The smoothed task route is as follows: Figure 2 shown.

[0039] The equation of the B-spline curve is as follows:

[0040]

[0041] Where p(u) represents the point corresponding to u on the B-spline curve; [P 0 P 1 ...P n ] indicates a control point; B i,k (u) represents the i-th k-order B-spline basis function; B n,k (u) represents the nth k-order B-spline basis function; u represents the independent variable, and the k-order B-spline is a k-1 degree curve about u; P i represents the i-th control point.

[0042] The basis functions can be obtained by the de Boer-Cox recursion:

[0043]

[0044] Among them, u i 、u i+1 、u i+k-1 、u i+k All represent independent variables; B i,k-1 (u) represents the i-th k-1 order B-spline basis function; B i+1,k-1 (u) represents the i+1th k-1th order B-spline basis function.

[0045] In this embodiment, the unmanned cruise ship needs to obtain surrounding environment information from other modules in order to plan a more accurate and economical driving trajectory, including:

[0046] Obtain obstacle information from the perception module, including identification, outline, location, speed, direction and other information, and time and predict the future movement trajectory of the obstacle within a specific range (predicted time);

[0047] Obtain the unmanned cruise ship's own information from the positioning information, including location, speed, direction, etc.

[0048] Get the boundary information of the relevant ship from the map.

[0049] S102: Calculate the closest point to the unmanned ship on the mission route according to the current position, and use the closest point as the starting point;

[0050] S103: acquiring an end point of the local route according to the starting point, and determining a Frenet coordinate system according to the starting point and the end point;

[0051] Wherein, obtaining the end point of the local route according to the starting point includes: obtaining a point whose cumulative angle deviation does not exceed a predetermined fixed value or whose maximum length does not exceed a predetermined fixed distance according to the starting point, and taking the point as the end point of the local route. By limiting the conditions of the cumulative angle deviation and the maximum length, it can be ensured that the relative position relationship between the end point of the local route and the starting point meets expectations. This helps to improve the accuracy and reliability of path planning; at the same time, the end point position can be flexibly adjusted according to different starting points and actual conditions to meet different navigation needs.

[0052] In this embodiment, determining the Frenet coordinate system according to the starting point and the end point includes taking a line connecting the starting point and the end point as an s-axis of the Frenet coordinate system, thereby obtaining the Frenet coordinate system.

[0053] In this embodiment, the conversion relationship between the Cartesian coordinate system and the Frenet coordinate system is as follows:

[0054] Convert from Cartesian coordinates to Frenet coordinates:

[0055]

[0056] Where, s represents the Frenet ordinate; Sr represents the Frenet ordinate of the reference point; represents the longitudinal velocity of Frenet; v x Represents the velocity in the Cartesian coordinate system; θ x Indicates the direction of the Cartesian coordinate system; θ r Indicates the reference point angle; k r represents the reference point curvature; l represents the Frenet abscissa; represents the longitudinal acceleration of Frenet; a x represents the acceleration in the Cartesian coordinate system; l' represents the derivative of the Frenet transverse coordinate with respect to the longitudinal coordinate; k x represents curvature; k r ' represents the derivative of the curvature of the reference point; x x and x The Cartesian abscissa and ordinate of the point on the line are represented respectively; x r and r denote the Cartesian abscissa and Cartesian ordinate of the reference point respectively; l" denotes the second derivative of the Frenet abscissa with respect to the ordinate;

[0057] From Frenet coordinates to Cartesian coordinates:

[0058]

[0059] Among them, the Frenet coordinate system: Cartesian coordinate system: represents the Frenet lateral velocity; represents the Frenet lateral acceleration; It represents the coordinates in the Cartesian coordinate system, which is a vector.

[0060] S104: sampling the motion trajectory of the unmanned ship in time and space in the Frenet coordinate system to obtain sampling points;

[0061] Since the trajectory is composed of a series of points containing spatial and temporal information, in order to include the motion states of all unmanned ships, the motion trajectory of the unmanned ships needs to be sampled in space and time respectively.

[0062] Specifically, the motion trajectory of the unmanned ship is sampled in time and space in the Frenet coordinate system, and the sampling points include:

[0063] Perform longitudinal sampling from the current position to the end point at a fixed step length to obtain longitudinal sampling points;

[0064] According to the map information, the lateral drivable range of each longitudinal sampling point is calculated, and sampling is performed within the lateral drivable range to obtain lateral sampling points;

[0065] Perform lateral velocity sampling according to a fixed step length to obtain lateral velocity sampling points;

[0066] Perform time sampling at fixed time intervals to obtain time sampling points;

[0067] The longitudinal velocity is sampled according to a fixed step size to obtain longitudinal velocity sampling points.

[0068] This embodiment takes into account all important parameters of the unmanned ship during its movement, including position, speed, time, etc., through the above sampling method, so as to comprehensively describe the movement state of the ship and further plan the driving route of the unmanned ship more accurately.

[0069] In one embodiment, the spatial sampling is divided into horizontal sampling and vertical sampling. For the vertical sampling, directly from the current position of the unmanned ship s 0 Sampling is performed at a fixed step length ds until the end point is reached, forming n longitudinal sampling points [s 0 ,s 1 , ..., s n-1 ].

[0070] For horizontal sampling, calculate each longitudinal sampling point s according to the map information. k The lateral drivable range at min , l max], and uniformly sample m lateral sampling points [l 0 , l 1 ,…,l m-1 ],in Where int() is the rounding function, and dl is the sampling density parameter. For the lateral velocity dl / ds, directly sample s lateral velocity sampling points according to the fixed step size dl [-dl max , -dl max +dl,…,0,…,dl max -dl,dl max ], where dl max Indicates the maximum sampling rate.

[0071] The sampling of time is the sampling of the trajectory time cycle, but simply sampling time will increase the computational complexity, so it is converted into a sequence of time sampling points with a fixed time interval dt [t 0 , t 1 ,…,t q-1 ] to sample the speed of the unmanned ship at each point on the surface. k , the speed range is [v 0 -t k a max , v 0 -t k a min ], then a similar method to the lateral velocity sampling is used to sample r longitudinal velocity sampling points containing the maximum and minimum velocities at a fixed step length dv [v 0 , v 1 , …, v r-1 ], where a max Indicates the maximum acceleration; a min Indicates the minimum acceleration.

[0072] S105: constructing a transverse planning function and a longitudinal planning function according to the sampling points, and acquiring time information and space information according to the transverse planning function and the longitudinal planning function;

[0073] In this embodiment, constructing a lateral planning function and a longitudinal planning function according to sampling points includes: combining all longitudinal sampling points, lateral sampling points, lateral speed sampling points, time sampling points and longitudinal speed sampling points to obtain multiple combination states; fitting the multiple combination states through polynomials to obtain the lateral planning function and the longitudinal planning function; by fitting the lateral planning function and the longitudinal planning function using multiple combination states, the combination states can be flexibly adjusted according to actual conditions, thereby improving the flexibility of route planning.

[0074] In one embodiment, all the states obtained in step S104 are combined to obtain (mqrs) n Different combination states[s k , l k ,dl k , v k , t k ], where m represents the number of transverse sampling points; q represents the number of time sampling points; r represents the number of longitudinal velocity sampling points; s represents the number of transverse velocity sampling points; n represents the number of longitudinal sampling points; s k represents the kth longitudinal sampling point; l k represents the kth horizontal sampling point; dl k represents the kth lateral velocity sampling point; v k represents the kth longitudinal velocity sampling point; t k represents the kth time sampling point;

[0075] In order to further obtain the sampling trajectory, it is necessary to functionalize the state, that is, to generate a planning function with the horizontal l(s) and the vertical s(t) according to the sampling state. Both planning functions are fitted by polynomials. By sampling the third-order polynomial fitting, the continuity of velocity and acceleration can be met, and the smoothness requirements of the trajectory can be guaranteed.

[0076] Specifically, for a state sampling point [s k , l k ,dl k , v k , t k ] to [s k+1 , l k+1 ,dl k+1 , v k+1 , t k+1 ]、[s k , v k , t k ] and [s k+1 , v k+1 , t k+1 ], determine the longitudinal planning function s(t):

[0077]

[0078] Among them, s k+1 represents the k+1th longitudinal sampling point; l k+1 Indicates the k+1th horizontal sampling point; dl k+1 represents the k+1th lateral velocity sampling point; v k+1 represents the k+1th longitudinal velocity sampling point; t k+1 represents the k+1th time sampling point; s(t) represents the longitudinal trajectory of the tth time period; c0 、c 1 、c 2 and c 3 are all polynomial coefficients; v(t) represents the velocity trajectory of the tth time period;

[0079] The constraint function of the longitudinal planning function is:

[0080]

[0081] Among them, s(t k ) represents the tth k The longitudinal trajectory of a time period; v(t k ) represents the tth k The velocity trajectory of a time period; s(t k+1 ) represents the tth k+1 The longitudinal trajectory of a time period; v(t k+1 ) represents the tth k+1 The velocity trajectory of a time period;

[0082] By solving the equation, we can get the polynomial coefficients, that is, the longitudinal planning function.

[0083] Similar to the longitudinal planning function, [s k , l k ,dl k ] and [s k+1 , l k+1 ,dl k+1 ]Determine the lateral planning function l(s):

[0084]

[0085] Where l(s) represents the lateral trajectory corresponding to the longitudinal displacement s; a 0 、a 1 、a 2 and a 3 are all polynomial coefficients; dl(s) represents the lateral offset corresponding to the longitudinal displacement s;

[0086] Its constraint function is:

[0087]

[0088] Among them, l(s k ) represents the longitudinal displacement s k The corresponding lateral trajectory; dl(s k ) represents the longitudinal displacement s k The corresponding lateral offset; l(s k+1 ) represents the longitudinal displacement s k+1 The corresponding lateral trajectory; dl(s k+1) represents the longitudinal displacement s k+1 The corresponding lateral offset.

[0089] By solving the equation, the polynomial coefficients can be obtained, that is, the lateral planning function can be obtained.

[0090] S106: converting the time information and the space information from the Frenet coordinate system to the Cartesian coordinate system to obtain a plurality of sampling trajectories;

[0091] In this embodiment, all the [s, l, dl, ddl, v, a, t] information (i.e., time information and space information) on each trajectory can be obtained according to the lateral planning function and the longitudinal planning function; then, the [s, l, dl, ddl, v, a, t] information is converted from the Frenet coordinate system to the Cartesian coordinate system to form a sampling trajectory that the controller can run, where an example of a sampling trajectory at a certain time interval is as follows: Figure 3 shown.

[0092] In this embodiment, after converting the time information and the space information from the Frenet coordinate system to the Cartesian coordinate system to obtain a plurality of sampling trajectories, the following steps include: performing curvature and collision detection on each sampling trajectory.

[0093] Specifically, collision detection for each sampled trajectory includes: modeling the shape of the unmanned ship and obstacles in the environment as convex polygons; then setting a separation axis for the two convex polygons, the potential separation axis can be the normal of any polygon, or the normal of the line segment connecting the vertices of the two polygons; then projecting the two convex polygons onto each separation axis, and then checking whether these projections overlap. If the projections on all axes overlap, then the two polygons intersect (that is, the unmanned ship and the obstacle will collide); if there is an axis where the projections do not overlap, then the polygons do not intersect (that is, the unmanned ship and the obstacle will not collide). Curvature and collision detection can make the sampled trajectory meet the kinematic control requirements and collision-free requirements of the unmanned ship. After performing curvature and collision detection on each sampled trajectory, a safe and reliable trajectory cluster can be obtained. Although these trajectory clusters all meet the control requirements of the unmanned ship, they are not optimal. Therefore, it is necessary to select a set of optimal operating trajectories from all safe and reliable trajectory clusters. For this purpose, it is necessary to introduce a trajectory evaluation function to evaluate the sampled trajectories. The trajectory evaluation function is mainly used to make the trajectory of the unmanned ship as close to the mission route as possible, while the speed is as smooth as possible, without sudden changes, to meet the comfort requirements, and as far away from obstacles as possible.

[0094] In this embodiment, the method for constructing the trajectory evaluation function includes:

[0095] According to the lateral planning function and the longitudinal planning function, the lateral deviation, lateral speed, lateral acceleration, longitudinal speed, longitudinal acceleration, longitudinal jerk, the distance between the trajectory and the obstacle and the parameters of the control cost change are obtained;

[0096] Calculate the lateral cost based on the lateral deviation, lateral velocity and lateral acceleration;

[0097] Calculate the longitudinal cost based on the longitudinal velocity, longitudinal acceleration and longitudinal jerk;

[0098] The obstacle cost is calculated based on the distance of the trajectory from the obstacle and the parameters that control the change of the cost;

[0099] The trajectory evaluation function is obtained by weighted calculation based on the lateral cost, longitudinal cost and obstacle cost.

[0100] Specifically, the trajectory evaluation function cost is:

[0101] cost = w lat cost lat +w lon cost lon +w obs cost obs

[0102] Among them, cost lat 、cost lon and cost obs Respectively represent the horizontal cost, vertical cost and obstacle cost; w lat ,q lon 、w obs are the weights of the horizontal cost, vertical cost and obstacle cost respectively. The calculation formulas of the horizontal cost, vertical cost and obstacle cost are as follows:

[0103]

[0104] Among them, l, dl, ddl represent lateral deviation, lateral velocity and lateral acceleration respectively; Respectively represent the weights of lateral deviation, lateral velocity and lateral acceleration;

[0105] v, a, and j represent the longitudinal velocity, acceleration, and jerk, respectively; They represent the weights of longitudinal velocity, acceleration and jerk respectively;

[0106] d represents the distance between the sampled trajectory and the obstacle; σ represents the parameter that controls the change of the cost; The cost weight for avoiding obstacles.

[0107] S107: Evaluate multiple sampling trajectories using a trajectory evaluation function, and select a sampling trajectory with the minimum cost as a local route according to the evaluation result;

[0108] S108: Control the unmanned boat to travel along the local route.

[0109] In one embodiment, after the local route is acquired, the local route is sent to a controller, and the controller controls the unmanned boat to operate autonomously according to the local route.

[0110] See also Figure 4 This embodiment provides a local planning device 400 for an unmanned ship, including:

[0111] An acquisition unit 401 is used to acquire a mission route and a current position of the unmanned ship;

[0112] A calculation unit 402 is used to calculate the closest point to the unmanned ship on the mission route according to the current position, and use the closest point as a starting point;

[0113] A determination unit 403 is used to obtain an end point of the local route according to the starting point, and determine a Frenet coordinate system according to the starting point and the end point;

[0114] A sampling unit 404 is used to sample the motion trajectory of the unmanned ship in time and space in the Frenet coordinate system to obtain sampling points;

[0115] A construction unit 405 is used to construct a lateral planning function and a longitudinal planning function according to the sampling points, and obtain time information and space information according to the lateral planning function and the longitudinal planning function;

[0116] A conversion unit 406, configured to convert the time information and the space information from the Frenet coordinate system to the Cartesian coordinate system to obtain a plurality of sampling trajectories;

[0117] An evaluation unit 407, used to evaluate the plurality of sampling trajectories using a trajectory evaluation function, and select the sampling trajectory with the minimum cost as the local route according to the evaluation result;

[0118] The driving unit 408 is used to control the unmanned boat to travel along the local route.

[0119] Furthermore, the sampling unit 404 includes:

[0120] A first acquisition subunit is used to perform longitudinal sampling from the current position to the end point at a fixed step length to obtain longitudinal sampling points;

[0121] A second acquisition subunit is used to calculate the lateral drivable range of each longitudinal sampling point according to the map information, and to perform sampling within the lateral drivable range to obtain a lateral sampling point;

[0122] A third acquisition subunit is used to perform lateral velocity sampling according to a fixed step length to obtain lateral velocity sampling points;

[0123] A fourth acquisition subunit is used to perform time sampling at fixed time intervals to obtain time sampling points;

[0124] The fifth acquisition subunit is used to perform longitudinal velocity sampling according to a fixed step length to obtain longitudinal velocity sampling points.

[0125] Furthermore, the construction unit 405 includes:

[0126] a combining subunit, used for combining all of the longitudinal sampling points, the transverse sampling points, the transverse velocity sampling points, the time sampling points and the longitudinal velocity sampling points to obtain a plurality of combined states;

[0127] The fitting subunit is used to fit the plurality of combined states through polynomials to obtain a lateral planning function and a longitudinal planning function.

[0128] Furthermore, the evaluation unit 407 is also used for:

[0129] Obtaining lateral deviation, lateral speed, lateral acceleration, longitudinal speed, longitudinal acceleration, longitudinal jerk, distance of trajectory from obstacle and parameters of control cost change according to the lateral planning function and the longitudinal planning function;

[0130] Calculating a lateral cost according to the lateral deviation, the lateral velocity and the lateral acceleration;

[0131] Calculating a longitudinal cost based on the longitudinal velocity, the longitudinal acceleration and the longitudinal jerk;

[0132] Calculating an obstacle cost based on a distance of the trajectory from the obstacle and a parameter of a control cost change;

[0133] The trajectory evaluation function is obtained by weighted calculation based on the lateral cost, longitudinal cost and obstacle cost.

[0134] Furthermore, the conversion unit 406 is further configured to:

[0135] Perform curvature and collision detection on each sampled trajectory.

[0136] Furthermore, the acquisition unit 401 is further configured to:

[0137] The task route is smoothed using a B-spline curve.

[0138] Furthermore, the determining unit 403 is further configured to:

[0139] A point whose cumulative angle deviation does not exceed a predetermined fixed value or whose maximum length does not exceed a predetermined fixed distance is obtained according to the starting point, and the point is used as the end point of the local route.

[0140] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working processes of the above-mentioned devices and units can refer to the corresponding processes in the aforementioned method embodiments, and will not be repeated here.

[0141] The present invention also provides a computer-readable storage medium on which a computer program is stored, and when the computer program is executed, the method provided in the above embodiment can be implemented. The storage medium may include: a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk, and other media that can store program codes.

[0142] The present invention also provides a computer device, which may include a memory and a processor, wherein a computer program is stored in the memory, and when the processor calls the computer program in the memory, the method provided in the above embodiment can be implemented. Of course, the computer device may also include various network interfaces, power supplies and other components.

[0143] The various embodiments in the specification are described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same and similar parts between the various embodiments can be referred to each other. For the device disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant parts can be referred to the method part description. It should be pointed out that for ordinary technicians in this technical field, without departing from the principle of the present invention, several improvements and modifications can be made to the present invention, and these improvements and modifications also fall within the scope of protection of the claims of the present invention.

[0144] It should also be noted that, in this specification, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprises", or any other variations thereof are intended to cover non-exclusive.

[0145] Inclusion, so that the process, method, article or device including a series of elements includes not only those elements, but also includes other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of more restrictions, the elements defined by the sentence "including a..." do not exclude the existence of other identical elements in the process, method, article or device including the elements.

Claims

1. A local planning method for an unmanned ship, characterized in that: include: Obtain the mission route and the current position of the unmanned vessel; Calculate the closest point to the unmanned ship on the mission route according to the current position, and use the closest point as the starting point; Acquire an end point of the local route according to the starting point, and determine a Frenet coordinate system according to the starting point and the end point; Sampling the motion trajectory of the unmanned ship in time and space in the Frenet coordinate system to obtain sampling points; Constructing a lateral planning function and a longitudinal planning function according to the sampling points, and acquiring time information and space information according to the lateral planning function and the longitudinal planning function; Converting the time information and the space information from the Frenet coordinate system to the Cartesian coordinate system to obtain a plurality of sampling trajectories; The trajectory evaluation function is used to evaluate multiple sampling trajectories, and the sampling trajectory with the minimum cost is selected as the local route according to the evaluation results; The unmanned boat is controlled to travel along the local route.

2. The local planning method of an unmanned ship according to claim 1, characterized in that: The sampling of the motion trajectory of the unmanned ship in time and space in the Frenet coordinate system to obtain the sampling points includes: Perform longitudinal sampling from the current position to the end point at a fixed step length to obtain longitudinal sampling points; According to the map information, the lateral drivable range of each longitudinal sampling point is calculated, and sampling is performed within the lateral drivable range to obtain lateral sampling points; Perform lateral velocity sampling according to a fixed step length to obtain lateral velocity sampling points; Perform time sampling at fixed time intervals to obtain time sampling points; The longitudinal velocity is sampled according to a fixed step size to obtain longitudinal velocity sampling points.

3. The local planning method of an unmanned ship according to claim 2, characterized in that: The constructing of the lateral planning function and the longitudinal planning function according to the sampling points comprises: Combining all of the longitudinal sampling points, the transverse sampling points, the transverse velocity sampling points, the time sampling points and the longitudinal velocity sampling points to obtain a plurality of combination states; The plurality of combined states are fitted by polynomials to obtain a lateral planning function and a longitudinal planning function.

4. The local planning method of an unmanned ship according to claim 1, characterized in that: The method for constructing the trajectory evaluation function includes: Obtaining lateral deviation, lateral speed, lateral acceleration, longitudinal speed, longitudinal acceleration, longitudinal jerk, distance of trajectory from obstacle and parameters of control cost change according to the lateral planning function and the longitudinal planning function; Calculating a lateral cost according to the lateral deviation, the lateral velocity and the lateral acceleration; Calculating a longitudinal cost based on the longitudinal velocity, the longitudinal acceleration and the longitudinal jerk; Calculating an obstacle cost based on a distance of the trajectory from the obstacle and a parameter of a control cost change; The trajectory evaluation function is obtained by weighted calculation based on the lateral cost, longitudinal cost and obstacle cost.

5. The local planning method of an unmanned ship according to claim 1, characterized in that: The converting the time information and the space information from the Frenet coordinate system to the Cartesian coordinate system to obtain a plurality of sampling trajectories comprises: Perform curvature and collision detection on each sampled trajectory.

6. The local planning method of an unmanned ship according to claim 1, characterized in that: The process of obtaining the mission route and the current position of the unmanned ship includes: The task route is smoothed using a B-spline curve.

7. The local planning method of an unmanned ship according to claim 1, characterized in that: The step of obtaining the end point of the local route according to the starting point comprises: A point whose cumulative angle deviation does not exceed a predetermined fixed value or whose maximum length does not exceed a predetermined fixed distance is obtained according to the starting point, and the point is used as the end point of the local route.

8. A local planning device for an unmanned ship, characterized in that: include: An acquisition unit, used to acquire the mission route and the current position of the unmanned ship; A calculation unit, configured to calculate the closest point to the unmanned ship on the mission route according to the current position, and use the closest point as a starting point; a determining unit, configured to obtain an end point of the local route according to the starting point, and determine a Frenet coordinate system according to the starting point and the end point; A sampling unit, used for sampling the motion trajectory of the unmanned ship in time and space in the Frenet coordinate system to obtain sampling points; A construction unit, configured to construct a lateral planning function and a longitudinal planning function according to the sampling points, and obtain time information and space information according to the lateral planning function and the longitudinal planning function; A conversion unit, used to convert the time information and the space information from the Frenet coordinate system to the Cartesian coordinate system to obtain a plurality of sampling trajectories; An evaluation unit, used for evaluating multiple sampling trajectories using a trajectory evaluation function, and selecting a sampling trajectory with the minimum cost as a local route according to the evaluation result; A driving unit is used to control the unmanned boat to travel along the local route.

9. A computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the local planning method for the unmanned ship according to any one of claims 1 to 7 is implemented.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, which, when executed by a processor, enables the processor to perform the local planning method for an unmanned ship as described in any one of claims 1 to 7.

Images