A hierarchical local path planning method based on cognitive risk model

Abstract

The present application discloses a kind of hierarchical local path planning methods based on cognitive risk model to make up the problem that traditional planning method ignores driver's cognition and trajectory planning real-time, it is related to the technical field of trajectory planning of automatic driving car, including the following steps: collecting environmental information and the state information of vehicle itself, according to environmental information and the state information of vehicle itself, establish risk cognitive grid map;According to risk cognitive grid map, preliminary planning local path is obtained Trajectory planning coarse solution discrete point;According to trajectory planning coarse solution discrete point, establish safe driving corridor;According to the fine planning of trajectory in safe driving corridor, obtain optimal trajectory.This method can not only identify and avoid high-risk area, but also optimize vehicle driving trajectory through safe corridor, ensure driving safety and efficiency.

Description

Technical Field

[0001] This invention relates to the field of trajectory planning technology for autonomous vehicles, specifically to a hierarchical local path planning method based on a cognitive risk model. Background Technology

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

[0003] While traditional trajectory planning methods can generate driving paths, they often overlook the combined effects of driver cognitive characteristics and environmental risks. This may lead to safety hazards or unreasonableness in real-world scenarios, or excessive constraints may affect the real-time performance of the planning, resulting in safety accidents.

[0004] Most current trajectory planning methods employ global planning to directly generate accurate paths. Global planning typically requires processing a large amount of environmental information, such as obstacle distribution, road boundaries, and lane information. Due to its high computational complexity, global planning struggles to update within a limited timeframe, resulting in insufficient real-time performance and an inability to handle unexpected situations. Furthermore, global planning usually focuses on macroscopic paths, prioritizing overall optimality while neglecting local, detailed optimization. Especially in narrow or obstacle-rich areas, global planning may generate paths that are not practically feasible, increasing the difficulty of vehicle trajectory execution. Summary of the Invention

[0005] To address the shortcomings of traditional planning methods that neglect driver cognition and real-time trajectory planning, this invention provides a hierarchical local path planning method based on a cognitive risk model. This method combines driver cognitive risk with the environmental risk field, while also considering vehicle stability risk, constructing a risk map and performing trajectory planning. This approach not only identifies and avoids high-risk areas but also optimizes vehicle trajectories through safe corridors, ensuring both driving safety and efficiency. It guarantees real-time performance and safety in complex traffic environments. This invention has broad application prospects in intelligent driving, autonomous vehicles, and driver assistance systems, providing crucial support for the future widespread adoption of autonomous driving technology.

[0006] To achieve the above objectives, the present invention adopts the following technical solution:

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

[0008] Collect environmental information and vehicle status information, and establish a risk perception grid map based on the environmental information and vehicle status information;

[0009] Based on the risk perception grid map, a preliminary local path is planned, and the discrete points of the coarse solution for trajectory planning are obtained.

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

[0011] The optimal trajectory is obtained by finely planning the trajectory based on the safe driving corridor.

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

[0013] Furthermore, the environmental information includes lane line positions, road boundaries, reference path information, and obstacle information, wherein the obstacle information includes the location of the obstacle's center (x). obs ,y obs The speed v of the obstacle obs The heading angle of the obstacle and the dimensions of the obstacle, including the longitudinal external dimension s of the obstacle. x and the lateral dimensions of the obstacle s y The vehicle's own state information includes its position (x, y) and speed v. host acceleration a host and heading angle

[0014] Furthermore, the establishment of the risk perception grid map based on environmental information and the vehicle's own status information specifically involves:

[0015] Determine the resolution of the risk perception grid map and establish a grid map with dimensions X and X'. g and Y g A rectangular gridded map;

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

[0017] Calculate the driver's cognitive risk, environmental risk, and obstacle risk for each point on the rectangular gridded map;

[0018] Based on the vehicle's speed, position, and steering angle information, a driver's cognitive risk field is constructed; driver's cognitive risk E d for:

[0019]

[0020] In the formula, p, s, t la These represent the risk field section length coefficient, arc length, and aiming time, respectively; L is the vehicle wheelbase; d is the steering angle; s is the risk field section width; and x is the ordinate of a point on the risk grid map. hostThe y-coordinate is the current vertical coordinate of the main vehicle, and y is the horizontal coordinate of a point on the risk grid map. host The current x-coordinate of the main vehicle;

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

[0022]

[0023] Among them, lane risk Lane risk parameters, lane w y represents the lane width, and y represents the x-coordinate of a point on the risk grid map.

[0024] Lane boundary risk E bound for:

[0025]

[0026] Among them, Bound risk Here, k is the lane boundary risk parameter, k is the boundary risk adjustment coefficient, and D is the lateral parameter;

[0027] For obstacle risk E obs static obstacle risk E obs_s and dynamic obstacle risk E obs_m composition,

[0028] E obs =E obs_s +E obs_m

[0029] Static obstacle risk E obs_s Taking into account the distance to the obstacle, its position near the lane line, and its shape and dimensions, the calculation formula is as follows:

[0030]

[0031] Where r is the obstacle field strength parameter, s x ,s y These are the longitudinal and lateral dimensions of the obstacle, x and x respectively. obs Let y be the vertical coordinate of the obstacle's center of mass. obs Let x be the x-coordinate of the obstacle's center of gravity, y be the y-coordinate of a point on the risk grid map, and b be the lane interference factor, which adjusts the impact of lane width on risk. lane Let lane be the x-coordinate of the lane line. w Lane width;

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

[0033]

[0034] Where Z is a constant, s y Let x' be the lateral dimension of the obstacle, y' be the ordinate of the obstacle considering the heading angle of the vehicle, and y' be the abscissa of the obstacle considering the heading angle of the vehicle. The specific formula is as follows:

[0035]

[0036] Where q' is the relative heading angle between the vehicle and the obstacle, calculated using the formula: α is the weighting factor for the impact of speed on risk;

[0037] Add the environmental risk to the obstacle risk, and then multiply it by the driver's perceived risk to obtain the environmental characteristic risk E with the driver's perception. t The specific formula is as follows:

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

[0039] Among them, E env For environmental risks, E obs For obstacle risk, E d To help drivers understand the risks;

[0040] Each point on the rectangular grid map has a corresponding environmental risk value that is perceived by the driver, and together they form a risk perception grid map.

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

[0042] Given a planning time, preliminary planning is performed for both lateral and longitudinal directions. Lateral planning, based on the current lane boundaries and using dl_c as the lateral sampling distance, generates the lateral planning endpoint: d. end1 ,d end2 ,...,d endi d endi For the i-th lateral planning endpoint, satisfying: d endi ∈[d min ,d max ];d min d is the lower limit of the lane boundary. max This refers to the upper limit of the lane boundary;

[0043] For longitudinal programming, given an acceleration a xAcceleration and deceleration are sampled as longitudinal velocity, and combined with the current vehicle speed, to form the planned endpoint state v of the longitudinal velocity. end =[v host -a x T c ,v host ,v host +a x T c ]

[0044] v host Given the current vehicle speed, a x T is the acceleration constant used for longitudinal coarse programming. c This is a rough timeframe;

[0045] Based on the fifth-degree polynomial d(t) = b0 + b1t + b2t 2 +b3t 3 +b4t 4 +b5t 5 , with t=0:dt:T c And the endpoint d of the lateral planning endi Generate a horizontal trajectory, where t is the time series, and b0, b1, b2, b3, b4, and b5 are coefficients to be determined, which are obtained based on the boundary conditions;

[0046] Based on the quartic polynomial s(t) = a0 + a1t + a2t 2 +a3t 3 +a4t 4 A longitudinal trajectory is generated, where a0, a1, a2, a3, and a4 are coefficients to be determined. The coefficients of the fourth-order polynomial are determined through boundary conditions. Based on the time series, the generated longitudinal trajectory is combined with the lateral trajectory to form a candidate trajectory cluster.

[0047] Using comfort and risk values ​​as selection criteria for preliminary planning, the total cost of preliminary planning is... cs Cost cs =l r Cost r +l c Cost c Cost r Cost c These represent the risk cost and comfort cost of the trajectory, respectively. r and l c The risk cost weight and comfort cost weight of the trajectory are respectively used to select the total cost from the candidate trajectory cluster. cs The minimum trajectory is the discrete point of the coarse solution for trajectory planning.

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

[0049] Determine the risk value E for each point on the risk perception grid map. t Does it exceed the risk threshold? th Points not exceeding the risk threshold are grouped into a safe drivable area F, and then the discrete points of the coarse solution of the trajectory planning are mapped into a binary set. Represents a gridded map;

[0050] In the risk perception grid map, A represents the discrete point set of the coarse solution for trajectory planning; a morphological structuring 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 as a rectangle with the same aspect ratio as the vehicle. Morphological expansion is performed based on the mapped binary set A and the established structuring element S, with the specific formula as follows:

[0051]

[0052] Among them, (S) z ={z+s|s∈S} represents the point in A that is translated from the morphological structural element S. The dilation operation is represented by z, the target position of the structuring element translation is represented by z, and s is an element in the morphological structuring element S, representing a relative position in the structuring element.

[0053] After morphological dilation, a morphologically dilated region M is obtained, which is generated by the coarse solution of trajectory planning. Taking the intersection of the safe driving region and the morphologically dilated region, the safe driving corridor is obtained:

[0054] C o =M∩F

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

[0056] Furthermore, the specific process of finely planning the trajectory based on the safe driving corridor to obtain the optimal trajectory is as follows:

[0057] Extract safe driving corridor C o boundary x b ,y b x b Let y be the ordinate of all boundary points. b The x-coordinates of all boundary points are included;

[0058] Calculate the execution time T of the fine-grained programming, using the vertical boundary limit x. bmax With the endpoint as the starting point, accelerate from the vehicle's current position at the x-coordinate. host To the longitudinal boundary limit x bmax The required time is taken as the execution time for completing the detailed plan, and the formula is: Solving this equation yields the execution time for the fine-grained planning to be completed, a. x Represents constant acceleration, v host Indicates the current vehicle speed;

[0059] For the horizontal direction, the horizontal boundary of the safety corridor is chosen as the upper and lower limits y. bmin ,y bmax Lateral velocity, lateral acceleration, and lateral jerk are all set to 0. The candidate set for the lateral endpoint is: [y bmin ,y bmin +Dd,...,y bmax ]; Dd represents the lateral sampling distance;

[0060] For the longitudinal direction, the lower limit of velocity is calculated using the execution time T and constant acceleration: v tmin =v host -a x T, the candidate set of longitudinal velocities is [v tmin ,v tmin +Dv,...,v tmax It is necessary to ensure that the longitudinal velocity set is within [0, v]. max Within the brackets, Dv represents longitudinal velocity sampling, and v tmin Indicates the lower limit of vehicle speed, v tmax Indicates the maximum speed limit;

[0061] Based on the candidate sets of lateral endpoints and longitudinal velocity candidates, lateral planning is performed using a fifth-order polynomial, and longitudinal planning is performed using a fourth-order polynomial. The generated lateral and longitudinal trajectories are combined to obtain a refined set of trajectory candidates.

[0062] Using the vehicle's maximum speed, maximum acceleration, maximum curvature, and highest tolerable risk as constraints, the candidate trajectory set for fine-planning is filtered, and trajectories that do not meet the constraints are eliminated, resulting in a new fine-planning trajectory set P. i ={P i1 ,P i2 ,...,P im}, P i For the i-th new fine-planned trajectory, P im This represents the m-th point on the path sample. If none of the requirements are met, we return to the coarse planning stage, select the coarse planning trajectory with the second smallest cost, and re-establish the safe corridor for fine planning.

[0063] Using trajectory risk value, trajectory driving stability cost, and comfort cost as evaluation criteria, the total cost is... t Cost t =l r Cost r +l s Costs +l c Cost c Cost r Indicates the cost of trajectory risk, l r Cost represents the weight of the trajectory risk cost. s The value of l represents the cost of trajectory driving stability. s Cost represents the weight of the trajectory driving stability cost. c Indicating the cost of comfort, l c The weights representing the comfort costs are derived from the new fine-planning trajectory set P. i ={P i1 ,P i2 ,...,P im The trajectory with the lowest cost is selected as the optimal trajectory.

[0064] Furthermore, the method for calculating the trajectory risk cost is as follows:

[0065] Based on the environmental risk field with driver cognition, the risk value R of each path in the new fine-planning trajectory set is calculated. path (P i The uncertainty of path risk is calculated based on the risk values ​​of all path samples. Where R mean Given the average risk value for all paths, and n as the number of path samples, updating the risk value for each path yields the trajectory risk cost: Cost in the formula r This indicates the cost of trajectory risk.

[0066] Furthermore, the method for calculating the trajectory driving stability cost is as follows:

[0067] Establish the stability cost circle for yaw rate w and centroid sideslip angle b, using the following formula:

[0068]

[0069] Cost s C represents the cost of trajectory driving stability. s The stability cost coefficients are p and q, which are the weighting coefficients for the sideslip angle b and yaw rate w, respectively. b0 and w0 are the steady-state sideslip angle and yaw rate values ​​calculated using a linear two-degree-of-freedom model, based on the planned longitudinal velocity v of the vehicle. x lateral acceleration a y Calculate the sideslip angle b of the center of gravity, and the yaw rate w is the vehicle's heading angle. The derivative of .

[0070] The beneficial effects of this invention are:

[0071] This invention not only identifies and avoids high-risk areas but also optimizes vehicle trajectories through safe corridors, ensuring driving safety and efficiency. It guarantees the real-time performance and safety of trajectory planning even in complex traffic environments. This invention has broad application prospects in intelligent driving, autonomous vehicles, and driver assistance systems, providing crucial support for the widespread adoption of future autonomous driving technology. Attached Figure Description

[0072] Figure 1 This is a flowchart of the hierarchical local path planning method based on a cognitive risk model proposed in this invention. Detailed Implementation

[0073] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of this application.

[0074] Example 1

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

[0076] The vehicle collects environmental information and its own status information through onboard sensors. Environmental information includes lane line positions, road boundaries, reference path information, and obstacle information, including the location (x) of the obstacle's center. obs ,y obs The speed v of the obstacle obs The heading angle of the obstacle and the dimensions of the obstacle, including the longitudinal external dimension s of the obstacle. x and the lateral dimensions of the obstacle s y The vehicle's own state information includes its position (x, y) and speed v. host acceleration a host and heading angle

[0077] A risk perception grid map is established based on environmental information and the vehicle's own status information; specifically:

[0078] Determine the resolution of the risk perception grid map and establish a grid map with dimensions X and X'. g and Y g A rectangular gridded map;

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

[0080] Calculate the driver's cognitive risk, environmental risk, and obstacle risk for each point on the rectangular gridded map;

[0081] Based on the vehicle's speed, position, and steering angle information, a driver's cognitive risk field is constructed; driver's cognitive risk E d for:

[0082]

[0083] In the formula, p, s, t la These represent the risk field section length coefficient, arc length, and aiming time, respectively; L is the vehicle wheelbase; d is the steering angle; s is the risk field section width; and x is the ordinate of a point on the risk grid map. host The y-coordinate is the current vertical coordinate of the main vehicle, and y is the horizontal coordinate of a point on the risk grid map. host The current x-coordinate of the main vehicle;

[0084] Environmental Risk E env Considering lane line risk and lane boundary risk, the lane line risk is E. lane :

[0085]

[0086] Among them, lane risk Lane risk parameters, lane w y represents the lane width, and y represents the x-coordinate of a point on the risk grid map.

[0087] Lane boundary risk E bound for:

[0088]

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

[0090] For obstacle risk E obs static obstacle risk E obs_s and dynamic obstacle risk E obs_m composition,

[0091] E obs =E obs_s +E obs_m

[0092] Static obstacle risk E obs_sTaking into account the distance to the obstacle, its position near the lane line, and its shape and dimensions, the calculation formula is as follows:

[0093]

[0094] Where r is the obstacle field strength parameter, s x ,s y These are the longitudinal and lateral dimensions of the obstacle, x and x respectively. obs Let y be the vertical coordinate of the obstacle's center of mass. obs Let x be the x-coordinate of the obstacle's center of gravity, y be the y-coordinate of a point on the risk grid map, and b be the lane interference factor, which adjusts the impact of lane width on risk. lane Let lane be the x-coordinate of the lane line. w Lane width;

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

[0096]

[0097] Where Z is a constant, s y Let x' be the lateral dimension of the obstacle, y' be the ordinate of the obstacle considering the heading angle of the vehicle, and y' be the abscissa of the obstacle considering the heading angle of the vehicle. The specific formula is as follows:

[0098]

[0099] Where q' is the relative heading angle between the vehicle and the obstacle, calculated using the formula: α is the weighting factor for the impact of speed on risk;

[0100] Add the environmental risk to the obstacle risk, and then multiply it by the driver's perceived risk to obtain the environmental characteristic risk E with the driver's perception. t The specific formula is as follows:

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

[0102] Among them, E env For environmental risks, E obs For obstacle risk, E d To help drivers understand the risks;

[0103] Each point on the rectangular grid map has a corresponding environmental risk value that is perceived by the driver, and together they form a risk perception grid map.

[0104] Based on the risk perception grid map, a preliminary local path is planned, resulting in a coarse solution for the trajectory planning discrete points; the specific process is as follows:

[0105] Given a planning time, preliminary planning is performed for both lateral and longitudinal directions. Lateral planning, based on the current lane boundaries and using dl_c as the lateral sampling distance, generates the lateral planning endpoint: d. end1 ,d end2 ,...,d endi d endi For the i-th lateral planning endpoint, satisfying: d endi ∈[d min ,d max ];d min d is the lower limit of the lane boundary. max This refers to the upper limit of the lane boundary;

[0106] For longitudinal programming, given an acceleration a x Acceleration and deceleration are sampled as longitudinal velocity, and combined with the current vehicle speed, to form the planned endpoint state v of the longitudinal velocity. end =[v host -a x T c ,v host ,v host +a x T c ]

[0107] v host Given the current vehicle speed, a x T is the acceleration constant used for longitudinal coarse programming. c This is a rough timeframe;

[0108] Based on the fifth-degree polynomial d(t) = b0 + b1t + b2t 2 +b3t 3 +b4t 4 +b5t 5 , with t=0:dt:T c And the endpoint d of the lateral planning endi Generate a horizontal trajectory, where t is the time series, and b0, b1, b2, b3, b4, and b5 are coefficients to be determined, which are obtained based on the boundary conditions;

[0109] Based on the quartic polynomial s(t) = a0 + a1t + a2t 2 +a3t 3 +a4t 4 A longitudinal trajectory is generated, where a0, a1, a2, a3, and a4 are coefficients to be determined. The coefficients of the fourth-order polynomial are determined through boundary conditions. Based on the time series, the generated longitudinal trajectory is combined with the lateral trajectory to form a candidate trajectory cluster.

[0110] Using comfort and risk values ​​as selection criteria for preliminary planning, the total cost of preliminary planning is... cs Cost cs =l r Cost r +l c Cost c Cost r Cost c These represent the risk cost and comfort cost of the trajectory, respectively. r and l c The risk cost weight and comfort cost weight of the trajectory are respectively used to select the total cost from the candidate trajectory cluster. cs The minimum trajectory is the discrete point of the coarse solution for trajectory planning.

[0111] A safe driving corridor is established based on the coarse discrete points obtained from trajectory planning; the specific process is as follows:

[0112] Determine the risk value E for each point on the risk perception grid map. t Does it exceed the risk threshold? th Points not exceeding the risk threshold are grouped into a safe drivable area F, and then the discrete points of the coarse solution of the trajectory planning are mapped into a binary set. Represents a gridded map;

[0113] In the risk perception grid map, A represents the discrete point set of the coarse solution for trajectory planning; a morphological structuring 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 as a rectangle with the same aspect ratio as the vehicle. Morphological expansion is performed based on the mapped binary set A and the established structuring element S, with the specific formula as follows:

[0114]

[0115] Among them, (S) z ={z+s|s∈S} represents the point in A that is translated from the morphological structural element S. The dilation operation is represented by z, the target position of the structuring element translation is represented by z, and s is an element in the morphological structuring element S, representing a relative position in the structuring element.

[0116] After morphological dilation, a morphologically dilated region M is obtained, which is generated by the coarse solution of trajectory planning. Taking the intersection of the safe driving region and the morphologically dilated region, the safe driving corridor is obtained:

[0117] C o =M∩F

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

[0119] The optimal trajectory is obtained through fine-grained trajectory planning based on the safe driving corridor. The specific process is as follows:

[0120] Extract safe driving corridor C o boundary x b ,y b x b Let y be the ordinate of all boundary points. b The x-coordinates of all boundary points are included;

[0121] Calculate the execution time T of the fine-grained programming, using the vertical boundary limit x. bmax With the endpoint as the starting point, accelerate from the vehicle's current position at the x-coordinate. host To the longitudinal boundary limit x bmax The required time is taken as the execution time for completing the detailed plan, and the formula is: Solving this equation yields the execution time for the fine-grained planning to be completed, a. x Represents constant acceleration, v host Indicates the current vehicle speed;

[0122] For the horizontal direction, the horizontal boundary of the safety corridor is chosen as the upper and lower limits y. bmin ,y bmax Lateral velocity, lateral acceleration, and lateral jerk are all set to 0. The candidate set for the lateral endpoint is: [y bmin ,y bmin +Dd,...,y bmax ]; Dd represents the lateral sampling distance;

[0123] For the longitudinal direction, the lower limit of velocity is calculated using the execution time T and constant acceleration: v tmin =v host -a x T, the candidate set of longitudinal velocities is [v tmin ,v tmin +Dv,...,v tmax It is necessary to ensure that the longitudinal velocity set is within [0, v]. max Within the brackets, Dv represents longitudinal velocity sampling, and v tmin Indicates the lower limit of vehicle speed, v tmax Indicates the maximum speed limit;

[0124] Based on the candidate sets of lateral endpoints and longitudinal velocity candidates, similar to coarse planning, lateral planning uses a fifth-order polynomial and longitudinal planning uses a fourth-order polynomial. The generated lateral and longitudinal trajectories are combined to obtain the candidate set of trajectory for fine planning.

[0125] Using the vehicle's maximum speed, maximum acceleration, maximum curvature, and highest tolerable risk as constraints, the candidate trajectory set for fine-planning is filtered, and trajectories that do not meet the constraints are eliminated, resulting in a new fine-planning trajectory set P. i ={P i1 ,P i2 ,...,P im}, P i For the i-th new fine-planned trajectory, P im This represents the m-th point on the path sample. If none of the requirements are met, we return to the coarse planning stage, select the coarse planning trajectory with the second smallest cost, and re-establish the safe corridor for fine planning.

[0126] Using trajectory risk value, trajectory driving stability cost, and comfort cost as evaluation criteria, the total cost is... t Cost t =l r Cost r +l s Cost s +l c Cost c Cost r Indicates the cost of trajectory risk, l r Cost represents the weight of the trajectory risk cost. s The value of l represents the cost of trajectory driving stability. s Cost represents the weight of the trajectory driving stability cost. c Indicating the cost of comfort, l c The weights representing the comfort costs are derived from the new fine-planning trajectory set P. i ={P i1 ,P i2 ,...,P im The trajectory with the lowest cost is selected as the optimal trajectory.

[0127] Calculate the trajectory risk value (Cost) r At the same time, considering the uncertainties of sensor errors, road condition changes, and the vehicle's own state, the reliability of path planning is enhanced by simulating path uncertainties. The specific method is as follows:

[0128] Based on the environmental risk field with driver cognition, the risk value R of each path in the new fine-planning trajectory set is calculated. path (P i The uncertainty of path risk is calculated based on the risk values ​​of all path samples. Where R mean Given the average risk value for all paths, and n as the number of path samples, updating the risk value for each path yields the trajectory risk cost: Cost in the formula r This indicates the cost of trajectory risk.

[0129] The method for calculating the trajectory driving stability cost is as follows:

[0130] Establish the stability cost circle for yaw rate w and centroid sideslip angle b, using the following formula:

[0131]

[0132] Cost s C represents the cost of trajectory driving stability. s The stability cost coefficients are p and q, which are the weighting coefficients for the sideslip angle b and yaw rate w, respectively. b0 and w0 are the steady-state sideslip angle and yaw rate values ​​calculated using a linear two-degree-of-freedom model, based on the planned longitudinal velocity v of the vehicle. x lateral acceleration a y Calculate the sideslip angle b of the center of gravity, and the yaw rate w is the vehicle's heading angle. The derivative of .

[0133] Example 2

[0134] This invention proposes a hierarchical local path planning system based on a cognitive risk model, corresponding to the method in Embodiment 1, comprising:

[0135] The data acquisition module is used to collect environmental information and the vehicle's own status information;

[0136] The risk perception grid map creation module is used to create a risk perception grid map based on environmental information and the vehicle's own status information.

[0137] The preliminary planning module is used to plan local paths based on the risk perception grid map and obtain the coarse solution discrete points of the trajectory planning.

[0138] The safe driving corridor establishment module is used to establish a safe driving corridor based on the coarse solution of discrete points in the trajectory planning.

[0139] The fine planning module is used to perform fine-grained trajectory planning based on the safe driving corridor to obtain the optimal trajectory.

[0140] The implementation methods of each module and its function in the system are completely consistent with the steps of the method in Implementation Example 1, so they will not be repeated here.

[0141] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed in this application can be implemented in electronic hardware or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0142] The above are merely preferred embodiments of the present invention. The scope of protection of the present invention is not limited to the above embodiments. All technical solutions falling within the scope of the present invention's concept are within the scope of protection of the present invention. It should be noted that for those skilled in the art, any improvements and modifications made without departing from the principle of the present invention should be considered within the scope of protection of the present invention.

Claims

1. A hierarchical local path planning method based on a cognitive risk model, characterized in that, Includes the following steps: Collect environmental information and vehicle status information, and establish a risk perception grid map based on the environmental information and vehicle status information; Based on the risk perception grid map, a preliminary local path is planned, and the discrete points of the coarse solution for trajectory planning are obtained. A safe driving corridor is established based on the coarse discrete points of trajectory planning; the specific process of establishing the safe driving corridor based on the coarse discrete points of trajectory planning is as follows: Determine the risk value E for each point on the risk perception grid map. t Does it exceed the risk threshold? th Points not exceeding the risk threshold are grouped into a safe drivable area F, and then the discrete points of the coarse solution of the trajectory planning are mapped into a binary set. Represents a gridded map; In the risk perception grid map, A represents the discrete point set of the coarse solution for trajectory planning; a morphological structuring 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 as a rectangle with the same aspect ratio as the vehicle. Morphological expansion is performed based on the mapped binary set A and the established structuring element S, with the specific formula as follows: Among them, (S) z ={z+s|s∈S} represents the point in A that is translated from the morphological structural element S. The dilation operation is represented by z, the target position of the structuring element translation is represented by z, and s is an element in the morphological structuring element S, representing a relative position in the structuring element. After morphological dilation, a morphologically dilated region M is obtained, which is generated by the coarse solution of trajectory planning. Taking the intersection of the safe driving region and the morphologically dilated region, the safe driving corridor is obtained: C o = M ∩ F; In the formula, C o for safe travel corridors; The optimal trajectory is obtained by finely planning the trajectory based on the safe driving corridor.

2. The hierarchical local path planning method based on a cognitive risk model as described in claim 1, characterized in that, The environmental information includes lane line positions, road boundaries, reference path information, and obstacle information. The obstacle information includes the location of the obstacle's center (x). obs ,y obs The speed v of the obstacle obs The heading angle of the obstacle and the dimensions of the obstacle, including the longitudinal external dimension σ of the obstacle. x And the lateral dimensions σ of the obstacle y The vehicle's own state information includes its position (x, y) and speed v. host acceleration a host and heading angle 3. The hierarchical local path planning method based on a cognitive risk model as described in claim 1, characterized in that, The specific steps for establishing a risk perception grid map based on environmental information and the vehicle's own status information are as follows: determining a resolution of the risk awareness grid map, establishing a rectangular grid map with length X and width Y g and g Y 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 gridded map; According to the speed, position and steering angle information of the vehicle, a driver cognitive risk field is constructed; the driver cognitive risk E d is: wherein 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, δ is the steering angle, σ is the risk field cross-section width, x is the longitudinal coordinate of a point on the risk grid map, x host is the current longitudinal coordinate of the host vehicle, y is the lateral coordinate of a point on the risk grid map, y host is the current lateral coordinate of the host vehicle; Environmental risk E env Considering the lane line risk and the lane boundary risk, the lane line risk is E lane : wherein lane risk is a lane line risk parameter, lane w is a lane line width, and y is a lateral coordinate of a point on the risk grid map; Lane boundary risk E bound for: Among them, Bound risk Here, k is the lane boundary risk parameter, k is the boundary risk adjustment coefficient, and Δ is the lateral parameter. For obstacle risk E obs 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 to the obstacle, its position near the lane line, and its shape and dimensions, the calculation formula is as follows: Where ρ is the obstacle field strength parameter, σ x ,σ y These are the longitudinal and lateral dimensions of the obstacle, x and x respectively. obs Let y be the vertical coordinate of the obstacle's center of mass. obs Let x be the x-coordinate of the obstacle's center of gravity, y be the y-coordinate of a point on the risk grid map, and β be the lane interference factor, which adjusts the impact of lane width on risk. lane Let lane be the x-coordinate of the lane line. w Lane width; Dynamic obstacle risk E obs_m The calculation formula is as follows: Where Z is a constant, σ y Let x' be the lateral dimension of the obstacle, y' be the ordinate of the obstacle considering the heading angle of the vehicle, and y' be the abscissa of the obstacle considering the heading angle of the vehicle. The specific formula is as follows: Where θ' is the relative heading angle between the vehicle and the obstacle, calculated using the formula: α is the weighting factor for the impact of speed on risk; It is the vehicle's heading angle, v obs It is the speed of the obstacle, v host It's the vehicle speed; Add the environmental risk to the obstacle risk, and then multiply it by the driver's perceived risk to obtain the environmental characteristic risk E with the driver's perception. t The specific formula is as follows: AND t =And d ·(AND env +E obs ); Among them, E env For environmental risks, E obs For obstacle risk, E d To help drivers understand the risks; Each point on the rectangular grid map has a corresponding environmental risk value that is perceived by the driver, and together they form a risk perception grid map.

4. The hierarchical local path planning method based on a cognitive risk model as described in claim 1, characterized in that, The specific process of initially planning a local path based on the risk perception grid map and obtaining the coarse solution discrete points for trajectory planning is as follows: Given a planning time, preliminary planning is performed for both lateral and longitudinal directions. Lateral planning, based on the current lane boundaries and using dl_c as the lateral sampling distance, generates the lateral planning endpoint: d. end1 ,d end2 ,...,d endi d endi For the i-th lateral planning endpoint, satisfying: d endi ∈[d min ,d max ];d min d is the lower limit of the lane boundary. max This refers to the upper limit of the lane boundary; For longitudinal programming, given an acceleration a x Acceleration and deceleration are sampled as longitudinal velocity, and combined with the current vehicle speed, to form the planned endpoint state v of the longitudinal velocity. end =[v host -a x T c ,v host ,v host +a x T c ]; v host Given the current vehicle speed, a x T is the acceleration constant used for longitudinal coarse programming. c This is a rough timeframe; Based on the fifth-degree polynomial d(t) = b0 + b1t + b2t 2 +b3t 3 +b4t 4 +b5t 5 , with t=0:dt:T c And the endpoint d of the lateral planning endi Generate a horizontal trajectory, where t is the time series, and b0, b1, b2, b3, b4, and b5 are coefficients to be determined, which are obtained based on the boundary conditions; Based on the quartic polynomial s(t) = a0 + a1t + a2t 2 +a3t 3 +a4t 4 A longitudinal trajectory is generated, where a0, a1, a2, a3, and a4 are coefficients to be determined. The coefficients of the fourth-order polynomial are determined through boundary conditions. Based on the time series, the generated longitudinal trajectory is combined with the lateral trajectory to form a candidate trajectory cluster. Using comfort and risk values ​​as selection criteria for preliminary planning, the total cost of preliminary planning is... cs Cost cs =λ r Cost r +λ c Cost c Cost r Cost c These represent the risk cost and comfort cost of the trajectory, respectively, λ. r and λ c The risk cost weight and comfort cost weight of the trajectory are respectively used to select the total cost from the candidate trajectory cluster. cs The minimum trajectory is the discrete point of the coarse solution for trajectory planning.

5. The hierarchical local path planning method based on a cognitive risk model as described in claim 1, characterized in that, The specific process of finely planning the trajectory based on the safe driving corridor to obtain the optimal trajectory is as follows: Extract safe driving corridor C o boundary x b ,y b x b Let y be the ordinate of all boundary points. b The x-coordinates of all boundary points are included; Calculate the execution time T of the fine-grained programming, using the vertical boundary limit x. bmax With the endpoint as the starting point, accelerate from the vehicle's current position at the x-coordinate. host To the longitudinal boundary limit x bmax The required time is taken as the execution time for completing the detailed plan, and the formula is: Solving this equation yields the execution time for the fine-grained planning to be completed, a. x Represents constant acceleration, v host Indicates the current vehicle speed; For the horizontal direction, the horizontal boundary of the safety corridor is chosen as the upper and lower limits y. bmin ,y bmax Lateral velocity, lateral acceleration, and lateral jerk are all set to 0. The candidate set for the lateral endpoint is: [y bmin ,y bmin +Δd,...,y bmax ]; Δd represents the lateral sampling distance; For the longitudinal direction, the lower limit of velocity is calculated using the execution time T and constant acceleration: v tmin =v host -a x T, the candidate set of longitudinal velocities is [v tmin ,v tmin +Δv,...,v tmax It is necessary to ensure that the longitudinal velocity set is within [0, v]. max Within ], Δv represents the longitudinal velocity sampling, v tmin Indicates the lower limit of vehicle speed, v tmax Indicates the maximum speed limit; Based on the candidate sets of lateral endpoints and longitudinal velocity candidates, lateral planning is performed using a fifth-order polynomial, and longitudinal planning is performed using a fourth-order polynomial. The generated lateral and longitudinal trajectories are combined to obtain a refined set of trajectory candidates. Using the vehicle's maximum speed, maximum acceleration, maximum curvature, and highest tolerable risk as constraints, the candidate trajectory set for fine-planning is filtered, and trajectories that do not meet the constraints are eliminated, resulting in a new fine-planning trajectory set P. i ={P i1 ,P i2 ,...,P im }, P i For the i-th new fine-planned trajectory, P im This represents the m-th point on the path sample. If none of the requirements are met, we return to the coarse planning stage, select the coarse planning trajectory with the second smallest cost, and re-establish the safe corridor for fine planning. Using trajectory risk value, trajectory driving stability cost, and comfort cost as evaluation criteria, the total cost is... t Cost t =λ r Cost r +λ s Cost s +λ c Cost c Cost r λ represents the cost of trajectory risk. r Cost represents the weight of the trajectory risk cost. s λ represents the cost of trajectory driving stability. s Cost represents the weight of the trajectory driving stability cost. c λ represents the cost of comfort. c The weights representing the comfort costs are derived from the new fine-planning trajectory set P. i ={P i1 ,P i2 ,...,P im The trajectory with the lowest cost is selected as the optimal trajectory.

6. The hierarchical local path planning method based on a cognitive risk model as described in claim 5, characterized in that, The method for calculating the trajectory risk cost is as follows: Based on the environmental risk field with driver cognition, the risk value R of each path in the new fine-planning trajectory set is calculated. path (P i The uncertainty of path risk is calculated based on the risk values ​​of all path samples. Where R mean Given the average risk value for all paths, and n as the number of path samples, updating the risk value for each path yields the trajectory risk cost: Cost in the formula r This indicates the cost of trajectory risk.

7. The hierarchical local path planning method based on a cognitive risk model as described in claim 5, characterized in that, The method for calculating the trajectory driving stability cost is as follows: Establish the stability cost circle for yaw rate ω and centroid sideslip angle β, using the following formula: Cost s C represents the cost of trajectory driving stability. s The stability cost coefficients are p and q, which are the weighting coefficients for the sideslip angle β and yaw rate ω, respectively. β0 and ω0 are the steady-state sideslip angle and yaw rate values ​​calculated using a linear two-degree-of-freedom model, based on the planned longitudinal velocity v of the vehicle. x lateral acceleration a y Calculate the sideslip angle β of the center of mass, and the yaw rate ω is the vehicle's heading angle. The derivative of .

Images