Map-free vehicle real-time track processing method and device and storage medium

Abstract

The invention discloses a map-free vehicle real-time track processing method. The method comprises the following steps that vehicle track data are obtained through a sensor, and the size of a sliding window for track processing is defined; obtaining track segment data through a sliding window; voting track course angles in the sliding window, and taking a mean value of course angles of all track points in an angle set with the most voting times as a reference course angle for deleting abnormal points; abnormal track point detection is carried out on the point set in the current track segment, and abnormal track points are deleted; and judging the motion state of the vehicle according to the remaining track points meeting the conditions, and smoothing the track of the vehicle according to the motion state of the vehicle. The method does not need to depend on a map and other prior information, the steps of course angle voting, abnormal point deletion and the like are executed in a sliding window mode, the calculation amount is greatly reduced, the vehicle track can be smoothed online in real time, and the smoothness and authenticity of the track are guaranteed.

Description

Technical Field

[0002] The present invention relates to a method, device, and storage medium for real-time vehicle trajectory processing without a map. Background Art

[0003] Vehicle trajectory smoothing has important application significance in the fields of intelligent transportation cameras and autonomous driving. In an intelligent transportation system, by smoothing the vehicle trajectory, the accuracy and real-time performance of traffic monitoring can be improved, and the path fluctuations caused by sensor errors or external interferences can be reduced. This enables intelligent transportation cameras to more stably identify and track vehicle movements, thereby optimizing functions such as traffic flow analysis, traffic violation monitoring, and accident warning. In the field of autonomous driving, trajectory smoothing technology ensures that vehicles can drive smoothly in complex road conditions and dynamic environments, avoiding unnatural driving behaviors such as sharp turns and sudden brakes, and enhancing the riding comfort and driving safety. By combining path planning with trajectory smoothing, the autonomous driving system can more accurately predict and control vehicle movements, smoothly handle interactions with other traffic participants, and further promote the popularization and application of autonomous driving technology. Therefore, trajectory smoothing not only improves the monitoring accuracy of intelligent transportation cameras but also provides technical support for achieving a safe, smooth, and comfortable driving experience in autonomous driving.

[0004] In existing technical solutions, Kalman filtering is widely applied to trajectory smoothing. The motion state of the vehicle is predicted through a model, and trajectory prediction is performed based on vehicle dynamics models (such as uniform linear motion, uniformly accelerated motion, etc.), and these predictions are fused with actual sensor data to generate a smooth trajectory. However, when smoothing the current point using the Kalman filtering trajectory smoothing method, only the state of the previous trajectory point of the current point is referred to, and the previous trajectory points of the vehicle are not referred to, and abnormal trajectory points are not separately processed. If there are abnormal fluctuation points in the trajectory, Kalman filtering cannot completely filter out these abnormal fluctuation points, but only reduces the amplitude of the fluctuation. There are also some solutions that will refer to a high-precision map or road network model when smoothing the vehicle trajectory, and constrain the direction of the vehicle trajectory according to the curvature of the road. However, these solutions must rely on map prior information, and additional costs are required to obtain high-precision maps and road network models, and the map may often change. For example, dynamic factors such as tidal lanes in the map or road redrawing will affect the trajectory smoothing process.

[0005] The patent with the publication number CN117775005A discloses a method and device for filtering and smoothing vehicle trajectories. It acquires multiple frames of vehicle trajectory data, extracts the multi-dimensional feature matrix of the trajectory signal using a preset filter bank, performs differential calculation on the multi-dimensional feature matrix to obtain the corresponding feature difference matrix. Then, it determines the multi-order feature differences corresponding to any frame of trajectory data in the multiple frames of trajectory data based on the feature difference matrix, and respectively determines whether the multi-order feature differences are greater than the corresponding smoothing thresholds. If they are greater than the corresponding smoothing thresholds, it determines that the trajectory data of this frame is abnormal, and then performs trajectory correction. This proposal uses Gaussian functions, first-order Gaussian derivatives, and second-order Gaussian derivatives as sliding filters to extract the features of the trajectory to form a feature difference matrix, and uses the mean of the feature differences multiplied by a coefficient as the smoothing threshold for filtering. If there are multiple frames of abnormal trajectory data in the sliding window, the extracted feature difference matrix cannot be used as a benchmark for removing abnormal trajectory data, and the abnormal trajectory data will have a certain impact on the smoothing threshold calculated by the feature difference matrix.

[0006] The patent with the publication number CN117930175A discloses a method and system for smoothing and simulating vehicle operation trajectories based on radar data. By obtaining the center line of the vehicle trajectory operation section and the vehicle operation trajectory data point set, removing the trajectory points with missing fields from the vehicle operation trajectory data point set to generate the first point set; removing the duplicate trajectory points from the first point set to generate the second point set; denoising the second point set to generate the third point set; performing smoothing processing on the third point set according to the Kalman filter to generate the fourth point set; correcting the fourth point set according to the center line of the vehicle trajectory operation section to generate the fifth point set; generating a drawn trajectory according to the fifth point set; performing manual correction on the drawn trajectory to generate the sixth point set, and screening the sixth point set according to the GIS data of the road to generate smooth trajectory points. This proposal processes the denoised point set according to the Kalman filter method. The Kalman filter is a recursive algorithm that only depends on the state estimates of the current and previous moments for update and cannot directly utilize the historical information of the global trajectory. In long-term trajectory smoothing, it may ignore some global information (such as historical path patterns), resulting in inconsistencies in long-term trajectory estimation.

[0007] The patent with the publication number CN118439041A discloses a trajectory smoothing method, product, storage medium, and electronic device. The driving trajectory to be smoothed is obtained. First, the burr segments in the driving trajectory are determined, and after processing the burr segments into smooth segments, a preliminary smoothed trajectory is obtained. Then, the vehicle kinematic constraint conditions are retrieved, and the trajectory points in the preliminary smoothed trajectory that do not meet the vehicle kinematic constraint conditions are processed to obtain a smoothed trajectory that meets the vehicle kinematic constraint conditions. This proposal establishes a Frenet coordinate system with the lane centerline as the reference, and constructs a loss function based on loss conditions such as the distance deviation value between the trajectory points and the lane centerline, the trajectory curvature, and the trajectory curvature change rate to smooth the trajectory. This proposal requires prior knowledge of the road centerline information in advance and cannot perform trajectory smoothing processing without a map. Summary of the Invention

[0008] The main objective of the present invention is to provide a mapless vehicle real-time trajectory processing method, device, and storage medium, aiming to solve the above technical problems.

[0009] To achieve the above objective, the present invention provides a mapless vehicle real-time trajectory processing method.

[0010] The mapless vehicle real-time trajectory processing method includes the following steps: Obtain vehicle trajectory data through sensors and define the sliding window size for trajectory processing; Obtain trajectory segment data through the sliding window; Vote on the trajectory heading angles in the sliding window, and take the average of all the trajectory point heading angles in the angle set with the most voting times as the reference heading angle for deleting abnormal points; Detect abnormal trajectory points in the point set of the current trajectory segment and delete the abnormal trajectory points; Judge the motion state of the vehicle according to the remaining eligible trajectory points, and smooth the vehicle's trajectory according to the motion state of the vehicle.

[0011] In one embodiment, the step of obtaining vehicle trajectory data through sensors and defining the sliding window size for trajectory processing includes: After obtaining the vehicle's position and heading angle through sensors and defining the sliding window size for trajectory processing, use the vehicle's current position as the right endpoint of the sliding window.

[0012] In one embodiment, the step of voting on the trajectory heading angles in the sliding window, and taking the average of all the trajectory point heading angles in the angle set with the most voting times as the reference heading angle for deleting abnormal points includes: Vote on the track heading angle in the sliding window, divide the angle at a preset angular resolution, count the number of track points in each angle set, and take the mean of the track point heading angles in the angle set with the most votes as the reference heading angle for abnormal point deletion.

[0013] In one embodiment, the step of detecting abnormal track points in the point set of the current track segment and deleting the abnormal track points includes: Detect abnormal track points in the point set of the current track segment, and eliminate abnormal track points by comparing whether the angular change between current track points conforms to the kinematic model of the vehicle and whether it is consistent with the reference heading angle.

[0014] In one embodiment, the step of detecting abnormal track points in the point set of the current track segment and deleting the abnormal track points further includes: For each point in the track segment, calculate its incoming angle and outgoing angle. The incoming angle is defined as the connection angle between the current track point and the previous track, and the outgoing angle is defined as the connection angle between the current track point and the next track point; If the difference between one of the incoming angle and the outgoing angle and the reference angle calculated by voting is greater than the preset threshold, then determine that the current track point is an abnormal track point; If the difference between the incoming angle and the outgoing angle is greater than the preset threshold, it means that the point has an abnormal turn, and it is also determined as an abnormal track point.

[0015] In one embodiment, the step of judging the motion state of the vehicle according to the remaining eligible track points and smoothing the vehicle's track according to the motion state of the vehicle includes: Judge whether the vehicle is in a stationary state; If the vehicle is in a stationary state, the smoothed position of the vehicle is the position of the previous track point; If the vehicle is in a moving state, then fit the remaining eligible track points based on the least squares method to obtain the reference direction of the vehicle's forward movement; Calculate the average driving speed of the vehicle in the current track segment through the length of the current track segment and the time difference between the first and last points, and obtain the current smoothed position of the vehicle according to the driving speed and the reference direction.

[0016] In addition, to achieve the above object, the present invention also provides a mapless vehicle real-time trajectory processing method. The mapless vehicle real-time trajectory processing method includes: a memory, a processor, and a mapless vehicle real-time trajectory processing program stored on the memory and executable on the processor. When the mapless vehicle real-time trajectory processing program is executed by the processor, the steps of the above-mentioned mapless vehicle real-time trajectory processing method are implemented.

[0017] In addition, to achieve the above object, the present invention further provides a computer-readable storage medium, on which a vehicle real-time trajectory processing program without a map is stored. When the vehicle real-time trajectory processing program without a map is executed by a processor, the steps of the data storage method described above are implemented. Beneficial effects that the present invention can achieve: A vehicle real-time trajectory processing method without a map proposed in an embodiment of the present invention obtains vehicle trajectory data through a sensor and defines the size of a sliding window for trajectory processing; obtains trajectory segment data through the sliding window; votes on the trajectory heading angles in the sliding window, and takes the mean value of the heading angles of all trajectory points in the angle set with the most voting times as the reference heading angle for abnormal point deletion; detects abnormal trajectory points in the point set of the current trajectory segment and deletes the abnormal trajectory points; determines the motion state of the vehicle based on the remaining qualified trajectory points, and smooths the vehicle trajectory according to the motion state of the vehicle.

[0018] The present invention does not need to rely on a map and other prior information. By performing steps such as heading angle voting and abnormal point deletion in a sliding window manner, the calculation amount is greatly reduced, and the vehicle trajectory can be smoothed in real time online, ensuring the smoothness and authenticity of the trajectory. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 is a schematic structural diagram of a device in a hardware operating environment related to the solution of an embodiment of the present invention; Figure 2 is a schematic flowchart of an embodiment of the vehicle real-time trajectory processing method without a map of the present invention; Figure 3 is a reference schematic diagram for trajectory heading angle voting of the present invention; Figure 4 is a reference schematic diagram for the process of deleting abnormal trajectory points of the present invention; Figure 5 is a reference schematic diagram for trajectory smoothing of the present invention; Figure 6 is a reference schematic diagram for trajectory deviation correction of the present invention.

[0020] The realization, functional features and advantages of the object of the present invention will be further described with reference to the embodiments and the accompanying drawings. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0021] 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.

[0022] As Figure 1 shown, Figure 1 is a schematic structural diagram of a terminal in a hardware operating environment related to the solution of an embodiment of the present invention.

[0023] The terminal in the embodiments of the present invention may be a PC, or a mobile terminal device with a display function such as a smart phone, a tablet computer, an e-book reader, an MP3 (Moving Picture Experts Group Audio Layer III) player, an MP4 (Moving Picture Experts Group Audio Layer IV) player, a portable computer, etc.

[0024] As Figure 1 shown, the terminal may include: a processor 1001, such as a CPU, a communication bus 1002, a user interface 1003, a network interface 1004, and a memory 1005. Among them, the communication bus 1002 is used to implement connection communication between these components. The user interface 1003 may include a display screen (Display) and an input unit such as a keyboard (Keyboard). Optionally, the user interface 1003 may further include a standard wired interface and a wireless interface. The network interface 1004 may optionally include a standard wired interface and a wireless interface (such as a WI-FI interface). The memory 1005 may be a high-speed RAM memory or a stable memory (non-volatile memory), such as a disk memory. Optionally, the memory 1005 may also be a storage device independent of the aforementioned processor 1001.

[0025] Optionally, the terminal may further include a camera, an RF (Radio Frequency) circuit, sensors, an audio circuit, a WiFi module, etc. Among them, the sensors include, for example, a light sensor, a motion sensor, and other sensors. Specifically, the light sensor may include an ambient light sensor and a proximity sensor. Among them, the ambient light sensor can adjust the brightness of the display screen according to the brightness of the ambient light, and the proximity sensor can turn off the display screen and / or the backlight when the mobile terminal is moved to the ear. As a kind of motion sensor, the gravity acceleration sensor can detect the magnitude of acceleration in all directions (generally three axes), and can detect the magnitude and direction of gravity when stationary, and can be used for applications that identify the posture of the mobile terminal (such as horizontal and vertical screen switching, related games, magnetometer posture calibration), vibration recognition related functions (such as a pedometer, tapping), etc.; of course, the mobile terminal may also be configured with other sensors such as a gyroscope, a barometer, a hygrometer, a thermometer, an infrared sensor, etc., which will not be elaborated here.

[0026] Those skilled in the art can understand that Figure 1 the terminal structure shown in

[0027] As Figure 1 shown, in the memory 1005 serving as a computer storage medium, there may be included an operating system, a network communication module, a user interface module, and a mapless vehicle real-time trajectory processing program.

[0028] In Figure 1 the terminal shown, the network interface 1004 is mainly used to connect to the background server and communicate with the background server for data; the user interface 1003 is mainly used to connect to the client (user side) and communicate with the client for data; and the processor 1001 can be used to call the mapless vehicle real-time trajectory processing program stored in the memory 1005 and perform the following operations: Obtain vehicle trajectory data through sensors and define the sliding window size for trajectory processing; Obtain trajectory segment data through the sliding window; Vote on the trajectory heading angles in the sliding window, and take the mean of the heading angles of all trajectory points in the angle set with the most votes as the reference heading angle for abnormal point deletion; Detect abnormal trajectory points in the point set of the current trajectory segment and delete the abnormal trajectory points; Judge the motion state of the vehicle according to the remaining qualified trajectory points, and smooth the vehicle trajectory according to the motion state of the vehicle.

[0029] Further, the processor 1001 can call the mapless vehicle real-time trajectory processing program stored in the memory 1005 and also perform the following operations: The step of obtaining vehicle trajectory data through sensors and defining the sliding window size for trajectory processing includes: After obtaining the vehicle position and heading angle through sensors and defining the sliding window size for trajectory processing, use the current vehicle position as the right endpoint of the sliding window.

[0030] Further, the processor 1001 can call the mapless vehicle real-time trajectory processing program stored in the memory 1005 and also perform the following operations: The step of voting on the trajectory heading angles in the sliding window and taking the mean of the heading angles of all trajectory points in the angle set with the most votes as the reference heading angle for abnormal point deletion includes: Vote on the trajectory heading angles in the sliding window, divide the angles with a preset angle resolution, count the number of trajectory points in each angle set, and take the mean of the heading angles of all trajectory points in the angle set with the most votes as the reference heading angle for abnormal point deletion.

[0031] Further, the processor 1001 may call the mapless vehicle real-time trajectory processing program stored in the memory 1005 and further perform the following operations: The step of detecting abnormal trajectory points in the point set of the current trajectory segment and deleting the abnormal trajectory points includes: Detect abnormal trajectory points in the point set of the current trajectory segment, and eliminate abnormal trajectory points by comparing whether the angular change between the current trajectory points conforms to the kinematic model of the vehicle and whether it is consistent with the reference heading angle.

[0032] Further, the processor 1001 may call the mapless vehicle real-time trajectory processing program stored in the memory 1005 and further perform the following operations: The step of detecting abnormal trajectory points in the point set of the current trajectory segment and deleting the abnormal trajectory points further includes: For each point in the trajectory segment, calculate its incoming angle and outgoing angle. The incoming angle is defined as the connecting line angle between the current trajectory point and the previous trajectory, and the outgoing angle is defined as the connecting line angle between the current trajectory point and the next trajectory point; If the difference between one of the incoming angle and the outgoing angle and the reference angle calculated by voting is greater than a preset threshold, it is determined that the current trajectory point is an abnormal trajectory point; If the difference between the incoming angle and the outgoing angle is greater than the preset threshold, it means that the point has an abnormal turn and is also determined as an abnormal trajectory point.

[0033] Further, the processor 1001 may call the mapless vehicle real-time trajectory processing program stored in the memory 1005 and further perform the following operations: The step of determining the motion state of the vehicle according to the remaining qualified trajectory points and smoothing the vehicle trajectory according to the motion state of the vehicle includes: Determine whether the vehicle is in a stationary state; If the vehicle is in a stationary state, the smoothed position of the vehicle is the position of the previous trajectory point; If the vehicle is in a moving state, perform fitting on the remaining qualified trajectory points based on the least squares method to obtain the reference direction of the vehicle's forward movement; Calculate the average driving speed of the vehicle in the current trajectory segment through the length of the current trajectory segment and the time difference between the first and last points, and obtain the current smoothed position of the vehicle according to the driving speed and the reference direction.

[0034] The specific embodiments of the mapless vehicle real-time trajectory processing device of the present invention are basically the same as those of the following data storage method embodiments and will not be elaborated here.

[0035] Refer to Figure 2, a first embodiment of the present invention provides a method for processing real-time vehicle trajectories without a map. The method for processing real-time vehicle trajectories without a map includes: Obtain vehicle trajectory data through a sensor and define the size of a sliding window for trajectory processing; Obtain trajectory segment data through the sliding window; Vote on the trajectory heading angles in the sliding window, and take the mean of the heading angles of all trajectory points in the angle set with the most voting times as the reference heading angle for deleting abnormal points; Detect abnormal trajectory points in the point set of the current trajectory segment and delete the abnormal trajectory points; Judge the motion state of the vehicle according to the remaining qualified trajectory points, and smooth the vehicle trajectory according to the motion state of the vehicle.

[0036] In this embodiment, first, vehicle trajectory data (position and heading angle) is obtained through a sensor, and the size of a sliding window for trajectory processing is defined. Trajectory smoothing is performed with the current position of the vehicle as the right endpoint of the sliding window. To avoid the influence of abnormal heading angles in the trajectory on trajectory smoothing, vote on the trajectory heading angles in the sliding window, divide the angles with a preset angular resolution, count the number of trajectory points in each angle set, and take the mean of the heading angles of all trajectory points in the angle set with the most voting times as the reference heading angle for deleting abnormal points. Then, detect abnormal trajectory points in the point set of the current trajectory segment, and eliminate abnormal trajectory points by comparing whether the angular change between the current trajectory points conforms to the kinematic model of the vehicle and whether it is consistent with the reference heading angle.

[0037] Judge whether the vehicle is stationary according to the remaining qualified trajectory points. If it is judged that the vehicle is in a stationary state, the position of the vehicle after smoothing is equal to the position of the previous trajectory point. If the vehicle is moving, the remaining qualified trajectory points are fitted based on the least squares method to obtain the reference direction of vehicle advancement.

[0038] Calculate the average driving speed of the vehicle in the current trajectory segment through the length of the current trajectory segment and the time difference between the first and last points. The current smoothed position of the vehicle can be calculated according to the driving speed and the reference direction. Since the fitting is performed with reference to the historical trajectory of the vehicle, the smoothness of the connection between the calculated position and the historical trajectory of the vehicle can be ensured. To avoid a large deviation between the smoothed trajectory and the actual trajectory of the vehicle, it is necessary to correct the smoothed trajectory points at regular intervals. In this application, the quality of the trajectory in the sliding window is evaluated. If the trajectory quality score in the current window is high, a suitable position is selected on the line connecting the smoothed trajectory point and the real trajectory point as the new smoothed trajectory point, so that the smoothed position is closer to the real trajectory and a large deviation from the real trajectory is avoided.

[0039] Further, please refer to Figure 3 and smooth the trajectory in a sliding window manner. Define the range size of the window in advance. Exemplarily, define the window size as 10 trajectory points, that is, taking the current position as the right endpoint and fetching the previous 10 trajectory points as the current trajectory segment to calculate the position of the smoothed current trajectory point. If the number of trajectory points is less than the window size, no smoothing is performed.

[0040] The trajectory heading angle voting step is to avoid the influence of abnormal trajectory heading angles in the current trajectory segment on the trajectory smoothing process. In this application, the reference heading angle during the smoothing process of the current trajectory segment is obtained by voting. The value range of the vehicle's heading angle is from 0 to 360 degrees (excluding 360), and the angle set is divided according to the pre-set angle resolution. The angle resolution is set to 30 degrees, and 12 angle sets can be divided. Count which angle set each trajectory point's heading angle in the current trajectory segment falls into, take the angle set with the largest number of votes, calculate the average value of all angle values in the angle set, and regard it as the reference angle of the trajectory segment for deleting abnormal trajectory points. If the number of points in multiple angle sets is the same, calculate the average value of all trajectory heading angles of these several angle sets.

[0041] Please refer to Figure 3 and if the number of trajectory points in the 60° to 90° set is the largest, then take all the trajectory point heading angle data of this angle set and calculate its average value.

[0042] Please refer to Figure 4 and for each point in the trajectory segment, calculate its incoming angle and outgoing angle. The incoming angle is defined as the connection angle between the current trajectory point and the previous trajectory, and the outgoing angle is defined as the connection angle between the current trajectory point and the next trajectory point. In particular, for the left endpoint and the right endpoint in the trajectory segment, the left endpoint only has an outgoing angle, and the right endpoint only has an incoming angle.

[0043] If the difference between one of the incoming angle and the outgoing angle and the reference angle calculated by voting is greater than the preset threshold, then it is determined that the current trajectory point is an abnormal trajectory point; If the difference between the incoming angle and the outgoing angle is greater than the preset threshold (this preset threshold can be set as the maximum steering angle of the vehicle), it means that the point has an abnormal turn, and it is also determined as an abnormal trajectory point.

[0044] Further, please refer to Figure 5, the vehicle stationary determination is based on the average speed of the vehicle in the current trajectory segment and the number of remaining qualified trajectory points to determine whether the vehicle is stationary. First, calculate the number of remaining trajectory points in the trajectory segment after deleting the abnormal points. When the vehicle is stationary, due to the noise of the sensor or the jitter of the detection frame, the trajectory points in the trajectory segment cannot completely coincide. The connection angles between its adjacent two points are mostly abnormal angles and will be eliminated, and the number of remaining trajectory points is small. Secondly, calculate the average speed of the trajectory according to the distance and time difference between the trajectory points at the left and right ends of the trajectory segment. If the number of remaining trajectory points is less than a certain threshold and the average speed of the vehicle is less than the minimum driving speed of the vehicle, it is determined that the vehicle at the current position is in a stationary state, and the position of the trajectory point at the previous moment is used as the position of the smoothed trajectory point at the current moment. If these two conditions are not met, trajectory fitting is performed to calculate the position of the smoothed trajectory point.

[0045] The trajectory fitting calculates the driving direction of the vehicle by fitting the data of the remaining qualified trajectory points in the current trajectory segment. In this application, the simplest least squares method is used to fit the linear equation of the vehicle's y coordinate with respect to the x coordinate and the linear equation of the x coordinate with respect to the y coordinate respectively. Then calculate their fitting errors respectively, and take the linear equation with the smaller fitting error to calculate the slope of the trajectory segment, and then the driving direction of the vehicle can be calculated. Among them, the calculation relationship between the angle β and the slope k of the trajectory segment is: β = arctan(k).

[0046] Based on the position and driving direction at the previous moment, calculate the current smoothed trajectory position according to the average speed of the vehicle in the current trajectory segment, as Figure 5 shown. The green pentagram represents the smoothed position, the red pentagram represents the current smoothed position, and the black origin represents the position of the original trajectory point In addition, this application can also prevent a large deviation between the smoothed trajectory and the original vehicle trajectory through trajectory deviation correction. When the quality of the original trajectory is good, correct the smoothed trajectory points to make them close to the original trajectory data. If the original trajectory points at the current moment are not deleted, the trajectory deviation correction judgment can be started.

[0047] The trajectory quality evaluation is mainly determined according to the number of remaining trajectory points, the trajectory fitting error, and the trajectory turning angle. The more the number of remaining trajectory points, the fewer the number of abnormal trajectory points and the better the quality. The trajectory fitting error is defined as the sum of the distances from each trajectory point in the trajectory segment to the fitting line, and the trajectory turning angle is defined as the difference between the incoming angle and the outgoing angle of each trajectory point in the trajectory segment.

[0048] The incoming angle and the outgoing angle at this time are recalculated for each trajectory point after deleting the abnormal points. If all three conditions are met, it is proved that the quality of the current trajectory segment is high, and a new point is selected on the line connecting the smoothed trajectory point and the original trajectory point as the new smoothed trajectory point.

[0049] The selection of the new smooth point also needs to satisfy the kinematic model of the vehicle. Taking the previous trajectory position as the center of the circle, multiplying the current average driving speed of the vehicle by a preset proportionality coefficient as the current maximum driving speed of the vehicle, calculating the product of the maximum driving speed of the vehicle and the time difference between two points as the maximum driving distance, taking the maximum driving distance as the radius, calculating the intersection point with the connection line as the new smoothed trajectory point, and the included angle between the connection line of the new smoothed trajectory point and the center of the circle does not exceed the maximum vehicle steering angle. Otherwise, reduce the proportionality coefficient and recalculate until the kinematic model constraint of the vehicle is satisfied. The schematic diagram of trajectory deviation correction is as Figure 6 shown, the green pentagram represents the smoothed position, and the red pentagram represents the smoothed position after deviation correction.

[0050] The present invention proposes a method for real-time trajectory smoothing of a vehicle without a map, which does not rely on map prior information, and only fits the trajectory by the least squares method to achieve the effect of trajectory smoothing, with small computational complexity and capable of performing trajectory smoothing in real time.

[0051] In this application, the trajectory is segmented by a sliding window to smooth the current trajectory point. First, the reference heading angle of the vehicle is determined by voting to avoid affecting the trajectory smoothing process due to inaccurate heading detection. Then, abnormal points are deleted, and abnormal trajectory points are removed according to the reference heading angle and vehicle kinematic constraints to avoid affecting the trajectory smoothing process due to sudden abnormal trajectory points. After deleting the abnormal points, the motion state of the vehicle is judged according to the remaining qualified trajectory points. If it is stationary, the previous trajectory point is copied, which can avoid generating an uneven trajectory due to sensor detection noise or fluctuations in image frame detection when the vehicle is stationary. If it is judged that the vehicle is moving, the current trajectory segment is fitted by the least squares method. Since the historical trajectory information of the vehicle is referred to and the smoothing is performed in a sliding window manner, which is equivalent to only changing the data of the left and right two endpoints, the smoothness of the trajectory can be guaranteed.

[0052] While smoothing the trajectory, the present invention also corrects the position of the smoothed trajectory. The original trajectory is evaluated according to the number of remaining trajectory points, the trajectory fitting error, and the trajectory steering angle. When the trajectory quality is good, taking the previous trajectory position as the center of the circle and the maximum driving distance as the radius, and being constrained by the maximum vehicle steering angle, a new smoothed trajectory position is obtained, making the smoothed position closer to the original trajectory and ensuring the authenticity of the trajectory.

[0053] Before trajectory fitting, the present invention divides the angle range through the heading angle voting method to vote and obtain an accurate reference heading angle, avoiding the influence of abnormal heading angles of trajectory points on the trajectory smoothing process. Then, according to the reference heading angle and vehicle kinematic constraints, abnormal trajectory points are removed to avoid the influence of abnormal trajectory positions on the trajectory smoothing process. After removing the abnormal point data, trajectory fitting is performed, which can improve the accuracy of trajectory fitting. In addition, an embodiment of the present invention further provides a computer-readable storage medium, on which a program for processing the real-time trajectory of a vehicle without a map is stored. When the program for processing the real-time trajectory of a vehicle without a map is executed by a processor, the following operations are implemented: Obtain vehicle trajectory data through sensors and define the sliding window size for trajectory processing; Obtain trajectory segment data through the sliding window; Vote on the trajectory heading angles in the sliding window, and take the mean of all the trajectory point heading angles in the angle set with the most votes as the reference heading angle for deleting abnormal points; Detect abnormal trajectory points in the point set of the current trajectory segment and delete the abnormal trajectory points; Judge the motion state of the vehicle according to the remaining qualified trajectory points, and smooth the vehicle's trajectory according to the motion state of the vehicle.

[0054] Further, when the program for processing the real-time trajectory of a vehicle without a map is executed by a processor, the following operations are also implemented: The step of obtaining vehicle trajectory data through sensors and defining the sliding window size for trajectory processing includes: After obtaining the vehicle's position and heading angle through sensors and defining the sliding window size for trajectory processing, use the vehicle's current position as the right endpoint of the sliding window.

[0055] Further, when the program for processing the real-time trajectory of a vehicle without a map is executed by a processor, the following operations are also implemented: The step of voting on the trajectory heading angles in the sliding window and taking the mean of all the trajectory point heading angles in the angle set with the most votes as the reference heading angle for deleting abnormal points includes: Vote on the trajectory heading angles in the sliding window, divide the angles with a preset angle resolution, count the number of trajectory points in each angle set, and take the mean of all the trajectory point heading angles in the angle set with the most votes as the reference heading angle for deleting abnormal points.

[0056] Further, when the program for processing the real-time trajectory of a vehicle without a map is executed by a processor, the following operations are also implemented: The step of detecting abnormal trajectory points in the point set of the current trajectory segment and deleting the abnormal trajectory points includes: Detect abnormal trajectory points in the point set of the current trajectory segment, and eliminate abnormal trajectory points by comparing whether the angular change between current trajectory points conforms to the kinematic model of the vehicle and whether it is consistent with the reference heading angle.

[0057] Furthermore, when the mapless vehicle real-time trajectory processing program is executed by a processor, the following operations are also implemented: The step of detecting abnormal trajectory points in the point set of the current trajectory segment and deleting the abnormal trajectory points further includes: For each point in the trajectory segment, calculate its incoming angle and outgoing angle. The incoming angle is defined as the connection angle between the current trajectory point and the previous trajectory, and the outgoing angle is defined as the connection angle between the current trajectory point and the next trajectory point; If the difference between one of the incoming angle and the outgoing angle and the reference angle calculated by voting is greater than a preset threshold, then determine that the current trajectory point is an abnormal trajectory point; If the difference between the incoming angle and the outgoing angle is greater than the preset threshold, it indicates that the point has an abnormal turn, and it is also determined as an abnormal trajectory point.

[0058] Furthermore, when the mapless vehicle real-time trajectory processing program is executed by a processor, the following operations are also implemented: The step of judging the motion state of the vehicle according to the remaining qualified trajectory points and smoothing the vehicle trajectory according to the motion state of the vehicle includes: Judge whether the vehicle is in a stationary state; If the vehicle is in a stationary state, the smoothed position of the vehicle is the position of the previous trajectory point; If the vehicle is in a moving state, fit the remaining qualified trajectory points based on the least squares method to obtain the reference direction of the vehicle's forward movement; Calculate the average driving speed of the vehicle in the current trajectory segment through the length of the current trajectory segment and the time difference between the first and last points, and obtain the current smoothed position of the vehicle according to the driving speed and the reference direction.

[0059] The specific embodiments of the computer-readable storage medium of the present invention are basically the same as those of the above-described embodiments of the mapless vehicle real-time trajectory processing method, and will not be elaborated herein.

[0060] It should be noted that in this text, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, such that a process, method, article or system comprising a series of elements not only includes those elements but also includes other elements not expressly listed, or elements inherent to such process, method, article or system. Without further limitation, an element defined by the phrase "comprising a..." does not exclude the presence of additional identical elements in the process, method, article or system comprising that element.

[0061] The serial numbers of the above embodiments of the present invention are for description only and do not represent the superiority or inferiority of the embodiments.

[0062] Through the description of the above embodiments, those skilled in the art can clearly understand that the above embodiment methods can be implemented by means of software plus a necessary general hardware platform. Of course, they can also be implemented by hardware, but in many cases the former is a better implementation method. Based on such an understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art can be embodied in the form of a software product. This computer software product is stored in a storage medium as described above (such as ROM / RAM, magnetic disk, optical disc), and includes several instructions to enable a terminal device (which can be a mobile phone, computer, server, air conditioner, or network device, etc.) to execute the methods described in various embodiments of the present invention.

[0063] The above are only the preferred embodiments of the present invention, and do not limit the patent scope of the present invention accordingly. Any equivalent structural or equivalent process transformation made by using the specification and drawings of the present invention, or directly or indirectly applied in other related technical fields, shall be equally included in the patent protection scope of the present invention.

Claims

1. A method for processing real-time vehicle trajectories without a map, characterized in that, The method for processing the real-time trajectory of a vehicle without a map includes the following steps: Obtain vehicle trajectory data through sensors and define the sliding window size for trajectory processing; Obtain trajectory segment data through the sliding window; Vote on the trajectory heading angles in the sliding window, and take the mean of the heading angles of all the trajectory points in the angle set with the most votes as the reference heading angle for deleting abnormal points; Detect abnormal trajectory points in the point set of the current trajectory segment and delete the abnormal trajectory points; Judge the motion state of the vehicle according to the remaining qualified trajectory points, and smooth the vehicle trajectory according to the motion state of the vehicle.

2. The method for processing real-time vehicle trajectories without maps according to claim 1, wherein, The step of obtaining vehicle trajectory data through sensors and defining the sliding window size for trajectory processing includes: After obtaining the position and heading angle of the vehicle through sensors and defining the sliding window size for trajectory processing, use the current position of the vehicle as the right endpoint of the sliding window.

3. The method for processing real-time vehicle trajectories without a map according to claim 1, wherein, The step of voting on the trajectory heading angles in the sliding window and taking the mean of the heading angles of all the trajectory points in the angle set with the most votes as the reference heading angle for deleting abnormal points includes: Vote on the trajectory heading angles in the sliding window, divide the angles with a preset angle resolution, count the number of trajectory points in each angle set, and take the mean of the heading angles of all the trajectory points in the angle set with the most votes as the reference heading angle for deleting abnormal points.

4. The method for processing real-time vehicle trajectories without a map according to claim 1, characterized in that, The step of detecting abnormal trajectory points in the point set of the current trajectory segment and deleting the abnormal trajectory points includes: Detect abnormal trajectory points in the point set of the current trajectory segment, and eliminate abnormal trajectory points by comparing whether the angle change between the current trajectory points conforms to the kinematic model of the vehicle and whether it is consistent with the reference heading angle.

5. The method for processing real-time vehicle trajectories without a map according to claim 4, wherein, The step of detecting abnormal trajectory points in the point set of the current trajectory segment and deleting the abnormal trajectory points further includes: For each point in the trajectory segment, calculate its incoming angle and outgoing angle. The incoming angle is defined as the connecting angle between the current trajectory point and the previous trajectory, and the outgoing angle is defined as the connecting angle between the current trajectory point and the next trajectory point; If the difference between one of the incoming angle and the outgoing angle and the reference angle calculated by voting is greater than the preset threshold, judge that the current trajectory point is an abnormal trajectory point; If the difference between the incoming angle and the outgoing angle is greater than the preset threshold, it means that the point has an abnormal turn, and it is also judged as an abnormal trajectory point.

6. The method for processing real-time vehicle trajectories without a map according to claim 1, characterized in that, The step of judging the motion state of the vehicle according to the remaining qualified trajectory points and smoothing the vehicle trajectory according to the motion state of the vehicle includes: Judge whether the vehicle is in a stationary state; If the vehicle is in a stationary state, the smoothed position of the vehicle is the position of the previous trajectory point; If the vehicle is in a moving state, fit the remaining qualified trajectory points based on the least squares method to obtain the reference direction of the vehicle's forward movement; Calculate the average driving speed of the vehicle in the current trajectory segment through the length of the current trajectory segment and the time difference between the first and last points, and obtain the current smoothed position of the vehicle according to the driving speed and the reference direction.

7. A vehicle real-time trajectory processing device without a map, characterized in that, The mapless vehicle real-time trajectory processing device includes: a memory, a processor, and a mapless vehicle real-time trajectory processing program stored on the memory and executable on the processor. When the mapless vehicle real-time trajectory processing program is executed by the processor, the steps of the mapless vehicle real-time trajectory processing method according to any one of claims 1 to 6 are implemented.

8. A computer-readable storage medium, characterized in that, A mapless vehicle real-time trajectory processing program is stored on the computer-readable storage medium. When the mapless vehicle real-time trajectory processing program is executed by the processor, the steps of the mapless vehicle real-time trajectory processing method according to any one of claims 1 to 6 are implemented.

Images