A method and system for estimating and displaying the motion state of a target in a laser radar point cloud

Abstract

The application provides a laser radar point cloud target motion state estimation and display method and system. The method comprises the following steps: receiving laser radar original point cloud data, adopting a spherical projection algorithm to reduce dimension and map three-dimensional point cloud data into a two-dimensional depth projection image; inputting the projection image into a pre-trained target detection model for 2D target detection; according to the 2D detection result, reversely searching target 3D point cloud in the original point cloud and generating a 3D bounding box; projecting the target 3D point cloud to a bird's eye view plane to generate two-dimensional contour parameters; calculating the target motion state based on continuous multiple frame contour parameters and performing smoothing processing; and presenting the target motion state in a visualized manner in real time. Through the fusion of 2D image semantic information and 3D point cloud spatial geometric information, the application solves the problems in the prior art, such as high point cloud target detection calculation complexity, low 2D to 3D reverse projection accuracy, inaccurate 3D target positioning and direction estimation, and realizes high-precision target detection, robust motion parameter estimation and real-time visualization.

Description

Technical Field

[0001] This invention relates to the fields of intelligent sensing and computer vision technology, and more particularly to a method and system for estimating and displaying the motion state of a target in a lidar point cloud. Background Technology

[0002] Point cloud data dynamically captured by lidar has irreplaceable application value in high-level perception scenarios such as autonomous driving, intelligent robots, remote monitoring, and autonomous ship navigation. As a core environmental perception sensor, lidar needs to achieve high-precision detection, localization, tracking, and motion state estimation of surrounding dynamic targets (such as vehicles, pedestrians, ships, and obstacles).

[0003] However, relying solely on lidar point clouds for target motion state estimation and visualization has obvious limitations: on the one hand, the point cloud data volume is large and the computational complexity is high; on the other hand, the point cloud quality is unstable due to environmental interference (such as lidar noise, rain, fog, and ocean splashes), which can easily lead to a decrease in estimation accuracy and a reduction in system efficiency.

[0004] Existing point cloud target motion state estimation and display methods have the following technical shortcomings:

[0005] First, the data sources are not fully utilized. Traditional methods typically process point clouds or images separately, making it difficult to fully leverage the advantages of both data sources. Direct point cloud detection methods (such as PointNet) have high computational complexity and are difficult to process in real time; image detection methods cannot accurately obtain 3D position information, and there is a lack of effective mechanisms to complement each other's strengths.

[0006] Second, the accuracy of 2D to 3D backprojection is low. When using 2D detection boxes to backproject and obtain the corresponding 3D point cloud, existing methods often use simple interpolation or coarse cropping strategies, which have obvious limitations: on the one hand, they do not fully consider the sparsity of point clouds, sensor noise and boundary blurring; on the other hand, they lack effective point cloud filtering, noise reduction and integrity enhancement mechanisms. Especially when the target is partially occluded, at a distance or in a complex background (such as water surface reflection, rain and fog interference), the reconstruction results often show positional shift, size distortion or even target loss.

[0007] Third, the 3D target localization and orientation estimation are inaccurate, making it difficult to generate a directed 3D bounding box (OBB) that can accurately describe the target's position and size as well as reflect its orientation; the simple minimum bounding cube method has low accuracy, and the PCA method is sensitive to point cloud distribution and has difficulty handling targets with irregular shapes.

[0008] Therefore, there is an urgent need for a new method for estimating and displaying the motion state of LiDAR point clouds targets. This method should be able to achieve high-precision 3D reconstruction and robust motion parameter estimation by fusing 2D semantic and 3D geometric information, while ensuring the integrity of spatial geometric information. It should also support real-time transmission and visualization, thereby comprehensively improving the perception capability and decision reliability of intelligent systems in complex dynamic environments. Summary of the Invention

[0009] To address the aforementioned technical problems of high computational complexity, low accuracy of 2D-to-3D back projection, and inaccurate 3D target positioning and orientation estimation in existing technologies, this invention provides a method and system for estimating and displaying the motion state of a target in a lidar point cloud. This invention primarily utilizes techniques such as spherical projection dimensionality reduction, 2D target detection, reverse 3D point cloud retrieval, bird's-eye view contour generation, and Kalman filtering smoothing to reduce computational complexity, improve detection accuracy, and achieve robust motion state estimation and real-time visualization.

[0010] The technical means employed in this invention are as follows: A method for estimating and displaying the motion state of a target in a lidar point cloud includes: S1. Receive raw point cloud data from lidar and use a spherical projection algorithm to reduce the dimensionality of the 3D point cloud data and map it into a 2D depth projection image. S2. Input the projected image data into the pre-trained target detection model to perform 2D target detection and output a 2D detection result containing the target category, confidence level and bounding box coordinates. S3. Based on the bounding box coordinates in the 2D detection results, the corresponding target 3D point cloud is retrieved in reverse from the original lidar point cloud, and a 3D bounding box is generated based on the retrieved target 3D point cloud. S4. The target 3D point cloud is projected onto the bird's-eye view plane, and the two-dimensional contour parameters of the target are generated based on the distribution of the projected point cloud. S5. Based on the two-dimensional contour parameters of the target in multiple consecutive frames, calculate the target's position, velocity, acceleration, and direction of motion, and use Kalman filtering to smooth the motion parameters to obtain the target's motion state. S6. Present the target motion state in a visual manner in real time, including the target trajectory line, velocity vector and motion direction.

[0011] Further, step S1 includes: S11. Convert the three-dimensional rectangular coordinates of each point in the original point cloud data of the lidar. Convert to spherical coordinates ,in It is the azimuth angle. The pitch angle, Radial distance; S12. According to the preset horizontal angle resolution and vertical angular resolution The spherical coordinates are mapped to two-dimensional image coordinates. ,in , ; S13, radial distance Using the pixel values ​​at the corresponding image coordinates, a two-dimensional depth projection image is generated; S14. Perform inverse distance weighted interpolation on the two-dimensional depth projection image to fill pixel holes caused by sparse point clouds.

[0012] Further, step S2 includes: S21. Train a 2D object detection model based on the labeled projected image dataset, and load the trained 2D object detection model; S22. Input the two-dimensional depth projection image into the 2D target detection model to identify the target region in the image; S23. Extract the category label, confidence score, and bounding box coordinates of each detected target in the image coordinate system, including the coordinates of the top left corner of the image. and the coordinates of the bottom right corner of the image , thus obtaining 2D detection results.

[0013] Further, step S3 includes: S31, Based on the coordinates of the upper left corner image and the coordinates of the bottom right corner of the image Combined with the horizontal angular resolution and vertical angular resolution Calculate the azimuth range of the target in the lidar coordinate system. and pitch angle range ; S32, within the azimuth range and pitch angle range The system internally retrieves the original point cloud of the lidar, searches for the target point cloud with the smallest radial distance, and obtains the closest distance. ; S33, Based on the nearest distance Given the azimuth and elevation angle ranges, calculate the target's three-dimensional dimensions, including the target's width. The height of the target Target length Depth factor; S34. Perform multi-dimensional constraint screening on each point in the original point cloud of the lidar, including distance constraint, horizontal angle constraint and vertical angle constraint, and retain the points that simultaneously satisfy all constraints as the target point cloud. S35. Based on the filtered target points, cloud computing minimum bounding boxes are used to generate 3D bounding boxes describing the target's location, 3D size, and orientation.

[0014] Furthermore, in step S34: The distance constraint is radial distance. exist Within the range; The horizontal angle constraint is the azimuth angle. exist Within the range; The vertical angle constraint is the pitch angle. exist Within the range.

[0015] Further, step S4 includes: S41. Project the target 3D point cloud onto the bird's-eye view plane, i.e., the XY plane, to obtain a two-dimensional projected point cloud; S42. Using the projection point of the center of the 3D bounding box onto the XY plane as the center of the ellipse, calculate the distance and orientation angle of all two-dimensional projection points to the center of the ellipse, and determine the endpoints of the major axis and minor axis of the ellipse. S43. Generate ellipse contour parameters, including the coordinates of the ellipse center. Major axis length minor axis length and major axis direction angle This yields the two-dimensional contour of the target.

[0016] Further, step S5 includes: S51. Maintain a historical record of the ellipse center coordinates for each target, saving the most recent... Frame location data, The preset positive integer; S52. Based on the coordinates of the center of the target ellipse in two adjacent frames and Location differences and time interval Calculate the instantaneous velocity of the target. ; S53, the instantaneous velocity of the target With preset speed threshold When comparing, When the target is determined to be stationary, its velocity is set to zero. S54. Based on the instantaneous velocity difference between two adjacent frames and time interval Calculate the target's acceleration ; S55, Apply Kalman filtering to the instantaneous velocity and acceleration Smoothing is performed to obtain the filtered speed. and acceleration ; S56. Calculate the target's motion direction angle based on the positional change trend of the ellipse center coordinates across multiple consecutive frames. .

[0017] Further, step S6 includes: S61. Generate a trajectory line visualization element based on the ellipse center coordinates in the historical records, use moving average smoothing, and set preset line width and color; S62. Based on the filtered speed Generate velocity vector visualization elements, centered on the current ellipse. The starting point is [starting point], and the ending point coordinates are [endpoint coordinates]. ,in Set a preset scaling factor and set a color gradient according to the speed; S63, Based on the motion direction angle Generate visual elements for directional indicators; S64. Real-time rendering and display of trajectory lines, velocity vectors, and motion directions in the visualization tool.

[0018] The present invention also provides a laser radar point cloud target motion state estimation and display system based on the above-mentioned laser radar point cloud target motion state estimation and display method, comprising: The point cloud projection processing module is used to receive raw point cloud data from the lidar and use a spherical projection algorithm to reduce the dimensionality of the 3D point cloud data and map it into a 2D depth projection image. projection Figure 2 The 2D object detection module is used to input the two-dimensional depth projection image into a pre-trained object detection model to perform 2D object detection and output 2D detection results including object category, confidence level and bounding box coordinates. The 3D point cloud retrieval and 3D bounding box generation module is used to retrieve the corresponding target 3D point cloud from the original point cloud of the lidar based on the bounding box coordinates in the 2D detection results, and generate a 3D bounding box based on the retrieved target 3D point cloud. The target BEV contour generation module is used to project the target 3D point cloud onto the bird's-eye view plane and generate the target's two-dimensional contour parameters based on the distribution of the projected point cloud. The target motion state estimation module is used to calculate the target's position, velocity, acceleration, and motion direction based on the two-dimensional contour parameters of the target in multiple consecutive frames, and to smooth the motion parameters using Kalman filtering. The target motion state visualization module is used to present the target motion state in a visual manner in real time, including the target trajectory line, velocity vector and motion direction.

[0019] Compared with the prior art, the present invention has the following advantages: 1. The laser radar point cloud target motion state estimation and display method provided by the present invention reduces the three-dimensional point cloud to a two-dimensional depth projection image by using a spherical projection algorithm, then uses mature 2D target detection technology for target recognition, and finally retrieves 3D point cloud information in reverse, realizing the effective fusion of 2D semantic information and 3D geometric information. While ensuring detection accuracy, it greatly reduces computational complexity and meets the requirements of real-time processing.

[0020] 2. The present invention adopts a two-stage 3D point cloud retrieval strategy. First, the point cloud search area is determined based on the angle range of the 2D detection box and the nearest distance point is found. Then, the three-dimensional size of the target is dynamically calculated and multi-dimensional constraint screening is applied. This effectively solves the problems of difficulty in depth acquisition, sparse point cloud, and blurred boundaries in the process of 2D to 3D back projection, and significantly improves the generation accuracy of 3D bounding boxes.

[0021] 3. This invention generates elliptical contour parameters by projecting the target 3D point cloud onto the bird's-eye view plane, and estimates the motion state based on the contour parameters of multiple consecutive frames combined with Kalman filtering. This achieves robust estimation of the target's position, velocity, acceleration, and direction of motion. At the same time, it effectively distinguishes between stationary and moving targets by judging the velocity threshold, thereby improving the accuracy of motion state estimation.

[0022] 4. This invention achieves multi-dimensional visualization display under the ROS framework, including visualization elements such as trajectory lines, velocity vectors, and motion directions, and uses color gradients and other methods to intuitively present speed information, providing intelligent navigation systems such as ships and unmanned surface vessels with intuitive and comprehensive target motion state perception capabilities.

[0023] In summary, the technical solution of this invention solves the problems of high computational complexity, low accuracy of 2D-to-3D back-projection, and inaccurate 3D target positioning and orientation estimation in existing technologies by fusing 2D image semantic information with 3D point cloud spatial geometric information. Therefore, the technical solution of this invention solves the problems of high computational complexity, poor real-time performance, and low accuracy in point cloud target motion state estimation in existing technologies.

[0024] Based on the above reasons, this invention can be widely applied in fields such as autonomous ship navigation, unmanned surface vessels, unmanned driving, intelligent robots, and remote monitoring. Attached Figure Description

[0025] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0026] Figure 1 This is a flowchart of the laser radar point cloud target motion state estimation and display method of the present invention.

[0027] Figure 2 This is a schematic diagram of the visualization of the original 3D point cloud data of the lidar of the present invention.

[0028] Figure 3 This is a schematic diagram comparing the 3D point cloud and 2D projection image of the lidar of the present invention.

[0029] Figure 4 This is a schematic diagram of 2D projection image target detection according to the present invention.

[0030] Figure 5 This is a schematic diagram illustrating the reverse retrieval of 3D point clouds based on 2D detection results according to the present invention.

[0031] Figure 6 This is a schematic diagram of the target motion trajectory of the present invention.

[0032] Figure 7 This is a schematic diagram illustrating the target motion state estimation and display of the present invention.

[0033] Figure 8 A radar wiring diagram provided for this invention. Detailed Implementation

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

[0035] It should be noted that the terms "first," "second," etc., in the specification, claims, and accompanying drawings of this invention are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate so that the embodiments of the invention described herein can be implemented in orders other than those illustrated or described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover a non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0036] like Figure 1 As shown, the present invention provides a method for estimating and displaying the motion state of a target in a lidar point cloud, comprising: S1. Receive raw point cloud data from lidar and use a spherical projection algorithm to reduce the dimensionality of the 3D point cloud data and map it into a 2D depth projection image. S2. Input the projected image data into the pre-trained target detection model to perform 2D target detection and output a 2D detection result containing the target category, confidence level and bounding box coordinates. S3. Based on the bounding box coordinates in the 2D detection results, the corresponding target 3D point cloud is retrieved in reverse from the original lidar point cloud, and a 3D bounding box is generated based on the retrieved target 3D point cloud. S4. The target 3D point cloud is projected onto the bird's-eye view plane, and the two-dimensional contour parameters of the target are generated based on the distribution of the projected point cloud. S5. Based on the two-dimensional contour parameters of the target in multiple consecutive frames, calculate the target's position, velocity, acceleration, and direction of motion, and use Kalman filtering to smooth the motion parameters to obtain the target's motion state. S6. Present the target motion state in a visual manner in real time, including the target trajectory line, velocity vector and motion direction.

[0037] In this embodiment, the present invention performs target detection by projecting 3D point clouds onto 2D images, then obtains 3D point clouds and 3D bounding boxes by backprojecting the 2D detection boxes, thereby generating a bird's-eye view outline, estimating the target motion state based on Kalman filtering, and finally realizing multi-dimensional visualization display under the ROS framework, providing accurate target detection, localization, and motion state estimation capabilities for intelligent navigation systems such as ships and unmanned surface vessels.

[0038] In a specific implementation, as a preferred embodiment of the present invention, step S1 includes: S11. Convert the three-dimensional rectangular coordinates of each point in the original point cloud data of the lidar. Convert to spherical coordinates ,in It is the azimuth angle. The pitch angle, Radial distance; such as Figure 2 The image shows the 3D point cloud information of the LiDAR, which is visualized in Rviz by subscribing to LiDAR data.

[0039] S12. According to the preset horizontal angle resolution and vertical angular resolution The spherical coordinates are mapped to two-dimensional image coordinates. ,in , ; S13, radial distance Using the pixel values ​​at the corresponding image coordinates, a two-dimensional depth projection image is generated; such as Figure 3 As shown, this is a 3D point cloud information and a 2D projected image of a LiDAR point cloud. The upper part shows the effect of projecting the 3D point cloud data into a 2D image, clearly showing the clear outlines of people and objects. The lower part shows the point cloud data acquired by the LiDAR at that time.

[0040] S14. Perform inverse distance weighted interpolation on the two-dimensional depth projection image to fill pixel holes caused by sparse point clouds.

[0041] In a specific implementation, as a preferred embodiment of the present invention, step S2 includes: S21. Train a 2D object detection model based on the labeled projected image dataset, and load the trained 2D object detection model; S22. Input the two-dimensional depth projection image into the 2D target detection model to identify the target region in the image; S23. Extract the category label, confidence score, and bounding box coordinates of each detected target in the image coordinate system, including the coordinates of the top left corner of the image. and the coordinates of the bottom right corner of the image This yields 2D detection results. For example... Figure 4 As shown, in order to perform target recognition on the projected image, the area selected by the rectangular box in the upper projection image corresponds one-to-one with the person in the point cloud image below.

[0042] In a specific implementation, as a preferred embodiment of the present invention, step S3 includes: S31, Based on the coordinates of the upper left corner image and the coordinates of the bottom right corner of the image Combined with the horizontal angular resolution and vertical angular resolution Calculate the azimuth range of the target in the lidar coordinate system. and pitch angle range ; S32, within the azimuth range and pitch angle range The system internally retrieves the original point cloud of the lidar, searches for the target point cloud with the smallest radial distance, and obtains the closest distance. ; S33, Based on the nearest distance Given the azimuth and elevation angle ranges, calculate the target's three-dimensional dimensions, including the target's width. The height of the target Target length Depth factor; S34. Perform multi-dimensional constraint screening on each point in the original point cloud of the lidar, including distance constraint, horizontal angle constraint and vertical angle constraint, and retain the points that simultaneously satisfy all constraints as the target point cloud. S35. Based on the filtered target points, use cloud computing to generate minimum bounding boxes, resulting in 3D bounding boxes describing the target's location, 3D dimensions, and orientation. For example... Figure 5 As shown, this demonstrates the extraction of 3D point clouds from targets identified after 2D projection. The positions of person1 and person2 identified in the projected image are obtained and extracted from the point cloud.

[0043] In a specific implementation, as a preferred embodiment of the present invention, step S34 includes: The distance constraint is radial distance. exist Within the range; The horizontal angle constraint is the azimuth angle. exist Within the range; The vertical angle constraint is the pitch angle. exist Within the range.

[0044] In a specific implementation, as a preferred embodiment of the present invention, step S4 includes: S41. Project the target 3D point cloud onto the bird's-eye view plane, i.e., the XY plane, to obtain a two-dimensional projected point cloud; S42. Using the projection point of the center of the 3D bounding box onto the XY plane as the center of the ellipse, calculate the distance and orientation angle of all two-dimensional projection points to the center of the ellipse, and determine the endpoints of the major axis and minor axis of the ellipse. S43. Generate ellipse contour parameters, including the coordinates of the ellipse center. Major axis length minor axis length and major axis direction angle This yields the two-dimensional contour of the target.

[0045] In a specific implementation, as a preferred embodiment of the present invention, step S5 includes: S51. Maintain a historical record of the ellipse center coordinates for each target, saving the most recent... Frame location data, The preset positive integer; S52. Based on the coordinates of the center of the target ellipse in two adjacent frames and Location differences and time interval Calculate the instantaneous velocity of the target. ; S53, the instantaneous velocity of the target With preset speed threshold When comparing, When the target is determined to be stationary, its velocity is set to zero. S54. Based on the instantaneous velocity difference between two adjacent frames and time interval Calculate the target's acceleration ; S55, Apply Kalman filtering to the instantaneous velocity and acceleration Smoothing is performed to obtain the filtered speed. and acceleration ; S56. Calculate the target's motion direction angle based on the positional change trend of the ellipse center coordinates across multiple consecutive frames. .like Figure 6 As shown, the motion trajectory of a point cloud target is visualized. As the target moves on the projected image, this movement appears merely as left-right movement in the 2D image, but it actually includes depth information. By extracting the 3D point cloud information from the 2D projected target, the target's trajectory can be visualized.

[0046] In a specific implementation, as a preferred embodiment of the present invention, step S6 includes: S61. Generate a trajectory line visualization element based on the ellipse center coordinates in the historical records, use moving average smoothing, and set preset line width and color; S62. Based on the filtered speed Generate velocity vector visualization elements, centered on the current ellipse. The starting point is [starting point], and the ending point coordinates are [endpoint coordinates]. ,in Set a preset scaling factor and set a color gradient according to the speed; S63, Based on the motion direction angle Generate visual elements for directional indicators; S64. Real-time rendering and display of trajectory lines, velocity vectors, and direction of motion in visualization tools. For example... Figure 7 As shown, the motion state and estimation of point cloud targets are illustrated. After a target is detected in the projection map and its 3D point cloud is extracted, the motion state and possible orientation estimates of each point cloud target are visualized.

[0047] The present invention also provides a laser radar point cloud target motion state estimation and display system based on the above-mentioned laser radar point cloud target motion state estimation and display method, comprising: The point cloud projection processing module is used to receive raw point cloud data from the lidar and use a spherical projection algorithm to reduce the dimensionality of the 3D point cloud data and map it into a 2D depth projection image. projection Figure 2 The 2D object detection module is used to input the two-dimensional depth projection image into a pre-trained object detection model to perform 2D object detection and output 2D detection results including object category, confidence level and bounding box coordinates. The 3D point cloud retrieval and 3D bounding box generation module is used to retrieve the corresponding target 3D point cloud from the original point cloud of the lidar based on the bounding box coordinates in the 2D detection results, and generate a 3D bounding box based on the retrieved target 3D point cloud. The target BEV contour generation module is used to project the target 3D point cloud onto the bird's-eye view plane and generate the target's two-dimensional contour parameters based on the distribution of the projected point cloud. The target motion state estimation module is used to calculate the target's position, velocity, acceleration, and motion direction based on the two-dimensional contour parameters of the target in multiple consecutive frames, and to smooth the motion parameters using Kalman filtering. The target motion state visualization module is used to present the target motion state in a visual manner in real time, including the target trajectory line, velocity vector and motion direction.

[0048] The embodiments of the present invention are described simply because they correspond to those in the embodiments above. For any similarities, please refer to the descriptions in the embodiments above, which will not be elaborated here.

[0049] Example The Youyeetoo X1 (hereinafter referred to as Youyeetoo) was used as a single-board microcomputer. The Youyeetoo was equipped with and ran the Linux distribution Ubuntu 20.04.6 LTS (Focal Fossa), configured with a Noetic ROS development environment. A folder named livox_ws was created under the Youyeetoo system's Home path; this folder serves as the ROS workspace for 3D environment perception. Within this workspace, a source code folder named src was created to store node source code; and a folder named pointcloud_filter was created to store the point cloud target tracking package for the ROS nodes. Within the pointcloud_filter folder, three folders were created: src for node source code, config for configuration files, and launch for launch files. The workspace was then initialized and compiled.

[0050] The Livox-Mid360 LiDAR is used as the practical basis for the algorithm. First, the Livox-Mid360 needs to be installed and connected. Regarding connectivity, the Livox Mid360 only supports static IP address mode, so before connecting, the computer's IP address needs to be set to 192.168.1.50, and the subnet mask to 255.255.255.0. After setting the computer's IP address, connect the Livox-Mid360 LiDAR to the computer using an M12-A-Code aviation connector cable. (Specific steps are as follows...) Figure 8 Connect. Next, download the LiDAR's startup file to your computer, install and run Livox-SDK2 and livox_ros_driver2. In the livox_ros_driver2 folder, go to the config folder and find the MID360_config.json file. Change the IP address in the lidar_config section of the JSON file to 192.168.1.1xx (where xx represents the last two digits of the LiDAR's serial number). Then, launch the rviz_MID360.launch file in the livox_ros_driver2 package using roslaunch to start the LiDAR.

[0051] To enable smooth movement of the LiDAR, this algorithm places the LiDAR on the desktop. Upon startup, the LiDAR inputs 3D LiDAR data from / livox / lidar. To convert this into 2D LiDAR data and visualize it in Rviz, we need to launch the LiDAR's startup file, rviz_MID360.launch. The LiDAR data will then be published under the topic / livox / lidar.

[0052] The specific process of this method is as follows: Step 1: Project the subscribed LiDAR data. After spherical coordinate calculation and projection, the 3D LiDAR data is converted into a 2D image with a horizontal resolution of 360 pixels and a vertical resolution of 30 pixels. Then, inverse distance weighted interpolation is performed on the projected image, and the depth data ( / livox / depth_data) and image data ( / livox / projection_full_image) are published.

[0053] Step 2: Prepare the dataset, use Python code to save and classify the image topics, use the labelimg tool to label and divide the dataset; finally, use the model to train and obtain the model with the best accuracy.

[0054] Step 3: The projected 2D image is used as data input to introduce the trained model for object detection. The 2D detection boxes are then published as topics.

[0055] Step 4: Subscribe to the raw point cloud data ( / livox / lidar) and 2D bounding box information ( / yolo_detection / detections) of the LiDAR, and use a two-stage localization method to extract the target point cloud and generate a 3D bounding box. The specific implementation steps are as follows: The first step is to obtain the approximate range of the point cloud based on the azimuth and elevation angle range of the 2D detection box corresponding to the lidar, and then perform inverse spherical projection to find the point cloud d_min that is closest to the lidar.

[0056] The second step is to determine the nearest distance. and the azimuth range of the 2D detection box and pitch angle range Dynamically calculate the actual size of the target: the width of the target. The height of the target Target length Depth coefficients are used to construct 3D detection boxes.

[0057] The third step is to perform multi-dimensional constraint checks on each point in the original point cloud: distance constraints (in...) Within the range), horizontal angle constraint (within the azimuth range of the detection box). (Inner) and vertical angle constraints (within the pitch angle range of the detection frame) (Within), only the points that simultaneously satisfy all constraints are retained as the target point cloud.

[0058] Finally, the extracted 3D point cloud ( / detection_bbox / pointcloud), detection information ( / detection_bbox / info), and 3D detection box visualization ( / visualization_marker_array) are published.

[0059] Step 5: Subscribe to the 3D target point cloud data / detection_bbox / pointcloud extracted from the 2D detection box, and use pointcloud_to_laserscan to project it to obtain its 2D projected point cloud.

[0060] Step 6: Using the 2D projection of the 3D detection box center as the center of the ellipse, calculate the distance and direction of all projected points from the center. Find the point with the largest distance as the endpoint of the major axis, and find the point with the largest distance in the direction perpendicular to the major axis as the endpoint of the minor axis. The generated ellipse parameters include the center coordinates, major axis length, minor axis length, and major axis direction angle. Publish the ellipse outline visualization at / detection_bbox / ellipse_markers.

[0061] Step 7: Maintain the historical record of the ellipse center position for each target (last 20 frames). Calculate the target's velocity using the position difference and time interval between adjacent frames. Apply a velocity threshold: velocities below 0.1 m / s are considered stationary. Calculate acceleration using the velocity difference between adjacent frames. Apply Kalman filtering to smooth the velocity and acceleration, improving the stability of the estimation. Publish ellipse state information ( / detection_bbox / ellipse_states), including ID, center, velocity, acceleration, direction, etc., and motion trajectory ( / detection_bbox / ellipse_trajectories), containing trajectory points from the last 5 seconds.

[0062] Step 8: Display the target's motion status in Rviz in real time, including: Trajectory line (LINE_STRIP type): Displays the target's historical trajectory, smoothed using a moving average, in red color, with a line width of 0.05 meters; Velocity vector: Displays the current velocity direction and magnitude of the target. The starting point is the target center, and the ending point is the center + velocity vector × scale. The color changes according to the velocity magnitude (green for 0 m / s, yellow for 2.5 m / s, and red for 5 m / s). The arrow shaft width is 0.05 meters. All visualization markers are published to the topics / detection_bbox / target_states, / detection_bbox / center_trajectory, and / detection_bbox / velocity_vectors.

[0063] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for estimating and displaying the motion state of a target in a lidar point cloud, characterized in that, include: S1. Receive raw point cloud data from lidar and use a spherical projection algorithm to reduce the dimensionality of the 3D point cloud data and map it into a 2D depth projection image. S2. Input the projected image data into the pre-trained target detection model to perform 2D target detection and output a 2D detection result containing the target category, confidence level and bounding box coordinates. S3. Based on the bounding box coordinates in the 2D detection results, the corresponding target 3D point cloud is retrieved in reverse from the original lidar point cloud, and a 3D bounding box is generated based on the retrieved target 3D point cloud. S4. The target 3D point cloud is projected onto the bird's-eye view plane, and the two-dimensional contour parameters of the target are generated based on the distribution of the projected point cloud. S5. Based on the two-dimensional contour parameters of the target in multiple consecutive frames, calculate the target's position, velocity, acceleration, and direction of motion, and use Kalman filtering to smooth the motion parameters to obtain the target's motion state. S6. Present the target motion state in a visual manner in real time, including the target trajectory line, velocity vector and motion direction.

2. The method for estimating and displaying the motion state of a target in a lidar point cloud according to claim 1, characterized in that, Step S1 includes: S11. Convert the three-dimensional rectangular coordinates of each point in the original point cloud data of the lidar. Convert to spherical coordinates ,in It is the azimuth angle. The pitch angle, Radial distance; S12. According to the preset horizontal angle resolution and vertical angular resolution The spherical coordinates are mapped to two-dimensional image coordinates. ,in , ; S13, radial distance Using the pixel values ​​at the corresponding image coordinates, a two-dimensional depth projection image is generated; S14. Perform inverse distance weighted interpolation on the two-dimensional depth projection image to fill pixel holes caused by sparse point clouds.

3. The method for estimating and displaying the motion state of a target in a lidar point cloud according to claim 1, characterized in that, Step S2 includes: S21. Train a 2D object detection model based on the labeled projected image dataset, and load the trained 2D object detection model; S22. Input the two-dimensional depth projection image into the 2D target detection model to identify the target region in the image; S23. Extract the category label, confidence score, and bounding box coordinates of each detected target in the image coordinate system, including the coordinates of the top left corner of the image. and the coordinates of the bottom right corner of the image , thus obtaining 2D detection results.

4. The method for estimating and displaying the motion state of a target in a lidar point cloud according to claim 1, characterized in that, Step S3 includes: S31, Based on the coordinates of the upper left corner image and the coordinates of the bottom right corner of the image Combined with the horizontal angular resolution and vertical angular resolution Calculate the azimuth range of the target in the lidar coordinate system. and pitch angle range ; S32, within the azimuth range and pitch angle range The system internally retrieves the original point cloud of the lidar, searches for the target point cloud with the smallest radial distance, and obtains the closest distance. ; S33, Based on the nearest distance Given the azimuth and elevation angle ranges, calculate the target's three-dimensional dimensions, including the target's width. The height of the target Target length Depth factor; S34. Perform multi-dimensional constraint screening on each point in the original point cloud of the lidar, including distance constraint, horizontal angle constraint and vertical angle constraint, and retain the points that simultaneously satisfy all constraints as the target point cloud. S35. Based on the filtered target points, cloud computing minimum bounding boxes are used to generate 3D bounding boxes describing the target's location, 3D size, and orientation.

5. The method for estimating and displaying the motion state of a target in a lidar point cloud according to claim 1, characterized in that, In step S34: The distance constraint is radial distance. exist Within the range; The horizontal angle constraint is the azimuth angle. exist Within the range; The vertical angle constraint is the pitch angle. exist Within the range.

6. The method for estimating and displaying the motion state of a target in a lidar point cloud according to claim 1, characterized in that, Step S4 includes: S41. Project the target 3D point cloud onto the bird's-eye view plane, i.e., the XY plane, to obtain a two-dimensional projected point cloud; S42. Using the projection point of the center of the 3D bounding box onto the XY plane as the center of the ellipse, calculate the distance and orientation angle of all two-dimensional projection points to the center of the ellipse, and determine the endpoints of the major axis and minor axis of the ellipse. S43. Generate ellipse contour parameters, including the coordinates of the ellipse center. Major axis length minor axis length and major axis direction angle This yields the two-dimensional contour of the target.

7. The method for estimating and displaying the motion state of a target in a lidar point cloud according to claim 1, characterized in that, Step S5 includes: S51. Maintain a historical record of the ellipse center coordinates for each target, saving the most recent... Frame location data, It is a preset positive integer; S52. Based on the coordinates of the center of the target ellipse in two adjacent frames and Location differences and time interval Calculate the instantaneous velocity of the target. ; S53, the instantaneous velocity of the target With preset speed threshold When comparing, When the target is determined to be stationary, its velocity is set to zero. S54. Based on the instantaneous velocity difference between two adjacent frames and time interval Calculate the target's acceleration ; S55, Apply Kalman filtering to the instantaneous velocity and acceleration Smoothing is performed to obtain the filtered speed. and acceleration ; S56. Calculate the target's motion direction angle based on the positional change trend of the ellipse center coordinates across multiple consecutive frames. .

8. The method for estimating and displaying the motion state of a target in a lidar point cloud according to claim 1, characterized in that, Step S6 includes: S61. Generate a trajectory line visualization element based on the ellipse center coordinates in the historical records, use moving average smoothing, and set preset line width and color; S62. Based on the filtered speed Generate velocity vector visualization elements, centered on the current ellipse. The starting point is [starting point], and the ending point coordinates are [endpoint coordinates]. ,in Set a preset scaling factor and set a color gradient according to the speed; S63, Based on the motion direction angle Generate visual elements for directional indicators; S64. Real-time rendering and display of trajectory lines, velocity vectors, and motion directions in the visualization tool.

9. A laser radar point cloud target motion state estimation and display system based on the laser radar point cloud target motion state estimation and display method according to any one of claims 1-8, characterized in that, include: The point cloud projection processing module is used to receive raw point cloud data from the lidar and use a spherical projection algorithm to reduce the dimensionality of the 3D point cloud data and map it into a 2D depth projection image. The projection image 2D target detection module is used to input the two-dimensional depth projection image into a pre-trained target detection model, perform 2D target detection, and output 2D detection results including target category, confidence level, and bounding box coordinates; The 3D point cloud retrieval and 3D bounding box generation module is used to retrieve the corresponding target 3D point cloud from the original point cloud of the lidar based on the bounding box coordinates in the 2D detection results, and generate a 3D bounding box based on the retrieved target 3D point cloud. The target BEV contour generation module is used to project the target 3D point cloud onto the bird's-eye view plane and generate the target's two-dimensional contour parameters based on the distribution of the projected point cloud. The target motion state estimation module is used to calculate the target's position, velocity, acceleration, and motion direction based on the two-dimensional contour parameters of the target in multiple consecutive frames, and to smooth the motion parameters using Kalman filtering. The target motion state visualization module is used to present the target motion state in a visual manner in real time, including the target trajectory line, velocity vector and motion direction.

Images