MODEL BASED VISUAL RELATIVE MOTION
ESTIMATION AND CONTROL OF A SPACECRAFT
UTILIZING COMPUTER GRAPHICS
Fuyuto Terui
Japan Aerospace Exploration Agency
7-44-1 Jindaiji-Higashimachi, Chofu-shi, Tokyo 182-8522, JAPAN TEL : 81-422-40-3164
E-mail : [email protected]
Abstract
An algorithm is developed for estimating the motion (relative attitude and rela-tive position) of large pieces of space debris, such as failed satellite. The algorithm is designed to be used by a debris removal space robot which would perform six degree-of-freedom motion control (control its position and attitude simultaneously). The information required as feedback signals for such a controller is relative – position, velocity, attitude and angular velocity – and these are expected to be measured or es-timated from image data. The algorithm uses a combination of stereo vision, 3D model matching, applying the ICP (Iterative Closest Point) algorithm, and extended Kalman Filter to increase the reliability of estimates. To evaluate the algorithm, a simulator is prepared to simulate the on-orbit optical environment in terrestrial experiments, and the motion of a miniature satellite model is estimated using images obtained from the simulator. In addition to that, six DOF(Degrees Of Freedom) manoeuvre simulation with developed algorithm for motion estimation using image data was succefully tried for the proximity flight around a failed satellite utilizing CG(Computer Graphics).
1
INTRODUCTION
As the number of satellites continues to increase, space debris is becoming an increasingly serious problem for near-Earth space activities and effective measures to mitigate it are important. Satellite end-of-life de-orbiting and orbital lifetime reduction capability will be effective in reducing the amount of debris by reducing the probability of collisions, but this approach cannot be applied to inert satellites and debris. On the other hand, a debris removal space robot comprising of a spacecraft (“chaser”) that actively removes space debris and retrieves malfunctioned satellites (“targets”) is considered as a complementary approach.[6] The concept of such a removal space robot is shown in Fig. 1. After rendezvous with the target and approach to approximately 50 m using data from ground observations, satellite navigation positioning and radar, the chaser maintains a constant distance from the target measured using images taken by onboard cameras. During this station-keeping phase, the chaser captures images of the target to allow remote visual inspection and measures its motion by image processing both onboard and on the ground. Since the target is non-cooperative, there is no communication, no special markings or retro-reflectors to assist
Figure 1: On-orbit operation of a debris removal space robot requiring measurement using image data; (top) station keeping and motion measurement; (middle) fly-around and final approach; (bottom) capture of the target
image processing, and no handles for capturing the target. The next phase isflying around and final approach. The chaser maneuvers towards the target to within the range of a capturing device such as manipulator arm in order to capture a designated part of the target.
During thefly-around and final approach phase, the chaser must control its position and attitude simultaneously. Such six degree-of-freedom control becomes more difficult if the target is changing attitude, such as by nutation. [7] The information required as feedback signals for such a controller is relative – position, velocity, attitude and angular velocity – and these are expected to be measured or estimated from image data onboard.
This paper deals with technical elements,“motion estimation” and “six degree-of-freedom control” expected to be necessary for the development of such a debris removal space robot. A terrestrial experiment facility called the “On-orbit Visual Environment Simulator” is used in this development and is described below. Stereo vision is applied to measure the three-dimensional shape of a miniature satellite model using images obtained from this simulator. After that, model-based 3D model matching between groups of point data (the ICP (Iteratice Closed Point) algorithm) is applied to estimate the relative attitude and position of the target with respect to the chaser. Then the output from ICP is used as inputs to extended Kalman Filter to estimate more reliable and accurate relative attitude and position. Since extended Kalman Filter uses the model of translational and attitude motion of both bodies (target and chaser) in space, it is expected that estimates from extended Kalman Filter would not be affected a lot even in the case of data loss from ICP.
For the verification of utility of proposed motion estimation algorithm and 6 DOF control algorithm, six DOF(Degrees Of Freedom) manoeuvre simulation with developed motion estimation using images from CG(Computer Graphics) was succefully tried for the proximity
flight around a failed satellite.
2
MOTION MEASUREMENT USING IMAGES
2.1
On-orbit visual environment simulator
The on-orbit visual environment has two characteristics that make image processing difficult: • intense, highly-directional (collimated) sunlight
Figure 2: On-orbit Visual Environment Simulator
The first characteristic gives very high image contrast, with the earth’s albedo the only diffuse light source.
Since the target is non-cooperative it is impossible to hope for markings on its sur-face specifically designed to assist image processing. Satellites are wrapped in Multi Layer Insulation (MLI) materials for thermal protection such as second-surface aluminized Kap-ton (gold, specular), beta cloth (white, matte) and carbon-polyester coated KapKap-ton (black, matte). Among these, aluminized Kapton is most commonly used and it gives the target the following optical characteristics.
• specular and wrinkled surface • smooth edges
Because of this, optical features such as texture changes according to the direction of lighting and view.
Considering that it is not easy to mimick such images by computer graphics with sufficient
fidelity to be used for developing image processing algorithms, a visual simulator is prepared in order to reproduce the characteristics of the on-orbit visual environment for preliminary terrestrial experiments. Fig. 2 shows the configuration of the visual simulator at JAXA. This facility is the 1/10 model of the actual on orbit configuration and generates simulated images taken by a chaser on-orbit. A light source simulating the sun illuminates a miniature satellite target and an earth albedo reflector that adds diffuse light to the environment. Actuators change the direction of the light source to simulate the change of sunlight direction caused by orbital motion. The attitude of the miniature satellite target is altered using a three-axis gimbal mechanism, and the position and attitude of the chaser stereo camera set is changed using a linear motion stage and a gimbal mechanism to simulate relative motion.
The monochrome stereo camera (640×480 pixels) with parallel lines-of-sight and 40 mm “base line” distance is located 1870 mm from the center of the model. The satellite model comprises a cube measuring approximately 300 mm×250 mm×200 mm with a 400 mm×200 mm solar paddle and a radar antenna. The surface of the model is wrapped by specular and wrin-kled sheet simulating MLI.
Figure 3: (top-left)A Motion Estimation using On-orbit Visual Environment Simulator, (top-right)Motion Estimation utilizing CG, (bottom-left)6-DOF manoeuvre simulation with motion estimation utilizing CG, (bottom-right)Hardware-In the Loop simulation for 6-DOF manoeuvre
2.2
Motion estimation algorithm
Various researchers have investigated image processing techniques for measuring the motion of targets in the space environment. Their methods vary depending upon their research objectives and assumptions. [4], [5], [1])
For this paper, we sought a strategy that would be able to use 3D shape information obtained from ordinary image data and that would not be affected by data loss from shad-ows, occulusion or specular reflection. [8] Fig. 4 shows our strategy for motion measurement applying to the experiment of fig. 3 : top-left. The area-based stereo matching algorithm gives 3D position data for the viewable area of the satellite model’s surface from a pair of cameras, but since the only limited area of the model is in view at any one instant, and the appearance of the area changes according to the model’s attitude and the direction of illumination, the number of measured points obtained tends to be limited. The stereo match-ing algorithm also gives many incorrectly measured points due to matchmatch-ing failures, further reducing the number of accurately measured points obtained. However, it is assumed that the geometry of the satellite can be obtained from design data, and so a three-dimensional shape model of the target may be constructed a priori and model matching can be applied between this model and the 3D measurement points obtained from stereo matching, allowing relative position and relative attitude to be estimated. A matching algorithm called the ICP
Figure 4: A Motion Estimation Strategy using Image
(Iterative Closest Point) algorithm is used for this purpose. Using measured relative posi-tion and attitude as a input, extended Kalman Filter outputs estimated these values with reduced noise and increased reliability. Extended Kalman Filter output predicted relative position and attitude as well as estimated ones. These are used for pre-alignment and front surface generation which is used as a model 3D points in the process of ICP resulting good matching result.
2.3
Stereo vision
The area-based stereo vision algorithm uses multiple images taken from different viewpoints and generates a 3D disparity map which contains displacement information between the cameras and points on the object. The final output of the algorithm is the 3D shape of the part of the object in view reconstructed from the disparity map. In the area-based stereo method, a small window (e.g. 17×17 pixels) around each pixel in the image is matched using “texture”; i.e. by minimizing an image similarity function such as SAD (Sum of Absolute Differences) of pixel intensities, defined as follows:
QSAD(I, J, d(I, J)) = N1 X p=−N1 N2 X q=−N2 |F1(I+p, J +q, d)−G0(I+p, J +q)| (1) F1(I, J, d) = G1(I+d, J). (2)
where the size of the matching window is (2N1+ 1)×(2N2+ 1). G0(I, J) is the intensity of a pixel in the reference camera image (left camera) and G1(I, J) is the intensity of the corresponding pixel in the right camera image. d(I, J) is the disparity, which is the positional difference of the corresponding point from the reference point, and this should be optimized to minimize QSAD(I, J, d) for each unit pixel. F1(I, J, d) in eq.(2) is the pixel intensity of a point (I +d, J) in G1 which is a candidate for the corresponding point to G0(I, J). It should be noted that since the pair of cameras used are aligned side-by-side with parallel lines-of-sight, the epipolar line for finding the matching window in the right camera image
Figure 5: Mmeasured 3D points by stereo vision
will be parallel to the horizontal image axis. Therefore, d appears only in the horizontal image coordinate of G1 in eq.(2). The optimized disparities d∗(I, J)
d∗(I, J) = arg d
minQSAD(I, J, d(I, J)) (3)
are then obtained for all pixels in G0, and make up the disparity map. To pre-process images for stereo matching, the LoG (Laplacian of Gaussian)filter, which is a spatial band pass filter, is used for extracting and enhancing specific features in image. Fig. 5 shows an example of the result of applying the stereo vision algorithm to a satellite model with specular reflection. The attitude of the miniature satellite is fixed and the stereo camera is located 1870 mm from the center of the model. The satellite model comprises a cube measuring approximately 300 mm×250 mm×200 mm with a 400 mm×200 mm solar paddle and a 320 mm×150 mm radar antenna. A pair of digital CCD cameras with resolution of 640×480 pixels are used. The left pictures in fig. 5 show the images taken by each camera after compensation of lens distortion. It is quite difficult to notice the difference between them atfirst glance, but differences in the horizontal position between corresponding parts (disparity) can be observed. The right of thefigure shows measured 3D points.
The original 3D points have lots of spurious noise-like “outliers” sprinkled around the satellite shape. Some of these are from the albedo reflector, the background curtain behind the model and the 3-axis gimbal mechanism beneath the model. The dots from the reflector and the curtain were removed using 3D position thresholding. The dots from the gimbal mechanism were suppressed by wrapping it using matte black cloths. Other noisy spurious points appear as if they were sprayed from the position of the camera and these are thought to be points where the stereo vision algorithm failed tofind a matching window using texture for determining disparity. These are eliminated by Curvature masking, Left-Right Consistency Checking and Median Filtering. It can be seen that 3D point positions are obtained for only a limited part of the model.
2.4
ICP algorithm
Fig. 6 shows the computational flow of the ICP algorithm.[3] The superscript (m) shows iteration number. This handles two point sets such as a measured data point set P and a model data point set X(m). It first establishes correspondences between the data sets by
finding the closest point y(m)
i in X
(m) for each point p
i in P (the 3rd box). Next, it finds the rotational transformation matrixR(m) and translational transformation matrix T(m) for
Figure 6: ICP algorithm
Y(m) which minimizes the cost functionJ shown in Fig. 6 (the 4th box). This cost function is the summation of the distance between corresponding points pi and y
(m)
i . The algorithm in Horn (Ref. [2]) gives closed-form solution for this problem using the eigenvalue problem and finally estimated relative position ˆx,y,ˆ zˆwhich gives T and the estimated quaternion ˆ
q which gives optimalR are directly calculated. X(m) and Y(m) are then transformed using
R(m) and T(m) (the 5th box). This process is repeated until convergence criteria E
≤TE is achieved or iteration exceeds the thresholdm ≥Tm (the 6th and 7th box).
2.5
extended Kalman
fi
lter and front surface model
The simple and intuitive ICP algorithm has a number of advantages. Since it is an algorithm for point sets, it is independent of shape representation and does not require any local feature extraction. It can also handle a reasonable amount of noise such as mismatched dots from stereo vision.
The main disadvantage of ICP alogorithm is that correct registration (matching) is not guaranteed. Depending on the initial relative position and attitude between two point sets, the result of matching could fall into a local minimum. In order to prevent this, pre-alignment of the model data set relative to the measured data set using some measure is required.
The information used for the pre-alignment is predicted measurement given by extended Kalman Filter. The state and measurement of extended Kalman Filter for position (Xp,Yp) and attitude (Xa, Ya) are as follows,
Xp = " rH T C ˙ rH T C # , Yp = " rC CT ∆rC CT # (4)
Figure 7: ICP using time series of images (0, 20, 40, 60, 80, 100 sec) [(red) measured points; (blue) model 3D points after ICP]
Xa = " qT I ωTIT # , Ya= " qT C ∆qTC # (5) where rH
T C represents relative position vector from target to chaser expressed in Hill frame {H} (see fig.9) and rCTC represents relative position vector from chaser to target expressed in chaser fixed frame {C}. qT
I represents quaternion from inertia frame {I} to target fixed frame {T} and qT
C represents quaternion from chaserfixed frame {C} to target fixed frame {T}. ∆means difference from the previous time step.
Using predicted measurement Yp(k + 1/k) and Ya(k + 1/k) it is possible to know the portion of the 3D model data point setX which could be seen from binocular camera and it corresponds to the measured data point set P. This “viewable” part of the 3D model data point set is called “Front Surface model” and is used as redefined X for ICP.
Figure 8: Estimated relative attitude (quaternion)
2.6
Motion estimation experiment using terrestrial simulator
A quasi-real-time program running on Personal Computer (Intel Pentium 4, 2.0GHz, 500MB RAM) was developed which repeatedly performs model satellite attitude motion simulation, gimbal mechanism drive, image capture, stereo processing and motion estimation by ICP and Kalman Filter. The attitude of the model satellite is simulated and is updated at 5 sec. time steps. Since the stereo vision processing time is approx. 5 sec. and attitude estimation takes a further 2—11 sec., the program was incapable of real-time processing and so the motion of the gimbal mechanism for the next time step was deferred until the estimation calculation was completed at each iteration.Several tests with different attitude motion have been carried out and one of the results is shown in Figs. 8 and 7. Fig. 8 shows the actual and estimated relative quaternions. Fig. 7 shows the ICP matching result at each iteration.
Measured points at t=0, 5, 10, 15, 45, 50, 55, 85, 90, 95, 100 (sec) are classified reliable and rest of points are classified unreliable. Number of measured points at t=0 (sec) is 1078 and corresponding “Front Surface Model” has 947 points. Both numbers are the result of thinned up to 1/30 for saving processing time. With the benefit of the first two reliable measured points, it seems that the chosen strategy for the motion estimation using time series of images worked properly in this case.
3
Six degrees of freedom manoeuvre simulation with
motion estimation utilizing Computer Graphics
After confirming feasibility of the motion estimation algorithm using images taken from the terrestrial experiment (fig 3 : top-left), the same algorithm is applied successfully to the software simulator using Computer Graphics (fig. 3 ; top-right). Then closed loop simulation for six degrees of freedom maneouvre with motion estimation utilizing computer grahics (fig. 3 ; bottom-left) is performed. Fig. 9 explains the conditions of the simulation. The
target is a failed satellite on Low Earth Orbit and the chaser is a “free flying” space robot which rendezvous to the target and flies in proximity to the target utilizing image data as measurement. {T},{C} are coordinate frames fixed to the target and chaser respectively and {H} is Hill frame with the same origin as {T}. Both position and attitude controller are designed applying sliding mode control. [7]
3.1
Position controller
The position control force obtained from sliding mode control is as follows,
F =−αpm
Sp |Sp|+²p
. (6)
whereαp is a feedback gain,m is mass of chaser and²p is small positive scalar for preventing ”chattering. The switching surfaceSp is defined
Sp =veC+kp rCe. (7)
kp is a constant defining the switching surface. rCe and veC in eq. (7) is relative position and velocity between required point for position control and the center of chaser defined eq. (8), (9) below, as well as shown in fig. 9.
reC = ˆrCC −DˆCT rreqT (8)
vCe = ˆvCC−DˆCT ωˆTIT rTreq (9)
where ˆDC
T is direction cosine matrix transforming from {T} to{C} and ˆωITT is attitude rate of the target. ˆrCC, ˆDCT, ˆvCC and ˆωITT are all estimated from image data.
3.2
Attitude controller
The sliding mode attitude control torque is
T =−αaI
Sa |Sa|+²a
. (10)
whereαa is a feedback gain, I is moment of inertia of chaser and²a is small positive scalar for preventing ”chattering. The switching surface Sa is defined as follows.
Sa=ωCe +ka1 qe vector sgn(qe vector) +ka2 qCT vector(2,1) sgn(q C
T scalar) (11)
ka1 is a constant defining the switching surface. qe vector in eq. (11) is a quaternion repre-senting the error angle θLOS between LOS vector of the onboard camera rLOS and relative position vector beween target and chaserrC as shown infig. 9. Since the major purpose for usingqe vector as feedback signal for attitude control is to minimizeθLOS, there still remains the attitude error along the LOS axis. In order to make it small,qT vectorC (2,1) which repre-sents attitude error between {T} and {C} along rLOS is also used as a feedback signal for defining switching surface.
The process for calculating qe from ˆrCC, rCLOSis shown in eqs. (12)-(15). ˆrCC is given from motion estimation using image data explained in previous section.
qe vector =λ sin à θLOS 2 ! (12)
Figure 9: Target, chaser, frames, vectors
Figure 10: Time sequence of the proximity maneouvre
qe scalar =cos à θLOS 2 ! (13) λ =− rˆ C C ×rCLOS |rˆC C ×rCLOS| (14) θLOS =arccos à − rˆ C C ·rLOSC |rˆC C| |rCLOS| ! (15) ωC
e in eq. (11) is defined below. ωCC is from Gyros on chaser. As is the case of the position control, ˆDCT and ˆωITT are all estimated from motion estimation utilizing image data.
ωeC =ωCC −DˆTC ωˆITT (16)
3.3
Numerical simulation
The time sequence of the proximity maneouvre is shown infig.10 and table1. It is a proximity maneouvre after rendezvous to the distance of approx.18 [m]. After the station keeping phase, it moves up for 10 [m] then moves back to the original position afterwards. During this maneouvre the chaser controls its position so that its own center of mass to be coincident with the desired position in the targetfixed frame{T} and controls its attitude so that the LOS of its onboard camera points to the mass center of the target. It is assumed that the target is not doing attitude motion. Images of the target was generated by CG(Computer Graphics) and these are processed by motion estimation algorithm (Stereo vision + ICP +
Table 1: Time table of the proximity maneouvre
Table 2: Specifications of target(left) and chaser(right)
extended Kalman Filter). It is assumed that the direction of sunlight is in−YT axis direction so that it could be suitable for image capturing.
Table2 shows specificatins of target and chaser. The chaser is designed as a micro satellite with the mass of 3.59 [kg] and it has three wheels for attitude control and six thrusters for position control.
Fig.11-14 show the result of the simulation. It can be seen that the position of the chaser is controlled with error of approx. ±3[m] in each direction. (see Figs.11 and 12) As is seen fromfig. 12, it is speculated that the error is mainly due to the estimated relative position error in re caused by attitude control measurement and control error in ˆDCT. Fig.13 shows the position of the center of the target in the FOV(Fiels Of View) of the chaser onboard camera. It is controlled by the attitude controller to be within the area of FOV where motion estimation from image is possible.
Fig.14 shows some examples of CG images of the left camera of binocular vision and result of motion estimation algorithm. Blue points in the lower row shows measured points using stereo vision and red points shows model 3D points after the matching by ICP(Iterative Closest Point) algorithm. The column at 5 [sec] shows the initial state, the column at 450 [sec] shows the intermediate position moving from [0, 18, 0][m] to [0, 18, 10][m], the column at 700 [sec] is at [0, 18, 10][m], the column at 750 [sec] is at the intermidiate position going back to the initial position from [0, 18, 10][m] and the collumn at 1000 [sec] is at the initial position. The image is captured every 5 [sec] and processed for motion estimation. It can be seen that blue red points was matched to the blue points successfully generating relative position and attitude.
Figure 11: 3D position of chaser
Figure 13: position of target in FOV
4
Concluding remarks and future work
Motion estimation of a large space debris object using image data was performed by applying Stereo Vision, the ICP(Iterative Closest Point) algorithm using a measured data point set and a model data point set, and extended Kalman Filter. Three-axis attitude motion and position were estimated in a terrestrial experiment using an On-orbit Visual Environment Simulator. Six DOF(Degrees Of Freedom) manoeuvre simulation with motion estimation based on CG(Computer Graphics) applying developed algorithm was succefully tried for the proximityflight around a failed satellite.
More challenging six DOF manoeuvre simulation such as for the chaser following nutating satellite would be the next goal of this research. In addition to that, hardware In the Loop simulation replacing the CG image by images taken in the ”On-orbit Visual Environment Simulator”(fig. 3 : bottom-right) is now under preparation.
References
[1] A. Cropp and P. Palmer. Pose estimation and relative orbit determination of a nearby target microsatellite using passive imagery. 5th Cranfield Conference on Dynamics and Control of Systems and Structures in Space 2002, pages 389—395, 2002.
[2] B. K. P. Horn. Closed-form solution of absolute orientation using unit quaternions.
Journal of Optical Society of America, 4-4:629—642, 1987.
[3] P. J.Besl and N. D.McKay. A method for registration of 3-d shapes. IEEE Transactions of Pattern Analysis and Machine Intelligence, 14-2:239—256, 1992.
[4] M. D. Lichter and S. Dubowsky. Estimation of state, shape, and inertial parameters of space objects from sequences of range images. Proc. SPIE Vol. 5267 : Intelligent Robots and Computer Vision XXI:Algorithms, Techniques, and Active Vision, D. P. Casasent, ed., pages 194—205, 2003.
[5] P. Jasiobedzki M. Abraham and M. Umasuthan. Robust 3d vision for autonomous space robotic operation. Proceedings of the 6th International Symposium on Artificial Intelligence and Robotics & Automation in Space : i-SAIRAS, June 18-22, 2001.
[6] S. Nishida. On-orbit servicing and assembly : Japanese perspective and more. Pro-ceedings of 24th International Symposium on Space Technology and Science, Miyazaki, JAPAN, 2004.
[7] F. Terui. Position and attitude control of a spacecraft by sliding mode control.Proceedings of the American Control Conference, pages 217—221, 1998.
[8] F. Terui. Relative motion estimation and control to a failed satellite by machine vision.
Space Technology, 27:90—96, 2007.