Sea surface target tracking method based on point cloud matching

Abstract

The invention discloses a sea surface target tracking method based on point cloud matching, and belongs to the technical field of intelligent navigation of ships, and the method comprises the steps: obtaining an original point cloud from a laser radar, and obtaining an object target instance in a scene through a three-dimensional target detection method; selecting a to-be-tracked target in the instance, and initializing a region-of-interest range, a target motion state and a template point cloud; inter-frame motion information is obtained by matching a single-frame target point cloud with a template point cloud, modeling is performed on target motion by using an extended Kalman filter, and filtered target motion state estimation is obtained; extracting a single-frame target point cloud used for constructing a template point cloud, and removing inter-frame distortion to complete updating of the template point cloud; and repeating the steps of estimating the motion state of the target and updating the template point cloud to finish continuous tracking of the target. According to the invention, the accuracy of sea surface high-dynamic target tracking in a short-distance range can be effectively improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of ship intelligent navigation, and more specifically, relates to a sea surface target tracking method based on point cloud matching. Background Art

[0002] With the development of artificial intelligence technology and the expansion of its application, the trend of autonomy and intelligence is profoundly affecting the shipping industry. In ships equipped with intelligent navigation functions, one of the core of the environmental perception system is to accurately track and estimate the state of surrounding dynamic objects, and obtain information such as their position, direction, speed, etc. The acquired target motion state information can be used to analyze the intention of the target ship, predict the behavior of the target ship, and further perform other tasks such as tracking and monitoring on this basis.

[0003] Traditional intelligent navigation algorithms generally use information from navigation radar and AIS for target identification and tracking. Since navigation radar has a large blind spot at close range and AIS updates slowly, it is not suitable for identifying and tracking nearby high-dynamic targets. LiDAR can supplement the field of view of nearby blind spots and has a faster data update frequency. However, existing LiDAR-based target tracking methods still have certain technical defects. For example, the tracking accuracy of the "detection-tracking" paradigm method is highly dependent on the detection accuracy, while the neural network-based method is prone to tracking failure when the point cloud is sparse and encounters unfamiliar objects. Summary of the invention

[0004] In view of the above defects or improvement needs of the prior art, the present 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 close range.

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

[0006] Obtain the original point cloud from the laser radar, process the original point cloud through the 3D target detection method, and obtain the object target instance in the scene;

[0007] Selecting a target to be tracked in the object target instance, and extracting a point cloud in an area of ​​interest, a target motion state, and a template point cloud according to the target to be tracked;

[0008] The inter-frame motion information is obtained by matching the point cloud in the region of interest with the template point cloud, and the target motion is modeled using an extended Kalman filter in combination with the target motion state, and the filtered target motion state estimation is obtained;

[0009] The single-frame target point cloud used to construct the template point cloud is extracted, the inter-frame distortion is removed, and the template point cloud is updated.

[0010] In some optional implementation schemes, the processing of the original point cloud by a three-dimensional target detection method to obtain an object target instance in the scene includes:

[0011] By checking whether the collinear eigenvalue and point intensity value are less than the threshold, the noise points returned by irradiating the sea surface clutter and water surface fog are removed from the original point cloud;

[0012] For the point cloud after noise removal, a clustering method is used to perform spatial point cloud clustering to distinguish and identify the various point cloud clusters in the point cloud data. For each point cloud cluster, the point cloud cluster orientation and bounding box range are calculated and determined to together constitute an object target instance.

[0013] In some optional implementation schemes, the removing of noise points returned by irradiating sea surface clutter and water surface fog from the original point cloud by determining whether the collinear feature value and the point intensity value are less than a threshold value includes:

[0014] Calculate each point (x) in the order of timestamps returned by the original point cloud. i ,y i ,z i ,i i ) and the previous adjacent point (x i-1 ,y i-1 ,z i-1 ,i i-1 ) and the next adjacent point (x i+1 ,y i+1 ,z i+1 ,i i+1 ) and compare the collinear feature d with the set threshold T d Make a judgment, if d is less than T d , then filter out the corresponding points, where x i ,y i ,z i Represents the coordinate value of the i-th point in the point cloud, i i Represents the intensity value of the i-th point in the point cloud;

[0015] If the point (x i ,y i ,z i ,i i ) intensity value i i Less than the threshold T in , then filter out the corresponding points.

[0016] In some optional implementation schemes, the point cloud after noise removal is clustered using a clustering method to distinguish and identify each point cloud cluster in the point cloud data, and for each point cloud cluster, the point cloud cluster orientation and bounding box range are calculated and determined to form an object target instance, including:

[0017] Use DBSCAN clustering method to cluster spatial point clouds and distinguish and identify each point cloud cluster in the point cloud data;

[0018] By performing principal component analysis on each point cloud cluster, the feature vector of the point cloud cluster is obtained, and the orientation of the point cloud cluster is obtained from the feature vector of the point cloud cluster. The minimum rectangular range that can envelop the point cloud cluster is taken as the vertex of the bounding box to form the 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 form the goal i , all targets in the scene together constitute the target list Among them, x max ,y max ,x min ,y min are 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 target instances in the current scene.

[0019] In some optional implementation schemes, extracting a point cloud within the region of interest, a target motion state, and a template point cloud based on the target to be tracked includes:

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

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

[0022] Take the center point position (x c ,y c ) as the position information in the initial motion state, take the bounding box direction θ as the target heading information θ, set the initial velocity v and the initial angular velocity is zero, together forming the target initial motion state X 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] Get the original point cloud of the latest frame from the LiDAR, predict the range of the region of interest in the previous frame according to the target motion state in the previous frame, extract the point cloud in the predicted region of interest from the original point cloud of the latest frame, and use it as the target single frame point cloud P of the latest frame i ;

[0025] The target single-frame point cloud P is realized by the closest point iterative matching method i With the template point cloud M i The registration between them is performed to calculate the pose transformation matrix between the current frame and the initial frame;

[0026] Solve the target position and target heading in the current frame pose transformation matrix, that is, the motion change of the current frame relative to the initial frame, and record it as the motion state observation value Z = [x, y, θ] T , where x, y are the target positions and θ is the target heading.

[0027] In some optional embodiments, the target single-frame point cloud P is realized by the closest point iterative matching method. i With the template point cloud M i The registration between them is used 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 the template point cloud M i The corresponding points between

[0029] Assume the target single frame point cloud P i With the 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 is the noise vector, m i is the template point cloud, p i It is 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] Solve the nonlinear least squares problem and get the posture transformation matrix T t :

[0032] Using the 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 the template point cloud M iThe corresponding points between them are used to optimize R and t continuously and iteratively until the set termination condition is met.

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

[0034] Establish the target motion model, and use the extended Kalman filter to locally linearize the nonlinear motion model and observation model through the first-order Taylor expansion;

[0035] The target motion state observation value is input, and the target motion state is estimated through the prediction-correction mode combined with the target initial motion state.

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

[0037] According to the estimated target motion state of the current frame, the bounding box position and orientation information are obtained, and the bounding box is enlarged and updated to the latest area of ​​interest;

[0038] Extract the latest point cloud of the region of interest, transform the coordinates of the latest point cloud of the region of interest, 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 the 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 implementation schemes, constructing the objective function, inputting the template point cloud before optimization, adjusting the point cloud distribution, removing inter-frame distortion, and updating to the latest template point cloud include:

[0041] Use rectangular grid area to divide the template point cloud before optimization;

[0042] The objective function is constructed in each grid to optimize the point cloud variance within a single grid to remove inter-frame distortion, where the objective function J2 is: The point cloud frame newly added to the template point cloud has n points, r x and r y are the unit vectors of the local coordinates of the grid in the X-axis and Y-axis directions, p0 is the local origin of the grid, m x and m y 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 * , so that the second objective function J2 is minimized:

[0044] Divide each grid area and transform the newly added point cloud frame according to the solved optimal pose transformation matrix T k * Perform coordinate transformation and add the template point cloud in sequence to complete the template point cloud M i Updates.

[0045] In general, the above technical solutions conceived by the present invention can achieve the following beneficial effects compared with the prior art:

[0046] The present invention collects laser radar point cloud signals in the sea environment, effectively removes interference such as sea clutter, and uses a sea surface target tracking method based on point cloud matching to continuously track the target to be tracked. By changing the observation method of the target state, the state observation method of the target motion state change in continuous frames is changed from a method based on detection bounding box matching to a method based on point cloud matching. Therefore, by using the structural information of the original point cloud, the defects of incomplete single observation due to rapid movement of the ship, change of perspective, etc. can be overcome, and the accuracy of target tracking in a dynamic environment can be effectively improved, close-range blind spots can be supplemented, and the safety of ship navigation can be improved to provide support for task execution. In particular, the present invention is particularly suitable for tasks that require high precision in estimating the motion state of a single target. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1 is a flow chart of a method provided by an embodiment of the present invention;

[0048] Figure 2 The present invention provides a target tracking data processing flow chart. DETAILED DESCRIPTION

[0049] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0050] Embodiment 1

[0051] Figure 1 A method for tracking a sea surface target based on point cloud matching provided by an embodiment of the present invention comprises the following steps:

[0052] S1: Obtain the original point cloud from the LiDAR, process the original point cloud through the 3D target detection method, and obtain the object target instance in the scene;

[0053] Furthermore, in step S1, the original point cloud information on the sea surface collected by the laser radar installed on the ship or shore is preprocessed and point cloud clustered to obtain a target instance list.

[0054] The preprocessing process includes: removing the noise points returned by the input laser radar point cloud due to the irradiation of sea clutter and water surface fog. The noise points are judged by whether the collinear feature value and the point intensity value are less than the threshold. The collinear feature is calculated by the distance between each point and the upper and lower adjacent points in the time series.

[0055] Among them, the point cloud clustering process includes identifying multiple independent objects with similar features from the point cloud frame and obtaining their bounding boxes, specifically including: 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, for each point cloud cluster, calculating and determining the point cloud cluster orientation and bounding box range, together forming a target list.

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

[0057] Furthermore, the initialization of the tracking 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 initial position, heading, speed, etc. of the target, and the speed is set to zero. The initial template point cloud includes the initial frame point cloud of the target, which is continuously updated during the tracking process. The initial region of interest is obtained by enlarging the bounding box of the target.

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

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

[0060] Among them, the single-frame target point cloud is obtained by extracting the point cloud within the predicted area of ​​interest to narrow the query scope.

[0061] The observation of the target motion state is carried out through the point cloud matching method to achieve the registration between the target single-frame point cloud and the template point cloud, so as to calculate the rotation and translation matrix between the current frame and the initial frame, and obtain the motion state such as the position of the current frame target.

[0062] The target motion state filtering estimation is used to filter the observed errors and noise. The target motion model is established, the target motion state observation value is input, and the target motion state is estimated through the prediction-correction mode.

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

[0064] Furthermore, the online updating of the template point cloud includes: extraction of the point cloud for updating and optimization of the template point cloud.

[0065] The extraction of the point cloud for updating is obtained by extracting the point cloud within the updated region of interest;

[0066] The template point cloud optimization method includes: grid division, point cloud distortion removal, and template point cloud update. Grid division distributes the point cloud to each grid; point cloud distortion removal removes inter-frame distortion by solving the optimal pose to optimize the point cloud variance within a single grid; template point cloud update is divided into each grid area, and the newly added point cloud frame is transformed according to the solved optimal pose transformation matrix, and then added to the template point cloud in turn to complete the online incremental update of the template point cloud.

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

[0068] Embodiment 2

[0069] S1: Taking the ship as the coordinate origin, according to the pre-calibrated shipborne lidar sensor external parameters, the point cloud information returned by the lidar is converted to the ship's coordinate system.

[0070] In the embodiment of the present invention, the point cloud that has been converted to the own-ship coordinate system is preprocessed and point cloud clustered.

[0071] Among them, preprocessing includes:

[0072] Jump point filtering, that is, quickly filtering out the jump points returned due to the irradiation of sea surface clutter: process them in sequence according to the timestamp information of each point returned by the point cloud. That is, calculate each point (x i ,y i ,z i ,i i ) and its previous adjacent point (x i-1 ,y i-1 ,z i-1 ,i i-1 ) and the next adjacent point (x i+1 ,y i+1 ,z i+1 ,i i+1 ) of the collinear feature d, if the collinear feature d is less than the set threshold Td , or the intensity value i at that point i Less than the set threshold T in (i.e. low intensity values ​​are filtered out, and low intensity reflection points on the sea surface are mostly noise points reflected by the laser irradiating the fog on the water surface), then they are filtered out. Among them, the collinear feature d is obtained according to the following equation:

[0073]

[0074] Through processing, the noise points returned by irradiating the water surface clutter and water surface fog can be effectively removed, and the point cloud actually irradiated on the sea surface obstacle target can be retained.

[0075] Among them, point cloud clustering includes:

[0076] Density-based spatial point cloud clustering. Using the DBSCAN clustering method, clusters are found by finding high-density areas and expanding to the low-density areas connected to them. This method can effectively identify point cloud clusters of various shapes and sizes in point cloud data, and is less affected by noise. Principal component analysis is performed on each point cloud cluster to obtain the feature vector of the point set, and thus the orientation and bounding box vertices of the point cloud cluster are obtained to fit the bounding box of each point cloud cluster. Take the i-th point cloud cluster P after clustering i For example, P i It is composed of N points and is represented by Its center of gravity is p p =[x p ,y p ,z p ,i p ]. When performing principal component analysis, ignore the z-axis direction and intensity value and only consider the first two dimensions. i Subtract the centroid from each point to get the standardized two-dimensional point set Q i . Calculate the matrix Q i Q i ', the eigenvector and eigenvalue of which the eigenvector corresponding to the maximum eigenvalue is the principal component. i Projecting to the new coordinate system composed of the principal components, we can obtain the maximum and minimum projection points in the principal component direction. Finally, we back-project the maximum and minimum projection points back to the original Cartesian coordinate system to obtain the bounding box vertices. Taking the principal component direction as the orientation of the bounding box, we can construct the bounding box B. i =[x max ,y max ,x min ,y min ,θ]. Each point cloud cluster P i And its corresponding bounding box B i Together they form the goal i , all targets in the scene together constitute the target list Among them, x max ,y max ,x min ,y min are 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 target instances in the current scene.

[0077] S2: Select the target to be tracked. Select the target O to be tracked from the target list O. i The selection method can be selected manually or by the program according to the judgment conditions. In particular, multiple targets to be tracked can be selected at one time and tracked simultaneously.

[0078] In the implementation case of the present 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 enlarged by a certain ratio γ (γ≥1) i , as the initial region of interest. This initial region of interest is recorded as ROI i =[γx max ,γy max ,γx min ,γy min ,θ]. Calculate the center point position of the bounding box:

[0079]

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

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

[0082] S3: Figure 2 The figure shows the target tracking data processing flow chart. After starting the target tracking, each frame of laser radar point cloud is input and used to update the observation of the target motion state in this frame. According to the target motion state, the range of the region of interest in the previous frame is predicted to narrow the query range and reduce the influence of interference. The point cloud in the region of interest is extracted from the original point cloud of the latest frame as the target single frame point cloud P of the latest frame. i The closest point iterative matching method (ICP) is used to align the two point clouds in the spatial coordinate system. The steps are as follows:

[0083] (1) Find the corresponding relationship. Find the target single frame point cloud P i With the template point cloud M i The KD-Tree data structure is used to query the nearest point in three-dimensional space. This data structure is often used for searching the nearest point of a point cloud and can be used to find the target single-frame point cloud P. i With the template point cloud M i The corresponding relationship between them.

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

[0085] (3) Iteration. Using matrix T t The coordinates of the target single-frame point cloud are transformed, and the corresponding points are searched and R and t are optimized iteratively until the set termination condition is met.

[0086] Solve the target position and heading in the current frame pose transformation matrix, that is, the motion change of the current frame relative to the initial frame, and record it as the motion state observation value Z = [x, y, θ] T .

[0087] Use extended Kalman filtering to filter the observed errors and noise. Establish a target motion model. The target motion model is set to a constant turn rate and velocity model (Constant Turn Rate and Velocity, CTRV). The specific expression is: Where x is the horizontal coordinate, y is the vertical coordinate, and θ is the yaw angle. The time interval from time k to time k+1 is Δt, and the target speed at time k is v k , the angular velocity is Thus, the state variable is defined as

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

[0089]

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

[0091] (1) Initialization. Let the observation vector Z = [x, y, θ] T , the state vector Let the initial observation pose be the initial estimate. Set the system state noise covariance matrix Q0 and the observation noise covariance matrix R0. The covariance represents the uncertainty of the state quantity, and the inverse of the covariance represents the quality of the state. The 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 steps (2) and (3). is the posterior estimate, i.e. the target motion state estimated in the current frame In the above table, they represent the target position X-axis coordinate position, Y-axis coordinate position, heading angle, speed, and angular velocity respectively.

[0097] S4: Perform online update of the template point cloud. According to the current frame target state Get the bounding box position and orientation information at this time, and zoom in and update it to the latest result of the area of ​​interest. Extract the point cloud in the area for template point cloud update. According to the current frame target state The single-frame point cloud is transformed into coordinates and aligned with the template point cloud in the same coordinate system, and they are used together as the template point cloud before optimization.

[0098] Construct the objective function, input the template point cloud before optimization, adjust the point cloud distribution, remove the inter-frame distortion, and update it to the latest template point cloud. The steps are as follows:

[0099] (1) Grid division. The template point cloud is divided into rectangular grid regions, where the resolution of the grid is adjustable. Each point is projected to the corresponding grid.

[0100] (2) Point cloud distortion removal. By constructing an objective function and optimizing the point cloud variance within a single grid, inter-frame distortion is removed. After coordinate transformation, the template point cloud before optimization is basically concentrated near the real surface of the object, but there is still a certain position noise caused by inter-frame distortion and observation noise. Assume that the point cloud frame newly added to the template point cloud has n points, and construct the second objective function J2 in each grid:

[0101]

[0102] Among them, r x and r y are the unit vectors of the local coordinates of the grid in the X-axis and Y-axis directions, p0 is the local origin of the grid, m x and m y are the mean vectors in the X and Y directions of the grid respectively. The optimization problem is to find the pose transformation matrix T of the k-th frame point cloud that acts on the newly added template point cloud k * , so that the second objective function J2 is minimized: T k It is the optimal transformation matrix from the kth frame point cloud newly added to the template point cloud to the template point cloud.

[0103] (3) Template point cloud update. Divide each grid area and transform the newly added point cloud frame according to the solved optimal pose transformation matrix T k * Perform coordinate transformation and add the template point cloud in sequence to complete the template point cloud M i Updates.

[0104] Repeat steps S3 to S4 to complete the observation of the target motion state by matching the single-frame point cloud and the template point cloud, and then update the template point cloud to overcome the defect of incomplete single observation and provide more detailed state information to obtain a more accurate observation of the target motion state, and finally complete the continuous tracking of the target. For the i-th target, that is, to obtain the target motion state estimation of its continuous k frames

[0105] It should be pointed out that, according to the needs of implementation, the various steps / components described in this application can be split into more steps / components, and two or more steps / components or partial operations of steps / components can be combined into new steps / components to achieve the purpose of the present invention.

[0106] It will be easily understood by those skilled in the art that the above description is only 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 in the protection scope of the present invention.

Claims

1. A method for tracking sea surface targets based on point cloud matching, characterized in that: include: Obtain the original point cloud from the laser radar, process the original point cloud through the 3D target detection method, and obtain the object target instance in the scene; Selecting a target to be tracked in the object target instance, and extracting a point cloud in an area of ​​interest, a target motion state, and a template point cloud according to the target to be tracked; The inter-frame motion information is obtained by matching the point cloud in the region of interest with the template point cloud, and the target motion is modeled using an extended Kalman filter in combination with the target motion state, and the filtered target motion state estimation is obtained; The single-frame target point cloud used to construct the template point cloud is extracted, the inter-frame distortion is removed, and the template point cloud is updated.

2. The method according to claim 1, characterized in that The method of processing the original point cloud by a three-dimensional target detection method to obtain an object target instance in the scene includes: By checking whether the collinear eigenvalue and point intensity value are less than the threshold, the noise points returned by irradiating the sea surface clutter and water surface fog are removed from the original point cloud; For the point cloud after noise removal, a clustering method is used to perform spatial point cloud clustering to distinguish and identify the various point cloud clusters in the point cloud data. For each point cloud cluster, the point cloud cluster orientation and bounding box range are calculated and determined to together constitute an object target instance.

3. The method according to claim 2, characterized in that The method of removing noise points returned by irradiating sea surface clutter and water surface fog from the original point cloud by determining whether the collinear eigenvalue and the point intensity value are less than a threshold value includes: Calculate each point (x) in the order of timestamps returned by the original point cloud. i ,y i ,z i ,i i ) and the previous adjacent point (x i-1 ,y i-1 ,z i-1 ,i i-1 ) and the next adjacent point (x i+1 ,y i+1 ,z i+1 ,i i+1 ) and compare the collinear feature d with the set threshold T d Make a judgment, if d is less than T d , then filter out the corresponding points, where x i ,y i ,z i Represents the coordinate value of the i-th point in the point cloud, i i Represents the intensity value of the i-th point in the point cloud; If the point (x i ,y i ,z i ,i i ) intensity value i i Less than the threshold T in , then filter out the corresponding points.

4. The method according to any one of claims 1 to 3, characterized in that: The point cloud after noise removal is clustered using a clustering method to cluster the spatial point cloud, distinguish and identify each point cloud cluster in the point cloud data, and for each point cloud cluster, calculate and determine the point cloud cluster orientation and bounding box range, which together constitute an object target instance, including: Use DBSCAN clustering method to cluster spatial point clouds and distinguish and identify each point cloud cluster in the point cloud data; By performing principal component analysis on each point cloud cluster, the feature vector of the point cloud cluster is obtained, and the orientation of the point cloud cluster is obtained from the feature vector of the point cloud cluster. The minimum rectangular range that can envelop the point cloud cluster is taken as the vertex of the bounding box to form the 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 form the goal i , all targets in the scene together constitute the target list Among them, x max ,y max ,x min ,y min are 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 target instances in the current scene.

5. The method according to claim 4, characterized in that The step of extracting a point cloud in the region of interest, a target motion state, and a template point cloud according to the target to be tracked includes: Extract the point cloud cluster P of the target to be tracked i is the initial template point cloud M i ; Enlarge the bounding box B corresponding to the target to be tracked at a preset ratio i As the initial region of interest; Take the center point position (x c ,y c ) as the position information in the initial motion state, take the bounding box direction θ as the target heading information θ, set the initial velocity v and the initial angular velocity is zero, together forming the target initial motion state X i,0 =[x c ,y c ,θ,0,0] T .

6. The method according to claim 5, characterized in that The step of obtaining inter-frame motion information by matching the point cloud in the region of interest with the template point cloud includes: Get the original point cloud of the latest frame from the LiDAR, predict the range of the region of interest in the previous frame according to the target motion state in the previous frame, extract the point cloud in the predicted region of interest from the original point cloud of the latest frame, and use it as the target single frame point cloud P of the latest frame i ; The target single-frame point cloud P is realized by the closest point iterative matching method i With the template point cloud M i The registration between them is performed to calculate the pose transformation matrix between the current frame and the initial frame; Solve the target position and heading in the current frame pose transformation matrix, that is, the motion change of the current frame relative to the initial frame, and record it as the motion state observation value Z = [x, y, θ] T , where x, y are the target positions and θ is the target heading.

7. The method according to claim 6, characterized in that The target single-frame point cloud P is realized by the nearest point iterative matching method. i With the template point cloud M i The registration between them is used to calculate the rotation and translation matrix between the current frame and the initial frame, including: Find the target single frame point cloud P i With the template point cloud M i The corresponding points between Assume the target single frame point cloud P i With the 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 is the noise vector, m i is the template point cloud, p i It is the target single frame point cloud; 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 ; Solve the nonlinear least squares problem and get the posture transformation matrix T t : Using the 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 the template point cloud M i The corresponding points between them are used to optimize R and t continuously and iteratively until the set termination condition is met.

8. The method according to claim 7, characterized in that The method of using an extended Kalman filter to model the target motion and obtain a filtered target motion state estimate includes: Establish the target motion model, and use the extended Kalman filter to locally linearize the nonlinear motion model and observation model through the first-order Taylor expansion; The target motion state observation value is input, and the target motion state is estimated through the prediction-correction mode combined with the target initial motion state.

9. The method according to claim 8, characterized in that The extracting of a single-frame target point cloud for constructing a template point cloud, removing inter-frame distortion, and completing the updating of the template point cloud includes: According to the estimated target motion state of the current frame, the bounding box position and orientation information are obtained, and the bounding box is enlarged and updated to the latest area of ​​interest; Extract the latest point cloud of the region of interest, transform the coordinates of the latest point cloud of the region of interest, align it with the template point cloud in the same coordinate system, and use them together as the template point cloud before optimization; Construct the 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.

10. The method according to claim 9, characterized in that The objective function is constructed, the template point cloud before optimization is input, the point cloud distribution is adjusted, the inter-frame distortion is removed, and the latest template point cloud is updated, including: Use rectangular grid area to divide the template point cloud before optimization; The objective function is constructed in each grid to optimize the point cloud variance within a single grid to remove inter-frame distortion, where the objective function J2 is: The point cloud frame newly added to the template point cloud has n points, r x and r y are the unit vectors in the X-axis and Y-axis directions of the local coordinates of the grid, p0 is the local origin of the grid, m x and m y are the mean vectors in the X and Y directions within the grid respectively; Find the pose transformation matrix T of the k-th frame point cloud that is added to the newly added template point cloud k * , so that the second objective function J2 is minimized: T k The optimal transformation matrix from the kth frame point cloud newly added to the template point cloud to the template point cloud; Divide each grid area and transform the newly added point cloud frame according to the solved optimal pose transformation matrix T k * Perform coordinate transformation and add the template point cloud in sequence to complete the template point cloud M i Updates.

Images