UNCOOPERATIVE OBJECTS POSE, MOTION AND INERTIA TENSOR ESTIMATION VIA STEREOVISION

Transcription

1 UNCOOPERATIVE OBJECTS POSE, MOTION AND INERTIA TENSOR ESTIMATION VIA STEREOVISION M. Lavagna, V. Pesce, and R. Bevilacqua 2 Politecnico di Milano, Aerospace Science and Technology Dept, Via La Masa 34, 256 Milano, Italy 2 University of Florida, 326 Gainesville, USA ABSTRACT Autonomous close proximity operations are an arduous and attractive problem in space mission design. In particular, the estimation of pose, motion and inertia properties of an uncooperative object is a challenging task because of the lack of available a priori information. In addition, good computational performance is necessary for real applications. This paper develops a method to estimate the relative position, velocity, angular velocity, attitude and inertia properties of an uncooperative space object using only stereo-vision measurements. The classical Extended Kalman Filter (EKF) and an Iterated Extended Kalman Filter () are used and compared for the estimation procedure. The relative simplicity and low computational cost of the proposed algorithm allow for an online implementation for real applications. The developed algorithm is validated by numerical simulations in MATLAB using different initial conditions and uncertainty levels. The goal of the simulations is to verify the accuracy and robustness of the proposed estimation algorithm. The obtained results show satisfactory convergence of the estimation errors for all the considered quantities. An analysis of the computational cost is addressed to confirm the possibility of an onboard application. The obtained results, in several simulations, outperform similar works present in literature. In addition, a video processing procedure is presented to reconstruct the geometrical properties of a body using cameras. This method has been experimentally validated at the ADAMUS (ADvanced Autonomous MUltiple Spacecraft) Lab at the University of Florida.. INTRODUCTION Over the past few decades, spacecraft autonomy has become a very important aspect in space mission design. In this paper, autonomous spacecraft proximity operations are discussed with particular attention to the estimation of position and orientation (pose), motion and inertia properties of an uncooperative object. The precise pose and motion estimation of an unknown object, such as a Resident Space Object (RSO) or an asteroid has many potential applications. In fact, it allows autonomous inspection, monitoring and docking. However, dealing with an uncooperative space body is a challenging problem because of the lack of available information about the motion and the structure of the target. Relative navigation between non-cooperative satellites can become a powerful tool in missions involving objects that cannot provide effective cooperative information, such as faulty or disabled satellite, space debris, hostile spacecraft, asteroids and so on. In particular, the precise pose and motion estimation of an uncooperative object has possible applications in the space debris removal field. The pose and the inertia matrix estimation is the first step to implement a system to recover and remove elements harmful to operational and active satellites. Additionally, the obtained algorithm can be installed on autonomous spacecraft for close-proximity operations to asteroids or for rendezvous manoeuvres. This work focuses on the problem of how to estimate the relative state and the inertia matrix of an unknown, uncooperative space object using only stereoscopic measurements. 2. DYNAMICAL MODEL The accurate description of the relative motion is a key point in space systems involving more than one object. Correct modeling of relative translational and rotational motion is essential for autonomous missions. In literature, a large number of studies about point-mass models for relative spacecraft translational motion can be found. The most famous and used model is the one presented by Clohessy and Wiltshire []. Usually, these models are not sufficiently accurate when the faced problem deals with multiple cooperative spacecraft. For this reason, a different model is here considered. The location of a point in a three dimensional space must be specified with respect to a reference system. An appropriate description of the used coordinate systems is provided in this section. Two objects are considered: a leader L and a target T. In this work, the leader is the inspecting spacecraft and the target is the unknown, uncooperative object. The standard Earth-centred, inertial, Cartesian right-hand reference frame is indicated with the

2 letter I. L is the local-vertical, local-horizontal (LVLH) reference frame. It is fixed to the leader spacecraft s centre of mass, the ˆx unit vector directed from the spacecraft radially outward, ẑ normal to the leader orbital plane, and ŷ completing the frame. Then with J, a Cartesian righthand body-fixed reference frame attached to the leader spacecraft s centre of mass is denoted. Finally T, a Cartesian right-hand body-fixed reference frame centred on the target spacecraft s centre of mass. It is also assumed that this frame is coincident with the principal axis of inertia. In this work, the frames J and L are assumed to be aligned. From now on, only the letter L is used to refer to both of them. Figure : Leader - Target Reference Frames The notation that is used in the formulation is now presented. The vector ρ is the vector connecting the leader center of mass with the target center of mass, expressed in the leader frame. Analogously, ρ i can be defined as the position vector, in the leader frame, between the leader center of mass and a feature point P i on the target. Consequently, ρ and ρ i are the translational velocities of the target centre of mass and of a generic feature point, expressed in L. The relative angular velocity is expressed as ω. This vector is the difference of the angular velocities of the leader and target respectively, expressed in the leader frame: ω = ω T L ω L L () The relative attitude is described using the rotation quaternion q = [q, q, q 2, q 3 ] T where the first component is the scalar part and the other three are the vector one. The classical formulation for the dynamical model in a Kalman Filter dealing with non-linear equations is: ẋ = f(x) + w(t) (2) where x is the state vector, f(x) is a non-linear function describing the process and w is a random zero-mean white noise. In our case, the state vector is defined as: x = [ρ T, ρ T, ω T, q T, P i T ] T (3) This is a 3 + 3N elements vector where N is the number of feature points. The relative dynamics is modelled considering the translational and the rotational motions decoupled. The relative translational dynamics is described by a set of non-linear ordinary differential equations. In particular: ẍ 2 ϑ L ẏ ϑ L y ϑ 2 µ(r L + x) Lx = [(r L + x) 2 + y 2 + z 2 ] µ r 2 L (4) ÿ+2 ϑ L ẋ+ ϑ L x ϑ 2 µy Ly = (5) [(r L + x) 2 + y 2 + z 2 ] 3 2 µz z = (6) [(r L + x) 2 + y 2 + z 2 ] 3 2 with r L being the norm of the position vector of the leader, µ is the Earth s gravitational constant and ρ L = [x, y, z] T. ϑl and ϑ L are the orbital angular velocity and acceleration of the leader and are equal to ϑ L = µ a L3 ( e L2 ) 3 ( + e L cos ϑ L ) 2 (7) ϑ L = 2ṙ ϑ L L (8) r L The rotational dynamics is described exploiting the Euler equation. Combining the Euler equations for both leader and target object, the following result is obtained: I L dω dt L L = I L DI T [N T ω T T I T ω T T ] I L ω L L ω L L [N L ω L L I L ω L L ] (9) where I L and I T are the inertia matrices of the leader and target, ω L and ω T the corresponding angular velocities, D is the rotation matrix and N L and N T are the external torque that are assumed to be zero in this case. It is also possible to describe the relative attitude kinematics, using the quaternion kinematic equations of motion: with q = 2 Qω T () q q 2 q 3 q Q(q) = q 3 q 3 q q 2 q q 2 q q The motion of the feature points can be defined as: Ṗ i T L = Ṗ i T T + ω P i T L = ω P i T L () This is expressed in leader frame. However, the dynamics of the feature points is simpler if expressed in the target frame. In fact, due to the rigid body assumption, a feature point cannot change its relative position with respect to the target center of mass. This leads to: Ṗ i T T = (2) Considering that the center of mass dynamics is described by eqs. (5) to (7), the position of a single feature point can be described as follows: ρ i = ρ + P i T L (3)

3 3. OBSERVATION MODEL The purpose of this section is to describe the observation model that allows to obtain information from the collected stereoscopic images. Suppose to have two cameras in a stereo configuration mounted on the leader spacecraft L. The center of projection of the right camera is assumed to coincide with the centre of mass of the leader. It is also the origin of the Cartesian right-hand camera frame [X, Y, Z]. The left camera is separated by a baseline b from the right camera. Using a pinhole camera model and exploiting the perspective projection model, a point in a 3D frame is described in the 2D image plane. With this method all the selected and tracked feature points are expressed in the 2D camera plane. For the line-of-sight ρ i between a generic feature point and the leader centre of mass, assuming to have a focal length equal to, the following expressions are derived: For the right camera respectively. The resulting difference is called disparity and it is defined as: d i = u L u R (2) The disparity allows to reconstruct information about the depth. The human brain does something similar, interpreting the difference in retinal position. In stereo vision applications, this can be performed exploiting the so called triangulation. A set of feature points is chosen and it is assumed that they are always in the view of the cameras. Therefore, according to our assumptions, the initial set of points is always traceable. At each time step, the discrete measurement vector provided by the cameras is: Z i = [w Ri, w Li, ẇ Ri, ẇ Li, d i ] (2) Therefore, the observation equation is: Z i = h(x) + v(t) (22) with v random zero-mean white noise. u R (i) = x i y i v R (i) = z i y i (4) 4. INERTIA RATIOS ESTIMATION and for the left camera u L (i) = x i b y i v L (i) = z i y i (5) where ρ i = [x i, y i, z i ] is expressed in the camera frame. We can also define w R = [u R v R ] and w L = [u L v L ]. Further information can be recovered from the acquired images, exploiting the optical flow. A formulation of the relation between the 3D motion and the optical flow is derived in [2]. This relationship is expressed by the following equations: [ ][ ] ρ ẇ R i = A(w R i ) B(w R i ) (6) y i ω and with and ẇ L i = B = [ ][ ] ρ A(w L i ) B(w L i ) y i ω A = [ ] w w 2 [ ] 2 w w 2 + w w 2 2 w 2 w w 2 w (7) (8) (9) where w and w 2 are the first and the second component of the vector w. It is important to underline that, in reality, cameras collect images at a given sampling frequency. Using two subsequent frames, the optical flow can be estimated and the image velocity computed. However, in the presented observation model, it is assumed that the information about the image velocity is recovered at each time step. Another problem is to determine the different location of the same point in the left and right image plane For the presented model, an a priori knowledge of the target inertia matrix is necessary. This is not a realistic assumption since we are dealing with a completely unknown and uncooperative space object. To overcome this contradiction, an estimation of the basic inertial properties is necessary. A torque free motion is assumed. In this condition, the inertia matrix is not fully observable. In fact, only two of three degrees of freedom are observable. Thus, two parameters are sufficient to represent the inertia matrix. With a parametrized inertia matrix, the motion can be propagated in the correct way. However, no geometrical or mass properties can be recovered. A proper parametrization of the inertia matrix is necessary. The selected formulation is the one proposed by Tweddle [3]. In particular: ( ) ( ) Ix Iy k = ln k 2 = ln (23) I y This formulation relies on the minimum number of parameters, equal to the number of degrees of freedom. k and k 2 do not have any additional constraints. The inertia ratios have to be greater than zero and they can be each value up to infinite. This is a consistent parametrization because the natural logarithm has the same validity domain. Using this parametrization, the target inertia matrix can be expressed as: I x Iy I T = = ek (24) e k2 I z I y At this point, these two parameters must be estimated by the filter. Therefore, a new augmented state can be defined as: x = [ρ T, ρ T, ω T, q T, P i T, k, k 2 ] T (25) I z

4 Also the dynamical model is different. In fact, the parametrized inertia matrix will substitute the previous value of the target inertia matrix in the rotational dynamics expression. Additionally, two equations for k and k 2 are considered. k t = (26) k 2 t = (27) Equation (26) and (27) are valid under the assumption of rigid body motion and without considering any mass variation. In order to improve the convergence of the filter a pseudo measurement constraint can be added. With this equality constraint, the value of the inertia matrix can be forced to converge to the correct value. In particular: = ω T + I T (ω T I T ω T ) (28) This is the new pseudo measurement. It is the classical Euler equation for the rotational dynamics of the target. A fundamental aspect to take into account is that in the pseudo measurement equation, the target angular acceleration is present. Information about this quantity have to be recovered from the actual measurement. However, with knowledge of ω L, ω L and ω, there is not an analytical expression independent on I T to compute ω T. This implies that the angular acceleration of the target has to be measured. With the knowledge of the optical flow, the value of ω at each time step can be recovered. Then, a numerical differentiation can be performed to find the relative angular acceleration. 5. NUMERICAL SIMULATIONS In this section, an evaluation of the performance and robustness of the filter is presented. Monte-Carlo simulations are performed for different values of the initial error covariance and initial relative position. Each simulation is performed considering a satellite and an object in low Earth orbit. In particular, the leader orbit is known. It is assumed that the orbit of the leader has eccentricity e L =.5, semi-major axis a L = 77 km, inclination i L = 5 deg, argument of the perigee ω = 34 deg and right ascension of ascending node Ω = deg. According to our parametrization, the leader inertia is [ ].83 I L = kg m 2 (29).83 In addition, two parallel cameras, in a stereo configuration and pointing in the same direction are mounted on the leader spacecraft. The baseline between the cameras is assumed equal to m. Moreover, only five feature points are supposed to be measured. This is an extreme case, in fact, more than five points are usually visible and detectable. However, this condition may occur when the object is not properly illuminated or if it is too bright. Additionally, considering only a small number of points, the robustness and convergence of the filter are tested also with poor available measurements. The detected features are assumed to be spread over the body of the target with a distance from the centre of mass in the order of.5 m. This can be varied according to the dimension of the target object. After defining the initial condition for the leader orbit, the state has to be initialized. The initial state vector is: x = [ρ, ρ, ω, q, P T i, k, k 2 ] (3) This vector will be defined for each simulation. At this point, the filter parameters have to be selected. In particular, the covariance matrices Q, R, P have to be chosen. R represents the measurement noise and it can be determined whenever the sensor accuracy is given. In the following simulations, the measurements noise of the and of the process is modelled as a zero-mean Gaussian with standard deviation of 5. Q, the process covariance matrix, has to be selected to ensure the convergence of the filter. Finally, the initial value of P, the error covariance matrix, represents the uncertainties in the initial estimation of the state. For each time step, the quaternion is normalized. According to the formulation, after the initial condition initialization, the predicted value of the state has to be computed using the dynamical model. The function ode45 is used in MATLAB to integrate the set of dynamical equations for each time step. Then, a centred difference method is used to compute the Jacobian of the process model. With this value, the transition matrix is computed and the new error covariance is predicted. At this point, a while cycle is used to implement the iterative procedure of the Iterated Extended Kalman Filter. A tolerance equal to. and a maximum number of iterations equal to is used. For the observation model, the equations are solved and linearised with the same approximate method. Finally, the filter innovation, innovation covariance and gain are iteratively computed and state and covariance are updated. In our simulations, a time step of second is used and the total time of the simulation is seconds. The computed errors are defined as: e ρ = (ρ x ρ x ) 2 + (ρ y ρ y ) 2 + (ρ z ρ z ) 2 (3) with e ρ being the error of the estimation of the centre of mass. In this notation, ρ denotes the estimated value of ρ. In the same way the relative translational velocity error can be defined: e ρ = ( ρ x ρ x ) 2 + ( ρ y ρ y ) 2 + ( ρ z ρ z ) 2 (32) And the relative angular velocity error: e ω = (ω x ω x ) 2 + (ω y ω y ) 2 + (ω z ω z ) 2 (33) For k and k 2 the error is simply: e k = (k k ) 2 e k2 = (k 2 k 2 ) 2 (34) The attitude error is defined in a different way. Recalling the definition of the inverse of a quaternion: q = q q 2 (35)

5 where q is the conjugate of q, the error quaternion is equal to: q e = q q (36) The symbol is defined as the product of two quaternions. Finally, the attitude estimation error can be defined as: error [m] ρ e θ = 2 cos (q e ) (37) where in out notation, q e is the scalar part of the error quaternion. In the following examples, the performance of the filter is analysed. 5.. Case A - with pseudo-measurement constraint In the first case scenario, the filter is tested with the following initial conditions: ρ = [, 6, ] m ρ = [.,.225,.] m/s ω = [.,.,.34] deg/s q = [,,, ] The components of the covariance matrix are chosen to represent a realistic situation. In particular, P is composed by: 2 σ ρ = [,, ] m 2 ; 2 σ ρ = [,, ] m 2 /s 2 ; σ 2 ω = [,, ] deg 2 /s 2 ; σ 2 q = [,,, ] 5 ; σ 2 P = [,, ] m 2 ; σ 2 I = [, ]. These values approximate the uncertainties in a real application. For this case, simulations are considered. The mean relative errors after seconds are evaluated according to eqs. (3) to (33) and (37). In this work, both EKF and are used. However, only the results corresponding to the are here reported. In fact, it results to perform better and to be more robust with respect to the simple Extended Kalman Filter. The presented results show robust convergence in all the analysed simulations. Very good results are obtained for the relative angular and translational velocity in fig. 3 and fig. 4. This is probably connected to the fact that the optical flow equation is exploited. The relative attitude is always difficult to estimate in a proper way and with good convergence. fig. 5 shows poor convergence of the relative angle error. The error tends to remain close to the initial value. The two inertia ratios have good convergence thanks to the imposed equality constraint, as reported in fig. 6 and fig. 7. In table, the results are summarized Case B - without pseudo-measurement constraint In this case, the equality constraint is removed. Therefore the filter performance is evaluated with no precise error [m/s] Figure 2: Relative Position Error- Case A ρ Figure 3: Relative Translational Velocity Error - Case A error [deg/s] ω Figure 4: Relative Angular Velocity Error - Case A error [deg] θ.5.5 Figure 5: Relative Attitude Error - Case A

6 Table : State Errors - Case A Percentiles ρ [m] ρ [m/s] ω [deg/s] θ [deg] k [ ] k 2 [ ] k rotational dynamics does not affect the translation. The angular velocity and attitude errors trends are comparable to the Case A outputs. The small values for the angular velocity are the reason why for these similarities. From the presented results, the inertia ratios errors seem to converge to zero. However, this is only due to the fact that the initial covariance is small. In fact, looking at the trend of the error in fig. 8 and fig. 9, it is clear how the error tends to be constant. This means that the estimated inertia ratio remains constant and does not converge. k Figure 6: k Inertia Ratio Error - Case A knowledge about the inertia properties. The Case A initial conditions are applied : Figure 8: Inertia Ratio Error - Case B ρ = [, 6, ] m.9 k2 ρ = [.,.225,.] m/s.85 ω = [.,.,.34] deg/s.8.75 q = [,,, ].7.65 For the covariance matrix, a smaller value is assumed for the inertia ratios: σ 2 ρ = [,, ] m 2 σ 2 ρ = [,, ] m 2 /s 2 σ 2 ω = [,, ] deg 2 /s 2 σ 2 q = [,,, ] 5 σ 2 P = [,, ] m 2 σ 2 I = [, ]/2. The filter keeps being robust under these new conditions too, and almost always converges. The position and translational velocity error trends do not change. Actually the Figure 9: Inertia Ratio Error - Case B In table 2, the error results are summarized. Table 2: State Errors - Case B k2 Percentiles ρ [m] ρ [m/s] ω [deg/s] θ [deg] k [ ] k 2 [ ] Figure 7: k2 Inertia Ratio Error - Case A 5.3. Case C - without constraint, high angular velocity So far, only small values for the relative angular velocity have been considered. In this simulation, the perfor-

7 mance of the filter without equality constraint is evaluated in a case with larger initial relative angular velocity. In particular:.35.3 k2.25 ρ = [, 6, ] m ρ = [.,.225,.] m/s ω = [,,.934] deg/s q = [,,, ] The value of ω is obtained increasing the value of ω T. The covariance matrix is, as before: σ 2 ρ = [,, ] m 2 σ 2 ρ = [,, ] m 2 /s 2 σ 2 ω = [,, ] deg 2 /s 2 σ 2 q = [,,, ] 5 σ 2 P = [,, ] m 2 σ 2 I = [, ]/2 Table 3: State Errors - Case C Percentiles ρ [m] ρ [m/s] ω [deg/s] θ [deg] k [ ] k 2 [ ] Table 3 shows how the estimation of the relative position and translational velocities is slightly affected by the change in the angular velocity. The relative angular velocity and primarily the relative attitude are badly affected by this change. In fact, in this case, the error in the estimation of the inertia matrix strongly affects the dynamical model propagation. Therefore, the incorrect inertia ratios lead to a decay in the estimation performance for angular velocity and attitude. However, using a larger value for the target angular velocity implies better results in the inertia ratios estimation also without the equality constraint imposed with the pseudo measurement. Figure and fig. show the converging trend of the inertia ratios: k Figure : Inertia Ratio Error - Case C This is justified by the fact that, with a larger angular velocity, the filtering process better performs in the estimation of the inertia components. Hence, the dynamical model and the measurement equations of the angular Figure : Inertia Ratio Error- Case C velocity, force the inertia ratios to converge to a consistent and exact value. This estimation can be obtained, in torque free motion conditions, only parametrizing the inertia matrix in a proper way. 6. INERTIA RECONSTRUCTION In the previous section, the estimation process of the inertia ratios has been shown. However, the inertia parameters, with small relative angular velocity, are correctly recovered only if information about the angular acceleration of the target is provided. This implies to numerically derive the available measures of the angular velocity. Numerical derivatives of a quantity that is usually noisy, can introduce instabilities and produce inaccurate results. In this section, a method to recover all the inertia components is described, without relying on any numerical method. A video and image process to recover mass properties is presented. This method is not computationally efficient and, in our application, relies on free and not optimized video/image processing software. The main idea is to collect a video or images of the observed body. From this set of images, a point cloud can be constructed according to video processing algorithms. Once a point cloud is available, a triangulate mesh can be built. The mesh gives us information about the geometry of the object. At this point, an assumption has to be done. In fact, knowing the geometry, the unknown density properties of the object do not allow a complete reconstruction of the mass properties of the body. However, generalizing the problem, the density can be assumed constant. This procedure has been experimentally validate at the University of Florida to demonstrate the validity of this method also with complex geometries. A very simple experimental setup is used. The video of a 3-DOF simulator (fig. 2) are collected using a Sony HandyCam HDR-CX. The obtained images are imported with VisualSFM [4] and the point cloud is extracted. Then, with MeshLab [5] a mesh is created. This mesh is exported to MATLAB (fig. 3) and the mass properties are recovered. The computer used for the video processing mounts an Intel Xeon E Ghz Mb cache Ivy Bridge Processor, 6Gb DDR3 SDRAM and a PNY Quadro K62

8 2Gb Video Card. The obtained results show how the volume of the object can be reconstructed with this procedure. In particular, the resulting errors of the component of the inertia matrix are always lower than 2% with respect to the reference value. This is obtained using a CAD model of the 3-DOF simulator, imposing the same constant density. processing procedure. In fact, the geometrical properties of a body can be reconstructed collecting multiple frames in time; the mass properties of the observed object can be then reconstructed under a uniform density distribution assumption. The step further in the research asks for the experimental campaign to validate the promising obtained numerical results and to tune the algorithms. REFERENCES Figure 2: 3-DOF Simulator Figure 3: MATLAB mesh 7. CONCLUSION This work proposes a new algorithm for estimating the pose, motion and inertia properties of an unknown, uncooperative space object. The presented results show how the algorithm, exploiting the equality constraint, allows for a precise estimation of the complete relative state and the inertia components. Moreover a quick convergence and a satisfactory accuracy are guaranteed. Several simulations are presented to demonstrate the robustness of the algorithm with different covariance matrix values and initial conditions. Moreover, it has been shown how the inertia components, in the filtering process, can converge without the equality constraint but only with a sufficiently high value for the target angular velocity. In most of the cases, the presented algorithm shows better results with respect to similar works. A novel approach to estimate the inertia components with very limited computational burden is proposed. In addition, it has been shown how the inertia properties can be reconstructed with a video [] W. Clohessy, Wiltshire, terminal guidance system for satellite rendezvous, 96 [2] D. J. Heeger and A. D. Jepson, Subspace methods for recovering rigid motion: algorithm and implementation, International Journal of Computer Vision, vol. 7, no. 2, pp.95-7, 992 [3] B. E. Tweddle, Computer vision-based localization and mapping of an unknown, uncooperative and spinning target for spacecraft proximity operations, PhD, Massachussets Institute of Technology, Cambridge, MA, 23 [4] C. Wu, VisualSFM: A visual structure from motion system cs. washington. edu/ ccwu/vsfm, vol.9, 2 [5] P. Cignoni, M. Callieri, M. Corsini, M. Dellepiane, F. Ganovelli and G. Ranzuglia, Meshlab: an open-source mesh processing tool, Eurographics Italian Chapter Conference, pp.29-36, 28 [6] S. Segal and Pini Gufil Stereovision-Based Estimation of Relative Dynamics Between Noncooperative Satellites: Theory and Experiments,Control Systems Technology, IEEE Transactions, vol.22, no.2, pp , 24