A target trajectory processing method based on multi-sensor fusion

Abstract

An embodiment of the present invention relates to a method for processing an object trajectory through multi-sensor fusion. The method includes: obtaining first observation data and a first object center point corresponding to a specified object at time t, and obtaining a first self-vehicle center point of the self-vehicle; determining a calibration reference point to generate a first calibration reference point; if the observation time i = t, adding a queue record and setting the first trajectory processing mode to the first mode; if the observation time i < t, inserting a queue record when i satisfies the recent time period range and setting the first trajectory processing mode to the second mode, and discarding the first calibration reference point and setting the first trajectory processing mode to the ignore mode when i does not satisfy the recent time period range; when the first trajectory processing mode is the ignore mode, no processing is performed; when it is the first mode, performing the latest trajectory prediction processing; when it is the second mode, performing the historical trajectory correction processing. Through the present invention, the smoothness and robustness of the object trajectory can be ensured.

Description

Technical Field

[0001] The present invention relates to the field of data processing technology, and in particular to a target trajectory processing method of multi-sensor fusion. Background Art

[0002] When the perception module of the autonomous driving system performs target tracking based on the Multi-Sensor Fusion (MSF) technology, it first extracts observation features from the perception data (lidar point cloud, camera image, millimeter-wave radar) of various sensors (lidar, camera, millimeter-wave radar) to generate corresponding observation data: the observation data of the lidar point cloud includes a polygon convex hull and / or a target detection box, the observation data of the camera image is a target detection box, and the observation data of the millimeter-wave radar point cloud is a radar observation point. The bird's-eye view map coordinate system is then used to synchronize the observation data of all types of sensors to the same space. The reference point calibration is then performed based on the sensor observation data at the current moment: if the sensor observation data at the current moment is lidar observation data, the center point of the corresponding polygon convex hull and / or the four vertices of the corresponding target detection box are selected as the calibration reference point. If it is camera observation data, the center point of the corresponding target detection box is used as the calibration reference point. If it is millimeter-wave radar observation data, the radar observation point is used as the calibration reference point. Then, according to the historical motion trajectory of the target and the calibration reference point, the interactive multiple models (IMMs) are used to calibrate the reference point. The filter implemented by the IMM (Integrated Motion Model) algorithm is used to filter the target so as to predict the motion state of the target at the current moment, and the predicted motion state is combined with the historical motion trajectory to update the target trajectory of the target.

[0003] However, when implementing the above solution, we found some problems: 1) The calibration reference point output at each moment is not necessarily the observation data from the same type of sensor, but may be any one of the seven points (5 points for lidar + 1 point for camera + 1 point for millimeter-wave radar). This will cause a relatively obvious coordinate offset between the calibration reference points at the previous and next moments, resulting in many jagged tracks in the tracked target trajectory, and poor smoothness and robustness; 2) The reception timing of the observation data of various sensors does not necessarily strictly follow the sequential timing, that is, it is possible to receive observation data from the previous moment (hereinafter referred to as the delayed moment) at the current moment t. The conventional processing mode will discard such delayed observation data, but in fact, the observation data at these delayed moments can have a good correction effect on the historical tracking trajectory of the target. Summary of the Invention

[0004] The purpose of the present invention is to address the shortcomings of the existing technology and provide a multi-sensor fusion target trajectory processing method, electronic device, and computer-readable storage medium. The method can dynamically adjust the offset of historical calibration reference points based on the sensor type corresponding to each observation data obtained, and perform Kalman filtering using the adjusted calibration reference point sequence as the observation sequence to obtain an updated target trajectory. The method can also adjust the historical motion state based on the observation data at the delayed moment. Through the present invention, it can ensure that the latest target trajectory output at each moment has good smoothness and robustness, and the observation data at the delayed moment can also be used to correct the historical target trajectory.

[0005] To achieve the above objectives, a first aspect of an embodiment of the present invention provides a target trajectory processing method using multi-sensor fusion, the method comprising:

[0006] At time t, the sensor observation data corresponding to the designated target is obtained as the corresponding first observation data M c,i , and obtain the center point position of the designated target at the latest moment and record it as the corresponding first target center point, and obtain the center point position of the ego vehicle at the latest moment and record it as the corresponding first ego vehicle center point; sensor type c includes laser radar, camera and millimeter wave radar; observation time i ≤ time t;

[0007] According to the sensor type c, the first observation data M c,i , the observation time i, the time t and the first vehicle center point are used to determine the calibration reference point to generate the corresponding first calibration reference point P i ;

[0008] If the observation time i is equal to the time t, then according to the first calibration reference point P i Performing queue record addition processing in a preset first relationship queue reflecting the corresponding relationship between the target motion state and the reference point coordinates to obtain a corresponding first added queue record, and setting the first trajectory processing mode to the first mode;

[0009] If the observation time i is less than the time t, then identify whether the observation time i meets the preset recent time range; if so, then according to the first calibration reference point P i Perform queue record insertion processing in the first relation queue to obtain the corresponding first insertion queue record, and set the first trajectory processing mode to the second mode; if not satisfied, set the first calibration reference point P i discarding and setting the first trajectory processing mode to ignore mode;

[0010] When the first trajectory processing mode is the ignore mode, no trajectory processing is performed; when the first trajectory processing mode is the first mode, the latest trajectory prediction processing is performed based on the first add queue record, the first relationship queue and the first target center point, and the latest first target trajectory is output; when the first trajectory processing mode is the second mode, the historical trajectory correction processing is performed based on the first insert queue record, the first relationship queue and the first target center point, and the corrected second target trajectory is output.

[0011] Preferably, when the sensor type c is a laser radar, the first observation data M c,i It includes a first polygon convex hull and / or a first target detection frame, wherein the first target detection frame includes four vertices; when the sensor type c is a camera, the first observation data M c,i Including a second target detection frame, the second target detection frame includes four vertices; when the sensor type c is a millimeter wave radar, the first observation data M c,i Includes the first radar observation point;

[0012] The coordinate systems of the first target center point, the first vehicle center point, the first polygon convex hull, the first target detection frame, the second target detection frame, and the first radar observation point are all the same bird's-eye view coordinate system;

[0013] The first relationship queue includes multiple first queue records; the first queue record includes a first time field, a first motion state field, a first sensor type field and a first reference point coordinate field; the time corresponding to the first time field should satisfy the preset recent time period range; the sensor type of the first sensor type field includes lidar, camera and millimeter wave radar.

[0014] Preferably, the sensor type c, the first observation data M c,i , the observation time i, the time t and the first vehicle center point are used to determine the calibration reference point to generate the corresponding first calibration reference point P i , specifically including:

[0015] When the sensor type c is a laser radar, the first observation data M c,i Confirm whether the first target detection frame is included; if it is confirmed that it is not included, estimate the center of gravity of the polygon convex hull of the first polygon and use the estimated center of gravity of the polygon convex hull as the corresponding first calibration reference point P iIf it is confirmed to be included, then identify whether the observation time i is less than the time t. If it is less than, the vertex farthest from the first vehicle center point among the four vertices of the first target detection frame is used as the corresponding first calibration reference point P i If it is not less than, the vertex closest to the first vehicle center point among the four vertices of the first target detection frame is used as the corresponding first calibration reference point P i ;

[0016] When the sensor type c is a camera, identify whether the observation time i is less than the time t; if it is less, the vertex farthest from the first vehicle center point among the four vertices of the second target detection frame is used as the corresponding first calibration reference point P i If it is not less than, the vertex closest to the first vehicle center point among the four vertices of the second target detection frame is used as the corresponding first calibration reference point P i ;

[0017] When the sensor type c is a millimeter wave radar, the first radar observation point is used as the corresponding first calibration reference point P i .

[0018] Preferably, the first calibration reference point P i Performing queue record addition processing in a preset first relationship queue reflecting the corresponding relationship between the target motion state and the reference point coordinates to obtain a corresponding first added queue record specifically includes:

[0019] Add a new first queue record at the end of the first relation queue as the corresponding first added queue record; and in the first added queue record, set the first time field to the first calibration reference point P i The observation time i is set, and the first motion state field is set to empty, and the first sensor type field is set to the first calibration reference point P i The corresponding sensor type c, and the first reference point coordinate field is set to the first calibration reference point P i The coordinates of the reference point in the bird's-eye view coordinate system.

[0020] Preferably, the first calibration reference point P i Performing queue record insertion processing in the first relationship queue to obtain a corresponding first insertion queue record specifically includes:

[0021] The first one in the first relationship queue that is larger than the first calibration reference point P iThe first queue record corresponding to the first time field of the observation time i is recorded as the current queue record; and a new first queue record is inserted at the previous queue record position of the current queue record as the corresponding first inserted queue record; and in the first inserted queue record, the first time field is set to the first calibration reference point P i The observation time i is set, and the first motion state field is set to empty, and the first sensor type field is set to the first calibration reference point P i The corresponding sensor type c, and the first reference point coordinate field is set to the first calibration reference point P i The coordinates of the reference point in the bird's-eye view coordinate system.

[0022] Preferably, performing the latest trajectory prediction process according to the first added queue record, the first relationship queue and the first target center point and outputting the latest first target trajectory specifically includes:

[0023] Using the first queue record preceding the first added queue record in the first relationship queue as the corresponding previous queue record; and using the first sensor type field of the previous queue record as the corresponding first sensor type;

[0024] When the first add queue record is inconsistent with the first sensor type field of the previous queue record, query the preset sensor offset vector list, extract the offset vector field of the sensor offset vector record whose source sensor field matches the first sensor type field of the previous queue record and whose target sensor field matches the first sensor type field of the first add queue record as the corresponding current offset vector; and perform coordinate offset conversion on the first reference point coordinate field of each of the first queue records before the first add queue record according to the current offset vector; and set the first sensor type field of each of the first queue records before the first add queue record to the sensor type consistent with the first sensor type field of the first add queue record; the sensor offset vector list includes a plurality of sensor offset vector records; the sensor offset vector record includes the source sensor field, the target sensor field and the offset vector field;

[0025] If the first sensor type field of the first added queue record is consistent with the first sensor type, the first motion state field of the previous queue record is extracted as the corresponding first initial motion state S 1，0 ; and extract the first reference point coordinate field of the first added queue record as the corresponding first observation coordinate D; with the first initial motion state S1，0 The first initial state quantity and the first observation quantity are input into a preset Kalman filter for one-step prediction to obtain a corresponding first motion state S; and the first motion state field of the first added queue record is updated according to the first motion state S; the first motion state S corresponds to the first observation coordinate D; and the first motion state S includes a position coordinate;

[0026] If the first sensor type field of the first added queue record is inconsistent with the first sensor type, the first motion state field of the first queue record in the first relationship queue is extracted as the corresponding second initial motion state S 2，0 ; and the number of records of the first queue records of the first relationship queue is counted to generate the corresponding first number a; and the first reference point coordinate fields of the first queue records from the 2nd to the ath in the first relationship queue are extracted one by one as the corresponding second observation coordinates E j , 1≤j≤a-1; and obtain a-1 of the second observation coordinates E j Sort the corresponding moments in chronological order to form the corresponding second observation coordinate sequence {E j}; and with the second initial motion state S 2，0 is the corresponding second initial state quantity, and the second observation coordinate sequence {E j} is the corresponding second observation sequence, the second initial state quantity and the second observation sequence are input into the preset Kalman filter to gradually predict the motion state of j from 1 to a-1 to obtain the corresponding second motion state sequence {S j}; and according to each second motion state S j The first motion state field of the first queue record corresponding to the first relationship queue is updated; the second motion state sequence {S j} includes a-1 of the second motion states S j The second motion state S j With the second observation coordinate E j According to the subscript j one-to-one correspondence; the second motion state S j Includes a location coordinate;

[0027] The position coordinates of the first motion state field of each first queue record of the first relationship queue that has completed the motion state field update are extracted as the corresponding first trajectory point coordinates (x1, y1); and the coordinate offset vector of the last first trajectory point coordinate (x1, y1) and the first target center point is calculated to generate the corresponding first offset vector (△x1, △y1); and according to the first offset vector (△x1, △y1), the horizontal and vertical offsets of each first trajectory point coordinate (x1, y1) are adjusted to generate the corresponding second trajectory point coordinates (x2=x1-△x1, y2=y1-△y1); and all the obtained second trajectory point coordinates (x2, y2) are sorted in the order of corresponding time to form the corresponding first target trajectory.

[0028] Preferably, performing historical trajectory correction processing according to the first insertion queue record, the first relationship queue, and the first target center point and outputting a corrected second target trajectory specifically includes:

[0029] Using the first queue record preceding the first inserted queue record in the first relational queue as the corresponding previous queue record; and using the first sensor type field of the previous queue record as the corresponding first sensor type;

[0030] When the first insert queue record is inconsistent with the first sensor type field of the previous queue record, query the preset sensor offset vector list, extract the offset vector field of the sensor offset vector record whose source sensor field matches the first sensor type field of the previous queue record and whose target sensor field matches the first sensor type field of the first insert queue record as the corresponding current offset vector; and perform coordinate offset conversion on the first reference point coordinate field of each of the first queue records except the first insert queue record according to the current offset vector; and set the first sensor type field of each of the first queue records except the first insert queue record to the sensor type consistent with the first sensor type field of the first insert queue record; the sensor offset vector list includes a plurality of sensor offset vector records; the sensor offset vector record includes the source sensor field, the target sensor field and the offset vector field;

[0031] If the first sensor type field of the first inserted queue record is consistent with the first sensor type, the first motion state field of the previous queue record is extracted as the corresponding third initial motion state S 3，0; and the number of records from the first insertion queue record to the last first queue record is counted to generate a corresponding second number b; and the first reference point coordinate fields from the first insertion queue record to the last first queue record are extracted one by one as the corresponding third observation coordinate F h , 1≤h≤b; and obtain the b third observation coordinates F h Sort the corresponding moments in chronological order to form the corresponding third observation coordinate sequence {F h}; and the third initial motion state S 3，0 is the corresponding third initial state quantity, and the third observation coordinate sequence {F h} is the corresponding third observation sequence, the third initial state quantity and the third observation sequence are input into the preset Kalman filter to gradually predict the motion state of h from 1 to b to obtain the corresponding third motion state sequence {S h}; and according to each third motion state S h The first motion state field of the first queue record corresponding to the first relationship queue is updated; the third motion state sequence {S h} includes b said third motion state S h , the third motion state S h With the third observation coordinate F h According to the subscript h one-to-one correspondence; the third motion state S h Includes a location coordinate;

[0032] If the first sensor type field of the first insertion queue record is inconsistent with the first sensor type, the first motion state field of the first first queue record in the first relation queue is extracted as the corresponding fourth initial motion state S 4,0 ; and the number of records of the first queue records of the first relationship queue is counted to generate the corresponding third quantity c; and the first reference point coordinate fields of the first queue records from the second to the cth in the first relationship queue are extracted one by one as the corresponding fourth observation coordinates G k , 1≤k≤c-1; and the obtained c-1 fourth observation coordinates G k The corresponding fourth observation coordinate sequence {G k}; and in the fourth initial motion state S 4,0 is the corresponding fourth initial state quantity, and the fourth observation coordinate sequence {G k} is the corresponding fourth observation sequence, the fourth initial state quantity and the fourth observation sequence are input into the Kalman filter to gradually predict the motion state of k from 1 to c-1 to obtain the corresponding fourth motion state sequence {S k}; and according to each fourth motion state S k The first motion state field of the first queue record corresponding to the first relationship queue is updated; the fourth motion state sequence {S k} includes c-1 said fourth motion state S k , the fourth motion state S k With the fourth observation coordinate G k According to the subscript k one-to-one correspondence; the fourth motion state S k Includes a location coordinate;

[0033] The position coordinates of the first motion state field of each first queue record of the first relationship queue that has completed the motion state field update are extracted as the corresponding third trajectory point coordinates (x3, y3); and the coordinate offset vector of the last third trajectory point coordinate (x3, y3) and the first target center point is calculated to generate the corresponding second offset vector (△x2, △y2); and according to the second offset vector (△x2, △y2), the horizontal and vertical offset amounts of each third trajectory point coordinate (x3, y3) are adjusted to generate the corresponding fourth trajectory point coordinates (x4=x3-△x2, y4=y3-△y2); and all the fourth trajectory point coordinates (x4, y4) obtained are sorted in the order of corresponding time to form the corresponding second target trajectory.

[0034] A second aspect of an embodiment of the present invention provides an electronic device, including: a memory, a processor, and a transceiver;

[0035] The processor is configured to be coupled to the memory, read and execute instructions in the memory, so as to implement the method steps described in the first aspect above;

[0036] The transceiver is coupled to the processor, and the processor controls the transceiver to send and receive messages.

[0037] A third aspect of an embodiment of the present invention provides a computer-readable storage medium, which stores computer instructions. When the computer instructions are executed by a computer, the computer executes the instructions of the method described in the first aspect.

[0038] The embodiment of the present invention provides a target trajectory processing method for multi-sensor fusion, an electronic device and a computer-readable storage medium. When receiving sensor observation data, a calibration reference point is determined based on the corresponding sensor type and the observation data time; and a trajectory processing mode is determined based on the observation data time. If the observation data time matches the current moment, the trajectory processing mode is confirmed to be the first mode. If the observation data time is earlier than the current moment and does not exceed the pre-set recent time period range, the trajectory processing mode is confirmed to be the second mode. If the observation data time is earlier than the current moment and has exceeded the pre-set recent time period range, the trajectory processing mode is confirmed to be the ignore mode. If the trajectory processing mode is the first mode, the corresponding latest trajectory prediction processing is started. When performing the latest trajectory prediction processing, if the sensor type at the current moment is inconsistent with the historical sensor type, the coordinates of all historical calibration reference points are uniformly adjusted based on the known source-target sensor offset vector and the adjusted calibration reference point sequence is used as the observation sequence for Kalman filtering is used to obtain the latest target trajectory. If the sensor type at the current moment is consistent with the historical sensor type, the calibration reference point at the current moment is used as the observation for one-step prediction to obtain the latest target trajectory. If the trajectory processing mode is the second mode, the corresponding historical trajectory correction processing is started. When performing the historical trajectory correction processing, if the sensor type at the delay moment obtained this time is inconsistent with the historical sensor type, the coordinates of all historical calibration reference points are uniformly adjusted based on the known source-target sensor offset vector, and the Kalman filtering is performed using the adjusted historical calibration reference point sequence as the observation sequence to obtain the corrected latest target trajectory. If the sensor type at the delay moment is consistent with the historical sensor type, the Kalman filtering is performed using the calibration reference point sequence from the delay moment to the last moment as the observation sequence to obtain the corrected latest target trajectory. If the trajectory processing mode is the ignore mode, it means that the observation data obtained this time has become outdated, so the calibration reference point of this time is discarded and no trajectory processing is performed. Through the present invention, the offset of the historical calibration reference point can be dynamically adjusted based on the sensor type corresponding to the observation data obtained each time, and the Kalman filter is performed using the adjusted calibration reference point sequence as the observation sequence to obtain an latest target trajectory, thereby ensuring that the latest target trajectory output at each moment has good smoothness and robustness; the historical motion state can also be adjusted based on the observation data at the delayed moment, thereby achieving the purpose of correcting the target historical trajectory using the observation data at the delayed moment. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] Figure 1 A schematic diagram of a target trajectory processing method for multi-sensor fusion provided in Example 1 of the present invention;

[0040] Figure 2 This is a structural diagram of an electronic device provided in Embodiment 2 of the present invention. DETAILED DESCRIPTION

[0041] To make the objectives, technical solutions, and advantages of the present invention more apparent, the present invention will be further described in detail below with reference to the accompanying drawings. It should be understood that the embodiments described herein are merely some, rather than all, of the present invention. All other embodiments derived by persons of ordinary skill in the art based on the embodiments of the present invention without inventive effort are intended to fall within the scope of protection of the present invention.

[0042] The first embodiment of the present invention provides a target trajectory processing method of multi-sensor fusion, such as Figure 1 The following is a schematic diagram of a target trajectory processing method for multi-sensor fusion provided in Example 1 of the present invention. The method mainly includes the following steps:

[0043] Step 1: At time t, obtain the sensor observation data corresponding to the specified target as the corresponding first observation data M c,i , and obtain the center point position of the designated target at the latest moment and record it as the corresponding first target center point, and obtain the center point position of the ego vehicle at the latest moment and record it as the corresponding first ego vehicle center point;

[0044] Among them, sensor type c includes laser radar, camera and millimeter wave radar; observation time i≤time t; when sensor type c is laser radar, the first observation data M c,i Including a first polygon convex hull and / or a first target detection frame, the first target detection frame includes four vertices; when the sensor type c is a camera, the first observation data M c,i Including a second target detection frame, the second target detection frame includes four vertices; when the sensor type c is a millimeter wave radar, the first observation data M c,i Including the first radar observation point; the coordinate systems of the first target center point, the first vehicle center point, the first polygon convex hull, the first target detection frame, the second target detection frame and the first radar observation point are all the same bird's-eye view coordinate system.

[0045] Here, the first observation data M c,i That is, the sensor observation data of a specified target, the first observation data M c,i It is the processed data after the perception module completes target detection and association based on the original perception data. The first observation data M c,i The data structure of is related to the corresponding sensor type c (lidar, camera and millimeter wave radar); the first observation data M c,i It is not necessarily the result of processing the perception data at the current time t, but may also be the result of processing the perception data at a previous time, so the first observation data M c,iThe observation time i is ≤ time t; if the observation time i is less than time t, then the center point position of the designated target at the latest time is the center point position at time t-1, and the center point position of the ego vehicle at the latest time is also the center point position at time t-1; if the observation time i is equal to time t, then the center point position of the designated target at the latest time is the center point position at time t, and the center point position of the ego vehicle at the latest time is also the center point position at time t; for example, after obtaining the perception data generated by the lidar at time t-1 at time t-1, the perception module will obtain the center point position of the designated target in the bird's-eye view coordinate system at time t-1 as the first target center point at the latest time based on the commonly used target detection algorithm and target association algorithm. In addition, the center point position of the ego vehicle in the bird's-eye view coordinate system at time t-1 is obtained by the positioning module of the autonomous driving system as the first ego vehicle center point at the latest time; after obtaining the perception data generated by the lidar at time t-3 at time t, the perception module will use the first target center point at time t-1 as the first target center point at the latest time, and the first ego vehicle center point at time t-1 as the first ego vehicle center point at the latest time.

[0046] Step 2: Based on the sensor type c and the first observation data M c,i , observe time i, time t and the first vehicle center point to determine the calibration reference point and generate the corresponding first calibration reference point P i ;

[0047] Specifically comprising: Step 21, when the sensor type c is a laser radar, the first observation data M c,i Confirm whether the first target detection frame is included; if it is confirmed not to be included, the center of gravity of the polygon convex hull is estimated for the first polygon convex hull and the estimated center of gravity of the polygon convex hull is used as the corresponding first calibration reference point P i If it is confirmed to be included, then identify whether the observation time i is less than the time t. If it is less than, the vertex farthest from the first vehicle center point among the four vertices of the first target detection frame is used as the corresponding first calibration reference point P i If it is not less than, the vertex closest to the first vehicle center point among the four vertices of the first target detection frame is used as the corresponding first calibration reference point P i ;

[0048] Here, when the sensor type c is a laser radar, the first observation data M received at that time is described. c,i is the laser radar sensor observation data; according to the known characteristics of the laser radar sensor observation data, the observation data must include a polygonal convex hull, namely the first polygonal convex hull, and may also include a target detection box, namely the first target detection box;

[0049] The embodiment of the present invention stipulates that if there is only the first polygon convex hull, the center of gravity of the polygon convex hull, that is, the center of gravity of the polygon convex hull, is used as the calibration reference point, that is, the first calibration reference point P i When calculating the centroid of the polygon convex hull, the embodiment of the present invention supports multiple algorithms, one of which is to calculate the mean value of all vertex coordinates of the first polygon convex hull to obtain the centroid of the polygon convex hull;

[0050] If the first polygon convex hull also includes the first target detection frame, the four vertices of the first target detection frame are screened for calibration reference points; during screening, if the observation time i < time t, it means that the first observation data M c,i The observation data at the delayed moment, at this time, the vertex farthest from the first vehicle center point among the four vertices should be used as the calibration reference point, that is, the first calibration reference point P i If the observation time i = time t, the first observation data M c,i The observation data at the current moment, at this time, the vertex closest to the first vehicle center point among the four vertices should be used as the calibration reference point, that is, the first calibration reference point P i ;

[0051] Step 22: When the sensor type c is a camera, identify whether the observation time i is less than the time t; if it is less, the vertex farthest from the first vehicle center point among the four vertices of the second target detection frame is used as the corresponding first calibration reference point P i If it is not less than, the vertex closest to the first vehicle center point among the four vertices of the second target detection frame is used as the corresponding first calibration reference point P i ;

[0052] Here, when the sensor type c is a camera, the first observation data M received at that time is described. c,i is the camera sensor observation data; according to the known characteristics of camera sensor observation data, the observation data only includes one target detection frame, namely the second target detection frame; when the four vertices of the second target detection frame are used for calibration reference point screening, if the observation time i < time t, it means that the first observation data M c,i The observation data at the delayed moment, at this time, the vertex farthest from the first vehicle center point among the four vertices should be used as the calibration reference point, that is, the first calibration reference point P i If the observation time i = time t, the first observation data M c,i The observation data at the current moment, at this time, the vertex closest to the first vehicle center point among the four vertices should be used as the calibration reference point, that is, the first calibration reference point P i ;

[0053] Step 23: When the sensor type c is a millimeter wave radar, the first radar observation point is used as the corresponding first calibration reference point P i .

[0054] Here, when the sensor type c is a millimeter wave radar, the first observation data M received at the time is described. c,i is the millimeter-wave radar sensor observation data; according to the known characteristics of millimeter-wave radar sensor observation data, the observation data only includes one radar observation point, namely the first radar observation point; at this time, the first radar observation point is directly used as the calibration reference point, namely the first calibration reference point P i .

[0055] Step 3: If the observation time i is equal to the time t, then according to the first calibration reference point P i Performing queue record adding processing in a preset first relationship queue reflecting the corresponding relationship between the target motion state and the reference point coordinates to obtain a corresponding first added queue record;

[0056] The first relationship queue includes multiple first queue records; the first queue record includes a first time field, a first motion state field, a first sensor type field, and a first reference point coordinate field; the time corresponding to the first time field should meet the preset recent time period range; the sensor type of the first sensor type field includes lidar, camera, and millimeter wave radar;

[0057] Here, the first relationship queue is a cache data queue for storing key data in an embodiment of the present invention, and the queue performs first-in-first-out management on queue records based on a preset recent time period range; each first queue record corresponds to a data moment; the first moment field is the corresponding data moment; the first motion state field is the predicted motion state corresponding to the moment, and the predicted motion state includes the position coordinates in a bird's-eye view coordinate system, which is often called the trajectory point coordinates, and also includes a set of motion component information, such as speed, acceleration, etc.; the first reference point coordinate field is the calibration reference point coordinate corresponding to the moment. It can be seen from the subsequent steps that the first reference point coordinate field is the original first calibration reference point P when the record is first added. i The reference point coordinates may be modulated into another reference point coordinate based on an offset; the first sensor type field is the sensor type corresponding to the first reference point coordinate field. From the subsequent steps, it can be seen that this field is the original first calibration reference point P when the first new record is added. i The corresponding sensor type may be modified later due to the offset modulation of the first reference point coordinate field;

[0058] Here, if the observation time i is equal to the time t, it means that the first observation data M c,iFor the observation data at the current moment, a new queue record, namely the first added queue record, should be added to the end of the first relationship queue, and the first trajectory processing mode should be set to the first mode so that the subsequent steps can start the corresponding latest trajectory prediction processing;

[0059] Specifically comprising: step 31, according to the first calibration reference point P i Perform queue record addition processing in the first relationship queue to obtain a corresponding first added queue record;

[0060] Specifically, it includes: adding a first queue record at the end of the first relation queue as the corresponding first added queue record; and setting the first time field in the first added queue record to the first calibration reference point P i The observation time i is set, and the first motion state field is set to empty, and the first sensor type field is set to the first calibration reference point P i The corresponding sensor type c, and the first reference point coordinate field is set to the first calibration reference point P i The coordinates of the reference point in the bird's-eye view coordinate system;

[0061] Step 32: Set the first trajectory processing mode to the first mode.

[0062] Step 4: If the observation time i is less than the time t, then identify whether the observation time i meets the preset recent time range; if so, then according to the first calibration reference point P i Perform queue record insertion processing in the first relation queue to obtain the corresponding first insertion queue record, and set the first trajectory processing mode to the second mode; if it is not satisfied, then set the first calibration reference point P i Discard and set the first track processing mode to ignore mode;

[0063] Here, if the observation time i is less than the time t, it means that the first observation data M c,i For the observation data at the delayed time, there are two cases. One is that the observation data within the preset recent time range, that is, the observation time i meets the recent time range. At this time, a new queue record, that is, the first insertion queue record, should be inserted at the corresponding position in the first relationship queue, and the first trajectory processing mode should be set to the second mode so that the subsequent steps can start the corresponding historical trajectory correction processing; the other is that the observation data earlier than the recent time range, that is, the observation time i does not meet the recent time range. At this time, the first observation data M c,i In fact, it has lost its timeliness. In the embodiment of the present invention, the first calibration reference point P is set by default. i Discard and set the first track processing mode to ignore mode, and no track processing operations will be performed in subsequent steps;

[0064] Specifically, the process includes: step 41, identifying whether the observation time i meets the preset recent time range; if so, proceeding to step 42; if not, proceeding to step 44;

[0065] Step 42: According to the first calibration reference point P i Performing queue record insertion processing in a preset first relationship queue reflecting the corresponding relationship between the target motion state and the reference point coordinates to obtain a corresponding first insertion queue record;

[0066] Specifically including: taking the first one in the first relationship queue that is larger than the first calibration reference point P i The first queue record corresponding to the first moment field of the observation time i is recorded as the current queue record; and a new first queue record is inserted at the previous queue record position of the current queue record as the corresponding first inserted queue record; and in the first inserted queue record, the first moment field is set to the first calibration reference point P i The observation time i is set, and the first motion state field is set to empty, and the first sensor type field is set to the first calibration reference point P i The corresponding sensor type c, and the first reference point coordinate field is set to the first calibration reference point P i The coordinates of the reference point in the bird's-eye view coordinate system;

[0067] Step 43, set the first trajectory processing mode to the second mode; go to step 5;

[0068] Step 44: The first calibration reference point P i Discard and set the first trace processing mode to ignore mode.

[0069] Step 5: When the first trajectory processing mode is the ignore mode, no trajectory processing is performed; when the first trajectory processing mode is the first mode, the latest trajectory prediction processing is performed based on the first add queue record, the first relationship queue and the first target center point, and the latest first target trajectory is output; when the first trajectory processing mode is the second mode, the historical trajectory correction processing is performed based on the first insert queue record, the first relationship queue and the first target center point, and the corrected second target trajectory is output;

[0070] Specifically comprising: step 51, when the first track processing mode is the ignore mode, no track processing is performed;

[0071] Here, when the first trajectory processing mode is the ignore mode, the first observation data M is described. c,i The timeliness has expired, and the embodiment of the present invention does not perform any trajectory processing operations at this time;

[0072] Step 52: When the first trajectory processing mode is the first mode, perform latest trajectory prediction processing according to the first added queue record, the first relationship queue and the first target center point and output the latest first target trajectory;

[0073] Here, when the first trajectory processing mode is the first mode, the corresponding latest trajectory prediction processing flow is activated;

[0074] Specifically comprising: step 521, taking the previous first queue record of the first added queue record in the first relation queue as the corresponding previous queue record; and taking the first sensor type field of the previous queue record as the corresponding first sensor type;

[0075] Step 522: When the first sensor type field of the first added queue record is inconsistent with that of the previous queue record, a preset sensor offset vector list is queried, and the offset vector field of the sensor offset vector record whose source sensor field matches the first sensor type field of the previous queue record and whose target sensor field matches the first sensor type field of the first added queue record is extracted as the corresponding current offset vector; and a coordinate offset conversion is performed on the first reference point coordinate field of each first queue record before the first added queue record according to the current offset vector; and the first sensor type field of each first queue record before the first added queue record is set to the sensor type consistent with the first sensor type field of the first added queue record;

[0076] The sensor offset vector list includes multiple sensor offset vector records; the sensor offset vector record includes a source sensor field, a target sensor field, and an offset vector field;

[0077] Here, the current step is to eliminate the coordinate offset of the calibration reference point caused by different sensor types based on the sensor offset vector list; the reason why the coordinate offset of the calibration reference point coordinates of observation data of different types of sensors exists is due to the relative installation positions and sensor parameter differences between different types of sensors. Once the sensor selection is confirmed and the mutual installation positions are confirmed, the coordinate offset between each other can be solidified. The embodiment of the present invention uses a pre-set sensor offset vector list to characterize the coordinate offset between different types of sensors. Each sensor offset vector record in the table corresponds to a set of conversion relationships; after completing the coordinate offset conversion, the reference point coordinates of the first reference point coordinate field of all first queue records can be approximately considered to be the reference point coordinates of the same type of sensor, and the sensor type is the sensor type corresponding to the first sensor type field of the first added queue record, so at this time, the first sensor type field of all first queue records before the first added queue record should be modified to a sensor type consistent with the first sensor type field of the first added queue record;

[0078] Step 523: If the first sensor type field of the first added queue record is consistent with the first sensor type, the first motion state field of the previous queue record is extracted as the corresponding first initial motion state S. 1，0 ; And extract the first reference point coordinate field of the first added queue record as the corresponding first observation coordinate D; with the first initial motion state S 1，0 The first initial state quantity and the first observation quantity are input into a preset Kalman filter for one-step prediction to obtain the corresponding first motion state S; and the first motion state field of the first added queue record is updated according to the first motion state S;

[0079] The first motion state S corresponds to the first observation coordinate D; the first motion state S includes a position coordinate;

[0080] Here, if the first sensor type field of the first added queue record is consistent with the first sensor type, it means that the first relationship queue has not been adjusted through the aforementioned step 522, that is, the smoothness and robustness of each first motion state field of the first relationship queue are guaranteed; then, at this time, the first motion state field of the previous queue record can be directly taken as the initial state quantity, and the first reference point coordinate field of the first added queue record can be taken as the observation quantity to perform a one-step prediction based on the preset Kalman filter to obtain the predicted motion state at the current time t, that is, the first motion state S; the one-step prediction principle of the Kalman filter here can be obtained through public technical literature, and the processing method of the perception module predicting the motion state at the current time based on the one-step prediction of the Kalman filter is also a common processing method, which will not be described in detail here; the first motion state S obtained by the Kalman filter includes a position coordinate in a bird's-eye view coordinate system, which is commonly known as the trajectory point coordinate, and also includes a set of motion component information, such as speed, acceleration, etc.; the obtained first motion state S is the predicted motion state at the current time t, so the first motion state field of the first added queue record should be updated according to the first motion state S;

[0081] Step 524: If the first sensor type field of the first added queue record is inconsistent with the first sensor type, the first motion state field of the first first queue record in the first relationship queue is extracted as the corresponding second initial motion state S. 2，0 ; and the number of records of the first queue records of the first relationship queue is counted to generate the corresponding first number a; and the first reference point coordinate fields of the first queue records from the 2nd to the ath in the first relationship queue are extracted one by one as the corresponding second observation coordinates E j , 1≤j≤a-1; and obtain a-1 second observation coordinates Ej Sort the corresponding moments in chronological order to form the corresponding second observation coordinate sequence {E j}; and in the second initial motion state S 2，0 is the corresponding second initial state quantity, and the second observation coordinate sequence {E j} is the corresponding second observation sequence, the second initial state quantity and the second observation sequence are input into the preset Kalman filter to gradually predict the motion state of j from 1 to a-1 to obtain the corresponding second motion state sequence {S j}; and according to each second motion state S j Updating the first motion status field of the corresponding first queue record in the first relationship queue;

[0082] Among them, the second motion state sequence {S j} includes a-1 second motion states S j ; Second motion state S j and the second observation coordinate E j One-to-one correspondence according to the subscript j; the second motion state S j Includes a location coordinate;

[0083] Here, if the first sensor type field of the first added queue record is inconsistent with the first sensor type, it means that the coordinate offset adjustment of the first relationship queue has been performed through the aforementioned step 522, that is, the smoothness and robustness of each first motion state field of the first relationship queue are not guaranteed; in order to ensure the smoothness of the first motion state field in the first relationship queue, it is necessary to re-predict the first motion state fields of all first queue records except the first first queue record; here, the embodiment of the present invention adopts the step-by-step prediction method of the Kalman filter for prediction processing; the so-called step-by-step prediction method is to use the second initial motion state S 2，0 is the previous motion state quantity, and the second observation coordinate E j＝1 The second motion state S of j=1 is obtained based on the one-step prediction principle of the Kalman filter. j＝1 Make a prediction and then use the second motion state S j＝1 is the previous motion state quantity, and the second observation coordinate E j＝2 The second motion state S of j=2 is obtained based on the one-step prediction principle of the Kalman filter. j＝2 Make predictions, and so on, until the second motion state S j＝a-2 is the previous motion state quantity, and the second observation coordinate E j＝a-1 The second motion state S of j=a-1 is obtained based on the one-step prediction principle of the Kalman filter. j＝a-1 Make a prediction; the final second motion state sequence {Sj The second motion state S in j＝1 The corresponding field is the first motion state field of the j+1=2th first queue record in the first relation queue, and the second motion state S j＝2 The corresponding field is the first motion state field of the j+1=3th first queue record in the first relationship queue, and so on. The second motion state S j＝a-1 The corresponding is the first motion state field of the a-th first queue record in the first relation queue, that is, the first added queue record; based on the second motion state sequence {S j Updating the first relationship queue can ensure that the first motion state field in the first relationship queue maintains good smoothness and robustness;

[0084] Step 525: Extract the position coordinates of the first motion state field of each first queue record of the first relational queue that has completed the motion state field update as the corresponding first trajectory point coordinates (x1, y1); calculate the coordinate offset vector between the last first trajectory point coordinate (x1, y1) and the first target center point to generate a corresponding first offset vector (△x1, △y1); and adjust the horizontal and vertical offsets of each first trajectory point coordinate (x1, y1) according to the first offset vector (△x1, △y1) to generate corresponding second trajectory point coordinates (x2 = x1-△x1, y2 = y1-△y1); and sort all the obtained second trajectory point coordinates (x2, y2) in the order of corresponding time to form the corresponding first target trajectory;

[0085] Here, the first motion state field of the first relationship queue outputted through either step 523 or step 524 has good smoothness and robustness, and the multiple first trajectory point coordinates (x1, y1) generated thereby also have good smoothness and robustness. That is, the trajectory formed by the multiple first trajectory point coordinates (x1, y1) is a smooth trajectory. After obtaining this smooth trajectory, the embodiment of the present invention performs an overall translation of the trajectory toward the target center point. The offset of the overall translation, i.e., the first offset vector (Δx1, Δy1), is determined by the coordinates of the first trajectory point closest to the first target center point, i.e., the coordinates of the last first trajectory point, and the relative offset vector of the first target center point. The smooth trajectory after the overall translation toward the target center point is also the first target trajectory.

[0086] Step 53: When the first trajectory processing mode is the second mode, a historical trajectory correction process is performed according to the first insertion queue record, the first relationship queue, and the first target center point, and a corrected second target trajectory is output;

[0087] Here, when the first trajectory processing mode is the second mode, the corresponding historical trajectory correction processing flow is activated;

[0088] Specifically comprising: step 531, taking the previous first queue record of the first inserted queue record in the first relation queue as the corresponding previous queue record; and taking the first sensor type field of the previous queue record as the corresponding first sensor type;

[0089] Step 532: When the first sensor type field of the first inserted queue record is inconsistent with that of the previous queue record, a preset sensor offset vector list is queried, and the offset vector field of the sensor offset vector record whose source sensor field matches the first sensor type field of the previous queue record and whose target sensor field matches the first sensor type field of the first inserted queue record is extracted as the corresponding current offset vector; and a coordinate offset conversion is performed on the first reference point coordinate field of each first queue record except the first inserted queue record according to the current offset vector; and the first sensor type field of each first queue record except the first inserted queue record is set to a sensor type consistent with the first sensor type field of the first inserted queue record;

[0090] Here, similar to step 522, the current step is to eliminate the coordinate offset of the calibration reference point caused by different sensor types based on the sensor offset vector list. After the coordinate offset conversion is completed, the reference point coordinates of the first reference point coordinate field of all first queue records can be approximately considered to be the reference point coordinates of the same type of sensor, and the sensor type is the sensor type corresponding to the first sensor type field of the first inserted queue record. Therefore, the first sensor type field of all first queue records except the first inserted queue record should be modified to the same sensor type as the first sensor type field of the first inserted queue record.

[0091] Step 533: If the first sensor type field of the first inserted queue record is consistent with the first sensor type, the first motion state field of the previous queue record is extracted as the corresponding third initial motion state S. 3，0 ; and the number of records from the first insertion queue record to the last first queue record is counted to generate the corresponding second number b; and the first reference point coordinate fields from the first insertion queue record to the last first queue record are extracted one by one as the corresponding third observation coordinate F h , 1≤h≤b; and obtain the b third observation coordinates F h Sort the corresponding moments in chronological order to form the corresponding third observation coordinate sequence {F h}; and in the third initial motion state S 3，0 is the corresponding third initial state quantity, and the third observation coordinate sequence {F h} is the corresponding third observation sequence, the third initial state quantity and the third observation sequence are input into the preset Kalman filter to gradually predict the motion state of h from 1 to b to obtain the corresponding third motion state sequence {S h}; and according to each third motion state S h Updating the first motion status field of the corresponding first queue record in the first relationship queue;

[0092] Among them, the third motion state sequence {S h} includes b third motion states S h , the third motion state S h and the third observation coordinate F h According to the subscript h one to one correspondence; the third motion state S h Includes a location coordinate;

[0093] Here, if the first sensor type field of the first insertion queue record is consistent with the first sensor type, it means that the coordinate offset adjustment of the first relationship queue has not been performed through the aforementioned step 532, that is, the smoothness and robustness of each first motion state field before the first insertion queue record in the first relationship queue are guaranteed, and there is no need to adjust each first motion state field before the first insertion queue record when updating the motion state; therefore, it is only necessary to re-predict the first motion state fields from the first insertion queue record to the last first queue record; here, the embodiment of the present invention adopts the step-by-step prediction method of the Kalman filter for prediction processing; the so-called step-by-step prediction method is based on the third initial motion state S 3，0 is the previous motion state quantity, with the third observation coordinate F h＝1 The observation quantity of h=1 is based on the one-step prediction principle of the Kalman filter for the third motion state S of h=1. h＝1 Make a prediction and then use the third motion state S h＝1 is the previous motion state quantity, with the third observation coordinate F h＝2 The observation quantity of h=2 is based on the one-step prediction principle of the Kalman filter for the third motion state S of h=2. h＝2 Make predictions, and so on, until the third motion state S h＝b-1 is the previous motion state quantity, with the third observation coordinate F h＝b The observation quantity of h=b is based on the one-step prediction principle of the Kalman filter for the third motion state S of h=b. h＝b Make a prediction; the final third motion state sequence {S h The third motion state S in h＝1 The corresponding field is the first motion state field of the first insertion queue record, and the third motion state S h＝2The corresponding is the first motion state field of the first queue record next to the first insertion queue record, and so on, the third motion state S h＝b The corresponding is the first motion state field of the last first queue record in the first relation queue; based on the third motion state sequence {S h Updating the first relationship queue can ensure that the first motion state field in the first relationship queue maintains good smoothness and robustness;

[0094] Step 534: If the first sensor type field of the first inserted queue record is inconsistent with the first sensor type, the first motion state field of the first first queue record in the first relation queue is extracted as the corresponding fourth initial motion state S. 4,0 ; and the number of records of the first queue records of the first relationship queue is counted to generate the corresponding third quantity c; and the first reference point coordinate fields of the first queue records from the 2nd to the cth in the first relationship queue are extracted one by one as the corresponding fourth observation coordinates G k , 1≤k≤c-1; and the c-1 fourth observation coordinates G k The corresponding fourth observation coordinate sequence {G k}; and in the fourth initial motion state S 4,0 is the corresponding fourth initial state quantity, and the fourth observation coordinate sequence {G k} is the corresponding fourth observation sequence, the fourth initial state quantity and the fourth observation sequence are input into the Kalman filter to gradually predict the motion state of k from 1 to c-1 to obtain the corresponding fourth motion state sequence {S k}; and according to each fourth motion state S k Updating the first motion status field of the corresponding first queue record in the first relationship queue;

[0095] Among them, the fourth motion state sequence {S k} includes c-1 fourth motion states S k , the fourth motion state S k With the fourth observation coordinate G k One-to-one correspondence according to the subscript k; the fourth motion state S k Includes a location coordinate;

[0096] Here, if the first sensor type field of the first inserted queue record is inconsistent with the first sensor type, it means that the coordinate offset adjustment of the first relationship queue has been performed through the aforementioned step 532, that is, the smoothness and robustness of each first motion state field in the first relationship queue are not guaranteed; in order to ensure that the first motion state field in the first relationship queue has good smoothness and robustness, it is necessary to adjust the first motion state field of all queue records except the first first queue record; therefore, it is necessary to re-predict the first motion state field of the first queue record from the second to the last; here, the embodiment of the present invention adopts the step-by-step prediction method of the Kalman filter for prediction processing; the so-called step-by-step prediction method is based on the fourth initial motion state S 4,0 is the previous motion state quantity, with the fourth observation coordinate G k＝1 The observation quantity of k=1 is based on the one-step prediction principle of the Kalman filter for the fourth motion state S of k=1. k＝1 Make a prediction and then use the fourth motion state S k＝1 is the previous motion state quantity, with the fourth observation coordinate G k＝2 The observation quantity of k=2 is based on the one-step prediction principle of the Kalman filter for the fourth motion state S of k=2. k＝2 Make predictions, and so on, until the fourth motion state S k＝c-2 is the previous motion state quantity, with the fourth observation coordinate G k＝c-1 The observation quantity of k=c-1 is based on the one-step prediction principle of the Kalman filter for the fourth motion state S of k=c-1. k＝c-1 Make a prediction; the final fourth motion state sequence {S k The fourth motion state S in k＝1 The corresponding field is the first motion state field of the second first queue record, and the fourth motion state S k＝2 The corresponding field is the first motion state field of the third first queue record, and so on, the fourth motion state S k＝c-1 The corresponding is the first motion state field of the last first queue record; based on the fourth motion state sequence {S k Updating the first relationship queue can ensure that the first motion state field in the first relationship queue maintains good smoothness and robustness;

[0097] Step 535, extract the position coordinates of the first motion state field of each first queue record of the first relation queue that has completed the motion state field update as the corresponding third trajectory point coordinates (x3, y3); and calculate the coordinate offset vector of the last third trajectory point coordinate (x3, y3) and the first target center point to generate the corresponding second offset vector (△x2, △y2); and adjust the horizontal and vertical offsets of each third trajectory point coordinate (x3, y3) according to the second offset vector (△x2, △y2) to generate the corresponding fourth trajectory point coordinates (x4=x3-△x2, y4=y3-△y2); and sort all the fourth trajectory point coordinates (x4, y4) obtained in the order of corresponding time to form the corresponding second target trajectory.

[0098] Here, the first motion state field of the first relationship queue outputted through either the aforementioned step 533 or 534 has good smoothness and robustness, and the multiple third trajectory point coordinates (x3, y3) generated thereby also have good smoothness and robustness, that is, the trajectory composed of multiple third trajectory point coordinates (x3, y3) is a smooth trajectory; after obtaining this smooth trajectory, the embodiment of the present invention will perform an overall translation of the trajectory toward the target center point, and the offset of the overall translation, that is, the second offset vector (△x2, △y2), is determined by the coordinates of the third trajectory points closest to the first target center point, that is, the last third trajectory point coordinates and the relative offset vector of the first target center point. The smooth trajectory after the overall translation toward the target center point is also the second target trajectory.

[0099] Figure 2 This is a schematic diagram of the structure of an electronic device provided in the second embodiment of the present invention. The electronic device may be the aforementioned terminal device or server, or may be a terminal device or server connected to the aforementioned terminal device or server to implement the method of the embodiment of the present invention. Figure 2 As shown, the electronic device may include: a processor 301 (such as a CPU), a memory 302, and a transceiver 303; the transceiver 303 is coupled to the processor 301, and the processor 301 controls the transceiver 303's transceiver actions. Various instructions may be stored in the memory 302 for completing various processing functions and implementing the processing steps described in the aforementioned method embodiment. Preferably, the electronic device involved in the embodiment of the present invention further includes: a power supply 304, a system bus 305, and a communication port 306. The system bus 305 is used to realize communication connections between components. The above-mentioned communication port 306 is used for connection and communication between the electronic device and other peripherals.

[0100] exist Figure 2The system bus 305 mentioned in the figure can be a Peripheral Component Interconnect (PCI) bus or an Extended Industry Standard Architecture (EISA) bus. The system bus can be divided into an address bus, a data bus, a control bus, etc. For ease of representation, Figure 2 The use of a single bold line in the diagram does not necessarily imply a single bus or type of bus. Communication interfaces enable communication between the database access device and other devices (e.g., clients, read-write libraries, and read-only libraries). Memory may include random access memory (RAM) and non-volatile memory (NVM), such as at least one disk drive.

[0101] The above-mentioned processor can be a general-purpose processor, including a central processing unit (CPU), a network processor (NP), a graphics processing unit (GPU), etc.; it can also be a digital signal processor (DSP), an application specific integrated circuit (ASIC), a field programmable gate array (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components.

[0102] It should be noted that an embodiment of the present invention further provides a computer-readable storage medium, which stores instructions. When the computer-readable storage medium is run on a computer, it enables the computer to execute the methods and processing procedures provided in the above embodiments.

[0103] An embodiment of the present invention further provides a chip for executing instructions, which is used to execute the processing steps described in the above method embodiment.

[0104] The embodiment of the present invention provides a target trajectory processing method for multi-sensor fusion, an electronic device and a computer-readable storage medium. When receiving sensor observation data, a calibration reference point is determined based on the corresponding sensor type and the observation data time; and a trajectory processing mode is determined based on the observation data time. If the observation data time matches the current moment, the trajectory processing mode is confirmed to be the first mode. If the observation data time is earlier than the current moment and does not exceed the pre-set recent time period range, the trajectory processing mode is confirmed to be the second mode. If the observation data time is earlier than the current moment and has exceeded the pre-set recent time period range, the trajectory processing mode is confirmed to be the ignore mode. If the trajectory processing mode is the first mode, the corresponding latest trajectory prediction processing is started. When performing the latest trajectory prediction processing, if the sensor type at the current moment is inconsistent with the historical sensor type, the coordinates of all historical calibration reference points are uniformly adjusted based on the known source-target sensor offset vector and the adjusted calibration reference point sequence is used as the observation sequence for Kalman filtering is used to obtain the latest target trajectory. If the sensor type at the current moment is consistent with the historical sensor type, the calibration reference point at the current moment is used as the observation for one-step prediction to obtain the latest target trajectory. If the trajectory processing mode is the second mode, the corresponding historical trajectory correction processing is started. When performing the historical trajectory correction processing, if the sensor type at the delay moment obtained this time is inconsistent with the historical sensor type, the coordinates of all historical calibration reference points are uniformly adjusted based on the known source-target sensor offset vector, and the Kalman filtering is performed using the adjusted historical calibration reference point sequence as the observation sequence to obtain the corrected latest target trajectory. If the sensor type at the delay moment is consistent with the historical sensor type, the Kalman filtering is performed using the calibration reference point sequence from the delay moment to the last moment as the observation sequence to obtain the corrected latest target trajectory. If the trajectory processing mode is the ignore mode, it means that the observation data obtained this time has become outdated, so the calibration reference point of this time is discarded and no trajectory processing is performed. Through the present invention, the offset of the historical calibration reference point can be dynamically adjusted based on the sensor type corresponding to the observation data obtained each time, and the Kalman filter is performed using the adjusted calibration reference point sequence as the observation sequence to obtain an latest target trajectory, thereby ensuring that the latest target trajectory output at each moment has good smoothness and robustness; the historical motion state can also be adjusted based on the observation data at the delayed moment, thereby achieving the purpose of correcting the target historical trajectory using the observation data at the delayed moment.

[0105] Professionals should also be further aware that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, computer software, or a combination of the two. In order to clearly illustrate the interchangeability of hardware and software, the above description has generally described the components and steps of each example according to their functions. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professionals and technicians can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of the present invention.

[0106] The steps of the methods or algorithms described in conjunction with the embodiments disclosed herein may be implemented using hardware, a software module executed by a processor, or a combination of the two. The software module may be placed in a random access memory (RAM), a memory, a read-only memory (ROM), an electrically programmable ROM, an electrically erasable programmable ROM, a register, a hard disk, a removable disk, a CD-ROM, or any other form of storage medium known in the art.

[0107] The specific implementation methods described above further illustrate the objectives, technical solutions and beneficial effects of the present invention in detail. It should be understood that the above description is only a specific implementation method of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A target trajectory processing method for multi-sensor fusion, characterized in that: The method comprises: At time t, the sensor observation data corresponding to the designated target is obtained as the corresponding first observation data M c,i , and obtain the center point position of the designated target at the latest moment and record it as the corresponding first target center point, and obtain the center point position of the ego vehicle at the latest moment and record it as the corresponding first ego vehicle center point; sensor type c includes laser radar, camera and millimeter wave radar; observation time i ≤ time t; According to the sensor type c, the first observation data M c,i , the observation time i, the time t and the first vehicle center point are used to determine the calibration reference point to generate the corresponding first calibration reference point P i ; If the observation time i is equal to the time t, then according to the first calibration reference point P i Performing queue record addition processing in a preset first relationship queue reflecting the corresponding relationship between the target motion state and the reference point coordinates to obtain a corresponding first added queue record, and setting the first trajectory processing mode to the first mode; If the observation time i is less than the time t, then identify whether the observation time i meets the preset recent time range; if so, then according to the first calibration reference point P i Perform queue record insertion processing in the first relation queue to obtain the corresponding first insertion queue record, and set the first trajectory processing mode to the second mode; if not satisfied, set the first calibration reference point P i discarding and setting the first trajectory processing mode to ignore mode; When the first trajectory processing mode is the ignore mode, no trajectory processing is performed; when the first trajectory processing mode is the first mode, the latest trajectory prediction processing is performed according to the first add queue record, the first relationship queue and the first target center point, and the latest first target trajectory is output; when the first trajectory processing mode is the second mode, the historical trajectory correction processing is performed according to the first insert queue record, the first relationship queue and the first target center point, and the corrected second target trajectory is output; When the sensor type c is a laser radar, the first observation data M c,i It includes a first polygon convex hull and / or a first target detection frame, wherein the first target detection frame includes four vertices; when the sensor type c is a camera, the first observation data M c,i Including a second target detection frame, the second target detection frame includes four vertices; when the sensor type c is a millimeter wave radar, the first observation data M c,i Includes the first radar observation point; The coordinate systems of the first target center point, the first vehicle center point, the first polygon convex hull, the first target detection frame, the second target detection frame, and the first radar observation point are all the same bird's-eye view coordinate system; According to the sensor type c, the first observation data M c,i , the observation time i, the time t and the first vehicle center point are used to determine the calibration reference point to generate the corresponding first calibration reference point P i , specifically including: When the sensor type c is a laser radar, the first observation data M c,i Confirm whether the first target detection frame is included; if it is confirmed that it is not included, estimate the center of gravity of the polygon convex hull of the first polygon and use the estimated center of gravity of the polygon convex hull as the corresponding first calibration reference point P i If it is confirmed to be included, then identify whether the observation time i is less than the time t. If it is less than, the vertex farthest from the first vehicle center point among the four vertices of the first target detection frame is used as the corresponding first calibration reference point P i If it is not less than, the vertex closest to the first vehicle center point among the four vertices of the first target detection frame is used as the corresponding first calibration reference point P i ; When the sensor type c is a camera, identify whether the observation time i is less than the time t; if it is less, the vertex farthest from the first vehicle center point among the four vertices of the second target detection frame is used as the corresponding first calibration reference point P i If it is not less than, the vertex closest to the first vehicle center point among the four vertices of the second target detection frame is used as the corresponding first calibration reference point P i ; When the sensor type c is a millimeter wave radar, the first radar observation point is used as the corresponding first calibration reference point P i .

2. The target trajectory processing method of multi-sensor fusion according to claim 1 is characterized in that: The first relationship queue includes multiple first queue records; the first queue record includes a first time field, a first motion state field, a first sensor type field and a first reference point coordinate field; the time corresponding to the first time field should satisfy the preset recent time period range; the sensor type of the first sensor type field includes lidar, camera and millimeter wave radar.

3. The target trajectory processing method of multi-sensor fusion according to claim 2 is characterized in that: According to the first calibration reference point P i Performing queue record addition processing in a preset first relationship queue reflecting the corresponding relationship between the target motion state and the reference point coordinates to obtain a corresponding first added queue record specifically includes: Add a new first queue record at the end of the first relation queue as the corresponding first added queue record; and in the first added queue record, set the first time field to the first calibration reference point P i The observation time i is set, and the first motion state field is set to empty, and the first sensor type field is set to the first calibration reference point P i The corresponding sensor type c, and the first reference point coordinate field is set to the first calibration reference point P i The coordinates of the reference point in the bird's-eye view coordinate system.

4. The target trajectory processing method of multi-sensor fusion according to claim 2 is characterized in that: According to the first calibration reference point P i Performing queue record insertion processing in the first relationship queue to obtain a corresponding first insertion queue record specifically includes: The first one in the first relationship queue that is larger than the first calibration reference point P i The first queue record corresponding to the first time field of the observation time i is recorded as the current queue record; and a new first queue record is inserted at the previous queue record position of the current queue record as the corresponding first inserted queue record; and in the first inserted queue record, the first time field is set to the first calibration reference point P i The observation time i is set, and the first motion state field is set to empty, and the first sensor type field is set to the first calibration reference point P i The corresponding sensor type c, and the first reference point coordinate field is set to the first calibration reference point P i The coordinates of the reference point in the bird's-eye view coordinate system.

5. The target trajectory processing method of multi-sensor fusion according to claim 2 is characterized in that: The performing the latest trajectory prediction process according to the first added queue record, the first relationship queue, and the first target center point and outputting the latest first target trajectory specifically includes: Using the first queue record preceding the first added queue record in the first relationship queue as the corresponding previous queue record; and using the first sensor type field of the previous queue record as the corresponding first sensor type; When the first add queue record is inconsistent with the first sensor type field of the previous queue record, query the preset sensor offset vector list, extract the offset vector field of the sensor offset vector record whose source sensor field matches the first sensor type field of the previous queue record and whose target sensor field matches the first sensor type field of the first add queue record as the corresponding current offset vector; and perform coordinate offset conversion on the first reference point coordinate field of each of the first queue records before the first add queue record according to the current offset vector; and set the first sensor type field of each of the first queue records before the first add queue record to the sensor type consistent with the first sensor type field of the first add queue record; the sensor offset vector list includes a plurality of sensor offset vector records; the sensor offset vector record includes the source sensor field, the target sensor field and the offset vector field; If the first sensor type field of the first added queue record is consistent with the first sensor type, the first motion state field of the previous queue record is extracted as the corresponding first initial motion state S 1，0 ; and extract the first reference point coordinate field of the first added queue record as the corresponding first observation coordinate D; with the first initial motion state S 1，0 The first initial state quantity and the first observation quantity are input into a preset Kalman filter for one-step prediction to obtain a corresponding first motion state S; and the first motion state field of the first added queue record is updated according to the first motion state S; the first motion state S corresponds to the first observation coordinate D; and the first motion state S includes a position coordinate; If the first sensor type field of the first added queue record is inconsistent with the first sensor type, the first motion state field of the first queue record in the first relationship queue is extracted as the corresponding second initial motion state S 2，0 ; and the number of records of the first queue records of the first relationship queue is counted to generate the corresponding first number a; and the first reference point coordinate fields of the first queue records from the 2nd to the ath in the first relationship queue are extracted one by one as the corresponding second observation coordinates E j , 1≤j≤a-1; and obtain a-1 of the second observation coordinates E j Sort the corresponding moments in chronological order to form the corresponding second observation coordinate sequence {E j }; and with the second initial motion state S 2，0 is the corresponding second initial state quantity, and the second observation coordinate sequence {E j } is the corresponding second observation sequence, the second initial state quantity and the second observation sequence are input into the preset Kalman filter to gradually predict the motion state of j from 1 to a-1 to obtain the corresponding second motion state sequence {S j }; and according to each second motion state S j The first motion state field of the first queue record corresponding to the first relationship queue is updated; the second motion state sequence {S j } includes a-1 of the second motion states S j The second motion state S j With the second observation coordinate E j According to the subscript j one-to-one correspondence; the second motion state S j Includes a location coordinate; The position coordinates of the first motion state field of each first queue record of the first relationship queue that has completed the motion state field update are extracted as the corresponding first trajectory point coordinates (x1, y1); and the coordinate offset vector of the last first trajectory point coordinate (x1, y1) and the first target center point is calculated to generate the corresponding first offset vector (△x1, △y1); and according to the first offset vector (△x1, △y1), the horizontal and vertical offsets of each first trajectory point coordinate (x1, y1) are adjusted to generate the corresponding second trajectory point coordinates (x2=x1-△x1, y2=y1-△y1); and all the obtained second trajectory point coordinates (x2, y2) are sorted in the order of corresponding time to form the corresponding first target trajectory.

6. The target trajectory processing method of multi-sensor fusion according to claim 2 is characterized in that: The performing historical trajectory correction processing according to the first insertion queue record, the first relationship queue, and the first target center point and outputting a corrected second target trajectory specifically includes: Using the first queue record preceding the first inserted queue record in the first relational queue as the corresponding previous queue record; and using the first sensor type field of the previous queue record as the corresponding first sensor type; When the first insert queue record is inconsistent with the first sensor type field of the previous queue record, query the preset sensor offset vector list, extract the offset vector field of the sensor offset vector record whose source sensor field matches the first sensor type field of the previous queue record and whose target sensor field matches the first sensor type field of the first insert queue record as the corresponding current offset vector; and perform coordinate offset conversion on the first reference point coordinate field of each of the first queue records except the first insert queue record according to the current offset vector; and set the first sensor type field of each of the first queue records except the first insert queue record to the sensor type consistent with the first sensor type field of the first insert queue record; the sensor offset vector list includes a plurality of sensor offset vector records; the sensor offset vector record includes the source sensor field, the target sensor field and the offset vector field; If the first sensor type field of the first inserted queue record is consistent with the first sensor type, the first motion state field of the previous queue record is extracted as the corresponding third initial motion state S 3，0 ; and the number of records from the first insertion queue record to the last first queue record is counted to generate a corresponding second number b; and the first reference point coordinate fields from the first insertion queue record to the last first queue record are extracted one by one as the corresponding third observation coordinate F h , 1≤h≤b; and obtain the b third observation coordinates F h Sort the corresponding moments in chronological order to form the corresponding third observation coordinate sequence {F h }; and the third initial motion state S 3，0 is the corresponding third initial state quantity, and the third observation coordinate sequence {F h } is the corresponding third observation sequence, the third initial state quantity and the third observation sequence are input into the preset Kalman filter to gradually predict the motion state of h from 1 to b to obtain the corresponding third motion state sequence {S h }; and according to each third motion state S h The first motion state field of the first queue record corresponding to the first relationship queue is updated; the third motion state sequence {S h } includes b said third motion state S h , the third motion state S h With the third observation coordinate F h According to the subscript h one-to-one correspondence; the third motion state S h Includes a location coordinate; If the first sensor type field of the first insertion queue record is inconsistent with the first sensor type, the first motion state field of the first first queue record in the first relation queue is extracted as the corresponding fourth initial motion state S 4,0 ; and the number of records of the first queue records of the first relationship queue is counted to generate the corresponding third quantity c; and the first reference point coordinate fields of the first queue records from the second to the cth in the first relationship queue are extracted one by one as the corresponding fourth observation coordinates G k , 1≤k≤c-1; and the obtained c-1 fourth observation coordinates G k The corresponding fourth observation coordinate sequence {G k }; and in the fourth initial motion state S 4,0 is the corresponding fourth initial state quantity, and the fourth observation coordinate sequence {G k } is the corresponding fourth observation sequence, the fourth initial state quantity and the fourth observation sequence are input into the Kalman filter to gradually predict the motion state of k from 1 to c-1 to obtain the corresponding fourth motion state sequence {S k }; and according to each fourth motion state S k The first motion state field of the first queue record corresponding to the first relationship queue is updated; the fourth motion state sequence {S k } includes c-1 said fourth motion state S k , the fourth motion state S k With the fourth observation coordinate G k According to the subscript k one-to-one correspondence; the fourth motion state S k Includes a location coordinate; The position coordinates of the first motion state field of each first queue record of the first relationship queue that has completed the motion state field update are extracted as the corresponding third trajectory point coordinates (x3, y3); and the coordinate offset vector of the last third trajectory point coordinate (x3, y3) and the first target center point is calculated to generate the corresponding second offset vector (△x2, △y2); and according to the second offset vector (△x2, △y2), the horizontal and vertical offset amounts of each third trajectory point coordinate (x3, y3) are adjusted to generate the corresponding fourth trajectory point coordinates (x4=x3-△x2, y4=y3-△y2); and all the fourth trajectory point coordinates (x4, y4) obtained are sorted in the order of corresponding time to form the corresponding second target trajectory.

7. An electronic device, characterized in that: include: memory, processors, and transceivers; The processor is configured to be coupled to the memory, read and execute instructions in the memory, so as to implement the method according to any one of claims 1 to 6; The transceiver is coupled to the processor, and the processor controls the transceiver to send and receive messages.

8. A computer-readable storage medium, characterized in that The computer-readable storage medium stores computer instructions, and when the computer instructions are executed by a computer, the computer is caused to execute the method according to any one of claims 1 to 6.

Images