CN105691388B - A kind of Automotive active anti-collision system and its method for planning track - Google Patents

Abstract

The invention discloses a vehicle active collision avoidance system and a trajectory planning method thereof. The system includes a forward-looking radar, a camera, a vehicle speed sensor, a yaw rate sensor, a center-of-mass side slip angle sensor, a signal processing module, an electronic control power supply ECU, and a throttle control controller, steering controller, brake controller. During the driving process of the car, the electronic control unit collects the signals from various sensors through the signal processing module in real time, and judges the current road conditions and vehicle conditions of the car in real time. If a dangerous situation may occur at this time, the ECU executes the internal preset The predetermined trajectory planning program generates a continuous non-collision executable trajectory, and outputs relevant signals to the accelerator controller, steering controller and brake controller for corresponding operations to avoid dangerous situations. The invention can assist the driver to operate the car in an emergency, and can improve the driving active safety performance.

Description

A vehicle active collision avoidance system and its trajectory planning method

technical field

The invention relates to the field of automobile auxiliary driving, in particular to an automobile active collision avoidance system and a trajectory planning method thereof.

Background technique

With the rise of intelligent transportation on a global scale, automobile assisted driving technology has received more and more attention. The main purpose of its research is to reduce the incidence of increasingly serious traffic accidents and improve the efficiency of existing road traffic. Numerous research institutions and industrial design units in the world are investing a lot of manpower, material and financial resources in the research and development process of related key technologies.

Active collision avoidance system is an important research content of automobile assisted driving technology. The main purpose of its research is to improve the safety performance of vehicle driving. It mainly uses modern information technology and sensor technology to expand the driver's perception ability and integrate external information (such as vehicle speed, obstacle distance, speed, direction, etc.) are transmitted to the driver while comprehensively utilizing the vehicle condition and road condition information to judge the safety of the current operating condition of the vehicle, and automatically take measures to control the vehicle in an emergency, so that the vehicle takes the initiative Avoid the danger as much as possible, ensure the safe driving of the car or reduce the degree of injury of the accident to the greatest possible extent. Only when the car has such active safety performance can it be possible to fundamentally reduce traffic accidents and improve traffic safety.

Trajectory planning technology is a key technology in the active collision avoidance system. In order to realize the intelligent control of the vehicle, the prerequisite is to generate a feasible reference trajectory and provide the parameters of the trajectory to the tracking controller to facilitate the control. The controller can control the car to drive according to the planned trajectory. Therefore, how to plan a feasible collision-free trajectory in an emergency is particularly important.

Contents of the invention

The technical problem to be solved by the present invention is to provide a kind of automobile active collision avoidance system and its trajectory planning method for the defects involved in the background technology, which solves the trajectory planning problem of the active collision avoidance system in emergency situations. The combined method can effectively avoid obstacles and avoid traffic accidents while ensuring the steering stability of the car, and realizes the active safety function of the car.

The present invention adopts the following technical solutions for solving the problems of the technologies described above:

An active collision avoidance system for a car, including a forward-looking radar, a camera, a signal processing module, a vehicle speed sensor, a yaw rate sensor, a center-of-mass side slip angle sensor, a steering wheel angle sensor, an electronic control power supply ECU, a throttle controller, a steering controller and brake controller;

The forward-looking radar and the camera are connected to the electronic control power supply ECU through a signal processing module; , the steering controller and the brake controller are connected;

Both the forward-looking radar and the camera are installed in front of the car to detect the road conditions in front of the car, and the measured signal is processed by the signal processing module and then transmitted to the electronic control unit ECU;

The vehicle speed sensor, the yaw rate sensor, the center of mass side slip angle sensor, and the steering wheel angle sensor are respectively used to sense the speed, yaw rate, center of mass side slip angle and front wheel steering angle of the vehicle, and the collected signals are processed Send to the electronic control unit ECU;

The electronic control unit ECU is used to output corresponding signals to the throttle controller, steering controller, and brake controller according to the received signals, and perform corresponding acceleration, deceleration, and braking operations to ensure driving safety.

The present invention also discloses a trajectory planning method based on the above active collision avoidance system for automobiles, which includes the following steps:

Step 1), obtain the distance, speed, acceleration and width of the obstacle in front of the car through the forward-looking radar and the camera, and compare the distance between the obstacle in front and the car with the preset safety distance threshold, if it is less than the preset Set the safety distance threshold, then perform step 2);

Step 2), obtain the speed of the car, the yaw rate, the side slip angle of the center of mass and the front wheel steering angle through the vehicle speed sensor, the yaw rate sensor, the center of mass side slip angle sensor, and the steering wheel angle sensor;

Step 3), according to the yaw angle of the car, the steering angle of the front wheels, the longitudinal speed, the distance between the front axle and the rear axle, and the longitudinal and lateral coordinates of the midpoint of the rear axle, the three-degree-of-freedom kinematics model of the car is established;

Step 4), parameterize the trajectory to be generated with a polynomial of degree seven;

Step 5), according to the vehicle three-degree-of-freedom kinematics model and the parameterized trajectory to be generated, set the trajectory optimization model constraints, set the objective function and optimization variables, and according to the vehicle's longitudinal velocity, yaw rate, center of mass sideslip angle, The front wheel steering angle, and the distance, speed, and acceleration of obstacles in front of the car are solved to obtain the trajectory optimization model;

Step 6), based on the dynamic particle swarm optimization algorithm, the established trajectory optimization model is solved to obtain the planned trajectory.

As a further optimization scheme of the trajectory planning method of the vehicle active collision avoidance system, the vehicle three-degree-of-freedom kinematics model described in step 3) is established according to the following formula:

Among them, x and y are the longitudinal coordinates and lateral coordinates of the midpoint of the rear axle of the car respectively, θ is the yaw angle of the car, δ is the steering angle of the front wheels of the car, v is the longitudinal speed of the car, and l is the distance between the front axle and the rear axle of the car. The spacing between the axes, t is the current time of trajectory planning.

As a further optimization scheme of the trajectory planning method of this automobile active collision avoidance system, the trajectory equation of the seventh degree polynomial parameterization described in step 5) is:

Among them, x _d0 , x _d1 , x _d2 , x _d3 , x _d4 , x _d5 , x _d6 , x _d7 , y _d0 , y _d1 , y _d2 , y _d3 , y _d4 , y _d5 , y _d6 , y _d7 are Undetermined coefficients of the polynomial, (x _d (t), y _d (t)) is the trajectory to be generated.

As a further optimization scheme of the trajectory planning method of the vehicle active collision avoidance system, the constraints of the trajectory optimization model described in step 6 are:

(R ₀ +R ₁ ) ² ≤[PL ^-1 (H ₁ -Mx _d6 -Nx _d7 )+x _d6 t ⁶ +x _d7 t ⁷ -x ₀ -v _x (tt ₀ )] ²

+[PL ^-1 (H ₂ -My _d6 -Ny _d7 )+y _d6 t ⁶ +y _d7 t ⁷ -y ₀ -v _y (tt ₀ )] ² ;

in,

P = [1 tt ² t ³ t ⁴ t ⁵ ],

t ₀ is the initial time of trajectory planning, t _f is the end time of trajectory planning, is the state of the car at the initial time t ₀ , is the state of the car at the final moment t _f ;

R ₀ is half the length of the car, R ₁ is half the width of the obstacle;

The objective function is Among them, w ₁ and w ₂ are weight coefficients, and w ₁ +w ₂ =1; a _y is the lateral acceleration of the vehicle;

The optimized variables are x _d6 , x _d7 , y _d6 , y _d7 .

Compared with the prior art, the present invention adopts the above technical scheme and has the following technical effects:

1. The trajectory generated by the trajectory planning method of the present invention satisfies various incomplete constraints and actuator constraints;

2. The trajectory trajectory curvature generated by the trajectory planning method of the present invention has continuity, has dynamic real-time performance, and can adapt to dynamically changing road environments;

3. By tracking the trajectory generated by the trajectory planning method of the present invention, the vehicle can effectively avoid obstacles and prevent traffic accidents.

Description of drawings

Fig. 1 is a schematic structural diagram of the active collision avoidance system of the present invention;

Fig. 2 is a schematic diagram of the active collision avoidance process of the present invention;

Fig. 3 is the three-degree-of-freedom kinematics model of the automobile of the present invention.

detailed description

Below in conjunction with accompanying drawing, technical scheme of the present invention is described in further detail:

As shown in Figure 1, the present invention discloses an active collision avoidance system for automobiles, comprising a forward-looking radar, a camera, a signal processing module, a vehicle speed sensor, a yaw rate sensor, a side slip angle sensor, a steering wheel angle sensor, and an electronic control unit ECU, accelerator controller, steering controller and brake controller;

The forward-looking radar and the camera are connected to the electronic control power supply ECU through a signal processing module; , the steering controller and the brake controller are connected;

Both the forward-looking radar and the camera are installed in front of the car to detect the road conditions in front of the car, and the measured signal is processed by the signal processing module and then transmitted to the electronic control unit ECU;

The vehicle speed sensor, the yaw rate sensor, the center of mass side slip angle sensor, and the steering wheel angle sensor are respectively used to sense the speed, yaw rate, center of mass side slip angle and front wheel steering angle of the vehicle, and the collected signals are processed Send to the electronic control unit ECU;

The electronic control unit ECU is used to output corresponding signals to the throttle controller, steering controller, and brake controller according to the received signals, and perform corresponding acceleration, deceleration, and braking operations to ensure driving safety.

The invention also discloses a trajectory planning method based on the automobile active collision avoidance system, which includes the following specific steps:

Step 1. Obtain the distance, speed, acceleration and width of the obstacle in front of the car through the forward-looking radar and camera, and compare the distance between the obstacle in front of the car and the preset safety distance threshold, if it is less than the preset If the safety distance threshold is set, go to step 2.

Step 2. Obtain the longitudinal speed, yaw rate, side slip angle and front wheel steering angle of the car through the vehicle speed sensor, yaw rate sensor, mass center slip angle sensor, and steering wheel angle sensor.

Step 3. Establish a three-degree-of-freedom kinematics model of the car, as shown in Figure 3:

Among them, x and y are the longitudinal coordinates and lateral coordinates of the midpoint of the rear axle of the car respectively, θ is the yaw angle of the car, δ is the steering angle of the front wheels of the car, v is the longitudinal speed of the car, and l is the distance between the front axle and the rear axle of the car. The spacing between the axes, t is the current time of trajectory planning.

Step 4, enter the cycle.

Step 5. Set the initial time of trajectory planning as t ₀ , and the end time of trajectory planning as t _f , and parameterize the trajectory to be generated with a polynomial of degree seven:

Among them, x _d0 , x _d1 , x _d2 , x _d3 , x _d4 , x _d5 , x _d6 , x _d7 , y _d0 , y _d1 , y _d2 , y _d3 , y _d4 , y _d5 , y _d6 , y _d7 are The undetermined coefficients of the polynomial.

Step 6. Set trajectory optimization model constraint conditions, objective function and optimization variables according to the vehicle three-degree-of-freedom kinematics model and the parameterized trajectory to be generated, and according to the vehicle's longitudinal velocity, yaw rate, center of mass sideslip angle, front The wheel steering angle, and the distance, speed, and acceleration of obstacles in front of the car are solved to obtain the trajectory optimization model:

1) Constraints:

Suppose the state of vehicle A at the initial time _t0 is The state of vehicle A at the final moment t _f is And the designed trajectory is (x _d (t), y _d (t)). Then, according to the vehicle kinematics model (1), the equation constraints imposed on the designed trajectory are as follows:

Substituting the trajectory equation (2) into the equality constraint (3) and transforming it into a matrix form, its coefficients can be determined by the following equations:

in,

and

Substituting equation (4) into trajectory equation (2) can obtain a further expression of trajectory equation:

where P = [1 tt ² t ³ t ⁴ t ⁵ ].

In order to achieve the requirements of collision avoidance, some inequality constraints need to be satisfied:

(R ₀ +R ₁ ) ² ≤[x _d (t)-x ₀ -v _x (tt ₀ )] ² +[y _d (t)-y ₀ -v _y (tt ₀ )] ² (6)

Among them, R ₀ is half of the length of the car, and R ₁ is half of the width of the obstacle;

Substituting equation (5) into (6) yields a further expression:

(R ₀ +R ₁ ) ² ≤[PL ^-1 (H ₁ -Mx _d6 -Nx _d7 )+x _d6 t ⁶ +x _d7 t ⁷ -x ₀ -v _x (tt ₀ )] ²

+[PL ^-1 (H ₂ -My _d6 -Ny _d7 )+y _d6 t ⁶ +y _d7 t ⁷ -y ₀ -v _y (tt ₀ )] ² (7)

2) The objective function, generally speaking, in the process of active collision avoidance of smart cars, the planned trajectory must meet some conditions, for example, to effectively avoid obstacles while ensuring the stability of the vehicle, based on this consideration, choose the following function as the objective function for optimization:

Among them, w ₁ and w ₂ are weight coefficients, and w ₁ +w ₂ =1; a _y is the lateral acceleration of the vehicle;

3) Optimizing variables, it is easy to see from equation (5), the variables to be optimized are: x _d6 , x _d7 , y _d6 , y _d7 .

Step 7. Based on the dynamic particle swarm optimization algorithm, solve the established trajectory optimization model to obtain the required trajectory:

Particle swarm optimization algorithm, also known as particle swarm optimization algorithm, is based on a group of particles, and the solution to each optimization problem is to find a particle in its feasible space. In order to allow particles to search globally and maintain particle diversity, the present invention uses dynamic particle swarm optimization (dynamic Particle Swarm Optimization, DPSO) to optimize the trajectory optimization model.

If the D-dimensional space position vector of the particle swarm is x _i =(x _i1 , x _i2 ,..., x _iD ), each x _i represents a potential feasible solution in the solution space, which can be calculated according to the objective function value to judge whether it is the optimal solution. The D-dimensional space velocity vector of the i-th particle is v _i =(v _i1 ,v _i2 ,...,v _iD ), the individual optimal position of the i-th particle P _i =(P _i1 ,P _i2 ,... ,P _iD ), the optimal position of particle swarm swarm L _i =(L _i1 ,L _i2 ,...,L _iD ), the global optimal position of particle swarm swarm G=(G ₁ ,G ₂ ,...,G _D ) The iteration formula is as follows:

v _i (t+1)＝ωv _i t+b ₁ r ₁ (p _i (t)-x _i (t))+b ₂ r ₂ (L _i (t)-x _i (t))+b ₂ r ₃ (G(t) _-xi (t)) (9)

In the formula: b ₁ , b ₂ , b ₃ are normal numbers; r ₁ , r ₂ , r ₃ are random numbers in [0,1]; parameter ω is the inertia factor.

Assuming that ω decreases linearly from ω _s to ω _e according to the number of cycles, the maximum number of cycles is I _max , and the current number of cycles is I _c , then the value of ω can be obtained by the following formula:

In the formula: ω _s is the inertia factor at the beginning of optimization; ω _e is the inertia factor at the end of optimization.

The position of the particle in the particle swarm at time t+1 is obtained by the following formula:

x _i (t+1) = x _i (t) + v _i (t+1) (11)

If the particle swarm is updated beyond the boundary of the domain of definition, the position of the particle needs to be readjusted so that it falls within the decision space, and the new position can be calculated according to the following formula:

x _i (t+1)= _xi (t)+λv _i (t+1) (12)

λ=2/(γ ² +2) (13)

In the formula: λ is the speed adjustment coefficient, which is between (0,1); γ is the number of adjustments, and when γ is greater than 3, the particle speed becomes reversed.

The distance between particles i and k ||x _i -x _k || can be obtained by the following formula:

In the formula: d is the dimension of the decision variable.

Generation of dynamic particle swarms: If m particle swarms have been generated, assuming that the nearest particle swarm to particle swarm a is b, if the distance between them is greater than D _max , then a particle swarm x _m+1 needs to be generated, the first in the group The k-th dimension component of the i particle It can be obtained by the following formula:

In the formula: C ₁ and C ₂ are random numbers within [0,1]; round(·) is a rounding function, therefore, round(0.5+C ₂ ) is 0 or 1.

Step 8. The ECU outputs relevant parameters of the generated trajectory to the accelerator controller, steering controller, and brake controller, and performs corresponding operations to accurately track the generated trajectory.

Step 9. Jump to step 4 to solve and track the trajectory at the next moment. This cycle continues until the planning is completed and the entire collision avoidance process is completed, as shown in Figure 2.

Those skilled in the art can understand that, unless otherwise defined, all terms (including technical terms and scientific terms) used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention belongs. It should also be understood that terms such as those defined in commonly used dictionaries should be understood to have a meaning consistent with the meaning in the context of the prior art, and will not be interpreted in an idealized or overly formal sense unless defined as herein Explanation.

The specific embodiments described above have further described the purpose, technical solutions and beneficial effects of the present invention in detail. It should be understood that the above descriptions are only specific embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present invention shall be included within the protection scope of the present invention.

Claims (4)

1. A trajectory planning method of an automobile active collision avoidance system, characterized in that, the automobile active collision avoidance system comprises a forward-looking radar, a camera, a signal processing module, a vehicle speed sensor, a yaw rate sensor, a side slip angle sensor, Steering wheel angle sensor, electronic control power supply ECU, accelerator controller, steering controller and brake controller; The forward-looking radar and the camera are connected to the electronic control power supply ECU through a signal processing module; , the steering controller and the brake controller are connected; Both the forward-looking radar and the camera are installed in front of the car to detect the road conditions in front of the car, and the measured signal is processed by the signal processing module and then transmitted to the electronic control unit ECU; The vehicle speed sensor, the yaw rate sensor, the center of mass side slip angle sensor, and the steering wheel angle sensor are respectively used to sense the speed, yaw rate, center of mass side slip angle and front wheel steering angle of the vehicle, and the collected signals are processed Send to the electronic control unit ECU; The electronic control unit ECU is used to output corresponding signals to the throttle controller, steering controller, and brake controller according to the received signals, and perform corresponding acceleration, deceleration, and braking operations to ensure driving safety; The trajectory planning method of the automobile active collision avoidance system comprises the following steps: Step 1), obtain the distance, speed, acceleration and width of the obstacle in front of the car through the forward-looking radar and the camera, and compare the distance between the obstacle in front and the car with the preset safety distance threshold, if it is less than the preset Set the safety distance threshold, then perform step 2); Step 2), obtain the speed of the car, the yaw rate, the side slip angle of the center of mass and the front wheel steering angle through the vehicle speed sensor, the yaw rate sensor, the center of mass side slip angle sensor, and the steering wheel angle sensor; Step 3), according to the yaw angle of the car, the steering angle of the front wheels, the longitudinal speed, the distance between the front axle and the rear axle, and the longitudinal and lateral coordinates of the midpoint of the rear axle, the three-degree-of-freedom kinematics model of the car is established; Step 4), parameterize the trajectory to be generated with a polynomial of degree seven; Step 5), according to the vehicle three-degree-of-freedom kinematics model and the parameterized trajectory to be generated, set the trajectory optimization model constraints, set the objective function and optimization variables, and according to the vehicle's longitudinal velocity, yaw rate, center of mass sideslip angle, The front wheel steering angle, and the distance, speed, and acceleration of obstacles in front of the car are solved to obtain the trajectory optimization model; Step 6), based on the dynamic particle swarm optimization algorithm, the established trajectory optimization model is solved to obtain the planned trajectory. 2. the trajectory planning method of automobile active collision avoidance system according to claim 1, is characterized in that, set up the automobile three-degree-of-freedom kinematics model described in step 3) according to following formula: <mfenced open = "{" close = ""><mtable><mtr><mtd><mrow><mover><mi>x</mi><mo>&amp;CenterDot;</mo></mover><mo>=</mo><mi>v</mi><mi>cos</mi><mrow><mo>(</mo><mi>&amp;theta;</mi><mo>)</mo></mrow></mrow></mtd></mtr><mtr><mtd><mrow><mover><mi>y</mi><mo>&amp;CenterDot;</mo></mover><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>=</mo><mi>v</mo>mi><mi>sin</mi><mrow><mo>(</mo><mi>&amp;theta;</mi><mo>)</mo></mrow></mrow></mtd></mtr><mtr><mtd><mrow><mover><mi>&amp;theta;</mi><mo>&amp;CenterDot;</mo></mover><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>=</mo><mi>v</mi><mi>tan</mi><mi>&amp;delta;</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>/</mo><mi>l</mi></mrow></mtd></mtr></mtable></mfenced> Among them, x and y are the longitudinal coordinates and lateral coordinates of the midpoint of the rear axle of the car respectively, θ is the yaw angle of the car, δ is the steering angle of the front wheels of the car, v is the longitudinal speed of the car, and l is the distance between the front axle and the rear axle of the car. The spacing between the axes, t is the current time of trajectory planning. 3. the trajectory planning method of automobile active collision avoidance system according to claim 2, is characterized in that, step 5) described in the trajectory equation of the seven degree polynomial parameterization is: <mfenced open = "{" close = ""><mtable><mtr><mtd><mrow><msub><mi>x</mi><mi>d</mi></msub><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>=</mo><msub><mi>x</mi><mrow><mi>d</mi><mn>0</mn></mrow></msub><mo>+</mo><msub><mi>x</mi><mrow><mi>d</mi><mn>1</mn></mrow></msub><mi>t</mi><mo>+</mo><msub><mi>x</mi><mrow><mi>d</mi><mn>2</mn></mrow></msub><msup><mi>t</mi><mn>2</mn></msup><mo>+</mo><msub><mi>x</mi><mrow><mi>d</mi><mn>3</mn></mrow></msub><msup><mi>t</mi><mn>3</mn></msup><mo>+</mo><msub><mi>x</mi><mrow><mi>d</mi><mn>4</mn></mrow></msub><msup><mi>t</mi><mn>4</mn></msup><mo>+</mo><msub><mi>x</mi><mrow><mi>d</mi><mn>5</mn></mrow></msub><msup><mi>t</mi><mn>5</mn></msup><mo>+</mo><msub><mi>x</mi><mrow><mi>d</mi><mn>6</mn></mrow></msub><msup><mi>t</mi><mn>6</mn></msup><mo>+</mo><msub><mi>x</mi><mrow><mi>d</mi><mn>7</mn></mrow></msub><msup><mi>t</mi><mn>7</mn></msup></mrow></mtd></mtr><mtr><mtd><mrow><msub><mi>y</mi><mi>d</mi></msub><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>=</mo><msub><mi>y</mi><mrow><mi>d</mi><mn>0</mn></mrow></msub><mo>+</mo><msub><mi>y</mi><mrow><mi>d</mi><mn>1</mn></mrow></msub><mi>t</mi><mo>+</mo><msub><mi>y</mi><mrow><mi>d</mi><mn>2</mn></mrow></msub><msup><mi>t</mi><mn>2</mn></msup><mo>+</mo><msub><mi>y</mi><mrow><mi>d</mi><mn>3</mn></mrow></msub><msup><mi>t</mi><mn>3</mn></msup><mo>+</mo><msub><mi>y</mi><mrow><mi>d</mi><mn>4</mn></mrow></msub><msup><mi>t</mi><mn>4</mn></msup><mo>+</mo><msub><mi>y</mi><mrow><mi>d</mi><mn>5</mn></mrow></msub><msup><mi>t</mi><mn>5</mn></msup><mo>+</mo><msub><mi>y</mi><mrow><mi>d</mi><mn>6</mn></mrow></msub><msup><mi>t</mi><mn>6</mn></msup><mo>+</mo><msub><mi>y</mi><mrow><mi>d</mi><mn>7</mn></mrow></msub><msup><mi>t</mi><mn>7</mn></msup></mrow></mtd></mtr></mtable></mfenced> 1 Among them, x _d0 , x _d1 , x _d2 , x _d3 , x _d4 , x _d5 , x _d6 , x _d7 , y _d0 , y _d1 , y _d2 , y _d3 , y _d4 , y _d5 , y _d6 , y _d7 are The undetermined coefficients of the polynomial, (x _d (t), y _d (t)) are the vertical and horizontal coordinates of the trajectory to be generated. 4. the trajectory planning method of automobile active collision avoidance system according to claim 3, is characterized in that, the constraint condition of the trajectory optimization model described in step 6 is: <mfenced open = "{" close = ""><mtable><mtr><mtd><mrow><msub><mi>x</mi><mi>d</mi></msub><mrow><mo>(</mo><msub><mi>t</mi><mn>0</mn></msub><mo>)</mo></mrow><mo>=</mo><msub><mi>x</mi><mn>0</mn></msub></mrow></mtd></mtr><mtr><mtd><mrow><msub><mover><mi>x</mi><mo>&amp;CenterDot;</mo></mover><mi>d</mi></msub><mrow><mo>(</mo><msub><mi>t</mi><mn>0</mn></msub><mo>)</mo></mrow><mo>=</mo><msub><mi>v</mi><mn>0</mn></msub><msub><mi>cos&amp;theta;</mi><mn>0</mn></msub></mrow></mtd></mtr><mtr><mtd><mrow><msub><mover><mi>x</mi><mo>&amp;CenterDot;&amp;CenterDot;</mo></mover><mi>d</mi></msub><mrow><mo>(</mo><msub><mi>t</mi><mn>0</mn></msub><mo>)</mo></mrow><mo>=</mo><msub><mover><mi>v</mi><mo>&amp;CenterDot;</mo></mover><mn>0</mn></msub><msub><mi>cos&amp;theta;</mi><mn>0</mn></msub><mo>-</mo><msubsup><mi>v</mi><mn>0</mn><mn>2</mn></msubsup><msub><mi>tan&amp;delta;</mi><mn>0</mn></msub><msub><mi>sin&amp;theta;</mi><mn>0</mn></msub><mo>/</mo><mi>l</mi></mrow></mtd></mtr><mtr><mtd><mrow><msub><mi>x</mi><mi>d</mi></msub><mrow><mo>(</mo><msub><mi>t</mi><mi>f</mi></msub><mo>)</mo></mrow><mo>=</mo><msub><mi>x</mi><mi>f</mi></msub></mrow></mtd></mtr><mtr><mtd><mrow><msub><mover><mi>x</mi><mo>&amp;CenterDot;</mo></mover><mi>d</mi></msub><mrow><mo>(</mo><msub><mi>t</mi><mi>f</mi></msub><mo>)</mo></mrow><mo>=</mo><msub><mi>v</mi><mi>f</mi></msub><msub><mi>cos&amp;theta;</mi><mi>f</mi></msub></mrow></mtd></mtr><mtr><mtd><mrow><msub><mover><mi>x</mi><mo>&amp;CenterDot;&amp;CenterDot;</mo></mover><mi>d</mi></msub><mrow><mo>(</mo><msub><mi>t</mi><mi>f</mi></msub><mo>)</mo></mrow><mo>=</mo><msub><mover><mi>v</mi><mo>&amp;CenterDot;</mo></mover><mi>f</mi></msub><msub><mi>cos&amp;theta;</mi><mi>f</mi></msub><mo>-</mo><msubsup><mi>v</mi><mi>f</mi><mn>2</mn></msubsup><msub><mi>tan&amp;delta;</mi><mi>f</mi></msub><msub><mi>sin&amp;theta;</mi><mi>f</mi></msub><mo>/</mo><mi>l</mi></mrow></mtd></mtr><mtr><mtd><mrow><msub><mi>y</mi><mi>d</mi></msub><mrow><mo>(</mo><msub><mi>t</mi><mn>0</mn></msub><mo>)</mo></mrow><mo>=</mo><msub><mi>y</mi><mn>0</mn></msub></mrow></mtd></mtr><mtr><mtd><mrow><msub><mover><mi>y</mi><mo>&amp;CenterDot;</mo></mover><mi>d</mi></msub><mrow><mo>(</mo><msub><mi>t</mi><mn>0</mn></msub><mo>)</mo></mrow><mo>=</mo><msub><mi>v</mi><mn>0</mn></msub><msub><mi>sin&amp;theta;</mi><mn>0</mn></msub></mrow></mtd></mtr><mtr><mtd><mrow><msub><mover><mi>y</mi><mo>&amp;CenterDot;&amp;CenterDot;</mo></mover><mi>d</mi></msub><mrow><mo>(</mo><msub><mi>t</mi><mn>0</mn></msub><mo>)</mo></mrow><mo>=</mo><msub><mover><mi>v</mi><mo>&amp;CenterDot;</mo></mover><mn>0</mn></msub><msub><mi>sin&amp;theta;</mi><mn>0</mn></msub><mo>+</mo><msubsup><mi>v</mi><mn>0</mn><mn>2</mn></msubsup><msub><mi>tan&amp;delta;</mi><mn>0</mn></msub><msub><mi>cos&amp;theta;</mi><mn>0</mn></msub><mo>/</mo><mi>l</mi></mrow></mtd></mtr><mtr><mtd><mrow><msub><mi>y</mi><mi>d</mi></msub><mrow><mo>(</mo><msub><mi>t</mi><mi>f</mi></msub><mo>)</mo></mrow><mo>=</mo><msub><mi>y</mi><mi>f</mi></msub></mrow></mtd></mtr><mtr><mtd><mrow><msub><mover><mi>y</mi><mo>&amp;CenterDot;</mo></mover><mi>d</mi></msub><mrow><mo>(</mo><msub><mi>t</mi><mi>f</mi></msub><mo>)</mo></mrow><mo>=</mo><msub><mi>v</mi><mi>f</mi></msub><msub><mi>sin&amp;theta;</mi><mi>f</mi></msub></mrow></mtd></mtr><mtr><mtd><mrow><msub><mover><mi>y</mi><mo>&amp;CenterDot;&amp;CenterDot;</mo></mover><mi>d</mi></msub><mrow><mo>(</mo><msub><mi>t</mi><mi>f</mi></msub><mo>)</mo></mrow><mo>=</mo><msub><mover><mi>v</mi><mo>&amp;CenterDot;</mo></mover><mi>f</mi></msub><msub><mi>sin&amp;theta;</mi><mi>f</mi></msub><mo>+</mo><msubsup><mi>v</mi>mi><mi>f</mi><mn>2</mn></msubsup><msub><mi>tan&amp;delta;</mi><mi>f</mi></msub><msub><mi>cos&amp;theta;</mi><mi>f</mi></msub><mo>/</mo><mi>l</mi></mrow></mtd></mtr></mtable></mfenced> (R ₀ +R ₁ ) ² ≤[PL ^-1 (H ₁ -Mx _d6 -Nx _d7 )+x _d6 t ⁶ +x _d7 t ⁷ -x ₀ -v _x (tt ₀ )] ² +[PL ^-1 (H ₂ -My _d6 -Ny _d7 )+y _d6 t ⁶ +y _d7 t ⁷ -y ₀ -v _y (tt ₀ )] ² ; in, P = [1 tt ² t ³ t ⁴ t ⁵ ], <mrow><mi>L</mi><mo>=</mo><mfenced open = "[" close = "]"><mtable><mtr><mtd><mn>1</mn></mtd><mtd><msub><mi>t</mi><mn>0</mn></msub></mtd><mtd><msubsup><mi>t</mi><mn>0</mn><mn>2</mn></msubsup></mtd><mtd><msubsup><mi>t</mi><mn>0</mn><mn>3</mn></msubsup></mtd><mtd><msubsup><mi>t</mi><mn>0</mn><mn>4</mn></msubsup></mtd><mtd><msubsup><mi>t</mi><mn>0</mn><mn>5</mn></msubsup></mtd></mtr><mtr><mtd><mn>0</mn></mtd><mtd><mn>1</mn></mtd><mtd><mrow><mn>2</mn><msub><mi>t</mi><mn>0</mn></msub></mrow></mtd><mtd><mrow><mn>3</mn><msubsup><mi>t</mi><mn>0</mn><mn>2</mn></msubsup></mrow></mtd><mtd><msubsup><mi>t</mi><mn>0</mn><mn>3</mn></msubsup></mtd><mtd><mrow><mn>5</mn><msubsup><mi>t</mi><mn>0</mn><mn>4</mn></msubsup></mrow></mtd></mtr><mtr><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>2</mn></mtd><mtd><mrow><mn>6</mn><msub><mi>t</mi><mn>0</mn></msub></mrow></mtd><mtd><mrow><mn>12</mn><msubsup><mi>t</mi><mn>0</mn><mn>2</mn></msubsup></mrow></mtd><mtd><mrow><mn>20</mn><msubsup><mi>t</mi><mn>0</mn><mn>3</mn></msubsup></mrow></mtd></mtr><mtr><mtd><mn>1</mn></mtd><mtd><msub><mi>t</mi><mi>f</mi></msub></mtd><mtd><msubsup><mi>t</mi><mi>f</mi><mn>2</mn></msubsup></mtd><mtd><msubsup><mi>t</mi><mi>f</mi><mn>3</mn></msubsup></mtd><mtd><msubsup><mi>t</mi><mi>f</mi><mn>4</mn></msubsup></mtd><mtd><msubsup><mi>t</mi><mi>f</mi><mn>5</mn></msubsup></mtd></mtr><mtr><mtd><mn>0</mn></mtd><mtd><mn>1</mn></mtd><mtd><mrow><mn>2</mn><msub><mi>t</mi><mi>f</mi></msub></mrow></mtd><mtd><mrow><mn>3</mn><msubsup><mi>t</mi><mi>f</mi><mn>2</mn></msubsup></mrow></mtd><mtd><mrow><mn>4</mn><msubsup><mi>t</mi><mi>f</mi><mn>3</mn></msubsup></mrow></mtd><mtd><mrow><mn>5</mn><msubsup><mi>t</mi><mi>f</mi><mn>4</mn></msubsup></mrow></mtd></mtr><mtr><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>2</mn></mtd><mtd><mrow><mn>6</mn><msub><mi>t</mi><mi>f</mi></msub></mrow></mtd><mtd><mrow><mn>12</mn><msubsup><mi>t</mi><mi>f</mi><mn>2</mn></msubsup></mrow></mtd><mtd><mrow><mn>20</mn><msubsup><mi>t</mi><mi>f</mi><mn>3</mn></msubsup></mrow></mtd></mtr></mtable></mfenced><mo>,</mo></mrow> t ₀ is the initial time of trajectory planning, t _f is the end time of trajectory planning, is the state of the car at the initial time t ₀ , is the state of the car at the final moment t _f ; R ₀ is half the length of the car, R ₁ is half the width of the obstacle; The objective function is Among them, w ₁ and w ₂ are weight coefficients, and w ₁ +w ₂ =1; a _y is the lateral acceleration of the vehicle; <mfenced open = "{" close = ""><mtable><mtr><mtd><mrow><msup><mi>x</mi><mo>&amp;prime;</mo></msup><mo>=</mo><mfrac><mrow><msub><mi>x</mi><mi>f</mi></msub><mo>-</mo><msub><mi>x</mi><mn>0</mn></msub></mrow><mrow><msub><mi>t</mi><mi>f</mi></msub><mo>-</mo><msub><mi>t</mi><mn>0</mn></msub></mrow></mfrac><mrow><mo>(</mo><mi>t</mi><mo>-</mo><msub><mi>t</mi><mn>0</mn></msub><mo>)</mo></mrow></mrow></mtd></mtr><mtr><mtd><mrow><msup><mi>y</mi><mo>&amp;prime;</mo></msup><mo>=</mo><mfrac><mrow><msub><mi>y</mi><mi>f</mi></msub><mo>-</mo><msub><mi>y</msub>mi><mn>0</mn></msub></mrow><mrow><msub><mi>t</mi><mi>f</mi></msub><mo>-</mo><msub><mi>t</mi><mn>0</mn></msub></mrow></mfrac><mrow><mo>(</mo><mi>t</mi><mo>-</mo><msub><mi>t</mi><mn>0</mn></msub><mo>)</mo></mrow></mrow></mtd></mtr></mtable></mfenced> The optimized variables are x _d6 , x _d7 , y _d6 , y _d7 .