An Uncertainty Weighted Non-Cooperative Target Pose Estimation Algorithm, Based on Intersecting Vectors

Abstract

Aiming at the relative pose estimation of non-cooperative targets in space traffic management tasks, a two-step pose estimation method, based on spatially intersecting straight lines, is proposed, which mainly includes three aspects: (1) Use binocular vision to reconstruct the straight space line, and based on the direction vector of the straight line and the intersection of the straight line, solve the pose of the measured target in the measurement coordinate system, and obtain the initial value of the pose estimation. (2) Analyze the uncertainty of the spatial straight-line imaging, construct the uncertainty description matrix of the line, and filter the line features, accordingly. (3) Analyze the problems existing in the current linear distance measurement, construct the spatial linear back-projection error in the parametric coordinate space, and use the linear imaging uncertainty to weigh the projection error term to establish the optimization objective function of the pose estimation. Finally, the nonlinear optimization algorithm is used to iteratively solve the above optimization problem, to obtain high-precision pose estimation results. The experimental results show that the two-step pose estimation algorithm, proposed in this paper, can effectively achieve a high-precision and robust pose estimation for non-cooperative spatial targets. When the measurement distance is 10 m, the position accuracy can reach 10 mm, and the attitude measurement accuracy can reach 1°, which meets the pose estimation accuracy requirements of space traffic management.

1. Introduction

The relative pose estimation is one of the basic technologies of space on-orbit operations and traffic management tasks [1,2,3]. For the estimation of the relative pose between spacecrafts, the ground telemetry method cannot obtain the high-precision 6-DoF (six-degree-of-freedom) parameters [4]. In recent years, pose estimation methods, based on LiDAR (light detection and ranging) and vision, have been favored by researchers [5]. LiDAR is not restricted by light conditions. However, LiDAR is an active sensor, the data it collects is sparse, and the power consumption and quality are large. Therefore, it is difficult to obtain high-precision measurement results in tasks, such as space traffic management. Compared with the LiDAR-based method, the vision-based method has the advantages of a low price, low power consumption, lightweight, and full-field measurements. Therefore, the vision-based method is widely used in aerospace missions [6].

For the relative pose estimation of non-cooperative spacecrafts, based on vision, there are currently mainly model-based methods, point-based methods, line-based methods, and deep learning-based methods [7]. The method, based on the model, is referred to the use of the camera images and the feature information of the observed building geometry model (such as shape, and size), and then with the known model is combined with the corresponding information for the reference estimation algorithm for the pose parameters, but this kind of method needs to know the geometry prior to the information of the target [8,9]. Therefore, its application scope is greatly limited. Based on the deep learning method, the use of the offline training network model for the non-cooperation target pose estimation, and then the images captured by the camera in real-time are processed online to achieve the relative pose estimation [10,11,12]. This method has a good robustness and is not sensitive to light, but it needs a lot of offline data to train the estimation network. However, this process of the pose estimation of non-cooperative targets is not easy. A method, based on the point feature, refers to extracting the feature points of the non-cooperative target, and then, based on the known geometry information, or by using stereo vision ways to realize the feature points of the 3D (three dimensional) reconstruction, adopt the corresponding pose estimation algorithm for the pose estimation [13,14,15]. The algorithm is simple to implement, therefore the pose estimation, based on the cooperative target, is widely used. However, this method is easy to be affected by illumination conditions, and the algorithm’s robustness is poor. The method, based on the online feature, uses the information of the line and the conic section in the collected image, combined with the stereo vision to realize the estimation of the pose parameters. Compared with the feature point method, this method has a stronger robustness, so it has received extensive attention in recent years. Pose estimation methods, based on linear features, can be divided into a linear method and an iterative method. The linear solving algorithm is to directly use the known 3D model features or obtain a 3D model through stereo vision, and then solve the PnL (Perspective-n-Line) problems, such as the PnP (Perspective-n-Point) [16,17,18]. The iterative method usually uses the linear method to obtain the initial value and then uses the iterative algorithm to minimize the objective function to obtain the optimal pose estimation results [19,20]. The pose estimation method, based on the conic reconstruction, mainly uses the spatial analytic geometry theory and the perspective projection imaging model to solve the characteristic parameters of the spatial conic model in the closed form and obtain the pose estimation results [21]. In addition, the linear-based motion estimation algorithms are often used in tasks, such as in the robot estimation and aircraft ground simulation tests [22,23]. The main contributions of our work are as follows:

(1)

We propose a two-step relative pose estimation algorithm, based on the uncertainty analysis of the line extraction. The method fully considers the uncertainty of the straight-line extraction to weigh the error term of the objective function, and is more robust in a space environment than the feature-point-based method.

(2)

A novel distance error model for the reprojection of a line, in which the mean value of the distance sum from the two ends of the original direct comparison line segment to the line error, is transformed into the sum of the distance between the two points in the parameter space, which is more reasonable, in theory.

(3)

The proposed approach was verified on a ground test simulation environment and has been extensively evaluated in experimental and simulation analyses.

The rest of the paper is organized as follows: In Section 2, a new pose estimation method for non-cooperative targets, based on the line features, is presented, that extracts the intersection vectors and the intersection of the straight line from the target images. Then, the uncertainty of the straight-line extraction and the back-projection error model are analyzed, and the uncertainty model and straight-line error model are established in Section 3. Section 4 comments on the experimental results and Section 5 concludes the paper.

2. Principle of the Non-Cooperative Target Relative to the Pose Estimate

2.1. Problem Description of the Relative Pose Estimation

As shown in Figure 1, let the coordinate system of the measured target be $O_{t}xyz$, The coordinate system of the binocular vision measurement unit is $O_{b}xyz$. In this paper, it is assumed that it coincides with the left camera, $L_j$ represents the line feature on the target, $j=1,\cdots ,N_f$, $N_f$ is the number of lines, $l_{ij}^l$ and $l_{ij}^r$ represent the imaging of the straight line $L_i$ on the left and right cameras, and the left and right camera measurements are independent of each other, $i$ represents the moment when the camera collects data. The internal and external calibration parameters of the binocular stereo vision measurement unit are $\left[A^l,A^r\right]$ and $\left[R_{lr},T_{rl}\right]$, respectively, and the calibration has been completed. Then, the relative pose estimation problem, based on the straight-line features, can be described as the following maximum a posteriori probability estimation problem:

Figure 1. Schematic diagram of the relative pose estimation of a non-cooperative spacecraft.

How to estimate the rigid body transformation relationship $x_i$ and the spatial straight line $L_j$ between the coordinate system of the measured object and the coordinate system of the binocular measurement unit, according to the linear imaging $l_i^l$ and $l_i^r$ of the spatial straight line $L_j$ on the measured object in the left and right cameras:

$$\begin{array}{cc}\overline{X}\hfill & =\arg \max P\left(x_{1:i}|z_{1:i}\right)\hfill \\ \hfill & =\arg \max {\displaystyle \prod _{i=1}^{N}P\left(L_j,R_{bt}^i,T_{tb}^i|L_j,R_{bt}^{i-1},T_{tb}^{i-1}\right)P\left(l_{ij}^l,l_{ij}^r|L_j,R_{bt}^{i-1},T_{tb}^{i-1}\right)}\hfill \end{array},$$

(1)

where $x_i$ represents the relative pose $\left[R_{bt}^i,T_{tb}^i\right]$ and the spatial line $L_j$ of the target at different times, $z_i$ represents the spatial line feature and its imaging $l_i^l$ and $l_i^r$ in the camera, $\overline{X}$ represents the maximum posterior probability estimate of the relative pose.

2.2. Relative Pose Estimation between Spacecrafts

In order to solve the above-mentioned relative pose estimation problem, this paper proposes a relative pose estimation algorithm, which mainly includes two parts: (1) Solve the initial value of the relative pose estimation; (2) Iterative optimization of the relative pose estimation. The above two parts will be described in detail below.

2.2.1. Initial Value Solution of the Relative Pose Estimation, based on the Intersection Vector

Theorem 1.

If it is known that there are $n\ge 2$ intersecting lines $L_i$ in the 3D space and their imaging $l_i^l$ and $l_i^r$ in the left and right views, and the internal and external parameters $[A_l,A_r,R_{lr},t_{rl}]$ of the binocular stereo vision have been calibrated, then the transformation matrix $C_{tc}=[R_{tc}|t_{ct}]$ between the visual measurement unit coordinate frame, relative to the target coordinate frame can be solved [23].

Proof of Theorem 1.

As shown in Figure 2, let the left camera coordinate system be the visual measurement unit coordinate system, if the spatial line set $N$ of the target has been solved, then a line $L_i$ whose direction vector $n_x^c$ is arbitrarily selected as the x-axis of the target coordinate frame, and then take any non-parallel line $L_j$, whose direction vector is $n_s^c$, and take a cross product with $n_x^c$ to obtain the y-axis direction vector of the target coordinate frame, as follows:

$$n_y^c=n_x^c\times n_s^c,$$

(2)

Figure 2. Schematic diagram of the spatial straight-line perspective projection imaging.

The z-axis direction vector of the target coordinate frame is obtained by the cross-product of the vectors:

$$n_z^c=n_x^c\times n_y^c,$$

(3)

Therefore, we can construct a new matrix:

$$D_c=\left(n_x^c,n_y^c,n_z^c\right),D_t=\left(n_x^t,n_y^t,n_z^t\right)=I,$$

(4)

where $D_c$ is the direction vector of the linear features in the camera coordinate system, $D_t$ is constructed as the direction vector of the coordinate axis of the camera coordinate system represented in the target coordinate system, and according to the construction principle of the target coordinate system, it is set as the identity matrix $I$. If it is assumed that $R_{tc}$ represents the rotation transformation relationship between the measurement unit coordinate system and the target coordinate system, then according to the definition of the transition relationship between the coordinate systems, we can write:

$$R_{tc}=D_t{D}_c^{-1},$$

(5)

For the solution of the displacement vector $t_{ct0}$, it can be obtained by the coordinate $O_t$ and $O_c$ of the straight line intersection in the target coordinate frame and the left camera coordinate frame (it is set to the measurement coordinate system), respectively:

$$t_{ct0}=O_t-R_{tc}{O}_c,$$

(6)

If the straight-line intersection in the target coordinate frame is taken as the origin of the target coordinate system, the above equation can be expressed as:

$$t_{ct0}=-R_{tc}{O}_c,$$

(7)

□

2.2.2. Nonlinear Optimization of the Pose Estimation

The initial relative pose relationship between the ontology and the target was obtained by solving the problem in the previous section. Ideally, if you were to project the direction vector of the line from the target coordinate system back to the camera coordinate system, they would be equal, but because of the error, they would not be equal. Therefore, for the attitude estimation, Equation (8) is adopted in this paper for a nonlinear optimization solution:

$$E\left(R_{lt}\right)=\underset{R_{lt}}{\arg \min }{\displaystyle \sum _{i=1}^M{\displaystyle \sum _{j\in (l,r)}{\Vert n_i^j-R_{jt}^{}•n_i^L\Vert }_{{\Sigma }_d}^2}},s.t.R_{rl}•R_{lt}=R_{rt},$$

(8)

where $R_{rl}$ represents the binocular vision external rotation parameter, $R_{jt}$ represents the rotation transformation matrix between the camera coordinate system and the target coordinate system, and $n_i^j$ represents the representation of the spatial straight line direction vector in the left and right camera coordinate system, $n_i^L$ represents the direction vector of a 3D straight line in the target frame, ${\Sigma }_d$ for the orientation uncertainty.

For the displacement $t_{lt0}$, firstly, the rotation transformation matrix $R_{tl}$ is obtained by the optimization solution Equation (8), and the initial value $t_{lt0}$ is obtained through the initial value solution method mentioned above. Then, the spatial straight lines are back-projected into the imaging plane, and the bundle adjustment algorithm is used to solve Equation (9) to obtain the final most pose estimation:

$$E\left(R_{lt},t_{tl}\right)=\arg \underset{R_{lt},t_{tl}}{\min }{\displaystyle \sum _{i=1}^M{\displaystyle \sum _{j\in (l,r)}{\Vert \left(K_j^{-1}[R_{lt}^j|t_{tl0}^j]L_i^{}\right)\oplus n_i^j\Vert }_{{\Sigma }_l}^2}},$$

(9)

wherein $M$ represents the number of straight lines in the 3D space, $L_i$ represents a spatial line on a non-cooperative target, $n_i^j$ represents the direction vector of the projected line of the 3D space straight line in the left and right cameras, $K_j$ represents the internal parameter matrix of the left and right cameras, ${\Sigma }_l$ represents the linear position uncertainty. The LM (Levenberg–Marquardt) optimization algorithm is used to solve the above two nonlinear optimization problems.

3. Line Features Reconstruction and the Uncertainty Analysis

3.1. Solving the 3D Space Straight Line and Its Intersection

According to the pose estimation algorithm proposed above, to realize the pose estimation of the non-cooperative target, the 3D space straight line equation needs to be solved, and the weighted uncertainty of the nonlinear optimization objective function of the pose estimation is determined. Therefore, this section first uses binocular vision to reconstruct the 3D space straight line, and then analyzes the uncertainty of the direction and position of the line imaging, and constructs its uncertainty description matrix.

As shown in Figure 2, it is assumed that the intrinsic parameters $K^i$ of the two cameras and the external parameters $[R_{rl},t_{rl}]$ between the two cameras have been calibrated. The images of a 3D space line in two views can be expressed as:

$$\{\begin{array}{l}{l}_l:a_{1}u+b_{1}v+c_1=0\hfill \\ l_r:a_{2}u+b_{2}v+c_2=0\hfill \end{array},$$

(10)

The two analytic planes ${\pi }_i^l$ and ${\pi }_i^r$ passing through the spatial straight line can be expressed as:

$$\{\begin{array}{l}{\pi }_i^l:A_{1}x+B_{1}y+C_{1}z+D_1=0\hfill \\ {\pi }_i^r:A_{2}x+B_{2}y+C_{2}z+D_2=0\hfill \end{array},$$

(11)

If the left camera coordinate frame is assumed to be the reference coordinate frame, then in the reference coordinate frame, a 3D space straight line can be expressed as:

$$\{\begin{array}{l}{u}^j=\frac{m_{11}^{j}x+m_{12}^{j}y+m_{13}^{j}z+m_{14}^j}{m_{13}^{j}x+m_{23}^{j}y+m_{33}^{j}z+m_{34}^j}\hfill \\ v^j=\frac{m_{21}^{j}x+m_{22}^{j}y+m_{23}^{j}z+m_{14}^j}{m_{13}^{j}x+m_{23}^{j}y+m_{33}^{j}z+m_{34}^j}\hfill \end{array},$$

(12)

where $j\in \left(l,r\right)$, the projection matrix can be expressed as:

$$P^j=\left(\begin{array}{cccc}{m}_{11}& m_{12}& m_{13}& m_{14}\\ m_{21}& m_{22}& m_{23}& m_{24}\\ m_{31}& m_{32}& m_{33}& m_{34}\end{array}\right),$$

(13)

When $j=r$, $P^r=C_{lr}{M}^r=\left[R_{lr},t_{lr}\right]M^r$. According to Equations (10)–(13), we can obtain:

$$\{\begin{array}{l}{A}_i=a_i{m}_{11}+b_i{m}_{31}-m_{21}\hfill \\ B_i=a_i{m}_{12}+b_i{m}_{32}-m_{22}\hfill \\ C_i=a_i{m}_{13}+b_i{m}_{34}-m_{23}\hfill \\ D_i=a_i{m}_{14}+b_i{m}_{34}-m_{24}\hfill \end{array},$$

(14)

wherein $\left(a_i,b_i\right)$ represents the coordinates of the points on the straight-line image. The direction vector of a straight line in the 3D space can be obtained by directly solving the following equations:

$$v=n_l^i\times n_r^i=\left(\left|\begin{array}{cc}{B}_1& C_1\\ B_2& C_2\end{array}\right|,\left|\begin{array}{cc}{C}_1& A_1\\ C_2& A_2\end{array}\right|,\left|\begin{array}{cc}{A}_1& B_1\\ A_2& B_2\end{array}\right|\right),$$

(15)

Similarly, the solution can be obtained: $[v_1,\cdots ,v_N]$. The intersection of the straight lines can be obtained by solving the above-mentioned 3D space straight lines (the straight lines of the different planes are the midpoints of the mid-perpendicular lines). For a detailed solution, please refer to [24].

3.2. Uncertainty Analysis of the Straight Line Extraction

As shown in Figure 3, the uncertainty of the feature extraction of a straight line can be described as the angle uncertainty $\Delta \theta$ and radius uncertainty $\Delta \rho$ of the feature extraction of a straight line.

Figure 3. Uncertainty analysis of the straight lines.

3.2.1. Uncertainty Modeling of the Straight Line

If the size of the uncertainty region in the parameter space is assumed to be: $\theta \in \left({\theta }_0-\Delta {\theta }_1,{\theta }_0+\Delta {\theta }_2\right)$, then according to the cosine law, we know that the uncertainty $\Delta \theta$ can be expressed as:

$$\Delta \theta =arc\cos \frac{l_1{l}_2}{\left|l_1\right|\left|l_2\right|},$$

(16)

where $l_1=\left(1,k_1\right)$, $l_2=\left(1,k_2\right)$, $k_1$ and $k_2$ can be obtained by solving the inner tangent of the two ellipses, and the following equation can be obtained after sorting:

$$\{\begin{array}{c}{\left(B_{1}b+2C_{1}kb+D_1+E_{1}k\right)}^2-4\left(A_1+B_{1}k+C_1{k}^2\right)\left(C_1{b}^2+E_{1}b+F_1\right)=0\\ {\left(B_{2}b+2C_{2}kb+D_2+E_{2}k\right)}^2-4\left(A_2+B_{2}k+C_2{k}^2\right)\left(C_2{b}^2+E_{2}b+F_2\right)=0\end{array},$$

(17)

In this formula, $A_i$, $B_i$, $C_i$, $D_i$, $E_i$ and $F_i$ are, respectively, the coefficients of the general equation of the ellipse with the positioning uncertainty of the two feature points on the line, which can be obtained from the positioning uncertainty of the points on the straight line as ellipse ${\Lambda }_e$ [25]. Therefore, the direction uncertainty ${\sum }_d$ of the straight line can be expressed as:

$${\sum }_d=\left[\begin{array}{ccc}\cos \Delta \theta & 0& 0\\ 0& \sin \Delta \theta & 0\\ 0& 0& 1\end{array}\right],$$

(18)

The position uncertainty of the straight line can define as $\rho \in \left({\rho }_{\min },{\rho }_{\max }\right)$, wherein $\Delta \rho ={\rho }_{\max }-{\rho }_{\min }$, then, according to the distance formula from the point to the line, we can get the position uncertainty of the straight line as:

$$\Delta \rho =\frac{\left|b_1-b_2\right|}{\sqrt{k^2+1}},$$

(19)

wherein $b_1$ and $b_2$ can be obtained by solving the outer tangents of the two ellipses in Equation (17).

Therefore, the uncertainty model ${\Sigma }_l$ of the linear projection in the 3D space can be modeled as a combination of the orientation uncertainty and the position uncertainty:

$${\Sigma }_l=\left[\begin{array}{ccc}\cos \Delta \theta & 0& 0\\ 0& \sin \Delta \theta & 0\\ 0& 0& \Delta \rho \end{array}\right],$$

(20)

3.2.2. Linear Filtering based on the Uncertainty Size

Assuming that the uncertainty covariance matrix of the 3D reconstruction of a spatial straight line is ${\Lambda }_L$. The imaging of a 3D space straight line in each camera can be defined as $l=\left(l_l^T,l_r^T\right)$, and because they are independent of each other, the covariance matrix of the spatial line can be represented as:

$${\Lambda }_l=diag\left({\Lambda }_{l_l},{\Lambda }_{l_r}\right),$$

(21)

wherein ${\Lambda }_{l_j}=diag\left(\cos {\theta }_j,\sin {\theta }_j\right)$, $j\in \left(l,r\right)$. Moreover, according to Equations (11) and (12), it can be known that the 3D space straight line $L=f\left(l\right)$. Therefore, the covariance matrix of a straight line solution in the 3D space can be expressed as:

$${\Lambda }_L=J{\Lambda }_l{J}^T=\frac{\partial f\left(l\right)}{\partial l}{\Lambda }_l\frac{\partial f{\left(l\right)}^T}{\partial l},$$

(22)

wherein $\partial f\left(l\right)/\partial l=\left[\partial f\left(l\right)/\partial l_l,\partial f\left(l\right)/\partial l_r\right]$, $\partial f\left(l\right)/\partial l_i=\left[\partial f\left(l\right)/\partial u_i,\partial f\left(l\right)/\partial v_i\right]$, $i=l,r$.

According to Equation (22), the uncertainty covariance matrix ${\Lambda }_L$ of the features of each straight line is calculated, and then the determinant size of the uncertainty covariance matrix is solved to exclude the feature straight lines with large uncertainties, so as to determine the lines participating in pose estimation and to improve the accuracy of the non-cooperative target pose estimation, based on the line.

3.3. Line Reprojection Error Analysis

The current reprojection error of the line is generally defined as the average distance between the two endpoints of the line segment and the line. As shown in Figure 4, this definition has the following problems:

Figure 4. Schematic diagram of the straight-line distance error analysis.

(1)

When the detection line segment and the reprojection line are in the situation, as shown in (a) and (b), the error means the two lines are completely inconsistent with the length being equal.

(2)

When the detection line segment and the directional projection line are in the situation, as shown in (c) and (d), its position on the line directly affects the error when the direction vector of the line remains unchanged.

Intuitively, the difference between the two lines can be more accurately described by directly solving the “distance” between them. To solve the above problems, this paper transforms a straight line into a parametric space and defines the reprojection error of the straight line by solving the distance between the two points in the parametric space Cartesian coordinate system.

As shown in Figure 5, the line is in the parametric space Cartesian coordinate system:

$$\mathrm{b}=-x\mathrm{a}+y,$$

(23)

Figure 5. Schematic diagram of the linear distance representation in the different coordinate systems.

Using Cartesian coordinates to represent a line, when the line is a vertical line or nearly vertical line, the slope of the line is infinite or nearly infinite, and thus cannot be represented in the parameter space a-b. To solve this problem, we can use the statement in polar coordinates:

$$\rho =x\cos \theta +y\sin \theta ,\rho \in \left[-\pi /2,\pi /2\right],$$

(24)

Therefore, according to the property that distance is independent of the coordinate system, the distance $e_p$ between the straight lines can be defined as:

$$e_p=\Vert p^{\prime }p\Vert =\sqrt{{\left({\theta }_{p^{\prime }}-{\theta }_p\right)}^2+{\left({\rho }_{p^{\prime }}-{\rho }_p\right)}^2},$$

(25)

Once the uncertainty model of the straight line and the direction projection error model of the straight line is obtained, they can be brought into the nonlinear optimization objective function and solved iteratively by the nonlinear optimization algorithm, to obtain an accurate non-cooperative objective pose estimation. The algorithm flow of the non-cooperative target pose estimation, considering the straight line uncertainty, can be represented, as shown in Figure 6.

Figure 6. Flow chart of the outer space non-cooperative target pose estimation algorithm.

4. Experiments and Analysis

To verify the feasibility, the measurement accuracy, and the measurement robustness of the proposed non-cooperative pose estimation algorithm, a simulation analysis and test experiments were carried out, respectively, in this paper.

4.1. Simulation and Analysis

MATLAB simulation software is used to generate the feature point data on the line by the numerical simulation, and then the binocular vision algorithm is used to process the data offline to verify the performance of the different pose estimation algorithms. The simulation model of the target movement in the experiment is shown in Table 1.

Table 1. Simulation parameter setting of the target motion.

Position/mm			Pose/°
tx	ty	tz	Yaw	Roll	Pitch
10 sin(0.2πt)	525 t	10 sin(0.2πt)	10 sin(0.2πt)	10 sin(0.2πt)	10 sin(0.2πt)

In this section, a mathematical simulation model is established, according to the measurement requirements and the characteristics of the measurement target, and the Monte Carlo method is used for the simulation. The specific process of the numerical simulation is as follows: the 3D motion of four intersecting edge lines on the rigid body in the 3D space is simulated (its size is: 500 mm × 500 mm), the fixed noise was added to the intersecting points, between the lines, and the uncertainty of the point location in the stereo vision 3D positioning system was simulated by the Gaussian white noise with the mean value of 0 and the variance of 20 mm. For occlusion, several frames of the 3D point coordinate data were eliminated in a short time, in order to simulate. The target moves in the same direction from the position 12,000 mm to 2000 mm. The outer space non-cooperative target is in the slow spin instability state, and the slow spin motion parameters of the target are as follows: the spin angular velocity range is 1~10°/s, the maximum nutation angle: 10°. Finally, the pose estimation algorithm, based on the line features proposed in this paper is solved and compared with the direct linear method [20], the feature point method [12], and the odometer method [15].

4.1.1. Analysis of the Relationship between the Measurement Accuracy and the Measurement Frequency

According to the actual research, the image acquisition frequency of general spaceborne high-resolution cameras does not exceed 10 Hz. To verify the relationship between the pose estimation algorithm and the sampling frequency of the measured data, this paper adopts the method of simulation analysis to simulate the measurement errors of the pose, when the data collection frequency is 10 Hz and 2.5 Hz, respectively, and make statistics. The results are shown in Figure 7.

Figure 7. Pose estimation accuracy at the different measurement frequencies. (a) Attitude measurement error at 10 Hz; (b) Position measurement error at 10 Hz; (c) Attitude measurement error at 2.5 Hz; (d) Position measurement error at 2.5 Hz.

Figure 7 shows the following results:

(1)

When the simulation data acquisition frequency is 10 Hz, the pose measurement results are shown in Figure 7a,b, the maximum measurement error of the attitude angle is less than 1° (3δ) and that of the position measurement is less than 2 mm (3δ), and the simulation results meet the pose measurement requirements.

(2)

When the data acquisition frequency is 2.5 Hz, the position and pose calculation results of the numerical simulation are shown in Figure 7c,d, the attitude measuring results had divergent trends in the shade, but the measurement error is bigger, this is due to the non-cooperative targets in space for the non-linear movement, and the amount of data collected is less, resulting in fewer constraints involved in the optimization using the linear model.

(3)

For the position measurement, it can be seen from Figure 7b,d that the position measurement is basically consistent with the theoretical setting value, and the position measurement results. By comparing the position measurement and attitude measurement results, the attitude measurement results are more sensitive to the 3D positioning accuracy, and the attitude measurement accuracy decreases significantly, compared with that when the sampling frequency is 10 Hz. This is mainly because the estimation hysteresis error, caused by the corresponding optimization algorithm, increases significantly with the decrease of the measurement data.

4.1.2. Algorithm Measurement Error Statistics and Analysis

To verify the advantages of the algorithm proposed in this paper, compared with the direct linear method [20], the feature point method [12], and the visual odometry method [15], the mean value of the absolute error was counted. Firstly, the parameters of the simulation analysis were set, according to Table 1, and each method was simulated 100 times to obtain the mean value and variance. The results are shown in Figure 8.

Figure 8. Statistics of the measurement accuracy of the different algorithms. (a) Attitude measurement error; (b) Position measurement error.

Figure 8 shows the following results:

(1)

By comparing the measurement results of the four methods, the accuracy of the pose estimation algorithm of the visual odometry method is the worst, reaching the maximum attitude angle of 5.8° and the maximum position error of 37 mm. This is mainly because the visual odometer method has a cumulative error, so its error will become larger and larger as time goes by.

(2)

According to the attitude error of the measurement result, the measuring accuracy of the proposed algorithm, and based on the measuring accuracy of the feature point is quite up to 0.6°, the position is 2.2 mm, but this article proposed a smaller algorithm of the attitude angle variance, this is mainly because the participation attitude angle calculating the straight linear feature uses the linear reconstruction uncertainty size selection.

(3)

In the measurement results of the attitude angle, the errors of the pitch and yaw angles are large, and the z-axis errors of the position error are large, which is mainly caused by the great uncertainty of the vision measurement in the z-axis direction.

4.1.3. Comparison Experiment of the Algorithm Robustness

To verify the influence of the image measurement noise on the relative pose estimation algorithm of the non-cooperative targets, this paper uses the method of simulation analysis for the simulation verification. We use $R_{true}$ and $t_{true}$ to express the true value, meanwhile, $R_{est}$ and $t_{est}$ to express the value of the pose estimation. The $rot\_err(\%)$ and $pos\_err(\%)$ are described as:

$$\{\begin{array}{l}pos\_err(\%)=\Vert t_{true}-t_{est}\Vert /t_{true}\\ rot\_err(\%)=\Vert R_{true}-R_{est}\Vert /\Vert R_{true}\Vert \end{array},$$

(26)

For the experimental results to be more statistically significant, we take the average of 100 times the experimental result under each parameter condition in this section.

To evaluate the influence of the pose estimation results, under the noise of different levels, we set the number of feature points on the line as 20 for the feature points method and visual odometry method with the error mean value of 0, the variance ${\sigma }_1$ is varied from 1 to 30 pixels, and set the feature line number to eight for the direct linear method and the proposed method, and eight lines are constructed by using the above feature points directly.

Figure 9 shows the following results:

Figure 9. The influence of the different image measurement noise levels on the measurement accuracy of the algorithm. (a) Attitude measurement error; (b) Position measurement error.

(1)

The measurement errors of the attitude and position points increase with the increase of the feature extraction errors, and the proposed calibration algorithm has the highest accuracy, compared with other algorithms participating in the comparison.

(2)

The algorithm proposed has a smaller error growth slope, compared with other algorithms, therefore this method has stronger robustness compared with other methods.

(3)

The position accuracy of the proposed algorithm is equivalent to that of the feature point method, mainly because it is equivalent, in theory, when the error level is the same. However, the algorithm proposed in this paper filters the straight lines involved in the pose estimation, so the positioning accuracy is slightly higher.

4.2. Actual Experiment and Analysis

To verify the pose estimation algorithm of the outer space non-cooperative target proposed in this paper, a set of binocular vision pose estimation systems is established. The system mainly includes two cameras with a resolution of 2048 × 2048 pixels and a pixel size of 0.0055 mm. A PC with a memory of 8 GB and a main frequency of 3.7 Hz; Two Zoom lens parameters are AF ZOOM-Nikon 24–85 mm/1:2.8-4D; A spacecraft with a natural size of 1000 mm × 900 mm and its motion simulator. The sampling frequency of the camera is 10 Hz; The algorithm is run in Matlab2017b.

4.2.1. Static Pose Estimation Experiment

Firstly, the binocular vision measurement system is calibrated. In this paper, the calibration algorithm, based on a planar target is adopted [26], and the calibration results are shown in Table 2.

Table 2. Calibration results of the binocular vision.

External parameter	Baseline/mm	1224.98
	rotate/°	(120.92, −14.55, −27.04)
Intrinsic parameter	radial distortion l,r	(−0.219, 0.072)	(−0.240, 0.129)
	focal l,r/mm	(10.37, 10.37)	(10.37, 10.37)
	principal point l,r/mm	(5.84, 5.47)	(5.48, 5.63)

Then, the feature point algorithm, the visual odometry algorithm, the direct linear algorithm, and the proposed algorithm are used to estimate the motion of the simulated spacecraft. The measurement results are shown in Table 3. The reference coordinate system of the pose parameters is the coordinate system of the simulated pose setting device, and the transformation relationship between it and the binocular vision measurement system is determined by using third-party calibration equipment (such as total station).

Table 3. Statistics of the static pose estimation experimental results.

	Average Error		Maximum Error
	Rotation Error (°)	Translation Error (mm)	Rotation Error (°)	Translation Error (mm)
Feature Point method	1.22	9.35	2.75	25.85
Visual Odometry method	5.51	15.15	4.85	55.38
Direct Linear method	1.30	12.12	2.58	39.25
Proposed method	1.19	9.24	2.35	18.74

As shown in Table 3, except that the average error is close to the feature point-based method, both the average error and the maximum error of the algorithm proposed in this paper are more accurate than other algorithms, which indicates that the algorithm proposed in this paper has a better stability and accuracy.

Moreover, since the distance between the adjacent frames is far in the static experiment, the odometer-based algorithm cannot obtain enough matching feature points to solve the pose in most moments, due to the large illumination variation of the target surface coating layer.

4.2.2. Dynamic Pose Estimation Experiment

To verify the dynamic test performance of the algorithm proposed in this paper, the linear motion unit is used, the relative attitude of the target remains unchanged, the target moves in a uniform phase from 12 m to 2 m, and the binocular camera is used to capture the target image, as shown in Figure 10.

Figure 10. Non-cooperative target motion acquisition images.

The algorithm proposed in this paper is used to estimate the pre-alignment pose between the camera and the target, and its error is calculated, as shown in Figure 11.

Figure 11. Non-cooperative target poses solution results.

As can be seen from Figure 11, the position measurement errors of the pose estimation algorithm proposed in this paper are all less than 20.0 mm and the attitude angle measurement errors are all less than 2.5° in the whole measurement process, indicating that the proposed algorithm fully meets the accuracy requirements of the rendezvous and docking of the non-cooperative target spacecraft.

Moreover, the actual measurement error is greater than the simulation error because the change of the illumination environment during the measurement process leads to a large error of line extraction. Meanwhile, the system pose measurement also involves the unified process of the global coordinate system, which will introduce errors into the final structure.

5. Conclusions

In this paper, binocular stereo vision is used to obtain the 3D space straight line of a non-cooperative spacecraft. The 3D reconstruction algorithms of the spatial straight lines are used to realize the 3D feature information perception of the target. Combined with the uncertainty analysis of the straight-line reconstruction, the extraction accuracy of the feature required for the pose estimation is improved, and the robustness of the algorithm is enhanced. Finally, according to the parameters of the straight line, combined with the intersection vector algorithm and the nonlinear optimization algorithm, the position and pose solution method is derived. The simulation and actual experiments show that the algorithm proposed in this paper can accurately estimate the relative pose of the spacecraft in the ultra-short range.