Trajectory smoothing method and device for holographic intersection based on Bezier curve

Abstract

The present application discloses a trajectory smoothing method and device for a holographic intersection based on a Bezier curve, and relates to the field of trajectory generation technology. The method includes: obtaining a standard Bezier curve corresponding to the current holographic intersection and the position information of the vehicle, and determining the target Bezier curve corresponding to each vehicle; predicting the prior state vector of the vehicle at time k based on the Kalman filter algorithm, obtaining the standard position at time k based on the target Bezier curve, and obtaining the observation vector of the vehicle at time k; smoothing the vehicle driving trajectory obtained by radar observation based on the prior state vector, the standard position and the observation vector. The present application is used to solve the problem of poor prediction accuracy caused by a series of monitoring problems when monitoring and predicting vehicle driving trajectories at holographic intersections in the prior art, and to improve the accuracy of driving trajectory prediction.

Description

Technical Field

[0001] The present application relates to the field of trajectory generation technology, and in particular to a trajectory smoothing processing method and device for a holographic intersection based on a Bezier curve. Background Art

[0002] With the increase in the number of vehicles and the complex management of road intersections, intelligent transportation systems have become one of the important means to improve traffic efficiency and safety. Radar, as a major sensor, is widely used in traffic monitoring and vehicle detection because it can operate all day and is not affected by weather. The application of radar systems is particularly important in busy traffic areas such as intersections, but the traffic conditions in these areas are complex and changeable, with frequent vehicles entering and exiting and driving estimates not fixed. It is very important to accurately monitor and predict the trajectory of vehicles.

[0003] In the application of intelligent transportation systems, intersections are usually equipped with multiple radars located in different directions, such as east, west, south, and north. The radar in each direction is responsible for monitoring vehicles entering the intersection from that direction and collecting data such as vehicle speed, location, and trajectory in real time. The vehicle's driving trajectory is generated from this data, which in turn provides data support for decision-making such as traffic signals, route optimization, and accident prevention.

[0004] At intersections, since vehicles come from all directions and may pass through the intersection at the same time, radar monitoring will inevitably cause some problems. Although the overall movement information of vehicles can be obtained by fusing multiple radar data, there are still some problems in practical applications.

[0005] For example, there is the problem of interference between multiple radars. Since multiple radars are deployed at intersections, due to the reflection and interference of electromagnetic waves, multiple radars may interfere with each other, especially when vehicles are moving in the center of the intersection, this interference effect is more obvious. The interference problem may not only cause radar signal attenuation, but also target recognition errors, which in turn leads to inaccurate identification of the vehicle's destination.

[0006] Another example is the occlusion problem. Fixed facilities such as buildings at intersections, overpass billboards, and mobile devices such as large vehicles (such as buses and trucks) can block radar signals. These obstacles may block the signal transmission between the radar and the target vehicle, making it impossible for the radar to effectively detect the real-time position of the target. Even if multiple radars are fused, the data blind spot problem cannot be eliminated due to the complexity of the occlusion angle and the occlusion.

[0007] Another example is the problem of data jitter and temporary data loss. Due to the complex environment of intersections, data jitter may occur during the radar data collection process due to changes in the radar signal reflection surface, short delays in the radar system, or noise interference. This jitter is mainly manifested as fluctuations in the target position of the vehicle, that is, the position information of the same vehicle will change frequently and slightly in a short period of time, and cannot accurately reflect the actual driving trajectory of the vehicle. In addition, the radar system may also experience temporary data loss, that is, the vehicle "disappears" for a period of time at a certain moment. This phenomenon may last from hundreds of milliseconds to a few seconds, which brings great uncertainty to vehicle trajectory prediction and traffic control. For intersections with dense traffic flow, this short-term data loss may have an adverse impact on traffic scheduling and safety warnings. Summary of the invention

[0008] In response to the above-mentioned problems and technical needs, the applicant has proposed a trajectory smoothing method and device for a holographic intersection based on a Bezier curve, so as to solve the problem of poor prediction accuracy caused by a series of monitoring problems when monitoring and predicting vehicle driving trajectories at holographic intersections in the prior art, thereby improving the accuracy of driving trajectory prediction.

[0009] The embodiment of the present application provides a trajectory smoothing method for a holographic intersection based on a Bezier curve, the method comprising:

[0010] Obtaining a standard Bezier curve corresponding to the current holographic intersection and the position information of the front of each vehicle in the current holographic intersection when it passes the stop line for the first time, wherein the standard Bezier curve is used to characterize the standard driving trajectory of the vehicle at the holographic intersection;

[0011] Determining a target Bezier curve corresponding to each vehicle based on the standard Bezier curve and the position information;

[0012] When the front of each vehicle passes through the stop line, predict the prior state vector of the vehicle at time k based on the Kalman filter algorithm, obtain the standard position at time k based on the target Bezier curve, and obtain the observation vector of the vehicle at time k, wherein the observation vector is obtained based on the actual position and speed of the vehicle at time k obtained by radar monitoring;

[0013] The vehicle driving trajectory obtained by radar observation is smoothed based on the prior state vector, the standard position and the observation vector.

[0014] According to a Bezier curve-based trajectory smoothing process of a holographic intersection according to an embodiment of the present application, the vehicle driving trajectory obtained by radar observation is smoothed based on the prior state vector, the standard position and the observation vector, including:

[0015] Inputting the prior state vector, the observation vector and the standard position into a preset trajectory smoothing processing formula to obtain a trajectory smoothing point output by the trajectory smoothing processing formula;

[0016] The trajectory smoothing formula includes:

[0017]

[0018] Among them, s k represents the trajectory smoothing point at time k, β∈[0,1] represents the weight coefficient, z k represents the observation vector, represents the prior state vector, t k represents the standard position, Dis() represents the Euler distance between two positions, Indicates that a smaller value is selected.

[0019] According to a Bezier curve-based trajectory smoothing process of a holographic intersection according to an embodiment of the present application, a priori state vector of a vehicle at time k is predicted based on a Kalman filter algorithm, including:

[0020] Get the posterior state vector at time k-1 and the posterior covariance matrix of the posterior state vector at time k-1;

[0021] Input the posterior state vector at time k-1 and the posterior covariance matrix at time k-1 into a preset Kalman filter prediction formula to obtain the prior state vector at time k output by the Kalman filter prediction formula and the prior covariance matrix of the prior state vector at time k;

[0022] Wherein, the Kalman filter prediction formula includes:

[0023]

[0024] in, represents the prior state vector at time k, a represents the preset state transfer vector, which is a constant, and x k-1 represents the posterior state vector at time k-1, b represents the preset control matrix, which is a constant, and u k-1 represents the state control vector at the preset k-1 time, represents the prior covariance matrix at time k, P k-1 represents the posterior covariance matrix at time k-1, Q represents the preset model noise covariance matrix, which is a constant.

[0025] According to the trajectory smoothing process of the holographic intersection based on the Bezier curve in one embodiment of the present application, after predicting the prior state vector of the vehicle at time k based on the Kalman filter algorithm, it also includes:

[0026] Inputting the prior state vector at time k and the prior covariance matrix at time k into a preset Kalman filter update formula to obtain the posterior state vector at time k and the posterior covariance matrix of the posterior state vector at time k output by the Kalman filter update formula;

[0027] Wherein, the Kalman filter update formula includes:

[0028]

[0029] Among them, x k represents the posterior state vector at time k, K k Represents the Kalman filter gain matrix at the preset k moment, which is a constant, Z k is the observation vector corresponding to the actual position and speed of the vehicle at time k obtained by radar monitoring, H represents the conversion matrix that converts the prior state vector at time k into the observation vector, I represents the unit matrix, represents the posterior covariance matrix at time k;

[0030] The priori state vector at time k+1 is obtained based on the a posteriori state vector at time k, the a posteriori covariance matrix at time k and the Kalman filter prediction formula.

[0031] According to a Bezier curve-based trajectory smoothing process of a holographic intersection according to an embodiment of the present application, based on the standard Bezier curve and the position information, a target Bezier curve corresponding to each vehicle is determined, including:

[0032] Determining a target lane for the vehicle in the holographic intersection based on the position information;

[0033] A candidate Bezier curve corresponding to the target lane is determined from the standard Bezier curves, and the candidate Bezier curve is determined as the target Bezier curve of the vehicle.

[0034] According to a Bezier curve-based trajectory smoothing process of a holographic intersection according to an embodiment of the present application, the generation process of the standard Bezier curve includes:

[0035] The following curve generation process is performed for each lane of the holographic intersection:

[0036] Obtaining a calculation formula for a universal third-order Bezier curve and a historical driving trajectory of the vehicle corresponding to the lane;

[0037] Wherein, the calculation formula includes:

[0038] B(t)=(1-t) 3 P0+3t(1-t) 2 P1+3t 2(1-t)P2+t 3 P3;

[0039] Wherein, B(t) represents a standard Bezier curve, t∈[0,1] represents a curve parameter, P0 represents a starting point, P1 represents a first intermediate point, P2 represents a second intermediate point, and P3 represents an end point;

[0040] Determining a starting point and an end point based on the direction of the lane, and determining a first intermediate point and a second intermediate point based on the historical driving trajectory;

[0041] The standard Bezier curve is obtained based on the starting point, the end point, the first middle point, the second middle point and the calculation formula.

[0042] According to an embodiment of the present application, the trajectory smoothing process of the holographic intersection based on the Bezier curve, after obtaining the standard Bezier curve, further includes:

[0043] Based on the historical driving trajectory and the direction of the lane, the standard Bezier curve is moved in parallel in the horizontal direction or the vertical direction to obtain an upper limit Bezier curve and a lower limit Bezier curve corresponding to the standard Bezier curve.

[0044] According to the trajectory smoothing processing of the holographic intersection based on the Bezier curve in one embodiment of the present application, the vehicle driving trajectory after smoothing cannot exceed the range corresponding to the upper limit Bezier curve and the lower limit Bezier curve;

[0045] The upper limit Bezier curve and the lower limit Bezier curve are obtained in advance based on the historical driving trajectory and the direction of the lane.

[0046] According to the trajectory smoothing process of the holographic intersection based on the Bezier curve in one embodiment of the present application, after obtaining the trajectory smoothing point at time k, the process further includes:

[0047] Determine whether the trajectory smoothing point exceeds the range corresponding to the upper limit Bezier curve and the lower limit Bezier curve;

[0048] When it is determined that the trajectory smoothing point exceeds the range corresponding to the upper limit Bezier curve and the lower limit Bezier curve, the weight coefficient is set to zero.

[0049] The embodiment of the present application also provides a trajectory smoothing processing device for a holographic intersection based on a Bezier curve, comprising:

[0050] An acquisition module, used to acquire a standard Bezier curve corresponding to the current holographic intersection, and position information of the front of each vehicle in the current holographic intersection when it passes the stop line for the first time, wherein the standard Bezier curve is used to characterize the standard driving trajectory of the vehicle at the holographic intersection;

[0051] A first determination module, configured to determine a target Bezier curve corresponding to each vehicle based on the standard Bezier curve and the position information;

[0052] A second determination module is used to predict the prior state vector of the vehicle at time k based on the Kalman filter algorithm when the front of each vehicle passes through the stop line, obtain the standard position at time k based on the target Bezier curve, and obtain the observation vector of the vehicle at time k, wherein the observation vector is obtained based on the actual position and speed of the vehicle at time k obtained by radar monitoring;

[0053] The smoothing processing module is used to smooth the vehicle driving trajectory obtained by radar observation based on the prior state vector, the standard position and the observation vector.

[0054] An embodiment of the present application also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, the steps of the trajectory smoothing method for a holographic intersection based on a Bezier curve as described in any one of the above items are implemented.

[0055] An embodiment of the present application also provides a non-transitory computer-readable storage medium having a computer program stored thereon. When the computer program is executed by a processor, the steps of the trajectory smoothing method of a holographic intersection based on a Bezier curve as described in any of the above items are implemented.

[0056] The trajectory smoothing method and device for a holographic intersection based on a Bezier curve provided in an embodiment of the present application obtains a target Bezier curve corresponding to each vehicle through a predetermined standard Bezier curve and the position information of the front of each vehicle in the current holographic intersection when it passes the stop line for the first time, thereby providing an effective data basis for providing a stable and coherent vehicle driving trajectory; and when the front of each vehicle passes the stop line, the prior state vector of the vehicle at time k is predicted based on the Kalman filter algorithm, the standard position at time k is obtained based on the target Bezier curve, and the observation vector of the vehicle at time k is obtained; the vehicle driving trajectory obtained by radar observation is smoothed based on the prior state vector, the standard position and the observation vector. The present application performs smoothing of the vehicle driving trajectory based on the prior prediction and actual observation, which can effectively solve the problems of inaccurate target position data due to radar signal attenuation, data loss due to obstacle occlusion, and trajectory instability caused by data jitter and temporary loss, thereby generating a continuous target trajectory that conforms to the actual motion law, solving the problem of poor prediction accuracy when monitoring and predicting vehicle driving trajectories at holographic intersections, and improving the accuracy of driving trajectory prediction. BRIEF DESCRIPTION OF THE DRAWINGS

[0057] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0058] Figure 1 It is a flowchart of a trajectory smoothing method for a holographic intersection based on a Bezier curve provided in an embodiment of the present application;

[0059] Figure 2 is a schematic diagram of a standard Bezier curve provided in an embodiment of the present application;

[0060] Figure 3 It is a structural schematic diagram of a trajectory smoothing processing device for a holographic intersection based on a Bezier curve provided in an embodiment of the present application;

[0061] Figure 4 It is a schematic diagram of the structure of an electronic device provided in an embodiment of the present application. DETAILED DESCRIPTION

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

[0063] In order to clearly explain this application, the usage scenario is further explained:

[0064] There are also methods for smoothing trajectories in the prior art, such as the moving average method and the weighted average method. Although the above methods can smooth the trajectory to a certain extent, their effects are limited, especially in complex intersection environments. When processing complex motion trajectories, the moving average method is prone to trajectory distortion and cannot accurately reflect the actual motion state of the vehicle. At the same time, these methods are also powerless against short-term data loss, and the lost data cannot be compensated by simple average calculation.

[0065] In order to solve the above problems, the embodiment of the present application provides a trajectory smoothing process of a holographic intersection based on a Bezier curve. The method can be applied to a smart terminal or a server. Some other descriptions in the embodiment of the present application are for illustrative purposes only and are not used to limit the scope of protection of the present application. They will not be described one by one later. The specific implementation of the method is as follows Figure 1 As shown:

[0066] Step 101, obtaining a standard Bezier curve corresponding to the current holographic intersection, and the position information of the front of each vehicle in the current holographic intersection when it passes the stop line for the first time.

[0067] Among them, the standard Bezier curve is used to characterize the standard driving trajectory of the vehicle at the holographic intersection.

[0068] Step 102: Determine a target Bezier curve corresponding to each vehicle based on the standard Bezier curve and the position information.

[0069] Step 103, when the front of each vehicle passes the stop line, predict the prior state vector of the vehicle at time k based on the Kalman filter algorithm, obtain the standard position at time k based on the target Bezier curve, and obtain the observation vector of the vehicle at time k.

[0070] Among them, the observation vector is obtained based on the actual position and speed of the vehicle at time k obtained by radar monitoring.

[0071] Step 104 , smoothing the vehicle driving trajectory obtained by radar observation based on the prior state vector, the standard position and the observation vector.

[0072] The trajectory smoothing method for a holographic intersection based on a Bezier curve provided in an embodiment of the present application obtains a target Bezier curve corresponding to each vehicle through a predetermined standard Bezier curve and the position information of the front of each vehicle in the current holographic intersection when it passes the stop line for the first time, thereby providing an effective data basis for providing a stable and coherent vehicle driving trajectory; and when the front of each vehicle passes the stop line, the prior state vector of the vehicle at time k is predicted based on the Kalman filter algorithm, the standard position at time k is obtained based on the target Bezier curve, and the observation vector of the vehicle at time k is obtained; the vehicle driving trajectory obtained by radar observation is smoothed based on the prior state vector, the standard position and the observation vector. The present application performs smoothing of the vehicle driving trajectory based on the prior prediction and actual observation, which can effectively solve the problems of inaccurate target position data due to radar signal attenuation, data loss due to obstacle occlusion, and trajectory instability caused by data jitter and short-term loss, thereby generating a continuous target trajectory that conforms to the actual motion law, solving the problem of poor prediction accuracy when monitoring and predicting vehicle driving trajectories at holographic intersections, and improving the accuracy of driving trajectory prediction.

[0073] In a specific embodiment, the process of generating a standard Bezier curve includes:

[0074] The following curve generation process is performed for each lane of the holographic intersection:

[0075] Obtain a calculation formula for a general third-order Bezier curve and a historical driving trajectory of the vehicle corresponding to the lane; determine a starting point and an end point based on the direction of the lane, and determine a first middle point and a second middle point based on the historical driving trajectory; obtain a standard Bezier curve based on the starting point, the end point, the first middle point, the second middle point and the calculation formula.

[0076] The calculation formula of the third-order Bezier curve is shown in formula (1):

[0077] B(t)=(1-t) 3 P0+3t(1-t) 2 P1+3t 2 (1-t)P2+t 3 P3....(1)

[0078] Wherein, B(t) represents a standard Bezier curve, t∈[0,1] represents a curve parameter, P0 represents a starting point, P1 represents a first intermediate point, P2 represents a second intermediate point, and P3 represents an end point.

[0079] The direction of the lane is used to characterize the movement trend of the vehicle.

[0080] Specifically, for a fixed lane at a fixed intersection, the starting point and the end point are determined, so the first middle point and the second middle point need to be determined. The historical driving trajectories of a large number of vehicles in the lane are obtained, and their distribution is analyzed. The first middle point and the second middle point are randomly determined from the trajectories corresponding to the point where the distribution probability is the largest. The obtained starting point, end point, first middle point and second middle point are input into formula (1) to obtain the standard Bezier curve of the vehicle.

[0081] The points in the curve are coordinate points. The curve parameters can be determined based on experience.

[0082] Among them, the Bezier curve can generate a smooth and controllable curve, which is suitable for describing complex motion trajectories and is commonly used in computer graphics, path planning and other fields. This application uses a third-order Bezier curve.

[0083] In a specific embodiment, after obtaining the standard Bezier curve, the standard Bezier curve is moved in parallel in the horizontal direction or vertical direction based on the historical driving trajectory and the direction of the lane to obtain the upper limit Bezier curve and the lower limit Bezier curve corresponding to the standard Bezier curve.

[0084] Specifically, a large number of historical driving trajectories of vehicles in the lane are obtained, their distribution is analyzed, and points corresponding to preset limit distribution probabilities are determined from the distribution probabilities, which are used as upper and lower limit points, and then the curve is translated to obtain upper and lower limit Bezier curves.

[0085] exist Figure 2In the example, the standard Bezier curve of the intersection from south to west is used as an example. a represents the standard Bezier curve, t up represents the upper limit Bezier curve, t down Represents a lower bound Bezier curve.

[0086] In a specific embodiment, the vehicle driving trajectory cannot exceed the range corresponding to the upper limit Bezier curve and the lower limit Bezier curve after smoothing.

[0087] The upper limit Bezier curve and the lower limit Bezier curve are obtained in advance based on the historical driving trajectory and the direction of the lane.

[0088] In a specific embodiment, based on the standard Bezier curve and the position information, the specific implementation of determining the target Bezier curve corresponding to each vehicle includes:

[0089] The target lane of the vehicle in the holographic intersection is determined based on the position information; a candidate Bezier curve corresponding to the target lane is determined from the standard Bezier curve, and the candidate Bezier curve is determined as the target Bezier curve of the vehicle.

[0090] Among them, the position information can not only represent the target lane where the vehicle is located, but also obtain the position of the front of the vehicle relative to the stop line when it passes the stop line. The target lane can be used to obtain the candidate Bezier curve, and the target Bezier curve of the vehicle can be obtained by translating the candidate Bezier curve horizontally or vertically to the point corresponding to the position information.

[0091] The point on the target Bezier curve is a standard point, and the position corresponding to the standard point is a standard position.

[0092] In a specific embodiment, the specific implementation of predicting the prior state vector of the vehicle at time k based on the Kalman filter algorithm includes:

[0093] Obtain the posterior state vector at time k-1 and the posterior covariance matrix of the posterior state vector at time k-1; input the posterior state vector at time k-1 and the posterior covariance matrix at time k-1 into a preset Kalman filter prediction formula to obtain the prior state vector at time k output by the Kalman filter prediction formula and the prior covariance matrix of the prior state vector at time k.

[0094] Among them, the Kalman filter prediction formula is shown in formula (2) and formula (3):

[0095]

[0096] in, represents the prior state vector at time k, a represents the preset state transfer vector, which is a constant, and x k-1represents the posterior state vector at time k-1, b represents the preset control matrix, which is a constant, and u k-1 represents the state control vector at the preset k-1 time, represents the prior covariance matrix at time k, P k-1 represents the posterior covariance matrix at time k-1, Q represents the preset model noise covariance matrix, which is a constant.

[0097] In a specific embodiment, after predicting the prior state vector of the vehicle at time k based on the Kalman filter algorithm, the prior state vector at time k and the prior covariance matrix at time k are input into a preset Kalman filter update formula to obtain the posterior state vector at time k and the posterior covariance matrix of the posterior state vector at time k output by the Kalman filter update formula; the prior state vector at time k+1 is obtained based on the posterior state vector at time k, the posterior covariance matrix at time k and the Kalman filter prediction formula.

[0098] The Kalman filter update formula is shown in formula (4) and formula (5):

[0099]

[0100] Among them, x k represents the posterior state vector at time k, K k Represents the Kalman filter gain matrix at the preset k moment, which is a constant, Z k is the observation vector corresponding to the actual position and speed of the vehicle at time k obtained by radar monitoring, H represents the conversion matrix that converts the prior state vector at time k into the observation vector, I represents the unit matrix, represents the posterior covariance matrix at time k.

[0101] Specifically, the Kalman filter algorithm is a recursive smoothing technology. The trajectory prediction based on the Kalman filter algorithm in this application mainly includes two stages: the Kalman filter prediction stage (corresponding to formula (2) and formula (3)) and the Kalman filter update stage (corresponding to formula (4) and formula (5)). The final smooth curve is obtained through iterative cycle calculation of the Kalman filter prediction stage and the Kalman filter update stage.

[0102] In a specific embodiment, the specific implementation of smoothing the vehicle driving trajectory obtained by radar observation based on the prior state vector, the standard position and the observation vector includes:

[0103] The prior state vector, the observation vector and the standard position are input into a preset trajectory smoothing processing formula to obtain a trajectory smoothing point output by the trajectory smoothing processing formula.

[0104] The trajectory smoothing formula is shown in formula (6):

[0105]

[0106] Among them, s k represents the trajectory smoothing point at time k, β∈[0,1] represents the weight coefficient, z k represents the observation vector, represents the prior state vector, t k represents the standard position, Dis() represents the Euler distance between two positions, Indicates that a smaller value is selected.

[0107] in, Among them, x z Indicates the X-axis coordinate value of the position monitored by the radar in the observation vector, y z Indicates the Y-axis coordinate value of the position monitored by the radar, x t Indicates the X-axis coordinate value of the standard position, y t Indicates the Y-axis coordinate value of the standard position.

[0108] in, Among them, x k Represents the X-axis coordinate value of the position in the prior state vector, y k Represents the Y-axis coordinate value of the position in the prior state vector.

[0109] Among them, when the weight coefficient is non-zero, the user can determine it according to his or her actual needs, and this application does not impose any restrictions.

[0110] In a specific embodiment, after obtaining the trajectory smoothing point at time k, it is determined whether the trajectory smoothing point exceeds the range corresponding to the upper limit Bezier curve and the lower limit Bezier curve; if it is determined that the trajectory smoothing point exceeds the range corresponding to the upper limit Bezier curve and the lower limit Bezier curve, the weight coefficient is set to zero; if it is determined that the trajectory smoothing point does not exceed the range corresponding to the upper limit Bezier curve and the lower limit Bezier curve, the trajectory smoothing point at time k+1 is calculated based on formulas (2) to (6) until the front of the vehicle passes the end point.

[0111] In a specific embodiment, if the observation vector at time k is not obtained, that is, the actual position of the vehicle at time k is not obtained, the weight coefficient is zero.

[0112] This application combines the Kalman filter algorithm and the standard Bezier curve to smooth the motion trajectory while retaining the original motion trajectory. In addition, the Kalman filter algorithm can effectively reduce the noise jitter in radar observation, solve the problem of poor prediction accuracy when monitoring and predicting vehicle driving trajectories at holographic intersections, and improve the accuracy of driving trajectory prediction.

[0113] The embodiment of the present application also provides a trajectory smoothing processing device for a holographic intersection based on a Bezier curve. The specific implementation of the device can refer to the description of the trajectory smoothing processing method for a holographic intersection based on a Bezier curve, and the repeated parts will not be repeated. Figure 3 As shown, the device comprises:

[0114] The acquisition module 301 is used to acquire the standard Bezier curve corresponding to the current holographic intersection and the position information of the front of each vehicle in the current holographic intersection when it passes the stop line for the first time, wherein the standard Bezier curve is used to represent the standard driving trajectory of the vehicle in the holographic intersection;

[0115] A first determination module 302, configured to determine a target Bezier curve corresponding to each vehicle based on the standard Bezier curve and the position information;

[0116] The second determination module 303 is used to predict the prior state vector of the vehicle at time k based on the Kalman filter algorithm when the front of each vehicle passes the stop line, obtain the standard position at time k based on the target Bezier curve, and obtain the observation vector of the vehicle at time k, wherein the observation vector is obtained based on the actual position and speed of the vehicle at time k obtained by radar monitoring;

[0117] The smoothing processing module 304 is used to smooth the vehicle driving trajectory obtained by radar observation based on the prior state vector, the standard position and the observation vector.

[0118] In a specific embodiment, the smoothing processing module 304 is used to input the prior state vector, the observation vector and the standard position into a preset trajectory smoothing processing formula to obtain a trajectory smoothing point output by the trajectory smoothing processing formula.

[0119] Among them, the trajectory smoothing formula includes:

[0120]

[0121] Among them, s k represents the trajectory smoothing point at time k, β∈[0,1] represents the weight coefficient, z k represents the observation vector, represents the prior state vector, t k represents the standard position, Dis() represents the Euler distance between two positions, Indicates that a smaller value is selected.

[0122] In a specific embodiment, the second determination module 303 is used to obtain the posterior state vector at time k-1 and the posterior covariance matrix of the posterior state vector at time k-1; the posterior state vector at time k-1 and the posterior covariance matrix at time k-1 are input into a preset Kalman filter prediction formula to obtain the prior state vector at time k output by the Kalman filter prediction formula and the prior covariance matrix of the prior state vector at time k.

[0123] Among them, the Kalman filter prediction formula includes:

[0124]

[0125] in, represents the prior state vector at time k, a represents the preset state transfer vector, which is a constant, and x k-1 represents the posterior state vector at time k-1, b represents the preset control matrix, which is a constant, and u k-1 represents the state control vector at the preset k-1 time, represents the prior covariance matrix at time k, P k-1 represents the posterior covariance matrix at time k-1, Q represents the preset model noise covariance matrix, which is a constant.

[0126] In a specific embodiment, the second determination module 303 is used to input the prior state vector at time k and the prior covariance matrix at time k into a preset Kalman filter update formula to obtain the posterior state vector at time k and the posterior covariance matrix of the posterior state vector at time k output by the Kalman filter update formula.

[0127] Among them, the Kalman filter update formula includes:

[0128]

[0129]

[0130] Among them, x k represents the posterior state vector at time k, x k Represents the Kalman filter gain matrix at the preset k moment, which is a constant, Z k is the observation vector corresponding to the actual position and speed of the vehicle at time k obtained by radar monitoring, H represents the conversion matrix that converts the prior state vector at time k into the observation vector, I represents the unit matrix, represents the posterior covariance matrix at time k.

[0131] The prior state vector at time k+1 is obtained based on the posterior state vector at time k, the posterior covariance matrix at time k and the Kalman filter prediction formula.

[0132] The first determination module 302 is used to determine the target lane of the vehicle in the holographic intersection based on the position information; determine a candidate Bezier curve corresponding to the target lane from the standard Bezier curves, and determine the candidate Bezier curve as the target Bezier curve of the vehicle.

[0133] In a specific embodiment, the device also includes a generation module for generating a standard Bezier curve, and the process includes: executing the following curve generation process for each lane of the holographic intersection: obtaining a calculation formula for a general third-order Bezier curve and a historical driving trajectory of the vehicle corresponding to the lane.

[0134] The calculation formula includes:

[0135] B(t)=(1-t) 3 P0+3t(1-t) 2 P1+3t 2 (1-t)P2+t 3 P3.

[0136] Wherein, B(t) represents a standard Bezier curve, t∈[0,1] represents a curve parameter, P0 represents a starting point, P1 represents a first intermediate point, P2 represents a second intermediate point, and P3 represents an end point.

[0137] The starting point and the end point are determined based on the direction of the lane, and the first intermediate point and the second intermediate point are determined based on the historical driving trajectory.

[0138] A standard Bezier curve is obtained based on the starting point, the end point, the first middle point, the second middle point and a calculation formula.

[0139] In a specific embodiment, the generating module is further used to parallel move the standard Bezier curve in the horizontal direction or the vertical direction based on the historical driving trajectory and the direction of the lane to obtain the upper limit Bezier curve and the lower limit Bezier curve corresponding to the standard Bezier curve.

[0140] In a specific embodiment, the vehicle driving trajectory cannot exceed the range corresponding to the upper limit Bezier curve and the lower limit Bezier curve after smoothing.

[0141] The upper limit Bezier curve and the lower limit Bezier curve are obtained in advance based on the historical driving trajectory and the direction of the lane.

[0142] In a specific embodiment, the smoothing processing module 304 is also used to determine whether the trajectory smoothing point exceeds the range corresponding to the upper limit Bezier curve and the lower limit Bezier curve; if it is determined that the trajectory smoothing point exceeds the range corresponding to the upper limit Bezier curve and the lower limit Bezier curve, the weight coefficient is set to zero.

[0143] Figure 4 An example of a physical structure diagram of an electronic device is shown in FIG. Figure 4 As shown, the electronic device may include: a processor 401, a communication interface 402, a memory 403 and a communication bus 404, wherein the processor 401, the communication interface 402 and the memory 403 communicate with each other through the communication bus 404. The processor 401 may call the logic instructions in the memory 403 to execute the trajectory smoothing processing method of the holographic intersection based on the Bezier curve.

[0144] In addition, the logic instructions in the above-mentioned memory 403 can be implemented in the form of a software functional unit and can be stored in a computer-readable storage medium when it is sold or used as an independent product. Based on such an understanding, the technical solution of the present invention can be essentially or partly embodied in the form of a software product that contributes to the prior art. The computer software product is stored in a storage medium, including several instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk.

[0145] On the other hand, the present invention also provides a computer program product, which includes a computer program stored on a non-transitory computer-readable storage medium, and the computer program includes program instructions. When the program instructions are executed by a computer, the computer can execute the trajectory smoothing processing method of the holographic intersection based on Bezier curves provided by the above-mentioned methods.

[0146] On the other hand, the present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, is implemented to execute the trajectory smoothing processing method for a holographic intersection based on Bezier curves provided in the above-mentioned embodiments.

[0147] The device embodiments described above are merely illustrative, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the scheme of this embodiment. Ordinary technicians in this field can understand and implement it without paying creative labor.

[0148] Through the description of the above implementation methods, those skilled in the art can clearly understand that each implementation method can be implemented by means of software plus a necessary general hardware platform, and of course, it can also be implemented by hardware. Based on this understanding, the above technical solution is essentially or the part that contributes to the prior art can be embodied in the form of a software product, and the computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, a disk, an optical disk, etc., including a number of instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in each embodiment or some parts of the embodiments.

[0149] Finally, it should be noted that the above is only the preferred implementation of the present application, and the present application is not limited to the above embodiments. It is understood that other improvements and changes directly derived or associated by those skilled in the art without departing from the spirit and concept of the present application should be considered to be included in the scope of protection of the present application.

Claims

1. A trajectory smoothing method for a holographic intersection based on a Bezier curve, characterized in that: The method comprises: Obtaining a standard Bezier curve corresponding to the current holographic intersection and the position information of the front of each vehicle in the current holographic intersection when it passes the stop line for the first time, wherein the standard Bezier curve is used to characterize the standard driving trajectory of the vehicle at the holographic intersection; Determining a target Bezier curve corresponding to each vehicle based on the standard Bezier curve and the position information; When the front of each vehicle passes through the stop line, predict the prior state vector of the vehicle at time k based on the Kalman filter algorithm, obtain the standard position at time k based on the target Bezier curve, and obtain the observation vector of the vehicle at time k, wherein the observation vector is obtained based on the actual position and speed of the vehicle at time k obtained by radar monitoring; The vehicle driving trajectory obtained by radar observation is smoothed based on the prior state vector, the standard position and the observation vector.

2. The trajectory smoothing method of a holographic intersection based on a Bezier curve according to claim 1, characterized in that: The method further comprises: performing smoothing processing on the vehicle driving trajectory obtained by radar observation based on the prior state vector, the standard position and the observation vector, comprising: Inputting the prior state vector, the observation vector and the standard position into a preset trajectory smoothing processing formula to obtain a trajectory smoothing point output by the trajectory smoothing processing formula; The trajectory smoothing formula includes: Among them, s k represents the trajectory smoothing point at time k, β∈[0,1] represents the weight coefficient, z k represents the observation vector, represents the prior state vector, t k represents the standard position, Dis() represents the Euler distance between two positions, Indicates that a smaller value is selected.

3. The trajectory smoothing method of a holographic intersection based on a Bezier curve according to claim 1, characterized in that: The prior state vector of the vehicle at time k is predicted based on the Kalman filter algorithm, including: Get the posterior state vector at time k-1 and the posterior covariance matrix of the posterior state vector at time k-1; Input the posterior state vector at time k-1 and the posterior covariance matrix at time k-1 into a preset Kalman filter prediction formula to obtain the prior state vector at time k output by the Kalman filter prediction formula and the prior covariance matrix of the prior state vector at time k; Wherein, the Kalman filter prediction formula includes: in, represents the prior state vector at time k, a represents the preset state transfer vector, which is a constant, and x k-1 represents the posterior state vector at time k-1, b represents the preset control matrix, which is a constant, and u k-1 represents the state control vector at the preset k-1 time, represents the prior covariance matrix at time k, P k-1 represents the posterior covariance matrix at time k-1, Q represents the preset model noise covariance matrix, which is a constant.

4. The trajectory smoothing method of a holographic intersection based on a Bezier curve according to claim 3, characterized in that: After predicting the prior state vector of the vehicle at time k based on the Kalman filter algorithm, it also includes: Inputting the prior state vector at time k and the prior covariance matrix at time k into a preset Kalman filter update formula to obtain the posterior state vector at time k and the posterior covariance matrix of the posterior state vector at time k output by the Kalman filter update formula; Wherein, the Kalman filter update formula includes: Among them, x k represents the posterior state vector at time k, K k Represents the Kalman filter gain matrix at the preset k moment, which is a constant, Z k is the observation vector corresponding to the actual position and speed of the vehicle at time k obtained by radar monitoring, H represents the conversion matrix that converts the prior state vector at time k into the observation vector, I represents the unit matrix, represents the posterior covariance matrix at time k; The priori state vector at time k+1 is obtained based on the a posteriori state vector at time k, the a posteriori covariance matrix at time k and the Kalman filter prediction formula.

5. The trajectory smoothing method of a holographic intersection based on a Bezier curve according to any one of claims 1 to 4, characterized in that: Determining a target Bezier curve corresponding to each vehicle based on the standard Bezier curve and the position information includes: Determining a target lane for the vehicle in the holographic intersection based on the position information; A candidate Bezier curve corresponding to the target lane is determined from the standard Bezier curves, and the candidate Bezier curve is determined as the target Bezier curve of the vehicle.

6. The trajectory smoothing method of a holographic intersection based on a Bezier curve according to any one of claims 1 to 4, characterized in that: The generation process of the standard Bezier curve includes: The following curve generation process is performed for each lane of the holographic intersection: Obtaining a calculation formula for a universal third-order Bezier curve and a historical driving trajectory of the vehicle corresponding to the lane; Wherein, the calculation formula includes: B(t)=(1-t) 3 P0+3t(1-t) 2 P1+3t 2 (1-t)P2+t 3 P3; Wherein, B(t) represents the standard Bezier curve, t∈[0,1] represents the curve parameter, P0 represents the starting point, P1 represents the first intermediate point, P2 represents the second intermediate point, and P3 represents the end point; Determining a starting point and an end point based on the direction of the lane, and determining a first intermediate point and a second intermediate point based on the historical driving trajectory; The standard Bezier curve is obtained based on the starting point, the end point, the first middle point, the second middle point and the calculation formula.

7. The trajectory smoothing method of a holographic intersection based on a Bezier curve according to claim 6, characterized in that: After obtaining the standard Bezier curve, the method further includes: Based on the historical driving trajectory and the direction of the lane, the standard Bezier curve is moved in parallel in the horizontal direction or the vertical direction to obtain an upper limit Bezier curve and a lower limit Bezier curve corresponding to the standard Bezier curve.

8. The trajectory smoothing method of a holographic intersection based on a Bezier curve according to claim 2, characterized in that: The vehicle driving trajectory cannot exceed the range corresponding to the upper limit Bezier curve and the lower limit Bezier curve after smoothing; The upper limit Bezier curve and the lower limit Bezier curve are obtained in advance based on the historical driving trajectory and the direction of the lane.

9. The trajectory smoothing method of a holographic intersection based on a Bezier curve according to claim 8, characterized in that: After obtaining the trajectory smoothing point at time k, it also includes: Determine whether the trajectory smoothing point exceeds the range corresponding to the upper limit Bezier curve and the lower limit Bezier curve; When it is determined that the trajectory smoothing point exceeds the range corresponding to the upper limit Bezier curve and the lower limit Bezier curve, the weight coefficient is set to zero.

10. A trajectory smoothing device for a holographic intersection based on a Bezier curve, characterized in that: The device comprises: An acquisition module, used to acquire a standard Bezier curve corresponding to the current holographic intersection, and position information of the front of each vehicle in the current holographic intersection when it passes the stop line for the first time, wherein the standard Bezier curve is used to characterize the standard driving trajectory of the vehicle at the holographic intersection; A first determination module, configured to determine a target Bezier curve corresponding to each vehicle based on the standard Bezier curve and the position information; A second determination module is used to predict the prior state vector of the vehicle at time k based on the Kalman filter algorithm when the front of each vehicle passes through the stop line, obtain the standard position at time k based on the target Bezier curve, and obtain the observation vector of the vehicle at time k, wherein the observation vector is obtained based on the actual position and speed of the vehicle at time k obtained by radar monitoring; The smoothing processing module is used to smooth the vehicle driving trajectory obtained by radar observation based on the prior state vector, the standard position and the observation vector.

Images