Track fusion processing method and device, computer equipment and storage medium

Abstract

The invention relates to a track fusion processing method and device, computer equipment and a storage medium, and the method comprises the steps: obtaining the track data of vehicle-mounted equipment and the sensing data of roadside sensing equipment; the track data and the sensing data comprise corresponding longitudes and latitudes and timestamps; the vehicle-mounted equipment is arranged on a target vehicle, and the roadside sensing equipment is arranged around a road; based on a pre-constructed cost matrix, matching the trajectory data with the sensing data to obtain an optimal mapping relation; and based on the optimal mapping relation, fusing the trajectory data and the sensing data to obtain a vehicle trajectory. Through the data fusion method and device, the problem that the accuracy of data fusion is affected due to deviation of two types of data on a time axis in the prior art is solved, and the two types of data are aligned by utilizing the optimal mapping relation obtained based on cost matrix matching, so that the accuracy of fusion of the two types of data is improved.

Description

Technical Field

[0001] The present application relates to the field of data processing technology, and in particular to a processing method, device, computer equipment and storage medium for trajectory fusion. Background Art

[0002] The devices that provide data collection functions in vehicles include on-board devices (OBU) and roadside sensing devices. Among them, on-board devices can provide accurate location, speed and other trajectory information of each vehicle, but it is difficult to fully perceive the entire traffic environment by relying solely on the information of a single vehicle. Among them, roadside sensing devices (such as road side units (RSU), cameras, lidar, etc.) can monitor road conditions, traffic flow and other environmental factors through sensors fixed around the road, but their coverage is usually limited. Therefore, a single data source cannot meet the perception needs in complex traffic environments.

[0003] In order to solve this problem, the fusion of roadside perception data from roadside perception equipment and trajectory data from vehicle-mounted equipment came into being. Combining the two types of data can not only provide more accurate vehicle location and trajectory information for the traffic management system, but also enhance the perception of the surrounding environment, thereby providing a solid data foundation for the realization of vehicle-road collaboration, autonomous driving and intelligent traffic management.

[0004] The collaborative work of on-board equipment and roadside perception equipment can provide rich traffic information. However, the two devices have different computational complexities in data collection and processing, which often leads to inconsistencies in the generated data in time, causing deviations between the two types of data on the time axis, which in turn affects the accuracy of data fusion.

[0005] There is currently no effective solution to the problem that in related technologies, there is a deviation between the two types of data on the time axis, which affects the accuracy of data fusion. Summary of the invention

[0006] In this embodiment, a trajectory fusion processing method, apparatus, computer device and storage medium are provided to solve the problem in the related art that the deviation of two types of data on the time axis affects the accuracy of data fusion.

[0007] In a first aspect, a trajectory fusion processing method is provided in this embodiment, including:

[0008] Acquire trajectory data of an on-board device and perception data of a roadside perception device; the trajectory data and the perception data both include corresponding longitude and latitude and timestamps; the on-board device is arranged on a target vehicle, and the roadside perception device is arranged around a road;

[0009] Based on a pre-constructed cost matrix, the trajectory data and the perception data are matched to obtain an optimal mapping relationship; the elements in the cost matrix are matching costs between the trajectory data and the perception data;

[0010] Based on the optimal mapping relationship, the trajectory data and the perception data are fused to obtain a vehicle trajectory.

[0011] In some embodiments, the method further comprises:

[0012] Based on the trajectory data, the missing trajectory data in the trajectory data is completed to obtain the completed trajectory data.

[0013] In some embodiments, the step of completing missing trajectory data in the trajectory data based on the trajectory data to obtain completed trajectory data includes:

[0014] According to a preset longitude and latitude interpolation formula and the trajectory data, the longitude and latitude in the missing trajectory data are supplemented to obtain longitude and latitude supplemented data;

[0015] Completing the timestamp in the missing trajectory data according to a preset timestamp interpolation formula and the trajectory data to obtain timestamp-completing data;

[0016] The completed trajectory data is obtained according to the latitude and longitude completion data and the timestamp completion data.

[0017] In some embodiments, matching the trajectory data and the perception data based on a pre-constructed cost matrix to obtain an optimal mapping relationship includes:

[0018] Constructing a cost matrix according to the trajectory data and the perception data;

[0019] Based on the update iteration of the cost matrix, an optimal mapping relationship between the trajectory data and the perception data is determined.

[0020] In some embodiments, the determining the optimal mapping relationship between the trajectory data and the perception data based on the iterative update of the cost matrix includes:

[0021] Adjusting the rows and columns of the cost matrix to obtain a zero-element cost matrix;

[0022] Based on the zero elements in the zero-element cost matrix, the optimal match between the trajectory data and the trajectory points of the perception data is searched to obtain the corresponding optimal mapping relationship.

[0023] In some embodiments, the step of fusing the trajectory data and the perception data based on the optimal mapping relationship to obtain a vehicle trajectory includes:

[0024] Determine the initial state and initial covariance matrix;

[0025] For each synchronization time point, predict the predicted state and predicted covariance matrix at the next moment according to the preset state transfer equation, the initial state and the initial covariance matrix; the synchronization time point is determined by the optimal mapping relationship and the time step;

[0026] According to the observed value, the predicted state and the predicted covariance matrix are updated to obtain the vehicle trajectory; the observed value is obtained by the optimal mapping relationship, the trajectory data and the perception data.

[0027] In some embodiments, determining the initial state includes:

[0028] Determining the initial state according to the average value of the initial longitude and latitude corresponding to the trajectory data;

[0029] Alternatively, the initial state is determined according to an average value of the initial longitude and latitude corresponding to the perception data.

[0030] In a second aspect, a trajectory fusion processing device is provided in this embodiment, including: an acquisition module, a matching module and a fusion module;

[0031] The acquisition module is used to acquire the trajectory data of the vehicle-mounted device and the perception data of the roadside perception device; the trajectory data and the perception data both include corresponding longitude and latitude and timestamp; the vehicle-mounted device is set on the target vehicle, and the roadside perception device is set around the road;

[0032] The matching module is used to match the trajectory data and the perception data based on a pre-constructed cost matrix to obtain an optimal mapping relationship; the elements in the cost matrix are the matching costs between the trajectory data and the perception data;

[0033] The fusion module is used to fuse the trajectory data and the perception data based on the optimal mapping relationship to obtain a vehicle trajectory.

[0034] In a third aspect, a computer device is provided in this embodiment, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, the trajectory fusion processing method described in the first aspect is implemented.

[0035] In a fourth aspect, in this embodiment, a storage medium is provided, on which a computer program is stored, and when the program is executed by a processor, the processing method for trajectory fusion described in the first aspect is implemented.

[0036] Compared with the related art, the trajectory fusion processing method, device, computer equipment and storage medium provided in this embodiment obtain the trajectory data of the vehicle-mounted device and the perception data of the roadside perception device; the trajectory data and the perception data both include corresponding longitude and latitude and timestamp; the vehicle-mounted device is set on the target vehicle, and the roadside perception device is set around the road; based on the pre-constructed cost matrix, the trajectory data and the perception data are matched to obtain the optimal mapping relationship; based on the optimal mapping relationship, the trajectory data and the perception data are fused to obtain the vehicle trajectory, which solves the problem of the deviation of the two types of data on the time axis in the related art, affecting the accuracy of data fusion, and uses the optimal mapping relationship obtained based on the cost matrix matching to align the two types of data to improve the fusion accuracy of the two types of data.

[0037] Details of one or more embodiments of the present application are set forth in the following drawings and description to make other features, objects, and advantages of the present application more readily apparent. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] The drawings described herein are used to provide a further understanding of the present application and constitute a part of the present application. The illustrative embodiments of the present application and their descriptions are used to explain the present application and do not constitute an improper limitation on the present application. In the drawings:

[0039] Figure 1 It is a hardware structure block diagram of a terminal device of a trajectory fusion processing method provided in an embodiment of the present application;

[0040] Figure 2 is a flow chart of a trajectory fusion processing method provided in an embodiment of the present application;

[0041] Figure 3 is a flowchart of a trajectory fusion processing method provided by another embodiment of the present application;

[0042] Figure 4 is a flowchart of step S220;

[0043] Figure 5 It is a structural block diagram of a trajectory fusion processing device provided in one embodiment of the present application.

[0044] In the figure: 102, processor; 104, memory; 106, transmission device; 108, input and output device; 210, acquisition module; 220, matching module; 230, fusion module. DETAILED DESCRIPTION

[0045] In order to more clearly understand the purpose, technical solutions and advantages of the present application, the present application is described and illustrated below in conjunction with the accompanying drawings and embodiments.

[0046] Unless otherwise defined, the technical terms or scientific terms involved in this application shall have the general meaning understood by people with ordinary skills in the technical field to which this application belongs. The words "one", "a", "a", "the", "these" and the like in this application do not represent quantitative restrictions, and they can be singular or plural. The terms "include", "comprise", "have" and any variants thereof involved in this application are intended to cover non-exclusive inclusions; for example, a process, method and system, product or device comprising a series of steps or modules (units) is not limited to the listed steps or modules (units), but may include unlisted steps or modules (units), or may include other steps or modules (units) inherent to these processes, methods, products or devices. The words "connect", "connected", "coupled" and the like involved in this application are not limited to physical or mechanical connections, but may include electrical connections, whether directly or indirectly. The "multiple" involved in this application refers to two or more. "And / or" describes the association relationship of associated objects, indicating that there can be three relationships. For example, "A and / or B" can mean: A exists alone, A and B exist at the same time, and B exists alone. Usually, the character " / " indicates that the objects associated with each other are in an "or" relationship. The terms "first", "second", "third", etc. involved in this application are only used to distinguish similar objects and do not represent a specific ordering of the objects.

[0047] The method embodiment provided in this embodiment can be executed in a terminal, a computer or a similar computing device. For example, running on a terminal, Figure 1 FIG. 1 is a hardware structure block diagram of a terminal of the trajectory fusion processing method of this embodiment. Figure 1 As shown, the terminal may include one or more ( Figure 1 Only one is shown in the figure) processor 102 and memory 104 for storing data, wherein processor 102 may include but is not limited to a processing device such as a microprocessor MCU or a programmable logic device FPGA. The above terminal may also include a transmission device 106 and an input and output device 108 for communication functions. It can be understood by those skilled in the art that Figure 1 The structure shown is only for illustration and does not limit the structure of the above terminal. Figure 1 More or fewer components as shown, or with Figure 1 Different configurations shown.

[0048] The memory 104 can be used to store computer programs, for example, software programs and modules of application software, such as the computer program corresponding to the processing method of trajectory fusion in this embodiment. The processor 102 executes various functional applications and data processing by running the computer program stored in the memory 104, that is, to implement the above method. The memory 104 may include a high-speed random access memory, and may also include a non-volatile memory, such as one or more magnetic storage devices, flash memory, or other non-volatile solid-state memory. In some examples, the memory 104 may further include a memory remotely arranged relative to the processor 102, and these remote memories may be connected to the terminal via a network. Examples of the above-mentioned network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and a combination thereof.

[0049] The transmission device 106 is used to receive or send data via a network. The above network includes a wireless network provided by the communication provider of the terminal. In one example, the transmission device 106 includes a network adapter (Network Interface Controller, referred to as NIC), which can be connected to other network devices through a base station so as to communicate with the Internet. In one example, the transmission device 106 can be a radio frequency (Radio Frequency, referred to as RF) module, which is used to communicate with the Internet wirelessly.

[0050] In this embodiment, a trajectory fusion processing method is provided. Figure 2 is a flow chart of the processing method of trajectory fusion of this embodiment, such as Figure 2 As shown, the process includes the following steps:

[0051] Step S210, obtaining the trajectory data of the vehicle-mounted device and the perception data of the roadside perception device; the trajectory data and the perception data both include corresponding longitude and latitude and timestamps; the vehicle-mounted device is set on the target vehicle, and the roadside perception device is set around the road;

[0052] Step S220, matching the trajectory data and the perception data based on a pre-constructed cost matrix to obtain an optimal mapping relationship; the elements in the cost matrix are the matching costs between the trajectory data and the perception data;

[0053] Step S230: Based on the optimal mapping relationship, the trajectory data and the perception data are integrated to obtain the vehicle trajectory.

[0054] Specifically, the target vehicle refers to a vehicle to which the method embodiment is applicable. On-board equipment and roadside perception equipment are provided on the vehicle. Among them, the on-board equipment (On-Board Unit, OBU) can provide trajectory data such as the precise position and speed of the vehicle. That is, the trajectory data includes a first longitude and latitude and a first timestamp; the trajectory data may also include a first speed, etc., and there is no restriction on this. Among them, the roadside perception equipment (such as a road side unit (RSU), a camera, a lidar, etc.) is fixed around the road and can monitor road conditions, traffic flow and other environmental factors. That is, the perception data includes a second longitude and latitude and a second timestamp of a passing vehicle; the perception data also includes the speed, license plate, etc. of the vehicle, and there is no restriction on this. In other words, both the trajectory data and the perception data include a collection of trajectory points.

[0055] Time synchronization is crucial to the accuracy of trajectory fusion. If the recognition time of the same object by the on-board equipment and the roadside perception equipment is not synchronized, then the problem of time misalignment will occur during trajectory fusion. Even if the two recognize the same object in space, due to different timestamps, the data after trajectory fusion may not accurately reflect the actual position or state of the object. Therefore, in this embodiment, a pre-constructed cost matrix is ​​used to match the trajectory data and the perception data to reduce the difficulty of data alignment; this is because the elements in the cost matrix are the matching costs between the trajectory data and the perception data, so as long as the matching pair with the smallest matching cost can be found, the matching can be completed, and then the optimal mapping relationship can be obtained. The optimal mapping relationship is the mapping relationship of the optimal match between the trajectory points in the trajectory data and the trajectory points in the perception data. Therefore, the timestamps between the trajectory data and the perception data can be aligned through the optimal mapping relationship to solve the problem of time misalignment during the fusion process.

[0056] After obtaining the optimal mapping relationship, the time synchronization between the trajectory data and the perception data can be achieved through the optimal mapping relationship; then the trajectory data and the perception data are quickly fused at the time step to obtain the vehicle trajectory, so as to improve the accuracy of the fusion of the two types of data.

[0057] In the related art, the data acquired by the two types of devices are directly fused. However, due to the different computational complexity of the two devices in data collection and processing, the generated data often have inconsistencies in time, resulting in deviations between the two types of data on the time axis, which in turn affects the accuracy of data fusion. In this embodiment, the trajectory data of the vehicle-mounted device and the perception data of the roadside perception device are obtained; the trajectory data and the perception data both include corresponding longitude and latitude and timestamps; the vehicle-mounted device is set on the target vehicle, and the roadside perception device is set around the road; based on the pre-constructed cost matrix, the trajectory data and the perception data are matched to obtain the optimal mapping relationship; based on the optimal mapping relationship, the trajectory data and the perception data are fused to obtain the vehicle trajectory, which solves the problem of the deviation of the two types of data on the time axis in the related art, affecting the accuracy of data fusion, and uses the optimal mapping relationship obtained based on the cost matrix matching to align the two types of data to improve the accuracy of the fusion of the two types of data.

[0058] The following is a detailed description of each of the above steps:

[0059] In some of the embodiments, Figure 3 As shown, in Figure 2 On the basis of, the processing method of trajectory fusion also includes the following steps:

[0060] Step S240: based on the trajectory data, completing the missing trajectory data in the trajectory data to obtain completed trajectory data.

[0061] Specifically, steps S210 to S230 have been described and will not be repeated here. Due to factors such as the signal strength and stability of the vehicle-mounted device, the trajectory data of the vehicle-mounted device may be lost. The lost data here can be considered as trajectory missing data; trajectory missing data includes missing data of latitude and longitude and timestamp.

[0062] In this embodiment, relevant completion algorithms (such as interpolation completion algorithms, LSTM completion algorithms, etc.) or neural network models can be used to complete the missing data in the trajectory data based on the trajectory data before and after the missing data to obtain the completed trajectory data.

[0063] For example, when using the interpolation completion algorithm, latitude and longitude and timestamp are used for completion. Among them, it can be considered that the trajectory data is recorded in chronological order, usually including the following information: Longitude: lon i ; Latitude: lat i ; Timestamp: t i(Usually Unix timestamp in seconds or milliseconds.) If some track points (latitude, longitude or timestamp) in the track data are missing, these missing track points can be supplemented by interpolation algorithm.

[0064] Through this embodiment, the missing trajectory data is quickly completed to obtain the completed trajectory data, which reduces the impact of the missing trajectory points on trajectory matching and fusion, and reduces the difficulty of subsequent processing.

[0065] It should be noted that the roadside sensing equipment is fixedly installed around the road and generally operates stably, so the sensing data does not need to be completed. If it is found that the sensing data also needs to be completed, it can be completed in a similar way, which will not be repeated.

[0066] In some of the embodiments, based on the trajectory data, completing the missing trajectory data in the trajectory data to obtain the completed trajectory data includes the following steps:

[0067] According to the preset longitude and latitude interpolation formula and trajectory data, the longitude and latitude in the missing trajectory data are completed to obtain the longitude and latitude completion data;

[0068] According to the preset timestamp interpolation formula and trajectory data, the timestamps in the missing trajectory data are completed to obtain timestamp completion data;

[0069] According to the latitude and longitude completion data and timestamp completion data, the completed trajectory data is obtained.

[0070] Specifically, the longitude and latitude interpolation formula and the timestamp interpolation formula are pre-set. The longitude and latitude interpolation formula is used to calculate the missing longitude and latitude in the missing trajectory data to obtain the longitude and latitude completion data; the timestamp interpolation formula is used to calculate the missing timestamp in the missing trajectory data to obtain the timestamp completion data; then the longitude and latitude completion data and the timestamp completion data are combined to obtain the completed trajectory data.

[0071] Through this embodiment, the trajectory data can be quickly and accurately completed, thereby improving the stability of operation.

[0072] For example, two known trajectory points in the trajectory data are: point P1 (lon1, lat1, t1); point P2 (lon2, lat2, t2); wherein t2>t1, indicating that trajectory point P2 appears later in time than trajectory point P1.

[0073] If there are missing trajectory points (trajectory missing data) between these two trajectory points, these trajectory missing data can be supplemented based on the known points P1 and P2 by linear interpolation.

[0074] Among them, for any time t between the time interval [t1, t2] i , the corresponding interpolation trajectory point P i (lon i ,lat i , t i ) can be estimated by linear interpolation, lon i P i Longitude, lat i P i The dimension t i P i The timestamp of the latitude and longitude interpolation formula is:

[0075]

[0076] In the formula, It is the linear weight of time, which ensures that the completion points are distributed equally between the known points P1 and P2 on the time axis.

[0077] Among them, for the missing trajectory point P i Timestamp t i , can be calculated by the known time step (usually the time interval between known trajectory points). For example, if the transmission frequency of trajectory data and perception data is 10 Hz, that is, 10 trajectory points are sent per second, then the time interval between adjacent trajectory points is 0.1 second.

[0078] If n missing trajectory points need to be interpolated between t1 and t2, the time step can be defined as:

[0079] Then the expression of the timestamp interpolation formula is: i =t1+i·Δt where i=1, 2,...,n.

[0080] The main code execution process of the interpolation completion algorithm is given below:

[0081] Load trajectory data including timestamp, longitude and latitude; call interpolate_missing_points(data, frequency); output interpolated and completed trajectory data.

[0082] The execution logic of calling interpolate_missing_points(data, frequency) can be: first detect the time difference between adjacent timestamps. If the time difference is greater than the sampling frequency, it means that there are multiple consecutive missing points, which are calculated by the timestamp interpolation formula. For each missing point, calculate its longitude and latitude respectively by the longitude and latitude interpolation formula, and insert the interpolation point into the result. Finally, return the complete trajectory data after completion.

[0083] In some of the embodiments, Figure 4 As shown, the matching of the trajectory data and the perception data based on the pre-constructed cost matrix in step S220 to obtain the optimal mapping relationship includes the following steps:

[0084] Step S221, constructing a cost matrix according to the trajectory data and the perception data;

[0085] Step S222: determining the optimal mapping relationship between the trajectory data and the perception data based on the update iteration of the cost matrix.

[0086] In this embodiment, the Hungarian algorithm is used to match the trajectory data and the perception data based on the pre-constructed cost matrix to obtain the optimal mapping relationship. The trajectory data can be represented as O = {O1, O2, ..., O n}; The perception data can be expressed as R = {R1, R2, ..., R n};

[0087] For each trajectory point O i And each trajectory point R j , including longitude, latitude and timestamp. The matching goal is to find the optimal one-to-one mapping relationship between the trajectory point O in the trajectory data and the trajectory point R in the perception data, so that the time and space costs between all matching trajectory points are minimized.

[0088] Among them, the cost matrix can be represented by C; its elements can be represented by C ij Indicates that it is the trajectory point O of the trajectory data i and the trajectory point R in the perception data j The matching cost between them is related to the spatial distance and time difference. The expression of the elements of the cost matrix is:

[0089] C ij =α·d(O i , R j )+β·Δt ij ;

[0090] Where: d(O i , R j ) is the trajectory point O i With trajectory point R j The Euclidean distance between (i.e., the distance in space); Δt ij =|t i -t j | is the trajectory point O i With trajectory point R j α and β are the weight coefficients of spatial distance and time difference, which are used to balance the relative importance of time and space in the cost.

[0091] For the Euclidean distance d(O i , R j ), which measures the spatial difference between the trajectory points in the trajectory data and the trajectory points in the perception data.

[0092] Specifically, assume that the trajectory point O i The longitude and latitude of i ,lon i ), and the trajectory point R j The longitude and latitude of j ,lon j ), then the Euclidean distance can be approximately expressed as:

[0093]

[0094] Through the above process, the construction of the cost matrix can be realized, and then based on the update iteration of the cost matrix, the time and space costs between all matching trajectory points are minimized, so as to accurately determine the optimal mapping relationship between trajectory data and perception data.

[0095] In some embodiments, the updating iteration based on the cost matrix in step S222 to determine the optimal mapping relationship between the trajectory data and the perception data includes the following steps:

[0096] Adjust the rows and columns of the cost matrix to obtain a zero-element cost matrix;

[0097] Based on the zero elements in the zero-element cost matrix, the optimal match between the trajectory points of the trajectory data and the perception data is searched to obtain the corresponding optimal mapping relationship.

[0098] Specifically, the purpose of adjusting the rows and columns of the cost matrix is ​​to make at least one zero cost in each row and column of the cost matrix to obtain a zero-element cost matrix. The specific implementation is: for each row in the cost matrix, subtract the minimum value of the row so that each row has at least one zero element. Then perform the same operation on each column in the cost matrix to ensure that each column has at least one zero element. After the process of row and column subtraction, a zero-element cost matrix is ​​obtained.

[0099] After obtaining the zero-element cost matrix, the optimal match between the trajectory points of the trajectory data and the perception data is searched based on the zero elements in the zero-element cost matrix. The process of obtaining the corresponding optimal mapping relationship can be considered as: finding the match of the zero element; marking and adjusting the matrix; outputting the optimal match.

[0100] Among them, the matching of finding zero elements is: in the zero element cost matrix obtained after adjustment, find the only zero element to construct an initial match. This process can be considered as achieving preliminary matching by finding zero elements that do not overlap with other trajectory points.

[0101] Among them, the marking and adjustment matrix is: if in the adjusted cost matrix (zero element cost matrix), a unique zero element equal to the number of trajectory points is found (that is, each trajectory point in the trajectory data can be matched with a unique trajectory point in the perception data, and all perception data are the same), it is ensured that during the adjustment and marking process, each row and column in the cost matrix has a zero, indicating that the corresponding elements in the trajectory data and perception data are uniquely matched, which means that all trajectory points can be successfully matched. When the number of zero elements is equal to the number of trajectory points, it means that a complete match has been achieved. It can be considered that: when the trajectory data and perception data each have N trajectory points, through the process of adjusting the cost matrix, N pairs of zero elements corresponding to unique positions are found. In this case, when all trajectory points of the trajectory data have matching trajectory points of the perception data, it represents an optimal match. After the matching is completed, the optimal match can be output and the algorithm ends early.

[0102] Otherwise, mark the unmatched rows and find the minimum element that covers these rows and columns. Find the minimum uncovered element (the goal is to find the minimum element value at the intersection of the marked row and the unmarked column, that is, the uncovered minimum value), and adjust the cost matrix based on the minimum element (that is, subtract the minimum value from the unmarked row and add the minimum value to the marked column). This step will generate new zero elements, obtain a new zero element cost matrix, and repeat the matching process until a complete match is found.

[0103] Among them, the optimal match is output as follows: by continuously updating the cost matrix, the optimal match between the trajectory points of the trajectory data and the trajectory points of the perception data is finally found, so that the sum of the costs of all matching trajectory points is minimized, and the corresponding optimal mapping relationship is output.

[0104] Through this embodiment, the Hungarian algorithm is used to solve the data association problem, reduce the difficulty of data alignment, and ensure the data alignment effect.

[0105] Example:

[0106] 1. Construct the cost matrix:

[0107] The 3x3 cost matrix is ​​expressed as:

[0108] |4 1 3|

[0109] |2 0 5|

[0110] |3 2 2|.

[0111] 2. Row and column subtraction:

[0112] Row subtraction (minus the minimum value of each row) yields:

[0113] |3 0 2|(4-1, 1-1, 3-1)

[0114] |2 0 5|(2-0, 0-0, 5-0)

[0115] |1 0 0|(3-2, 2-2, 2-2).

[0116] Column subtraction (subtracting the minimum value of each column) yields:

[0117] |2 0 2|(3-1, 0-0, 2-0)

[0118] |1 0 5|(2-1, 0-0, 5-0)

[0119] |0 0 0|(1-1, 0-0, 0-0).

[0120] 3. Find the match of zero element:

[0121] Find the match from the adjusted cost matrix (zero element cost matrix):

[0122] First row: In the first row, only column 2 has zeros. Second row: In the second row, only column 2 has zeros (this is a conflict with the first row, because the same column cannot be matched with multiple rows, and needs to be optimized). Third row: In the third row, because there are two zeros in columns 1 and 2, you can choose to match. Since there will be uncovered after matching at this time, rows 1 and 2 are kept ready for selection and adjustment.

[0123] 4. Mark and adjust the matrix:

[0124] The marking process is as follows:

[0125] Mark unmatched rows: Mark the first and second rows that do not participate in any zeros in the match.

[0126] Marking step: mark column 2 with zero elements in row 1; since the zero elements in row 2 have never been covered, continue to the second step; for column 2, since it is connected to row 3 zero, also mark row 3; if no new markable elements are found, the execution is completed.

[0127] The adjustment process is as follows:

[0128] Find the minimum non-covered element: Find the minimum element at the intersection of unlabeled rows and unlabeled columns. From the current cost matrix, find the minimum non-zero element in the uncovered area, which is 1.

[0129] Adjust the cost matrix: subtract the minimum value (1) from each element in the unmarked row; add the minimum value (1) to the marked column (such as column 2); then the adjusted cost matrix becomes:

[0130] |1 0 2|

[0131] |0 0 5|

[0132] |0 1 1|.

[0133] Pairing is done by selecting only one zero at a time in each row and column, and no other zeros in the same column or row can be selected.

[0134] First row: Select the first zero in column 2, pairing: Row1—>Column2. Second row: Find the zero in the first column, pairing: Row2—>Column1. Third row: The first and second columns of the third row are already occupied, the only option is to find its match in column 3, pairing: Row3—>Column3.

[0135] After this matching step, the complete matching solution is: the first row matches the second column; the second row matches the first column; the third row matches the third column; thus, the optimal match is obtained to determine the corresponding optimal mapping relationship.

[0136] In some embodiments, the step S230 of fusing the trajectory data and the perception data based on the optimal mapping relationship to obtain the vehicle trajectory includes the following steps:

[0137] Determine the initial state and initial covariance matrix;

[0138] For each synchronization time point, the predicted state and predicted covariance matrix of the next moment are predicted according to the preset state transfer equation, initial state and initial covariance matrix; the synchronization time point is determined by the optimal mapping relationship and time step;

[0139] According to the observation value, the predicted state and the predicted covariance matrix are updated to obtain the vehicle trajectory; the observation value is obtained from the trajectory data and the perception data.

[0140] Specifically, in this embodiment, it can be considered that it is implemented based on Kalman filtering, and the trajectory fusion is converted into a linear way for fusion; that is, the predicted target vehicle is taken as a system, and the state transition model and observation model of the state space model in the system are both linear. If the noise of the system is Gaussian white noise and the system model can be accurately described, then the Kalman filter is a standard Kalman filter.

[0141] Furthermore, the above process can be considered as follows: first obtain the timestamp and longitude and latitude of the trajectory data and perception data; and, the sampling frequency per second is 10Hz, that is, one data point every 0.1 seconds, so the time step is 0.1; the synchronization time point is each time point in the trajectory data and perception data that needs to be fused, and the synchronization time point of the next moment is predicted from the current synchronization time point. Then, through the standard Kalman filter, the time step is determined based on the optimal mapping relationship, and at each time step, the longitude and latitude of the trajectory data and perception data are fused to obtain the vehicle trajectory, thereby simplifying the calculation process while improving the accuracy of trajectory fusion.

[0142] Among them, the state space model of the above system can be defined as:

[0143] 1. State variable X k The expression for is (defining the vehicle's state as 2D position and velocity):

[0144]

[0145] Among them, x k and k are the latitude and longitude coordinates of the vehicle at time k, and is the speed of the vehicle in the east-west and north-south directions.

[0146] 2. The expression of the state transfer model is (assuming that the law of the system's state change at each time step can be expressed as a uniform motion model): X k+1 =FX k +w k ;

[0147] in,

[0148] F is the state transfer matrix, which represents the equation for updating the state over time, Δt is the time step (in this embodiment, Δt = 0.1 seconds), w k is the process noise.

[0149] 3. The observation model is expressed as follows (the observation data of the vehicle is the longitude and latitude position directly measured, that is, the longitude and latitude in the trajectory data and perception data): Z k =HX k +v k ;

[0150] in,

[0151] H represents the observation matrix (only the observation position x k and k ), and v k is the measurement noise and can usually be ignored.

[0152] Based on the above definition, the steps to implement Kalman filtering are:

[0153] 1. Determine the initial conditions (initial state and initial covariance matrix):

[0154] The initial state X0 can be obtained by averaging the initial longitude and latitude of the trajectory data or the perception data, or estimated based on the system information. The initial covariance matrix P0 needs to be set to a larger value (for example, assuming that the uncertainty of the initial state is large).

[0155] 2. Prediction step (for each synchronization time point, according to the preset state transfer equation, initial state and initial covariance matrix, predict the predicted state and predicted covariance matrix at the next moment):

[0156] According to the state transfer equation, the state (predicted state) and covariance matrix (predicted covariance matrix) of the next moment are predicted, which are expressed as:

[0157]

[0158] P k+1|k =FP k|k F T +Q;

[0159] in, is the predicted state, P k+1|k is the prediction covariance matrix, and Q is the process noise covariance matrix.

[0160] 3. Update step (update the predicted state and predicted covariance matrix according to the observed value to obtain the vehicle trajectory):

[0161] Update the state estimate based on the observations:

[0162] K k =P k|k-1 H T (HP k|k-1 H T +R) -1 ;

[0163]

[0164] P k|k =(IK k H)P k|k-1 ;

[0165] Among them, K k is the Kalman gain, R is the observation noise covariance matrix, and I is the identity matrix; the set of all predicted states The vehicle trajectory can be obtained.

[0166] Example:

[0167] 1) If the predicted state of k-1 at the current moment is:

[0168]

[0169] Among them, the longitude and latitude are (34.0522, -118.2437), the latitude speed is 0.001 (unit: degree / second), and the longitude speed is 0.002 (unit: degree / second).

[0170] If the time interval Δt = 1 second, then the state transition matrix F is:

[0171]

[0172] 2) Forecast status prediction:

[0173] The predicted state at the next moment (k) is predicted by the above formula:

[0174] Substituting F into matrix multiplication, we get:

[0175]

[0176] Then the calculation result is:

[0177]

[0178] 3) Prediction covariance matrix prediction:

[0179] Assume that the covariance matrix P of the current time k-1 is k-1|k-1 for:

[0180]

[0181] The diagonal elements of the covariance matrix represent the uncertainty in position and velocity.

[0182] Let the process noise covariance matrix be:

[0183]

[0184] The prediction covariance matrix at time k is calculated by the above formula:

[0185] P k|k-1 =FP k-1|k-1 F T +Q;

[0186] First, calculate FP k-1|k-1 F T for:

[0187]

[0188] Substituting F into matrix multiplication, we get:

[0189]

[0190] Then the calculation result is:

[0191]

[0192] Multiply by F T ,get:

[0193]

[0194] The final calculation is:

[0195]

[0196] Now add this result to the process noise Q and we get:

[0197] Finally, we get:

[0198]

[0199] 4) Update steps:

[0200] The predicted state is estimated to be:

[0201] The observed values ​​(collected longitude and latitude) are: The state transfer matrix H (assuming that only longitude and latitude are measured, not speed) is:

[0202] The prediction covariance matrix is: The observation noise covariance matrix is: Calculate the gain K k ; K k =P k|k-1 H T (HP k|k-1 H T +R) -1 ;

[0203] Calculate HP k|k-1 H T :

[0204]

[0205] Then, calculate HP k|k-1 H T +R:

[0206]

[0207] Next, take its inverse and we get:

[0208]

[0209] The calculation results are:

[0210]

[0211] Then calculate the gain: K k =P k|k-1 H T (HP k|k-1 H T +R) -1 ;

[0212] Seeking H T :

[0213]

[0214] Therefore, we can get:

[0215]

[0216] Update the state estimate:

[0217] Use the observed value to update the state:

[0218] Compute the observation residuals:

[0219]

[0220] Substituting this into the update formula:

[0221]

[0222] By calculating K k , and then substitute it to get the updated state estimate.

[0223] Update the covariance matrix: P k|k =(IK k H)P k|k-1 ;

[0224] Construct the identity matrix I; if the state vector is four-dimensional (for example, it contains latitude, longitude, latitude speed, and longitude speed), the identity matrix I is:

[0225]

[0226] Calculating IK k H, and assuming that:

[0227]

[0228] Substituting H into the matrix, we get:

[0229]

[0230] Then calculate IK k H:

[0231]

[0232] Calculate P k|k : Multiplying this matrix with the prediction covariance matrix yields:

[0233] P k|k =(IK k H)P k|k-1 ;

[0234]

[0235] The result of this multiplication will produce a new covariance matrix P k|k , and reduce the original uncertainty.

[0236] The following is an explanation from the perspective of code implementation:

[0237] Process KalmanFilterFusion(Zk_OBU, Zk_RSU, F, H, Q, R): pending state

[0238] The estimated covariance P=P0. (derived from initial perception data or trajectory data);

[0239] P0 = initial covariance matrix estimate.

[0240] For each synchronization time point k:

[0241] #Prediction step

[0242] (F is the state transfer matrix (assumed to be the identity matrix))

[0243] P prediction = F × P × F transpose + Q (Q is the process noise covariance matrix)

[0244] #Update step (weighted fusion of OBU and RSU data)

[0245] Zk_weight = 0.8 × Zk_OBU + 0.2 × Zk_RSU (Zk_OBU is trajectory data;

[0246] Zk_RSU: is the perception data; the weight of trajectory data is 0.8, and the weight of perception data is 0.2)

[0247] (H is the observation matrix (usually the identity matrix))

[0248] S = H × P prediction × HT + R (R represents the observation noise covariance matrix)

[0249] K = P prediction × HT × S inverse matrix

[0250]

[0251] P = (IK × H) × P prediction

[0252] Output P.

[0253] In some of the embodiments, determining the initial state comprises the following steps:

[0254] Determine the initial state according to the average value of the initial longitude and latitude corresponding to the trajectory data;

[0255] Alternatively, the initial state is determined according to the average value of the initial longitude and latitude corresponding to the perception data.

[0256] Specifically, the initial state can be achieved in a variety of ways. In this embodiment, in order to improve the accuracy of the initial state, the average value of the initial longitude and latitude corresponding to the trajectory data or the perception data is used as the initial state. In other embodiments, the initial state can also be determined by other data provided by the vehicle system (such as GPS positioning data), which is not limited to this.

[0257] The following is a description and explanation of the implementation of the overall code:

[0258] The matching of trajectory data and perception data through the Hungarian algorithm is:

[0259] Function match_trajectories(OBU_data, roadside_data): Input: OBU_data (trajectory data), roadside_data (perception data).

[0260] Get valid data index valid_OBU_indices in OBU_data (trajectory data); initialize the cost matrix cost_matrix; for each valid OBU data obu_idx: get the corresponding OBU data point obu_point. For each perception data roadside_point: calculate the Haversine distance between obu_point and roadside_point to fill the cost matrix.

[0261] The optimal matching using the Hungarian algorithm linear_sum_assignment is as follows: convert the matched OBU index row_ind back to the index of the original OBU data; return row_ind, col_ind (matching result).

[0262] The trajectory fusion based on Kalman filtering is:

[0263] Function fuse_trajectories(OBU_data, roadside_data): Input: OBU_data (trajectory data), roadside_data (perception data); call match_trajectories function for data matching.

[0264] Initialize the fused trajectory fused_trajectory; take the average of the trajectory data and the perception data as the fusion result, and add the fusion result to the fused trajectory fused_trajectory; call apply_kalman_filter to smooth the fused trajectory and return fused_trajectory, smoothed_trajectory.

[0265] The trajectory smoothing of Kalman filter is: function apply_kalman_filter(trajectory): input: trajectory (two-dimensional trajectory data). Initialize the Kalman filter, the initial state is the first point of the trajectory; call the smooth function to smooth the trajectory; return the smoothed state smoothed_state_means.

[0266] Through this embodiment, a method combining Hungarian matching and Kalman filtering is implemented for trajectory fusion of trajectory data and perception data. The Hungarian algorithm is first used to solve the data association problem, and then the Kalman filter is used to predict and update the trajectory, thereby improving the timeliness and accuracy of trajectory fusion.

[0267] It should be noted that the steps shown in the above process or the flowchart in the accompanying drawings can be executed in a computer system such as a set of computer executable instructions, and although a logical order is shown in the flowchart, in some cases, the steps shown or described can be executed in an order different from that shown here.

[0268] In this embodiment, a processing device for trajectory fusion is also provided, which is used to implement the above embodiments and preferred implementation modes, and will not be repeated hereafter. The terms "module", "unit", "subunit", etc. used below can implement a combination of software and / or hardware for a predetermined function. Although the devices described in the following embodiments are preferably implemented in software, the implementation of hardware, or a combination of software and hardware, is also possible and conceivable.

[0269] Figure 5 is a structural block diagram of the trajectory fusion processing device of this embodiment, such as Figure 5 As shown, the device includes: an acquisition module 210, a matching module 220 and a fusion module 230;

[0270] The acquisition module 210 is used to acquire the trajectory data of the vehicle-mounted device and the perception data of the roadside perception device; the trajectory data and the perception data both include corresponding longitude and latitude and timestamps; the vehicle-mounted device is set on the target vehicle, and the roadside perception device is set around the road;

[0271] A matching module 220 is used to match the trajectory data and the perception data based on a pre-constructed cost matrix to obtain an optimal mapping relationship; the elements in the cost matrix are the matching costs between the trajectory data and the perception data;

[0272] The fusion module 230 is used to fuse the trajectory data and the perception data based on the optimal mapping relationship to obtain the vehicle trajectory.

[0273] Through the above device, the problem of deviation between the two types of data on the time axis in the related technology, which affects the accuracy of data fusion, is solved. The optimal mapping relationship obtained based on cost matrix matching is used to align the two types of data to improve the accuracy of the fusion of the two types of data.

[0274] In some of the embodiments, the trajectory fusion processing device further includes a completion module;

[0275] The completion module is used to complete the missing trajectory data in the trajectory data based on the trajectory data to obtain the completed trajectory data.

[0276] In some of the embodiments, the completion module is further used to complete the longitude and latitude in the missing trajectory data according to a preset longitude and latitude interpolation formula and the trajectory data to obtain longitude and latitude completion data;

[0277] Completing the timestamp in the missing trajectory data according to a preset timestamp interpolation formula and the trajectory data to obtain timestamp-completing data;

[0278] The completed trajectory data is obtained according to the latitude and longitude completion data and the timestamp completion data.

[0279] In some of the embodiments, the matching module 220 is further configured to construct a cost matrix according to the trajectory data and the perception data;

[0280] Based on the update iteration of the cost matrix, an optimal mapping relationship between the trajectory data and the perception data is determined.

[0281] In some of the embodiments, the matching module 220 is further used to adjust the rows and columns of the cost matrix to obtain a zero-element cost matrix;

[0282] Based on the zero elements in the zero-element cost matrix, the optimal match between the trajectory data and the trajectory points of the perception data is searched to obtain the corresponding optimal mapping relationship.

[0283] In some of the embodiments, the fusion module 230 is further used to determine an initial state and an initial covariance matrix;

[0284] For each synchronization time point, predict the predicted state and predicted covariance matrix at the next moment according to the preset state transfer equation, the initial state and the initial covariance matrix; the synchronization time point is determined by the optimal mapping relationship and the time step;

[0285] According to the observed value, the predicted state and the predicted covariance matrix are updated to obtain the vehicle trajectory; the observed value is obtained by the optimal mapping relationship, the trajectory data and the perception data.

[0286] In some of the embodiments, the fusion module 230 is further used to determine the initial state according to the average value of the initial longitude and latitude corresponding to the trajectory data;

[0287] Alternatively, the initial state is determined according to an average value of the initial longitude and latitude corresponding to the perception data.

[0288] It should be noted that the above modules can be functional modules or program modules, and can be implemented by software or hardware. For modules implemented by hardware, the above modules can be located in the same processor; or the above modules can be located in different processors in any combination.

[0289] In this embodiment, a computer device is further provided, including a memory and a processor, wherein a computer program is stored in the memory, and the processor is configured to run the computer program to execute the steps in any one of the above method embodiments.

[0290] Optionally, the computer device may further include a transmission device and an input / output device, wherein the transmission device is connected to the processor, and the input / output device is connected to the processor.

[0291] Optionally, in this embodiment, the processor may be configured to perform the following steps through a computer program:

[0292] S1, obtaining the trajectory data of the vehicle-mounted device and the perception data of the roadside perception device; the trajectory data and the perception data both include the corresponding longitude and latitude and timestamp; the vehicle-mounted device is set on the target vehicle, and the roadside perception device is set around the road;

[0293] S2, based on the pre-constructed cost matrix, the trajectory data and the perception data are matched to obtain the optimal mapping relationship; the elements in the cost matrix are the matching costs between the trajectory data and the perception data;

[0294] S3, based on the optimal mapping relationship, the trajectory data and perception data are integrated to obtain the vehicle trajectory.

[0295] It should be noted that the specific examples in this embodiment can refer to the examples described in the above embodiments and optional implementation modes, and will not be repeated in this embodiment.

[0296] In addition, in combination with the processing method for trajectory fusion provided in the above embodiments, a storage medium may be provided in this embodiment to implement the processing method. The storage medium stores a computer program; when the computer program is executed by a processor, any one of the processing methods for trajectory fusion in the above embodiments is implemented.

[0297] It should be noted that the information and data involved in this application are all authorized by the user or fully authorized by all parties and will be used legally.

[0298] It should be understood that the specific embodiments described herein are only used to explain the application, rather than to limit it. Based on the embodiments provided in this application, all other embodiments obtained by ordinary technicians in this field without creative work are within the protection scope of this application.

[0299] Obviously, the drawings are only some examples or embodiments of the present application. For ordinary technicians in the field, the present application can also be applied to other similar situations based on these drawings without creative work. In addition, it is understandable that although the work done in this development process may be complicated and lengthy, for ordinary technicians in the field, certain changes in design, manufacturing or production based on the technical content disclosed in this application are only conventional technical means and should not be regarded as insufficient content disclosed in this application.

[0300] The term "embodiment" in this application refers to a specific feature, structure or characteristic described in conjunction with the embodiment that can be included in at least one embodiment of the present application. The appearance of this phrase in various locations in the specification does not necessarily mean the same embodiment, nor does it mean that it is mutually exclusive with other embodiments and is independent or optional. It is clearly or implicitly understood by those of ordinary skill in the art that the embodiments described in this application can be combined with other embodiments without conflict.

[0301] The above-mentioned embodiments only express several implementation methods of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of patent protection. It should be pointed out that, for a person of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present application, and these all belong to the scope of protection of the present application. Therefore, the scope of protection of the present application shall be subject to the attached claims.

Claims

1. A trajectory fusion processing method, characterized in that: include: Acquire trajectory data of the vehicle-mounted device and perception data of the roadside perception device; the trajectory data and the perception data both include corresponding longitude and latitude and timestamp; The vehicle-mounted device is arranged on the target vehicle, and the roadside sensing device is arranged around the road; Based on a pre-constructed cost matrix, the trajectory data and the perception data are matched to obtain an optimal mapping relationship; the elements in the cost matrix are matching costs between the trajectory data and the perception data; Based on the optimal mapping relationship, the trajectory data and the perception data are fused to obtain a vehicle trajectory.

2. The trajectory fusion processing method according to claim 1, characterized in that: The method further comprises: Based on the trajectory data, the missing trajectory data in the trajectory data is completed to obtain the completed trajectory data.

3. The trajectory fusion processing method according to claim 2, characterized in that: The method of completing missing trajectory data in the trajectory data based on the trajectory data to obtain completed trajectory data includes: According to a preset longitude and latitude interpolation formula and the trajectory data, the longitude and latitude in the missing trajectory data are supplemented to obtain longitude and latitude supplemented data; Completing the timestamp in the missing trajectory data according to a preset timestamp interpolation formula and the trajectory data to obtain timestamp-completing data; The completed trajectory data is obtained according to the latitude and longitude completion data and the timestamp completion data.

4. The trajectory fusion processing method according to any one of claims 1 to 3, characterized in that: The matching of the trajectory data and the perception data based on the pre-constructed cost matrix to obtain an optimal mapping relationship includes: Constructing a cost matrix according to the trajectory data and the perception data; Based on the update iteration of the cost matrix, an optimal mapping relationship between the trajectory data and the perception data is determined.

5. The trajectory fusion processing method according to claim 4 is characterized in that: The updating iteration based on the cost matrix determines the optimal mapping relationship between the trajectory data and the perception data, including: Adjusting the rows and columns of the cost matrix to obtain a zero-element cost matrix; Based on the zero elements in the zero-element cost matrix, the optimal match between the trajectory data and the trajectory points of the perception data is searched to obtain the corresponding optimal mapping relationship.

6. The trajectory fusion processing method according to claim 4, characterized in that: The step of fusing the trajectory data and the perception data based on the optimal mapping relationship to obtain a vehicle trajectory includes: Determine the initial state and initial covariance matrix; For each synchronization time point, predict the predicted state and predicted covariance matrix at the next moment according to the preset state transfer equation, the initial state and the initial covariance matrix; the synchronization time point is determined by the optimal mapping relationship and the time step; According to the observed value, the predicted state and the predicted covariance matrix are updated to obtain the vehicle trajectory; the observed value is obtained by the optimal mapping relationship, the trajectory data and the perception data.

7. The trajectory fusion processing method according to claim 6, characterized in that: The determining of the initial state comprises: Determining the initial state according to the average value of the initial longitude and latitude corresponding to the trajectory data; Alternatively, the initial state is determined according to an average value of the initial longitude and latitude corresponding to the perception data.

8. A trajectory fusion processing device, characterized in that: include: Acquisition module, matching module and fusion module; The acquisition module is used to acquire the trajectory data of the vehicle-mounted device and the perception data of the roadside perception device; the trajectory data and the perception data both include corresponding longitude and latitude and timestamp; the vehicle-mounted device is set on the target vehicle, and the roadside perception device is set around the road; The matching module is used to match the trajectory data and the perception data based on a pre-constructed cost matrix to obtain an optimal mapping relationship; The elements in the cost matrix are the matching costs between the trajectory data and the perception data; The fusion module is used to fuse the trajectory data and the perception data based on the optimal mapping relationship to obtain a vehicle trajectory.

9. A computer device comprising a memory and a processor, characterized in that: A computer program is stored in the memory, and the processor is configured to run the computer program to execute the steps of the trajectory fusion processing method according to any one of claims 1 to 7.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the trajectory fusion processing method according to any one of claims 1 to 7 are implemented.

Images