Vision-based relative pose determination of cooperative spacecraft in neutral buoyancy environment

Abstract

Neutral buoyancy systems simulate the microgravity environment by taking advantage of buoyancy forces of water to offset the gravity of test bodies. Functional verification of space robots in neutral buoyancy system is of great importance for ground tests. The relative pose determination of a spacecraft plays an essential role in the on-orbit operation of space robots. In order to meet the requirement of on-orbit operation ground verification for space robots, this paper develops a vision-based system for determining the relative pose of cooperative spacecraft in neutral buoyancy environment. Cooperative markers and underwater binocular vision system are designed for the pose determination, and a cooperative spacecraft model is built. A detection and recognition method based on the topological characteristic is proposed for the cooperative marker. An underwater imaging model of binocular camera is established, and its refraction parameters are calibrated. The marker points are measured with an underwater binocular 3D measurement algorithm. Furthermore, the pose of cooperative spacecraft is determined using axisymmetric plane feature points. Additionally, the stable and reliable pose and velocity are obtained after the data are further processed with a Kalman filter. Finally, the experiments are carried out and the experimental results show that the proposed system can achieve a stable and reliable high-precision relative pose determination for cooperative spacecraft in neutral buoyancy environment.

Appendix

1.1 Formula derivation of Kalman filter

The state equation and observation equation of the linear system are defined by Eqs. (12) and (13), respectively.

$$ x_{k} = A \cdot x_{k - 1} + w_{k} $$

(12)

$$ z_{k} = H \cdot x_{k} + v_{k} $$

(13)

where x_k is state vector, z_k is observation vector, A is state transition matrix, H is observation matrix, w_k is system noise vector, and v_k is observation noise vector, respectively; w_k and v_k are assumed to satisfy positive definite, symmetric and uncorrelated, zero-mean Gaussian white noise vector. Their covariance matrixes satisfy Eqs. (14), (15), and (16).

$$ E\left[ {w_{k} w_{i}^{T} } \right] = \left\{ {\begin{array}{*{20}l} {0,} \hfill & {i \ne k} \hfill \\ {Q_{k} ,} \hfill & {i = k} \hfill \\ \end{array} } \right. $$

(14)

$$ E\left[ {v_{k} v_{i}^{T} } \right] = \left\{ {\begin{array}{*{20}l} {0,} \hfill & {i \ne k} \hfill \\ {R_{k} ,} \hfill & {i = k} \hfill \\ \end{array} } \right. $$

(15)

$$ E\left[ {w_{k} v_{i}^{T} } \right] = 0 $$

(16)

When Kalman filter is used, the object motion state at the moment of k is estimated by Eq. (17) and a prior estimate x’_k is obtained.

$$ x_{k}^{'} = A \cdot x_{k - 1} $$

(17)

The prior estimation error is:

$$ e_{k}^{'} = x_{k} - x_{k}^{'} $$

(18)

The covariance matrix of prior estimation error is:

$$ P_{k}^{'} = E\left[ {e_{k}^{'} \cdot e_{k}^{'T} } \right] = E\left[ {\left( {x_{k} - x_{k}^{'} } \right)\left( {x_{k} - x_{k}^{'} } \right)^{T} } \right] $$

(19)

The prior estimate x’_k is corrected by using the observed data z_k, and it can be written as Eq. (20):

$$ \overline{x}_{k} = x_{k}^{'} + K_{k} \left( {z_{k} - H \cdot x_{k}^{'} } \right) $$

(20)

where $\overline{x}_{k}$ is modified value of prior estimate x’_k, which is called the posterior estimate value; K_k is the gain matrix of Kalman filter. Equations (21) and (22) give the expression of posterior estimation error and its covariance matrix, respectively.

$$ e_{k} = x_{k} - \overline{x}_{k} $$

(21)

$$ P_{k} = E\left[ {e_{k} \cdot e_{k}^{T} } \right] = E\left[ {\left( {x_{k} - \overline{x}_{k} } \right)\left( {x_{k} - \overline{x}_{k} } \right)^{T} } \right] $$

(22)

The objective is to get a K_k that minimizes the error between the posterior estimate and the actual value (i.e., the posterior estimate error). Substituting Eq. (13) into Eq. (20), and then substituting the result into Eq. (22), we get:

$$\begin{aligned} P_{k} &= E\left\{ \left[ {\left( {x_{k} - x_{k}^{'} } \right) - K_{k} \left( {H \cdot x_{k} + v_{k} - H \cdot x_{k}^{'} } \right)} \right]\right. \\ &\left.\quad\times\left[ {\left( {x_{k} - x_{k}^{'} } \right) - K_{k} \left( {H \cdot x_{k} + v_{k} - H \cdot x_{k}^{'} } \right)} \right]^{T} \right\} \end{aligned} $$

(23)

where $\left( {x_{k} - x_{k}^{'} } \right)$ is a priori estimation error, it is irrelevant to the observation error v_k, so:

$$ P_{k} = \left( {I - K_{k} H} \right)P_{k}^{'} \left( {I - K_{k} H} \right)^{T} + K_{k} R_{k} K_{k}^{T} $$

(24)

Thus, the K_k which enables the minimum error can be written as Eq. (25).

$$ K_{k} = P_{k}^{'} H^{T} \left( {H \cdot P_{k}^{'} H^{T} + R_{k} } \right)^{ - 1} $$

(25)

By substituting Eq. (25) into Eq. (24), we obtain:

$$ P_{k} = \left( {I - K_{k} H} \right)P_{k}^{'} $$

(26)

Since w_k−1 describes the cumulative error before processing from the moment of k − 1 to k, x’_k can be predicted by Eq. (27).

$$ x_{k}^{'} = Ax_{k} $$

(27)

Also, because w_k−1 and e_k−1 are not correlated, the covariance matrix of prior estimation error corresponding to x’_k is written as Eq. (28). So we can get K_k, and then, $\overline{x}$ can be estimated.

$$ P_{k}^{'} = E\left[ {\left( {A \cdot e_{k - 1} + w_{k - 1} } \right)\left( {A \cdot e_{k - 1} + w_{k - 1} } \right)^{T} } \right] = A \cdot P_{k - 1} \cdot A^{T} + Q_{k - 1} $$

(28)