Robust 3D Vision for Autonomous Robots

There are several on-orbit servicing missions on the horizon that will require capabilities for autonomous rendezvous & docking, autonomous rendezvous & capture, proximity operation of space vehicles, constellation satellites, and formation flying [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14]. Successful accomplishment of these missions critically relies on a fault-tolerant sensor system to obtain real-time relative pose information (in 6-dof) of two spacecraft during proximity operations. There are different vision systems capable of estimating the pose (position and orientation) of moving objects. However, among them, an active vision system such as the Neptec Laser Camera System (LCS) is preferable because of its robustness in the face of the harsh lighting conditions of space [15, 1]. As verified during the STS-105 space mission, the 3D imaging technology used in the LCS can indeed operate in space environment. The use of laser range data has also been proposed for the motion estimation of free-floating space objects [16, 17]. All vision systems, however, provide discrete and noisy pose data at a relatively low rate, which is typically 1 Hz. It is worth mentioning that another 3D sensor system called TriDAR is an emerging technology that combines the complementary nature of LCS triangulation and Lidar time-of-flight for enhancing the scan range [18, 19].

Vision-based pose estimation algorithms are 3D registration processes, by which the range data set from different views are aligned in a common coordinate system [20, 21]. In general, the 3D registration process consists of two steps: coarse registration and its refinement. The coarse registration process gives a rough estimate of the 3D transformation by using a positioning sensor that is then used as an initial 3D pose in the refinement step. The ICP (Iterative Closest Point) algorithm is the most widely used algorithm as the pose refinement method for geometric alignment of slightly misaligned 3D models [22, 23]. Although the basic ICP algorithm has proven to be very useful in the processing of range data [23], a number of variations on the basic method have been also developed to optimize different phases of the algorithm [23, 24, 10, 25, 26].

Taking advantage of the simple dynamics of a free-floating object, which is not acted upon by any external force or moment, researchers have employed different observers to track and predict the motion of a target satellite [27, 28, 29, 8]. Using the Iterative Recursive Least Squares pose refinement [30], the visual information from camera has been used for relative pose estimation for autonomous rendezvous & docking [31]. In some circumstances, e.g., when there are occlusions, no sensor data is available. Therefore, prediction of the motion of the target based on the latest estimation of its states and dynamic parameters are needed for a continuous pose estimation. This work is focused on the integration of the Kalman filter and the ICP algorithm in a closed-loop configuration for accurate and fault-tolerant pose estimation of a free-falling tumbling satellite [1]. In the conventional pose estimation algorithm, the range data from a rangefinder scanner along with a CAD-generated surface model are used by the ICP algorithm to estimate the target’s pose. The estimation can be made more robust by placing the ICP algorithm and the KF estimator in a closed-loop configuration, wherein the initial guess for the ICP is provided by the estimate prediction [32]. The pose estimation based on the ICP-KF configuration has the following advantages:

1. The KF reduced the measurement noise, thus providing a more precise pose estimation.

2. Faster convergence of the ICP iteration and maintaining tracking over a wider range of object velocity are achieved as a consequence of increased precision of the ICP initial guess provided by the KF.

3. Continuous pose estimation becomes possible even in the presence of temporary sensor failure or obstruction as a consequence of incorporation of the dynamic model of the target in the estimation process.

The KF estimator is designed so that it can estimate not only the target’s states, but also its dynamic parameters. Specifically, the dynamic parameters are the ratios of the moments of inertia of the target, the location of its center of mass, and the orientation of its principal axes. Not only does this allow long-term prediction of the motion of the target, which is needed for motion planning, but also it provides accurate pose feedback for the control system when there are blackout, i.e., no observation data are available [33, 29]. In this work, we use the Euler-Hill equations [34] to derive a discrete-time model that captures the evolution of the relative translational motion of a tumbling target satellite with respect to a chaser satellite which is freely falling in a nearby orbit.

This section reviews the basic ICP algorithm originally proposed by Besl and McKay [22]. The ICP algorithm is an iterative procedure which minimizes a distance between point cloud in one dataset and the closest points in the other. Suppose that we are given a 3D point cloud dataset $U=\{u_1\cdots u_m\}$ that corresponds to a single shape represented by model set $M$. It is known that for each point $u_i\in R^3$ from the 3D points data set $U$, there exists at least one point, $v_i$, on the surface of $M$ which is closer to $u_i$ than other points in $M$ [35]. The ICP algorithm assumes that the initial rigid transformation $\{A_0,r_0\}$ between sets $U$ and $M$ is known; $r_0$ and $A_0$ denote the translation vector and the rotation matrix, respectively. Then, the problem of finding the correspondence between the two sets can be formally expressed by

and then set $V=\{v_1\cdots v_m\}$ is formed accordingly. Now, we have two independent sets of 3D points $V$ and $U$ both of which corresponds to a single shape. The problem is to find a fine alignment $\{A,r\}$ which minimizes the distance between these two sets of points [22]. This can be formally stated as

where $1_3$ is the $3\times 3$ identity matrix. There are several closed-form solutions to the above least-squares minimization problem [36, 37], of which the quaternion-based algorithm [38] is the preferable choice for $m>3$ over other methods [22]. The rotation matrix can be written as a function of unit quaternion as

where $q_v$ and $q_o$ are the vector and scaler parts of the quaternion, i.e., $q=[q_v^T{q}_o{]}^T$. Consider the cross-covariance matrix of the sets $V$ and $U$ given by

where

are the corresponding centroids of the sets. Then, it can be shown that minimization problem (2) is tantamount to find the solution of the following quadratic programming [38]

where the symmetric weighting matrix $W$ can be constructed from cross-covariance matrix, $S$, as follow

where $s_{ij}$ denotes the $ij$th entry of matrix $S$. The unit quaternion, $\breve{q}$, that maximizes the quadratic function of (6) is the eigenvector ${\xi }_i$ corresponding to the largest eigenvalue of the matrix $W$, i.e., $\breve{q}={\xi }_i\left(W\right)$ so that ${\lambda }_i\left(W\right)={\lambda }_{max}$. Then, the translation can be obtained from

The ICP-based matching algorithm may proceed through the following steps:

1. Given an initial coarse alignment $\{q_0,r_0\}$, find the closest point pairs $V$ from scan 3D points set $U$ and model set $M$ according to (1).

2. Calculate the fine alignment translation $\{\breve{q},\breve{r}\}$ minimizing the mean square error the distance between two data sets $U$ and $V$ according to (6)-(7)

3. Apply the incremental transformation from step ii to step i.

4. Iterate until either the residual error $d$ is less than a threshold, $d_{min}$, or the maximum number of iteration is reached.

It has been shown that the above ICP algorithm is guaranteed to converge to a local minimum [22]. However, a convergence to a global minimum depends on a good initial alignment [39, 40]. In pose estimation of moving objects, “good” initial poses should be provided at the beginning of every ICP iteration. The initial guess for the pose can be taken from the previous estimated pose obtained from the ICP [15]. However, this can make the estimation process fragile when dealing with relatively fast moving target [41]. This is because, if the ICP does not converge for a particular pose, e.g., due to occlusion, in the next estimation step, the initial guess of the pose may be too far from its actual value. If the initial pose happens to be outside the global convergence region of the ICP process, from that point on, the pose tracking is most likely lost for good. The estimation can be made more robust by placing the ICP and a dynamic estimator in a closed-loop configuration, whereby the initial guess for the ICP is provided by the estimate prediction of the moving object. The following sections describe the design of a Kalman filter which will be capable of not only estimating the states but also the parameters of a free-floating object.

Fig. 1 illustrates the chaser and the target satellites as rigid bodies moving in nearby orbits. Coordinate frames $\left\{A\right\}$ and $\left\{B\right\}$ are attached to the chaser and the target, respectively. The origins of frames $\left\{A\right\}$ and $\left\{B\right\}$ are located at center-of-masses (CM)s of the observing and target satellites. The axes of $\left\{B\right\}$ are chosen to be parallel to the principal axes of the target satellite, while $\left\{A\right\}$ is orientated so that its $x$-axis is parallel to a line connecting the Earth’s center to the chaser CM and pointing outward, and its $y$-axis lies on the orbital plane; see Fig. 1. The coordinate frame $\left\{C\right\}$ is fixed to the target at its point of reference (POR) located at distance $\rho$ from the origin of $\left\{B\right\}$. It is the pose of $\left\{C\right\}$ that is measured by the laser camera. We further assume that the target satellite tumbles with angular velocity $\omega$. Also, notice that the coordinate frame $\left\{A\right\}$ is not inertial; rather, it moves with the chaser satellite. Below, vectors $\rho$ and $\omega$ are expressed in $\left\{B\right\}$.

We choose to express the target orientation in the body-attached frames coincident with its principal axes, $\left\{B\right\}$, while the translational motion variables are expressed in the body attached frame of the chaser, $\left\{A\right\}$. The advantages of such a mixed coordinate selection are twofold: Firstly, the inertia matrix becomes diagonal and will be independent of the target orientation, thus resulting in a simple attitude model. Secondly, choosing to express the relative translational motion dynamics in $\left\{A\right\}$ leads to decoupled translational and rotational dynamics that greatly contributes to the simplification of the mathematical model; note that, since $\left\{B\right\}$ is a rotating frame, the translational acceleration seen from this frame depends on both the relative angular velocity and relative acceleration. In the following, subscripts $r$ and $t$ denote quantities associated with the rotational and translational dynamics of the system, respectively.

The ICP algorithm gives noisy measurements of the position, $\breve{r}$, and orientation, $\breve{q}$, of the target. In the followings, the measurement variables will be expressed in terms of the states variables. It is apparent from (1) that

Also, recall from (8) that quaternion $q$ can be described as a function of $\eta$ and $\mu$, i.e., $q(\mu ,\eta )=\eta \otimes \mu$. Consider nominal quaternions $\overline{\eta }$ and $\overline{\mu }$ which are chosen to be close to the actual quaternions; as will be later discussed in the next section. Then, the variation of the quaternion, $\delta q$, with respect to nominal quaternions is defined as

Thus, the observation equation can be written as

where $\nu$ is the measurement noise, which is assumed to be white with the covariance $E\left[\nu {\nu }^T\right]=R$, and

Note that the observation vector (27) is a nonlinear function of the states[46]. To linearize the observation vector, one also needs to derive the sensitivity of the nonlinear observation vector with respect to the system states. It can readily be seen from (3) that, for a small rotation $\delta \mu$, i.e., $\parallel \delta {\mu }_v\parallel \ll 1$ and $\delta {\mu }_0\approx 1$, the first-order approximation of the rotation matrix can be written as

Hence, (24) can be approximated as

where $\overline{A}=A\left(\overline{\mu }\right)$. Moreover, in view of the definition of the quaternion product and assuming small quaternion variations, we can say that

It is apparent from (28) and (29) that the measurement equations are bilinear functions of the states. Consequently, we can derive the sensitivity matrix $H=\frac{\partial h}{\partial x}$ as

In this section, experimental results are presented that show comparatively the performance of the pose estimation with and without KF in the loop. The experimental setup includes a satellite mockup attached to a manipulator arm, which is driven by a simulator according to orbital dynamics [1]. The simulator generates the evolution of the relative position and orientation of the two satellites according to (12) and (17). The Neptec laser rangefinder scanner is used to obtain the pose measurements at a rate of 2 Hz. For the spacecraft simulator that drives the manipulator, parameters are selected as

The solid model of the satellite mockup, Fig. 3(a), and the point-cloud data, generated by the laser rangefinder sensor, Fig. 3(b), are used by the ICP algorithm to estimate the satellite’s pose according to the ICP initialization configurations illustrated in Fig. 2. Three test runs conduced to demonstrate the capability of the estimator are: (i) identification of the spacecraft dynamics parameters, (ii) accuracy and robustness of pose tracking at high velocity.

A robust 3D vision system for autonomous operation of space robots has been presented. A method for pose estimation of free-floating space objects which incorporates a dynamic estimator in the ICP algorithm has been developed. A Kalman filter was used for estimating the relative pose of two free-falling satellites that move in close orbits near each other using position and orientation data provided by a laser vision system. Not only does the filter estimate the system states, but also all the dynamics parameters of the target. Experimental results obtained from scanning a tumbling satellite mockup demonstrated that the pose tracking based on the ICP alone did not converge. On the other hand, the integration scheme of the KF and ICP yielded a robust pose tracking.