CN108519773B - Path planning method for unmanned vehicle in structured environment - Google Patents

Path planning method for unmanned vehicle in structured environment Download PDF

Info

Publication number
CN108519773B
CN108519773B CN201810187540.3A CN201810187540A CN108519773B CN 108519773 B CN108519773 B CN 108519773B CN 201810187540 A CN201810187540 A CN 201810187540A CN 108519773 B CN108519773 B CN 108519773B
Authority
CN
China
Prior art keywords
vehicle
path
planning
information
lane
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
Application number
CN201810187540.3A
Other languages
Chinese (zh)
Other versions
CN108519773A (en
Inventor
孙宏滨
吴金强
马荣波
王潇
辛景民
郑南宁
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Wufang Smart Car Technology Co ltd
Original Assignee
Xian Jiaotong University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Xian Jiaotong University filed Critical Xian Jiaotong University
Priority to CN201810187540.3A priority Critical patent/CN108519773B/en
Publication of CN108519773A publication Critical patent/CN108519773A/en
Application granted granted Critical
Publication of CN108519773B publication Critical patent/CN108519773B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Traffic Control Systems (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

A method for path planning for an unmanned vehicle in a structured environment, the method comprising the steps of: s1, positioning; s2, analyzing the road condition of the current lane; judging whether complete re-planning is needed, if so, turning to step S3; if not, go to step S4; s3, completely re-planning; and S4, updating the local planning result. The invention aims to design a planning method of an unmanned vehicle under a structured environment integrating path planning and lane changing decision, and ensure the driving safety and comfort of the unmanned vehicle.

Description

Path planning method for unmanned vehicle in structured environment
Technical Field
The invention belongs to the technical field of unmanned driving, and particularly relates to a path planning method for an unmanned vehicle in a structured environment.
Background
In recent years, along with the development of science and technology, unmanned automobiles have gradually become realistic from the concept. The unmanned automobile is an intelligent automobile, which can be called as a wheeled mobile robot, and mainly depends on an intelligent driver mainly comprising a computer system in the automobile to realize unmanned driving. The unmanned technology mainly comprises four parts of perception of environmental information, intelligent decision of driving behaviors, planning of collision-free paths and motion control of vehicles. The planning module is an important component in the unmanned technology and plays a role in starting and stopping environment perception and motion control.
Path planning is an important component of a planning module, and currently, a mainstream path generation algorithm mainly includes: a graph search based planning algorithm, a sampling based planning algorithm and a curve interpolation based planning algorithm. The planning algorithm based on graph search mainly comprises the following steps: dijkstra algorithm, A-algorithm and State lattices algorithm, wherein the Dijkstra algorithm is a typical shortest path algorithm, is expanded outwards layer by taking a starting point as a center until the Dijkstra algorithm is expanded to a terminal point, is suitable for global planning of structured and unstructured environments, but is not efficient under the condition of huge number of points, so that the Dijkstra algorithm is not suitable for scenes needing real-time planning; the algorithm A needs to set a proper heuristic function, comprehensively estimates the cost value of each expansion searching node, selects the most promising points for expansion by comparing the size of the cost value of each expansion node until a target node is found, has few expansion nodes, good robustness and quick response to environmental information, but needs to pay attention to the limitation caused by the volume of the moving body in practical application; state tables can handle multidimensional states (position, velocity, acceleration, time), are suitable for local planning and dynamic environments, but are computationally expensive. The sampling-based method is mainly RRT and its derived algorithm, which can provide a fast solution in a multidimensional system, and is suitable for global planning and local planning, but the generated track can not be continuous and is unstable. The method based on curve interpolation mainly comprises the following steps: b-spline curves, Bezier curves and polynomial curves have the advantages that the calculation cost is low, but certain generation conditions need to be met when the tracks are generated, the B-spline curves, the Bezier curves and the polynomial curves are not flexible enough to be qualified in complex traffic scenes.
Chinese patent CN105549597A discloses an unmanned vehicle dynamic path planning method based on environmental uncertainty, which considers deconstruction characteristics and kinematics characteristics of vehicles, solves coefficients of a sextic polynomial using a multidimensional state of the vehicle itself as a condition, and generates a candidate path from a starting point to an end point. The above patent does not consider the behavior of the rear vehicle during lane changing, and does not consider the traffic sign encountered by the unmanned vehicle during driving, which is likely to cause traffic accidents during automatic driving.
Disclosure of Invention
The invention aims to overcome the defects of the prior art and provides a path planning method of an unmanned vehicle in a structured environment, which can ensure the program operation efficiency and reasonably cope with more complex traffic scenes under the condition of meeting the unmanned safety.
The technical scheme of the invention specifically comprises the following steps:
step S1, positioning;
firstly, loading global map data, then reading the state information of the vehicle, and finally finding out a global path point with the shortest distance from the vehicle as a global positioning point;
step S2, analyzing the road condition of the current lane;
firstly, acquiring traffic scene information, namely obstacle information, traffic sign information and lane line information by a sensor; secondly, judging whether the acquired traffic scene information needs to be completely re-planned, if so, turning to the step S3 to be completely re-planned, otherwise, turning to the step S4 to update a local planning result;
step S3, completely re-planning;
firstly, the lane change analysis acquires the longitudinal distance, speed and acceleration of the unmanned vehicle and the front and rear vehicles, and calculates to obtain whether the unmanned vehicle can change lanes in the current traffic scene; secondly, generating 31 alternative paths by adopting a quintic spline interpolation algorithm in the template generation and combining a vehicle kinematics model, wherein a set of the 31 alternative paths is called as a template; finally, calculating the cost of each alternative path, and updating the local planning result by adopting the alternative path with the minimum cost;
step S4, updating a local planning result;
and updating the local planning result to adapt to the continuous change of the traffic scene, wherein the updating comprises two parts of speed setting, current local path updating and track expansion.
The step S1 includes the following sub-steps:
step S101: acquiring global map information and vehicle state information;
the global map data includes coordinates (x) of the waypointsi,yi) I belongs to N, a traffic sign, and the vehicle state information comprises the coordinates (x) of the unmanned vehiclec,yc) Velocity VcAcceleration acDirection of orientation thetacDirection change rate θc’；
Step S102, calculating a global positioning point;
calculating the coordinates (x) of the host vehiclec,yc) To each global waypoint coordinate (x)i,yi) Finding out the global path point with the minimum distance from the vehicle as a positioning point in a global coordinate system.
The step S2 includes the following sub-steps:
step S201, obtaining perception information;
the perception information comprises barrier information, traffic sign information and lane line information, the detected barriers are represented by rectangular boxes and comprise attributes of length, width, direction, speed and acceleration, the traffic sign information comprises speed limit boards, arrows and traffic lights, and the lane line information is used for telling that no-man vehicles have several lanes at present and are in which lane at present, and whether lane changing is allowed or not;
step S202, judging whether complete re-planning is needed;
judging whether the re-planning is completed or not, and firstly, judging whether an obstacle exists in front or not; secondly, checking whether the lane line is a solid line; finally, checking whether the traffic sign prohibits lane changing; and the complete re-planning condition is that the current lane has an obstacle smaller than the vehicle speed, the lane line is not a solid line and has no traffic sign for forbidding lane change, otherwise, the complete re-planning is not carried out uniformly, and only the current lane is kept to run.
Step S3, the complete re-planning includes the following sub-steps:
s301, channel change analysis;
firstly, the front and the back of the vehicle are divided into 9 sub-areas by taking the vehicle as the center, the numbers are A1 and A2 … A9 from left to right and from top to bottom respectively, wherein the areas where the vehicle can be positioned are A4, A5 and A6, and the symbols are A4, A5 and A6cE { A4, A5, A6}, the obstacle may be in the form of acAny region outside the region, denoted by symbol Ao∈{A}andAo≠AcA is a complete set including all subregions;
lane change to the left analysis the test car was in area a 5;
①, obtaining the latest vehicle information in A2 area from the sensing module, and recording the speed as Vo2Acceleration is denoted as ao2. If Vo2Greater than the speed V of the experimental vehiclecOr simulating the velocity V after two secondso2+2×ao2Greater than VcIf so, the current lane is considered to be capable of normally driving, and the following is selected to keep the current local path;
②, if the judgment result in the step ① is negative, judging whether the A4 area is occupied, if so, selecting to follow and keeping the current local path;
③ if the determination in step ② is NO, then a determination is made as to whether the A1 region is occupied, and if so, the condition V is satisfiedo1Less than VcAnd V iso1+2×ao1Less than VcIf so, selecting following and keeping the current local path;
④ if region A1 is not occupied, or condition V is satisfiedo1Greater than VcOr Vo1+2×ao1Greater than VcThen determine if the a7 area is occupied;
⑤, if the A7 region is not occupied in step ④, selecting the least costly candidate path;
⑥ if the A7 region is occupied in step ④, then V is determinedo7×2+2×ao7Whether greater than dis + V c2, dis is the longitudinal distance from the head of the area A7 to the tail of the test vehicle, 2 is simulation time, if yes, following is selected, the current local path is kept, and if not, the candidate path with the minimum cost is selected;
step S302, generating a template;
the following formula is satisfied:
Figure GDA0001655497100000041
establishing a vehicle state model S ═ x, y, theta, delta, v according to a simple bicycle kinematics model]TWherein S is the motion state of the vehicle, x and y are the transverse coordinate and the longitudinal coordinate of the center of the rear axle of the vehicle relative to the origin of the global coordinate, theta is the orientation angle of the vehicle, delta is the rotation angle of the front wheel of the vehicle, v is the current speed of the vehicle, L is the distance from the center of the front wheel of the bicycle to the center of the rear wheel,
Figure GDA0001655497100000055
Figure GDA0001655497100000056
&first derivatives of x, y, and theta with respect to time, respectively;
generating a candidate path by adopting a quintic spline interpolation method, wherein the x coordinate of a point on the candidate path should satisfy the following formula:
a. b, c, d, e and f are coefficients of a quintic spline, s is the distance traveled by the vehicle, where x is derived not from time t but from distance traveled by the vehicle s, and the formula is derived as follows:
dx/dt＝vcosθ
ds/dt＝v
Figure GDA0001655497100000052
first, the vehicle own state S (x) is acquired0,y0,θ0,δ0,v0) Substituting the starting condition of the spline function into a formula (#) to obtain a formula system:
Figure GDA0001655497100000053
wherein theta is0’＝tan(δ0)/L；
Then, the local route end point information is written as (x)se,yse,θse) Substituting the formula (#) as an end point condition to obtain an equation set:
Figure GDA0001655497100000054
writing the system of equations after arrangement into an AB ═ C matrix equation form:
wherein
A＝[a b c]
Figure GDA0001655497100000061
C＝[sin(θ0)θ0'se2/2-cos(θ0)se-x0+xsesin(θ0)θ0'se-cos(θ0)+cos(θse)sin(θ0)θ0']
By matrix inversion operation A ═ CB-1Solving the value of the matrix A, thus solving all coefficients a, b, c, d, e and f of a quintic spline, and finally calculating the x coordinates of all candidate path points according to the quintic spline function;
calculating the y coordinate of the candidate path point by the same method;
generating a candidate path from the experimental vehicle to a local path end point according to the x coordinate and the y coordinate of each point of the candidate path, then generating other candidate paths by taking other end points of the end point set as end point conditions respectively, and finally generating a candidate path set, namely a template;
step S303, selecting an alternative path;
calculating the cost of the left 15 candidate paths according to the following formula, and finding out the candidate path with the minimum cost:
Figure GDA0001655497100000062
if the candidate path meets the obstacle, the cost is infinite and does not participate in the formula calculation, wherein (x)c,yc) As the vehicle's own coordinates, (x)i,yi) And I is the candidate path end point coordinate, the value range of I is 1 to 15, and I represents the candidate path serial number.
The step S4 includes the following sub-steps:
step S401, speed setting;
firstly, the self vehicle speed V is readcFront vehicle speed VoAnd the distance d between the two vehicles, and then the target vehicle speed V is calculated using the following formulaobj：
Figure GDA0001655497100000071
Where t is min {1, α V ═ VcThe parameter k and the parameter alpha are properly adjusted according to the performance of the real vehicle and the performance of the control module;
s402, updating the current local path and the expansion track;
rule of current local path update:
a. the unmanned vehicle drives to the end point of the current local path;
b. obstacles exist on the expansion path or the current local path;
c. the extended path is a global path point from a target point last time to the target point this time, and because the planning program continuously positions a new target point on the global path, the extended path is updated all the time.
The local planning result is updated by the method so as to adapt to the continuous change of the traffic scene, and the real-time adjustment is beneficial to ensuring the safety and the comfort. The method comprises two parts of speed setting, current local path updating and track expansion.
Drawings
FIG. 1 is a schematic overall framework of the present invention;
FIG. 2 is a schematic diagram showing a relationship between a current local path and an extended path between a test vehicle and a vehicle ahead;
FIG. 3 is a schematic view of a traffic scene prior to lane change;
FIG. 4 is a schematic view of a lane change analysis process;
FIG. 5 is a schematic view of a traffic scene after lane change;
FIG. 6 is a simple bicycle kinematics model;
Detailed Description
The present invention is described in further detail below with reference to the attached drawings.
As shown in fig. 1, a method for planning a path of an unmanned vehicle in a structured environment includes the following steps:
and step S1, positioning. Specifically, as shown in the first dashed box of fig. 1, the method includes the following sub-steps:
s101, acquiring global map information and vehicle state information;
specifically, the global map information includes coordinates (x) of the waypointsi,yi) I belongs to N and contains traffic sign information; the state information of the vehicle comprises unmanned automobileVehicle coordinates (x)c,yc) Velocity VcAcceleration acDirection change rate θc', toward thetac。
Step S102, calculating a global positioning point;
specifically, the coordinates (x) of the host vehicle are calculatedc,yc) To each global waypoint coordinate (x)i,yi) Finding out the global path point with the minimum distance from the vehicle as a global positioning point.
And step S2, analyzing the road condition of the current lane. Specifically, as shown in the second dashed box of fig. 1, the method includes the following sub-steps:
step S201, obtaining perception information;
specifically, the perception information includes obstacle information, traffic sign information, and lane line information. The detected obstacles are represented by a rectangular box and comprise the attributes of length, width, orientation, speed and acceleration, and the traffic sign information mainly comprises a speed limit board, an arrow, traffic lights and lane line information, wherein the lane line information is used for telling the current lanes of the unmanned vehicle, and whether lane change is allowed or not.
Step S202, judging whether complete re-planning is needed;
three judgments are needed to judge whether the re-planning is completed. Firstly, judging whether an obstacle exists in front or not; secondly, checking whether the lane line is a solid line; finally, whether the traffic sign prohibits lane changing is checked.
Specifically, whether a front obstacle exists or not is judged, as shown in fig. 2, the test vehicle has traveled a distance along the current local path, and the extended path also extends a distance forward, and the extended path is a global path point from the target point last time to the target point this time. In fig. 2, the front vehicle is in the same lane as the test vehicle on the extended path, and if the speed of the front vehicle is less than the speed of the test vehicle, it is indicated that the current lane cannot run smoothly. And then checking lane line information, judging whether the lane line information is a solid line, if the lane line information is the solid line, not needing complete re-planning, otherwise, checking whether the lane change is forbidden by the traffic sign, and if the lane change is forbidden by no traffic sign, performing step S3 complete re-planning. In summary, the conditions for a complete re-planning are that there are obstacles in the current lane that are less than the vehicle speed, and that the lane lines are not solid and there are no traffic signs prohibiting lane changes. Otherwise, complete re-planning is not performed uniformly, and only the current lane is kept running.
And step S3, finishing the re-planning. Specifically, as shown in the third dashed box of fig. 1, the method includes the following sub-steps:
s301, channel change analysis;
specifically, as shown in fig. 3, first, the front and the rear of the vehicle are divided into 9 sub-regions with numbers from left to right and a1 and a2 … a9 from top to bottom, respectively, with the vehicle itself as the center. The areas in which the vehicle itself may be located are A4, A5 and A6, the symbols being AcE { A4, A5, A6}, the obstacle may be in the form of acAny region outside the region, denoted by symbol Ao∈{A}andAo≠AcAnd A is a complete set including all sub-regions.
The lane change to the left flow is shown in fig. 4, and the experimental vehicle is in the area a 5.
①, obtaining the latest vehicle information in A2 area from the sensing module, and recording the speed as Vo2Acceleration is denoted as ao2. If Vo2Greater than the speed V of the experimental vehiclecOr simulating the velocity V after two secondso2+2×ao2Greater than VcIf so, the current lane is considered to be capable of normally driving, and the following is selected to keep the current local path;
②, if the judgment result in the step ① is negative, judging whether the A4 area is occupied, if so, selecting to follow and keeping the current local path;
③ if the determination in step ② is NO, then a determination is made as to whether the A1 region is occupied, and if so, the condition V is satisfiedo1Less than VcAnd V iso1+2×ao1Less than VcIf so, selecting following and keeping the current local path;
④ if region A1 is not occupied, or condition V is satisfiedo1Greater than VcOr Vo1+2×ao1Greater than VcThen determine if the a7 area is occupied;
⑤, if the A7 region is not occupied in step ④, selecting the least costly candidate path;
⑥ if the A7 region is occupied in step ④, then V is determinedo7×2+2×ao7Whether greater than dis + VcAnd x 2(dis is the longitudinal distance from the head of the A7 area to the tail of the test vehicle, and 2 is simulation time), if so, selecting to follow, keeping the current local path, and otherwise, selecting to switch the lane to the left. Fig. 3 is a schematic diagram before lane changing, and fig. 5 is a schematic diagram after lane changing.
Step S302, generating a template;
FIG. 6 is a simple bicycle kinematics model, satisfying the following formula:
Figure GDA0001655497100000101
establishing a vehicle state model S ═ x, y, theta, delta, v according to a simple bicycle kinematics model]TWherein S is the motion state of the vehicle, (x, y) are the transverse coordinate and the longitudinal coordinate of the center of the rear axle of the vehicle relative to the origin of the global coordinate, theta is the orientation angle of the vehicle, delta is the rotation angle of the front wheel of the vehicle, v is the current speed of the vehicle, and L is the distance from the center of the front wheel of the bicycle to the center of the rear wheel,
Figure GDA0001655497100000102
the first derivatives of x, y, and theta with respect to time, respectively.
The invention adopts a quintic spline interpolation method to generate candidate paths, and the calculation of x-axis coordinates is explained in detail here. The x-coordinate of a point on the candidate path should satisfy the following formula:
Figure GDA0001655497100000103
a. b, c, d, e and f are the coefficients of a quintic spline, s is the distance traveled by the vehicle, where x is derived not from time t, but from distance traveled by the vehicle s, somewhat differently from a simple bicycle kinematics model, the formula is derived as follows:
dx/dt＝vcosθ
ds/dt＝v
Figure GDA0001655497100000104
first, the vehicle own state S (x) is acquired0,y0,θ0,δ0,v0) Substituting the starting condition of the spline function into a formula (#) to obtain a formula system:
Figure GDA0001655497100000111
wherein theta is0’＝tan(δ0)/L。
Then, the local route end point information is written as (x)se,yse,θse) Substituting the formula (#) as an end point condition to obtain an equation set:
Figure GDA0001655497100000112
writing the system of equations after arrangement into an AB ═ C matrix equation form:
wherein
A＝[a b c]
Figure GDA0001655497100000113
C＝[sin(θ0)θ0'se2/2-cos(θ0)se-x0+xsesin(θ0)θ0'se-cos(θ0)+cos(θse)sin(θ0)θ0']
Next, a ═ CB is calculated by matrix inversion-1The matrix A is evaluated, and all the coefficients a, b, c, d, e, and f are evaluated as quintic splines. Finally, the x coordinates of all candidate path points are calculated according to a quintic spline function. And similarly, calculating the y coordinate of the candidate path point.
The x coordinate and the y coordinate of each point of the candidate path are obtained through the method, and a candidate path from the test vehicle to the local path end point is generated. Then, other end points of the end point set are respectively used as end point conditions to generate other candidate paths, and finally, a generated candidate path set, namely a template, is shown in fig. 3.
Step S303, selecting an alternative path;
calculating the cost of the left 15 candidate paths according to the following formula, and finding out the candidate path with the minimum cost:
Figure GDA0001655497100000114
if the candidate path meets the obstacle, the cost is infinite, and the candidate path does not participate in the formula calculation. Wherein (x)c,yc) As the vehicle's own coordinates, (x)i,yi) And I is the candidate path end point coordinate, the value range of I is 1 to 15, and I represents the candidate path serial number.
And step S4, updating the local planning result. Specifically, as shown in the fourth dashed box of fig. 1, the method includes the following sub-steps:
step S401, speed setting;
specifically, the vehicle speed V is read firstcFront vehicle speed VoAnd the distance d between the two cars. The target vehicle speed V is then calculated using the following formulaobj：
The vehicle target speed is calculated using the third order bessel formula, where t ═ min {1, α VcAnd/d, because t epsilon (0,1), adding a condition limit for taking the minimum value, and properly adjusting the parameters k and alpha according to the performance of the real vehicle and the performance of the control module. The design has the advantages that when the front vehicle speed is slower than the self vehicle speed, the initial braking acceleration of the unmanned vehicle is larger, and the braking acceleration is gradually reduced along with the reduction of the vehicle speed and the shortening of the distance, so that the vehicle speed can be quickly reduced to reach a safe speed, and then the unmanned vehicle slowly slides to a certain safe distance away from an obstacleThe device stops when the device leaves, and is safe and comfortable.
S402, updating the current local path and the expansion track;
specifically, the current local path update rule is as follows:
a. the unmanned vehicle drives to the end point of the current local path;
b. obstacles exist on the expansion path or the current local path;
c. the extended path is a global path point from a target point last time to the target point this time, and because the planning program continuously positions a new target point on the global path, the extended path is updated all the time.
In summary, the present invention provides a method for planning a route of an unmanned vehicle in a structured environment, which mainly includes an algorithm for generating a route, a speed planning, and a selection of a trajectory. The planning method has the advantages of reliable feasibility and good expansibility, and can ensure the safety and the comfort in the driving process of the unmanned automobile.
It will be apparent to a person skilled in the art of unmanned driving that automatic driving of an unmanned vehicle can be achieved according to the planning method described above or that corresponding changes and appropriate adjustments are made based thereon, and all such changes and adjustments are intended to fall within the scope of the claims.

Claims (4)

1. A method for planning the path of an unmanned vehicle in a structured environment is characterized by comprising the following steps:
step S1, positioning;
firstly, loading global map data, then reading the state information of the vehicle, and finally finding out a global path point with the shortest distance from the vehicle as a global positioning point;
step S2, analyzing the road condition of the current lane;
firstly, acquiring traffic scene information, namely obstacle information, traffic sign information and lane line information by a sensor; secondly, judging whether the acquired traffic scene information needs to be completely re-planned, if so, turning to the step S3 to be completely re-planned, otherwise, turning to the step S4 to update a local planning result;
step S3, completely re-planning;
step S301, a lane change analysis is performed to obtain the longitudinal distance, the speed and the acceleration of the unmanned vehicle and the front and rear vehicles, and whether the unmanned vehicle can change lanes in the current traffic scene is calculated, specifically:
firstly, the front and the back of the vehicle are divided into 9 sub-areas by taking the vehicle as the center, the numbers are A1 and A2 … A9 from left to right and from top to bottom respectively, wherein the areas where the vehicle can be positioned are A4, A5 and A6, and the symbols are A4, A5 and A6cE { A4, A5, A6}, the obstacle may be in the form of acAny region outside the region, denoted by symbol Ao∈{A}and Ao≠AcA is a complete set including all subregions;
lane change to the left analysis the test car was in area a 5;
①, obtaining the latest vehicle information in A2 area from the sensing module, and recording the speed as Vo2Acceleration is denoted as ao2If V iso2Greater than the speed V of the experimental vehiclecOr simulating the velocity V after two secondso2+2×ao2Greater than VcIf so, the current lane is considered to be capable of normally driving, and the following is selected to keep the current local path;
②, if the judgment result in the step ① is negative, judging whether the A4 area is occupied, if so, selecting to follow and keeping the current local path;
③ if the determination in step ② is NO, then a determination is made as to whether the A1 region is occupied, and if so, the condition V is satisfiedo1Less than VcAnd V iso1+2×ao1Less than VcIf so, selecting following and keeping the current local path;
④ if region A1 is not occupied, or condition V is satisfiedo1Greater than VcOr Vo1+2×ao1Greater than VcThen determine if the a7 area is occupied;
⑤, if the A7 region is not occupied in step ④, selecting the least costly candidate path;
⑥ if the A7 region is occupied in step ④, then V is determinedo7×2+2×ao7Whether greater than dis + Vc2, dis is the longitudinal distance from the head of the area A7 to the tail of the test vehicle, 2 is simulation time, if yes, following is selected, the current local path is kept, and if not, the candidate path with the minimum cost is selected;
step S302, template generation: adopting a quintic spline interpolation algorithm, combining a vehicle kinematic model, and generating 31 alternative paths, wherein a set of the 31 alternative paths is called as a template, and specifically:
the following formula is satisfied:
Figure FDA0002265292500000021
establishing a vehicle state model S ═ x, y, theta, delta, v according to a simple bicycle kinematics model]TWherein S is the motion state of the vehicle, x and y are the transverse coordinate and the longitudinal coordinate of the center of the rear axle of the vehicle relative to the origin of the global coordinate, theta is the orientation angle of the vehicle, delta is the rotation angle of the front wheel of the vehicle, v is the current speed of the vehicle, L is the distance from the center of the front wheel of the bicycle to the center of the rear wheel,
Figure FDA0002265292500000023
first derivatives of x, y, and theta with respect to time, respectively;
generating a candidate path by adopting a quintic spline interpolation method, wherein the x coordinate of a point on the candidate path should satisfy the following formula:
a. b, c, d, e and f are coefficients of a quintic spline, s is the distance traveled by the vehicle, where x is derived not from time t but from distance traveled by the vehicle s, and the formula is derived as follows:
dx/dt＝vcosθ
ds/dt＝v
Figure FDA0002265292500000031
first, obtainTaking the vehicle self state S (x)0,y0,θ0,δ0,v0) Substituting the starting condition of the spline function into a formula (#) to obtain a formula system:
Figure FDA0002265292500000032
wherein theta is0’＝tan(δ0)/L；
Then, the local route end point information is written as (x)se,yse,θse) Substituting the formula (#) as an end point condition to obtain an equation set:
Figure FDA0002265292500000033
writing the system of equations after arrangement into an AB ═ C matrix equation form:
wherein
A＝[a b c]
Figure FDA0002265292500000034
C＝[sin(θ0)θ0'se2/2-cos(θ0)se-x0+xsesin(θ0)θ0'se-cos(θ0)+cos(θse) sin(θ0)θ0']
By matrix inversion operation A ═ CB-1Solving the value of the matrix A, thus solving all coefficients a, b, c, d, e and f of a quintic spline, and finally calculating the x coordinates of all candidate path points according to the quintic spline function;
calculating the y coordinate of the candidate path point by the same method;
generating a candidate path from the experimental vehicle to a local path end point according to the x coordinate and the y coordinate of each point of the candidate path, then generating other candidate paths by taking other end points of the end point set as end point conditions respectively, and finally generating a candidate path set, namely a template;
step S303, alternative path selection: calculating the cost of each alternative path, and updating the local planning result by adopting the alternative path with the minimum cost, specifically:
calculating the cost of the left 15 candidate paths according to the following formula, and finding out the candidate path with the minimum cost:
Figure FDA0002265292500000041
if the candidate path meets the obstacle, the cost is infinite and does not participate in the formula calculation, wherein (x)c,yc) As the vehicle's own coordinates, (x)i,yi) The value range of I is 1 to 15 for the candidate route terminal point coordinate, and I represents the candidate route serial number;
step S4, updating a local planning result;
and updating the local planning result to adapt to the continuous change of the traffic scene, wherein the updating comprises two parts of speed setting, current local path updating and track expansion.
2. The method for planning the path of the unmanned vehicle in the structured environment according to claim 1, wherein:
the step S1 includes the following sub-steps:
step S101: acquiring global map information and vehicle state information;
the global map data includes coordinates (x) of the waypointsi,yi) I belongs to N, a traffic sign, and the vehicle state information comprises the coordinates (x) of the unmanned vehiclec,yc) Velocity VcAcceleration acDirection of orientation thetacDirection change rate θc’；
Step S102, calculating a global positioning point;
calculating the coordinates (x) of the host vehiclec,yc) To each global waypoint coordinate (x)i,yi) Finding out the global path point with the minimum distance from the vehicle as a positioning point in a global coordinate system.
3. The method for planning the path of the unmanned vehicle in the structured environment according to claim 1, wherein:
the step S2 includes the following sub-steps:
step S201, obtaining perception information;
the perception information comprises barrier information, traffic sign information and lane line information, the detected barriers are represented by rectangular boxes and comprise attributes of length, width, direction, speed and acceleration, the traffic sign information comprises speed limit boards, arrows and traffic lights, and the lane line information is used for telling that no-man vehicles have several lanes at present and are in which lane at present, and whether lane changing is allowed or not;
step S202, judging whether complete re-planning is needed;
judging whether the re-planning is completed or not, and firstly, judging whether an obstacle exists in front or not; secondly, checking whether the lane line is a solid line; finally, checking whether the traffic sign prohibits lane changing; and the complete re-planning condition is that the current lane has an obstacle smaller than the vehicle speed, the lane line is not a solid line and has no traffic sign for forbidding lane change, otherwise, the complete re-planning is not carried out uniformly, and only the current lane is kept to run.
4. The method for planning the path of the unmanned vehicle in the structured environment according to claim 1, wherein:
the step S4 includes the following sub-steps:
step S401, speed setting;
firstly, the self vehicle speed V is readcFront vehicle speed VoAnd the distance d between the two vehicles, and then the target vehicle speed V is calculated using the following formulaobj：
Figure FDA0002265292500000051
Where t is min {1, α V ═ VcThe parameter k and the parameter alpha are properly adjusted according to the performance of the real vehicle and the performance of the control module;
s402, updating the current local path and the expansion track;
rule of current local path update:
a. the unmanned vehicle drives to the end point of the current local path;
b. obstacles exist on the expansion path or the current local path;
c. the extended path is a global path point from a target point last time to the target point this time, and because the planning program continuously positions a new target point on the global path, the extended path is updated all the time.
CN201810187540.3A 2018-03-07 2018-03-07 Path planning method for unmanned vehicle in structured environment Active CN108519773B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810187540.3A CN108519773B (en) 2018-03-07 2018-03-07 Path planning method for unmanned vehicle in structured environment

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810187540.3A CN108519773B (en) 2018-03-07 2018-03-07 Path planning method for unmanned vehicle in structured environment

Publications (2)

Publication Number Publication Date
CN108519773A CN108519773A (en) 2018-09-11
CN108519773B true CN108519773B (en) 2020-01-14

Family

ID=63433546

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810187540.3A Active CN108519773B (en) 2018-03-07 2018-03-07 Path planning method for unmanned vehicle in structured environment

Country Status (1)

Country Link
CN (1) CN108519773B (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
RU2782521C1 (en) * 2020-10-28 2022-10-28 Чонгкинг Чанган Аутомобайл Ко., Лтд Method and system for planning of transverse trajectory of automatic change of car lane, car, and data carrier

Families Citing this family (37)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110940346B (en) * 2018-09-21 2021-07-13 上汽通用汽车有限公司 High-precision map processing method and device for automatic driving lane changing
CN109752017B (en) * 2018-12-29 2021-02-02 同济大学 Travel route track generation system for unmanned low-speed vehicle
CN111457931B (en) * 2019-01-21 2021-09-24 广州汽车集团股份有限公司 Method, device, system and storage medium for controlling local path re-planning of autonomous vehicle
CN109708656A (en) * 2019-01-24 2019-05-03 爱驰汽车有限公司 Route planning method, system, equipment and storage medium based on real-time road
CN109631929B (en) * 2019-02-20 2021-04-30 百度在线网络技术（北京）有限公司 Re-navigation method and device based on blacklist and storage medium
CN109814576B (en) * 2019-02-22 2022-01-28 百度在线网络技术（北京）有限公司 Method, apparatus and storage medium for speed planning of autonomous vehicles
CN109712421B (en) * 2019-02-22 2021-06-04 百度在线网络技术（北京）有限公司 Method, apparatus and storage medium for speed planning of autonomous vehicles
CN109945882B (en) * 2019-03-27 2021-11-02 上海交通大学 Unmanned vehicle path planning and control system and method
CN110008577B (en) * 2019-04-01 2020-12-11 清华大学 Vehicle automatic lane changing function evaluation method based on worst global risk degree search
CN109947113A (en) * 2019-04-10 2019-06-28 厦门大学嘉庚学院 A kind of manned automobile and pilotless automobile road surface sharing method
CN110244742B (en) * 2019-07-01 2023-06-09 阿波罗智能技术(北京)有限公司 Method, apparatus and storage medium for unmanned vehicle tour
CN110672111B (en) * 2019-09-24 2021-06-25 广州大学 Vehicle driving path planning method, device, system, medium and equipment
CN111002984A (en) * 2019-12-24 2020-04-14 北京汽车集团越野车有限公司 Automatic driving method and device, vehicle and automatic driving equipment
CN111338335B (en) * 2019-12-31 2021-02-26 清华大学 Vehicle local track planning method under structured road scene
CN113515111B (en) * 2020-03-25 2023-08-25 宇通客车股份有限公司 Vehicle obstacle avoidance path planning method and device
CN111736486A (en) * 2020-05-01 2020-10-02 东风汽车集团有限公司 Sensor simulation modeling method and device for L2 intelligent driving controller
CN111650934A (en) * 2020-05-26 2020-09-11 坤泰车辆系统（常州）有限公司 Method for planning local path of automatic driving system
CN111811517A (en) * 2020-07-15 2020-10-23 中国科学院上海微系统与信息技术研究所 Dynamic local path planning method and system
CN111966096B (en) * 2020-07-31 2024-10-01 智慧航海（青岛）科技有限公司 Automatic selection method for intelligent ship local path planning terminal
TWI737437B (en) * 2020-08-07 2021-08-21 財團法人車輛研究測試中心 Trajectory determination method
CN113906360A (en) * 2020-08-07 2022-01-07 深圳市大疆创新科技有限公司 Control method and device for movable platform and computer readable storage medium
CN112101128B (en) * 2020-08-21 2021-06-22 东南大学 Unmanned formula racing car perception planning method based on multi-sensor information fusion
CN112148002B (en) * 2020-08-31 2021-12-28 西安交通大学 Local trajectory planning method, system and device
CN112051797B (en) * 2020-09-07 2023-12-26 腾讯科技（深圳）有限公司 Foot robot motion control method, device, equipment and medium
CN112068574A (en) * 2020-10-19 2020-12-11 中国科学技术大学 Control method and system for unmanned vehicle in dynamic complex environment
CN112124314B (en) * 2020-10-28 2021-09-03 重庆长安汽车股份有限公司 Method and system for planning transverse path of vehicle for automatic lane change, vehicle and storage medium
CN112362074B (en) * 2020-10-30 2024-03-19 重庆邮电大学 Intelligent vehicle local path planning method under structured environment
CN112666833B (en) * 2020-12-25 2022-03-15 吉林大学 Vehicle speed following self-adaptive robust control method for electric automatic driving vehicle
CN112964271B (en) * 2021-03-15 2023-03-31 西安交通大学 Multi-scene-oriented automatic driving planning method and system
CN112985445B (en) * 2021-04-20 2021-08-13 速度时空信息科技股份有限公司 Lane-level precision real-time motion planning method based on high-precision map
CN113516749B (en) * 2021-09-14 2021-12-17 中国汽车技术研究中心有限公司 Method, device, equipment and medium for acquiring data of automatic driving vision sensor
CN113932823A (en) * 2021-09-23 2022-01-14 同济大学 Unmanned multi-target-point track parallel planning method based on semantic road map
CN113870316B (en) * 2021-10-19 2023-08-15 青岛德智汽车科技有限公司 Front vehicle path reconstruction method under GPS-free following scene
CN114137960A (en) * 2021-11-01 2022-03-04 天行智控科技(无锡)有限公司 Unmanned vehicle cooperation method of intelligent transportation system of closed area
CN114442633A (en) * 2022-01-28 2022-05-06 天津优控智行科技有限公司 Method for planning local path of logistics vehicle in unmanned park
CN114859917B (en) * 2022-05-10 2024-09-20 嘉兴学院 Unstructured road automatic driving path planning method, system and vehicle
CN115079702B (en) * 2022-07-18 2023-06-06 江苏集萃清联智控科技有限公司 Intelligent vehicle planning method and system under mixed road scene

Family Cites Families (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101033971B (en) * 2007-02-09 2011-02-16 中国科学院合肥物质科学研究院 Mobile robot map building system and map building method thereof
DE102010046479B4 (en) * 2010-08-31 2023-10-12 Lacos Computerservice Gmbh Method for collecting data for site-specific treatment or processing of agricultural land
US9117185B2 (en) * 2012-09-19 2015-08-25 The Boeing Company Forestry management system
US9711851B1 (en) * 2016-02-04 2017-07-18 Proxy Technologies, Inc. Unmanned vehicle, system and method for transmitting signals
CN106441319B (en) * 2016-09-23 2019-07-16 中国科学院合肥物质科学研究院 A kind of generation system and method for automatic driving vehicle lane grade navigation map
CN107085437A (en) * 2017-03-20 2017-08-22 浙江工业大学 A kind of unmanned aerial vehicle flight path planing method based on EB RRT
CN107167811B (en) * 2017-04-26 2019-05-03 西安交通大学 The road drivable region detection method merged based on monocular vision with laser radar
CN107264531B (en) * 2017-06-08 2019-07-12 中南大学 The autonomous lane-change of intelligent vehicle is overtaken other vehicles motion planning method in a kind of semi-structure environment
CN107490382A (en) * 2017-07-31 2017-12-19 中北智杰科技（北京）有限公司 A kind of pilotless automobile path planning system and control method
CN107702716B (en) * 2017-08-31 2021-04-13 广州小鹏汽车科技有限公司 Unmanned driving path planning method, system and device
CN107749079B (en) * 2017-09-25 2020-03-17 北京航空航天大学 Point cloud quality evaluation and track planning method for unmanned aerial vehicle scanning reconstruction

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
RU2782521C1 (en) * 2020-10-28 2022-10-28 Чонгкинг Чанган Аутомобайл Ко., Лтд Method and system for planning of transverse trajectory of automatic change of car lane, car, and data carrier

Also Published As

Publication number Publication date
CN108519773A (en) 2018-09-11

Similar Documents

Publication Publication Date Title
CN108519773B (en) Path planning method for unmanned vehicle in structured environment
CN109927716B (en) Autonomous vertical parking method based on high-precision map
CN110298122B (en) Unmanned vehicle urban intersection left-turn decision-making method based on conflict resolution
EP3626568B1 (en) Adjusting speeds along a path for autonomous driving vehicles
US10823575B2 (en) Reference line smoothing method using piecewise spiral curves with weighted geometry costs
US10429849B2 (en) Non-linear reference line optimization method using piecewise quintic polynomial spiral paths for operating autonomous driving vehicles
US10884422B2 (en) Method for generating trajectories for autonomous driving vehicles (ADVS)
US10800408B2 (en) Determining driving paths for autonomous driving that avoid moving obstacles
CN111089594B (en) Autonomous parking trajectory planning method suitable for multiple scenes
US11520335B2 (en) Determining driving paths for autonomous driving vehicles based on map data
CN110955236B (en) Curvature correction path sampling system for an autonomous vehicle
US10921135B2 (en) Real-time map generation scheme for autonomous vehicles based on prior driving trajectories
US10908613B2 (en) Optimal longitudinal trajectory generation under varied lateral acceleration constraints
CN110096054B (en) Method and system for generating a reference line for an autonomous vehicle using multiple threads
CN110345957A (en) Vehicle route identification
US11099017B2 (en) Determining driving paths for autonomous driving vehicles based on offset points
US11353878B2 (en) Soft-boundary based path optimization for complex scenes for autonomous driving vehicles
CN113608531B (en) Unmanned vehicle real-time global path planning method based on safety A-guidance points
CN112009487B (en) Determining speed of an autonomous vehicle
US20190325223A1 (en) Tracking objects with multiple cues
CN111896004A (en) Narrow passage vehicle track planning method and system
JP2016125824A (en) Information processing device
CN111649751A (en) Ultra-free sewing method for reference line smoothing
CN115447607A (en) Method and device for planning a vehicle driving trajectory
CN113561992A (en) Method, device, terminal device and medium for generating trajectory of automatic driving vehicle

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
TR01 Transfer of patent right

Effective date of registration: 20221221

Address after: No. 2, Yunshen Road, Changshu Hi tech Industrial Development Zone, Suzhou City, Jiangsu Province, 215506

Patentee after: WUFANG SMART CAR TECHNOLOGY Co.,Ltd.

Address before: Beilin District Xianning West Road 710049, Shaanxi city of Xi'an province No. 28

Patentee before: XI'AN JIAOTONG University

TR01 Transfer of patent right