Auxiliary early warning method based on radar detection reaction delay

Abstract

The invention belongs to the technical field of automotive electronics, and discloses an auxiliary early warning method based on radar detection response delay, which comprises the following steps: firstly, acquiring real-time data of a vehicle and the surrounding environment thereof, including motion state information of the vehicle and fixed parameters of the vehicle, and utilizing a vehicle positioning and vehicle track prediction algorithm to predict the real-time data of the vehicle; and processing and analyzing the acquired data so as to predict the future trajectory of the vehicle. Meanwhile, the potential collision probability between the vehicle and the obstacle is calculated based on the vehicle trajectory prediction result and the position information of the obstacle. And when the radar system fails to give out early warning in time due to response delay, the collision probability is sent to the early warning decision module to assist the radar in early warning or not. The whole process does not depend on an external positioning system (such as GPS) or network connection, the method effectively makes up for the defects of a radar system, and improves the timeliness and accuracy of early warning.

Description

Technical Field

[0001] The invention belongs to the technical field of automobile electronics, and in particular relates to an auxiliary early warning method based on radar detection reaction delay. Background Art

[0002] With the rapid advancement of science and technology and people's increasing demand for travel safety and convenience, automotive electronic technology has undergone a leapfrog development from simple circuit control to intelligent and networked systems, playing an indispensable role in improving vehicle performance and ensuring driving safety.

[0003] At present, an innovative driving safety detection algorithm has emerged. It mainly targets a variety of driving safety hazards, including radar response delay, which are often caused by the limitations of the sensor itself or the interference of environmental factors. The algorithm can provide rapid and accurate warning information to the driver or the autonomous driving system by real-time and accurate perception and prediction of obstacles and potential risks around the vehicle, effectively avoiding the occurrence of safety accidents such as collisions. More importantly, when the algorithm detects that the radar has a feedback delay, it can immediately assist the radar to issue a warning signal to reduce the failure rate of radar warning. This forward-looking warning function greatly reduces the incidence of traffic accidents and provides a solid guarantee for the safety of drivers and passengers. In contrast, it is difficult to ensure that the warning is not delayed in complex environments by relying solely on radar sensors. In a low-speed real-time warning system that requires a fast response, the algorithm must be able to process massive sensor data in a very short time and make decisions quickly.

[0004] Therefore, there is an urgent need to develop an auxiliary warning method based on radar detection reaction delay, so as to respond quickly and issue warning prompts in real time, thereby significantly reducing driving risks and ensuring driving safety. Summary of the invention

[0005] In view of this, the purpose of the present invention is to provide an auxiliary warning method based on radar detection reaction delay. The present invention aims to effectively solve the key problems of assisting radar warning in the case of slow response of the radar system without the need to use GPS to identify the coordinates of obstacles around the vehicle.

[0006] The present invention provides an auxiliary early warning method based on radar detection reaction delay, comprising the following steps:

[0007] S1. Detect obstacles around the vehicle through ultrasonic radar and collect perception information of the obstacles. Then, the perception information is converted into the coordinates of the obstacles in the vehicle coordinate system through data processing algorithms;

[0008] Establish a geodetic coordinate system with the center point of the vehicle's rear wheelbase as the radius, the vehicle's forward direction as the X-axis, and the vertical X as the Y-axis;

[0009] Collect the number of wheel pulse signals, calculate the vehicle's XY coordinates through the vehicle positioning algorithm, and calculate the vehicle's current position in real time through the number of wheel pulse signals collected in real time during the vehicle's driving process;

[0010] Integrate the yaw angular acceleration collected in real time to obtain the vehicle heading angle;

[0011] Calculate the coordinates of the vehicle in the geodetic coordinate system based on the vehicle wheel speed pulse and the yaw angular acceleration;

[0012] S2. Combine the obstacle and vehicle coordinate systems with the vehicle positioning algorithm to calculate the XY coordinates of the obstacle in the geodetic coordinate system;

[0013] The vehicle's geodetic coordinate system is given according to the vehicle coordinate system coordinates combined with the vehicle positioning algorithm, and the obstacle's geodetic coordinate system coordinates are obtained through coordinate conversion;

[0014] S3. Design a trajectory prediction algorithm based on the vehicle parameters and real-time motion status information;

[0015] The trajectory prediction algorithm uses the center of the vehicle's rear wheelbase as the coordinate origin and performs trajectory prediction based on an accurate vehicle kinematic model;

[0016] S4. During the prediction process, the potential collision probability between the vehicle and the obstacle is analyzed and calculated in real time;

[0017] S5. When the radar system fails to issue a warning in time due to delayed response, the collision probability is sent to the warning decision module to assist the radar in issuing a warning.

[0018] Furthermore, in step S1, the coordinates of the vehicle in the earth coordinate system are calculated based on the vehicle wheel speed pulse and the yaw angular acceleration, including the following steps:

[0019] I. Calculate the travel distances ΔS_Left and ΔS_Right of the left and right rear tires by the number of wheel speed pulse changes of the left and right rear tires within a unit time ΔT;

[0020] II. Compare the distance traveled by the left and right rear tires per unit time. If the difference between ΔS_Left and ΔS_Right is less than 0.00001, the vehicle is traveling in a straight line, otherwise it is not traveling in a straight line.

[0021] III. Calculate the vehicle heading angle θ by integrating the yaw rate;

[0022] IV. Vehicle straight travel coordinates XY calculation formula:

[0023] X = Xpre + Dir * ΔS * cos (θ);

[0024] Y = Ypre + Dir * ΔS * sin (θ);

[0025] Where Xpre represents the wheel coordinate X of the previous step; Ypre represents the wheel coordinate Y of the previous step; Dir represents the direction of vehicle travel, forward is positive and backward is negative;

[0026] V. Calculation formula for vehicle non-straight-ahead driving coordinates XY:

[0027] Δθ=(ΔS_Left-ΔS_Right) / VehRearSuspension;

[0028] X=Xpre+2*Dir*|ΔS / Δθ*sin(Δθ / 2)|*cos(θ+Δθ / 2);

[0029] Y=Ypre+2*Dir*|ΔS / Δθ*sin(Δθ / 2)|*sin(θ+Δθ / 2);

[0030] Where, VehRearSuspension represents the length of the rear axle of the vehicle body;

[0031] VI. Calculate the coordinates of the left rear and right rear in the geodetic coordinate system (Xl, Yl)(Xr, Yr), and then calculate the vehicle coordinates XY:

[0032] X = (Xl + Xr) / 2;

[0033] Y=(Yl+Yr) / 2.

[0034] Furthermore, the trajectory prediction algorithm of step S3 is designed based on a kinematic model through a vehicle positioning algorithm, obstacle perception, vehicle parameters and real-time motion state information, and is used to predict the future trajectory of the vehicle;

[0035] The trajectory prediction algorithm comprises the following steps:

[0036] I. Obtain the vehicle's current motion state information and the coordinates of obstacles according to the vehicle positioning algorithm and radar detection algorithm; the vehicle's motion state information includes the vehicle's x coordinate, the vehicle's y coordinate, the vehicle's speed v, the heading angle θ, the steering wheel angle δ, and the gear position g;

[0037] II. Adaptively set the time step and total prediction distance of trajectory prediction according to the current vehicle speed;

[0038] III. Calculate the vehicle's motion state at the next moment through the position and heading angle formula:

[0039] x′=x+vcos(θ)Δt

[0040] y′=y+vsin(θ)Δt

[0041]

[0042] VehicleState′=update(VehicleState)=(x′,y′,v,θ′,δ,g)

[0043] Where Δt is the time step at the next moment; L is the front and rear wheelbase; R is the distance from the center line of the vehicle to the instantaneous steering center;

[0044] IV. Obtain the trajectory coordinates of the center of the front axle of the vehicle according to the current position and heading angle of the vehicle; on the two-dimensional plane, regard the center of the rear axle as the origin, and move forward along the direction of the heading angle θ by the distance of the wheelbase L to obtain the relative coordinates of the center of the front axle; use the conversion formula from polar coordinates to rectangular coordinates to convert the above relative coordinates into the global coordinate system;

[0045] Assume that the coordinates of the rear axle center are (x_rear, y_rear), the heading angle is θ, and the wheelbase is L;

[0046] In the global coordinate system, the coordinates of the front axle center (x_front, y_front) are calculated as follows:

[0047] x_front=x_rear+Lcos(θ)

[0048] y_front=y_rear+Lsin(θ)

[0049] V. Based on a series of coordinate points of the predicted trajectory, find the predicted trajectory point with the smallest coordinates with the radar obstacle, and calculate the distance between the two and compare it with the safety distance;

[0050] If it is less than the safe distance, the collision point is output and related information is output; at the same time, the driving trajectory distance and total time step of the vehicle predicted to the collision point are calculated by the integral method, and the driving trajectory distance and total time step to the collision point are used as the collision distance and collision time respectively;

[0051] If there are multiple obstacles, output the coordinates of the obstacle closest to the vehicle.

[0052] If the collision point does not exist, an invalid value is output.

[0053] Beneficial effects:

[0054] The method of the present invention provides an auxiliary warning method based on radar detection reaction delay, which obtains data through radar, wheel pulse and inertial sensor, and obtains the probability of collision between the vehicle and the obstacle through the vehicle positioning algorithm and vehicle trajectory prediction algorithm, thereby assisting the radar to feedback the delay after detecting the obstacle and assisting the radar warning function. The whole process does not rely on external positioning systems (such as GPS) or network connections. The method of the present invention effectively makes up for the shortcomings of the radar system and improves the timeliness and accuracy of the warning.

[0055] Other advantages, objectives and features of the present invention will be described in the following description to some extent, and to some extent, will be obvious to those skilled in the art based on the following examination and study, or can be taught from the practice of the present invention. The objectives and other advantages of the present invention can be realized and obtained through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] Figure 1 This is a flow chart of an auxiliary warning method based on radar detection reaction delay of the present invention;

[0057] Figure 2 A schematic diagram of vehicle positioning;

[0058] Figure 3 Flowchart of the trajectory prediction algorithm. DETAILED DESCRIPTION

[0059] In order to make the technical solutions, advantages and purposes of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below in conjunction with the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the described embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work belong to the protection scope of this application.

[0060] The vehicle parameter configuration that needs to be calibrated in the present invention includes:

[0061] Tire length: used to calculate the distance the tire travels in one circle;

[0062] One wheel speed pulse: used to calculate the travel distance of each pulse;

[0063] Vehicle rear overhang: used to calculate the angle Δθ within ΔT.

[0064] like Figure 1-3 As shown, the present invention provides an auxiliary warning method based on radar detection reaction delay, comprising the following steps:

[0065] S1. Use ultrasonic radar to emit ultrasonic waves to detect obstacles around the vehicle and collect perception information of the obstacles. Then, use data processing algorithms to convert the perception information into coordinates of the obstacles in the vehicle coordinate system.

[0066] Establish a geodetic coordinate system with the center point of the vehicle's rear wheelbase as the radius, the vehicle's forward direction as the X-axis, and the vertical X as the Y-axis;

[0067] The number of wheel pulse signals collected is used to calculate the vehicle's XY coordinates through the geometric relationship of "vehicle Ackerman steering" through the vehicle positioning algorithm; during the vehicle's driving process, the vehicle's current position is calculated in real time through the number of wheel pulse signals collected in real time;

[0068] Integrate the yaw angular acceleration collected in real time to obtain the vehicle heading angle;

[0069] Calculate the coordinates of the vehicle in the geodetic coordinate system based on the vehicle wheel speed pulse and the yaw angular acceleration;

[0070] S2. Combine the obstacle and vehicle coordinate system with the vehicle positioning algorithm, calculate the XY coordinates of the obstacle in the geodetic coordinate system and save them in the EEPROM, which can record multiple current positions based on the vehicle;

[0071] The vehicle's geodetic coordinate system is given according to the vehicle coordinate system coordinates combined with the vehicle positioning algorithm, and the obstacle's geodetic coordinate system coordinates are obtained through coordinate conversion;

[0072] S3. Design a trajectory prediction algorithm based on the vehicle parameters and real-time motion status information;

[0073] The trajectory prediction algorithm uses the center of the vehicle's rear wheelbase as the coordinate origin and performs trajectory prediction based on an accurate vehicle kinematic model;

[0074] S4. During the prediction process, the potential collision probability between the vehicle and the obstacle is analyzed and calculated in real time;

[0075] S5. When the radar system fails to issue a warning in time due to delayed response, the collision probability is sent to the warning decision module to assist the radar in issuing a warning;

[0076] Decisions are made in real time based on the increased collision probability in vehicle trajectory prediction and the radar working status. If the warning conditions are met, a warning will be issued.

[0077] As a preferred embodiment of this embodiment, the coordinates of the vehicle in the geodetic coordinate system are calculated based on the vehicle wheel speed pulse and the yaw angular acceleration, including the following steps:

[0078] I. Calculate the travel distances ΔS_Left and ΔS_Right of the left and right rear tires by the number of wheel speed pulse changes of the left and right rear tires within a unit time ΔT;

[0079] II. Compare the distance traveled by the left and right rear tires per unit time. If the difference between ΔS_Left and ΔS_Right is less than 0.00001, the vehicle is traveling in a straight line, otherwise it is not traveling in a straight line.

[0080] III. Calculate the vehicle heading angle θ by integrating the yaw rate;

[0081] θ=θpre+YawRate*ΔT

[0082] Where YawRate represents the yaw angular acceleration; θpre represents the vehicle heading angle in the previous step;

[0083] IV. Vehicle straight travel coordinates XY calculation formula:

[0084] Xl=Xlpre+Dir*ΔS_Left*cos(θ);

[0085] Yl=Ylpre+Dir*ΔS_Left*sin(θ);

[0086] Xr=Xrpre+Dir*ΔS_Right*cos(θ);

[0087] Yr=Yrpre+Dir*ΔS_Right*sin(θ);

[0088] In the formula, Dir represents the direction of vehicle travel, forward is positive and backward is negative;

[0089] V. Calculation formula for vehicle non-straight-ahead driving coordinates XY:

[0090] Δθ=(ΔS_Left-ΔS_Right) / VehRearSuspension;

[0091] Xl=Xlpre+2*Dir*|ΔS_Left / Δθ*sin(Δθ / 2)|*cos(θ+Δθ / 2);

[0092] Yl=Ylpre+2*Dir*|ΔS_Left / Δθ*sin(Δθ / 2)|*sin(θ+Δθ / 2);

[0093] Xr=Xrpre+2*Dir*|ΔS_Right / Δθ*sin(Δθ / 2)|*cos(θ+Δθ / 2);

[0094] Yr=Yrpre+2*Dir*|ΔS_Right / Δθ*sin(Δθ / 2)|*sin(θ+Δθ / 2);

[0095] Where, VehRearSuspension represents the length of the rear axle of the vehicle body;

[0096] VI. Calculate the coordinates of the left rear and right rear in the geodetic coordinate system (Xl, Yl)(Xr, Yr), and then calculate the vehicle coordinates XY:

[0097] X = (Xl + Xr) / 2;

[0098] Y=(Yl+Yr) / 2;

[0099] The present invention successfully achieves positioning without relying on GPS positioning and solves the positioning problem based on vehicle body sensors.

[0100] As a preferred embodiment of this embodiment, the trajectory prediction algorithm of step S3 is designed based on a kinematic model through a vehicle positioning algorithm, obstacle perception, vehicle parameters and real-time motion state information, and is used to predict the future trajectory of the vehicle and calculate the collision probability;

[0101] The trajectory prediction algorithm comprises the following steps:

[0102] I. Initialize the fixed parameters of the vehicle, including the front and rear wheelbase, body width, body length, and the total duration of the predicted future trajectory;

[0103] II. Parameter transmission and data preprocessing. According to the vehicle positioning algorithm, radar detection algorithm and CAN communication, the vehicle's current motion state information (x, y, v, θ, δ, g) (vehicle x coordinate, vehicle y coordinate, vehicle speed, heading angle, steering wheel angle, gear position) and obstacle coordinates are obtained, and these parameters are processed and converted into the required format and requirements;

[0104] III. Adaptively set the time step and total prediction distance of trajectory prediction according to the current vehicle speed to improve the accuracy, real-time nature and adaptability of trajectory prediction to different driving scenarios;

[0105]

[0106] IV. Determine whether the vehicle is moving forward or backward based on the vehicle's gear position. And predict the trajectory coordinates of the vehicle's next time step based on the kinematic model. Calculate the vehicle's motion state at the next moment using the position and heading angle formula:

[0107] x′=x+vcos(θ)Δt

[0108] y′=y+vsin(θ)Δt

[0109]

[0110] VehicleState′=update(VehicleState)=(x′,y′,v,θ′,δ,g)

[0111] Where Δt is the time step at the next moment; L is the front and rear wheelbase; R is the distance from the center line of the vehicle to the instantaneous steering center;

[0112] Then, the trajectory coordinates of the center of the front axle of the vehicle are obtained according to the current position and heading angle of the vehicle. Specifically, in the global coordinate system, according to the geometric characteristics of the vehicle, the position of the center of the front axle relative to the center of the rear axle can be represented by a vector along the longitudinal direction of the vehicle with a length of the wheelbase L.

[0113] On a two-dimensional plane, if we regard the center of the rear axle as the origin and move forward the distance of the wheelbase L in the direction of the heading angle θ, we can get the relative coordinates of the center of the front axle. Using the conversion formula from polar coordinates to rectangular coordinates, or based on vector addition, we can transform this relative coordinate into the global coordinate system.

[0114] Assume that the coordinates of the rear axle center are (x_rear, y_rear), the heading angle is θ (in radians), and the wheelbase is L.

[0115] Therefore, in the global coordinate system, the coordinates of the center of the front axis (x_front, y_front) can be calculated by the following formula:

[0116] x_front=x_rear+Lcos(θ)

[0117] y_front=y_rear+Lsin(θ)

[0118] Finally, through continuous position updates and predictions, the vehicle trajectory state at future time points can be gradually calculated. At the same time, it is necessary to continuously receive sensor data (vehicle speed, steering wheel angle, etc.) to adjust the prediction accordingly. Finally, the predicted trajectory of the vehicle's front axle center and rear axle center is obtained.

[0119] V. Based on a series of coordinate points of the predicted trajectory, find the predicted trajectory point with the smallest coordinates of the radar obstacle, and calculate the distance between the two and compare it with the safe distance. If it is less than the safe distance, output that the collision point exists and output relevant information. At the same time, use the integral method to obtain the predicted driving trajectory distance of the vehicle to this point and the total time step of this point as the collision distance and collision time respectively. If there are multiple obstacles, output the relevant information of the coordinates of the obstacle closest to the vehicle. If the collision point does not exist, output an invalid value. The specific steps are as follows:

[0120] T is a set of coordinate points of the predicted trajectory, where Ti = (xi, yi) represents the coordinates of the i-th predicted trajectory point. O is a set of obstacle coordinates detected by the radar, where Oj = (xj, yj) represents the coordinates of the j-th obstacle. d(Ti, Oj) represents the Euclidean distance function between two points Ti and Oj. Dsafe is the safety distance threshold. Scollision is the set of collision points. Lcollision is the collision distance (i.e., the driving trajectory distance predicted from the vehicle to the collision point). Tcollision is the collision time (i.e., the total time step predicted from the vehicle to the collision point). N is the number of predicted trajectory points. M is the number of obstacles.

[0121] For each point Ti = (xi, yi) in the predicted trajectory and each obstacle Oj = (xj, yj) detected by the radar, calculate the distance dij between them:

[0122]

[0123] For each dij, a comparison is made with the safety distance Dsafe.

[0124]

[0125] If there are multiple collision risk points, select the predicted trajectory point Tnearest corresponding to the obstacle closest to the current position of the vehicle (assuming T0 = (x0, y0)):

[0126]

[0127] The collision distance and collision time can be obtained by the following formula:

[0128]

[0129] As a preference of this embodiment, a method of outputting all obstacle information or only the nearest obstacle information may be flexibly selected according to specific scenarios and requirements.

[0130] The method of the present invention obtains data through radar, wheel pulses and inertial sensors, and obtains the probability of collision between the vehicle and the obstacle through the vehicle positioning algorithm and the vehicle trajectory prediction algorithm, thereby assisting the radar to feedback the delay after detecting the obstacle and assisting the radar early warning function. The core of the present invention is to make full use of the vehicle's own devices: ultrasonic radar to perceive the surrounding environment, and the data collected by the vehicle pulses and inertial sensors are converted into the position information of the obstacle in the geodetic coordinate system through internal calculations. The entire process does not rely on external positioning systems (such as GPS) or network connections. The method of the present invention effectively makes up for the shortcomings of the radar system and improves the timeliness and accuracy of the early warning.

[0131] It is hereby declared that the above embodiments are only used to illustrate the technical solutions of the present invention and are not limiting. Although the present invention is described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solutions of the present invention can be modified or replaced by equivalents without departing from the purpose and scope of the technical solutions, which should be included in the scope of the claims of the present invention.

Claims

1. An auxiliary warning method based on radar detection reaction delay, characterized in that: The following steps are involved: S1. Detect obstacles around the vehicle through ultrasonic radar and collect perception information of the obstacles. Then, the perception information is converted into the coordinates of the obstacles in the vehicle coordinate system through data processing algorithms; Establish a geodetic coordinate system with the center point of the vehicle's rear wheelbase as the radius, the vehicle's forward direction as the X-axis, and the vertical X as the Y-axis; Collect the number of wheel pulse signals, calculate the vehicle's XY coordinates through the vehicle positioning algorithm, and calculate the vehicle's current position in real time through the number of wheel pulse signals collected in real time during the vehicle's driving process; Integrate the yaw angular acceleration collected in real time to obtain the vehicle heading angle; Calculate the coordinates of the vehicle in the geodetic coordinate system based on the vehicle wheel speed pulse and the yaw angular acceleration; S2. Combine the obstacle and vehicle coordinate systems with the vehicle positioning algorithm to calculate the XY coordinates of the obstacle in the geodetic coordinate system; The vehicle's geodetic coordinate system is given according to the vehicle coordinate system coordinates combined with the vehicle positioning algorithm, and the obstacle's geodetic coordinate system coordinates are obtained through coordinate conversion; S3. Design a trajectory prediction algorithm based on the vehicle parameters and real-time motion status information; The trajectory prediction algorithm uses the center of the vehicle's rear wheelbase as the coordinate origin and performs trajectory prediction based on an accurate vehicle kinematic model; S4. During the prediction process, the potential collision probability between the vehicle and the obstacle is analyzed and calculated in real time; S5. When the radar system fails to issue a warning in time due to delayed response, the collision probability is sent to the warning decision module to assist the radar in issuing a warning.

2. The auxiliary warning method based on radar detection reaction delay according to claim 1 is characterized in that: In the step S1, the coordinates of the vehicle in the geodetic coordinate system are calculated based on the vehicle wheel speed pulse and the yaw angular acceleration, including the following steps: I. Calculate the travel distances ΔS_Left and ΔS_Right of the left and right rear tires by the number of wheel speed pulse changes of the left and right rear tires within a unit time ΔT; II. Compare the distance traveled by the left and right rear tires per unit time. If the difference between ΔS_Left and ΔS_Right is less than 0.00001, the vehicle is traveling in a straight line, otherwise it is not traveling in a straight line. III. Calculate the vehicle heading angle θ by integrating the yaw rate; IV. Vehicle straight travel coordinates XY calculation formula: X = Xpre + Dir * ΔS * cos (θ); Y = Ypre + Dir * ΔS * sin (θ); Where Xpre represents the wheel coordinate X of the previous step; Ypre represents the wheel coordinate Y of the previous step; Dir represents the direction of vehicle travel, forward is positive and backward is negative; V. Calculation formula for vehicle non-straight-ahead driving coordinates XY: Δθ=(ΔS_Left-ΔS_Right) / VehRearSuspension; X=Xpre+2*Dir*|ΔS / Δθ*sin(Δθ / 2)|*cos(θ+Δθ / 2); Y=Ypre+2*Dir*|ΔS / Δθ*sin(Δθ / 2)|*sin(θ+Δθ / 2); Where, VehRearSuspension represents the length of the rear axle of the vehicle body; VI. Calculate the coordinates of the left rear and right rear in the geodetic coordinate system (Xl, Yl)(Xr, Yr), and then calculate the vehicle coordinates XY: X = (Xl + Xr) / 2; Y=(Yl+Yr) / 2.

3. The auxiliary warning method based on radar detection reaction delay according to claim 2 is characterized in that: The trajectory prediction algorithm of step S3 is designed based on a kinematic model by using a vehicle positioning algorithm, obstacle perception, vehicle parameters and real-time motion state information to predict the future trajectory of the vehicle; The trajectory prediction algorithm comprises the following steps: I. Obtain the vehicle's current motion state information and the coordinates of obstacles according to the vehicle positioning algorithm and radar detection algorithm; the vehicle's motion state information includes the vehicle's x coordinate, the vehicle's y coordinate, the vehicle's speed v, the heading angle θ, the steering wheel angle δ, and the gear position g; II. Adaptively set the time step and total prediction distance of trajectory prediction according to the current vehicle speed; III. Calculate the vehicle's motion state at the next moment through the position and heading angle formula: x′=x+vcos(θ)Δt y′=y+vsin(θ)Δt VehicleState′=update(VehicleState)=(x′,y′,v,θ′,δ,g) Where Δt is the time step at the next moment; L is the front and rear wheelbase; R is the distance from the center line of the vehicle to the instantaneous steering center; IV. Obtain the trajectory coordinates of the center of the front axle of the vehicle according to the current position and heading angle of the vehicle; on the two-dimensional plane, regard the center of the rear axle as the origin, and move forward along the direction of the heading angle θ by the distance of the wheelbase L to obtain the relative coordinates of the center of the front axle; use the conversion formula from polar coordinates to rectangular coordinates to convert the above relative coordinates into the global coordinate system; Assume that the coordinates of the rear axle center are (x_rear, y_rear), the heading angle is θ, and the wheelbase is L; In the global coordinate system, the coordinates of the front axle center (x_front, y_front) are calculated as follows: x_front=x_rear+Lcos(θ) y_front=y_rear+Lsin(θ) V. Based on a series of coordinate points of the predicted trajectory, find the predicted trajectory point with the smallest coordinates with the radar obstacle, and calculate the distance between the two and compare it with the safety distance; If it is less than the safe distance, the collision point is output and related information is output; at the same time, the driving trajectory distance and total time step of the vehicle predicted to the collision point are calculated by the integral method, and the driving trajectory distance and total time step to the collision point are used as the collision distance and collision time respectively; If there are multiple obstacles, output the coordinates of the obstacle closest to the vehicle. If the collision point does not exist, an invalid value is output.

Images