# Relative Attitude Estimation for a Uniform Motion and Slowly Rotating Noncooperative Spacecraft

Authors: Liu Zhang; Zongming Liu; Dong Ye; Guanyu Zhang
Journal: Mathematical Problems in Engineering
 (2017)

---

## Abstract

This paper presents a novel relative attitude estimation approach for a uniform motion and slowly rotating noncooperative spacecraft. It is assumed that the uniform motion and slowly rotating noncooperative chief spacecraft is in failure or out of control and there is no a priori rotation rate information. We utilize a very fast binary descriptor based on binary robust independent elementary features (BRIEF) to obtain the features of the target, which are rotational invariance and resistance to noise. And then, we propose a novel combination of single candidate random sample consensus (RANSAC) with extended Kalman filter (EKF) that makes use of the available prior probabilistic information from the EKF in the RANSAC model hypothesis stage. The advantage of this combination obviously reduces the sample size to only one, which results in large computational savings without the loss of accuracy. Experimental results from real image sequence of a real model target show that the relative angular error is about 3.5% and the mean angular velocity error is about 0.1 deg/s.


---

## Body

## 1. Introduction

With the development of the space technology, the number of spacecraft on orbit is increasing rapidly year by year. At the same time, the space debris including thousands of inactive satellites, fragments of broken-up spacecraft, and equipment lost by astronauts is mounting [1]. And there has been a growing interest in autonomously servicing spacecraft on orbit to perform tasks such as spacecraft rescue and repairing, refueling, and removal. Several space programs have been completed or are being developed as technology demonstration of these tasks, such as JAXA’s ETS-7 [2], NASA’s DART program [3], DARPA’s Orbital Express program [4], and SUMO program [5]. On one hand, the malfunctioning spacecraft take up the valuable and limited orbit, which increases the potential security risks. On the other hand, they are also a kind of available resources on orbit. Therefore, the capture of a slowly rotating spacecraft has received detailed attentions in recent years [6–9].Nowadays, vision-based navigation systems, which have the advantage of being entirely passive and of low power consumption, are usually used to determine the relative attitude and position [10, 11]. The vision measurement has been separated into two aspects of model based and model-independent methods [12]. The first class usually needs the CAD model of the target object or some other kinds of a priori knowledge. Cropp et al. [13] extracted the line features in an image by a monocular camera and calculated the matched relationship with a priori model. They got the relative pose of two stable spacecraft by the nonlinear least square optimization theory. The constraint function was the orthogonality between the normal vector of line in the image frame and the plane composed of the same line and the optic center in the camera frame. Kelsey et al. [12] obtained the pose of a slowly rotating spacecraft model by three steps which include the initialization by triangulation, the refinement by Iterative Reweighted Least Squares, and the tracking by EKF. In their experiments in laboratory, the attitude error was up to about 20 degrees and the range error was more than 10% of range.The model-independent method is more challenging because no prior information will be used. In other words, the target is absolutely a noncooperative object. Priggemeyer et al. [14] acquired the target picture sequence by a stereo camera system with a baseline of 0.2 m. Afterwards, they extracted the point features with SURF. And then the DLT algorithm [15] solving a linear equation system was applied to find the projection matrices. Finally, a Kalman filter was applied as a recursive least squares filter to reduce noise. Oumer and Panin [16] and Segal et al. [17] presented a system for feature-based 3D tracking of a noncooperative satellite. Sequential 3D motion estimation from a stereo camera has also been tackled using filtering schemes.Up to now, most of the algorithms and systems are focused on a scene that the camera optical axis is parallel to the spin direction of the target. This hypothesis greatly reduces the degree of difficulty, because the feature face is in the field of view all the way. In our paper, we propose the optical axis and the spin direction, mutually perpendicular. So there is no fixed feature face to our camera. This proposes high demand of the robustness and universality of the algorithm. The contribution of this paper is the proposal of a new single candidate RANSAC algorithm that exploits the probabilistic prediction obtained from the EKF in order to increase the efficiency of the spurious match rejection step. The calculation speed is further improved by using a very fast feature extractor. Meanwhile, we introduce the image pyramid and the binary descriptor for the sake of ensuring the scale invariance and rotational invariance.


## 2. Feature Initialization

### 2.1. Reference Frames

As in Figure1, there are some frames defined as follows.Figure 1
The demonstration of the frames.(1) Pixel Frame
(
O
p
-
u
v
)
. The frame is centered at the top left point of the resolution, the u-axis is directed to the right level, and v-axis is orthogonal with u-axis in the same plane.(2) Image Frame
(
O
I
-
x
y
)
. The frame is centered at the place where the optic axis intersects with the image plane; the x-axis and y-axis are parallel to the u-axis and v-axis, respectively.(3) Camera Frame
(
O
c
-
X
c
Y
c
Z
c
)
. The frame is centered at the optic center, x-axis is parallel to the u-axis, y-axis is parallel to the v-axis, and z-axis is defined as the cross-product of the other two axes.(4) World Frame
(
O
w
-
X
w
Y
w
Z
w
)
. The frame has its origin and axes anywhere in the real world space.


### 2.2. Feature Detection and Description

Our proposed feature builds on the well-known fast keypoint detector [18] and the recently developed BRIEF descriptor [19]. Both these techniques are attractive because of their good performance and low cost. FAST takes one parameter, the intensity threshold between the center pixel and those in a circular ring around the center. We use circular radius of 9, which has good performance. FAST does not produce multiscale features. We employ a scale pyramid of the image and produce fast features at each level in the pyramid.Our approach uses a simple but effective measure of corner orientation, the intensity centroid [20]. Rosin defines the moments of a patch as(1)mpq=∑u,vupvqIu,v,where u and v belong to a circular region of radius r in the pixel frame. With these moments, we may find the centroid:(2)C=m10m00,m01m00.We can construct a vector from the corner’s center,O, to the centroid, OC→. The orientation of the patch then simply is(3)θ=arctan⁡m01m10.The BRIEF descriptor is a bit string description of an image patch constructed from a set of binary intensity tests. Consider a smoothed image patch,p. A binary test τ is defined by(4)τp;u,v≔1,pu<pv0,pu≥pv,where p· is the intensity of p at a point (u,v). The feature is defined as a vector of n binary tests:(5)fnp≔∑1≤i≤n2i-1τp;ui,vi.The vector lengthn could be chosen as 128, 256, 512, and so on. BRIEF has no orientation, so if we want to allow BRIEF to be invariant to in-plane rotation, we should add one to it. Just as described in [21], we use the steered BRIEF operator to solve the problem.(6)gnp,θ≔fnp∣ui,vi∈Sθ.


## 2.1. Reference Frames

As in Figure1, there are some frames defined as follows.Figure 1
The demonstration of the frames.(1) Pixel Frame
(
O
p
-
u
v
)
. The frame is centered at the top left point of the resolution, the u-axis is directed to the right level, and v-axis is orthogonal with u-axis in the same plane.(2) Image Frame
(
O
I
-
x
y
)
. The frame is centered at the place where the optic axis intersects with the image plane; the x-axis and y-axis are parallel to the u-axis and v-axis, respectively.(3) Camera Frame
(
O
c
-
X
c
Y
c
Z
c
)
. The frame is centered at the optic center, x-axis is parallel to the u-axis, y-axis is parallel to the v-axis, and z-axis is defined as the cross-product of the other two axes.(4) World Frame
(
O
w
-
X
w
Y
w
Z
w
)
. The frame has its origin and axes anywhere in the real world space.


## 2.2. Feature Detection and Description

Our proposed feature builds on the well-known fast keypoint detector [18] and the recently developed BRIEF descriptor [19]. Both these techniques are attractive because of their good performance and low cost. FAST takes one parameter, the intensity threshold between the center pixel and those in a circular ring around the center. We use circular radius of 9, which has good performance. FAST does not produce multiscale features. We employ a scale pyramid of the image and produce fast features at each level in the pyramid.Our approach uses a simple but effective measure of corner orientation, the intensity centroid [20]. Rosin defines the moments of a patch as(1)mpq=∑u,vupvqIu,v,where u and v belong to a circular region of radius r in the pixel frame. With these moments, we may find the centroid:(2)C=m10m00,m01m00.We can construct a vector from the corner’s center,O, to the centroid, OC→. The orientation of the patch then simply is(3)θ=arctan⁡m01m10.The BRIEF descriptor is a bit string description of an image patch constructed from a set of binary intensity tests. Consider a smoothed image patch,p. A binary test τ is defined by(4)τp;u,v≔1,pu<pv0,pu≥pv,where p· is the intensity of p at a point (u,v). The feature is defined as a vector of n binary tests:(5)fnp≔∑1≤i≤n2i-1τp;ui,vi.The vector lengthn could be chosen as 128, 256, 512, and so on. BRIEF has no orientation, so if we want to allow BRIEF to be invariant to in-plane rotation, we should add one to it. Just as described in [21], we use the steered BRIEF operator to solve the problem.(6)gnp,θ≔fnp∣ui,vi∈Sθ.


## 3. EKF-Based Estimation

### 3.1. State Vector Definition

The whole state vectorXk at step k is composed of a set of camera parameters xc and feature set y. All of these are referred to by the world frame W.(7)Xk=xcyPk=PckPckyPyckPy,where Pk is the covariance matrix. The estimated feature set y is composed of n point features yi,y=y1T,y2T,…,ynTT. Point features are parametrized in inverse depth coordinate [22] which can be defined by the dimension 6-state vector:(8)yi,Id=xi,yi,zi,θi,ϕi,ρiT.The inverse depth parametrization stores in its six parameters the 3D camera position when the feature was initializedxi,yi,ziT, the azimuth-elevation pair θi,ϕiT encoding the unit ray pointing to the feature, and its inverse depth along the ray ρ [23]. They could be converted to Euclidean XYZ coordinates yi,E=Xi,Yi,ZiT if and when the projection equation becomes linear enough.(9)yi,E=XiYiZi=xiyizi+1ρimθi,ϕi,where mθi,φi=cos⁡ϕisin⁡θi,-sin⁡ϕi,cos⁡ϕicos⁡θiT.


### 3.2. Dynamic Model

The camera statexc is composed of pose terms: rWC camera optical center position and σWC modified Rodrigues parameters (MRPs) defining orientation; linear and angular velocity vW and ωC relative to world frame W and camera frame C.The state update equation for the camera is(10)fv=rk+1WCσk+1WCvk+1Wωk+1C=rkWC+vkW+VkWΔt141-σkWCTσkWCI+2σkWC×+2σkWCσkWCTωkC+ΩCΔtvkW+VWωkC+ΩC,where(11)σkWC×=0-σkzWCσkyWCσkzWC0-σkxWC-σkyWCσkxWC0.vW and ΩC are zero-mean Gaussianly distributed velocity noise coming from an impulse of acceleration.


### 3.3. Measurement Model

The measurement model used in the experiments of the paper is a pinhole camera model plus a two-parameter radial distortion. The camera is assumed to be calibrated in advance. Inverse depth and Euclidean points in the state vector are first transformed to the camera reference frame:(12)hC=hxhyhz=hi,E=RCWXiYi-rWCZi(13)hC=hxhyhz=hi,Id=RCWρixiyizi-rWC+mθi,φi,where RCW represents a rotation matrix computed from the state MRPs and m(θi,φi)  m is the function converting azimuth-elevation angles to a unit vector. Points in the camera frame are then projected using the standard pinhole model:(14)hu=uv=u0-fdxhxhzv0-fdyhyhz.Heref stands for the focal length of the camera, dx and dy represent the pixel size, and u0,v0T are the image center coordinates. The imaged point is finally transformed using the two parameters k1, k2, modeled below, resulting in the distorted measurement(15)hd=udvd=u0+u-u01+k1rd2+k2rd4v0+v-v01+k1rd2+k2rd4ru=dxu-u02+dyv-v02ru=rd1+k1rd2+k2rd4.


## 3.1. State Vector Definition

The whole state vectorXk at step k is composed of a set of camera parameters xc and feature set y. All of these are referred to by the world frame W.(7)Xk=xcyPk=PckPckyPyckPy,where Pk is the covariance matrix. The estimated feature set y is composed of n point features yi,y=y1T,y2T,…,ynTT. Point features are parametrized in inverse depth coordinate [22] which can be defined by the dimension 6-state vector:(8)yi,Id=xi,yi,zi,θi,ϕi,ρiT.The inverse depth parametrization stores in its six parameters the 3D camera position when the feature was initializedxi,yi,ziT, the azimuth-elevation pair θi,ϕiT encoding the unit ray pointing to the feature, and its inverse depth along the ray ρ [23]. They could be converted to Euclidean XYZ coordinates yi,E=Xi,Yi,ZiT if and when the projection equation becomes linear enough.(9)yi,E=XiYiZi=xiyizi+1ρimθi,ϕi,where mθi,φi=cos⁡ϕisin⁡θi,-sin⁡ϕi,cos⁡ϕicos⁡θiT.


## 3.2. Dynamic Model

The camera statexc is composed of pose terms: rWC camera optical center position and σWC modified Rodrigues parameters (MRPs) defining orientation; linear and angular velocity vW and ωC relative to world frame W and camera frame C.The state update equation for the camera is(10)fv=rk+1WCσk+1WCvk+1Wωk+1C=rkWC+vkW+VkWΔt141-σkWCTσkWCI+2σkWC×+2σkWCσkWCTωkC+ΩCΔtvkW+VWωkC+ΩC,where(11)σkWC×=0-σkzWCσkyWCσkzWC0-σkxWC-σkyWCσkxWC0.vW and ΩC are zero-mean Gaussianly distributed velocity noise coming from an impulse of acceleration.


## 3.3. Measurement Model

The measurement model used in the experiments of the paper is a pinhole camera model plus a two-parameter radial distortion. The camera is assumed to be calibrated in advance. Inverse depth and Euclidean points in the state vector are first transformed to the camera reference frame:(12)hC=hxhyhz=hi,E=RCWXiYi-rWCZi(13)hC=hxhyhz=hi,Id=RCWρixiyizi-rWC+mθi,φi,where RCW represents a rotation matrix computed from the state MRPs and m(θi,φi)  m is the function converting azimuth-elevation angles to a unit vector. Points in the camera frame are then projected using the standard pinhole model:(14)hu=uv=u0-fdxhxhzv0-fdyhyhz.Heref stands for the focal length of the camera, dx and dy represent the pixel size, and u0,v0T are the image center coordinates. The imaged point is finally transformed using the two parameters k1, k2, modeled below, resulting in the distorted measurement(15)hd=udvd=u0+u-u01+k1rd2+k2rd4v0+v-v01+k1rd2+k2rd4ru=dxu-u02+dyv-v02ru=rd1+k1rd2+k2rd4.


## 4. Single Candidate RANSAC for EKF Estimation

In the case of the new single candidate RANSAC presented here, extra information for the camera motion comes from the probability distribution function that the EKF naturally propagates over time. Computational savings with respect to standard RANSAC [24] can be easily derived. The number of RANSAC random hypotheses nhyp necessary to guarantee that at least one of them is mismatch-free with probability μ can be computed using the following formula:(16)nhyp=log⁡1-μlog⁡1-1-εm,where ε is the assumed inlier ratio and m the number of measurements that instantiate the model. As a simple but illustrative example, if the inlier ratio ε is 0.4 and the probability μ equals 0.99, the number of random hypotheses would be reduced from 447 for m=5 to only 9 for m=1. The efficiency is highly improved. The update is performed in two steps using the standard extended Kalman filter equations.Firstly, we will find the matching relationship between the point features estimated from those in the last image and those extracted in the current one. The inliers in the current image are represented byzIC={z1,z2,…,zi,…,zn}.Next, we could update the state vectorxi by picking up one point zi from the set of zIC randomly.(17)Sk+1=Hk+1Pk+1∣kHk+1T+Rk+1Kk+1=Pk+1∣kHk+1TSk+1-1x^i=x^k+1∣k+Kk+1zi-hkPk+1=I-Kk+1Hk+1Pk+1∣k.Consequently we get a newzIC set, and then we finally update the EKF state and covariance.(18)Kk+1=Pk+1∣kHk+1THk+1Pk+1∣kHk+1T+Rk+1-1x^k+1=x^k+1∣k+Kk+1zi,NEW-hkPk+1=I-Kk+1Hk+1Pk+1∣k.The initial measurement for a newly observed feature inserted into the state vector is(19)fr^WC,q^WC,h,ρ0=x^i,y^i,z^i,θ^i,ϕ^i,ρ^iT,where r^WC,q^WC represent the current camera pose estimate, h=u,vT is the image observation, and ρ0 is the depth prior.In (17),(20)r^WC=x^i,y^i,z^iThW=RWCσ^WCu,v,1T,where u and v are normalized retina image coordinates. Despite hW being a non-unit directional vector, the angles by which we parametrize its direction can be calculated as(21)θiϕi=arctan⁡hxW,hzWarctan⁡-hYW,hxW2+hzW2.The covariance ofx^i, y^i, z^i, θ^i, and ϕ^i is derived from the image measurement error covariance Ri and the state covariance estimate P^k∣k. We set ρ^0=0.1 and σρ=0.5.The state covariance after feature initialization is(22)P^k∣knew=JP^k∣k000Ri000σi2JTJ=I0∂f∂rWC∂f∂qWC00∂f∂h∂f∂ρ.After each estimation step, the linearity index(23)Ld=∂2u/∂d22σd∂u/∂d=4σdd1cos⁡αis computed for every feature coded in inverse depth:(24)hxyzW=x^i-r^WC,σd=σρρi2,σρ=Pyiyi6,6,di=hxyzW,cos⁡α=mThxyzWhxyzW-1,where x^i is computed using (9) and Pyiyi is the submatrix 6∗6 covariance matrix or the corresponding considered feature. The full state covariance matrix P is transformed with the corresponding Jacobian:(25)Pnew=JPJTJ=diag⁡I,∂xi∂yi,I.In Algorithm1, a pseudocode is shown to state the core steps of single candidate RANSAC for EKF estimation.Algorithm 1:The algorithm flow.
(
1
)    input: x^k+1∣k, Pk+1∣k, zIC
(
2
)    output: x^k+1, Pk+1
(
3
)    max_hypothesis_num=0
(
4
)    nhyp=500
(
5
)    for i=1 to nhyp
(
6
)    zi=random_match(zIC)
(
7
)    hk=predict_camera_measurement(x^k+1∣k)
(
8
)    x^i=state_updatezi,x^k+1∣k,hk
(
9
)    hypothesis_num=compute_hypothesis(x^i,hk)
(
10
)  ifhypothesis_num>max_hypothesis_num
(
11
)  max_hypothesis_num=hypothesis_num
(
12
)  ε=1-hypothesis_numsizeof(zIC)
(
13
)  nhyp=log⁡1-μlog⁡ε
(
14
)  end if
(
15
)  zi,NEW=update(zIC)
(
16
)  end for
(
17
)  x^k+1,Pk+1=EKF_update(x^k+1∣k,Pk+1∣k,zi,NEW)


## 5. Experiment Results

The performance of our algorithm has been tested on real image sequences acquired with a monocular camera under space lighting conditions, based on a terrestrial simulation mockup. The experiment was carried out with a real size model and a camera with a 40° field of view and640×480 resolution, capturing monochrome image sequences at 10 fps. In the end of a dark room, the model covered with a reflective golden foil was mounted on a swivel table which operated at the speed of 2 deg/s around the vertical direction. And on the other side, the camera was fixed on a 3-degree-of-freedom rail. A direct high-power floodlight, simulating the spectrum of the sun, illuminates the surface of the model at different incident angles.Figures2(a)–2(d) show a spaceflight mockup from different view with the red circles on behalf of the tracked features and the blue ones representing the lost ones. Figures 3 and 4 display the equivalent trajectory of the camera in 2 dimensions and 3 dimensions, respectively. Because the camera was fixed in front of the mockup, the trajectory is almost round. Because the satellite is rotating continuously, the measured angle changes periodically. Figure 5 correctly exhibits the situation. There are two lines in the left column in Figure 6: the blue dotted one stands for the actual value, while the red one represents the estimated value. Meanwhile, the absolute error and relative error are shown in Figure 7. We present 3 cycles. Because we use Euler angle to express the rotation, the maximum degree is 180 deg. And the jump points just show the phase switches. If we use the angular accumulation to express the rotation, there will be no jump points. We present here a fitting function using the angular dataset and the image sequence, based on linear polynomial regression algorithm.(26)ft=0.2073t+0.1953,where t is the number of the frame in image sequence and f(t) represents the corresponding angle. Through analysis, it is pointed out that the mean error between actual spinning rate and the estimated is below 0.1 deg/s.Figure 2
Example of images taken from the monocular sequence.
(a)
(b)
(c)
(d)Figure 3
The corresponding equivalent trajectory of the camera in 2 dimensions.Figure 4
The corresponding equivalent trajectory of the camera in 3 dimensions.Figure 5
The curve of the rotating angular estimation.Figure 6
Actual and estimated curves.Figure 7
Estimation results for the experiment.


## 6. Conclusions

In this paper, we presented a system for feature-based 3D tracking of a noncooperative spaceflight. The rotational angle and velocity of the target are estimated in camera frame, using image-plane motion fields. Preliminary experimental results have been obtained from a simulation mockup, demonstrating robustness and accuracy properties of the proposed method. The rigid motion is estimated by a bank of single candidate RANSAC EKF which greatly promotes the efficiency of computation. With the combination of scale and rotational invariance feature extractor, the system becomes more robust.Future system improvements will be the integration of a laser and a camera or two cameras to obtain the depth information, possibly including different features (points, vanish points, lines, planes, etc.), and introducing loop-closure mechanisms and bag of words to improve the accuracy and stability.



---

*Source: 1787530-2017-12-27.xml*