An unmanned ship target motion state detection method based on laser radar
By filtering and rasterizing marine point cloud data, and combining the DBSCAN algorithm and centroid correlation, the problems of false alarms and target loss in target tracking of unmanned surface vessels under high sea states were solved, and stable tracking and status monitoring of water targets were achieved.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- CHINA SHIP SCIENTIFIC RESEARCH CENTER
- Filing Date
- 2024-01-26
- Publication Date
- 2026-06-05
AI Technical Summary
Existing technologies struggle to effectively track and identify surface targets on unmanned surface vessels, especially in high sea states where point cloud data is sparse and easily affected by waves, leading to false alarms and target loss.
The influence of ocean waves is removed by filtering with prior knowledge and rasterization, the target is separated by the DBSCAN algorithm, and the target speed and heading are calculated by centroid association and point cloud matching. Multi-frame smoothing operation is combined to stabilize the target tracking.
It enables effective tracking and status monitoring of moving targets on water under high sea states, improves the accuracy and stability of target identification, and obtains precise speed and heading information.
Smart Images
Figure CN117930264B_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the field of computer vision 3D point cloud processing, mainly targeting the environmental perception field of unmanned surface vessels at sea, specifically involving a method for detecting the motion state of unmanned surface vessels based on lidar. Background Technology
[0002] LiDAR combines the advantages of marine radar and optical imaging, acquiring both target distance and orientation information for target localization and obstacle avoidance navigation, and target 3D contour information for obstacle segmentation and identification. Furthermore, LiDAR employs active laser ranging, making it less susceptible to lighting and weather conditions, and boasts a high scanning frequency, making it ideal for short-to-medium range target detection and identification on unmanned platforms. In recent years, 3D scanning LiDAR has been widely used in unmanned vehicle navigation and urban mapping. However, due to the late development of unmanned surface vessels (USVs) and the complexity of water surface environments, LiDAR algorithms for water surface environmental perception are still immature. Researching LiDAR-based water target detection and identification technology can, on the one hand, complement the advantages of traditional sensors, meeting the environmental perception needs of USVs and promoting the improvement of USV perception systems. On the other hand, 3D point cloud processing is at the forefront of imaging detection technology, and extending it from road surfaces to water surfaces is of great significance.
[0003] Because the point cloud datasets acquired by lidar are highly sparsity, have limited information content, and are subject to numerous clutter interferences and motion distortions, false alarms and missed detections are difficult to avoid when detecting single-frame point clouds. Furthermore, when tracking moving targets in dynamic environments, issues such as target loss and occlusion frequently occur. Summary of the Invention
[0004] To address the aforementioned problems and technical requirements, the inventors have proposed a method for detecting the motion status of unmanned surface vessels (USVs) based on lidar. Considering the high-speed movement of maritime targets and the interference caused by wave fluctuations, the distribution of point clouds scanned by lidar is constantly changing. This method can still effectively track and monitor the motion status of moving targets in the aquatic environment, thereby obtaining information on the speed, orientation, and distance of the moving targets.
[0005] The technical solution of the present invention is as follows:
[0006] A method for detecting the motion state of an unmanned surface vessel (USV) based on lidar includes the following steps:
[0007] In high sea state scenarios, lidar is used to acquire continuous frame point cloud information of the surrounding environment on the water surface;
[0008] Each frame of water surface point cloud is filtered using prior knowledge, and then the filtered water surface point cloud is separated into targets using a clustering method.
[0009] Associate identical targets between consecutive frames based on the centroid positions of the separated targets;
[0010] The transformation matrix between the associated target pairs is calculated using a point cloud matching algorithm, and the speed and heading information of each target is calculated based on the transformation matrix.
[0011] For each target, the obtained speed is filtered according to preset conditions, and the speeds that meet the conditions are smoothed over multiple frames to obtain the final target speed in the current frame.
[0012] Its further technical solution is to filter the water surface point cloud in each frame using prior knowledge, including:
[0013] Based on the height characteristics of water waves and wakes, the pre-processed water surface point cloud is rasterized.
[0014] Calculate the relative height difference and number of point clouds for the lowest and highest points in each grid cell, and delete any grid point cloud that is below the corresponding threshold to filter out the influence of ocean waves.
[0015] The further technical solution involves using a clustering method to separate targets from the filtered water surface point cloud in each frame, including:
[0016] The DBSCAN algorithm is used to cluster point clouds that are close to each other in space into a cluster, and each clustered point cloud set is used as a separate target.
[0017] A further technical solution involves associating identical targets across consecutive frames based on the separated target centroid positions, including:
[0018] Calculate the centroids of each target obtained after the clustering operation in the current frame, and construct a KD tree based on the centroids of each target;
[0019] In the target of the point cloud in the previous frame, find the n preferred targets that are closest to the centroid of each target in the current frame point cloud based on the KD tree, and calculate the intersection between each preferred target and the target in the current frame.
[0020] If the intersection of all targets is greater than a given threshold, then related targets are determined from the n preferred targets and assigned the same batch number;
[0021] Otherwise, reselect the associated target from the target in the previous frame's point cloud.
[0022] A further technical solution involves determining the associated target from n preferred targets, including:
[0023] Select the target with the most intersection as the associated target of the current frame;
[0024] Alternatively, when the intersections are the same, the preferred target with the highest intersection-union ratio is selected as the associated target of the current frame target.
[0025] A further technical solution involves reselecting associated targets from the target pool in the previous frame's point cloud, including:
[0026] Determine whether the target from the previous frame's point cloud exists within the preset range of the target in the current frame;
[0027] If it exists, the nearest target that can be matched within that range will be taken as the associated target of the current frame target and assigned the same batch number;
[0028] Otherwise, the target in the current frame is treated as a new target and assigned a new batch number.
[0029] A further technical solution involves calculating the speed and heading information of each target based on the transformation matrix, including:
[0030] For each target, the associated targets of each frame are pushed into the target queue. The target movement distance between the first and last frames of the queue is calculated based on the transformation matrix. Then, the target speed and heading information of the current frame is calculated based on the target movement distance and pushed into the speed queue.
[0031] A further technical solution involves filtering the calculated speed for each target according to preset conditions, including:
[0032] For each item in the velocity queue, calculate the difference between the target movement distance corresponding to that item and the target centroid distance between the first and last frames of the queue, and compare this difference with the target length.
[0033] If the difference is less than the length of the target, then the target speed calculated based on the target's movement distance is considered to be the effective speed.
[0034] If the difference is not less than the length of the target, the target speed that meets the first preset condition is marked as an effective speed; otherwise, it is marked as an outlier.
[0035] If the marked velocity queue meets the second preset condition, the output velocity queue is used to calculate the final target velocity of the current frame; otherwise, wait for the target queue and velocity queue to be updated.
[0036] Its further technical solution is that the first preset condition includes:
[0037] Calculate the average movement distance of the four corner points of the target bounding box in the top view generated during clustering for the two frames at the head and tail of the formation corresponding to the target speed of the project.
[0038] The included angles of the movement directions of the four corner points are all no greater than the second threshold.
[0039] The length, width, and area of the target do not exceed the third threshold.
[0040] The further technical solution is that the second preset condition includes:
[0041] The percentage of valid speeds in the marked speed queue is greater than the fourth threshold;
[0042] The angle between the target movement direction of two consecutive frames in the marked velocity queue is no greater than the fifth threshold.
[0043] The beneficial technical effects of this invention are:
[0044] 1. This method uses the height characteristics of water waves and wakes to filter sea surface point clouds, and uses rasterization to remove raster point clouds with small relative height differences or few point clouds, which is beneficial for extracting high-quality target boxes.
[0045] 2. This method establishes the connection between targets in the current frame by building a data structure based on the target centroid, and quickly associates targets between frames based on distance filtering.
[0046] 3. This method uses a point cloud matching algorithm to calculate the transformation matrix between associated targets, which can calculate accurate heading and speed information.
[0047] 4. This method marks the obtained speed based on the angle between the target's moving distance and the direction of movement, and performs multi-frame smoothing operation on the speeds that meet the conditions to obtain the final target speed in the current frame, making the tracking effect more stable. Attached Figure Description
[0048] Figure 1 This is a schematic diagram of the unmanned surface vessel target motion state detection method provided in this application.
[0049] Figure 2 This is the same target association algorithm flow provided in this application. Detailed Implementation
[0050] The specific embodiments of the present invention will be further described below with reference to the accompanying drawings.
[0051] Please refer to Figure 1 As shown, this application provides a method for detecting the motion state of unmanned surface vessels based on lidar, which specifically includes the following steps:
[0052] Step 1: In high sea state scenarios, use lidar to acquire continuous frame water surface point cloud information of the surrounding environment.
[0053] In practical applications, the unmanned surface vessel (USV) system continuously receives broadcast information from lidar. It calculates the position of the point cloud in each message using a correction table, and defines a frame of surface point cloud information as the water point cloud information collected after one complete rotation of the lidar, based on the lidar's azimuth angle. The calculation of the point cloud position in each message using a correction table is based on existing technology and will not be described in detail here.
[0054] Step 2: Filter the water surface point cloud in each frame using prior knowledge, specifically including:
[0055] Based on the height characteristics of water waves and wakes, the preprocessed water surface point cloud is rasterized. For example, using a 1m long grid, a 200m*200m area is divided into grids centered on the lidar. The relative height difference between the lowest and highest point clouds in each grid and the number of point clouds are calculated. Grid point clouds with either value below a corresponding threshold are deleted. For example, grid filtering is used to remove grid point clouds with a relative height difference less than 0.5m or fewer than 2 point clouds to filter out the influence of ocean waves.
[0056] Step 3: Then, target separation is performed on each frame of the filtered water surface point cloud using a clustering method, specifically including:
[0057] Objects on water are typically separated by a certain distance. The DBSCAN (Density-Based Spatial Clustering of Applications with Noise) algorithm groups point clouds that are close in distance into clusters, and treats each clustered point cloud as a separate target. These objects on water can include fixed objects on the water surface (such as water towers) or moving objects (such as ships).
[0058] Step 4: Associate identical targets across consecutive frames based on the separated target centroid positions. For example... Figure 2 As shown, it specifically includes:
[0059] Step 4.1: Calculate the centroid of each target obtained after the clustering operation in the current frame, and construct a KD tree based on the centroid of each target.
[0060] Step 4.2: Among the targets in the point cloud of the previous frame, find the n preferred targets whose centroid distances are closest to each target in the current frame's point cloud based on the KD tree, and calculate the intersection between each preferred target and the target in the current frame. Optionally, in this embodiment, n = 5.
[0061] Step 4.3: If all intersections are greater than the given threshold of 0.1, then determine the associated targets from the n preferred targets and assign them the same batch number. Specifically, select the preferred target with the most intersections as the associated target of the current frame target; or, when the intersections are the same, select the preferred target with the highest intersection-union ratio as the associated target of the current frame target.
[0062] Step 4.4: If the intersection of all targets is not greater than the given threshold of 0.1, then reselect the associated target from the target in the point cloud of the previous frame.
[0063] Specifically, it determines whether a target from the previous frame's point cloud exists within a preset range (e.g., five meters) of the target in the current frame. If it does, the nearest matching target within that range is taken as the associated target of the current frame and assigned the same batch number. Otherwise, the target in the current frame is treated as a new target and assigned the next batch number.
[0064] Optionally, if a historical target in the previous frame of the point cloud cannot be associated with any target in the current frame, the target is considered temporarily lost and is retained for 1 second so that attempts can continue to be made to associate the historical target with the targets in the next frame of the point cloud.
[0065] Step 5: Calculate the transformation matrix between the associated target pairs using the point cloud matching algorithm, and calculate the speed and heading information of each target based on the transformation matrix.
[0066] In this embodiment, the ICP (Iterative Closest Point) algorithm is used to register and associate the same target between two frames. The registration process only extracts the point cloud portion belonging to the target from both frames for calculation, suppressing errors introduced by noise. During registration, the transformation matrix between the point clouds of the two frames before and after association can be obtained, used to calculate the target's true motion between the two frames, and subsequently, the target's speed and heading information. Specifically, for each target, the targets from each frame after association are first pushed into a target queue. In this embodiment, the maximum length of the target queue is set to 10. Then, the target movement distance between the first and last frames is calculated based on the transformation matrix. The target speed and heading information for the current frame is then calculated based on the target movement distance and pushed into a speed queue. In this embodiment, the maximum length of the speed queue is set to 5.
[0067] Step 6: For each target, filter the obtained speed according to preset conditions, specifically including:
[0068] Step 6.1: For each item in the velocity queue, calculate the difference between the target movement distance corresponding to that item and the target centroid distance between the first and last frames of the queue, and compare this difference with the target length.
[0069] Step 6.2: If the difference is less than the length of the target, then the target speed calculated based on the target's movement distance is considered to be the effective speed.
[0070] Step 6.3: If the difference is not less than the length of the target, then the target speed that meets the first preset condition is marked as an effective speed; otherwise, it is marked as an outlier. The first preset condition includes:
[0071] Calculate the average movement distance of the four corner points of the target bounding box in the top view generated during clustering for the two frames at the head and tail of the formation corresponding to the target speed of the project.
[0072] The included angles of the movement directions of the four corner points are all no greater than the second threshold.
[0073] The length, width, and area of the target do not exceed the third threshold.
[0074] In this embodiment, the first threshold is set to 0.25 meters, the second threshold is 45°, and the third threshold is 40% of the original length, width, and area of the target.
[0075] Step 6.4: If the marked velocity queue meets the second preset condition, output this velocity queue to calculate the final target velocity of the current frame; otherwise, do not output this velocity queue, and wait for the target queue and velocity queue to be updated. During the update, remove the frame at the head of the queue and add the next frame at the tail. The second preset condition includes:
[0076] The percentage of valid speeds in the marked speed queue is greater than the fourth threshold;
[0077] The angle between the target movement direction of two consecutive frames in the marked velocity queue is no greater than the fifth threshold.
[0078] In this embodiment, the fourth threshold is set to 80%, and the fifth threshold is set to 60°.
[0079] Step 7: Perform multi-frame smoothing on the speeds that meet the conditions to obtain the final target speed for the current frame.
[0080] Specifically, if step 6 outputs a speed queue, then the five frames of speed in the speed queue are smoothed to obtain the final target speed for the current frame; otherwise, the target speed for the current frame is not updated.
[0081] Based on the above steps, multi-target tracking can be achieved, and the instantaneous speed, orientation, and distance information of moving targets during the tracking process can be obtained. This method is of great significance for the development of autonomous navigation of unmanned maritime vessels.
[0082] The above descriptions are merely preferred embodiments of this application, and the present invention is not limited to the above embodiments. It is understood that other improvements and variations directly derived or conceived by those skilled in the art without departing from the spirit and concept of the present invention should be considered to be included within the protection scope of the present invention.
Claims
1. A method for detecting the motion state of an unmanned surface vessel (USV) based on lidar, characterized in that, The method includes: In high sea state scenarios, lidar is used to acquire continuous frame point cloud information of the surrounding environment on the water surface; Each frame of water surface point cloud is filtered using prior knowledge, and then the filtered water surface point cloud is separated into targets using a clustering method. Associate identical targets between consecutive frames based on the centroid positions of the separated targets; The transformation matrix between the associated target pairs is calculated using a point cloud matching algorithm, and the speed and heading information of each target is calculated based on the transformation matrix. For each target, the obtained speed is filtered according to preset conditions, and the speeds that meet the conditions are smoothed over multiple frames to obtain the final target speed in the current frame. The filtering of each frame of water surface point cloud using prior knowledge includes: Based on the height characteristics of water waves and wakes, the pre-processed water surface point cloud is rasterized. Calculate the relative height difference and number of points in the lowest and highest points of each grid cell, and delete grid points that are below the corresponding threshold to filter out the influence of ocean waves. The calculation of the speed and heading information of each target based on the transformation matrix includes: For each target, the associated targets of each frame are pushed into the target queue. The target movement distance between the first and last frames of the queue is calculated according to the transformation matrix. Then, the target speed and heading information of the current frame is calculated according to the target movement distance and pushed into the speed queue. For each target, the calculated speed is filtered according to preset conditions, including: For each item in the velocity queue, calculate the difference between the target movement distance corresponding to that item and the target centroid distance between the first and last frames of the queue, and compare the difference with the length of the target. If the difference is less than the length of the target, then the target speed calculated based on the target's movement distance is considered to be the effective speed. If the difference is not less than the length of the target, the target speed that meets the first preset condition is marked as an effective speed; otherwise, it is marked as an outlier. If the marked velocity queue meets the second preset condition, the velocity queue is output to calculate the final target velocity of the current frame; otherwise, the target queue and the velocity queue are waited for to be updated. The first preset condition includes: Calculate the average movement distance of the four corner points of the target bounding box in the top view generated during clustering for the two frames at the head and tail of the formation corresponding to the target speed of the project. The included angles of the movement directions of the four corner points are all no greater than the second threshold. The length, width, and area of the target do not exceed the third threshold. The second preset condition includes: The proportion of effective speeds in the marked speed queue is greater than the fourth threshold. The angle between the target movement direction of two consecutive frames in the marked velocity queue is not greater than the fifth threshold.
2. The method for detecting the motion state of an unmanned surface vessel based on lidar according to claim 1, characterized in that, The next step involves target separation of each frame of water surface point cloud after filtering using a clustering method, including: The DBSCAN algorithm is used to cluster point clouds that are close to each other in space into a cluster, and each clustered point cloud set is used as a separate target.
3. The method for detecting the motion state of an unmanned surface vessel based on lidar according to claim 1, characterized in that, Associating identical targets across consecutive frames based on the separated target centroid positions includes: Calculate the centroids of each target obtained after the clustering operation in the current frame, and construct a KD tree based on the centroids of each target; In the target of the point cloud in the previous frame, based on the KD tree, find the n preferred targets that are closest to the centroid distance of each target in the current frame point cloud, and calculate the intersection between each preferred target and the target in the current frame. If all the intersections are greater than a given threshold, then related targets are determined from the n preferred targets and assigned the same batch number; Otherwise, reselect the associated target from the target in the previous frame's point cloud.
4. The method for detecting the motion state of an unmanned surface vessel based on lidar according to claim 3, characterized in that, The step of determining the associated target from n preferred targets includes: Select the target with the most intersection as the associated target of the current frame target; Alternatively, when the intersections are the same, the preferred target with the highest intersection-union ratio is selected as the associated target of the current frame target.
5. The method for detecting the motion state of an unmanned surface vessel based on lidar according to claim 3, characterized in that, The step of reselecting the associated target from the target in the previous frame of the point cloud includes: Determine whether a target from the previous frame's point cloud exists within a preset range of the target in the current frame; If it exists, the nearest target that can be matched within that range is taken as the associated target of the current frame target and assigned the same batch number; Otherwise, the current frame target is treated as a new target and assigned a new batch number.