CN107121980B - A Virtual Constraint-Based Path Planning Method for Autonomous Driving Vehicles - Google Patents

Abstract

The present invention relates to a kind of automatic driving vehicle paths planning method based on virtual constraint, comprising the following steps: the lane center course under bodywork reference frame is obtained according to lane detection result；The course of vehicle is modified using the lane center course of Kalman filter and acquisition；The path point in original path is corrected using revised vehicle course；Based on three-dimensional laser radar grating map, the opposed lateral positions relationship of automatic driving vehicle and road is obtained；Generate virtual constraint；According to the decision instruction of automated driving system, expected path is generated.The present invention merges the accurate positioning that GPS, vision and laser radar information complete vehicle and road opposed lateral positions, high accuracy positioning equipment and high-precision map are not needed, help to improve automatic driving vehicle structuring, have the lane under the city of clear carriageway line or highway environment keep with lane-change ability, be of great significance for safe and stable, the smooth traveling of automatic driving vehicle.

Description

A Virtual Constraint-Based Path Planning Method for Autonomous Driving Vehicles

technical field

The invention relates to the technical field of automatic driving, in particular to a path planning method for automatic driving vehicles based on virtual constraints.

Background technique

Path planning is one of the core technologies of autonomous vehicles, and its role is to ensure that the vehicle can track the global path safely. In the urban road environment or highway environment, the autonomous vehicle not only needs to drive smoothly and stably in the lane where the vehicle is located, but also needs to combine environmental perception information and intelligent decision-making instructions to perform more complex actions such as lane changing and merging. In the lane keeping condition, in order to ensure that the vehicle accurately follows the center line of the lane and maintains lateral stability, the vehicle needs to be constrained in its lane; in the lane changing condition, it is necessary to ensure that the vehicle can accurately change into In the target lane, avoid riding on the line or driving under the line. In order to achieve the above goals, the path planning of the autonomous vehicle needs to rely on the precise positioning of the relative lateral position of the vehicle and the road. Some studies have used the combination of differential GPS and high-precision maps to achieve precise positioning of the relationship between the vehicle and the lateral position of the road, but this method is limited by the range of differential GPS signals and the mapping of high-precision maps.

At this stage, 3D lidars and cameras that can provide rich environmental perception information such as road edges and lane lines for autonomous vehicles are still the most important sensors on autonomous vehicles. It is necessary to use existing sensors to design a path planning method that integrates environmental perception information and general GPS positioning information.

SUMMARY OF THE INVENTION

In view of the above analysis, the present invention aims to provide a path planning method for autonomous driving vehicles based on virtual constraints, so as to solve the problem that the prior art is limited by the range of action of differential GPS signals and high-precision maps.

The object of the present invention is mainly achieved through the following technical solutions:

A virtual constraint-based path planning method for an autonomous vehicle, comprising the following steps:

Step S1: Obtain the heading of the lane centerline in the vehicle body coordinate system;

Step S2: using the lane centerline heading obtained in step S1 to correct the vehicle heading;

Step S3: using the vehicle heading corrected in step S2 to correct the waypoints in the original path;

Step S4: obtaining the relative lateral position relationship between the autonomous vehicle and the road based on the three-dimensional lidar grid map;

Step S5: generate virtual constraints according to the relative lateral positional relationship between the autonomous driving vehicle and the road obtained in step S4, where the virtual constraints include boundary constraints of the lane and adjacent lane boundary constraints;

Step S6: Generate a desired path according to the virtual constraints generated in step S5 and the decision instruction of the automatic driving system.

The step S1 also includes the following steps:

Step S101: determine whether the lane line detection result is valid, if valid, execute step S102; if invalid, continue to use the lane line detection result of the previous cycle;

Step S102: Obtain the heading of the lane centerline in the vehicle body coordinate system.

The step S102 further includes the following steps:

Step S1021: Calculate the position of the lane centerline

Among them, the lane line detection results (x ₁ , y ₁ ), (x ₂ , y ₂ ), (x ₃ , y ₃ ) and (x ₄ , y ₄ ) are the coordinate points of the left and right lane lines in the vehicle body coordinate system , (x ₁ , y ₁ ) is the approach point in the front left of the vehicle, (x ₂ , y ₂ ) is the far approach point in the front left, (x ₃ , y ₃ ) is the approach point in the front right, (x ₄ , y _{4 )} ) is the far vehicle point in front of the right;

(x _c1 , y _c1 ) and (x _c2 , y _c2 ) represent the near-vehicle end and the far-vehicle end of the lane center line, respectively;

Step S1022: Calculate the heading of the lane centerline:

where θ _Lane represents the heading of the lane centerline in the vehicle body coordinate system.

The step S2 also includes the following steps:

Step S201: initialize the Kalman filter;

Step S202: Predict the predicted value of the vehicle heading in the geodetic coordinate system in this period according to the current turning radius and speed of the vehicle:

Among them, θ _veh-pred is the predicted value of the current vehicle heading in the geodetic coordinate system, θ _{veh-correct-old} is the corrected value of the vehicle heading in the previous cycle in the geodetic coordinate system, v is the current speed of the vehicle, and R is the current vehicle heading The turning radius of , T is the planning period;

Step S203: Calculate the difference between the original path heading and the lane centerline heading in the vehicle body coordinate system:

θ _error = θ _road - θ _Lane

Among them, θ _error is the difference between the original path heading and the lane centerline heading in the vehicle body coordinate system, θ _road is the original path heading in the vehicle body coordinate system, and θ _Lane is the lane center line heading;

Step S204: Calculate the heading measurement value of the vehicle in the geodetic coordinate system:

θ _veh-measure = θ _veh-gps + θ _error ,

Among them, θ _veh-measure is the heading measurement value of the vehicle in the geodetic coordinate system, and θ _veh-gps is the GPS heading value of the vehicle in the geodetic coordinate system;

Step S205: Use θ _veh-pred in step S202 and θ _veh-measure in step S204 as the input of the Kalman filter to obtain the heading correction value θ _veh-correct of the vehicle in the geodetic coordinate system;

Step S206: Assign θ _veh-correct to θ _{veh-correct-old} and enter the next cycle.

The step S3 also includes the following steps:

Step S301: Calculate the deviation between the vehicle heading correction value and the vehicle GPS heading value in the geodetic coordinate system:

Δθ = _{θveh -correct- θveh-gps} ;

Among them, θ _veh-correct is the heading correction value of the vehicle in the geodetic coordinate system, and θ _veh-gps is the GPS heading value of the vehicle in the geodetic coordinate system;

Step S302: Calculate the corrected waypoint using the result in step S301:

Among them, [x _correct y _correct θ _correct ] ^T is the coordinate of the corrected path in the vehicle body coordinate system, [x _veh-road y _veh-road θ _veh-road ] ^T is the original path before correction in the vehicle body coordinate system the coordinates below.

The step S4 also includes the following steps:

Step S401 : count the number of grids OGN _i occupied in each column in the lidar grid map;

Step S402: According to the number OGN _i of the grids occupied by each column, calculate the statistical sign amount OGS _i of each column of grids:

where i is the column number in the grid map, OGS _i represents the statistical flag amount of the i-th grid in the grid map, and O _t represents the occupancy flag threshold of the grid map;

Step S403: Generate a convolution kernel according to the road model width RW and the road model width deviation RWT:

where K _j is the convolution kernel corresponding to the grid in the jth column, RW is the width of the road model, RWT is the width deviation of the road model, G _s is the column number of the grid selected as the pulse, and the value range of G _s is [ 0, RWT];

Step S404: Convolve the result OGS _i in step S402 with the result K _j in step S403

Among them, MW is the number of grid columns in the grid map. During the convolution process, if re=1, then the matching value hit of the corresponding grid column is increased by 1; if re=-1, then the corresponding grid column is not matched. The value of miss is incremented by 1; if re=0, then the unknown value uknow of the corresponding grid column is incremented by 1;

Step S405: According to the result in step S404, obtain the probability GC _i that a certain column of grids is the center of the road:

where GC _i represents the probability that the i-th grid is the center of the road;

Step S406: Find the maximum value of GC _i , and the corresponding i is the column number of the grid where the road center is located;

C _r = i

where C _r is the grid column number where the road center is located;

Step S407: Calculate the grid column numbers where the left and right road edges are located;

Among them, L _r and R _r are the column numbers of the grid where the left and right road edges are located;

Step S408: Calculate the relative lateral positional relationship between the vehicle and the road

where D _l and D _r are the distances from the vehicle to the left and right curbs, respectively.

In the step S401, statistics are started from the middle column of the grid map, that is, the column where the vehicle is located, along the direction perpendicular to the corrected path to both sides of the grid map.

The step S5 also includes the following steps:

Step S501: Calculate the total number of lanes Cz;

Step S502: locate the lane C where the vehicle is located;

The serial number of the lane where the vehicle is located:

Among them, C represents the left-numbered serial number of the lane where the vehicle is located, and LW is the lane width;

Step S503: Calculate the distance D _c from the vehicle to the center line of the lane where it is located according to the positions of the center line of the lane (x _c1 , y _c1 ) and (x _c2 , y _c2 );

Step S504: Calculate the distance D _cl from the vehicle to the center line of the left lane and the distance D _cr from the vehicle to the center line of the right lane according to the distance D _c from the vehicle to the center line of the lane where it is located and the lane width LW:

D _cl =LW-D _c

_Dcr =LW+ _Dc

Step S505: Generate virtual constraints according to the distances between the vehicle and the center line of each lane, where the virtual constraints include boundary constraints of the lane where it is located and boundary constraints of adjacent lanes.

When there are lanes on the left and right sides of the lane, the virtual constraints are the left constraint _Vll of the left lane, the left constraint _{Vcl of the lane, the right constraint Vcr of} the lane, and the right constraint _Vrr of the right lane; When there is a lane on the side, the false constraint is the left constraint V _ll of the left lane, the left constraint V _cl of the current lane, and the right constraint V _cr of the current lane; when there is only a lane on the right side, the virtual constraint is the The left constraint V _cl , the right constraint V _cr of the current lane, and the right constraint V _rr of the right lane.

The step S6 also includes the following steps:

Step S601: determine whether the decision-making command changes lanes, if so, go to step S603; if not, go to step S602;

Step S602: Translate the corrected path to the centerline of the lane where it is located and track it as the desired path, and go to step S604;

Step S603: Translate the corrected path to the centerline of the target lane as a desired path for tracking, and go to step S604;

Step S604: Perform path tracking according to the desired path.

The beneficial effects of the present invention are as follows:

The virtual constraint-based automatic driving vehicle path planning method proposed by the invention integrates GPS, vision and lidar information to complete the precise positioning of the relative lateral position between the vehicle and the road, without requiring high-precision positioning equipment and high-precision maps. When driving in lane, it can effectively ensure that the vehicle travels along the center line of the lane where it is located, ensuring the lateral stability of the vehicle when driving automatically; when changing lanes, it can ensure that the vehicle changes to the target lane accurately, avoiding the automatic driving process. The vehicle rides or presses the line. The invention helps to improve the lane keeping and lane changing capability of the automatic driving vehicle in a structured city or highway environment with clear lane lines, and is of great significance to the safe, stable and smooth driving of the automatic driving vehicle.

Other features and advantages of the invention will be set forth in the description which follows, and in part will be apparent from the description, or may be learned by practice of the invention. The objectives and other advantages of the invention may be realized and attained by the structure particularly pointed out in the written description, claims, and drawings.

Description of drawings

The drawings are for the purpose of illustrating specific embodiments only and are not to be considered limiting of the invention, and like reference numerals refer to like parts throughout the drawings.

Fig. 1 is the overall flow of the automatic driving vehicle path planning method based on virtual constraints in the present invention;

Fig. 2 is the flow chart that the present invention utilizes the Kalman filter principle to correct the heading of the vehicle;

FIG. 3 is a flow chart of obtaining the relative lateral position relationship between the vehicle and the road based on the lidar grid map in the present invention;

4 is a schematic diagram of counting the number of grids occupied by each column on the lidar grid map in the present invention;

5 is a schematic diagram of a generation method of a convolution kernel when obtaining a relative lateral positional relationship between a vehicle and a road in the present invention;

6 is a flow chart of virtual constraint generation in the present invention;

7 is a schematic diagram of virtual constraint generation in the present invention;

FIG. 8 is a schematic diagram of generating a desired path in the present invention.

Reference number:

401 - Vehicle body coordinate system, 402 - Vehicle, 403 - A column of grids in the middle of the grid map, 404 - Corrected path, 405 - Left and right boundaries of grid map, 406 - Statistical direction;

501-road model width deviation RWT, 502-raster selected as impulse, 503-boundary of road model width, 504-road boundary, 507-generated convolution kernel _Kj ;

702-virtual constraints, 703-lane lines;

805 - Lane centerline, 807 - Desired path when changing lanes left, 808 - Desired path when lane keeping, 809 - Desired path when changing lanes right.

Detailed ways

The preferred embodiments of the present invention are described below in detail with reference to the accompanying drawings, wherein the accompanying drawings constitute a part of the present application, and together with the embodiments of the present invention, serve to explain the principles of the present invention.

According to a preferred embodiment of the present invention, a virtual constraint-based automatic driving vehicle path planning method is provided, as shown in FIG. 1 , including the following steps:

Step S1: obtaining the heading of the lane center line in the vehicle body coordinate system according to the lane line detection result;

Specifically, the step S1 further includes the following sub-steps:

Step S101: determine whether the lane line detection result is valid, if valid, execute step S102; if invalid, continue to use the lane line detection result of the previous cycle;

In this embodiment, the autonomous vehicle performs lane keeping and lane changing in a structured environment with clear lane lines and road edges, and the visual sensor of the autonomous vehicle can detect the lane line information, but considering that there is no lane information at the intersection Therefore, it is necessary to judge whether the detection result of the lane line is valid.

Step S102: obtaining the heading of the lane centerline in the vehicle body coordinate system;

The lane line detection results (x ₁ , y ₁ ), (x ₂ , y ₂ ), (x ₃ , y ₃ ) and (x ₄ , y ₄ ) are the coordinate points of the left and right lane lines in the vehicle body coordinate system, where , (x ₁ , y ₁ ) is the approach point in the front left of the vehicle, (x ₂ , y ₂ ) is the far approach point in the front left, (x ₃ , y ₃ ) is the approach point in the front right, (x ₄ , y _{4 )} ) is the far vehicle point in front of the right. According to these four points, find the position and heading of the lane center line in the vehicle body coordinate system:

Step S1021: Calculate the position of the lane centerline

where (x _c1 , y _c1 ) and (x _c2 , y _c2 ) represent the near-vehicle end and the far-vehicle end of the lane centerline, respectively;

Step S1022: Calculate the heading of the lane centerline

where θ _Lane represents the heading of the lane centerline in the vehicle body coordinate system.

Step S2: correct the heading of the vehicle by using the Kalman filter and the heading of the lane center line obtained in step S1;

Since the heading of the vehicle obtained from the on-board GPS has time lag and spatial deviation, when it is directly used to convert the global path in the geodetic coordinate system to the local path in the vehicle body coordinate system, it will lead to a large error, so that the vehicle cannot accurately track the actual path;

The heading of the lane center line is relatively stable, so the heading of the lane center line can be used to correct the heading of the vehicle. Considering the influence of measurement noise, the corrected heading of the vehicle needs to be maintained by the Kalman filter;

Specifically, as shown in FIG. 2 , the step S2 includes the following steps:

Step S201: initialize the Kalman filter;

Step S202: Predict the predicted value θ _veh-pred of the vehicle heading under the geodetic coordinate system in this period according to the current turning radius and speed of the vehicle;

where θ _veh-pred is the predicted value of the current vehicle heading in the geodetic coordinate system, θ _{veh-correct-old} is the corrected value of the vehicle heading in the previous cycle in the geodetic coordinate system, v is the current speed of the vehicle, and R is the current vehicle heading Turning radius, T is the planning period.

Step S203: Calculate the difference between the original path heading and the lane centerline heading in the vehicle body coordinate system:

The path heading and the lane centerline heading are both headings in the vehicle body coordinate system, and the lane centerline heading is parallel to the tangent direction of the path. Since the heading of the lane center line is directly calculated in the vehicle body coordinate system according to the detection result of the lane line, and the path heading is obtained by converting the GPS coordinates of the vehicle as the base point, it is only required to obtain the path heading in the vehicle body coordinate system. The heading with the lane centerline is equivalent to finding the difference θ _error between the GPS heading of the vehicle and its true heading:

θ _error = θ _road - θ _Lane

Among them, θ _error is the difference between the original path heading and the lane centerline heading in the vehicle body coordinate system, θ _road is the original path heading in the vehicle body coordinate system, and θ _Lane is the lane center line heading;

Step S204: Calculate the heading measurement value of the vehicle in the geodetic coordinate system:

θ _veh-measure = θ _veh-gps + θ _error

Among them, θ _veh-measure is the heading measurement value of the vehicle in the geodetic coordinate system, and θ _veh-gps is the GPS heading value of the vehicle in the geodetic coordinate system;

Step S205: Use θ _veh-pred in step S202 and θ _veh-measure in step S204 as the input of the Kalman filter to obtain the heading correction value θ _veh-correct of the vehicle in the geodetic coordinate system;

Step S206: Assign θ _veh-correct to θ _{veh-correct-old} and enter the next cycle.

Step S3: using the vehicle heading corrected in step S2 to correct the waypoints in the original path;

The original path is a path under vehicle body coordinates, and the starting point of the original path is the origin of the vehicle body coordinate system;

Due to the inaccuracy of the vehicle heading, the original path in the vehicle body coordinate system also has errors. Therefore, it is necessary to correct the original path according to the corrected vehicle heading. The heading of the corrected path is consistent with the heading of the lane where the vehicle is located.

Specifically, it includes the following steps:

Step S301: Calculate the deviation between the vehicle heading correction value and the vehicle GPS heading value in the geodetic coordinate system:

Δ _θ = θ _veh-correct -θ _veh-gps

Step S302: Calculate the corrected waypoint using the result in Step S301:

Where [x _correct y _correct θ _correc ] _t ^T is the coordinate of the corrected path in the vehicle body coordinate system, [x _veh-road y _veh-road θ _veh-road ] ^T is the original path before correction in the vehicle body coordinate system the coordinates below.

Step S4: obtaining the relative lateral position relationship between the autonomous vehicle and the road based on the three-dimensional lidar grid map;

The three-dimensional lidar grid map is a grid map in the vehicle body coordinate system, and each grid corresponds to a value indicating whether the grid is occupied by an obstacle;

Due to the existence of road GPS point coordinate errors, it is impossible to determine the lateral positioning of the vehicle on the road only by relying on the GPS positioning of the vehicle, the GPS positioning of the waypoint and the width of the road model, so it is impossible to determine which lane the vehicle is in. In the present invention, based on the lidar grid map, the position of the road center in the grid map is obtained by using a statistical method, and then the relative relationship between the vehicle and the grid map and the width of the road model can be used to obtain the relative relationship between the vehicle and the road. The position of the center, the left edge of the road, and the right edge of the road.

Specifically, as shown in Figure 3, based on the three-dimensional lidar grid map, the process of obtaining the relative lateral positional relationship between the autonomous vehicle and the road includes the following steps:

Step S401 : count the number of grids OGN _i occupied in each column in the lidar grid map;

Preferably, as shown in FIG. 4 , statistics are started from the middle column of the grid map (ie, the column where the vehicle is located) along the direction perpendicular to the corrected path to the two sides of the grid map.

Step S402: According to the number OGN _i of the grids occupied by each column, calculate the statistical sign amount OGS _i of each column of grids:

where i is the column number in the grid map, OGS _i represents the statistical flag amount of the i-th grid in the grid map, and O _t represents the occupancy flag threshold of the grid map;

The principle of assigning the statistical result flag is as follows: if the number of grids occupied by the i-th column is greater than O _t , then the statistical result flag of this column of grids is assigned 1; if the number of grids occupied by the i-th column is not If it is greater than O _t , then check whether the number of grids occupied in the i-1th column is greater than O _t , if so, then the statistical result flag of the i-th column grid is assigned to 0, if not, then the i-th column The statistical result flag of the grid is assigned a value of -1.

Step S403: Generate a convolution kernel according to the road model width RW and the road model width deviation RWT:

where K _j is the convolution kernel corresponding to the grid in the jth column, RW is the width of the road model, RWT is the width deviation of the road model, G _s is the column number of the grid selected as the pulse, and the value range of G _s is [ 0, RWT];

As shown in Figure 5, the rules for generating convolution kernels are as follows: select a certain column grid (assuming the column number is G _s ) in the range of [0, RWT] as the pulse grid, if the column number j is less than G _s , then the corresponding K _j is assigned as 0; if the column number j is equal to G _s , then the corresponding K _j is assigned as 1; if the column number j is greater than G _s and less than RW-G _s , then the corresponding K _j is assigned as - 1; if the column number is equal to RW-G _s , then the corresponding K _j is assigned a value of 1; if the column number j is greater than RW-G _s and less than RW, then the corresponding K _j is assigned a value of 0. Among them, G _s should be taken from 0 to RWT in sequence.

Step S404: Convolve the result OGS _i in step S402 with the result K _j in step S403

Among them, MW is the number of grid columns in the grid map. During the convolution process, if re=1, then the matching value hit of the corresponding grid column is increased by 1; if re=-1, then the corresponding grid column is not matched. The value miss is incremented by 1; if re=0, the unknown value uknow of the corresponding grid column is incremented by 1.

Step S405: According to the result in step S404, obtain the probability GC _i that a certain column of grids is the center of the road:

where GC _i represents the probability that the i-th grid is the center of the road;

Since the value ranges of OGS _i and K _j are both {-1, 0, 1}, and the convolution value range of j is [-RW/2, G _s ], that is, K _j is not taken during the convolution process. -1, if the convolution result re=1 during the convolution process, then the matching value hit of the corresponding grid column is increased by 1; if re=-1, then the unmatched value miss of the corresponding grid column is increased by 1; if re =0, then the unknown value uknow of the corresponding grid column is increased by 1. Finally, if hit=2, then the probability that the corresponding i-th grid is the road center is assigned a value of 0.9; if hit=1, miss=1 or hit=1, uknow=1, then the corresponding i-th grid is a road The probability of the center is assigned a value of 0.5; if miss=2 or uknow=2 or miss=1, uknow=1, then the probability that the corresponding i-th column grid is the road center is assigned a value of 0.3.

Step S406: Find the maximum value of GC _i , and the corresponding i is the column number of the grid where the road center is located;

C _r = i

where C _r is the grid column number where the road center is located.

Step S407: Calculate the grid column numbers where the left and right road edges are located;

Among them, L _r and R _r are the column numbers of the grid where the left and right road edges are located, respectively.

Step S408: Calculate the relative lateral positional relationship between the vehicle and the road

where D _l and D _r are the distances from the vehicle to the left and right curbs, respectively.

Step S5: generate virtual constraints according to the relative lateral positional relationship between the autonomous driving vehicle and the road obtained in step S4, where the virtual constraints include boundary constraints of the lane and adjacent lane boundary constraints;

Since the behavior of the vehicle is divided into lane keeping and lane changing in the present invention, in the process of lane keeping, only the boundary constraints and collision avoidance of the lane where the vehicle is located only need to be considered; Obstacles in the target lane are sufficient. Therefore, according to the relative positioning relationship between the vehicle and the road, the width of the lane and the width of the road, the boundary constraints of the lane where the vehicle is currently located and the boundary constraints of the adjacent lanes are virtualized, so that the four virtual constraints can determine three lanes, so the vehicle can be kept laterally when the lane is kept. The stability of the movement ensures the accurate change to the centerline position of the target lane when changing lanes, and avoids riding on the line or driving on the line;

Specifically, as shown in FIG. 6 , the process of virtualizing the boundary constraints of the current lane and the boundary constraints of the adjacent lanes includes the following steps:

Step S501: Calculate the total number of lanes Cz;

Step S502: locate the lane C where the vehicle is located;

The serial number of the lane where the vehicle is located:

Among them, C is the left-numbered serial number of the lane where the vehicle is located, and LW is the lane width.

Step S503: Calculate the distance D _c from the vehicle to the center line of the lane where it is located according to the positions of the center line of the lane (x _c1 , y _c1 ) and (x _c2 , y _c2 );

Step S504: Calculate the distance D _cl from the vehicle to the center line of the left lane and the distance D _cr from the vehicle to the center line of the right lane according to the distance D _c from the vehicle to the center line of the lane where it is located and the lane width LW:

D _cl =LW-D _c

_Dcr =LW+ _Dc

Step S505: Generate a virtual constraint according to the distance between the vehicle and the center line of each lane, and the virtual constraint includes the boundary constraint of the lane where it is located and the boundary constraint of the adjacent lane;

Specifically, when there are lanes on the left and right sides of the lane, as shown in FIG. 7( a ), the virtual constraints are the left constraint _Vll of the left lane, the left constraint _{Vcl of the lane, and the right constraint Vcr of} the lane and the right constraint V _rr of the right lane; when there is only a lane on the left, as shown in Figure 7(b), the false constraints are the left constraint V _ll of the left lane, the left constraint V _cl of the lane where the lane is located, and the Right constraint V _cr ; when there is only a lane on the right side, as shown in Figure 7(b), the virtual constraints are the left constraint V _cl of the current lane, the right constraint V _cr of the current lane, and the right constraint V _rr of the right lane ;

Virtual constraints close to the road boundary have a safe distance from the road boundary.

Step S6: Generate a desired path according to the virtual constraints generated in step S5 and the decision instruction of the automatic driving system.

The corrected path can ensure that the heading is consistent with the direction of the lane line, but it cannot guarantee that the path is located in the center of the current lane. Therefore, the corrected path first needs to be translated to the center line of the current lane. the desired path when changing lanes, and translate the desired path to the centerline position of the target lane when changing lanes;

Fig. 8 is a schematic diagram of desired path generation;

Specifically, it includes the following steps:

Step S601: determine whether the decision-making command changes lanes, if so, go to step S603; if not, go to step S602;

Step S602: Translate the corrected path to the centerline of the lane where it is located as a desired path for tracking, and go to step S604;

Step S603: Translate the corrected path to the centerline of the target lane as a desired path for tracking, and go to step S604;

Step S604: Perform path tracking according to the desired path.

To sum up, the embodiments of the present invention provide a path planning method for an autonomous driving vehicle based on virtual constraints, which integrates GPS, vision and lidar information to complete the precise positioning of the relative lateral position of the vehicle and the road without requiring high-precision positioning equipment. and high-resolution maps. When driving in lane keeping, it can effectively ensure that the vehicle travels along the center line of the lane where it is located, ensuring the lateral stability of the vehicle during automatic driving; when changing lanes, it can ensure that the vehicle changes to the target lane accurately, avoiding the automatic driving process. The vehicle rides or presses the line. The invention helps to improve the lane keeping and lane changing capability of the self-driving vehicle in a structured city or highway environment with clear lane lines, and is of great significance to the safe, stable and smooth driving of the self-driving vehicle.

Those skilled in the art can understand that all or part of the process of implementing the methods in the above embodiments can be completed by instructing relevant hardware through a computer program, and the program can be stored in a computer-readable storage medium. Wherein, the computer-readable storage medium is a magnetic disk, an optical disk, a read-only storage memory or a random storage memory, and the like.

The above description is only a preferred embodiment of the present invention, but the protection scope of the present invention is not limited to this. Substitutions should be covered within the protection scope of the present invention.

Claims (10)

1. a kind of automatic driving vehicle path planning method based on virtual constraints, is characterized in that, comprises the following steps: Step S1: Obtain the heading of the lane centerline in the vehicle body coordinate system; Step S2: using the lane centerline heading obtained in step S1 to correct the vehicle heading; Step S3: using the vehicle heading corrected in step S2 to correct the waypoints in the original path; Step S4: obtaining the relative lateral position relationship between the autonomous vehicle and the road based on the three-dimensional lidar grid map; Step S5: generate virtual constraints according to the relative lateral positional relationship between the autonomous driving vehicle and the road obtained in step S4, and the virtual constraints include boundary constraints of the lane where it is located and boundary constraints of adjacent lanes; Step S6: Generate a desired path according to the virtual constraints generated in step S5 and the decision instruction of the automatic driving system. 2. The method according to claim 1, wherein the step S1 comprises the following steps: Step S101: determine whether the lane line detection result is valid, if valid, execute step S102; if invalid, continue to use the lane line detection result of the previous cycle; Step S102: Obtain the heading of the lane centerline in the vehicle body coordinate system. 3. The method according to claim 2, the step S102 comprises the following steps: Step S1021: Calculate the position of the lane centerline Among them, the lane line detection results (x ₁ , y ₁ ), (x ₂ , y ₂ ), (x ₃ , y ₃ ) and (x ₄ , y ₄ ) are the coordinate points of the left and right lane lines in the vehicle body coordinate system , (x ₁ , y ₁ ) is the approach point in the front left of the vehicle, (x ₂ , y ₂ ) is the far approach point in the front left, (x ₃ , y ₃ ) is the approach point in the front right, (x ₄ , y _{4 )} ) is the far vehicle point in front of the right; (x _c1 , y _c1 ) and (x _c2 , y _c2 ) represent the near-vehicle end and the far-vehicle end of the lane center line, respectively; Step S1022: Calculate the heading of the lane centerline: where θ _Lane represents the heading of the lane centerline in the vehicle body coordinate system. 4. The method according to claim 2 or 3, characterized in that, step S2 adopts Kalman filter to correct the vehicle heading, comprising the following steps: Step S201: initialize the Kalman filter; Step S202: Predict the predicted value of the vehicle heading in the geodetic coordinate system in this period according to the current turning radius and speed of the vehicle: Among them, θ _veh-pred is the predicted value of the current vehicle heading in the geodetic coordinate system, θ _{veh-correct-old} is the corrected value of the vehicle heading in the previous cycle in the geodetic coordinate system, v is the current speed of the vehicle, and R is the current vehicle heading The turning radius of , T is the planning period; Step S203: Calculate the difference between the original path heading and the lane centerline heading in the vehicle body coordinate system: θ _error = θ _road - θ _Lane Among them, θ _error is the difference between the original path heading and the lane centerline heading in the vehicle body coordinate system, θ _road is the original path heading in the vehicle body coordinate system, and θ _Lane is the lane center line heading; Step S204: Calculate the heading measurement value of the vehicle in the geodetic coordinate system: θ _veh-measure = θ _veh-gps + θ _error , Among them, θ _veh-measure is the heading measurement value of the vehicle in the geodetic coordinate system, and θ _veh-gps is the GPS heading value of the vehicle in the geodetic coordinate system; Step S205: using θ _veh-pred in step S202 and θ _veh-measure in step S204 as the input of the Kalman filter to obtain the heading correction value θ _veh-correct of the vehicle in the geodetic coordinate system; Step S206: Assign θ _veh-correct to θ _{veh-correct-old} and enter the next cycle. 5. The method according to claim 1, wherein the step S3 comprises the following steps: Step S301: Calculate the deviation between the vehicle heading correction value and the vehicle GPS heading value in the geodetic coordinate system: Δθ = _{θveh -correct- θveh-gps} ; Among them, θ _veh-correct is the heading correction value of the vehicle in the geodetic coordinate system, and θ _veh-gps is the GPS heading value of the vehicle in the geodetic coordinate system; Step S302: Calculate the corrected waypoint using the result in step S301: Among them, [x _correct y _correct θ _correct ] ^T is the coordinate of the corrected path in the vehicle body coordinate system, [x _veh-road y _veh-road θ _veh-road ] ^T is the original path before correction in the vehicle body coordinate system the coordinates below. 6. The method according to claim 1, wherein the step S4 comprises the following steps: Step S401 : count the number of grids OGN _i occupied in each column in the lidar grid map; Step S402: According to the number OGN _i of the grids occupied by each column, calculate the statistical sign amount OGS _i of each column of grids: where i is the column number in the grid map, OGS _i represents the statistical flag amount of the i-th grid in the grid map, and O _t represents the occupancy flag threshold of the grid map; Step S403: Generate a convolution kernel according to the road model width RW and the road model width deviation RWT: where K _j is the convolution kernel corresponding to the grid in the jth column, RW is the width of the road model, RWT is the width deviation of the road model, G _s is the column number of the grid selected as the pulse, and the value range of G _s is [ 0, RWT]; Step S404: Convolve the result OGS _i in step S402 with the result K _j in step S403 Among them, MW is the number of grid columns in the grid map. During the convolution process, if re=1, then the matching value hit of the corresponding grid column is increased by 1; if re=-1, then the corresponding grid column is not matched. The value of miss is incremented by 1; if re=0, then the unknown value uknow of the corresponding grid column is incremented by 1; Step S405: According to the result in step S404, obtain the probability GC _i that a certain column of grids is the center of the road: where GC _i represents the probability that the i-th grid is the center of the road; Step S406: Find the maximum value of GC _i , and the corresponding i is the column number of the grid where the road center is located; C _r = i where C _r is the grid column number where the road center is located; Step S407: Calculate the grid column numbers where the left and right road edges are located; Among them, L _r and R _r are the column numbers of the grid where the left and right road edges are located; Step S408: Calculate the relative lateral positional relationship between the vehicle and the road where D _l and D _r are the distances from the vehicle to the left and right curbs, respectively. 7 . The method according to claim 6 , wherein in the step S401 , starting from a row in the middle of the grid map, that is, the row where the vehicle is located, along a direction perpendicular to the corrected path. 8 . Statistics are started on both sides of the grid map. 8. The method according to claim 6 or 7, wherein the step S5 comprises the following steps: Step S501: Calculate the total number of lanes Cz; Step S502: locate the lane C where the vehicle is located; The serial number of the lane where the vehicle is located: Among them, C represents the left-numbered serial number of the lane where the vehicle is located, and LW is the lane width; Step S503: Calculate the distance D _c from the vehicle to the center line of the lane where it is located according to the positions of the center line of the lane (x _c1 , y _c1 ) and (x _c2 , y _c2 ); Step S504: Calculate the distance D _cl from the vehicle to the center line of the left lane and the distance D _cr from the vehicle to the center line of the right lane according to the distance D _c from the vehicle to the center line of the lane where it is located and the lane width LW: D _cl =LW-D _{c Dcr} =LW+ _Dc Step S505: Generate virtual constraints according to the distances between the vehicle and the center line of each lane, where the virtual constraints include boundary constraints of the lane where it is located and boundary constraints of adjacent lanes. 9. The method according to claim 8, wherein when there are lanes on the left and right sides of the lane, the virtual constraints are the left constraint _{Vll of the left lane, the left constraint Vcl of} the lane, and the right side of the lane Constraint V _cr and the right constraint V _rr of the right lane; when there is only a lane on the left side, the virtual constraints are the left constraint V _ll of the left lane, the left constraint V _cl of the current lane, and the right constraint V _cr of the current lane; when When there is only a lane on the right side, the virtual constraints are the left constraint V _cl of the current lane, the right constraint V _cr of the current lane, and the right constraint V _rr of the right lane. 10. The method according to claim 1, wherein the step S6 comprises the following steps: Step S601: determine whether the decision-making command changes lanes, if so, go to step S603; if not, go to step S602; Step S602: Translate the corrected path to the centerline of the lane where it is located as a desired path for tracking, and go to step S604; Step S603: Translate the corrected path to the centerline of the target lane as a desired path for tracking, and go to step S604; Step S604: Perform path tracking according to the desired path.