Narrow scene automatic driving track planning method based on dynamic puffing footprint model

Abstract

The invention relates to the technical field of automatic driving vehicle trajectory planning, and particularly discloses a narrow scene automatic driving trajectory planning method based on a dynamic puffing footprint model, and the technical key points are that the method is divided into two steps, the first step is trajectory planning problem modeling, and the second step is trajectory planning problem solving. As an unfavorable result of scratching an obstacle during closed-loop tracking of the open-loop track during approaching the obstacle for driving, compared with a plurality of defects that the traditional scheme is too conservatively aggressive or the degree of being far away from the obstacle cannot be directly controlled, the problem can be thoroughly solved; according to the method, trajectory planning problem modeling which accurately considers and prevents a factor of scratching of a close-range obstacle is realized through an original'dynamic puffing footprint model ', and then the problem model is efficiently solved, so that a safe driving trajectory meeting requirements can be obtained.

Description

Technical Field

[0001] The patent of this invention relates to the technical field of autonomous driving vehicle trajectory planning, and specifically, to a method for narrow scene autonomous driving trajectory planning based on a dynamic puffing footprint model. Background Art

[0002] The trajectory planning of an intelligent vehicle aims to generate a space-time trajectory that reflects the local path and speed of the vehicle, so that the vehicle can migrate from a given starting motion state (generally including vehicle position, posture, driving speed, acceleration, front wheel angle, and front wheel angular velocity, etc.) to a predetermined terminal motion state, in accordance with the vehicle's internal kinematic constraints and external environment obstacle avoidance constraints, and optimize the predetermined performance indicators (generally from the perspective of driving safety hazards, trajectory smoothness, and ride comfort) as much as possible while strictly ensuring that the above constraints are met.

[0003] When driving, vehicles often encounter narrow scenes, such as Figure 1 As shown. At the bottleneck of the scene, the vehicle should reduce speed appropriately and drive in the center, which means that the open-loop trajectory planning needs to generate a trajectory that reduces speed and does not get too close to obstacles, that is, a low-speed, centered driving trajectory. If the trajectory planning cannot meet the above requirements, then the planned open-loop trajectory will be difficult to be effectively tracked by the closed-loop control module, resulting in a collision risk.

[0004] In response to the above problems, the existing technologies on the market are roughly the following solutions:

[0005] The first one, such as Figure 2 As shown in the figure, the obstacle boundary is expanded outward, and the potential uncertain collision risk of edge driving is regarded as an actual collision and avoided. This means that both gray and black areas are regarded as obstacles, and then the trajectory generated by trajectory planning is required to avoid the gray + black areas in the environment. This method can effectively achieve centering driving in narrow areas and ensure that there will be no scratching accidents caused by driving too close to obstacles. However, once the gray buffer area is set too large, the width of the originally unobstructed channel will be lower than the width of the vehicle, thereby blocking the channel, which often induces the trajectory planning algorithm to fail.

[0006] The second type, such as Figure 3 As shown in the figure, a centered driving route is designed in advance along the bottleneck of the scene, and a penalty term is added to the cost function of the trajectory planning problem, thereby requiring the trajectory planning algorithm to drive along the reference route as much as possible. However, the reference route is a rough broken line that does not take into account the vehicle's mobility, and the trajectory generated by the trajectory planning may not be able to drive along the centered reference route. This means that this method can only indirectly induce the vehicle to drive in the center, but cannot directly, actively, and quantitatively achieve this task, and the effect is very poor in engineering practice.

[0007] In summary, there is no technical solution in the existing technology to completely solve the problem of trajectory planning results being close to narrow scene boundaries. The above two mainstream methods in the existing technology are essentially just some overly conservative (first type of method) or indirect (second type of method) processing on the outer layer of trajectory planning, which cannot completely solve the problem.

[0008] To this end, the inventors of the present application propose a narrow scene autonomous driving trajectory planning method based on a dynamic puffing footprint model to solve the above-mentioned problems in the prior art. Summary of the invention

[0009] The purpose of the present invention is to solve the technical problems existing in the prior art mentioned in the above background technology, and to provide a narrow scene autonomous driving trajectory planning method based on a dynamic puffing footprint model.

[0010] The above-mentioned purpose of the present invention is achieved like this:

[0011] The method for trajectory planning of autonomous driving in narrow scenarios based on a dynamic puffing footprint model includes the following steps:

[0012] S1. Trajectory planning problem model construction: It is constructed as a constrained dynamic optimization problem, which consists of decision variables, constraints and cost functions;

[0013] S2. Solving the trajectory planning problem: After constructing the trajectory planning problem model, it is solved, wherein a numerical optimization method is used to solve the dynamic optimization problem.

[0014] Furthermore, in step S1, the construction of the trajectory planning problem model is specifically as follows:

[0015] S1-1. Decision variables

[0016] 1) Define (x(t), y(t)) to represent the coordinates of a fixed reference point on the vehicle body in the Earth's inertial coordinate system at time t;

[0017] 2) Define v(t) and a(t) to represent the velocity and acceleration of the vehicle body reference point, respectively;

[0018] 3) Define φ(t) as the front wheel deflection angle of the vehicle, ω(t) as the angular velocity of the front wheel deflection angle, and θ(t) as the attitude angle of the vehicle in the earth's inertial coordinate system;

[0019] That is, the decision variables include the above x(t), y(t), v(t), a(t), φ(t), ω(t) and θ(t);

[0020] S1-2. Cost function

[0021] The cost function is used to reflect the stability and comfort of vehicle driving. Assuming that the time domain of trajectory planning is t∈[0,10s], the cost function is constructed as:

[0022]

[0023] Among them, w a ,w w >0 is the weight specified by the user;

[0024] S1-3. Constraints

[0025] The constraints in the dynamic optimization problem model include vehicle kinematic constraints, two-point boundary value constraints, and obstacle avoidance constraints, specifically:

[0026] a) Vehicle kinematic constraints:

[0027] The motion of the vehicle in the inertial coordinate system is subject to the following differential equations:

[0028]

[0029] b) Two-point boundary value constraints:

[0030] At the starting time t=0 in the trajectory planning time domain, the vehicle is in the current actual motion state. If the acceleration is a0, the velocity is v0, the attitude angle is theta0, the front wheel angle is phi0, the front wheel angular velocity is omega0, the horizontal coordinate position is x0, and the vertical coordinate position is y0, then the following constraints are imposed:

[0031]

[0032] If the vehicle is in the expected motion state at the end time t = 10s of the trajectory planning time domain, with acceleration af, velocity vf, attitude angle thetaf, front wheel angle phif, front wheel angular velocity omegaf, horizontal coordinate position xf, and vertical coordinate position yf, then the following constraints are imposed:

[0033]

[0034] c) Obstacle avoidance constraints:

[0035] At every moment in the trajectory planning time domain t∈[0,10s], the vehicle should effectively avoid collisions with obstacles in the environment, and eliminate the risk of small scratches between the vehicle and obstacles through obstacle avoidance constraints; a dynamic puffing footprint model is used, specifically:

[0036] 1) The shape of the body contour on the plane is regarded as a rectangle. The four vertices of the rectangular body contour are called A, B, C, and D respectively; the four points A, B, C, and D are functions of x(t), y(t), and θ(t), and the relationship is as follows:

[0037]

[0038] Where A=(x A ,y A ), B=(x B ,y B ), C=(x C ,y C ) and D=(x D ,y D ) represent the coordinates of vertices ABCD at time t;

[0039] 2) Based on the basic footprint ABCD, a dynamic expansion footprint model is introduced, through four variables e 左 (t)≥0, e 右 (t)≥0, e 前 (t)≥0 and e 后 (t) ≥ 0 controls the expansion scale of the dynamic puffing footprint A'B'C'D' compared to the basic footprint at time t;

[0040] 3) Define e using the following formula 左 (t), e 右 (t), e 前 (t) and e 后 (t), then:

[0041]

[0042] Among them, k1 and k2 are non-negative weight coefficients;

[0043] 4) Combining the above method, the coordinates of the four vertices A'B'C'D' of the vehicle's dynamic expansion footprint at time t are defined as follows: as well as

[0044]

[0045] According to the fact that the dynamically expanded body footprint A'B'C'D' does not collide with obstacles in the environment, assuming that the area occupied by the obstacles on the plane is a set S, the obstacle avoidance constraint can be constructed as follows:

[0046]

[0047] The trajectory generated by applying the obstacle avoidance constraint using formula (8) is used to ensure the most basic obstacle avoidance requirements; that is, the complete trajectory planning problem is constructed as the following dynamic optimization problem:

[0048]

[0049] Furthermore, in step S2, formula (9) is solved.

[0050] Furthermore, the specific steps of using the interior point algorithm to solve the constrained optimization problem are:

[0051] The dynamic optimization problem to be solved (9) can be concisely described as:

[0052]

[0053] Where χ represents the decision variable. By introducing the relaxation vector s>0, the inequality constraint g(χ)<0 in (A1) is transformed into an equality constraint, that is:

[0054]

[0055] At this point, if the only inequality constraint s>0 in Proposition (A2) is converted into an internal penalty function term and added to the cost function J, a standard form constrained optimization problem containing only equality constraints can be constructed:

[0056]

[0057] Among them, J μ (χ,s) represents the cost function including the internal penalty function term; μ IPM >0 is the barrier factor, and its value is closer to 0 + Then through the internal penalty function μ IPM ln(s) describes inequalities s>0 more accurately.

[0058] Furthermore, the execution flow of the interior point algorithm is as follows:

[0059] start;

[0060] Abstract the actual trajectory planning problem into an optimization problem (A1);

[0061] Initialize μ IPM ,threshold,

[0062] while(μ IPM >threshold),do

[0063] Update μ IPM ←μ IPM 0.5;

[0064] Under the current internal penalty function weight, (A1) is expressed as (A3);

[0065] The equality constraints in (A3) are inserted into the cost function of (A3) in the form of Lagrange multiplier polynomials, thereby converting (A3) into an unconstrained optimization problem (temporarily denoted as prob);

[0066] Solve the unconstrained optimization problem prob using Newton's method;

[0067] Derive Newton's method to solve the decision variable χ of prob;

[0068] endwhile

[0069] Output χ;

[0070] Finish.

[0071] The difficulty and significance of the technical solution of the present invention in solving the technical problem lies in: the present invention proposes a trajectory planning method dedicated to preventing the vehicle from driving close to the boundary of an obstacle, because driving close to an obstacle is more likely to result in the adverse result of scratching the obstacle when tracking the open-loop trajectory in a closed loop. Compared with the many defects of traditional solutions that are often too conservative and radical or cannot directly control the degree of distance from obstacles, the present invention can completely solve this problem. Its core idea is: through the original "dynamic puffing footprint model", the trajectory planning problem modeling that accurately considers the factors of preventing scratches from close-range obstacles can be achieved, and then the problem model can be efficiently solved to obtain a safe driving trajectory that meets the needs.

[0072] Compared with the prior art, the present invention has the following beneficial effects:

[0073] 1. The method of the present invention completely solves the problems that traditional solutions are often too conservative and radical or cannot directly control the degree of distance from obstacles;

[0074] 2. In the method of the present invention, the original "dynamic puffing footprint model" is used to accurately consider the factors of preventing scratches from close-range obstacles to achieve trajectory planning problem modeling, and then the problem model is efficiently solved to obtain a safe driving trajectory that meets the requirements;

[0075] 3. The method of the present invention proposes a new method to directly and thoroughly solve the problem that the planning results are too close to the edge of obstacles in narrow scenes at the trajectory planning level, which is convenient for improving the intelligence and safety level of autonomous driving vehicles. BRIEF DESCRIPTION OF THE DRAWINGS

[0076] Figure 1 It is a schematic diagram of a vehicle encountering a narrow scene during driving;

[0077] Figure 2 It is a schematic diagram of one of the trajectory planning methods in the prior art;

[0078] Figure 3 It is a schematic diagram of another trajectory planning method in the prior art;

[0079] Figure 4 is a schematic diagram of a dynamic puffing footprint model in an embodiment of the present invention;

[0080] Figure 5 It is a diagram showing the expansion scale effect of the basic footprint at time t in the dynamic puffing footprint model in an embodiment of the present invention. DETAILED DESCRIPTION

[0081] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0082] The implementation of the present invention is described in detail below in conjunction with specific embodiments. For those skilled in the art, the specific meanings of the above terms can be understood according to specific circumstances.

[0083] The following are preferred embodiments of the present invention:

[0084] This embodiment provides a trajectory planning method that aims to prevent vehicles from driving close to the boundary of obstacles, because when driving close to obstacles, it is more likely to have the unfavorable result of scratching obstacles when tracking the open-loop trajectory in a closed loop. Compared with the many defects of traditional solutions that are often too conservative and radical or cannot directly control the degree of distance from obstacles, the present invention can completely solve this problem. Its core idea is: through the original "dynamic puffing footprint model", the trajectory planning problem modeling that accurately considers the factors of preventing scratches from close-range obstacles can be achieved, and then the problem model can be efficiently solved to obtain a safe driving trajectory that meets the needs.

[0085] The method of the present invention is divided into two steps from the overall architecture. The first step is to model the trajectory planning problem, and the second step is to solve the trajectory planning problem. Specifically:

[0086] Step 1: Modeling of trajectory planning problem:

[0087] Building an accurate model for the trajectory planning problem is an important basis for implementing a smooth solution. The following are the methods for accurately building a trajectory planning problem that takes into account the factors of preventing driving too close to obstacles:

[0088] Like the modeling of all trajectory planning problems, it is constructed as a constrained dynamic optimization problem. This optimization problem consists of decision variables, constraints, and cost functions. The following is a detailed description of the modeling process:

[0089] Decision variables:

[0090] The decision variables in the dynamic optimization problem model include the following:

[0091] 1) Define (x(t), y(t)), which represents the coordinates of a fixed reference point on the vehicle body in the earth's inertial coordinate system at time t;

[0092] 2) Define v(t) and a(t) to represent the velocity and acceleration of the vehicle body reference point, respectively;

[0093] 3) Define φ(t) as the front wheel deflection angle of the vehicle, ω(t) as the angular velocity of the front wheel deflection angle, and θ(t) as the attitude angle of the vehicle in the earth's inertial coordinate system.

[0094] Therefore, the decision variables include the above-mentioned x(t), y(t), v(t), a(t), φ(t), ω(t) and θ(t).

[0095] Cost function:

[0096] The cost function in the dynamic optimization problem model is generally used to reflect the stability and comfort of vehicle driving. Assuming that the time domain of trajectory planning is t∈[0,10s], the cost function is generally constructed as:

[0097]

[0098] Among them, w a ,w w >0 is the weight specified by the user.

[0099] constraint:

[0100] The constraints in the dynamic optimization problem model include vehicle kinematic constraints, two-point boundary value constraints, and obstacle avoidance constraints. Among them, the vehicle kinematic constraints and the two-point boundary value constraints adopt common modeling methods, and the modeling method of the obstacle avoidance constraint is the creative design point of the present invention, which is as follows:

[0101] a) Vehicle kinematic constraints:

[0102] The motion of the vehicle in the inertial coordinate system is subject to the following differential equations:

[0103]

[0104] b) Two-point boundary value constraints:

[0105] At the starting time t=0 in the trajectory planning domain, the vehicle is in the current actual motion state. For example, at this time, the acceleration is a0, the speed is v0, the attitude angle is theta0, the front wheel angle is phi0, the front wheel angular velocity is omega0, the horizontal coordinate position is x0, and the vertical coordinate position is y0. Then the following constraints should be imposed:

[0106]

[0107] Similarly, at the end time t=10s of the trajectory planning time domain (the present invention always takes the time domain from 0 to 10 seconds as an example), the vehicle is in the expected motion state, for example, the acceleration is af, the speed is vf, the attitude angle is thetaf, the front wheel angle is phif, the front wheel angular velocity is omegaf, the horizontal coordinate position is xf, and the vertical coordinate position is yf, then the following constraints should be applied:

[0108]

[0109] c) Obstacle avoidance constraints:

[0110] The vehicle should effectively avoid collisions with obstacles in the environment at every moment in the trajectory planning time domain t∈[0,10s]. Furthermore, we hope that the obstacle avoidance constraint can eliminate the risk of small scratches between the vehicle and obstacles. To this end, the present invention proposes a dynamic puffing footprint model, the principle of which is as follows.

[0111] The shape of the body contour on the plane is regarded as a rectangle, and the four vertices of the rectangular body contour (generally referred to as the footprint in academia) are called A, B, C, and D (such as Figure 4 shown);

[0112] Obviously, these four points are functions of x(t), y(t), and θ(t), and the relationship is as follows:

[0113]

[0114] Where A=(x A ,y A ), B=(x B ,y B ), C=(x C ,y C ) and D=(x D ,y D ) represent the coordinates of vertices ABCD at time t.

[0115] Based on the basic footprint ABCD, the present invention introduces a dynamic expansion footprint model, through four variables e 左(t)≥0, e 右 (t)≥0, e 前 (t)≥0 and e 后 (t)≥0 controls the dynamic expansion footprint A'B'C'D' compared to the expansion scale of the basic footprint at time t, and its effect is as follows Figure 5 shown.

[0116] The method of the present invention uses the following formula (6) to define e 左 (t), e 右 (t), e 前 (t) and e 后 (t), which is the creative design point of the method of the present invention, wherein k1 and k2 are both non-negative weight coefficients.

[0117]

[0118] Combination Figure 4 and Figure 5 The coordinates of the four vertices A'B'C'D' of the vehicle's dynamic puffing footprint at time t can be defined as follows, where as well as

[0119]

[0120]

[0121] Based on this, we require that the dynamically expanded body footprint A'B'C'D' does not collide with obstacles in the environment. Assuming that the area occupied by obstacles on the plane is a set S, the obstacle avoidance constraint can be constructed as:

[0122]

[0123] The following is an analysis of the rationality of formula (8). The purpose of the analysis is to explain why the above constraint is novel and why it is effective in keeping the vehicle centered. First, at any time t, e 左 (t), e 右 (t), e 前 (t) and e 后 (t) are all non-negative values, so the dynamically expanded body footprint A'B'C'D' will not be less than the basic body footprint ABCD, which means that the most basic obstacle avoidance capability can be guaranteed. The trajectory generated by applying the obstacle avoidance constraint using formula 8 can guarantee the most basic obstacle avoidance requirements. On this basis, why is it said that the obstacle avoidance constraint corresponding to formula (8) can enable the vehicle to prevent the risk of scratching obstacles? The following is a further explanation:

[0124] According to our definition of φ(t), φ(t)>0 means the wheel turns left, and φ(t)<0 means the wheel turns right. Based on this conclusion, it is not difficult to find from formula (6) that when the front wheel turning angle is φ(t) is negative, e 左 (t) takes the value of 0, which means that if the front wheel of the vehicle is turning right, the left side of the vehicle footprint does not need to expand outward, and the expansion scale is 0. On the contrary, if the front wheel turning angle φ(t) is positive, e 左 The value of (t) is affected by the combined effect of the wheel deflection amplitude and the driving speed, which means that the faster the vehicle speed and the larger the wheel deflection amplitude, the larger the expansion to the left. This is very consistent with human driving experience and expectations: if you turn with a large amplitude and at a high speed, then if there is an obstacle in the near distance, once there is a slight error in the closed-loop tracking, it is very easy to cause a scratch or collision. 右 The analysis of (t) is mirror-symmetrical with the above process, so it will not be repeated here. When the vehicle is moving forward, v(t)>0, and the forward expansion scale e 前 (t) is related to vehicle speed. The faster the vehicle speed, the larger the expansion scale e 前 The larger (t) is, the 前 (t) is relatively large and building obstacle avoidance constraints based on it is beneficial to avoid forward collisions. On the contrary, if the vehicle is moving backward, v(t) < 0, and the probability of forward collision is 0. Therefore, according to Formula 6, it is not difficult to find that e 前 (t) = 0. For e 后 (t) analysis and e 前 (t) is symmetrical and therefore will not be described in detail.

[0125] Compared with the traditional method of arbitrarily and roughly applying a buffer zone or indirectly attracting a reference line, the present invention directly and accurately describes the cause of the risk of scratches in the obstacle avoidance constraint part: large-scale steering + high-speed driving, by applying obstacle avoidance constraints between the dynamic puffing footprint and the obstacle, it cleverly ensures that the vehicle will not come into close contact with the side where there is a risk of scratches in a dangerous manner. If the narrow area is really very narrow, then the puffing scale has to take a smaller amplitude, which will force decision variables such as vehicle speed and vehicle front wheel turning angle to take reasonable smaller values, which means that the vehicle is forced to reduce its speed in narrow areas of the environment. Since the values ​​of the four variables corresponding to the puffing scale at each moment are not fixed, and are related to the signs and amplitudes of state quantities such as vehicle speed and turning angle at the current moment, we call this method of flexibly adjusting the puffing footprint scale a "dynamic" puffing footprint model.

[0126] At this point, the complete trajectory planning problem is constructed as the following dynamic optimization problem:

[0127]

[0128] Step 2: Solving the trajectory planning problem

[0129] After successfully building the model of the trajectory planning problem (i.e., formula (9)), it should be solved. We use numerical optimization methods to solve the dynamic optimization problem, such as interior point algorithm or sequential quadratic programming algorithm. For example, interior point algorithm (see below):

[0130] A. and L.Biegler, "On the implementation of an interior-pointfilter line-search algorithm for large-scale nonlinear programming," Mathematical Programming, vol.106, no.1, pp.25–57, 2006.

[0131] The interior point algorithm is an optimization solution technology. The following takes the interior point algorithm as an example:

[0132] The following describes how the interior point algorithm solves constrained optimization problems.

[0133] First, the dynamic optimization problem (9) to be solved can be concisely described as

[0134]

[0135] Where χ represents the decision variable. By introducing the relaxation vector s>0, the inequality constraint g(χ)<0 in (A1) can be transformed into an equality constraint, that is,

[0136]

[0137] At this point, if the only inequality constraint s>0 in Proposition (A2) is converted into an internal penalty function term and added to the cost function J, a standard form constrained optimization problem containing only equality constraints can be constructed:

[0138]

[0139] Among them, J μ (χ,s) represents the cost function including the internal penalty function term; μ IPM >0 is the barrier factor, and its value is closer to 0 + Then through the internal penalty function μ IPM ln(s) describes inequalities s>0 more accurately.

[0140] The interior point algorithm solves Proposition (A1) by sequentially solving μ IPM The value gradually approaches 0+ The corresponding series of sub-problems are as follows (A3).

[0141] With μ IPM Approaching 0 + , the extreme value of the subproblem will tend to the extreme value of the original proposition (A1). In the process of sequential calculation, for a specific obstacle factor μ IPM The method for solving subproblem (A3) is Lagrange augmented Newton method. From the above analysis, we can see that the interior point algorithm solves NLP propositions through a two-layer loop architecture: in the outer loop, μ is updated. IPM , iteratively solve the given μ in the inner layer IPM Sub-problem (A3) under the condition.

[0142] Therefore, the execution of the interior point algorithm is as follows, and the pseudo code is given below:

[0143] start

[0144] Abstract the actual trajectory planning problem into an optimization problem (A1);

[0145] Initialize μ IPM ,threshold,

[0146] while(μ IPM >threshold),do

[0147] Update μ IPM ←μ IPM 0.5;

[0148] Under the current internal penalty function weight, (A1) is expressed as (A3);

[0149] The equality constraints in (A3) are inserted into the cost function of (A3) in the form of Lagrange multiplier polynomials, thereby converting (A3) into an unconstrained optimization problem (temporarily denoted as prob);

[0150] Solve the unconstrained optimization problem prob using Newton's method;

[0151] Derive Newton's method to solve the decision variable χ of prob;

[0152] end while

[0153] Output χ;

[0154] Finish.

[0155] In addition, the method of an embodiment of the present invention is based on statistical data from the long-term (13 months) use of autonomous driving mining trucks in open-pit mines. The use of this technology can reduce the number of scratch events triggered by the trajectory planning algorithm of autonomous driving mining trucks in narrow shovel pits from 0.13 per ten thousand to 0.002 per ten thousand.

[0156] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A trajectory planning method for autonomous driving in narrow scenarios based on a dynamic puffing footprint model. It is characterized in that The following steps are involved: S1. Trajectory planning problem model construction: It is constructed as a constrained dynamic optimization problem, which consists of decision variables, constraints and cost functions; S2. Solving the trajectory planning problem: After constructing the trajectory planning problem model, it is solved, wherein a numerical optimization method is used to solve the dynamic optimization problem.

2. According to claim 1, the method for autonomous driving trajectory planning in narrow scenarios based on the dynamic puffing footprint model, It is characterized in that In step S1, the construction of the trajectory planning problem model is specifically as follows: S1-1. Decision variables 1) Define (x(t), y(t)) to represent the coordinates of a fixed reference point on the vehicle body in the Earth's inertial coordinate system at time t; 2) Define v(t) and a(t) to represent the velocity and acceleration of the vehicle body reference point, respectively; 3) Define φ(t) as the front wheel deflection angle of the vehicle, ω(t) as the angular velocity of the front wheel deflection angle, and θ(t) as the attitude angle of the vehicle in the geodetic inertial coordinate system; That is, the decision variables include the above x(t), y(t), v(t), a(t), φ(t), ω(t) and θ(t); S1-2. Cost function The cost function is used to reflect the stability and comfort of vehicle driving. Assuming that the time domain of trajectory planning is t∈[0,10s], the cost function is constructed as: Among them, w a ,w w >0 is the weight specified by the user; S1-3. Constraints The constraints in the dynamic optimization problem model include vehicle kinematic constraints, two-point boundary value constraints, and obstacle avoidance constraints, specifically: a) Vehicle kinematic constraints: The motion of the vehicle in the inertial coordinate system is subject to the following differential equations: b) Two-point boundary value constraints: At the starting time t=0 in the trajectory planning time domain, the vehicle is in the current actual motion state. If the acceleration is a0, the velocity is v0, the attitude angle is theta0, the front wheel angle is phi0, the front wheel angular velocity is omega0, the horizontal coordinate position is x0, and the vertical coordinate position is y0, then the following constraints are imposed: If the vehicle is in the expected motion state at the end time t = 10s of the trajectory planning time domain, with acceleration af, velocity vf, attitude angle thetaf, front wheel angle phif, front wheel angular velocity omegaf, horizontal coordinate position xf, and vertical coordinate position yf, then the following constraints are imposed: c) Obstacle avoidance constraints: At every moment in the trajectory planning time domain t∈[0,10s], the vehicle should effectively avoid collisions with obstacles in the environment, and eliminate the risk of small scratches between the vehicle and obstacles through obstacle avoidance constraints; a dynamic puffing footprint model is used, specifically: 1) The shape of the body contour on the plane is regarded as a rectangle. The four vertices of the rectangular body contour are called A, B, C, and D respectively; the four points A, B, C, and D are functions of x(t), y(t), and θ(t), and the relationship is as follows: Where A=(x A ,y A ), B=(x B ,y B ), C=(x C ,y C ) and D=(x D ,y D ) represent the coordinates of vertex ABCD at time t; 2) Based on the basic footprint ABCD, a dynamic expansion footprint model is introduced, through four variables e 左 (t)≥0, e 右 (t)≥0, e 前 (t)≥0 and e 后 (t) ≥ 0 controls the expansion scale of the dynamic expansion footprint A'B'C'D' compared to the basic footprint at time t; 3) Define e using the following formula 左 (t), e 右 (t), e 前 (t) and e 后 (t), then: where k 1 , k 2 are all non-negative weight coefficients; 4) Combining the above method, the coordinates of the four vertices A'B'C'D' of the vehicle's dynamic expansion footprint at time t are defined as follows: as well as According to the fact that the dynamically expanded body footprint A'B'C'D' does not collide with obstacles in the environment, assuming that the area occupied by the obstacles on the plane is a set S, the obstacle avoidance constraint can be constructed as follows: The trajectory generated by applying the obstacle avoidance constraint using formula (8) is used to ensure the most basic obstacle avoidance requirements; that is, the complete trajectory planning problem is constructed as the following dynamic optimization problem:

3. The method for autonomous driving trajectory planning in narrow scenarios based on a dynamic puffing footprint model according to claim 2, It is characterized in that In step S2, formula (9) is solved.

4. The method for autonomous driving trajectory planning in narrow scenarios based on a dynamic puffing footprint model according to claim 3, It is characterized in that The specific steps of using interior point algorithm to solve constrained optimization problems are: The dynamic optimization problem to be solved (9) can be concisely described as: Where χ represents the decision variable. By introducing the relaxation vector s>0, the inequality constraint g(χ)<0 in (A1) is transformed into an equality constraint, that is: At this point, if the only inequality constraint s>0 in Proposition (A2) is converted into an internal penalty function term and added to the cost function J, a standard form constrained optimization problem containing only equality constraints can be constructed: Among them, J μ (χ,s) represents the cost function including the internal penalty function term; μ IPM >0 is the barrier factor, and its value is closer to 0 + Then through the internal penalty function μ IPM ln(s) describes inequalities s>0 more accurately.

5. According to claim 4, the method for autonomous driving trajectory planning in narrow scenarios based on a dynamic puffing footprint model, It is characterized in that The execution flow of the interior point algorithm is as follows: start; Abstract the actual trajectory planning problem into an optimization problem (A1); Initialize μ IPM ,threshold, while(μ IPM >threshold),do Update μ IPM ←μ IPM 0.5; Under the current internal penalty function weight, (A1) is expressed as (A3); The equality constraints in (A3) are inserted into the cost function of (A3) in the form of Lagrange multiplier polynomials, thereby converting (A3) into an unconstrained optimization problem (temporarily denoted as prob); Solve the unconstrained optimization problem prob using Newton's method; Derive Newton's method to solve the decision variable χ of prob; endwhile Output χ; Finish.

Images