Method for eliminating turning vehicle course angle abnormity caused by positioning deviation in intersection scene

Abstract

The invention relates to a method for eliminating turning vehicle course angle abnormity caused by positioning deviation in an intersection scene, belongs to the technical field of holographic intersections, and mainly solves the problem that positioning measurement values obtained in a target tracking process are deviated due to the fact that a visual sensor detects that the center point of the lower edge of a turning vehicle pixel frame is not fixed at a certain position of a vehicle. Further, deviation exists between the course angle of the vehicle and the actual value, and finally, the phenomena of unsmooth turning of the vehicle and unconformity between the course angle and the position in holographic intersection visual display are caused. The abnormal course angle tracked by the vehicle due to the fact that the center point of the lower edge of the pixel frame of the turning vehicle is not fixed to a certain position of the vehicle is eliminated, and the course angle of a static vehicle is not affected.

Description

Technical Field

[0001] This invention relates to the field of holographic intersection technology, specifically a method for eliminating abnormal heading angles of turning vehicles caused by positioning deviations in intersection scenarios. Background Technology

[0002] Holographic intersections, a key development area in the "smart transportation" industry, rely on advanced sensor detection technology. Currently, camera terminals are widely used in sensor technology for traffic infrastructure at urban intersections to detect motor vehicles, non-motorized vehicles, and pedestrians in real time. However, for vehicles turning within the intersection, the center point of the lower edge of the pixel frame output by the camera during detection is not fixed at a specific vehicle position. This leads to deviations in the positioning measurements obtained during target tracking, resulting in discrepancies between the vehicle's heading angle and the actual value. Ultimately, this results in an uneven display of vehicle turns in the holographic intersection visualization, with the heading angle and position not matching the actual value. Therefore, resolving the abnormal heading angle phenomenon caused by the inconsistent position of the pixel values ​​output by the camera for detecting turning vehicles is a pressing issue that needs to be addressed in the application of cameras in traffic scenarios. Summary of the Invention

[0003] The purpose of this invention is to provide a method to eliminate the abnormal turning heading angle of turning vehicles in intersection scenarios, where the pixel value is not fixed at a certain position of the vehicle. This method eliminates the phenomenon that the turning heading angle of turning vehicles in intersections does not change correctly with the change of position by means of coordinate transformation, target tracking, trajectory fitting, state judgment, and trend judgment.

[0004] This invention provides a method for eliminating abnormal heading angles of turning vehicles caused by positioning deviations in intersection scenarios. The specific steps of this method are as follows:

[0005] S1: Vehicle data preprocessing:

[0006] (1) Data acquisition: Acquire the current frame vehicle data detected in real time by the camera at the target intersection, mainly the ID and pixel coordinates of each vehicle, and store them in set A;

[0007] (2) Coordinate transformation: Convert the pixel coordinates of all vehicles in A to UTM coordinates;

[0008] S2: Vehicle status update:

[0009] (1) Vehicle tracking: Assuming that the historical position, speed and heading angle of the vehicle are stored in set B, Kalman filtering is performed on all vehicles in set B based on the latest vehicle position measurement value obtained in set A, and the position, speed and heading angle of the vehicles in set B with new measurement values ​​are updated; for vehicles in set B with a frame number less than N1, the default value is 3, and the vehicle motion state is updated.

[0010] (2) Filter vehicles in set B that meet the following conditions: the number of historical data frames of vehicles in set B is greater than or equal to N1;

[0011] (3) Vehicle motion state update: For vehicles that meet the conditions selected in set B, update their motion state in the current frame;

[0012] Assuming the current update is of the vehicle's VEH motion state, the update method for the motion states of other vehicles is the same as that for the vehicle's VEH.

[0013] Let the current time be t, and the two velocity components in the UTM coordinate system output by the Kalman tracking be v. x v y Then the speed of the vehicle at the current moment The velocities v at times t-2 and t-1 t-2 v t-1 Similarly, we can deduce that if status represents the target's motion state,

[0014] S3: Trajectory Fitting

[0015] Assuming that the current trajectory fitting is performed on the vehicle's VEH, the trajectory fitting method for other vehicles is the same as that for the vehicle's VEH;

[0016] (1) Filter vehicles in B that meet the following conditions:

[0017] 1) The number of frames in the vehicle historical data in set B is greater than or equal to N2 (the default value is 20);

[0018] 2) The vehicle is in motion. t =1;

[0019] (2) Fitting the vehicle trajectory:

[0020] 1) Determine if the time is abnormal, calculate the time difference between two adjacent frames; obtain the timestamp of the current frame. t Previous frame timestamp t-1 Determine if time is monotonically increasing and if any anomalies have occurred; if there are no anomalies in time, then Δt = timestamp. t -timestamp t-1 Then, perform heading angle calibration; if the timing is abnormal, end the heading angle calibration for that frame.

[0021] 2) Fit a circle and obtain the real-time radius; obtain the positions at three time points: the current frame, the eleventh frame from the end, and the twentieth frame from the end. Use these three positions to fit a circle and update the radius of the circle corresponding to the trajectory in real time; let the vehicle's UTM coordinates in the current frame after the S2 state variable update be (x... t ,yt The UTM coordinates of the vehicle at time t-10, the eleventh frame from the end, are (x'). t-10 ,y' t-10 The UTM coordinates of the vehicle at time t-19 (the twentieth frame from the end) are (x' t-19 ,y' t-19 The following describes a method for finding a circle using three points:

[0022] Define slopes k1 and k2, if y t ≠y t-10 And y t-10 ≠y t-19 ,but Let the coordinates of the center of the circle be (x h ,y h ),but Let the radius of the circle be R, then

[0023] S4: Calculate the change in heading angle:

[0024] Assuming that the current calculation is for the vehicle's VEH heading angle change, the calculation method for other vehicles is the same as for the vehicle's VEH;

[0025] Based on the radius of the fitted circle, the time difference between two adjacent frames, and the velocity of the current frame, calculate the difference in heading angle between the current frame and the previous frame, i.e., the change in heading angle. radians to degrees

[0026] S5: Judging the trend of heading angle change:

[0027] Assuming the current analysis focuses on the trend of VEH heading angle change in the vehicle being analyzed, the analysis methods for other vehicles are consistent with the vehicle's VEH;

[0028] Assuming the vehicle's final output heading angle is stored in set C, if the accumulated historical heading angles in set C are greater than or equal to N2 frames, retrieve the heading angles at three times: the current frame, the eleventh frame from the end, and the twentieth frame from the end. t ,heading t-10 ,heading t-19 Perform heading angle trend analysis; if the cumulative historical heading angle in set C is less than N2 frames, end the heading angle calibration for that frame.

[0029] If this is the first time determining the trend of heading angle changes, then only the heading angle needs to be compared. t ,heading t-19 The comparison method is as follows:

[0030] 1) If heading t >heading t-19 or headingt <45 and heading t-19 If the heading angle is greater than 350, then the heading angle increases.

[0031] 2) If heading t <heading t-19 or heading t-19 <45 and heading t If the heading angle is greater than 350, then the heading angle decreases.

[0032] If it is the nth time (n>1), then it is necessary to compare the headings. t ,heading t-10 ,heading t-19 And combine this with historical increasing and decreasing patterns to conduct current trend analysis:

[0033] 1) If the previous trend was decreasing, and the heading condition is not met. t >heading t-10 or heading t <45 and heading t-10 >350, heading t-10 >heading t-19 or heading t-10 <45 and heading t-19 If the value is greater than 350, the heading angle decreases; if the condition is met, the heading angle increases.

[0034] 2) If the previous trend was increasing, and if the heading condition is met... t <heading t-10 or heading t-10 <45 and heading t >350, heading t-10 <heading t-19 or heading t-19 <45 and heading t-10 If the heading angle is greater than 350, then the heading angle decreases; if this condition is not met, then the heading angle increases.

[0035] S6: Heading angle recalculated:

[0036] The heading angle change Δω is obtained from S4, and the heading angle change trend is obtained from S5. Let the heading angle after calibration in the current frame be heading. t ',but

[0037] Among headings t-1 It is the heading angle after calibration in the previous frame;

[0038] Since the vehicle's heading angle ranges from [0° to 360°], a final correction is made to the final output heading angle.

[0039]

[0040] Preferably, in step S2, the principle for updating the vehicle motion state in set B where the number of historical data frames is greater than or equal to N1 is as follows:

[0041] Current static state conditions:

[0042] 1) If status t-1 =0 and does not satisfy v t-2 <=v t-1 <=v t v t ≥v motion (v motion This is the minimum speed that the vehicle must increase to to transition from stationary to moving (default value is 1, unit: m / s). If the target is currently stationary, then its status is unchanged. t =0;

[0043] 2) If status t-1 =1 and satisfy v t-2 >=v t-1 >=v t v t ≤v stop (v stop The maximum speed at which the vehicle should decrease from motion to rest (default value is 2, unit: m / s) indicates that the target is currently stationary, i.e., its status is... t =0;

[0044] Current motion state conditions:

[0045] 1) If status t-1 =0 and satisfy v t-2 <=v t-1 <=v t v t ≥v motion If the target is currently in motion, then its status is... t =1;

[0046] 2) If status t-1 =1 and does not satisfy v t-2 >=v t-1 >=v t v t ≤v stop If the target is still in motion at the current moment, then its status is... t =1.

[0047] Preferably, for vehicles in set B with a frame count less than N1, the vehicle motion state is updated according to the following principle: Let the current time be t, and the two component velocities in the UTM coordinate system output by Kalman tracking be v x v y Then the speed of the vehicle at the current moment If v t ≥v go v go The default value is 1, in m / s, which means the target is currently in motion; otherwise, the target is stationary.

[0048] Compared with existing technologies, the beneficial effects of this invention are as follows: This invention analyzes the speed change patterns of vehicles when stationary and in motion, summarizes the speed change characteristics when switching vehicle motion states, and based on these characteristics, performs real-time judgment and updates of the vehicle's operating state, promptly identifying moving vehicles; it also performs real-time trajectory fitting using vehicle history and current position information to obtain an approximate circle and radius of the vehicle's trajectory; based on geometric relationships, it uses the radius of the circle, the vehicle's current speed, and the time difference between two adjacent frames to obtain the change in the heading angle of the current frame; by analyzing the increasing and decreasing patterns of the heading angle, it summarizes the characteristics of different trends in vehicle heading angle changes, and based on these characteristics, it performs real-time judgment and updates of the vehicle's heading angle change trend, promptly obtaining the heading angle change trend; combining the heading angle change amount and the heading angle change trend, it obtains the heading angle of the current frame, eliminating the abnormal heading angle phenomenon for turning vehicles. This invention eliminates the phenomenon of abnormal turning heading angles caused by pixel values ​​not being fixed at a certain vehicle position in intersection scenarios, solving the problem of abnormal vehicle detection and tracking heading angles brought about by cameras in traffic scene applications. Attached Figure Description

[0049] Figure 1 This is a flowchart illustrating the implementation of the present invention;

[0050] Figure 2 For trajectory fitting in this invention;

[0051] Figure 3 This refers to the change in heading angle in this invention. Detailed Implementation

[0052] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to specific embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0053] Example 1

[0054] See Figure 1-3This invention relates to the field of holographic intersection technology, and mainly solves the problem that the center point of the lower edge of the pixel frame of the visual sensor detecting a turning vehicle is not fixed at a certain position of the vehicle, which leads to deviations in the positioning measurement values ​​obtained during target tracking, and in turn, deviations between the vehicle's heading angle and the actual value. Ultimately, this results in an unsmooth turning of the vehicle in the holographic intersection visualization, with the heading angle and position not matching. The first step involves acquiring real-time vehicle data detected by intersection cameras and converting the vehicle pixel coordinates to UTM coordinates. The second step uses Kalman filtering based on the UTM coordinates to track vehicles, obtaining their real-time position, speed, and heading angle. The third step determines the vehicle's current motion state based on its historical data and current speed. The fourth step fits a circle using the vehicle's historical data and current position, calculating the circle's real-time radius. The fifth step uses geometric relationships, the circle's radius, current speed, and the time difference between the current and previous frames, to calculate the heading angle change. The sixth step analyzes the heading angle's trend (increasing or decreasing) based on the vehicle's historical data and the current Kalman tracking output. The seventh step combines the heading angle from the previous frame with its trend and change to calculate the heading angle for the current frame. This process eliminates the anomalies in the tracking heading angle caused by the non-fixed lower edge center point of the pixel frame of turning vehicles, while ensuring that it does not affect the heading angle of stationary vehicles.

[0055] A method for eliminating abnormal heading angles of turning vehicles caused by positioning deviations in intersection scenarios, the specific steps of which are as follows:

[0056] S1: Vehicle data preprocessing:

[0057] (1) Data acquisition: Acquire the current frame vehicle data detected in real time by the camera at the target intersection, mainly the ID and pixel coordinates of each vehicle, and store them in set A;

[0058] (2) Coordinate transformation: Convert the pixel coordinates of all vehicles in A to UTM coordinates;

[0059] S2: Vehicle status update:

[0060] (1) Vehicle tracking: Assuming that the historical vehicle position, speed, and heading angle are stored in set B, Kalman filtering is performed on all vehicles in set B based on the latest vehicle position measurement value obtained in set A. The position, speed, and heading angle of vehicles in set B with new measurement values ​​are updated. For vehicles in set B with a frame number less than N1, the default value is 3. The vehicle motion state is updated according to the following principle: Let the current time be t, and the two components of velocity in the UTM coordinate system output by Kalman tracking be v. x v y Then the speed of the vehicle at the current moment If v t≥v go v go The default value is 1, in m / s, which means the target is currently in motion; otherwise, the target is stationary.

[0061] (2) Filter vehicles in set B that meet the following conditions: the number of historical data frames of vehicles in set B is greater than or equal to N1;

[0062] (3) Vehicle motion state update: For vehicles that meet the conditions selected in set B, update their motion state in the current frame;

[0063] Assuming the current update is of the vehicle's VEH motion state, the update method for the motion states of other vehicles is the same as that for the vehicle's VEH.

[0064] Let the current time be t, and the two velocity components in the UTM coordinate system output by the Kalman tracking be v. x v y Then the speed of the vehicle at the current moment The velocities v at times t-2 and t-1 t-2 v t-1 Similarly, we can deduce that if status represents the target's motion state,

[0065] The principles for updating motion status are as follows:

[0066] Current static state conditions:

[0067] 1) If status t-1 =0 and does not satisfy v t-2 <=v t-1 <=v t v t ≥v motion (v motion This is the minimum speed that the vehicle must increase to to transition from stationary to moving (default value is 1, unit: m / s). If the target is currently stationary, then its status is unchanged. t =0;

[0068] 2) If status t-1 =1 and satisfy v t-2 >=v t-1 >=v t v t ≤v stop (v stop The maximum speed at which the vehicle should decrease from motion to rest (default value is 2, unit: m / s) indicates that the target is currently stationary, i.e., its status is... t =0;

[0069] Current motion state conditions:

[0070] 1) If status t-1 =0 and satisfy v t-2 <=v t-1 <=v t v t ≥v motion If the target is currently in motion, then its status is... t =1;

[0071] 2) If status t-1 =1 and does not satisfy v t-2 >=v t-1 >=v t v t ≤v stop If the target is still in motion at the current moment, then its status is... t =1.

[0072] S3: Trajectory Fitting

[0073] Assuming that the current trajectory fitting is performed on the vehicle's VEH, the trajectory fitting method for other vehicles is the same as that for the vehicle's VEH;

[0074] (1) Filter vehicles in B that meet the following conditions:

[0075] 1) The number of frames in the vehicle historical data in set B is greater than or equal to N2 (the default value is 20);

[0076] 2) The vehicle is in motion. t =1;

[0077] (2) Fitting the vehicle trajectory:

[0078] 1) Determine if the time is abnormal, calculate the time difference between two adjacent frames; obtain the timestamp of the current frame. t Previous frame timestamp t-1 Determine if time is monotonically increasing and if any anomalies have occurred; if there are no anomalies in time, then Δt = timestamp. t -timestamp t-1 Then, perform heading angle calibration; if the timing is abnormal, end the heading angle calibration for that frame.

[0079] 2) Fit a circle and obtain the real-time radius; obtain the positions at three time points: the current frame, the eleventh frame from the end, and the twentieth frame from the end. Use these three positions to fit a circle and update the radius of the circle corresponding to the trajectory in real time; let the vehicle's UTM coordinates in the current frame after the S2 state variable update be (x... t ,y t The UTM coordinates of the vehicle at time t-10, the eleventh frame from the end, are (x'). t-10,y' t-10 The UTM coordinates of the vehicle at time t-19 (the twentieth frame from the end) are (x' t-19 ,y' t-19 The following describes a method for finding a circle using three points:

[0080] Define slopes k1 and k2, if y t ≠y t-10 And y t-10 ≠y t-19 ,but Let the coordinates of the center of the circle be (x h ,y h ),but Let the radius of the circle be R, then

[0081] S4: Calculate the change in heading angle:

[0082] Assuming that the current calculation is for the vehicle's VEH heading angle change, the calculation method for other vehicles is the same as for the vehicle's VEH;

[0083] Based on the radius of the fitted circle, the time difference between two adjacent frames, and the velocity of the current frame, calculate the difference in heading angle between the current frame and the previous frame, i.e., the change in heading angle. radians to degrees

[0084] S5: Judging the trend of heading angle change:

[0085] Assuming the current analysis focuses on the trend of VEH heading angle change in the vehicle being analyzed, the analysis methods for other vehicles are consistent with the vehicle's VEH;

[0086] Assuming the vehicle's final output heading angle is stored in set C, if the accumulated historical heading angles in set C are greater than or equal to N2 frames, retrieve the heading angles at three times: the current frame, the eleventh frame from the end, and the twentieth frame from the end. t ,heading t-10 ,heading t-19 Perform heading angle trend analysis; if the cumulative historical heading angle in set C is less than N2 frames, end the heading angle calibration for that frame.

[0087] If this is the first time determining the trend of heading angle changes, then only the heading angle needs to be compared. t ,heading t-19 The comparison method is as follows:

[0088] 1) If heading t >heading t-19 or heading t <45 and heading t-19 If the heading angle is greater than 350, then the heading angle increases.

[0089] 2) If heading t <heading t-19 or heading t-19 <45 and heading t If the heading angle is greater than 350, then the heading angle decreases.

[0090] If it's the nth time (n>1), then it's necessary to compare the headings. t ,heading t-10 ,heading t-19 And combine this with historical increasing and decreasing patterns to conduct current trend analysis:

[0091] 1) If the previous trend was decreasing, and the heading condition is not met. t >heading t-10 or

[0092] heading t <45 and heading t-10 >350, heading t-10 >heading t-19 or

[0093] heading t-10 <45 and heading t-19 If the value is greater than 350, the heading angle decreases; if the condition is met, the heading angle increases.

[0094] 2) If the previous trend was increasing, and if the heading condition is met... t <heading t-10 or

[0095] heading t-10 <45 and heading t >350, heading t-10 <heading t-19 or

[0096] heading t-19 <45 and heading t-10 If the heading angle is greater than 350, then the heading angle decreases; if this condition is not met, then the heading angle increases.

[0097] S6: Heading angle recalculated:

[0098] The heading angle change Δω is obtained from S4, and the heading angle change trend is obtained from S5. Let the heading angle after calibration in the current frame be heading. t ',but

[0099] Among headings t-1 It is the heading angle after calibration in the previous frame;

[0100] Since the vehicle's heading angle ranges from [0° to 360°], a final correction is made to the final output heading angle.

[0101]

[0102] The above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any equivalent substitutions or modifications made by those skilled in the art within the scope of the technology disclosed in the present invention, based on the technical solution and inventive concept of the present invention, should be covered within the scope of protection of the present invention.

Claims

1. A method for eliminating abnormal heading angles of turning vehicles caused by positioning deviations in intersection scenarios, characterized in that: The specific steps of this method to eliminate abnormal heading angles of turning vehicles caused by positioning errors in intersection scenarios are as follows: S1: Vehicle data preprocessing: (1) Data acquisition: Acquire the current frame vehicle data detected in real time by the camera at the target intersection, mainly the ID and pixel coordinates of each vehicle, and store them in set A; (2) Coordinate transformation: Convert the pixel coordinates of all vehicles in A to UTM coordinates; S2: Vehicle status update: (1) Vehicle tracking: Assuming that the historical position, speed and heading angle of the vehicle are stored in set B, based on the latest vehicle position measurement value obtained in set A, Kalman filter tracking is performed on all vehicles in set B, and the position, speed and heading angle of the vehicles in set B with new measurement values ​​are updated. For vehicles in set B with a frame number less than N1, the default value is 3, and their current motion state is updated. (2) Filter vehicles in set B that meet the following conditions: the number of historical data frames of vehicles in set B is greater than or equal to N1; (3) Vehicle motion state update: For vehicles that meet the conditions selected in set B, update their motion state in the current frame; Assuming the current update is of the vehicle's VEH motion state, the update method for the motion states of other vehicles is the same as that for the vehicle's VEH. Let the current time be t, and the two velocity components in the UTM coordinate system output by the Kalman tracking be v. x v y Then the speed of the vehicle at the current moment The velocities v at times t-2 and t-1 t-2 v t-1 Similarly, we can deduce that if status represents the target's motion state, S3: Trajectory Fitting Assuming that the current trajectory fitting is performed on the vehicle's VEH, the trajectory fitting method for other vehicles is the same as that for the vehicle's VEH; (1) Filter vehicles in B that meet the following conditions: 1) The number of historical vehicle data frames in set B is greater than or equal to N2, with a default value of 20; 2) The vehicle is in motion. t =1; (2) Fitting the vehicle trajectory: 1) Determine if the time is abnormal, calculate the time difference between two adjacent frames; obtain the timestamp of the current frame. t Previous frame timestamp t-1 Determine if time is monotonically increasing and if any anomalies have occurred; if there are no anomalies in time, then Δt = timestamp. t -timestamp t-1 Then, perform heading angle calibration; if the timing is abnormal, end the heading angle calibration for that frame. 2) Fit a circle and obtain the real-time radius; obtain the positions at three time points: the current frame, the eleventh frame from the end, and the twentieth frame from the end. Use these three positions to fit a circle and update the radius of the circle corresponding to the trajectory in real time; let the vehicle's UTM coordinates in the current frame after the S2 state variable update be (x... t ,y t The UTM coordinates of the vehicle at time t-10, the eleventh frame from the end, are (x'). t-10 ,y' t-10 The UTM coordinates of the vehicle at time t-19 (the twentieth frame from the end) are (x' t-19 ,y' t-19 The following describes a method for finding a circle using three points: Define slopes k1 and k2, if y t ≠y t-10 And y t-10 ≠y t-19 ,but Let the coordinates of the center of the circle be (x h ,y h ),but Let the radius of the circle be R, then S4: Calculate the change in heading angle: Assuming that the current calculation is for the vehicle's VEH heading angle change, the calculation method for other vehicles is the same as for the vehicle's VEH; Based on the radius of the fitted circle, the time difference between two adjacent frames, and the velocity of the current frame, calculate the difference in heading angle between the current frame and the previous frame, i.e., the change in heading angle. radians to degrees S5: Judging the trend of heading angle change: Assuming the current analysis focuses on the trend of VEH heading angle change in the vehicle being analyzed, the analysis methods for other vehicles are consistent with the vehicle's VEH; Assuming the vehicle's final output heading angle is stored in set C, if the accumulated historical heading angles in set C are greater than or equal to N2 frames, retrieve the heading angles at three times: the current frame, the eleventh frame from the end, and the twentieth frame from the end. t ,heading t-10 ,heading tR19 Perform heading angle trend analysis; if the cumulative historical heading angle in set C is less than N2 frames, end the heading angle calibration for that frame. If this is the first time determining the trend of heading angle changes, then only the heading angle needs to be compared. t ,heading tR19 The comparison method is as follows: 1) If heading t >heading tR19 or heading t <45 and heading t-19 If the heading angle is greater than 350, then the heading angle increases. 2) If heading t <heading t-19 or heading tR19 <45 and heading t If the heading angle is greater than 350, then the heading angle decreases. If it's the nth time (n>1), then it's necessary to compare the headings. t ,heading tR10 ,heading tR19 And combine this with historical increasing and decreasing patterns to conduct current trend analysis: 1) If the previous trend was decreasing, and the heading condition is not met. t >heading t-10 or heading t <45 and heading t-10 >350, heading t-10 >heading t-19 or heading t-10 <45 and heading t-19 If the value is greater than 350, the heading angle decreases; if the condition is met, the heading angle increases. 2) If the previous trend was increasing, and if the heading condition is met... t <heading t-10 or heading t-10 <45 and heading t >350, heading t-10 <heading t-19 or heading t-19 <45 and heading t-10 If the heading angle is greater than 350, then the heading angle decreases; if this condition is not met, then the heading angle increases. S6: Heading angle recalculated: The heading angle change Δω is obtained from S4, and the heading angle change trend is obtained from S5. Let the heading angle after calibration in the current frame be heading. t ',but Among headings t-1 It is the heading angle after calibration in the previous frame; Because the vehicle's heading angle range is Make a final correction to the final output heading angle.

2. The method for eliminating abnormal heading angles of turning vehicles caused by positioning deviations in intersection scenarios according to claim 1, characterized in that: In step S2, the motion state update principle is as follows: Current static state conditions: 1) If status t-1 =0 and does not satisfy v t-2 <=v t-1 <=v t v t ≥v motion (v motion This is the minimum speed that the vehicle must increase to to transition from stationary to moving (default value is 1, unit: m / s). If the target is currently stationary, then its status is unchanged. t =0; 2) If status t-1 =1 and satisfy v t-2 >=v t-1 >=v t v t ≤v stop (v stop The maximum speed at which the vehicle should decrease from motion to rest (default value is 2, unit: m / s) indicates that the target is currently stationary, i.e., its status is... t =0; Current motion state conditions: 1) If status t-1 =0 and satisfy v t-2 <=v t-1 <=v t v t ≥v motion If the target is currently in motion, then its status is... t =1; 2) If status t-1 =1 and does not satisfy v t-2 >=v t-1 >=v t v t ≤v stop If the target is still in motion at the current moment, then its status is... t =1.

3. The method for eliminating abnormal heading angles of turning vehicles caused by positioning deviations in intersection scenarios according to claim 1, characterized in that: For vehicles in set B with a frame number less than N1, update the vehicle motion state according to the following principle: Let the current time be t, and the two component velocities in the UTM coordinate system output by Kalman tracking be v. x v y Then the speed of the vehicle at the current moment If v t ≥v go v go The default value is 1, in m / s, which means the target is currently in motion; otherwise, the target is stationary.

Images