Relative pose determination of uncooperative known target based on extracting region of interest

This paper deals with the problem of relative pose determination between a model-known uncooperative space target and a chaser spacecraft using three-dimensional point cloud. A novel, reliable and real-time relative pose determination framework is proposed, which is composed of a region of interest–based initial pose acquisition method and a template matching iterative closest point algorithm for tracking pose. The initial pose is obtained by the principal component analysis method which aligns the region of interest extracted from the scanning point cloud with the region of interest of the known model point cloud, and a three-dimensional convex hull is constructed to extract the region of interest fast. To improve the iterative closest point, the registration between the template point cloud generated by the pose of last frame and the point cloud of the current frame is used to replace the registration between the local scanning point cloud and the global model point cloud. In addition, the performance (stability, low cost and their robustness against noise) of the proposed initial pose acquisition method and the accuracy and reliability of the pose tracking algorithm have been demonstrated by the numerical experiments. Finally, the effectiveness of the proposed method is verified by the field experiment.


Introduction
The relative pose determination in the aerospace field computes the relative position and attitude between the chaser spacecraft and the target spacecraft. As one of the critical technologies for many advanced space applications, like on-orbit servicing (OOS) 1,2 and active debris removal (ADR), 3,4 the relative pose determination algorithm has been successfully applied to the projects 5-7 between two cooperative spacecrafts, which are equipped with auxiliary equipments such as marker and answering machine.
However, most of the spacecraft of the on-orbit service and space debris are uncooperative targets without available auxiliary markers or communication devices, which brings great challenges to the pose determination between the chaser and uncooperative targets and has been frequently concerned by the majority of researchers. The method of the pose estimation depends on the type of sensors installed on board of the chaser spacecraft. Such sensors can be monocular vision, 8 stereo vision 9 and Light Detection and Ranging (LiDAR). 10,11 Due to the robustness in the variable illumination conditions, various types of LiDAR, such as scanning LiDAR and Flash LiDAR, have become the dominant types in the research field of the pose determination of uncooperative targets. 12 In this paper, a fast and accurate initial pose estimation algorithm based on region of interest (ROI) extraction is proposed for the uncooperative spacecraft using the three-dimensional (3D) point cloud obtained from Flash LiDAR or scanning LiDAR. Due to the speed requirement, the 3D convex hull method is used to assist in extracting ROI. In addition, the PCA (principal component analysis) method 13 is adopted to obtain the initial pose. In the pose tracking step, a stable strategy for judging the correctness of the initial pose is proposed first. Aiming at the phenomenon of error increase in registration between the whole model point cloud and the local scanning point cloud in pose tracking, the template matching iterative closest point (TM-ICP) method is proposed which matches the scanning point cloud and the template point cloud generated by the pose of last frame using the ICP algorithm. And the effectiveness of the proposed method is verified by extensive numerical and field experiments.
The rest of this paper is organized as follows. In the ''Related work'' section, the related work in recent years is described in detail. In the ''ROI-based algorithm for relative pose determination'' section, the details of proposed relative pose estimation method are presented. The ''Pose tracking'' section describes the pose tracking stage. Finally, numerical and field experiments and conclusions are shown in the ''Experiments'' and ''Conclusion'' sections, respectively.

Related work
Uncooperative targets are usually divided into two categories. One has known geometrical information about their shape and size at least, and the other is fully unknown. In the following step, we provide a brief overview of the relative pose determination for the model-known uncooperative target, which exists in large numbers in space. The process of pose estimation for the model-known uncooperative target is generally divided into the initial pose acquisition stage and the pose tracking stage. 8,14,15 The initial pose acquisition for the model-known uncooperative target aligns the acquired data with the target model, which is built as model point cloud, templates or complex features, corresponding to the pointbased algorithm and the feature-based algorithm. A 3D binary template matching (TM) algorithm is presented in Greenspan and Jasiobedzki. 10 It divides the 3D template points into grid elements called voxels, and quantifies the occupied voxel as a value of 1 and an empty voxel as a value of 0.
Opromolla et al. 14 build the 3D point template either off-line or directly on board by sampling the attitude parameter space with a fixed angular step and estimate the relative position by means of the centroiding approach. These algorithms take up the amount of onboard data storage or highly computational time (tens of seconds), which is not available for uncooperative spacecrafts with a high tumbling rate. To optimize computational efficiency and restrain the amount of onboard data storage, a PCA-based online TM algorithm is proposed. 16 Similar to Greenspan and Jasiobedzki, 10 the PCA algorithm is used to estimate the main axis of the target, which can resolve two rotational components of the target pose around the other two axes. The remaining unresolved rotation around the target main axis finds the best match by generating templates online. The performance of these techniques has been assessed by means of numerical simulations.
The methods proposed by Ruel et al. 15,17 are to store the pairs of points and their associated topological information to trade space for time. The corresponding relationship between the model and the scanning point is found by looking for corresponding polygons or congruent tetrahedron composed of pairs of points, and then the initial pose parameters are estimated. Such methods attributed to the fast lookup capabilities of the hash table are available in real time.
In addition, there are some feature-based approaches that store features extracted from the model off-line, for example, the 3D semi-global descriptor OUR-CVFH (Oriented, Unique, and Repeatable Clustered Viewpoint Feature Histograms). 18 Based on this feature, Woods and colleagues 11,19 presented an OUR-CVFH algorithm for the initial pose estimation of uncooperative targets, and the high correctness and the low time complexity of the algorithm are verified by simulation experiments. However, the accuracy and stability of feature extraction from point cloud are usually sensitive to noise.
Without storing features extracted from the model, templates or other topological information of points, the Globally Optimal ICP (Go-ICP) algorithm 20 only needs to store the model point cloud directly. It is the first globally optimal solution to Euclidean registration of two 3D points that is applied to the initial pose estimation of uncooperative targets. While the Go-ICP method takes a long time (tens of seconds at 500 points) to reduce the translation domain, Liu et al. 21 use PCA to calculate the axis-aligned bounding box (AABB) to obtain the new domain and then use the Go-ICP algorithm to search in the 3D space.
Man-made targets often have obvious geometric shapes, such as cylinders, cones, and rectangles. Martı´nez et al. 22 detect the cylinder body of the uncooperative rocket and project the point cloud onto the axis of symmetry to obtain the barycenter. The initial attitude is obtained by the relative position among the detected nozzle, side tanks, and fairing of target. Inspired by this, and taking into account the structural characteristics of satellites, this paper proposes a realtime method based on the ROI for some man-made spacecraft with large plane features, which only needs to store the model point cloud, and has higher success rate (SR), accuracy and noise resistance.
After acquiring the initial pose, it will be necessary to further refine the pose by applying the iterative closest point (ICP) algorithm which refines the pose in a continuous parameter space by minimizing an error metric iteratively [23][24][25] or bounded Hough transform (BHT) 26,27 operating in the discrete domain and effectively trades off reduced precision for increased efficiency. For the ICP method in the pose tracking stage, the error will gradually increase-one of the reasons for this is the wrong correspondence between the model point cloud and the local scanning point cloud. In this paper, the TM-ICP method combining the TM and ICP algorithm is presented to solve this problem effectively.
The proposed method for initial pose acquisition in this paper is a point-based method too. And the relative pose is estimated by aligning the ROI extracted from the model point cloud and the scanning LiDAR point cloud. The strategy combined with ICP and TM is adopted to track pose.

ROI-based algorithm for relative pose determination
The flow of proposed method for determining the relative pose between chaser and target is illustrated in Figure 1. Following the general pipeline, we also divide the process of pose determination into two steps: initial pose acquisition and pose tracking. In the initial pose acquisition step, a ROI-based algorithm was investigated. An algorithm combined with ICP and TM is used in pose tracking step for reliability and accuracy.
First, the coordinate system of the chaser spacecraft and the target spacecraft should be introduced as shown in Figure 2. O c À X c Y c Z c is the chaser bodyfixed reference frame while O s À X s Y s Z s is the sensor reference frame, and the LiDAR sensor is equipped on the chaser. O t À X t Y t Z t is the target reference frame. The objective is to determine a transformation T ct (Equation (1)) which represents the relative rotation matrix R ct , which aligns the target reference frame to the chaser reference frame, and the translation vector t ct , which is expressed in the chaser reference frame. The transformation T cs is constant because of the fixed assembly between the chaser and the sensor. Therefore, only the relative transformation T st between the sensor and the target is required

ROI extracting
For most spacecrafts, 28 there are large-scale solar arrays compared to the body size as shown in Figure 3; the length of solar arrays is significantly larger than d bs (the maximum distance between the point on the body of spacecraft (not on the solar arrays) and the point on the solar arrays), which is the promise of the ROI extraction in this paper. The ROI in this paper mainly refers to the points on the plane where the solar array is located. The initial pose is obtained by aligning the ROI of the scanning point cloud (SROI) with the ROI of the model point cloud (MROI). Since the geometric model of the uncooperative spacecraft is known, the plane ROI can be directly determined based on the distance between the scanning points. The process of extracting the ROI is shown in Algorithm 1.
The plane parameters can be calculated by three non-collinear scanning points p 1 , p 2 and p 3 . To find these three points faster, a 3D convex hull 29 of  scanning point cloud S is constructed, and the searching process is performed in the vertices of the convex hull. The greatest distance between the point pairs of convex hull vertices is L max = p 1 À p 2 k k. If the distance L max is greater than the threshold th 1 (taking into account the measurement errors of scanning points, the distance threshold is set to th 1 and th 1 . d bs ), it is can be considered that the two points p 1 and p 2 lie on the plane defined by the solar arrays. Then, taking p 1 as the endpoint and looking for the point p 3 in the vertices of the convex hull after excluding p 2 , where p 1 À p 3 k k. th 1 , p 1 , p 2 and p 3 are non-collinear. From these three points p 1 , p 2 and p 3 , the plane of the solar arrays can be determined. Considering the influence of noise, the plane is not determined by p 1 , p 2 and p 3 directly, but by the mean values of their neighbors in the scanning point cloud within radius r, respectively. Finally, the distance from the scanning point cloud to the plane is calculated. If the distance d is less than th 2 , the point is considered to belong to SROI (Figure 4). It should be noted that, even if the point where the main body attached to the solar arrays is incorrectly assigned to the ROI, it will not significantly affect the calculation of PCA later.

Acquiring the initial pose
When the SROI is extracted, the initial pose can be acquired by the flow as shown in Figure 5.
The PCA algorithm is used for acquiring the initial pose. The covariance matrix Q S is written in Equation (2), and the relative position vector t 0 is calculated by the centroid of the ROI in Equation (3) break for. 9: end if 10: end for 11: if p 3 is found then 12: for p j , j = 1, 2, 3 do 13: finding the neighbors N j = N j k n o of p j from S i f g within the radius r. 14: p The matrix R S is composed by the eigenvectors of Q S as described in Equation (4); T are the eigenvectors corresponding to the eigenvalues l 1 , l 2 and l 3 , respectively, and l 1 . l 2 . l 3 . The matrix R M can be composed following the procedure of R S . Then, the relative attitude R 0 rotating from the target reference frame to the sensor reference frame can be calculated by the Equation (5)-both R S and R M are orthonormal The PCA allows estimation of the three main axes targets without ambiguity, but their directions remain undetermined. Thus, four possible solutions were generated. That is to say, there are four possibilities for R M corresponding to R S . If the Y, Z and X axes of the model are the first, second and third main directions, The four candidate rotation matrices R 01 , R 02 , R 03 and R 04 are determined by R S and R M1 , R M2 , R M3 and R M4 according to Equation (4), respectively. To validate the right rotation matrix from these four rotation matrices, the ICP algorithm is used. The four rotation matrices, along with t 0 , are taken as the initial transformation matrices of the ICP algorithm to align the model point cloud and the scanning point cloud. The transformation matrix with the minimum score f (Equation (7)) of ICP is selected as the correct initial transformation matrix where M i is the point of the model point cloud and S i is the point of the scanning point cloud.

Pose tracking
The correctness of the initial pose estimation directly affects the effectiveness of the pose tracking. Therefore, the correctness of the initial pose should be judged before entering the pose tracking phase. And the procedure is described in Algorithm 2. The poses T k st and T kÀ1 st of the point clouds of two adjacent frames are calculated by the ROI-based method, respectively. The relative pose of the two frames calculated by the two initial poses is DT k, kÀ1 st ( = T k st (T kÀ1 st ) À1 ). On the contrary, the ICP algorithm is used to obtain the  st are considered to be correct. The Euler angles with a 231 sequence, that is, db, dg and da, are used to measure the relative pose DT k st , and b 0 , l 0 and a 0 are the threshold. If da \ a 0 , db \ b 0 , and dg \ g 0 , it is considered DT k, kÀ1 st 'DT k . Since there is an error in the initial pose estimation, if the thresholds are set too small, even if the initial pose estimation is correct, it may always be judged to fail, resulting in the inability to enter the pose tracking normally. If the thresholds are set too large, it may lead to false positive and enter the pose tracking under the wrong initial pose, leading to the pose tracking failure. Therefore, the setting of the thresholds needs to be adjusted according to the accuracy of the initial pose estimation algorithm.
The general ICP-based pose tracking process is shown in Figure 6(a). However, due to the influence of the initial pose and the registration of the local point cloud and the overall model point cloud, the error will gradually increase during the tracking process (although the error will decrease later). Generally, the registration between adjacent frames is relatively accurate and stable, and is inspired by the TM algorithm which is used for initial pose acquisition; 16 an improved tracking strategy named TM-ICP is presented as shown in Figure 6(b). Different from the general ICP-based pose tracking process to register the scanning point cloud with the whole model point cloud, the ideal local point cloud (template) is generated by the pose of the last frame 14 and is used to register with the current scanning point cloud to obtain the pose. The template provides a local point cloud, which has a large overlap with the current scanning point cloud, and provides a truth pose, which can make the ICP have high precision and stability. Pseudo code of TM-ICP method is shown in Algorithm 2 (TM-ICP tracking).

Experiments
In this section, numerical simulation experiments and field experiments have been performed to test the 3: Using the ROI-based algorithm to calculate the transformation T k st from M to S k . 4: Using ICP algorithm to estimate the relative pose DT k between S kÀ1 and S k .
6: Computing the Euler angles db, dg, and da from DT k st . 7: if (da \ a 0 ) & (db \ b 0 ) & (dg \ g 0 ) (b 0 , l 0 , and a 0 are the thresholds) then 8: k = k + 1; go to step 13. 9: else 10: T kÀ1 st = T k st ; k = k + 1; go to step 3. 11: end if 12: TM-ICP tracking: 13: Generating the template TM kÀ1 using the relative pose T kÀ1 st . 14: Using ICP algorithm to estimate the relative pose DT k between TM kÀ1 and S k . 15: T k st = DT k T kÀ1 st . 16: Calculating the 6-DOF relative pose parameters from T k st . 17: k = k + 1, and go to step 13. 18: end while performance of the proposed algorithm. Figure 7 shows the uncooperative target models used in the experiments and the Lissajous scan pattern used by the sensor, which generates sparse point cloud with an uneven density.
The size of the simulation model (Figure 7(a)) is 1521 3 559 3 870 mm, and the size of the simplified mock-up (Figure 7(b)) is 2350 3 660 3 1016 mm.
A commercial desktop PC equipped with an Intel ä I5 CPU at 2.4 GHz is used to obtain the experimental results and the proposed algorithm is implemented in C++ .

Simulation experiments
ROI-based algorithm test. It is difficult to quantify the performance of the ROI-based algorithm of large pose variations in laboratory tests; thus, a simulation environment which had been described in Yin et al., 15 including simulated sensor point clouds and providing the true relative pose between the uncooperative space target and the sensor, is used. The performance of the initial attitude estimation algorithm is tested in terms of both computational cost and pose estimation performance first, and then, the effectiveness of the pose tracking strategy is tested.
All  , g and a).
Since we cannot exhaust all the attitudes in the rotation space, we sample at intervals of 10°in each rotation region of the three rotation axes, which means that there are 26,011 sets of point cloud data in each group. And the number of scanning points is below 500. The anti-noise performance of the proposed method should be test too. Therefore, six groups' simulation experiments were designed in different noise conditions as shown in Table 1. The noise range [a, b] represents the range of random noise along each axis direction.
SR (Equation (8)) and loss rate (LR, Equation (9)) are defined to discriminate whether the algorithm is successful or not. A 5°angular error threshold is used to consider whether the result of pose estimation succeeds or not The SR and LR in different noise are shown in Figure 8.
From Figure 8, it can be seen that the proposed algorithm has a high SR, even with a maximum random noise of 150 mm. The SR is more than 90% and the LR is less than 3%. As the noise increases, the SR decreases slightly, but within the noise range tested here, SR and LR are high enough for pose acquisition.
The average values of the absolute estimation error are collected in Figure 9. In the maximum noise 200 mm tested in this paper, the position error is less than 200 mm and the attitude error is less than 4°. Although the position error and the attitude error increase as the noise increases, the accuracy is still high enough for pose acquisition.
The computational time of the proposed algorithm is collected in Table 2. It can be seen that under different noises, there is no significant difference in computational time and the algorithm is faster than 5 Hz, which  is fast enough for the pose tracking step of tumbling target with a high tumbling angular velocity.
Pose tracking test. As the spinning uncooperative spacecraft usually rotates around the maximum or minimum inertia axis, the two cases of rotating around the Y axis and Z axis (Figure 7(a)) were tested. And the TM-ICP algorithm and ICP algorithm for tracking are compared. Since the sampling frequency of LiDAR is generally higher, the distortion of the scanning point cloud is not considered in the rotation process of the target. In test 1, the initial attitude (a, b, g) is set as (85°, 0°, 90°). The target rotates around the Z axis of the model and is sampled at 5°interval. So the number of the simulated sensor point cloud data is 73. The Gaussian white noise with a standard deviation of 10 mm is added to X, Y and Z direction, and a Monte-Carlo simulation with 100 independent runs is performed.
The average rotation error and translation error of test 1 are given in Figures 10 and 11. The shading in the figures represents the boundary of the standard deviation of the error.
From Figures 10 and 11, it can be found that the ROI-based algorithm can provide an accurate initial attitude and a good initial position, and the TM-ICP pose tracking method has higher accuracy and stability than ICP algorithm. The rotation error is less than 1°a nd the position error is less than 10 mm.
In test 2, the initial attitude (a, b, g) is set as (0°, 80°, 10°). The target rotates around the Y axis of the model and is sampled at 5°interval. So the number of the   simulated sensor point cloud data is 73. The Gaussian white noise with a standard deviation of 10 mm is added to X, Y and Z direction, and a Monte-Carlo simulation with 100 independent runs is performed. The rotation error and translation error of test 2 are given in Figures  12 and 13. The shading in the figures represents the boundary of the standard deviation of the error. From Figures 12 and 13, it can be seen that the ROIbased algorithm can still provide an accurate initial attitude and a good initial position, and the TM-ICP pose tracking method has higher accuracy and stability than ICP algorithm. When the target rotates with its maximum inertia axis, it has the approximate precision and stability when rotating around the minimum inertia axis.

Field experiments
The field experiment system shown in Figure 14 consists of a simplified mock-up which is mounted on a three-axis turntable with a precision of 0.01°, a singleline scanning 3D-LiDAR system which is developed by   BICE and installed on the manipulator arm, and a airbearing guideway which floats on a near-perfectly flat floor, providing translation motion and rotation.
The target is still, and the LiDAR rotates around the target with the air-bearing guideway at a distance of 7 m from the target, and rotation speed is 2°/s. The frequency of point cloud data acquisition is set to 1 Hz, and the experimental results are shown in Figures 15  and 16.
From the Figures 15 and 16, it can be found that ROI-based method can provide the accurate enough initial pose for the next pose tracking step. In the pose tracking step, the absolute error of the attitudes is about 5°and the absolute error of position is less than 150 mm. In addition, it should be noted that, around frame 50 and frame 159, the angle b rotating on Y axis becomes larger. By looking at the point cloud (as shown in the Figure 17 in different views), it can be found that only one plane on the side of the target model can be scanned and the measurement error of the LiDAR system is large and complex, which leads to the increase of the pose estimation error, but the error of the subsequent frame converges with the increase of the features scanned later.