A method and system for motion estimation of moving objects based on optimized guidance

Abstract

The application discloses a kind of motion estimation method and system of motion target based on optimization guide, the method includes: obtaining the two-dimensional monocular image of motion target and carrying out geometric shape segmentation processing, obtain the geometric shape segmentation data of motion target;Three-dimensional sparse point cloud data of motion target are obtained and combined with the geometric shape segmentation data of motion target and carry out point cloud densification processing, obtain the three-dimensional dense point cloud data of motion target;Adaptive extended Kalman filter model is constructed to the motion estimation of three-dimensional dense point cloud data of motion target, obtain the motion state estimation of motion target.The application can improve the accuracy of motion target pose observation and the precision of filtering result.The application is a kind of motion estimation method and system of motion target based on optimization guide, can be widely applied in motion target estimation technical field.

Description

Technical Field

[0001] This invention relates to the field of moving target estimation technology, and in particular to a motion estimation method and system for moving targets based on optimization guidance. Background Technology

[0002] In robot-assisted industrial grasping tasks, both the robot and the general moving target, such as mechanical workpieces and drones, are in motion, which increases the difficulty of the task. Therefore, the estimation of the relative motion state of the general moving target is necessary and is a prerequisite for industrial grasping tasks. Vision systems, as non-contact measurement systems, are widely used for the perception of general moving targets. It is well known that cameras have feature information but lack scale information, while LiDAR has scale information but lacks feature information. Therefore, the fusion of vision and point cloud data can achieve information complementarity and is an effective solution. However, traditional pose observation and motion estimation frameworks have the following problems: first, they cannot segment the complete geometry of general moving targets under occlusion conditions; second, the features of sparse point clouds are not obvious, which easily leads to poor performance of pose observation algorithms; and third, nonlinear Kalman estimation does not model higher-order remainder terms, causing truncation errors in state transitions, thereby reducing filtering accuracy. Summary of the Invention

[0003] To address the aforementioned technical problems, the present invention aims to provide a motion estimation method and system for moving targets based on optimization guidance, which can improve the accuracy of moving target pose observation and the precision of filtering results.

[0004] The first technical solution adopted in this invention is: a motion estimation method for a moving target based on optimization guidance, comprising the following steps:

[0005] A two-dimensional monocular image of the moving target is acquired and geometric shape segmentation is performed to obtain the geometric shape segmentation data of the moving target;

[0006] The three-dimensional sparse point cloud data of the moving target is acquired and combined with the geometric shape segmentation data of the moving target to perform point cloud densification processing to obtain the three-dimensional dense point cloud data of the moving target.

[0007] An adaptive extended Kalman filter model is constructed to estimate the motion of the moving target using the 3D dense point cloud data, thereby obtaining an estimate of the moving target's motion state.

[0008] Furthermore, the step of acquiring a two-dimensional monocular image of the moving target and performing geometric shape segmentation processing to obtain the geometric shape segmentation data of the moving target specifically includes:

[0009] A two-dimensional monocular image of a moving target is acquired and moving target detection is performed. The bounding box of the moving target is obtained, and the bounding box represents the minimum detection rectangle of the moving target.

[0010] A geometric feature detection model of the moving target is constructed based on the bounding box of the moving target. The geometric feature detection model includes a backbone block, a neck block, and a prediction block.

[0011] The multi-constraint, multi-objective optimization problem of the moving target is determined based on the geometric feature detection model of the moving target;

[0012] Solve the multi-constraint, multi-objective optimization problem of the moving target to obtain the edge vertex features of the moving target;

[0013] Based on the principle of visual projection, the edge vertex features of the moving target are sorted, and the edge vertex features are connected end to end according to the sorting results to obtain the geometric shape segmentation data of the moving target.

[0014] Furthermore, the step of determining the multi-constraint, multi-objective optimization problem of the moving target based on the geometric feature detection model of the moving target specifically includes:

[0015] The geometric parameters of the bounding box of the moving target are predicted by the complete intersection and union ratio method, and the bounding box geometric parameter error function is constructed.

[0016] The category of the bounding box of the moving target is predicted by the binary cross-entropy method, and a bounding box category error function is constructed.

[0017] The vertices of the bounding box of the moving target are predicted by the directional kernel similarity method, and a bounding box vertex error function is constructed.

[0018] The confidence of the vertices of the bounding box of the moving target is predicted by the complete intersection and union ratio method and the binary cross-entropy method, and a bounding box vertex confidence error function is constructed.

[0019] By integrating the bounding box geometric parameter error function, the bounding box category error function, the bounding box vertex error function, and the bounding box vertex confidence error function, and performing vertex feature constraint processing, the multi-constraint multi-objective optimization problem of the moving target is obtained.

[0020] Furthermore, the specific expression for the multi-constraint, multi-objective optimization problem of the moving target is as follows:

[0021]

[0022]

[0023] In the above formula, L geo L represents the bounding box geometric parameter error function. cls L represents the bounding box category error function. vex L represents the bounding box vertex error function. cof Let [m1, m2, ..., m] represent the bounding box vertex confidence error function. k ] = M(I SNCT ,φ) represents the geometric feature detection model, w k h represents the width of the bounding box. k Let λ represent the height of the bounding box, k represent the k-th bounding box, q represent the q-th vertex, φ represent the parameters of the geometric feature detection model, and λ represent the height of the bounding box. geo , λ cls , λ vex and λ cof This represents the corresponding weighting coefficient. This represents the predicted x-coordinate of the q-th vertex. This represents the predicted value of the x-coordinate of the bounding box center. This represents the predicted value of the y-coordinate of the q-th vertex. This represents the predicted value of the ordinate of the bounding box center.

[0024] Furthermore, the step of acquiring the 3D sparse point cloud data of the moving target and combining it with the geometric shape segmentation data of the moving target to perform point cloud densification processing to obtain the 3D dense point cloud data of the moving target specifically includes:

[0025] The geometric segmentation data of the moving target is sampled using a curvature-guided strategy to obtain the two-dimensional location information of the missing points of the moving target.

[0026] Obtain the three-dimensional sparse point cloud data of the moving target;

[0027] Based on the three-dimensional sparse point cloud data of the moving target and the two-dimensional monocular image of the moving target, a projection model from point cloud data to monocular image is constructed.

[0028] The three-dimensional sparse point cloud data of the moving target is segmented according to the projection model from the point cloud data to the monocular image to obtain the segmented three-dimensional sparse point cloud data of the moving target.

[0029] Search processing is performed based on the two-dimensional location information of the missing point of the moving target to obtain the nearby point cloud data of the missing point of the moving target;

[0030] A convex quadratic programming problem with quadratic constraints is introduced to obtain a local shared plane formed by the neighboring point cloud data of the missing points of the moving target;

[0031] The missing point optimization process is performed on the local shared plane to obtain the three-dimensional dense point cloud data of the moving target.

[0032] Furthermore, the step of optimizing the missing points of the local shared plane to obtain the three-dimensional dense point cloud data of the moving target specifically includes:

[0033] Based on the constraints of the local shared plane, a visual and point cloud fusion model of the moving target is constructed;

[0034] Obtain the plane error and the distance error between the local shared plane corresponding to the missing point of the moving target and the local shared plane corresponding to the nearest point of the missing point of the moving target.

[0035] Based on the plane error and the local shared plane distance error, an optimization problem with a suppression term is constructed to minimize the local shared plane error;

[0036] By introducing the visual projection constraint as the objective function into the optimization problem of minimizing the local shared plane error with the suppression term, we obtain the optimization problem of minimizing the local shared plane error without constraints.

[0037] The optimization problem of minimizing the local shared plane error without constraints is solved to obtain the three-dimensional dense point cloud data of the moving target.

[0038] Furthermore, the step of constructing an adaptive extended Kalman filter model to estimate the motion of the moving target's three-dimensional dense point cloud data, thereby obtaining an estimate of the moving target's motion state, specifically includes:

[0039] The three-dimensional dense point cloud data of the moving target is processed by a point cloud matching algorithm to obtain the pose observation value of the moving target.

[0040] Based on the pose observation values ​​of the moving target, construct the state observation equation of the moving target and the nonlinear state transition equation of the moving target;

[0041] The state transition relationship between the prior estimate and the posterior estimate of the moving target is obtained based on the nonlinear state transition equation of the moving target.

[0042] Based on the state transition relationship between the prior estimate and the posterior estimate of the moving target, and through modeling and processing by a neural network, a truncated compensator is constructed.

[0043] Obtain the error of the prior estimate of the moving target, and determine the error covariance matrix of the prior estimate;

[0044] Obtain the error of the posterior estimate of the moving target, and determine the error covariance matrix of the posterior estimate;

[0045] Based on the state observation equation of the moving target, the prior estimated error covariance matrix and the posterior estimated error covariance matrix are corrected by matching method to construct an adaptive correction model;

[0046] By combining the truncation compensator and the adaptive correction model, an adaptive extended Kalman filter model is constructed.

[0047] Motion estimation is performed on the pose observations of the moving target based on the adaptive extended Kalman filter model to obtain the motion state estimate of the moving target.

[0048] Furthermore, the step of constructing an adaptive correction model by correcting the prior estimated error covariance matrix and the posterior estimated error covariance matrix based on the state observation equation of the moving target using a matching method specifically includes:

[0049] Obtain the covariance matrix of the innovation and the covariance matrix of the residual, wherein the innovation represents the state observation equation of the moving target minus the product of the state observation matrix and the prior estimate, and the residual represents the state observation equation of the moving target minus the product of the state observation matrix and the posterior estimate;

[0050] An adaptive correction model is constructed based on the covariance matrix of the new information and the covariance matrix of the residual. The adaptive correction model includes the position covariance matrix of the moving target and the velocity covariance matrix of the moving target.

[0051] Furthermore, the specific expression of the adaptive extended Kalman filter model is as follows:

[0052]

[0053]

[0054]

[0055]

[0056] In the above formula, F represents the covariance matrix of the prior estimate error. k Represents the state transition matrix. Q represents the transpose of the state transition matrix. k-1 K represents the covariance matrix of the process noise. k H represents the Kalman gain. k Represents the state observation matrix, R represents the transpose of the state observation matrix. k The covariance matrix representing the velocity of a moving target. This represents the posterior estimate of the state variables of the moving target. This represents the prior estimate of the state variables of the moving target. Let I represent the covariance matrix of the error of the posterior estimate, and let I represent the two-dimensional monocular image of the moving target.

[0057] The second technical solution adopted in this invention is: a motion estimation system for moving targets based on optimization guidance, comprising:

[0058] The geometric shape segmentation module is used to acquire a two-dimensional monocular image of a moving target and perform geometric shape segmentation processing to obtain the geometric shape segmentation data of the moving target;

[0059] The point cloud segmentation and densification module is used to acquire the three-dimensional sparse point cloud data of the moving target and combine it with the geometric shape segmentation data of the moving target to perform point cloud densification processing to obtain the three-dimensional dense point cloud data of the moving target.

[0060] The pose observation and motion estimation module is used to construct an adaptive extended Kalman filter model to perform motion estimation on the three-dimensional dense point cloud data of the moving target, thereby obtaining the motion state estimate of the moving target.

[0061] The beneficial effects of the method and system of this invention are as follows: This invention acquires a two-dimensional monocular image of a moving target and performs geometric shape segmentation processing to obtain geometric shape segmentation data of the moving target. By constraining the vertex features of the bounding box of the moving target, the accuracy and inference speed of geometric shape segmentation of general moving targets can be improved. Furthermore, the invention acquires three-dimensional sparse point cloud data of the moving target and performs point cloud densification processing in combination with the geometric shape segmentation data of the moving target. By improving the accuracy of point cloud registration, the three-dimensional geometric features of general moving targets can be enhanced, and the accuracy of pose observation can be improved. Finally, an adaptive extended Kalman filter model is constructed to perform motion estimation on the three-dimensional dense point cloud data of the moving target. By obtaining the pose observation value of the moving target for estimation, the accuracy of prior estimation of motion state and filtering results can be improved. Attached Figure Description

[0062] Figure 1 This is a flowchart illustrating the steps of a motion estimation method for a moving target based on optimization guidance according to an embodiment of the present invention.

[0063] Figure 2 This is a structural block diagram of a motion estimation system for a moving target based on optimization guidance according to an embodiment of the present invention;

[0064] Figure 3This is a schematic diagram of the structure of a geometric feature detection model for a general moving target according to a specific embodiment of the present invention;

[0065] Figure 4 This is a schematic diagram of the geometric shape segmentation of a general moving target according to a specific embodiment of the present invention;

[0066] Figure 5 This is a schematic diagram of point cloud densification based on vision and point cloud fusion in a specific embodiment of the present invention. Detailed Implementation

[0067] The present invention will now be described in further detail with reference to the accompanying drawings and specific embodiments. The step numbers in the following embodiments are only for ease of explanation and do not limit the order of the steps. The execution order of each step in the embodiments can be adapted according to the understanding of those skilled in the art.

[0068] Reference Figure 1 This invention provides a motion estimation method for a moving target based on optimization guidance, the method comprising the following steps:

[0069] S1. Acquire a two-dimensional monocular image of the moving target and perform geometric shape segmentation processing to obtain the geometric shape segmentation data of the moving target;

[0070] S11. Obtain a two-dimensional monocular image of the moving target and perform moving target detection. Obtain the bounding box of the moving target. The bounding box represents the smallest detection rectangle of the moving target.

[0071] S12. Construct a geometric feature detection model for the moving target based on the bounding box of the moving target. The geometric feature detection model includes a backbone block, a neck block, and a prediction block.

[0072] Specifically, suppose I represents the captured image with a resolution of w×h. It contains the visual projections of K general moving objects within the camera's field of view. The geometric feature detection model considers both bounding box detection and simple visual feature detection. For the bounding box of the k-th general moving object, s k :=[u k ,v k ] T w represents the pixel coordinates of its center. k and h k c represents width and height respectively. k p represents the confidence level. k :=[p k,1 ,p k,2 ,…,p k,C ] T(C represents the number of categories) denotes the category probability vector. Therefore, the bounding box prediction vector contains (5+C) elements. Taking vertex features as an example, assume that each general moving object in the image has Q vertices (other simple visual features include lines and circles, etc.). For the w-th vertex in the k-th bounding box, s k,q :=[u k,q ,v k,q ] T c represents pixel coordinates. k,Q This represents the confidence level. Therefore, the vertex prediction vector contains 3Q elements. Ultimately, the prediction vector for each image contains a total of (5+C+3Q) elements, expressed as:

[0073]

[0074] In the above formula, m k This represents an element in a geometric feature detection model. w is the transpose matrix representing the pixel coordinates of the bounding box center of a general moving target. k h represents the width of the bounding box of a general moving target. k c represents the height of the bounding box of a general moving target. k This represents the confidence level of the bounding box vertices. The device matrix representing the class probability vector. c is the transpose matrix of the pixel coordinates of the first vertex in the k-th bounding box. k,1 Let represent the confidence level of the first vertex in the k-th bounding box. c represents the transpose of the pixel coordinates of the Q-th vertex in the k-th bounding box. k,Q This represents the confidence level of the Q-th vertex in the k-th bounding box.

[0075] like Figure 3 As shown, the geometric feature detection model can be represented as:

[0076] [m1,m2,…,m k ]:=M(I,Φ)

[0077] In the above formula, Φ represents the parameters of the geometric feature detection model, and I represents the two-dimensional monocular image of the moving target.

[0078] S13. Determine the multi-constraint, multi-objective optimization problem of the moving target based on the geometric feature detection model of the moving target;

[0079] Specifically, the geometric parameters of the bounding box of the moving target are predicted using the complete intersection and union ratio method, constructing a bounding box geometric parameter error function; the category of the bounding box of the moving target is predicted using the binary cross-entropy method, constructing a bounding box category error function; the vertices of the bounding box of the moving target are predicted using the directional kernel similarity method, constructing a bounding box vertex error function; the confidence of the vertices of the bounding box of the moving target is predicted using the complete intersection and union ratio method and the binary cross-entropy method, constructing a bounding box vertex confidence error function; the bounding box geometric parameter error function, bounding box category error function, bounding box vertex error function, and bounding box vertex confidence error function are integrated and vertex feature constraint processing is applied to obtain a multi-constraint multi-objective optimization problem for the moving target.

[0080] In this embodiment, x and These represent the actual value and the predicted value of the variable, respectively.

[0081] Furthermore, regarding the bounding box geometric parameters g k :=[u k ,v k ,w k ,h k ] T The prediction is based on the CIoU (Complete Intersection Union Ratio) error function, which is expressed as follows:

[0082]

[0083] In the above formula, G represents k and CIoU, g k This represents the true values ​​of the bounding box's geometric parameters. L represents the predicted value of the bounding box geometry parameters. geo Let K represent the error function of the bounding box geometry parameters, K represent the bounding boxes of K general moving targets, and k represent the kth bounding box.

[0084] For bounding box category prediction, the error function is established using BCE (Binary Cross Entropy), and its expression is:

[0085]

[0086] In the above formula, L cls p represents the error function for predicting bounding box categories. k,i This represents the actual value of the bounding box category. This represents the predicted value for the bounding box category, where C represents the number of categories.

[0087] The expression for binary cross-entropy is:

[0088] f BCE (x,y):=-[x log(y)+(1-x)log(1-y)]

[0089] In the above formula, f BCE (·) represents the binary cross-entropy function.

[0090] For vertex prediction, the error function is established using OKS (Oriented Kernel Similarity):

[0091]

[0092]

[0093] In the above formula, δ k,q σ represents a flag indicating a vertex that can be inferred. q L represents the normalization factor of the q-th vertex. vex The error function representing the prediction of the vertex, s k,q This represents the actual vertex value of the bounding box. f represents the vertex observations of the bounding box. OKS,k This represents the directional kernel similarity.

[0094] For the prediction confidence of bounding box vertices, the error function is established using CIoU and BCE:

[0095]

[0096] In the above formula, L cof The error function represents the prediction confidence of the bounding box vertices.

[0097] To ensure that vertex features are within the bounding box, constraints on the vertex features are also required. Therefore, the multi-constraint, multi-objective optimization problem of geometric feature detection can be expressed as:

[0098]

[0099]

[0100] In the above formula, L geo L represents the bounding box geometric parameter error function. cls L represents the bounding box category error function. vex L represents the bounding box vertex error function. cof Let [m1, m2, ..., m] represent the bounding box vertex confidence error function. k ] = M(I SNCT ,φ) represents the geometric feature detection model, w k h represents the width of the bounding box. kLet λ represent the height of the bounding box, k represent the k-th bounding box, q represent the q-th vertex, φ represent the parameters of the geometric feature detection model, and λ represent the height of the bounding box. geo , λ cls , λ vex and λ cof This represents the corresponding weighting coefficient. This represents the predicted x-coordinate of the q-th vertex. This represents the predicted value of the x-coordinate of the bounding box center. This represents the predicted value of the y-coordinate of the q-th vertex. This represents the predicted value of the ordinate of the bounding box center.

[0101] S14. Solve the multi-constraint multi-objective optimization problem of the moving target to obtain the edge vertex features of the moving target;

[0102] S15. Based on the principle of visual projection, the edge vertex features of the moving target are sorted, and the edge vertex features are connected end to end according to the sorting results to obtain the geometric shape segmentation data of the moving target.

[0103] Specifically, such as Figure 4 As shown, obtain all vertices of the k-th general moving target I, where s k,1 The central vertex is represented. To segment the geometry, all edge vertices need to be ordered. According to the principle of visual projection, any moving target of a convex polyhedron shape projected onto the visual plane will always be a convex polygon. Therefore, all edge vertices are distributed in order as shown by s. k,1 In the polar coordinate system with pole s k,q Angle α relative to the polar axis k,q It can be represented as:

[0104]

[0105] In the above formula, α k,q Δv represents the angles of all vertices of a moving target relative to the polar axis. k,q Let Δu represent the difference in the ordinate between the q-th vertex and the center vertex. k,q This represents the difference in x-coordinate between the q-th vertex and the center vertex.

[0106] The expression for all vertices of a general moving target is:

[0107] Δs k,q :=s k,q -s k,1 =[Δu k,q ,Δv k,q ] T

[0108] In the above formula, Δsk,q This represents the difference between the other vertices of a moving target and the central vertex.

[0109] like Figure 4 As shown, connecting all edge vertices end to end allows for the segmentation and acquisition of the geometry of a general moving target.

[0110] S2. Obtain the 3D sparse point cloud data of the moving target and combine it with the geometric shape segmentation data of the moving target to perform point cloud densification processing to obtain the 3D dense point cloud data of the moving target.

[0111] S21. The geometric shape segmentation data of the moving target is sampled using a curvature-guided strategy to obtain the two-dimensional position information of the missing points of the moving target.

[0112] Specifically, the camera and LiDAR have different orders of magnitude resolution. Therefore, reconstructing all depth-missing points on the image plane is neither practical nor necessary. In this case, it is possible to sample only N. den The sampling probability is set to a certain number of points, and it is stipulated that the sampling probability in regions with obvious features should be greater than that in other regions. Curvature is an important feature of point clouds and can be used to establish constraints for point cloud registration, thereby improving the accuracy of relative pose estimation. Generally, the curvature of a moving target at its vertices, at the boundary between two surfaces, and in other regions is c0, respectively. ver c lin and c oth (c ver >c lin >c oth The sampling probabilities are p. ver p lin and p oth Therefore, p ver >p lin >p oth .

[0113] S22. Obtain the three-dimensional sparse point cloud data of the moving target;

[0114] S23. Based on the 3D sparse point cloud data of the moving target and the 2D monocular image of the moving target, construct a projection model from the point cloud data to the monocular image.

[0115] S24. Based on the projection model from point cloud data to monocular image, the three-dimensional sparse point cloud data of the moving target is segmented to obtain the segmented three-dimensional sparse point cloud data of the moving target.

[0116] Specifically, let {cam}, {lidar}, and {img} represent the camera coordinate system, radar coordinate system, and image coordinate system, respectively. cam R lidar and cam t lidarLet {lidar} and {cam} represent the rotation matrix and translation vector, respectively, relative to {cam}. lidar q、 cam Let q and s represent the coordinates relative to {lidar}, {cam}, and {img}, respectively. Therefore, the projection model from the point cloud to the image can be represented as:

[0117] cam q = cam R lidar lidar q+ cam t lidar

[0118] s = F cam P( cam q)+s0

[0119] In the above formula, cam q represents the coordinates of a typical moving target in the camera coordinate system. cam R lidar This represents the rotation matrix of a typical moving target in the radar coordinate system. lidar q represents the coordinates of a typical moving target in the radar coordinate system. cam t lidar Let F represent the translation vector of a general moving target in the camera coordinate system, s represent the coordinates of the general moving target in the image coordinate system, and F represent the translation vector of the target in the camera coordinate system. cam Let represent the camera's focal length matrix, P represent the point cloud of a general moving target, and s0 represent the camera's optical axis offset vector.

[0120] Among them, F cam := diag(f u ,f v ), s0:=[u0,v0] T , (f u ,f v (u0, v0) represents the camera's intrinsic parameters. This represents the projection function.

[0121] Extract the point cloud P of a general moving target, and the mapping result of the extracted point cloud is as follows: Figure 5 As shown, the point cloud is sparse and lacks distinct features. This reduces the accuracy of point cloud registration, thus affecting the effectiveness of relative pose observation and motion state estimation. Therefore, depth completion is needed for points lacking depth.

[0122] S25. Based on the two-dimensional location information of the missing points of the moving target, perform search processing to obtain the neighboring point cloud data of the missing points of the moving target;

[0123] S26. Introduce a convex quadratic programming problem with quadratic constraints to obtain a local shared plane formed by the nearest point cloud data of the missing points of the moving target;

[0124] Specifically, considering the complementary advantages of camera edge features and radar scale information, this invention proposes a general moving target point cloud densification algorithm based on the fusion of vision and point cloud. For example... Figure 5 As shown, it assumes that the depth of the missing point is related to its neighboring N ner The known point clouds belong to the same local plane. Therefore, by combining the local shared plane constraint and the visual projection constraint, their 3D coordinates are reconstructed using optimization methods.

[0125] Assumption This represents the pixel coordinates of the j-th depth-defect point relative to {img}. Let Q represent the Cartesian coordinates of the point relative to {cam}. j express N ner A point cloud composed of neighboring points cam q j :=[ cam x j,n , cam y j,n , cam z j,n ] T Let s represent the Cartesian coordinates of the nth nearest neighbor point relative to {cam}. j,n :=[u j,n ,v j,n ] T This represents the pixel coordinates of the point relative to {img}. and Q j The parameters of the locally shared plane can be obtained by solving a convex quadratic programming problem with quadratic constraints, namely:

[0126]

[0127]

[0128] In the above formula, and Let N and Y represent the normal and bias of the locally shared plane, respectively. The prerequisite for the above convex quadratic programming problem with quadratic constraints to have a solution is that N... ner ≥3.

[0129] S27. Perform missing point optimization on the local shared plane to obtain the three-dimensional dense point cloud data of the moving target.

[0130] Specifically, based on the constraints of the local shared plane, a visual and point cloud fusion model of the moving target is constructed; the plane error and distance error between the local shared plane corresponding to the missing point of the moving target and the local shared plane corresponding to the neighboring point of the missing point are obtained; based on the plane error and the distance error of the local shared plane, an optimization problem of minimizing the local shared plane error with a suppression term is constructed; the visual projection constraint is introduced as the objective function into the optimization problem of minimizing the local shared plane error with a suppression term, resulting in an unconstrained optimization problem of minimizing the local shared plane error; the unconstrained optimization problem of minimizing the local shared plane error is solved to obtain the 3D dense point cloud data of the moving target.

[0131] In this embodiment, The back projection model can be expressed as:

[0132]

[0133] in, This represents the back projection function.

[0134] because and cam q j,n Given the existence of a locally shared plane constraint, the vision and point cloud fusion model of a general moving target can be represented as:

[0135]

[0136] The back projection model can provide two information about The constraints are such that each neighboring point provides a locally shared plane constraint. Therefore, a minimum of three neighboring points are needed to achieve this. Reconstruction will inevitably reveal redundant information. To fully utilize the effective information from all neighboring points, we can... The deep recovery model was optimized. and cam q j,n The local shared plane error between them can be expressed as:

[0137]

[0138] In addition, to suppress outlier phenomena at reconstructed points, a local shared plane distance error is introduced, the expression of which is:

[0139]

[0140] Under the premise of satisfying the visual projection constraints, we establish an optimization problem with a suppression term to minimize the local shared plane error, the expression of which is:

[0141]

[0142]

[0143] Introducing visual projection constraints into the objective function, the original optimization problem of minimizing the local shared plane error with suppression terms is equivalent to an unconstrained optimization problem, expressed as:

[0144]

[0145]

[0146] in,

[0147] The optimal depth of the depth missing point is obtained by solving the problem. Furthermore, its three-dimensional coordinates can be reconstructed based on the back-projection model.

[0148] S3. Construct an adaptive extended Kalman filter model to estimate the motion of the moving target using the 3D dense point cloud data, thereby obtaining the motion state estimate of the moving target.

[0149] S31. Process the three-dimensional dense point cloud data of the moving target using a point cloud matching algorithm to obtain the pose observation value of the moving target.

[0150] S32. Based on the pose observations of the moving target, construct the state observation equation and the nonlinear state transition equation of the moving target.

[0151] S33. Obtain the state transition relationship between the prior estimate and the posterior estimate of the moving target based on the nonlinear state transition equation of the moving target.

[0152] S34. Based on the state transition relationship between the prior estimate and the posterior estimate of the moving target, and through neural network modeling, a truncated compensator is constructed.

[0153] Specifically, assuming ref q k , ref υ k , ref θ k and ref ω k These represent the position, velocity, ZYX Euler angles, and angular velocity of the NCS (nonlinear Kalman filter) relative to the same reference coordinate system, respectively. Let represent the state variables of the NCS. Therefore, the nonlinear state transition equation for a general moving target can be expressed as:

[0154] ref x k =f(ref x k-1 )+w k-1

[0155] Where, f( ref x k-1 ) represents the state transition function, w k-1 ~N(0,Q) k-1 ) represents process noise. Indicates w k-1 The covariance matrix.

[0156] The state of a moving target is generally not fully observed. The pose observations are obtained using a point cloud matching algorithm. Therefore, the state observation equation for a general moving target can be expressed as:

[0157] ref z k =H k ref x k +v k

[0158]

[0159] Among them, H k Represents the state observation matrix, v k ~N(0,R k () indicates measurement noise. Indicates v k The covariance matrix.

[0160] Furthermore, a truncated compensator for the state transition equation is constructed based on a neural network, and the parameters of the model are updated online.

[0161] Where, assuming and Let A and B represent the prior and posterior estimates of the state variables, respectively. The state transition relationship between them can be expressed as:

[0162]

[0163] in, Represents the state transition matrix. It includes a Taylor approximation truncation term and other external disturbances. The truncation compensator can be modeled using a neural network:

[0164]

[0165] Where i = 1, 2, ..., N EC N EC This indicates the number of neurons in the hidden layer;

[0166] in:

[0167]

[0168] Assumption This represents the error of the prior estimate, which follows a Gaussian distribution. express The covariance matrix, This represents the error of the posterior estimate, which follows a Gaussian distribution. express The covariance matrix. The parameter update model of the neural network can be represented as:

[0169]

[0170] S35. Obtain the error of the prior estimate of the moving target and determine the error covariance matrix of the prior estimate;

[0171] S36. Obtain the error of the posterior estimate of the moving target and determine the error covariance matrix of the posterior estimate;

[0172] S37. Based on the state observation equation of the moving target, the error covariance matrix estimated in the prior period and the error covariance matrix estimated in the posterior period are corrected by matching method to construct an adaptive correction model.

[0173] Specifically, the covariance matrix of the innovation and the covariance matrix of the residual are obtained. The innovation represents the state observation equation of the moving target minus the product of the state observation matrix and the prior estimate, and the residual represents the state observation equation of the moving target minus the product of the state observation matrix and the posterior estimate. Based on the covariance matrix of the innovation and the covariance matrix of the residual, an adaptive correction model is constructed. The adaptive correction model includes the position covariance matrix and the velocity covariance matrix of the moving target.

[0174] S38. Combining the truncation compensator and the adaptive correction model, construct an adaptive extended Kalman filter model;

[0175] Specifically, assuming This represents the information, which follows a Gaussian distribution. express The covariance matrix, that is:

[0176]

[0177] Assumption Let K represent the residual, where K is the residual. k It is the Kalman gain. Therefore, It can be represented as:

[0178]

[0179] Assume M represents the size of the sliding window. and They can be approximated as follows:

[0180]

[0181]

[0182] The covariance matrix R of the velocity of a typical moving target k The adaptive correction model can be expressed as:

[0183]

[0184] The covariance matrix Q of process noise k-1 The adaptive correction model can be expressed as:

[0185]

[0186] S39. Based on the adaptive extended Kalman filter model, motion estimation is performed on the pose observations of the moving target to obtain the motion state estimate of the moving target.

[0187] Specifically, the adaptive extended Kalman filter equation based on truncation compensation can be expressed as:

[0188]

[0189]

[0190]

[0191]

[0192] In the above formula, F represents the covariance matrix of the prior estimate error. k Represents the state transition matrix. Q represents the transpose of the state transition matrix. k-1 K represents the covariance matrix of the process noise. k H represents the Kalman gain. k Represents the state observation matrix, R represents the transpose of the state observation matrix. k The covariance matrix representing the velocity of a moving target. This represents the posterior estimate of the state variables of the moving target. This represents the prior estimate of the state variables of the moving target. Let I represent the covariance matrix of the error of the posterior estimate, and let I represent the two-dimensional monocular image of the moving target.

[0193] Reference Figure 2 A motion estimation system for moving targets based on optimization guidance, comprising:

[0194] The geometric shape segmentation module is used to acquire a two-dimensional monocular image of a moving target and perform geometric shape segmentation processing to obtain the geometric shape segmentation data of the moving target;

[0195] The point cloud segmentation and densification module is used to acquire 3D sparse point cloud data of moving targets and combine it with the geometric shape segmentation data of moving targets to perform point cloud densification processing to obtain 3D dense point cloud data of moving targets.

[0196] The pose observation and motion estimation module is used to construct an adaptive extended Kalman filter model to estimate the motion of a moving target from 3D dense point cloud data, thereby obtaining an estimate of the moving target's motion state.

[0197] The content of the above method embodiments is applicable to this system embodiment. The specific functions implemented in this system embodiment are the same as those in the above method embodiments, and the beneficial effects achieved are also the same as those achieved in the above method embodiments.

[0198] The above is a detailed description of the preferred embodiments of the present invention. However, the present invention is not limited to the embodiments described. Those skilled in the art can make various equivalent modifications or substitutions without departing from the spirit of the present invention. All such equivalent modifications or substitutions are included within the scope defined by the claims of this application.

Claims

1. A motion estimation method for a moving target based on optimization guidance, characterized in that, Includes the following steps: A two-dimensional monocular image of the moving target is acquired and geometric shape segmentation is performed to obtain the geometric shape segmentation data of the moving target; The three-dimensional sparse point cloud data of the moving target is acquired and combined with the geometric shape segmentation data of the moving target to perform point cloud densification processing to obtain the three-dimensional dense point cloud data of the moving target. An adaptive extended Kalman filter model is constructed to estimate the motion of the moving target using the 3D dense point cloud data, thereby obtaining an estimate of the moving target's motion state. The step of constructing an adaptive extended Kalman filter model to estimate the motion state of the moving target from the 3D dense point cloud data specifically includes: The three-dimensional dense point cloud data of the moving target is processed by a point cloud matching algorithm to obtain the pose observation value of the moving target. Based on the pose observation values ​​of the moving target, construct the state observation equation of the moving target and the nonlinear state transition equation of the moving target; The state transition relationship between the prior estimate and the posterior estimate of the moving target is obtained based on the nonlinear state transition equation of the moving target. Based on the state transition relationship between the prior estimate and the posterior estimate of the moving target, and through modeling and processing by a neural network, a truncated compensator is constructed. Obtain the error of the prior estimate of the moving target, and determine the error covariance matrix of the prior estimate; Obtain the error of the posterior estimate of the moving target, and determine the error covariance matrix of the posterior estimate; Based on the state observation equation of the moving target, the prior estimated error covariance matrix and the posterior estimated error covariance matrix are corrected by matching method to construct an adaptive correction model; By combining the truncation compensator and the adaptive correction model, an adaptive extended Kalman filter model is constructed. Based on the adaptive extended Kalman filter model, motion estimation is performed on the pose observations of the moving target to obtain the motion state estimate of the moving target; The specific expression for the adaptive extended Kalman filter model is as follows: In the above formula, The covariance matrix represents the error of the prior estimate. Represents the state transition matrix. The transpose of the state transition matrix. The covariance matrix representing the process noise. Indicates Kalman gain, Represents the state observation matrix, The transpose of the state observation matrix. The covariance matrix representing the velocity of a moving target. This represents the posterior estimate of the state variables of the moving target. This represents the prior estimate of the state variables of the moving target. The covariance matrix represents the error of the posterior estimate. A two-dimensional monocular image representing a moving target.

2. The motion estimation method for a moving target based on optimization guidance according to claim 1, characterized in that, The step of acquiring a two-dimensional monocular image of the moving target and performing geometric shape segmentation processing to obtain the geometric shape segmentation data of the moving target specifically includes: A two-dimensional monocular image of a moving target is acquired and moving target detection is performed. The bounding box of the moving target is obtained, and the bounding box represents the minimum detection rectangle of the moving target. A geometric feature detection model of the moving target is constructed based on the bounding box of the moving target. The geometric feature detection model includes a backbone block, a neck block, and a prediction block. The multi-constraint, multi-objective optimization problem of the moving target is determined based on the geometric feature detection model of the moving target; Solve the multi-constraint, multi-objective optimization problem of the moving target to obtain the edge vertex features of the moving target; Based on the principle of visual projection, the edge vertex features of the moving target are sorted, and the edge vertex features are connected end to end according to the sorting results to obtain the geometric shape segmentation data of the moving target.

3. The motion estimation method for a moving target based on optimization guidance according to claim 2, characterized in that, The step of determining the multi-constraint, multi-objective optimization problem of the moving target based on the geometric feature detection model of the moving target specifically includes: The geometric parameters of the bounding box of the moving target are predicted by the complete intersection and union ratio method, and the bounding box geometric parameter error function is constructed. The category of the bounding box of the moving target is predicted by the binary cross-entropy method, and a bounding box category error function is constructed. The vertices of the bounding box of the moving target are predicted by the directional kernel similarity method, and a bounding box vertex error function is constructed. The confidence of the vertices of the bounding box of the moving target is predicted by the complete intersection and union ratio method and the binary cross-entropy method, and a bounding box vertex confidence error function is constructed. By integrating the bounding box geometric parameter error function, the bounding box category error function, the bounding box vertex error function, and the bounding box vertex confidence error function, and performing vertex feature constraint processing, the multi-constraint multi-objective optimization problem of the moving target is obtained.

4. The motion estimation method for a moving target based on optimization guidance according to claim 3, characterized in that, The specific expression for the multi-constraint, multi-objective optimization problem of the moving target is as follows: In the above formula, This represents the bounding box geometric parameter error function. This represents the bounding box category error function. This represents the bounding box vertex error function. This represents the bounding box vertex confidence error function. This represents a geometric feature detection model. Indicates the width of the bounding box. Indicates the height of the enclosure. Indicates the first A surrounding box, Indicates the first One vertex, The parameters represent the geometric feature detection model. , , and This represents the corresponding weighting coefficient. Indicates the first The predicted value of the x-coordinate of each vertex. This represents the predicted value of the x-coordinate of the bounding box center. Indicates the first The predicted value of the y-coordinate of each vertex. This represents the predicted value of the ordinate of the bounding box center.

5. The motion estimation method for a moving target based on optimization guidance according to claim 1, characterized in that, The step of acquiring the 3D sparse point cloud data of the moving target and performing point cloud densification processing in conjunction with the geometric shape segmentation data of the moving target to obtain the 3D dense point cloud data of the moving target specifically includes: The geometric segmentation data of the moving target is sampled using a curvature-guided strategy to obtain the two-dimensional location information of the missing points of the moving target. Obtain the three-dimensional sparse point cloud data of the moving target; Based on the three-dimensional sparse point cloud data of the moving target and the two-dimensional monocular image of the moving target, a projection model from point cloud data to monocular image is constructed. The three-dimensional sparse point cloud data of the moving target is segmented according to the projection model from the point cloud data to the monocular image to obtain the segmented three-dimensional sparse point cloud data of the moving target. Search processing is performed based on the two-dimensional location information of the missing point of the moving target to obtain the nearby point cloud data of the missing point of the moving target; A convex quadratic programming problem with quadratic constraints is introduced to obtain a local shared plane formed by the neighboring point cloud data of the missing points of the moving target; The missing point optimization process is performed on the local shared plane to obtain the three-dimensional dense point cloud data of the moving target.

6. The motion estimation method for a moving target based on optimization guidance according to claim 5, characterized in that, The step of optimizing the missing points of the local shared plane to obtain the three-dimensional dense point cloud data of the moving target specifically includes: Based on the constraints of the local shared plane, a visual and point cloud fusion model of the moving target is constructed; Obtain the plane error and the distance error between the local shared plane corresponding to the missing point of the moving target and the local shared plane corresponding to the nearest point of the missing point of the moving target. Based on the plane error and the local shared plane distance error, an optimization problem with a suppression term is constructed to minimize the local shared plane error; By introducing the visual projection constraint as the objective function into the optimization problem of minimizing the local shared plane error with the suppression term, we obtain the optimization problem of minimizing the local shared plane error without constraints. The optimization problem of minimizing the local shared plane error without constraints is solved to obtain the three-dimensional dense point cloud data of the moving target.

7. The motion estimation method for a moving target based on optimization guidance according to claim 1, characterized in that, The step of constructing an adaptive correction model by correcting the prior estimated error covariance matrix and the posterior estimated error covariance matrix based on the state observation equation of the moving target through a matching method specifically includes: Obtain the covariance matrix of the innovation and the covariance matrix of the residual, wherein the innovation represents the state observation equation of the moving target minus the product of the state observation matrix and the prior estimate, and the residual represents the state observation equation of the moving target minus the product of the state observation matrix and the posterior estimate; An adaptive correction model is constructed based on the covariance matrix of the new information and the covariance matrix of the residual. The adaptive correction model includes the position covariance matrix of the moving target and the velocity covariance matrix of the moving target.

8. A system for implementing the motion estimation method for a moving target based on optimization guidance as described in any one of claims 1-7, characterized in that, Includes the following modules: The geometric shape segmentation module is used to acquire a two-dimensional monocular image of a moving target and perform geometric shape segmentation processing to obtain the geometric shape segmentation data of the moving target; The point cloud segmentation and densification module is used to acquire the three-dimensional sparse point cloud data of the moving target and combine it with the geometric shape segmentation data of the moving target to perform point cloud densification processing to obtain the three-dimensional dense point cloud data of the moving target. The pose observation and motion estimation module is used to construct an adaptive extended Kalman filter model to perform motion estimation on the three-dimensional dense point cloud data of the moving target, thereby obtaining the motion state estimate of the moving target.

Images