Multi-sensor target fusion method and system based on sequential filtering

Abstract

The invention provides a multi-sensor target fusion method and system based on sequential filtering, and belongs to the technical field of intelligent driving, and the method comprises the steps: obtaining a fusion track of a plurality of target sensors, and calculating a track state at a current moment according to a track state at a previous moment; adopting a greedy algorithm to respectively match the plurality of target sensors with the current time track; and updating the track state by adopting sequential filtering according to the matched measurement value. According to the method, the greedy algorithm is adopted for matching, the time complexity is far smaller than that of a KM algorithm and the like, but an approximate global optimal matching result can be achieved in most cases, and the calculation amount can be reduced to a great extent. According to the method, sequential filtering is adopted to update the flight path state, the method serves as a recursive filtering algorithm, measurement data are comprehensively considered, meanwhile, the method is easy to implement, optimal estimation of the system state can be provided based on the minimum mean square error criterion, and in addition, filtering parameters and measurement weight values can be adjusted according to the actual situation.

Description

Technical Field

[0001] This invention relates to the field of intelligent driving technology, and in particular to a multi-sensor target fusion method and system based on sequential filtering. Background Technology

[0002] In recent years, intelligent driving has been developing rapidly, and the perception of the vehicle's surrounding environment, as the foundation of intelligent driving, has become particularly important.

[0003] Currently, to achieve comprehensive perception of the surrounding environment, vehicles are generally equipped with multiple sensors such as radar and cameras. Multi-sensor target fusion is an important component of intelligent driving. Multi-sensor target fusion mainly includes spatiotemporal synchronization, matching algorithms, state estimation, and trajectory management.

[0004] However, existing technologies focus on optimizing matching algorithms and rarely involve optimizing state estimation, which is an important factor in whether the final fused output track state is stable. Summary of the Invention

[0005] This invention aims to solve at least one of the above-mentioned technical problems in the prior art, and proposes a multi-sensor target fusion technology scheme based on sequential filtering. This scheme combines Kalman filtering and sequential filtering to optimize state estimation, ensuring that the final output state is stable and reliable when multiple sensor targets are associated with the track, thus ensuring the stability of intelligent driving.

[0006] In a first aspect, embodiments of the present invention provide a multi-sensor target fusion method based on sequential filtering, comprising:

[0007] Acquire the fused trajectories from multiple target sensors, and calculate the current trajectory state based on the trajectory state of the previous moment;

[0008] A greedy algorithm is used to match multiple target sensors with their current trajectories.

[0009] The track status is updated using sequential filtering based on the matched measurements.

[0010] In a preferred embodiment, before the step of acquiring the fused trajectory of multiple target sensors and calculating the trajectory state at the current moment based on the trajectory state at the previous moment, the method further includes:

[0011] Measurement preprocessing of multiple target sensors;

[0012] The measurements from multiple target sensors are synchronized in time and space.

[0013] In a preferred embodiment, the step of acquiring the fused trajectory of multiple target sensors and calculating the trajectory state at the current moment based on the trajectory state at the previous moment includes:

[0014] Based on the state estimation results of the trajectory after sequential filtering at time k-1 and the estimated error covariance matrix Calculate the predicted value at time k. and prediction error covariance matrix as follows:

[0015]

[0016]

[0017] in, It is the state transition matrix at time k-1. It is the control input weight matrix at time k-1. It is the process noise covariance matrix at time k-1.

[0018] In a preferred embodiment, the step of matching multiple target sensors with the current trajectory using a greedy algorithm includes:

[0019] Initialize the distance matrix AxB, where A and B are the number of tracks and the number of measurements from the first target sensor, respectively.

[0020] Use an initial threshold to exclude tracks and measurements that cannot be matched;

[0021] Calculate the distances between various measurements and tracks using Mahalanobis distance, and fill them into the corresponding positions in the matrix;

[0022] Find the point with the smallest distance value in the distance matrix as a set of optimal matches and delete the corresponding row and column in the distance matrix until the distance matrix is ​​empty;

[0023] Repeat the above steps to complete the matching of all target sensors.

[0024] In a preferred embodiment, in the step of using an initial threshold to exclude impossible-to-match tracks and measurements:

[0025] Set the dynamic initial threshold (gate) using the following formula:

[0026]

[0027] in, The distance between the track and the vehicle. , It is a custom coefficient, with a different threshold value set for each of the four dimensions: vertical distance px, horizontal distance py, vertical velocity vx, and horizontal velocity vy.

[0028] Calculate the longitudinal distance difference, lateral distance difference, longitudinal velocity difference, and lateral velocity difference between each measurement and each track. If any of them is greater than the threshold, use the maximum value as the distance between the two target points and fill it into the corresponding position in the distance matrix.

[0029] The step of calculating the distances between various measurements and tracks using Mahalanobis distance and filling them into the corresponding positions in the matrix includes:

[0030] Iterate through each track and sensor measurement, calculate the Mahalanobis distance between them, and fill it into the distance matrix. The Mahalanobis distance calculation formula is as follows:

[0031]

[0032] in, It is a four-dimensional row vector, including the longitudinal distance difference, lateral distance difference, longitudinal velocity difference, and lateral velocity difference between the track target and the sensor-measured target. It is the covariance matrix.

[0033] In a preferred embodiment, the step of updating the track state using sequential filtering based on the matched measurement values ​​includes:

[0034] The measurements of the first target sensor are updated according to the following equation. For the fused state estimate:

[0035] ,

[0036] in, It is the measurement matrix of the first target sensor at time k;

[0037] The gain matrix of the first target sensor at time k The calculation is as follows:

[0038] ,

[0039] in, It is the measurement noise covariance matrix of the first target sensor at time k;

[0040] State estimation error covariance matrix The inverse calculation is as follows:

[0041] ,

[0042] If N target sensors and tracks match, then target sensor i, The state estimation update for the fusion result is as follows:

[0043]

[0044]

[0045]

[0046] in, The measured value of the target sensor i. Let i be the measurement matrix of the target sensor. This is the state estimate after being updated by the (i-1)th target sensor. Let be the state estimation error covariance matrix after being updated by the (i-1)th target sensor. Let be the measurement noise covariance matrix of the target sensor i;

[0047] After traversing N target sensors, the obtained state estimates and their error covariance matrices are as follows:

[0048] ,

[0049] .

[0050] In a preferred embodiment, it further includes:

[0051] New track generation steps: When multiple target sensor measurements fail to match a track, the measurement from the second target sensor that failed to match a track is set as a row of the new track in the distance matrix, and the measurement from the third target sensor that failed to match a track is set as a column of the distance matrix. A greedy algorithm is then used for matching. Measurements from the third target sensor that failed to match the second target sensor are added to the new track as rows of the distance matrix. Then, measurements from the third target sensor that failed to match a track are set as columns of the distance matrix. A greedy algorithm is then used for matching. This process continues until the last target sensor measurement is matched. After matching, if the probability of generating a new track is greater than a preset probability threshold, the new track is generated.

[0052] Track management steps: The newly generated track and the previously existing track are judged: if the similarity between two tracks is higher than the preset similarity threshold, they are merged into one track and output; if a track has not matched any measurement within the recent preset time period, the track is deleted.

[0053] Output track steps: Output the track after track management.

[0054] In a second aspect, embodiments of the present invention provide a multi-sensor target fusion system based on sequential filtering, the system being configured to implement any of the methods described in the first aspect, the system comprising:

[0055] The state prediction module is used to acquire the trajectories fused from multiple target sensors and calculate the current trajectory state based on the trajectory state of the previous moment.

[0056] The greedy algorithm matching module is used to match multiple target sensors with their current trajectories using a greedy algorithm.

[0057] The sequential filtering module is used to update the track status based on the matched measurement values ​​using sequential filtering.

[0058] Thirdly, embodiments of the present invention provide an electronic device comprising: one or more processors; a memory for storing one or more programs; and, when the one or more programs are executed by the one or more processors, causing the one or more processors to implement the method as described in any embodiment of the first aspect.

[0059] Fourthly, embodiments of the present invention provide a computer-readable medium storing a computer program that, when executed by a processor, implements the steps of the method as described in any embodiment of the first aspect.

[0060] Beneficial effects of this invention:

[0061] This invention uses a greedy matching algorithm, which has a much lower time complexity than the KM algorithm (or Hungarian algorithm), but can achieve an approximate global optimal matching result in most cases, thus greatly reducing the amount of computation.

[0062] This invention uses sequential filtering to update the track state. As a recursive filtering algorithm, it is easy to implement while taking into account all measurement data. Based on the minimum mean square error criterion, it can provide the optimal estimate of the system state. In addition, the filtering parameters and the weight values ​​of each measurement can be adjusted according to the actual situation. Attached Figure Description

[0063] Figure 1 This is a flowchart illustrating a multi-sensor target fusion method based on sequential filtering, provided in an embodiment of the present invention.

[0064] Figure 2 This is a schematic diagram of the overall process of a multi-sensor target fusion method based on sequential filtering, provided in an embodiment of the present invention.

[0065] Figure 3 This is a flowchart illustrating an optional specific implementation method for the preprocessing and spatiotemporal synchronization steps in an embodiment of the present invention.

[0066] Figure 4 This is a flowchart illustrating an optional specific implementation method of step S2 in an embodiment of the present invention.

[0067] Figure 5 This is a structural block diagram of an electronic device provided in an embodiment of the present invention. Detailed Implementation

[0068] To enable those skilled in the art to better understand the technical solutions of the present invention, exemplary embodiments of the present invention are described below in conjunction with the accompanying drawings, including various details of the embodiments of the present invention to aid understanding. These should be considered merely exemplary. Therefore, those skilled in the art should recognize that various changes and modifications can be made to the embodiments described herein without departing from the scope and spirit of the present invention. Similarly, for clarity and brevity, descriptions of well-known functions and structures are omitted in the following description.

[0069] Where there is no conflict, the various embodiments of the present invention and the features thereof may be combined with each other.

[0070] As used herein, the term “and / or” includes any and all combinations of one or more related enumerated entries.

[0071] The terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the invention. As used herein, the singular forms “a” and “the” are intended to include the plural forms as well, unless the context clearly indicates otherwise. It will also be understood that when the terms “comprising” and / or “made of” are used in this specification, the presence of the stated feature, integral, step, operation, element, and / or component is specified, but the presence or addition of one or more other features, integrals, steps, operations, elements, components, and / or groups thereof is not excluded. Terms such as “connected” or “linked” are not limited to physical or mechanical connections but can include electrical connections, whether direct or indirect.

[0072] Unless otherwise specified, all terms used herein (including technical and scientific terms) have the same meaning as commonly understood by one of ordinary skill in the art. It will also be understood that terms such as those defined in commonly used dictionaries should be interpreted as having the meaning consistent with their meaning in the context of the relevant art and the invention, and will not be interpreted as having an idealized or overly formal meaning unless expressly so defined herein.

[0073] In the technical solution of this invention, the collection, storage, use, processing, transmission, provision, and disclosure of user personal information all comply with relevant laws and regulations and do not violate public order and good morals. The use of user data in this technical solution follows relevant national laws and regulations (e.g., the "Information Security Technology - Personal Information Security Specification"). For example: appropriate measures are taken for personal information access control; restrictions are imposed on the display of personal information; the purpose of using personal information does not exceed the scope of direct or reasonable association; and explicit identity targeting is eliminated when using personal information to avoid precisely locating a specific individual.

[0074] Definitions of abbreviations and key terms:

[0075] Sequential filtering is a data processing strategy that reduces computational complexity and improves the real-time performance of algorithms by decomposing the joint update of multi-source data into a series of sequential single-source data updates.

[0076] CV model - constant velocity model, CA model - constant acceleration model, CTRV model - constant turning rate and velocity model are very classic motion models in target tracking and state estimation.

[0077] A greedy algorithm is an algorithm that makes the best or optimal choice in each step of the current state, hoping to lead to a globally best or optimal result.

[0078] The track is the final product of the target fusion system. It is a complete, continuous, and high-precision spatiotemporal state description of a dynamic target and is the most direct basis for situational awareness, threat assessment, and decision-making.

[0079] Figure 1 This is a flowchart illustrating a multi-sensor target fusion method based on sequential filtering, provided in an embodiment of the present invention. Figure 2 This is a schematic diagram of the overall process of a multi-sensor target fusion method based on sequential filtering, provided by an embodiment of the present invention; as shown below. Figure 1 , Figure 2 As shown, the method includes:

[0080] Step S1: Obtain the fused trajectory of multiple target sensors, and calculate the trajectory state at the current moment based on the trajectory state at the previous moment;

[0081] Step S2: A greedy algorithm is used to match the current trajectory with the multiple target sensors respectively;

[0082] Step S3: Update the track status using sequential filtering based on the matched measurement values.

[0083] The advantage of using a greedy algorithm to match multiple target sensors with the trajectory at the current time is that its time complexity is much smaller than that of the KM algorithm (or Hungarian algorithm), but it can achieve an approximate global optimal matching result in most cases, which can greatly reduce the amount of computation.

[0084] In some embodiments, such as Figure 3 As shown, step S1, which involves acquiring the fused trajectory from multiple target sensors and calculating the current trajectory state based on the trajectory state from the previous moment, further includes the following steps before:

[0085] Step S01: Preprocess the measurements from the acquired target sensors.

[0086] Step S02: Spatiotemporally synchronize the measurements from the acquired multiple target sensors.

[0087] In the preprocessing section, the main focus is on verifying the measurement values ​​input from each sensor and deleting measurements that are obviously problematic, such as measurements where the location is outside the sensor's field of view (FOV) or measurements that do not conform to the actual situation, such as measurements where the absolute speed of the detected target vehicle exceeds 200 km / h.

[0088] In the spatiotemporal synchronization section, firstly, the reference coordinates of the target output by each sensor may be inconsistent; some are based on the sensor's installation position, while others are based on the center of the vehicle's rear axle. It's necessary to ensure a consistent coordinate system. Secondly, the reference point of the target output by each sensor may also be inconsistent, such as the target's center point output by the camera and the nearest point output by the radar. It's also necessary to ensure a common reference point, such as the nearest point. These operations are to ensure that the target output by each sensor is the same target point based on the same coordinate system, i.e., spatial synchronization. Additionally, the time at which each sensor detects the target may be inconsistent. A unified prediction at the current time k is needed to ensure that the target state between the track and the sensor is consistent in time when matching and associating with the track. Prediction can use CV, CA, or CTRV motion models depending on the scenario; the prediction error will not be too large in a short time.

[0089] In some embodiments, step S1, obtaining the fused trajectory of multiple target sensors and calculating the trajectory state at the current moment based on the trajectory state at the previous moment, includes the following steps:

[0090] Based on the state estimation results of the trajectory after sequential filtering at time k-1 and the estimated error covariance matrix Calculate the predicted value at time k. and prediction error covariance matrix as follows:

[0091]

[0092]

[0093] in, This is the state transition matrix at time k-1. For different scenarios, CV, CA, or CTRV motion models can be used. It is the control input weight matrix at time k-1. It is the process noise covariance matrix at time k-1.

[0094] In some embodiments, such as Figure 4 As shown, step S2, which uses a greedy algorithm to match multiple target sensors with the trajectory at the current time, includes:

[0095] Step S21: Initialize the distance matrix AxB, where A and B are the number of tracks and the number of measurements taken by the first target sensor, respectively;

[0096] Step S22: Use the initial threshold to exclude tracks and measurements that cannot be matched;

[0097] Step S23: Calculate the distances between each measurement and track using Mahalanobis distance and fill them into the corresponding positions in the matrix;

[0098] Step S24: Find the point with the smallest distance value from the distance matrix as a set of optimal matches and delete the corresponding row and column from the distance matrix until the distance matrix is ​​empty;

[0099] Step S25: Repeat the above steps to complete the matching of all target sensors.

[0100] In some embodiments, step S22, which uses an initial threshold to exclude tracks and measurements that cannot be matched:

[0101] Set the dynamic initial threshold (gate) using the following formula:

[0102]

[0103] in, The distance between the track and the vehicle. , These are custom coefficients. The basic principle is that the farther away from the vehicle, the larger the threshold. Different threshold values ​​are set for the four dimensions: vertical distance px, horizontal distance py, vertical speed vx, and horizontal speed vy.

[0104] Calculate the longitudinal distance difference, lateral distance difference, longitudinal velocity difference, and lateral velocity difference between each measurement and each track. If any of them is greater than the threshold, use the LARGE_NUM value (a maximum value, such as 9999) as the distance between the two target points and fill it into the corresponding position in the distance matrix.

[0105] In some embodiments, step S23, which involves calculating the distances between various measurements and tracks using Mahalanobis distance and filling them into the corresponding positions in the matrix, includes:

[0106] Iterate through each track and sensor measurement, calculate the Mahalanobis distance between them, and fill it into the distance matrix. The Mahalanobis distance calculation formula is as follows:

[0107]

[0108] in, It is a four-dimensional row vector, including the longitudinal distance difference, lateral distance difference, longitudinal velocity difference, and lateral velocity difference between the track target and the sensor-measured target. It is a covariance matrix, and the matrix values ​​can be adjusted according to the actual situation to adjust the weights between the four dimensions.

[0109] In some embodiments, when a sensor detects four measurements at time k, a greedy algorithm is used for matching. The specific calculation method is as follows:

[0110] Taking a scenario with 3 flight paths as an example, a 3x4 distance matrix is ​​constructed as follows:

[0111] ,

[0112] 1. Initial threshold setting:

[0113] Set the dynamic initial threshold (gate) using the following formula:

[0114] ,

[0115] in It is the distance between the flight path and the vehicle. , These are custom coefficients. The basic principle is that the farther away from the vehicle, the larger the threshold. Different threshold values ​​are set for each of the four dimensions: vertical distance (px), horizontal distance (py), vertical speed (vx), and horizontal speed (vy).

[0116] Calculate the longitudinal distance difference, lateral distance difference, longitudinal velocity difference, and lateral velocity difference between each measurement and each track. If any of these values ​​exceeds a threshold, directly use the LARGE_NUM value (a maximum value, such as 9999) as the distance between the two target points and fill it into the corresponding position in the distance matrix. For example, if measurement 2 and track 1 do not meet the threshold requirements, the distance matrix will be as follows:

[0117]

[0118] 2. Mahalanobis distance calculation:

[0119] Iterate through each track and sensor measurement, calculate the Mahalanobis distance between them, and fill it into the distance matrix. The Mahalanobis distance calculation formula is as follows:

[0120]

[0121] in It is a four-dimensional row vector, including the longitudinal distance difference, lateral distance difference, longitudinal velocity difference, and lateral velocity difference between the track target and the sensor-measured target. It is a covariance matrix, and the matrix values ​​can be adjusted according to the actual situation to adjust the weights between the four dimensions.

[0122] 3. Greedy algorithm matching:

[0123] If the distance matrix is ​​calculated in the above two steps, the result is as follows:

[0124]

[0125] According to the greedy algorithm, the search finds the global minimum value of the distance matrix, which has a minimum distance value of 2. This means that the distance between track 1 and measurement 3 is the minimum. The two are matched, and then the row and column of the matched track are set to the LARGE_NUM value. The result is as follows:

[0126]

[0127] The search for the global minimum continues based on the new distance matrix. At this point, the minimum value of the matrix is ​​12, meaning the distance between track 2 and measurement 2 is the smallest. The two are matched, and then the LARGE_NUM value is set for the row and column containing the match. The result is as follows:

[0128]

[0129] The search for the global minimum continues based on the new distance matrix. At this point, the minimum value of the matrix is ​​13, which means that the distance between track 3 and measurement 4 is the smallest. The two are matched, and then the row and column where it is located are set to the LARGE_NUM value. Finally, only the LARGE_NUM value remains in the distance matrix, and the matching between this sensor measurement and track ends.

[0130] In some embodiments, step S3, updating the track state using sequential filtering based on the matched measurement values, includes:

[0131] The measurements of the first target sensor are updated according to the following equation. For the fused state estimate:

[0132] ,

[0133] in, It is the measurement matrix of the first target sensor at time k;

[0134] The gain matrix of the first target sensor at time k The calculation is as follows:

[0135] ,

[0136] in, It is the measurement noise covariance matrix of the first target sensor at time k;

[0137] State estimation error covariance matrix The inverse calculation is as follows:

[0138] ,

[0139] If N target sensors and tracks match, then target sensor i, The state estimation update for the fusion result is as follows:

[0140]

[0141]

[0142]

[0143] in, The measured value of the target sensor i. Let i be the measurement matrix of the target sensor. This is the state estimate after being updated by the (i-1)th target sensor. Let be the state estimation error covariance matrix after being updated by the (i-1)th target sensor. Let be the measurement noise covariance matrix of the target sensor i;

[0144] As shown above, each state estimation is based on the estimation result of the previous sensor to perform the next round of estimation. After traversing N target sensors, the final state estimate and its error covariance matrix are as follows:

[0145] ,

[0146] .

[0147] In some embodiments, it also includes:

[0148] New track generation steps:

[0149] After the greedy algorithm matching, there may be many sensor measurements that do not match the track. These measurements need to be determined whether to be generated as new tracks and enter the next time-cycle matching update.

[0150] Before generating a new track, unmatched measurements between sensors need to be matched to prevent a new target from being detected by multiple sensors simultaneously, resulting in multiple new tracks being generated due to lack of matching. The matching strategy can also use the greedy algorithm described above to reduce computational load;

[0151] First, measurements from sensor 1 that did not match a track are set as new tracks, forming rows of the distance matrix, while measurements from sensor 2 that did not match a track are formed as columns of the distance matrix. A greedy matching algorithm is then used to construct the distance matrix. Next, measurements from sensor 2 that did not match a track from sensor 1 are added to the new tracks, forming rows of the distance matrix. These are then combined with measurements from sensor 3 that did not match a track to form columns of the distance matrix, and a greedy matching algorithm is used again. This process is repeated until all sensor measurements are matched.

[0152] After the matching is completed, various strategies are used to determine whether a new track needs to be generated. For example, if the sensor matched with the new track is a camera, the probability of generating it is higher. If the new track only matches a stationary radar point, the probability of generating it is lower. If the new track is very close to the vehicle, the probability of generating it is higher, and so on. A new track is generated only when the probability of generation is greater than the threshold.

[0153] Track management steps:

[0154] Based on the newly generated tracks and the existing tracks, determine which tracks need to be output to the backend functions for control, which tracks need to be deleted, and which tracks are pending.

[0155] If two tracks are highly similar (e.g., close in distance, similar in speed, and of the same category), they need to be merged into a single track for output. If a track has not matched any measurements in the recent past, it is considered to have disappeared and needs to be deleted. New tracks may not be stable at their initial moment, so they need to be retained for a period of time and not output temporarily.

[0156] Steps to output the track:

[0157] Only stable tracks will be output to the backend functions to prevent the functions from triggering erroneous actions. This includes, but is not limited to, tracks with a high probability of existence and a long duration.

[0158] Based on the same inventive concept, embodiments of the present invention also provide a multi-sensor target fusion system based on sequential filtering, the system being configured to implement any of the methods described in the above embodiments, the system comprising:

[0159] The state prediction module is used to acquire the trajectories fused from multiple target sensors and calculate the current trajectory state based on the trajectory state of the previous moment.

[0160] The greedy algorithm matching module is used to match multiple target sensors with their current trajectories using a greedy algorithm.

[0161] The sequential filtering module is used to update the track status based on the matched measurement values ​​using sequential filtering.

[0162] Based on the same inventive concept, embodiments of the present invention also provide an electronic device. Figure 5 This is a structural block diagram of an electronic device provided in an embodiment of the present invention. Figure 5 As shown, an embodiment of the present invention provides an electronic device including: one or more processors 101, a memory 102, and one or more I / O interfaces 103. The memory 102 stores one or more programs, which, when executed by the one or more processors, cause the one or more processors to implement any of the methods described in the above embodiments; the one or more I / O interfaces 103 are connected between the processor and the memory, configured to enable information interaction between the processor and the memory.

[0163] The processor 101 is a device with data processing capabilities, including but not limited to a central processing unit (CPU); the memory 102 is a device with data storage capabilities, including but not limited to random access memory (RAM, more specifically SDRAM, DDR, etc.), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), and flash memory (FLASH); the I / O interface (read / write interface) 103 is connected between the processor 101 and the memory 102, and can realize information interaction between the processor 101 and the memory 102, including but not limited to a data bus (Bus).

[0164] In some embodiments, the processor 101, memory 102, and I / O interface 103 are interconnected via bus 104, and thus connected to other components of the computing device.

[0165] In some embodiments, the one or more processors 101 include a field-programmable gate array.

[0166] This invention also provides a computer-readable medium. The computer-readable medium stores a computer program, which, when executed by a processor, implements the steps of any of the methods described in the above embodiments. The computer-readable storage medium may be a volatile or non-volatile computer-readable storage medium.

[0167] Those skilled in the art will understand that all or some of the steps, systems, and apparatuses disclosed above, and their functional modules / units, can be implemented as software, firmware, hardware, or suitable combinations thereof. In hardware implementations, the division between functional modules / units mentioned above does not necessarily correspond to the division of physical components; for example, a physical component may have multiple functions, or a function or step may be performed collaboratively by several physical components. Some or all physical components may be implemented as software executed by a processor, such as a central processing unit, digital signal processor, or microprocessor, or as hardware, or as an integrated circuit, such as an application-specific integrated circuit (ASIC). Such software can be distributed on a computer-readable storage medium, which may include computer storage media (or non-transitory media) and communication media (or transient media).

[0168] As is known to those skilled in the art, the term computer storage medium includes volatile and non-volatile, removable and non-removable media implemented in any method or technology for storing information, such as computer-readable program instructions, data structures, program modules, or other data. Computer storage media includes, but is not limited to, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM), static random access memory (SRAM), flash memory or other memory technologies, portable compact disc read-only memory (CD-ROM), digital versatile disc (DVD) or other optical disc storage, magnetic cartridges, magnetic tape, disk storage or other magnetic storage devices, or any other medium that can be used to store desired information and is accessible to a computer. Furthermore, it is known to those skilled in the art that communication media typically contain computer-readable program instructions, data structures, program modules, or other data in modulated data signals such as carrier waves or other transmission mechanisms, and may include any information delivery medium.

[0169] The computer-readable program instructions described herein can be downloaded from computer-readable storage media to various computing / processing devices, or downloaded via a network, such as the Internet, local area network, wide area network, and / or wireless network, to an external computer or external storage device. The network may include copper transmission cables, fiber optic transmission, wireless transmission, routers, firewalls, switches, gateway computers, and / or edge servers. A network adapter card or network interface in each computing / processing device receives the computer-readable program instructions from the network and forwards them to the computer-readable storage media in the respective computing / processing device.

[0170] The computer program instructions used to perform the operations of this invention may be assembly instructions, instruction set architecture (ISA) instructions, machine instructions, machine-dependent instructions, microcode, firmware instructions, state setting data, or source code or object code written in any combination of one or more programming languages, including object-oriented programming languages ​​such as Smalltalk, C++, etc., and conventional procedural programming languages ​​such as the "C" language or similar programming languages. The computer-readable program instructions may be executed entirely on the user's computer, partially on the user's computer, as a standalone software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In cases involving a remote computer, the remote computer may be connected to the user's computer via any type of network—including a local area network (LAN) or a wide area network (WAN)—or may be connected to an external computer (e.g., via the Internet using an Internet service provider). In some embodiments, electronic circuitry, such as programmable logic circuitry, field-programmable gate arrays (FPGAs), or programmable logic arrays (PLAs), is personalized by utilizing state information from the computer-readable program instructions. This electronic circuitry can execute the computer-readable program instructions to implement various aspects of the invention.

[0171] The computer program product described herein can be implemented specifically through hardware, software, or a combination thereof. In one alternative embodiment, the computer program product is specifically embodied in a computer storage medium; in another alternative embodiment, the computer program product is specifically embodied in a software product, such as a software development kit (SDK), etc.

[0172] Various aspects of the present invention are described herein with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It should be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer-readable program instructions.

[0173] These computer-readable program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, or other programmable data processing apparatus to produce a machine such that, when executed by the processor of the computer or other programmable data processing apparatus, they create means for implementing the functions / actions specified in one or more blocks of the flowchart and / or block diagram. These computer-readable program instructions can also be stored in a computer-readable storage medium that causes a computer, programmable data processing apparatus, and / or other device to operate in a particular manner; thus, the computer-readable medium storing the instructions comprises an article of manufacture that includes instructions for implementing aspects of the functions / actions specified in one or more blocks of the flowchart and / or block diagram.

[0174] Computer-readable program instructions may also be loaded onto a computer, other programmable data processing apparatus, or other device to cause a series of operational steps to be performed on the computer, other programmable data processing apparatus, or other device to produce a computer-implemented process, thereby causing the instructions executed on the computer, other programmable data processing apparatus, or other device to perform the functions / actions specified in one or more boxes of a flowchart and / or block diagram.

[0175] The flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of systems, methods, and computer program products according to various embodiments of the present invention. In this regard, each block in a flowchart or block diagram may represent a module, segment, or portion of an instruction, which contains one or more executable instructions for implementing a specified logical function. In some alternative implementations, the functions marked in the blocks may occur in a different order than those shown in the drawings. For example, two consecutive blocks may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved. It should also be noted that each block in the block diagrams and / or flowcharts, and combinations of blocks in the block diagrams and / or flowcharts, may be implemented using a dedicated hardware-based system that performs the specified function or action, or using a combination of dedicated hardware and computer instructions.

[0176] Example embodiments have been disclosed herein, and while specific terminology has been used, it is for illustrative purposes only and should be construed as such, and is not intended to be limiting. In some instances, it will be apparent to those skilled in the art that features, characteristics, and / or elements described in conjunction with particular embodiments may be used alone, or in combination with features, characteristics, and / or elements described in conjunction with other embodiments, unless otherwise expressly indicated. Therefore, those skilled in the art will understand that various changes in form and detail may be made without departing from the scope of the invention as set forth in the appended claims.

Claims

1. A multi-sensor target fusion method based on sequential filtering, characterized in that, include: Acquire the fused trajectories from multiple target sensors, and calculate the current trajectory state based on the trajectory state of the previous moment; A greedy algorithm is used to match multiple target sensors with their current trajectories. The track status is updated using sequential filtering based on the matched measurements.

2. The method according to claim 1, characterized in that, Before the step of acquiring the fused trajectory from multiple target sensors and calculating the current trajectory state based on the trajectory state of the previous moment, the method further includes: Measurement preprocessing of multiple target sensors; The measurements from multiple target sensors are synchronized in time and space.

3. The method according to claim 1 or 2, characterized in that, The step of obtaining the fused trajectory from multiple target sensors and calculating the trajectory state at the current moment based on the trajectory state at the previous moment includes: Based on the state estimation results of the trajectory after sequential filtering at time k-1 and the estimated error covariance matrix Calculate the predicted value at time k. and prediction error covariance matrix as follows: in, It is the state transition matrix at time k-1. It is the control input weight matrix at time k-1. It is the process noise covariance matrix at time k-1.

4. The method according to claim 3, characterized in that, The step of matching multiple target sensors with their current trajectories using a greedy algorithm includes: Initialize the distance matrix AxB, where A and B are the number of tracks and the number of measurements from the first target sensor, respectively. Use an initial threshold to exclude tracks and measurements that cannot be matched; Calculate the distances between various measurements and tracks using Mahalanobis distance, and fill them into the corresponding positions in the matrix; Find the point with the smallest distance value in the distance matrix as a set of optimal matches and delete the corresponding row and column in the distance matrix until the distance matrix is ​​empty; Repeat the above steps to complete the matching of all target sensors.

5. The method according to claim 4, characterized in that, In the step of using an initial threshold to exclude impossible-to-match tracks and measurements: Set the dynamic initial threshold (gate) using the following formula: in, The distance between the track and the vehicle. , It is a custom coefficient, with a different threshold value set for each of the four dimensions: vertical distance px, horizontal distance py, vertical velocity vx, and horizontal velocity vy. Calculate the longitudinal distance difference, lateral distance difference, longitudinal velocity difference, and lateral velocity difference between each measurement and each track. If any of them is greater than the threshold, use the maximum value as the distance between the two target points and fill it into the corresponding position in the distance matrix. The step of calculating the distances between various measurements and tracks using Mahalanobis distance and filling them into the corresponding positions in the matrix includes: Iterate through each track and sensor measurement, calculate the Mahalanobis distance between them, and fill it into the distance matrix. The Mahalanobis distance calculation formula is as follows: in, It is a four-dimensional row vector, including the longitudinal distance difference, lateral distance difference, longitudinal velocity difference, and lateral velocity difference between the track target and the sensor-measured target. It is the covariance matrix.

6. The method according to claim 1, characterized in that, The step of updating the track state using sequential filtering based on the matched measurement values ​​includes: The measurements of the first target sensor are updated according to the following equation. For the fused state estimate: ， in, It is the measurement matrix of the first target sensor at time k; The gain matrix of the first target sensor at time k The calculation is as follows: ， in, It is the measurement noise covariance matrix of the first target sensor at time k; State estimation error covariance matrix The inverse calculation is as follows: ， If N target sensors and tracks match, then target sensor i, The state estimation update for the fusion result is as follows: in, The measured value of the target sensor i. Let i be the measurement matrix of the target sensor. This is the state estimate after being updated by the (i-1)th target sensor. Let be the state estimation error covariance matrix after being updated by the (i-1)th target sensor. Let be the measurement noise covariance matrix of the target sensor i; After traversing N target sensors, the obtained state estimates and their error covariance matrices are as follows: ， 。 7. The method according to claim 1, characterized in that, Also includes: New track generation steps: When multiple target sensor measurements fail to match a track, the measurement from the second target sensor that failed to match a track is set as a row of the new track in the distance matrix, and the measurement from the third target sensor that failed to match a track is set as a column of the distance matrix. A greedy algorithm is then used for matching. Measurements from the third target sensor that failed to match the second target sensor are added to the new track as rows of the distance matrix. Then, measurements from the third target sensor that failed to match a track are set as columns of the distance matrix. A greedy algorithm is then used for matching. This process continues until the last target sensor measurement is matched. After matching, if the probability of generating a new track is greater than a preset probability threshold, the new track is generated. Track management steps: The newly generated track and the previously existing track are judged: if the similarity between two tracks is higher than the preset similarity threshold, they are merged into one track and output; if a track has not matched any measurement within the recent preset time period, the track is deleted. Output track steps: Output the track after track management.

8. A multi-sensor target fusion system based on sequential filtering, characterized in that, The system is configured to implement the method as described in any one of claims 1 to 7, the system comprising: The state prediction module is used to acquire the trajectories fused from multiple target sensors and calculate the current trajectory state based on the trajectory state of the previous moment. The greedy algorithm matching module is used to match multiple target sensors with their current trajectories using a greedy algorithm. The sequential filtering module is used to update the track status based on the matched measurement values ​​using sequential filtering.

9. An electronic device, characterized in that, include: One or more processors; Memory, used to store one or more programs; When the one or more programs are executed by the one or more processors, the one or more processors implement the method as described in any one of claims 1 to 7.

10. A computer-readable medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the steps of the method as described in any one of claims 1 to 7.

Images