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

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 waypoints_i,y_i) I belongs to N, a traffic sign, and the vehicle state information comprises the coordinates (x) of the unmanned vehicle_c,y_c) Velocity V_cAcceleration a_cDirection of orientation theta_cDirection change rate θ_c’；

Step S102, calculating a global positioning point;

calculating the coordinates (x) of the host vehicle_c,y_c) To each global waypoint coordinate (x)_i,y_i) 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 A6_cE { A4, A5, A6}, the obstacle may be in the form of a_cAny region outside the region, denoted by symbol A_o∈{A}andA_o≠A_cA 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 V_o2Acceleration is denoted as a_o2. If V_o2Greater than the speed V of the experimental vehicle_cOr simulating the velocity V after two seconds_o2+2×a_o2Greater than V_cIf 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 satisfied_o1Less than V_cAnd V is_o1+2×a_o1Less than V_cIf so, selecting following and keeping the current local path;

④ if region A1 is not occupied, or condition V is satisfied_o1Greater than V_cOr V_o1+2×a_o1Greater than V_cThen 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 determined_o7×2+2×a_o7Whether 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:

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,

&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

first, the vehicle own state S (x) is acquired₀,y₀,θ₀,δ₀,v₀) Substituting the starting condition of the spline function into a formula (#) to obtain a formula system:

wherein theta is₀’＝tan(δ₀)/L；

Then, the local route end point information is written as (x)_se,y_se,θ_se) Substituting the formula (#) as an end point condition to obtain an equation set:

writing the system of equations after arrangement into an AB ═ C matrix equation form:

wherein

A＝[a b c]

C＝[sin(θ₀)θ₀'se²/2-cos(θ₀)se-x₀+x_sesin(θ₀)θ₀'se-cos(θ₀)+cos(θ_se)sin(θ₀)θ₀']

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:

if the candidate path meets the obstacle, the cost is infinite and does not participate in the formula calculation, wherein (x)_c,y_c) As the vehicle's own coordinates, (x)_i,y_i) 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 read_cFront vehicle speed V_oAnd the distance d between the two vehicles, and then the target vehicle speed V is calculated using the following formula_obj：

Where t is min {1, α V ═ V_cThe 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 waypoints_i,y_i) I belongs to N and contains traffic sign information; the state information of the vehicle comprises unmanned automobileVehicle coordinates (x)_c,y_c) Velocity V_cAcceleration a_cDirection change rate θ_c', toward theta_c。

Step S102, calculating a global positioning point;

specifically, the coordinates (x) of the host vehicle are calculated_c,y_c) To each global waypoint coordinate (x)_i,y_i) 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 A_cE { A4, A5, A6}, the obstacle may be in the form of a_cAny region outside the region, denoted by symbol A_o∈{A}andA_o≠A_cAnd 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 V_o2Acceleration is denoted as a_o2. If V_o2Greater than the speed V of the experimental vehicle_cOr simulating the velocity V after two seconds_o2+2×a_o2Greater than V_cIf 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 satisfied_o1Less than V_cAnd V is_o1+2×a_o1Less than V_cIf so, selecting following and keeping the current local path;

④ if region A1 is not occupied, or condition V is satisfied_o1Greater than V_cOr V_o1+2×a_o1Greater than V_cThen 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 determined_o7×2+2×a_o7Whether greater than dis + V_cAnd 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:

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,

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:

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

first, the vehicle own state S (x) is acquired₀,y₀,θ₀,δ₀,v₀) Substituting the starting condition of the spline function into a formula (#) to obtain a formula system:

wherein theta is₀’＝tan(δ₀)/L。

Then, the local route end point information is written as (x)_se,y_se,θ_se) Substituting the formula (#) as an end point condition to obtain an equation set:

writing the system of equations after arrangement into an AB ═ C matrix equation form:

wherein

A＝[a b c]

C＝[sin(θ₀)θ₀'se²/2-cos(θ₀)se-x₀+x_sesin(θ₀)θ₀'se-cos(θ₀)+cos(θ_se)sin(θ₀)θ₀']

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:

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,y_c) As the vehicle's own coordinates, (x)_i,y_i) 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 first_cFront vehicle speed V_oAnd the distance d between the two cars. The target vehicle speed V is then calculated using the following formula_obj：

The vehicle target speed is calculated using the third order bessel formula, where t ═ min {1, α V_cAnd/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 A6_cE { A4, A5, A6}, the obstacle may be in the form of a_cAny region outside the region, denoted by symbol A_o∈{A}and A_o≠A_cA 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 V_o2Acceleration is denoted as a_o2If V is_o2Greater than the speed V of the experimental vehicle_cOr simulating the velocity V after two seconds_o2+2×a_o2Greater than V_cIf 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 satisfied_o1Less than V_cAnd V is_o1+2×a_o1Less than V_cIf so, selecting following and keeping the current local path;

④ if region A1 is not occupied, or condition V is satisfied_o1Greater than V_cOr V_o1+2×a_o1Greater than V_cThen 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 determined_o7×2+2×a_o7Whether 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, 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:

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,

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

first, obtainTaking the vehicle self state S (x)₀,y₀,θ₀,δ₀,v₀) Substituting the starting condition of the spline function into a formula (#) to obtain a formula system:

wherein theta is₀’＝tan(δ₀)/L；

Then, the local route end point information is written as (x)_se,y_se,θ_se) Substituting the formula (#) as an end point condition to obtain an equation set:

writing the system of equations after arrangement into an AB ═ C matrix equation form:

wherein

A＝[a b c]

C＝[sin(θ₀)θ₀'se²/2-cos(θ₀)se-x₀+x_sesin(θ₀)θ₀'se-cos(θ₀)+cos(θ_se) sin(θ₀)θ₀']

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:

if the candidate path meets the obstacle, the cost is infinite and does not participate in the formula calculation, wherein (x)_c,y_c) As the vehicle's own coordinates, (x)_i,y_i) 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 waypoints_i,y_i) I belongs to N, a traffic sign, and the vehicle state information comprises the coordinates (x) of the unmanned vehicle_c,y_c) Velocity V_cAcceleration a_cDirection of orientation theta_cDirection change rate θ_c’；

Step S102, calculating a global positioning point;

calculating the coordinates (x) of the host vehicle_c,y_c) To each global waypoint coordinate (x)_i,y_i) 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 read_cFront vehicle speed V_oAnd the distance d between the two vehicles, and then the target vehicle speed V is calculated using the following formula_obj：

Where t is min {1, α V ═ V_cThe 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.

Images