Trajectory tracking method and system based on laser radar and camera fusion perception

Abstract

The invention relates to the technical field of image and video recognition, in particular to a trajectory tracking method and system based on laser radar and camera fusion perception. The method comprises the following steps: monitoring motion parameters of a target object; when the motion parameter of the target object reaches a preset monitoring threshold value, judging that the target object enters a monitoring state, and recording initial motion planning information; the three-dimensional motion direction information of the target object is obtained through the laser radar, and the two-dimensional dynamic image information of the target object is obtained through the camera. Through a data processing technology, an image processing technology and a video identification technology, three-dimensional motion direction information and a two-dimensional dynamic image of a target object are obtained, space-time correlation is carried out, and target fusion track information is constructed; and the trajectory deviation of the target fusion trajectory information is timely and accurately identified, and dynamic trajectory correction is performed on the trajectory deviation, so that the trajectory tracking accuracy is improved.

Description

Technical Field

[0001] The present invention relates to the field of image and video recognition technology, and in particular to a trajectory tracking method and system based on laser radar and camera fusion perception. Background Art

[0002] Initially, trajectory perception mainly relied on a single sensor, such as a camera or a lidar. Cameras can provide rich visual information, but lack depth information and are greatly affected by lighting. Lidar can accurately perceive distance, but it is expensive and generates sparse 3D point clouds. Fusion of the two can make up for the shortcomings of a single sensor. For example, in target detection, the point cloud data of the lidar can determine the position and shape of the target, and the image data of the camera can identify the category of the target. Fusion technology has been further developed and is not only limited to target detection and positioning, but also extended to the field of trajectory tracking tasks. However, when the target motion state changes, the existing trajectory tracking method cannot update the trajectory information in a timely and accurate manner, resulting in the accumulation of trajectory deviations. In addition, the adjustment of trajectory planning deviations is usually relatively static, lacking the ability to respond to dynamic environments and changes in target behavior in real time. Once the target trajectory deviates from the initial plan, it is impossible to evaluate the deviation and make corrections in a timely and accurate manner. Summary of the invention

[0003] Based on this, it is necessary to provide a trajectory tracking method and system based on laser radar and camera fusion perception to solve at least one of the above technical problems.

[0004] To achieve the above purpose, a trajectory tracking method based on laser radar and camera fusion perception is provided, the method comprising the following steps: Step S1: monitoring the motion parameters of the target object; when the motion parameters of the target object reach a preset monitoring threshold, determining that the target object enters a monitoring state, and recording initial motion planning information; Step S2: obtaining three-dimensional motion direction information of the target object through a laser radar, and obtaining two-dimensional dynamic image information of the target object through a camera; performing spatiotemporal correlation based on the three-dimensional motion direction information and the two-dimensional dynamic image to construct target fusion trajectory information; Step S3: judging the motion trajectory range of the target fusion trajectory information and the initial motion planning information. If there is a trajectory planning direction deviation between the target fusion trajectory information and the initial motion planning information, generating trajectory planning deviation data; evaluating the deviation impact factor of the trajectory planning deviation data to obtain the deviation impact factor data; Step S4: Generate trajectory correction data according to the deviation influencing factor data, and perform trajectory correction on the laser radar and camera based on the trajectory correction data to obtain trajectory perception correction data; perform trajectory tracking optimization on the target fusion trajectory information through the trajectory perception correction data to generate a fusion trajectory tracking report.

[0005] By setting a preset monitoring threshold, the present invention can accurately determine whether the target object enters the monitoring state, and timely record the initial motion planning information to ensure that the starting point of trajectory tracking has a clear and accurate reference basis, thereby providing a basis for subsequent trajectory tracking and correction. This threshold-based monitoring method can effectively reduce misjudgment and improve response speed and accuracy. The three-dimensional motion direction information of the target object is obtained by the laser radar, and the two-dimensional dynamic image information of the target object is obtained by the camera, and the two are spatially and temporally associated to construct the target fusion trajectory information, which makes full use of the high precision of the laser radar in distance perception and the advantages of the camera in visual information acquisition, and can more comprehensively and accurately reflect the motion state of the target object. This multi-source data fusion method not only improves the accuracy and stability of trajectory tracking, but also effectively makes up for the shortcomings of a single sensor data and enhances the robustness of the system. Comparing and judging the target fusion trajectory information with the initial motion planning information can timely detect the deviation of trajectory planning direction, generate trajectory planning deviation data, further evaluate the deviation influencing factors, realize quantitative analysis and accurate evaluation of trajectory deviation, and provide a scientific basis for subsequent trajectory correction. Through this deviation detection and evaluation mechanism, the source and degree of trajectory deviation can be identified in real time, so as to achieve more accurate trajectory correction. Generate trajectory correction data according to the deviation influencing factor data, and perform trajectory correction on the lidar and camera based on this data. Optimize the target fusion trajectory information through trajectory perception correction data, realize real-time correction of trajectory deviation, and further improve the accuracy and reliability of trajectory tracking through optimization algorithms; the generated fusion trajectory tracking report can more realistically reflect the actual motion trajectory of the target object, and provide high-quality trajectory data for related applications. In addition, this correction mechanism can also effectively reduce the probability of mismatching and improve the overall performance of trajectory tracking. Therefore, the present invention obtains the three-dimensional motion direction information and two-dimensional dynamic image of the target object through data processing technology, image processing technology and video recognition technology, and performs time-space correlation to construct target fusion trajectory information; timely and accurately identifies the trajectory deviation of the target fusion trajectory information, and dynamically corrects the trajectory deviation, thereby improving the accuracy of trajectory tracking.

[0006] Preferably, step S1 comprises the following steps: Step S11: setting the laser radar emission frequency to 5-20 times / second, and the laser pulse width of each emission to 5-20 nanoseconds; collecting the movement time of the target object based on the laser radar, and recording it as the target movement time; Step S12: setting the shooting frequency of the camera to be in the range of 10-60 frames / second, and the exposure time of each shooting to be in the range of 1 / 1000-1 / 500 seconds; collecting the motion position coordinates of the target object based on the camera, and recording them as the target motion coordinate quantity; Step S13: storing the target movement time and the target movement coordinates in data buffers respectively, wherein the capacity of each buffer ranges from 512 to 2048 bytes and the data storage format is a floating point number, so as to obtain a target monitoring data buffer; Step S14: reading the target motion time and target motion coordinates collected for the last 5-20 times from the target monitoring data buffer, calculating the average position and average speed of the target object, so as to generate the target object motion parameters; Step S15: determining whether the average position of the target object satisfies 0-10 meters, and determining whether the average speed of the target object satisfies 0.1-5 meters per second. If the average position and average speed of the target object are both within the set monitoring threshold range, the target object is determined to enter the monitoring state; Step S16: Record the initial moment when the target object enters the monitoring state as the initial motion time point; perform initial motion trajectory planning based on the initial motion time point and the motion parameters of the target object to generate initial motion planning information.

[0007] The present invention sets the laser radar emission frequency to 5-20 times / second and the laser pulse width to 5-20 nanoseconds, combined with the camera shooting frequency of 10-60 frames / second and the exposure time of 1 / 1000-1 / 500 seconds, to accurately collect the movement time and movement position coordinates of the target object, ensuring high-precision monitoring of the movement state of the target object; the target movement time and target movement coordinates are stored in a data buffer with a capacity of 512-2048 bytes, and a floating point format is used to facilitate fast reading and accurate calculation, effectively improve data processing efficiency, and reduce system delay; read the data collected for the last 5-20 times from the target monitoring data buffer, calculate The average position and average speed of the target object generate accurate target object motion parameters, providing a reliable basis for subsequent motion trajectory planning; by judging whether the average position of the target object is within the range of 0-10 meters and whether the average speed is within the range of 0.1-5 meters per second, it can accurately determine whether the target object has entered the monitoring state, ensuring the reliability and accuracy of the monitoring results; record the initial moment when the target object enters the monitoring state as the initial motion time point, and perform initial motion trajectory planning based on the target object motion parameters at this time and point, generate initial motion planning information, and realize real-time and accurate planning of the target object's motion trajectory to meet the trajectory tracking needs in complex scenarios.

[0008] Preferably, step S2 comprises the following steps: Step S21: Start the laser radar and set the scanning angle range of the laser radar to 0° to 360° in the horizontal direction and -20° to +20° in the vertical direction; Step S22: Setting the scanning frequency of the laser radar to 5-20 times per second, the width of each laser pulse emitted is between 5-20 nanoseconds, and obtaining the multi-point distance information of the target by measuring the round-trip time difference of the laser pulse; Step S23: performing point cloud data processing on the target multi-point distance information, calculating the three-dimensional spatial coordinates of the target multi-point distance information, and extracting the movement direction information of the target object to generate three-dimensional movement direction information; Step S24: Start the camera, set the camera's shooting frequency to 10-30 frames per second, the exposure time to between 1 / 1000-1 / 300 seconds, and adjust the camera's aperture size to F2.8-F5.6; Step S25: collecting contour information of the target object, and determining the centroid position, contour area and contour perimeter, and generating two-dimensional dynamic image information; Step S26: Performing spatiotemporal association based on the three-dimensional motion direction information and the two-dimensional dynamic image to construct target fusion trajectory information.

[0009] The present invention realizes all-round scanning and monitoring of the target object by setting the scanning angle range of the laser radar to 0° to 360° in the horizontal direction and -20° to +20° in the vertical direction, ensuring coverage without blind spots, and is suitable for target tracking tasks in complex environments; the laser radar measures the multi-point distance information of the target with a scanning frequency of 5-20 times per second and a pulse width of 5-20 nanoseconds, and can accurately obtain the dynamic distance data of the target object, perform point cloud data processing on the multi-point distance information of the target, calculate the three-dimensional space coordinates and extract the motion direction information, generate three-dimensional motion direction information, and realize the motion state of the target object in three-dimensional space. Accurate description improves the accuracy of trajectory tracking; the camera collects the contour information of the target object with a shooting frequency of 10-30 frames per second, an exposure time of 1 / 1000-1 / 300 seconds and an aperture size of F2.8-F5.6. It can clearly obtain the center of mass position, contour area and contour perimeter of the target object, generate high-quality two-dimensional dynamic image information, and construct target fusion trajectory information based on time and space correlation of three-dimensional motion direction information and two-dimensional dynamic images, realize deep fusion of lidar and camera data, effectively improve the accuracy and reliability of trajectory tracking, and meet the needs of accurate tracking of target trajectories in complex scenarios.

[0010] Preferably, step S26 includes the following steps: Step S261: extracting the motion direction timestamp from the three-dimensional motion direction information to obtain the motion direction timestamp; extracting the dynamic image timestamp from the two-dimensional dynamic image to obtain the dynamic image timestamp; Step S262: synchronize the motion direction timestamp with the dynamic image timestamp to generate time synchronization data; match the three-dimensional motion direction information with the centroid position in three-dimensional space according to the time synchronization data to generate a target space motion path; Step S263: taking the contour area and the contour perimeter as target contour features, and performing target feature addition based on the target contour features and the target spatial motion path to obtain target trajectory additional data; Step S264: performing a data compliance check on the target trajectory additional data, wherein the data compliance is determined as the contour area is less than or equal to 1000 square pixels, and the contour perimeter is less than or equal to 400 pixels; for data points that do not meet the conditions, mark them as invalid additional data; Step S265: performing trajectory correction and fusion on the target space motion path according to the invalid additional data to generate target fusion trajectory information.

[0011] The present invention extracts the motion direction timestamp of the three-dimensional motion direction information and the dynamic image timestamp of the two-dimensional dynamic image, and performs time processing synchronization to ensure that the data collected by the laser radar and the camera are accurately aligned in the time dimension, thereby providing a high-precision time reference for subsequent data fusion; based on the time synchronization data, the three-dimensional motion direction information is matched with the center of mass position in the three-dimensional space to achieve accurate reconstruction of the motion trajectory of the target object in the three-dimensional space and improve the spatial accuracy of trajectory tracking; the contour area and contour perimeter are used as target contour features, and these features are attached to the target space motion path to generate target trajectory additional data, enrich the trajectory information, and provide more comprehensive feature support for trajectory analysis and application; the target trajectory additional data is checked for data compliance, and it is determined that the contour area is less than or equal to 1000 square pixels and the contour perimeter is less than or equal to 400 pixels as compliance conditions, and the data points that do not meet the conditions are marked as invalid additional data to ensure the accuracy and reliability of the trajectory data.

[0012] As an example of the present invention, refer to Figure 2 As shown, in this example, step S3 includes: Step S31: Calculate the distance difference between the target coordinate value at each time point in the target fusion trajectory information and the initial coordinate value at the corresponding time point in the initial motion planning information to obtain the motion trajectory distance difference; Step S32: Calculate the angle difference between the target motion direction at each time point in the target fusion trajectory information and the target motion direction at the corresponding time point in the initial motion planning information to obtain the motion trajectory angle difference; Step S33: Calculate the area difference between the target contour area at each time point in the target fusion trajectory information and the initial contour area at the corresponding time point in the initial motion planning information to obtain the motion trajectory area difference; Step S34: Calculate the circumference difference between the target contour circumference at each time point in the target fusion trajectory information and the initial contour circumference at the corresponding time point in the initial motion planning information to obtain the motion trajectory circumference difference; Step S35: determining the trajectory planning direction deviation by using the motion trajectory distance difference, motion trajectory angle difference, motion trajectory area difference and motion trajectory perimeter difference to generate trajectory planning deviation data; Step S36: Evaluate the deviation impact factor of the trajectory planning deviation data to obtain deviation impact factor data.

[0013] The present invention realizes accurate quantification of the spatial deviation between the actual motion trajectory of the target and the planned trajectory by calculating the distance difference between the target coordinate value at each time point in the target fusion trajectory information and the initial coordinate value at the corresponding time point in the initial motion planning information; calculates the angle difference between the target motion direction at each time point in the target fusion trajectory information and the target motion direction at the corresponding time point in the initial motion planning information, accurately evaluates the deviation of the target motion direction, and provides an important basis for trajectory adjustment; calculates the area difference between the target contour area at each time point in the target fusion trajectory information and the initial contour area at the corresponding time point in the initial motion planning information, and detects the dynamics of the target contour area. The multi-dimensional evaluation of trajectory deviation is further improved by calculating the target contour circumference difference at each time point in the target fusion trajectory information and the initial contour circumference at the corresponding time point in the initial motion planning information. The distance difference, angle difference, area difference and circumference difference of the motion trajectory are comprehensively processed to fully reflect the deviation between the actual motion trajectory of the target and the planned trajectory. The deviation influencing factor of the trajectory planning deviation data is evaluated to accurately analyze the influence of the deviation on the trajectory tracking task, so as to provide a scientific basis for optimizing trajectory planning and adjusting the tracking strategy.

[0014] Preferably, step S35 includes the following steps: Step S351: determining the trajectory deviation according to the motion trajectory distance difference. If the motion trajectory distance difference is greater than 2 cm, marking the motion trajectory distance difference as an abnormal trajectory distance deviation type; Step S352: determining the trajectory deviation according to the motion trajectory angle difference. If the motion trajectory angle difference is greater than 2 degrees, marking the motion trajectory angle difference as an abnormal trajectory angle deviation type; Step S353: determining the trajectory deviation according to the motion trajectory area difference. If the motion trajectory area difference is greater than 10 square pixels, marking the motion trajectory area difference as an abnormal trajectory angle deviation type; Step S354: determining the trajectory deviation according to the motion trajectory circumference difference. If the motion trajectory circumference difference is greater than 5 pixels, marking the motion trajectory circumference difference as a trajectory circumference abnormal deviation type; Step S355: merging the track distance abnormal deviation type, the track angle abnormal deviation type, the track angle abnormal deviation type and the track circumference abnormal deviation type to generate track planning deviation data.

[0015] The present invention determines the trajectory deviation according to the motion trajectory distance difference. When the motion trajectory distance difference is greater than 2 centimeters, it is marked as the trajectory distance abnormal deviation type, which can accurately identify the significant deviation of the target trajectory in the spatial position, and ensure the sensitivity of the trajectory tracking to the position change; the trajectory deviation is determined according to the motion trajectory angle difference. When the motion trajectory angle difference is greater than 2 degrees, it is marked as the trajectory angle abnormal deviation type, and the deviation of the target trajectory in the motion direction is accurately classified, providing a clear directional basis for trajectory adjustment; the trajectory deviation is determined according to the motion trajectory area difference. When the motion trajectory area difference is greater than 10 square pixels, it is marked as the trajectory area abnormal deviation type, effectively detecting the change deviation of the target contour area, and enhancing the recognition ability of the trajectory tracking for the change of the target morphology; the trajectory deviation is determined according to the motion trajectory perimeter difference. When the motion trajectory perimeter difference is greater than 5 pixels, it is marked as the trajectory perimeter abnormal deviation type, accurately determining the change deviation of the target contour perimeter, and further improving the multi-dimensional detection of the trajectory deviation; the trajectory distance abnormal deviation type, the trajectory angle abnormal deviation type, the trajectory area abnormal deviation type and the trajectory perimeter abnormal deviation type are merged to realize the comprehensive quantification and classification of the target trajectory deviation.

[0016] Preferably, step S36 includes the following steps: Step S361: if at each time point in the target fusion trajectory information, only one of the trajectory distance abnormal deviation type or the trajectory angle abnormal deviation type exists, and neither the trajectory angle abnormal deviation type nor the trajectory circumference abnormal deviation type exists, then the time point is marked as a trajectory low deviation type; Step S362: if each time point in the target fusion trajectory information has any one of the trajectory distance abnormal deviation type and the trajectory angle abnormal deviation type, but does not have the trajectory angle abnormal deviation type and the trajectory circumference abnormal deviation type, then mark the time point as the trajectory deviation type; Step S363: if each time point in the target fusion trajectory information has a trajectory distance abnormal deviation type, a trajectory angle abnormal deviation type, a trajectory angle abnormal deviation type, and a trajectory circumference abnormal deviation type at the same time, then the time point is marked as a trajectory high deviation type; Step S364: assigning an impact factor of 0.5 to the low deviation type, assigning an impact factor of 1.0 to the medium deviation type, and assigning an impact factor of 1.5 to the high deviation type, so as to obtain deviation impact factor data.

[0017] The present invention performs deviation type analysis on each time point in the target fusion trajectory information. When only any one of the trajectory distance abnormal deviation type or the trajectory angle abnormal deviation type exists, and the trajectory area abnormal deviation type and the trajectory circumference abnormal deviation type do not exist, the time point is marked as a trajectory low deviation type, which can accurately identify the severity of the trajectory deviation and provide a clear classification basis for subsequent deviation evaluation; when any one of the trajectory distance abnormal deviation type and the trajectory angle abnormal deviation type exists at the same time, but the trajectory area abnormal deviation type and the trajectory circumference abnormal deviation type do not exist, the time point is marked as a trajectory medium deviation type. This classification process is carried out. The complexity of trajectory deviation is further refined to ensure the accuracy of deviation assessment; when there are trajectory distance abnormal deviation type, trajectory angle abnormal deviation type, trajectory area abnormal deviation type and trajectory circumference abnormal deviation type at the same time, the time point is marked as a trajectory high deviation type, which can comprehensively identify the severity of trajectory deviation and provide key information for trajectory correction; the trajectory low deviation type is assigned an impact factor of 0.5, the trajectory medium deviation type is assigned an impact factor of 1.0, and the trajectory high deviation type is assigned an impact factor of 1.5. Different weights can be assigned according to the specific circumstances of the deviation type to ensure that the dynamic adjustment of trajectory tracking is more reasonable and effective.

[0018] As an example of the present invention, refer to Figure 3 As shown, in this example, step S4 includes: Step S41: When the deviation influence factor data is 0.5, the acquisition frequency of the laser radar and the camera is adjusted to 10%, and the camera image is smoothed by a 3×3 Gaussian filter to obtain low deviation factor adjustment data; Step S42: When the deviation influence factor data is 1.0, the scanning angle of the laser radar is adjusted, the horizontal scanning angle is increased by 5 degrees, and the image of the camera is smoothed by a 5×5 Gaussian filter to obtain the medium deviation factor adjustment data; Step S43: When the deviation influence factor data is 1.5, the scanning frequency of the laser radar is adjusted to increase the scanning frequency by 20%, and the image of the camera is smoothed by a 7×7 Gaussian filter. At the same time, the collected data of the laser radar and the camera are time-synchronized and corrected to obtain high deviation factor adjustment data; Step S44: performing trajectory correction on the laser radar and the camera based on the low deviation factor adjustment data, the medium deviation factor adjustment data, and the high deviation factor adjustment data to obtain trajectory perception correction data; Step S45: Optimize the target fusion trajectory information by trajectory sensing correction data to generate a fusion trajectory tracking report.

[0019] When the deviation impact factor data is 0.5, the present invention adjusts the acquisition frequency of the laser radar and the camera to 10%, and uses a 3×3 Gaussian filter to smooth the camera image, which can reduce system resource consumption and reduce image noise through slight smoothing. When the deviation impact factor data is 1.0, the horizontal scanning angle of the laser radar is increased by 5 degrees, and a 5×5 Gaussian filter is used to smooth the camera image, which can expand the monitoring range of the laser radar, and at the same time, the image is moderately smoothed to improve the accuracy and reliability of trajectory tracking. When the deviation impact factor data is 1.5, the scanning frequency of the laser radar is increased by 20%, and a 7×7 Gaussian filter is used to smooth the camera image. The head image is smoothed and the collected data is time-synchronized and corrected, which can significantly improve the accuracy and stability of trajectory tracking. Especially in the case of high deviation, the accuracy of trajectory tracking can be ensured by enhancing data collection and processing capabilities; based on the low deviation factor adjustment data, the medium deviation factor adjustment data and the high deviation factor adjustment data, the trajectory of the lidar and the camera is corrected, and the system parameters can be flexibly adjusted according to different deviation conditions to ensure that the trajectory tracking can maintain high accuracy and high stability under different deviation levels; the trajectory tracking is optimized by the trajectory perception correction data to optimize the target fusion trajectory information, and the trajectory can be finely adjusted according to the corrected data to ensure the accuracy and completeness of the trajectory tracking results.

[0020] Preferably, step S45 includes the following steps: Step S451: optimizing the two-dimensional trajectory direction of the target fusion trajectory information by using the trajectory perception correction data to obtain two-dimensional trajectory direction optimization data; Step S452: performing three-dimensional trajectory path optimization on the target fusion trajectory information according to the two-dimensional trajectory direction optimization data to obtain three-dimensional trajectory path optimization data; Step S453: performing trajectory tracking update based on the two-dimensional trajectory direction optimization data and the three-dimensional trajectory path optimization data, and generating a fused trajectory tracking report.

[0021] The present invention optimizes the two-dimensional trajectory direction of the target fused trajectory information through trajectory perception correction data, can accurately adjust the movement direction of the target object on the two-dimensional plane, ensure the accuracy of the trajectory direction, provide reliable basic data for subsequent three-dimensional path optimization, and improve the accuracy of trajectory tracking in two-dimensional space; according to the two-dimensional trajectory direction optimization data, the three-dimensional trajectory path optimization is performed on the target fused trajectory information, and the movement path of the target object in the three-dimensional space is further adjusted to ensure the accuracy and completeness of the trajectory path, and improve the accuracy and reliability of trajectory tracking in three-dimensional space; based on the two-dimensional trajectory direction optimization data and the three-dimensional trajectory path optimization data, the trajectory tracking is updated, and the target fused trajectory information is comprehensively updated to ensure the accuracy and completeness of the trajectory tracking result.

[0022] In this specification, a trajectory tracking system based on laser radar and camera fusion perception is provided, which is used to execute the above-mentioned trajectory tracking method based on laser radar and camera fusion perception. The trajectory tracking system based on laser radar and camera fusion perception includes: The target object parameter monitoring module is used to monitor the target object motion parameters; when the target object motion parameters reach a preset monitoring threshold, the target object is determined to enter the monitoring state and the initial motion planning information is recorded; The target fusion trajectory association module is used to obtain the three-dimensional motion direction information of the target object through the laser radar, and obtain the two-dimensional dynamic image information of the target object through the camera; based on the three-dimensional motion direction information and the two-dimensional dynamic image, the time-space association is performed to construct the target fusion trajectory information; The deviation impact assessment module is used to determine the motion trajectory range of the target fusion trajectory information and the initial motion planning information. If there is a trajectory planning direction deviation between the target fusion trajectory information and the initial motion planning information, trajectory planning deviation data is generated; the deviation impact factor of the trajectory planning deviation data is evaluated to obtain the deviation impact factor data; The trajectory perception correction module is used to generate trajectory correction data according to the deviation influencing factor data, and perform trajectory correction on the lidar and camera based on the trajectory correction data to obtain trajectory perception correction data; the trajectory tracking optimization of the target fusion trajectory information is performed through the trajectory perception correction data to generate a fusion trajectory tracking report.

[0023] The target object parameter monitoring module of the present invention can accurately determine whether the target object enters the monitoring state by setting a preset monitoring threshold, and timely record the initial motion planning information to ensure that the starting point of trajectory tracking has a clear and accurate reference basis, thereby providing a basis for subsequent trajectory tracking and correction. This threshold-based monitoring method can effectively reduce misjudgment and improve response speed and accuracy. Through the target fusion trajectory association module, the laser radar obtains the three-dimensional motion direction information of the target object, and the camera is used to obtain the two-dimensional dynamic image information of the target object, and the two are temporally and spatially associated to construct the target fusion trajectory information, which makes full use of the high precision of the laser radar in distance perception and the advantages of the camera in visual information acquisition, and can more comprehensively and accurately reflect the motion state of the target object. This multi-source data fusion method not only improves the accuracy and stability of trajectory tracking, but also effectively makes up for the shortcomings of single sensor data and enhances the robustness of the system. Through the target fusion trajectory association module, the target fusion trajectory information is compared and judged with the initial motion planning information, which can timely discover the trajectory planning direction deviation and generate trajectory planning deviation data, further evaluate the deviation influencing factors, realize the quantitative analysis and accurate evaluation of trajectory deviation, and provide a scientific basis for subsequent trajectory correction. Through this deviation detection and evaluation mechanism, the source and degree of trajectory deviation can be identified in real time, so as to achieve more accurate trajectory correction. Through the trajectory perception correction module, trajectory correction data is generated according to the deviation influencing factor data, and the trajectory of the laser radar and camera is corrected based on this data. The target fusion trajectory information is optimized through the trajectory perception correction data, and the real-time correction of trajectory deviation is realized. The accuracy and reliability of trajectory tracking are further improved through the optimization algorithm; the generated fusion trajectory tracking report can more realistically reflect the actual motion trajectory of the target object, and provide high-quality trajectory data for related applications. In addition, this correction mechanism can also effectively reduce the probability of mismatching and improve the overall performance of trajectory tracking. Therefore, the present invention obtains the three-dimensional motion direction information and two-dimensional dynamic image of the target object through data processing technology, image processing technology and video recognition technology, and performs time-space correlation to construct target fusion trajectory information; timely and accurately identifies the trajectory deviation of the target fusion trajectory information, and dynamically corrects the trajectory deviation, thereby improving the accuracy of trajectory tracking. BRIEF DESCRIPTION OF THE DRAWINGS

[0024] Figure 1 A schematic diagram of the steps of a trajectory tracking method based on laser radar and camera fusion perception; Figure 2 for Figure 1 Detailed implementation steps of step S3 in FIG. Figure 3 for Figure 1Detailed implementation steps of step S4 in FIG. The realization of the purpose, functional features and advantages of the present invention will be further explained in conjunction with embodiments and with reference to the accompanying drawings. DETAILED DESCRIPTION

[0025] The technical method of the present invention is described clearly and completely below in conjunction with the accompanying drawings. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by technicians in this field without creative work are within the scope of protection of the present invention.

[0026] In addition, the accompanying drawings are only schematic illustrations of the present invention and are not necessarily drawn to scale. The same reference numerals in the figures represent the same or similar parts, and their repeated description will be omitted. Some of the block diagrams shown in the accompanying drawings are functional entities and do not necessarily correspond to physically or logically independent entities. The functional entities can be implemented in software form, or implemented in one or more hardware modules or integrated circuits, or implemented in different networks and / or processor methods and / or microcontroller methods.

[0027] It should be understood that, although the terms "first", "second", etc. may be used herein to describe various units, these units should not be limited by these terms. These terms are used only to distinguish one unit from another unit. For example, without departing from the scope of the exemplary embodiments, the first unit may be referred to as the second unit, and similarly the second unit may be referred to as the first unit. The term "and / or" used herein includes any and all combinations of one or more of the listed associated items.

[0028] To achieve this, please refer to Figures 1 to 3 , a trajectory tracking method based on laser radar and camera fusion perception, the method comprising the following steps: Step S1: monitoring the motion parameters of the target object; when the motion parameters of the target object reach a preset monitoring threshold, determining that the target object enters a monitoring state, and recording initial motion planning information; Step S2: obtaining three-dimensional motion direction information of the target object through a laser radar, and obtaining two-dimensional dynamic image information of the target object through a camera; performing spatiotemporal correlation based on the three-dimensional motion direction information and the two-dimensional dynamic image to construct target fusion trajectory information; Step S3: judging the motion trajectory range of the target fusion trajectory information and the initial motion planning information. If there is a trajectory planning direction deviation between the target fusion trajectory information and the initial motion planning information, generating trajectory planning deviation data; evaluating the deviation impact factor of the trajectory planning deviation data to obtain the deviation impact factor data; Step S4: Generate trajectory correction data according to the deviation influencing factor data, and perform trajectory correction on the laser radar and camera based on the trajectory correction data to obtain trajectory perception correction data; perform trajectory tracking optimization on the target fusion trajectory information through the trajectory perception correction data to generate a fusion trajectory tracking report.

[0029] By setting a preset monitoring threshold, the present invention can accurately determine whether the target object enters the monitoring state, and timely record the initial motion planning information to ensure that the starting point of trajectory tracking has a clear and accurate reference basis, thereby providing a basis for subsequent trajectory tracking and correction. This threshold-based monitoring method can effectively reduce misjudgment and improve response speed and accuracy. The three-dimensional motion direction information of the target object is obtained by the laser radar, and the two-dimensional dynamic image information of the target object is obtained by the camera, and the two are spatially and temporally associated to construct the target fusion trajectory information, which makes full use of the high precision of the laser radar in distance perception and the advantages of the camera in visual information acquisition, and can more comprehensively and accurately reflect the motion state of the target object. This multi-source data fusion method not only improves the accuracy and stability of trajectory tracking, but also effectively makes up for the shortcomings of a single sensor data and enhances the robustness of the system. Comparing and judging the target fusion trajectory information with the initial motion planning information can timely detect the deviation of trajectory planning direction, generate trajectory planning deviation data, further evaluate the deviation influencing factors, realize quantitative analysis and accurate evaluation of trajectory deviation, and provide a scientific basis for subsequent trajectory correction. Through this deviation detection and evaluation mechanism, the source and degree of trajectory deviation can be identified in real time, so as to achieve more accurate trajectory correction. Generate trajectory correction data according to the deviation influencing factor data, and perform trajectory correction on the lidar and camera based on this data. Optimize the target fusion trajectory information through trajectory perception correction data, realize real-time correction of trajectory deviation, and further improve the accuracy and reliability of trajectory tracking through optimization algorithms; the generated fusion trajectory tracking report can more realistically reflect the actual motion trajectory of the target object, and provide high-quality trajectory data for related applications. In addition, this correction mechanism can also effectively reduce the probability of mismatching and improve the overall performance of trajectory tracking. Therefore, the present invention obtains the three-dimensional motion direction information and two-dimensional dynamic image of the target object through data processing technology, image processing technology and video recognition technology, and performs time-space correlation to construct target fusion trajectory information; timely and accurately identifies the trajectory deviation of the target fusion trajectory information, and dynamically corrects the trajectory deviation, thereby improving the accuracy of trajectory tracking.

[0030] In the embodiment of the present invention, reference Figure 1 FIG. 1 is a schematic diagram of a step flow of a trajectory tracking method based on laser radar and camera fusion perception of the present invention. In this example, the trajectory tracking method based on laser radar and camera fusion perception includes the following steps: Step S1: monitoring the motion parameters of the target object; when the motion parameters of the target object reach a preset monitoring threshold, determining that the target object enters a monitoring state, and recording initial motion planning information; In the embodiment of the present invention, a laser radar is used to collect point cloud data of the target object and the surrounding environment, and a camera is used to collect image data of the target object and the surrounding environment. The laser radar scans the target object at a specific scanning frequency (such as 10Hz) to obtain point cloud data in its three-dimensional space, including information such as the outline, shape and position of the target object. The camera captures the image of the target object at a set frame rate (such as 30fps) to obtain its two-dimensional visual information; then, the collected point cloud data and image data are registered, and the image data with the smallest time difference with the point cloud data is matched based on the timestamp of the point cloud data, so as to complete the time synchronization of the point cloud data and the image data. Then, the internal and external parameter matrices are obtained through the registration and calibration of the camera internal parameters and the external parameters of the laser radar and the camera, and the information of the point cloud data and the image data are synchronized; after completing the data registration, the point cloud trajectory information is generated based on the registered point cloud data, and the point cloud data target detection algorithm and point cloud segmentation are used to extract the key target point cloud feature information of the point cloud data; specifically, the point cloud segmentation algorithm is used to separate the target object from the background point cloud, and the key point cloud features of the target object, such as the center point and boundary point of the target object, are extracted; finally, the point cloud trajectory information is generated based on the extracted key target point cloud feature information, and the image trajectory information is generated based on the registered image data. The image data detection algorithm is used to perform target detection on the registered image data, and the image data information is extracted to generate the image trajectory information. Specifically, the target object is located in the image through the target detection algorithm, and the two-dimensional bounding box and other information of the target object are extracted, and the image trajectory information is generated based on this information; after the point cloud trajectory information and the image trajectory information are generated, their weight coefficients are determined. For the weight coefficient of the point cloud trajectory information, the number of point clouds and the distance between the laser radar and the target object are obtained based on the collected point cloud data, and the relationship between the number of point clouds and the distance is constructed to determine the relationship between the weight coefficient of the point cloud trajectory information and the distance. For the weight coefficient of the image trajectory information, it is determined based on the relationship between the number of point clouds and the distance. When the motion parameters (such as speed, acceleration, etc.) of the target object reach the preset monitoring threshold, the target object is determined to enter the monitoring state and the initial motion planning information is recorded. For example, the speed threshold of the target object is set to a specific value. When the speed of the target object calculated by the point cloud data and image data reaches the threshold, the monitoring state is triggered, and the initial motion planning information such as the position, speed, and direction of the target object at this time is recorded.

[0031] Step S2: obtaining three-dimensional motion direction information of the target object through a laser radar, and obtaining two-dimensional dynamic image information of the target object through a camera; performing spatiotemporal correlation based on the three-dimensional motion direction information and the two-dimensional dynamic image to construct target fusion trajectory information; In the embodiment of the present invention, a laser radar is used to scan the target object to obtain point cloud data. The laser radar works at a scanning frequency of 10 Hz to scan the target object and its surrounding environment. After obtaining the point cloud data, the point cloud data is denoised to remove noise points while retaining the local features of the scanned object. Specifically, a statistical filtering method is used to calculate the local density of each point, and points with low density are removed, thereby removing noise points. The point cloud segmentation algorithm is used to separate the target object from the background point cloud. The specific operations are as follows: first, a seed point is selected, and then it is gradually grown according to the geometric features of the point cloud (such as the normal direction and curvature), and points with similar features are classified as the same target object; the quality of the target object in two consecutive frames of point cloud data is calculated. The change of the center of mass of the target object in the current frame and the previous frame is calculated, and then the vector difference between the center of mass coordinates of the two frames is calculated, which is the three-dimensional motion direction vector of the target object; the camera is used to capture the image of the target object at a frame rate of 30fps, and the image is grayed to convert the color image into a grayscale image; the weighted average method is used to convert the RGB image into a grayscale image, that is, the grayscale value = 0.299×R+0.587×G+0.114×B; the YOLOv4 target detection algorithm is used to process the image, and the two-dimensional bounding box information of the target object is extracted. After Backbone and Neck extract features, the Head part predicts the position and confidence of the bounding box, and finally the non-maximum value is used. Suppression (NMS) removes overlapping bounding boxes; specifically, the image is input into the YOLOv4 network, and the network outputs the bounding box coordinates (x, y, w, h) and confidence of each target object, and then the bounding boxes with higher confidence are screened out according to the confidence threshold (such as 0.5), and finally the bounding boxes with higher overlap are removed by the NMS algorithm; the three-dimensional motion direction information obtained by the lidar and the two-dimensional dynamic image information obtained by the camera are temporally and spatially associated to construct the target fusion trajectory information. For each target object, its historical position information in continuous frames is extracted and mapped to a high-dimensional feature vector; specifically, the position information of the target object in continuous frames (such as the center of mass coordinates) is extracted, and then through the fully connected layer and position encoding, the target object is fused into a target object. These position information are mapped to a high-dimensional feature space; the spatiotemporal relationship matrix between the target objects is calculated and converted into a binary matrix as the adjacency matrix of the trajectory graph. Specifically, the similarity of the distance and motion direction between each target object is calculated, and then the matrix is ​​converted into a binary matrix according to the similarity threshold (such as 0.5). A value of 1 in the matrix indicates that there is an association between the two target objects, and a value of 0 indicates that there is no association; using the spatiotemporal correlation constraints, a secondary detection is performed on the unmatched unrelated targets in the current frame, and the three-dimensional motion direction information obtained by the lidar and the two-dimensional dynamic image information obtained by the camera are fused. Then, for the unmatched unrelated targets, their relative spatial relationship and motion characteristics with the surrounding reliable objects are used.

[0032] Step S3: judging the motion trajectory range of the target fusion trajectory information and the initial motion planning information. If there is a trajectory planning direction deviation between the target fusion trajectory information and the initial motion planning information, generating trajectory planning deviation data; evaluating the deviation impact factor of the trajectory planning deviation data to obtain the deviation impact factor data; In the embodiment of the present invention, the target fusion trajectory information is compared with the initial motion planning information to determine whether there is a trajectory planning direction deviation, and parameters such as the current position, speed and motion direction of the target object are extracted from the target fusion trajectory information. At the same time, the parameters such as the planned position, speed and movement direction of the target object are extracted from the initial motion planning information; the deviation values ​​of the target object in the current position, speed and movement direction are calculated respectively, and the specific position deviation is obtained by calculating the Euclidean distance between the two positions; the speed deviation is obtained by calculating the difference between the two speed vectors; the movement direction deviation is obtained by calculating the angle between the two direction vectors; the threshold of the trajectory planning direction deviation is set, such as the position deviation threshold is 5 meters, the speed deviation threshold is 2 meters / second, and the movement direction deviation threshold is 10 degrees. If the deviation value of the target object exceeds the corresponding threshold, it is determined that there is a trajectory planning direction deviation; if it is determined that there is a trajectory planning direction deviation, trajectory planning deviation data is generated; the position deviation, speed deviation and movement direction deviation of the target object are recorded to form trajectory planning deviation data, which includes the specific value of the deviation, the occurrence time, the identification of the target object, etc.; the trajectory planning deviation data is organized according to a preset format, and the deviation data is stored as a structured table, each row represents the deviation information of a target object, and each column represents a different deviation parameter; the deviation influencing factor of the trajectory planning deviation data is evaluated, and the evaluation index is determined. According to the actual needs of trajectory tracking, the evaluation index of the deviation influencing factor is determined. Specifically, the deviation affects the continuity, stability, safety and other aspects of the target object's motion trajectory. For each evaluation indicator, the deviation influencing factor of the target object is calculated. For example, the influencing factor of trajectory continuity is calculated by analyzing the position change trend of the target object before and after the deviation occurs. The influencing factor of trajectory stability is calculated by analyzing the speed change of the target object.

[0033] Step S4: Generate trajectory correction data according to the deviation influencing factor data, and perform trajectory correction on the laser radar and camera based on the trajectory correction data to obtain trajectory perception correction data; perform trajectory tracking optimization on the target fusion trajectory information through the trajectory perception correction data to generate a fusion trajectory tracking report.

[0034] In an embodiment of the present invention, trajectory correction data is generated based on deviation influencing factor data, and numerical values ​​of evaluation indicators such as trajectory continuity, stability and safety of the target object are extracted from the deviation influencing factor data; specific parameters of trajectory correction are determined based on the numerical values ​​of the evaluation indicators, and for target objects with poor trajectory continuity, the position correction weight is increased; for target objects with poor trajectory stability, the speed correction weight is increased; based on the determined correction parameters, trajectory correction data is generated, and the trajectory correction data includes information such as the corrected position, speed and movement direction of the target object; the trajectory correction data is used to perform trajectory correction on the laser radar and the camera to obtain trajectory perception correction data, and the specific operations are as follows: based on the trajectory correction data, the scanning parameters of the laser radar, such as the scanning angle and the scanning frequency, are adjusted to improve the detection accuracy of the target object, and for target objects with large deviations in the movement direction, the scanning frequency of the laser radar is increased to obtain a higher frequency. The method comprises the following steps: obtaining the position information of the target object in a simple manner; adjusting the shooting parameters of the camera, such as focal length and exposure time, according to the trajectory correction data, and improving the image quality of the target object; adjusting the focal length of the camera for the target object with a large position deviation so that the target object can be captured more clearly; fusing the corrected data of the laser radar and the camera to generate trajectory perception correction data, which includes information such as the corrected position, speed and movement direction of the target object; optimizing the target fusion trajectory information using the trajectory perception correction data, and generating a fusion trajectory tracking report, wherein the specific operations are as follows: smoothing the target fusion trajectory information to reduce the jitter and discontinuity of the trajectory, and using Gaussian process regression to smooth the 3D position of the target object, thereby improving the smoothness of the trajectory; optimizing the target fusion trajectory information according to the trajectory perception correction data, and adjusting the trajectory prediction model of the target object. For example, the parameters of the trajectory prediction model can be adjusted through the optimization algorithm so that it can more accurately predict the future position of the target object; the optimized target fusion trajectory information is organized into a fusion trajectory tracking report, which includes the trajectory information, correction data and optimization results of the target object; for example, the report records in detail the position, speed and direction of movement of the target object at different time points, as well as the specific parameters and results of the correction and optimization.

[0035] Preferably, step S1 comprises the following steps: Step S11: setting the laser radar emission frequency to 5-20 times / second, and the laser pulse width of each emission to 5-20 nanoseconds; collecting the movement time of the target object based on the laser radar, and recording it as the target movement time; Step S12: setting the shooting frequency of the camera to be in the range of 10-60 frames / second, and the exposure time of each shooting to be in the range of 1 / 1000-1 / 500 seconds; collecting the motion position coordinates of the target object based on the camera, and recording them as the target motion coordinate quantity; Step S13: storing the target movement time and the target movement coordinates in data buffers respectively, wherein the capacity of each buffer ranges from 512 to 2048 bytes and the data storage format is a floating point number, so as to obtain a target monitoring data buffer; Step S14: reading the target motion time and target motion coordinates collected for the last 5-20 times from the target monitoring data buffer, calculating the average position and average speed of the target object, so as to generate the target object motion parameters; Step S15: determining whether the average position of the target object satisfies 0-10 meters, and determining whether the average speed of the target object satisfies 0.1-5 meters per second. If the average position and average speed of the target object are both within the set monitoring threshold range, the target object is determined to enter the monitoring state; Step S16: Record the initial moment when the target object enters the monitoring state as the initial motion time point; perform initial motion trajectory planning based on the initial motion time point and the motion parameters of the target object to generate initial motion planning information.

[0036] In an embodiment of the present invention, the laser radar parameters are set, and the emission frequency of the laser radar is set to 10 times / second through the control interface of the laser radar, and the laser pulse width of each emission is set to 10 nanoseconds, which is adjusted through the configuration software or API of the laser radar; after the laser radar emits a laser pulse each time and receives a reflected signal, the timestamp of the measurement is recorded, and these timestamps are the target motion time; through the control interface of the camera, the shooting frequency of the camera is set to 30 frames / second, and the exposure time of each shooting is set to 1 / 1000 second, which is adjusted through the configuration software or API of the camera; after the camera captures the image of the target object, the image is processed using a target detection algorithm (such as YOLOv11) to extract the coordinates of the center point of the two-dimensional bounding box of the target object, which are the target motion coordinates; two data buffers are created, respectively used to store the target motion time and the target motion coordinates, the capacity of each buffer is set to 1024 bytes, and the data storage format is a floating point number; the collected target motion time and target motion coordinates are sequentially stored in the corresponding data buffer to form a target monitoring data buffer; the target motion time and target motion coordinates collected for the last 10 times are read from the target monitoring data buffer, the position coordinates of the target object in 10 consecutive measurements are accumulated and then averaged, and the average speed of the target object is calculated according to the position change and time interval of the target object in 10 consecutive measurements; it is determined whether the average position of the target object is within the range of 0-10 meters, and whether the average speed of the target object is within the range of 0.1-5 meters per second; if the average position and average speed of the target object are both within the set monitoring threshold range, it is determined that the target object enters the monitoring state; the initial moment when the target object enters the monitoring state is recorded as the initial motion time point, and the initial motion planning information is generated according to the average position and average speed of the target object, combined with the initial motion time point, including parameters such as the expected position and speed of the target object.

[0037] Preferably, step S2 comprises the following steps: Step S21: Start the laser radar and set the scanning angle range of the laser radar to 0° to 360° in the horizontal direction and -20° to +20° in the vertical direction; Step S22: Setting the scanning frequency of the laser radar to 5-20 times per second, the width of each laser pulse emitted is between 5-20 nanoseconds, and obtaining the multi-point distance information of the target by measuring the round-trip time difference of the laser pulse; Step S23: performing point cloud data processing on the target multi-point distance information, calculating the three-dimensional spatial coordinates of the target multi-point distance information, and extracting the movement direction information of the target object to generate three-dimensional movement direction information; Step S24: Start the camera, set the camera's shooting frequency to 10-30 frames per second, the exposure time to between 1 / 1000-1 / 300 seconds, and adjust the camera's aperture size to F2.8-F5.6; Step S25: collecting contour information of the target object, and determining the centroid position, contour area and contour perimeter, and generating two-dimensional dynamic image information; Step S26: Performing spatiotemporal association based on the three-dimensional motion direction information and the two-dimensional dynamic image to construct target fusion trajectory information.

[0038] In the embodiment of the present invention, the laser radar is started through the control interface or configuration software of the laser radar. Specifically, when the Livox Mid-360 laser radar is used, the device is started through its supporting configuration software or ROS driver, and the scanning angle range of the laser radar is set to 0° to 360° in the horizontal direction and -20° to +20° in the vertical direction; the laser radar is adjusted through the configuration file or API, and specifically in the configuration file of the Livox laser radar, the scanning angle range is defined by setting the extrinsic_parameter in lidar_configs; the scanning frequency of the laser radar is set to 10 times per second. The selection of the scanning frequency needs to be determined according to the specific application scenario and the movement speed of the target object; the width of the laser pulse emitted each time is set to 10 nanoseconds. The choice of pulse width will affect the ranging accuracy and detection distance of the laser radar; the laser radar obtains the multi-point distance information of the target by measuring the round-trip time difference of the laser pulse; the multi-point distance information of the target is processed into point cloud data, and the three-dimensional spatial coordinates of the multi-point distance information of the target are calculated, and the distance information is specifically converted into point cloud data in the Cartesian coordinate system; the movement direction information of the target object is extracted by analyzing the movement trend of the target object in the point cloud data, and this is achieved by calculating the position change of the target object in the continuous point cloud data; the extracted movement direction information is integrated into three-dimensional movement direction information for subsequent trajectory tracking; the camera is started through the camera control interface or configuration software, and the camera shooting frequency is set to 30 frames per second. The choice of shooting frequency needs to be determined according to the movement speed of the target object and the performance of the camera; the exposure time is set to 1 / 1000 second. The choice of exposure time will affect the clarity and dynamic range of the image, and the aperture size of the camera is adjusted to F2.8. The choice of aperture size will affect the brightness and depth of field of the image; after the camera captures the image of the target object, it uses an image processing algorithm (such as an edge detection algorithm) to collect the contour information of the target object, calculates the geometric center of the target object's contour, determines the center of mass position, and uses the image processing algorithm to calculate the contour area and perimeter of the target object, and integrates the collected contour information, center of mass position, contour area and perimeter into two-dimensional dynamic image information; performs spatiotemporal association based on the three-dimensional motion direction information and the two-dimensional dynamic image, and achieves this by analyzing the correspondence between the motion direction of the target object in three-dimensional space and the contour information in the two-dimensional image; integrates the associated three-dimensional motion direction information and two-dimensional dynamic image information into target fusion trajectory information.

[0039] More importantly, step S23 includes the following steps: Step S231: performing point cloud data processing on the target multi-point distance information, and setting three coordinate values ​​of X, Y, and Z for each point to generate target point cloud data; Step S232: Calculate the distance between each target point cloud data and the surrounding points. If the distance between a point and the surrounding points exceeds a set threshold, mark the point as a noise point and remove it to obtain denoised point cloud data. Step S233: clustering the denoised point cloud data, dividing the denoised point cloud data into multiple clusters, and calculating the centroid coordinates of each cluster as the target center point coordinates; Step S234: Calculate the initial position and direction of the target object and use it as a reference point; perform translation and rotation transformation on the denoised point cloud data, and align the target center point coordinates with the reference point to obtain target alignment data; Step S235: Calculate the time motion vector of the target alignment data to obtain the time motion vector; perform three-dimensional motion direction mapping on the time motion vector to generate three-dimensional motion direction information.

[0040] In an embodiment of the present invention, the target multi-point distance information collected by the laser radar is processed, and the polar coordinate data (distance, angle) of each point is converted into point cloud data in a Cartesian coordinate system. Specifically, the X, Y, and Z coordinate values ​​of each point are calculated according to the scanning angle of the laser radar and the measured distance; the calculated X, Y, and Z coordinate values ​​are assigned to each point to form target point cloud data, and for each target point cloud data, the distance between it and the surrounding points is calculated, and a statistical filtering method, such as a statistical outlier removal (SOR) method, is used to determine whether there are outliers by calculating the average distance or standard deviation of points in the neighborhood of each point; if the distance between a point and the surrounding points exceeds a set threshold (for example, twice the standard deviation of the average distance), the point is marked as a noise point and removed. After the above processing, the point cloud data after noise removal, that is, the denoised point cloud data, is obtained. The denoised point cloud data is clustered and divided into multiple clusters. The DBSCAN algorithm is used, which can divide the point cloud into different clusters according to the density of the point cloud. For each cluster, its centroid coordinates are calculated as the target center point coordinates, and the centroid coordinates are obtained by calculating the average coordinates of all points in the cluster; the initial position and direction of the target object are determined by analyzing the centroid coordinates of the target object and the geometric shape of the cluster, and the initial position and direction are used as reference points; the denoised point cloud data is translated and rotated to align the target center point coordinates with the reference point, which specifically involves calculating the translation vector and rotation matrix of the target object and applying them to the point cloud data; after the translation and rotation transformation, the target alignment data is obtained; the time motion vector is calculated for the target alignment data, and the time motion vector of each target object is calculated by analyzing the position change of the target object in consecutive frames; the time motion vector is mapped to a three-dimensional motion direction, specifically converting the time motion vector into a motion direction in three-dimensional space.

[0041] Preferably, step S26 includes the following steps: Step S261: extracting the motion direction timestamp from the three-dimensional motion direction information to obtain the motion direction timestamp; extracting the dynamic image timestamp from the two-dimensional dynamic image to obtain the dynamic image timestamp; Step S262: synchronize the motion direction timestamp with the dynamic image timestamp to generate time synchronization data; match the three-dimensional motion direction information with the centroid position in three-dimensional space according to the time synchronization data to generate a target space motion path; Step S263: taking the contour area and the contour perimeter as target contour features, and performing target feature addition based on the target contour features and the target spatial motion path to obtain target trajectory additional data; Step S264: performing a data compliance check on the target trajectory additional data, wherein the data compliance is determined as the contour area is less than or equal to 1000 square pixels, and the contour perimeter is less than or equal to 400 pixels; for data points that do not meet the conditions, mark them as invalid additional data; Step S265: performing trajectory correction and fusion on the target space motion path according to the invalid additional data to generate target fusion trajectory information.

[0042] In the embodiment of the present invention, the timestamp of each data point is extracted from the 3D motion direction information, and these timestamps record the specific time when the laser radar collects data; the timestamp of each image frame is extracted from the 2D dynamic image information, and these timestamps record the specific time when the camera captures the image; the motion direction timestamp and the dynamic image timestamp are time-synchronized. Since the working frequencies of the laser radar and the camera are different, it is necessary to convert the observation data of the same target obtained at different times into a unified fusion time, that is, to calibrate in time; specifically, a hard synchronization method or a soft synchronization method is adopted. The hard synchronization method realizes time synchronization through hardware triggering, while the soft synchronization method performs time calibration through a software algorithm; according to the time synchronization data, the 3D motion direction information is matched with the center of mass position, and the 3D motion direction information is aligned with the center of mass position through interpolation or the nearest neighbor method to generate the target space motion path; the contour area and contour perimeter are used as the target wheel Contour features are extracted from two-dimensional dynamic image information; additional operations are performed based on the target contour features and the target spatial motion path, and the contour area and contour perimeter are combined with the target spatial motion path to obtain the target trajectory additional data; data compliance checks are performed on the target trajectory additional data to check whether the contour area is less than or equal to 1000 square pixels and whether the contour perimeter is less than or equal to 400 pixels; data points that do not meet the conditions are marked as invalid additional data; the target spatial motion path is corrected according to the invalid additional data, and the target spatial motion path is adjusted by analyzing the position and characteristics of the invalid data points, and the abnormal part is removed or corrected; the corrected trajectory is fused with the original trajectory to generate target fused trajectory information, completing the whole process from timestamp extraction, time synchronization, target feature addition, data compliance check to trajectory correction and fusion, and finally generating target fused trajectory information, which provides a basis for subsequent trajectory tracking.

[0043] More importantly, step S265 includes the following steps: Step S2651: Identify path mutation nodes on the target space motion path, and record the speed and direction change characteristics of the path mutation nodes to obtain mutation node feature data; Step S2652: performing correlation analysis on the mutation node feature data and the invalid additional data to obtain node additional correlation data; performing node correction on the target space motion path according to the node additional correlation data to generate node correction data; Step S2653: smoothing the node correction data, inserting multiple interpolation points between each key node, and adjusting the number of interpolation points to obtain node adjustment data; Step S2654: Connect adjacent nodes of the target space motion path according to the node adjustment data, and perform trajectory correction fusion to generate target fusion trajectory information.

[0044] In an embodiment of the present invention, the target space motion path is analyzed, and mutation nodes in the path are identified. Specifically, a method based on wavelet transform is used to detect mutation points. Specifically, the target space motion path is decomposed into wavelet components of different scales and frequencies using wavelet transform, and the changes in detail coefficients are analyzed to find points where the detail coefficients mutate. For the identified mutation nodes, their speed and direction change characteristics are recorded. The speed change characteristics are obtained by calculating the speed difference before and after the mutation node, and the direction change characteristics are obtained by calculating the angle between the direction vectors before and after the mutation node. The mutation node feature data and the invalid additional data are correlated and analyzed. Analyze the position and characteristics of the mutation node to determine the relationship between it and the invalid additional data; if there are more invalid additional data near the mutation node, it means that the mutation node has a large error. According to the results of the association analysis, the mutation node in the target space motion path is corrected. For the mutation node with a large error, it is corrected by interpolation or smoothing to generate node correction data; the node correction data is smoothed and multiple interpolation points are inserted between each key node, such as using the cubic spline interpolation method, and the coordinates of the interpolation points are calculated according to the position and direction information of the key nodes; according to the movement speed and trajectory complexity of the target object, the number of interpolation points is adjusted, and for areas with faster movement speed or larger trajectory changes, the number of interpolation points is increased to finally obtain the node adjustment data; according to the node adjustment data, the adjacent nodes in the target space motion path are connected, and the connection vectors between the adjacent nodes are calculated; the connected trajectory is corrected and fused. During the fusion process, the node adjustment data and the original trajectory information are comprehensively considered to optimize the shape and direction of the trajectory.

[0045] As an example of the present invention, refer to Figure 2 As shown, in this example, step S3 includes: Step S31: Calculate the distance difference between the target coordinate value at each time point in the target fusion trajectory information and the initial coordinate value at the corresponding time point in the initial motion planning information to obtain the motion trajectory distance difference; Step S32: Calculate the angle difference between the target motion direction at each time point in the target fusion trajectory information and the target motion direction at the corresponding time point in the initial motion planning information to obtain the motion trajectory angle difference; Step S33: Calculate the area difference between the target contour area at each time point in the target fusion trajectory information and the initial contour area at the corresponding time point in the initial motion planning information to obtain the motion trajectory area difference; Step S34: Calculate the circumference difference between the target contour circumference at each time point in the target fusion trajectory information and the initial contour circumference at the corresponding time point in the initial motion planning information to obtain the motion trajectory circumference difference; Step S35: determining the trajectory planning direction deviation by using the motion trajectory distance difference, motion trajectory angle difference, motion trajectory area difference and motion trajectory perimeter difference to generate trajectory planning deviation data; Step S36: Evaluate the deviation impact factor of the trajectory planning deviation data to obtain deviation impact factor data.

[0046] In the embodiment of the present invention, the target coordinate value (X, Y, Z) at each time point is extracted from the target fusion trajectory information, and the initial coordinate value at the corresponding time point is extracted from the initial motion planning information; for each time point, the distance difference between the target coordinate value and the initial coordinate value is calculated, and the difference between the target coordinate and the initial coordinate in the three directions of X, Y, and Z is calculated respectively, and then the final distance difference is calculated by the Euclidean distance formula, and the distance difference at each time point is recorded; the target motion direction vector at each time point is extracted from the target fusion trajectory information, and the initial motion direction vector at the corresponding time point is extracted from the initial motion planning information; for each time point, the angle difference between the target motion direction vector and the initial motion direction vector is calculated, specifically by the point of the vector The angle between the two vectors is calculated using the multiplication formula, and then the angle difference is calculated. The angle difference is recorded at each time point to obtain the angle difference of the motion trajectory; the target contour area at each time point is extracted from the target fusion trajectory information, and the initial contour area at the corresponding time point is extracted from the initial motion planning information; for each time point, the difference between the target contour area and the initial contour area is calculated, the absolute value of the difference between the two area values ​​is directly calculated, and the area difference at each time point is recorded; the target contour circumference at each time point is extracted from the target fusion trajectory information, and the initial contour circumference at the corresponding time point is extracted from the initial motion planning information; for each time point, the difference between the target contour circumference and the initial contour circumference is calculated, and the absolute value of the difference between the two circumference values ​​is directly calculated. Record the perimeter difference at each time point to obtain the perimeter difference of the motion trajectory; conduct a comprehensive analysis of the distance difference, angle difference, area difference and perimeter difference of the motion trajectory, and judge whether the deviation at each time point exceeds the threshold range according to the preset deviation threshold (for example, the distance difference threshold is 1 meter, the angle difference threshold is 10 degrees, the area difference threshold is 100 square pixels, and the perimeter difference threshold is 20 pixels); for the time point exceeding the threshold, record its deviation information, including the time point, deviation type (distance, angle, area, perimeter) and specific deviation value, which will be used as trajectory planning deviation data; analyze the trajectory planning deviation data to evaluate the impact of each deviation on trajectory tracking; assign different weights according to the importance of the deviation type, such as the weight of distance deviation is 0.4, the weight of angle deviation is 0.3, the weight of area deviation is 0.2, and the weight of perimeter deviation is 0.1; calculate the deviation impact factor at each time point according to the weight and deviation value, specifically divide each deviation value by its corresponding threshold, and then multiply it by the corresponding weight, and finally add all the results, record the deviation impact factor at each time point, and obtain the deviation impact factor data.

[0047] Preferably, step S35 includes the following steps: Step S351: determining the trajectory deviation according to the motion trajectory distance difference. If the motion trajectory distance difference is greater than 2 cm, marking the motion trajectory distance difference as an abnormal trajectory distance deviation type; Step S352: determining the trajectory deviation according to the motion trajectory angle difference. If the motion trajectory angle difference is greater than 2 degrees, marking the motion trajectory angle difference as an abnormal trajectory angle deviation type; Step S353: determining the trajectory deviation according to the motion trajectory area difference. If the motion trajectory area difference is greater than 10 square pixels, marking the motion trajectory area difference as an abnormal trajectory angle deviation type; Step S354: determining the trajectory deviation according to the motion trajectory circumference difference. If the motion trajectory circumference difference is greater than 5 pixels, marking the motion trajectory circumference difference as a trajectory circumference abnormal deviation type; Step S355: merging the track distance abnormal deviation type, the track angle abnormal deviation type, the track angle abnormal deviation type and the track circumference abnormal deviation type to generate track planning deviation data.

[0048] In the embodiment of the present invention, the target coordinate value (X, Y, Z) at each time point is extracted from the target fusion trajectory information, and the initial coordinate value at the corresponding time point is extracted from the initial motion planning information; for each time point, the distance difference between the target coordinate and the initial coordinate is calculated, and the difference between the target coordinate and the initial coordinate in the three directions of X, Y, and Z is calculated respectively, and then the final distance difference is calculated by the Euclidean distance formula; the calculated distance difference is compared with a preset threshold value (2 cm), and if the distance difference is greater than 2 cm, the motion trajectory distance difference at the time point is marked as "trajectory distance abnormal deviation type", and the specific deviation is recorded. Numerical value and time point information; extract the target motion direction vector at each time point from the target fusion trajectory information, and extract the initial motion direction vector at the corresponding time point from the initial motion planning information; for each time point, calculate the angle difference between the target motion direction vector and the initial motion direction vector, the specific operation is: calculate the angle between the two vectors by the dot product formula of the vectors, and then find the angle difference; compare the calculated angle difference with the preset threshold (2 degrees), if the angle difference is greater than 2 degrees, mark the motion trajectory angle difference at this time point as the trajectory angle abnormal deviation type, and record the specific value and time point information of the deviation; The target contour area at each time point is extracted from the target fusion trajectory information, and the initial contour area at the corresponding time point is extracted from the initial motion planning information; for each time point, the difference between the target contour area and the initial contour area is calculated, and the absolute value of the difference between the two area values ​​is calculated; the calculated area difference is compared with the preset threshold (10 square pixels). If the area difference is greater than 10 square pixels, the motion trajectory area difference at that time point is marked as the trajectory area abnormal deviation type, and the specific value and time point information of the deviation are recorded; the target contour perimeter at each time point is extracted from the target fusion trajectory information, and the initial motion planning information is extracted from the initial motion planning information. The initial contour circumference of the corresponding time point is extracted from the initial motion planning information; for each time point, the difference between the target contour circumference and the initial contour circumference is calculated, and the absolute value of the difference between the two circumference values ​​is calculated; the calculated circumference difference is compared with the preset threshold (5 pixels). If the circumference difference is greater than 5 pixels, the motion trajectory circumference difference at that time point is marked as the trajectory circumference abnormal deviation type, and the specific value and time point information of the deviation are recorded; the merged data is organized into trajectory planning deviation data, including the deviation type, deviation value and corresponding time point information at each time point, which will serve as the basis for subsequent trajectory correction and optimization.

[0049] Preferably, step S36 includes the following steps: Step S361: if at each time point in the target fusion trajectory information, only one of the trajectory distance abnormal deviation type or the trajectory angle abnormal deviation type exists, and neither the trajectory angle abnormal deviation type nor the trajectory circumference abnormal deviation type exists, then the time point is marked as a trajectory low deviation type; Step S362: if each time point in the target fusion trajectory information has any one of the trajectory distance abnormal deviation type and the trajectory angle abnormal deviation type, but does not have the trajectory angle abnormal deviation type and the trajectory circumference abnormal deviation type, then mark the time point as the trajectory deviation type; Step S363: if each time point in the target fusion trajectory information has a trajectory distance abnormal deviation type, a trajectory angle abnormal deviation type, a trajectory angle abnormal deviation type, and a trajectory circumference abnormal deviation type at the same time, then the time point is marked as a trajectory high deviation type; Step S364: assigning an impact factor of 0.5 to the low deviation type, assigning an impact factor of 1.0 to the medium deviation type, and assigning an impact factor of 1.5 to the high deviation type, so as to obtain deviation impact factor data.

[0050] In an embodiment of the present invention, a data extraction tool (such as a Pandas library) is used to extract deviation type information of each time point from the target fusion trajectory information, including trajectory distance abnormal deviation type, trajectory angle abnormal deviation type, trajectory area abnormal deviation type, and trajectory perimeter abnormal deviation type; a data table is created, each row represents a time point, and each column represents a deviation type and its corresponding value; the data of each time point is traversed to check whether the following conditions exist: only any one of the trajectory distance abnormal deviation type or the trajectory angle abnormal deviation type exists; there is no trajectory area abnormal deviation type and trajectory perimeter abnormal deviation type; check whether at least one of the trajectory distance abnormal deviation type and the trajectory angle abnormal deviation type is a non-zero value, and check whether the trajectory area abnormal deviation type and the trajectory perimeter abnormal deviation type are both zero values; if the above conditions are met, a new column is added to the data table for the time point, marked as a trajectory deviation type set; and the specific information of the time point is recorded, including a timestamp, a deviation type, and a deviation value. According to the set of trajectory deviation types, different impact factors are defined: the impact factors are 0.5, 1.0, and 1.5; each time point in the data table is traversed, and the corresponding deviation impact factor is assigned according to the marked deviation type; a new column is added to the data table to store the deviation impact factor at each time point.

[0051] As an example of the present invention, refer to Figure 3 As shown, in this example, step S4 includes: Step S41: When the deviation influence factor data is 0.5, the acquisition frequency of the laser radar and the camera is adjusted to 10%, and the camera image is smoothed by a 3×3 Gaussian filter to obtain low deviation factor adjustment data; Step S42: When the deviation influence factor data is 1.0, the scanning angle of the laser radar is adjusted, the horizontal scanning angle is increased by 5 degrees, and the image of the camera is smoothed by a 5×5 Gaussian filter to obtain the medium deviation factor adjustment data; Step S43: When the deviation influence factor data is 1.5, the scanning frequency of the laser radar is adjusted to increase the scanning frequency by 20%, and the image of the camera is smoothed by a 7×7 Gaussian filter. At the same time, the collected data of the laser radar and the camera are time-synchronized and corrected to obtain high deviation factor adjustment data; Step S44: performing trajectory correction on the laser radar and the camera based on the low deviation factor adjustment data, the medium deviation factor adjustment data, and the high deviation factor adjustment data to obtain trajectory perception correction data; Step S45: Optimize the target fusion trajectory information by trajectory sensing correction data to generate a fusion trajectory tracking report.

[0052] In an embodiment of the present invention, when the deviation impact factor data is 0.5, the acquisition frequencies of the laser radar and the camera are adjusted; specifically, the acquisition frequencies of the laser radar and the camera are reduced to 10% of the original frequency. If the original acquisition frequency is 10Hz, the adjusted acquisition frequency is 1Hz; a 3×3 Gaussian filter is used to smooth the image of the camera. The Gaussian filter smoothes each pixel in the image by weighted averaging, creates a 3×3 Gaussian filter kernel, and its standard deviation is usually set to 1.0. The filter kernel is applied to each pixel of the image, and the image is smoothed by a convolution operation; the adjusted acquisition frequency and the image smoothing result are recorded as low deviation factor adjustment data, including the adjusted acquisition frequency, image smoothing parameters, and the corresponding deviation impact factor; when the deviation impact factor data is 1.0, the scanning angle of the laser radar is adjusted, specifically, the horizontal scanning angle of the laser radar is increased by 5 degrees. If the original horizontal scanning angle is 180 degrees, the adjusted horizontal scanning angle is 185 degrees; use a 5×5 Gaussian filter to smooth the image of the camera, specifically create a 5×5 Gaussian filter kernel, whose standard deviation is usually set to 1.5, apply the filter kernel to each pixel of the image, and smooth the image through a convolution operation; record the adjusted scanning angle and the image smoothing result as the deviation factor adjustment data, including the adjusted scanning angle, image smoothing parameters and the corresponding deviation influence factor; when the deviation influence factor data is 1.5, adjust the scanning frequency of the lidar, specifically increase the scanning frequency of the lidar by 20%.If the original scanning frequency is 10Hz, the adjusted scanning frequency is 12Hz; use a 7×7 Gaussian filter to smooth the image of the camera. Specifically, create a 7×7 Gaussian filter kernel, whose standard deviation is usually set to 2.0, apply the filter kernel to each pixel of the image, and smooth the image through a convolution operation; perform time synchronization correction on the collected data of the lidar and camera, specifically use a hard synchronization method to ensure that the data collection time of the lidar and camera is consistent through a hardware trigger signal; record the adjusted scanning frequency, image smoothing processing results, and time synchronization correction results as high deviation factor adjustment data, including the adjusted scanning frequency, image smoothing processing parameters, time synchronization correction parameters, and corresponding deviation Influencing factors; based on the low deviation factor adjustment data, the medium deviation factor adjustment data and the high deviation factor adjustment data, the laser radar and camera acquisition data are fused; and the adjusted laser radar point cloud data and camera image data are time-aligned; the fused data is used to perform trajectory correction on the target fusion trajectory information, and the trajectory data is optimized through the Kalman filter to reduce the jitter and error of the trajectory; the corrected trajectory data is recorded as trajectory perception correction data, including the corrected trajectory coordinates, speed, direction and other information; the trajectory perception correction data is used to optimize the target fusion trajectory information for trajectory tracking, and the parameters of the trajectory tracking algorithm are adjusted according to the corrected trajectory data; a fusion trajectory tracking report is generated according to the optimized trajectory data. The report content includes information such as the trajectory coordinates, speed, direction, and contour features of the target object, as well as the specific parameters and results of trajectory correction and optimization.

[0053] Preferably, step S45 includes the following steps: Step S451: optimizing the two-dimensional trajectory direction of the target fusion trajectory information by using the trajectory perception correction data to obtain two-dimensional trajectory direction optimization data; Step S452: performing three-dimensional trajectory path optimization on the target fusion trajectory information according to the two-dimensional trajectory direction optimization data to obtain three-dimensional trajectory path optimization data; Step S453: performing trajectory tracking update based on the two-dimensional trajectory direction optimization data and the three-dimensional trajectory path optimization data, and generating a fused trajectory tracking report.

[0054] In an embodiment of the present invention, the two-dimensional part of the target fusion trajectory information is extracted from the trajectory perception correction data, including the two-dimensional coordinates (X, Y) and movement direction of the target object, and the data is stored in a structured data table, where each row represents a time point and each column represents a parameter (such as X coordinate, Y coordinate, and movement direction); the two-dimensional trajectory direction is optimized using a Kalman filter, and the Kalman filter estimates the motion state of the target object through two steps of prediction and update; the state vector and covariance matrix of the Kalman filter are initialized for each target object. The state vector includes the current position and velocity of the target object, and the covariance matrix represents the uncertainty of the initial state; based on the state at the previous moment, the position and velocity of the target object at the current moment are predicted, and the prediction result is corrected in combination with the observation data of the laser radar and the camera. By comparing the predicted value and the observed value, the state estimate of the target object is adjusted; the above prediction and update steps are repeated for each time point, and the two-dimensional trajectory direction of the target object is gradually optimized, and the optimized two-dimensional trajectory direction data is recorded as the two-dimensional trajectory direction optimization data, including the optimized two-dimensional coordinates and motion direction; the three-dimensional part of the target fusion trajectory information is extracted from the trajectory perception correction data, including the three-dimensional coordinates (X, Y, Z) and motion direction of the target object, and these data are stored in a structured data table, where each row represents a time point and each column represents a parameter (such as X coordinate, Y coordinate, Z coordinate, motion direction); the three-dimensional trajectory path is smoothed using a Gaussian filter, and the Gaussian filter smoothes the trajectory data by weighted averaging; appropriate Gaussian filter parameters are selected according to the complexity of the trajectory, such as the size and standard deviation of the filter, such as For more complex trajectories, use a filter size (such as 7×7) and a higher standard deviation; apply a Gaussian filter to the three-dimensional trajectory data, smooth the coordinates of each time point, repeat the above smoothing steps for each time point, and gradually optimize the three-dimensional trajectory path of the target object; record the smoothed three-dimensional trajectory path data as three-dimensional trajectory path optimization data, including optimized three-dimensional coordinates and motion direction; fuse the two-dimensional trajectory direction optimization data and the three-dimensional trajectory path optimization data to generate the final optimized trajectory information, and for each time point, combine the motion direction in the two-dimensional optimization data with the path information in the three-dimensional optimization data to ensure the consistency of the trajectory in two-dimensional and three-dimensional space; align the two-dimensional and three-dimensional optimization data by timestamps to ensure the correct data correspondence at each time point; use the fused optimized trajectory information to update the target fused trajectory information.In each frame, adjust the position and direction of movement of the target object according to the optimized trajectory data; for newly appearing target objects, initialize their trajectory, set the initial state and parameters; for existing target objects, update their state according to the optimized trajectory data, including position, speed and direction of movement, repeat the above update steps for each time point, and gradually optimize the trajectory of the target object; generate a fusion trajectory tracking report based on the updated trajectory information. The report content includes information such as the trajectory coordinates, speed, direction, and contour features of the target object; store the report content in a structured file, such as a CSV file or a JSON file, for subsequent analysis and application.

[0055] In this specification, a trajectory tracking system based on laser radar and camera fusion perception is provided, which is used to execute the above-mentioned trajectory tracking method based on laser radar and camera fusion perception. The trajectory tracking system based on laser radar and camera fusion perception includes: The target object parameter monitoring module is used to monitor the target object motion parameters; when the target object motion parameters reach a preset monitoring threshold, the target object is determined to enter the monitoring state and the initial motion planning information is recorded; The target fusion trajectory association module is used to obtain the three-dimensional motion direction information of the target object through the laser radar, and obtain the two-dimensional dynamic image information of the target object through the camera; based on the three-dimensional motion direction information and the two-dimensional dynamic image, the time-space association is performed to construct the target fusion trajectory information; The deviation impact assessment module is used to determine the motion trajectory range of the target fusion trajectory information and the initial motion planning information. If there is a trajectory planning direction deviation between the target fusion trajectory information and the initial motion planning information, trajectory planning deviation data is generated; the deviation impact factor of the trajectory planning deviation data is evaluated to obtain the deviation impact factor data; The trajectory perception correction module is used to generate trajectory correction data according to the deviation influencing factor data, and perform trajectory correction on the lidar and camera based on the trajectory correction data to obtain trajectory perception correction data; the trajectory tracking optimization of the target fusion trajectory information is performed through the trajectory perception correction data to generate a fusion trajectory tracking report.

[0056] The target object parameter monitoring module of the present invention can accurately determine whether the target object enters the monitoring state by setting a preset monitoring threshold, and timely record the initial motion planning information to ensure that the starting point of trajectory tracking has a clear and accurate reference basis, thereby providing a basis for subsequent trajectory tracking and correction. This threshold-based monitoring method can effectively reduce misjudgment and improve response speed and accuracy. Through the target fusion trajectory association module, the laser radar obtains the three-dimensional motion direction information of the target object, and the camera is used to obtain the two-dimensional dynamic image information of the target object, and the two are temporally and spatially associated to construct the target fusion trajectory information, which makes full use of the high precision of the laser radar in distance perception and the advantages of the camera in visual information acquisition, and can more comprehensively and accurately reflect the motion state of the target object. This multi-source data fusion method not only improves the accuracy and stability of trajectory tracking, but also effectively makes up for the shortcomings of single sensor data and enhances the robustness of the system. Through the target fusion trajectory association module, the target fusion trajectory information is compared and judged with the initial motion planning information, which can timely discover the trajectory planning direction deviation and generate trajectory planning deviation data, further evaluate the deviation influencing factors, realize the quantitative analysis and accurate evaluation of trajectory deviation, and provide a scientific basis for subsequent trajectory correction. Through this deviation detection and evaluation mechanism, the source and degree of trajectory deviation can be identified in real time, so as to achieve more accurate trajectory correction. Through the trajectory perception correction module, trajectory correction data is generated according to the deviation influencing factor data, and the trajectory of the laser radar and camera is corrected based on this data. The target fusion trajectory information is optimized through the trajectory perception correction data, and the real-time correction of trajectory deviation is realized. The accuracy and reliability of trajectory tracking are further improved through the optimization algorithm; the generated fusion trajectory tracking report can more realistically reflect the actual motion trajectory of the target object, and provide high-quality trajectory data for related applications. In addition, this correction mechanism can also effectively reduce the probability of mismatching and improve the overall performance of trajectory tracking. Therefore, the present invention obtains the three-dimensional motion direction information and two-dimensional dynamic image of the target object through data processing technology, image processing technology and video recognition technology, and performs time-space correlation to construct target fusion trajectory information; timely and accurately identifies the trajectory deviation of the target fusion trajectory information, and dynamically corrects the trajectory deviation, thereby improving the accuracy of trajectory tracking.

[0057] Therefore, the embodiments should be regarded as illustrative and non-restrictive from all points, and the scope of the present invention is limited by the appended claims rather than the above description, and it is intended that all changes falling within the meaning and range of equivalent elements of the application documents are included in the present invention.

[0058] The above description is only a specific embodiment of the present invention, so that those skilled in the art can understand or implement the present invention. Various modifications to these embodiments will be apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but should conform to the widest scope consistent with the principles and novel features invented herein.

Claims

1. A trajectory tracking method based on laser radar and camera fusion perception, characterized in that: The following steps are involved: Step S1: monitoring the motion parameters of the target object; when the motion parameters of the target object reach a preset monitoring threshold, determining that the target object enters a monitoring state, and recording initial motion planning information; Step S2: obtaining three-dimensional motion direction information of the target object through a laser radar, and obtaining two-dimensional dynamic image information of the target object through a camera; performing spatiotemporal correlation based on the three-dimensional motion direction information and the two-dimensional dynamic image to construct target fusion trajectory information; Step S3: judging the motion trajectory range of the target fusion trajectory information and the initial motion planning information. If there is a trajectory planning direction deviation between the target fusion trajectory information and the initial motion planning information, generating trajectory planning deviation data; evaluating the deviation impact factor of the trajectory planning deviation data to obtain the deviation impact factor data; Step S4: Generate trajectory correction data according to the deviation influencing factor data, and perform trajectory correction on the laser radar and camera based on the trajectory correction data to obtain trajectory perception correction data; perform trajectory tracking optimization on the target fusion trajectory information through the trajectory perception correction data to generate a fusion trajectory tracking report.

2. The trajectory tracking method based on laser radar and camera fusion perception according to claim 1 is characterized in that: Step S1 includes the following steps: Step S11: setting the laser radar emission frequency to 5-20 times / second, and the laser pulse width of each emission to 5-20 nanoseconds; collecting the movement time of the target object based on the laser radar, and recording it as the target movement time; Step S12: setting the shooting frequency of the camera to be in the range of 10-60 frames / second, and the exposure time of each shooting to be in the range of 1 / 1000-1 / 500 seconds; collecting the motion position coordinates of the target object based on the camera, and recording them as the target motion coordinate quantity; Step S13: storing the target movement time and the target movement coordinates in data buffers respectively, wherein the capacity of each buffer ranges from 512 to 2048 bytes and the data storage format is a floating point number, so as to obtain a target monitoring data buffer; Step S14: reading the target motion time and target motion coordinates collected for the last 5-20 times from the target monitoring data buffer, calculating the average position and average speed of the target object, so as to generate the target object motion parameters; Step S15: determining whether the average position of the target object satisfies 0-10 meters, and determining whether the average speed of the target object satisfies 0.1-5 meters per second. If the average position and average speed of the target object are both within the set monitoring threshold range, the target object is determined to enter the monitoring state; Step S16: Record the initial moment when the target object enters the monitoring state as the initial motion time point; perform initial motion trajectory planning based on the initial motion time point and the motion parameters of the target object to generate initial motion planning information.

3. The trajectory tracking method based on laser radar and camera fusion perception according to claim 1 is characterized in that: Step S2 includes the following steps: Step S21: Start the laser radar and set the scanning angle range of the laser radar to 0° to 360° in the horizontal direction and -20° to +20° in the vertical direction; Step S22: Setting the scanning frequency of the laser radar to 5-20 times per second, the width of each laser pulse emitted is between 5-20 nanoseconds, and obtaining the multi-point distance information of the target by measuring the round-trip time difference of the laser pulse; Step S23: performing point cloud data processing on the target multi-point distance information, calculating the three-dimensional spatial coordinates of the target multi-point distance information, and extracting the movement direction information of the target object to generate three-dimensional movement direction information; Step S24: Start the camera, set the camera's shooting frequency to 10-30 frames per second, the exposure time to between 1 / 1000-1 / 300 seconds, and adjust the camera's aperture size to F2.8-F5.6; Step S25: collecting contour information of the target object, and determining the centroid position, contour area and contour perimeter, and generating two-dimensional dynamic image information; Step S26: Performing spatiotemporal association based on the three-dimensional motion direction information and the two-dimensional dynamic image to construct target fusion trajectory information.

4. The trajectory tracking method based on laser radar and camera fusion perception according to claim 3 is characterized in that: Step S26 includes the following steps: Step S261: extracting the motion direction timestamp from the three-dimensional motion direction information to obtain the motion direction timestamp; extracting the dynamic image timestamp from the two-dimensional dynamic image to obtain the dynamic image timestamp; Step S262: synchronize the motion direction timestamp with the dynamic image timestamp to generate time synchronization data; match the three-dimensional motion direction information with the centroid position in three-dimensional space according to the time synchronization data to generate a target space motion path; Step S263: taking the contour area and the contour perimeter as target contour features, and performing target feature addition based on the target contour features and the target spatial motion path to obtain target trajectory additional data; Step S264: performing a data compliance check on the target trajectory additional data, wherein the data compliance is determined as the contour area is less than or equal to 1000 square pixels, and the contour perimeter is less than or equal to 400 pixels; for data points that do not meet the conditions, mark them as invalid additional data; Step S265: performing trajectory correction and fusion on the target space motion path according to the invalid additional data to generate target fusion trajectory information.

5. The trajectory tracking method based on laser radar and camera fusion perception according to claim 1 is characterized in that: Step S3 includes the following steps: Step S31: Calculate the distance difference between the target coordinate value at each time point in the target fusion trajectory information and the initial coordinate value at the corresponding time point in the initial motion planning information to obtain the motion trajectory distance difference; Step S32: Calculate the angle difference between the target motion direction at each time point in the target fusion trajectory information and the target motion direction at the corresponding time point in the initial motion planning information to obtain the motion trajectory angle difference; Step S33: Calculate the area difference between the target contour area at each time point in the target fusion trajectory information and the initial contour area at the corresponding time point in the initial motion planning information to obtain the motion trajectory area difference; Step S34: Calculate the circumference difference between the target contour circumference at each time point in the target fusion trajectory information and the initial contour circumference at the corresponding time point in the initial motion planning information to obtain the motion trajectory circumference difference; Step S35: determining the trajectory planning direction deviation by using the motion trajectory distance difference, motion trajectory angle difference, motion trajectory area difference and motion trajectory perimeter difference to generate trajectory planning deviation data; Step S36: Evaluate the deviation impact factor of the trajectory planning deviation data to obtain deviation impact factor data.

6. The trajectory tracking method based on laser radar and camera fusion perception according to claim 5 is characterized in that: Step S35 includes the following steps: Step S351: determining the trajectory deviation according to the motion trajectory distance difference. If the motion trajectory distance difference is greater than 2 cm, marking the motion trajectory distance difference as an abnormal trajectory distance deviation type; Step S352: determining the trajectory deviation according to the motion trajectory angle difference. If the motion trajectory angle difference is greater than 2 degrees, marking the motion trajectory angle difference as an abnormal trajectory angle deviation type; Step S353: determining the trajectory deviation according to the motion trajectory area difference. If the motion trajectory area difference is greater than 10 square pixels, marking the motion trajectory area difference as an abnormal trajectory angle deviation type; Step S354: determining the trajectory deviation according to the motion trajectory circumference difference. If the motion trajectory circumference difference is greater than 5 pixels, marking the motion trajectory circumference difference as a trajectory circumference abnormal deviation type; Step S355: merging the track distance abnormal deviation type, the track angle abnormal deviation type, the track angle abnormal deviation type and the track circumference abnormal deviation type to generate track planning deviation data.

7. The trajectory tracking method based on laser radar and camera fusion perception according to claim 6 is characterized in that: Step S36 includes the following steps: Step S361: if at each time point in the target fusion trajectory information, only one of the trajectory distance abnormal deviation type or the trajectory angle abnormal deviation type exists, and neither the trajectory angle abnormal deviation type nor the trajectory circumference abnormal deviation type exists, then the time point is marked as a trajectory low deviation type; Step S362: if each time point in the target fusion trajectory information has any one of the trajectory distance abnormal deviation type and the trajectory angle abnormal deviation type, but does not have the trajectory angle abnormal deviation type and the trajectory circumference abnormal deviation type, then mark the time point as the trajectory deviation type; Step S363: if each time point in the target fusion trajectory information has a trajectory distance abnormal deviation type, a trajectory angle abnormal deviation type, a trajectory angle abnormal deviation type, and a trajectory circumference abnormal deviation type at the same time, then the time point is marked as a trajectory high deviation type; Step S364: assigning an impact factor of 0.5 to the low deviation type, assigning an impact factor of 1.0 to the medium deviation type, and assigning an impact factor of 1.5 to the high deviation type, so as to obtain deviation impact factor data.

8. The trajectory tracking method based on laser radar and camera fusion perception according to claim 7 is characterized in that: Step S4 includes the following steps: Step S41: When the deviation influence factor data is 0.5, the acquisition frequency of the laser radar and the camera is adjusted to 10%, and the camera image is smoothed by a 3×3 Gaussian filter to obtain low deviation factor adjustment data; Step S42: When the deviation influence factor data is 1.0, the scanning angle of the laser radar is adjusted, the horizontal scanning angle is increased by 5 degrees, and the image of the camera is smoothed by a 5×5 Gaussian filter to obtain the medium deviation factor adjustment data; Step S43: When the deviation influence factor data is 1.5, the scanning frequency of the laser radar is adjusted to increase the scanning frequency by 20%, and the image of the camera is smoothed by a 7×7 Gaussian filter. At the same time, the collected data of the laser radar and the camera are time-synchronized and corrected to obtain high deviation factor adjustment data; Step S44: performing trajectory correction on the laser radar and the camera based on the low deviation factor adjustment data, the medium deviation factor adjustment data, and the high deviation factor adjustment data to obtain trajectory perception correction data; Step S45: Optimize the target fusion trajectory information by trajectory sensing correction data to generate a fusion trajectory tracking report.

9. The trajectory tracking method based on laser radar and camera fusion perception according to claim 8 is characterized in that: Step S45 includes the following steps: Step S451: optimizing the two-dimensional trajectory direction of the target fusion trajectory information by using the trajectory perception correction data to obtain two-dimensional trajectory direction optimization data; Step S452: performing three-dimensional trajectory path optimization on the target fusion trajectory information according to the two-dimensional trajectory direction optimization data to obtain three-dimensional trajectory path optimization data; Step S453: performing trajectory tracking update based on the two-dimensional trajectory direction optimization data and the three-dimensional trajectory path optimization data, and generating a fused trajectory tracking report.

10. A trajectory tracking system based on laser radar and camera fusion perception, characterized in that: Used to execute the trajectory tracking method based on laser radar and camera fusion perception as claimed in claim 1, the trajectory tracking system based on laser radar and camera fusion perception includes: The target object parameter monitoring module is used to monitor the target object motion parameters; when the target object motion parameters reach a preset monitoring threshold, the target object is determined to enter the monitoring state and the initial motion planning information is recorded; The target fusion trajectory association module is used to obtain the three-dimensional motion direction information of the target object through the laser radar, and obtain the two-dimensional dynamic image information of the target object through the camera; based on the three-dimensional motion direction information and the two-dimensional dynamic image, the time-space association is performed to construct the target fusion trajectory information; The deviation impact assessment module is used to determine the motion trajectory range of the target fusion trajectory information and the initial motion planning information. If there is a trajectory planning direction deviation between the target fusion trajectory information and the initial motion planning information, trajectory planning deviation data is generated; the deviation impact factor of the trajectory planning deviation data is evaluated to obtain the deviation impact factor data; The trajectory perception correction module is used to generate trajectory correction data according to the deviation influencing factor data, and perform trajectory correction on the lidar and camera based on the trajectory correction data to obtain trajectory perception correction data; the trajectory tracking optimization of the target fusion trajectory information is performed through the trajectory perception correction data to generate a fusion trajectory tracking report.

Images