CN115509260A - Trajectory planning method, apparatus, device and storage medium - Google Patents
Trajectory planning method, apparatus, device and storage medium Download PDFInfo
- Publication number
- CN115509260A CN115509260A CN202211314159.1A CN202211314159A CN115509260A CN 115509260 A CN115509260 A CN 115509260A CN 202211314159 A CN202211314159 A CN 202211314159A CN 115509260 A CN115509260 A CN 115509260A
- Authority
- CN
- China
- Prior art keywords
- path
- point
- planning
- obstacle
- trajectory planning
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 64
- 230000036461 convulsion Effects 0.000 claims abstract description 33
- 230000001133 acceleration Effects 0.000 claims description 21
- 238000012795 verification Methods 0.000 claims description 13
- 238000012545 processing Methods 0.000 claims description 9
- 230000004044 response Effects 0.000 claims description 9
- 238000001514 detection method Methods 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 11
- 238000004364 calculation method Methods 0.000 description 6
- 238000004891 communication Methods 0.000 description 5
- 238000000926 separation method Methods 0.000 description 5
- 230000008569 process Effects 0.000 description 4
- 238000009499 grossing Methods 0.000 description 3
- 238000005457 optimization Methods 0.000 description 3
- 238000005070 sampling Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Traffic Control Systems (AREA)
Abstract
The invention belongs to the technical field of automatic driving, and discloses a trajectory planning method, a trajectory planning device, trajectory planning equipment and a storage medium. The method comprises the following steps: when an obstacle is detected to exist in a target traffic area, planning an optimal obstacle avoidance path; 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. By the mode, the path points with proper intervals are selected, the generated track is prevented from deviating from the original path due to the fact that the distance between the adjacent path points is too large, speed planning is carried out by utilizing a minimum jerk algorithm, jerks of all segmented tracks are kept in a certain numerical range, and accuracy, smoothness and stability of track planning are improved.
Description
Technical Field
The invention relates to the technical field of automatic driving, in particular to a trajectory planning method, a trajectory planning device, trajectory planning equipment and a storage medium.
Background
At present, an obstacle avoidance path is planned by adopting search sampling algorithms such as A or RRT, the consumed computing power is huge, the real-time requirement is difficult to meet, and in an air flight scene, the search space is huge but the obstacles are sparse, so the efficiency of using the algorithms is too low. The problem that the smooth track and the original track are too large in deviation easily occurs in the current path smoothing and speed planning mode, the track positioning precision of the aircraft is reduced, and the stability of the track is influenced.
The above is only for the purpose of assisting understanding of the technical aspects of the present invention, and does not represent an admission that the above is prior art.
Disclosure of Invention
The invention mainly aims to provide a track planning method, a track planning device, track planning equipment and a storage medium, and aims to solve the technical problem that the deviation between a smoothed track and an original path is too large easily in the conventional path smoothing and speed planning mode.
In order to achieve the above object, the present invention provides a trajectory planning method, comprising the steps of:
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 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.
Optionally, after the flight time and the point location information corresponding to each path point are used as input, and a speed planning is performed by using a minimum jerk algorithm to generate a driving track, the method further includes:
verifying the generated running 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.
Optionally, when it is detected that an obstacle exists in the target traffic 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.
Optionally, 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.
Optionally, the determining, according to the optimal obstacle avoidance path, a plurality of path points that meet a preset interval requirement 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, newly 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.
Optionally, the multiple path points at least include a terminal point, and the point location information corresponding to each path point at least includes a terminal speed direction and a terminal speed magnitude corresponding to the terminal point;
before the flight time and the point location information corresponding to each path point are used as input, and a minimum jerk algorithm is used for speed planning to generate a driving track, 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 final point and the path point before the final point.
Optionally, before planning the optimal obstacle avoidance path when it is detected that an obstacle exists in the target traffic area, the method further includes:
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.
In addition, to achieve the above object, the present invention further provides a trajectory planning device, including:
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 preset interval requirement 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.
In addition, to achieve the above object, the present invention further provides a trajectory planning apparatus, including: 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 a trajectory planning method as described above.
In addition, to achieve the above object, the present invention further provides a storage medium having a trajectory planning program stored thereon, where the trajectory planning program, when executed by a processor, implements the trajectory planning method as described above.
According to the method, when the obstacle existing in the target traffic area is detected, an optimal obstacle avoidance path is planned; 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. By the mode, the path points with proper intervals are selected, the generated track is prevented from deviating from the original path due to the fact that the distance between the adjacent path points is too large, speed planning is carried out by utilizing a minimum jerk algorithm, jerks of all segmented tracks are kept in a certain numerical range and meet the limitation of flight time, and accuracy, smoothness and stability of track planning are improved.
Drawings
Fig. 1 is a schematic structural diagram of a trajectory planning device of a hardware operating environment according to an embodiment of the present invention;
FIG. 2 is a schematic flow chart diagram illustrating a first embodiment of a trajectory planning method according to the present invention;
FIG. 3 is a schematic view of a traffic area of the trajectory planning method of the present invention;
FIG. 4 is a schematic flow chart illustrating a second embodiment of a trajectory planning method according to the present invention;
FIG. 5 is a flowchart illustrating a third embodiment of a trajectory planning method according to the present invention;
FIG. 6 is a schematic diagram of path planning according to the trajectory planning method of the present invention;
FIG. 7 is a schematic view of a detailed flow chart of an example of a trajectory planning method according to the present invention;
FIG. 8 is a flowchart illustrating a fourth embodiment of a trajectory planning method according to the present invention;
fig. 9 is a block diagram of the first embodiment of the trajectory planning device according to the present invention.
The implementation, functional features and advantages of the objects of the present invention will be further explained with reference to the accompanying drawings.
Detailed Description
It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.
Referring to fig. 1, fig. 1 is a schematic structural diagram of a trajectory planning device of a hardware operating environment according to an embodiment of the present invention.
As shown in fig. 1, the trajectory planning apparatus may include: a processor 1001, such as a Central Processing Unit (CPU), a communication bus 1002, a user interface 1003, a network interface 1004, and a memory 1005. The communication bus 1002 is used to implement connection communication among 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, a wireless interface. The network interface 1004 may optionally include a standard wired interface, a Wireless interface (e.g., a Wireless-Fidelity (Wi-Fi) interface). The Memory 1005 may be a Random Access Memory (RAM) or a Non-Volatile Memory (NVM), such as a disk Memory. The memory 1005 may alternatively be a storage device separate from the processor 1001 described previously.
It will be appreciated by those skilled in the art that the configuration shown in figure 1 does not constitute a limitation of the trajectory planning device and may include more or fewer components than shown, or some components may be combined, or a different arrangement of components.
As shown in fig. 1, a memory 1005, which is a storage medium, may include therein an operating system, a network communication module, a user interface module, and a trajectory planning program.
In the trajectory planning device shown in fig. 1, the network interface 1004 is mainly used for data communication with a network server; the user interface 1003 is mainly used for data interaction with a user; the processor 1001 and the memory 1005 in the trajectory planning device of the present invention may be disposed in the trajectory planning device, and 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 in the embodiment of the present invention.
An embodiment of the present invention provides a trajectory planning method, and referring to fig. 2, fig. 2 is a schematic flow diagram 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: and when the obstacle existing in the target traffic area is detected, planning an optimal obstacle avoidance path.
It can be understood that the execution main body of the embodiment is a trajectory planning device, and the trajectory planning device can be arranged on a vehicle, an unmanned aerial vehicle, an aircraft, a flying vehicle, a driving robot and other machine devices. The present embodiment is described by taking an aircraft as an example.
It should be noted that the target passing area is an area where the aircraft will pass in a future period of time according to a preset route. When no obstacle is present within the airway boundary, the aircraft is flying along a preset course at all times. And when the obstacle is detected to exist in the channel and is positioned in the target passing area, performing local obstacle avoidance and planning an optimal obstacle avoidance path.
In the specific implementation, the current driving data of the aircraft and the obstacle information under the environment are acquired, the environment is constructed according to the acquired current driving data and the obstacle information, a plurality of obstacle avoidance paths are searched in the constructed environment according to a preset algorithm, and an optimal obstacle avoidance path is selected from the plurality of obstacle avoidance paths, wherein the preset algorithm can be a width-first search (BFS), dijkstra algorithm (Dijkstra), and the like, and the method is not limited in this embodiment. Preferably, a plurality of trapezoidal obstacle avoidance paths are searched in the constructed environment, and an optimal trapezoidal obstacle avoidance path is selected from the trapezoidal obstacle avoidance paths.
Further, before the step S10, the method further includes: 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.
In the specific implementation, a route is generated by a preset global planning strategy, and a channel boundary is generated according to a pre-planned route. Optionally, the preset response range is a detection range of a sensor on the aircraft for detecting the obstacle; optionally, the preset response range is an obstacle detection parameter value determined in advance according to the computing power resource, and can be adjusted according to actual requirements. Specifically, referring to fig. 3, fig. 3 is a schematic diagram of a traffic area of the trajectory planning method of the present invention, where a reference line is a part of a flight line cut according to a preset response range, for example, the preset response range is 200m in front of an aircraft, a length of the reference line is 200m, and a channel boundary is a boundary of a defined travelable area at a certain distance from the reference line, for example, the channel boundary is a boundary 20m away from the reference line.
Step S20: and determining a plurality of path points meeting the preset interval requirement according to the optimal obstacle avoidance path.
Optionally, one path point is extracted from the optimal obstacle avoidance path at intervals according to a preset interval requirement, so as to obtain a plurality of path points.
Preferably, the optimal obstacle avoidance path is a trapezoidal obstacle avoidance path, the top points of the broken lines are extracted from the optimal obstacle avoidance path to serve as path points, and the path points are added or deleted between the top points of the adjacent broken lines according to the preset interval requirement, so that a plurality of path points are obtained, and the increase of the difficulty of track fitting caused by the complicated sectional shape between the adjacent path points is avoided.
Step S30: and distributing corresponding flight time according to the spacing distance between two adjacent path points.
It will be appreciated that the time of flight of the segment formed by two adjacent waypoints is determined in dependence upon the separation distance between the two adjacent waypoints divided by the maximum speed limit.
Step S40: and 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.
It should be noted that, in this embodiment, smooth optimization and speed planning are performed on the planned optimal obstacle avoidance path with the goal of minimizing jerk. The multiple path points comprise a starting point, an end point and a middle point of the optimal obstacle avoidance path, wherein point location information carried by the starting point comprises position information, speed information and acceleration information, point location information carried by the end point comprises position information, speed information and acceleration information, and point location information carried by the middle point comprises position information. And taking the flight time and point location information corresponding to each path point as the input of a minimum jerk algorithm, so as to obtain a smooth and continuous driving track carrying position information, speed information, acceleration information and time information. The driving track is given in a discrete point form at fixed time intervals, and each track point comprises position information, speed information, acceleration information and time information, so that the control module can conveniently control driving according to the track points.
In the embodiment, when the obstacle existing in the target traffic area is detected, an optimal obstacle avoidance path is planned; 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. By the mode, the path points with proper intervals are selected, the generated track is prevented from deviating from the original path due to the fact that the distance between the adjacent path points is too large, speed planning is carried out by utilizing a minimum jerk algorithm, jerk of each segmented track is kept in a certain numerical range, the limitation of flight time is met, and the accuracy, smoothness and stability of track planning are improved.
Referring to fig. 4, fig. 4 is a flowchart illustrating a second embodiment of the trajectory planning method according to the present invention.
Based on the first embodiment, after the step S40, the trajectory planning method of this embodiment further includes:
step S401: and verifying the generated running track according to the maximum speed limit value and the maximum acceleration limit value.
It will be appreciated that the maximum speed limit and the maximum acceleration limit are the maximum speed and the maximum acceleration that can be achieved by the dynamic structure of the aircraft itself. The driving track comprises a plurality of track points at fixed time intervals, each track point comprises position information, speed information, acceleration information and time information, whether the speed corresponding to each track point is smaller than a maximum speed limit value or not is judged, whether the acceleration corresponding to each track point is smaller than a maximum acceleration limit value or not is judged, if the speeds corresponding to the track points are smaller than the maximum speed limit value and the accelerations corresponding to the track points are smaller than the maximum acceleration limit value, the verification is determined to be passed, and if the speed corresponding to any track point is larger than the maximum speed limit value or the acceleration corresponding to any track point is larger than the maximum acceleration limit value, the verification is determined to be failed.
Step S402: and when the verification fails, adjusting the flight time, and returning to execute the step S40.
It should be noted that, in the embodiment, an iterative optimization mode is adopted, and when the verification passes, a running track meeting the requirement is obtained; and when the verification and verification are not passed, adjusting the distributed flight time, and performing path smoothing and speed planning processing again until the current iteration number reaches the maximum iteration number or the output driving track meets the speed requirement and the acceleration requirement.
In the specific implementation, when the verification fails, the flight time is increased, and the adjusted flight time and point location information of each path point are used as the input of the minimum jerk algorithm to perform speed planning. Optionally, a preset time adjustment fixed value is obtained, the time adjustment fixed value is added on the basis of the flight time of the segment formed by the two adjacent waypoints originally, the adjusted flight time is obtained, for example, the time adjustment fixed value is 1s, and when the verification fails, the +1s processing is performed on the flight time of each segment.
Optionally, a trace point which fails to pass the verification is determined, two adjacent path points of the target where the target is located are determined according to the position information of the trace point, and the flight time corresponding to the two adjacent path points of the target is adjusted, specifically, a time adjustment fixed value is added on the basis of the original flight time of the two adjacent path points of the target, so that the adjusted flight time is obtained.
In the embodiment, the generated running track is verified according to the maximum speed limit and the maximum acceleration limit; and when the verification fails, adjusting the flight time, returning to execute the step of 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. In the embodiment, an iterative optimization mode is adopted for speed planning, and the flight time between adjacent path points in the track is optimized, so that the generated maximum speed limit is not exceeded, the maximum acceleration limit is not exceeded, and the generated path does not deviate from the original path too much, and the accuracy, smoothness and stability of track planning are improved.
Referring to fig. 5, fig. 5 is a schematic flow chart diagram of a trajectory planning method according to a third embodiment of the present invention.
Based on the first embodiment, the step S10 of the trajectory planning method of this embodiment includes:
step S101: when the obstacle is detected to exist in the target passing area, the position information and the shape information of the obstacle are determined.
Step S102: processing 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 the shape information of the obstacle in the target traffic area through the sensor installed on the aircraft, and the obstacle is approximately processed into a standardized three-dimensional shape according to the shape of the obstacle, and optionally, the standardized three-dimensional shape is any one of a sphere, a cuboid, a cylinder and the like, so that the distance between each position and the obstacle can be conveniently calculated, and the calculation power consumption of path planning is reduced.
Step S103: and generating a plurality of trapezoidal obstacle-detouring paths with different distances from the flight path reference line according to the standardized three-dimensional shape and the position information.
It should be noted that a path planning environment is constructed according to the standardized three-dimensional shape, the position information and the current driving data, and a plurality of trapezoidal obstacle detouring paths with different distances from the flight path reference line are searched in the constructed environment. The route reference line is a part intercepted from the globally planned route according to a preset response range. Referring to fig. 6, fig. 6 is a schematic path planning diagram of the trajectory planning method of the present invention, and a plurality of trapezoidal barrier-detouring paths deviating from the reference line by different distances as shown in fig. 6 are generated along the upper, lower, left and right sides of the reference line. The starting point of the path is where the aircraft is located and the end point is a point along the reference line at a distance of 200 m. And (3) updating the obstacle avoidance paths at preset time intervals along with the advance of the aircraft, wherein the number of the generated trapezoidal obstacle avoidance paths can be determined according to hardware computing power and real-time requirements.
Step S104: and selecting an optimal obstacle avoidance path from the plurality of trapezoidal obstacle avoidance paths.
Optionally, the multiple trapezoidal obstacle avoidance paths are comprehensively evaluated according to the length of each trapezoidal obstacle avoidance path, the distance between each trapezoidal obstacle avoidance path and an 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 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 to the route reference line from the plurality of target obstacle avoidance paths as an optimal obstacle avoidance path.
It should be understood that the distance between each trapezoidal obstacle-detouring path and the obstacle is calculated, the paths colliding with the obstacle and having the distance with the obstacle smaller than the preset safe distance are excluded, and the path with the minimum distance deviating from the reference line is selected from the remaining paths as the finally determined optimal obstacle avoidance path.
In a specific implementation, if it is determined that none of the generated plurality of trapezoidal obstacle detouring paths can avoid the obstacle, hovering and prompting are performed, and manual intervention is waited.
Accordingly, the step S20 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, newly 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.
It should be noted that, a plurality of polyline vertices on the optimal obstacle avoidance path are extracted and selected, and the positions of the plurality of polyline vertices are used as the input of the minimum jerk 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 separation distance between adjacent waypoints is too large, for example, any integer value of 25m, 50m, 25m-50m, and if the separation distance between adjacent waypoints is greater than the first preset distance value, it indicates that the separation distance between adjacent waypoints is too large. The current position is the current position of the aircraft, namely the starting point of the trapezoidal obstacle detouring path. The second preset distance value is a critical value, for example, 15m, set in advance, for determining whether the distance between the path point and the current position is too small, and if the distance between the path point and the current position is smaller than the second preset distance value, it indicates that the distance between the path point and the current position is too small.
In a specific implementation, the determined path points are adjusted: and calculating the distance between the adjacent path points, and inserting an additional path point between the adjacent path points if the distance is too large, so that the maximum distance between the adjacent path points is smaller than a certain value, and the generated track is prevented from deviating from the original path due to the fact that the distance between the two path points is too large. Further, removing waypoints that are too close to the aircraft, for example, removing waypoints that are within 15m of the current position of the aircraft, avoids unreasonable trajectory planning caused by the aircraft passing waypoints in a short time.
Compared with the traditional search sampling algorithm, the method for searching the obstacle avoidance path in the embodiment reduces the operation time, wherein the obstacle far away from the air route is directly excluded from the processing range by planning the air channel and the air channel boundary; the obstacle in the channel is approximately processed into a standardized three-dimensional shape such as a sphere or a cuboid, so that the calculation amount of the distance from the obstacle can be reduced. In this embodiment, the fewer obstacles are detected, the shorter the time taken for calculation is, and the method is suitable for a scene where a driving area is open and obstacles are rare, such as air flight. The calculation time of the traditional algorithm is not obviously influenced by the number of obstacles, and a large amount of time is still needed to be spent on calculation in the open scene.
The following describes the trajectory planning method of this embodiment with reference to an example:
referring to fig. 7, fig. 7 is a schematic diagram of a specific flow of an example of the trajectory planning method of the present invention, in which a globally planned route (generally, several kilometers or more) is used as an input to generate a channel boundary according to a predetermined route; if the obstacle is not detected, taking the current point of the aircraft and the tail point of the reference line as path points; if the obstacle exists within 200m in front of the channel, the obstacle is approximately processed into a cuboid or a sphere, an obstacle avoidance path is planned based on a safe distance, an optimal obstacle avoidance path is found, and when the obstacle avoidance path is not found, the navigation device hovers to wait for manual intervention; extracting and adjusting the optimal obstacle avoidance path to obtain path points; primarily distributing time for each section of the route driven by the adjacent route points; calculating a trajectory by a minimum jerk algorithm; checking whether the speed and the acceleration of each track point meet the requirements or not; if yes, outputting the track; if not, increasing the time allocated to each section of path, and returning to the step of calculating the track by the minimum jerk algorithm until the track meeting the requirements is obtained.
In the embodiment, when the obstacle is detected to exist in the target passing area, the position information and the shape information of the obstacle are determined; 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 the air route 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. By the mode, the obstacle is processed into the standardized three-dimensional shape, the trapezoidal obstacle-detouring path is conveniently planned, the calculation power consumption of path planning is reduced, and the real-time requirement of path planning is met.
Referring to fig. 8, fig. 8 is a schematic flowchart of a trajectory planning method according to a fourth embodiment of the present invention.
Based on the first embodiment, in the trajectory planning method of this embodiment, the plurality of path points at least include a terminal point, and the point location information corresponding to each path point at least includes a terminal speed direction and a terminal speed magnitude corresponding to the terminal point;
before the step S40, the method further includes:
step S301: and determining the final speed direction according to the direction of a connecting line between the terminal point and a path point before the terminal point.
Step S302: and determining the size of the final speed according to the distance between the final point and the path point before the final point.
It can be understood that the final speed is generally set to 0 in the conventional trajectory planning method, and the final speed is determined according to the information between the last two path points in the embodiment, so that the accuracy of trajectory planning is improved. Specifically, the direction of the last speed is the same as the direction of a connecting line of the last two path points, and the size of the last speed is determined by the distance between the last two path points, wherein the larger the distance between the last point and the path point before the last point is, the larger the last speed is, and the maximum speed limit value is taken as the upper limit; the smaller the distance between the end point and the path point immediately preceding the end point, the smaller the end speed, and 0 is the lower limit.
In the embodiment, the final speed direction is determined 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 final point and the path point before the final point. Through the mode, the traditional minimum jerk algorithm is improved, the input final speed is determined according to the actual path point information, the actual driving requirement is met, and the accuracy of trajectory planning is improved.
In addition, an embodiment of the present invention further provides a storage medium, where a trajectory planning program is stored on the storage medium, and when executed by a processor, the trajectory planning program implements the trajectory planning method described above.
Since the storage medium adopts all technical solutions of all the embodiments, at least all the beneficial effects brought by the technical solutions of the embodiments are achieved, and no further description is given here.
Referring to fig. 9, fig. 9 is a block diagram of the first embodiment of the trajectory planning device according to the present invention.
As shown in fig. 9, the trajectory planning apparatus provided in the embodiment of the present invention includes:
and the path planning module 10 is configured to plan an optimal obstacle avoidance path when an obstacle is detected in the target traffic area.
And the determining module 20 is configured to determine, according to the optimal obstacle avoidance path, multiple path points that meet a preset interval requirement.
And the distribution module 30 is configured to distribute corresponding flight time according to the separation distance between any two adjacent waypoints.
And the track planning module 40 is configured to perform speed planning by using the minimum jerk algorithm with the flight time and the point location information corresponding to each path as inputs, and generate a driving track.
It should be understood that the above is only an example, and the technical solution of the present invention is not limited in any way, and in a specific application, a person skilled in the art may set the technical solution as needed, and the present invention is not limited thereto.
In the embodiment, when the obstacle existing in the target traffic area is detected, an optimal obstacle avoidance path is planned; 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. By the mode, the path points with proper intervals are selected, the generated track is prevented from deviating from the original path due to the fact that the distance between the adjacent path points is too large, speed planning is carried out by utilizing a minimum jerk algorithm, jerks of all segmented tracks are kept in a certain numerical range and meet the limitation of flight time, and accuracy, smoothness and stability of track planning are improved.
It should be noted that the above-mentioned work flows are only illustrative and do not limit the scope of the present invention, and in practical applications, those skilled in the art may select some or all of them according to actual needs to implement the purpose of the solution of the present embodiment, and the present invention is not limited herein.
In addition, the technical details that are not described in detail in this embodiment may refer to the trajectory planning method provided in any embodiment of the present invention, and are not described herein again.
In an embodiment, the trajectory planning device further comprises an iteration module;
the iteration module is used for verifying the generated running 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.
In an embodiment, the path planning module 10 is further configured to determine position information and shape information of an obstacle when the obstacle is detected to exist in the target traffic area; 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.
In an embodiment, the path planning module 10 is further configured to remove, from the plurality of trapezoidal obstacle detouring paths, a path whose distance to the obstacle is smaller than a preset safety distance, so as to obtain a plurality of target obstacle detouring paths; and selecting a path with the minimum distance to the route reference line from the plurality of target obstacle avoidance paths as an optimal obstacle avoidance path.
In an embodiment, the path planning module 10 is further configured to determine a plurality of polygonal line vertices 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, newly 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.
In an embodiment, the plurality of path points at least include an end point, and the point location information corresponding to each path point at least includes a last speed direction and a last speed magnitude corresponding to the end point;
the trajectory planning device further comprises a determining module;
the determining module is configured to determine a final speed direction according to a direction of a connection line between the end point and a path point before the end 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.
In an embodiment, the trajectory planning apparatus further comprises a range determination module;
the range determining module is used for generating a channel boundary according to a preset route; and determining a target passing area according to the channel boundary and a preset response range.
Further, it is to be noted that, in this document, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or system that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or system. Without further limitation, an element defined by the phrases "comprising a," "8230," "8230," or "comprising" does not exclude the presence of other like elements in a process, method, article, or system comprising the element.
The above-mentioned serial numbers of the embodiments of the present invention are only for description, and do not represent the advantages and disadvantages of the embodiments.
Through the above description of the embodiments, those skilled in the art will clearly understand that the method of the above embodiments can be implemented by software plus a necessary general hardware platform, and certainly can also be implemented by hardware, but in many cases, the former is a better implementation manner. Based on such understanding, the technical solution of the present invention or portions thereof that contribute to the prior art may be embodied in the form of a software product, where the computer software product is stored in a storage medium (e.g. Read Only Memory (ROM)/RAM, magnetic disk, optical disk), and includes several instructions for enabling a terminal device (e.g. a mobile phone, a computer, a server, or a network device) to execute the method according to the embodiments of the present invention.
The above description is only a preferred embodiment of the present invention, and not intended to limit the scope of the present invention, and all modifications of equivalent structures and equivalent processes, which are made by using the contents of the present specification and the accompanying drawings, or directly or indirectly applied to other related technical fields, are included in the scope of the present invention.
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.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211314159.1A CN115509260A (en) | 2022-10-25 | 2022-10-25 | Trajectory planning method, apparatus, device and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211314159.1A CN115509260A (en) | 2022-10-25 | 2022-10-25 | Trajectory planning method, apparatus, device and storage medium |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115509260A true CN115509260A (en) | 2022-12-23 |
Family
ID=84512972
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211314159.1A Pending CN115509260A (en) | 2022-10-25 | 2022-10-25 | Trajectory planning method, apparatus, device and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115509260A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116151036A (en) * | 2023-04-17 | 2023-05-23 | 中铁九局集团有限公司 | Path planning method and device for automatic binding of reinforcing steel bars |
-
2022
- 2022-10-25 CN CN202211314159.1A patent/CN115509260A/en active Pending
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116151036A (en) * | 2023-04-17 | 2023-05-23 | 中铁九局集团有限公司 | Path planning method and device for automatic binding of reinforcing steel bars |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP6757820B2 (en) | Reference line smoothing method using a piecewise spiral curve with a weighted geometric cost | |
| CN111426326B (en) | Navigation method, device, equipment, system and storage medium | |
| CN109955853B (en) | Method, system and storage medium for operating an autonomous vehicle | |
| US10775801B2 (en) | Determining speeds along a path for autonomous driving vehicles | |
| EP3327530B1 (en) | Method for determining command delays of autonomous vehicles | |
| US11199847B2 (en) | Curvature corrected path sampling system for autonomous driving vehicles | |
| CN110325935A (en) | The lane guide line based on Driving Scene of path planning for automatic driving vehicle | |
| CN110119140A (en) | System and method for acceleration curve projection | |
| EP2821876A2 (en) | Route planning method, route planning unit, and autonomous mobile device | |
| JP2023547745A (en) | Vehicle obstacle avoidance methods, devices, electronic devices, storage media | |
| CN108628298A (en) | Control type planning for automatic driving vehicle and control system | |
| JP2021054393A (en) | Method, system, device and medium for determining u-turn path of vehicle | |
| CN112327888B (en) | Path planning method and device, electronic equipment and storage medium | |
| US11529951B2 (en) | Safety system, automated driving system, and methods thereof | |
| EP4119412A1 (en) | Vehicle-based data processing method and apparatus, computer, and storage medium | |
| CN115509260A (en) | Trajectory planning method, apparatus, device and storage medium | |
| CN113415288A (en) | Sectional type longitudinal vehicle speed planning method, device, equipment and storage medium | |
| CN116700344A (en) | Unmanned aerial vehicle autonomous obstacle avoidance method, flight execution method and system | |
| AU2021334408B2 (en) | Method and apparatus for coordinating multiple cooperative vehicle trajectories on shared road networks | |
| CN116088538B (en) | Vehicle track information generation method, device, equipment and computer readable medium | |
| CN114407919B (en) | Collision detection method and system based on automatic driving | |
| CN113788028B (en) | Vehicle control method, device and computer program product | |
| CN114115216A (en) | Vehicle formation control method, device, equipment and storage medium | |
| CN114911266B (en) | Unmanned aerial vehicle search route establishment method and device | |
| CN114518760A (en) | Unmanned vehicle navigation method and system server based on risk area management |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |