Layered local path planning method based on cognitive risk model

Abstract

In order to solve the problem that a traditional planning method neglects driver cognition and track planning real-time performance, the invention discloses a layered local path planning method based on a cognitive risk model, and relates to the technical field of track planning of automatic driving automobiles. Establishing a risk cognition grid map according to the environment information and the state information of the vehicle; according to the risk cognition grid map, a local path is preliminarily planned, and trajectory planning rough solution discrete points are obtained; a safe driving corridor is established according to the trajectory planning rough solution discrete points; and according to the safe driving corridor, fine planning of the track is carried out to obtain an optimal track. According to the method, the high-risk area can be identified and avoided, the vehicle driving track can be optimized through the safety corridor, and the driving safety and efficiency are ensured.

Description

Technical Field

[0001] The present invention relates to the technical field of trajectory planning for autonomous vehicles, and particularly to a hierarchical local path planning method based on a cognitive risk model. Background Art

[0002] With the development of autonomous driving technology, trajectory planning, as one of the core modules, plays a crucial role in ensuring the safe driving of vehicles and improving driving efficiency.

[0003] Although traditional trajectory planning methods can generate driving paths, they usually ignore the combined effects of driver cognitive characteristics and environmental risks. This may lead to potential safety hazards or unreasonable problems in the trajectory planning results in actual scenarios, and may also affect the real-time performance of the planning due to excessive constraint conditions, resulting in the occurrence of safety accidents.

[0004] Currently, most trajectory planning methods adopt the way of directly generating accurate paths through global planning. Global planning usually needs to process a large amount of environmental information, such as obstacle distribution, road boundaries, lane information, etc. Due to the high computational complexity, global planning is difficult to complete updates within a limited time, resulting in insufficient real-time performance of the system and being unable to handle emergencies. At the same time, global planning usually targets macro paths and pays more attention to overall optimality, making it difficult to take into account local detail optimization. Especially in narrow or obstacle-dense areas, global planning may generate paths that do not meet actual feasibility, increasing the difficulty of vehicle trajectory execution. Summary of the Invention

[0005] In order to make up for the problems of traditional planning methods that ignore driver cognition and the real-time performance of trajectory planning, the present invention provides a hierarchical local path planning method based on a cognitive risk model, which combines driver cognitive risk with an environmental risk field, and at the same time takes into account the stability risk of the vehicle, constructs a risk map and conducts trajectory planning. This method can not only identify and avoid high-risk areas, but also optimize the vehicle driving trajectory through a safety corridor to ensure driving safety and efficiency. It ensures the real-time performance and safety of trajectory planning in a complex traffic environment. The present invention has broad application prospects in the fields of intelligent driving, driverless vehicles, assisted driving systems, etc., and provides important support for the popularization of future autonomous driving technology.

[0006] To achieve the above object, the present invention adopts the following technical solutions:

[0007] A hierarchical local path planning method based on a cognitive risk model, comprising the following steps:

[0008] Collect environmental information and the state information of the vehicle itself, and establish a risk cognitive grid map according to the environmental information and the state information of the vehicle itself;

[0009] According to the risk perception grid map, preliminarily plan the local path to obtain the rough solution discrete points of trajectory planning;

[0010] Establish a safe driving corridor based on the rough solution discrete points of trajectory planning;

[0011] Perform fine planning of the trajectory according to the safe driving corridor to obtain the optimal trajectory.

[0012] To optimize the above technical solution, the specific measures taken also include:

[0013] Further, the environmental information includes lane line position, road boundary, reference path information, and obstacle information. The obstacle information includes the position (x obs , y obs ) of the obstacle centroid, the speed v obs of the obstacle, the heading angle of the obstacle, and the size of the obstacle. The size of the obstacle includes the longitudinal outer dimension s x of the obstacle and the transverse outer dimension s y of the obstacle; the state information of the vehicle itself includes the position (x, y) of the vehicle, the vehicle speed v host , the acceleration a host and the heading angle

[0014] Further, the establishment of the risk perception grid map according to the environmental information and the state information of the vehicle itself is specifically as follows:

[0015] Determine the resolution of the risk perception grid map, and establish a rectangular grid map with lengths and widths of X g and Y g respectively;

[0016] Risk perception includes driver perception risk, environmental risk, and obstacle risk;

[0017] Calculate the driver perception risk, environmental risk, and obstacle risk of each point on the rectangular grid map respectively;

[0018] According to the vehicle speed, position, and steering angle information, construct a driver perception risk field; the driver perception risk E d is:

[0019]

[0020] In the formula, p, s, t la are the risk field cross-section length coefficient, arc length, and preview time respectively, L is the vehicle wheelbase, d is the steering angle, s is the risk field cross-section width, x is the ordinate of a certain point on the risk grid map, x hostis the current ordinate of the host vehicle, y is the abscissa of a point on the risk grid map, y host is the current abscissa of the host vehicle;

[0021] Environmental risk E env Considering the lane line risk and the lane boundary risk, the lane line risk is E lane :

[0022]

[0023] where lane risk is the lane line risk parameter, lane w is the lane line width, y is the abscissa of a point on the risk grid map;

[0024] Lane boundary risk E bound is:

[0025]

[0026] where Bound risk is the lane boundary risk parameter, k is the boundary risk adjustment coefficient, D is the lateral parameter;

[0027] For the obstacle risk E obs , it consists of the static obstacle risk E obs_s and the dynamic obstacle risk E obs_m as follows,

[0028] E obs = E obs_s + E obs_m

[0029] Static obstacle risk E obs_s Considering the distance of the obstacle, the position of the obstacle near the lane line, and the shape and size of the obstacle, the calculation formula is as follows:

[0030]

[0031] where r is the obstacle field strength parameter, s x , s y are the longitudinal and lateral outer dimensions of the obstacle respectively, x obs is the ordinate of the obstacle centroid, y obs is the abscissa of the obstacle centroid, x is the ordinate of a point on the risk grid map, y is the abscissa of a point on the risk grid map, b is the lane interference factor, which adjusts the influence of the lane width on the risk, y lane is the abscissa of the lane line, lane w is the lane line width;

[0032] Dynamic obstacle risk E obs_mThe calculation formula is as follows:

[0033]

[0034] Among them, Z is a constant, s y is the lateral contour dimension of the obstacle, x' is the rotated ordinate considering the heading angle of the obstacle vehicle, y' is the rotated abscissa considering the heading angle of the obstacle vehicle, and the specific formula is:

[0035]

[0036] Among them, q' is the relative heading angle between the vehicle and the obstacle, and the calculation formula is α is the weighted coefficient of the influence of speed on risk;

[0037] Adding the environmental risk and the obstacle risk, and then multiplying by the driver's cognitive risk to obtain the environmental characteristic risk E with driver's cognition t , and the specific formula is:

[0038] E t = E d ×(E env + E obs )

[0039] Among them, E env is the environmental risk, E obs is the obstacle risk, E d is the driver's cognitive risk;

[0040] Each point on the rectangular grid map has its corresponding environmental characteristic risk value with driver's cognition, and when combined, it is the risk cognition grid map.

[0041] Furthermore, the specific process of initially planning a local path based on the risk cognition grid map to obtain the rough solution discrete points of the trajectory planning is as follows:

[0042] Given the planning time, conduct initial planning for the lateral and longitudinal directions. Among them, for the lateral planning, based on the current lane boundary, with dl_c as the lateral sampling distance, generate the lateral planning end points: d end1 , d end2 ,..., d endi , d endi is the i-th lateral planning end point, satisfying: d endi ∈[d min , d max ; d min is the lower limit of the lane boundary, d max is the upper limit of the lane boundary;

[0043] For the longitudinal planning, with the given acceleration a xPerform acceleration and deceleration as sampling of the longitudinal speed, and combine with the current vehicle speed as the planned end state v of the longitudinal vehicle speed end =[v host -a x T c ,v host ,v host +a x T c

[0044] v host is the current vehicle speed, a x is the acceleration constant for longitudinal rough planning, T c is the rough planning time;

[0045] According to the fifth-degree polynomial d(t)=b 0 +b 1 t+b 2 t 2 +b 3 t 3 +b 4 t 4 +b 5 t 5 , with t = 0:dt:T c and the lateral planning end point d endi generate the lateral trajectory, t is the time series, b 0 ,b 1 ,b 2 ,b 3 ,b 4 ,b 5 are the coefficients to be solved and obtained according to the boundary conditions;

[0046] Based on the fourth-degree polynomial s(t)=a 0 +a 1 t+a 2 t 2 +a 3 t 3 +a 4 t 4 generate the longitudinal trajectory, a 0 ,a 1 ,a 2 ,a 3 ,a 4 are the coefficients to be solved. Determine the coefficients of the fourth-degree polynomial through the boundary conditions. Based on the time series, combine the generated longitudinal trajectory with the lateral trajectory to form a candidate trajectory cluster;

[0047] Take comfort and risk value as the selection conditions for the preliminary planning. The total cost Cost of the preliminary planning cs is: Cost cs =l r Cost​r +l c Cost c , where Cost r , Cost c are the risk cost and comfort cost of the trajectory respectively, and l r and l c are the risk cost weight and comfort cost weight of the trajectory respectively. Select the trajectory with the minimum total cost Cost cs from the candidate trajectory cluster as the discrete points of the rough solution of trajectory planning.

[0048] Furthermore, the specific process of establishing a safe driving corridor based on the discrete points of the rough solution of trajectory planning is as follows:

[0049] Judge whether the risk value E t of each point on the risk perception grid map exceeds the risk threshold risk th . The points that do not exceed the risk threshold form a safe drivable area F, and then map the discrete points of the rough solution of trajectory planning into a binary set representing the grid map;

[0050] In the risk perception grid map, A represents the set of discrete points of the rough solution of trajectory planning; establish a morphological structure element S, which indicates a set of discrete coordinates around a certain point, representing the shape and size of the neighborhood to be expanded. S is set as a rectangle with the same aspect ratio as the vehicle length and width. Perform morphological dilation according to the mapped binary set A and the established structure element S. The specific formula is:

[0051]

[0052] where (S) z ={z + s|s ∈ S} represents translating the morphological structure element S to the point in A, represents the dilation operation, z represents the target position of the structure element translation, and s is an element in the morphological structure element S, representing a relative position in the structure element;

[0053] After morphological dilation, a morphological dilation area M generated by the rough solution of trajectory planning is obtained. Take the intersection of the safe driving area and the morphological dilation area to obtain the safe driving corridor:

[0054] C o = M ∩ F

[0055] In the formula, C o is the safe driving corridor.

[0056] Furthermore, the specific process of performing fine planning of the trajectory according to the safe driving corridor to obtain the optimal trajectory is as follows:

[0057] Extract the safe driving corridor C o The boundary x b , y b , x b is the ordinate of all boundary points, and y b is the abscissa of all boundary points;

[0058] Calculate the execution time T of the fine planning. With the longitudinal boundary limit x bmax as the end point, and from the ordinate x host of the current vehicle position to the longitudinal boundary limit x bmax at a constant acceleration, the required time is taken as the execution time for the completion of the fine planning. The formula is: Solve this equation to obtain the execution time for the completion of the fine planning. a x represents the constant acceleration, and v host represents the current vehicle speed;

[0059] For the transverse direction, select the transverse boundaries of the safe corridor as the upper and lower limits y bmin , y bmax . The transverse speed, transverse acceleration, and transverse jerk are taken as 0. The candidate set for the transverse end point is: [y bmin , y bmin + Dd,..., y bmax ; Dd represents the transverse sampling distance;

[0060] For the longitudinal direction, calculate the lower limit of the speed with the execution time T and the constant acceleration: v tmin = v host - a x T. The candidate set for the longitudinal speed is [v tmin , v tmin + Dv,..., v tmax . It is necessary to ensure that the longitudinal speed set is within [0, v max . Dv represents the longitudinal speed sampling, v tmin represents the lower limit of the vehicle speed, and v tmax represents the upper limit of the vehicle speed;

[0061] Based on the candidate set for the transverse end point and the candidate set for the longitudinal speed, the transverse planning is carried out using a fifth-degree polynomial, and the longitudinal planning is carried out using a fourth-degree polynomial. Combine the generated transverse and longitudinal trajectories to obtain the candidate set for the fine planning trajectory;

[0062] Using the maximum vehicle driving speed, maximum acceleration, maximum driving curvature, and maximum tolerable risk as constraint conditions, screen the candidate set for the fine planning trajectory, and eliminate the trajectories that do not meet the constraint conditions to obtain a new fine planning trajectory set P i = {P i1 , Pi2 ,..., P im},P i is the i-th new fine planning trajectory, P im represents the m-th point on the path sample. If none of them meet the requirements, return to the rough planning stage, select the rough planning trajectory with the second smallest cost, and re-establish the safety corridor for fine planning;

[0063] Taking the trajectory risk value, the trajectory driving stability cost, and the comfort cost as evaluation criteria, the total cost Cost t is: Cost t = l r Cost r + l s Cost s + l c Cost c Cost r represents the trajectory risk cost, l r represents the weight of the trajectory risk cost, Cost s represents the trajectory driving stability cost, l s represents the weight of the trajectory driving stability cost, Cost c represents the comfort cost, l c represents the weight of the comfort cost. Select the trajectory with the smallest cost from the new fine planning trajectory set P i = {P i1 , P i2 ,..., P im} as the optimal trajectory.

[0064] Furthermore, the calculation method of the trajectory risk cost is as follows:

[0065] Based on the environmental feature risk field with driver cognition, calculate the risk value R path (P i ) of each path in the new fine planning trajectory set, and calculate the uncertainty of the path risk according to the risk values of all path samples: where R mean is the average risk value of all paths, n is the number of path samples, and update the risk value of each path to obtain the trajectory risk cost: In the formula, Cost r represents the trajectory risk cost.

[0066] Furthermore, the calculation method of the trajectory driving stability cost is as follows:

[0067] Establish a stability cost circle for the yaw rate w and the sideslip angle b of the center of mass. The specific formula is:

[0068]

[0069] Among them, Cost s represents the cost of the stability of the trajectory vehicle, and C s is the stability cost coefficient. p and q are the weight coefficients of the sideslip angle b of the center of mass and the yaw rate w respectively. b 0 and w 0 are the values of the sideslip angle of the center of mass and the yaw rate at steady state calculated by the linear two-degree-of-freedom model. Based on the longitudinal speed v x of the planned vehicle, and the lateral acceleration a y , the sideslip angle b of the center of mass is calculated, and the yaw rate w is the derivative of the vehicle heading angle .

[0070] The beneficial effects of the present invention are as follows:

[0071] The present invention can not only identify and avoid high-risk areas, but also optimize the vehicle driving trajectory through a safety corridor to ensure driving safety and efficiency. The real-time performance and safety of trajectory planning are ensured in a complex traffic environment. The present invention has broad application prospects in the fields of intelligent driving, driverless vehicles, assisted driving systems, etc., and provides important support for the popularization of future autonomous driving technologies. BRIEF DESCRIPTION OF THE DRAWINGS

[0072] Figure 1 is a flowchart of the hierarchical local path planning method based on the cognitive risk model proposed by the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0073] The following will clearly and completely describe the technical solutions in the embodiments of the present application with reference to the accompanying drawings in the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments in the present application without creative efforts shall fall within the protection scope of the present application.

[0074] Embodiment 1

[0075] The present invention proposes a hierarchical local path planning method based on a cognitive risk model, including the following steps:

[0076] Collect environmental information and the state information of the vehicle itself through sensors installed on the vehicle. The environmental information includes the position of lane lines, road boundaries, reference path information, and obstacle information. The obstacle information includes the position (x obs , y obs ) of the center of mass of the obstacle, the speed v obs of the obstacle, the heading angle of the obstacle, and the size of the obstacle. The size of the obstacle includes the longitudinal external dimension sx and the lateral dimension s of the obstacle y ; the state information of the vehicle itself includes the position (x, y) of the vehicle, the vehicle speed v host , the acceleration a host and the heading angle

[0077] Establish a risk perception grid map based on the environmental information and the state information of the vehicle itself; specifically:

[0078] Determine the resolution of the risk perception grid map, and establish a rectangular grid map with the length and width being X g and Y g respectively;

[0079] Risk perception includes driver perception risk, environmental risk and obstacle risk;

[0080] Calculate the driver perception risk, environmental risk and obstacle risk of each point on the rectangular grid map respectively;

[0081] Construct a driver perception risk field according to the speed, position and steering angle information of the vehicle; the driver perception risk E d is:

[0082]

[0083] In the formula, p, s, t la are the risk field cross-section length coefficient, arc length and preview time respectively, L is the wheelbase of the vehicle, d is the steering angle, s is the width of the risk field cross-section, x is the ordinate of a certain point on the risk grid map, and x host is the current ordinate of the host vehicle, y is the abscissa of a certain point on the risk grid map, and y host is the current abscissa of the host vehicle;

[0084] The environmental risk E env considers the lane line risk and the lane boundary risk. The lane line risk is E lane :

[0085]

[0086] Among them, lane risk is the lane line risk parameter, lane w is the lane line width, and y is the abscissa of a certain point on the risk grid map;

[0087] The lane boundary risk E bound is:

[0088]

[0089] Among them, Boundrisk is the lane boundary risk parameter, k is the boundary risk adjustment coefficient, and D is the lateral parameter;

[0090] For the obstacle risk E obs , it consists of the static obstacle risk E obs_s and the dynamic obstacle risk E obs_m .

[0091] E obs = E obs_s + E obs_m

[0092] The static obstacle risk E obs_s Considering the distance of the obstacle, the position of the obstacle near the lane line, and the shape and size of the obstacle, the calculation formula is as follows:

[0093]

[0094] Among them, r is the obstacle field strength parameter, s x , s y are the longitudinal and lateral outer dimensions of the obstacle respectively, x obs is the ordinate of the centroid of the obstacle, y obs is the abscissa of the centroid of the obstacle, x is the ordinate of a point on the risk grid map, y is the abscissa of a point on the risk grid map, b is the lane interference factor, which adjusts the influence of the lane width on the risk, y lane is the abscissa of the lane line, lane w is the lane line width;

[0095] The dynamic obstacle risk E obs_m The calculation formula is as follows:

[0096]

[0097] Among them, Z is a constant, s y is the lateral outer dimension of the obstacle, x' is the rotated ordinate considering the heading angle of the obstacle vehicle, y' is the rotated abscissa considering the heading angle of the obstacle vehicle, and the specific formula is:

[0098]

[0099] Among them, q' is the relative heading angle between the vehicle and the obstacle, and the calculation formula is α is the weighted coefficient of the influence of speed on risk;

[0100] Adding the environmental risk and the obstacle risk, and then multiplying by the driver's cognitive risk to obtain the environmental characteristic risk E t with driver's cognition, and the specific formula is:

[0101] Et = E d ×(E env + E obs )

[0102] Among them, E env is the environmental risk, E obs is the obstacle risk, E d is the driver's cognitive risk;

[0103] Each point on the rectangular grid map has its corresponding environmental characteristic risk value with driver cognition, and when combined, it forms a risk cognition grid map.

[0104] According to the risk cognition grid map, a local path is initially planned to obtain the discrete points of the rough solution of the trajectory planning; the specific process is as follows:

[0105] Given the planning time, preliminary planning is carried out for the horizontal and vertical directions. Among them, for the horizontal planning, based on the current lane boundary, with dl_c as the horizontal sampling distance, the horizontal planning end points are generated: d end1 , d end2 ,..., d endi , d endi is the i-th horizontal planning end point, satisfying: d endi ∈[d min , d max ; d min is the lower limit of the lane boundary, d max is the upper limit of the lane boundary;

[0106] For the vertical planning, the acceleration and deceleration with the given acceleration a x are used as the sampling of the vertical speed, combined with the current vehicle speed, as the planned end state v end = [v host - a x T c , v host , v host + a x T c

[0107] v host is the current vehicle speed, a x is the acceleration constant for the vertical rough planning, T c is the rough planning time;

[0108] Based on the fifth-degree polynomial d(t) = b 0 + b 1 t + b 2 t 2 + b 3 t 3 + b 4 t 4 ​+b 5 t 5 , with t = 0:dt:T c and the lateral planning end point d endi to generate a lateral trajectory, where t is the time series and b 0 ,b 1 ,b 2 ,b 3 ,b 4 ,b 5 is the coefficient to be solved and is obtained according to the boundary conditions;

[0109] Based on the quartic polynomial s(t) = a 0 +a 1 t + a 2 t 2 +a 3 t 3 +a 4 t 4 to generate a longitudinal trajectory, where a 0 ,a 1 ,a 2 ,a 3 ,a 4 are the coefficients to be solved. The coefficients of the quartic polynomial are determined through boundary conditions. Based on the time series, the generated longitudinal trajectory and lateral trajectory are combined to form a candidate trajectory cluster;

[0110] Taking comfort and risk value as the selection conditions for the preliminary planning, the total cost Cost cs of the preliminary planning is: Cost cs = l r Cost r + l c Cost c , where Cost r ,Cost c are the risk cost and comfort cost of the trajectory respectively, and l r and l c are the risk cost weight and comfort cost weight of the trajectory respectively. The trajectory with the minimum total cost Cost cs is selected from the candidate trajectory cluster as the discrete points of the rough solution of the trajectory planning.

[0111] Establish a safe driving corridor based on the discrete points of the rough solution of the trajectory planning; the specific process is as follows:

[0112] Judge whether the risk value E t of each point on the risk perception grid map exceeds the risk threshold risk th . The points that do not exceed the risk threshold form a safe drivable area F, and then the discrete points of the rough solution of the trajectory planning are mapped into a binary set representing the grid map;

[0113] In the risk perception grid map, A represents the discrete point set of the rough solution of trajectory planning; a morphological structure element S is established, which represents a set of discrete coordinates around a certain point and represents the shape and size of the neighborhood to be expanded. S is set as a rectangle with an aspect ratio equal to that of the vehicle. According to the mapped binary set A and the established structure element S, morphological dilation is performed. The specific formula is:

[0114]

[0115] where (S) z ={z + s|s ∈ S} represents translating the morphological structure element S to the point in A, represents the dilation operation, z represents the target position of the translation of the structure element, and s is an element in the morphological structure element S, representing a relative position in the structure element;

[0116] After morphological dilation, a morphological dilation region M generated by the rough solution of trajectory planning is obtained. The intersection of the safe driving region and the morphological dilation region is taken to obtain the safe driving corridor:

[0117] C o = M ∩ F

[0118] In the formula, C o is the safe driving corridor.

[0119] According to the safe driving corridor, fine planning of the trajectory is carried out to obtain the optimal trajectory. The specific process is as follows:

[0120] Extract the boundary x o of the safe driving corridor C b , y b , x b is the ordinate containing all boundary points, and y b is the abscissa containing all boundary points;

[0121] Calculate the execution time T of the fine planning. Taking the longitudinal boundary limit x bmax as the end point, the time required to reach the longitudinal boundary limit x host from the current vehicle position ordinate x bmax with a constant acceleration is used as the execution time for the completion of the fine planning. The formula is: Solve this equation to obtain the execution time for the completion of the fine planning. a x represents the constant acceleration, and v host represents the current vehicle speed;

[0122] For the transverse direction, select the transverse boundary of the safe corridor as the upper and lower limits y bmin , y bmax, the lateral speed, lateral acceleration, and lateral jerk are taken as 0, and the candidate set for the lateral end point is: [y bmin , y bmin + Dd,..., y bmax ; Dd represents the lateral sampling distance;

[0123] For the longitudinal direction, the lower limit of the speed is calculated based on the execution time T and the constant acceleration: v tmin = v host - a x T, and the candidate set for the longitudinal speed is [v tmin , v tmin + Dv,..., v tmax , and it is necessary to ensure that the longitudinal speed set is within [0, v max , Dv represents the longitudinal speed sampling, v tmin represents the lower limit of the vehicle speed, and v tmax represents the upper limit of the vehicle speed;

[0124] Based on the candidate set for the lateral end point and the candidate set for the longitudinal speed, similar to the rough planning, the lateral planning uses a fifth-order polynomial for planning, and the longitudinal planning uses a fourth-order polynomial for planning. By combining the generated lateral and longitudinal trajectories, a candidate set for the refined planning trajectory is obtained;

[0125] Using the maximum vehicle speed, maximum acceleration, maximum driving curvature, and maximum tolerable risk during vehicle driving as constraint conditions, the candidate set for the refined planning trajectory is screened, and the trajectories that do not meet the constraint conditions are eliminated to obtain a new refined planning trajectory set P i ={P i1 , P i2 ,..., P im}, where P i is the i-th new refined planning trajectory, and P im represents the m-th point on the path sample. If none of them meet the requirements, return to the rough planning stage, select the rough planning trajectory with the second smallest cost, and re-establish the safety corridor for refined planning;

[0126] Using the trajectory risk value, the cost of trajectory driving stability, and the comfort cost as evaluation criteria, the total cost Cost t is: Cost t = l r Cost r + l s Cost s + l c Cost c , where Cost r represents the trajectory risk cost, l r represents the weight of the trajectory risk cost, and Cost s represents the cost of trajectory driving stability, ls The weight representing the cost of trajectory driving stability, Cost c Represents the comfort cost, l c The weight representing the comfort cost, from the new fine-planned trajectory set P i ={P i1 , P i2 ,..., P im}, select the trajectory with the minimum cost as the optimal trajectory.

[0127] When calculating the trajectory risk value Cost r , considering sensor errors, road condition changes, and the uncertainty of the vehicle's own state, enhance the reliability of path planning by simulating the uncertainty of the path. The specific method is:

[0128] Based on the environmental feature risk field with driver cognition, calculate the risk value R of each path in the new fine-planned trajectory set path (P i ), calculate the uncertainty of path risk according to the risk values of all path samples: Where R mean Is the average risk value of all paths, n is the number of path samples, update the risk value of each path to obtain the trajectory risk cost: In the formula, Cost r Represents the trajectory risk cost.

[0129] The calculation method of the trajectory driving stability cost is:

[0130] Establish a stability cost circle for yaw rate w and sideslip angle b of the center of mass. The specific formula is:

[0131]

[0132] Among them, Cost s Represents the trajectory driving stability cost, C s Is the stability cost coefficient, p and q are the weight coefficients of the sideslip angle b of the center of mass and the yaw rate w respectively, b 0 And w 0 Are the values of the sideslip angle of the center of mass and the yaw rate at steady state calculated by the linear two-degree-of-freedom model. Based on the longitudinal speed v of the planned vehicle x , the lateral acceleration a y , calculate the sideslip angle b of the center of mass, and the yaw rate w is the derivative of the vehicle heading angle .

[0133] Example Two

[0134] The present invention proposes a hierarchical local path planning system based on a cognitive risk model corresponding to the method of Example One, including:

[0135] A collection module, configured to collect environmental information and the status information of the vehicle itself;

[0136] A risk perception grid map establishment module, configured to establish a risk perception grid map according to the environmental information and the status information of the vehicle itself;

[0137] A preliminary planning module, configured to preliminarily plan a local path according to the risk perception grid map to obtain discrete points of a rough solution of trajectory planning;

[0138] A safe driving corridor establishment module, configured to establish a safe driving corridor according to the discrete points of the rough solution of trajectory planning;

[0139] A fine planning module, configured to perform fine planning of the trajectory according to the safe driving corridor to obtain an optimal trajectory.

[0140] The implementation manners of each module and the module functions in the system are completely consistent with the steps of the method in Embodiment 1, so details are not described herein again.

[0141] Those of ordinary skill in the art can realize that the units and algorithm steps of each example described in combination with the embodiments disclosed in this application can be implemented by electronic hardware or a combination of computer software and electronic hardware. Whether these functions are executed in a hardware or software manner depends on the specific application and design constraint conditions of the technical solution. Professional technicians can use different methods to implement the described functions for each specific application, but such implementation should not be considered to exceed the scope of this application.

[0142] The above are only the preferred implementation manners of the present invention. The protection scope of the present invention is not limited to the above embodiments. All technical solutions falling within the idea of the present invention belong to the protection scope of the present invention. It should be noted that for those of ordinary skill in the art in this technical field, several improvements and refinements made without departing from the principle of the present invention should be regarded as within the protection scope of the present invention.

Claims

1. A hierarchical local path planning method based on cognitive risk model, characterized in that: The following steps are involved: Collect environmental information and vehicle status information, and establish a risk awareness grid map based on the environmental information and vehicle status information; According to the risk perception grid map, the local path is preliminarily planned to obtain the discrete points of the trajectory planning rough solution; Establish a safe driving corridor based on the discrete points of trajectory planning; The trajectory is carefully planned according to the safe driving corridor to obtain the optimal trajectory.

2. The hierarchical local path planning method based on cognitive risk model according to claim 1, characterized in that: The environmental information includes lane line position, road boundary, reference path information and obstacle information. The obstacle information includes the position of the obstacle centroid (x obs ,y obs ), the speed of the obstacle v obs , the heading angle of the obstacle and the size of the obstacle, the size of the obstacle including the longitudinal dimension s of the obstacle x and the lateral dimensions s of the obstacle y The vehicle's own status information includes the vehicle's position (x, y), speed v host , acceleration a host and heading angle 3. The hierarchical local path planning method based on cognitive risk model according to claim 1, characterized in that: The risk awareness grid map is established based on the environmental information and the vehicle's own status information as follows: Determine the resolution of the risk awareness grid map and create a grid with a length and width of X g and Y g A rectangular grid map of Risk perception includes driver perceived risk, environmental risk, and obstacle risk; Calculate the driver's cognitive risk, environmental risk and obstacle risk for each point on the rectangular grid map; The driver’s cognitive risk field is constructed based on the vehicle’s speed, position and steering angle information; the driver’s cognitive risk E d for: In the formula, p, s, t la are the risk field section length coefficient, arc length and preview time, L is the vehicle wheelbase, d is the steering angle, s is the risk field section width, x is the ordinate of a point on the risk grid map, and x host is the current ordinate of the main vehicle, y is the abscissa of a point on the risk grid map, and y host The current horizontal coordinate of the main vehicle; Environmental Risk env Considering the lane line risk and lane boundary risk, the lane line risk is E lane : Among them, lane risk is the lane risk parameter, lane w is the lane width, y is the horizontal coordinate of a point on the risk grid map; Lane boundary risk E bound for: Among them, Bound risk is the lane boundary risk parameter, k is the boundary risk adjustment coefficient, and D is the lateral parameter; For obstacle risk E obs , the static obstacle risk E obs_s and dynamic obstacle risk E obs_m composition, AND obs =And obs_s +E obs_m Static obstacle risk E obs_s Taking into account the distance of the obstacle, the position of the obstacle near the lane line, and the shape and size of the obstacle, the calculation formula is as follows: Among them, r is the obstacle field strength parameter, s x ,s y are the longitudinal and lateral dimensions of the obstacle, respectively, obs is the ordinate of the obstacle mass center, y obs is the horizontal coordinate of the obstacle center, x is the vertical coordinate of a point on the risk grid map, y is the horizontal coordinate of a point on the risk grid map, b is the lane interference factor, and the influence of adjusting lane width on risk, y lane is the horizontal coordinate of the lane line, lane w is the lane width; Dynamic obstacle risk E obs_m The calculation formula is as follows: Among them, Z is a constant, s y is the lateral dimension of the obstacle, x' is the rotation ordinate considering the heading angle of the obstacle vehicle, y' is the rotation abscissa considering the heading angle of the obstacle vehicle, and the specific formula is: Among them, q' is the relative heading angle between the vehicle and the obstacle, and the calculation formula is: α is the weighting coefficient of the impact of speed on risk; Add the environmental risk to the obstacle risk, and then multiply it by the driver's cognitive risk to get the environmental characteristic risk E with driver cognition. t , the specific formula is: AND t =And d ×(And env +E obs ) Among them, E env is environmental risk, E obs is the obstacle risk, E d Identify risks for drivers; Each point on the rectangular grid map has its corresponding environmental characteristic risk value that is perceived by the driver, and the combination of these is a risk perception grid map.

4. The hierarchical local path planning method based on cognitive risk model according to claim 1, characterized in that: The specific process of preliminarily planning a local path based on the risk perception grid map and obtaining the discrete points of the trajectory planning rough solution is as follows: Given a planning time, preliminary planning is performed for the lateral and longitudinal directions. The lateral planning is based on the current lane boundary and takes dl_c as the lateral sampling distance to generate the lateral planning end point: d end1 ,d end2 ,...,d endi , d endi is the end point of the i-th horizontal planning, satisfying: d endi ∈[d min ,d max ];d min is the lower limit of the lane boundary, d max is the upper limit of the lane boundary; For longitudinal planning, given the acceleration a x Acceleration and deceleration are performed as the sampling of the longitudinal speed, combined with the current vehicle speed, as the planning end state v of the longitudinal speed end =[v host -a x T c ,v host ,v host +a x T c ] v host is the current vehicle speed, a x is the acceleration constant used for longitudinal rough planning, T c To plan the time roughly; According to the quintic polynomial d(t)=b0+b1t+b2t 2 +b3t 3 +b4t 4 +b5t 5 , with t = 0: dt: Tc and the lateral planning end point d endi Generate a lateral trajectory, t is the time series, b0, b1, b2, b3, b4, b5 are the coefficients to be calculated, which are obtained according to the boundary conditions; Based on the quartic polynomial s(t) = a0+a1t+a2t 2 +a3t 3 +a4t 4 Generate longitudinal trajectories, where a0, a1, a2, a3, and a4 are the coefficients to be determined. The coefficients of the quartic polynomial are determined by boundary conditions. Based on the time series, the generated longitudinal trajectories are combined with the transverse trajectories to form a candidate trajectory cluster. Taking comfort and risk value as the selection conditions for preliminary planning, the total cost of preliminary planning is Cost cs Cost cs = l r Cost r +l c Cost c , where Cost r ,Cost c are the risk cost and comfort cost of the trajectory, respectively, l r and l c are the risk cost weight and comfort cost weight of the trajectory respectively, and the total cost Cost is selected from the candidate trajectory cluster cs The smallest trajectory is the discrete point of the rough solution of trajectory planning.

5. The hierarchical local path planning method based on cognitive risk model according to claim 1, characterized in that: The specific process of establishing a safe driving corridor based on the rough solution of discrete points in trajectory planning is as follows: Determine the risk value E of each point on the risk perception grid map t Whether it exceeds the risk threshold risk th , the points that do not exceed the risk threshold form a safe drivable area F, and then the trajectory planning rough solution discrete points are mapped into a binary set Represents a gridded map; In the risk perception grid map, A represents the discrete point set of the rough solution of trajectory planning; a morphological structural element S is established, which indicates a set of discrete coordinates around a certain point, representing the shape and size of the neighborhood to be expanded. S is set to a rectangle with the same aspect ratio as the vehicle. Morphological expansion is performed based on the mapped binary set A and the established structural element S. The specific formula is: Among them, (S) z ={z+s|s∈S} represents translating the morphological structure element S to a point in A, represents the dilation operation, z represents the target position of the structural element translation, s is an element in the morphological structural element S, and represents a relative position in the structural element; After morphological expansion, a morphological expansion area M generated by the rough solution of trajectory planning is obtained. The intersection of the safe driving area and the morphological expansion area is taken to obtain the safe driving corridor: C o =M∩F In the formula, C o For safe driving corridor.

6. The hierarchical local path planning method based on cognitive risk model according to claim 1, characterized in that: The specific process of finely planning the trajectory according to the safe driving corridor to obtain the optimal trajectory is as follows: Extract safe driving corridor C o The boundary x b ,y b , x b is the ordinate of all boundary points, y b is the horizontal coordinate that contains all boundary points; Calculate the execution time T of the refined plan, taking the longitudinal boundary limit x bmax As the end point, the vehicle moves from the current vertical coordinate x to the end point at a constant acceleration. host To the longitudinal boundary limit x bmax The time required is taken as the execution time for the detailed planning to be completed, and the formula is: Solving this equation yields the execution time for detailed planning, a x represents constant acceleration, v host Indicates the current vehicle speed; For the horizontal direction, select the horizontal boundaries of the safety corridor as the upper and lower limits y bmin ,y bmax , the lateral velocity, lateral acceleration and lateral jerk are taken as 0, and the candidate set of the lateral end point is: [y bmin ,y bmin +Dd,...,y bmax ]; Dd represents the lateral sampling distance; For the longitudinal direction, the lower velocity limit is calculated with execution time T and constant acceleration: tmin =v host -a x T, the longitudinal velocity candidate set is [v tmin ,v tmin +Dv,...,v tmax ], it is necessary to ensure that the longitudinal velocity set is in [0,v max ], Dv represents the longitudinal velocity sampling, v tmin Indicates the lower speed limit, v tmax Indicates the upper speed limit; Based on the candidate set of lateral endpoints and the candidate set of longitudinal speeds, quintic polynomials are used for lateral planning and quartic polynomials are used for longitudinal planning. The generated lateral and longitudinal trajectories are combined to obtain a candidate set of finely planned trajectories. Taking the maximum speed, maximum acceleration, maximum curvature and maximum tolerable risk of the vehicle as constraints, the candidate set of fine-planned trajectories is screened, and the trajectories that do not meet the constraints are eliminated to obtain a new fine-planned trajectory set P. i = {P i1 ,P i2 ,...,P im }, P i is the i-th new refined planning trajectory, P im represents the mth point on the path sample. If none of them meet the requirements, it returns to the rough planning stage and selects the rough planning trajectory with the second smallest cost to re-establish the safety corridor for fine planning; The trajectory risk value, trajectory driving stability cost and comfort cost are used as evaluation criteria, and the total cost Cost t Cost t = l r Cost r +l s Cost s +l c Cost c , Cost r represents the trajectory risk cost, l r The weight of the trajectory risk cost, Cost s represents the trajectory stability cost, l s The weight of the trajectory driving stability cost, Cost c represents the comfort cost, l c Represents the weight of the comfort cost, from the new fine planning trajectory set P i = {P i1 ,P i2 ,...,P im } selects the trajectory with the smallest cost as the optimal trajectory.

7. The hierarchical local path planning method based on cognitive risk model according to claim 6, characterized in that: The calculation method of the trajectory risk cost is: Based on the environmental characteristic risk field with driver cognition, the risk value R of each path in the new fine planning trajectory set is calculated. path (P i ), calculate the uncertainty of path risk based on the risk value of all path samples: Where R mean is the average risk value of all paths, n is the number of path samples, and the risk value of each path is updated to obtain the trajectory risk cost: Cost r represents the trajectory risk cost.

8. The hierarchical local path planning method based on cognitive risk model according to claim 6, characterized in that: The calculation method of the trajectory driving stability cost is: Establish the stability cost circle of yaw rate w and center of mass sideslip angle b. The specific formula is: Among them, Cost s represents the trajectory stability cost, C s is the stability cost coefficient, p and q are the weight coefficients of the center of mass slip angle b and the yaw rate w, b0 and w0 are the center of mass slip angle and yaw rate values ​​in the steady state calculated by the linear two-degree-of-freedom model, and according to the planned vehicle longitudinal speed v x , lateral acceleration a y , calculate the sideslip angle b of the center of mass, and the yaw rate w is the vehicle heading angle The derivative of .

Images