Method for fast measurement of relative position and pose of space non-cooperative slow-rotating target by laser radar

Abstract

This invention discloses a method for rapid relative pose measurement of non-cooperative slow-rotating targets in space using lidar, specifically relating to the field of aerospace technology. The method includes the following steps: S1, after rapidly acquiring the target point cloud using a linear array lidar, reading the target reference point cloud, entering the navigation process, and preprocessing the measurement point cloud; S2, after preprocessing the measurement point cloud, performing initial pose estimation; for the initially acquired target point cloud data, an improved principal component analysis method is used to calculate the initial value; for point clouds not from the first frame, the pose result of the previous frame is selected as the initial value; S3, after steps S1 and S2, a reliable initial pose estimate is obtained, and the ICP algorithm is used for accurate pose estimation. This invention can achieve rapid pose measurement of slow-rotating targets in space. Through features sampling, rapid coarse registration, and Kalman filtering, it can achieve real-time and stable measurement with high algorithm measurement accuracy, meeting the task requirements for real-time automatic navigation of targets in space-related missions.

Description

Technical Field

[0001] This invention relates to the field of aerospace technology, and in particular to a method for rapid relative attitude measurement of non-cooperative slow-rotating targets in space using lidar. Background Technology

[0002] Rapid measurement of the position and attitude of a space target is one of the key technologies for achieving autonomous relative navigation between a spacecraft and the target. This typically involves measurement sensors and a corresponding measurement data processing system. During rendezvous measurements from a distance, when approaching to close range (150m), the measurement system is required to measure the target's three-axis relative position, relative attitude, and rate of change in real time, continuing until docking or capture conditions are met. In the close-range approach phase, for non-cooperative targets, photoelectric measurement sensors based on lidar and monocular / binocular cameras are currently considered the only effective relative measurement technology.

[0003] Based on current research experience at home and abroad, the relative pose measurement of non-cooperative slow-spinning unstable targets at close range faces the following challenges: (1) Non-cooperative targets do not have artificially designed special markers and active transponders, and their shape and motion information are unknown. Usually, three-dimensional reconstruction, motion estimation and other sensing operations need to be performed first when entering the approach phase; (2) For real-time relative pose measurement of non-cooperative slow-spinning unstable targets, compared with stable targets, the algorithm running time of the measurement system is more demanding; (3) The target is in a slow spin state, which will produce certain motion distortion for non-instantaneous imaging measurement sensors. Summary of the Invention

[0004] This invention aims to provide a method for rapid relative pose measurement of non-cooperative slowly rotating targets in space using lidar, a fast and reliable relative pose determination algorithm, which overcomes the rotational motion distortion of the target, optimizes key aspects of the algorithm, and ensures the real-time performance of the algorithm.

[0005] To achieve the above objectives, the technical solution of the present invention is as follows: A method for rapid relative pose measurement of a non-cooperative slowly rotating target in space using a lidar system, comprising the following steps:

[0006] S1. After quickly acquiring the target point cloud using a linear array lidar, read the target reference point cloud, enter the navigation process, and preprocess the measured point cloud.

[0007] S2. After preprocessing the measured point cloud, perform initial pose estimation. For the target point cloud data acquired for the first time, use the improved principal component analysis method to calculate the initial value. For point clouds that are not the first frame, select the pose result of the previous frame as the initial value.

[0008] S3. After steps S1 and S2, a reliable initial pose estimate is obtained, and the ICP algorithm is used for accurate pose estimation.

[0009] Furthermore, the specific steps of step S1 are as follows:

[0010] S1.1 Perform distance-based denoising preprocessing on the measured point cloud to remove discrete spatial noise points; calculate the average distance between each point in the measured point cloud and all points in its neighborhood, which is called the point density of each point, and calculate the mean and standard deviation of all point densities; since discrete noise points have a density much greater than that of target points, points with a density greater than twice the mean error are removed as noise points.

[0011] S1.2, Perform motion compensation on the measured point cloud; let the acquisition time of the first laser point be t0, the target spin angular velocity be v, and the i-th laser point p i The measurement time is t i p i If the spin angle is α, then

[0012] α(t i )=v(t i -t0)

[0013] Based on the acquisition timescale of each point, calculate the angle of that point relative to the start time of laser measurement using the above formula, then use the angle to calculate the attitude compensation matrix R, and apply it to p. i Correction yields p′ i ,but

[0014] p′ i =R(α(t) i ))*p i ;

[0015] S1.3. Perform feature-based downsampling on the compensated measurement point cloud; first, use principal component analysis to calculate p′ for each point. i normal vector n i Construct the covariance matrix C, then

[0016]

[0017] Perform eigenvalue decomposition on matrix C to obtain eigenvectors [x1...x6] and eigenvalues ​​[λ1≥...≥λ6]; then, for each point p′ in the corrected measurement point cloud... i Calculate matrix v i =[p′ i ×n i n i ], for each feature vector x j (j=1...6) and matrix v i Calculate the influence factor of each point by performing a dot product. And according to The point sets are sorted in descending order of size; finally, according to the requirement of the number of output sampling points, the points with the highest influence factors are selected as sampling points.

[0018] Furthermore, the specific steps of step S2 are as follows:

[0019] S2.1 Perform principal component analysis transformation on the preprocessed first frame target point cloud, extract the principal component components U, and establish the estimated target body coordinate system in the lidar imaging coordinate system; the target body coordinate system is defined by the reference point cloud and is set as U0;

[0020]

[0021] S2.2, Target relative to initial attitude transformation matrix R0,

[0022] R0=U*U0

[0023] S2.3. Solve for the translation matrix T0 between the target body coordinate system and the lidar imaging coordinate system. Transform the target measurement point cloud p through the rotation matrix R0, and with the reference point cloud q, solve for the initial translation matrix T0. Then the target's relative initial pose is obtained.

[0024] T0 = ​​q - R0 * p.

[0025] Furthermore, the specific steps of step S3 are as follows:

[0026] S3.1. Convert the current frame point cloud data p into a new current frame point cloud to be registered p′=D0·p using the initial pose value D0;

[0027] S3.2 Search for the point in the reference point cloud data q that is closest to the target point cloud p′ to be registered, and use it as a one-to-one corresponding point set. S is the point set corresponding to the point cloud p′ to be registered in the reference point cloud data.

[0028] S3.3 Calculate the rotation and translation matrices between S and p′ using SVD decomposition. Let H be the covariance matrix of the corresponding point set. Perform SVD decomposition on H, and the rotation matrix R and translation matrix T between S and p′ are as follows:

[0029] H=U∧V t

[0030] R = V·U t

[0031] T = SR·p′

[0032] Substituting the pose result (R, T) into the distance function, calculate the distance E between the measured point cloud and the reference point cloud at this time.

[0033]

[0034] Repeat steps S2 to S3 until the distance function E between point clouds no longer decreases, at which point the iteration stops and the optimal pose result can be obtained.

[0035] Furthermore, the measurement method also includes:

[0036] S4. After the accurate pose solution is obtained, a Kalman filter is added to update the current pose result and predict the pose value at the next moment.

[0037] Furthermore, the first step in the Kalman filter is the prediction of the state vector. Due to the high measurement frequency, the motion model of adjacent frames can be regarded as a uniform motion model; therefore, the state prediction equation is:

[0038]

[0039] P k+1|k =ΦP k|k Φ T +Q k

[0040] in, It is based on the state at the previous moment. The predicted state vector, Φ is the state transition matrix, and P k+1|k and P k|k Q represents the covariance matrix of the prediction model. k It consists of random noise and disturbances during the processing, which conform to a Gaussian distribution;

[0041] The position and attitude calculated by the ICP fine registration process are used as the measured value Z. k The estimated value of the state, together with the above prediction equation, is called the update;

[0042]

[0043]

[0044] Among them, K k+1 This is called the Kalman gain matrix, Z. k+1 Let h represent the measured value and the relationship between the measured value and the state vector, and S represent the measured value. k+1 This represents the updated covariance matrix.

[0045] Compared with existing technologies, the beneficial effects of this solution are:

[0046] This scheme enables rapid pose measurement of slowly rotating targets in space. Through techniques such as feature sampling, fast coarse registration, and Kalman filtering, it can stably measure the relative pose of the target in real time. The algorithm has high measurement accuracy, with position accuracy at the centimeter level and angle accuracy better than 2°. It can solve the problem of six-degree-of-freedom relative pose measurement of targets in the approach phase within 100m and meet the mission requirements for real-time automatic navigation of targets in space-related tasks. Attached Figure Description

[0047] Figure 1 This is a flowchart of the method for rapid relative pose measurement of a non-cooperative slow-rotating target in space using a lidar according to the present invention;

[0048] Figure 2 This is a schematic diagram of a simulation experiment of relative navigation of a 64-line lidar against a non-cooperative target in this embodiment;

[0049] Figure 3 This is a target sequence point cloud data map continuously acquired by the lidar in this embodiment;

[0050] Figure 4 This is a diagram showing the relative position measurement results of the simulation experiment in this embodiment;

[0051] Figure 5 This is a diagram showing the relative attitude measurement results of the simulation experiment in this embodiment. Detailed Implementation

[0052] The present invention will be further described in detail below through specific embodiments:

[0053] Example

[0054] This invention uses a common satellite configuration with a solar panel as an example. Through 64-line laser imaging radar simulation technology, it simulates the rendezvous process of a laser radar platform approaching a target from a distance, generating a continuous target point cloud. The target rotates slowly around its own inertial axis at a speed of 5° / s. Figure 2 The image shows a simulation diagram of a 64-line laser imaging radar for relative navigation of a non-cooperative target. Figure 3 This is to simulate the process of a lidar continuously measuring the target from 100m to 10m with a frequency of 1Hz to obtain the target's three-dimensional point cloud.

[0055] As attached Figure 1 As shown, the method for rapid relative pose measurement of a non-cooperative, slowly rotating target in space using lidar includes the following steps:

[0056] S1. After quickly acquiring the target point cloud using a linear array lidar, the target reference point cloud is read, and the navigation process begins. Preprocessing is performed on the measured point cloud, including denoising, motion compensation, and feature sampling. Then, the pre-stored target reference point cloud is read, and pose calculation begins. The specific steps are as follows:

[0057] S1.1 Perform distance-based denoising preprocessing on the measured point cloud to remove discrete spatial noise points; calculate the average distance between each point in the measured point cloud and all points in its neighborhood, which is called the point density of each point, and calculate the mean and standard deviation of all point densities; since discrete noise points have a density much greater than that of target points, points with a density greater than twice the mean error are removed as noise points.

[0058] S1.2, Perform motion compensation on the measured point cloud; let the acquisition time of the first laser point be t0, the target spin angular velocity be v, and the i-th laser point p i The measurement time is t i p i If the spin angle is α, then

[0059] α(t i )=v(t i -t0)

[0060] Based on the acquisition timescale of each point, calculate the angle of that point relative to the start time of laser measurement using the above formula, then use the angle to calculate the attitude compensation matrix R, and apply it to p. i Correction yields p′ i ,but

[0061] p′ i =R(α(t) i ))*p i ;

[0062] S1.3. Perform feature-based downsampling on the compensated measurement point cloud; first, use principal component analysis to calculate p′ for each point. i normal vector n i Construct the covariance matrix C, then

[0063]

[0064] Perform eigenvalue decomposition on matrix C to obtain eigenvectors [x1...x6] and eigenvalues ​​[λ1≥...≥λ6]; then, for each point p′ in the corrected measurement point cloud... i Calculate matrix v i =[p′i×n i n i ], for each feature vector x j (j=1...6) and matrix v i Calculate the influence factor of each point by performing a dot product. And according to The point sets are sorted in descending order of size; finally, according to the requirement of the number of output sampling points, the points with the highest influence factors are selected as sampling points.

[0065] S2. After preprocessing the measured point cloud, perform initial pose estimation to quickly obtain reliable initial relative pose values. For the target point cloud data acquired for the first time, an improved principal component analysis method is used to calculate the initial values; for point clouds that are not from the first frame, the pose result of the previous frame is selected as the initial value; the specific steps are as follows:

[0066] S2.1. Perform principal component analysis transformation on the preprocessed first frame of target point cloud, calculate the covariance matrix of the target point cloud, and perform eigenvalue decomposition on the covariance matrix. The eigenvectors are the three-axis components of the target body coordinate system in the lidar measurement coordinate system. Then, in conjunction with the body coordinate system defined by the reference point cloud, calculate the initial rotation matrix and translation matrix.

[0067] Covariance matrix and matrix decomposition: Suppose a 3D target point cloud p contains n points,

[0068]

[0069] Calculate the center of the point set Decentralize the point set to obtain Then the covariance matrix Q = A T A. According to formula A T Ax = λx is used to find the eigenvalues ​​and eigenvectors of the covariance matrix. The eigenvectors are the three-axis components U. A target body coordinate system is estimated in the lidar imaging coordinate system; the target body coordinate system is defined by the reference point cloud and denoted as U0.

[0070]

[0071] S2.2, Target relative to initial attitude transformation matrix R0,

[0072] R0=U*U0

[0073] S2.3. Solve for the translation matrix T0 between the target body coordinate system and the lidar imaging coordinate system. Transform the target measurement point cloud p through the rotation matrix R0, and with the reference point cloud q, solve for the initial translation matrix T0. Then the target's relative initial pose is obtained.

[0074] T0 = ​​q - R0 * p.

[0075] S3. After steps S1 and S2, a reliable initial pose estimate is obtained. The ICP algorithm is then used for accurate pose estimation. The specific steps are as follows:

[0076] S3.1. Convert the current frame point cloud data p into a new current frame point cloud to be registered p′=D0·p using the initial pose value D0;

[0077] S3.2 Search for the point in the reference point cloud data q that is closest to the target point cloud p′ to be registered, and use it as a one-to-one corresponding point set. S is the point set corresponding to the point cloud p′ to be registered in the reference point cloud data.

[0078] S3.3 Calculate the rotation and translation matrices between S and p′ using SVD decomposition. Let H be the covariance matrix of the corresponding point set. Perform SVD decomposition on H, and the rotation matrix R and translation matrix T between S and p′ are as follows:

[0079] H=U∧V t

[0080] R = V·U t

[0081] T = SR·p′

[0082] Substituting the pose result (R, T) into the distance function, calculate the distance E between the measured point cloud and the reference point cloud at this time.

[0083]

[0084] Repeat steps S2 to S3 until the distance function E between point clouds no longer decreases. Then the iteration stops when the distance difference between two adjacent points is less than the threshold e = 1e-5. At this point, the optimal pose result can be obtained.

[0085] S4. To prevent irreversible registration errors caused by mismatch in a single frame during tracking, a Kalman filter is added after the accurate pose calculation to update the current pose result and predict the pose value at the next moment. This step effectively avoids interruptions in the tracking process due to sudden changes or errors in the results of a single frame. The first step of the Kalman filter is the prediction of the state vectors (position, attitude, and velocity). Due to the high measurement frequency, the motion model of adjacent frames can be regarded as a uniform motion model. Therefore, the state prediction equation is:

[0086]

[0087] P k+1|k =ΦP k|k Φ T +Q k

[0088] in, It is based on the state at the previous moment. The predicted state vector, Φ is the state transition matrix, and P k+1|k and P k|k Q represents the covariance matrix of the prediction model. k It consists of random noise and disturbances during the processing, which conform to a Gaussian distribution;

[0089] The position and attitude calculated by the ICP fine registration process are used as the measured value Z. k The estimated value of the state, together with the above prediction equation, is called the update;

[0090]

[0091]

[0092] Among them, K k+1 This is called the Kalman gain matrix, Z. k+1 Let h represent the measured value and the relationship between the measured value and the state vector, and S represent the measured value. k+1 This represents the updated covariance matrix.

[0093] The simulation results of the non-cooperative slow-rotating target relative pose measurement method based on linear array laser imaging radar proposed in this embodiment are as follows: Figure 4 and Figure 5 As shown, Figure 4 In the diagram, (1) is the position result in the X direction, (2) is the position result in the Y direction, and (3) is the position result in the Z direction; Figure 5 In the figure, (1) is the attitude angle result in the X direction, (2) is the attitude angle result in the Y direction, and (3) is the attitude angle result in the Z direction. The horizontal axis represents the number of target image frames acquired, and the vertical axis represents the measurement result and the true value.

[0094] The above are merely embodiments of the present invention, and common knowledge such as specific structures and / or characteristics in the solutions are not described in detail here. It should be noted that those skilled in the art can make various modifications and improvements without departing from the structure of the present invention, and these should also be considered within the scope of protection of the present invention. These modifications and improvements will not affect the effectiveness of the implementation of the present invention or the practicality of the patent. The scope of protection claimed in this application should be determined by the content of its claims, and the specific embodiments described in the specification can be used to interpret the content of the claims.

Claims

1. A method for rapid relative pose measurement of a non-cooperative, slowly rotating target in space using lidar, characterized in that: Includes the following steps: S1. After quickly acquiring the target point cloud using a linear array lidar, read the target reference point cloud, enter the navigation process, and preprocess the measured point cloud. S2. After preprocessing the measured point cloud, perform initial pose estimation. For the target point cloud data acquired for the first time, use the improved principal component analysis method to calculate the initial value. For point clouds that are not the first frame, select the pose result of the previous frame as the initial value. S3. After steps S1 and S2, a reliable initial pose estimate is obtained, and the ICP algorithm is used for accurate pose estimation. The specific steps of step S1 are as follows: S1.1 Perform distance-based denoising preprocessing on the measured point cloud to remove discrete spatial noise points; calculate the average distance between each point in the measured point cloud and all points in its neighborhood, which is called the point density of each point, and calculate the mean and standard deviation of all point densities; since discrete noise points have a density much greater than that of target points, points with a density greater than twice the mean error are removed as noise points. S1.2, Perform motion compensation on the measured point cloud; let the acquisition time of the first laser point be t0, the target spin angular velocity be v, and the i-th laser point... The measurement time is , The spin angle is ,but Based on the acquisition timescale of each point, calculate the angle of that point relative to the start time of laser measurement using the above formula, and then use the angle to calculate the attitude compensation matrix. and to Correction obtained ,but ； S1.3, Perform feature-based downsampling on the compensated measurement point cloud; First, principal component analysis is used to calculate the value of each point. normal vector Construct the covariance matrix ,but For matrix Perform eigenvalue decomposition to obtain eigenvectors. and eigenvalues Then, for each point in the corrected measurement point cloud... Calculate the matrix , each feature vector With matrix Calculate the influence factor of each point by performing a dot product. ,in and according to The point sets are sorted in descending order of size; finally, according to the requirement of the number of output sampling points, the points with the highest influence factors are selected as sampling points; The specific steps of step S2 are as follows: S2.1 Perform principal component analysis transformation on the preprocessed first frame of target point cloud to extract principal component components. The target body coordinate system is estimated in the lidar imaging coordinate system; the target body coordinate system is defined by the reference point cloud and set as follows. ; S2.2, Target relative to initial attitude transformation matrix , S2.3 Solve for the translation matrix between the target body coordinate system and the lidar imaging coordinate system. Target measurement point cloud By rotation matrix Transformation, and reference point cloud Solve for the initial translation matrix Then the target relative to the initial pose ; 。 2. The method for rapid relative pose measurement of a non-cooperative, slowly rotating target in space using a lidar according to claim 1, characterized in that: The specific steps of step S3 are as follows: S3.1, Initialize the pose of the current frame point cloud data p. Convert to a new current frame point cloud to be registered ; S3.2 Search for the target point cloud in the reference point cloud data q. The nearest points are taken as the point set with one-to-one correspondence, and S is the point cloud to be registered. The corresponding point set in the reference point cloud data; S3.3, Using SVD decomposition to calculate S and Let S be the rotation and translation matrix between S and H, and let H be the covariance matrix of the corresponding point set. Performing SVD decomposition on H, we obtain S and... Rotation matrix between Translation matrix They are respectively: pose result Substitute the values ​​into the distance function to calculate the distance between the measured point cloud and the reference point cloud. , Repeat steps S3.2 to S3.3 until the distance function E between point clouds no longer decreases, then stop the iteration. At this point, the optimal pose result can be obtained.

3. The method for rapid relative pose measurement of a non-cooperative slow-rotating target in space using a lidar according to claim 1 or 2, characterized in that: The measurement method further includes: S4. After the accurate pose solution is obtained, a Kalman filter is added to update the current pose result and predict the pose value at the next moment.

4. The method for rapid relative pose measurement of a non-cooperative, slowly rotating target in space using a lidar according to claim 3, characterized in that: The first step in Kalman filtering is the prediction of the state vector. Due to the high measurement frequency, the motion model of adjacent frames can be regarded as a uniform motion model; therefore, the state prediction equation is: in, It is based on the state at the previous moment. The predicted state vector, It is the state transition matrix. and This represents the covariance matrix of the prediction model. It consists of random noise and disturbances during the processing, which conform to a Gaussian distribution; The position and attitude calculated by the ICP fine registration process are used as the measurement values. The estimated value of the state, together with the above prediction equation, is called the update; in, This is called the Kalman gain matrix. For measured values, This represents the relationship between the measured values ​​and the state vector. This represents the updated covariance matrix.

Images