Relative Attitude and Position Estimation for Spacecraft from Multiple Geometric Features

Jianying Wang¹, Haizhao Liang², Hui Liu¹, Dong Ye³,Zhaowei Sun³

(1. Science and Technology on Space Physics Laboratory, Beijing 100076, China; 2.Beijing Institute of Long March Vehicle, Beijing 100076, China;3.Research Center of Satellite Technology, Harbin Institute of Technology,Harbin 150001, China.)



Abstract:

This paper investigates the pose and motion estimation problem using single camera measurement for spacecraft. The leader spacecraft of three-dimensional shape is observed by a calibrated camera fixed on the follower spacecraft. Based on dual numbers, an integrated observation model is proposed based on a combination of multiple geometric features including points, lines and circles, which can improve the robustness and accuracy of the estimation algorithm. A six-degree-of-freedom relative motion model is proposed by using the dual quaternion representation, in which the rotation-translation coupling effect due to the points deviating from the center of the mass is described. Employing the proposed observation model and dynamics model, an Extended Kalman Filter is presented to estimate the relative state between the two spacecraft. Numerical simulations are performed to evaluate the proposed approaches, showing the convergence of relative estimation errors and superior estimation performance.

Key words:  dual quaternion  coupled dynamics  vision-based estimation  Kalman filtering

DOI£º10.11916/j.issn.1005-9113.2016.02.013

Clc Number:TP391.7

Fund:



Wang Jianying, Liang Haizhao, Liu Hui, Ye Dong, Sun Zhaowei. Relative Attitude and Position Estimation for Spacecraft from Multiple Geometric Features[J]. Journal of Harbin Institute of Technology, 2016, 23(2): 87-96. DOI: 10.11916/j.issn.1005-9113.2016.02.013.
Corresponding author E-mail:diandian3866@126.com Article history Received: Dec 26, 2014



Contents            Abstract            Full text            Figures/Tables            PDF


Relative Attitude and Position Estimation for Spacecraft from Multiple Geometric Features
Wang Jianying¹, Liang Haizhao², Liu Hui¹, Ye Dong³, Sun Zhaowei³    
1. Science and Technology on Space Physics Laboratory, Beijing 100076, China;
2. Beijing Institute of Long March Vehicle, Beijing 100076, China;
3. Research Center of Satellite Technology, Harbin Institute of Technology,Harbin 150001, China

Received: Dec 26, 2014
Corresponding author: E-mail:diandian3866@126.com


Abstract: This paper investigates the pose and motion estimation problem using single camera measurement for spacecraft. The leader spacecraft of three-dimensional shape is observed by a calibrated camera fixed on the follower spacecraft. Based on dual numbers, an integrated observation model is proposed based on a combination of multiple geometric features including points, lines and circles, which can improve the robustness and accuracy of the estimation algorithm. A six-degree-of-freedom relative motion model is proposed by using the dual quaternion representation, in which the rotation-translation coupling effect due to the points deviating from the center of the mass is described. Employing the proposed observation model and dynamics model, an Extended Kalman Filter is presented to estimate the relative state between the two spacecraft. Numerical simulations are performed to evaluate the proposed approaches, showing the convergence of relative estimation errors and superior estimation performance.
Key words: dual quaternion    coupled dynamics    vision-based estimation    Kalman filtering    
1 IntroductionIn recent years,it has witnessed a tremendous research interest in the spacecraft missions that consist of multiple space vehicles,such as spacecraft formation flying,autonomous rendezvous and proximity operations^[1-3]. A successful implementation of these multiple-spacecraft missions is highly dependent on the ability to determine the relative motion between the spacecraft. Typically,to estimate the relative spacecraft motion,it requires both high-fidelity dynamical models and specialized measurement methods.

The relative motion between two spacecraft is a six-degree-of-freedom (6-DOF) motion,which is composed of relative rotational motion,relative translational motion and the coupling thereof^[4]. A growing interest has been paid for the modeling of 6-DOF spacecraft relative motion in recent years. For example,Pan and Kapila^[5] addressed the coupled translational and rotational dynamics for the leader-follower spacecraft formation; Kristiansen^[6] derived the Lagrange-like 6-DOF model by combining the relative translational dynamics and the rotational one. It is worth pointing out that the translational parts in the aforementioned models only account for the relative position between the centers of the mass (c.m.) of two spacecraft,and the rotation-translation coupling in this case is just induced by external torques. However,in general missions,the relative translational dynamics of arbitrary points on the spacecraft may be concerned. Whenever one of these points does not coincide with the spacecraft¡¯s c.m,a kinematic coupling,which is essentially a projection of the rotational motion about the c.m. onto the relative translational configuration space,is obtained^[4]. It is this kinematic coupling that the current paper is concerned with. As shown in the previous work,dual quaternion has been an effective tool for motion description and dynamics modeling^[7-13].Among these works,Wang^[13] was the first to use dual quaternion for spacecraft motion modeling.To go further step,this paper employs dual quaternion to model the relative motion between arbitrary points of spacecraft,and derives the kinematically-coupled relative dynamics.

Estimating the relative motion also involves the problem of sensing the relative information between spacecraft. In the past years,the single vision-based method has been used widely for the pose estimation problem ^[14-15]. Primary researches have focused on using two-dimensional/three-dimensional (2D/3D) point correspondences^[16-19]. Among these literatures,Kim ^[18] and Xing ^[19] employed the line-of-sight (LOS) measurements and Extended Kalman Filter (EKF) to estimate the relative position and attitude for spacecraft. However,the point-based methods are often limited to the difficulty of point matching and high sensitivity to noise. In view of these issues,several researchers have investigated the pose estimation problem based on higher-level geometric features such as lines and curves. For instance,using a set of 2D/3D line correspondences,the direct solutions^[20-21],and the iterative method ^[22-23] have been proposed to estimate the external camera parameters. From a pair of conics in both 2D and 3D,Song^[24] introduced an iterative technique for the pose estimation problem. However,to the best knowledge of the authors,these methods have been seldom used for spacecraft relative motion estimation problem. In addition,nearly all the existing solutions concentrate on one specific type of correspondences,that is,they either use points or lines or conics but not a combination of features. In order to improve the robustness and accuracy for estimating the relative motion,this paper describes the feature points,lines and circles in dual number algebra,and establishes an integrated observation model based on multiple geometric features.

2 Kinematic-Coupled Dynamics Model Using Dual NumberThroughout the development of the dynamics model,some standard reference frames are to be used (see Fig. 1): ¦·_I,the standard Earth-centered,inertial,Cartesian right-hand reference frame; ¦·_l,a local-vertical,local-horizontal reference frame fixed to the c.m. of leader spacecraft,with its x_l axis directing from the spacecraft radially outward,z_l axis normal to the leader orbital plane,and y_l axis set up by the right-hand rule; ¦·_lb (resp. ¦·_fb),the body-fixed reference frame of the leader spacecraft (resp. the follower spacecraft),with its origin in the center of mass of the spacecraft,and axes pointing along the principal axes of inertia. In the following discussion,it is assumed that the leader orbital frame ¦·_l is controlled aligned with the body-fixed frame¦·_lb.

Figure 1
Figure 1 Defination of reference frames



As shown in Fig. 1(b),there exists an arbitrary feature point P_l located on the leader spacecraft. Let p_l^l be the position vector directed from the point P_l to the c.m. of the leader spacecraft. By translating the coordinate system ¦·_lb along the opposite direction of p_l^l ,one can obtain a new coordinate system ¦·_lP with its origin being the point P_l,and axes parallel to those of ¦·_lb. In the same way,a new coordinate system ¦·_fP for the follower spacectaft is also derived.

In the remainder of this paper,the following notations are employed: ${{\dot{x}}^{f}}$(resp.${{\dot{x}}^{l}}$,${{\dot{x}}^{fp}}$,${{\dot{x}}^{lp}}$) denotes the time derivative of x in ¦·_fb (resp. ¦·_lb,¦·_fP,¦·_lP); x^f (resp. x^l,x^fP,x^lP) denotes the components of x expressed in ¦·_fb (resp. ¦·_lb,¦·_fP,¦·_lP) ; ¦Ø_f^f (resp. ¦Ø_l^l, ¦Ø_fP^fP,¦Ø_lP^lP) denotes the angular velocity of ¦·_fb (resp. ¦·_lb,¦·_fP,¦·_lP) relative to ¦·_I; ¦Ø_fl,P^fP denotes the angular velocity of ¦·_fP relative to ¦·_lP,expressed in ¦·_fP. For the mathematical preliminaries including definitions of dual numbers and dual quaternions,one can refer to Refs. ^[8] and ^[13].

By definingthe operations ${{\hat{P}}_{l}}$=1+¦Å[p_l^l¡Á] and ${{\hat{P}}_{f}}$f=1+¦Åp_f^f¡Á, the velocity motor of ¦·_lP (resp.¦·_fP) can be calculated from that of ¦·_lb(resp. ¦·_fb) using the following equations:

$\left\{ \begin{align} & \hat{\omega }_{lP}^{lP}=\hat{\omega }_{lP}^{l}={{{\hat{P}}}_{l}}\hat{\omega }_{l}^{l} \\ & \hat{\omega }_{fP}^{fP}=\hat{\omega }_{fP}^{f}={{{\hat{P}}}_{f}}\hat{\omega }_{f}^{f} \\ \end{align} \right.$ (1)

where $\left[ {{\Lambda }^{\times }} \right]=\left[ \begin{matrix} 0 & -{{\Lambda }_{z}} & {{\Lambda }_{y}} \\ {{\Lambda }_{z}} & 0 & -{{\Lambda }_{x}} \\ -{{\Lambda }_{y}} & {{\Lambda }_{x}} & 0 \\\end{matrix} \right]$ represents a skew-symmetric matrix of the vector $\Lambda ={{\left[ \begin{matrix} {{\Lambda }_{x}} & {{\Lambda }_{y}} & {{\Lambda }_{z}} \\\end{matrix} \right]}^{\text{T}}};\hat{\omega }_{l}^{l}$ is the velocity motor of ¦·_lb relative to ¦·_I,which is given by $\hat{\omega }_{l}^{l}=\omega _{l}^{l}+\varepsilon \left( \omega _{l}^{l}\times r_{l}^{l} \right)$ with ¦Ø_l^l and r_l^l being the angular velocity and position vector of ¦·_lb,respectively; $\hat{\omega }_{f}^{f}=\omega _{f}^{f}+\varepsilon \left( \omega _{f}^{f}\times r_{f}^{f} \right)$ is defined in a similar way to denote the velocity motor of ¦·_fb relative to ¦·_I,with ¦Ø_f^f and r_f^f being the angular velocity and position vector of ¦·_fb,respectively; $\hat{\omega }_{lP}^{lP}$ is the velocity motor of ¦·_lP relative to ¦·_I,the definition of which is given by $\hat{\omega }_{lP}^{lP}=\omega _{lP}^{lP}+\varepsilon \left[ \omega _{lP}^{lP}\times \left( r_{l}^{l}-p_{l}^{l} \right) \right];\hat{\omega }_{fP}^{fP}$ is the velocity motor of ¦·_fP with $\hat{\omega }_{fP}^{fP}={{\omega }^{fP}}_{fP}+\varepsilon \left[ \omega _{fP}^{fP}\times \left( r_{f}^{f}-p_{f}^{f} \right) \right]$. It is noted that the properties ¦Ø_lP^lP=¦Ø_l^l and ¦Ø_fP^fP=¦Ø_f^f are used in the calculation of Eq. (1). Employing ${{\hat{q}}_{fl,P}}$ to describe the motion of ¦·_fP relative to ¦·_lP,the relative kinematics can be written as

$2{{\dot{\hat{q}}}_{fl,P}}={{\hat{q}}_{fl,P}}\cdot {{\hat{\omega }}_{fPfl,P}}$ (2)

where ${{\hat{q}}_{fl,P}}={{q}_{fl,P}}+\varepsilon \frac{1}{2}{{q}_{fl,P}}\cdot r_{fl,P}^{fP}$ with q_fl,P being the relative attitude quaternion and r_fl,P^fP being the position vector from P_l to P_f;$\hat{\omega }_{fl,P}^{fP}$ denotes the velocity motor of ¦·_fP relative to ¦·_lP,which is defined by $\hat{\omega }_{fl,P}^{fP}=\omega _{fl,P}^{fP}+\varepsilon \left( \dot{r}_{fl,P}^{fP}+\omega _{fl,P}^{fP}\times {{r}_{fPfl,P}} \right)$ with ¦Ø_fl,P^fP being the relative angular velocity. Throughout this paper,the following appointment is made: a vector is treated as a quaternion with null scalar part when calculated with quaternions,and a dual vector is regarded as a dual quaternion with null dual scalar part when calculated with dual quaternions.

In terms of Eq. (1),$\hat{\omega }_{fl,P}^{fP}$ can be also expressed as

$\hat{\omega }_{fl,P}^{fP}={{\hat{P}}_{f}}\hat{\omega }_{f}^{f}-\hat{q}_{fl,P}^{*}\cdot \left( {{{\hat{P}}}_{l}}\hat{\omega }_{l}^{l} \right)\cdot {{\hat{q}}_{fl,P}}$ (3)

where $\hat{q}_{fl,P}^{*}=q_{fl,P}^{*}+\varepsilon {{{q}'}^{*}}$ is the conjugate of ${{\hat{q}}_{fl,P}}$. By differentiating Eq. (3) and according to the dynamics of a single spacecraft in Ref. ^[13],one can obtain

$\begin{align} & \dot{\hat{\omega }}_{fl,P}^{fP}={{{\hat{P}}}_{f}}\dot{\hat{\omega }}_{f}^{f}-\hat{q}_{fl,P}^{*}\cdot \left( {{{\hat{P}}}_{l}}\dot{\hat{\omega }}_{l}^{l} \right)\cdot {{{\hat{q}}}_{fl,P}}- \\ & \hat{q}_{fl,P}^{*}\cdot \left( {{{\hat{P}}}_{l}}\hat{\omega }_{l}^{l} \right)\cdot \hat{q}_{fl,P}^{\cdot }- \\ & \dot{\hat{q}}_{fl,P}^{*}\cdot \left( {{{\hat{P}}}_{l}}\hat{\omega }_{l}^{l} \right)\cdot {{{\hat{q}}}_{fl,P}} \\ & ={{{\hat{P}}}_{f}}\dot{\hat{\omega }}_{f}^{f}-\hat{q}_{fl,P}^{*}\cdot \left( {{{\hat{P}}}_{l}}\dot{\hat{\omega }}_{l}^{l} \right)\cdot {{{\hat{q}}}_{fl,P}}- \\ & \hat{q}_{fl,P}^{*}\cdot \left( {{{\hat{P}}}_{l}}\hat{\omega }_{\text{l}}^{\text{l}} \right)\cdot \frac{1}{2}{{{\hat{q}}}_{fl,P}}\cdot \hat{\omega }_{fl,P}^{f}+ \\ & \frac{1}{2}\hat{\omega }_{fl,P}^{\text{f}}\cdot \hat{q}_{fl,P}^{*}\cdot \left( {{{\hat{P}}}_{l}}\hat{\omega }_{l}^{l} \right)\cdot {{{\hat{q}}}_{fl,P}} \\ & ={{{\hat{P}}}_{f}}\left( -\hat{M}_{f}^{-1}\left( \hat{\omega }_{f}^{f}\times {{{\hat{M}}}_{f}}\hat{\omega }_{f}^{f} \right)+\hat{M}_{f}^{-1}\hat{F}_{f}^{f} \right)- \\ & \hat{q}_{fl,P}^{*}\cdot \left( {{{\hat{P}}}_{l}}\dot{\hat{\omega }}_{l}^{l} \right)\cdot {{{\hat{q}}}_{fl,P}}+\hat{\omega }_{fl,P}^{f}\times \\ & \left( \hat{q}_{fl,P}^{*}\cdot \left( {{{\hat{P}}}_{l}}\hat{\omega }_{l}^{l} \right)\cdot {{{\hat{q}}}_{fl,P}} \right) \\ \end{align}$

where the dual inertia matrix of the follower spacecraft is given by ${{\hat{M}}_{f}}={{m}_{f}}\frac{\text{d}}{\text{d}\varepsilon }I+\varepsilon {{J}_{f}}$ with m_f being the mass and J_f being the inertia matrix,the inverse of ${{\hat{M}}_{f}}$ is defined as $\hat{M}_{f}^{-1}=J_{f}^{-1}\frac{\text{d}}{\text{d}\varepsilon }+\varepsilon \frac{1}{{{m}_{\text{f}}}}I;\hat{F}_{f}^{f}=f_{f}^{f}+\tau _{f}^{f}$ with f_f^f and ¦Ó_f^f being the force vector and the torque of the follower,respectively; $\hat{\omega }_{f}^{f}$ can be obtained by $\hat{\omega }_{f}^{f}=\hat{P}_{f}^{*}\left( \hat{\omega }_{fl,P}^{fP}+q_{fl,P}^{*}\cdot \left( {{{\hat{P}}}_{l}}\hat{\omega }_{l}^{l} \right)\cdot {{{\hat{q}}}_{fl,P}} \right)$ with $\hat{P}_{f}^{*}=1-\varepsilon \left[ p_{f}^{f\times } \right]$.

3 Observation Models Using Different Geometric Feature CorrespondencesBefore developing the observation models in this section,two reference frames are defined. The camera reference frame ¦·_c,a 3D Cartesian right-hand coordinate system with its origin is located at the lens center of the camera,x_c axis along the horizontal direction of the image,y_c axis along the vertical direction of the image,and z_c axis parallel to the optical axis of the camera. The image reference frame ¦·_i,a 2D coordinate system,contained in the image plane,and is parallel to the x_c and y_c axes of ¦·_c. Without loss of generality,it is assumed that ¦·_c is aligned with ¦·_fP which is defined in the previous section.

3.1 Observation Model Based on Feature PointsIn this subsection,the pinhole lens model is used as the perspective projection model,and the process of a 3-D point to its 2-D projection is briefly summarized as follows.

Let D be a 3D point fixed on the leader spacecraft. The first step is to transform the point D to the camera reference frame. In the dual quaternion algebra,this transformation can be expressed as

${{\hat{D}}^{fP}}=\hat{q}_{fl,P\varepsilon }^{*}{{\hat{D}}^{lP}}{{\hat{q}}_{fl,P}}$

where ${{\hat{q}}_{fl,P}}$ represents the relative motion from ¦·_lP to ¦·_fP, $\hat{q}_{fl,P\varepsilon }^{*}-q_{fl,P}^{*}-\varepsilon {{{q}'}^{*}}_{fl,P}$ is another definition of the conjugate of ${{\hat{q}}_{fl,P}};{{\hat{D}}^{fP}}=1+\varepsilon {{d}^{fP}}$ and ${{\hat{D}}^{lP}}=1+\varepsilon {{d}^{lP}}$ are the representations of the point D in dual quaternion algebra,with d^fP and d^lP being the coordinates relative to ¦·_fP and ¦·_lP,respectively.

Next step is to project the transformed points onto the image plane. The result is the x and y coordinates of the projected points. Given the camera coordinates ${{d}^{fP}}={{\left[ \begin{matrix} {{d}_{fpx}} & {{d}_{fpy}} & {{d}_{fpz}} \\\end{matrix} \right]}^{\text{T}}}$,the image coordinates are calculated by

${{x}_{id}}=\lambda \frac{{{d}_{fpx}}}{{{d}_{fpz}}},{{y}_{id}}=\lambda \frac{{{d}_{fpy}}}{{{d}_{fpz}}}$ (4)

where ¦Ë is the focal length of the camera.

3.2 Observation Model Based on Feature LinesIn this section,the given model features from the leader spacecraft are lines. The projection of a 3D line onto the 2D image is shown in Fig. 2. In the dual quaternion algebra,the expression of the feature line L relative to ¦·_lP is represented by

Figure 2
Figure 2 Observation model based on feature Lines



${{\hat{L}}^{lP}}={{l}^{lP}}+\varepsilon {{m}^{lP}}$

where l^lP denotes the unit direction vector and m^lP is the moment with respect to the origin of ¦·_lP. Using dual quaternion,one can calculate the expression of $\hat{L}$ in ¦·_fP as follows:

${{\hat{L}}^{fP}}={{l}^{fP}}+\varepsilon {{m}^{fP}}=\hat{q}_{fl,p}^{*}\cdot {{\hat{L}}^{lP}}\cdot {{\hat{q}}_{fl,p}}$ (5)

Perspective projection is then applied to the transformed lines. In terms of projection geometry,an equation of the projected line is obtained:

${{m}_{x}}{{x}_{i}}+{{m}_{y}}{{y}_{i}}={{m}_{z}}\lambda $ (6)

where x_i and y_i are the image plane coordinates. From Eq. (9),one can calculate the direction vector of the image line by

${{l}_{i}}={{\left[ \begin{matrix} {{l}_{ix}} & {{l}_{iy}} & {{l}_{iz}} \\\end{matrix} \right]}^{\text{T}}}={{\left[ \begin{matrix} \frac{-{{m}_{y}}}{\sqrt{m_{x}^{2}+m_{y}^{2}}} & \frac{{{m}_{x}}}{\sqrt{m_{x}^{2}+m_{y}^{2}}} & 0 \\\end{matrix} \right]}^{\text{T}}}$ (7)

Since the momentum of the image line m_i defines the same plane as m,one may have

${{m}_{i}}={{\left[ \begin{matrix} {{m}_{ix}} & {{m}_{iy}} & {{m}_{iz}} \\\end{matrix} \right]}^{\text{T}}}={{d}_{pc}}\frac{m}{\left\| m \right\|}=\frac{\lambda }{\sqrt{m_{x}^{2}+m_{y}^{2}}}m$

where d_pc is the distance from the image line to the perspective center.

A format is needed to compare these transformed lines with lines measured from the acquired images. In Fig. 2(b),the line point^[23],which is defined as the intersection of the line feature with a line passing through the image origin that is perpendicular to the line feature,is employed. From the image line,the coordinates of the line point are calculated as

${{x}_{il}}={{l}_{iy}}{{m}_{iz}},{{y}_{il}}=-{{l}_{ix}}{{m}_{iz}}$ (8)

In terms of Eqs. (7) and (8),the line point can be also derived from the 3D transformed line by the equation:

${{x}_{il}}=\lambda \frac{{{m}_{x}}{{m}_{z}}}{m_{x}^{2}+m_{y}^{2}},{{y}_{il}}=\lambda \frac{{{m}_{y}}{{m}_{z}}}{m_{x}^{2}+m_{y}^{2}}$ (9)

Therefore,the relations between the line point from the acquired image and the 3D line on the leader spacecraft are established by Eqs. (5) and (9),and these equations can be used as the observation model for the estimation problem.

3.3 Observation Model Based on Feature CirclesBesides the point and line features,circle features are also common tobe found on a spacecraft. However,in the dual quaternion algebra,circles cannot be represented and transformed directly like the way of the points and lines. For the aim of dealing with all the three types of features in a unified algebra,this section proposes an operation definition for circles. With this definition,the equations which related the 3D circles and 2D conics are formalized accordingly.

As shown in Fig. 3,a circle can be regarded as a twist L_t modeling a general rotation and a starting point S₀ on the circle. According to the definition of screw motion,this twist transformation can be understood as a screw motion with the screw pitch being zero.Given the generating twist parameters,the twist transformation corresponds to the dual quaternion:

Figure 3
Figure 3 The operational definition of circles



${{\hat{q}}_{\phi }}=\left[ \cos \frac{\phi }{2},{{{\hat{L}}}_{t}}\sin \frac{\phi }{2} \right]$

where $\hat{\phi }=\phi +\varepsilon 0$ with $\phi $ being the rotation angle; ${{\hat{L}}_{t}}={{l}_{t}}+\varepsilon {{m}_{t}}$ denotes the generating twist with l_t being the direction vector and m_t being the moment with respect to S₀. By this dual quaternion,an arbitrary point on the circle can be simply obtained from the starting point by

$\hat{S}_{\phi }^{lP}={{\hat{q}}_{\phi }}\hat{S}_{0}^{lP}\hat{q}_{\phi ,\varepsilon }^{*}$ (10)

where $\hat{S}_{0}^{lP}=1+\varepsilon s_{0}^{lP}$ and $\hat{S}_{\phi }^{lP}=1+\varepsilon s_{\phi }^{lP}$ denote the expressions of the starting point S₀ and the result point S_¦Õ in ¦·_lP,respectively. $\hat{S}_{\phi }^{lP}$ is then transformed to the camera coordinates using the following equation:

$\hat{S}_{\phi }^{fP}=1+\varepsilon S_{\phi }^{fP}=q_{fl,P\varepsilon }^{*}\hat{S}_{\phi }^{lP}{{\hat{q}}_{fl,P}}$

In terms of Eq. (4),the image coordinates of the point S_¦Õ are calculated by

${{x}_{is}}=\lambda \frac{{{s}_{\phi fPx}}}{{{S}_{\phi fPz}}},{{y}_{is}}=\lambda \frac{{{s}_{\phi fPy}}}{{{S}_{\phi fPz}}}$ (11)

where s_¦ÕfPx,s_¦ÕfPy and s_¦ÕfPz are the components of the s_¦Õ^fP.

Taking the image coordinatesx_is,y_is as the measurement variables,the observation model based on the circle features is now set up with the above equations.It should be noted that ${{\hat{q}}_{fl,p}}$ are the only unknowns to be estimated in the measurement modelsof thefeature points and lines. While for the case of feature circles,the computing loads become higher due to the parameterization of the circle itself. For the measurement equations,the angle ¦Õ is the additional unknown parameter.Therefore,the relative estimation problem using circle features means to estimate both the dual quaternion ${{\hat{q}}_{fl,p}}$ and the angle ¦Õ.

4 Relative Motion Estimation Based on Multiple Features4.1 Process ModelThe state vector of the system is selected as

$X={{\left[ \begin{matrix} {{{\hat{q}}}_{fl,P}} & \hat{\omega }_{fl,P}^{fP} & \Phi \\\end{matrix} \right]}^{\text{T}}}$

where ¦µ=[¦Õ₁¡­¦Õ_3n] are the unknown parameters of the observed n feature circles. Based on the kinematics and dynamics proposed in Section 2,one may have the following continuous-time process model:

$\dot{X}\left( t \right)=f\left( X\left( t \right),t \right)+w\left( t \right)$

where f(X(t),t) is a non-linear function given by

$\begin{align} & f\left( X\left( t \right),t \right)={{\left[ \begin{matrix} {{{\dot{\hat{q}}}}_{fl,P}} & \dot{\hat{\omega }}_{fl,P}^{fP} & {\dot{\Phi }} \\\end{matrix} \right]}^{\text{T}}}= \\ & \left[ \begin{matrix} \frac{1}{2}{{{\hat{q}}}_{fl,P}}\cdot \hat{\omega }_{fl,P}^{fP} \\ \begin{matrix} {{{\hat{P}}}_{f}}\left( -\hat{M}_{f}^{-1}\left( \hat{\omega }_{f}^{f}\times {{{\hat{M}}}_{f}}\hat{\omega }_{f}^{f} \right)+\hat{M}_{f}^{-1}\hat{F}_{f}^{f} \right)- \\ \hat{q}_{fl,P}^{*}\cdot \left( {{{\hat{P}}}_{l}}\dot{\hat{\omega }}_{l}^{l} \right)\cdot {{{\hat{q}}}_{fl,P}}+\hat{\omega }_{fl,P}^{fP}\times \left( \hat{q}_{fl,P}^{*}\cdot \left( {{{\hat{P}}}_{l}}\hat{\omega }_{l}^{l} \right)\cdot {{{\hat{q}}}_{fl,P}} \right) \\\end{matrix} \\ {{0}_{3n\times 1}} \\\end{matrix} \right] \\ \end{align}$

where w(t) is a white Gaussian process noise with power spectral density matrix Q.

Linearizing and discretizing of the process model yields

${{X}_{k}}={{F}_{k,k-1}}{{X}_{k-1}}+{{W}_{k-1}}$

where F_k,k£­1=I+C_k£­1T with T being the sample time,and

${{\left. {{C}_{k-1}}=\frac{\partial \text{f}}{\partial {{X}^{\text{T}}}} \right|}_{X={{X}_{k-1}}}}=\left[ \begin{matrix} \frac{\partial {{{\dot{\hat{q}}}}_{fl,P}}}{\partial {{{\hat{q}}}^{\text{T}}}_{fl,P}} & \frac{\partial {{{\dot{\hat{q}}}}_{fl,P}}}{\partial \hat{\omega }_{fl,P}^{fPT}} & {{0}_{8\times 3n}} \\ \frac{\partial {{{\dot{\hat{\omega }}}}_{fl,P}}}{\partial {{{\hat{q}}}^{\text{T}}}_{fl,P}} & \frac{\partial {{{\dot{\hat{\omega }}}}_{fl,P}}}{\partial {{{\hat{\omega }}}^{fPT}}_{fl,P}} & {{0}_{6\times 3n}} \\ {{0}_{3n\times 8}} & {{0}_{3n\times 6}} & {{0}_{3n\times 3n}} \\\end{matrix} \right]$

with the expressions given as follows:

$\begin{align} & \frac{\partial {{{\dot{\hat{q}}}}_{fl,P}}}{\partial {{{\hat{q}}}^{\text{T}}}_{fl,P}}=\frac{1}{2}\left[ \hat{\omega }_{fl,P}^{fP-} \right],\frac{\partial {{{\dot{\hat{q}}}}_{fl,P}}}{\partial \hat{\omega }_{fl,P}^{fPT}}=\frac{1}{2}\left[ \hat{q}_{fl,P}^{+} \right] \\ & \frac{\partial {{{\dot{\hat{\omega }}}}_{fl,P}}}{\partial {{{\hat{q}}}^{\text{T}}}_{fl,P}}=-\left( {{\left[ \hat{q}_{fl,P}^{+} \right]}^{\text{T}}}\left[ {{\left( {{{\hat{P}}}_{l}}\hat{\omega }_{l}^{l} \right)}^{+}} \right]+ \right. \\ & \left. \left[ \hat{q}_{fl,P}^{-} \right]\left[ {{\left( {{{\hat{P}}}_{l}}\dot{\hat{\omega }}_{l}^{l} \right)}^{-}} \right]{{E}_{8}} \right)+ \\ & \left[ \hat{\omega }_{fl,P}^{fP\times } \right]\left( {{\left[ \hat{q}_{fl,P}^{+} \right]}^{\text{T}}}\left[ {{\left( {{{\hat{P}}}_{l}}\hat{\omega }_{l}^{l} \right)}^{+}} \right] \right.+ \\ & \left. \left[ \hat{q}_{fl,P}^{-} \right]\left[ {{\left( {{{\hat{P}}}_{l}}\hat{\omega }_{l}^{l} \right)}^{-}} \right]{{E}_{8}} \right)+ \\ & {{{\hat{P}}}_{f}}\left( \hat{M}_{f}^{-1}\left( \left( \left[ {{\left( {{{\hat{M}}}_{f}}\hat{\omega }_{f}^{f} \right)}^{\times }} \right] \right. \right. \right.- \\ & \left. \left. \left. \left[ \hat{\omega }_{f}^{f\times } \right]{{{\hat{M}}}_{f}} \right)\frac{\partial \hat{\omega }_{f}^{f}}{\partial \hat{q}_{fl,P}^{\text{T}}}+\frac{\partial \hat{F}_{f}^{f}}{\partial \hat{q}_{fl,P}^{\text{T}}} \right) \right) \\ & \frac{\partial {{{\dot{\hat{\omega }}}}_{fl,P}}}{\partial {{{\hat{\omega }}}^{fPT}}_{fl,P}}={{{\hat{P}}}_{f}}\left( \hat{M}_{f}^{-1}\left( \left[ {{\left( {{{\hat{M}}}_{f}}\hat{\omega }_{f}^{f} \right)}^{\times }} \right]-\left[ \hat{\omega }_{f}^{f\times } \right]{{{\hat{M}}}_{f}} \right) \right)- \\ & \left[ {{\left( \hat{q}_{fl,P}^{*}\cdot {{{\hat{P}}}_{f}}\hat{\omega }_{l}^{l}\cdot {{{\hat{q}}}_{fl,P}} \right)}^{\times }} \right]+{{{\hat{P}}}_{f}}\left( \hat{M}_{f}^{-1}\frac{\partial \hat{F}_{f}^{f}}{\partial \hat{\omega }_{fl,P}^{fPT}} \right) \\ \end{align}$

where

$\frac{\partial \hat{\omega }_{f}^{f}}{\partial {{{\hat{q}}}^{\text{T}}}_{fl,P}}=\hat{P}_{f}^{*}\left( {{\left[ \hat{q}_{fl,P}^{+} \right]}^{\text{T}}}\left[ {{\left( {{{\hat{P}}}_{l}}\hat{\omega }_{l}^{l} \right)}^{+}} \right]+\left[ \hat{q}_{fl,P}^{-} \right]\left[ {{\left( {{{\hat{P}}}_{l}}\hat{\omega }_{l}^{l} \right)}^{-}} \right]{{E}_{8}} \right)$

considering the gravitation force $f_{f}^{f}=\frac{-{{\mu }_{\oplus }}{{m}_{f}}}{{{\left\| r_{f}^{f} \right\|}^{3}}}r_{f}^{f}$,one can obtain that $\frac{\partial \hat{F}_{f}^{f}}{\partial \hat{q}_{fl,P}^{\text{T}}}=\frac{\partial \hat{F}_{f}^{f}}{\partial r_{f}^{f\text{T}}}\frac{\partial r_{f}^{f}}{\partial \hat{q}_{fl,P}^{\text{T}}}=\left[ \frac{3{{\mu }_{\oplus }}{{m}_{f}}}{{{\left\| r_{f}^{f} \right\|}^{5}}}r_{f}^{f}r_{f}^{f\text{T}}-\frac{{{\mu }_{\oplus }}{{m}_{f}}}{{{\left\| r_{f}^{f} \right\|}^{3}}}{{I}_{3}};{{0}_{3\times 3}} \right]\frac{\partial r_{f}^{f}}{\partial \hat{q}_{fl,P}^{\text{T}}}$ and ªµ$\frac{\partial \hat{F}_{f}^{f}}{\partial \hat{\omega }_{fl,P}^{fPT}}={{0}_{6\times 6}}$,with $r_{f}^{f}=r_{fl,P}^{fP}+q_{fl,P}^{*}\cdot \left( r_{l}^{l}-p_{l}^{l} \right)\cdot {{q}_{fl,P}}+p_{f}^{f}$ and ${{\partial r_f^f} \over {\partial q_{fl,P}^{\rm{T}}}} = 2\left[ {q\rm{'}_{fl,P}^ - } \right]{E_4} + {\left[ {q_{fl.P}^ + } \right]^{\rm{T}}}\left[ {{{\left( {r_l^l - p_l^l} \right)}^ + }} \right] + \left[ {q_{fl,P}^ - } \right]\left[ {{{\left( {r_l^l - p_l^l} \right)}^ - }} \right]{E_4},$${{\partial r_f^f} \over {\partial q\rm{'}_{fl,P}^{\rm{T}}}} = 2{\left[ {q_{fl,P}^ + } \right]^{\rm{T}}};\left[ {{{\hat \Lambda }^ \times }} \right] = \left[ {\matrix{ {\left[ {{\Lambda ^ \times }} \right]} & {{0_{3 \times 3}}} \cr {\left[ {\Lambda ' \times } \right]} & {\left[ {{\Lambda ^ \times }} \right]} \cr } } \right]$ is defined for $\hat{\Lambda }=\Lambda +\varepsilon {\Lambda }';{{E}_{4}}=\left[ \begin{matrix} 1 & {{0}_{1\times 3}} \\ {{0}_{3\times 1}} & -{{I}_{3}} \\\end{matrix} \right],{{E}_{8}}=\left[ \begin{matrix} {{E}_{4}} & {{0}_{4\times 4}} \\ {{0}_{4\times 4}} & {{E}_{4}} \\\end{matrix} \right]$.

4.2 Integrated Observation Model It is supposed that there are g feature points,m feature lines and n feature circles can be observed,the measurement variable can be denoted as

Z=[x_id1 y_id1¡­x_idg y_idg x_il1 y_il1¡­x_ilm y_ilm x_is1 y_is1¡­x_isn y_isn]^T

and the nonlinear measurement function is given by

$Z\left( t \right)=h\left( X\left( t \right),t \right)+v\left( t \right)$ (12)

where h(X(t),t) can be obtained from Eqs. (4),(9) and (11),v(t) is the noise of the measurement which is assumed to be white Gaussian noise with power spectral density matrix R. After linearizing and discretizing Eq. (12),one can obtain

${{Z}_{k}}={{H}_{k}}{{X}_{k}}+{{V}_{k}}$

where H_k is the observation matrix,which can be derived from the partial derivative of h(X(t),t):

${{\left. {{H}_{k}}=\frac{\partial h}{\partial {{X}^{\text{T}}}} \right|}_{X={{X}_{k}}}}={{\left[ \frac{\partial {{x}_{id1}}}{\partial {{X}^{\text{T}}}}\frac{\partial {{y}_{id1}}}{\partial {{X}^{\text{T}}}}\cdots \frac{\partial {{x}_{idg}}}{\partial {{X}^{\text{T}}}}\frac{\partial {{y}_{idg}}}{\partial {{X}^{\text{T}}}}\frac{\partial {{x}_{il1}}}{\partial {{X}^{\text{T}}}}\frac{\partial {{y}_{il1}}}{\partial {{X}^{\text{T}}}}\cdots \frac{\partial {{x}_{ilm}}}{\partial {{X}^{\text{T}}}}\frac{\partial {{y}_{ilm}}}{\partial {{X}^{\text{T}}}}\frac{\partial {{x}_{is1}}}{\partial {{X}^{\text{T}}}}\frac{\partial {{y}_{is1}}}{\partial {{X}^{\text{T}}}}\cdots \frac{\partial {{x}_{isn}}}{\partial {{X}^{\text{T}}}}\frac{\partial {{y}_{isn}}}{\partial {{X}^{\text{T}}}} \right]}^{\text{T}}}$

with the expressions given as follows.

For the measurement variables of feature points,

$\begin{align} & \frac{\partial {{x}_{id}}}{\partial {{d}^{fPT}}}=\left[ \begin{matrix} \frac{\lambda }{{{d}_{fPz}}} & 0 & -\frac{\lambda {{d}_{fPx}}}{d_{fPz}^{2}} \\\end{matrix} \right] \\ & \frac{\partial {{y}_{id}}}{\partial {{d}^{fPT}}}=\left[ \begin{matrix} 0 & \frac{\lambda }{{{d}_{fPz}}} & -\frac{\lambda {{d}_{fPx}}}{d_{fPz}^{2}} \\\end{matrix} \right] \\ & \frac{\partial {{d}^{fP}}}{\partial {{X}^{\text{T}}}}={{\left[ \begin{matrix} \frac{\partial {{d}^{fP}}}{\partial q_{fl,P}^{\text{T}}} & \frac{\partial {{d}^{fP}}}{\partial q\rm{'}_{fl,P}^{\text{T}}} & \frac{\partial {{d}^{fP}}}{\partial \hat{\omega }_{fl,P}^{fP\text{T}}} & \frac{\partial {{d}^{fP}}}{\partial {{\Phi }^{\text{T}}}} \\\end{matrix} \right]}^{\text{T}}} \\ \end{align}$

where

$\begin{align} & \frac{\partial {{x}_{id}}}{\partial {{d}^{fPT}}}=\left[ \begin{matrix} \frac{\lambda }{{{d}_{fPz}}} & 0 & -\frac{\lambda {{d}_{fPx}}}{d_{fPz}^{2}} \\\end{matrix} \right] \\ & \frac{\partial {{y}_{id}}}{\partial {{d}^{fPT}}}=\left[ \begin{matrix} 0 & \frac{\lambda }{{{d}_{fPz}}} & -\frac{\lambda {{d}_{fPx}}}{d_{fPz}^{2}} \\\end{matrix} \right] \\ & \frac{\partial {{d}^{fP}}}{\partial {{X}^{\text{T}}}}={{\left[ \begin{matrix} \frac{\partial {{d}^{fP}}}{\partial q_{fl,P}^{\text{T}}} & \frac{\partial {{d}^{fP}}}{\partial q\rm{'}_{fl,P}^{\text{T}}} & \frac{\partial {{d}^{fP}}}{\partial \hat{\omega }_{fl,P}^{fP\text{T}}} & \frac{\partial {{d}^{fP}}}{\partial {{\Phi }^{\text{T}}}} \\\end{matrix} \right]}^{\text{T}}} \\ \end{align}$

with

$\begin{align} & \frac{\partial {{d}^{fP}}}{\partial q_{fl,P}^{\text{T}}}=\left[ q_{fl,P}^{-} \right]\left[ {{d}^{lP-}} \right]{{E}_{4}}+{{\left[ q_{fl,P}^{+} \right]}^{\text{T}}}\left[ {{d}^{lP+}} \right]+ \\ & \left[ q\rm{'}_{fl,P}^{-} \right]{{E}_{4}}-{{\left[ q\rm{'}_{fl,P}^{+} \right]}^{\text{T}}} \\ & \frac{\partial {{d}^{fP}}}{\partial q\rm{'}_{fl,P}^{\text{T}}}={{\left[ q_{fl,P}^{+} \right]}^{\text{T}}}-\left[ q_{fl,P}^{-} \right]{{E}_{4}} \\ & \frac{\partial {{d}^{fP}}}{\partial \hat{\omega }_{fl,P}^{fPT}}={{0}_{3\times 6}},\frac{\partial {{d}^{fP}}}{\partial {{\Phi }^{\text{T}}}}={{0}_{3\times 3n}} \\ \end{align}$

For the measurement variables of feature lines,

$\frac{\partial {{x}_{il}}}{\partial {{X}^{\text{T}}}}=\frac{\partial {{x}_{il}}}{\partial {{m}^{fPT}}}\frac{\partial {{m}^{fP}}}{\partial {{X}^{\text{T}}}},\frac{\partial {{y}_{il}}}{\partial {{X}^{\text{T}}}}=\frac{\partial {{y}_{il}}}{\partial {{m}^{fPT}}}\frac{\partial {{m}^{fP}}}{\partial {{X}^{\text{T}}}}$

where

$\begin{align} & \frac{\partial {{x}_{il}}}{\partial {{m}^{fPT}}}={{\left[ \begin{matrix} \frac{\lambda {{m}_{z}}}{m_{x}^{2}+m_{y}^{2}}-2\frac{\lambda m_{x}^{2}{{m}_{z}}}{{{\left( m_{x}^{2}+m_{y}^{2} \right)}^{2}}} \\ -2\frac{\lambda {{m}_{x}}{{m}_{y}}{{m}_{z}}}{{{\left( m_{x}^{2}+m_{y}^{2} \right)}^{2}}} \\ \frac{\lambda {{m}_{x}}}{m_{x}^{2}+m_{y}^{2}} \\\end{matrix} \right]}^{\text{T}}} \\ & \frac{\partial {{y}_{il}}}{\partial {{m}^{fPT}}}=\left[ \begin{matrix} -2\frac{\lambda {{m}_{x}}{{m}_{y}}{{m}_{z}}}{{{\left( m_{x}^{2}+m_{y}^{2} \right)}^{2}}} \\ \frac{\lambda {{m}_{z}}}{m_{x}^{2}+m_{y}^{2}}-2\frac{\lambda m_{y}^{2}{{m}_{z}}}{{{\left( m_{x}^{2}+m_{y}^{2} \right)}^{2}}} \\ \frac{\lambda {{m}_{y}}}{m_{x}^{2}+m_{y}^{2}} \\\end{matrix} \right] \\ & \frac{\partial {{m}^{fP}}}{\partial {{X}^{\text{T}}}}={{\left[ \begin{matrix} \frac{\partial {{m}^{fP}}}{\partial \hat{q}_{fl,P}^{\text{T}}} & \frac{\partial {{m}^{fP}}}{\partial \hat{\omega }_{fl,P}^{fPT}} & \frac{\partial {{m}^{fP}}}{\partial {{\Phi }^{\text{T}}}} \\\end{matrix} \right]}^{\text{T}}} \\ \end{align}$

where

$\begin{align} & \frac{\partial {{d}^{fP}}}{\partial q_{fl,P}^{\text{T}}}=\left[ q_{fl,P}^{-} \right]\left[ {{d}^{lP-}} \right]{{E}_{4}}+{{\left[ q_{fl,P}^{+} \right]}^{\text{T}}}\left[ {{d}^{lP+}} \right]+ \\ & \left[ q\rm{'}_{fl,P}^{-} \right]{{E}_{4}}-{{\left[ q\rm{'}_{fl,P}^{+} \right]}^{\text{T}}} \\ & \frac{\partial {{d}^{fP}}}{\partial q\rm{'}_{fl,P}^{\text{T}}}={{\left[ q_{fl,P}^{+} \right]}^{\text{T}}}-\left[ q_{fl,P}^{-} \right]{{E}_{4}} \\ & \frac{\partial {{d}^{fP}}}{\partial \hat{\omega }_{fl,P}^{fPT}}={{0}_{3\times 6}},\frac{\partial {{d}^{fP}}}{\partial {{\Phi }^{\text{T}}}}={{0}_{3\times 3n}} \\ \end{align}$

For the measurement variables of feature circles,

$\frac{\partial {{x}_{is}}}{\partial {{X}^{\text{T}}}}=\frac{\partial {{x}_{is}}}{\partial s_{\phi }^{fPT}}\frac{\partial s_{\phi }^{fP}}{\partial {{X}^{\text{T}}}},\frac{\partial {{y}_{is}}}{\partial {{X}^{\text{T}}}}=\frac{\partial {{y}_{is}}}{\partial s_{\phi }^{fPT}}\frac{\partial s_{\phi }^{fP}}{\partial {{X}^{\text{T}}}}$

where

$\begin{align} & \frac{\partial {{x}_{is}}}{\partial s_{\phi }^{fPT}}=\left[ \begin{matrix} \frac{\lambda }{{{s}_{\phi fPz}}} & 0 & -\frac{\lambda {{s}_{\phi fPx}}}{s_{\phi fPz}^{2}} \\\end{matrix} \right] \\ & \frac{\partial {{y}_{is}}}{\partial s_{\phi }^{fPT}}=\left[ \begin{matrix} 0 & \frac{\lambda }{{{s}_{\phi fPz}}} & -\frac{\lambda {{s}_{\phi fPy}}}{s_{\phi fPz}^{2}} \\\end{matrix} \right] \\ & \frac{\partial s_{\phi }^{fP}}{\partial {{X}^{\text{T}}}}={{\left[ \begin{matrix} \frac{\partial s_{\phi }^{fP}}{\partial q_{fl,P}^{\text{T}}} & \frac{\partial s_{\phi }^{fP}}{\partial q\rm{'}_{fl,P}^{\text{T}}} & \frac{\partial s_{\phi }^{fP}}{\partial \hat{\omega }_{fl,P}^{fPT}} & \frac{\partial s_{\phi }^{fP}}{\partial {{\Phi }^{\text{T}}}} \\\end{matrix} \right]}^{\text{T}}} \\ \end{align}$

where

$\begin{align} & \frac{\partial s_{\phi }^{fP}}{\partial q_{fl,P}^{\text{T}}}=\left[ q_{fl,P}^{-} \right]\left[ s_{\phi }^{lP-} \right]{{E}_{4}}+{{\left[ q_{fl,P}^{+} \right]}^{\text{T}}}\left[ s_{\phi }^{lP+} \right]+ \\ & \left[ q\rm{'}_{fl,P}^{-} \right]{{E}_{4}}-{{\left[ q\rm{'}_{fl,P}^{+} \right]}^{\text{T}}} \\ & \frac{\partial s_{\phi }^{fP}}{\partial q_{fl,P}^{\text{T}}}={{\left[ q_{fl,P}^{+} \right]}^{\text{T}}}-\left[ q_{fl,P}^{-} \right]{{E}_{4}},\frac{\partial s_{\phi }^{fP}}{\partial \hat{\omega }_{fl,P}^{fPT}}={{0}_{3\times 6}} \\ & \frac{\partial s_{\phi }^{fP}}{\partial {{\Phi }^{\text{T}}}}=\frac{\partial s_{\phi }^{fP}}{\partial \hat{q}_{\phi }^{\text{T}}}\frac{\partial \hat{q}}{\partial {{\Phi }^{\text{T}}}} \\ \end{align}$

where $\frac{\partial {{{\hat{q}}}_{\phi }}}{\partial {{\Phi }^{\text{T}}}}=\left[ \begin{matrix} 0 & \frac{\partial {{q}_{\phi }}}{\partial {{\phi }_{j}}} & 0 \\\end{matrix} \right]$ with ¦Õ_j being the rotation angle of the resulting point,

$\begin{align} & \frac{\partial s_{\phi }^{fP}}{\partial q_{\phi }^{\text{T}}}={{\left[ q_{fl,P}^{+} \right]}^{\text{T}}}\left[ q_{fl,P}^{-} \right]{{\left( \left[ q_{\phi }^{-} \right] \right.}^{\text{T}}}\left[ s_{0}^{lP-} \right]+ \\ & \left. \left[ q_{\phi }^{+} \right]\left[ s_{0}^{lP+} \right]{{E}_{4}}+\left[ q\rm{'}_{\phi }^{+} \right]{{E}_{4}}+{{\left[ q\rm{'}_{\phi }^{-} \right]}^{\text{T}}} \right) \\ & \frac{\partial s_{\phi }^{fP}}{\partial q_{\phi }^{\text{T}}}={{\left[ q_{fl,P}^{+} \right]}^{\text{T}}}\left[ q_{fl,P}^{-} \right]\left( {{\left[ q_{\phi }^{-} \right]}^{\text{T}}}-\left[ q_{\phi }^{+} \right]{{E}_{4}} \right) \\ & \frac{\partial {{q}_{\phi }}}{\partial {{\phi }_{j}}}=\frac{1}{2}{{\left[ \begin{matrix} -\sin \left( \frac{{{\phi }_{j}}}{2} \right) & l_{l}^{\text{T}}\cos \left( \frac{{{\phi }_{j}}}{2} \right) \\\end{matrix} \right]}^{\text{T}}} \\ & \frac{\partial {{{{q}'}}_{\phi }}}{\partial {{\phi }_{j}}}={{\left[ \begin{matrix} 0 & \frac{1}{2}m_{t}^{\text{T}}\cos \left( \frac{{{\phi }_{j}}}{2} \right) \\\end{matrix} \right]}^{\text{T}}} \\ \end{align}$

To illustrate the reason why a combination of features can improve the performance of the navigation system,the following analysis is presented.

Let ${{\dot{x}}_{k}}$ be any unbiased estimator of the real state ${{\dot{x}}_{k}}$ based on all kinds of measurements collected up to and including time ${{\dot{x}}_{k}},\text{i}\text{.e}\text{.}$,

${{Z}_{lk}}=\left\{ {{z}_{11}},{{z}_{12}},...,{{z}_{1k}},{{z}_{21}},{{z}_{22}},...,{{z}_{2k}},...{{z}_{l1}},{{z}_{l2}},...,{{z}_{lk}} \right\}$

where l denotes the number of independent measurements,k denotes the current moment. The covariance of ${{\dot{x}}_{k}}$ has a lower bound expressed by

${{C}_{k}}=E\left[ \left( {{x}_{k}}-{{{\hat{x}}}_{k}} \right){{\left( {{x}_{k}}-{{{\hat{x}}}_{k}} \right)}^{\text{T}}} \right]\ge J_{k}^{-1}$

where J_k is the Fisher matrix with

${{J}_{k}}=E\left\{ \left[ \frac{\partial \ln p\left( {{z}_{11:lk}}\left| x \right. \right)}{\partial x} \right]{{\left[ \frac{\partial \ln p\left( {{z}_{11:lk}}\left| x \right. \right)}{\partial x} \right]}^{\text{T}}} \right\}$

According to the condition of independent observation,$p\left( {{z}_{11:lk}}\left| x \right. \right)=\prod\limits_{i=11}^{lk}{p\left( {{z}_{i}}\left| x \right. \right)}$,thus we have

${{J}_{k}}=\sum\limits_{i=11}^{lk}{E}\left\{ \left[ \frac{\partial \ln p\left( {{z}_{i}}\left| x \right. \right)}{\partial x} \right]{{\left[ \frac{\partial \ln p\left( {{z}_{i}}\left| x \right. \right)}{\partial x} \right]}^{\text{T}}} \right\}$ (13)

From Eq. (13),it is seen that as the number of independent measurements l getting more J_k becomes larger,thus the lower bound of the covariance of ${{\hat{x}}_{k}}$ becomes smaller.So one can draw the conclusion that more observed information may improve the performance of the whole navigation system.

4.3 Equations of Extended Kalman FilterIn this paper,based on the process model and the integrated observation model provided in the previous subsections,the EKF method is utilized to estimate the relative state between two spacecraft. A summary of the EKF equations are shown as:

$\left\{ \begin{align} & {{X}_{k,k-1}}={{X}_{k-1}}+f\left( {{X}_{k-1}} \right)T \\ & {{P}_{k,k-1}}={{F}_{k,k-1}}{{P}_{k-1}}F_{k,k-1}^{\text{T}}+{{Q}_{k-1}} \\ & {{K}_{k}}={{P}_{k,k-1}}H_{k}^{\text{T}}{{\left( {{H}_{k}}{{P}_{k,k-1}}H_{k}^{\text{T}}+{{R}_{k}} \right)}^{-1}} \\ & {{P}_{k}}=\left( I-{{K}_{k}}{{H}_{k}} \right){{P}_{k,k-1}} \\ & {{X}_{k}}={{X}_{k,k-1}}+{{K}_{k}}\left( {{Z}_{k}}-h\left( {{x}_{k,k-1}} \right) \right) \\ \end{align} \right.$

Seen from the equations,the inputs of the filter consist of the measurements Z_k,the measurement noise covariance R_k,the process noise covariance Q_k£­1 and an initial error covariance matrix P₀; the outputs are the next state estimate ${{\tilde{X}}_{k}}$ and the error covariance matrix P_k.

5 Simulation ResultsIn this section,the simulations are presented to characterize the performance of the proposed integrated estimation technique. In the simulation,the 3D data,which consist of 3D points,3D lines and 3D circles,are given in advance. The 2D data are generated by projecting the 3D data onto the image plane,followed by perturbing the projected image data with Guassian white noise. From the generated 2D-3D data,the relative attitude and position are estimated using the proposed integrated estimation algorithm. The simulation parameters are presented as follows.

The leader spacecraft is specified to follow an elliptic orbit with the semi-major axis of 7 200 000 m and the eccentricity of 0.02,and it is assumed that the leader body-fixed frame is perfectly aligned with its orbit frame. The focal length of the camera on the follower spacecraft is 0.5 m. The mass and the inertia matrix of the follower spacecraft are given by m_f=500 kg and J_f=diag522,420,600 kg¡¤m²,respectively.

The observed feature points fixed on the leader spacecraft are

${{D}_{1}}={{\left[ 1,1,0 \right]}^{\text{T}}};\text{ }{{D}_{2}}={{\left[ -1,1,0 \right]}^{\text{T}}}$

${{D}_{3}}={{\left[ -1,-1,0 \right]}^{\text{T}}};\text{ }{{D}_{4}}={{\left[ 1,-1,0 \right]}^{\text{T}}}$

The observed feature lines are represented by the following dual vectors:

$\begin{matrix} {{{\hat{L}}}_{1}}={{\left[ -1,0,0 \right]}^{\text{T}}}+\varepsilon {{\left[ 0,0,1 \right]}^{\text{T}}} \\ {{{\hat{L}}}_{2}}={{\left[ 0,-1,0 \right]}^{\text{T}}}+\varepsilon {{\left[ 0,0,1 \right]}^{\text{T}}} \\ {{{\hat{L}}}_{3}}={{\left[ 1,0,0 \right]}^{\text{T}}}+\varepsilon {{\left[ 0,0,1 \right]}^{\text{T}}} \\ {{{\hat{L}}}_{4}}={{\left[ 0,1,0 \right]}^{\text{T}}}+\varepsilon {{\left[ 0,0,1 \right]}^{\text{T}}} \\\end{matrix}$

The observed feature circle 1,which is denoted as ¡Ñ₁,has its twist ${{\hat{L}}_{t1}}$ and starting point S₁:

$\begin{matrix} {{{\hat{L}}}_{t1}}={{\left[ 0,\sqrt{2}/2,\sqrt{2}/2 \right]}^{\text{T}}}+\varepsilon {{\left[ -3,1,-1 \right]}^{\text{T}}} \\ {{S}_{1}}={{\left[ 1,3,-2 \right]}^{\text{T}}} \\\end{matrix}$

By rotating the start point S₁ around the line $ with ¦Õ₁=0,¦Õ₂=¦Ð/2 and ¦Õ₃=¦Ð,three resulting points on Circle 1 can be obtained using Eq. (15). Similarly,for the case of Circle 2,denoted as ¡Ñ₂,the parameters are

${{\hat{L}}_{t2}}={{\left[ 1,0,0 \right]}^{\text{T}}}+\varepsilon {{\left[ 0,1,0 \right]}^{\text{T}}},{{S}_{2}}={{\left[ 2,1,-1 \right]}^{\text{T}}}$

and the rotation angles corresponding to the resulting points are ${{{\phi }'}_{1}}=\pi /6,{{{\phi }'}_{2}}=2\pi /3,{{{\phi }'}_{3}}=2\pi $.

The realinitial relative states are given as follows

$\begin{align} & {{q}_{fl,P}}\left( 0 \right)={{\left[ 0.9982,0.0336,0.0366,0.0336 \right]}^{\text{T}}} \\ & r_{fl,P}^{fP}\left( 0 \right)={{\left[ -32.1,40,51.2 \right]}^{\text{T}}}\text{m,}\hat{\omega }_{fl,P}^{fP}\left( 0 \right)=\hat{0} \\ \end{align}$

The initial guess of the estimation is

$\begin{align} & {{{\tilde{q}}}_{fl,P}}\left( 0 \right)={{\left[ 1,0,0,0 \right]}^{\text{T}}} \\ & \tilde{r}_{fl,P}^{fP}\left( 0 \right)={{\left[ -30,45,50 \right]}^{\text{T}}}\text{m} \\ & \tilde{\hat{\omega }}_{fl,P}^{fP}\left( 0 \right)=\hat{0} \\ \end{align}$

The sample time of the filter is T=1 s,and the other parameters are given by

$P\left( 0 \right)=\text{diag}\left( \sigma _{{{W}_{q}}}^{2}{{I}_{4}},\sigma _{{{W}_{{{q}'}}}}^{2}{{I}_{4}},\sigma _{{{W}_{\omega }}}^{2}{{I}_{3}},\sigma _{{{W}_{\phi }}}^{2}{{I}_{6}} \right)$

where $\sigma _{{{W}_{q}}}^{2}=0.000\text{ }{{01}^{2}},\sigma _{{{W}_{{{q}'}}}}^{2}=1,\sigma _{{{W}_{\omega }}}^{2}=0.000\text{ }{{01}^{2}},\sigma _{{{W}_{v}}}^{2}=0.001,\sigma _{{{W}_{\phi }}}^{2}={{0.001}^{2}}$,

$Q=\text{diag}\left( \sigma _{{{Q}_{q}}}^{2}{{I}_{4}},\sigma _{{{Q}_{{{q}'}}}}^{2}{{I}_{4}},\sigma _{{{Q}_{\omega }}}^{2}{{I}_{3}},\sigma _{{{Q}_{v}}}^{2}{{I}_{3}}\sigma _{{{Q}_{\phi }}}^{2}{{I}_{6}} \right)$

where $\sigma _{{{Q}_{q}}}^{2}=0.000\text{ }{{\text{5}}^{2}},\sigma _{{{Q}_{{{q}'}}}}^{2}=0.01,\sigma _{{{Q}_{\omega }}}^{2}=0.000\text{ }{{01}^{2}},\sigma _{{{Q}_{v}}}^{2}=0.01,\sigma _{{{Q}_{\phi }}}^{2}={{0.001}^{2}}$,and R=0.0005²I₂₄

Based on a combination of the measurements from feature points (D₁,D₃),lines (${{\hat{L}}_{1}},{{\hat{L}}_{2}},{{\hat{L}}_{3}},{{\hat{L}}_{4}}$) and circles (¡Ñ₁,¡Ñ₂),the attitude and position of the follower spacecraft relative to the leader spacecraft are estimated using the proposed strategy. Simulation results are presented in Figs. 4 and 5. Seen from Fig. 4,it is clear that both the translational and rotational estimation errors converge to a small value after about 400 s. The first and second subfigures show the time histories of relative attitude and angular velocity estimation errors,respectively. Seen from these subfigures,the Euler angle estimation errors are within 0.02^o,and the relative angular velocity estimation errors converge to below 0.001^o/s. The third subfigure shows the estimation error of the relative position between the points P_l and P_f. It is seen that the position knowledge is within 0.02 m. The relative velocity estimation error is shown in the last subfigure,which demonstrates that each norm of its component converges to below 0.01 m/s. Fig. 5 shows the rotation angle estimation errors for ¡Ñ₁ and ¡Ñ₂,and it is seen that the errors also converge to a small value fast. Therefore,it can be concluded that despite the noise of the measurement,both the relative translation and rotation estimation errors converge rapidly with the desired high estimation accuracy.

Figure 4
Figure 4 Estimation errors of the method using feature points,lines and circles



Figure 5
Figure 5 Rotation angle estimation errors for circles in the integrated method



To compare the performance of the proposed integrated estimation algorithm with other combinations of the features,the following cases are considered: 1) Use point features only (D₁,D₂,D₃,D₄); 2) Use line features only (${{\hat{L}}_{1}},{{\hat{L}}_{2}},{{\hat{L}}_{3}},{{\hat{L}}_{4}}$); 3) Use circle features only (¡Ñ₁,¡Ñ₂); 4) Use point and line features (${{\hat{L}}_{1}},{{\hat{L}}_{2}},{{\hat{L}}_{3}},{{\hat{L}}_{4}}$ and D₁,D₂,D₃,D₄); 5) Use point and circle features (D₁,D₂,D₃,D₄ and ¡Ñ₁); 6) Use line and circle features (${{\hat{L}}_{1}},{{\hat{L}}_{2}},{{\hat{L}}_{3}},{{\hat{L}}_{4}}$ and ¡Ñ₁). Case 1-3 use only one type of feature while Case 4-6 apply different combinations of geometric features. The numerical results for Case 1-6 are shown in Figs. 6-11,respectively. A summarization of the results for all cases is given in Table 1,where the accuracy is represented by the maximum norm of the errors after they converge. Seen from the simulation results,it is obvious that all the approaches can estimate the relative position and attitude effectively,but with different performance. Compared to Case 1-6,the integrated technique yields a superior estimation accuracy and fastest convergence rate. It can be also found that using more features will help to improve accuracy and robustness.

Figure 6
Figure 6 Estimation errors of the method using feature points



Figure 7
Figure 7 Estimation errors of the method using feature lines



Figure 8
Figure 8 Estimation errors of the method using feature circles



Figure 9
Figure 9 Estimation errors of the method using feature points and lines



Figure 10
Figure 10 Estimation errors of the method using feature points and circles



Figure 11
Figure 11 Estimation errors of the method using feature lines and circles



±í 1
Table 1 Estimation errors using different combinations of geometric features MethodsEuler angle(¡ã)Angular velocity(¡ã/s)Position(m)Velocity(m/s)

Point0.027 51.183 9e-30.069 30.008 6

Line0.016 34.481 5e-40.064 00.007 8

Circle0.029 58.437 4e-40.022 50.003 5

Point-Line0.014 23.701 1e-40.043 70.005 9

Point-Circle0.019 97.623 8e-40.021 20.003 2

Line-Circle0.015 94.259 9e-40.022 10.003 1

Point-Line-Circle0.010 72.677 0e-40.020 60.002 8



Table 1 Estimation errors using different combinations of geometric features



In order to validate the proposed method in the case of lager initial errors,the initial guess of the estimation is given as

$\begin{align} & {{{\tilde{q}}}_{fl,P}}\left( 0 \right)={{\left[ 0.919\text{ }7,0.026\text{ }5,0.065\text{ }4,0.386\text{ }1 \right]}^{\text{T}}} \\ & \tilde{r}_{fl,P}^{fP}\left( 0 \right)={{\left[ -60,80,100 \right]}^{\text{T}}}\text{m,}\tilde{\hat{\omega }}_{fl,P}^{fP}\left( 0 \right)=\hat{0} \\ \end{align}$

The simulation results are shown in Fig. 12,which validate the effectiveness of the proposed estimation method in the case of lager initial errors. However,the larger initial estimation errors cost longer time to converge and the estimation convergence errors become larger. To improve the performance of the estimation,the author considers employing other advanced filtering algorithms such as the Unscented Kalman Filter (UKF) and Partical Fliter (PF),and it will be the direction for the future research.

Figure 12
Figure 12 stimation errors of the method using feature lines and circles (larger initial estimation errors)



6 ConclusionsBy employing dual numbers,an integrated observation model is developed based on a combination of multiple geometric features including points,lines and circles. In addition,a six-degree-of-freedom relative motion model is derived,which can describe the rotation-translation coupling effect due to the points deviating from the center of the mass. Based on the proposed relative dynamics and the integrated observation model,Extended Kalman Filter is presented to estimate the relative state between spacecraft. Finally,numerical simulations are provided to illustrate the effectiveness of the proposed algorithms. The simulation results demonstrate that high accuracy can be achieved by using the presented estimation technique,and by comparing with other algorithms using different combinations of features,the integrated technique yields a superior estimation accuracy and fastest convergence rate.


References
[1]Segal S, Gurfil P. Stereoscopic vision-based spacecraft relative state estimation.AIAA Guidance, Navigation, and Control Conference.AIAA,2009: 2009-6094.(0)

[2]Alfriend K T, Vadali S R, Gurfil P, et al. Spacecraft formation flying-dynamics, control and navigation.Elsevier Astrodynamics Series,2010: 1-8.(0)

[3]Subbarao K, Welsh S. Nonlinear control of motion synchronization for satellite proximity operations.Journal of Guidance,Control and Dynamics,2008, 31(5): 1284-1294.(0)

[4]Segal S, Gurfil P. Effect of Kinematic rotation-translation coupling on relative spacecraft translational dynamics.Journal of Guidance,Control and Dynamics,2009, 32(3): 1045-1050.(0)

[5]Pan H, Kapila V. Adaptive nonlinear control for spacecraft formation flying with coupled translational and attitude dynamics.Proceedings of 40th IEEE Conference on Decision and Control. New York,2001: 2057-2062.(0)

[6]Kristiansen R, Nicklasson P J, Gravdahl J T. Spacecraft coordination control in 6DOF:integrator backstepping vs passivity-based control.Automatica,2008, 44(11): 2896-2901.(0)

[7]Clifford W K. Preliminary sketch of bi-quaternions.Proc. London Mathematics Society,1873, 1: 381-395.(0)

[8]Brodsky V, Shoham M. Dual numbers representation of rigid body dynamics.Mechanism and Machine Theory,1999, 34(5): 693-718.(0)

[9]Wu Y, Hu X, Hu D, et al. Strapdown inertial navigation system algorithms based on dual quaternions.IEEE Transactions on Aerospace and Electronic Systems,2005, 41(1): 110-132.(0)

[10]Gan D, Liao Q, Wei S, et al. Dual quaternion-based inverse Kinematics of the general spatial 7R mechanism.Proceedings of the Institution of Mechanical Engineering Science, Part C: Journal of Mechanical Engineering Science,2008, 222(8): 1593-1598.(0)

[11]Alba Perez, McCarthy J M. Dual quaternion synthesis of constrained robotic systems.Journal of Mechanical Design,2004, 126(3): 425-435.(0)

[12]Han D, Wei Q, Li Z, et al. Control of oriented mechanical systems: a method based on dual quaternion.Proceedings of the 17th World Congress the International Federation of automatic Control. Seoul,2008: 3836-3841.(0)

[13]Wang J, Liang H, Sun Z, et al. Finite-time control for spacecraft formation with dual-number-based description.Journal of Guidance, Control and Dynamics,2012, 35(3): 950-962.(0)

[14]Bertozzi M, Broggi A, Fascioli A. Vision-based intelligent vehicles: state of the art and perspectives.Robotics and Autonomous Systems,2000, 32: 1-16.(0)

[15]Segal S, Carmi A, Gurfil P. Vision-based relative state estimation of non-cooperative spacecraft under modeling uncertainty.Proceedings of the IEEE Aerospace Conference. Big Sky,2011: 1-8.(0)

[16]Tsai R. A versatile camera calibration technique for high-accuracy 3D machine vision metrology using off-the-shelf TV cameras and lens.IEEE Journal of Robotics and Automation,1987, 3: 223-244.(0)

[17]Holt R J, Netravali A N. Camera calibration problem: some new results.Computer Vision, Graphics, and Image Processing,1991, 54(3): 368-383.(0)

[18]Kim S G, Crassidis J L, Cheng Y, et al. Kalman filtering for relative spacecraft attitude and position estimation.Journal of Guidance, Control and Dynamics,2007, 30(1): 133-143.(0)

[19]Xing Y, Cao X, Zhang S, et al. Relative position and attitude estimation for satellite formation with coupled translational and rotational dynamics.ACTA Astronautica,2010, 67: 455-467.(0)

[20]Chen S Y, Tsai W H. Systematic approach to analytic determination of camera parameters by line features.Pattern Recognition,1990, 23(8): 859-897.(0)

[21]Liu Y, Huang T, Faugeras O D. Determination of camera locations from 2d to 3d Line and point correspondence.IEEE Transactions on Pattern Analysis and Machine Intelligence,1990, 12(1): 28-37.(0)

[22]Chiang Y T, Huang P Y, Chen H W, et al. Estimation of 3-D transformation from 2-D observing image using dual quaternion.Proceedings of 17th World Congress, the International Federation of Automatic Control. Seoul,2008: 10445-10450.(0)

[23] Goddard J S. Pose and motion estimation from vision using dual quaternion-based extended Kalman filtering. The University of Tennessee, Knoxville, Tennessee, 1997. (0)

[24]Song D M. Conics-based stereo,motion estimation, and pose determination.International Journal of Computer Vision,1993, 10(1): 7-25.(0)