A high-precision motion estimation method for spatial unstable targets based on continuous point cloud sequences

Abstract

The application discloses a high-precision motion estimation method for a spatial unstable target based on a continuous point cloud sequence, and comprises the following steps: a voxel grid neighborhood method is used to perform data down-sampling on a point cloud sequence of an input motion spatial unstable target; initial motion parameters of spinning and precession of the spatial unstable target are obtained in a hierarchical manner through a motion model of the spatial unstable target; a statistical point cloud registration method is used to iteratively converge the initial motion parameters of the spinning and precession, so as to obtain final motion parameters of the spatial unstable target; under an array measurement system (with intra-frame motion difference), the motion model (transmission model) of the spatial unstable target is used to obtain high-precision motion estimation results of the spinning and precession, and simulation experiment numerical verification is conducted to verify the effectiveness and robustness of the application on the three-dimensional topography and motion state of the spatial target.

Description

Technical Field

[0001] This invention belongs to the field of fully automated products and relates to a high-precision motion estimation method for spatially unstable targets based on continuous point cloud sequences. Background Technology

[0002] Pose measurement and motion estimation of non-cooperative targets in space have been a focus of scientific research for decades. Researchers have used measurement devices (image sensors) [1-3] LiDAR 4,5] and multi-source sensors [6] Updates and gradual improvement of motion models [7-10] Efficient solution of motion parameters [11-13] To improve the efficiency of motion estimation. However, due to the lack of prior information, motion estimation for diverse spatial non-cooperative targets of various shapes remains a challenging research task.

[14] Based on published literature, motion estimation of space targets under non-cooperative conditions can be mainly divided into two categories: one is motion estimation method based on feature point trajectories, and the other is motion estimation method based on the overall target model.

[0003] Motion estimation based on feature point trajectories was the mainstream method for motion estimation of non-cooperative targets in early space applications. Since most spacecraft have typical geometric components (such as triangular supports, rectangular antenna frames, or circular engine nozzles), researchers typically track feature points on these components and estimate motion parameters based on their trajectories. For triangular structures mounted on non-cooperative spacecraft, Song et al.

[15] A method for estimating the motion of non-cooperative space targets during tracking, docking, and rendezvous is proposed using a monocular vision measurement system. Researchers specifically address rectangular features mounted on spacecraft. [16,17] The relative poses of non-cooperative targets in space were estimated based on structured light vision and stereo vision, respectively. For circular and near-circular features on spatial targets, Peng et al.

[18] A measurement scheme with high computational accuracy and high operating efficiency based on a stereo vision measurement system was proposed by Sabatini et al.

[19] A relative pose measurement method for autonomous navigation is proposed. In a linear measurement system, targeting the corner features on solar panels, Sun et al. [8] A motion estimation method is proposed for estimating the spin and precession parameters of a spatially unstable target. This method is further improved by introducing self-constrained motion models and particle swarm optimization. [9] A motion estimation method was developed that requires a maximum of 11 consecutive point cloud frames to obtain high-precision motion parameters. Recently, Sun et al.

[10] By establishing a new motion model (transmission model), the nonlinear solution of motion parameters is transformed into a polynomial structure solution, further improving the efficiency of motion estimation. Motion estimation methods based on feature point trajectories can efficiently obtain motion parameters within milliseconds, but the accuracy of motion estimation often depends on the completeness and accuracy of the feature point trajectory description. In the case of incomplete or inaccurate feature point trajectories, the error in motion estimation will inevitably be very large. Furthermore, feature point trajectory-based methods typically lack generalization ability for the 3D shape of spatial targets; different feature point extraction techniques are only applicable to spatial targets with specific shapes, exhibiting significant limitations for other shapes.

[0004] With the increasing ease of acquiring point cloud data of target models, motion estimation methods based on the overall 3D model of non-cooperative spatial targets have gradually become the mainstream research method in motion estimation. In 2006, Terui et al.

[20] Using the classic ICP method

[21] Registration was performed on point cloud sequences acquired at equal time intervals to obtain motion parameters of non-cooperative targets in space. To reduce the interference of measurement noise, Aghili and Parsa

[11] combined ICP with Kalman filtering to improve the robustness of motion estimation for unstable targets in space. Ma et al. [7] Quaternion differential equations for spatially unstable targets were established using the quaternion method, thereby obtaining analytical solutions to the dynamic model. The convergence and accuracy of the method were numerically verified. Meanwhile, Pesce et al.

[12] Estimation of motion parameters for non-cooperative targets in space is presented using both the classical Extended Kalman Filter (EKF) and the Iterative Extended Kalman Filter (IEKF). Recently, Xu et al.

[13] A motion estimation method based on a dual registration matrix is ​​proposed, which estimates the spin parameters of non-cooperative targets in space by performing two registrations on a point cloud sequence. In a linear measurement system, Li et al.

[22] Even ignoring intra-frame motion differences, motion estimation results for the spin parameters of spatially unstable targets are provided based on the classic ICP method. However, since spatially unstable targets exhibit a composite motion of spin, precession, and nutation, although the measurement time is short and nutation can be ignored, for cases where precession is significant, considering only the spin motion of the spatially unstable target is clearly insufficient to obtain an accurate motion model.

[0005] Space-instable targets exist in a state of combined spin, precession, and nutation. Due to the short measurement time, nutation can be neglected. Laser linear array imaging radar, with its advantages of high imaging resolution and large measurement field of view, is an ideal tool for the precise measurement of large space targets. However, several problems remain to be solved in motion estimation of space-instable targets with complex motion states under linear measurement systems:

[0006] 1. Although motion estimation methods based on feature point trajectories can efficiently perform high-precision motion estimation for spatially unstable targets, they have limitations for spatially unstable targets where feature point trajectories are not fully extracted or where specified feature points are not available.

[0007] 2. Although existing motion estimation methods based on point cloud sequences have overcome the limitations in Problem 1, they only estimate the spin motion parameters of spatially unstable targets. For cases where precession is not negligible, the motion estimation error is large.

[0008] 3. In the acquired point cloud sequence, there are shape differences between neighboring point clouds due to self-occlusion. Furthermore, there are unavoidable intra-frame motion differences in linear measurement systems. Overcoming these differences to estimate the motion of spatially unstable targets is the third problem this invention aims to solve. Summary of the Invention

[0009] To address the following problems existing in the prior art:

[0010] 1. Although motion estimation methods based on feature point trajectories can efficiently perform high-precision motion estimation for spatially unstable targets, they have limitations for spatially unstable targets where feature point trajectories are not fully extracted or where specified feature points are not available.

[0011] 2. Although existing motion estimation methods based on point cloud sequences have overcome the limitations in Problem 1, they only estimate the spin motion parameters of spatially unstable targets. For cases where precession is not negligible, the motion estimation error is large.

[0012] 3. In the acquired point cloud sequence, there are shape differences between neighboring point clouds due to self-occlusion. Furthermore, there are unavoidable intra-frame motion differences in linear measurement systems. Overcoming these differences to estimate the motion of spatially unstable targets is the third problem this invention aims to solve.

[0013] The technical solution adopted in this invention is:

[0014] A high-precision motion estimation method for spatially unstable targets based on continuous point cloud sequences includes the following steps:

[0015] The voxel grid neighborhood method was used to downsample the point cloud sequence of the input moving spatially unstable target;

[0016] By using the motion model of the spatially unstable target, the initial motion parameters of the spin and precession of the spatially unstable target are obtained in a hierarchical manner.

[0017] The statistical point cloud registration method iteratively converges the initial motion parameters of spin and precession to obtain the final motion parameters of the spatially unstable target.

[0018] Furthermore, the step of obtaining the initial spin and precession motion parameters of a spatially unstable target hierarchically through its motion model includes the following steps:

[0019] The precession parameters of the spatially unstable target are initially estimated using the motion model of the target.

[0020] The precession correction is performed on the acquired continuous point cloud sequence based on the initial estimated parameters of precession, so that the point cloud sequence contains only spin motion;

[0021] First, the difference rate of the number of point clouds in adjacent frames is used to determine whether the group of point clouds meets the registration requirements. Then, all input point cloud sequences are traversed, and the orthogonal rotation matrix sequence and translation vector sequence between adjacent frame point cloud data that meet the registration requirements are estimated by the point cloud registration method. Then, the trust region method is used to make an initial estimate of the spin motion parameters based on the estimated rotation matrix sequence and translation vector sequence.

[0022] Furthermore, the initial estimation process of the precession parameters of a spatially unstable target using its motion model is as follows:

[0023] Based on the motion model of a spatially unstable target, for a given feature point P i and the spatial position P obtained at equal time intervals Δt i+1 P i+2 , ..., P i+n It can be passed through the corresponding orthogonal rotation matrix R i R i+1 , ..., R i+n-1 By obtaining the unique transfer matrix G, the precession axis direction l can be estimated. P =(m P ,n P ,p P ) T Precession angular velocity ω p and the spatial position O of the precession shaft p ;

[0024] First, the iterative nearest-point algorithm is used to process the acquired continuous frame point cloud sequence with equal time intervals Δt. Two M i M i+1 Estimate the corresponding orthogonal rotation matrix sequence. Translation vector sequence Specifically:

[0025] M i+1 =R i M i +T i (8)

[0026] Secondly, the classical nonlinear equation system solution method is adopted, based on the estimated orthogonal rotation matrix sequence. Translation vector sequence Make an initial estimate of the precession motion parameters.

[0027] Furthermore, the process of initially estimating the precession parameters based on the estimated orthogonal rotation matrix sequence and translation vector sequence using the classical nonlinear equations solution method is as follows:

[0028] (1) Based on the spatial instability target motion model, the objective function is constructed, and the self-constrained transfer matrix G is solved nonlinearly using the trust region method. Specifically,

[0029]

[0030] Wherein, G is represented by a general self-constrained orthogonal matrix as:

[0031]

[0032] Solving for the target transfer matrix G will be transformed into a self-constrained optimization problem with undetermined parameters α, β, and γ within a finite range.

[0033] (2) Based on the obtained transfer matrix G, the precession axis direction l P =(m P ,nP,p P ) T and precession angular velocity ω p The initial estimate is calculated as follows:

[0034]

[0035]

[0036] tr() is used to calculate the trace of a matrix;

[0037] (3) Construct the objective function with respect to the spatial position O of the precession axis based on the spatial instability target motion model. p Perform a nonlinear solution; specifically as follows:

[0038]

[0039] Where: I is a 3×3 identity matrix; O p R represents the spatial position of the precession shaft. i T is a 3×3 orthogonal rotation matrix. i It is a 3×1 translation vector.

[0040] Furthermore: the process of performing precession correction on the acquired continuous point cloud sequence so that the point cloud sequence contains only spin motion is as follows:

[0041] Let the initial acquisition time be the reference time t0, and let the other times t be... i The acquired point cloud data is based on the estimated precession motion parameters, i.e., the precession axis direction l. P Precession angular velocity ω p and precession axis spatial position O p The inverse transformation is performed to obtain the point cloud data at the reference time t0, thus completing the precession correction of the point cloud sequence; specifically, for each data point of the i-th frame of point cloud data... New position after correction for:

[0042]

[0043] For all other times t i Repeat this process to obtain a new continuous point cloud sequence with precession eliminated.

[0044] Further: The process of first using the difference rate of point cloud quantities in adjacent frames to determine whether the point cloud group meets the registration requirements, then traversing all new point cloud sequences, estimating the orthogonal rotation matrix sequence and translation vector sequence between adjacent frame point cloud data that meet the registration requirements using the point cloud registration method, and then using the trust region method to perform an initial estimation of the spin motion parameters based on the estimated rotation matrix sequence and translation vector sequence is as follows:

[0045] (1) Statistical analysis of the point cloud data differences Δc between point cloud sequences i Set the point cloud difference rate k, and for the point cloud to meet the registration requirement Δc i ∈U(E({Δc i}),kσ({Δc i Point clouds of adjacent frames and Estimate its orthogonal rotation matrix Translation vector

[0046]

[0047] here, count() is the counting function for calculating point clouds, and E() and σ() are the functions for calculating the mean and standard deviation, respectively.

[0048] (2) Constructing the objective function by estimating the sequence of orthogonal rotation matrices Translation vector sequence The trust region method is used to analyze the spin matrix R. s A nonlinear solution is performed. Specifically,

[0049]

[0050] (3) Based on the obtained spin matrix R s Spin axis direction at reference time t0 and spin angular velocity ω s The initial estimate is calculated as follows:

[0051]

[0052]

[0053] Here, tr() is used to calculate the trace of the matrix.

[0054] Furthermore: The statistical point cloud registration method iteratively converges the initial spin and precession motion parameters to obtain the final spatial instability target motion parameters as follows:

[0055] The motion parameters obtained by the trust region method are determined by the inter-frame similarity discrimination rule.

[0056] The minimum nearest distance between point cloud sequences is used as the iterative objective function to achieve iterative convergence of motion parameters.

[0057] Furthermore, the process of using the inter-frame similarity discrimination rule to discriminate the motion parameters obtained by the trust region method is as follows:

[0058] set up and Given point cloud sequences with different frame numbers (i.e., point cloud quantity i and i+1), the solution to the nonlinear equation system obtained under the selected conditions is given if and only if...

[0059]

[0060] ε is a given threshold, let's take ε = 10 -3 If the nonlinear solution is successful, it is determined that the solution is unsuccessful; otherwise, it is determined that the solution is unsuccessful.

[0061] Furthermore: the process of using the minimum nearest distance between point cloud sequences as the iterative objective function to complete the iterative convergence of motion parameters is as follows:

[0062] (1) Calculate the i-th corrected global point cloud model Each point cloud data With the (i+1)th corrected global point cloud model closest distance Specifically:

[0063]

[0064] (2) Statistical analysis of nearest distance sequences The mean and standard deviation are calculated, and mismatched points caused by shape differences are eliminated based on the outlier detection approach. The specific elimination principles are as follows:

[0065]

[0066] λ is a given threshold, and we take λ = 0.3;

[0067] (3) The corrected point cloud sequence Repeat steps (1) and (2), and sum the nearest distances of the point cloud data that meet the requirements as the objective function for the nonlinear solution. The specific expression is as follows:

[0068]

[0069] here, Point cloud data after outlier removal.

[0070] A high-precision motion estimation device for spatially unstable targets based on continuous point cloud sequences, comprising:

[0071] Downsampling module: Used to downsample the point cloud sequence of the input moving spatially unstable target using the voxel grid neighborhood method;

[0072] Acquisition module: used to acquire the initial spin and precession motion parameters of a spatially unstable target in a hierarchical manner by using the motion model of the target.

[0073] Iterative convergence module: Used to iteratively converge the initial motion parameters of spin and precession using a statistical point cloud registration method to obtain the final motion parameters of the spatially unstable target.

[0074] This invention provides a high-precision motion estimation method for spatially unstable targets based on continuous point cloud sequences. The method first downsamples dense point cloud data through point cloud preprocessing (voxel mesh neighborhood method), preserving contour information while reducing the computational burden of subsequent processing. Then, based on the motion model (transmission model) of the spatially unstable target, a hierarchical motion estimation method for obtaining initial spin and precession motion parameters is proposed, significantly reducing the impact of intra-frame motion differences on motion estimation. Finally, a statistical point cloud registration method is proposed to iteratively converge the initial spin and precession motion parameters, overcoming the self-occlusion problem of the point cloud sequence while obtaining high-precision motion parameters. This invention, in a linear array measurement system (with intra-frame motion differences), uses the motion model (transmission model) of the spatially unstable target to provide high-precision motion estimation results for spin and precession. Simulation experiments numerically verify the effectiveness and robustness of this invention for the three-dimensional shape and motion state of spatial targets. Attached Figure Description

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

[0076] Figure 1 This refers to the processing steps of this method;

[0077] Figure 2 This is a flowchart of the method steps;

[0078] Figure 3 This is a comparison chart of this method with other methods. Detailed Implementation

[0079] It should be noted that, unless otherwise specified, the embodiments and features in the embodiments of the present invention can be combined with each other. The present invention will be described in detail below with reference to the accompanying drawings and embodiments.

[0080] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. The following description of at least one exemplary embodiment is merely illustrative and is in no way intended to limit the present invention or its application or use. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0081] It should be noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the scope of exemplary embodiments according to the invention. As used herein, the singular form is intended to include the plural form as well, unless the context clearly indicates otherwise. Furthermore, it should be understood that when the terms "comprising" and / or "including" are used in this specification, they indicate the presence of features, steps, operations, devices, components, and / or combinations thereof.

[0082] Unless otherwise specifically stated, the relative arrangement, numerical expressions, and values ​​of the components and steps described in these embodiments do not limit the scope of the invention. It should also be understood that, for ease of description, the dimensions of the various parts shown in the drawings are not drawn to actual scale. Techniques, methods, and devices known to those skilled in the art may not be discussed in detail, but where appropriate, such techniques, methods, and devices should be considered part of the specification. In all examples shown and discussed herein, any specific values ​​should be interpreted as merely exemplary and not as limitations. Therefore, other examples of exemplary embodiments may have different values. It should be noted that similar reference numerals and letters in the following figures denote similar items; therefore, once an item is defined in one figure, it need not be further discussed in subsequent figures.

[0083] In the description of this invention, it should be understood that the orientation or positional relationship indicated by directional terms such as "front, back, up, down, left, right", "horizontal, vertical, horizontal" and "top, bottom" is generally based on the orientation or positional relationship shown in the accompanying drawings, and is only for the convenience of describing this invention and simplifying the description. Unless otherwise stated, these directional terms do not indicate or imply that the device or element referred to must have a specific orientation or be constructed and operated in a specific orientation, and therefore should not be construed as a limitation on the scope of protection of this invention. The directional terms "inner" and "outer" refer to the inner and outer contours relative to the outline of each component itself.

[0084] For ease of description, spatial relative terms such as "above," "over," "on the upper surface of," "above," etc., are used herein to describe the spatial positional relationship of a device or feature as shown in the figures to other devices or features. It should be understood that spatial relative terms are intended to encompass different orientations in use or operation besides the orientation of the device as described in the figures. For example, if the device in the figures is inverted, a device described as "above" or "above" other devices or structures would subsequently be positioned as "below" or "under" other devices or structures. Thus, the exemplary term "above" can include both "above" and "below." The device may also be positioned in other different ways (rotated 90 degrees or in other orientations), and the spatial relative descriptions used herein will be interpreted accordingly.

[0085] Furthermore, it should be noted that the use of terms such as "first" and "second" to define components is merely for the purpose of distinguishing the corresponding components. Unless otherwise stated, the above terms have no special meaning and therefore should not be construed as limiting the scope of protection of this invention.

[0086] Figure 1This refers to the processing steps of this method;

[0087] Figure 2 This is a flowchart of the method steps;

[0088] A high-precision motion estimation method for spatially unstable targets based on continuous point cloud sequences includes the following steps:

[0089] S1: The voxel grid neighborhood method was used to downsample the point cloud sequence of the input moving spatially unstable target;

[0090] S2: By using the motion model of the spatially unstable target, the initial motion parameters of the spin and precession of the spatially unstable target are obtained in a hierarchical manner;

[0091] S3: The statistical point cloud registration method iteratively converges the initial motion parameters of spin and precession to obtain the final spatial instability target motion parameters.

[0092] The final spatially unstable target motion parameters are more accurate than the initial motion parameters.

[0093] The S1 / S2 / S3 sequence is processed;

[0094] The process of downsampling the point cloud sequence of the input moving spatially unstable target using the voxel grid neighborhood method is as follows:

[0095] To reduce the computational complexity of point cloud data processing, we employ a voxel grid neighborhood method to downsample the input point cloud sequence. This method divides each frame of point cloud data into a series of voxels and ultimately replaces the point cloud data within each voxel with a point representing that voxel. Specifically,

[0096] 1.1 Axial Voxel Division

[0097] Traverse the point cloud data along the axes (X-axis, Y-axis, Z-axis) and determine the value range of the point cloud data on each of the three axes. Assume the side length of the cube bounding box of the voxel is d. Len The initial number of voxels along each axis can be determined based on the value range of different axes. The calculation method is as follows:

[0098]

[0099] Here, X min Y min and Z min These represent the minimum values ​​of the point cloud data along the three axes (X-axis, Y-axis, Z-axis); X max Y max and Z maxThese are the maximum values ​​of the point cloud data on the three axes, respectively; ceil() is the round-up function.

[0100] 1.2 Voxel Count Optimization

[0101] To ensure the convexity or concavity of the surface in the neighborhood of each new data point after downsampling of the point cloud data, it is necessary to ensure that the average number of points within the bounding box of each voxel cube is [missing information]. The value is between 80 and 200, therefore it is necessary to adjust the side length of the bounding box to d. Len Number of initial axial bounding boxes After optimization, the final number of axial bounding boxes are as follows:

[0102]

[0103] here, and The average number of point clouds is satisfied by the three axes (X-axis, Y-axis, Z-axis). The optimal number of voxels.

[0104] 1.3 Voxel Resampling

[0105] For each voxel V with optimized size i We resample the point cloud data within the voxel bounding box. Here, we select the centroid as the volume V. i The representative, specifically,

[0106]

[0107] Here, k i Voxel V i Number of internal data points; centroid For voxel V i Representative of.

[0108] Furthermore, the step of obtaining the initial spin and precession motion parameters of a spatially unstable target hierarchically through its motion model includes the following steps:

[0109] 2.1.1 Establish a motion model (transmission model) for a spatially unstable target.

[0110] A spatially unstable target exists in a state of combined spin, precession, and nutation motion. Due to the short measurement time, nutation can be neglected; only the spin (rotation of the target around its spin axis) and precession (rotation of the spin axis around its precession axis) of the spatially unstable target are considered. According to the motion law of the spatially unstable target (combined motion of spin and precession), for a point P on the spatially unstable target... i After time Δt, the three-dimensional coordinates P of this point are... i+1 It can be represented as:

[0111] P i+1 =R i P i +T i (4)

[0112] Here, R i T is a 3×3 orthogonal rotation matrix. i It is a 3×1 translation vector. Meanwhile, P i+1 The three-dimensional coordinate point P after an equal time interval Δt i+2 It can be represented as:

[0113] P i+2 =R i+1 P i+1 +T i+1 (5)

[0114] Here, R i+1 T is a 3×3 orthogonal rotation matrix. i+1 It is a 3×1 translation vector. Based on the motion model (transmission model) of the spatially unstable target, the orthogonal rotation matrix R... i and R i+1 Translation vector T i and T i+1 Satisfying Relationship:

[0115]

[0116] Here, G is a 3×3 orthogonal transfer matrix; I is a 3×3 identity matrix; O p Let G be the spatial position of the precession axis. Here, G extends from the precession axis direction l. P =(m P ,n P ,p P ) T and precession angular velocity ω p This can be jointly determined and specifically expressed as:

[0117]

[0118] in,

[0119]

[0120]

[0121] 2.1.2 Precession Parameter Estimation

[0122] Based on the motion model of the spatially unstable target (Equations (4), (5) and (6)), for a given feature point P i and the spatial position (P) obtained at equal time intervals Δt i+1 Pi+2 , ..., P i+n It can be achieved through the corresponding orthogonal rotation matrix (R) i R i+1 , ..., R i+n-1 By obtaining the unique transfer matrix G, the precession axis direction l can be estimated. P =(m P ,n P ,p P ) T Precession angular velocity ω p and the spatial position O of the precession shaft p Considering the limitations of feature point trajectory-based motion estimation methods (which lack generalization ability to the shape of the measured target), this invention directly applies this motion estimation approach to the overall model of the space target, proposing a motion estimation method based on the overall model of the space target.

[0123] First, the Iterative Closest Point (ICP) algorithm is used to process the acquired continuous frame point cloud sequence with equal time intervals Δt. Two by two (M) i M i+1 Estimate the corresponding sequence of orthogonal rotation matrices. Translation vector sequence Specifically:

[0124] M i+1 =R i M i +T i (8)

[0125] Secondly, the classical nonlinear equation system solution method (trust region method) is used based on the estimated orthogonal rotation matrix sequence. Translation vector sequence For precession parameters (precession axis direction l) P Precession angular velocity ω p and the spatial position O of the precession shaft p Make an initial estimate.

[0126] (1) Based on the spatial instability target motion model (Equation (6)), the objective function is constructed, and the self-constrained transfer matrix G is solved nonlinearly using the trust region method. Specifically,

[0127]

[0128] Wherein, G is represented by a general self-constrained orthogonal matrix as:

[0129]

[0130] Therefore, solving for the target transfer matrix G will be transformed into a self-constrained optimization problem with undetermined parameters α, β, and γ within a finite range.

[0131] (2) Based on the obtained transfer matrix G, the precession axis direction l P =(m P ,n P ,p P ) T and precession angular velocity ω p The initial estimate is calculated as follows:

[0132]

[0133]

[0134] Here, tr() is used to calculate the trace of the matrix.

[0135] (3) Construct the objective function with respect to the spatial position O of the precession axis based on the spatial instability target motion model (formula (6)). p A nonlinear solution is performed. Specifically:

[0136]

[0137] It is worth noting that the application background of this invention is in the context of a linear array measurement system. Since there are always intra-frame motion differences in a linear array measurement system, the initial estimation of the precession parameters will inevitably have errors. These errors will be greatly reduced during the iterative convergence phase.

[0138] 2.2 Precession Correction of Point Cloud Sequences

[0139] During the measurement time, the spatially unstable target exists in a combined motion state of spin and precession. To accurately estimate the spin motion parameters of the spatially unstable target, we will perform precession correction on the acquired continuous point cloud sequence so that the point cloud sequence contains only spin motion. Without loss of generality, let the initial acquisition time be the reference time t0, and let the other times t be... i The acquired point cloud data is based on the estimated precession motion parameters (precession axis direction l). P Precession angular velocity ω p and precession axis spatial position O p The inverse transformation is performed to obtain the point cloud data at the reference time t0, thus completing the precession correction of the point cloud sequence. Specifically, for each data point of the i-th frame of point cloud data... New position after correction for:

[0140]

[0141] Here, A p and B(-iω)p For the definition of ), please refer to formula (7). For all other times t i Repeat this process to obtain a new continuous point cloud sequence with precession eliminated.

[0142] 2.3 Initial Spin Estimation

[0143] When a new continuous point cloud sequence Theoretically, given that two consecutive point cloud sequences differ only by a unique orthogonal rotation matrix and translation vector, we can obtain this orthogonal rotation matrix and translation vector using any two adjacent frames through point cloud registration (here, the Iterative Closest Point (ICP) algorithm). However, in reality, shape differences arise between consecutive point cloud sequences due to self-occlusion, resulting in different rotation matrices and translation vectors obtained by registering any two frames. Furthermore, due to errors in our estimated precession parameters, the new point cloud sequence contains anisotropic motion distortion that cannot be ignored by the point cloud registration method. Moreover, linear array measurement systems always exhibit non-negligible intra-frame motion differences. These factors render the approach of directly registering any two consecutive frames to obtain spin motion parameters impractical.

[0144] Based on the above, the present invention first uses adjacent frame point clouds ( and The difference rate of the number of points is used to determine whether the point cloud group meets the registration requirements, and then all new point cloud sequences are traversed. The orthogonal rotation matrix sequence between adjacent frame point cloud data that meets the registration requirements is estimated using a point cloud registration method. Translation vector sequence Then, the trust region method is used based on the estimated rotation matrix sequence. Translation vector sequence Regarding spin motion parameters (initial spin axis direction) and spin angular velocity ω s A preliminary estimate is made. Specifically,

[0145] (1) Statistical analysis of the point cloud data differences Δc between point cloud sequences i Set the point cloud difference rate k (here, k can be 0.2, 0.3, or 0.5), and for points that meet the registration requirements (Δc... i ∈U(E({Δc i}),kσ({Δc i Point clouds of adjacent frames (}))) and Estimate its orthogonal rotation matrix Translation vector

[0146]

[0147] here, `count()` is the counting function for calculating point clouds. `E()` and `σ()` are the functions for calculating the mean and standard deviation, respectively.

[0148] (2) To reduce spin estimation errors caused by intra-frame motion differences and incomplete precession correction, this invention constructs an objective function based on an estimated sequence of orthogonal rotation matrices. Translation vector sequence The trust region method is used to analyze the spin matrix R. s A nonlinear solution is performed. Specifically,

[0149]

[0150] (3) Based on the obtained spin matrix R s Spin axis direction at reference time t0 and spin angular velocity ω s The initial estimate is calculated as follows:

[0151]

[0152]

[0153] Here, tr() is used to calculate the trace of the matrix.

[0154] Furthermore, a statistical point cloud registration method is used to iteratively converge the initial motion parameters of spin and precession to obtain the final spatial instability target motion parameters.

[0155] Hierarchical spin and precession initial estimations are insufficient to completely eliminate the influence of intra-frame motion differences and shape differences caused by self-occlusion on motion estimation. Therefore, the inventors further iteratively converged the spin and precession motion parameters. Here, we used the trust region method based on the inter-frame similarity discrimination rule to iteratively converge the motion parameters, and constructed a statistically based minimum nearest distance (point cloud registration) as the objective function for iterative convergence. Experimental numerical values ​​verified the effectiveness of this iterative convergence.

[0156] 3.1 Similarity Judgment Rules for Different Frames

[0157] To suppress the inherent instability of nonlinear solutions, this invention employs a frame-similar discrimination rule (based on the similarity of solutions under different frame numbers to determine successful solution determination) to successfully determine the motion parameters obtained by the trust region method, based on the existence and uniqueness of the true solution to this problem. Specifically,

[0158] set up and The solutions to the nonlinear equations obtained under different frame numbers (point cloud sequences with point cloud quantities of i and i+1) are given if and only if

[0159]

[0160] Here, ε is a given threshold, and we take ε = 10. -3 If the nonlinear solution is successful, it is determined that the solution is unsuccessful; otherwise, it is determined that the solution is unsuccessful.

[0161] 3.2 Iterative convergence objective function

[0162] For point cloud sequences acquired at equal time intervals If based on the correct motion parameters Perform inverse motion transform (also known as distortion correction) to obtain the corrected point cloud sequence. Since point cloud data will overlap, this invention uses the minimum nearest distance between point cloud sequences as the iterative objective function to achieve iterative convergence of motion parameters. Simultaneously, considering the unavoidable shape differences caused by self-occlusion during measurement, a small portion of point cloud data may not find correct matching points. This invention employs statistical methods to remove these point cloud data, thereby eliminating registration errors caused by shape differences. Specifically,

[0163] (1) Calculate the i-th corrected global point cloud model Each point cloud data With the (i+1)th corrected global point cloud model closest distance Specifically:

[0164]

[0165] (2) Statistical analysis of nearest distance sequences The mean and standard deviation are calculated, and mismatched points caused by shape differences are eliminated based on the outlier detection approach. The specific elimination principles are as follows:

[0166]

[0167] Here, λ is a given threshold, and we take λ = 0.3.

[0168] (3) The corrected point cloud sequence Repeat steps (1) and (2), and sum the nearest distances of the point cloud data that meet the requirements as the objective function for the nonlinear solution. The specific expression is as follows:

[0169]

[0170] here, Point cloud data after outlier removal.

[0171] A high-precision motion estimation device for spatially unstable targets based on continuous point cloud sequences, comprising:

[0172] Downsampling module: Used to downsample the point cloud sequence of the input moving spatially unstable target using the voxel grid neighborhood method;

[0173] Acquisition module: used to acquire the initial spin and precession motion parameters of a spatially unstable target in a hierarchical manner by using the motion model of the target.

[0174] Iterative convergence module: Used to iteratively converge the initial motion parameters of spin and precession using a statistical point cloud registration method to obtain the final motion parameters of the spatially unstable target.

[0175] Figure 3 This is a comparison chart of this method with other methods.

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

[0177] [1]Yingying G,Li W.Fast-swirl space non-cooperative target spin statemeasurements based on a monocular camera.Acta Astronautica.2020;166:156-161.doi:10.1016 / j.actaastro.2019.08.011

[0178] [2]Capuano V,Kim K,Harvard A,Chung SJ.Monocular-based posedetermination of uncooperative space objects.Acta Astronautica.2020;166:493-506.

[0179] doi:10.1016 / j.actaastro.2019.09.027.

[0180] [3]Pasqualetto Cassinis L,Fonod R,Gill E,Ahrns I,Gil-FernándezJ.Evaluation of tightly-and loosely-coupled approaches in CNN-based poseestimation systems for uncooperative spacecraft.

[0181] [4]Yin F,Chou W,Wu Y,Yang G,Xu S.Sparse Unorganized Point Cloud BasedRelative Pose Estimation for Uncooperative Space Target.Sensors.2018;18(4):1009.doi:10.3390 / s18041009

[0182] [5]Zhao D,Sun C,Zhu Z,Wan W,Zheng Z,Yuan J.Multi-spacecraftcollaborative attitude determination of space tumbling target withexperimental verification.Acta Astronautica.2021;185:1-13.doi:10.1016 / j.actaastro.2021.04.029

[0183] [6]Li J,Zhuang Y,Peng Q,Zhao L.Pose Estimation of Non-CooperativeSpace Targets Based on Cross-Source Point Cloud Fusion.Remote Sensing.

[0184] 2021;13(21):4239.doi:10.3390 / rs13214239

[0185] [7]Ma C,Dai H,Yuan J.Estimation of inertial characteristics oftumbling spacecraft using constant state filter.Advances in SpaceResearch.2017;60(3):513-530.doi:10.1016 / j.asr.2017.03.032

[0186] [8] Sun Riming, Li Jiangdao, Lin Tingting, et al. A method for distortion correction in linear array imaging of spatially unstable targets [J]. Infrared and Laser Engineering, 2019, 48(09):280-289.

[0187] [9]Sun Riming,Yang Yichen,Ma Yongfeng,Kong Xiangyu,Li Changxian,High-precision motion estimation for unstable space targets,Infrared and LaserEngineering.2021;50(1):20200104.doi:10.3788 / IRLA20200104

[0188]

[10] Sun R, Yang Y, Ma Y, Wang SA transmission model for motion estimation of unstable space targets. Computers&Graphics.2021;98:29-36.doi:10.1016 / j.cag.2021.04.009

[0189] Acta Astronautica.2021;182:189-202.doi:10.1016 / j.actaastro.2021.01.035.

[0190]

[11] Aghili F,Parsa K.Motion and Parameter Estimation of Space ObjectsUsing Laser-Vision Data.Journal of Guidance,Control,and Dynamics.2009;32(2):538-550.doi:10.2514 / 1.37129

[0191]

[12] Pesce V,Lavagna M,Bevilacqua R.Stereovision-based pose andinertia estimation of unknown and uncooperative space objects.Advances inSpace Research.2017;59(1):236-251.doi:10.1016 / j.asr.2016.10.002

[0192]

[13] Xu X,Shen Z,Zhao J,Zou J.Rotational motion estimation of non-cooperativetarget in space based on the 3D point cloud sequence.Advances inSpace Research.2022;69(3):1528-1537.doi:10.1016 / j.asr.2021.10.054.

[0193]

[14] Liu K,Wang L,Liu H,Zhang X.A Relative Pose Estimation MethodofNon-Cooperative Space Targets.J Phys:Conf Ser.2022;2228(1):012029.doi:10.1088 / 1742-6596 / 2228 / 1 / 012029

[0194]

[15] Song J,Cao C.Pose Self-Measurement of Noncooperative SpacecraftBased onSolar Panel Triangle Structure.Journal of Robotics.2015;2015:1-6.doi:10.1155 / 2015 / 472461.

[0195]

[16] Gao X,Liang B,Du X,Xu W.Pose measurement of large non-cooperativesatellite using structured light vision sensor.In:2012 IEEEInternational Conferenceon Information Science and Technology.IEEE;2012:101-108.doi:10.1109 / ICIST.2012.6221616.

[0196]

[17] Du X,Liang B,Xu W,Qiu Y.Pose measurement of large non-cooperativesatellite based on collaborative cameras.Acta Astronautica.2011;68(11-12):2047-2065.doi:10.1016 / j.actaastro.2010.10.021.

[0197]

[18] Peng J,Xu W,Yuan H.An Efficient Pose Measurement Method of aSpaceNon-Cooperative Target Based on Stereo Vision.IEEE Access.2017;5:22344-22362.doi:10.1109 / ACCESS.2017.2759798.

[0198]

[19] Sabatini M,Palmerini GB,Gasbarri P.A testbed for visual basednavigation andcontrol during space rendezvous operations.ActaAstronautica.2015;117:184-196.doi:10.1016 / j.actaastro.2015.07.026

[0199]

[20] Terui F,Kamimura H,Nishida S I.Motion Estimation to a FailedSatellite onOrbit using Stereo Vision and 3D Model Matching.In:2006 9thInternationalConference on Control,Automation,Robotics and Vision.IEEE;2006:1-8.doi:10.1109 / ICARCV.2006.345305.

[0200]

[21] Paul J.Besl,Neil D.McKay.A method for registration of 3-Dshapes.IEEETrans Pattern Anal Mach Intell.1992;14(2):239-256.doi:10.1109 / 34.121791

[0201]

[22] Li R,Li J,Chen F,Xiao Y.A method of relative pose measurement bysingleload for geo instability target.J Astronaut 2017;38(10):1105–13.doi:10.3873 / j.issn.1000-1328.2017.10.011.

Claims

1. A method for high-precision motion estimation of spatial unstable targets based on a sequence of continuous point clouds, characterized in that: The method comprises the following steps: The point cloud sequence of the input motion space unstable target is down-sampled by using a voxel grid neighborhood method; Initial motion parameters of the precession and spin of the space unstable target are obtained in a hierarchical manner through a motion model of the space unstable target; Statistical point cloud registration is used to iteratively converge the initial motion parameters of the precession and spin, so as to obtain final motion parameters of the space unstable target; The initial motion parameters of the precession and spin of the space unstable target are obtained in a hierarchical manner through a motion model of the space unstable target, and the method comprises the following steps: The precession parameters of the space unstable target are initially estimated through a motion model of the space unstable target; The obtained continuous point cloud sequence is corrected for precession, so that only the spin motion is contained in the point cloud sequence; First, the difference rate of the number of point clouds of adjacent frames is used to determine whether the point clouds meet the registration requirement, then all the input point cloud sequences are traversed, the orthogonal rotation matrix sequence and the translation vector sequence between the adjacent frame point cloud data meeting the registration requirement are estimated by using the point cloud registration method, and then the spin motion parameters are initially estimated according to the estimated rotation matrix sequence and translation vector sequence by using the trust region method; The process of correcting the obtained continuous point cloud sequence for precession, so that only the spin motion is contained in the point cloud sequence, is as follows: Let the initial acquisition time as the reference time t0, the other time t i The acquired point cloud data is inversely transformed into the point cloud data of the reference time t0 according to the estimated precession motion parameters, i.e. the precession axis direction l P , the precession angular velocity ω p And the spatial position of the precession axis O p , complete the precession correction of the point cloud sequence; specifically, for each data point The new position after correction : for all other time instants t i The process is repeated, thus obtaining a new sequence of continuous point clouds with precession eliminated 2. The method of claim 1, wherein: The initial estimation of the precession parameters of the space unstable target through a motion model of the space unstable target is specifically as follows: Based on the motion model of a spatially unstable target, for a given feature point P i and the spatial position P obtained at equal time intervals Δt i+1 P i+2 , ..., P i+n It can be passed through the corresponding orthogonal rotation matrix R i R i+1 , ..., R i+n-1 By obtaining the unique transfer matrix G, the precession axis direction l can be estimated. P =(m P ,n P ,p P ) T Precession angular velocity ω p and the precession shaft spatial position O p ; Firstly, the iterative closest point algorithm is used to obtain the point cloud sequence of continuous frames with equal time interval Δt M i , M i+1 Estimate its corresponding orthogonal rotation matrix sequence And translation vector sequence Specifically: M i+1 = R i M i + T i (8) Secondly, the classical nonlinear equations solving method is used to estimate the initial precession motion parameters according to the estimated orthogonal rotation matrix sequence and translation vector sequence and translation vector sequence and translation vector sequence and translation vector sequence and translation vector sequence and translation vector sequence and translation vector sequence and translation vector sequence and translation vector sequence 3. The method of claim 2, wherein: The process of initially estimating the precession motion parameters according to the estimated orthogonal rotation matrix sequence and translation vector sequence by using a classical nonlinear equation solving method is as follows: (1) According to the motion model of the space unstable target, a target function is constructed to solve the transmission matrix G in a self-constrained form by using the trust region method, and specifically, Wherein, G is represented by a general self-constrained orthogonal matrix as follows: The solution of the target transmission matrix G will be converted into a self-constrained optimization problem with limited range of undetermined parameters α, β and γ; (2) The initial estimation is made on the basis of the obtained transmission matrix G and the precession axis direction l P = (m P , n P , p P ) T and the precession angular velocity ω p . The specific calculation method is as follows: tr() is the calculation of the matrix trace. (3) According to the spatial unstable target motion model, a target function is constructed for the spatial position O of the precession axis p Nonlinear solving is performed; specifically as follows where: I is a 3x3 identity matrix; O p is the spatial position of the precession axis, R i is a 3x3 orthogonal rotation matrix, T i is a 3x1 translation vector.

4. The method of claim 1, wherein: The process of initially estimating the spin motion parameters according to the estimated rotation matrix sequence and translation vector sequence by using the trust region method is as follows: (1) Statistics point cloud sequence point cloud data difference Δc i , set point cloud difference rate k, to meet the registration requirements Δc i ∈U(E({Δc i}),kσ({Δc i})) adjacent frame point cloud And Estimate its orthogonal rotation matrix And translation vector Here, count() is a counting function for the point cloud, E() and σ() are the calculation functions of the mean and standard deviation, respectively; (2) constructing an objective function through the estimated sequence of orthogonal rotation matrices and the sequence of translation vectors solving the spin matrix R s nonlinearly, specifically, (3) The spin matrix R obtained s The spin axis direction at the reference time t0 And the spin angular velocity ω s The initial estimate is made, and the specific calculation method is: Here, tr() is the calculation of the matrix trace.

5. The method of claim 1, wherein: The process of iteratively converging the initial motion parameters of the precession and spin by using the statistical point cloud registration method to obtain the final motion parameters of the space unstable target is as follows: The motion parameters obtained by the trust region method are discriminated by using a different frame similarity discrimination rule; The minimum nearest distance between the point cloud sequences is used as an iterative objective function to complete the iterative convergence of the motion parameters.

6. The method of claim 5, wherein: The process of discriminating the motion parameters obtained by the trust region method by using a different frame similarity discrimination rule is as follows: Let and are point cloud sequences with different frame numbers, i.e., point cloud numbers, i and i+1, respectively, the solution of the nonlinear equation system obtained under the selection condition if and only if ε is a given threshold, take ε = 10 -3 , determine that the nonlinear solution is successful, otherwise, determine that the solution is unsuccessful.

7. The method of claim 5, wherein: The process of using the minimum nearest distance between the point cloud sequences as an iterative objective function to complete the iterative convergence of the motion parameters is as follows: (1) calculating the i-th corrected overall point cloud model each point cloud data the nearest distance to the i+1-th corrected overall point cloud model the nearest distance to the i+1-th corrected overall point cloud model Specifically: (2) Statistics the mean and standard deviation of the recent distance sequence and eliminate the false matching points caused by shape difference according to the outlier discrimination idea The elimination principle is specifically: λ is a given threshold, and λ = 0.3 is taken; (3) the point cloud sequence after correction Repeat steps (1), (2), and sum the nearest distances of the point cloud data meeting the requirements as the objective function of the nonlinear solution, and the specific expression is: Here, is the point cloud data after outliers are removed.

8. A device for high-precision motion estimation of a spatially unstable target based on a sequence of continuous point clouds, characterized in that: The method comprises the following steps: The downsampling module is configured to perform data downsampling on the input point cloud sequence of the motion space unstable target by using a voxel grid neighborhood method. The acquisition module is configured to acquire initial motion parameters of precession and spin of the space unstable target in a hierarchical manner through a motion model of the space unstable target. The iterative convergence module is configured to perform iterative convergence on the initial motion parameters of precession and spin based on a statistical point cloud registration method to obtain final motion parameters of the space unstable target. The method for acquiring initial motion parameters of precession and spin of the space unstable target in a hierarchical manner through a motion model of the space unstable target comprises the following steps: The motion model of the space unstable target is used to preliminarily estimate the precession parameters of the space unstable target. The obtained continuous point cloud sequence is corrected for precession according to the preliminarily estimated precession parameters, so that only spin motion is contained in the point cloud sequence. First, the difference rate of the number of adjacent frame point clouds is used to determine whether the point cloud meets the registration requirement, then all input point cloud sequences are traversed, the orthogonal rotation matrix sequence and the translation vector sequence between the adjacent frame point cloud data meeting the registration requirement are estimated by using the point cloud registration method, and then the spin motion parameters are preliminarily estimated according to the estimated rotation matrix sequence and translation vector sequence by using the trust region method. The process of correcting the obtained continuous point cloud sequence for precession so that only spin motion is contained in the point cloud sequence is as follows: Let the initial acquisition time as the reference time t0, the other time t i The acquired point cloud data is inversely transformed into the point cloud data of the reference time t0 according to the estimated precession motion parameters, i.e. the precession axis direction l P , the precession angular velocity ω p and the precession axis spatial position O p , to complete the precession correction of the point cloud sequence; specifically, for each data point of the i-th frame of point cloud data, the new position after correction is: for all other time instants t i The process is repeated, thus obtaining a new sequence of continuous point clouds with precession eliminated

Images