CN105691388B

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

Info

Publication number: CN105691388B
Application number: CN201610023691.6A
Authority: CN
Inventors: 赵万忠; 徐志江; 王春燕; 崔滔文
Original assignee: Nanjing University of Aeronautics and Astronautics
Current assignee: Nanjing University of Aeronautics and Astronautics
Priority date: 2016-01-14
Filing date: 2016-01-14
Publication date: 2017-11-14
Anticipated expiration: 2036-01-14
Also published as: CN105691388A

Abstract

The invention discloses a kind of Automotive active anti-collision system and its method for planning track, system includes forward-looking radar, camera, vehicle speed sensor, yaw-rate sensor, side slip angle sensor, signal processing module, Electronic Control power ECU, throttle control, steering controller, brake monitor.Automobile is in the process of moving, electronic control unit gathers the signal that each sensor transmits through signal processing module in real time, and road conditions and vehicle condition residing for real-time judge current time automobile, if it may now cause danger situation, the executable track that then ECU produces a continuous nothing and touched by performing internal trajectory planning program set in advance, and coherent signal is output to throttle control, steering controller and brake monitor and carries out corresponding operating, to avoid the generation of dangerous situation.The present invention in case of emergency can aid in driver to operate automobile, it is possible to increase driving active safety performance.

Description

Automobile active collision avoidance system and track planning method thereof

Technical Field

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

Background

With the rise of intelligent traffic in the global scope, the automobile auxiliary driving technology is receiving more and more attention, and the main purpose of research is to reduce the incidence of increasingly serious traffic accidents and improve the efficiency of the existing road traffic. In the international research institutions, industrial design units are investing a great deal of manpower, material resources and financial resources in the research and development process of the key technology.

The active collision avoidance system is used as an important research content of an automobile auxiliary driving technology, the main research purpose of the active collision avoidance system is to improve the safety performance of automobile driving, the sensing capability of a driver is expanded by mainly utilizing a modern information technology and a sensing technology, external information (such as speed, barrier distance, speed, direction and the like) is transmitted to the driver, meanwhile, the information of automobile conditions and road conditions is comprehensively utilized, the safety degree of the current running condition of the automobile is judged, measures can be automatically taken to control the automobile in an emergency situation, so that the automobile is actively prevented from danger, and the safe running of the automobile is ensured or the injury degree of accidents is reduced to the maximum extent possible. Only if the automobile has the active safety performance, the traffic accidents can be fundamentally reduced, and the traffic safety is improved.

The track planning technology is a key technology in an active collision avoidance system, and a precondition of the track planning technology is to generate a feasible reference track and provide track parameters to a tracking controller so that the controller can control an automobile to run according to the planned track, so that how to plan a feasible collision-free track in an emergency is very important.

Disclosure of Invention

The invention aims to solve the technical problem of the prior art, provides an automobile active collision avoidance system and a track planning method thereof, solves the problem of track planning of the active collision avoidance system in an emergency, effectively avoids obstacles while ensuring the operation stability of an automobile in a mode of combining software and hardware, avoids traffic accidents and realizes the active safety function of the automobile.

The invention adopts the following technical scheme for solving the technical problems:

an automobile active collision avoidance system comprises a forward looking radar, a camera, a signal processing module, a vehicle speed sensor, a yaw velocity sensor, a mass center yaw angle sensor, a steering wheel corner sensor, an electronic control power supply ECU, an accelerator controller, a steering controller and a brake controller;

the forward looking radar and the camera are connected with the electronic control power supply ECU through a signal processing module; the electronic control power supply ECU is respectively connected with a vehicle speed sensor, a yaw rate sensor, a mass center slip angle sensor, a steering wheel corner sensor, an accelerator controller, a steering controller and a brake controller;

the forward looking radar and the camera are both arranged in front of the automobile and used for detecting the road condition in front of the automobile, and transmitting the detected signals to the electronic control unit ECU after being processed by the signal processing module;

the vehicle speed sensor, the yaw rate sensor, the mass center slip angle sensor and the steering wheel angle sensor are respectively used for sensing the speed, the yaw rate, the mass center slip angle and the front wheel steering angle of the vehicle, and collected signals are processed and then sent to the electronic control unit ECU;

and the electronic control unit ECU is used for outputting corresponding signals to the accelerator controller, the steering controller and the brake controller according to the received signals, and carrying out corresponding acceleration, deceleration and braking operations so as to ensure the driving safety.

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

step 1), acquiring the distance, speed, acceleration and width of an obstacle in front of an automobile through a forward-looking radar and a camera, comparing the distance between the obstacle in front and the automobile with a preset safety distance threshold, and executing step 2 if the distance is smaller than the preset safety distance threshold;

step 2), acquiring the speed, the yaw rate, the centroid yaw angle and the front wheel steering angle of the automobile through a vehicle speed sensor, a yaw rate sensor, a centroid yaw angle sensor and a steering wheel angle sensor;

step 3), establishing an automobile three-degree-of-freedom kinematic model according to the yaw angle, the steering angle of the front wheels, the longitudinal speed, the distance between the front axle and the rear axle, and the longitudinal coordinate and the lateral coordinate of the middle point of the rear axle;

step 4), parameterizing the track to be generated by using a seventh polynomial;

step 5), setting a track optimization model constraint condition, setting an objective function and an optimization variable according to the three-degree-of-freedom kinematic model of the automobile and the parameterized track to be generated, and solving the track optimization model according to the longitudinal speed, the yaw angular velocity, the mass center side slip angle, the front wheel steering angle, the distance, the speed and the acceleration of the front obstacle of the automobile to obtain the track optimization model;

and 6), solving the established track optimization model based on the dynamic particle swarm optimization algorithm to obtain a planned track.

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

wherein x and y are respectively the longitudinal coordinate and the lateral coordinate of the middle point of the rear axle of the automobile, theta is the yaw angle of the automobile and is the steering angle of the front wheels of the automobile, v is the longitudinal speed of the automobile, l is the distance between the front axle and the rear axle of the automobile, and t is the current time of the trajectory planning.

As a further optimization scheme of the trajectory planning method of the automobile active collision avoidance system, the trajectory equation parameterized by the seventh-order polynomial in the step 5) is as follows:

wherein x is_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_d7Is the undetermined coefficient 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 active collision avoidance system of the automobile, the constraint conditions of the trajectory optimization model in step 6 are as follows:

(R₀+R₁)²≤[PL^-1(H₁-Mx_d6-Nx_d7)+x_d6t⁶+x_d7t⁷-x₀-v_x(t-t₀)]²

+[PL^-1(H₂-My_d6-Ny_d7)+y_d6t⁶+y_d7t⁷-y₀-v_y(t-t₀)]²；

wherein,

P＝[1 t t²t³t⁴t⁵]，

t₀planning an initial time, t, for the trajectory_fFor the end of the planning of the trajectory,is an initial time t₀The state of the vehicle is such that,to end the time t_fThe state of the vehicle;

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

an objective function ofWherein, w₁And w₂Is a weight coefficient, and w₁+w₂＝1；a_yIs the vehicle lateral acceleration;

the optimized variable is x_d6、x_d7、y_d6、y_d7。

Compared with the prior art, the invention adopting the technical scheme has the following technical effects:

1. the track generated by the track planning method meets various incomplete constraints and execution mechanism constraints;

2. the track curvature generated by the track planning method has continuity and dynamic real-time performance, and can adapt to dynamically changing road environment;

3. the track generated by tracking the track planning method can effectively avoid the automobile from the obstacles and prevent traffic accidents.

Drawings

FIG. 1 is a schematic structural diagram of an 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 a three-degree-of-freedom kinematic model of an automobile according to the present invention.

Detailed Description

The technical scheme of the invention is further explained in detail by combining the attached drawings:

as shown in fig. 1, the invention discloses an active collision avoidance system for an automobile, which comprises a forward looking radar, a camera, a signal processing module, a vehicle speed sensor, a yaw rate sensor, a mass center side deviation angle sensor, a steering wheel corner sensor, an Electronic Control Unit (ECU), an accelerator controller, a steering controller and a brake controller, wherein the forward looking radar is connected with the camera;

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

step 1, obtaining the distance, speed, acceleration and width of an obstacle in front of an automobile through a forward-looking radar and a camera, comparing the distance between the obstacle in front and the automobile with a preset safe distance threshold value, and executing step 2 if the distance is smaller than the preset safe distance threshold value.

And 2, acquiring the longitudinal speed, the yaw rate, the mass center slip angle and the steering angle of the front wheel of the automobile through a vehicle speed sensor, a yaw rate sensor, a mass center slip angle sensor and a steering wheel angle sensor.

Step 3, establishing an automobile three-degree-of-freedom kinematic model as shown in fig. 3:

And 4, entering a cycle.

Step 5, setting the initial moment of the track planning as t₀The final time of the trajectory planning is t_fAnd parameterizing the track to be generated by using a seventh polynomial:

wherein x is_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_d7Is the undetermined coefficient of the polynomial.

Step 6, setting a track optimization model constraint condition, setting an objective function and an optimization variable according to the three-degree-of-freedom kinematic model of the automobile and the parameterized track to be generated, and solving the track optimization model according to the longitudinal speed, the yaw angular velocity, the mass center side slip angle, the front wheel steering angle of the automobile, the distance, the speed and the acceleration of the front obstacle of the automobile to obtain the track optimization model:

1) constraint conditions are as follows:

set at an initial time t₀The state of the vehicle A isAt the end time t_fThe state of the vehicle A isAnd the designed track is (x)_d(t),y_d(t)). Then, according to the vehicle kinematics model (1), the equality constraints imposed on the designed trajectory are as follows:

the trajectory equation (2) is substituted into the equation constraint (3) and is converted into a matrix form, the coefficients of which can be determined by the following equation:

wherein,

and is

Substituting equation (4) into trajectory equation (2) may result in a further expression for the trajectory equation:

wherein P ═ 1 t t²t³t⁴t⁵]。

In order to meet the collision avoidance requirement, some inequality constraint conditions need to be met:

(R₀+R₁)²≤[x_d(t)-x₀-v_x(t-t₀)]²+[y_d(t)-y₀-v_y(t-t₀)]²(6)

wherein R is₀Half the length of the car, R₁Is half of the width of the obstacle；

Substituting equation (5) into (6) may result in a further expression:

(R₀+R₁)²≤[PL^-1(H₁-Mx_d6-Nx_d7)+x_d6t⁶+x_d7t⁷-x₀-v_x(t-t₀)]²

+[PL^-1(H₂-My_d6-Ny_d7)+y_d6t⁶+y_d7t⁷-y₀-v_y(t-t₀)]²(7)

2) the objective function, generally speaking, in the process of active collision avoidance of an intelligent automobile, the planned trajectory must satisfy some conditions, for example, effectively avoid obstacles while ensuring the stability of the automobile, and based on such consideration, the following function is selected as the optimized objective function:

wherein, w₁And w₂Is a weight coefficient, and w₁+w₂＝1；a_yIs the vehicle lateral acceleration; (ii) a

3) Optimizing variables, as can be readily seen from equation (5), the variables to be optimized are: x is the number of_d6、x_d7、y_d6、y_d7。

And 7, solving the established track optimization model based on a dynamic particle swarm optimization algorithm to obtain a required track:

particle swarm optimization, also called particle swarm optimization, is based on a swarm of particles, and the solution to each optimization problem is to find one particle in the feasible space. In order to enable the particles to search in a global scope and maintain the diversity of the particles, the invention adopts a Dynamic Particle Swarm Optimization (DPSO) to optimize a 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_iAnd representing a potential feasible solution in the solution space, and judging whether the solution is the optimal solution according to the adaptive value calculated by the objective function. The D-dimensional space velocity vector of the ith particle is v_i＝(v_i1,v_i2,...,v_iD) I th individual optimum position P of particle_i＝(P_i1,P_i2,...,P_iD) Optimum location L of particle swarm_i＝(L_i1,L_i2,...,L_iD) The global optimal position G ═ G (G) of the particle swarm₁,G₂,...,G_D) The iterative formula is as follows:

v_i(t+1)＝ωv_it+b₁r₁(p_i(t)-x_i(t))+b₂r₂(L_i(t)-x_i(t))+b₂r₃(G(t)-x_i(t)) (9)

in the formula: b₁、b₂、b₃Is a normal number; r is₁、r₂、r₃Is [0,1 ]]A random number within; the parameter ω is an inertia factor.

Let omega depend on cycle times from omega_sLinearly decreasing to omega_eMaximum number of cycles is I_maxThe current number of cycles is I_cThe value of ω can then be given by:

in the formula: omega_sTo optimize the initial inertia factor; omega_eFor optimising the inertia of the terminationA factor.

The position of the particle at time t +1 in the particle population is determined by the following equation:

x_i(t+1)＝x_i(t)+v_i(t+1) (11)

if the particle swarm is updated to exceed the defined domain limit later, the positions of the particles need to be readjusted to fall within the decision space, and the new positions can be calculated according to the following formula:

x_i(t+1)＝x_i(t)+λv_i(t+1) (12)

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

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

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

Generation of dynamic particle swarm: if m particle groups have been generated, let b be the particle group closest to the particle group a, if the distance between them is greater than D_maxThen a particle group x needs to be generated_m+1Component of the kth dimension of the ith particle in the groupCan be obtained by the following formula:

in the formula: c₁、C₂Is [0,1 ]]A random number within; round (-) is a round letterNumber, therefore, round (0.5+ C)₂) Is 0 or 1.

And 8, the ECU outputs the relevant parameters of the generated track to an accelerator controller, a steering controller and a brake controller, and executes corresponding operations to accurately track the generated track.

And 9, jumping to the step 4, performing the track solving and tracking at the next moment, and repeating the steps until the planning is finished to finish the whole collision avoidance process, as shown in fig. 2.

It will be understood by those skilled in the art that, unless otherwise defined, all terms (including technical 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 will be further understood that terms, such as those defined in commonly used dictionaries, should be interpreted as having a meaning that is consistent with their meaning in the context of the prior art and will not be interpreted in an idealized or overly formal sense unless expressly so defined herein.

The above-mentioned embodiments, objects, technical solutions and advantages of the present invention are further described in detail, it should be understood that the above-mentioned embodiments are only illustrative of the present invention and are not intended to limit the present invention, and any modifications, equivalents, improvements and the like made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims

1. A track planning method of an automobile active collision avoidance system is 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 angle sensor, a mass center lateral deviation angle sensor, a steering wheel corner sensor, an electronic control power supply ECU, an accelerator controller, a steering controller and a brake controller;

the electronic control unit ECU is used for outputting corresponding signals to the accelerator controller, the steering controller and the brake controller according to the received signals, and carrying out corresponding acceleration, deceleration and braking operations so as to ensure driving safety;

the track planning method of the automobile active collision avoidance system comprises the following steps:

2. The trajectory planning method for the active collision avoidance system of the automobile according to claim 1, wherein the three-degree-of-freedom kinematic model of the automobile in the step 3) is established according to the following formula:

<mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mover> <mi>x</mi> <mo>&CenterDot;</mo> </mover> <mo>=</mo> <mi>v</mi> <mi>cos</mi> <mrow> <mo>(</mo> <mi>&theta;</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mover> <mi>y</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mi>v</mi> <mi>sin</mi> <mrow> <mo>(</mo> <mi>&theta;</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mover> <mi>&theta;</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mi>v</mi> <mi>tan</mi> <mi>&delta;</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>/</mo> <mi>l</mi> </mrow> </mtd> </mtr> </mtable> </mfenced>

3. The trajectory planning method of the active collision avoidance system of claim 2, wherein the trajectory equation parameterized by the seventh-order polynomial in step 5) 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

wherein x is_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_d7Is the undetermined coefficient 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 the active collision avoidance system of the automobile according to claim 3, wherein the constraint conditions of the trajectory optimization model in the step 6 are as follows:

<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>&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&theta;</mi> <mn>0</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>x</mi> <mo>&CenterDot;&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>&CenterDot;</mo> </mover> <mn>0</mn> </msub> <msub> <mi>cos&theta;</mi> <mn>0</mn> </msub> <mo>-</mo> <msubsup> <mi>v</mi> <mn>0</mn> <mn>2</mn> </msubsup> <msub> <mi>tan&delta;</mi> <mn>0</mn> </msub> <msub> <mi>sin&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>&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&theta;</mi> <mi>f</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>x</mi> <mo>&CenterDot;&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>&CenterDot;</mo> </mover> <mi>f</mi> </msub> <msub> <mi>cos&theta;</mi> <mi>f</mi> </msub> <mo>-</mo> <msubsup> <mi>v</mi> <mi>f</mi> <mn>2</mn> </msubsup> <msub> <mi>tan&delta;</mi> <mi>f</mi> </msub> <msub> <mi>sin&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>&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&theta;</mi> <mn>0</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>y</mi> <mo>&CenterDot;&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>&CenterDot;</mo> </mover> <mn>0</mn> </msub> <msub> <mi>sin&theta;</mi> <mn>0</mn> </msub> <mo>+</mo> <msubsup> <mi>v</mi> <mn>0</mn> <mn>2</mn> </msubsup> <msub> <mi>tan&delta;</mi> <mn>0</mn> </msub> <msub> <mi>cos&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>&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&theta;</mi> <mi>f</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>y</mi> <mo>&CenterDot;&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>&CenterDot;</mo> </mover> <mi>f</mi> </msub> <msub> <mi>sin&theta;</mi> <mi>f</mi> </msub> <mo>+</mo> <msubsup> <mi>v</mi> <mi>f</mi> <mn>2</mn> </msubsup> <msub> <mi>tan&delta;</mi> <mi>f</mi> </msub> <msub> <mi>cos&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_d6t⁶+x_d7t⁷-x₀-v_x(t-t₀)]²

+[PL^-1(H₂-My_d6-Ny_d7)+y_d6t⁶+y_d7t⁷-y₀-v_y(t-t₀)]²；

wherein,

P＝[1 t t²t³t⁴t⁵]，

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

<mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msup> <mi>x</mi> <mo>&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>&prime;</mo> </msup> <mo>=</mo> <mfrac> <mrow> <msub> <mi>y</mi> <mi>f</mi> </msub> <mo>-</mo> <msub> <mi>y</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 variable is x_d6、x_d7、y_d6、y_d7。