Assisted driving system, method, terminal and medium based on optimal tracking algorithm

Abstract

The present invention provides an assisted driving system, method, terminal and medium based on an optimization tracking algorithm, which can effectively enable each target to obtain the optimal estimated state information in each frame of data by classifying all the points used after local optimization and not participating in subsequent target clustering, fitting and other processes. Because all the tracks of the previous frame are pre-processed in the association module for the corresponding target point track optimization after local optimization processing, so that only the newly appeared target point track data enters the clustering module in the current frame, the amount of data required to be processed by the multi-target tracking method can be significantly reduced, and the actual computing power required for data processing can be reduced, so that the actual needs can be met under the condition of low computing power load, and the computing power cost of the vehicle-mounted radar itself is also reduced.

Description

Technical Field

[0001] The present invention relates to the field of intelligent driving, and in particular to an assisted driving system, method, terminal and medium based on an optimal tracking algorithm. Background Art

[0002] As the number of cars continues to increase, frequent traffic accidents have become an increasingly serious public safety issue. The complex driving environment and frequent road traffic accidents have led to an increasing demand for the ADAS (Advanced Driver Assistant System) function of smart cars. ADAS functions effectively assist the driver's eyes, ears and other senses, expand the driver's ability to perceive dangerous driving in the driving environment, better detect the environmental information around the vehicle itself and exchange information with other vehicles or infrastructure in traffic, and assist the driver in precise control of the vehicle.

[0003] The driving task of a vehicle consists of functions such as information perception and decision control, which can improve the vehicle's ability to perceive environmental information and the warning rate when there are dangerous conditions in the environmental information, helping the driver to make accurate decisions and better avoid accidents caused by danger. As one of the key components for the vehicle to perceive environmental information, the on-board radar sensor and the ADAS functions developed based on the on-board radar often require strict time and space definition processing to collect valid radar data, and then use the valid radar data to develop ADAS functions.

[0004] In the existing ADAS warning function developed based on vehicle-mounted radar, the multi-target tracking method mainly clusters the target point traces output by the radar, starts the track, and then predicts, associates and updates the multiple tracks to achieve multi-target tracking. In the multi-target tracking method, all the point traces of the current frame will be processed in each module. All target point traces will be traversed during clustering, and then all data information will still be traversed in the prediction, association and update modules, so that the computing power required for this process is relatively high. It is often considered to increase the computing power by adding hardware such as FPGA and GPU. At the same time, because each module is traversing and calculating all data, especially each frame of data is clustered and then associated with the track, it is impossible to record the best point of the track and the target point trace in the next frame. When the best point track cannot be associated, it is impossible to update to the optimal state of the target at the current moment when the state is updated, that is, the target position, speed and angle are not optimal.

[0005] Therefore, in order to solve the problem of more accurate functional warnings in ADAS systems, improve the vehicle's ability to predict dangers during road driving, and reduce the computing power consumption required by the algorithm, the present invention has developed an assisted driving solution based on a multi-target tracking method of a vehicle-mounted radar, and applies intelligent car assisted driving. Summary of the invention

[0006] To solve the above technical problems, the present invention provides an assisted driving system based on an optimal tracking algorithm, comprising a vehicle driving data acquisition module, a radar point trace data acquisition module, and a multi-target tracking algorithm module; the vehicle driving data acquisition module is used to collect vehicle driving data, and the vehicle driving data includes the vehicle speed, the vehicle displacement, and the yaw rate; the radar point trace data acquisition module is used to collect the state data of the target, and the state data of the target includes the azimuth, distance, displacement, speed, and acceleration of the target relative to the vehicle; the multi-target tracking algorithm module is used to perform an optimal state estimation of the position and speed of the target at the next moment based on the vehicle driving data and the state data of the target at the current moment, so as to obtain the optimal position estimation and optimal speed estimation of the target; the optimal position estimation and optimal speed estimation of the target are input into the ADAS system for collision warning to assist driving.

[0007] Optionally, the multi-target tracking algorithm module is used to calculate a first evaluation displacement based on the displacement, velocity and acceleration of the target relative to the vehicle at the current moment, and use the yaw angular rate of the vehicle at the current moment to compensate and estimate the first evaluation displacement to obtain the optimal position estimate of the target.

[0008] Optionally, the multi-target tracking algorithm module is used to calculate the first evaluation displacement based on the displacement, velocity and acceleration of the target relative to the vehicle at the current moment, including:

[0009] The first evaluation displacement at the next moment is calculated according to the following formulas (1)-(2):

[0010] x k =x k-1 +v x *t+0.5*a x *t 2 (1)

[0011] Among them, x k is the target longitudinal displacement at the next moment, x k-1 is the target longitudinal displacement at the current moment, v x is the target longitudinal velocity at the current moment, a x is the target longitudinal acceleration at the current moment, and t is the unit time;

[0012] y k =y k-1+v y *t+0.5*a y *t 2 (2)

[0013] Among them, y k is the lateral displacement of the target at the next moment, y k-1 is the lateral displacement of the target at the current moment, v y is the target lateral velocity at the current moment, a y is the target lateral acceleration at the current moment; the first evaluation displacement is composed of the target longitudinal displacement at the next moment and the target lateral displacement at the next moment;

[0014] The multi-target tracking algorithm module is used to use the yaw rate of the vehicle at the current moment to perform compensation estimation on the first evaluation displacement, and obtain the optimal position estimation of the target, including:

[0015] The first evaluation displacement is compensated and estimated according to the following formula (3) to obtain the optimal position estimate of the target:

[0016]

[0017] Where x represents the longitudinal displacement after compensation, y represents the lateral displacement after compensation, and θ yawrate is the yaw angular rate; the optimal position estimate is composed of the compensated longitudinal displacement and the compensated lateral displacement.

[0018] Optionally, the multi-target tracking algorithm module is used to calculate a first evaluation speed based on the azimuth angle and speed of the target relative to the vehicle, and the speed of the vehicle, and use the yaw angular rate of the vehicle at the current moment to compensate and estimate the first evaluation speed to obtain the optimal speed estimate of the target.

[0019] Optionally, the multi-target tracking algorithm module is used to calculate the first evaluation speed based on the azimuth and speed of the target relative to the vehicle and the speed of the vehicle, including:

[0020] The velocity of the target relative to the vehicle is converted into the absolute radial velocity of the target relative to the ground according to the following formula (4):

[0021] V obj =V vehicle cos(theta)+V radar (4)

[0022] Among them, V vehicle is the current speed of the vehicle, theta is the azimuth of the target relative to the vehicle, V radar is the target speed detected by the radar, V obj is the absolute radial velocity of the target relative to the ground;

[0023] According to the absolute radial velocity V of the target relative to the ground obj , using the azimuth angle of the target relative to the vehicle, decompose it into the vehicle rectangular coordinate system to obtain the target longitudinal velocity and target lateral velocity, which are specifically calculated as follows:

[0024] V x =V obj cos(theta)

[0025] V y =V obj sin(theta) (5)

[0026] Among them, V x is the target longitudinal velocity, V y is the target lateral speed; the first evaluation speed includes the target longitudinal speed and the target lateral speed;

[0027] Using the vehicle yaw rate θ yawrate The angle change produced per unit time is used to compensate the first evaluation speed for the target speed to obtain the optimal speed estimate at the next moment:

[0028]

[0029] The optimal speed estimate includes the compensated v x 'Target longitudinal velocity and compensated target v y 'lateral speed,

[0030] Optionally, the multi-target tracking algorithm module is also used to determine whether the target meets the screening conditions based on the distance and speed of the target relative to the vehicle before inputting the optimal position estimate and optimal speed estimate of the target into the ADAS system. If so, the optimal position estimate and optimal speed estimate of the target are input into the ADAS system for collision warning.

[0031] Optionally, the assisted driving system also includes a multi-target tracking visualization module, a tracking algorithm time consumption evaluation module and a hazard warning device; the multi-target tracking visualization module is used to graphically display the target status data; the tracking algorithm time consumption evaluation module is used to count the time consumed by the timely tracking algorithm to determine the real-time performance of the system; the hazard warning device is used to receive the hazard warning signal output by the ADAS system and issue an alarm for dangerous targets.

[0032] The present invention also provides an assisted driving method based on an optimal tracking algorithm, comprising:

[0033] Collecting vehicle driving data, wherein the vehicle driving data includes vehicle speed, vehicle displacement, and yaw rate;

[0034] Collecting target status data, wherein the target status data includes the target's azimuth, distance, displacement, speed, and acceleration relative to the vehicle;

[0035] Based on the vehicle driving data at the current moment and the state data of the target, an optimal state estimation is performed on the position and speed of the target at the next moment to obtain an optimal position estimation and an optimal speed estimation of the target;

[0036] The optimal position estimate and optimal speed estimate of the target are input into the ADAS system for collision warning to assist driving.

[0037] The present invention also provides a vehicle-mounted terminal, comprising a processor, a memory and a communication bus;

[0038] The communication bus is used to realize the connection and communication between the processor and the memory;

[0039] The processor is used to execute one or more programs stored in the memory to implement the steps of the assisted driving method based on the optimal tracking algorithm as described above.

[0040] The present invention also provides a storage medium storing one or more programs, which can be executed by one or more processors to implement the steps of the assisted driving method based on the optimal tracking algorithm as described above.

[0041] The beneficial effects of the present invention are:

[0042] According to the assisted driving system, method, terminal and medium based on the optimization tracking algorithm provided by the present invention, by classifying all the points used after local optimization and not participating in the subsequent target clustering, fitting and other processes, each target can effectively obtain the optimal estimated state information in each frame of data. Because all the tracks of the previous frame are processed by local optimization and the corresponding target point track optimization association processing is pre-performed in the association module, only the newly appeared target point track data enters the clustering module in the current frame, which can significantly reduce the amount of data that needs to be processed by the multi-target tracking method, reduce the actual computing power required for data processing, and achieve low computing power load conditions. Under the condition, it can also meet the actual needs, and also reduce the computing power cost of the vehicle-mounted radar itself. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] Figure 1 This is a schematic diagram of the structure of an assisted driving system based on an optimal tracking algorithm according to the first embodiment of the present invention;

[0044] Figure 2 This is a schematic diagram of a multi-target tracking process according to the first embodiment of the present invention;

[0045] Figure 3 A schematic diagram showing a comparison between the system and true value data regarding a longitudinal historical trajectory according to the first embodiment of the present invention;

[0046] Figure 4 A schematic diagram showing a comparison between the system and true value data regarding horizontal historical trajectories according to the first embodiment of the present invention;

[0047] Figure 5 This is a schematic diagram of the time consumption statistics of each module of multi-target tracking in embodiment 1 of the present invention;

[0048] Figure 6 This is a flow chart of an assisted driving method based on an optimal tracking algorithm according to a second embodiment of the present invention;

[0049] Figure 7 This is a schematic diagram of the structure of the vehicle-mounted terminal according to the third embodiment of the present invention. DETAILED DESCRIPTION

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

[0051] Embodiment 1:

[0052] In order to solve the problems of non-optimal target state estimation, high computing power consumption, low efficiency and accuracy of assisted driving system function verification in the vehicle-mounted radar multi-target tracking method, it is necessary to use the vehicle-mounted radar multi-target tracking algorithm to estimate the target's position, speed and other state information to the best, minimize the time consumption, and enable the dangerous target information to be transmitted to each functional module in the assisted driving system in real time or faster and more accurately. The tracked target needs to calculate the target's optimal state information. The present invention provides an assisted driving system based on an optimal tracking algorithm, which improves the accuracy of tracking target state information estimation through local optimization, greatly reduces the number of points in a single frame of data that participate in global calculations, and makes the algorithm less time-consuming and more real-time.

[0053] See also Figure 1The assisted driving system includes a vehicle driving data acquisition module 11, a radar point trace data acquisition module 12, and a multi-target tracking algorithm module 13; wherein the vehicle driving data acquisition module 11 is used to collect vehicle driving data, and the vehicle driving data includes the vehicle speed, the vehicle displacement, and the yaw rate; the radar point trace data acquisition module 12 is used to collect the target state data, and the target state data includes the azimuth, distance, displacement, speed, and acceleration of the target relative to the vehicle; the multi-target tracking algorithm module 13 is used to perform an optimal state estimation of the position and speed of the target at the next moment based on the vehicle driving data at the current moment and the target state data, and obtain the optimal position estimation and optimal speed estimation of the target; the optimal position estimation and optimal speed estimation of the target are input into the ADAS system for collision warning to assist driving.

[0054] Optionally, the multi-target tracking algorithm module 13 is used to calculate the first evaluation displacement based on the displacement, velocity and acceleration of the target relative to the vehicle at the current moment, including:

[0055] The first evaluation displacement at the next moment is calculated according to the following formulas (6)-(7):

[0056] x k =x k-1 +v x *t+0.5*a x *t 2 (6)

[0057] Among them, x k is the longitudinal displacement of the target at the next moment (the longitudinal direction can be understood as the direction of the vehicle’s movement), x k-1 is the longitudinal displacement of the target at the current moment, v x is the target longitudinal velocity at the current moment, a x is the target longitudinal acceleration at the current moment, and t is the unit time;

[0058] y k =y k-1 +v y *t+0.5*a y *t 2 (7)

[0059] Among them, y k is the target lateral displacement at the next moment (lateral displacement can be understood as perpendicular to the vehicle body direction), y k-1 is the lateral displacement of the target at the current moment, v y is the target lateral velocity at the current moment, a y is the target lateral acceleration at the current moment; the first evaluation displacement is composed of the target longitudinal displacement at the next moment and the target lateral displacement at the next moment;

[0060] The multi-target tracking algorithm module 13 is used to use the yaw rate of the vehicle at the current moment to compensate and estimate the first evaluation displacement, and obtain the optimal position estimation of the target including:

[0061] The first evaluation displacement is compensated and estimated according to the following formula (8) to obtain the optimal position estimate of the target:

[0062]

[0063] Where x represents the longitudinal displacement after compensation, y represents the lateral displacement after compensation, and θ yawrate is the yaw rate; the optimal position estimate is composed of the compensated longitudinal displacement and the compensated lateral displacement.

[0064] Optionally, the multi-target tracking algorithm module 13 is used to calculate a first evaluation speed based on the azimuth angle and speed of the target relative to the vehicle, and the speed of the vehicle, and to compensate and estimate the first evaluation speed using the yaw rate of the vehicle at the current moment to obtain an optimal speed estimate of the target.

[0065] Optionally, the multi-target tracking algorithm module 13 is used to calculate the first evaluation speed based on the azimuth and speed of the target relative to the vehicle and the speed of the vehicle, including:

[0066] The velocity of the target relative to the vehicle is converted into the absolute radial velocity of the target relative to the ground according to the following formula (9):

[0067] V obj =V vehicle cos(theta)+V radar (9)

[0068] Among them, V vehicle is the current speed of the vehicle, theta is the azimuth of the target relative to the vehicle, V radar is the target speed detected by the radar, V obj is the absolute radial velocity of the target relative to the ground;

[0069] According to the absolute radial velocity V of the target relative to the ground obj , using the azimuth angle of the target relative to the vehicle, decompose it into the vehicle rectangular coordinate system (the vehicle rectangular coordinate system is parallel to the earth), and obtain the target longitudinal velocity and target lateral velocity, which are specifically calculated as follows:

[0070] V x =V obj cos(theta)

[0071] V y =V obj sin(theta) (10)

[0072] Among them, V x is the target longitudinal velocity, V y is the target lateral speed; the first evaluation speed includes the target longitudinal speed and the target lateral speed;

[0073] Using the vehicle yaw rate θ yawrate The angle change produced per unit time is used to compensate the first evaluation speed for the target speed to obtain the optimal speed estimate at the next moment:

[0074]

[0075] Among them, the optimal speed estimate includes the compensated v x 'Target longitudinal velocity and compensated v y 'Target lateral velocity,

[0076] The yaw rate θ yawrate The yaw rate is positive or negative according to the vehicle's steering direction. When turning left, the yaw rate is positive, and when turning right, the yaw rate is negative. The speed compensation is estimated based on the inference to achieve the optimal estimation calculation of the target state compensation.

[0077] In other embodiments of the present invention, the multi-target tracking algorithm module 13 is also used to determine whether the target meets the screening conditions according to the distance and speed of the target relative to the vehicle before inputting the optimal position estimate and the optimal speed estimate of the target into the ADAS system. If so, the optimal position estimate and the optimal speed estimate of the target are input into the ADAS system for collision warning. The closer the distance of the target to the vehicle and the greater the speed, the greater the probability of collision with the vehicle, and the more dangerous it is. The targets with higher danger levels are preferentially output to the ADAS functional system to verify whether the driving danger warning signal will be triggered in the ADAS functional system, and the warning device is triggered by the danger warning signal. The danger warning device actively feeds back the danger to the driver to complete the driving assistance verification. The auxiliary driving system function is mainly applied to scenarios such as blind spot collision danger, rear vehicle collision danger, rear vehicle lane change collision danger, reverse parking collision danger, and front collision danger.

[0078] In other embodiments of the present invention, the assisted driving system also includes a multi-target tracking visualization module 14, a tracking algorithm time consumption evaluation module 15 and a hazard warning device 16; the multi-target tracking visualization module 14 is used to display the target status data in a graphical manner; the tracking algorithm time consumption evaluation module 15 is used to count the time consumed by the timely tracking algorithm to determine the real-time performance of the system; the hazard warning device 16 is used to receive the hazard warning signal output by the ADAS system and issue an alarm for dangerous targets.

[0079] The first step of the tracking algorithm is to cluster the targets from the point track data, start a new track for each target, and make the best estimate for each target track. Then, combined with the historical trajectory of the target movement, the current optimal state of the target is associated with the target point track at the next moment. The optimal distance is sought in the distance dimension, that is, the distance from the point track to the current position of the track is the shortest, and it is judged whether the optimal condition is met (that is, the optimal distance in the horizontal direction is 0.5m and the longitudinal direction is 1m under the condition of satisfying the radar resolution). When there is no optimal distance condition, the strategy is adjusted to the suboptimal distance. According to the physical size of the vehicle, the suboptimal distance in the horizontal direction is 2m and the longitudinal direction is 5m.

[0080] When searching for optimal association in the distance dimension, it is still possible to associate the clutter interference points. At this time, the matching association in the speed dimension is used to solve whether the minimum error between the point track and the track meets the set speed threshold, such as 1m / s, which is the minimum allowable range of speed error of the radar detection target, and also meets the speed resolution.

[0081] Please attend Figure 2 The multi-target tracking process shown in the figure mainly associates the track with the input track through optimal search, rather than clustering all the tracks into the required targets first and then using the clustered targets to associate with the track targets. In the local optimal search and association tracking algorithm, matching tracks are first found in the distance dimension, and then matching tracks are found in the speed dimension. When a track with a high similarity to the target is associated, the track that is optimally associated is classified and marked as a processed track, that is, the track has been matched to the corresponding target track, and other tracks are classified as unprocessed tracks, which are treated as newly appeared target tracks. The clustering algorithm is used to cluster the tracks belonging to the same target in the distance dimension and the speed dimension, and the coordinates of the center point representing the target are calculated using all the target tracks in the cluster, and then the target track is started using the center point. For the track that has been associated with the target point, the heading angle of the target relative to the vehicle coordinate system is used to calculate the point in the target fitting box that is closest to the vehicle. The point information is used to update the track distance, speed and other state information and extended information to complete the optimal estimation output of the track target state.

[0082] When there is a risk of collision, the ADAS system uses the status flag to indicate the warning sign, generates a warning signal, and sends it to the warning device on the vehicle via the CAN bus to trigger the alarm.

[0083] The present invention provides an auxiliary driving system, which verifies that a radar has a function of predicting the existence of dangers in the road environment during vehicle driving, so as to solve the problem that the vehicle makes inaccurate judgments on dangerous working conditions in the road environment or discovers them untimely, reduces the occurrence of road traffic accidents, keeps the vehicle within a close distance of 80 meters, can accurately track and predict dangers in real time, generate alarm signals, and send the danger signals to the vehicle alarm device through the CAN bus, so as to verify the danger signals that warn the driver to drive safely in time.

[0084] In order to achieve the above-mentioned purpose, the present invention adopts actual road test data of vehicle-mounted radar and develops a multi-target tracking algorithm based on historical trajectory for local optimization of point traces, so that the algorithm can make the best estimate of the state information of each target detected by the radar, and score the target according to the relative position relationship and speed of the target to the vehicle. It is calculated that the higher the danger level of the target, the higher the score. The target with a higher danger level is output to the ADAS function system in priority to verify whether a driving danger warning signal will be triggered in the ADAS function system. The vehicle warning device is triggered by the danger warning signal, and the vehicle warning device actively feeds back the danger to the driver to complete the driving assistance verification.

[0085] It should be understood that the vehicle-mounted millimeter-wave radar can accurately detect the distance, speed, and angle of the target, and according to the target's own material's electromagnetic wave reflection characteristics and reflection distance, it couples and outputs data of different energy intensities, and the distance, speed, angle, energy intensity and other information corresponding to different targets in the road environment are also represented differently in the data. For road environment targets, stationary targets have no speed and no heading angle; moving targets have speed and heading angle, among which low-speed targets have low speeds and heading angles change slowly in moving scenes; non-rigid targets such as pedestrians will change their posture in the data detected by the radar, while rigid targets will not change their posture in the data detected by the radar, and their physical size is fixed.

[0086] The targets detected by radar in the vehicle driving road environment have their own characteristics in terms of distance, speed, angle and energy intensity. The warning generated by the danger of the target relative to the vehicle mainly considers whether a collision will occur, which involves the relative motion relationship in distance and speed. Whether the distance is represented by the center of mass of the target or the closest point, the distance that ultimately causes the collision risk is still the closest distance. The closest point is selected as the target representation point.

[0087] The multi-target tracking algorithm uses the Kalman filter algorithm to make the best estimate of the target information represented. During the vehicle movement, it is divided into forward movement, backward movement or static state. The forward or backward movement is further divided into three motion states: straight, left turn, and right turn. The radar can only directly detect the distance and speed of the target, and cannot estimate the impact of the steering movement of the vehicle on the radar detection target. This embodiment compensates the target detected by the radar by integrating the error impact caused by the steering of the vehicle. When turning, there will be a turning radius and the vehicle will generate a yaw rate. The yaw rate will generate a heading change in a small angle range per unit time. The position speed in the longitudinal x-axis direction and the lateral y-axis direction are respectively decomposed into the longitudinal and lateral directions according to the angle generated by the yaw rate per unit time. If the vehicle moves in the same direction, it is compensated by addition, and if the vehicle moves in the opposite direction, it is compensated by subtraction. When predicting the state of the target at the next moment, first add the displacement of the velocity and acceleration changes in the unit time to the current displacement to predict the actual displacement of the target after the unit time; when predicting the speed, add the velocity change generated by the acceleration in the unit time to the current moment speed to predict the actual speed of the target after the unit time.

[0088] Optionally, after the track is optimally predicted, when the point track and the track are associated, the associated points and the associated tracks are first screened out, and then for the unassociated point tracks and tracks, the distance of the current local position of the historical track where the point track and the track have moved is associated, and the point track and the track that meet the gate threshold are calculated and associated, and then the difference between the current speed of the point track and the track meets the speed association threshold, and the speed of the same target is matched. If the distance and speed meet the association conditions at the same time, the local optimal association calculation is completed, and the optimal track found is recorded in the structured memory corresponding to the track, and classified as a processed point track.

[0089] The target state is updated using the optimal trace to estimate the target optimal state information. At this time, the acceleration of the stationary or stopped target is set to zero, because non-moving targets have no acceleration; for moving targets within the range of 20m in the vertical and horizontal directions at close range, the acceleration is limited to a smaller range. According to the radar distance resolution and velocity resolution, the vertical limit is plus or minus 0.2m / s, and the horizontal limit is plus or minus 0.05m / s. When the acceleration value is within this range, it remains unchanged. If it exceeds the range, the maximum or minimum value in the corresponding range is selected as the current value to ensure that the influence range of the close-range acceleration on the change in speed and distance per unit time is relatively stable. The acceleration value calculated by the normal update iteration is used for the next prediction and update of the long-distance target. Finally, according to the state information of the target optimally estimated by the tracking algorithm, the target information with a higher danger level is output to the auxiliary driving function module first to verify whether the warning signal will be generated in various dangerous collision scenarios, and the danger signal is fed back to the driver through the warning device to efficiently realize the verification of the auxiliary driving function.

[0090] Optionally, in order to improve the efficiency and accuracy of the warning system, the application scenarios of the multi-target tracking algorithm are first divided according to the vehicle driving conditions, and the vehicle motion conditions are divided into stationary (targets that will not generate motion trends and remain stationary), stopped (targets that have motion trends, such as vehicles preparing to start moving), low speed, and high speed.

[0091] Under different vehicle motion conditions, the distance, speed and angle of the target detected by the radar are changing continuously over time, that is, the change of target information relative to the vehicle motion varies with different working conditions. By reading the real-time motion information of the vehicle through the CAN line and compensating it to the radar detection target information, the absolute state information of the target detected by the radar relative to the ground can be calculated.

[0092] When the vehicle is stationary, stopped, slow, or fast, the radar detects the same target in different discrete points in the same frame and different position change rates in consecutive frames. When processing the same target in the same frame, different parameters are configured for the vehicle's motion conditions, so that multiple points of the same target in the same frame are classified as one target, which is used to start the track for optimal state estimation and tracking; when processing the same target in consecutive frames, due to the existence of historical tracks, the distance, speed, and angle parameters are first configured for different conditions, and the current target information is associated to update the track information, and then the target classification and start track are performed for the target information that has not been processed for tracking.

[0093] At the same time, the design divides the different attributes of the target, sets the target with a low speed or zero speed as a stationary or stopped target, and sets the target with speed as a moving target. Because the moving target is more dangerous than the vehicle, the moving target is designed as a dangerous target with a higher priority. At the same time, the target with a closer distance is set as a target with a higher danger level, because the closer the distance, the more likely it is to have a dangerous collision. Combined with the design of priority, the targets with a higher danger level are reported first. The radar's internal DSP floating-point operation method is used for calculation optimization to reduce the tracking algorithm's demand for DSP computing power load.

[0094] Finally, the tracking output data is used to verify the assisted driving function system. The multi-target tracking algorithm outputs the target historical trajectory and compares it with the target trajectory measured synchronously by the true value system to evaluate and verify the accuracy of the target trajectory output by the vehicle-mounted radar multi-target tracking algorithm. Figure 3-4 .

[0095] For calculations using clock timing, see Figure 5When tracking the target, the time consumption of each algorithm submodule and the entire integrated module is counted, and the real-time performance of the algorithm is verified by time consumption evaluation. While effectively verifying the accuracy and real-time performance of the multi-target tracking algorithm, the optimal estimated data of the multi-target tracking algorithm is output to the assisted driving system function module to verify whether the assisted driving function of the corresponding scene accurately triggers the warning signal to the driver, completing the assisted driving verification.

[0096] The assisted driving system provided by the present invention can effectively handle and track more dangerous targets in vehicle driving scenes, and score targets with higher danger levels according to the relationship between collision distances, and arrange them in the position with the highest priority to trigger alarms according to the scores. At this time, the assisted driving function can be verified using the multi-target tracking result data to observe whether an alarm signal is generated when the assisted driving function is triggered by a dangerous target, and whether the trigger device responds to an alarm. The present invention has good theoretical guidance and application value for the development of multi-target tracking algorithms for on-board radars of smart cars and the development of assisted driving verification systems.

[0097] Embodiment 2:

[0098] This embodiment provides an assisted driving method based on the optimal tracking algorithm on the basis of the above embodiment 1. Figure 6 , mainly including:

[0099] S601, collecting vehicle driving data, where the vehicle driving data includes vehicle speed, vehicle displacement, and yaw rate;

[0100] S602, collecting target status data, where the target status data includes the target's azimuth, distance, displacement, speed, and acceleration relative to the vehicle;

[0101] S603, based on the vehicle driving data at the current moment and the state data of the target, perform an optimal state estimation on the position and speed of the target at the next moment, and obtain an optimal position estimation and an optimal speed estimation of the target;

[0102] The first evaluation displacement at the next moment is calculated according to the following formulas (11)-(12):

[0103] x k =x k-1 +v x *t+0.5*a x *t 2 (11)

[0104] Among them, x k is the longitudinal displacement of the target at the next moment (the longitudinal direction can be understood as the direction of the vehicle’s movement), x k-1 is the longitudinal displacement of the target at the current moment, v x is the target longitudinal velocity at the current moment, a xis the target longitudinal acceleration at the current moment, and t is the unit time;

[0105] y k =y k-1 +v y *t+0.5*a y *t 2 (12)

[0106] Among them, y k is the target lateral displacement at the next moment (lateral displacement can be understood as perpendicular to the vehicle body direction), y k-1 is the lateral displacement of the target at the current moment, v y is the target lateral velocity at the current moment, a y is the target lateral acceleration at the current moment; the first evaluation displacement is composed of the target longitudinal displacement at the next moment and the target lateral displacement at the next moment;

[0107] The first evaluation displacement is compensated and estimated according to the following formula (13) to obtain the optimal position estimate of the target:

[0108]

[0109] Where x represents the longitudinal displacement after compensation, y represents the lateral displacement after compensation, and θ yawrate is the yaw rate; the optimal position estimate is composed of the compensated longitudinal displacement and the compensated lateral displacement.

[0110] The velocity of the target relative to the vehicle is converted into the absolute radial velocity of the target relative to the ground according to the following formula (14):

[0111] V obj =V vehicle cos(theta)+V radar (14)

[0112] Among them, V vehicle is the current speed of the vehicle, theta is the azimuth of the target relative to the vehicle, V radar is the target speed detected by the radar, V obj is the absolute radial velocity of the target relative to the ground;

[0113] According to the absolute radial velocity V of the target relative to the ground obj , using the azimuth angle of the target relative to the vehicle, decompose it into the vehicle rectangular coordinate system (the vehicle rectangular coordinate system is parallel to the earth), and obtain the target longitudinal velocity and target lateral velocity, which are specifically calculated as follows:

[0114] V x =V obj cos(theta)

[0115] V y =V obj sin(theta) (15)

[0116] Among them, V x is the target longitudinal velocity, V y is the target lateral speed; the first evaluation speed includes the target longitudinal speed and the target lateral speed;

[0117] Using the vehicle yaw rate θ yawrate The angle change produced per unit time is used to compensate the first evaluation speed for the target speed to obtain the optimal speed estimate at the next moment:

[0118]

[0119] Among them, the optimal speed estimation includes the compensated v x 'Target longitudinal velocity and compensated v y 'Target lateral velocity,

[0120] The yaw rate θ yawrate The yaw rate is positive or negative according to the vehicle's steering direction. When turning left, the yaw rate is positive, and when turning right, the yaw rate is negative. The speed compensation is estimated based on the inference to achieve the optimal estimation calculation of the target state compensation.

[0121] S604: Input the optimal position estimate and the optimal speed estimate of the target into the ADAS system for collision warning to assist driving.

[0122] Please refer to the description in the above embodiment 1 for details, which will not be repeated here.

[0123] Embodiment three:

[0124] This embodiment provides a vehicle-mounted terminal based on the above-mentioned embodiment 1 / embodiment 2. Figure 7 , the vehicle-mounted terminal includes a processor 71, a memory 72 and a communication bus 73;

[0125] The communication bus 73 is used to realize the connection and communication between the processor 71 and the memory 72;

[0126] The processor 71 is used to execute one or more programs stored in the memory 72 to implement the steps of the assisted driving method based on the optimal tracking algorithm as described in the second embodiment.

[0127] Please refer to the description in the above-mentioned embodiment 2 for details, which will not be repeated here.

[0128] Embodiment 4:

[0129] Based on the above-mentioned embodiment 1 / embodiment 2, this embodiment provides a computer-readable storage medium, which stores one or more programs, and the one or more programs can be executed by one or more processors to implement the steps of the assisted driving method based on the optimal tracking algorithm as described in embodiment 2. For details, please refer to the description in the above-mentioned embodiment 2, which will not be repeated here.

[0130] Obviously, those skilled in the art should understand that the modules or steps of the present invention described above can be implemented by a general-purpose computing device, they can be concentrated on a single computing device, or distributed on a network composed of multiple computing devices, and optionally, they can be implemented by a program code executable by a computing device, so that they can be stored in a computer storage medium (ROM / RAM, magnetic disk, optical disk) and executed by the computing device, and in some cases, the steps shown or described can be executed in a different order than that here, or they can be made into individual integrated circuit modules, or multiple modules or steps therein can be made into a single integrated circuit module for implementation. Therefore, the present invention is not limited to any specific combination of hardware and software.

[0131] The above contents are further detailed descriptions of the present invention in combination with specific implementation methods, and it cannot be determined that the specific implementation of the present invention is limited to these descriptions. For ordinary technicians in the technical field to which the present invention belongs, several simple deductions or substitutions can be made without departing from the concept of the present invention, which should be regarded as falling within the scope of protection of the present invention.

Claims

1. An assisted driving system based on an optimal tracking algorithm, characterized in that: It includes a vehicle driving data acquisition module, a radar point trace data acquisition module, and a multi-target tracking algorithm module; the vehicle driving data acquisition module is used to collect vehicle driving data, and the vehicle driving data includes the vehicle speed, the vehicle displacement, and the yaw rate; the radar point trace data acquisition module is used to collect the state data of the target, and the state data of the target includes the azimuth, distance, displacement, speed, and acceleration of the target relative to the vehicle; the multi-target tracking algorithm module is used to perform an optimal state estimation of the position and speed of the target at the next moment based on the vehicle driving data and the state data of the target at the current moment, and obtain the optimal position estimation and optimal speed estimation of the target; the optimal position estimation and optimal speed estimation of the target are input into the ADAS system for collision warning to assist driving; The multi-target tracking algorithm module is used to calculate a first evaluation displacement based on the displacement, velocity and acceleration of the target relative to the vehicle at the current moment, including: The first evaluation displacement at the next moment is calculated according to the following formulas (1)-(2): x k =x k-1 +v x *t+0.5*a x *t 2 (1) Among them, x k is the target longitudinal displacement at the next moment, x k-1 is the longitudinal displacement of the target at the current moment, v x is the target longitudinal velocity at the current moment, a x is the target longitudinal acceleration at the current moment, and t is the unit time; y k =y k-1 +v y *t+0.5*a y *t 2 (2) Among them, y k is the lateral displacement of the target at the next moment, y k-1 is the lateral displacement of the target at the current moment, v y is the target lateral velocity at the current moment, a y is the target lateral acceleration at the current moment; the first evaluation displacement is composed of the target longitudinal displacement at the next moment and the target lateral displacement at the next moment; The multi-target tracking algorithm module is used to use the yaw rate of the vehicle at the current moment to perform compensation estimation on the first evaluation displacement, and obtain the optimal position estimation of the target, including: The first evaluation displacement is compensated and estimated according to the following formula (3) to obtain the optimal position estimate of the target: Where x represents the longitudinal displacement after compensation, y represents the lateral displacement after compensation, and θ yawrate is the yaw angular rate; the optimal position estimate is composed of the compensated longitudinal displacement and the compensated lateral displacement, and the first evaluation displacement is compensated and estimated using the yaw angular rate of the vehicle at the current moment to obtain the optimal position estimate of the target.

2. The assisted driving system based on the optimal tracking algorithm as claimed in claim 1, characterized in that: The multi-target tracking algorithm module is used to calculate a first evaluation speed based on the azimuth angle and speed of the target relative to the vehicle and the speed of the vehicle, and to compensate and estimate the first evaluation speed using the yaw rate of the vehicle at the current moment to obtain an optimal speed estimate of the target.

3. The assisted driving system based on the optimal tracking algorithm as claimed in claim 2, characterized in that: The multi-target tracking algorithm module is used to calculate the first evaluation speed based on the azimuth and speed of the target relative to the vehicle and the speed of the vehicle, including: The velocity of the target relative to the vehicle is converted into the absolute radial velocity of the target relative to the ground according to the following formula (4): V obj =V vehicle cos(theta)+V radar (4) Among them, V vehicle is the current speed of the vehicle, theta is the azimuth of the target relative to the vehicle, V radar is the target speed detected by the radar, V obj is the absolute radial velocity of the target relative to the ground; According to the absolute radial velocity V of the target relative to the ground obj , using the azimuth angle of the target relative to the vehicle, decompose it into the vehicle rectangular coordinate system to obtain the target longitudinal velocity and target lateral velocity, which are specifically calculated as follows: V x =V obj cos(theta) V y =V obj sin(theta)(5) Among them, V x is the target longitudinal velocity, V y is the target lateral speed; the first evaluation speed includes the target longitudinal speed and the target lateral speed; Using the vehicle yaw rate θ yawrate The angle change produced per unit time is used to compensate the first evaluation speed for the target speed to obtain the optimal speed estimate at the next moment: The optimal speed estimate includes the compensated v x 'Target longitudinal velocity and compensated v y 'Target lateral velocity.

4. The assisted driving system based on the optimal tracking algorithm according to any one of claims 1 to 3, characterized in that: The multi-target tracking algorithm module is also used to determine whether the target meets the screening conditions based on the distance and speed of the target relative to the vehicle before inputting the optimal position estimate and optimal speed estimate of the target into the ADAS system. If so, the optimal position estimate and optimal speed estimate of the target are input into the ADAS system for collision warning.

5. The assisted driving system based on the optimal tracking algorithm as claimed in claim 4, characterized in that: The assisted driving system also includes a multi-target tracking visualization module, a tracking algorithm time consumption evaluation module and a danger warning device; the multi-target tracking visualization module is used to display the target status data in a graphical manner; the tracking algorithm time consumption evaluation module is used to count the time consumed by the timely tracking algorithm to determine the real-time performance of the system; the danger warning device is used to receive the danger warning signal output by the ADAS system and issue a warning for dangerous targets.

6. An assisted driving method based on an optimal tracking algorithm, characterized in that: include: Collecting vehicle driving data, wherein the vehicle driving data includes vehicle speed, vehicle displacement, and yaw rate; Collecting target status data, wherein the target status data includes the target's azimuth, distance, displacement, speed, and acceleration relative to the vehicle; Based on the vehicle driving data at the current moment and the state data of the target, an optimal state estimation is performed on the position and speed of the target at the next moment to obtain an optimal position estimation and an optimal speed estimation of the target, wherein a first evaluation displacement is calculated based on the displacement, speed and acceleration of the target relative to the vehicle at the current moment, including: The first evaluation displacement at the next moment is calculated according to the following formulas (6)-(7): x k =x k-1 +v x *t+0.5*a x *t 2 (6) Among them, x k is the target longitudinal displacement at the next moment, x k-1 is the longitudinal displacement of the target at the current moment, v x is the target longitudinal velocity at the current moment, a x is the target longitudinal acceleration at the current moment, and t is the unit time; y k =y k-1 +v y *t+0.5*a y *t 2 (7) Among them, y k is the lateral displacement of the target at the next moment, y k-1 is the lateral displacement of the target at the current moment, v y is the target lateral velocity at the current moment, a y is the target lateral acceleration at the current moment; the first evaluation displacement is composed of the target longitudinal displacement at the next moment and the target lateral displacement at the next moment; Using the yaw rate of the vehicle at the current moment to perform compensation estimation on the first evaluation displacement to obtain the optimal position estimation of the target includes: The first evaluation displacement is compensated and estimated according to the following formula (8) to obtain the optimal position estimate of the target: Where x represents the longitudinal displacement after compensation, y represents the lateral displacement after compensation, and θ yawrate is the yaw rate; the optimal position estimate is composed of the compensated longitudinal displacement and the compensated lateral displacement, and the first evaluation displacement is compensated and estimated using the yaw rate of the vehicle at the current moment to obtain the optimal position estimate of the target; The optimal position estimate and optimal speed estimate of the target are input into the ADAS system for collision warning to assist driving.

7. A vehicle-mounted terminal, characterized in that: The vehicle-mounted terminal includes a processor, a memory and a communication bus; The communication bus is used to realize the connection and communication between the processor and the memory; The processor is used to execute one or more programs stored in the memory to implement the steps of the assisted driving method based on the optimal tracking algorithm as described in claim 6.

8. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores one or more programs, and the one or more programs can be executed by one or more processors to implement the steps of the assisted driving method based on the optimal tracking algorithm as described in any one of claim 6.

Images