CN113721637A - Intelligent vehicle dynamic obstacle avoidance path continuous planning method and system and storage medium - Google Patents
Intelligent vehicle dynamic obstacle avoidance path continuous planning method and system and storage medium Download PDFInfo
- Publication number
- CN113721637A CN113721637A CN202111286331.2A CN202111286331A CN113721637A CN 113721637 A CN113721637 A CN 113721637A CN 202111286331 A CN202111286331 A CN 202111286331A CN 113721637 A CN113721637 A CN 113721637A
- Authority
- CN
- China
- Prior art keywords
- path
- planning
- obstacle
- vehicle
- point
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 53
- 230000033001 locomotion Effects 0.000 claims description 63
- 230000008859 change Effects 0.000 claims description 10
- 239000000126 substance Substances 0.000 claims description 10
- 238000004590 computer program Methods 0.000 claims description 6
- 230000001965 increasing effect Effects 0.000 claims description 3
- 230000009466 transformation Effects 0.000 abstract description 6
- 230000009286 beneficial effect Effects 0.000 abstract description 3
- 230000002708 enhancing effect Effects 0.000 abstract description 2
- 230000008569 process Effects 0.000 description 17
- 238000006243 chemical reaction Methods 0.000 description 7
- 238000010586 diagram Methods 0.000 description 6
- 239000013598 vector Substances 0.000 description 5
- 230000004888 barrier function Effects 0.000 description 4
- 230000003068 static effect Effects 0.000 description 4
- 238000010276 construction Methods 0.000 description 3
- 238000005457 optimization Methods 0.000 description 3
- 230000001186 cumulative effect Effects 0.000 description 2
- 238000009826 distribution Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 150000001875 compounds Chemical class 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000018109 developmental process Effects 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000011426 transformation method Methods 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 or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
- G05D1/0274—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means using mapping information stored in a memory device
Abstract
The invention discloses a method and a system for continuously planning a dynamic obstacle avoidance path of an intelligent vehicle and a storage medium. The method comprises the following steps: the Frenet coordinate transformation is utilized to convert the local path planning problem when the vehicle runs along the complex curved road into the double-movement route planning problem under the straight road, thereby greatly reducing the planning complexity; dividing the path of the double-moving line into an obstacle avoidance road section and a regression road section, and respectively planning the path based on a Bezier curve; when the surrounding environment changes dynamically, particularly when the vehicle tracks the local path to run, the obstacle suddenly accelerates, decelerates or moves laterally, the terminal point of the planned path is updated correspondingly, the local path is re-planned, and the curvature continuity of the connection point of the path is ensured. The method and the system can improve the real-time performance of the planned path of the intelligent vehicle and are beneficial to enhancing the adaptability of the intelligent vehicle to the complex traffic environment.
Description
Technical Field
The invention relates to the technical field of automatic driving decision planning, in particular to a method for continuously replanning a dynamic obstacle avoidance path of an intelligent vehicle based on a Frenet coordinate system and a Bezier curve, and specifically relates to a method, a system and a storage medium for continuously planning the dynamic obstacle avoidance path of the intelligent vehicle.
Background
The automatic driving technology can effectively guarantee traffic safety, improve traffic efficiency and improve a travel mode, the bottom layer architecture of automatic driving and 95% of conventional technical problems are solved, and the remaining 5% of long tail problems become keys for restricting the automatic driving landing application. The long tail problems relate to key technologies such as reliable perception, rapid prediction and optimized decision planning in various complex scenes.
For the vehicle motion planning problem, the current solution is that when an obstacle is detected in front, a decision-making planning layer outputs a local obstacle avoidance path according to current environment information, and then tracks the vehicle. The scheme is mostly static obstacle avoidance path planning, wherein static refers to that obstacle avoidance path planning is carried out only by using obstacle motion state information (position, attitude, speed and the like) before obstacle avoidance, and real-time updating and optimization are not carried out on an obstacle avoidance path in an obstacle avoidance process. However, for a complex and dynamic traffic environment, once the motion state of an obstacle suddenly changes when the vehicle runs along an obstacle avoidance path, a planned path before obstacle avoidance is no longer reliable, the obstacle avoidance path and the obstacle path are easily interfered, and a great collision risk exists, so that the running safety of automatic driving is influenced.
Disclosure of Invention
In order to overcome the defects of the prior art, the invention provides a method, a system and a storage medium for continuously planning a dynamic obstacle avoidance path of an intelligent vehicle, which can be used for re-planning the path in real time according to the change of the motion state of an obstacle and ensuring the continuity of the curvature of the newly planned path and the curvature of the original path.
According to one aspect of the description of the invention, a method for continuously planning a dynamic obstacle avoidance path of an intelligent vehicle is provided, which comprises the following steps:
constructing a local path planning framework based on a Frenet coordinate system;
local path planning of the road sections is carried out on the basis of the Bezier curve;
and carrying out vehicle tracking driving according to the planned local path, acquiring the motion state information of the obstacle in a preset time step length, and re-planning the path based on a Bezier curve when the motion state of the obstacle changes, wherein the re-planned path curvature is continuous with the original path curvature.
According to the technical scheme, a local path planning framework based on a Frenet coordinate system is constructed firstly, mutual conversion of Frenet coordinates and Cartesian coordinates is achieved, the local path planning problem when a vehicle runs along a complex curved road is converted into the double-movement line path planning problem under a straight road, path planning of sub-road sections is conducted under the Frenet coordinate system based on a Bezier curve, and path planning complexity is greatly reduced; according to the technical scheme, when the vehicle is tracked and driven, the real-time motion state of the obstacle is considered, the path planning is carried out again when the real-time motion state of the obstacle changes, the local path of the vehicle is updated in real time, the continuous dynamic updating of the obstacle avoidance path is realized under the complex dynamic traffic environment, and the curvature continuity of the front and back planned paths and the smoothness of the whole path are guaranteed.
As a further technical solution, the local path planning of the branch segment includes: and planning an obstacle avoidance road section path and a regression road section path, wherein the end point of the obstacle avoidance road section is consistent with the starting point of the regression road section. After the local path planning problem when a vehicle runs along a complex curved road is converted into the double-moving route path planning problem under a straight road by using Frenet coordinate transformation, the double-moving route path is divided into an obstacle avoidance road section and a regression road section, so that path planning is respectively carried out, and the planning complexity is reduced.
As a further technical solution, the method further comprises: when the self-vehicle finds a front obstacle and drives to the starting point of the local path obstacle-avoiding section along the global path, determining the end point of the obstacle-avoiding section according to the initial motion states of the self-vehicle and the front obstacle, and then planning the path based on the Bezier curve; when the self-vehicle runs to the end point of the obstacle avoidance section, the end point of the regression section is determined according to the current motion states of the self-vehicle and the front obstacle, and then path planning is carried out based on the Bezier curve. According to the technical scheme, the self-vehicle finds the front obstacle and performs the sectional path planning, and when the tracking driving is performed on each branch section, the motion state of the front obstacle does not change, namely the whole obstacle avoidance process is completed on the basis of one-time path planning.
As a further technical solution, the method further comprises: when the vehicle is tracked and driven according to the planned path, if the change of the motion state of the front obstacle is known, the path terminal point of the current road section is recalculated, and the path is re-planned based on the Bezier curve according to the current position of the vehicle and the recalculated path terminal point. According to the technical scheme, when the self-vehicle tracks and drives, the motion state of the front obstacle is obtained according to the preset time step length, and if the motion state of the front obstacle changes, the planned path is not applicable any more, path re-planning is carried out. According to the technical scheme, the route end point of the current road section is determined again, and the route is re-planned according to the new end point, so that the re-planned obstacle avoidance route can adapt to obstacle avoidance operation after the movement state of the obstacle changes, and continuous dynamic updating of the obstacle avoidance route under the complex dynamic traffic environment is achieved.
As a further technical scheme, when the vehicle tracks and runs according to a planned route, if an obstacle suddenly accelerates, the route end point is increased; reducing the end of the path if the obstacle suddenly deceleratessCoordinates; changing the end of the path if the obstacle has moved sideways to some extentlAnd (4) coordinates.
As a further technical solution, the method further comprises:
acquiring a global path;
acquiring information of a map, a vehicle motion state and an obstacle motion state under a Cartesian coordinate system;
when an obstacle appears in the front of the vehicle, a global path is taken as a referential property, a map acquired in real time in a Cartesian coordinate system, a vehicle motion state and an obstacle motion state are converted into a Frenet coordinate system, and local path planning is carried out;
and converting the planned local path in the Frenet coordinate system into a local path in a Cartesian coordinate system, and tracking the running.
According to the technical scheme, when the vehicle runs along the global path, an obstacle appears in front of the vehicle, local path planning is carried out to avoid the obstacle, and the vehicle returns to the global path to continue running after the obstacle avoidance is finished. The global path on the structured road is usually a lane center line, and the global path in the open environment is a static obstacle avoidance path from a starting point to an end point calculated by the global planning layer.
As a further technical solution, the method performs path re-planning based on a third-order bezier curve, which is determined by four control points, wherein the first control point and the fourth control point are a start point and an end point of the path, and the second control point and the third control point are used for controlling curvature change of the path.
As a further technical solution, the second control point is solved according to the following formula
Wherein the content of the first and second substances,
，
is the coordinates of the first control point,
， 
is the coordinates of the second control point,mis the distance between the first control point and the second control point, is the first variable to be optimized of the Bezier curve,
the course angle of the vehicle at the first control point under the Frenet coordinate is shown;
the third control point is solved according to the following formula
Wherein the content of the first and second substances,
，
is the coordinates of the third control point,
， 
is the coordinates of the fourth control point,mthe distance between the third control point and the fourth control point, the second variable to be optimized of the Bezier curve,
the course angle of the obstacle avoidance road section terminal point or the course angle of the regression road section terminal point under the Frenet coordinate.
According to one aspect of the description of the invention, a system for continuously planning a dynamic obstacle avoidance path of an intelligent vehicle is provided, which is realized by adopting the method, and the system comprises:
the building module is used for building a local path planning framework based on a Frenet coordinate system;
the path planning module is used for carrying out local path planning and re-planning based on the Bezier curve, and the re-planned path curvature is continuous with the originally planned path curvature;
and the tracking driving module is used for tracking and driving the vehicle according to the planned local path, acquiring the motion state information of the obstacle in a preset time step length, and triggering the path planning module to carry out path re-planning when the motion state of the obstacle changes.
According to the technical scheme, a local path planning framework based on a Frenet coordinate system is constructed through a construction module, conversion from the Frenet coordinate system to a Cartesian coordinate system is achieved, real-time state information under the Cartesian coordinate system is converted into the Frenet coordinate system, local path planning is conducted through a path planning module, tracking driving of a vehicle is conducted through a tracking driving module, when the motion state of a front obstacle serving as the basis of previous path planning changes, the path planning module is started to conduct path re-planning, and tracking driving is conducted through the tracking driving module according to the re-planned path until an obstacle avoidance process is completed. According to the technical scheme, the influence of the real-time motion state of the obstacle on the obstacle avoidance process, which is taken as the basis of obstacle avoidance path planning, is fully considered, the obstacle avoidance path is dynamically updated according to the real-time motion state of the obstacle, and the smooth completion of the obstacle avoidance process and the smoothness and the continuity of curvature of the obstacle avoidance path are ensured.
According to an aspect of the present specification, there is provided a computer-readable storage medium, on which a computer program is stored, where the computer program, when executed by a processor, implements the steps of the method for continuously planning a dynamic obstacle avoidance path of an intelligent vehicle.
Compared with the prior art, the invention has the beneficial effects that:
(1) the invention provides a method, which converts the local path planning problem of a vehicle running along a complex curved road into the double-movement route path planning problem under a straight road by using Frenet coordinate transformation, thereby greatly reducing the planning complexity; dividing the path of the double-moving line into an obstacle avoidance road section and a regression road section, and respectively planning the path based on a Bezier curve; when the surrounding environment changes dynamically, particularly when the vehicle tracks the local path to run, the obstacle suddenly accelerates, decelerates or moves laterally, the terminal point of the planned path is updated correspondingly, the local path is re-planned, and the curvature continuity of the connection point of the path is ensured. The method and the system can improve the real-time performance of the planned path of the intelligent vehicle and are beneficial to enhancing the adaptability of the intelligent vehicle to the complex traffic environment.
(2) The invention provides a system, which constructs a local path planning framework based on a Frenet coordinate system through a construction module, realizes the conversion from the Frenet coordinate to a Cartesian coordinate, converts real-time state information under the Cartesian coordinate system to the Frenet coordinate system, carries out local path planning through a path planning module, then carries out tracking driving of a vehicle through a tracking driving module, starts the path planning module to carry out path re-planning when the motion state of a front obstacle based on the previous path planning changes, and carries out tracking driving according to the re-planned path through the tracking driving module until the obstacle avoidance process is completed. The system fully considers the influence of the real-time motion state of the obstacle on the obstacle avoidance process, which is taken as the basis of obstacle avoidance path planning, and dynamically updates the obstacle avoidance path according to the real-time motion state of the obstacle, thereby ensuring the smooth completion of the obstacle avoidance process and the smoothness and the continuity of curvature of the obstacle avoidance path.
Drawings
Fig. 1 is a schematic diagram of a method for continuously planning a dynamic obstacle avoidance path of an intelligent vehicle according to an embodiment of the invention.
Fig. 2 is a schematic diagram of local path planning in a Frenet coordinate system according to an embodiment of the present invention.
Fig. 3 is a schematic diagram of replanning an obstacle avoidance section and a regression section according to an embodiment of the present invention.
Fig. 4 is a schematic diagram of bezier curves under different control point distributions according to an embodiment of the present invention.
Fig. 5 is a schematic diagram illustrating an update of an automatic driving real-time obstacle avoidance path according to an embodiment of the present invention.
Detailed Description
The technical solutions of the embodiments of the present invention will be described clearly and completely with reference to the accompanying drawings, and it is to be understood that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments of the present invention without any inventive step, are within the scope of the present invention.
The invention provides a method and a system for continuously planning a dynamic obstacle avoidance path of an intelligent vehicle, which realize continuous re-planning of the dynamic obstacle avoidance path of the intelligent vehicle based on a Frenet coordinate system and a Bezier curve, can realize continuous dynamic updating of the obstacle avoidance path in a complex dynamic traffic environment, and ensure the smoothness and the continuous curvature of the whole path.
Example 1
As shown in fig. 1, the present embodiment provides a method for continuously planning a dynamic obstacle avoidance path of an intelligent vehicle. In the embodiment, when the path is planned, the real-time motion state of the barrier is considered, the local path of the vehicle is updated in real time at a certain time step, and the continuity of the curvature is ensured.
The method of the embodiment comprises the following steps: constructing a local path planning framework based on a Frenet coordinate system; curvature continuity path planning based on a Bezier curve; and updating the real-time obstacle avoidance path in the complex dynamic environment.
In this embodiment, constructing a local path planning architecture based on a Frenet coordinate system includes:
(1) the global path on the structured road is usually a lane central line, and the global path in the open environment is a static obstacle avoidance path from a starting point to an end point calculated by a global planning layer. The purpose of local path planning is to present an obstacle in front of the vehicle when the vehicle is traveling along the global path, perform local path planning to avoid the obstacle, and be able to return to the global path to continue traveling.
(2) Obtaining high-precision map information, self-vehicle motion state information and barrier motion state information in real time, wherein the barriers mainly refer to other traffic participants (vehicles, pedestrians, riders and the like) around the self-vehicle, and the motion state information mainly comprises positions
Posture, posture
Speed, velocity
Acceleration of the vehicle
And all the information mentioned above is based on the global coordinate system of each sensor(Cartesian coordinate system) acquisition.
(3) And according to the Frenet coordinate conversion rule, the real-time acquired high-precision map information and the motion state information of the vehicle and the obstacle are converted into a Frenet coordinate system by taking the global path as a reference line.
(4) In the Frenet coordinate system, all the local obstacle avoidance path planning problems along the global path are converted into a double-shift line path planning problem under the straight line road, as shown in fig. 2, the double-shift line local path includes: and performing single-moving route path planning (obstacle avoidance road section AB) once to avoid the obstacle, updating in real time in the tracking process, returning to the global route after avoiding the obstacle, performing single-moving route path planning (regression road section BC) once again, and updating in real time in the tracking process.
(5) And converting the planned local path in the Frenet coordinate system into a local path in a Cartesian coordinate system for tracking the running.
Further, the principle of the above-described Frenet coordinate transformation is described as follows: the global path is used as a reference line, the tangential direction of each point on the global path is taken as a horizontal axis S (the vehicle driving direction is positive), the normal direction of each point on the global path is taken as a vertical axis L (the left side of the vehicle driving direction is positive), and the main purpose of coordinate transformation is to realize coordinates under a Cartesian coordinate system
Coordinates in Frenet coordinate System
And (4) the conversion between the two.
In this embodiment, the Frenet coordinate transformation is implemented based on a global path discrete point, and the transformation method includes:
(1) cartesian coordinate system
Rotational Frenet coordinate
In an autonomous driving system, a global path is usually published in the form of discrete points, known as a set of discrete points at some fixed spacing, and in the form of a set of discrete pointsk3, each row represents a point, and one to three columns are respectively the path length of each point under Cartesian coordinatessTransverse position of the rollerxLongitudinal position of the shaftyWherein the path lengthsA cumulative approximation of the linear distance between the points may be used instead. The Frenet coordinate system is established with the global path as a reference line, the tangential direction of each point on the global path as a horizontal axis S (the vehicle traveling direction is positive), and the normal direction of each point on the global path as a vertical axis L (the left side of the vehicle traveling direction is positive).
It is known that
Finding the corresponding Frenet coordinate
。
Firstly, finding out points under a Cartesian coordinate system
Nearest point to global path
I.e. by
At a minimum, the global path is, in the table of global paths,
corresponding to
Is a point
In Frenet coordinate SystemsTo do so
I.e. in the Frenet coordinate systemlAbsolute value of (a).
Then using the cross product of two vectors to judgelPositive and negative (right-hand rule). Recording points on global path
Is a point
Let a space vector
、
，
Then space vectorcPositive and negative of the third element arelPositive and negative of (2), positive represents a point
On the left side of the global path, negative represents the right side.
(2) Frenet coordinate
Cartesian coordinate system
In known Frenet coordinates
To obtain corresponding
。
First of all, it is knownsThe flute can be known according to the global path tableUnder Karl coordinate
Closest point on the global path
Then according to the root point
And point
Calculating unit direction vector of nearest point
；lIs a regular one
Rotate clockwise
，lIf it is negative, it will
Rotate clockwise
Get a point
And point
Unit vector of connecting line
Wherein
，
(ii) a Finally, the point is obtained
Wherein
。
According to the coordinate conversion method, high-precision map information, obstacle information and vehicle information in a Cartesian coordinate system are converted into a Frenet coordinate system, the local path planning problem under a curved road can be converted into the planning problem under a straight road, then double-movement line path planning is carried out based on a Bezier curve, the local path is updated in real time according to the change of the motion state of an obstacle, and the continuity of curvature is guaranteed.
In this embodiment, path planning and re-planning based on a bezier curve are performed based on the established Frenet coordinate system.
As shown in fig. 2, the local path is divided into an obstacle avoidance section AB and a regression section BC, a coordinate of a point a needs to be determined before the local path is planned, an end point B of the obstacle avoidance section is determined according to a motion state of a current obstacle, and then the path planning of the obstacle avoidance section is performed based on a bezier curve.
In the process of driving the vehicle to track the obstacle avoidance section, if the motion state of the obstacle changes, which can cause the change of the end point of the obstacle avoidance section, a path needs to be planned from the current position of the vehicle to connect with a new end point, and the continuity of curvature of each connecting point is ensured. As shown in fig. 3, the hollow origin in the graph represents a position of a vehicle when the movement state of the obstacle changes and the path needs to be re-planned, and the star point is a road segment end point in different movement states of the obstacle.
When the vehicle drives to the point B, the regression road section is planned and dynamically updated in the same way, and it is noted that the terminal point of the regression road section must be on the global path no matter how the terminal point changes, namely
。
As shown in fig. 3, according to the longitudinal following model with the timing distance, when a stationary obstacle or an obstacle with too low speed exists in front of the global path, the own vehicle needs to perform local path planning, and coordinates of a planned starting point a
Determined by a timing-distance longitudinal following model
Wherein the content of the first and second substances,
is the S coordinate of the obstacle when the self-vehicle is at the point A,
in order to obtain the speed of the bicycle,
and
the following time interval and the parking distance are constant respectively.
After the starting point of the local path planning is determined, the local path planning is performed by using a third-order bezier curve, as shown in fig. 2, the local path includes two parts, and firstly, the transverse displacement at a certain distance is realized to avoid the obstacle, namely an AB section (obstacle avoidance section) in fig. 2, and then, the local path returns to the global path, namely a BC section (regression section) in fig. 2.
As shown in fig. 5, when the host vehicle travels along the global route to the local route planning start point a, the coordinates of the obstacle avoidance section end point B are calculated according to the movement state of the obstacle ahead in the following manner
Wherein the content of the first and second substances,
、
respectively representing the S coordinate of the obstacle and the moving speed of the obstacle in the S direction when the self-vehicle is at the point A,
forecasting time consumption for avoiding the obstacle road section, wherein the forecasting time consumption is equal to the road section terminal point and the current pointsThe difference is divided by the current vehicle speed,
、
、
respectively the width of the obstacle, the width of the own vehicle and the desired lateral spacing between the own vehicle and the obstacle.
From equation 2, the end point of the road section is related to the real-time position, speed and size of the obstacle, and the vehicle is driven by a certain time step Δt sUpdating the motion state information of the obstacle when the motion state of the obstacle changes (such as suddenly accelerates, decelerates or generates a certain lateral movement), namely updating the real-time position of the obstacle
、 
And speed
Change in value of (e.g. of
Change over
Or
Change over
And correspondingly updating the road segment end point, replanning the local path according to the updated end point and the current position of the vehicle, and ensuring the continuous curvature of the connecting point.
As shown in fig. 5, when the vehicle moves to the point a ', the obstacle moving state changes, and the obstacle avoidance link end point is updated to the point B'.
In this embodiment, the obstacle avoidance road section local path is constructed based on a third-order bezier curve, wherein the third-order bezier curve principle and the curvature continuity replanning method are specifically described as follows:
the third-order Bessel curve consists of four control points
、
、
、
It was determined that, as shown in fig. 4, for a diagram of bezier curves under different control point distributions,
、
i.e. the start and end points of the path,
、
for controlling the change in curvature of the path.
By using
Points on the Bezier curve are represented, then
Wherein the content of the first and second substances,
，
，
varying from 0 to 1, the smaller the spacing, the denser the points on the bezier curve.
The determination of the third-order Bessel curve requires finding the coordinates of four control points in the Frenet coordinate system
、
、
、
。
First control point of Bezier curve
The current position of the vehicle may be a starting point a of an obstacle avoidance section or a starting point B of a regression section in the local path, or may be a position of the vehicle when a motion state of an obstacle changes at any time during the running of the vehicle along the local path.
Second control point of Bezier curve
Solving as follows
Wherein the content of the first and second substances,mas a control point
、
Is the first variable to be optimized of the bezier curve,
this is the heading angle of the vehicle at the first control point in Frenet coordinates, and ensures that the curvature at the start of the path is continuous.
Fourth control point of Bezier curve
And the terminal point of the AB obstacle avoidance section or the BC regression section in the local path is obtained.
Third control point of Bezier curve
Solving as follows
Wherein the content of the first and second substances,nas a control point
、
Is the second variable to be optimized of the bezier curve,
the course angle of the point B or the course angle of the point C under the Frenet coordinate can ensure the curvature continuity at the end point of the path.
As shown in fig. 5, the starting point of the obstacle avoidance section (point a or point in fig. 5)
) And end point (point B or point in FIG. 5)
) I.e. the first control point of the third order bezier curve
And a fourth control point
Second control point
And a third control point
Is calculated as follows
Wherein the content of the first and second substances,mas a control point
、
Is the first variable to be optimized of the bezier curve,
the heading angle of the vehicle at the first control point in the Frenet coordinate,nas a control point
、
Is the second variable to be optimized of the bezier curve,
the course angle of the road section end point is obtained, the curvature continuity at the path end point can be ensured by the processing, and the first control edge and the third control edge of the third-order Bezier curve are tangent lines of the first control point and the fourth control point.
The Bezier curve has two variables to be optimizedm、nThe embodiment targets the cumulative sum of curvature increments of adjacent path points as the minimumm、nCarry out optimization
In the formula 8, the first and second groups of the compound,
and
respectively represent
Point and first
The curvature of the point or points is such that,
an optimization function is represented.
In conclusion, when the vehicle runs to the point A of the local path planning starting point, the obstacle avoidance road section terminal point B is calculated, and the road section terminal point is updated in real time in the running process; the path connecting the current point of the vehicle and the road section terminal point is calculated by a Bezier curve, the Bezier curve has two variables to be optimized, the dynamic local path of the obstacle avoidance road section can be obtained by solving through the determined objective function, and the continuous curvature of each connecting point is ensured.
In this embodiment, when the vehicle travels to the end point of the obstacle avoidance section, the local path planning of the regression section is started, and the specific steps are the same as the planning and updating process of the obstacle avoidance section. First, the end point of the regression road section is determined, and it needs to be noted that the end point of the regression road section can only be on the global path, namely
And then planning and connecting the current point and the road section terminal point by using the Bezier curve, updating the regression road section terminal point when the movement state of the barrier changes in the driving process, and re-planning the path between the current point and the road section terminal point.
The embodiment provides a real-time obstacle avoidance path updating strategy in a complex dynamic environment, which includes:
(1) when the self-vehicle finds the front obstacle and drives to the starting point A of the local path planning along the global path, firstly, the terminal point of the local path obstacle avoidance section is determined according to the initial motion states of the self-vehicle and the front obstacle
Then using third order shellfishAnd planning the path by using the Sehr curve.
(2) The self-vehicle runs along the planned path and in a certain time step
Updating the motion state information of the obstacle, and when the motion state of the obstacle changes (such as suddenly accelerating, decelerating or generating a certain lateral movement), correspondingly adjusting (increasing, decreasing) the path end point B
Or change
）。
(3) And (4) changing the path end point, and re-planning the path based on the Bezier curve so as to ensure that the curvature of the updated path is continuous with that of the original path at the connecting point.
(4) When the self-vehicle runs to the terminal point B of the local path obstacle avoidance section, determining the terminal point of the local path regression section according to the current state
And then, the path dynamic updating is carried out in the same way until the whole local path planning and the tracking driving are completed.
Example 2
This embodiment provides an intelligent vehicle developments obstacle avoidance path continuous planning system, includes: the building module is used for building a local path planning framework based on a Frenet coordinate system; the path planning module is used for carrying out local path planning and re-planning based on the Bezier curve, and the re-planned path curvature is continuous with the originally planned path curvature; and the tracking driving module is used for tracking and driving the vehicle according to the planned local path, acquiring the motion state information of the obstacle in a preset time step length, and triggering the path planning module to carry out path re-planning when the motion state of the obstacle changes.
In the embodiment, a local path planning framework based on a Frenet coordinate system is constructed through a construction module, the conversion from the Frenet coordinate system to a Cartesian coordinate system is realized, real-time state information under the Cartesian coordinate system is converted into the Frenet coordinate system, local path planning is carried out through a path planning module, then tracking driving of a vehicle is carried out through a tracking driving module, when the motion state of a front obstacle based on previous path planning changes, the path planning module is started to carry out path re-planning, and then tracking driving is carried out through the tracking driving module according to the re-planned path until the obstacle avoidance process is completed.
The method and the device fully consider the influence of the real-time motion state of the obstacle on the obstacle avoidance process, and dynamically update the obstacle avoidance path according to the real-time motion state of the obstacle, so that the smooth completion of the obstacle avoidance process and the smoothness and curvature continuity of the obstacle avoidance path are ensured.
According to an aspect of the present specification, there is provided a computer-readable storage medium, on which a computer program is stored, where the computer program, when executed by a processor, implements the steps of the method for continuously planning a dynamic obstacle avoidance path of an intelligent vehicle.
Finally, it should be noted that: the above examples are only intended to illustrate the technical solution of the present invention, but not to limit it; although the present invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some or all of the technical features may be equivalently replaced; and the modifications or the substitutions do not make the essence of the corresponding technical solutions deviate from the technical solutions of the embodiments of the present invention.
Claims (10)
1. The intelligent vehicle dynamic obstacle avoidance path continuous planning method is characterized by comprising the following steps:
constructing a local path planning framework based on a Frenet coordinate system;
local path planning of the road sections is carried out on the basis of the Bezier curve;
and carrying out vehicle tracking driving according to the planned local path, acquiring the motion state information of the obstacle in a preset time step length, and re-planning the path based on a Bezier curve when the motion state of the obstacle changes, wherein the re-planned path curvature is continuous with the original path curvature.
2. The intelligent vehicle dynamic obstacle avoidance path continuous planning method according to claim 1, wherein the local path planning of the sub-section comprises: and planning an obstacle avoidance road section path and a regression road section path, wherein the end point of the obstacle avoidance road section is consistent with the starting point of the regression road section.
3. The intelligent vehicle dynamic obstacle avoidance path continuous planning method according to claim 2, further comprising: when the self-vehicle finds a front obstacle and drives to the starting point of the local path obstacle-avoiding section along the global path, determining the end point of the obstacle-avoiding section according to the initial motion states of the self-vehicle and the front obstacle, and then planning the path based on the Bezier curve; when the self-vehicle runs to the end point of the obstacle avoidance section, the end point of the regression section is determined according to the current motion states of the self-vehicle and the front obstacle, and then path planning is carried out based on the Bezier curve.
4. The intelligent vehicle dynamic obstacle avoidance path continuous planning method according to claim 3, further comprising: when the vehicle is tracked and driven according to the planned path, if the change of the motion state of the front obstacle is known, the path terminal point of the current road section is recalculated, and the path is re-planned based on the Bezier curve according to the current position of the vehicle and the recalculated path terminal point.
5. The intelligent vehicle dynamic obstacle avoidance path continuous planning method according to claim 4, wherein the step of re-determining the path end point of the current road section further comprises: if the obstacle suddenly accelerates, increasing the route end point; reducing the end of the path if the obstacle suddenly deceleratessCoordinates; changing the end of the path if the obstacle is moved laterallylAnd (4) coordinates.
6. The method for continuously planning the dynamic obstacle avoidance path of the intelligent vehicle according to claim 1, wherein constructing a local path planning architecture based on a Frenet coordinate system further comprises:
acquiring a global path;
acquiring information of a map, a vehicle motion state and an obstacle motion state under a Cartesian coordinate system;
when an obstacle appears in the front of the vehicle, a global path is taken as a referential property, a map acquired in real time in a Cartesian coordinate system, a vehicle motion state and an obstacle motion state are converted into a Frenet coordinate system, and local path planning is carried out;
and converting the planned local path in the Frenet coordinate system into a local path in a Cartesian coordinate system, and tracking the running.
7. The intelligent vehicle dynamic obstacle avoidance path continuous planning method according to claim 1, wherein the method performs path re-planning based on a third-order bezier curve, the third-order bezier curve is determined by four control points, wherein the first control point and the fourth control point are a start point and an end point of the path, and the second control point and the third control point are used for controlling curvature change of the path.
8. The intelligent vehicle dynamic obstacle avoidance path continuous planning method according to claim 7, wherein the second control point is solved according to the following formula
Wherein the content of the first and second substances,
，
is the coordinates of the first control point,
， 
is the coordinates of the second control point,mis the distance between the first control point and the second control point, is the first variable to be optimized of the Bezier curve,
the course angle of the vehicle at the first control point under the Frenet coordinate is shown;
the third control point is solved according to the following formula
Wherein the content of the first and second substances,
，
is the coordinates of the third control point,
， 
is the coordinates of the fourth control point,mthe distance between the third control point and the fourth control point, the second variable to be optimized of the Bezier curve,
the course angle of the obstacle avoidance road section terminal point or the course angle of the regression road section terminal point under the Frenet coordinate.
9. The intelligent vehicle dynamic obstacle avoidance path continuous planning system is realized by adopting the method of any one of claims 1 to 8, and is characterized by comprising the following steps:
the building module is used for building a local path planning framework based on a Frenet coordinate system;
the path planning module is used for carrying out local path planning and re-planning based on the Bezier curve, and the re-planned path curvature is continuous with the originally planned path curvature;
and the tracking driving module is used for tracking and driving the vehicle according to the planned local path, acquiring the motion state information of the obstacle in a preset time step length, and triggering the path planning module to carry out path re-planning when the motion state of the obstacle changes.
10. A computer-readable storage medium, wherein a computer program is stored on the computer-readable storage medium, and when being executed by a processor, the computer program implements the steps of the method for continuously planning the dynamic obstacle avoidance path of the intelligent vehicle according to any one of claims 1 to 8.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111286331.2A CN113721637B (en) | 2021-11-02 | 2021-11-02 | Intelligent vehicle dynamic obstacle avoidance path continuous planning method and system and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111286331.2A CN113721637B (en) | 2021-11-02 | 2021-11-02 | Intelligent vehicle dynamic obstacle avoidance path continuous planning method and system and storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113721637A true CN113721637A (en) | 2021-11-30 |
| CN113721637B CN113721637B (en) | 2022-05-03 |
Family
ID=78686441
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111286331.2A Active CN113721637B (en) | 2021-11-02 | 2021-11-02 | Intelligent vehicle dynamic obstacle avoidance path continuous planning method and system and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113721637B (en) |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114355921A (en) * | 2021-12-28 | 2022-04-15 | 北京易航远智科技有限公司 | Vehicle tracking track generation method and device, electronic equipment and storage medium |
| CN114442491A (en) * | 2022-04-08 | 2022-05-06 | 杭州国辰机器人科技有限公司 | Local path planning and tracking control method, device and medium for indoor robot |
| CN114510055A (en) * | 2022-02-18 | 2022-05-17 | 科大国创合肥智能汽车科技有限公司 | Method for constructing rear lane line |
| CN114518123A (en) * | 2022-02-24 | 2022-05-20 | 高德软件有限公司 | Information processing method |
| CN115571164A (en) * | 2022-11-03 | 2023-01-06 | 中科天极(新疆)空天信息有限公司 | Obstacle avoidance method and system based on automatic driving |
| CN117622227A (en) * | 2024-01-24 | 2024-03-01 | 广汽埃安新能源汽车股份有限公司 | Vehicle lane changing obstacle avoidance control method and device |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109501799A (en) * | 2018-10-29 | 2019-03-22 | 江苏大学 | A kind of dynamic path planning method under the conditions of car networking |
| CN110766220A (en) * | 2019-10-21 | 2020-02-07 | 湖南大学 | Local path planning method for structured road |
| CN111273668A (en) * | 2020-02-18 | 2020-06-12 | 福州大学 | Unmanned vehicle motion track planning system and method for structured road |
| CN111457931A (en) * | 2019-01-21 | 2020-07-28 | 广州汽车集团股份有限公司 | Method, device, system and storage medium for controlling local path re-planning of autonomous vehicle |
| CN111750887A (en) * | 2020-06-11 | 2020-10-09 | 上海交通大学 | Unmanned vehicle trajectory planning method and system for reducing accident severity |
| CN112362074A (en) * | 2020-10-30 | 2021-02-12 | 重庆邮电大学 | Intelligent vehicle local path planning method under structured environment |
| CN112673234A (en) * | 2020-01-17 | 2021-04-16 | 华为技术有限公司 | Path planning method and path planning device |
| CN113581181A (en) * | 2021-08-04 | 2021-11-02 | 武汉理工大学 | Intelligent vehicle overtaking track planning method |
-
2021
- 2021-11-02 CN CN202111286331.2A patent/CN113721637B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109501799A (en) * | 2018-10-29 | 2019-03-22 | 江苏大学 | A kind of dynamic path planning method under the conditions of car networking |
| CN111457931A (en) * | 2019-01-21 | 2020-07-28 | 广州汽车集团股份有限公司 | Method, device, system and storage medium for controlling local path re-planning of autonomous vehicle |
| CN110766220A (en) * | 2019-10-21 | 2020-02-07 | 湖南大学 | Local path planning method for structured road |
| CN112673234A (en) * | 2020-01-17 | 2021-04-16 | 华为技术有限公司 | Path planning method and path planning device |
| CN111273668A (en) * | 2020-02-18 | 2020-06-12 | 福州大学 | Unmanned vehicle motion track planning system and method for structured road |
| CN111750887A (en) * | 2020-06-11 | 2020-10-09 | 上海交通大学 | Unmanned vehicle trajectory planning method and system for reducing accident severity |
| CN112362074A (en) * | 2020-10-30 | 2021-02-12 | 重庆邮电大学 | Intelligent vehicle local path planning method under structured environment |
| CN113581181A (en) * | 2021-08-04 | 2021-11-02 | 武汉理工大学 | Intelligent vehicle overtaking track planning method |
Non-Patent Citations (5)
| Title |
|---|
| CK.REN的博客: "轨迹规划1：Frenet坐标转化公式推导", 《CSDN博客》 * |
| GOPHAE的博客: "Bezier(贝塞尔)曲线的轨迹规划在自动驾驶中的应用(一)", 《CSDN博客》 * |
| YANG LINQUAN: "Path planning algorithm for mobile robot obstacle avoidance adopting Bezier curve based on Genetic Algorithm", 《IEEE》 * |
| 曾鹏云: "智能汽车主动避障轨迹优化及自适应协同控制研究", 《万方数据库在线》 * |
| 秦东方: "基于智能车换道行为的轨迹规划与跟踪控制研究", 《万方数据库在线》 * |
Cited By (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114355921A (en) * | 2021-12-28 | 2022-04-15 | 北京易航远智科技有限公司 | Vehicle tracking track generation method and device, electronic equipment and storage medium |
| CN114355921B (en) * | 2021-12-28 | 2022-10-18 | 北京易航远智科技有限公司 | Vehicle tracking track generation method and device, electronic equipment and storage medium |
| CN114510055A (en) * | 2022-02-18 | 2022-05-17 | 科大国创合肥智能汽车科技有限公司 | Method for constructing rear lane line |
| CN114518123A (en) * | 2022-02-24 | 2022-05-20 | 高德软件有限公司 | Information processing method |
| CN114442491A (en) * | 2022-04-08 | 2022-05-06 | 杭州国辰机器人科技有限公司 | Local path planning and tracking control method, device and medium for indoor robot |
| CN115571164A (en) * | 2022-11-03 | 2023-01-06 | 中科天极(新疆)空天信息有限公司 | Obstacle avoidance method and system based on automatic driving |
| CN117622227A (en) * | 2024-01-24 | 2024-03-01 | 广汽埃安新能源汽车股份有限公司 | Vehicle lane changing obstacle avoidance control method and device |
| CN117622227B (en) * | 2024-01-24 | 2024-04-16 | 广汽埃安新能源汽车股份有限公司 | Vehicle lane changing obstacle avoidance control method and device |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113721637B (en) | 2022-05-03 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113721637B (en) | Intelligent vehicle dynamic obstacle avoidance path continuous planning method and system and storage medium | |
| CN112068545B (en) | Method and system for planning running track of unmanned vehicle at crossroad and storage medium | |
| CN107702716B (en) | Unmanned driving path planning method, system and device | |
| CN111338340B (en) | Model prediction-based local path planning method for unmanned vehicle | |
| CN109540159B (en) | Rapid and complete automatic driving track planning method | |
| CN110954122B (en) | Automatic driving track generation method under high-speed scene | |
| CN110361013B (en) | Path planning system and method for vehicle model | |
| CN113359757A (en) | Improved hybrid A-x algorithm unmanned vehicle path planning and trajectory tracking method | |
| CN114234998A (en) | Unmanned multi-target-point track parallel planning method based on semantic road map | |
| CN111681452B (en) | Unmanned vehicle dynamic lane change track planning method based on Frenet coordinate system | |
| WO2020081536A1 (en) | Trajectory initialization | |
| CN113608531B (en) | Unmanned vehicle real-time global path planning method based on safety A-guidance points | |
| CN111006667B (en) | Automatic driving track generation system under high-speed scene | |
| CN111830979A (en) | Trajectory optimization method and device | |
| CN112327830B (en) | Planning method for automatic driving lane-changing track of vehicle and electronic equipment | |
| CN112810630A (en) | Method and system for planning track of automatic driving vehicle | |
| CN114771563A (en) | Method for realizing planning control of track of automatic driving vehicle | |
| CN113895463B (en) | Path planning method suitable for turning around of automatic driving vehicle | |
| CN113255998B (en) | Expressway unmanned vehicle formation method based on multi-agent reinforcement learning | |
| CN111896004A (en) | Narrow passage vehicle track planning method and system | |
| CN116185014A (en) | Intelligent vehicle global optimal track planning method and system based on dynamic planning | |
| CN112965476A (en) | High-speed unmanned vehicle trajectory planning system and method based on multi-window sampling | |
| US20220371613A1 (en) | Vehicle trajectory determination | |
| CN113670305A (en) | Parking trajectory generation method and device, computer equipment and storage medium | |
| CN111891116A (en) | Method for improving stability of lateral control of automatic driving |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |