Lidar and camera trajectory tracking method and system based on bidirectional association

Abstract

This invention discloses a method and system for tracking the trajectory of a LiDAR and camera based on bidirectional correlation. The method includes acquiring first sensing data from the LiDAR and second sensing data from the camera, and performing spatiotemporal registration and coordinate unification; filtering the first sensing data to obtain a first predicted state sequence of the target; completing the state of trajectory interruptions in the second sensing data to obtain a second supplementary state sequence of the target; making a correlation decision for the same target based on the first predicted state sequence and the second supplementary state sequence; and using a multi-model filtering algorithm to fuse and update the target state based on the correlation decision result, outputting a continuous moving trajectory of the target ahead. This invention synchronously registers LiDAR and camera data, fuses radar-based predicted trajectories and vision-based completed trajectories for bidirectional correlation, and finally outputs a continuous target trajectory after multi-model filtering.

Description

Technical Field

[0001] This invention relates to the field of information fusion technology for monitoring open-pit mines, and in particular to a method and system for tracking the trajectory of lidar and camera based on bidirectional correlation. Background Technology

[0002] In the field of autonomous driving, fusing LiDAR and cameras for forward target tracking is a key technology for improving the reliability of environmental perception. Currently, most mainstream fusion tracking methods adopt data-level or decision-level fusion frameworks, but they still face a series of core bottlenecks that have not yet been properly resolved in practical deployments.

[0003] First, at the hardware and data level, the asynchronous data between low-frame-rate LiDAR and high-frame-rate cameras can easily lead to abrupt changes and inconsistencies in the fusion trajectory.

[0004] Secondly, at the level of association and matching algorithms, existing methods rely heavily on calibrated geometric projections for cross-modal data association, which is extremely sensitive to sensor calibration errors and has a high mismatch rate in dense target or dynamic scenes. At the same time, most methods only shallowly fuse detection boxes and fail to make full use of deep features such as point cloud intensity and image semantics, resulting in large deviations in motion prediction of high-speed maneuvering targets, lagging updates of appearance models, and frequent ID switching.

[0005] Furthermore, in terms of adaptability to complex scenarios, traditional methods struggle to maintain trajectory continuity due to severe occlusion and target overlap caused by vehicle platooning and traffic congestion. For fast-moving small targets, the tracking failure rate is high due to sensor resolution limitations and insufficient model generalization ability.

[0006] Finally, regarding system real-time performance, traditional multi-target tracking algorithms suffer from an explosion of state space dimensions and high computational complexity, making it difficult to meet the ≤0.1-second real-time response requirements of in-vehicle systems. While end-to-end deep fusion networks can improve accuracy, they also incur significant computational overhead, making efficient deployment on resource-constrained in-vehicle embedded platforms difficult. These intertwined problems severely restrict the accuracy, robustness, and practicality of existing fusion tracking systems in real-world, complex road scenarios. Summary of the Invention

[0007] The purpose of this invention is to overcome the shortcomings of the existing technology. To achieve the above objective, a method and system for tracking the trajectory of a LiDAR and a camera based on bidirectional correlation is adopted to solve the problems mentioned in the background technology.

[0008] A method for tracking the trajectory of a lidar and camera based on bidirectional correlation includes the following steps:

[0009] Acquire the first perception data from the lidar and the second perception data from the camera, and perform spatiotemporal registration and coordinate unification; The first sensed data is filtered to obtain the first predicted state sequence of the target; The trajectory interruption in the second sensing data is filled in with state information to obtain the second supplementary state sequence of the target; Based on the first predicted state sequence and the second supplementary state sequence, an associated decision is made for the same target; Based on the associated decision results, a multi-model filtering algorithm is used to fuse and update the target state, and output the continuous movement trajectory of the target ahead.

[0010] As a further aspect of the present invention: the coordinate unification in step S1 specifically includes: Establish the extrinsic parameter transformation matrix between the lidar coordinate system, the camera coordinate system, and the vehicle coordinate system; An optimization algorithm based on Lie groups is used to perform online iterative optimization of the extrinsic parameter transformation matrix to compensate for sensor calibration errors, especially the impact of pitch angle deviation on longitudinal distance measurement.

[0011] As a further aspect of the present invention: step S2 specifically involves: using an extended Kalman filter (EKF) to filter the point cloud data of the lidar; The process noise matrix Q of the EKF can be adaptively adjusted according to the point cloud density: when the point cloud density is lower than a preset threshold, the process noise corresponding to the acceleration term in the state vector is increased.

[0012] As a further aspect of the present invention: the association decision in step S4 includes a forward association step: based on the predicted position of the first predicted state sequence, an elliptical association gate is established, and the semi-axis length of the association gate is determined based on the 3σ criterion of the state estimation covariance.

[0013] As a further aspect of the present invention: the association decision in step S4 further includes a reverse backtracking step: for the trajectory interrupted by target occlusion in the second perception data, cubic spline interpolation is used to generate complete state points to form the second supplementary state sequence.

[0014] As a further aspect of the present invention: the association decision in step S4 is based on the comprehensive association degree, which is the weighted sum of Mahalanobis distance and feature matching degree; The feature matching degree is calculated based on the reflection intensity features extracted from the point cloud of the first sensing data and the semantic or texture features extracted from the image of the second sensing data.

[0015] As a further aspect of the present invention: the multi-model filtering algorithm in step S5 is an interactive multi-model (IMM) filter.

[0016] As a further aspect of the present invention, step S5 further includes: constructing a joint sensing measurement vector for the IMM filter. The joint sensing measurement vector It includes at least 3D position, velocity, and point cloud density from the LiDAR, and 2D pixel coordinates, target category, and semantic confidence from the camera; and the joint sensing measurement vector... Different physical quantities in the data are normalized to eliminate dimensional differences; The model set used by the IMM filter includes the uniform velocity CV model, the uniform acceleration CA model, and the constant speed CT model. The state vectors of the uniform velocity CV model are position and velocity, the state vectors of the uniform acceleration CA model are position, velocity and acceleration, and the constant angular velocity CT model is used to describe turning motion with a fixed angular velocity.

[0017] As a further aspect of the present invention: the spatiotemporal registration in step S1 includes: using a hardware synchronization device based on the PTP or gPTP protocol to perform microsecond-level time synchronization between the point cloud generation of the lidar and the image exposure of the camera.

[0018] The second aspect of the technical solution: A forward target movement trajectory tracking system combining lidar and camera, used to implement the method as described in any one of the above statements, the system comprising: The data acquisition and registration module is used to acquire the first sensing data from the lidar and the second sensing data from the camera, and to perform spatiotemporal registration and coordinate unification. The first state prediction module is used to filter the first sensing data to obtain the first predicted state sequence of the target. The second state completion module is used to complete the state of trajectory interruptions in the second sensing data to obtain the second supplementary state sequence of the target. A bidirectional trajectory association module is used to make association decisions for the same target based on the first predicted state sequence and the second supplementary state sequence; The multi-model fusion tracking module is used to fuse and update the target state based on the associated decision results using a multi-model filtering algorithm, and output the continuous movement trajectory of the target ahead.

[0019] Compared with the prior art, the present invention has the following technical advantages: The above technical solution first acquires perception data from LiDAR and cameras simultaneously, and performs temporal and spatial registration and coordinate unification. Then, it filters and predicts the LiDAR data, and completes the state of interrupted trajectories in the camera data due to occlusion or other reasons. Next, it determines whether the predicted state sequence and the completed state sequence belong to the same target by associating them. Finally, based on the association results, it uses interactive multi-model (IMM) filtering algorithms to fuse and update the target state, thereby outputting a continuous and smooth movement trajectory of the target ahead. Through the innovative framework of "prediction-completion-bidirectional association," it effectively solves the problems of trajectory interruption and ID switching caused by target occlusion, sensor asynchrony, and data association failure in traditional methods. Combined with multi-model filtering, it improves the algorithm's adaptability to complex motion modes such as vehicle acceleration and turning, ensuring high-precision tracking while meeting the stringent real-time requirements of automotive embedded platforms, significantly enhancing the environmental perception robustness of the autonomous driving system in complex traffic scenarios. Attached Figure Description

[0020] The specific embodiments of the present invention will now be described in detail with reference to the accompanying drawings: Figure 1 This is a schematic diagram illustrating the steps of the trajectory tracking method according to an embodiment of this application; Figure 2 This is a flowchart of an embodiment disclosed in this application. Detailed Implementation

[0021] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0022] Please refer to Figure 1 As shown in the figure, in this embodiment of the invention, a method for tracking the trajectory of a LiDAR and a camera based on bidirectional correlation includes the following steps: Step S1: Acquire the first perception data from the lidar and the second perception data from the camera, and perform spatiotemporal registration and coordinate unification; The coordinate unification in step S1 specifically includes: Establish the extrinsic parameter transformation matrix between the lidar coordinate system, the camera coordinate system, and the vehicle coordinate system; An optimization algorithm based on Lie groups is used to perform online iterative optimization of the extrinsic parameter transformation matrix to compensate for sensor calibration errors, especially the impact of pitch angle deviation on longitudinal distance measurement.

[0023] The spatiotemporal registration in step S1 includes: using a hardware synchronization device based on the PTP or gPTP protocol to perform microsecond-level time synchronization between the point cloud generation of the lidar and the image exposure of the camera.

[0024] The specific steps for collecting raw perception data are as follows: collecting raw perception data from LiDAR and cameras is a key foundation for achieving multi-sensor fusion.

[0025] LiDAR, based on the time-of-flight method, measures distance by emitting laser pulses and calculating the time difference of reflection. The system includes a laser emitter, a receiver, and a rotating mirror, which generates 3D point cloud data (including coordinates, reflection intensity, and other information) by scanning the environment.

[0026] The camera captures ambient light through its lens, and the CMOS sensor converts the light signal into an electrical signal to form a 2D image frame (containing RGB color and texture information).

[0027] The steps for correcting coordinate transformation errors are as follows: Existing heterogeneous sensor information fusion methods are primarily data-level fusion, meaning they directly fuse the detection data from different sensors. The main challenges of data-level fusion include: ① The inconsistent data rates of different sensors result in differences in the target revisit period for different sensors; ② Measurement data from heterogeneous sensors usually come from different coordinate systems, resulting in coordinate inconsistency.

[0028] Therefore, spatial coordinate transformation is required to fuse measurement information from lidar and cameras.

[0029] Establish the transformation matrix T from the LiDAR and camera to the vehicle coordinate system, considering the installation offset angle. and displacement ( , ).

[0030]

[0031] Considering that coordinate transformation introduces coordinate transformation errors and error coupling problems, a Lie group optimization algorithm is introduced to compensate for sensor calibration errors, especially the impact of pitch angle deviation on longitudinal distance measurement.

[0032] Step S2: Filter the first sensing data to obtain the first predicted state sequence of the target; Step S2 specifically involves filtering the point cloud data of the lidar using an extended Kalman filter (EKF). The process noise matrix Q of the EKF can be adaptively adjusted according to the point cloud density: when the point cloud density is lower than a preset threshold, the process noise corresponding to the acceleration term in the state vector is increased.

[0033] Specifically, the filtering steps of the EKF filter layer in lidar are as follows: The error-corrected measurement values ​​of the lidar are filtered using EKF. Construct a 5-dimensional state vector The observation model considers the distance-angle measurement of the centroid of the lidar point cloud clusters:

[0034] The process noise matrix Q is adaptively adjusted, and the acceleration noise term is increased when the radar point cloud density is less than 200 points / second.

[0035] Step S3: Complete the state of the trajectory interruption in the second sensing data to obtain the second supplementary state sequence of the target; Step S4: Based on the first predicted state sequence and the second supplementary state sequence, make an association decision for the same target; In this embodiment, the association decision in step S4 includes a forward association step: based on the predicted position of the first predicted state sequence, an elliptical association gate is established, and the semi-axis length of the association gate is determined based on the 3σ criterion of the state estimation covariance.

[0036] In this embodiment, the association decision in step S4 also includes a reverse backtracking step: for the trajectory interrupted by target occlusion in the second perception data, cubic spline interpolation is used to generate complete state points to form the second supplementary state sequence.

[0037] In this embodiment, the association decision in step S4 is based on the comprehensive association degree, which is the weighted sum of Mahalanobis distance and feature matching degree; The feature matching degree is calculated based on the reflection intensity features extracted from the point cloud of the first sensing data and the semantic or texture features extracted from the image of the second sensing data.

[0038] Specifically, the spatiotemporal registration steps are as follows: LiDAR and cameras need to be synchronized in time and registered in space; otherwise, the fused data will be distorted.

[0039] Time synchronization: The synchronization box adopts the PTP / gPTP protocol for unified time synchronization, and the exposure and point cloud generation are controlled by trigger pulses to achieve microsecond-level synchronization accuracy.

[0040] Spatial data alignment: Projecting point clouds onto the image coordinate system to match pixels with 3D points.

[0041] The steps for trajectory association are as follows: The broken section of the preceding vehicle's trajectory is then correlated with the camera's measurement information at the point where the preceding vehicle's trajectory is interrupted, using a two-way threshold correlation strategy: Forward association: An elliptical association gate is established based on the EKF predicted position, with a semi-axis length of 3. ; Backtracking: Use cubic spline interpolation to fill in the missing segments of the interrupted camera trajectory; Association criteria: combining Mahalanobis distance (weight 0.6) and feature matching degree (weight 0.4).

[0042] Step S5: Based on the associated decision results, a multi-model filtering algorithm is used to fuse and update the target state, and output the continuous movement trajectory of the target ahead.

[0043] In this embodiment, the multi-model filtering algorithm in step S5 is an interactive multi-model (IMM) filter.

[0044] Specifically, step S5 further includes: constructing a joint sensing measurement vector for the IMM filter. The joint sensing measurement vector It includes at least 3D position, velocity, and point cloud density from the LiDAR, and 2D pixel coordinates, target category, and semantic confidence from the camera; and the joint sensing measurement vector... Different physical quantities in the data are normalized to eliminate dimensional differences; The model set used by the IMM filter includes the uniform velocity CV model, the uniform acceleration CA model, and the constant speed CT model. The state vectors of the uniform velocity CV model are position and velocity, the state vectors of the uniform acceleration CA model are position, velocity and acceleration, and the constant angular velocity CT model is used to describe turning motion with a fixed angular velocity.

[0045] Specifically, the implementation steps for IMM multi-model fusion are as follows: (1) The steps for constructing the associated joint sensing measurement are as follows: LiDAR data: target 3D position (x,y,z), velocity (vx,vy,vz), point cloud density (reflecting the confidence level of the target ahead).

[0046] Camera data: 2D pixel coordinates (u,v), target category (e.g., vehicle / pedestrian), semantic confidence (0-1 probability value).

[0047] Joint vector: ; The difference in dimensions is eliminated by normalization (e.g., the unit of position is unified to meters, and the unit of velocity is unified to m / s).

[0048] (2) The design steps for the state model set are as follows: The successfully correlated perception measurement and design state model set are brought into the IMM filter.

[0049] Design three typical motion models: 1) Uniform velocity model (CV): constant velocity Process noise .

[0050]

[0051]

[0052] : Three-dimensional identity matrix, used to construct the block structure of the state transition matrix to ensure the linear superposition of position and velocity; T: sampling time interval; : The velocity components of the target in the x and y directions.

[0053] 2) Uniform acceleration model (CA): constant acceleration

[0054]

[0055] : The acceleration components of the target in the x and y directions.

[0056] 3) Transition Model (CT): Constant rotation rate and speed.

[0057]

[0058] : Fixed angular velocity (rotation rate), unit is rad / s; Time step; Finally, the trajectory of the target ahead is output.

[0059] The second aspect of the technical solution: A forward target movement trajectory tracking system combining lidar and camera, used to implement the method as described in any one of the above statements, the system comprising: The data acquisition and registration module is used to acquire the first sensing data from the lidar and the second sensing data from the camera, and to perform spatiotemporal registration and coordinate unification. The first state prediction module is used to filter the first sensing data to obtain the first predicted state sequence of the target. The second state completion module is used to complete the state of trajectory interruptions in the second sensing data to obtain the second supplementary state sequence of the target. A bidirectional trajectory association module is used to make association decisions for the same target based on the first predicted state sequence and the second supplementary state sequence; The multi-model fusion tracking module is used to fuse and update the target state based on the associated decision results using a multi-model filtering algorithm, and output the continuous movement trajectory of the target ahead.

[0060] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention. The scope of the invention is defined by the appended claims and their equivalents, all of which should be included within the scope of protection of the invention.

Claims

1. A method for tracking the trajectory of a moving target in front using a combination of lidar and camera, characterized in that, Includes the following steps: Acquire the first perception data from the lidar and the second perception data from the camera, and perform spatiotemporal registration and coordinate unification; The first sensed data is filtered to obtain the first predicted state sequence of the target; The trajectory interruption in the second sensing data is filled in with state information to obtain the second supplementary state sequence of the target; Based on the first predicted state sequence and the second supplementary state sequence, an associated decision is made for the same target; Based on the associated decision results, a multi-model filtering algorithm is used to fuse and update the target state, and output the continuous movement trajectory of the target ahead.

2. The method according to claim 1, characterized in that, The coordinate unification in step S1 specifically includes: Establish the extrinsic parameter transformation matrix between the lidar coordinate system, the camera coordinate system, and the vehicle coordinate system; An optimization algorithm based on Lie groups is used to perform online iterative optimization of the extrinsic parameter transformation matrix to compensate for sensor calibration errors, especially the impact of pitch angle deviation on longitudinal distance measurement.

3. The method according to claim 1, characterized in that, Step S2 specifically involves: using an extended Kalman filter (EKF) to filter the point cloud data of the lidar. The process noise matrix Q of the EKF can be adaptively adjusted according to the point cloud density: when the point cloud density is lower than a preset threshold, the process noise corresponding to the acceleration term in the state vector is increased.

4. The method according to claim 1, characterized in that, The association decision in step S4 includes a forward association step: based on the predicted position of the first predicted state sequence, an elliptical association gate is established, and the semi-axis length of the association gate is determined based on the 3σ criterion of the state estimation covariance.

5. The method according to claim 4, characterized in that, The association decision in step S4 also includes a reverse backtracking step: for the trajectory interrupted by target occlusion in the second perception data, cubic spline interpolation is used to generate complete state points to form the second supplementary state sequence.

6. The method according to claim 5, characterized in that, The association decision in step S4 is based on the comprehensive association degree, which is the weighted sum of Mahalanobis distance and feature matching degree. The feature matching degree is calculated based on the reflection intensity features extracted from the point cloud of the first sensing data and the semantic or texture features extracted from the image of the second sensing data.

7. The method according to claim 1, characterized in that, The multi-model filtering algorithm in step S5 is an interactive multi-model (IMM) filter.

8. The method according to claim 7, characterized in that, Step S5 further includes: constructing a joint sensing measurement vector for the IMM filter. The joint sensing measurement vector It includes at least 3D position, velocity, and point cloud density from the LiDAR, and 2D pixel coordinates, target category, and semantic confidence from the camera; and the joint sensing measurement vector... Different physical quantities in the data are normalized to eliminate dimensional differences; The model set used by the IMM filter includes the uniform velocity CV model, the uniform acceleration CA model, and the constant speed CT model. The state vectors of the uniform velocity CV model are position and velocity, the state vectors of the uniform acceleration CA model are position, velocity and acceleration, and the constant angular velocity CT model is used to describe turning motion with a fixed angular velocity.

9. The method according to claim 1, characterized in that, The spatiotemporal registration in step S1 includes: using a hardware synchronization device based on the PTP or gPTP protocol to perform microsecond-level time synchronization between the point cloud generation of the lidar and the image exposure of the camera.

10. A forward target movement trajectory tracking system combining lidar and camera, characterized in that, The system for implementing the method as described in any one of claims 1 to 9 comprises: The data acquisition and registration module is used to acquire the first sensing data from the lidar and the second sensing data from the camera, and to perform spatiotemporal registration and coordinate unification. The first state prediction module is used to filter the first sensing data to obtain the first predicted state sequence of the target. The second state completion module is used to complete the state of trajectory interruptions in the second sensing data to obtain the second supplementary state sequence of the target. A bidirectional trajectory association module is used to make association decisions for the same target based on the first predicted state sequence and the second supplementary state sequence; The multi-model fusion tracking module is used to fuse and update the target state based on the associated decision results using a multi-model filtering algorithm, and output the continuous movement trajectory of the target ahead.

Images