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

A path planning method for unmanned vehicles in structured environments

technical field

The invention belongs to the technical field of unmanned driving, and in particular relates to a path planning method for unmanned vehicles in a structured environment.

Background technique

In recent years, with the development of technology, driverless cars have gradually become a reality from a concept. A driverless car is a smart car, also known as a wheeled mobile robot, which mainly relies on a computer-based intelligent driver in the car to achieve unmanned driving. Driverless technology mainly includes four parts: perception of environmental information, intelligent decision-making of driving behavior, planning of collision-free paths and vehicle motion control. Among them, the planning module is a very important part of unmanned driving technology, which plays a linking role for environmental perception and motion control.

Path planning is an important part of the planning module. At present, the mainstream path generation algorithms mainly include: the planning algorithm based on graph search, the planning algorithm based on sampling and the planning algorithm based on curve interpolation. The planning algorithms based on graph search mainly include: Dijkstra algorithm, A* algorithm and State lattices algorithm, among which Dijkstra algorithm is a typical shortest path algorithm, which expands from the starting point to the outer layer until it reaches the end point. , which is suitable for global planning in structured and unstructured environments, but is inefficient when the number of points is huge, so it is not suitable for scenarios that require real-time planning; the A* algorithm needs to set a suitable heuristic function to fully estimate The cost value of each extended search node, by comparing the cost value of each extended node, select the most promising point to expand until the target node is found, the A* algorithm has few extended nodes, good robustness, and fast response to environmental information. , but in practical applications, attention should be paid to the limitations of the moving body's own volume; State lattices can handle multi-dimensional states (position, velocity, acceleration, time), and are suitable for local planning and dynamic environments, but the computational cost is too high. Sampling-based methods are mainly RRT and its derivatives, which can provide a fast solution in multi-dimensional systems and are suitable for global planning and local planning, but the trajectory it generates cannot be sustained, so it is unstable. The methods based on curve interpolation mainly include: B-spline curve, Bezier curve, and polynomial curve. Their advantages are low computational cost, but certain generation conditions need to be met when generating trajectories, which is not flexible enough to handle complex traffic scenarios. .

Chinese patent CN105549597A discloses a dynamic path planning method for unmanned vehicles based on environmental uncertainty, which considers the deconstruction characteristics and kinematic characteristics of the vehicle, and uses the multi-dimensional state of the vehicle itself as a condition to solve the coefficient of the sixth degree polynomial to generate A candidate path from the start point to the end point. The above-mentioned patent does not consider the behavior of the vehicles behind when changing lanes, nor does it consider the traffic signs encountered by the unmanned vehicle during driving, which is likely to cause traffic accidents during the automatic driving process.

SUMMARY OF THE INVENTION

The purpose of the present invention is to overcome the above-mentioned shortcomings of the prior art, and proposes a path planning method for unmanned vehicles in a structured environment, which can ensure the efficiency of program operation under the condition of satisfying unmanned safety conditions, and can be reasonably to deal with more complex traffic scenarios.

The technical solution of the present invention specifically includes the following steps:

Step S1, positioning;

First load the global map data, then read the vehicle status information, and finally find the global path point with the shortest distance from the vehicle as the global positioning point;

Step S2, analysis of current vehicle road conditions;

First, the traffic scene information, that is, obstacle information, traffic sign information, and lane line information, is acquired by the sensor; second, it is judged whether the acquired traffic scene information needs complete re-planning, and if so, go to step S3 for complete re-planning, otherwise , go to step S4, update the local planning result;

Step S3, complete re-planning;

First, the lane change analysis obtains the longitudinal distance, speed and acceleration between the unmanned vehicle and the front and rear vehicles, and calculates whether the unmanned vehicle can change lanes in the current traffic scene; The vehicle kinematics model generates 31 alternative paths, and the set of these 31 alternative paths is called a template; finally, the cost of each alternative path is calculated, and the local planning result is updated with the alternative path with the least cost;

Step S4, updating the local planning result;

Update the local planning results to adapt to the constant changes of the traffic scene, including speed setting and updating the current local path and expanding the trajectory.

The step S1 includes the following sub-steps:

Step S101: obtain global map information and vehicle status information;

The global map data includes the coordinates (x _i , y _i ) of the waypoints, i∈N, the traffic signs, and the vehicle state information includes the coordinates (x _c , y _c ) of the driverless car, the speed V _c , the acceleration a _c , towards θ _c , towards the rate of change θ _c ';

Step S102, calculating the global positioning point;

Calculate the distance from the coordinates (x _c , y _c ) of the vehicle to the coordinates (x _i , y _i ) of each global path point, and find the global path point with the smallest distance from the vehicle as the positioning point in the global coordinate system.

The step S2 includes the following sub-steps:

Step S201, acquiring perception information;

The perception information includes obstacle information, traffic sign information and lane line information. The detected obstacles are represented by a rectangular box, including length, width, orientation, speed, and acceleration attributes. The traffic sign information includes speed limit signs, arrows, traffic lights, and lane lines. The information is used to tell how many lanes the unmanned vehicle currently has, which lane it is currently in, and whether it is allowed to change lanes;

Step S202, judging whether complete re-planning is required;

To judge whether the replanning is complete, first, determine whether there are obstacles ahead; secondly, check whether the lane line is a solid line; finally, check whether the traffic signs prohibit changing lanes; the condition for complete replanning is that there are obstacles in the current lane that are less than the speed of the vehicle, And the lane line is not a solid line and there is no traffic sign prohibiting changing lanes, otherwise, the complete re-planning will not be carried out, and only the current lane will be maintained.

Step S3, the complete re-planning includes the following sub-steps:

Step S301, lane change analysis;

First, take the car itself as the center, divide the front and rear of the car into 9 sub-areas, numbered from left to right, and from top to bottom are A1, A2...A9, of which the car itself may be in the areas A4, A5 and A6, the symbols Denoted as A _c ∈{A4,A5,A6}, the obstacle may be in any area except the A _c area, the symbol is expressed as A _o ∈{A}and A _o ≠A _c , A is the complete set, including all sub-areas;

The analysis of changing lanes to the left is as follows, the experimental vehicle is in area A5;

①. Obtain the latest vehicle information in the A2 area from the perception module, the speed is recorded as V _o2 , and the acceleration is recorded as a _o2 . If V _o2 is greater than the experimental vehicle speed V _c , or the speed V _o2 +2×a _o2 after the simulation for two seconds is greater than V _c , it is considered that the current lane can be driven normally, choose to follow, and keep the current local path;

②. If the judgment result in step ① is no, then judge whether the A4 area is occupied, if so, choose to follow and keep the current local path;

③. If the judgment result in step ② is no, judge whether the A1 area is occupied. If yes, and the condition V _o1 is less than V _c and V _o1 +2×a _o1 is less than V _c , choose to follow and keep the current local path ;

④. If the area A1 is not occupied, or the condition V _o1 is greater than V _c or V _o1 +2×a _o1 is greater than V _c , then judge whether the area A7 is occupied;

⑤. If the A7 area is not occupied in step ④, select the candidate path with the least cost;

⑥. If the A7 area is occupied in step ④, judge whether V _o7 ×2+2×a _o7 is greater than dis+V _c ×2, dis is the longitudinal distance from the front of the A7 area to the rear of the experimental vehicle, and 2 is the simulation time. , then choose to follow, keep the current local path, otherwise, choose the candidate path with the least cost;

Step S302, template generation;

The following formulas are satisfied:

&分别为x、y、θ对时间的一阶导数；The vehicle state model S=[x,y,θ,δ,v] ^T is established according to the simple bicycle kinematics model, where S is the vehicle motion state, and x, y are the transverse and longitudinal coordinates of the center of the rear axle relative to the global coordinate origin. Coordinates, θ is the vehicle heading angle, δ is the front wheel rotation angle, v is the current speed of the vehicle, L is the distance from the center of the front wheel to the center of the rear wheel,

& are the first derivatives of x, y, and θ with respect to time, respectively;

The candidate path is generated by the quintic spline interpolation method, and the x-coordinate of the point on the candidate path should satisfy the following formula:

a, b, c, d, e, and f are the coefficients of the quintic spline, and s is the distance the car moves. Here, x is not derived from the time t, but the derivation of the car movement distance s. The formula is derived as follows:

dx/dt=vcosθ

ds/dt=v

First, obtain the vehicle's own state S(x ₀ , y ₀ , θ ₀ , δ ₀ , v ₀ ) as the initial condition of the spline function, and substitute it into the formula (*) to obtain the equation system:

where θ ₀ '=tan(δ ₀ )/L;

Then, denote the end point information of the local path as (x _se , y _se , θ _se ) as the end point condition, and substitute it into the formula (*) to obtain the equation system:

After arranging the equation system, it is written in the form of AB=C matrix equation:

in

A=[a b c]

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

The value of matrix A is obtained by the matrix inversion operation A=CB ^-1 , so all the coefficients a, b, c, d, e and f of the quintic spline are obtained. Finally, all candidate paths are calculated according to the quintic spline function the x-coordinate of the point;

In the same way, calculate the y-coordinate of the candidate waypoint;

According to the x-coordinate and y-coordinate of each point of the candidate path, generate a candidate path from the experimental vehicle itself to the end point of the local path, and then use the other end points of the end point set as end point conditions to generate other candidate paths, and finally generate the candidate path Collection, both templates;

Step S303, alternative path selection;

Calculate the cost of the 15 candidate paths on the left according to the following formula, and find the candidate path with the least cost:

If the candidate path encounters an obstacle, the cost is infinite and does not participate in the calculation of the above formula, where (x _c , y _c ) are the coordinates of the vehicle itself, ( _xi , y _i ) are the coordinates of the end point of the candidate path, and the value range of i is 1 To 15, I represents the candidate path number.

The step S4 includes the following sub-steps:

Step S401, speed setting;

First read the own vehicle speed V _c , the speed of the vehicle ahead V _o and the distance d of the two vehicles, and then use the following formula to calculate the target vehicle speed V _obj :

Where t=min{1, αV _c /d}, because t∈(0,1), the conditional restriction of taking the minimum value is added, and the parameters k and α need to be adjusted appropriately according to the actual vehicle performance and the performance of the control module;

Step S402, update the current local path and extended track;

The current local path update rules:

a. The unmanned vehicle travels to the end point of the current local path;

b. There are obstacles on the extended path or on the current local path;

c. The expansion path is the global path point from the last target point to the current target point. Because the planning program continuously locates new target points on the global path, the expansion path is always updated.

The present invention updates the local planning result in order to adapt to the constant change of the traffic scene, and the real-time adjustment is beneficial to the guarantee of safety and comfort. It includes speed setting and updating the current local path and extending the track.

Description of drawings

Fig. 1 is the overall framework schematic diagram of the present invention;

Figure 2 is a schematic diagram of the positional relationship between the experimental vehicle and the vehicle ahead, the current local path and the extended path;

Figure 3 is a schematic diagram of the traffic scene before changing lanes;

Figure 4 is a schematic diagram of a lane change analysis process;

Figure 5 is a schematic diagram of the traffic scene after changing lanes;

Figure 6 is a simple bicycle kinematics model;

Detailed ways

The present invention will be further described in detail below with reference to the accompanying drawings.

As shown in Figure 1, a path planning method for unmanned vehicles in a structured environment includes the following steps:

Step S1, positioning. Specifically, as shown in the first dashed box in Figure 1, it includes the following sub-steps:

Step S101, obtaining global map information and vehicle status information;

Specifically, the global map information includes the coordinates (x _i , y _i ) of the waypoints, i∈N, and traffic sign information; the vehicle state information includes the coordinates (x _c , y _c ) of the unmanned vehicle, the speed V _c , The acceleration a _c , towards the rate of change θ _c ', towards the θ _c .

Step S102, calculating the global positioning point;

Specifically, the distance from the coordinates (x _c , y _c ) of the vehicle to the coordinates ( _xi , y _i ) of each global path point is calculated, and the global path point with the smallest distance from the vehicle is found as the global positioning point.

Step S2, analyzing the current vehicle and road conditions. Specifically, as shown in the second dashed box in Figure 1, the following sub-steps are included:

Step S201, acquiring perception information;

Specifically, the perception information includes obstacle information, traffic sign information and lane line information. The detected obstacles are represented by rectangular boxes, including length, width, orientation, speed, and acceleration attributes. The traffic sign information mainly includes speed limit signs, arrows, traffic lights, and lane line information to tell the unmanned vehicle how many lanes there are currently. Which lane is it in, and whether lane changes are allowed.

Step S202, judging whether complete re-planning is required;

Three judgments need to be made to judge whether the re-planning is complete or not. First, determine whether there is an obstacle ahead; secondly, check whether the lane line is a solid line; finally, check whether the traffic sign prohibits changing lanes.

Specifically, it is determined whether there is an obstacle ahead. As shown in Figure 2, the experimental vehicle has traveled a certain distance along the current local path, and the extended path has also extended forward for a certain distance. The extended path is from the last target point to The purpose of the global path point of the target point this time is to discover the obstacles ahead earlier during automatic driving, and give the experimental vehicle sufficient distance to make corresponding adjustments. In Figure 2, the vehicle ahead is on the extended path and is in the same lane as the test vehicle. If the speed of the vehicle ahead is lower than the speed of the test vehicle, it means that the current lane cannot be driven smoothly. Then check the lane line information to determine whether it is a solid line. If it is a solid line, complete re-planning is not required. Otherwise, check whether the traffic signs prohibit changing lanes. If no traffic signs prohibit changing lanes, perform step S3 for complete re-planning. In short, the conditions for complete replanning are that there are obstacles in the current lane that are smaller than the vehicle speed, and the lane lines are not solid lines and there are no traffic signs prohibiting lane changing. Otherwise, complete re-planning will not be carried out, and only the current lane will be kept.

Step S3, complete re-planning. Specifically, as shown in the third dashed box in Figure 1, the following sub-steps are included:

Step S301, lane change analysis;

Specifically, as shown in FIG. 3 , first, with the vehicle itself as the center, the front and rear of the vehicle are divided into 9 sub-areas, numbered from left to right, and from top to bottom as A1, A2...A9. The areas where the car itself may be located are A4, A5 and A6, which are represented by A _c ∈ {A4, A5, A6}, and the obstacle may be in any area except the A _c area, which is represented by A _o ∈ {A} andA _o ≠A _c , A is the complete set, including all subregions.

The process of changing lanes to the left is shown in Figure 4, and the experimental vehicle is in the area A5.

①. Obtain the latest vehicle information in the A2 area from the perception module, the speed is recorded as V _o2 , and the acceleration is recorded as a _o2 . If V _o2 is greater than the experimental vehicle speed V _c , or the speed V _o2 +2×a _o2 after the simulation for two seconds is greater than V _c , it is considered that the current lane can be driven normally, choose to follow, and keep the current local path;

②. If the judgment result in step ① is no, then judge whether the A4 area is occupied, if so, choose to follow and keep the current local path;

③. If the judgment result in step ② is no, judge whether the A1 area is occupied. If yes, and the condition V _o1 is less than V _c and V _o1 +2×a _o1 is less than V _c , choose to follow and keep the current local path ;

④. If the area A1 is not occupied, or the condition V _o1 is greater than V _c or V _o1 +2×a _o1 is greater than V _c , then judge whether the area A7 is occupied;

⑤. If the A7 area is not occupied in step ④, select the candidate path with the least cost;

⑥. If the A7 area is occupied in step ④, judge whether V _o7 ×2+2×a _o7 is greater than dis+V _c ×2 (dis is the longitudinal distance from the front of the A7 area to the rear of the experimental vehicle, 2 is the simulation time), If so, choose to follow and keep the current local path, otherwise, choose to change lanes to the left. FIG. 3 is a schematic diagram before the lane change, and FIG. 5 is a schematic diagram after the lane change.

Step S302, template generation;

Figure 6 is a simple bicycle kinematics model that satisfies the following formula:

分别为x、y、θ对时间的一阶导数。The vehicle state model S=[x,y,θ,δ,v] ^T is established according to the simple bicycle kinematics model, where S is the vehicle motion state, and (x,y) is the lateral coordinate of the center of the rear axle relative to the global coordinate origin and the longitudinal coordinate, θ is the vehicle heading angle, δ is the front wheel rotation angle, v is the current speed of the vehicle, L is the distance from the center of the front wheel to the center of the rear wheel,

are the first derivatives of x, y, and θ with respect to time, respectively.

The present invention adopts the quintic spline interpolation method to generate candidate paths, and the calculation of the x-axis coordinates is described in detail here. The x-coordinates of points on the candidate path should satisfy the following formula:

a, b, c, d, e and f are the coefficients of the quintic spline, s is the distance the car moves, where x is not derived from the time t, but the derivation of the distance s moved by the car, which is similar to the simple bicycle movement. The learning model is somewhat different, and the formula is derived as follows:

dx/dt=vcosθ

ds/dt=v

First, obtain the vehicle's own state S(x ₀ , y ₀ , θ ₀ , δ ₀ , v ₀ ) as the initial condition of the spline function, and substitute it into the formula (*) to obtain the equation system:

where θ ₀ ′=tan(δ ₀ )/L.

Then, denote the end point information of the local path as (x _se , y _se , θ _se ) as the end point condition, and substitute it into the formula (*) to obtain the equation system:

After arranging the equation system, it is written in the form of AB=C matrix equation:

in

A=[a b c]

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

Next, the value of the matrix A is obtained through the matrix inversion operation A=CB ^-1 , and then all the coefficients a, b, c, d, e and f of the fifth-order spline are obtained. Finally, the x-coordinates of all candidate path points are calculated according to the quintic spline function. Similarly, calculate the y-coordinate of the candidate path point.

Through the above method, the x-coordinate and y-coordinate of each point of the candidate path are obtained, and a candidate path from the experimental vehicle itself to the end point of the local path 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 the generated candidate path set, that is, a template, is shown in Figure 3.

Step S303, alternative path selection;

Calculate the cost of the 15 candidate paths on the left according to the following formula, and find the candidate path with the least cost:

If the candidate path encounters an obstacle, the cost is infinite and does not participate in the calculation of the above formula. Where (x _c , y _c ) are the coordinates of the vehicle itself, (x _i , y _i ) are the coordinates of the end point of the candidate path, the value of i ranges from 1 to 15, and I represents the sequence number of the candidate path.

Step S4, updating the local planning result. Specifically, as shown in the fourth dashed box in Figure 1, the following sub-steps are included:

Step S401, speed setting;

Specifically, first read the own vehicle speed V _c , the speed V _o of the preceding vehicle, and the distance d between the two vehicles. The target vehicle speed V _obj is then calculated using the following formula:

Here, the third-order Bessel formula is used to calculate the target speed of the car, where t=min{1,αV _c /d}, because t∈(0,1), the conditional restriction of taking the minimum value is added, and the parameters k and α need to be Adjust appropriately according to the performance of the actual vehicle and the performance of the control module. The advantage of its design is that the initial braking acceleration of the unmanned vehicle will be relatively large when the speed of the vehicle ahead is slower than that of the vehicle itself. Reduce the speed of the vehicle to a safe speed, and then slowly slide to a safe distance from the obstacle to stop, which is safe and comfortable.

Step S402, update the current local path and extended track;

Specifically, the current local path update rules:

a. The unmanned vehicle travels to the end point of the current local path;

b. There are obstacles on the extended path or on the current local path;

c. The expansion path is the global path point from the last target point to the current target point. Because the planning program continuously locates new target points on the global path, the expansion path is always updated.

In summary, the present invention provides a path planning method for an unmanned vehicle in a structured environment, the main contents including a path generation algorithm, speed planning, and trajectory selection. This planning method has real feasibility and good scalability, and can ensure the safety and comfort of the driverless vehicle during driving.

For those skilled in the field of unmanned driving, the automatic driving of unmanned vehicles can be realized according to the planning method described above, or corresponding changes and appropriate adjustments can be made based on them, and all these changes and adjustments shall belong to the present invention within the protection scope of the claims.

Claims (4)

1. the path planning method of unmanned vehicle under a structured environment, it is characterized in that comprising the following steps: Step S1, positioning; First load the global map data, then read the vehicle status information, and finally find the global path point with the shortest distance from the vehicle as the global positioning point; Step S2, analysis of current vehicle road conditions; First, the sensor obtains traffic scene information, that is, obstacle information, traffic sign information, and lane line information; second, judges whether the obtained traffic scene information requires complete re-planning, and if so, go to step S3 for complete re-planning, otherwise , go to step S4, update the local planning result; Step S3, complete re-planning; In step S301, the lane change analysis obtains the longitudinal distance, speed and acceleration between the unmanned vehicle and the front and rear vehicles, and calculates whether the unmanned vehicle can change lanes in the current traffic scene, specifically: First, take the car itself as the center, divide the front and rear of the car into 9 sub-areas, numbered from left to right, and from top to bottom are A1, A2...A9, of which the car itself may be in the areas A4, A5 and A6, the symbols Denoted as A _c ∈{A4,A5,A6}, the obstacle may be in any area except the A _c area, the symbol is expressed as A _o ∈{A}and A _o ≠A _c , A is the complete set, including all sub-areas ; The analysis of changing lanes to the left is as follows, the experimental vehicle is in area A5; ①. Obtain the latest vehicle information in the A2 area from the perception module. The speed is recorded as V _o2 , and the acceleration is recorded as a _o2 . If V _o2 is greater than the experimental vehicle speed V _c , or the speed V _o2 +2×a _o2 after the simulation for two seconds is greater than V o2 _c , it is considered that the current lane can be driven normally, choose to follow, and keep the current local path; ②. If the judgment result in step ① is no, then judge whether the A4 area is occupied, if so, choose to follow and keep the current local path; ③. If the judgment result in step ② is no, judge whether the A1 area is occupied. If yes, and the condition V _o1 is less than V _c and V _o1 +2×a _o1 is less than V _c , choose to follow and keep the current local path ; ④. If the area A1 is not occupied, or the condition V _o1 is greater than V _c or V _o1 +2×a _o1 is greater than V _c , then judge whether the area A7 is occupied; ⑤. If the A7 area is not occupied in step ④, select the candidate path with the least cost; ⑥. If the A7 area is occupied in step ④, judge whether V _o7 ×2+2×a _o7 is greater than dis+V _c ×2, dis is the longitudinal distance from the front of the A7 area to the rear of the experimental vehicle, and 2 is the simulation time. , then choose to follow, keep the current local path, otherwise, choose the candidate path with the least cost; Step S302, template generation: adopting the fifth-order spline interpolation algorithm, combined with the vehicle kinematics model, to generate 31 alternative paths, the set of these 31 alternative paths is called a template, specifically: The following formulas are satisfied:

The vehicle state model S=[x,y,θ,δ,v] ^T is established according to the simple bicycle kinematics model, where S is the vehicle motion state, and x, y are the transverse and longitudinal coordinates of the center of the rear axle relative to the global coordinate origin. Coordinates, θ is the vehicle heading angle, δ is the front wheel rotation angle, v is the current speed of the vehicle, L is the distance from the center of the front wheel to the center of the rear wheel,

are the first-order derivatives of x, y, and θ with respect to time, respectively; The candidate path is generated by the quintic spline interpolation method, and the x-coordinate of the point on the candidate path should satisfy the following formula: a, b, c, d, e, and f are the coefficients of the quintic spline, and s is the distance the car moves. Here, x is not derived from the time t, but from the distance s moved by the car. The formula is derived as follows: dx/dt=vcosθ ds/dt=v

First, obtain the vehicle's own state S(x ₀ , y ₀ , θ ₀ , δ ₀ , v ₀ ) as the initial condition of the spline function, and substitute it into the formula (*) to obtain the equation system:

where θ ₀ '=tan(δ ₀ )/L; Then, denote the end point information of the local path as (x _se , y _se , θ _se ) as the end point condition, and substitute it into the formula (*) to obtain the equation system:

After arranging the equation system, it is written in the form of AB=C matrix equation: in A=[a b c]

C=[sin(θ ₀ )θ ₀ 'se ² /2-cos(θ ₀ )se-x ₀ +x _se sin(θ ₀ )θ ₀ 'se-cos(θ ₀ )+cos(θ _se ) sin (θ ₀ )θ ₀ '] The value of matrix A is obtained by the matrix inversion operation A=CB ^-1 , so all the coefficients a, b, c, d, e and f of the quintic spline are obtained. Finally, all candidate paths are calculated according to the quintic spline function the x-coordinate of the point; In the same way, calculate the y-coordinate of the candidate waypoint; According to the x-coordinate and y-coordinate of each point of the candidate path, generate a candidate path from the experimental vehicle itself to the end point of the local path, and then use the other end points of the end point set as end point conditions to generate other candidate paths, and finally generate the candidate path. Collections, i.e. templates; Step S303, alternative path selection: calculate the cost of each alternative path, and use the alternative path with the least cost to update the local planning result, specifically: Calculate the cost of the 15 candidate paths on the left according to the following formula, and find the candidate path with the least cost:

If the candidate path encounters an obstacle, the cost is infinite and does not participate in the calculation of the above formula, where (x _c , y _c ) are the coordinates of the vehicle itself, ( _xi , y _i ) are the coordinates of the end point of the candidate path, and the value range of i is 1 To 15, I represents the candidate path sequence number; Step S4, updating the local planning result; Update the local planning results to adapt to the constant changes of the traffic scene, including speed setting and updating the current local path and expanding the trajectory. 2. The path planning method of unmanned vehicle under structured environment according to claim 1, it is characterized in that: The step S1 includes the following sub-steps: Step S101: obtain global map information and vehicle status information; The global map data includes the coordinates (x _i , y _i ) of the waypoints, i∈N, the traffic signs, and the vehicle state information includes the coordinates (x _c , y _c ) of the driverless car, the speed V _c , the acceleration a _c , towards θ _c , towards the rate of change θ _c '; Step S102, calculating the global positioning point; Calculate the distance from the coordinates (x _c , y _c ) of the vehicle to the coordinates (x _i , y _i ) of each global path point, and find the global path point with the smallest distance from the vehicle as the positioning point in the global coordinate system. 3. The path planning method of unmanned vehicle under structured environment according to claim 1, is characterized in that: The step S2 includes the following sub-steps: Step S201, acquiring perception information; Perception information includes obstacle information, traffic sign information and lane line information. The detected obstacles are represented by rectangular boxes, including length, width, orientation, speed, and acceleration attributes. Traffic sign information includes speed limit signs, arrows, traffic lights, and lane lines. The information is used to tell how many lanes the unmanned vehicle currently has, which lane it is currently in, and whether it is allowed to change lanes; Step S202, judging whether complete re-planning is required; To judge whether complete re-planning, first, judge whether there are obstacles ahead; secondly, check whether the lane line is a solid line; finally, check whether the traffic signs prohibit changing lanes; And the lane line is not a solid line and there is no traffic sign prohibiting changing lanes, otherwise, the complete re-planning will not be carried out, and only the current lane will be maintained. 4. The path planning method for an unmanned vehicle in a structured environment according to claim 1, wherein: The step S4 includes the following sub-steps: Step S401, speed setting; First read the own vehicle speed V _c , the speed of the vehicle ahead V _o and the distance d of the two vehicles, and then use the following formula to calculate the target vehicle speed V _obj :

Where t=min{1, αV _c /d}, because t∈(0,1), the conditional restriction of taking the minimum value is added, and the parameters k and α need to be adjusted appropriately according to the actual vehicle performance and the performance of the control module; Step S402, update the current local path and extended track; The current local path update rules: a. The unmanned vehicle travels to the end point of the current local path; b. There are obstacles on the extended path or on the current local path; c. The expansion path is the global path point from the last target point to the current target point. Because the planning program continuously locates new target points on the global path, the expansion path is always updated.

Images