A holographic vehicle trajectory correction algorithm for road perception device data anomalies

Abstract

This invention discloses a holographic vehicle trajectory correction algorithm for abnormal data from road sensing devices. It overcomes the problem in existing technologies where cluttered millimeter-wave radar point clouds and positional anomalies reduce the realism and reliability of holographic digital road vehicle trajectories. The algorithm includes the following steps: S1: Obtain the position points of each exit lane at the current intersection and number them; S2: Calculate the current target exit lane for each vehicle based on its existing trajectory; S3: Calculate the positional relationship between vehicles at the intersection; S4: Based on the positional relationship between vehicles, sequentially correct the vehicle trajectory based on the target exit lane, thus achieving vehicle trajectory correction. When the original trajectory data from the road sensing device deviates abnormally, the algorithm promptly corrects the vehicle trajectories in the holographic digital road, effectively improving the realism and reliability of holographic digital road vehicle trajectories.

Description

Technical Field

[0001] This invention relates to the field of holographic digital road vehicle trajectory correction technology, and in particular to a holographic vehicle trajectory correction algorithm for abnormal data from road sensing devices. Background Technology

[0002] Intelligent transportation refers to a model that utilizes modern information technology and intelligent equipment to optimize the operation and management of transportation systems, thereby improving traffic efficiency and safety. Holographic digital roads are an important component of intelligent transportation. They perceive real-world road traffic conditions through various sensors (such as millimeter-wave radar and video cameras) and project this real-world road traffic data onto a virtual platform through simulation modeling to construct a realistic real-time road traffic model. In this process, the quality of the traffic trajectory data detected by sensors such as millimeter-wave radar or video cameras directly determines the reliability of the holographic digital road.

[0003] Limited by current road traffic sensing equipment (such as millimeter-wave radar or video cameras), the detection range of these devices often cannot cover the entire intersection. This means that a single device cannot completely capture the trajectory data of a vehicle as it enters and exits the intersection from the approach lane. Furthermore, vehicle trajectories at the edges of the field of view can exhibit significant positional deviations. Millimeter-wave radar trajectory data may experience trajectory positional deviations and cluttered point clouds throughout its entire field of view, while video cameras may experience trajectory deviations and large-angle abnormal deflections at the edges of the field of view. These problems reduce the realism and reliability of vehicle trajectories in holographic digital roads, and may even lead to vehicle collisions due to abnormal trajectory data from millimeter-wave radar or video cameras.

[0004] Patent CN114910912A describes a multi-radar trajectory relay method, device, storage medium, and equipment. It solves the problem that a single sensor device cannot effectively cover the entire intersection area by relaying the trajectory of the same vehicle through multiple devices. However, it does not solve the problem of the impact of cluttered millimeter-wave radar point clouds and abnormal positions on holographic digital roads. Summary of the Invention

[0005] The purpose of this invention is to overcome the problem that the clutter and abnormal position of millimeter-wave radar point clouds in the prior art reduce the authenticity and reliability of vehicle trajectories in holographic digital roads. It provides a holographic vehicle trajectory correction algorithm for abnormal data from road sensing devices. When the original trajectory data of the road sensing device deviates abnormally, the algorithm corrects the vehicle trajectory in the holographic digital road in a timely manner, effectively improving the authenticity and reliability of vehicle trajectories in holographic digital roads.

[0006] To achieve the above objectives, the present invention adopts the following technical solution: a holographic vehicle trajectory correction algorithm for abnormal data from road sensing devices, comprising the following steps:

[0007] S1: Obtain the location points of each exit lane at the current intersection and number the exit lanes;

[0008] S2: Calculate the current target exit lane for each vehicle based on the existing trajectories of each vehicle within the current intersection.

[0009] S3: Calculate the current positional relationships between all vehicles within the intersection;

[0010] S4: Based on the positional relationship between vehicles, the vehicles in the intersection are corrected sequentially according to the target exit lane to achieve vehicle trajectory correction.

[0011] By utilizing the existing trajectories of vehicles within the current intersection, the target exit lane of each vehicle is obtained. Combined with calculations of the positional relationships between vehicles within the intersection, new routes are planned for vehicles whose trajectories have deviated abnormally. This invention's solution for abnormal trajectory deviation is applied to holographic digital roads, effectively improving the reliability and realism of holographic vehicle trajectories. It is also universally applicable to various sensing devices, such as millimeter-wave radar and video cameras.

[0012] Preferably, step S2 further includes:

[0013] S2.1: Obtain the vehicle's smallest original trajectory point P1(lon1,lat1) and its distance from this latest original trajectory point D. dist The nearest original trajectory point P2(lon2,lat2) is more than 1 meter away;

[0014] S2.2: Connect trajectory point P1 with trajectory point P2 as the starting point, and calculate the slope k and intercept b of the straight line where the current target direction ray of the vehicle is located;

[0015] S2.3: Combine the vehicle's current target direction ray with the position points of each exit lane at the current intersection to calculate the vehicle's most likely target exit lane.

[0016] S2.4: Following the steps in S2.1-S2.3, traverse all vehicles in the current intersection and calculate the most likely target exit lane for each vehicle.

[0017] Slope: Intercept: b = lat1 - k * lon1.

[0018] Preferably, step S2.3 further includes:

[0019] S2.3.1: Let the position point of a certain exit lane be Q(lon q , lat q ), and calculate the foot of the perpendicular q(q x , q y ) of the position point Q on the straight line where the ray of the vehicle's target direction lies;

[0020] S2.3.2: Calculate the distance D1 between the foot of the perpendicular q and the latest trajectory point P1, the distance D2 between the foot of the perpendicular q and the previous trajectory point P2, and the distance D between the trajectory point P1 and the trajectory point P2 12 ;

[0021] S2.3.3: If D1 > D2 and D1 > D 12 , then exclude the position point Q in the subsequent steps; otherwise, calculate the distance D Q between the position point Q and the current target direction ray of the vehicle;

[0022] S2.3.4: Traverse and calculate the relationship between the position points of all exit lanes within the current intersection and the target direction ray of the vehicle in sequence, and select the position point with the minimum D Q value from the position points on the target direction ray of the vehicle. The corresponding exit lane is the most likely target exit lane of the vehicle currently.

[0023] If D1 > D2 and D1 > D 12 , it indicates that the position point Q is not on the target direction of the vehicle's trajectory, and the position point Q needs to be excluded; otherwise, it indicates that the position point Q is on the target direction of the vehicle's trajectory.

[0024] Preferably, the step S4 further includes:

[0025] S4.1: For a vehicle within the intersection, calculate the maximum target exit lane number N among other vehicles in the left side of the vehicle, and the target exit lane number of the vehicle itself is M;

[0026] S4.2: Judge the relationship between N and M. If N < M, no correction is required. If N ≥ M, correct the vehicle based on the target exit lane and plan a new travel route.

[0027] If N < M, it indicates that the target paths of other vehicles located on the left side of the vehicle do not cross the target path of the vehicle itself, and continuing to drive along this trajectory will not result in vehicle collisions, that is, the original trajectory data of the current sensor has not deviated abnormally and does not require correction; if N ≥ M, it indicates that the target paths of other vehicles located on the left side of the vehicle cross the target path of the vehicle itself, and continuing to drive along this trajectory will result in vehicle collisions, that is, the original trajectory data of the current sensor has deviated abnormally, so it should be corrected.

[0028] Preferably, the step S4.2 further includes: updating the lane number of the target exit lane of the vehicle to N + 1, connecting the current trajectory point of the vehicle to the position point of the updated target exit lane of the vehicle to obtain a corrected route, and the vehicle advances along the corrected route at the last driving speed before the trajectory abnormally deviates.

[0029] When N ≥ M, first update the lane number of the target exit lane of the vehicle to N + 1 to avoid the crossing of the target paths between vehicles and thus avoid collisions.

[0030] Preferably, the step S3 is further expressed as: for each vehicle in the intersection, respectively, with its current latest original trajectory point P1(lon1, lat1) as the center of the circle and its current target direction angle as the reference angle, detect whether there are other vehicles within a fan-shaped range with a left-side distance threshold of D left meters and an angle threshold of [Angle1, Angle2] and record them.

[0031] Both the distance threshold and the angle threshold can be adjusted according to actual needs.

[0032] Preferably, in the step S4, it successively includes: judging whether there are other vehicles on the left side of all vehicles in the intersection, sorting all vehicles in the intersection from small to large according to the number of other vehicles on the left side of the vehicles, and then sequentially correcting the vehicles in the intersection based on the target exit lane.

[0033] Preferably, the step S1 further includes: obtaining the midpoint of the starting line of each exit lane in the current intersection as the position point of the exit lane, and numbering each exit lane from small to large in the order from the inside to the outside. Numbering the lanes facilitates the subsequent steps of calculation.

[0034] Preferably, in the step S4.2: if N + 1 is greater than the maximum value of the lane numbers of this exit lane, the lane number of the target exit lane of the vehicle is updated to N.

[0035] Therefore, the present invention has the following beneficial effects:

[0036] 1. By utilizing the existing trajectories of each vehicle within the current intersection, the target exit lane of the vehicle is obtained. Combined with the calculation of the positional relationship between each vehicle within the intersection, the trajectory of the vehicle that has deviated abnormally is corrected, and a new travel route is planned. The original trajectory abnormal deviation solution proposed in this invention can be applied to holographic digital roads, which can effectively improve the reliability and authenticity of the trajectory of holographic vehicles, and can be universally used in various sensing devices, such as millimeter-wave radar and video cameras.

[0037] 2. By avoiding complex hardware optimization, the software effectively solves the problem of abnormal data offset of various road traffic sensing devices, thereby reducing hardware costs. Attached Figure Description

[0038] Figure 1 This is a flowchart illustrating the specific operation of the method of the present invention.

[0039] Figure 2 The values ​​of the parameters for the holographic vehicle trajectory correction algorithm in this embodiment are shown.

[0040] Figure 3 This is a schematic diagram of the holographic vehicle target exit lane and the left-side recognition range area. Detailed Implementation

[0041] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments:

[0042] like Figure 1 The illustrated embodiment demonstrates a holographic vehicle trajectory correction algorithm for addressing data anomalies from road sensing devices. The algorithm's operation is as follows: Step 1: Obtain the position points of each exit lane at the current intersection and number them; Step 2: Calculate the target exit lane for each vehicle based on its existing trajectory within the intersection; Step 3: Calculate the positional relationships between vehicles within the intersection; Step 4: Based on these positional relationships, sequentially correct the vehicle trajectory for each vehicle within the intersection according to its target exit lane, thus achieving vehicle trajectory correction. This algorithm can be applied to holographic digital roads, effectively improving the reliability and realism of holographic vehicle trajectories, and can be universally used with various sensing devices, such as millimeter-wave radar and video cameras.

[0043] The technical solution of this application is further illustrated below with a specific example. In this embodiment, the parameter values ​​of the holographic vehicle trajectory correction algorithm are as follows: Figure 2 As shown.

[0044] Step 1: Obtain the location points of each exit lane at the current intersection and number the exit lanes.

[0045] The midpoint of the starting line of each exit lane at the current intersection is obtained as the position point of the exit lane. Each exit lane is then numbered in ascending order from the inside to the outside for calculation in subsequent steps. A schematic diagram of the holographic vehicle target exit lane and the left-side recognition range is shown below. Figure 3 As shown.

[0046] Step 2: Based on the existing trajectories of each vehicle at the current intersection, calculate the current target exit lane for each vehicle.

[0047] Taking a vehicle at the current intersection as an example:

[0048] (1) Let P1(lon1,lat1) be the latest original trajectory point of the vehicle, and obtain the distance D from the latest original trajectory point. dist The nearest original trajectory point P2(lon2,lat2) is more than 1 meter away.

[0049] (2) Connect trajectory point P1 with trajectory point P2 as the starting point, and calculate the slope k and intercept b of the straight line containing the current target direction ray of the vehicle:

[0050]

[0051] b = lat1 - k * lon1

[0052] This allows us to obtain the current target direction ray of the vehicle.

[0053] (3) Based on the current location points of each exit lane at the intersection and the vehicle's current target direction ray, calculate the vehicle's most likely target exit lane:

[0054] Let Q(lon) be the location of one of the exit lanes. q ,lat q ), calculate the foot of the perpendicular q(q) of the position point Q on the straight line containing the ray in the target direction of the vehicle. x ,q y ):

[0055]

[0056]

[0057] Calculate the distance D1 between the foot of the perpendicular q and the latest original trajectory point P1:

[0058]

[0059] Calculate the distance D2 between the foot of the perpendicular q and the nearest original trajectory point P2:

[0060]

[0061] Calculate the distance D between the latest original trajectory point P1 and the nearest original trajectory point P2. 12 :

[0062]

[0063] The following judgment formula is given:

[0064] D1>D2 and D1>D 12

[0065] If the above calculation result satisfies the judgment formula, it means that the perpendicular foot q does not belong to a point on the current target direction ray of the vehicle, that is, the position point Q is not in the target direction of the vehicle trajectory, and the point will be directly excluded in subsequent calculations;

[0066] If the above formula is not satisfied, it means that the foot of the perpendicular q belongs to a point on the current target direction ray of the vehicle, that is, the position point Q is in the target direction of the vehicle trajectory.

[0067] If position point Q is in the target direction of the vehicle's trajectory, calculate the distance D between position point Q and the ray of the vehicle's current target direction. Q :

[0068]

[0069] Following the method described above, the relationship between the position points of all exit lanes within the current intersection and the target direction ray of the vehicle is sequentially traversed and calculated to obtain the corresponding distance D. Q Select point D from the position point located on the target direction ray of the vehicle. Q The position with the smallest value corresponds to the exit lane that is most likely to be the target exit lane for that vehicle.

[0070] (4) Following the methods in (1)-(3), traverse all vehicles in the current intersection in turn and calculate the most likely exit lane corresponding to each vehicle.

[0071] Step 3: Calculate the current positional relationships between all vehicles within the intersection.

[0072] For each vehicle within the intersection, using its current latest original trajectory point P1(lon1,lat1) as the center and its current target direction angle as the reference angle, detect and record whether there are other vehicles within a fan-shaped range with a left distance threshold of 10 meters (threshold adjustable) and an angle threshold of [-10°, -170°] (threshold adjustable).

[0073] Step 4: Based on the positional relationship between the vehicles, the vehicles in the intersection are sequentially corrected according to the target exit lane to achieve vehicle trajectory correction.

[0074] Determine whether there are other vehicles on the left side of all vehicles within the intersection. Sort all vehicles within the intersection from smallest to largest according to the number of other vehicles on the left side of the vehicle, and then based on this order, perform deviation correction on the vehicles within the intersection in turn based on the lanes of the target exit lanes.

[0075] Specifically manifested as:

[0076] (1) For a certain vehicle within the intersection, calculate the maximum target exit lane number among the other vehicles in the left range recorded by the vehicle and denote it as N, and denote the target exit lane number of the vehicle itself as M.

[0077] (2) Judge the relationship between N and M:

[0078] If N < M, it means that the target paths of the other vehicles on the left side of the vehicle do not cross the target path of the vehicle itself, and continuing to drive along this trajectory will not cause vehicle collisions, that is, the original trajectory data of the current sensor has not deviated abnormally and no deviation correction is required;

[0079] If N ≥ M, it means that the target paths of the other vehicles on the left side of the vehicle cross the target path of the vehicle itself, and continuing to drive along this trajectory will cause vehicle collisions, that is, the original trajectory data of the current sensor has deviated abnormally, so deviation correction should be performed on it.

[0080] The specific method of deviation correction is:

[0081] For the vehicle trajectory with abnormal deviation, it can be considered that the original trajectory data of the vehicle by the sensor is no longer credible, and a new travel route needs to be planned for it.

[0082] When N ≥ M, update the target exit lane number of the vehicle to N + 1 to avoid the crossing of the target paths between vehicles and thus avoid collisions. Then, for this vehicle, stop using the original trajectory data of the sensor. Obtain the corrected route by connecting the current trajectory point of the vehicle with the position point of the vehicle's updated target exit lane, and then make the vehicle move forward along the corrected route at the last driving speed before the trajectory abnormal deviation.

[0083] In this embodiment, if N + 1 is greater than the maximum value of the numbers of the lanes of this exit lane, the target exit lane number of the vehicle is updated to N.

[0084] This embodiment first numbers each exit lane and performs subsequent calculations. Then, by utilizing the existing trajectories of each vehicle within the current intersection, it obtains the target exit lane corresponding to each vehicle. Finally, by combining the calculated positional relationships between the vehicles within the intersection, it corrects the trajectories of vehicles with abnormal deviations and plans new routes. The original trajectory abnormal deviation solution proposed in this embodiment can be applied to holographic digital roads, effectively improving the reliability and realism of holographic vehicle trajectories, and can be universally used with various sensing devices, such as millimeter-wave radar and video cameras. On the other hand, the solution in this embodiment avoids complex hardware optimization, effectively solving the data abnormal deviation problem of various road traffic sensing devices at the software level, thus reducing hardware costs.

[0085] The embodiments described above are merely preferred embodiments of the present invention and are not intended to limit the present invention in any way. Other variations and modifications are possible without departing from the technical solutions described in the claims.

Claims

1. A holographic vehicle trajectory correction algorithm for abnormal data from road sensing devices, characterized in that, include: S1: Obtain the location points of each exit lane at the current intersection and number the exit lanes; S2: Based on the existing trajectories of each vehicle within the current intersection, calculate the current target exit lane for each vehicle; obtain the vehicle's latest original trajectory point P1 and the distance D from this latest original trajectory point. dist Find the nearest original trajectory point P2 at a distance of more than 1 meter. Connect trajectory point P1 with trajectory point P2 as the starting point to obtain the slope k and intercept b of the straight line containing the current target direction ray of the vehicle. Calculate the perpendicular foot q of the position point Q of a certain exit lane on the straight line containing the target direction ray of the vehicle, the distance D1 between the perpendicular foot q and P1, the distance D2 between the perpendicular foot q and P2, and the distance D between P1 and P2. 12 If D1>D2 and D1>D 12 If the condition is met, then position point Q is excluded; otherwise, calculate the distance D between position point Q and the ray pointing in the current target direction of the vehicle. Q ; Iterate through and calculate the relationship between the position points of all exit lanes within the current intersection and the target direction ray of the vehicle, then select D from the position points on the target direction ray of the vehicle. Q The exit lane corresponding to the location with the minimum value is the current target exit lane for that vehicle. S3: Calculate the current positional relationships between all vehicles within the intersection; S4: Based on the positional relationship between vehicles, the vehicles in the intersection are corrected sequentially according to the target exit lane to achieve vehicle trajectory correction.

2. The holographic vehicle trajectory correction algorithm for abnormal data from road sensing devices according to claim 1, characterized in that, Step S2 includes: sequentially traversing all vehicles in the current intersection and calculating the current target exit lane corresponding to each vehicle.

3. The holographic vehicle trajectory correction algorithm for abnormal data from road sensing devices according to claim 1, characterized in that, Step S4 further includes: S4.1: For a vehicle in an intersection, calculate the maximum target exit lane number N among other vehicles to the left of the vehicle, and the target exit lane number of the vehicle itself is M. S4.2: Determine the relationship between N and M. If N < M, no correction is needed. If N ≥ M, then perform correction based on the target exit lane and plan a new route for the vehicle.

4. The holographic vehicle trajectory correction algorithm for abnormal data from road sensing devices according to claim 3, characterized in that, Step S4.2 further includes: updating the target exit lane number of the vehicle to N+1, connecting the current trajectory point of the vehicle with the updated target exit lane position point of the vehicle to obtain the corrected route, and the vehicle proceeding along the corrected route at the last driving speed before the abnormal trajectory deviation.

5. A holographic vehicle trajectory correction algorithm for abnormal data from road sensing devices according to claim 1, 2, or 3, characterized in that, Step S3 is further expressed as follows: For each vehicle within the intersection, taking its current latest original trajectory point P1 (lon1,lat1) as the center and its current target direction angle as the reference angle, detect its left distance threshold D. left Record whether there are other vehicles within a sector range of meters and angle thresholds [Angle1, Angle2].

6. A holographic vehicle trajectory correction algorithm for abnormal data from road sensing devices according to claim 1, 2, or 3, characterized in that, In step S4, the steps include: determining whether there are other vehicles on the left side of all vehicles in the intersection, sorting all vehicles in the intersection from smallest to largest according to the number of other vehicles on the left side of each vehicle, and then performing lane correction based on the target exit lane for each vehicle in the intersection according to this order.

7. A holographic vehicle trajectory correction algorithm for abnormal data from road sensing devices according to claim 1, 2, or 3, characterized in that, Step S1 further includes: obtaining the midpoint of the starting line of each exit lane at the current intersection as the location point of the exit lane, and numbering each exit lane in ascending order from the inside to the outside.

8. The holographic vehicle trajectory correction algorithm for abnormal data from road sensing devices according to claim 4, characterized in that, In step S4.2: if N+1 is greater than the maximum value of the exit lane number, then the target exit lane number of the vehicle is updated to N.

Images