Cooperative Angles-Only Relative Navigation Algorithm for Multi-Spacecraft Formation in Close-Range

Sha Wang,Chenglong He,Baichun Gong,Xin Ding and Yanhua Yuan

1State Key Laboratory of Satellite Navigation System and Equipment Technology,Shijiazhuang,050081,China

2Nanjing University of Aeronautics and Astronautics,Nanjing,210016,China

3Beijing Institute of Control and Electronic Technology,Beijing,100038,China

ABSTRACT As to solve the collaborative relative navigation problem for near-circular orbiting small satellites in close-range under GNSS denied environment,a novel consensus constrained relative navigation algorithm based on the lever arm effect of the sensor offset from the spacecraftcenter of mass is proposed.Firstly, the orbital propagation model for the relative motion of multi-spacecraftis established based on Hill-Clohessy-Wiltshire dynamics and the line-of-sight measurement under sensor offset condition is modeled in Local Vertical Local Horizontal frame.Secondly,the consensus constraint model for the relative orbit state is constructed by introducing the geometry constraint between the spacecraft,based on which the consensus unscented Kalman filter is designed.Thirdly,the observability analysis is done and the necessary conditions of the sensor offset to make the state observable are obtained.Lastly,digital simulations are conducted to verify the proposed algorithm,where the comparison to the unconstrained case is also done.The results show that the estimated error of the relative position converges very quickly,the location error is smaller than 10 m under the condition of 10-3 rad level camera and 5 m offset.

KEYWORDS Relative navigation;spacecraftformation;observability analysis;angles-only measurement

1 Introduction

Maintaining formation configuration and restructuring control is essential for cooperative spacecraft to accomplish specific missions, and formation control depends on precise relative navigation between the members[1–3].Estimating based on certain dynamic model,measuring with sensors on the spacecraft, and finally using the EKF filter algorithm or the UKF filter algorithm to update the relative state is the basic framework of the current navigation algorithm [4,5].Relative dynamic model,the accuracy of on-board sensors together with navigation algorithm affect the accuracy and the observability of relative navigation.

At present,the commonly used sensors for relative measurement of spacecraft formation flying mainly include:relative GPS,microwave radar,LIDAR,visible light camera,infrared camera and laser rangefinder,etc.However,relative navigation based on GPS can be easily interfered by the environment.In addition,members in the formation will lose common star to do the relative measurement if they are far away from each other.Radio ranging navigation has the defect of mirror orbit [6,7].Angles-only navigation method based on optical camera has the problem of unobservability or weak observability in relative orbit,and the environment in space greatly affects measurement accuracy of the camera[8–10].In addition,the method that combines radio signals and laser can be applied,which uses the full sky coverage characteristics of radio beams to achieve target orientation,and then guides laser signals to complete distance measurement.However, formation flying of spacecraft often has strict restrictions on the volume and size of measurement equipment and payloads,which requires the use of as few measurement devices as possible to determine the relative orbit.

Range-only measurement via radio signals and angels-only optical navigation devices are relatively simple and reliable in spacecraft formation flying,thus becoming major trend in the field,and lots of research has been conducted by scholars.Dianetti et al.[11]utilized UWB ranging and phase angle difference information to solve the relative spacecraft navigation problem in short-range rendezvous and docking.Woffiden et al.[12]proposed orbital maneuver method to improve angles-only relative navigation system,but frequent orbital maneuvers will cause fuel consumption.Gaias et al.[13]studied the angles-only relative navigation from the perspective of relative orbit elements,and concluded that the semi-major axis of the orbit is not observable.Newman et al.[14] established a second-order nonlinear relative motion equation using QV(Quadratic Volterra)series,thus,to some extent,solved the observability problem of angles-only relative navigation,though the computational complexity is relatively large.Chen et al.[15–17] adopted a cooperative dual-satellite measurement strategy, Gao et al.[18]introduced measurement baseline to solve the observability problem of angles-only relative navigation by adopting dual-camera measurement strategy,which increases hardware cost.Wang et al.[19]solved the unobservability of range-only relative navigation in near-circular orbit by introducing consensus constraints in spacecraft formation, but relative orbit ambiguity problem still exists for angles-only relative navigation.

The main contribution of this paper is to develop a novel consensus constrained relative navigation algorithm for Multi-Spacecraft Formation in Close-Range that based on the lever arm effect of the sensor offset from the spacecraft center of mass, which will avoid angles-only relative navigation algorithm from converging to the mirror orbit.Orbital maneuver and computational burden brought by complex dynamic model are avoided in the proposed method,only single optical camera is needed to realize relative navigation of close-range spacecraft formation flying.

This paper begins with a brief review of Hill-Clohessy-Wiltshire dynamics and camera offset measurement model in Section 2.In Section 3, the consensus constraint model for the relative orbit state is constructed based on which the consensus unscented Kalman filter is designed.In Section 4,the observability analysis of the proposed angles-only relative navigation algorithm is addressed.In Section 5, Monte Carlo simulations with errors and uncertainties are conducted to verify the theoretical results.At last,conclusions are presented in Section 6.

2 Problem Statement and Formulation

2.1 Relative Motion Dynamic

The origin of a rotating local vertical local horizontal(LVLH)reference frame is collocated with the chief spacecraft center of mass.The axes of the LVLH frame are aligned with the chief spacecraft inertial position vector (x axis or radial), the normal to orbit plane (z axis or cross track), and the along-track direction(y axis completes the orthogonal set).

The position and velocity of the deputy spacecraft center of mass relative to the chief center of mass observed from the chief LVLH coordinates is denoted byr(t)andv(t),respectively.Let the relative orbit state isX(t)= [rT,vT]T, the superscriptTstands for the operator of transposition.Vectors without a superscript are assumed to be coordinatized in LVLH coordinates.

Then,under the assumptions of two-body problem and the range between the chief and deputy spacecraft is small compared to the radial distance to the center of Earth,the relative motion of the deputy with respect to the chief that is orbiting near-circular can be governed by the well-known Hill-Clohessy-Wiltshire dynamics[20]as follows:

whereK= diag[3n2,0,-n2],B= [03×3,I3×3]T,D= -2ω×, andω×is a skew-symmetric matrix representation of the cross product defined by

wherenis the angular velocity of the chief spacecraft.Bis the input matrix of control forcesu(t)=[ux,uy,uz]T,which is loaded on the deputy along the three axes of LVLH frame.Lastly,I3×3denotes 3 by 3 identity matrix.The analytic solution of Hill-Clohessy-Wiltshire dynamics for a given initial state can be expressed as follows:

Φ(t,t0) is the transition matrix from time instancet0tot, by takingtdenotingt-t0for short,Φ(t,t0)can be given in partition form as follows:

2.2 Angles-Only Measurement Model

It is assumed that the origin of the chief-fixed body reference frame is co-located with the chief center of mass.Without loss of generality,an optical-sensor(camera is considered in this work)offset from the chaser center of mass in the chief fixed body frame, i.e.,db= [dx,dy,dz]T, is assumed to be fixed.The unit line-of-sight(also named angles-only)measurement can be modeled in the LVLH frame as follows

where ‖·‖ is the operator of 2-norm;εis the line-of-sight measurement noise which is commonly modeled as zero-mean Gaussian noise with the covarianceis the instantaneous body-to-LVLH attitude rotation matrix at time instanttiand assumed to be known for it can be calculated by using the knowledge of inertial attitude,position,and velocity of the chief.It is assumed that the camera measurement frame is aligned with the focal-plane of the camera,and its orientation relative to the chief-fixed body frame is assumed to be a known state.If the chief holds a known attitude relative to the LVLH frame, an alternative expression of measurement model for line-of-sight vector can be governed as follows:

where the assumption of constant attitude could be realized by attitude control which has been demonstrated by D’Amico et al.[21].

3 Relative Orbit Estimation Algorithm

Consider a formation of multiple (at least two) spacecraft, in which the inertial orbit of each spacecraft is assumed to be unknown.Furthermore, it is assumed that each spacecraft installs a directed camera used to measure the line-of-sight relative to other spacecraft and transmits its own estimation to other spacecraft by undirected broadcasting network.

As shown in Fig.1,there is the distributed measurement and estimation scheme,each spacecraft obtains the measurement and estimate the relative orbit towards another spacecraft in its own LVLH frame.Obviously,if three or more spacecraft are involved in the formation,the relative orbits could form a vector loop which may be used to constrain the estimation.However,the constraint of vector loop would disappeared if only two spacecraft are considered.Then,different estimation method could be designed based on different available information.Thus,these two cases are discussed respectively in the following subsections.

Figure 1:Distributed measurement and estimation scheme where si denotes spacecraft i

3.1 Two-Spacecraft Formation Case

When only two spacecraft are in flight as a formation,there will be no other information except the angles-only measurements could be used to estimate the relative orbit.EKF algorithm is only suitable for the estimation of weakly nonlinear system because it expands the original system and measurement by Taylor series and retains only the linear term.Since the proposed angles-only navigation algorithm represents a nonlinear system,then the UKF introduced by Wan et al.[22]can be utilized to process the measurements for estimation.UKF does not have the linearization process for any nonlinear systems,so it can obtain higher estimation accuracy than EKF.Under the assumption of that the process and measurement noises are purely additive,four steps of the addictive form of UKF algorithm are summarized as follows:

(1) Initialization

(2) Calculate sigma points and scale weights

wherenis the dimension of the states,α,βandτare the scaling parameters for sigma points.τis calculated as

whereα=10-3,β=2 andκ=0 are chosen in this work.

(3) Time update

(4) Measurement update

where the superscript–marks the priori estimate,Pkis the estimate error covariance matrix.

3.2 Three or More Spacecraft Formation Case

When multiple (at least three or more) spacecraft are involved in the formation, the constraint based on geometrical topology information between spacecraft may be used to improve the estimation.Then,Consensus Unscented Kalman Filter(CUKF)is a good and easy way to utilize the constraint to achieve a better estimation.The key of conducting CUKF to the orbital estimation is to construct the consensus condition.Thus,the consensus would be modeled firstly for the problem and then used in designing CUKF algorithm in the following.

As can be seen from Fig.1,the position vectors of every three spacecraft are formed a vector loop which naturally is a physical constraint on the orbit estimations.From the viewpoint of observability,the observability of a system improves whenever additional constraints are applied on the system[19].Thus, it is natural and feasible to force the orbit estimations to satisfy this physical constraint for improving the state observability.Further,satisfying the constraint is also a process of achieving consensus between the members of the formation.

Since the distributed estimate strategy is considered,the relative orbit estimations are resolved in different LVLH frames of each spacecraft.Thus,after coordinate transformation,the position vector loop can be expressed as follows:

wherei,j,andkare the labels of different spacecraft;rijis the projection of the relative position from spacecraftitojin the LVLH frame of spacecrafti.

Differentiating on the both side of(26)yields the constraint between the spacecraft velocities

Then,combing and re-organizing(26)and in matrix form produces

whereXijis the projection of the relative orbital state from spacecraftitojin the LVLH frame of spacecrafti,ωistands for the orbital angular velocity of spacecraftiin the LVLHiframe.

So far,the physical constraint on the relative orbital vectors of the in-loop spacecraft is achieved,which can be used as the consensus condition.

Next,the distributed Consensus Unscented Kalman Filter is considered to be used to estimate the relative orbit for each spacecraft, because of the convergence characteristic of CUKF when smooth and bounded vector field of the dynamics and the measurement are given [23].According to the theorem of CUKF, all the other steps of CUKF are the same as those of UKF shown in (11)–(25)except the measurement update,as follows:

whereλ ＞0 is the consensus feedback gain which have to be chosen carefully to ensure the convergence of consensus [24].‖·‖F(xiàn)is the operator of Frobenius norm,presents the priori estimation in the LVLHiframe under the physical constraint shown in,i.e.,is calculated from the priori estimation ofandas follows:

4 Observability Analysis for Relative Orbit

In this section, the Lie derivative method of the observability analysis for nonlinear systems is introduced, then theoretical observability analysis for the proposed offset camera line-of-sight measurement relative navigation system is presented.

4.1 Lie Derivative Criteria

For a general nonlinear dynamic system defined as

wherex= [x1,x2,...,xn]T∈Rn×1is the estimated state vector,u= [u1,u2,...,ul]T∈Rl×1is the input control vector,andy= [y1,y2,...,ym]T∈Rm×1is the measurement vector,the observability matrix is defined by thenth-order Lie derivatives as

It has been shown that if rank(N)=n,the system is locally observable.According to the theory of local weak observability of nonlinear systems,when the observability matrix composed of low-order Lie derivatives does not satisfy the rank criterion,high-order Lie derivatives in terms of the system must be introduced for analysis until the observability criterion is satisfied.In addition,the system state is unobservable when the recursive Lie derivative does not satisfy the observability rank criterion.

4.2 Observability Analysis

The system state is a 6-dimensional vector,and the observation state is a 3-dimensional unit vector but 2 bearing angles in essence.In order to make the observability matrix potentially full rank,the Lie derivatives are required to calculated at least three times.Without loss of generality,the line-of-sight measurement is adopted to perform the observability analysis of the system.The complex observability matrix is as follows:

Consider a constant vectorc= [c1;c2] ∈R6,whereci∈R3,i= 1,2.If the homogeneous linear equationsNc= 0 in consideration ofconly have zero solutions, then the system observability is guaranteed.By expandingNc=0,we can get

the relationship betweenc1andr-ddescribed in Eq.(39)is

whereαis an arbitrary scalar.Substituting Eq.(42)into Eq.(40)follows:

therefore,we can infer that

whereβis another scalar.Substituting Eqs.(42) and (44) into Eq.(41) which is the last constraint results

similarly,from Eq.(45)we can get

whereηis an arbitrary scalar.Put all parameters to the left of equation we have

If and only if(r-d),v+ω×(r-d)andv+ω×(r-d), which represents position, velocity and acceleration vector respectively, are not in the same plane, theα=β=η=0 can be inferred.Then we havec1=c2=0 from Eqs.(42)and(44)under the condition ofα=β=0,which means the observability matrix is full rank and the system is locally observable.At the same time, a necessary condition for observability can be obtained simply given by

Eq.(48) tells us that the system is unobservable ifd= 0 which indicates that the camera offset is not considered.But we must be aware thatd≠ 0 is a necessary but not sufficient condition.For example,when the camera bias is in the direction of along-track,which means,the Eq.(48)is still violated.

5 Numerical Simulations

The proposed algorithm is established in MATLAB simulation environment to verify theoretical conclusions mentioned above.The spacecraft parameter settings are shown in Table 1,and UKF filter parameter settings are shown in Table 2.The Hill-Clohessy-Wiltshire dynamics assumes that the chief spacecraft is running in a near-circular orbit.Therefore,the eccentricity settings of the three spacecraft should be small enough,as shown in Table 1 is 0,0.0002 and 0.0003,respectively.The spacecraft fly in near-earth orbit, and the distance between the members is 1～7 km, and the perturbation factors such asJ2perturbation and atmospheric drag are not considered in the simulation.Precision of camera angles measurement is set 8.4×10-4rad(0.048°).The offset of angles-only camera is related to the distance between the spacecraft.As the distance between spacecraft increases,the offset of the camera should also increase accordingly.For the relative navigation of the spacecraft within 10 km,it is more appropriate to set the offset of the camera at 5～10 m.The consensus feedback coefficient is an optimized value obtained through empirical adjustment,which is set to 0.03 in the simulation.

Table 1:Spacecraft orbit parameter settings

Table 2:Key parameters in consensus unscented Kalman filter

In order to verify the effectiveness and performance of the proposed algorithm,the following two sets of simulation are performed:in the first group,offset situations are simulated to see whether the relative motion trajectory estimated by the UKF algorithm converges,and in the second group,same offset condition is set to compare convergence performance of CUKF algorithm and UKF algorithm.The statistics of 200 Monte Carlo shooting results are shown in Figs.2 and 3,the green lines depict the mean estimated error while the red and blue lines describes±3σuncertainty boundaries for a 99.973%confidence.

5.1 Two-Spacecraft Formation Case

Fig.2 shows three-axis position and velocity estimation error of the UKF forX12in consideration of 5 meters camera offset in radius direction.The filter convergence time is half orbit period, and the mean error is close to zero after converging.It can be seen from the left three diagrams that the ±3σboundary of x-axis position estimation error is 2.1 and -2.5 m, respectively.For y-axis position estimation error, the boundary is 7.6 and -5.1 m.And for z-axis position estimation error,the boundary is 1.4 and-1.5 m.Filter accuracy of y-axis is worse than the other two axes.The three diagrams on the right indicate the relative velocity estimation error.In general, position estimation accuracy is within 10 m and the filtering algorithm is convergent, which proves the feasibility of the proposed off-set camera navigation method.

Figure 2:UKF relative position error for X12 when d =[5m,0,0]

Figure 3:Three-axis position estimation error of X23 under UKF and CUKF filter

In order to intuitively compare the effects of different camera-offset length on the angles-only relative navigation algorithm proposed in this paper,the following statistics are defined

whereemdenotes the average of the absolute value of the difference between the estimatedand the trueXikfromt0when the filter algorithm converges tot1when the filter algorithm finishes in the k-th Monte Carlo simulation.

Applying mean errorMmand covariance errorPm, the estimation accuracy and stability of the filter algorithm can be compared.A smallerMmandPmindicate a more accurate and stable estimation result.Choosing the filter convergence timet0as one orbit period,Table 3 shows the estimation error statistical results of the angles-only relative navigation algorithm when camera-offsets are set as 1,5 and 10 m along the radial direction of the spacecraft orbit, respectively.Taking the x-axis relative position estimation error as an example,the mean errors for 1,5 and 10 m camera-offset situations are 61.9,3.8,1.0 m and the standard deviations are 60.2,44.8,0.4 m,respectively.In conclusion,it can be seen from Table 3 that the larger the camera-offset is set,the smaller the mean and standard deviation of the estimation error of the filtering algorithm,thus a more accurate and stable estimation result can be obtained.

Table 3:Statistical results of estimation error for X12 under different camera-offset

5.2 Three or More Spacecraft Formation Case

Suppose the camera offset of the three spacecraft are

d1=[5m,0,0],d2=[0,5m,0],d3=[0,0,5m]

Under such camera offset conditions, combined with the results of the previous observability analysis,we can predict:X23will converge to the fuzzy orbit under the UKF algorithm,but converges to the true relative orbit under the CUKF algorithm.Fig.3 shows the position error ofX23using UKF and CUKF.The simulation results are consistent with the prediction.The relative orbit estimated by UKF algorithm converges to fuzzy orbit.But the relative orbit estimated by the CUKF algorithm gradually converges to the true relative orbit at a cost of more convergence time.

6 Conclusions

A new angles-only cooperative relative navigation algorithm for spacecraft formation in closerange is studied in this paper.Based on the Hill-Clohessy-Wiltshire dynamics,this paper studied the convergence of UKF and CUKF when the measurement sensor(camera)is installed offset from the center of mass of the spacecraft.The research work of this paper mainly includes four aspects:(1)The relative motion model between spacecraft and the sensor measurement model with camera installed away from the center of mass are established.(2)The observability of the estimated state is analyzed by introducing the Lie derivative criterion, and the observability conditions of relative position and velocity are obtained.(3)A decentralized estimation strategy based on consistent unscented Kalman filter is designed and the consistent estimation is constructed by using multiple physical constraints.(4) The effectiveness of the algorithm is verified by standard Monte Carlo simulation, and the performance of the algorithm is tested.The results show that for the spacecraft formation with a short distance of 1–7 km, the relative navigation accuracy is within 10 m when 5 m camera offset is designed.The relative navigation algorithm proposed in this paper is based on three spacecraft.When more spacecraft participate in formation or cluster,the decentralized strategy and nonlinear estimation algorithm will be more complex,which will be the main work of future research.

Funding Statement:This work is supported in part by the Natural Science Foundation of China(11802119), Science and Technology on Aerospace Flight Dynamics Laboratory (6142210200306),and Foundation of Science and Technology on Space Intelligent Control Laboratory(6142208200303).

Conflicts of Interest:The authors declare that they have no conflicts of interest to report regarding the present study.