A sea surface target tracking method based on point cloud matching

Abstract

The application discloses a sea surface target tracking method based on point cloud matching and belongs to the technical field of intelligent navigation of ships, and comprises the following steps: obtaining original point clouds from a laser radar, obtaining object target instances in a scene through a three-dimensional target detection method, selecting a target to be tracked in the instances, initializing a region of interest range, a target motion state and a template point cloud, obtaining interframe motion information by matching a single-frame target point cloud with the template point cloud, modeling the target motion by using an extended Kalman filter and obtaining a filtered target motion state estimation, extracting a single-frame target point cloud used for constructing the template point cloud, removing interframe distortion, completing the update of the template point cloud, repeating the target motion state estimation and the template point cloud update steps and completing the continuous tracking of the target. The application can effectively improve the accuracy of the tracking of high-dynamic targets on a sea surface in a close range.

Description

Technical Field

[0001] This invention belongs to the field of intelligent navigation technology for ships, and more specifically, relates to a method for tracking sea surface targets based on point cloud matching. Background Technology

[0002] With the development of artificial intelligence technology and the expansion of its applications, the trends of autonomy and intelligence are profoundly impacting the shipbuilding industry. In ships equipped with intelligent navigation capabilities, one of the core aspects of the environmental perception system is the accurate tracking and state estimation of surrounding dynamic objects, acquiring information such as their position, orientation, and speed. The acquired target motion state information can be used to analyze the target ship's intentions, predict its behavior, and further perform other tasks such as tracking and surveillance.

[0003] Traditional intelligent navigation algorithms typically use navigation radar and AIS information for target identification and tracking. However, due to the large blind spots of navigation radar at close range and the slow update speed of AIS, they are not suitable for identifying and tracking highly dynamic targets at close range. LiDAR can supplement the field of view in near-range blind spots and has a faster data update frequency. However, existing LiDAR-based target tracking methods still have certain technical shortcomings. For example, the tracking accuracy of the "detection-tracking" paradigm is highly dependent on the detection accuracy, while neural network-based methods are prone to tracking failure when the point cloud is sparse or when encountering unfamiliar objects. Summary of the Invention

[0004] To address the aforementioned deficiencies or improvement needs of existing technologies, this invention proposes a sea surface target tracking method based on point cloud matching, which can effectively improve the accuracy of tracking high dynamic targets on the sea surface within a short range.

[0005] To achieve the above objectives, the present invention provides a sea surface target tracking method based on point cloud matching, comprising:

[0006] The original point cloud is obtained from the LiDAR, and then processed using a 3D target detection method to obtain object target instances within the scene.

[0007] Select the target to be tracked from the object target instance, and extract the point cloud, target motion state and template point cloud within the region of interest based on the target to be tracked;

[0008] Inter-frame motion information is obtained by matching point clouds within the region of interest with template point clouds. The target motion is then modeled using an extended Kalman filter in conjunction with the target motion state, and the filtered target motion state estimate is obtained.

[0009] Extract the single-frame target point cloud used to construct the template point cloud, remove inter-frame distortion, and complete the update of the template point cloud.

[0010] In some optional implementations, the step of processing the original point cloud using a 3D object detection method to obtain object target instances within the scene includes:

[0011] By checking whether the collinear eigenvalues ​​and point intensity values ​​are less than a threshold, noise points returned from the original point cloud due to illumination of sea surface clutter and water surface fog are removed.

[0012] After noise removal, a clustering method is used to cluster the point cloud spatially, distinguishing and identifying each point cloud cluster in the point cloud data. For each point cloud cluster, the orientation and bounding box range are calculated and determined, which together constitute an object target instance.

[0013] In some optional implementations, the step of removing noise points returned by sea surface clutter and water surface fog from the original point cloud by checking whether the collinear eigenvalues ​​and point intensity values ​​are less than a threshold includes:

[0014] Calculate each point (x) sequentially according to the timestamp returned from the original point cloud. i ,y i ,z i i i ) and the previous nearest point (x) i-1 ,y i-1 ,z i-1 i i-1 ) and the next nearest point (x) i+1 ,y i+1 ,z i+1 i i+1 The collinearity feature d is compared with a set threshold T. d Make a judgment; if d is less than T d Then the corresponding points are filtered out, where, x i ,y i ,z i This represents the coordinates of the i-th point in the point cloud. i This represents the intensity value of the i-th point in the point cloud;

[0015] If point (x i ,y i ,z i i i Intensity value i i Less than threshold T in If so, then the corresponding points will be filtered out.

[0016] In some optional implementations, the point cloud after noise removal is clustered using a clustering method to distinguish and identify individual point cloud clusters in the point cloud data. For each point cloud cluster, the orientation and bounding box range are calculated and determined to collectively constitute an object target instance, including:

[0017] The DBSCAN clustering method is used to cluster spatial point clouds and distinguish and identify individual point cloud clusters in the point cloud data.

[0018] Principal component analysis is performed on each point cloud cluster to obtain its feature vector. The orientation of the point cloud cluster is then determined from its feature vector. The smallest rectangular area that can enclose the point cloud cluster is taken as the vertices of the bounding box, forming bounding box B. i =[x max ,y max ,x min ,y min [,θ], each point cloud cluster P i and the corresponding bounding box B i Together they constitute target O i All targets within the scene together constitute the target list. Where, x max ,y max ,x min ,y min θ represents the maximum and minimum values ​​of the point cloud cluster in the x and y directions, respectively; θ is the rotation angle of the point cloud cluster around the z-axis; and M is the number of object instances in the current scene.

[0019] In some optional implementations, the step of extracting point clouds within the region of interest, target motion state, and template point clouds based on the target to be tracked includes:

[0020] Extract the point cloud cluster P of the target to be tracked i For the initial template point cloud M i ;

[0021] Enlarge the bounding box B corresponding to the target to be tracked by a preset ratio. i As the initial region of interest;

[0022] Take the center point (x) of the bounding box corresponding to the target to be tracked. c ,y c As the position information in the initial motion state, the bounding box direction θ is taken as the target heading information θ, and the initial velocity v and initial angular velocity are set. The values ​​are all zero, together constituting the initial motion state X of the target. i,0 =[x c ,y c ,θ,0,0] T .

[0023] In some optional implementations, the step of obtaining inter-frame motion information by matching the point cloud within the region of interest with the template point cloud includes:

[0024] The raw point cloud of the latest frame is obtained from the LiDAR. Based on the target motion state in the previous frame, the region of interest (ROI) of the previous frame is predicted. The point cloud within the predicted ROI is then extracted from the raw point cloud of the latest frame, and this predicted ROI is used as the target single-frame point cloud P of the latest frame. i ;

[0025] The target single-frame point cloud P is achieved through the nearest point iterative matching method. i With template point cloud M i Registration is performed between the current frame and the initial frame to calculate the pose transformation matrix between the current frame and the initial frame;

[0026] Solving for the target position and heading in the pose transformation matrix of the current frame yields the motion changes of the current frame relative to the initial frame, recorded as the motion state observation values ​​Z = [x, y, θ]. T Where x and y are the target positions, and θ is the target heading.

[0027] In some alternative implementations, the target single-frame point cloud P is achieved through the nearest-point iterative matching method. i With template point cloud M i Registration between the two frames is performed to calculate the rotation and translation matrix between the current frame and the initial frame, including:

[0028] Find the target single-frame point cloud P i With template point cloud M i Corresponding points between them;

[0029] Let the target single-frame point cloud P i With template point cloud M i The corresponding points between them satisfy m i =Rp i +t+N i Where R is a 3×3 rotation matrix, t is a 3×1 translation vector, and N... i Let m be the noise vector. i For the template point cloud, p i For the target single-frame point cloud;

[0030] Find a set of R and t that minimizes the first objective function J1, where, N represents the target single-frame point cloud P. i The number of points in;

[0031] Solving the nonlinear least squares problem yields the pose transformation matrix T. t :

[0032] Using matrix T t Perform coordinate transformation on the target single-frame point cloud, and continue to search for the target single-frame point cloud P. i With template point cloud M iThe corresponding points between them are used to iterate and optimize R and t until the set termination condition is met.

[0033] In some optional implementations, the step of using an extended Kalman filter to model the target motion and obtaining a filtered target motion state estimate includes:

[0034] A target motion model is established, and the nonlinear motion model and observation model are locally linearized by first-order Taylor expansion using extended Kalman filtering.

[0035] Input the target motion state observations, and estimate the target motion state by combining the initial motion state of the target with the prediction-correction mode.

[0036] In some optional implementations, the step of extracting the single-frame target point cloud for constructing the template point cloud, removing inter-frame distortion, and updating the template point cloud includes:

[0037] Based on the estimated target motion state of the current frame, obtain the bounding box position and orientation information, and zoom in to update it to the latest region of interest;

[0038] Extract the latest region of interest (ROI) point cloud, transform the coordinates of the latest ROI point cloud, align it with the template point cloud in the same coordinate system, and use them together as the template point cloud before optimization.

[0039] Construct an objective function, input the template point cloud before optimization, adjust the point cloud distribution, remove inter-frame distortion, and update to the latest template point cloud.

[0040] In some optional implementations, the construction of the objective function, taking the unoptimized template point cloud as input, adjusting the point cloud distribution, removing inter-frame distortion, and updating to the latest template point cloud, includes:

[0041] The template point cloud before optimization was divided using rectangular coordinate grid regions;

[0042] An objective function is constructed for each raster to optimize the point cloud variance within a single raster to remove inter-frame distortion. The objective function J2 is... The newly added template point cloud frame has n points, r x and r y Let p0 be the unit vectors along the X and Y axes of the grid local coordinate system, respectively, and m be the origin of the grid local coordinate system. x and m y These are the mean vectors in the X and Y directions within the grid, respectively;

[0043] Find the pose transformation matrix T acting on the newly added k-th frame point cloud. k * This minimizes the second objective function J2:

[0044] Divide the data into grid regions and apply the newly added point cloud frames according to the solved optimal pose transformation matrix T. k * Perform coordinate transformations and add them sequentially to the template point cloud to complete the template point cloud M. i Update.

[0045] In summary, compared with the prior art, the above-described technical solutions conceived by this invention can achieve the following beneficial effects:

[0046] This invention acquires lidar point cloud signals from the sea surface environment, effectively removing interference from waves and other clutter. It then uses a point cloud matching-based sea surface target tracking method to continuously track the target. By changing the target state observation method, the method for observing the target's motion state changes in consecutive frames is shifted from a detection bounding box matching-based method to a point cloud matching-based method. This allows the use of the original point cloud's structural information, overcoming the limitations of incomplete single-observation due to rapid ship movement and changing viewpoints. This effectively improves the accuracy of target tracking in dynamic environments, fills in near-range blind spots, enhances ship navigation safety, and provides support for mission execution. In particular, this invention is especially suitable for tasks requiring high accuracy in estimating the motion state of a single target. Attached Figure Description

[0047] Figure 1 This is a flowchart of a method provided in an embodiment of the present invention;

[0048] Figure 2 This is a flowchart of target tracking data processing provided in an embodiment of the present invention. Detailed Implementation

[0049] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention. Furthermore, the technical features involved in the various embodiments of this invention described below can be combined with each other as long as they do not conflict with each other.

[0050] Example 1

[0051] Figure 1 A sea surface target tracking method based on point cloud matching, provided in this embodiment of the invention, includes the following steps:

[0052] S1: Obtain the original point cloud from the LiDAR, process the original point cloud using a 3D target detection method, and obtain object target instances within the scene;

[0053] Furthermore, in step S1, the raw point cloud information on the sea surface collected by the lidar installed on the ship or shore is preprocessed and clustered to obtain a list of target instances.

[0054] The preprocessing process includes removing noise points from the input lidar point cloud caused by sea surface clutter and water fog. Noise points are identified by collinearity feature values ​​and whether the point intensity value is less than a threshold. Collinearity features are calculated using the distances between each point and its immediate and next-to-last neighboring points in the time series.

[0055] The point cloud clustering process includes identifying multiple independent objects with similar features from the point cloud frame and obtaining their bounding boxes. Specifically, it includes: using a clustering method to perform spatial point cloud clustering, distinguishing and identifying each point cloud cluster in the point cloud data; after completing the point cloud clustering, calculating and determining the orientation and bounding box range of each point cloud cluster, which together constitute a target list.

[0056] S2: Target initialization: Select the target to be tracked from the object target instance, and extract the point cloud, target motion state and template point cloud within the region of interest based on the target to be tracked;

[0057] Furthermore, the initialization of the tracked target includes: setting the initial motion state; extracting the initial template point cloud; and setting the initial region of interest for subsequent searches. The initial motion state includes the target's initial position, heading, and velocity, with the velocity set to zero. The initial template point cloud includes the target's initial frame point cloud, which is continuously updated during tracking. The initial region of interest is obtained by magnifying the target's bounding box.

[0058] S3: Target motion state estimation: Inter-frame motion information is obtained by matching the point cloud in the region of interest with the template point cloud. Based on the target motion state, the target motion is modeled using an extended Kalman filter, and the filtered target motion state estimate is obtained.

[0059] Furthermore, target motion state estimation is a continuous estimation of target position, heading, velocity, angular velocity, etc., including extraction of target point cloud in a single frame, observation of target motion state, and filtered estimation of target motion state.

[0060] In this process, the target point cloud of a single frame is obtained by extracting the point cloud within the predicted region of interest, thereby narrowing the query range.

[0061] The target's motion state is observed through a point cloud matching method, which achieves registration between the target's single-frame point cloud and the template point cloud, thereby calculating the rotation and translation matrix between the current frame and the initial frame, and obtaining the target's position and other motion states in the current frame.

[0062] The filtered estimation of the target motion state is used to filter out observation errors and noise. A target motion model is established, the observed values ​​of the target motion state are input, and the target motion state is estimated through a prediction-correction mode.

[0063] S4: Online update of template point cloud: Extract the single-frame target point cloud used to construct the template point cloud, remove inter-frame distortion, and complete the update of the template point cloud;

[0064] Furthermore, online template point cloud updates include: extraction of point clouds for updating and optimization of template point clouds.

[0065] Among them, the point cloud used for updating is obtained by extracting the point cloud within the updated region of interest;

[0066] The template point cloud optimization method includes: grid partitioning, point cloud distortion removal, and template point cloud updating. Grid partitioning distributes the point cloud to various grids; point cloud distortion removal removes inter-frame distortion by optimizing the variance of the point cloud within a single grid through solving for the optimal pose; template point cloud updating divides the grid into regions, transforms the coordinates of newly added point cloud frames according to the solved optimal pose transformation matrix, and sequentially adds them to the template point cloud, completing the online incremental update of the template point cloud.

[0067] Repeat steps S3 to S4 to complete continuous tracking of the target.

[0068] Example 2

[0069] S1: Using the ship as the origin of the coordinate system, the point cloud information returned by the lidar is converted to the ship's coordinate system based on the pre-calibrated extrinsic parameters of the shipborne lidar sensor.

[0070] In this embodiment of the invention, the point cloud that has been transformed to the ship's coordinate system is preprocessed and clustered.

[0071] Preprocessing includes:

[0072] Jump point filtering, i.e., quickly filtering out jump points returned due to sea surface clutter: processing is performed sequentially according to the timestamp information of each point returned from the point cloud. That is, each point (x...) is calculated sequentially according to the order of the returned timestamps. i ,y i ,z i i i ) and its nearest neighbor (x) i-1 ,y i-1 ,z i-1 i i-1 ) and the next nearest point (x) i+1 ,y i+1 ,z i+1 i i+1 The collinearity feature d is less than a set threshold T.d , or the intensity value i at that point i Less than the set threshold T in (That is, low-intensity values ​​are filtered out; low-intensity reflection points on the sea surface are mostly noise points reflected from laser light hitting water mist), and are therefore filtered out. The collinearity characteristic d is obtained according to the following equation:

[0073]

[0074] Through processing, noise points reflected from water surface clutter and fog can be effectively removed, while retaining the point cloud that is actually irradiated onto the sea surface obstacle target.

[0075] Point cloud clustering includes:

[0076] Density-based spatial point cloud clustering. The DBSCAN clustering method is used to discover clusters by finding high-density regions and extending to connected low-density regions. This effectively identifies point cloud clusters of various shapes and sizes with minimal noise interference. Principal component analysis is performed on each cluster to obtain the feature vectors of the point sets, thereby obtaining the orientation and bounding box vertices of the clusters, which are then used to fit a bounding box to each cluster. The i-th cluster P after clustering is selected. i For example, P i Composed of N points, represented as Its centroid is p p =[x p ,y p ,z p i p When performing principal component analysis, the z-axis direction and intensity value are ignored, and only the first two dimensions are considered. P i Subtracting the centroid from each point yields the standardized two-dimensional point set Q. i Calculate matrix Q i Q i The eigenvectors and eigenvalues ​​of Q' are such that the eigenvector corresponding to the largest eigenvalue is the principal component. i Projecting onto the new coordinate system formed by the principal components yields the maximum and minimum projection points along the principal component directions. Finally, backprojecting these points back to the original Cartesian coordinate system yields the bounding box vertices. Using the principal component directions as the orientation of the bounding box, bounding box B is constructed. i =[x max ,y max ,x min ,y min ,θ]. Each point cloud cluster P i and its corresponding bounding box B i Together they constitute target O i All targets within the scene together constitute the target list. Where, x max ,y max ,x min ,y min θ represents the maximum and minimum values ​​of the point cloud cluster in the x and y directions, respectively; θ is the rotation angle of the point cloud cluster around the z-axis; and M is the number of object instances in the current scene.

[0077] S2: Select the target to be tracked. Select the target O from the target list O. i The selection method can be manual or automatically selected by the program based on the judgment criteria. In particular, multiple targets can be selected at once for simultaneous tracking.

[0078] In this embodiment of the invention, the crew manually selects the target to be tracked from the target list displayed on the bridge display device. The bounding box B corresponding to the target to be tracked is then magnified by a certain ratio γ (γ≥1). i This is taken as the initial region of interest. This initial region of interest is denoted as ROI. i =[γx max ,γy max ,γx min ,γy min ,θ]. Calculate the position of the center point of the bounding box:

[0079]

[0080] Set the target to be tracked O i The state of motion is The initial motion state is set as X. i,0 =[x c ,y c ,θ,0,0] T .

[0081] Extract the point cloud cluster P of the target to be tracked. i For the initial template point cloud M i At this point, the initial template point cloud contains only one frame of point cloud information.

[0082] S3: As Figure 2 The diagram shows the target tracking data processing flowchart. After target tracking begins, each input frame of LiDAR point cloud is used to update the target motion state observation in that frame. Based on the target motion state, the region of interest (ROI) range of the previous frame is predicted to narrow the query range and reduce the impact of interference. Points within the ROI are extracted from the raw point cloud of the latest frame to form the target single-frame point cloud P. i The two point clouds were registered in spatial coordinate system using the Iterative Closest Point Matching (ICP) method. The steps are as follows:

[0083] (1) Find the correspondence. Find the target single-frame point cloud P. i With template point cloud M i The corresponding points between them. The KD-Tree data structure is used to query the nearest point in 3D space. This data structure is commonly used for nearest point search in point clouds and can be used to find the target single-frame point cloud P. i With template point cloud M i The correspondence between them.

[0084] (2) Optimize the pose transformation matrix. Let the target single-frame point cloud P be... i With template point cloud M i The corresponding points between them satisfy the following relationship: m i =Rp i +t+N i Where R is a 3×3 rotation matrix, t is a 3×1 translation vector, and N... i Let be the noise vector. The problem is constructed as finding a set of R and t that minimizes the first objective function J1: Solving the nonlinear least squares problem yields the pose transformation matrix T. t :

[0085] (3) Iteration. Using matrix T t Perform coordinate transformation on the target single-frame point cloud, and continue to perform iterative processes of finding the corresponding point and optimizing R and t until the set termination condition is met.

[0086] Solving for the target position and heading in the pose transformation matrix of the current frame yields the motion changes of the current frame relative to the initial frame, recorded as the motion state observation values ​​Z = [x, y, θ]. T .

[0087] Extended Kalman filtering is used to filter observation errors and noise. A target motion model is established. The target motion model is set as a constant turn rate and velocity (CTRV) model. Specifically, it is expressed as: Where x is the abscissa, y is the ordinate, and θ is the yaw angle. The time interval from time k to time k+1 is Δt, and the target velocity at time k is v. k angular velocity is Therefore, the state variable is defined as follows:

[0088] An extended Kalman filter (EKF) is used to locally linearize the nonlinear motion and observation models through a first-order Taylor expansion to complete the state estimation. Considering the nonlinear system, its motion and observation model formulas are as follows: Where k is a discrete-time variable, x k For the state, z k For observation, u k For input, w k For noise, v k The noise is observational. f(·) and h(·) are nonlinear functions. A first-order Taylor expansion is performed near the current state estimate to linearize the motion and observation models: in,

[0089]

[0090] The steps of extended Kalman filtering are as follows:

[0091] (1) Initialization. Let the observation vector be Z = [x, y, θ]. T State vector Let the initial observed pose be the initial estimate. Define the system state noise covariance matrix Q0 and the observation noise covariance matrix R0. Covariance represents the uncertainty of state variables, and the inverse of the covariance represents the quality of the state. These parameters can be adjusted according to actual needs. Given P0.

[0092] (2) Prediction: Calculate the Jacobian matrix A and update it using the following formula:

[0093]

[0094] (3) Correction: Calculate the Jacobian matrix H and update it using the following formula:

[0095]

[0096] Iterate through steps (2) and (3). Among them... The posterior estimate is the target motion state estimated in the current frame. In the diagram, the X-axis coordinate, Y-axis coordinate, heading angle, velocity, and angular velocity represent the target position in sequence.

[0097] S4: Perform online updates to the template point cloud. Based on the target status of the current frame. Obtain the current bounding box position and orientation information, and zoom in to update it with the latest region of interest result. Extract the point cloud within the region for template point cloud updates. Based on the target state of the current frame. The coordinates of this single-frame point cloud are transformed and aligned with the template point cloud in the same coordinate system, serving as the template point cloud before optimization.

[0098] Construct an objective function by taking the unoptimized template point cloud as input, adjusting the point cloud distribution, removing inter-frame distortion, and updating it with the latest template point cloud. The steps are as follows:

[0099] (1) Grid division. The template point cloud is divided using a rectangular coordinate grid area, where the resolution of the grid can be adjusted. Each point is projected onto the corresponding grid.

[0100] (2) Point cloud distortion removal. Inter-frame distortion is removed by constructing an objective function to optimize the variance of the point cloud within a single grid cell. After coordinate transformation, the unoptimized template point cloud is basically concentrated near the actual surface of the object, but some positional noise still exists due to inter-frame distortion and observation noise. Assuming the newly added template point cloud frame has n points, a second objective function J2 is constructed in each grid cell:

[0101]

[0102] Where, r x and r y Let p0 be the unit vectors along the X and Y axes of the grid local coordinate system, respectively, and m be the origin of the grid local coordinate system. x and m y Let T be the mean vectors in the X and Y directions within the grid, respectively. The optimization problem is to find the pose transformation matrix T acting on the k-th frame point cloud of the newly added template point cloud. k * This minimizes the second objective function J2: T k The optimal transformation matrix from the k-th frame point cloud to the template point cloud is given.

[0103] (3) Template point cloud update. Divide the grid into regions and update the newly added point cloud frames according to the solved optimal pose transformation matrix T. k * Perform coordinate transformations and add them sequentially to the template point cloud to complete the template point cloud M. i Update.

[0104] Repeat steps S3 to S4 to observe the target's motion state by matching the single-frame point cloud with the template point cloud. Then update the template point cloud to overcome the incompleteness of a single observation and provide more refined state information, thereby obtaining a more accurate observation of the target's motion state and ultimately completing continuous tracking of the target. For the i-th target, this means obtaining its motion state estimates for k consecutive frames.

[0105] It should be noted that, depending on the implementation needs, the various steps / components described in this application can be broken down into more steps / components, or two or more steps / components or parts of the operation of steps / components can be combined into new steps / components to achieve the purpose of this invention.

[0106] Those skilled in the art will readily understand that the above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for tracking sea surface targets based on point cloud matching, characterized in that, include: The original point cloud is obtained from the LiDAR, and then processed using a 3D target detection method to obtain object target instances within the scene. Select the target to be tracked from the object target instance, and extract the point cloud, target motion state and template point cloud within the region of interest based on the target to be tracked; Inter-frame motion information is obtained by matching point clouds within the region of interest with template point clouds. The target motion is then modeled using an extended Kalman filter in conjunction with the target motion state, and the filtered target motion state estimate is obtained. Extract the single-frame target point cloud used to construct the template point cloud, remove inter-frame distortion, and complete the update of the template point cloud; The step of extracting the single-frame target point cloud used to construct the template point cloud, removing inter-frame distortion, and updating the template point cloud includes: Based on the estimated target motion state of the current frame, obtain the bounding box position and orientation information, and zoom in to update it to the latest region of interest; Extract the latest region of interest (ROI) point cloud, transform the coordinates of the latest ROI point cloud, align it with the template point cloud in the same coordinate system, and use them together as the template point cloud before optimization. Construct an objective function, input the template point cloud before optimization, adjust the point cloud distribution, remove inter-frame distortion, and update to the latest template point cloud; The construction of the objective function, taking the unoptimized template point cloud as input, adjusting the point cloud distribution, removing inter-frame distortion, and updating to the latest template point cloud, includes: The template point cloud before optimization was divided using rectangular coordinate grid regions; Construct the objective function in each grid Optimize the variance of point cloud within a single grid to remove inter-frame distortion; the newly added template point cloud has n points in the point cloud frame. Find the first point cloud that acts on the newly added template. Optimal pose transformation matrix of frame point cloud , so that the objective function minimize; Divide the data into grid regions and apply the newly added point cloud frames to the optimal pose transformation matrix obtained by solving the problem. Perform coordinate transformation and add the template point cloud sequentially to complete the template point cloud update.

2. The method according to claim 1, characterized in that, The process of processing the original point cloud using a 3D object detection method to obtain object target instances within the scene includes: Remove noise points reflected from the original point cloud due to illumination from sea surface clutter and water surface fog; After noise removal, a clustering method is used to cluster the point cloud spatially, distinguishing and identifying each point cloud cluster in the point cloud data. For each point cloud cluster, the orientation and bounding box range are calculated and determined, which together constitute an object target instance.

3. The method according to claim 2, characterized in that, The point cloud after noise removal is then clustered using a clustering method to distinguish and identify individual point cloud clusters. For each point cloud cluster, its orientation and bounding box range are calculated and determined, collectively forming an object target instance, including: The DBSCAN clustering method is used to cluster spatial point clouds and distinguish and identify individual point cloud clusters in the point cloud data. Principal component analysis is performed on each point cloud cluster to obtain its feature vector. The orientation of the point cloud cluster is then determined from its feature vector. The smallest rectangular area that can enclose the point cloud cluster is taken as the vertices of the bounding box, thus forming the bounding box. Each point cloud cluster and the corresponding enclosure Together constitute the goal All targets within the scene together constitute the target list. ,in, Point cloud clusters are respectively x direction, y Maximum and minimum values ​​in the direction, It is a cluster of clouds facing around z The rotation angle of the axis, This represents the number of object instances in the current scene.

4. The method according to claim 3, characterized in that, The step of extracting point clouds within the region of interest, target motion state, and template point clouds based on the target to be tracked includes: Extract point cloud clusters of the target to be tracked Point cloud for initial template ; Enlarge the bounding box corresponding to the target to be tracked by a preset ratio. As the initial region of interest; Find the center point of the bounding box corresponding to the target to be tracked. As the position information in the initial motion state, the bounding box direction is taken. As heading information, the initial velocity is set. With initial angular velocity The values ​​are all zero, together constituting the initial motion state of the target. .

5. The method according to claim 4, characterized in that, The step of obtaining inter-frame motion information by matching the point cloud within the region of interest with the template point cloud includes: The raw point cloud of the latest frame is obtained from the lidar. Based on the target motion state in the previous frame, the range of the region of interest in the previous frame is predicted. The point cloud of the predicted region of interest is extracted from the raw point cloud of the latest frame and used as the target single-frame point cloud of the latest frame. The target single-frame point cloud and template point cloud are obtained by using the nearest point iterative matching method. Registration is performed between the current frame and the initial frame to calculate the pose transformation matrix between the current frame and the initial frame; Solving for the target position and heading in the pose transformation matrix of the current frame yields the motion changes of the current frame relative to the initial frame, which are recorded as motion state observations. ,in, For the target location, For the target heading.

6. The method according to claim 5, characterized in that, The method of nearest-point iterative matching is used to achieve the target single-frame point cloud and template point cloud. Registration between the two frames is performed to calculate the pose transformation matrix between the current frame and the initial frame, including: Finding the target single-frame point cloud and the template point cloud Corresponding points between them; Let the target single-frame point cloud and the template point cloud be defined. The corresponding points between them satisfy ,in, For one The rotation matrix, It is The translation vector, For noise vectors, For template point cloud, For the target single-frame point cloud; Found a group and Make the first objective function Minimize, where, , N Indicates the number of points in a single frame of the target point cloud; Solve the nonlinear least squares problem to obtain the pose transformation matrix. : ; Using matrices Perform coordinate transformation on the target single-frame point cloud, and continue to search for the target single-frame point cloud and the template point cloud. Corresponding points between them, to pair and The optimization process is iterated continuously until the set termination condition is met.

7. The method according to claim 6, characterized in that, The step of modeling the target motion using an extended Kalman filter and obtaining a filtered target motion state estimate includes: A target motion model is established, and the nonlinear motion model and observation model are locally linearized by first-order Taylor expansion using extended Kalman filtering. Input the target motion state observations, and estimate the target motion state by combining the initial motion state of the target with the prediction-correction mode.

Images