Intelligent automobile double-layer path planning method considering tire road adhesion limit

Abstract

The invention discloses an intelligent automobile double-layer path planning method considering a tire road adhesion limit. The method comprises the following steps: 1) acquiring prior map information, vehicle sensing information and a road adhesion coefficient; 2) constructing a path planner; 3) an upper layer pre-planner plans a pre-planned path based on an improved fast search random tree algorithm; 4) the lower layer re-planner establishes a non-linear prediction model according to the pre-planned path planned by the upper layer pre-planner, and solves the non-linear prediction model by adopting a non-linear model prediction control algorithm to obtain a re-planned path; and 5) transmitting the re-planned path to a path tracking controller as a reference path, transmitting the front wheel turning angle of the vehicle as feedback information to the vehicle by the path tracking controller, controlling the vehicle to run, and returning to the step 1) until the vehicle arrives at the destination. According to the method, the obstacle can be successfully avoided, the transverse offset is minimum on the premise that it is ensured that the vehicle does not collide with the obstacle vehicle, and it is ensured that the vehicle rapidly returns to the original driving lane.

Description

Technical Field

[0001] The invention relates to the field of path planning, and in particular to a double-layer path planning method for an intelligent vehicle taking tire-road adhesion limits into consideration. Background Art

[0002] Smart cars, also known as driverless cars or self-driving cars, refer to cars that are controlled by computers to perceive the environment, make autonomous decisions, plan and control motion without human intervention. In recent years, smart cars have attracted widespread attention from research institutions, automobile companies and other sectors of society due to their potential advantages in alleviating traffic congestion, improving traffic efficiency and driving safety. Path planning, as one of the core technologies of smart cars, aims to determine a geometric path from an initial position to a given target position, ensuring that each position and state on the path can meet the requirements of no collision and comply with a series of motion constraints (such as road and lane boundaries) and traffic rules, and finally pass the reference path to the control execution layer.

[0003] However, in obstacle avoidance scenarios, the safety and stability of intelligent vehicle path planning face higher requirements. Although the existing path planning method has the obstacle avoidance function, the search efficiency is low, the planned path is steep, and the obstacle avoidance effect is not ideal. Therefore, it is necessary to design a path planning method that comprehensively considers the geometric characteristics of obstacles and the interference between obstacles and can achieve the same ideal obstacle avoidance effect in different scenarios. Summary of the invention

[0004] The object of the present invention is to provide a dual-layer path planning method for an intelligent vehicle taking into account tire-road adhesion limits, comprising the following steps:

[0005] 1) Obtain prior map information, vehicle perception information, and road adhesion coefficient.

[0006] 2) Build a path planner.

[0007] The path planner includes an upper layer pre-planner and a lower layer re-planner.

[0008] 3) According to the prior map information, vehicle perception information and road adhesion coefficient, the upper-level pre-planner plans a pre-planned path based on the improved fast search random tree algorithm.

[0009] 4) The lower-level replanner establishes a nonlinear prediction model based on the pre-planned path planned by the upper-level pre-planner, and uses a nonlinear model predictive control algorithm to solve the nonlinear prediction model to obtain the re-planned path.

[0010] 5) The replanned path is passed to the path tracking controller as a reference path. The path tracking controller passes the front wheel steering angle of the vehicle as feedback information to the vehicle, controls the vehicle to travel, and returns to step 1) until the vehicle reaches the destination.

[0011] Furthermore, the vehicle perception information includes vehicle longitudinal speed, vehicle lateral acceleration, vehicle yaw angular velocity, vehicle heading angle, vehicle gross mass, vehicle yaw moment of inertia, distance from vehicle center of mass to front axle, distance from vehicle center of mass to rear axle, front wheel cornering stiffness, rear wheel cornering stiffness, vehicle length, vehicle width, obstacle vehicle length, obstacle vehicle width, and obstacle vehicle position.

[0012] Furthermore, the road adhesion coefficient is estimated by a road adhesion coefficient estimator.

[0013] Furthermore, the steps of the upper-level pre-planner planning a pre-planned path based on the improved fast search random tree algorithm according to the prior map information, vehicle perception information and road adhesion coefficient are as follows:

[0014] 3.1) Establish a random tree T = (V, E) and determine the target position q goal , and the vehicle's starting position q init Add it to the random tree as the root node, where V represents the node in the random tree and E represents the path between the nodes of the random tree.

[0015] 3.2) Randomly generate a coordinate point q in the map space rand as sampling point.

[0016] 3.3) Traverse the nodes stored in the random tree and find the distance sampling point q rand The nearest node is used as the parent node q parent .

[0017] 3.4) Adaptively adjust the expansion step size of the random tree to obtain the adaptive step size l step , as shown below:

[0018] l step =λ s v x (1)

[0019] In the formula, v x is the longitudinal velocity of the vehicle. s is the adaptive coefficient.

[0020] 3.5) Considering the angle constraint of the tire-road adhesion limit, we can get parent The sampling point q that satisfies the angle constraint samp .

[0021] 3.6) Based on the artificial potential field method, the sampling point q samp Make corrections and get the corrected new node q new .

[0022] 3.7) Check the parent node q parent and the new node q new Will the connection between them collide with the infeasible region? If not, the new node q new Add to the random tree T as a child node and store the parent node q parent If yes, go to step 3.8).

[0023] 3.8) Taking the geometric center of the obstacle in the infeasible region as the expansion center, the obstacle is expanded as follows:

[0024]

[0025] In the formula, l exp 、w exp are the length and width of the obstacle after expansion. obs 、w obs are the length and width of the obstacle respectively. veh 、w veh are the length and width of the vehicle respectively. safe 、w safe They are the longitudinal and lateral safety expansion dimensions of the obstacle respectively.

[0026] 3.9) Return to step 3.2) until the iteration end condition is met.

[0027] Furthermore, the angle constraint considering the tire-road adhesion limit is obtained at the parent node q parent The sampling point q that satisfies the angle constraint samp The steps are as follows:

[0028] 3.5.1) Based on the parent node q parent and path angle θ pre , randomly generate an expansion angle of a child node under the expansion angle constraint

[0029] 3.5.2) Along the extension angle Expand and obtain the sampling point q that satisfies the constraints samp .

[0030] The sampling point q samp The coordinates are as follows:

[0031]

[0032] In the formula, xparent ,y parent They are parent nodes q parent The horizontal and vertical coordinates of x samp ,y samp The sampling points q samp The horizontal and vertical coordinates. step is the adaptive step size.

[0033] Among them, the random sampling angle As shown below:

[0034]

[0035] Among them, the angle constraint limit value As shown below:

[0036]

[0037] Where g is the acceleration due to gravity. y,max is the maximum lateral acceleration of the vehicle. x is the vehicle longitudinal velocity.

[0038] μ is the dynamic friction coefficient.

[0039] Wherein, the time interval Δt is as follows:

[0040]

[0041] Furthermore, the artificial potential field method is used to calculate the sampling point q samp Make corrections and get the corrected new node q new The steps are as follows:

[0042] 3.6.1) Establish sampling point q samp For the parent node q parent The gravitational potential field function is as follows:

[0043] U samp (q parent ) = k samp (q samp -q parent ) (7)

[0044] Where U samp (q parent ) is the sampling point q samp For the parent node q parent gravitational potential field function. k samp is the sampling point q samp The gravitational weight coefficient.

[0045] According to the gradient descent method, the sampling point q samp Relative to the parent node qparent The gravitational force in the direction is as follows:

[0046]

[0047] In the formula, is the sampling point q samp Relative to the parent node q parent Direction of gravity. is the gradient function.

[0048] 3.6.2) Establish the target position q goal For the parent node q parent The gravitational potential field function is as follows:

[0049] U goal (q parent ) = k goal (q goal -q parent ) (9)

[0050] Where U goal (q parent ) is the target position q goal For the parent node q parent gravitational potential field function. k goal is the gravitational weight coefficient of the target point.

[0051] According to the gradient descent method, the target position q goal Relative to the parent node q parent The gravitational force in the direction is as follows:

[0052]

[0053] In the formula, is the target position q goal Relative to the parent node q parent Direction of gravity.

[0054] 3.6.3) Considering the geometric width of the vehicle, the repulsive potential field function of the road boundary is established as follows:

[0055]

[0056] Where U road (q parent ) is the repulsive potential field function of the road boundary. k road is the road boundary repulsion weight coefficient. l ,y r are the ordinates of the left and right road boundaries, respectively, and w veh is the vehicle width. parent The parent node q parent The vertical coordinate of .

[0057] The repulsion force for the road boundary is as follows:

[0058]

[0059] In the formula, is the repulsive force of the road boundary.

[0060] 3.6.4) Determine the sampling point q samp Is the resultant force at the node within the angle constraint range of the tire-road adhesion limit? If so, the direction of the resultant force is taken as the expansion direction of the new node. If not, the corresponding angle constraint limit value is taken as the expansion direction, and the parent node q parent is the root node, and a single step is extended in this direction to obtain a new node q new .

[0061] Furthermore, the iteration termination condition includes that the search path reaches the target position or reaches the maximum sampling number.

[0062] Further, the lower-layer replanner establishes a nonlinear prediction model according to the pre-planned path planned by the upper-layer pre-planner in the following steps:

[0063] 4.1) Establish the vehicle motion equation of the bicycle model as follows:

[0064]

[0065] Where m is the mass of the car. y is the lateral velocity of the vehicle. x is the longitudinal velocity of the vehicle. γ is the yaw rate of the vehicle. y,f 、F y,r I is the total lateral force of the front and rear wheels of the vehicle, respectively. z is the vehicle's yaw moment of inertia. f , l r are the distances from the front and rear axles to the center of mass. ψ is the vehicle heading angle. g , Y g are the longitudinal and lateral positions of the vehicle's center of mass in the global coordinate system, respectively. · is the first-order derivative function.

[0066] The lateral force is given by:

[0067]

[0068] In the formula, the subscript i=f, r represents the front wheel or the rear wheel. y,i is the tire cornering stiffness. z,i is the tire vertical force. μ is the dynamic friction coefficient.

[0069] Wherein, the tire slip angle αi As shown below:

[0070]

[0071] Where δ is the front wheel turning angle. f , α r Represent the sideslip angles of the front and rear wheels respectively.

[0072] 4.2) Based on the vehicle motion equation of the bicycle model, the nonlinear dynamic state space equation in path planning is established as follows:

[0073]

[0074] Where u is the control variable, and u = δ. x is the state variable, and x = [v y ,γ,ψ,X g ,Y g ] T . y is the output variable, and y=Y g . is the updated state variable.

[0075] Among them, the state transfer function f(·) and the observation function h(·) are as follows:

[0076]

[0077] In the formula, C f , C r are the cornering stiffness of the front and rear wheels respectively. x1, x2, x3, x5 are all components of the state variables. f1(x,u), f2(x,u), f3(x,u), f4(x,u), f5(x,u) are all components of the state transfer function.

[0078] 4.3) The state space is discretized using the forward Euler method to obtain the system prediction state and system prediction output at time k.

[0079] The system prediction state at time k is as follows:

[0080]

[0081] Where N p is the prediction time domain. c is the control time domain. T is the sampling period.

[0082] The system prediction output at time k is as follows:

[0083]

[0084] Furthermore, the constraints of the nonlinear prediction model include constraints on the control quantity in the state space, constraints on the vehicle lateral acceleration, and constraints on the lateral acceleration in the prediction time domain.

[0085] The constraints of the control quantity in the state space are as follows:

[0086] U min ≤U(k)≤U max (twenty one)

[0087] Where U(k) is the control quantity at time k, U(k)=[u(k+1|k),u(k+2|k),…,u(k+N c |k)] T , U min , U max are the minimum and maximum values ​​of the control quantity, respectively, and δ min , δ max are the minimum and maximum constraints of the front wheel angle respectively.

[0088] The vehicle lateral acceleration constraint conditions are as follows:

[0089]

[0090] Where g is the acceleration due to gravity. y is the lateral acceleration of the vehicle.

[0091] The constraints of the lateral acceleration in the prediction time domain are as follows:

[0092]

[0093] In the formula, Γ max is the limit value of lateral acceleration in the prediction time domain. Γ min =-Γ max X(k) is the predicted state of the system at time k, X(k) = [x(k+1|k), x(k+2|k),…, x(k+N p |k)] T .

[0094] Among them, the state parameter Ψ and the control parameter Θ are as follows:

[0095]

[0096]

[0097] Among them, C and D are intermediate parameters, and

[0098] Furthermore, when the lower-level replanner solves the nonlinear prediction model, the path replanning is converted into the solution of a nonlinear optimization problem, and the cost function of the nonlinear optimization problem is as follows:

[0099]

[0100] Where J is the cost function. i is the time sequence number, N p is the prediction time domain. c Y is the control time domain. ref (k+i|k) is the lateral reference position in the prediction time domain, Q and R are the output matrix and control weight matrix respectively. y(k+i|k) is the system prediction output at time k. u(k+i|k) is the control amount at time k.

[0101] The nonlinear optimization problem is as follows:

[0102]

[0103] The technical effect of the present invention is unquestionable. The present invention can successfully avoid obstacles, and the lateral offset is minimized while ensuring that there is no collision with the obstacle vehicle. Therefore, the present invention can ensure that the vehicle quickly returns to the original driving lane.

[0104] The cost function in the lower-level path replanner of the present invention not only takes into account the lateral deviation of the path, but also includes the front wheel turning angle. Therefore, the lower-level replanning can be further optimized based on the upper-level pre-planned path, thereby obtaining a smoother and more stable path result.

[0105] The present invention achieves stable path maintenance by introducing the front wheel turning angle into the cost function of the lower layer replanning. BRIEF DESCRIPTION OF THE DRAWINGS

[0106] Figure 1 Schematic diagram of the overall architecture of obstacle avoidance path planning;

[0107] Figure 2 Schematic diagram of angle constraint sampling considering tire-road adhesion limit;

[0108] Figure 3 Schematic diagram of sampling correction based on artificial potential field;

[0109] Figure 4 This is a schematic diagram of the IRRT algorithm code;

[0110] Figure 5 It is a two-degree-of-freedom bicycle model;

[0111] Figure 6 This is a schematic diagram for comparing the initial moment pre-planning algorithms in a static obstacle avoidance scenario; Figure 6(a) Schematic diagram of the pre-planned path of the RRT* algorithm; Figure 6 (b) Schematic diagram of the pre-planned path of the IRRT algorithm; Figure 6 (c) is a schematic diagram comparing the heading angles of the paths of the two algorithms;

[0112] Figure 7 This is a schematic diagram of the static obstacle avoidance scene results; Figure 7 (a) is a schematic diagram of obstacle avoidance path comparison; Figure 7 (b) is a schematic diagram for comparing the front wheel turning angles; Figure 7 (c) is a schematic diagram of lateral acceleration comparison; Figure 7 (d) is a schematic diagram for comparing the sideslip angle of the center of mass;

[0113] Figure 8 A schematic diagram showing the comparison of initial moment pre-planning algorithms in a dynamic obstacle avoidance scenario; Figure 8 (a) Schematic diagram of the pre-planned path of the RRT* algorithm; Figure 8 (b) Schematic diagram of the pre-planned path of the IRRT algorithm; Figure 8 (c) is a schematic diagram comparing the heading angles of the paths of the two algorithms;

[0114] Fig. 9 This is a schematic diagram of the dynamic obstacle avoidance scene results; Fig. 9 (a) is a schematic diagram of obstacle avoidance path comparison; Fig. 9 (b) is a schematic diagram for comparing the front wheel turning angles; Fig. 9 (c) is a schematic diagram of lateral acceleration comparison; Fig. 9 (d) is a schematic diagram of the comparison of the sideslip angle at the center of mass. DETAILED DESCRIPTION

[0115] The present invention is further described below in conjunction with the embodiments, but it should not be understood that the above subject matter of the present invention is limited to the following embodiments. Without departing from the above technical ideas of the present invention, various substitutions and changes are made according to the common technical knowledge and customary means in the art, which should all be included in the protection scope of the present invention.

[0116] Embodiment 1:

[0117] See also Figures 1 to 9 , a dual-layer path planning method for intelligent vehicles considering tire-road adhesion limit includes the following steps:

[0118] 1) Obtain prior map information, vehicle perception information, and road adhesion coefficient.

[0119] 2) Build a path planner.

[0120] The path planner includes an upper layer pre-planner and a lower layer re-planner.

[0121] 3) According to the prior map information, vehicle perception information and road adhesion coefficient, the upper-level pre-planner plans a pre-planned path based on the improved fast search random tree algorithm.

[0122] 4) The lower-level replanner establishes a nonlinear prediction model based on the pre-planned path planned by the upper-level pre-planner, and uses a nonlinear model predictive control algorithm to solve the nonlinear prediction model to obtain the re-planned path.

[0123] 5) The replanned path is passed to the path tracking controller as a reference path. The path tracking controller passes the front wheel steering angle of the vehicle as feedback information to the vehicle, controls the vehicle to travel, and returns to step 1) until the vehicle reaches the destination.

[0124] Embodiment 2:

[0125] A double-layer path planning method for an intelligent vehicle considering tire-road adhesion limit, the main technical content is shown in Example 1. Further, the vehicle perception information includes vehicle longitudinal speed, vehicle lateral acceleration, vehicle yaw angular velocity, vehicle heading angle, vehicle gross mass, vehicle yaw moment of inertia, distance from vehicle center of mass to front axle, distance from vehicle center of mass to rear axle, front wheel cornering stiffness, rear wheel cornering stiffness, vehicle length, vehicle width, obstacle vehicle length, obstacle vehicle width, and obstacle vehicle position.

[0126] Embodiment 3:

[0127] A dual-layer path planning method for an intelligent vehicle taking into account tire-road adhesion limit, the main technical content of which is shown in any one of embodiments 1 to 2. Furthermore, the road adhesion coefficient is estimated by a road adhesion coefficient estimator.

[0128] Embodiment 4:

[0129] The dual-layer path planning method for intelligent vehicles considering tire-road adhesion limit, the main technical content of which is shown in any one of embodiments 1 to 3. Further, according to the prior map information, vehicle perception information and road adhesion coefficient, the upper-layer pre-planner plans a pre-planned path based on the improved fast search random tree algorithm as follows:

[0130] 3.1) Establish a random tree T = (V, E) and determine the target position q goal , and the vehicle's starting position q init Add it to the random tree as the root node, where V represents the node in the random tree and E represents the path between the nodes of the random tree.

[0131] 3.2) Randomly generate a coordinate point q in the map space rand as sampling point.

[0132] 3.3) Traverse the nodes stored in the random tree and find the distance sampling point qrand The nearest node is used as the parent node q parent .

[0133] 3.4) Adaptively adjust the expansion step size of the random tree to obtain the adaptive step size l step , as shown below:

[0134] l step =λ s v x (1)

[0135] In the formula, v x is the longitudinal velocity of the vehicle. s is the adaptive coefficient.

[0136] 3.5) Considering the angle constraint of the tire-road adhesion limit, we can get parent The sampling point q that satisfies the angle constraint samp .

[0137] 3.6) Based on the artificial potential field method, the sampling point q samp Make corrections and get the corrected new node q new .

[0138] 3.7) Check the parent node q parent and the new node q new Will the connection between them collide with the infeasible region? If not, the new node q new Add to the random tree T as a child node and store the parent node q parent If yes, go to step 3.8).

[0139] 3.8) Taking the geometric center of the obstacle in the infeasible region as the expansion center, the obstacle is expanded as follows:

[0140]

[0141] In the formula, l exp 、w exp are the length and width of the obstacle after expansion. obs 、w obs are the length and width of the obstacle respectively. veh 、w veh are the length and width of the vehicle respectively. safe 、w safe They are the longitudinal and lateral safety expansion dimensions of the obstacle respectively.

[0142] 3.9) Return to step 3.2) until the iteration end condition is met.

[0143] Embodiment 5:

[0144] The intelligent vehicle double-layer path planning method considering the tire-road adhesion limit, the main technical content of which is shown in any one of embodiments 1 to 4, further, the angle constraint considering the tire-road adhesion limit is obtained at the parent node q parent The sampling point q that satisfies the angle constraint samp The steps are as follows:

[0145] 3.5.1) Based on the parent node q parent and path angle θ pre , randomly generate an expansion angle of a child node under the expansion angle constraint

[0146] 3.5.2) Along the extension angle Expand and obtain the sampling point q that satisfies the constraints samp .

[0147] The sampling point q samp The coordinates are as follows:

[0148]

[0149] In the formula, x parent ,y parent They are parent nodes q parent The horizontal and vertical coordinates of x samp ,y samp The sampling points q samp The horizontal and vertical coordinates. step is the adaptive step size.

[0150] Among them, the random sampling angle As shown below:

[0151]

[0152] Among them, the angle constraint limit value As shown below:

[0153]

[0154] Where g is the acceleration due to gravity. y,max is the maximum lateral acceleration of the vehicle. x is the vehicle longitudinal velocity.

[0155] μ is the dynamic friction coefficient.

[0156] Wherein, the time interval Δt is as follows:

[0157]

[0158] Embodiment 6:

[0159] A dual-layer path planning method for an intelligent vehicle considering tire-road adhesion limit, the main technical content of which is shown in any one of embodiments 1 to 5. Further, the artificial potential field method is used to calculate the sampling point q samp Make corrections and get the corrected new node q new The steps are as follows:

[0160] 3.6.1) Establish sampling point q samp For the parent node q parent The gravitational potential field function is as follows:

[0161] U samp (q parent ) = k samp (q samp -q parent ) (7)

[0162] Where U samp (q parent ) is the sampling point q samp For the parent node q parent gravitational potential field function. k samp is the sampling point q samp The gravitational weight coefficient.

[0163] According to the gradient descent method, the sampling point q samp Relative to the parent node q parent The gravitational force in the direction is as follows:

[0164]

[0165] In the formula, is the sampling point q samp Relative to the parent node q parent Direction of gravity. is the gradient function.

[0166] 3.6.2) Establish the target position q goal For the parent node q parent The gravitational potential field function is as follows:

[0167] U goal (q parent ) = k goal (q glal -q parent ) (9)

[0168] Where U goal (q parent ) is the target position q goal For the parent node q parent gravitational potential field function. k goal is the gravitational weight coefficient of the target point.

[0169] According to the gradient descent method, the target position q goal Relative to the parent node q parent The gravitational force in the direction is as follows:

[0170]

[0171] In the formula, is the target position q goal Relative to the parent node q parent Direction of gravity.

[0172] 3.6.3) Considering the geometric width of the vehicle, the repulsive potential field function of the road boundary is established as follows:

[0173]

[0174] Where U road (q parent ) is the repulsive potential field function of the road boundary. k road is the road boundary repulsion weight coefficient. l ,y r are the ordinates of the left and right road boundaries, respectively, and w veh is the vehicle width. parent The parent node q parent The vertical coordinate of .

[0175] The repulsion force for the road boundary is as follows:

[0176]

[0177] In the formula, is the repulsive force of the road boundary.

[0178] 3.6.4) Determine the sampling point q samp Is the resultant force at the node within the angle constraint range of the tire-road adhesion limit? If so, the direction of the resultant force is taken as the expansion direction of the new node. If not, the corresponding angle constraint limit value is taken as the expansion direction, and the parent node q parent is the root node, and a single step is extended in this direction to obtain a new node q new .

[0179] Embodiment 7:

[0180] A double-layer path planning method for an intelligent vehicle considering tire-road adhesion limit, the main technical content of which is shown in any one of embodiments 1 to 6. Furthermore, the iteration end condition includes that the search path reaches the target position or reaches the maximum number of sampling times.

[0181] Embodiment 8:

[0182] The dual-layer path planning method for an intelligent vehicle considering the tire-road adhesion limit, the main technical content of which is shown in any one of Embodiments 1 to 7. Further, the lower-layer replanner establishes a nonlinear prediction model according to the pre-planned path planned by the upper-layer pre-planner as follows:

[0183] 4.1) Establish the vehicle motion equation of the bicycle model as follows:

[0184]

[0185] Where m is the mass of the car. y is the lateral velocity of the vehicle. x is the longitudinal velocity of the vehicle. γ is the yaw rate of the vehicle. y,f 、F y,r I is the total lateral force of the front and rear wheels of the vehicle, respectively. z is the vehicle's yaw moment of inertia. f , l r are the distances from the front and rear axles to the center of mass. ψ is the vehicle heading angle. g , Y g are the longitudinal and lateral positions of the vehicle's center of mass in the global coordinate system, respectively. · is the first-order derivative function.

[0186] The lateral force is given by:

[0187]

[0188] In the formula, the subscript i=f, r represents the front wheel or the rear wheel. y,i is the tire cornering stiffness. z,i is the tire vertical force. μ is the dynamic friction coefficient.

[0189] Wherein, the tire slip angle α i As shown below:

[0190]

[0191] Where δ is the front wheel turning angle. f , α r Represent the sideslip angles of the front and rear wheels respectively.

[0192] 4.2) Based on the vehicle motion equation of the bicycle model, the nonlinear dynamic state space equation in path planning is established as follows:

[0193]

[0194] Where u is the control variable, and u = δ. x is the state variable, and x = [v y ,γ,ψ,X g ,Yg ] T . y is the output variable, and y=Y g . is the updated state variable.

[0195] Among them, the state transfer function f(·) and the observation function h(·) are as follows:

[0196]

[0197] h(x)=x5 (18)

[0198] In the formula, C f , C r are the cornering stiffness of the front and rear wheels respectively. x1, x2, x3, x5 are all components of the state variables. f1(x,u), f2(x,u), f3(x,u), f4(x,u), f5(x,u) are all components of the state transfer function.

[0199] 4.3) The state space is discretized using the forward Euler method to obtain the system prediction state and system prediction output at time k.

[0200] The system prediction state at time k is as follows:

[0201]

[0202] Where N p is the prediction time domain. c is the control time domain. T is the sampling period.

[0203] The system prediction output at time k is as follows:

[0204]

[0205] Embodiment 9:

[0206] A double-layer path planning method for an intelligent vehicle taking into account tire-road adhesion limits, the main technical content of which can be found in any one of Examples 1 to 8. Furthermore, the constraints of the nonlinear prediction model include constraints on the control quantity in the state space, constraints on the vehicle lateral acceleration, and constraints on the lateral acceleration in the prediction time domain.

[0207] The constraints of the control quantity in the state space are as follows:

[0208] U min ≤U(k)≤U max (twenty one)

[0209] Where U(k) is the control quantity at time k, U(k)=[u(k+1|k),u(k+2|k),…,u(k+Nc |k)] T , U min , U max are the minimum and maximum values ​​of the control quantity, respectively, and δ min , δ max are the minimum and maximum constraints of the front wheel angle respectively.

[0210] The vehicle lateral acceleration constraint conditions are as follows:

[0211]

[0212] Where g is the acceleration due to gravity. y is the lateral acceleration of the vehicle.

[0213] The constraints of the lateral acceleration in the prediction time domain are as follows:

[0214]

[0215] In the formula, Γ max is the limit value of lateral acceleration in the prediction time domain. Γ min =-Γ max X(k) is the predicted state of the system at time k, X(k) = [x(k+1|k), x(k+2|k),…, x(k+N p |k)] T .

[0216] Among them, the state parameter Ψ and the control parameter Θ are as follows:

[0217]

[0218] Among them, C and D are intermediate parameters, and Embodiment 10:

[0219] A dual-layer path planning method for an intelligent vehicle considering tire-road adhesion limit, the main technical content of which is shown in any one of embodiments 1 to 9. Further, when the lower-layer replanner solves the nonlinear prediction model, the path replanning is converted into the solution of a nonlinear optimization problem, and the cost function of the nonlinear optimization problem is as follows:

[0220]

[0221] Where J is the cost function. i is the time sequence number, N p is the prediction time domain. c Y is the control time domain. ref(k+i|k) is the lateral reference position in the prediction time domain, Q and R are the output matrix and control weight matrix respectively. y(k+i|k) is the system prediction output at time k. u(k+i|k) is the control amount at time k.

[0222] The nonlinear optimization problem is as follows:

[0223] J

[0224] st(21)(23)(27).

[0225] Embodiment 11:

[0226] See also Figures 1 to 9 , a two-layer path planning method for intelligent vehicles considering tire-road adhesion limit, including:

[0227] 1) Obtain prior map information, vehicle perception information, and road adhesion coefficient.

[0228] 2) Constructing a path planner. The path planner includes an upper-level pre-planner and a lower-level re-planner.

[0229] 3) According to the prior map information, vehicle perception information and road adhesion coefficient, the upper-level pre-planner plans a pre-planned path based on the improved fast search random tree algorithm.

[0230] 4) The lower-level replanner establishes a nonlinear prediction model based on the pre-planned path planned by the upper-level pre-planner, and uses a nonlinear model predictive control algorithm to solve the nonlinear prediction model to obtain the re-planned path.

[0231] 5) The replanned path is passed to the path tracking controller as a reference path. The path tracking controller passes the front wheel steering angle of the vehicle as feedback information to the vehicle, controls the vehicle to travel, and returns to step 1) until the vehicle reaches the destination.

[0232] Embodiment 12:

[0233] A double-layer path planning method for an intelligent vehicle considering tire-road adhesion limit, the main technical content is shown in Example 11. Furthermore, the vehicle perception information includes vehicle longitudinal speed, vehicle lateral acceleration, vehicle yaw angular velocity, vehicle heading angle, vehicle gross mass, vehicle yaw moment of inertia, distance from vehicle center of mass to front axle, distance from vehicle center of mass to rear axle, front wheel cornering stiffness, rear wheel cornering stiffness, vehicle length, vehicle width, obstacle vehicle length, obstacle vehicle width, and obstacle vehicle position.

[0234] Embodiment 13:

[0235] A double-layer path planning method for an intelligent vehicle taking into account tire-road adhesion limit, the main technical content of which can be found in any one of Examples 11 to 12. Furthermore, the road adhesion coefficient is estimated by a road adhesion coefficient estimator.

[0236] Embodiment 14:

[0237] The dual-layer path planning method for an intelligent vehicle considering the tire-road adhesion limit, the main technical content of which is shown in any one of Embodiments 11 to 13. Further, the steps of the upper-layer pre-planner planning a pre-planned path based on an improved fast search random tree algorithm according to the prior map information, the vehicle perception information and the road adhesion coefficient are as follows:

[0238] 3.1) Establish a random tree T = (V, E) and determine the target position q goal , and the vehicle's starting position q init Add it to the random tree as the root node, where V represents the node in the random tree and E represents the path between the nodes of the random tree.

[0239] 3.2) Randomly generate a coordinate point q in the map space rand as sampling point.

[0240] 3.3) Traverse the nodes stored in the random tree and find the distance sampling point q rand The nearest node is used as the parent node q parent .

[0241] 3.4) Adaptively adjust the expansion step size of the random tree to obtain the adaptive step size l step , as shown below:

[0242] l step =λ s v x (1)

[0243] In the formula, v x is the longitudinal velocity of the vehicle. s is the adaptive coefficient.

[0244] 3.5) Considering the angle constraint of the tire-road adhesion limit, we can get parent The sampling point q that satisfies the angle constraint samp .

[0245] 3.6) Based on the artificial potential field method, the sampling point q samp Make corrections and get the corrected new node q new .

[0246] 3.7) Check the parent node q parent and the new node q newWill the connection between them collide with the infeasible region? If not, the new node q new Add to the random tree T as a child node and store the parent node q parent If yes, go to step 3.8).

[0247] 3.8) Taking the geometric center of the obstacle in the infeasible region as the expansion center, the obstacle is expanded as follows:

[0248]

[0249] In the formula, l exp 、w exp are the length and width of the obstacle after expansion. obs 、w obs are the length and width of the obstacle respectively. veh 、w veh are the length and width of the vehicle respectively. safe 、w safe They are the longitudinal and lateral safety expansion dimensions of the obstacle respectively.

[0250] 3.9) Return to step 3.2) until the iteration end condition is met.

[0251] Embodiment 15:

[0252] The intelligent vehicle double-layer path planning method considering the tire-road adhesion limit, the main technical content of which is shown in any one of embodiments 11 to 14, further, the angle constraint considering the tire-road adhesion limit is obtained at the parent node q parent The sampling point q that satisfies the angle constraint samp The steps are as follows:

[0253] 3.5.1) Based on the parent node q parent and path angle θ pre , randomly generate an expansion angle of a child node under the expansion angle constraint

[0254] 3.5.2) Along the extension angle Expand and obtain the sampling point q that satisfies the constraints samp .

[0255] The sampling point q samp The coordinates are as follows:

[0256]

[0257] In the formula, x parent ,y parent They are parent nodes q parent The horizontal and vertical coordinates of xsamp ,y samp The sampling points q samp The horizontal and vertical coordinates. step is the adaptive step size. Among them, the random sampling angle As shown below:

[0258]

[0259] Among them, the angle constraint limit value As shown below:

[0260]

[0261] Where g is the acceleration due to gravity. y,max is the maximum lateral acceleration of the vehicle. x is the longitudinal velocity of the vehicle. μ is the dynamic friction coefficient. Wherein, the time interval Δt is as follows:

[0262]

[0263] Embodiment 16:

[0264] A dual-layer path planning method for an intelligent vehicle considering tire-road adhesion limit, the main technical content of which is shown in any one of embodiments 11 to 15. Further, the artificial potential field method is used to calculate the sampling point q samp Make corrections and get the corrected new node q new The steps are as follows:

[0265] 3.6.1) Establish sampling point q samp For the parent node q parent The gravitational potential field function is as follows:

[0266] U samp (q parent ) = k samp (q samp -q parent ) (7)

[0267] Where U samp (q parent ) is the sampling point q samp For the parent node q psrent gravitational potential field function. k samp is the sampling point q samp The gravitational weight coefficient. According to the gradient descent method, the sampling point q samp Relative to the parent node q parent The gravitational force in the direction is as follows:

[0268]

[0269] In the formula, is the sampling point q samp Relative to the parent node q parent Direction of gravity. is the gradient function.

[0270] 3.6.2) Establish the target position q goal For the parent node q parent The gravitational potential field function is as follows:

[0271] U goal (q parent ) = k goal (q goal -q parent ) (9)

[0272] Where U goal (q parent ) is the target position q goal For the parent node q parent gravitational potential field function. k goal is the gravity weight coefficient of the target point. According to the gradient descent method, the target position q goal Relative to the parent node q parent The gravitational force in the direction is as follows:

[0273]

[0274] In the formula, is the target position q goal Relative to the parent node q parent Direction of gravity.

[0275] 3.6.3) Considering the geometric width of the vehicle, the repulsive potential field function of the road boundary is established as follows:

[0276]

[0277] Where U road (q parent ) is the repulsive potential field function of the road boundary. k road is the road boundary repulsion weight coefficient. l ,y r are the ordinates of the left and right road boundaries, respectively, and w veh is the vehicle width. parent The parent node q parent The repulsion force of the road boundary is as follows:

[0278]

[0279] In the formula, is the repulsive force of the road boundary.

[0280] 3.6.4) Determine the sampling point q samp Is the resultant force at the node within the angle constraint range of the tire-road adhesion limit? If so, the direction of the resultant force is taken as the expansion direction of the new node. If not, the corresponding angle constraint limit value is taken as the expansion direction, and the parent node q parent is the root node, and a single step is extended in this direction to obtain a new node q new .

[0281] Embodiment 17:

[0282] A double-layer path planning method for an intelligent vehicle taking into account tire-road adhesion limits, the main technical content of which can be found in any one of Examples 11 to 16. Furthermore, the iteration end condition includes the search path reaching the target position or reaching the maximum number of sampling times.

[0283] Embodiment 18:

[0284] The dual-layer path planning method for an intelligent vehicle considering the tire-road adhesion limit, the main technical content of which is shown in any one of Embodiments 11 to 17. Further, the lower-layer replanner establishes a nonlinear prediction model according to the pre-planned path planned by the upper-layer pre-planner as follows:

[0285] 4.1) Establish the vehicle motion equation of the bicycle model as follows:

[0286]

[0287] Where m is the mass of the car. y is the lateral velocity of the vehicle. x is the longitudinal velocity of the vehicle. γ is the yaw rate of the vehicle. y,f 、F y,r I is the total lateral force of the front and rear wheels of the vehicle, respectively. z is the vehicle's yaw moment of inertia. f , l r are the distances from the front and rear axles to the center of mass. ψ is the vehicle heading angle. g , Y g are the longitudinal and lateral positions of the vehicle's center of mass in the global coordinate system. · is the first-order derivative function.

[0288] The lateral force is given by:

[0289]

[0290] In the formula, the subscript i=f, r represents the front wheel or the rear wheel. y,i is the tire cornering stiffness. z,i is the tire vertical force. μ is the dynamic friction coefficient.

[0291] Wherein, the tire slip angle α iAs shown below:

[0292]

[0293] Where δ is the front wheel turning angle. f , α r Represent the sideslip angles of the front and rear wheels respectively.

[0294] 4.2) Based on the vehicle motion equation of the bicycle model, the nonlinear dynamic state space equation in path planning is established as follows:

[0295]

[0296] Where u is the control variable, and u = δ. x is the state variable, and x = [v y ,γ,ψ,X g ,Y g ] T . y is the output variable, and y=Y g . is the updated state variable.

[0297] Among them, the state transfer function f(·) and the observation function h(·) are as follows:

[0298]

[0299] In the formula, C f , C r are the cornering stiffness of the front and rear wheels respectively. x1, x2, x3, x5 are all components of the state variables. f1(x,u), f2(x,u), f3(x,u), f4(x,u), f5(x,u) are all components of the state transfer function.

[0300] 4.3) The state space is discretized using the forward Euler method to obtain the system prediction state and system prediction output at time k.

[0301] The system prediction state at time k is as follows:

[0302]

[0303] Where N p is the prediction time domain. c is the control time domain. T is the sampling period.

[0304] The system prediction output at time k is as follows:

[0305]

[0306] Embodiment 19:

[0307] A double-layer path planning method for an intelligent vehicle taking into account tire-road adhesion limits, the main technical content of which can be found in any one of Examples 11 to 18. Furthermore, the constraints of the nonlinear prediction model include constraints on the control quantity in the state space, constraints on the vehicle lateral acceleration, and constraints on the lateral acceleration in the prediction time domain.

[0308] The constraints of the control quantity in the state space are as follows:

[0309] U min ≤U(k)≤U max (twenty one)

[0310] Where U(k) is the control quantity at time k, U(k)=[u(k+1|k),u(k+2|k),…,u(k+N c |k)] T , U min , U max are the minimum and maximum values ​​of the control quantity, respectively, and δ min , δ max are the minimum and maximum constraints of the front wheel angle respectively.

[0311] The vehicle lateral acceleration constraint conditions are as follows:

[0312]

[0313] Where g is the acceleration due to gravity. y is the lateral acceleration of the vehicle.

[0314] The constraints of the lateral acceleration in the prediction time domain are as follows:

[0315]

[0316] In the formula, Γ max is the limit value of lateral acceleration in the prediction time domain. Γ min =-Γ max X(k) is the predicted state of the system at time k, X(k) = [x(k+1|k), x(k+2|k),…, x(k+N p |k)] T .

[0317] Among them, the state parameter Ψ and the control parameter Θ are as follows:

[0318]

[0319]

[0320] Among them, C and D are intermediate parameters, and Embodiment 20:

[0321] The dual-layer path planning method for intelligent vehicles considering tire-road adhesion limit, the main technical content is shown in any one of embodiments 11 to 19, further, the role of the NMPC path planner is to make the planning result as close as possible to the pre-planned reference path and ensure the stability of the front wheel turning angle under the vehicle and road constraints. When the lower-layer replanner solves the nonlinear prediction model, the path replanning is converted into the solution of a nonlinear optimization problem, and the cost function of the nonlinear optimization problem is as follows:

[0322]

[0323] Where J is the cost function. i is the time sequence number, N p is the prediction time domain. c Y is the control time domain. ref (k+i|k) is the lateral reference position in the prediction time domain, Q and R are the output matrix and control weight matrix respectively. y(k+i|k) is the system prediction output at time k. u(k+i|k) is the control amount at time k.

[0324] The nonlinear optimization problem is as follows:

[0325] J

[0326] st(21)(23)(27).

[0327] Embodiment 21:

[0328] See also Figures 1 to 9 , a dual-layer path planning method for intelligent vehicles considering tire-road adhesion limit, the main technical contents include:

[0329] The overall architecture of the path planning method proposed in this paper is as follows: Figure 1As shown. The path planner needs to obtain prior map information, vehicle perception information and the vehicle's own state parameters. In addition, it also needs the road adhesion coefficient output by the road adhesion coefficient estimator. Here, the existing estimation method is used in the road adhesion coefficient estimator. In each planner execution cycle, the path planner will perform two-level path planning: upper-level pre-planning and lower-level re-planning. In the upper-level pre-planning, the standard RRT (Rapidly-exploring RandomTree) algorithm is improved through step size adaptation, angle constraint sampling and sampling correction to obtain a pre-planned path. The lower-level re-planning receives the path results from the upper-level pre-planning, and uses the NMPC (Nonlinear Model Predictive Control) algorithm to establish a prediction model, design constraints and perform optimization solutions to achieve path re-planning in the rolling time domain. Finally, the path results obtained by the lower-level re-planning are passed to the path tracking controller as a reference path, and the controller passes the front wheel angle as feedback information to the vehicle, thereby completing the path planning-tracking control closed loop.

[0330] The specific steps of the pre-planning IRRT algorithm are as follows: 1. Initialization: Establish a random tree T = (V, E), and set the starting position q init Add it to the random tree as the root node. 2. Random sampling: randomly generate a coordinate point q in the map space rand 3. Determine the parent node: traverse the nodes stored in the random tree and find the node closest to the random sampling point as the parent node q parent 4. Step size adaptation: Using the vehicle longitudinal speed information, the expansion step size of the random tree is adaptively adjusted to obtain the adaptive step size l step :

[0331] l step =λ s v x (1)

[0332] In the formula, λ s is the adaptive coefficient.

[0333] 5. Angle constraint sampling considering tire-road adhesion limit: Figure 2 The proposed angle constraint sampling strategy considering the tire-road adhesion limit is shown. parent and its path angle θ pre , in the extended angle constraint Randomly generate an expansion angle of a child node within the range By expanding in this direction, we can obtain the sampling point q that satisfies the constraint conditions. sampThe path extension direction of the node in the random tree can be regarded as the heading angle of the vehicle when following the path. Then the angle change of the child node path direction relative to the parent node path direction (i.e. Figure 2 middle ) can be considered as the change in the heading angle of the vehicle after passing through this path.

[0334] Assuming the vehicle is a rigid body and ignoring the lateral velocity change of the vehicle's center of mass, the vehicle's yaw angular velocity can be written as:

[0335]

[0336] In the formula, a y is the lateral acceleration of the vehicle. The change in the vehicle heading angle after a single step can be written as:

[0337]

[0338] Where Δt is the time interval, which can be calculated as follows:

[0339]

[0340] Based on the maximum adhesion that the ground can provide, the maximum lateral acceleration allowed by the vehicle is:

[0341] a y,max =μg (5)

[0342] In the formula, g is the gravitational acceleration. Combining formulas (3) and (5), the maximum expansion angle of the node is:

[0343]

[0344] Perform random sampling within the maximum extended angle range to obtain a random sampling angle Then the sampling angle satisfies:

[0345]

[0346] The sampling angle that satisfies the angle constraint is determined After that, the sampling point q samp The coordinates of can be calculated as follows:

[0347]

[0348] In the formula, x parent and parent They are parent nodes q parent The horizontal and vertical coordinates of the parent node q parent is the initial node q init , there is no corresponding parent node path, so θ pre Take it as the current vehicle heading angle.

[0349] 6. Sampling correction based on artificial potential field: The artificial potential field method generally establishes corresponding gravitational potential fields and repulsive potential fields based on the target points and obstacles in the map space, and then uses the gradient descent method to find the path. Since there is already collision detection between the vehicle and obstacles in the RRT algorithm, the obstacle repulsion is ignored in the design of the artificial potential field function. In order to combine the characteristics of the artificial potential field method and the RRT algorithm, it is assumed that the sampling point and the target point have gravitational forces on the parent node to guide the random tree to expand to the target point, while avoiding the problem of falling into the local optimal solution in the traditional artificial potential field. The gravitational potential field function of the sampling point to the parent node is designed as:

[0350] U samp (q parent ) = k samp (q samp -q parent ) (9)

[0351] In the formula, k samp is the gravity weight coefficient of the sampling point. According to the gradient descent method, the gravity of the sampling point relative to the parent node is:

[0352]

[0353] Conventional artificial potential field methods generally design the gravity of the target point as a function related to the relative distance between the current position and the target position. This will cause the gravity in the direction of the target point to gradually decrease when the vehicle approaches the target point, thereby reducing the efficiency of the path search in the area around the target point. Therefore, the gravitational potential field function of the target point to the parent node is designed as:

[0354] U goal (q parent ) = k goal (q goal -q parent ) (11)

[0355] In the formula, k goal is the gravity weight coefficient of the target point. Then the gravity of the target point relative to the parent node is:

[0356]

[0357] In the vehicle obstacle avoidance scenario, the vehicle should always stay in the safe area and avoid driving to the road boundary. Therefore, it is assumed that the road boundary exerts a repulsive force on the vehicle, guiding the random tree to expand to the safe area on the road. Considering the geometric width of the vehicle, the repulsive potential field function of the road boundary is designed as:

[0358]

[0359] In the formula, kroad is the road boundary repulsion weight coefficient; y l and r are the ordinates of the left and right road boundaries, respectively, and w veh is the vehicle width. Therefore, the repulsive force on the road boundary is:

[0360]

[0361] like Figure 3 As shown, the total gravitational (the sum of the gravitational force at the sampling point and the gravitational force at the target point) and the total repulsive force (Road boundary repulsion) is synthesized at the sampling point to obtain the resultant force

[0362]

[0363] If the combined force If the direction of is within the angle constraint range specified in the previous section, the direction of the resultant force is taken as the expansion direction of the new node; if it exceeds the angle constraint range, the corresponding angle constraint limit value is taken as the expansion direction. parent is the root node, and a single step can be extended in this direction to obtain a new node q new .

[0364] 7. No-collision detection: Check whether the parent node-new node connection and the infeasible region collide. If there is no collision, the new node will be added to the random tree as a child node and its parent node information will be stored; if there is a collision, proceed to the next step. In the RRT algorithm, the vehicle geometry is regarded as a point, so the vehicle geometry parameters should be considered when performing no-collision detection. The geometric center of the obstacle in the infeasible region is used as the expansion center, and the obstacle is expanded as follows:

[0365]

[0366] In the formula, l exp and w exp are the length and width of the obstacle after expansion; l obs and w obs are the length and width of the obstacle respectively; l veh and w veh are the length and width of the vehicle respectively; l safe and w safe They are the longitudinal and lateral safety expansion dimensions of the obstacle respectively.

[0367] 8. Iteration: Repeat steps 2-7 until the search path reaches the target position or the maximum number of sampling times is reached.

[0368] After reaching the target location, the path planning result in the pre-planning can be obtained by traversing the parent nodes and extended paths in the random tree from the target location in reverse.

[0369] The steps of re-planning NMPC method are as follows: 1. Establish prediction model: Figure 5 The two-degree-of-freedom bicycle model shown models the vehicle dynamics. The vehicle equations of motion for the bicycle model are:

[0370]

[0371] Where m is the mass of the car; v y is the lateral velocity of the vehicle; γ is the yaw angular velocity of the vehicle; F y,f and F y,r are the total lateral forces of the front and rear wheels of the vehicle respectively; I z is the vehicle yaw moment of inertia; l f and l r are the distances from the front and rear axles to the center of mass respectively; ψ is the vehicle heading angle; X g and Y g are the longitudinal and lateral positions of the vehicle center of mass in the global coordinate system, respectively. The brush tire model is used to describe the lateral forces of the front and rear wheels:

[0372]

[0373] In the formula, the subscripts i=f and r represent the front wheel and rear wheel respectively; C y,i is the tire cornering stiffness; F z,i is the tire vertical force; α i is the tire slip angle, which can be calculated by the following formula:

[0374]

[0375] Where δ is the front wheel steering angle.

[0376] Based on the established vehicle dynamics model, the state variable x is defined as [v y ,γ,ψ,X g ,Y g ] T , control variable u=δ, output variable y=Y g , then the nonlinear dynamic state space equation in path planning can be written as:

[0377]

[0378] In the formula, f(·) and h(·) are the state transfer function and observation function, respectively, and their expressions are:

[0379]

[0380] h(x)=x5 (22)

[0381] Discretize the state space using the forward Euler method:

[0382]

[0383] Where T is the sampling period.

[0384] Assume that the prediction time domain is N p , the control time domain is N c , then the system prediction state at time k can be expressed as:

[0385]

[0386] Similarly, the system prediction output at time k can be expressed as:

[0387]

[0388] 2. Constraints and optimization solutions: Considering the front wheel steering angle limit and the maximum adhesion that the road can provide to the tire, the front wheel steering angle and lateral acceleration of the vehicle are constrained to ensure the safety and stability of the vehicle during obstacle avoidance. The front wheel steering angle in the control time domain at time k, that is, the constraint condition of the control quantity in the state space, can be written as:

[0389] U min ≤U(k)≤U max (26)

[0390] In the formula, U(k)=[u(k+1|k),u(k+2|k),…,u(k+N c |k)] T , δ min With δ max are the minimum and maximum constraints of the front wheel turning angle respectively. In order to prevent the vehicle from entering the tire-road adhesion limit state and causing vehicle instability, the vehicle lateral acceleration needs to be limited within the road adhesion limit range. The vehicle lateral acceleration constraint can be expressed as:

[0391]

[0392] Combining equation (18) with the state space definition, let η = a y As constraint output, write the above equation in matrix form:

[0393] η(k)=Cx(k)+Du(k) (28)

[0394] In the formula,

[0395] The vehicle lateral acceleration in the prediction domain at time k can be predicted by the following formula:

[0396]

[0397] Write the above formula in matrix form:

[0398] Γ(k)=ΨX(k)+ΘU(k) (30)

[0399] In the formula, Γ(k)=[n(k+1|k),n(k+2|k),…,n(k+N) p |k)] T ,X(k)=[x(k+1|k),x(k+2|k),…,x(k+N p |k)] T ,

[0400] Combined with formula (27), the constraint of lateral acceleration in the prediction time domain can be expressed as:

[0401] Γ min ≤ΨX(k)+ΘU(k)≤Y max (31)

[0402] In the formula,

[0403] The above equation can be written in matrix form suitable for NMPC:

[0404]

[0405] The role of the NMPC path planner is to make the planning result as close to the pre-planned reference path as possible and ensure the stability of the front wheel turning angle while satisfying the vehicle and road constraints. Therefore, the cost function in this nonlinear optimization problem can be defined as:

[0406]

[0407] Where Y ref (k+i|k) is the lateral reference position in the prediction time domain, Q and R are the output and control weight matrices respectively. Path replanning can be transformed into solving the following nonlinear optimization problem:

[0408]

[0409] The optimal control sequence can be obtained by solving the above equation, and then the longitudinal position sequence and lateral position sequence of the vehicle center of mass in the global coordinate system in the prediction time domain can be calculated according to equation (24), which is the path replanning result.

[0410] Embodiment 22:

[0411] See also Figures 1 to 9 , A two-layer path planning method for intelligent vehicles considering tire-road adhesion limit. In order to verify the effectiveness and real-time performance of the proposed path planning method, the two-layer obstacle avoidance path planning algorithm proposed in this paper is implemented based on the CarSim / Simulink joint simulation platform, and the MPC path tracking controller is used to perform simulation verification and analysis in static obstacle avoidance scenarios and dynamic obstacle avoidance scenarios. The static obstacle avoidance scenario is designed to verify the effectiveness of the path planning method under the tire-road adhesion limit state, and the dynamic obstacle avoidance scenario is to verify the obstacle avoidance effect of the path planning method when the feasible safe channel between the two dynamic obstacle vehicles is relatively narrow and the channel width gradually decreases. The single lane width is set to 3.5m in CarSim, and the parameter configurations of the test vehicles and obstacle vehicles used are shown in Table 1. The front wheel turning angle limit in the NMPC replanning algorithm is [-30,30]deg.

[0412] Table 1 CarSim configuration parameters

[0413]

[0414] The following four path planning methods are compared in the embodiment:

[0415] RRT* algorithm: The RRT* algorithm adds a heuristic function for the shortest path based on the standard RRT algorithm. In this section, only this method is used for pre-planning and compared with the pre-planning path results of the IRRT algorithm.

[0416] IRRT algorithm: The IRRT algorithm proposed in this paper is used for real-time path planning and works together with the path tracking controller to achieve obstacle avoidance function, which is represented by "IRRT" in the diagram.

[0417] NMPC method: An obstacle avoidance function is added to the NMPC replanning method used in this paper to achieve the obstacle avoidance function, which is represented by "NMPC" in the diagram. The obstacle avoidance cost function of the predicted state at the i-th step at time k in the NMPC method is designed as:

[0418]

[0419] Where, X g (k+i|k) and Y g (k+i|k) are the longitudinal and lateral positions of the vehicle in the prediction time domain, respectively. obs is the number of obstacles, X obs,j and Y obs,j are the longitudinal and lateral positions of the jth obstacle respectively. Then the total cost function in the NMPC method can be written as:

[0420]

[0421] In the formula, S is the weight coefficient of the obstacle avoidance cost function. There is no path pre-planning algorithm in this method, so the lateral reference position Y ref (k+i|k) is always 1.75m, which is the centerline of the right lane. In addition, this method considers the same constraints as the replanning in this paper, so the nonlinear optimization problem can be described as:

[0422]

[0423] IRRT-NMPC method: This is the two-layer obstacle avoidance path planning method proposed in this paper, which includes IRRT path pre-planning and NMPC path re-planning, represented by "IRRT-NMPC" in the diagram.

[0424] Embodiment 23:

[0425] See also Figure 6 to Figure 7 , a dual-layer path planning method for an intelligent vehicle considering tire-road adhesion limit, the main technical content of which is shown in Example 22:

[0426] The road adhesion coefficient of the static obstacle avoidance scenario is set to 0.4, the ego vehicle speed is set to 54 km / h, the initial position is (0m, 1.75m), the target position is (60m, 1.75m), and the positions of the stationary obstacle vehicles are (25m, 1.75m) and (35m, 1.75m) respectively. Figure 6 Comparison of the pre-planning of the RRT* algorithm and the IRRT algorithm at the initial moment in the static obstacle avoidance scenario. Figure 7 Obstacle avoidance results of the IRRT algorithm, NMPC method and IRRT-NMPC method in static obstacle avoidance scenarios. Figure 6 (a) and (b) show that both the RRT* and IRRT algorithms can generate a collision-free path that meets the safety conditions. However, since the IRRT algorithm uses the artificial potential field method to modify the sampling process, the number of random tree nodes in the IRRT algorithm (144) is much less than the number of nodes in the RRT* algorithm (319), which significantly improves the search efficiency. Figure 6 As can be seen from (b), the random tree nodes in the IRRT algorithm are mainly distributed in front of the obstacle and far away from the right road boundary, indicating that the target point gravity and road boundary repulsion designed in the sampling correction process play a role. In addition, the path result of the IRRT algorithm is smoother and more in line with the rationality of the vehicle obstacle avoidance process. Figure 6(c) It can be seen that although the path result of the RRT* algorithm can achieve the purpose of collision avoidance, its path heading angle and angle increment are too large, and in some positions (such as the longitudinal displacement of 9.92m and 49.40m), the path heading angle even violates the basic driving requirements of the vehicle. Thanks to the angle constraint sampling, the path heading angle of the IRRT algorithm changes smoothly, without excessive heading angles and angle increments, which meets the requirements of vehicle dynamics.

[0427] Figure 7 (a) shows that the three path planning methods of IRRT, NMPC and IRRT-NMPC can successfully avoid obstacles. Under the premise of ensuring no collision with the obstacle vehicle, the lateral offset of the IRRT-NMPC method is the smallest, only 2.99m; the lateral offset of the NMPC method is the largest, reaching 3.60m; and the lateral offset of the IRRT algorithm is 3.19m. Therefore, the IRRT-NMPC method can ensure that the ego vehicle quickly returns to the original driving lane, while the NMPC method is relatively slow. The reason for this difference is that there are two major difficulties in the NMPC method: first, the design of the ideal obstacle avoidance function is more difficult, and it is necessary to comprehensively consider factors such as the geometric characteristics of the obstacle and the interference of the functions between obstacles; second, the debugging of the output, control weight matrix and obstacle avoidance weight coefficient is more complicated, and the same set of parameters cannot guarantee the same ideal obstacle avoidance effect in different scenarios. These two elements need to be met to make the obstacle avoidance path in each scenario safe and smooth, and to ensure that the ego vehicle can quickly return to the original lane. In contrast, the IRRT algorithm treats obstacles as geometric shapes with a safe distance, which simplifies the process of implementing the obstacle avoidance function without designing complex obstacle avoidance functions and debugging the corresponding weights while ensuring no collision. Figure 7 In (b), the front wheel turning angle result of the NMPC method is the smoothest, followed by the IRRT-NMPC method, and there are some sudden changes in the turning angle in the result of the IRRT algorithm. This is because the angle constraint sampling process in the IRRT algorithm has a certain degree of randomness. The cost function in the lower-level NMPC path replanning of the IRRT-NMPC method not only considers the lateral deviation of the path, but also includes the front wheel turning angle. Therefore, the lower-level replanning can make further optimization based on the upper-level pre-planned path, thereby obtaining a smoother and more stable path result. Figure 7 As can be seen in (c), in the range of longitudinal displacement of about 22 to 45 m, all three methods are close to the lateral acceleration limit that the road surface can provide. Since the three methods all contain different forms of lateral acceleration constraints, the lateral acceleration of the vehicle is constrained within the tire-road adhesion limit, such as Figure 7The center of mass sideslip angles of the three methods shown in (d) are also kept within a reasonable range. The IRRT-NMPC method considers the lateral acceleration constraints based on different vehicle dynamics models in the upper pre-planning and lower re-planning. Figure 7 In (c), the duration of being within the 10% range of the lateral acceleration limit is the shortest among the three methods, about 1.1s. The duration of the NMPC method is relatively long, about 1.35s; and the duration of the IRRT method is about 1.25s.

[0428] Embodiment 24:

[0429] See also Figures 8 to 9 , a dual-layer path planning method for an intelligent vehicle considering tire-road adhesion limit, the main technical content of which is shown in Example 22:

[0430] The road adhesion coefficient of the dynamic obstacle avoidance scene is set to 0.6, the vehicle speed is set to 72km / h, the initial position is (0m, 1.75m), and the target position is (100m, 1.75m). The initial positions of dynamic obstacle vehicles 1 and 2 are (20m, 1.75m) and (40m, 5.25m), respectively. Obstacle vehicle 1 moves forward at a constant speed of 36km / h, and obstacle vehicle 2 moves at a constant deceleration of -2m / s2 with an initial speed of 36km / h, that is, the feasible safe passage between the two obstacle vehicles gradually narrows. Figure 8 Comparison of the pre-planning of the RRT* algorithm and the IRRT algorithm at the initial moment in the dynamic obstacle avoidance scenario. Fig. 9 The obstacle avoidance results of the IRRT algorithm, NMPC method and IRRT-NMPC method in the dynamic obstacle avoidance scenario. Compared with the static obstacle avoidance scenario, the longitudinal displacement of the vehicle in this scenario is longer. Figure 8 (b) The number of random tree nodes of the RRT* algorithm has increased to 450. Figure 8 The number of nodes in the IRRT algorithm in (a) is reduced to 91. There are three main reasons: first, the randomness of node expansion; second, the vehicle speed in this scenario is higher, and the random tree expansion step increases with the increase of vehicle speed; third, there is only one obstacle vehicle on the right lane in this scenario, and the longitudinal distance required for the random tree to bypass the obstacle vehicle is shortened, thereby reducing the number of random tree expansions. Figure 8 (c) It can be seen that the longer longitudinal displacement also leads to more unreasonable heading angles and heading angle increments in the pre-planned path of the RRT* algorithm, while the IRRT algorithm can still obtain smooth path heading angle changes in dynamic scenes, ensuring the smoothness and rationality of the path.

[0431] Fig. 9 The time interval between each two marked points on the same curve in (a) is 1s. Before the longitudinal displacement of 40m, the driving trajectories of the three methods are not much different. Fig. 9 (b) The front wheel steering angles of the three methods are exactly the same before the longitudinal displacement of 25.70m. There are two main reasons. First, before entering the feasible channel between the obstacle vehicles, the obstacle avoidance paths planned by the three methods are not much different. Second, the front wheel steering angle increment is considered in the cost function and constraints of the MPC path tracking controller, resulting in the same front wheel steering angle output by the controller when the reference path is similar. After the longitudinal displacement of 25.70m, the front wheel steering angles of the three methods are significantly different, and the ego vehicle is about to enter the feasible channel between the two obstacle vehicles. When the longitudinal displacement is approximately in the range of 40 to 70m, the ego vehicle is located in a gradually narrowing feasible channel. In this stage, the path results of the IRRT-NMPC and IRRT algorithms are not much different, but are significantly different from the NMPC method. The relative distance between the ego vehicle and the brake obstacle vehicle 2 in the three methods is the smallest at about 60m, among which the relative distance of the NMPC method is -0.18m, at which time the two vehicles have collided; the relative distance of the IRRT-NMPC method is the largest among the three methods, which is 1.17m, which is relatively safe; the distance of the IRRT algorithm is 0.96m. However, the shortest relative distance between the ego vehicle and the uniform speed obstacle vehicle 1 in the three methods is greater than 1.2m, which is in a safe state. In addition, the IRRT and IRRT-NMPC methods can return to the original lane faster than the NMPC method. However, the sampling randomness of the IRRT algorithm causes the trajectory of the vehicle to fluctuate after returning to the original lane, while the IRRT-NMPC method achieves stable path maintenance by introducing the front wheel steering angle into the cost function of the lower-level replanning. Fig. 9 (c) shows that none of the three path planning methods reached the road adhesion limit, and the maximum lateral acceleration of the three methods appeared at around 30m. The maximum lateral acceleration of the IRRT-NMPC method was -0.56g, the IRRT algorithm was -0.55g, and the NMPC method was -0.54g, with no obvious difference. Fig. 9 The sideslip angles of the center of mass of the three methods in (d) are all kept within a reasonable range.

Claims

1. A dual-layer path planning method for intelligent vehicles considering tire-road adhesion limit, characterized in that: The following steps are involved: 1) Obtain prior map information, vehicle perception information, and road adhesion coefficient. 2) Build a path planner; The path planner includes an upper layer pre-planner and a lower layer re-planner. 3) Based on the prior map information, vehicle perception information and road adhesion coefficient, the upper-level pre-planner plans a pre-planned path based on the improved fast search random tree algorithm; 4) The lower-level replanner establishes a nonlinear prediction model based on the pre-planned path planned by the upper-level pre-planner, and uses a nonlinear model predictive control algorithm to solve the nonlinear prediction model to obtain a re-planned path; 5) The replanned path is passed to the path tracking controller as a reference path. The path tracking controller transmits the front wheel steering angle of the vehicle as feedback information to the vehicle, controls the vehicle to travel, and returns to step 1) until the vehicle reaches the destination.

2. The intelligent vehicle double-layer path planning method considering tire-road adhesion limit according to claim 1 is characterized in that: The vehicle perception information includes vehicle longitudinal speed, vehicle lateral acceleration, vehicle yaw angular velocity, vehicle heading angle, vehicle total mass, vehicle yaw moment of inertia, distance from vehicle center of mass to front axle, distance from vehicle center of mass to rear axle, front wheel cornering stiffness, rear wheel cornering stiffness, vehicle length, vehicle width, obstacle vehicle length, obstacle vehicle width, and obstacle vehicle position.

3. The intelligent vehicle double-layer path planning method considering tire-road adhesion limit according to claim 1, characterized in that: The road adhesion coefficient is estimated by a road adhesion coefficient estimator.

4. The intelligent vehicle double-layer path planning method considering tire-road adhesion limit according to claim 1, characterized in that: The steps of the upper-level pre-planner planning a pre-planned path based on the improved fast search random tree algorithm according to the prior map information, vehicle perception information and road adhesion coefficient are as follows: 3.1) Establish a random tree T = (V, E) and determine the target position q goal , and the vehicle's starting position q init Add it to the random tree as the root node, where V represents the node in the random tree and E represents the path between the nodes of the random tree; 3.2) Randomly generate a coordinate point q in the map space rand As sampling point; 3.3) Traverse the nodes stored in the random tree and find the distance sampling point q rand The nearest node is used as the parent node q parent ; 3.4) Adaptively adjust the expansion step size of the random tree to obtain the adaptive step size l step , as shown below: l step =λ s v x (1) In the formula, v x is the longitudinal velocity of the vehicle; s is the adaptive coefficient; 3.5) Considering the angle constraint of the tire-road adhesion limit, we can get parent The sampling point q that satisfies the angle constraint samp ; 3.6) Based on the artificial potential field method, the sampling point q samp Make corrections and get the corrected new node q new ; 3.7) Check the parent node q parent and the new node q new Will the connection between them collide with the infeasible region? If not, the new node q new Add to the random tree T as a child node and store the parent node q parent information, and go to step 3.9); if yes, go to step 3.8); 3.8) Taking the geometric center of the obstacle in the infeasible region as the expansion center, the obstacle is expanded as follows: In the formula, l exp 、w exp are the length and width of the obstacle after expansion; l obs 、w obs are the length and width of the obstacle respectively; l veh 、w veh are the length and width of the vehicle respectively; l safe 、w safe They are the longitudinal and lateral safe expansion dimensions of the obstacle respectively; 3.9) Return to step 3.2) until the iteration end condition is met.

5. The intelligent vehicle double-layer path planning method considering tire-road adhesion limit according to claim 4 is characterized in that: The angle constraint considering the tire-road adhesion limit is obtained at the parent node q parent The sampling point q that satisfies the angle constraint samp The steps are as follows: 3.5.1) Based on the parent node q parent and path angle θ pre , randomly generate an expansion angle of a child node under the expansion angle constraint 3.5.2) Along the extension angle Expand and obtain the sampling point q that satisfies the constraints samp ; The sampling point q samp The coordinates are as follows: In the formula, x parent ,y parent They are parent nodes q parent The horizontal and vertical coordinates of samp ,y samp The sampling points q samp The horizontal and vertical coordinates of step is the adaptive step size; Among them, the random sampling angle As shown below: Among them, the angle constraint limit value As shown below: Where g is the acceleration due to gravity; a y,max is the maximum lateral acceleration of the vehicle; v x is the longitudinal velocity of the vehicle; μ is the dynamic friction coefficient; Wherein, the time interval Δt is as follows:

6. The intelligent vehicle double-layer path planning method considering tire-road adhesion limit according to claim 4 is characterized in that: The artificial potential field method is used to calculate the sampling point q samp Make corrections and get the corrected new node q new The steps are as follows: 3.6.1) Establish sampling point q samp For the parent node q parent The gravitational potential field function is as follows: U samp (q parent )=k samp (q samp -q parent ) (7) Where U samp (q parent ) is the sampling point q samp For the parent node q parent gravitational potential field function; k samp is the sampling point q samp The gravitational weight coefficient of According to the gradient descent method, the sampling point q samp Relative to the parent node q parent The gravitational force in the direction is as follows: In the formula, is the sampling point q samp Relative to the parent node q parent Directional attraction; is the gradient function; 3.6.2) Establish the target position q goal For the parent node q parent The gravitational potential field function is as follows: U goal (q parent )=k goal (q goal -q parent ) (9) Where U goal (q parent ) is the target position q goal For the parent node q parent gravitational potential field function; k goal is the gravitational weight coefficient of the target point; According to the gradient descent method, the target position q goal Relative to the parent node q parent The gravitational force in the direction is as follows: In the formula, is the target position q goal Relative to the parent node q parent Directional attraction; 3.6.3) Considering the geometric width of the vehicle, the repulsive potential field function of the road boundary is established as follows: Where U road (q parent ) is the repulsive potential field function of the road boundary; k road is the road boundary repulsion weight coefficient; y l ,y r are the ordinates of the left and right road boundaries, respectively, and w veh is the vehicle width; y parent The parent node q parent The vertical coordinate of The repulsion force for the road boundary is as follows: In the formula, is the repulsive force of the road boundary; 3.6.4) Determine the sampling point q samp Is the resultant force at the node within the angle constraint range of the tire-road adhesion limit? If so, the direction of the resultant force is taken as the expansion direction of the new node. If not, the corresponding angle constraint limit value is taken as the expansion direction, and the parent node q parent is the root node, and a single step is extended in this direction to obtain a new node q new .

7. The intelligent vehicle double-layer path planning method considering tire-road adhesion limit according to claim 4 is characterized in that: The iteration termination condition includes that the search path reaches the target position or reaches the maximum sampling number.

8. The intelligent vehicle double-layer path planning method considering tire-road adhesion limit according to claim 1, characterized in that: The steps of establishing a nonlinear prediction model by the lower-level replanner according to the pre-planned path planned by the upper-level pre-planner are as follows: 4.1) Establish the vehicle motion equation of the bicycle model as follows: Where m is the mass of the car; v y is the lateral velocity of the vehicle; v x is the longitudinal velocity of the vehicle; γ is the yaw rate of the vehicle; F y,f 、F y,r are the total lateral forces of the front and rear wheels of the vehicle respectively; I z is the vehicle yaw moment of inertia; l f , l r are the distances from the front and rear axles to the center of mass respectively; ψ is the vehicle heading angle; X g , Y g are the longitudinal and lateral positions of the vehicle's center of mass in the global coordinate system respectively; · is the first-order derivative function; The lateral force is given by: In the formula, the subscript i=f, r represents the front wheel or the rear wheel; C y,i is the tire cornering stiffness; F z,i is the tire vertical force; μ is the dynamic friction coefficient; Wherein, the tire slip angle α i As shown below: Where δ is the front wheel turning angle; α f , α r Respectively represent the side slip angles of the front and rear wheels; 4.2) Based on the vehicle motion equation of the bicycle model, the nonlinear dynamic state space equation in path planning is established as follows: In the formula, u is the control variable, and u = δ; x is the state variable, and x = [v y ,γ,ψ,X g ,Y g ] T ; y is the output variable, and y=Y g ; is the updated state variable; Among them, the state transfer function f(·) and the observation function h(·) are as follows: h(x)=x5 (18) In the formula, C f , C r are the cornering stiffness of the front and rear wheels respectively; x1, x2, x3, x5 are all components of the state variables; f1(x,u), f2(x,u), f3(x,u), f4(x,u), f5(x,u) are all components of the state transfer function; 4.3) The state space is discretized using the forward Euler method to obtain the system prediction state and system prediction output at time k; The system prediction state at time k is as follows: Where N p is the prediction time domain; N c is the control time domain; T is the sampling period; The system prediction output at time k is as follows:

9. The intelligent vehicle double-layer path planning method considering tire-road adhesion limit according to claim 8, characterized in that: The constraints of the nonlinear prediction model include constraints on the control quantity in the state space, constraints on the vehicle lateral acceleration, and constraints on the lateral acceleration in the prediction time domain; The constraints of the control quantity in the state space are as follows: IN min ≤U(k)≤U max (21) Where U(k) is the control quantity at time k, U(k)=[u(k+1|k),u(k+2|k),…,u(k+N c |k)] T , U min , U max are the minimum and maximum values ​​of the control quantity, respectively, and δ min , δ max are the minimum and maximum constraints of the front wheel turning angle respectively; The vehicle lateral acceleration constraint conditions are as follows: Where g is the acceleration due to gravity; a y is the lateral acceleration of the vehicle; The constraints of the lateral acceleration in the prediction time domain are as follows: In the formula, Γ max is the limit value of lateral acceleration in the prediction time domain; Γ min =-Γ max ; X(k) is the predicted system state at time k, X(k)=[x(k+1|k),x(k+2|k),…,x(k+N p |k)] T ; Among them, the state parameter Ψ and the control parameter Θ are as follows: Among them, C and D are intermediate parameters, and 10. The intelligent vehicle double-layer path planning method considering tire-road adhesion limit according to claim 1, characterized in that: When the lower-level replanner solves the nonlinear prediction model, the path replanning is converted into the solution of a nonlinear optimization problem. The cost function of the nonlinear optimization problem is as follows: In the formula, J is the cost function; i is the time sequence number, N p is the prediction time domain; N c To control the time domain; Y ref (k+i|k) is the lateral reference position in the prediction time domain, Q and R are the output matrix and control weight matrix respectively; y(k+i|k) is the system prediction output at time k; u(k+i|k) is the control amount at time k; The nonlinear optimization problem is as follows: min J st(21)(23) (27).

Images