Three-dimensional relative positioning and tracking using LDRI

[0002] This is a regular application of a provisional application having an application No. 60/223,556, and a filing date of Aug. 5, 2000.

[0004] 1. Field of the Present Invention

[0005] The present invention relates to a relative positioning and tracking process, and more particularly to a process and system for three-dimensional (3D) relative positioning and tracking, utilizing a range image and reflectance image producer, including a laser dynamic range imager (LDRI), wherein a complete suite of unique 3D relative positioning and tracking algorithms and processing methods, including cross plane correlation, subpixel tracking, focal length determination, Kalman filtering, and orientation determination, is employed to take full advantage of the range and reflectance information provided by LDRI to provide relative position and orientation of a target with respect to the LDRI's carrier.

[0006] 2. Description of Related Arts

[0007] Relative positioning and tracking are critical in some applications involving structural-dynamic measurements, automated navigation, tracking of projectiles, munition guidance, collision avoidance, surveillance of military subjects, target recognition, metrology, robotic vision, and highly accurate automated docking of unmanned micro shuttles to the docking module of satellites in orbit.

[0008] The Laser Dynamic Range Imager (LDRI) was developed by the Sandia National Laboratories to take care of the illumination problem by using a continuous wave (cw) laser radar to illuminate the scene. An LDRI unit is able to provide the reflectance imagery and range imagery simultaneously, with a pixel-by-pixel corresponding relationship.

[0009] The LDRI is a known technology as illustrated in the following references:

[0010] (A) Lin, C. F., Modern Navigation, Guidance, and Control Processing. Englewood Cliffs, N.J., Prentice-Hall, 1990.

[0011] (B) Lin, C. F., Advanced Control System Design. Englewood Cliffs, N.J., Prentice-Hall, 1993.

[0012] (C) Pecina, Jose N., Mike Snyder, Clyde Sapp, Kevin Crosby, Vincent Elliott, Premkumar Saganti, Greg Byrne, and Mike Gaunce, “Dynamic of the solar array motion: an image analysis approach,” 1998 SEM Spring Conference on Experimental and Applied Mechanics, Houston, Tex. Jun. 1-3, 1998.

[0013] (D) James, George, III, David Zimmerman, Mike Grygier, George Studor, Jose N. Pecina, Robert Nellums, “Development of non-contact sensors and data analysis tools for ISS structural dynamics,” AIAA1999 Annual Technical Symposium. Developing Space Operations, Technology, & Utilization,Houston, Tex. May, 27-31.

[0014] (E) Press, W. H., S. A. Teukosky, W. T. Vetterling, and B. P. Flannery. Numerical Recipes in C: The art of Scientific Computing, Second Edition. Cambridge University Press, 1992.

[0015] (F) Studor, George, “Laser Dynamic Range Imager Space Shuttle Flight Demonstration,” NASA JSC, Houston, Tex. 77058, Structural Mechanics Report, 1998.

[0016] (G) Gelb, A. Applied Optimal Estimation. The MIT Press, 1974.

[0017] (H) Bierman, G. J. Factorization Methods for Discrete Sequential Estimation. Academic Press, 1977.

[0018] (I) Park, P., and T. Kailath. “New square root algorithms for Kalman filtering,” IEEE Trans. Automatic Control, Vol. 40, No. 5, pp. 895-899, 1995.

[0019] (J) Golub, G. H., and C. F. Van Loan. Matrix Computations. Johns Hopkins University Press, 1983.

[0020] (K) Schmitt, R. L., R. J. Williams, and J. D. Matthews, “High frequency scannerless imaging laser radar for industrial inspection and measurement applications,” Sandia Report, SAND96-2739, UC-906, 1996.

[0021] The Laser Dynamic Range Imager (LDRI) is a dynamic version of the Scannerless Range Imager (SRI) and the Point Tracking (PT) algorithm. The SRI provides the range measurement from the laser unit to the field of view for far or near inspection distances, but it functions only on statics scenes. The Point Tracking (PT) software package provides the cross-plane time history motion, while a dynamic adaptation of the SRI generates the range measurements from the laser unit to the field of view. The advantage of the LDRI over the conventional photogrammetric methods resides in the feasibility in obtaining, simultaneously, the 3-D motion of multiple points without the necessity of using multiple cameras and specific targets. The resolution of the position of a point is dramatically improved, and the elimination of special illuminating conditions in the scene are unnecessary. Commercial applications exist in flight vehicle docking automated navigation, flight refueling, precision materials fabrication, collision avoidance, and modal analysis.

[0022] It's still challenging to provide a processing method for the LDRI to realize the potential advantages of the LDRI.

[0023] A main objective of the present invention is to provide a three-dimensional (3D) relative positioning and tracking process utilizing the laser dynamic range imager (LDRI), wherein the reflectance imagery and range imagery are processed to simultaneously obtain the 3-D motion of multiple points of a target without the necessity of using multiple cameras and specific targets.

[0024] Another objective of the present invention is to provide a three-dimensional (3D) relative positioning and tracking process utilizing the laser dynamic range imager (LDRI), wherein the reflectance imagery and range imagery are further processed to obtain the relative attitude of the target with respect to the LDRI's carrier.

[0025] Another objective of the present invention is to provide a three-dimensional (3D) relative positioning and tracking process utilizing the laser dynamic range imager (LDRI), wherein a practical focal length determination algorithm is disclosed for the Laser Dynamic Range Imager (LDRI) imagery, which does not require any control points. Focal length determination enables the translation of each pixel in the LDRI range imagery into the world coordinates in Cartesian representation.

[0026] Another objective of the present invention is to provide a three-dimensional (3D) relative positioning and tracking process utilizing the laser dynamic range imager (LDRI), wherein the world coordinates in Cartesian representation of the imagery enables to carry out orientation determination, which in turn aids the point tracking to project the trajectory in the world coordinate system. This provides a means to aid the intensity image processing by the range image processing, which is otherwise impossible without the utilization of the LDRI images.

[0027] FIG. 1 is a block diagram illustrating the relationship and processing flow among the various modules of the 3D relative positioning and tracking process, which includes cross-plane correlation, subpixel tracking, focal length determination, Kalman filtering, and orientation determination according to the preferred embodiment of the present invention.

[0028] FIG. 2 is a graphical illustration showing the selection of the pattern of interest and the surrounding search window.

[0029] FIG. 3 is a graphical illustration showing the procedures of the cross-plane correlation process.

[0030] FIG. 4 is a graphical illustration showing the process of finding out the maximum of a 2D cubic spline interpolation function.

[0031] FIG. 5 shows the definition of the coordinate frames involved in relative orientation determination.

[0032] FIG. 6 is a graphical illustration showing the procedure of finding the range at a subpixel level.

[0033] FIG. 7 shows the Cartesian Coordinate System and Standard Spherical Coordinate System.

[0034] FIG. 8 shows the LDRI Pseudo-Spherical Coordinate System.

[0035] FIG. 9 shows the Conceptual Graphical Principle for a LDRI.

[0036] Referring to FIGS. 1 to 9, a three-dimensional (3D) relative positioning and tracking process utilizing the laser dynamic range imager (LDRI) is illustrated, wherein the reflectance imagery and range imagery are processed to simultaneously obtain the 3-D motion of multiple points of a target without the necessity of using multiple cameras and specific targets.

[0037] As shown in FIG. 9, it is a known art that operation of the Laser Dynamic Range Imager relies on measurements of the phase difference between an intensity-modulated probe laser beam that has been initially diffused over the field of view, and its reflected beam from the target. In a Laser Dynamic Rang Imager (LDRI), a laser diode is intensity modulated at a given frequency f_m. This laser illuminates the entire field of view of the scene of interest. Light is collected by a lens from the scene onto an image intensifier tube and then transmitted to a Charge Coupled Device (CCD) camera.

[0038] The LDRI generates two kinds of imagery, namely a range imagery and a reflectance imagery. Reflectance imagery is a digital image obtained directly from the scene. The phase shift for a range image can be determined from the reflectance image. The intensity at each pixel of each reflectance image satisfies the following equation:

[0039] I=A+Bcos(φ+δ)  (1)

[0040] where I is the intensity pixel values of the reflectance image; A is a constant offset proportional to the illumination intensity, 1/R², the object albedo, and the optical receiver gain; B is a constant that defines a depth of modulation; φ is the phase shift to the pixel in the scene; and δ is an electronic phase shift between the transmitted and received signals.

[0041] Assume a series of N reflectance images with phase shifts δ_ibetween the transmitter and receiver known. The intensity of a pixel in the i-th reflectance image can be written as:

[0042] I_i=A+Bcos (φ+δ_i)=a₀+a₁cos δ_i+a₂sin δ_I  (2)

[0043] where

[0044] a₀=A

[0045] a₁=Bcos φ  (3)

[0046] a₂=−Bsin φ

[0047] From equation (2), at least three reflectance images are necessary to construct a range image, by executing a least-squares-fit algorithm. In fact, the intensity is modulated to generate at least 3 different intensity images of the same field of view. When N≧3, a least-squares-fit algorithm can provide a least-squares solution to the unknowns a₀, a₁, and a₂. Once a₀, a₁, and a₂are known, the phase shift to the scene pixel is calculated by:

[0048] φ=arctan(−a₂/a₁)  (4)

[0049] In Equation (3), the arctan function returns a value of φ between −π/2 and π/2. It is possible, however, to determine the value of φ over a full range of [0, 2π], since we know the value of sin φ and cos φ. Nevertheless, an ambiguity in the range could be present for phase shifts φ≧2 π. Using a laser continuous wave radar range finder can solve this problem.

[0050] This phase shift to each pixel, φ, is proportional to the distance between the laser unit and the target pixel. This distance is given by the range equation: $\begin{array}{cc}R=\frac{c\phi }{4\pi f_m}& \left(5\right)\end{array}$

[0051] In this equation, c is the speed of light, φ is the phase shift between the incident and reflected laser beams, and f_mis the frequency at which the intensity of the laser diode was modulated. The laser source and the image intensifier are synchronously modulated at the same frequency to produce, in both, a sinusoidal variation in the scene illumination intensity. By integrating and storing the output of the image intensifier on a CCD camera based receiver, an image containing the phase shift information for each pixel in the scene is constructed. This image contains information about the range values, and it is called the range image.

[0052] The range resolution is a function of the modulation frequency, and the signal-to-noise-ratio (SNR) of the data. The range resolution for the LDRI is given by: $\begin{array}{cc}\delta R=\frac{c}{4\pi f_m\sqrt{2}\left(\mathrm{SNR}\right)}& \left(6\right)\end{array}$

[0053] Changing the focal length can modify the LDRI field-of-view. However, to improve the range resolution, it is necessary to increase the modulation frequency, f_m, or to increase the signal-to-noise ratio of the data.

[0054] Typically, the sizes of the reflectance image and range image from a LDRI are both 640×480 pixels. In digital format, each pixel of the reflectance image is stored in a 1-byte integer, while each pixel of the range image is stored in a 4-byte float-pointing number.

[0055] Therefore, the size of a reflectance image is:

[0056] ti Reflectance image size: 640×480=307,200 bytes

[0057] The size of a range image is:

[0058] Range image size: 640×480×4=1,228,800 bytes

[0059] An LDRI unit is able to generate 30 frames of reflectance images in one second. Assume that 4 frames of reflectance images are used to generate 1 frame of a range image. A one-minute LDRI operation collects:

[0060] Reflectance Image Data in 1 minute:

[0061] 60×7.5×4×307,200=552,960,000 bytes=540 Mb

[0062] and Range Image Data in 1 minute:

[0063] 60×7.5×1,228,800=552,960,000 bytes=540 Mb

[0064] Such a huge amount of data, very effective tracking process, algorithms, and very efficient algorithm implementation must be invented for extraction of relative position, velocity, and attitude of multi points of the target to meet requirements of some applications.

[0065] Referring to FIG. 1, a three-dimensional relative positioning and tracking process 2 utilizing the laser dynamic range imager (LDRI) of the present invention is achieved by means of a cross-plane correlation module 21, a subpixel tracking module 22, a focal length determination module 23, a Kalman filter module 24, a orientation determination module 26, and a pattern update module 27. Obviously, the process of the present invention can be applied for any range image and reflectance image producer, including a laser dynamic range imager (LDRI), which can produce range images and reflectance images of a target.

[0066] Referring to FIG. 1, the three-dimensional relative positioning and tracking process 2 of the present invention, utilizing a range image and reflectance image producer, such as a laser dynamic range imager (LDRI), comprises the steps of:

[0067] (1) pinpointing points of interest in an image of a target with pixel accuracy;

[0068] (2) obtaining a subpixel resolution by means of a numerical interpolation in a neighborhood of a pixel with a maximum correlation of the points of interest;

[0069] (3) determining a focal length of lens of the range image and reflectance image producer;

[0070] (4) computing 3-dimensional positions of the points of interest of the target with respect to a carrier of the LDRI; and

[0071] (5) determining a relative attitude of the target with respect to the carrier from three selected 3-dimensional positions of the 3-dimensional positions of the target.

[0072] When the target is moving, the size and orientation of the pattern change accordingly. Therefore, the pattern should be updated during tracking, in order to keep track of the pattern. A few approaches are possible to update the pattern.

[0073] Therefore, after the step (5), the three-dimensional relative positioning and tracking process 2 of the present invention further comprises an additional step of:

[0074] (6) updating the points of interest in the step (1) by using the 3-dimensional positions obtained in the step (4) and the relative attitude of the target obtained in the step (5).

[0075] The following embodiment details the 3D relative positioning and tracking algorithms to take full advantage of the range and reflectance information provided by the LDRI, which includes cross-plane correlation, subpixel tracking, focal length determination, Kalman filtering, and orientation determination, referring to FIG. 1.

[0076] For the preferred embodiment of the present invention, the following two coordinate systems are involved:

[0077] (A) Pseudo-spherical coordinate system.

[0078] (B) World Cartesian coordinate system.

[0079] For image processing and orientation determination purposes, it is preferable to work in a world Cartesian (x, y, z) coordinate system. The images produced by the LDRI, however, can be best described in a pseudo-spherical coordinate system (i, j, R), which is detailed below. Therefore, a transformation from the LDRI pseudo-spherical coordinate system into the Cartesian coordinate system must be established.

[0080] Before the pseudo-spherical coordinate system is described in more detail, the relationship between a standard spherical coordinate system and the Cartesian coordinate system, as shown in FIG. 7, is summarized. A point in space, P, which has Cartesian coordinates (x, y, z), can be represented in a spherical coordinate system by spherical coordinates (R, φ, θ), $\begin{array}{cc}R=\sqrt{x^2+y^2+z^2}\phi =\arccos \left(\frac{z}{\sqrt{x^2+y^2+z^2}}\right)\theta =\arctan \left(\frac{y}{x}\right)& \left(7\right)\end{array}$

[0081] where R is the distance from the origin O to the point P, φ is the angle between the z axis and the line segment OP, and θ is the angle between the x axis and the projection of OP onto the x-y plane. Inversely, the transformation from spherical coordinates (R, φ, θ) to Cartesian coordinates (x, y, z) is:

[0082] x=Rcos θ sin φ

[0083] y=Rsin θ sin φ  (8)

[0084] z=Rcos φ

[0085] In the LDRI imagery, object points are imaged on a photosensitive array detector, i.e., the photo-cathode of an image intensifier, by a lens. For simplicity, the origin of the Cartesian coordinate system is positioned at the LDRI lens, as shown in FIG. 8. The point P is imaged onto the photocathode of the LDRI. Tracing the optical ray that goes through the center of the lens, the angles φ and θ remain the same for the image as those for the object. This can be deduced from the geometric relations in FIG. 8. Based upon the fact that the image plane is located one focal length away from the lens, the z coordinate of the image point is fixed at z=f for all object points, where f is the focal length of the lens.

[0086] Define the pseudo-spherical coordinate system (i, j, R) for the LDRI, where

[0087] i is the vertical coordinate of the pixel in the image plane,

[0088] j is the horizontal coordinate of the pixel in the image plane, and

[0089] R is the range to the scene pixel.

[0090] The units of i and j are those of the image plane, the photocathode of an image intensifier tube. If each pixel in the CCD is spaced 16 μm apart and a 3:1 fiber optic taper is used to couple the CCD into the image intensifier tube, each i or j increment is 48 μm.

[0091] The units of i and j, however, become irrelevant, if we select the unit of the lens focal length the same as those of i and j. This fact is important and must be kept in mind in the following focal length determination application.

[0092] The relationship between the pseudo-spherical coordinates (i, j, R) and the spherical coordinates (R, φ, θ) is: $\begin{array}{cc}R=R\phi =\arccos \left(\frac{f}{\sqrt{i^2+j^2+f^2}}\right)\theta =\arctan \left(\frac{j}{i}\right)& \left(9\right)\end{array}$

[0093] where f is the focal length of the lens used to image the scene onto the photocathode of the image intensifier tube.

[0094] It can be calculated that $\begin{array}{cc}\sin \phi =\frac{\sqrt{i^2+j^2}}{\sqrt{i^2+j^2+f^2}}\cos \phi =\frac{f}{\sqrt{i^2+j^2+f^2}}\sin \theta =\frac{j}{\sqrt{i^2+j^2}}\cos \theta =\frac{i}{\sqrt{i^2+j^2}}& \left(10\right)\end{array}$

[0095] The transformation from the LDRI pseudo-spherical coordinates (i, j, R) to the Cartesian coordinates (x, y, z) is established as: $\begin{array}{cc}x=R\frac{i}{\sqrt{i^2+j^2+f^2}}y=R\frac{j}{\sqrt{i^2+j^2+f^2}}z=R\frac{f}{\sqrt{i^2+j^2+f^2}}& \left(11\right)\end{array}$

[0096] Similarly, the transformation from the Cartesian coordinates (x, y, z) to the LDRI pseudo-spherical coordinates (i, j, R) is: $\begin{array}{cc}i=f\frac{x}{z}j=f\frac{y}{z}R=\sqrt{x^2+y^2+z^2}& \left(12\right)\end{array}$

[0097] For the purpose of 3D tracking, at least three points of interest are to be tracked continuously in consecutive frames. A pixel in the reflectance image, however, does not provide enough information for tracking. Therefore, referring to FIG. 2, a pattern which includes the pixel of interest, is usually selected and located in every frame. Once the pattern is located, the pixel of interest is located as well.

[0098] In order to reduce computation, the size of the pattern is usually chosen to be much smaller than the size of the image. Since the cross-plane correlation measures the degree of similarity of two images with the same size, cross-plane correlation is performed at various amounts of shifting of the entire image relative to the pattern. Maximum correlation occurs at the x-y shift where the two images are most similar. A normalized cross-correlation is used, in order to prevent loss of track due to uniform changes in illumination.

[0099] Usually, it is not necessary to shift over the entire image to do the cross-plane correlation. A search window can be selected in the image, and cross-plane correlation is performed against the search window instead of the entire image. This reduces the computation, depending on the size of the search window. FIGS. 2 and 3 illustrate this idea.

[0100] Referring to FIG. 3, the step (1) further comprises the steps of:

[0101] (1.1) defining a pattern to be tracked;

[0102] (1.2) selecting a search window containing the pattern;

[0103] (1.3) passing the pattern over the search window and computing a cross-correlation; and

[0104] (1.4) determining the 3-dimensional positions of the points of interest of the search window that match with the pattern when the cross-correlation takes a highest value thereof at a pixel position.

[0105] As a preferred embodiment, the step (1.1) is performed through a database of specific patterns. The database of specific patterns extracted from a target, such as a resupply satellite, is selected for pattern recognition and selection. This database will provide an artificial intelligence and decision aid in the selection of a docking trajectory. This procedure provides a smart determination of the relative orientation and position of the target, such as a resupply and mother satellites.

[0106] The most computation-intensive portion in the multiple point tracking algorithms is the cross-plane correlation. If the cross-plane correlation algorithm can be optimized, the speed of the software can be improved significantly. We have developed an optimized cross-plane correlation algorithm to reduce the computation.

[0107] As shown below, when passing the pattern a(x, y) over the search window, the pattern remains unchanged, while the subset of the search window b(x, y) is changing. $\mathrm{Crosscorr}\left(k,l\right)=\frac{\sum _{\mathrm{xy}}\left(a\left(x+k,y+l\right)-\stackrel{\_}{a\left(x+k,y+l\right)}\right)\left(b\left(x,y\right)-\stackrel{\_}{b}\right)}{{\left[\sum _{\mathrm{xy}}{\left(a\left(x+k,y+l\right)-\stackrel{\_}{a\left(x+k,y+1\right)}\right)}^2\sum _{\mathrm{xy}}{\left(b\left(x,y\right)-\stackrel{\_}{b}\right)}^2\right]}^{1/2}}$

[0108] Therefore, for each frame, the mean of the b(x, y), {overscore (b)}, needs to be computed only once. Furthermore, the term in the denominator, $\sum _{\mathrm{xy}}{\left(b\left(x,y\right)-\stackrel{\_}{b}\right)}^2,$

[0109] also needs to be computed only once and saved for later use. This reduces the computation by one third.

[0110] Pixel resolution can be gained by finding the maximum of the cross plane correlation function. However, by considering other pixels in the neighborhood of the pixel with the maximum correlation, much higher resolution, i.e., subpixel resolution, can be obtained from the correlation function. The key idea to obtain subpixel resolution is to use numerical interpolation in the neighborhood of the pixel with the maximum correlation.

[0111] Both the LDRI reflectance and range images provide the target information at the pixel level. Using the subpixel tracking algorithm, the position of the point of interest can be determined at the subpixel level. However, no range data corresponding to a subpixel is available. The step (2) uses a cubic spline interpolation algorithm to extract subpixel range data.

[0112] The idea is straightforward. Referring to FIG. 6, once the subpixel position is determined, we first select the closest pixel to that subpixel. Centered at this pixel, a few more points are taken in each direction (left, right, up, and down), thus forming a mesh matrix. Then, a standard 2D cubic spline interpolation is performed, to find out the subpixel range corresponding to the original subpixel position.

[0113] Referring to FIG. 4, an effective and efficient algorithm for subpixel tracking is shown. Based on an analytical algorithm to find the peak of a 1D cubic spline interpolation function, this algorithm does not require evaluating the 2D spline function every, say, 0.01 pixel. The 1D cubic spline interpolation function is a piecewise 3-order polynomial function, and the derivative of the 1D cubic spline interpolation function is a piecewise quadratic polynomial function. The peak of the 1D cubic spline interpolation function at each interval can be found either at either end of the interval, or at the zero of its derivative. The zero of the derivative, a quadratic equation, can be found analytically. When the peak of the 1D cubic spline interpolation function at each interval is found, its peak over the entire range can be selected among them. This constitutes a feasible solution to the maximum of a 1D cubic spline interpolation function.

[0114] To find the maximum of a 2D cubic spline interpolation function, we start with the maximization of the 1D cubic spline interpolation function along the row which passes through the center point 1. The maximum of this 1D cubic spline interpolation function is found to be at Point 2. Then we find the maximum of the 1D cubic spline interpolation function along the column which passes Point 2, which leads to Point 3. Next, we find the maximum of the 1D cubic spline interpolation function along the row which passes Point 3, which leads to Point 4. Next, we find the maximum of the 1D cubic spline interpolation function along the column which passes Point 4, which leads to Point 5. This process continues until we get a convergent point, which is the final solution to the maximum of a 2D cubic spline interpolation function.

[0115] The focal length is a parameter necessary in the transformations between the LDRI pseudo-spherical coordinate system and the Cartesian world system. The LDRI system provides a range image where the magnitude of this distance from the laser aperture to the point tracked is given at each pixel in the scene. The focal length is changing with the motion of the tracked object. Different distances from the camera to the field-of-view need focal length adjustments for obtaining a well-defined image. The determination of the camera focal length is, however, not a trivial problem. A practical focal length determination algorithm, which does not require any control points, is disclosed.

[0116] From an LDRI image, the pseudo-spherical coordinates (i, j, R) can be easily obtained for each pixel. Provided that for an LDRI image, the focal length of the camera is known, the pseudo-spherical coordinates can be readily transformed into the Cartesian coordinates. Assume that the pseudo-spherical coordinates for two pixels P₁and P₂are known, i.e., (i₁, j₁, R₁) and (i₂, j₂, R₂), respectively. Also, the distance between these two points in the Cartesian coordinate system is known, wherein these two points correspond to these two pixels, D. Denote the focal length of the camera for this image by f. Based upon the pseudo-spherical coordinates and the distance between these two points, we can construct an equation which contains only one unknown variable, f, as follows: ${\left(R_2\frac{i_2}{\sqrt{i_2^2+j_2^2+f^2}}-R_1\frac{i_1}{\sqrt{i_1^2+j_1^2+f^2}}\right)}^2+{\left(R_2\frac{j_2}{\sqrt{i_2^2+j_2^2+f^2}}-R_1\frac{j_1}{\sqrt{i_1^2+j_1^2+f^2}}\right)}^2+{\left(R_2\frac{f}{\sqrt{i_2^2+j_2^2+f^2}}-R_1\frac{f}{\sqrt{i_1^2+j_1^2+f^2}}\right)}^2-D^2=0$

[0117] The equation constructed in this way contains only one unknown variable, f. Solving this equation leads to the determination of the focal length.

[0118] A close inspection of the constructed equation reveals that it is essentially a quadratic polynomial equation for f². Therefore, an analytical solution for f²can be established for this equation. Once f²is known, the focal length f can be determined as well.

[0119] Expanding the constructed Equation simplifies it to the following form: $\frac{i_1{i}_2+j_1{j}_2+f^2}{\sqrt{i_1^2+j_1^2+f^2}\sqrt{i_2^2+j_2^2+f^2}}=\frac{R_1^2+R_2^2-D^2}{2R_1{R}_2}$

[0120] Name x=f², and denote $\{\begin{array}{c}a=\frac{R_1^2+R_2^2-D^2}{2R_1{R}_2}\\ b=i_1{i}_2+j_1{j}_2\\ c=i_1^2+j_1^2\\ d=i_2^2+j_2^2\end{array}$

[0121] The Equation can be reformulated into:

[0122] (a²−1)x²+(a²c+a²d−2b)x+(a²cd−b²)=0

[0123] The above Equation is a quadratic polynomial equation for x, from which two solutions for x can be obtained analytically. Since x=f², the focal length f can be determined once x is known.

[0124] To obtain a more accurate focal length, more equations can be established for more than two points, if the distances among them are known. An average of the solutions may provide a better solution to the focal length.

[0125] For image processing and orientation determination purposes, it is preferable to work in a world Cartesian (x, y, z) coordinate system. The images produced by the LDRI, however, can be best described in a pseudo-spherical coordinate system (i, j, R). Therefore, a transformation from the LDRI pseudo-spherical coordinate system into the Cartesian coordinate system must be established. Based on the optical geometry, the mutual transformations between the LDRI pseudo-spherical coordinate system and the Cartesian world system have been developed, with the focal length of the lens as an intermediate variable.

[0126] LDRI imagery and image processing technologies allow for tracking in the 3-dimensional world coordinate frame. Kalman filtering techniques can thus be utilized to improve the tracking accuracy. Kalman filtering is a model-based estimation technique. By modeling the relative motion with an appropriate acceleration model, the Kalman filtering techniques is ideally suited for 3D tracking purposes.

[0127] The benefits of introducing a Kalman filter into LDRI tracking, which is formulated in the 3D world coordinates, lie in two aspects. First, Kalman filtering and smoothing improve tracking accuracy based on motion dynamics formulation, which makes subpixel accuracy feasible. Second, the Kalman filter can be used to predict the 3D world coordinates of a point of interest in the next frame from the current 3D world coordinates. Such a prediction can be used to determine the center and size of search window for the Point Tracking algorithm.

[0128] The latter is very important for the point tracking algorithms. Proper location and size of the search window are crucial for a successful tracking of a point of interest. An incorrect location or too small a size of the search window will definitely lead to loss of track. If the search window is chosen too large, more computation is needed to carry out the cross plane correlation. By incorporating the target 3D world position into the Kalman filter states, the state estimate can be used to determine the center of the search window in the image pseudo-spherical coordinates, and the state estimate covariance matrix can be used to determine the size of the search window in the image coordinates.

[0129] The system model of the tracked target characterizes the motion dynamics. It can be naturally described in a continuous-time form, using the dynamic relationship among the position, the velocity, and the acceleration in the world 3D coordinate frame. In the case that the measurements are available at equally spaced time intervals, the dynamic equation can be discretized in advance and be formulated in an equivalent discrete-time form.

[0130] The motion dynamics of the tracked target is described in the continuous-time form:

[0131] ti {dot over (x)}(t)=Fx(t)+w(t)

[0132] where x(t) is the state vector, F is the system matrix, and w(t) is the system noise with a covariance matrix Q(t). In our application, x(t) is a 9-dimensional vector which consists of the 3D target position, the velocity, and the acceleration, and F is a 9×9 matrix. Specifically, $x=\left(\begin{array}{c}x\\ y\\ z\\ v_x\\ v_y\\ v_z\\ a_x\\ a_y\\ a_z\end{array}\right),F=\left(\begin{array}{ccccccccc}0& 0& 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 1\\ 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right)$

[0133] where x, y, z are 3D coordinates, respectively, v_x, v_y, v_zare the 3D velocities, respectively, and a_x, a_ya_zare the 3D accelerations respectively. In above, a Markovian model is used for the acceleration, which is suitable for the slow relative movement between the mother satellite and the resupply satellite.

[0134] The measurement equations can be established based upon different considerations. One choice is to use the pseudo-spherical coordinates of the tracked target, (i, j, R), which are related to the position in the 3D world coordinate frame in the following: $i=f\frac{x}{z}$ $j=f\frac{y}{z}$ $R=\sqrt{x^2+y^2+z^2}$

[0135] Note that Kalman filtering is executed after the focal length is determined, referring to FIG. 1. Therefore, in Equation above, f is known, and the relationship between (i, j, R) and (x, y, z) is fully determined. Also it should be noted that the measurement equation is nonlinear, which requires approximate linearization during the Kalman filtering.

[0136] An alternative of the nonlinear measurement equations is to convert the pseudo-spherical coordinates of the tracked target (i, j, R) into the 3D world coordinates (x, y, z) first. The 3D world coordinates of the tracked target (x, y, z) are then used as the measurements of the Kalman filter, which leads to a linear measurement equation. According to this consideration, the measurement equation is formulated as

[0137] z(k)=Hx(k)+v(k)

[0138] where z(k) represents the 3D target position in the world coordinate frame at the k-th sampling time t_k, H is a measurement matrix, and v(k) is the measurement noise with a variance of R(k). It should be noted that the measurement matrix can be formulated by $H=\left(\begin{array}{ccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0& 0\end{array}\right)$

[0139] We choose using the linear measurement equation, since it simplifies the Kalman filter formulation and reduces the computation load. The above discussion constitutes a complete model for the Kalman filter, which is used to improve the point tracking accuracy and to predict the search window for LDRI tracking.

[0140] To ensure the numerical stability and accuracy, a QR decomposition based factorized implementation of the Kalman filter has been applied for the step (4).

[0141] After step (4), coordinates of multi points of interest with respect to the LDRI world coordinate system are known, at least three points, are known and tracked. These relative position and velocity data can be output and input to the step (5) for determination of the attitude of the target.

[0142] Orientation determination is critical for 3D tracking. With the LDRI, the determination of the relative orientation, which is otherwise extremely difficult even with stereo cameras, can be readily accomplished. At least three points, whose coordinates with respect to the LDRI world coordinate system are known, have to be tracked in order to determine the relative orientation.

[0143] Orientation of a rigid body platform is determined by the orientation of the specified body frame coordinate system with respect to the reference coordinate system. In the 3D tracking application, the LDRI world coordinate system may be selected as the reference system, with the x-y plane in the lens plane and the z-axis pointing towards the outside. Such a reference coordinate system may not coincide with the body frame coordinate system, but has a fixed relationship with the latter. The orientation of the specified body frame coordinate system with respect to the reference coordinate system can thus be translated easily into orientation with respect to the body frame coordinate system.

[0144] The body frame platform is determined by three points, referring to FIG. 5. Once the body platform is defined, a body frame coordinate system can be specified within the chosen platform. In the 3D tracking applications, assume that the three points form a plane. Select Point 1 as the origin of the body frame coordinate system and the line segment from Point 1 to Point 2 as the body frame y-axis. The body frame x-axis lies in the plane defined by the three points and pointing to the right of the y-axis. The body frame z-axis then completes the right-handed system. Since the distances among the points are known precisely, the points' coordinates in the body frame coordinate system can be calculated immediately. These body frame coordinates remain unchanged during all kinematic movements for a rigid body configuration.

[0145] The orientation of the body frame coordinate system with respect to the LDRI world coordinate system can be defined by a sequence of three (3) angles, Euler angles, and a prescription for rotating the reference coordinate frame through these angles to bring it into alignment with the body frame. A typical Euler rotation sequence starts from the reference frame and rotates in the following sequence:

[0146] 1) Rotate about the z-axis with a yaw angle ψ.

[0147] 2) Rotate about the x-axis with a roll angle φ.

[0148] 3) Rotate about the y-axis with a pitch angle θ.

[0149] An applied direct computation method does not require knowledge of the points' body frame coordinates and only uses the reference coordinates of the three points that define the platform. Consider Point 2. The reference coordinates of Point 2 and its body coordinates are related by the transformation matrix. Solving the relationship equation for the baseline between Point 1 and Point 2 determines the yaw and roll angles of the body platform.

[0150] Consider Point 2 in FIG. 5. The reference coordinates of Point 2 are x_R2=(X₂^˜ X₁, Y₂^˜ Y₁, Z₂^˜ Z₁)^T, and the body coordinates are x_B2=(0, L₁₂, 0)^T. They are related by transformation matrix as follows:

[0151] x_R2=T_B/Rx_B2

[0152] which can be expanded into: $\left(\begin{array}{c}{X}_2-X_1\\ Y_2-Y_1\\ Z_2-Z_1\end{array}\right)=\left(\begin{array}{ccc}*& \cos \mathrm{\phi sin}\psi & *\\ *& \cos \phi \cos \psi & *\\ *& -\sin \phi & *\end{array}\right)\left(\begin{array}{c}0\\ L_{12}\\ 0\end{array}\right)=\left(\begin{array}{c}\cos \mathrm{\phi sin}\psi L_{12}\\ \cos \mathrm{\phi cos}{\mathrm{\psi L}}_{12}\\ -\sin \phi L_{12}\end{array}\right)$

[0153] where * means that the value of this element is not important. It can be easily shown that: $\tan \psi =\frac{X_2-X_1}{Y_2-Y_1}$ $\tan \phi =-\frac{Z_2-Z_1}{\sqrt{{\left(X_2-X_1\right)}^2+{\left(Y_2-Y_1\right)}^2}}$

[0154] which leads to: $\psi =\arctan \frac{X_2-X_1}{Y_2-Y_1}$ $\phi =-\arctan \frac{Z_2-Z_1}{\sqrt{{\left(X_2-X_1\right)}^2+{\left(Y_2-Y_1\right)}^2}}$

[0155] It can be seen from above that the baseline between Point 1 and Point 2 actually determines the yaw and roll angles of the body platform.

[0156] Once the yaw and roll angles are obtained, continue to consider Point 3 in FIG. 5. The reference coordinates of Point 3 are x_R3=(X₃−X₁, Y₃−Y₁, Z₃−Z₁)^T, and the body coordinates are x_B3=(L₁₃sin α, L₁₃cos α, 0)^T. They are related by the transformation matrix as follows:

[0157] x_B3=T_R/BX_R3

[0158] which can be expanded into: $\left(\begin{array}{c}{L}_{13}\sin \alpha \\ L_{13}\cos \alpha \\ 0\end{array}\right)=\left(\begin{array}{ccc}\cos \theta & 0& \sin \theta \\ 0& 1& 0\\ -\sin \theta & 0& \cos \theta \end{array}\right)\underset{\underset{\mathrm{Merge}}{⏟}}{\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \phi & -\sin \phi \\ 0& \sin \phi & \cos \phi \end{array}\right)\left(\begin{array}{ccc}\cos \psi & -\sin \psi & 0\\ \sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{c}{X}_3-X_1\\ Y_3-Y_1\\ Z_3-Z_1\end{array}\right)}$

[0159] Merging three items in the right-hand side of above together leads to: $\left(\begin{array}{c}{L}_{13}\sin \alpha \\ L_{13}\cos \alpha \\ 0\end{array}\right)=\left(\begin{array}{ccc}\cos \theta & 0& \sin \theta \\ 0& 1& 0\\ -\sin \theta & 0& \cos \theta \end{array}\right)\left(\begin{array}{c}{X}_{31}\\ Y_{31}\\ Z_{31}\end{array}\right)$

[0160] From the third row in the above equation, pitch angle can be computed as $\theta =\arctan \frac{Z_{31}}{X_{31}}$

[0161] For the direct computation for yaw, pitch, and roll angles, the only information used is the reference coordinates of the three points, calculated from the LDRI range and reflectance images.

[0162] The above description details the 3D relative positioning and tracking algorithms, which includes cross-plane correlation, subpixel tracking, focal length determination, Kalman filtering, and orientation determination, to take full advantage of the range and reflectance information provided by the LDRI. Besides these, the following are incorporated into the algorithms to further improve the algorithms and enhance the capability.

[0163] When the target is moving, the size and orientation of the pattern change accordingly. Therefore, the pattern should be updated during tracking, in order to keep track of the pattern. A few approaches for implementation of the step (6) are possible to update the pattern.

[0164] (6A) The approach is to use the pattern located in the latest frame as the pattern to be tracked in the current frame, regardless of the change of the shape, size, or orientation. This approach works pretty well when the target is moving very slowly with respect to the LDRI unit, such as the satellite docking application.

[0165] (6B) Another approach is more complicated. When tracking starts, either the user selects the patterns for objects of interest, or the patterns for objects of interest are loaded from a preset database. In either case, the feature patterns are only available for a predetermined view angle with a certain size. If the object is tilted or rotated at a different angle, or moves closer to or farther away from the camera, the feature patterns captured in the images look different. If the orientation and the distance of the object are known, the feature pattern can be remapped into a new one to accommodate the change of orientation and/or distance. Instead of searching the predetermined pattern, the new pattern can be used in the cross-plane correlation to improve the tracking accuracy and reduce the probability of miss. This approach requires the distance and orientation information.

[0166] Referring to FIG. 1, a three dimensional relative positioning and tracking system comprises:

[0167] a range image and reflectance image producer 1, including a LDRI, for generating the range images and reflectance images to a field of view for a target;

[0168] a cross-plane correlation module 21, for pinpointing points of interest in the image of the target with pixel accuracy;

[0169] a subpixel tracking module 22, for obtaining a subpixel resolution by means of numerical interpolation in a neighborhood of a pixel with a maximum correlation;

[0170] a focal length determination module 23 for determining the focal length;

[0171] a Kalman filter module 24, for computing the three-dimensional positions of the points of interest of the target; and

[0172] a orientation determination module 26, for determining a relative attitude of the target between the target and a carrier of the LDRI.

[0173] When the target is moving, the size and orientation of the pattern change accordingly. Therefore, the pattern should be updated during tracking, in order to keep track of the pattern. A few approaches are possible to update the pattern.

[0174] Therefore, the three dimensional relative positioning and tracking system further comprises a pattern update module 27, for updating the pattern from the a cross-plane correlation module 21.