CN113721637B - 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
- CN113721637B CN113721637B CN202111286331.2A CN202111286331A CN113721637B CN 113721637 B CN113721637 B CN 113721637B CN 202111286331 A CN202111286331 A CN 202111286331A CN 113721637 B CN113721637 B CN 113721637B
- Authority
- CN
- China
- Prior art keywords
- path
- obstacle
- planning
- point
- vehicle
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 57
- 230000033001 locomotion Effects 0.000 claims description 64
- 230000008569 process Effects 0.000 claims description 19
- 230000008859 change Effects 0.000 claims description 12
- 238000004590 computer program Methods 0.000 claims description 6
- 230000001965 increasing effect Effects 0.000 claims description 3
- 238000005457 optimization Methods 0.000 claims description 3
- 230000009466 transformation Effects 0.000 abstract description 7
- 230000009286 beneficial effect Effects 0.000 abstract description 3
- 230000002708 enhancing effect Effects 0.000 abstract description 2
- 238000006243 chemical reaction Methods 0.000 description 6
- 238000010586 diagram Methods 0.000 description 6
- 230000003068 static effect Effects 0.000 description 4
- 238000010276 construction Methods 0.000 description 3
- 230000004888 barrier function 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
- 230000001133 acceleration Effects 0.000 description 1
- 230000001186 cumulative 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
- 230000036544 posture Effects 0.000 description 1
- 239000000126 substance Substances 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, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Aviation & Aerospace Engineering (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Driving Devices And Active Controlling Of Vehicle (AREA)
- Traffic Control Systems (AREA)
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; if the obstacle generates a certain lateral movement, the coordinate of the path end point is changed.
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 s is0，loIs the coordinate of the first control point, s1，l1Is the coordinate of the second control point, m is the distance between the first control point and the second control point, and is the first variable to be optimized of the Bezier curve, psi0The 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 s is2，l2Is the coordinate of the third control point, s3，l3Is the coordinate of the fourth control point, m is the distance between the third control point and the fourth control point, and is the second variable to be optimized of the Bezier curve, psi3The 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 double-movement line path 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) The method comprises the steps of acquiring high-precision map information, self-vehicle motion state information and obstacle motion state information in real time, wherein obstacles mainly refer to other traffic participants (vehicles, pedestrians, riders and the like) around the self-vehicle, the motion state information mainly comprises positions (x, y), postures psi, speeds v, accelerations a and the like, and all the information is acquired by sensors based on a global coordinate system (Cartesian coordinate system).
(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 main purpose of coordinate transformation is to realize the mutual transformation between coordinates (x, y) in a cartesian coordinate system and coordinates (S, L) in a Frenet coordinate system, with 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), as a reference line of the global path.
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 (x, y) to Frenet coordinates (s, l)
In an autopilot system, the global path is usually distributed in the form of discrete points, known as a set of discrete points at a fixed distance, and stored in the form of a k × 3 table, where each row represents a point, and one to three columns represent the path length s, the lateral position x, and the longitudinal position y of each point in cartesian coordinates, respectively, where the path length s can be replaced by an accumulated approximation of the linear distance between the points. 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).
Knowing (x, y), find (s, l) in the corresponding Frenet coordinate.
Firstly, finding out the point (x, y) nearest to the global path in a Cartesian coordinate systemn，yn) I.e. (x-x)n)2+(y-yn)2At minimum, (x) is in the table of global pathsn，yn) Corresponding SnI.e., s for point (x, y) in the Frenet coordinate system, and [ (x-x)n)2+(y-yn)2]1/2I.e. the absolute value of l in the Frenet coordinate system.
Then using two vectorsThe cross product of (d) is determined as the positive or negative of (l) (right hand rule). Noting the point (x) on the global pathn，yn) Is the point (x)nn，ynn) Let space vector a be (x)nn-xn，ynn-yn，0)、b＝(x-xn，y-yn0), c is a × b, the positive and negative of the third element of the space vector c are the positive and negative of l, the positive representative point (x, y) is on the left side of the global path, and the negative represents the right side.
(2) Frenet coordinate (s, l) to Cartesian coordinate system (x, y)
It is known that (s, l) in Frenet coordinates finds the corresponding (x, y).
First, knowing s, the closest point (x, y) on the global path in Cartesian coordinates (x, y) can be known from the global path tablen，yn) Then according to the root point (x)n，yn) And point (x)nn，ynn) Calculating a unit direction vector e of the closest point1＝(ex1，ey1) (ii) a l is regular e1Clockwise rotation of theta 3 pi/2, and negative l will e1Rotate clockwise by theta pi/2 to obtain point (x)n，yn) Unit vector e connecting to point (x, y)2＝(ex2，ey2) Wherein e isx2＝ey1sinθ+ex1cosθ，ey2＝ey1cosθ-ex1sin θ; finally, point (x, y) is obtained, where x ═ xn+|l|ex2，y＝yn+|l|ey2。
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 by the same method, and it is noted that the terminal point of the regression road section must be on the global path regardless of the change, i.e. lC＝0。
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(s) of a planned starting point aA，lA) Determined by the timing distance longitudinal following model,
wherein s isob，AIs the S coordinate, v, of the obstacle at point AegoIs the speed of the bicycle, tfAnd dfThe 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 follows the global pathWhen the vehicle runs to the point A of the local path planning starting point, the coordinate(s) of the obstacle avoidance section terminal point B is calculated according to the motion state of the front obstacle in the following modeB，lB)：
Wherein s isob，A、vob，A、l0b，ARespectively is the S coordinate, the moving speed in the S direction and the L coordinate of the obstacle when the self vehicle is at the point A, and delta t1Estimating the consumed time for avoiding the obstacle road section, wherein the estimated consumed time is equal to the difference value s between the road section end point and the current point divided by the current vehicle speed, wob、wegoAnd w are the width of the obstacle, the width of the own vehicle and the expected lateral distance 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 Δ tsUpdating the motion state information of the obstacle when the motion state of the obstacle changes (such as suddenly accelerating, decelerating or moving in a certain lateral direction), i.e. updating the real-time position Sob, l of the obstacleobAnd velocity vobChange in value of (e.g. /)obVariation exceeding. + -. 0.1m or vobAnd if the change exceeds +/-0.1 m/s, 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 is composed of four control points P0、P1、P2、P3Determining, as shown in FIG. 4, a Bezier curve diagram under different control point distributions, P0、P3I.e. the start and end points of the path,P1、P2for controlling the change in curvature of the path.
By PbPoints on the Bezier curve are represented, then
Pb＝(1-τ)P2，1+τP2，2Formula 3;
wherein the content of the first and second substances,
τ varies from 0 to 1, with smaller spacing, denser points on the bezier curve.
The determination of the third-order Bessel curve requires finding the coordinate P of four control points in the Frenet coordinate system0(s0，l0)、P1(s1，l1)、P2(s2，l2)、P3(s3，l3)。
First control point P of the Bezier curve0(s0，l0) 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 P of the Bezier curve1(s1，l1) The solution is as follows:
wherein m is a control point P0、P1For the first variable to be optimized of the Bezier curve, #0This 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 P of the Bezier curve3(s3，l3) And the terminal point of the AB obstacle avoidance section or the BC regression section in the local path is obtained.
Third control point P of the Bezier curve2(s2，l2) The solution is as follows:
wherein n is a control point P2、P3For the second variable to be optimized of the Bezier curve, #3The 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 (point a or point a 'in fig. 5) and the end point (point B or point B' in fig. 5) of the obstacle avoidance segment are the first control point P of the third-order bezier curve0(s0，l0) And a fourth control point P3(s3，l3) Second control point P1(s1，l1) And a third control point P2(s2，l2) Calculated as follows:
wherein m is a control point P0、P1For the first variable to be optimized of the Bezier curve, #0Is the heading angle of the vehicle at the first control point in Frenet coordinate, and n is the control point P2、P3For the second variable to be optimized of the Bezier curve, #3The 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 m and n to be optimized, the embodiment optimizes m and n with the aim of minimizing the cumulative sum of curvature increments of adjacent path points,
in equation 8, ρiAnd ρi-1Respectively representing the curvatures of the ith point and the ith-1 point, J represents an optimization function, and k is the number of path points.
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. Firstly, the terminal point of the regression road section is determined, and it needs to be noted that the terminal point of the regression road section can only be on the global path, namely lCAnd when the current point and the road section terminal point are connected by using Bezier curve planning, 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 B of the local path obstacle avoidance section is determined according to the initial motion states of the self-vehicle and the front obstacle(s)B，lB) And then planning a path by utilizing a third-order Bezier curve.
(2) The self-vehicle runs along the planned route and is driven at a certain time step delta tsUpdating the motion state information of the obstacle, and when the motion state of the obstacle changes (such as suddenly accelerates, decelerates or generates certain lateral movement), correspondingly adjusting (increasing) the path end point BDecrease sBOr change lB)。
(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 C(s) of the local path regression section according to the current stateB，lB) 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 plan the path again 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, a computer-readable storage medium is provided, and a computer program is stored on the computer-readable storage medium, and when the computer program is executed by a processor, the steps of the method for continuously planning a dynamic obstacle avoidance path of an intelligent vehicle are implemented.
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 (9)
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; in the Frenet coordinate system, all local obstacle avoidance path planning problems along the global path are converted into double-moving line path planning problems under the straight line road, and the double-moving line local path comprises the following steps: performing primary single-movement route path planning for avoiding the obstacle, updating in real time in the tracking process, returning to the global route after avoiding the obstacle, performing primary single-movement route path planning again, and updating in real time in the tracking process;
the method comprises the following steps of carrying out vehicle tracking driving according to a planned local path, obtaining motion state information of an obstacle in a preset time step length, and carrying out path re-planning 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;
when the self-vehicle drives to a point A of a local path planning starting point along a global path and the motion state of the obstacle at the current side is updated, the real-time position s of the obstacleob、lobAnd velocity vobChange in the value ofCorrespondingly updating the road section terminal, and calculating the coordinate(s) of the obstacle avoidance road section terminal B in the following modeB，lB)：
Wherein s isob，A、vob，A、lob，ARespectively is the S coordinate, the moving speed in the S direction and the L coordinate of the obstacle when the self vehicle is at the point A, and delta t1The estimated time consumption for avoiding the obstacle road section is equal to the difference value of the road section end point and the current point divided by the current speed, wob、wegoW is the width of the obstacle, the width of the self vehicle and the expected lateral distance between the self vehicle and the obstacle respectively;
the corresponding updating of the segment end point further comprises: when the obstacle suddenly accelerates, increasing the route end point; when the obstacle decelerates suddenly, the s coordinate of the path end point is reduced; when the obstacle moves laterally, changing the coordinate of the path end point;
the starting point and the end point of the obstacle avoidance road section are the first control point P of the third-order Bezier curve0(s0，l0) And a fourth control point P3(s3，l3) Second control point P1(s1，l1) And a third control point P2(s2，l2) Calculated as follows:
wherein m is a control point P0、P1For the first variable to be optimized of the Bezier curve, #0Is the heading angle of the vehicle at the first control point in Frenet coordinate, and n is the control point P2、P3For the second variable to be optimized of the Bezier curve, #3A course angle of a road section terminal point;
optimizing m and n by taking the accumulative sum of curvature increments of adjacent path points as the minimum as a target,
in equation 8, ρiAnd ρi-1Respectively representing the curvatures of the ith point and the ith-1 point, J represents an optimization function, and k represents the number of path points.
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, when 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 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.
6. 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.
7. The intelligent vehicle dynamic obstacle avoidance path continuous planning method according to claim 6, wherein the second control point is solved according to the following formula:
wherein s is0，l0Is the coordinate of the first control point, s1，l1Is the coordinate of the second control point, m is the distance between the first control point and the second control point, and is the first variable to be optimized of the Bezier curve, psi0The 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 s is2，l2Is the coordinate of the third control point, s3，l3Is the coordinate of the fourth control point, m is the distance between the third control point and the fourth control point, and is the second variable to be optimized of the Bezier curve, psi3The 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.
8. The intelligent vehicle dynamic obstacle avoidance path continuous planning system is realized by adopting the method of any one of claims 1 to 7, 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.
9. 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 7.
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 CN113721637A (en) | 2021-11-30 |
| CN113721637B true 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) |
Families Citing this family (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114355921B (en) * | 2021-12-28 | 2022-10-18 | 北京易航远智科技有限公司 | Vehicle tracking track generation method and device, electronic equipment and storage medium |
| CN114510055B (en) * | 2022-02-18 | 2022-11-08 | 科大国创合肥智能汽车科技有限公司 | Method for constructing rear lane line |
| CN114518123B (en) * | 2022-02-24 | 2024-06-11 | 高德软件有限公司 | 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 |
| CN117622227B (en) * | 2024-01-24 | 2024-04-16 | 广汽埃安新能源汽车股份有限公司 | Vehicle lane changing obstacle avoidance control method and device |
| CN118135542A (en) * | 2024-05-06 | 2024-06-04 | 武汉未来幻影科技有限公司 | Obstacle dynamic and static state judging method and related equipment thereof |
Family Cites Families (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109501799B (en) * | 2018-10-29 | 2020-08-28 | 江苏大学 | Dynamic path planning method under condition of Internet of vehicles |
| CN111457931B (en) * | 2019-01-21 | 2021-09-24 | 广州汽车集团股份有限公司 | 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 |
| EP4092386A4 (en) * | 2020-01-17 | 2023-02-01 | Huawei Technologies Co., Ltd. | Path planning method and path planning apparatus |
| CN111273668B (en) * | 2020-02-18 | 2021-09-03 | 福州大学 | Unmanned vehicle motion track planning system and method for structured road |
| CN111750887B (en) * | 2020-06-11 | 2023-11-21 | 上海交通大学 | Unmanned vehicle track planning method and system for reducing accident severity |
| CN112362074B (en) * | 2020-10-30 | 2024-03-19 | 重庆邮电大学 | Intelligent vehicle local path planning method under structured environment |
| CN113581181B (en) * | 2021-08-04 | 2022-09-27 | 武汉理工大学 | Intelligent vehicle overtaking track planning method |
-
2021
- 2021-11-02 CN CN202111286331.2A patent/CN113721637B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN113721637A (en) | 2021-11-30 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113721637B (en) | Intelligent vehicle dynamic obstacle avoidance path continuous planning method and system and storage medium | |
| CN107702716B (en) | Unmanned driving path planning method, system and device | |
| CN112673234B (en) | Path planning method and path planning device | |
| CN110333714B (en) | Unmanned vehicle path planning method and device | |
| CN111338340B (en) | Model prediction-based local path planning method for unmanned vehicle | |
| CN109927716B (en) | Autonomous vertical parking method based on high-precision map | |
| CN111898804B (en) | Road self-adaptive sampling method for local planning | |
| CN110361013B (en) | Path planning system and method for vehicle model | |
| CN114234998A (en) | Unmanned multi-target-point track parallel planning method based on semantic road map | |
| CN110954122B (en) | Automatic driving track generation method under high-speed scene | |
| JP6740477B2 (en) | Automatic parking controller | |
| CN113916246A (en) | Unmanned obstacle avoidance path planning method and system | |
| US11586209B2 (en) | Differential dynamic programming (DDP) based planning architecture for autonomous driving vehicles | |
| CN113608531B (en) | Unmanned vehicle real-time global path planning method based on safety A-guidance points | |
| CN111634292B (en) | Collision prediction method for mining area | |
| 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 | |
| CN111006667A (en) | Automatic driving track generation system under high-speed scene | |
| CN113255998B (en) | Expressway unmanned vehicle formation method based on multi-agent reinforcement learning | |
| CN116185014A (en) | Intelligent vehicle global optimal track planning method and system based on dynamic planning | |
| CN112947469A (en) | Automobile track-changing track planning and dynamic track tracking control method | |
| CN112965476A (en) | High-speed unmanned vehicle trajectory planning system and method based on multi-window sampling | |
| US11780464B2 (en) | Autonomous vehicle trajectory generation using velocity-based steering limits | |
| CN111891116A (en) | Method for improving stability of lateral control of automatic driving | |
| CN111896004A (en) | Narrow passage vehicle track planning method and system |
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 |