CN115509260A - Trajectory planning method, device, equipment and storage medium - Google Patents

Abstract

The invention belongs to the technical field of automatic driving, and discloses a trajectory planning method, device, equipment and storage medium. The method includes: when an obstacle is detected in the target passing area, planning an optimal obstacle avoidance path; determining a plurality of path points satisfying preset interval requirements according to the optimal obstacle avoidance path; The flight time corresponding to the separation distance is allocated; the flight time and the point information corresponding to each path point are used as input, and the speed planning is carried out by using the minimum jerk algorithm to generate the driving trajectory. Through the above method, select the path points with appropriate intervals, avoid the generated trajectory deviating from the original path due to the excessive distance between adjacent path points, and use the minimum jerk algorithm for speed planning, so that the jerk of each segment trajectory can be kept at A certain value range improves the accuracy, smoothness and stability of trajectory planning.

Description

Trajectory planning method, device, equipment and storage medium

technical field

The present invention relates to the technical field of automatic driving, in particular to a trajectory planning method, device, equipment and storage medium.

Background technique

At present, the search and sampling algorithms such as A* or RRT are mostly used for planning obstacle avoidance paths, which consume a huge amount of computing power and are difficult to meet the real-time requirements. In the air flight scene, the search space is huge but the obstacles are sparse. The algorithm is too inefficient. In the current path smoothing and speed planning methods, there is a problem that the smoothed trajectory deviates too much from the original path, which reduces the trajectory positioning accuracy of the aircraft and affects the stability of the trajectory.

The above content is only used to assist in understanding the technical solution of the present invention, and does not mean that the above content is admitted as prior art.

Contents of the invention

The main purpose of the present invention is to provide a trajectory planning method, device, equipment and storage medium, aiming to solve the technical problem that the smoothed trajectory and the original path tend to deviate too much in the current path smoothing and speed planning methods.

To achieve the above object, the present invention provides a trajectory planning method, the method includes the following steps:

When detecting obstacles in the target passing area, plan the optimal obstacle avoidance path;

Determining a plurality of waypoints satisfying preset interval requirements according to the optimal obstacle avoidance path;

Allocate the corresponding flight time according to the distance between two adjacent waypoints;

The flight time and the point information corresponding to each path point are used as input, and the jerk minimization algorithm is used for speed planning to generate a driving trajectory.

Optionally, using the flight time and the point information corresponding to each way point as input, using the jerk minimization algorithm for speed planning, and after generating the driving trajectory, the method further includes:

Verifying the generated driving trajectory according to the maximum speed limit value and the maximum acceleration limit value;

When the check fails, adjust the flight time, and return to the step of using the flight time and the point information corresponding to each path point as input, using the jerk minimization algorithm for speed planning, and generating the driving trajectory. .

Optionally, when an obstacle is detected in the target passing area, planning an optimal obstacle avoidance path includes:

When an obstacle is detected in the target passing area, determine the position information and shape information of the obstacle;

processing the obstacle into a corresponding standardized three-dimensional shape according to the shape information;

Generate a plurality of trapezoidal obstacle avoidance paths with different distances from the route reference line according to the standardized three-dimensional shape and the position information;

An optimal obstacle avoidance path is selected from the plurality of trapezoidal obstacle avoidance paths.

Optionally, the selecting the optimal obstacle avoidance path from the plurality of trapezoidal obstacle avoidance paths includes:

removing a path whose distance from the obstacle is less than a preset safety distance from the plurality of trapezoidal obstacle avoidance paths to obtain a plurality of target obstacle avoidance paths;

Selecting the path with the smallest distance from the route reference line from the plurality of target obstacle avoidance paths as the optimal obstacle avoidance path.

Optionally, the determining a plurality of path points satisfying a preset interval requirement according to the optimal obstacle avoidance path includes:

Determining a plurality of polyline vertices corresponding to the optimal obstacle avoidance path;

Judging whether the interval distance between vertices of adjacent polylines is greater than a first preset distance value;

If the interval distance between adjacent polyline vertices is greater than the first preset distance value, a new reference point is added between adjacent polyline vertices;

A point whose distance from the current position is smaller than a second preset distance value is removed from the plurality of polyline vertices and the reference point, so as to obtain a plurality of route points satisfying a preset interval requirement.

Optionally, the plurality of waypoints include at least an end point, and the point information corresponding to each waypoint includes at least a terminal velocity direction and a terminal velocity magnitude corresponding to the end point;

Said using the flight time and the point information corresponding to each path point as input, using the jerk minimization algorithm for speed planning, before generating the driving track, the method also includes:

determining the final velocity direction according to the direction of the connection line between the terminal point and the previous path point of the terminal point;

Determine the magnitude of the terminal velocity according to the distance between the terminal point and the previous path point of the terminal point.

Optionally, when detecting an obstacle in the target passing area, before planning an optimal obstacle avoidance path, the method further includes:

Generate channel boundaries according to preset routes;

The target passage area is determined according to the channel boundary and the preset response range.

In addition, in order to achieve the above purpose, the present invention also proposes a trajectory planning device, which includes:

The path planning module is used to plan the optimal obstacle avoidance path when obstacles are detected in the target passing area;

A determining module, configured to determine a plurality of path points satisfying preset interval requirements according to the optimal obstacle avoidance path;

An allocation module, configured to allocate corresponding flight time according to the distance between any two adjacent waypoints;

The trajectory planning module is used to use the flight time and the point information corresponding to each path point as input, use the jerk minimization algorithm to perform speed planning, and generate a driving trajectory.

In addition, in order to achieve the above object, the present invention also proposes a trajectory planning device, which includes: a memory, a processor, and a trajectory planning program stored in the memory and operable on the processor. The trajectory planning program described above is configured to implement the trajectory planning method as described above.

In addition, to achieve the above object, the present invention also proposes a storage medium, on which a trajectory planning program is stored, and when the trajectory planning program is executed by a processor, the trajectory planning method as described above is implemented.

The present invention plans the optimal obstacle avoidance path when detecting obstacles in the target passage area; determines a plurality of path points that meet the preset interval requirements according to the optimal obstacle avoidance path; The flight time corresponding to the distance allocation; the flight time and the point information corresponding to each way point are used as input, and the speed planning is carried out by using the jerk minimization algorithm to generate the driving trajectory. Through the above method, select the path points with appropriate intervals, avoid the generated trajectory deviating from the original path due to the excessive distance between adjacent path points, and use the minimum jerk algorithm for speed planning, so that the jerk of each segment trajectory can be kept at A certain range of values and meeting the flight time limit improve the accuracy, smoothness and stability of trajectory planning.

Description of drawings

Fig. 1 is a schematic structural diagram of a trajectory planning device for a hardware operating environment involved in the solution of an embodiment of the present invention;

Fig. 2 is a schematic flow chart of the first embodiment of the trajectory planning method of the present invention;

Fig. 3 is a schematic diagram of the passing area of the trajectory planning method of the present invention;

Fig. 4 is a schematic flow chart of the second embodiment of the trajectory planning method of the present invention;

Fig. 5 is a schematic flow chart of the third embodiment of the trajectory planning method of the present invention;

Fig. 6 is a schematic diagram of path planning of the trajectory planning method of the present invention;

Fig. 7 is a schematic flow chart of an example of the trajectory planning method of the present invention;

FIG. 8 is a schematic flowchart of a fourth embodiment of the trajectory planning method of the present invention;

Fig. 9 is a structural block diagram of the first embodiment of the trajectory planning device of the present invention.

The realization of the purpose of the present invention, functional characteristics and advantages will be further described in conjunction with the embodiments and with reference to the accompanying drawings.

detailed description

It should be understood that the specific embodiments described here are only used to explain the present invention, not to limit the present invention.

Referring to FIG. 1 , FIG. 1 is a schematic structural diagram of a trajectory planning device for a hardware operating environment involved in the solution of an embodiment of the present invention.

As shown in FIG. 1 , the trajectory planning device may include: a processor 1001 , such as a central processing unit (Central Processing Unit, CPU), a communication bus 1002 , a user interface 1003 , a network interface 1004 , and a memory 1005 . Wherein, the communication bus 1002 is used to realize connection and communication between these components. The user interface 1003 may include a display screen (Display), an input unit such as a keyboard (Keyboard), and the optional user interface 1003 may also include a standard wired interface and a wireless interface. The network interface 1004 may optionally include a standard wired interface and a wireless interface (such as a Wireless-Fidelity (Wi-Fi) interface). The memory 1005 may be a high-speed random access memory (Random Access Memory, RAM), or a stable non-volatile memory (Non-Volatile Memory, NVM), such as a disk memory. Optionally, the memory 1005 may also be a storage device independent of the aforementioned processor 1001 .

Those skilled in the art can understand that the structure shown in FIG. 1 does not constitute a limitation to the trajectory planning device, and may include more or less components than shown in the figure, or combine some components, or arrange different components.

As shown in FIG. 1 , the memory 1005 as a storage medium may include an operating system, a network communication module, a user interface module, and a trajectory planning program.

In the trajectory planning device shown in Figure 1, the network interface 1004 is mainly used for data communication with the network server; the user interface 1003 is mainly used for data interaction with the user; the processor 1001 and the memory 1005 in the trajectory planning device of the present invention can be Set in the trajectory planning device, the trajectory planning device calls the trajectory planning program stored in the memory 1005 through the processor 1001, and executes the trajectory planning method provided by the embodiment of the present invention.

An embodiment of the present invention provides a trajectory planning method. Referring to FIG. 2 , FIG. 2 is a schematic flowchart of a first embodiment of the trajectory planning method according to the present invention.

In this embodiment, the trajectory planning method includes the following steps:

Step S10: When an obstacle is detected in the target passing area, plan an optimal obstacle avoidance path.

It can be understood that the execution subject of this embodiment is a trajectory planning device, and the trajectory planning device can be installed on machines such as automobiles, drones, aircrafts, flying cars, and driving robots. In this embodiment, an aircraft is taken as an example for description.

It should be noted that the target passage area is an area that the aircraft will pass through within a certain period of time in the future according to a preset route. When there is no obstacle within the boundary of the flight path, the aircraft will always fly along the preset route. When an obstacle is detected in the channel and the obstacle is in the target passage area, local barriers are performed to plan the optimal obstacle avoidance path.

In the specific implementation, the current driving data of the aircraft and the obstacle information in the environment are obtained, and the environment is constructed according to the obtained current driving data and obstacle information, and multiple obstacle avoidance paths are searched according to the preset algorithm in the constructed environment , select an optimal obstacle avoidance path from multiple obstacle avoidance paths, wherein the preset algorithm may be breadth-first search (BFS), Dijkstra algorithm (Dijkstra) and other algorithms, which are not limited in this embodiment. Preferably, multiple trapezoidal obstacle avoidance paths are searched in the constructed environment, and an optimal trapezoidal obstacle avoidance path is selected therefrom.

Further, before the step S10, the method further includes: generating a waterway boundary according to a preset route; and determining a target passage area according to the waterway boundary and a preset response range.

In a specific implementation, the route is generated by a preset global planning strategy, and the waterway boundary is generated according to the pre-planned route. Optionally, the preset response range is the detection range of the sensor used to detect obstacles on the aircraft; optionally, the preset response range is an obstacle detection parameter value determined in advance according to computing resources, which can be adjusted according to actual needs . Specifically, referring to FIG. 3, FIG. 3 is a schematic diagram of the passage area of the trajectory planning method of the present invention. The reference line is a part of the route intercepted according to the preset response range. For example, the preset response range is 200m in front of the aircraft, and the length corresponding to the reference line is 200m, and the fairway boundary is the boundary of the demarcated drivable area at a certain distance from the reference line, for example, the fairway boundary is the boundary 20m away from the reference line.

Step S20: Determine a plurality of route points satisfying the preset distance requirement according to the optimal obstacle avoidance route.

Optionally, a path point is extracted at intervals according to preset interval requirements on the optimal obstacle avoidance path, so as to obtain multiple path points.

Preferably, the optimal obstacle avoidance path is a trapezoidal obstacle avoidance path, the polyline vertices are extracted from the optimal obstacle avoidance path as path points, and path points are added or deleted between adjacent polyline vertices according to the preset interval requirements, thereby obtaining multiple path points, which avoids the difficulty of trajectory fitting caused by the complex shape of segments between adjacent path points.

Step S30: Allocate the corresponding flight time according to the distance between two adjacent waypoints.

It should be understood that the flight time of the segment formed by two adjacent waypoints is determined according to the distance between two adjacent waypoints divided by the maximum speed limit value.

Step S40: Taking the flight time and the point information corresponding to each way point as input, use the jerk minimization algorithm to carry out speed planning, and generate a driving trajectory.

It should be noted that in this embodiment, with the goal of minimizing jerk, smooth optimization and speed planning are performed on the planned optimal obstacle avoidance path. The multiple path points include the start point, end point and intermediate point of the optimal obstacle avoidance path, wherein the point information carried by the start point includes position information, speed information and acceleration information, and the point information carried by the end point includes position information, speed information and acceleration information, and the point information carried by the intermediate point includes position information. Taking the flight time and the point information corresponding to each path point as the input of the jerk minimization algorithm, a smooth and continuous driving trajectory carrying position information, speed information, acceleration information, and time information can be obtained. The driving trajectory is given in the form of discrete points at fixed time intervals, and each trajectory point contains position information, speed information, acceleration information and time information, which is convenient for the control module to perform driving control according to the trajectory points.

In this embodiment, when obstacles are detected in the target passage area, the optimal obstacle avoidance path is planned; according to the optimal obstacle avoidance path, a plurality of path points that meet the preset interval requirements are determined; according to the distance between two adjacent path points The flight time corresponding to the separation distance is assigned; the flight time and the point information corresponding to each path point are used as input, and the speed planning is carried out by using the jerk minimization algorithm to generate the driving trajectory. Through the above method, select the path points with appropriate intervals, avoid the generated trajectory deviating from the original path due to the excessive distance between adjacent path points, and use the minimum jerk algorithm for speed planning, so that the jerk of each segment trajectory can be kept at A certain range of values and meeting the flight time limit improve the accuracy, smoothness and stability of trajectory planning.

Referring to FIG. 4 , FIG. 4 is a schematic flowchart of a second embodiment of the trajectory planning method of the present invention.

Based on the first embodiment above, the trajectory planning method of this embodiment, after step S40, further includes:

Step S401: Verify the generated driving trajectory according to the maximum speed limit value and the maximum acceleration limit value.

It can be understood that the maximum speed limit value and the maximum acceleration limit value are the maximum speed and maximum acceleration that the aircraft's own power structure can achieve. The driving trajectory includes multiple trajectory points at fixed time intervals, each trajectory point contains position information, speed information, acceleration information and time information, and judges whether the speed corresponding to each trajectory point is less than the maximum speed limit value, and judges whether each trajectory point corresponds to Whether the acceleration is less than the maximum acceleration limit value, if the speeds corresponding to multiple track points are less than the maximum speed limit value, and the accelerations corresponding to multiple track points are all less than the maximum acceleration limit value, then it is determined that the verification is passed. If there is any track If the speed corresponding to the point is greater than the maximum speed limit value, or the acceleration corresponding to any track point is greater than the maximum acceleration limit value, it is determined that the verification fails.

Step S402: When the verification fails, adjust the flight time, and return to step S40.

It should be noted that this embodiment adopts an iterative optimization method. When the verification is passed, the driving trajectory that meets the requirements is obtained; Until the current number of iterations reaches the maximum number of iterations or the output driving trajectory meets the speed requirements and acceleration requirements.

In the specific implementation, when the verification fails, the flight time is increased, and the adjusted flight time and the point information of each path point are used as the input of the jerk minimization algorithm for speed planning. Optionally, obtain a preset time adjustment fixed value, add the time adjustment fixed value on the basis of the flight time of the segment formed by the original two adjacent waypoints, and obtain the adjusted flight time, for example, the time adjustment fixed value is 1s, when the verification fails, execute +1s processing on the flight time of each segment.

Optionally, determine the track point that fails the verification, determine the two adjacent path points of the target according to the position information of the track point, and adjust the flight time corresponding to the two adjacent path points of the target, specifically, in the original The time adjustment fixed value is added to the flight time of two adjacent waypoints of the target to obtain the adjusted flight time.

In this embodiment, the generated driving trajectory is verified according to the maximum speed limit and the maximum acceleration limit; when the verification fails, the flight time is adjusted, and the return execution takes the flight time and the point information corresponding to each path point as input, and uses The step of minimizing the jerk algorithm for speed planning and generating the driving trajectory. In this embodiment, an iterative optimization method is used for speed planning, and the flight time between adjacent path points in the trajectory is optimized, so that the generated one does not exceed the maximum speed limit, does not exceed the maximum acceleration limit, and does not deviate too much from the original path, which improves the trajectory planning. accuracy, smoothness and stability.

Referring to FIG. 5 , FIG. 5 is a schematic flowchart of a third embodiment of the trajectory planning method of the present invention.

Based on the first embodiment above, the step S10 of the trajectory planning method in this embodiment includes:

Step S101: When an obstacle is detected in the target passing area, determine the position information and shape information of the obstacle.

Step S102: Process the obstacle into a corresponding standardized three-dimensional shape according to the shape information.

It can be understood that the aircraft detects the position information and shape information of obstacles in the target passing area through its own sensors, and approximates the obstacles as a standardized three-dimensional shape according to the shape of the obstacle. Optionally, the standardized three-dimensional shape is a sphere Any one of shapes such as , cuboid, cylinder, etc., is convenient for calculating the distance between each position and obstacles, and reduces the computing power consumption of path planning.

Step S103: Generate multiple trapezoidal obstacle avoidance paths with different distances from the route reference line according to the standardized three-dimensional shape and the position information.

It should be noted that the path planning environment is constructed based on the standardized three-dimensional shape, location information and current driving data, and multiple trapezoidal obstacle avoidance paths with different distances from the route reference line are searched in the constructed environment. The route reference line is a part intercepted from the global planned route according to the preset response range. Referring to Fig. 6, Fig. 6 is a schematic diagram of the path planning of the trajectory planning method of the present invention. Along the up, down, left, and right sides of the reference line, several trapezoidal obstacle avoidance paths with different distances from the reference line are generated as shown in Fig. 6 . The starting point of the path is the location of the aircraft, and the ending point is a point 200m away from the reference line. As the aircraft moves forward, the obstacle avoidance path is updated every preset time period, and the number of generated trapezoidal obstacle avoidance paths can be determined according to hardware computing power and real-time requirements.

Step S104: Select an optimal obstacle avoidance path from the plurality of trapezoidal obstacle avoidance paths.

Optionally, multiple trapezoidal obstacle avoidance paths are comprehensively evaluated according to the length of each trapezoidal obstacle avoidance path, the distance to the obstacle, and the rotation angle to obtain a comprehensive score, and the path with the highest comprehensive score is selected as the optimal obstacle avoidance path .

Specifically, the step S104 includes: removing a path whose distance from the obstacle is smaller than a preset safety distance from the plurality of trapezoidal obstacle avoidance paths to obtain a plurality of target obstacle avoidance paths; Among the target obstacle avoidance paths, the path with the smallest distance to the route reference line is selected as the optimal obstacle avoidance path.

It should be understood that the distance between each trapezoidal obstacle avoidance path and the obstacle is calculated, the path that collides with the obstacle and the distance between the obstacle and the obstacle is less than the preset safety distance is excluded, and the distance from the reference line is selected from the remaining paths The smallest path is used as the final optimal obstacle avoidance path.

In a specific implementation, if it is determined that none of the generated multiple trapezoidal obstacle avoidance paths can avoid obstacles, hover and prompt, and wait for manual intervention.

Correspondingly, the step S20 includes: determining a plurality of polyline vertices corresponding to the optimal obstacle avoidance path; judging whether the distance between adjacent polyline vertices is greater than a first preset distance value; if the distance between adjacent polyline vertices is If the interval distance between the multiple polyline vertices and the reference point is greater than the first preset distance value, a new reference point is added between the adjacent polyline vertices; the distance between the multiple polyline vertices and the reference point is less than the second Points with a preset distance value can obtain multiple path points that meet the preset interval requirements.

It should be noted that multiple polyline vertices on the optimal obstacle avoidance path are extracted, and the positions of multiple polyline vertices are used as the input of the jerk minimization algorithm, that is, the ABCDEF point in Fig. 6 . The first preset distance value is a critical value set in advance for determining whether the distance between adjacent waypoints is too large, for example, any integer value in 25m, 50m, 25m-50m, if the distance between adjacent waypoints If the distance is greater than the first preset distance value, it indicates that the distance between adjacent waypoints is too large. The current position is the current position of the aircraft, which is the starting point of the trapezoidal obstacle path. The second preset distance value is a critical value set in advance for determining whether the distance between the waypoint and the current location is too small, for example, 15m. If the distance between a waypoint and the current location is smaller than the second preset distance value, it means The distance between the waypoint and the current position is too small.

In the specific implementation, adjust the determined path points: calculate the distance between adjacent path points, if the distance is too large, insert additional path points in the middle of adjacent path points, so that the maximum distance between adjacent path points is less than A certain value prevents the generated trajectory from deviating from the original path due to the excessive distance between the two path points. Further, removing the waypoints that are too close to the aircraft, for example, removing the waypoints within 15m from the current position of the aircraft, avoids unreasonable trajectory planning caused by the aircraft passing through the waypoints in a short time.

Compared with the traditional search and sampling algorithm, the method of finding the obstacle avoidance path in this embodiment reduces the calculation time, wherein, by planning the channel and the boundary of the channel, the obstacles far away from the route are directly excluded from the processing range; the obstacles in the channel are approximated Treating to normalized 3D shapes like spheres or cuboids can reduce the amount of computation needed to calculate distances to obstacles. In this embodiment, the fewer obstacles are detected, the less time it takes to calculate, which is suitable for scenarios such as flying in the air where the driving area is empty and there are few obstacles. The calculation time of the traditional algorithm is not significantly affected by the number of obstacles, and it still takes a lot of time to calculate in the face of an empty scene.

The trajectory planning method of this embodiment is described below in conjunction with an example:

With reference to Fig. 7, Fig. 7 is the specific flow chart diagram of an example of trajectory planning method of the present invention, the route (generally more than several kilometers) obtained by global planning is used as input, and the route boundary is generated according to the established route; if no obstacle is detected, the The current point of the aircraft and the end point of the reference line are used as the path points; if an obstacle is found within 200m in front of the flight path, the obstacle is approximated as a cuboid or sphere, and the obstacle avoidance path is planned based on a safe distance to find the optimal obstacle avoidance Path, when the obstacle avoidance path is not found, hover and wait for manual intervention; extract and adjust the path points from the optimal obstacle avoidance path; initially allocate time for each path that is driven by adjacent path points; minimize the jerk The algorithm calculates the trajectory; check whether the velocity and acceleration of each trajectory point meet the requirements; if so, output the trajectory; if not, increase the time allocated to each path, and return to the step of calculating the trajectory by minimizing the jerk algorithm, until the required trajectory is obtained.

In this embodiment, when an obstacle is detected in the target passing area, the position information and shape information of the obstacle are determined; the obstacle is processed into a corresponding standardized three-dimensional shape according to the shape information; Multiple trapezoidal obstacle avoidance paths with different distances between the reference lines; select the optimal obstacle avoidance path from the multiple trapezoidal obstacle avoidance paths. Through the above method, the obstacle is processed into a standardized three-dimensional shape, which facilitates the planning of trapezoidal obstacle avoidance paths, reduces the computing power consumption of path planning, and meets the real-time requirements of path planning.

Referring to FIG. 8 , FIG. 8 is a schematic flowchart of a fourth embodiment of a trajectory planning method according to the present invention.

Based on the above-mentioned first embodiment, in the trajectory planning method of this embodiment, the multiple path points include at least the terminal point, and the point information corresponding to each path point includes at least the terminal velocity direction and the final velocity direction corresponding to the terminal point. speed size;

Before the step S40, the method also includes:

Step S301: Determine the final velocity direction according to the direction of the line between the terminal point and the previous path point of the terminal point.

Step S302: Determine the magnitude of the terminal velocity according to the distance between the terminal point and the previous path point of the terminal point.

It can be understood that in the traditional trajectory planning method, the terminal velocity is generally set to 0, but in this embodiment, the terminal velocity is determined according to the information between the last two waypoints, which improves the accuracy of trajectory planning. Specifically, the direction of the final velocity is the same as the direction of the line connecting the last two path points, and the magnitude of the final velocity is determined by the distance between the last two path points, where the distance between the final point and the previous path point of the final point is The greater the distance, the greater the final speed, with the maximum speed limit as the upper limit; the smaller the distance between the end point and the previous path point of the end point, the smaller the end speed, with 0 as the lower limit.

In this embodiment, the final speed direction is determined according to the connection line between the terminal point and the previous path point of the final point; the terminal speed is determined according to the distance between the final point and the previous path point of the terminal point. Through the above method, the traditional jerk minimization algorithm is improved, and the final input velocity is determined according to the actual path point information, which meets the actual driving needs and improves the accuracy of trajectory planning.

In addition, an embodiment of the present invention also proposes a storage medium, on which a trajectory planning program is stored, and when the trajectory planning program is executed by a processor, the trajectory planning method as described above is implemented.

Since the storage medium adopts all the technical solutions of all the above-mentioned embodiments, it at least has all the beneficial effects brought by the technical solutions of the above-mentioned embodiments, which will not be repeated here.

Referring to FIG. 9, FIG. 9 is a structural block diagram of the first embodiment of the trajectory planning device of the present invention.

As shown in Figure 9, the trajectory planning device proposed by the embodiment of the present invention includes:

The path planning module 10 is configured to plan an optimal obstacle avoidance path when an obstacle is detected in the target passing area.

The determination module 20 is configured to determine a plurality of route points satisfying a preset interval requirement according to the optimal obstacle avoidance route.

The allocation module 30 is configured to allocate the corresponding flight time according to the distance between any two adjacent waypoints.

The trajectory planning module 40 is configured to use the flight time and point information corresponding to each waypoint as input, use the jerk minimization algorithm to perform speed planning, and generate a driving trajectory.

It should be understood that the above is only an example, and does not constitute any limitation to the technical solution of the present invention. In specific applications, those skilled in the art can make settings according to needs, and the present invention is not limited thereto.

In this embodiment, when obstacles are detected in the target passage area, the optimal obstacle avoidance path is planned; according to the optimal obstacle avoidance path, a plurality of path points that meet the preset interval requirements are determined; according to the distance between two adjacent path points The flight time corresponding to the separation distance is assigned; the flight time and the point information corresponding to each path point are used as input, and the speed planning is carried out by using the jerk minimization algorithm to generate the driving trajectory. Through the above method, select the path points with appropriate intervals, avoid the generated trajectory deviating from the original path due to the excessive distance between adjacent path points, and use the minimum jerk algorithm for speed planning, so that the jerk of each segment trajectory can be kept at A certain range of values and meeting the flight time limit improve the accuracy, smoothness and stability of trajectory planning.

It should be noted that the workflow described above is only illustrative and does not limit the protection scope of the present invention. In practical applications, those skilled in the art can select part or all of them to implement according to actual needs. The purpose of the scheme of this embodiment is not limited here.

In addition, for technical details not exhaustively described in this embodiment, reference may be made to the trajectory planning method provided in any embodiment of the present invention, which will not be repeated here.

In one embodiment, the trajectory planning device further includes an iteration module;

The iterative module is used to verify the generated driving trajectory according to the maximum speed limit value and the maximum acceleration limit value; when the verification fails, adjust the flight time and return to execute the described flight time And the point information corresponding to each path point is used as input, and the step of using the jerk minimization algorithm for speed planning to generate the driving trajectory.

In one embodiment, the path planning module 10 is further configured to determine the position information and shape information of the obstacle when an obstacle is detected in the target passing area; It is processed into a corresponding standardized three-dimensional shape; according to the standardized three-dimensional shape and the position information, a plurality of trapezoidal obstacle avoidance paths with different distances from the route reference line are generated; from the plurality of trapezoidal obstacle avoidance paths, the most Excellent obstacle avoidance path.

In an embodiment, the path planning module 10 is further configured to remove a path whose distance from the obstacle is smaller than a preset safety distance from the plurality of trapezoidal obstacle-avoiding paths to obtain a plurality of target obstacle-avoiding paths A path: selecting the path with the smallest distance from the route reference line from the plurality of target obstacle avoidance paths as the optimal obstacle avoidance path.

In an embodiment, the path planning module 10 is further configured to determine a plurality of polyline vertices corresponding to the optimal obstacle avoidance path; determine whether the distance between adjacent polyline vertices is greater than a first preset distance value; If the distance between adjacent polyline vertices is greater than the first preset distance value, add a reference point between adjacent polyline vertices; remove the difference between the plurality of polyline vertices and the reference point and the current position The distance between the points is smaller than the second preset distance value, and a plurality of path points satisfying the preset interval requirement are obtained.

In an embodiment, the multiple waypoints include at least an end point, and the point position information corresponding to each waypoint includes at least the terminal velocity direction and terminal velocity magnitude corresponding to the end point;

The trajectory planning device also includes a determination module;

The determining module is configured to determine the terminal speed direction according to the connection line between the terminal point and the previous path point of the terminal point; according to the terminal point and the previous path point of the terminal point The distance between waypoints determines the final velocity magnitude.

In one embodiment, the trajectory planning device further includes a range determination module;

The range determination module is configured to generate a waterway boundary according to a preset route; and determine a target passage area according to the waterway boundary and a preset response range.

Furthermore, it should be noted that in this document, the term "comprises", "comprises" or any other variation thereof is intended to cover a non-exclusive inclusion such that a process, method, article or system comprising a set of elements includes not only those elements, but also other elements not expressly listed, or elements inherent in such a process, method, article, or system. Without further limitations, an element defined by the phrase "comprising a..." does not preclude the presence of additional identical elements in the process, method, article or system comprising that element.

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

Through the description of the above embodiments, those skilled in the art can clearly understand that the methods of the above embodiments can be implemented by means of software plus a necessary general-purpose hardware platform, and of course also by hardware, but in many cases the former is better implementation. Based on such an understanding, the essence of the technical solution of the present invention or the part that contributes to the prior art can be embodied in the form of a software product, and the computer software product is stored in a storage medium (such as a read-only memory (Read Only Memory) , ROM)/RAM, magnetic disk, optical disk), including several instructions to make a terminal device (which can be a mobile phone, computer, server, or network device, etc.) execute the methods described in various embodiments of the present invention.

The above are only preferred embodiments of the present invention, and are not intended to limit the patent scope of the present invention. Any equivalent structure or equivalent process conversion made by using the description of the present invention and the contents of the accompanying drawings, or directly or indirectly used in other related technical fields , are all included in the scope of patent protection of the present invention in the same way.

Claims (10)

1. A trajectory planning method, characterized in that the trajectory planning method comprises:

planning an optimal obstacle avoidance path when an obstacle is detected to exist in the target traffic area;

determining a plurality of path points meeting the preset interval requirement according to the optimal obstacle avoidance path;

distributing corresponding flight time according to the interval distance between two adjacent path points;

and taking the flight time and point location information corresponding to each path point as input, and performing speed planning by using a minimum jerk algorithm to generate a driving track.

2. The trajectory planning method according to claim 1, wherein the flight time and the point location information corresponding to each path point are used as input, a minimum jerk algorithm is used for speed planning, and after the driving trajectory is generated, the method further comprises:

verifying the generated driving track according to the maximum speed limit value and the maximum acceleration limit value;

and adjusting the flight time when the verification fails, returning to execute the step of taking the flight time and the point location information corresponding to each path point as input, and performing speed planning by using a minimum jerk algorithm to generate a driving track.

3. The trajectory planning method according to claim 1, wherein when it is detected that an obstacle exists in the target passing area, planning an optimal obstacle avoidance path includes:

when an obstacle is detected to exist in a target passing area, determining position information and shape information of the obstacle;

processing the obstacle into a corresponding standardized three-dimensional shape according to the shape information;

generating a plurality of trapezoidal obstacle-detouring paths with different distances from a flight path reference line according to the standardized three-dimensional shape and the position information;

and selecting an optimal obstacle avoidance path from the plurality of trapezoidal obstacle avoidance paths.

4. The trajectory planning method according to claim 3, wherein the selecting an optimal obstacle avoidance path from the plurality of trapezoidal obstacle avoidance paths includes:

removing paths with the distance to the obstacle smaller than a preset safe distance from the plurality of trapezoidal obstacle detouring paths to obtain a plurality of target obstacle detouring paths;

and selecting a path with the minimum distance from the multiple target obstacle avoidance paths as an optimal obstacle avoidance path.

5. The trajectory planning method according to claim 3, wherein the determining a plurality of path points satisfying a preset interval requirement according to the optimal obstacle avoidance path includes:

determining a plurality of broken line vertexes corresponding to the optimal obstacle avoidance path;

judging whether the spacing distance between the vertexes of the adjacent fold lines is larger than a first preset distance value or not;

if the spacing distance between the vertexes of the adjacent fold lines is larger than a first preset distance value, adding a reference point between the vertexes of the adjacent fold lines;

and removing point positions with the distance between the point positions and the current position smaller than a second preset distance value from the plurality of broken line vertexes and the reference point to obtain a plurality of path points meeting preset interval requirements.

6. The trajectory planning method according to claim 1, wherein the plurality of path points include at least an end point, and the point location information corresponding to each path point includes at least an end speed direction and an end speed magnitude corresponding to the end point;

before the flight time and the point location information corresponding to each path point are used as input, a minimum jerk algorithm is used for speed planning, and a driving track is generated, the method further comprises the following steps:

determining a final speed direction according to the direction of a connecting line between the terminal point and a path point before the terminal point;

and determining the size of the final speed according to the distance between the terminal point and the path point before the terminal point.

7. The trajectory planning method according to any one of claims 1 to 6, wherein, before planning an optimal obstacle avoidance path upon detection of an obstacle existing within the target traffic area, the method further comprises:

generating a channel boundary according to a preset air route;

and determining a target passing area according to the channel boundary and a preset response range.

8. A trajectory planning device, characterized in that the trajectory planning device comprises:

the route planning module is used for planning an optimal obstacle avoidance route when an obstacle is detected to exist in the target traffic area;

the determining module is used for determining a plurality of path points meeting the requirement of a preset interval according to the optimal obstacle avoidance path;

the distribution module is used for distributing corresponding flight time according to the interval distance between any two adjacent path points;

and the track planning module is used for taking the flight time and the point location information corresponding to each path point as input, planning the speed by utilizing a minimum jerk algorithm and generating a driving track.

9. A trajectory planning apparatus, characterized in that the apparatus comprises: a memory, a processor, and a trajectory planning program stored on the memory and executable on the processor, the trajectory planning program being configured to implement the trajectory planning method of any one of claims 1 to 7.

10. A storage medium, characterized in that the storage medium has stored thereon a trajectory planning program which, when executed by a processor, implements the trajectory planning method according to any one of claims 1 to 7.

Images