Relative Motion Estimation Algorithm for Noncooperative Targets Considering Multiple Solutions of Rotational Parameters

Abstract

On-orbit servicing using a space robot is gaining popularity among the space community for both economic and safety aspects. In particular, the estimation of the relative motion of a noncooperative target is a challenging problem. This study presents a relative motion estimation scheme based on stereovision for noncooperative targets considering multiple solutions of rotational parameters. Specifically, the mass distribution of the target is identified based on the least-square method and the principle of conservation of angular momentum. Then, the determination of a unique principal axis coordinate frame of the target is employed to resolve the multiple-solution problem. In addition, an EKF (extended Kalman filter)-based filter with global observability is designed to estimate the full motion states and inertia parameters of the target. The convergence performance of the proposed method is verified by numerical simulation. The results also demonstrate that the method is robust to occlusion.

1. Introduction

The ever-growing number of malfunctioned spacecrafts remain in orbit with intense space activity, which seriously threatens the safety of operational spacecraft. On-orbit servicing (OOS) technology for repairing, refueling, and deorbiting these defunct spacecrafts has attracted widespread interest in the last decade. Relative pose and motion estimation of the target to be serviced is a key technology in the OOS mission. Frequently, these missions are considered cooperative. In this case, the state of the target can be measured through a global positioning system (GPS) and position-sensing diode (PSD) mounted on the target [1]. However, some defunct spacecrafts are noncooperative targets; i.e., they are unable to actively (i.e., though a communication link) or passively (i.e., though an auxiliary maker) exchange information with the servicing spacecraft, which makes the cooperative architecture inapplicable [2]. Thus, relative motion estimation technology for noncooperative targets has become urgently demanded and challenging.

To address this issue, the servicing spacecraft has to detect the target remotely on its own. Recent work suggests that electronic optical (EO) sensors are the best option for relative motion estimation purposes [3,4] when proximity operation with noncooperative targets is required. Active LIDAR (Light Detection and Ranging) systems and passive monocular/stereovision are typical EO sensors for space application. An LIDAR system can acquire the 3D point cloud of the target, which can be used for motion estimation. The iterative closest point (ICP) may be the most popular algorithm to deal with point clouds for tracking the pose of a target [4]. In [5], the pose is initialized by matching the silhouette image template data with the LIDAR points. The templates are built offline and the sample are restricted to the 2D attitude domain to simplify template matching. Aghili et al. [6] used the pose calculated through the ICP algorithm as the measurement for an extended Kalman filter and derived the covariance of measurement noise. However, LIDAR systems have obvious drawbacks in terms of mass, power consumption, computation load, and hardware complexity, especially for servicing spacecraft with limited weight and energy budget [7].

Comparatively, passive sensor-based approaches have been given more attention for noncooperative close-proximity operations. These methods rely on the features of the target surface, which are extracted from a sequence of images, to realize motion estimation. In the single-camera vision system, a model-based estimation architecture is proposed in [8], using the line segments identified from the edge of the target in images. The pose is then computed by solving a feature-matching problem using the efficient perspective-n-point (PNP) method. Since satellite nozzles and docking rings are equivalent to spatial circles, several researchers select circle or ellipse features as the recognized object to estimate the pose of the target [9,10]. However, the symmetry of these features will result in ambiguity of the vector normal and the loss of one rotational degree of freedom [11]. Zhang et al. [12] exploited the elliptical cone model to determine the pose of the docking ring and addressed the duality by introducing images of a redundant nozzle. In [13], a convolutional neural network (CNN) was applied to monocular images for pose determination. Then, an unscented Kalman filter with adaptive process noise was designed to estimate the motion of the target. Nevertheless, reliable datasets with labeled images of different motion states and illumination conditions are required for CNN training, which is costly for space applications. Because monocular vision offers bearing information only, it will suffer scale ambiguity regarding the position magnitude, which limits its application [14].

A stereovision system can acquire two perspective views of the features, and therefore, the depth information can be recovered. Several studies have utilized stereovision to address motion estimation concerning uncooperative targets. In [15], the rectangle feature of the framework on the backboard was recognized by two collaborative cameras to realize pose measurement. Hu et al. [16] introduced extra line features to recover information on the roll angle around the circle normal. However, the above methods will not work if particular artificial features, e.g., rectangles, lines, or circles, are not attainable on the target. The point feature always exists on the noncooperative target, making it an ideal candidate feature for recognition, especially when no a priori knowledge of the target’s structure or appearance is accessible [14]. An example of the point-based scheme in which the pose as well as the linear and angular velocity are estimated is shown in [1]. Segal et al. [17] built the observation model of a set of feature points based on the coupling translational–rotational kinematic. Several iterated EKFs with different inertial tensors are exploited and the optimal one is determined by adopting a maximum a posteriori identification. Another work [18] reorganized the Euler equation and incorporated the pseudo-measurement equation into the observation model.

Since there is no direct information about the target’s attitude in feature point measurement data, it is a crucial aspect to define a target-fixed coordinate frame to describe the orientation of the target. According to recent research work, the principal axis coordinate frame is typically preferred for motion estimation problems of a noncooperative target [6,18,19,20]. However, the principal axis coordinate frame is not unique for a rigid body. If a principal axis coordinate frame is designed as a state to be estimated in the filter without any constraint, it will bring multiple solution problems to angular velocity, the inertia matrix, and coordinates of features, because these values completely depend on which coordinate frame is utilized to describe the target’s rotation. In other words, multiple sets of these rotational parameters will share the same measurement history, resulting in the lack of global observability of the estimation problem [21]. The multiple-solution problem of rotational parameters, to the knowledge of the authors, has not been mentioned and investigated in the literature concerning relative navigation. Another aspect to take into account is that the rotation of the target will inevitably lead to the occlusion of feature points. During the occlusion period, the estimates will solely rely on the propagation of the dynamic model until these feature points become visible to the sensor again. In this circumstance, the filter may suffer serious convergence problems if the global observability cannot be guaranteed, making it vulnerable to occlusion.

Motivated by this, this study developed a relative motion estimation algorithm for noncooperative targets considering multiple solutions of rotational parameters. The method proposed herein only depends on the tracking of feature points by using stereovision measurements and prior information about the geometric shape of the target is not required. The original contributions of our work are twofold: First, we propose a method to determine the attitude of the target, which has a unique solution of the principal axis coordinate frame. Second, we use EKF along with a uniquely determined principal axis coordinate frame to guarantee global observability. In numerical simulation, the robustness of the algorithm to occlusion is presented and validated.

The rest of the article is organized as follows: Section 2 introduces the observation model of the stereovision system, as well as the dynamic model of the noncooperative target. Section 3 illustrates the multiple-solution problem of rotational parameters in detail and introduces the method for the determination of the principal axis coordinate frame. Section 4 formulates the EKF-based filtering scheme with a determined principal axis coordinate frame. Then, in Section 5, the simulation results are presented. Finally, a conclusion is drawn in Section 6.

2. Mathematical Model

The aim of the relative motion estimation problem is to estimate the relative translational and rotational motion states and inertial parameters of noncooperative targets using the stereovision equipped on the servicing satellite. In this section, the system model, namely, the measurement model of the stereovision and the dynamic model of the target, are presented. Several coordinate systems are introduced to help describe these models.

2.1. Measurement Model of Stereovision

As depicted in Figure 1, a stereovision system is employed on the space robot to observe the target. A simplified measurement model is applied, characterized by two parallel image planes that are perpendicular to the optical axis. Let $C$ denote the sensor coordinate frame which is attached to the center of projection of the left camera. The x axis of frame $C$ point to the center of projection of the right camera, the y axis is aligned with the optical axis, and the z axis obeys the right-hand rule. It is assumed that $N$ feature points on the surface of the target can be detected. The projection of the $i$ th feature points in the left and right image planes is denoted as $(u_{il},v_{il})$ and $(u_{ir},v_{ir})$, respectively. Then, the coordinate expressed in frame $C$ can be recovered using a pinhole camera model [17]:

$$m_i=\left[\begin{array}{ccc}{x}_i& y_i& z_i\end{array}\right]=\left[\begin{array}{ccc}{z}_i\frac{u_{il}}{f}& z_i\frac{v_{il}}{f}& \frac{fb}{u_{il}-u_{ir}}\end{array}\right]$$

(1)

where $f$ is the focal length and $b$ is the baseline length.

2.2. Dynamic Model of Rotational Motion

In this article, the attitude of the target is parameterized using a quaternion. The kinematics of the quaternion is given as [22,23]

$${}_I{}^{G}q=\frac{1}{2}\left[\begin{array}{c}{}^G\omega _{\mathit{GI}}\\ 0\end{array}\right]\otimes {}_I{}^{G}q$$

(2)

where ${}_I{}^{G}q$ describes the orientation of any target-fixed coordinate frame ($G$) with respect to the inertial coordinate frame, $I$. ${}^G\omega _{\mathit{GI}}$ is the angular velocity of frame $G$ with respect to frame $I$ expressed in $G$, and $\otimes$ represents the quaternion multiplication. When the target is regarded as a rigid body, the rotational dynamics is given as [6]

$$I\dot{\omega }=-\left[\omega \times \right]I\omega +L$$

(3)

where $I$ denotes the target’s inertia matrix and $L$ is external disturbance torque.

2.3. Dynamics Model of Translational Motion

Assuming that the servicing spacecraft moves in a circular orbit, the translational motion of the target in the Hill coordinate frame of the space robot (denoted by $L$) can be described by the Hill equation [24]:

$$\left[\begin{array}{c}\dot{r}\\ \dot{v}\end{array}\right]=\left[\begin{array}{cc}{0}_{3\times 3}& I_{3\times 3}\\ E_1& E_2\end{array}\right]\left[\begin{array}{l}r\hfill \\ v\hfill \end{array}\right]+\left[\begin{array}{l}{0}_{3\times 3}\hfill \\ I_{3\times 3}\hfill \end{array}\right]n_t$$

(4)

where

$$E_1=\left[\begin{array}{ccc}0& 0& 0\\ 0& -{{\omega }_c}^2& 0\\ 0& 0& 3{{\omega }_c}^2\end{array}\right]E_2=\left[\begin{array}{ccc}0& 0& 2{\omega }_c\\ 0& 0& 0\\ -2{\omega }_c& 0& 0\end{array}\right]$$

(5)

$r={\left[\begin{array}{ccc}x& y& z\end{array}\right]}^{\mathrm{T}}$ and $v={\left[\begin{array}{ccc}\dot{x}& \dot{y}& \dot{z}\end{array}\right]}^{\mathrm{T}}$ are the relative position and velocity of the target with respect to the servicing satellite expressed in frame $L$. ${\omega }_c$ is the orbital angular velocity of the servicing spacecraft.

3. Determination of Principal Axis Coordinate Frame

As mentioned in the introduction, the principal axis coordinate frame, the target-fixed coordinate frame aligned with its principal axis of inertia, is more preferable to describe the orientation of a noncooperative target and often set as the state to be estimated in a filter. One reason is that the principal axis coordinate frame can reflect the mass distribution of the target, which is useful information for the subsequent design of the capture strategy. Moreover, because the corresponding inertia matrix is diagonal, it will reduce the dimension of unknown inertia parameters. Notice that there are different ways to define a principal axis coordinate frame. (Figure 2 shows a total of 24 principal axis coordinate frames for a rigid body. The principal axes of the target are represented by dashed lines.) Because the principal axis is only determined by the mass property of the target, it is usually unable to be directly measured by the optical sensor. In other words, the target-fixed frame, $G$, which is related to visual measurement, does not coincide with a principal axis frame in general. As shown in Figure 2, the attitudes of these principal axis coordinate frames are different from each other, which further causes different values of the corresponding angular velocity, inertial matrix, and coordinates of features. These 24 sets of rotational parameters can produce the same time history of measurement and lead to a multiple-solution problem. Consequently, the navigation filter will lose global observability.

Motivated by this, an algorithm for the determination of a unique principal axis coordinate frame is proposed in this section. In this phase, three non-collinear feature points are exploited to define frame $G$ due to the lack of direct pose measurement of the target. The coordinates of these points are computed based on Equation (1). Three orthogonal unit vectors, $c_i$, can be obtained using the following equation:

$$\left\{\begin{array}{l}{c}_1=\left(m_2-m_1\right)/\left|\left(m_2-m_1\right)\right|\hfill \\ c_3=\left(m_2-m_1\right)\times \left(m_3-m_1\right)/\left|\left(m_2-m_1\right)\times \left(m_3-m_1\right)\right|\hfill \\ c_2=c_3\times c_1\hfill \end{array}\right.$$

(6)

Then, a target-fixed photogrammetric coordinate frame, $G$, is defined, which conforms to

$${}_C{}^{G}R={\left[\begin{array}{ccc}{c}_1& c_2& c_3\end{array}\right]}^{\mathrm{T}}$$

(7)

where ${}_C{}^{G}R$ is the rotation matrix from frame $C$ to frame $G$. The corresponding inertial matrix expressed in frame $G$ is denoted as

$${}^{G}I=\left[\begin{array}{ccc}{I}_{xx}& I_{xy}& I_{xz}\\ I_{xy}& I_{yy}& I_{yz}\\ I_{xz}& I_{yz}& I_{zz}\end{array}\right]$$

(8)

Since these feature points are randomly distributed and selected, the product of inertia is set as non-diagonal elements of the inertia matrix without loss of generality. It is worth noting that only five inertial parameters are independent because the inertial matrix will always conform to Equation (3), even when multiplied by any constant. After being divided by the first element, the inertial matrix can be normalized as

$${}^G\overline{I}=\left[\begin{array}{ccc}1& {\overline{I}}_{xy}& {\overline{I}}_{xz}\\ {\overline{I}}_{xy}& {\overline{I}}_{yy}& {\overline{I}}_{yz}\\ {\overline{I}}_{xz}& {\overline{I}}_{yz}& {\overline{I}}_{zz}\end{array}\right]$$

(9)

and the constant inertia ratio vector is expressed as $l={[\begin{array}{ccccc}{\overline{I}}_{yy}& {\overline{I}}_{zz}& {\overline{I}}_{zy}& {\overline{I}}_{xz}& {\overline{I}}_{yz}\end{array}]}^T$. According to attitude dynamics, the angular momentum of target can be formulated as

$${}^G\overline{I}\omega ={}_I{}^{G}RH$$

(10)

where $H={[\begin{array}{ccc}{h}_1& h_2& h_3\end{array}]}^T$ denotes the angular momentum expressed in frame $I$. It is assumed that the target is a torque-free tumbling rigid body; the principle of the conservation of angular momentum can be adopted to estimate the inertia parameters. In that case, $H$ will remain constant. Consequently, Equation (10) can be rewritten as the following linear equation:

$$a(t)X=b(t)$$

(11)

where

$$a(t)=\left[\begin{array}{cccccc}0& 0& {\omega }_y(t)& {\omega }_z(t)& 0& \\ {\omega }_y(t)& 0& {\omega }_x(t)& 0& {\omega }_z(t)& -{}_I{}^{G}R(t)\\ 0& {\omega }_z(t)& 0& {\omega }_x(t)& {\omega }_y(t)& \end{array}\right],b(t)=\left[\begin{array}{c}-{\omega }_x\\ 0\\ 0\end{array}\right]$$

(12)

$X={[\begin{array}{cc}{l}^T& H^T\end{array}]}^T$. The unknown constant $X$ is estimated by the least-square method if observation data from different epochs are acquired:

$$\widehat{x}=(A^{\mathrm{T}}A{)}^{-1}{A}^{\mathrm{T}}B$$

(13)

where

$$A=\left[\begin{array}{c}a(t_1)\\ a(t_2)\\ ⋮\\ a(t_k)\end{array}\right],B=\left[\begin{array}{c}b(t_1)\\ b(t_2)\\ ⋮\\ b(t_k)\end{array}\right]$$

(14)

To estimate the angular velocity in Equation (12), we rewrite Equation (2) as

$$\omega =2{\Xi }^{\mathrm{T}}(q)\stackrel{.}{q}$$

(15)

where

$$\Xi (q)\equiv \left[\begin{array}{c}{q}_4{I}_3+\left[q_{1:3}\times \right]\\ -q_{1:3}^T\end{array}\right]$$

(16)

The angular velocity can be approximated from the numerical differentiation of $q$ [25].

The resulting $\widehat{\overline{I}}$ relates the mass distribution of the target to the stereovision measurement. Therefore, the orientation of a principal axis coordinate frame, $T$, can be determined through orthogonal diagonalization of $\overline{I}$:

$${}_G{}^{T}R{}^G\widehat{\overline{I}}{{}_G{}^{T}R}^{\mathrm{T}}={\left[\begin{array}{ccc}{r}_1& r_2& r_3\end{array}\right]}^{\mathrm{T}}{}^G\widehat{\overline{I}}\left[\begin{array}{ccc}{r}_1& r_2& r_3\end{array}\right]=\left[\begin{array}{ccc}{\lambda }_1& & \\ & {\lambda }_2& \\ & & {\lambda }_3\end{array}\right]$$

(17)

where ${\lambda }_i$ are eigenvalues of $\overline{I}$. The corresponding eigenvectors, $r_i$, are the column vectors of the rotation matrix from frame $T$ to frame G. Note that multiple solutions of frame $T$ exist, resulting from the selection of $r_i$, as shown in Figure 2. Therefore, an approach to uniquely determine frame T is proposed as follows: Specifically, $r_i$ is chosen so that the inequality constraint ${\lambda }_1>{\lambda }_2>{\lambda }_3$ can be satisfied. Furthermore, the first element of $r_1$ is set to be positive. In fact, these conditions are equivalent to the constraints that the x axis and z axis of frame $T$ are aligned with the principal axis of the largest and smallest moment of inertia, respectively, and that the x axis of frame $T$ forms an acute angle with the x axis of frame $G$. In this way, the principal axis coordinate frame, T, can be uniquely determined.

4. Extended Kalman Filter with Determined Principal Axis Coordinate Frame

In our implementation, an EKF-based scheme is employed using the estimated attitude of frame $T$ and observed feature points to estimate the rotational and translational motion of the target. Meanwhile, the orientation of frame $T$ is set as the rotational state of the target to be filtered. Because frame $T$ has been defined and a rough estimation is directly acquired in Section 3, the multiple-solution problem will be resolved.

We denote the inertia matrix of the principal axis coordinate frame, $T$, as

$$I=\left[\begin{array}{ccc}{I}_x& 0& 0\\ 0& I_y& 0\\ 0& 0& I_z\end{array}\right]$$

(18)

which is parameterized in a similar way as in [6].

$$p_x=\frac{I_y-I_z}{I_x},p_y=\frac{I_z-I_x}{I_y},p_z=\frac{I_x-I_y}{I_z}$$

(19)

where $p={\left[\begin{array}{ccc}{p}_x& p_y& p_z\end{array}\right]}^{\mathrm{T}}$ is the inertia ratio with

$$\dot{p}=0$$

(20)

Therefore, Euler dynamics (3) can be rewritten in terms of the inertia ratio as

$$\dot{\omega }=K(\omega )+J(p)n_r$$

(21)

where

$$K(\omega )=\left[\begin{array}{l}{p}_x{\omega }_y{\omega }_z\hfill \\ p_y{\omega }_x{\omega }_z\hfill \\ p_z{\omega }_x{\omega }_y\hfill \end{array}\right],J(p)=\left[\begin{array}{ccc}1& 0& 0\\ 0& \frac{1-p_y}{1+p_x}& 0\\ 0& 0& \frac{1+p_z}{1-p_x}\end{array}\right]$$

(22)

$n_r$ is the disturbance torque.

Let $f_i$ denote the coordinate of feature points in frame $T$, which is constant, i.e.,

$${\dot{f}}_i=0$$

(23)

The state vector to be estimated by the filter is therefore

$$x={[\begin{array}{ccccccccc}{{}_I{}^{T}q}^{\mathrm{T}}& {\omega }^{\mathrm{T}}& p^{\mathrm{T}}& r^{\mathrm{T}}& v^{\mathrm{T}}& f_1^{\mathrm{T}}& f_2^{\mathrm{T}}& \cdots & f_N^{\mathrm{T}}\end{array}]}^{\mathrm{T}}$$

(24)

From Equations (2), (4), (20), (21) and (23), the system model is described by

$$x=f(x)$$

(25)

Considering the composition rules of the quaternion, the error-state vector is given by

$$\Delta x={[\begin{array}{ccccccccc}\delta {\theta }^{\mathrm{T}}& \delta {\omega }^{\mathrm{T}}& \delta p^{\mathrm{T}}& \delta r^{\mathrm{T}}& \delta v^{\mathrm{T}}& \delta f_1^{\mathrm{T}}& \delta f_2^{\mathrm{T}}& \cdots & \delta f_N^{\mathrm{T}}\end{array}]}^{\mathrm{T}}$$

(26)

For the angular velocity, inertia ratio, position, velocity, and coordinates of feature points, the error in the estimated $\widehat{b}$ of a state ($b$) is defined as $\delta b=b-\widehat{b}$. For the quaternion, the error is parameterized using a rotation vector ($\delta \theta$) which satisfies

$$q=\left[\begin{array}{c}\frac{1}{2}\delta \theta \\ \sqrt{1-{‖\frac{1}{2}\delta \theta ‖}^2}\end{array}\right]\otimes \widehat{q}$$

(27)

The linearized continuous-time model for the error states is obtained by retaining only the first-order term of the Taylor expansion of Equation (25) around the current estimated value [22]:

$$\Delta \dot{x}=F\Delta x+Gn$$

(28)

where

$$F=\left[\begin{array}{cccccc}-[\widehat{\omega }\times ]& I_3& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3N}\\ 0_{3\times 3}& M(\widehat{\omega },\widehat{p})& N(\widehat{\omega })& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3N}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3N}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& I_{3\times 3}& 0_{3\times 3N}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& E_1& E_2& 0_{3\times 3N}\\ 0_{3N\times 3}& 0_{3N\times 3}& 0_{3N\times 3}& 0_{3N\times 3}& 0_{3N\times 3}& 0_{3N\times 3N}\end{array}\right],G=\left[\begin{array}{cc}{0}_{3\times 3}& 0_{3\times 3}\\ J(\overline{p})& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& I_3\\ 0_{3N\times 3}& 0_{3N\times 3}\end{array}\right]$$

(29)

$n={[\begin{array}{cc}{n}_r^{\mathrm{T}}& n_t^{\mathrm{T}}\end{array}]}^{\mathrm{T}}$ with

$$M(\widehat{\omega },\widehat{p})=\left[\begin{array}{ccc}0& p_x{\omega }_z& p_x{\omega }_y\\ p_y{\omega }_z& 0& p_y{\omega }_x\\ p_z{\omega }_y& p_z{\omega }_x& 0\end{array}\right],N(\widehat{\omega })=\left[\begin{array}{ccc}{\omega }_y{\omega }_z& 0& 0\\ 0& {\omega }_x{\omega }_z& 0\\ 0& 0& {\omega }_x{\omega }_y\end{array}\right]$$

(30)

The covariance of process noise ($n$) is denoted by $Q$.

As mentioned above, the estimated attitude of frame $T$ in Section 3 offers the direction observation of the state within the filter, the measurement model of which is simply

$$y_0={}_I{}^{T}q={}_C{}^{T}q\otimes {}_I{}^{C}q$$

(31)

where ${}_I{}^{C}q$ is computed based on the attitude installation matrix of stereovision and the attitude estimation of the servicing spacecraft. Meanwhile, the coordinate of the $i$ th feature point in frame $L$ satisfies

$$y_i=r+{}_I{}^{L}R{}_T{}^{I}R{f}_i$$

(32)

which is obtained from the absolute orbit determination of the servicing spacecraft. Putting all the components together, the observation model of filter is defined as

$$y=h(x)={\left[\begin{array}{ccccc}{y}_0^{\mathrm{T}}& y_1^{\mathrm{T}}& y_2^{\mathrm{T}}& \cdots & y_N^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}$$

(33)

The error measurement model is approximated by linearizing Equation (33):

$$\Delta y=H\Delta x+\eta$$

(34)

where $\eta$ is the measurement noise, and the measurement sensitive matrix ($H$) is given as

$$H=\left[\begin{array}{cccccc}{I}_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ -{}_T{}^L\widehat{R}[{\widehat{f}}_1\times ]& 0_{3\times 3}& 0_{3\times 3}& I_{3\times 3}& 0_{3\times 3}& {}_T{}^L\widehat{R}\\ \cdots & \cdots & \cdots & \cdots & \cdots & \cdots \\ -{}_T{}^L\widehat{R}[{\widehat{f}}_N\times ]& 0_{3\times 3}& 0_{3\times 3}& I_{3\times 3}& 0_{3\times 3}& {}_T{}^L\widehat{R}\end{array}\right]$$

(35)

In this study, a continuous–discrete type of EKF is employed to solve the relative motion estimation problem. The implementation of the EKF is based on two processes: prediction and update.

In the prediction step, the optimal estimation of the state (x) and error covariance (P) are propagated for the time interval $(t_k,t_{k+1})$ through

$$x_{k+1}^{-}=x_k^{+}+{\displaystyle {\int }_{t_k}^{t_{k+1}}f}(x(t))dt$$

(36)

$$P_{\mathrm{k}+1}^{-}=\Phi (t_{k+1},t_k)P_k^{+}{\Phi }^T(t_{k+1},t_k)+Q_k$$

(37)

The error-state transition matrix $\Phi (t_{k+1},t_k)$ can be computed by numerical simulation of the following differential equation:

$$\dot{\Phi }(\tau ,t_k)=F(\tau )\Phi (\tau ,t_k)$$

(38)

With the initial condition of $\Phi (t_k,t_k)=I$. $Q_k$ is the discrete covariance of system noise and is calculated by [26]

$$Q_k={\displaystyle {\int }_{t_k}^{t_{K+1}}\Phi (t_{k+1},\tau )}GQ{G}^{\mathrm{T}}{\Phi }^{\mathrm{T}}(t_{k+1},\tau )d\tau$$

(39)

Once the measurement is available, the error state ($\Delta x$) and the corresponding covariance are corrected according to

$$\Delta x_{k+1}=K_{k+1}\Delta y_{k+1}$$

(40)

$$P_{\mathrm{k}+1}^{+}=\left(I-K_{k+1}{H}_{k+1}\right)P_{\mathrm{k}+1}^{-}{\left(I-K_{k+1}{H}_{k+1}\right)}^{\mathrm{T}}+K_{k+1}{R}_{k+1}{K}_{k+1}^{\mathrm{T}}$$

(41)

where $R_{k+1}$ is the covariance of measurement noise and $K_{k+1}$ is the Kalman gain.

$$K_{k+1}=P_{\mathrm{k}+1}^{-}{H}_{k+1}^{\mathrm{T}}{\left(H_{k+1}{P}_{\mathrm{k}+1}^{-}{H}_{k+1}^{\mathrm{T}}+R_{k+1}\right)}^{-1}$$

(42)

The optimal estimation of filter states, except the attitude, can be updated as

$$x_{k+1}^{+}=x_{k+1}^{-}+\Delta x_{k+1}$$

(43)

To satisfy the unit norm constraint, the update of quaternion is realized through

$$q_{k+1}^{+}=\left[\begin{array}{c}\frac{1}{2}\delta {\theta }_{k+1}\\ \sqrt{1-{‖\frac{1}{2}\delta {\theta }_{k+1}‖}^2}\end{array}\right]\otimes q_{k+1}^{-}$$

(44)

5. Numerical Simulation

In this section, the numerical simulations are carried out to verify the proposed relative motion estimation method. Specifically, the objectives of the simulative experiment are to (a) evaluate the validity of the determination of the principal axis coordinate frame and (b) investigate the estimation performance of the EKF-based filter with a uniquely determined attitude.

It is assumed that the servicing spacecraft is in a circular orbit with a radius of 6800 km, and thus, the angular rate is 0.0012 rad/s. The initial relative position and velocity of the target with respect to the servicing spacecraft are set as $r_0={\left[10,10,10\right]}^{\mathrm{T}}$ m and $v_0={\left[-0.3889,-0.4932,-0.8264\right]}^{\mathrm{T}}$ m/s. In this simulation, a microsatellite, mentioned in [6], is selected as the target spacecraft, which has the inertia matrix $\mathrm{diag}([8,5,4]) \mathrm{kg}\cdot {\mathrm{m}}^2$. The initial attitude of the target parameterized by the quaternion is given as $q_0={\left[0,0,0,1\right]}^{\mathrm{T}}$. According to [27], a malfunctional spacecraft may tumble at a rate varying greatly, from 2.9 deg/s to 36 deg/s. Based on this, a typical value of $w_0={\left[0.1,0.1,0.1\right]}^{\mathrm{T}}$ rad/s is considered as the initial angular velocity. We assume that the coordinates of feature points expressed in frame $T$ are subject to uniform distribution, with lower and upper bounds of −1.5 m and 1.5 m, which is the same as in [17]. Six instead of ten feature points are supposed to be measured to test the proposed method in extreme conditions. The measurement is generated at a rate of 10 Hz (similarly to [28]). Due to the self-occlusion of the target caused by rotation, these feature points will inevitably be unobservable for some time intervals. The availability of a feature point can be identified by evaluating whether the angle between the observation direction and the normal direction is acute [29]. However, this algorithm is reliable only when the body is convex. For simplicity, a hypothesis of a fixed occlusion period is made, which is sufficient for the purpose of this study. In view of the magnitude of the initial angular rate, $\left|w_0\right|\approx 10$ deg/s, we consider a complete unavailability of measurement within 20 s, about half of the rotation period of the target. Figure 3 illustrates this concept. In addition, Figure 4 shows the visible trajectories of these six feature points.

(1)

Results of principal axis coordinate determination

This section presents the principal axis coordinate determination results. To evaluate the influence of the measurement noise level, we consider the stereovision system with an angular resolution of $0.1\times {10}^{-5}\mathrm{rad}$, $0.5\times {10}^{-5}\mathrm{rad}$, $2\times {10}^{-5}\mathrm{rad}$, and $5\times {10}^{-5}\mathrm{rad}$, similar to the setup used in [30]. The root mean square error (RMSE) is applied as the metric of the algorithm, which is defined as

$$\mathrm{RMSE}=\sqrt{\frac{1}{N}{\displaystyle \sum _{i=1}^N{e}_i^T{e}_i}}$$

(45)

where $e$ is the estimated error and $N$ is the number of Monte Carlo runs.

The RMSE of the estimated attitude of the principal axis coordinate frame is shown in Figure 5, with different levels of noise over 500 Monte Carlo runs. As seen, all RMSE plots at different noise levels converge quickly in a few seconds with acceptable performance, even before the occurrence of occlusion. It is clearly shown that the noise covariance variations have a great influence on the accuracy and convergence performance: an increased noise level implies lower accuracy and slower convergence. This is because the determination of the principal axis coordinate frame is based on the identification and diagonalization of the inertial matrix, the accuracy of which is directly related to the measurement accuracy. Overall, it is safe to state that the principal axis coordinate frame can be determined uniquely with relatively high accuracy through the proposed algorithm, which is used during the subsequent filter phase.

(2)

Results of motion estimation

In this section, the EKF with the determination of the principal axis coordinate frame (DPACF) is implemented to estimate the motion of the target. The covariance of the orbital force and disturbance torque are set as ${\sigma }_t^2=2\times {10}^{-6}{I}_3 {\mathrm{m}}^2{/\mathrm{s}}^4$ and ${\sigma }_r^2=2.5\times {10}^{-5}{I}_3 {\mathrm{rad}}^2{/\mathrm{s}}^4$, the same as in [6]. Note that the states and parameters are predicted by solely relying on their latest values and propagation of dynamics during the occlusion period shown in Figure 3. We evaluate the performance of the proposed scheme by comparing it with the method in [6], not employing DPACF. The simulation time is 200 s.

Figure 6, Figure 7, Figure 8 and Figure 9 show the estimation errors of rotational parameters, i.e., the attitude angle, angular velocity, inertia ratio, and coordinates of feature points. It is evident from the figure that estimation errors of the method without DPACF are characterized by significant nonzero mean deviation in the whole process. This is because, due to the multiple solutions of the principal axis coordinate frame, the rotational parameters are not unique. In other words, the filter is not globally observable. On the other hand, when the feature points are missing during occlusion, the noise process will lead to a deviation of estimates from the solution it is supposed to converge to. Therefore, there is no guarantee that the filter will converge to the previous or any specific solution of the 24 candidates in the next estimation phase when the measurements are available again, which results in the fluctuation of estimates. This makes the estimation process fragile to occlusion. Comparatively, the error curves of our method can converge in some periods of time and finally remain within a small neighborhood around zero. This mainly benefits from the determination of the principal axis coordinate frame to force the filter to converge to a unique solution, defined in Section 3. The estimation errors of the relative position and velocity are shown in Figure 10 and Figure 11. It can be seen that the divergence of rotational parameters even causes translational states to deviate, although there is no multiple-solution problem in these states. In conclusion, the proposed method is expected to remove the multiple-solution problem and exhibit robustness to inevitable occlusion.

6. Conclusions

A relative motion estimation scheme for noncooperative targets considering multiple solutions of rotational parameters is presented in this paper. This scheme depends on the determination of the principal axis coordinate frame before the implementation of a filter. Through the identification of the mass distribution of the target and the diagonalization of the normalized inertia matrix, the multiple-solution problem of rotational parameters is prohibited and the global observability of the estimation problem is guaranteed. The results demonstrate good convergence performance and robustness to occlusion, owing to the new filter structure with a unique solution. It should be noted that such an algorithm improves the estimation capability and provides a feasible solution for the relative motion estimation problem of future OOS missions.