Feedback-Stabilized Minimum Distance Maintenance ... - IEEE Xplore
IEEE TRANSACTIONS ON ROBOTICS, VOL. 21, NO. 5, OCTOBER 2005
Fig. 2. Distortion corrections on an observed part of the planar calibration grid and of a 3-D object. (a), (d) Original images with strong nonparametric distortions. (b), (e) Corrected image using Bouguet’s method. (c), (f) Corrected image using two quadratic 17 17 B-spline distortion models (our method).
1009
[11] W. Press, B. Flannery, S. Teukolsky, and W. Vetterling, Numerical Recipes in C: The Art of Scientific Computing, 2nd ed. Cambridge, U.K.: Cambridge Univ. Press, 1992, pp. 59–70 and 681–688. [12] R. Hartley, “In defense of the 8-point algorithm,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 19, no. 6, pp. 580–593, Jun. 1997. [13] D. Rogers and J. Adams, Mathematical Elements for Computer Graphics, 2nd ed. New York: McGraw-Hill, 1990, pp. 289–351 and 435–458. [14] J.-Y. Bouguet, “Visual methods for three-dimensional modeling,” Ph.D. dissertation, Univ. California, 1999. [15] J. Heikkilä, “Geometric camera calibration using circular control points,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 22, no. 10, pp. 1066–1077, Oct. 2000. [16] C. Harris and M. Stephens, “A combined corner and edge detector,” in Proc. 4th Alvey Vis. Conf., 1988, pp. 147–151.
2
of squares (SS) of the distances from the detected points to the best straight line that passes through these points is computed (i.e., the 2 of the least-squares approximation), as in [7]. The DE is the sum of all SSs. The DEs in Table III clearly show that our proposed method is best in removing any distortions. The results are limited by the remaining residual error associated with noise and the aliasing effect of the pixel accuracy of the edge detection. For noiseless aliased lines, the DE is still greater than 74.6. IV. CONCLUSION The proposed multiview camera-calibration framework offers a generic, adaptive, and intuitive B-spline distortion model which can deal with both parametric and nonparametric distortions. This model is estimated using an iterative surface fitting framework with data redundancy provided from multiple views, and is naturally refined within the whole nonlinear camera-calibration process. Experimental results have shown that the proposed framework corrects standard lens distortions with better accuracy than two other existing multiview methods. Unlike the other two methods, it also corrects nonparametric distortions, but with slightly less precision. REFERENCES [1] O. Faugeras, Q.-T. Luong, and C. B. T. Papadopoulo, The Geometry of Multiple Images. Cambridge, MA: MIT Press, 2001, pp. 173–245. [2] R. Tsai, “A versatile camera calibration technique for high-accuracy 3D machine vision metrology using off-the-shelf tv cameras and lenses,” IEEE J. Robot. Autom., vol. RA-3, no. 4, pp. 323–344, Aug. 1987. [3] Z. Zhang, “A flexible new technique for camera calibration,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 22, no. 11, pp. 1330–1334, Nov. 2000. [4] X. Meng and Z. Hu, “A new easy camera calibration technique based on circular points,” Pattern Recogn., vol. 36, pp. 1155–1164, 2003. [5] M. Pollefeys and L. V. Gool, “Stratified self-calibration with the modulus constraint,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 21, no. 8, pp. 707–724, Aug. 1999. [6] A. Basu and S. Licardie, “Alternative models for fish-eye lenses,” Pattern Recogn. Lett., vol. 16, pp. 433–441, 1995. [7] F. Devernay and O. Faugeras, “Straight lines have to be straight,” Mach. Vis. Applicat., vol. 13, pp. 14–24, 2001. [8] S. B. Kang, “Radial distortion snakes,” in Proc. IAPR Workshop Mach. Vis. Applicat., 2000, pp. 603–606. [9] R. Mohr, B. Boufama, and P. Brand, Eds., Accurate Projective Reconstruction. New York: Springer-Verlag, 1993, vol. 825, Lecture Notes In Computer Science, pp. 257–276. [10] A. Goshtasby, “Correction of image deformation from lens distortion using Bezier patches,” CVGIP Image Understanding, vol. 47, no. 3, pp. 385–394, 1989.
Feedback-Stabilized Minimum Distance Maintenance for Convex Parametric Surfaces Volkan Patoglu and R. Brent Gillespie Abstract—A new minimum-distance tracking algorithm is presented for moving convex bodies represented using tiled-together parametric surface patches. The algorithm is formulated by differentiating the geometric minimization problem with respect to time. This produces a hybrid dynamical system that incorporates dependence on rigid body motion, surface shape, and surface boundary interconnectedness. The minimum distance between a pair of previously identified closest features is found by feedback stabilizing the dynamical equations and numerically solving the resulting closed-loop system equations. Maintenance of the minimum distance and the associated closest points during motion is achieved through the action of a feedforward controller and a switching algorithm. The feedforward controller simultaneously accounts for surface shape and motion while the switching controller triggers updates to the extremal feature pair when extremal points on one body cross between Voronoi regions of the other body. In contrast to previously available minimum distance determination algorithms, attractive properties of the new algorithm include a means of dethat maintains stability under a given distermining the highest gain cretization scheme and a large and easily characterized basin of attraction of the stabilized closest points. These properties may be used to achieve higher computational efficiency. Simulation results are presented for various planar and spatial systems composed of a body and point or composed of two bodies. Index Terms—Closest point determination, collision detection, haptic rendering, minimum distance tracking, parametric surfaces.
I. INTRODUCTION Collision detection is an essential technology in many applications, including virtual environments, computer animation, and robot simulation. A collision detector finds the points in time at which geometric objects first make contact with one another. It might further be called upon to report the minimum distance between a set of objects and to produce the corresponding pair of closest points. Desirable properties of a collision detector include computational efficiency, ease of implementation, Manuscript received September 20, 2003; revised June 22, 2004. This paper was recommended for publication by Associate Editor N. Amato and Editor H. Arai upon evaluation of the reviewers’ comments. This work was supported by the National Science Foundation under Award IIS:PECASE 0093290. The authors are with the Department of Mechanical Engineering, University of Michigan, Ann Arbor, MI 48109 USA (e-mail: [email protected]; [email protected]). Digital Object Identifier 10.1109/TRO.2005.847569
1552-3098/$20.00 © 2005 IEEE
1010
and broad applicability. Computational speed is critical, as a collision detector generally shares computational resources with dynamic simulation, collision response, and graphic or haptic rendering algorithms. Computational efficiency is at a particular premium in virtual environments with haptic rendering, where collisions must be found and the entire dynamic response computed in real-time at rates in excess of 1 kHz. To increase computational efficiency, it is very common to handle the collision detection problem in two parts: a broad phase which involves a coarse global search for potentially interacting surfaces and a narrow phase, which is initialized by the broad phase and is usually based on a fast local optimization scheme [1]. Local operations, if they can be used to improve a proposed solution by some form of gradient descent operation, can be made quite fast. Thus, the use of a narrow-phase algorithm contributes to efficiency, especially when the distance problem is called not just once for a particular pair of objects but at each time step in order to track the evolution of the minimum distance during simulation. Additionally, restriction to convex objects or features is often made in the narrow phase, since in such case the distance problem admits fast, iterative solution by convex optimization. Our interest in this paper lies primarily in narrow-phase algorithms for parametric surfaces. Most narrow-phase collision-detection algorithms available to date are applicable only to polyhedral objects or polyhedral approximations of continuously defined objects, called polygonal meshes [2]–[4]. Less well developed are collision-detection algorithms that operate directly on objects modeled with parametric surfaces. Parametric surfaces are used in computer-aided design (CAD) and computer-aided engineering (CAE) tools, where the surface representation must provide smoothness and continuity independent of the resolution of a particular rendering. One important type of parametric surface is the nonuniform rational B-spline (NURBS), which has the advantages of compact representation, high-order continuity, and easily computable surface derivatives and normals [5]. Although polygonal meshes and other indirect methods [6], [7] can be successfully used to convert a parametric model into a polyhedral model for some applications, there also exist cases in which the polyhedral approximations grow very large in the number of polygons or the intermediate representations fail to approximate surfaces with high curvature. Thompson et al. contribute a tracking-type closest point algorithm for nonpolyhedral models. Their narrow-phase algorithm is based on Newton’s method. After initializing the algorithm with the closest point, it maintains or “tracks” the closest point on the body surface [8]. Extensions to this work include [9], which handles a moving surface, and [10], which makes use of higher order derivatives and tangent plane projections at singularities. Finally, in [11], this approach is generalized to surface-to-surface interactions and combined with the “velocity formulation,” which keeps track of the exact extremal distance during contact and penetration as surfaces move, given exact initial conditions. In this paper, we present an efficient algorithm suitable for collision detection between objects modeled with parametric surfaces. Our algorithm makes direct use of the parametric surface representations and thus eliminates the need for polygonal meshes. Its efficiency derives from its full use of spatial and temporal coherence. It maintains two points on a pair of convex parametric surface patches, taking into account the motion of the bodies to which those surface patches belong. The motion of each closest point is found as a function of the motion of both bodies and the shape of both surfaces. Our algorithm is based on the formulation of a control problem which is then solved with the design of a feedback-stabilizing controller. We differentiate the geometric minimization problem to form the differential kinematics and then find and track the minimum
IEEE TRANSACTIONS ON ROBOTICS, VOL. 21, NO. 5, OCTOBER 2005
distance by driving to zero the projections of a candidate minimum distance vector onto the surface tangents at the pair of candidate closest points (witness points). The witness points are moved using a control signal comprising a feedforward term that embodies the effects of object motion and object shape and a feedback term containing the projection errors. Thanks to the feedback term in the controller, our algorithm has a large basin of attraction, that is, it does not require exact initial conditions as required by other tracking methods. In previous work [12], we presented a feedback-stabilized minimumdistance tracking algorithm for general convex parametric surfaces but not surface patches. In this paper, we extend those results into a hybrid dynamical control system so that we can handle parametric convex surfaces built from tiled-together convex surface pathes. Consideration of parametric surfaces by themselves is not quite sufficient since, within a parametric modeling environment, objects are generally modeled using collections of tiled-together surface patches. Each surface patch is bordered by curve segments formed at the intersection of two adjacent surface patches and each curve segment is bordered in turn by points formed at the intersection of three or more adjacent surface patches. These surface patches, curve segments, and points of parametric objects may be termed faces, edges, and vertices, respectively, or, generally: features. They correspond to planar surface patches, line segments, and points of polyhedral objects. For a parametric object formed using tiled-together surface patches, a Voronoi diagram also exists [13], [14]. Note that determination of Voronoi regions for curved objects can be computationally expensive; however, Voronoi diagrams are generally computed numerically before the simulation is initiated. Consequently, this step does not affect the real-time performance of Voronoi-based algorithms. In this paper, we restrict the surface patches to be convex and, for now, we also require the bodies to be convex. That is, the convex surface patches shall be oriented and joined together at their boundaries in such a way that a line joining any two points in the interior of the compact body will be wholly contained in that body. While defining convexity for a body is straightforward even when it is a tiled body, defining convexity for a surface patch requires special consideration. Like the definition for a convex space curve [15], we have defined a convex surface patch as any patch cut from a compact convex body. Convexity of the surface patch thus depends on convexity of the body from which it is cut and not on the curvature of the bounding curves that lie in the surface. If each of the two surface patches in question are both convex and the closest points lie within the Voronoi regions of each opposing surface, then there exists a unique minimum distance whose endpoints (closest points) lie within the surface patches [3], [4]. Under our algorithm, this unique minimum solution enjoys a large basin of attraction. Thus, large initialization errors are tolerated. A local guarantee follows from the asymptotic stability of the system formed by wrapping the minimum distance tracking problem with a stabilizing feedback controller. Stability of this dynamical control system may be preserved through discretization, even when explicit integration routines are used. Once convergence to the unique minimum distance between convex surfaces is achieved, the algorithm tracks the minimum distance making primary use of the feedforward term. If not only all surface patches comprising an object are convex, but the objects taken pairwise into consideration are also convex (each volume forms a convex set of points), then the uniqueness of the minimum distance can be extended to the object pairs. If, in addition, any two surface normals taken from a single surface patch never span more than 180 , then the Voronoi diagram can be used to further advantage to create efficient algorithms to find the unique minimum distance between object pairs. The 180 restriction is equivalent to requiring that the Gauss map of the surface patch
IEEE TRANSACTIONS ON ROBOTICS, VOL. 21, NO. 5, OCTOBER 2005
1011
9 9 f
1R 9 9
f
Define scalars u , v as the projections of onto the tangents u and v of surface patch and similarly define r , s as the projections onto the tangents s and r of surface patch as follows: of
f 1R
h
h
h
1 1R 1 f 9u =1 u v 1 9 =11R 1 fv 1 1R 1 h 9r =1 r 1 1R 1 h : 9s =1 s
h(
u
can be fit into an open hemisphere [16]. This stipulates that even the sphere and ellipse are to be tiled into at least two surface patches. Virtually without modification, the Lin–Canny algorithm, based on the Voronoi diagrams, may be applied to convex parametric objects to find the closest features. Computational efficiency follows from the incorporation of the Lin–Canny closest feature algorithm since now pairs of whole objects may be considered rather than all possible pairs of surface patches. In this paper, we present a discrete switching algorithm based on Lin–Canny that extends the applicability of our narrow phase to whole convex objects. We show how the discrete switching controller acting on features and the continuous controller acting on witness points within surface patches may be combined into a hybrid dynamical system to effectively extend the basin of attraction of the unique minimum distance. The body of the paper begins in Section II with the presentation of the proposed dynamic model. Controller design and analysis takes place in Section III. The switching algorithm that generalizes the method to a collision detector that may be used on objects composed of convex surface patches will be presented in Section IV. In Section V, simulation results are presented, and, finally, Section VI concludes the paper. II. MODELING Let there exist a parametric representation for the surface patch shown in Fig. 1. Use to denote a position vector to a point on the surface patch, and use u; v to refer to the mapping from
Fig. 2. Distortion corrections on an observed part of the planar calibration grid and of a 3-D object. (a), (d) Original images with strong nonparametric distortions. (b), (e) Corrected image using Bouguet’s method. (c), (f) Corrected image using two quadratic 17 17 B-spline distortion models (our method).
1009
[11] W. Press, B. Flannery, S. Teukolsky, and W. Vetterling, Numerical Recipes in C: The Art of Scientific Computing, 2nd ed. Cambridge, U.K.: Cambridge Univ. Press, 1992, pp. 59–70 and 681–688. [12] R. Hartley, “In defense of the 8-point algorithm,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 19, no. 6, pp. 580–593, Jun. 1997. [13] D. Rogers and J. Adams, Mathematical Elements for Computer Graphics, 2nd ed. New York: McGraw-Hill, 1990, pp. 289–351 and 435–458. [14] J.-Y. Bouguet, “Visual methods for three-dimensional modeling,” Ph.D. dissertation, Univ. California, 1999. [15] J. Heikkilä, “Geometric camera calibration using circular control points,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 22, no. 10, pp. 1066–1077, Oct. 2000. [16] C. Harris and M. Stephens, “A combined corner and edge detector,” in Proc. 4th Alvey Vis. Conf., 1988, pp. 147–151.
2
of squares (SS) of the distances from the detected points to the best straight line that passes through these points is computed (i.e., the 2 of the least-squares approximation), as in [7]. The DE is the sum of all SSs. The DEs in Table III clearly show that our proposed method is best in removing any distortions. The results are limited by the remaining residual error associated with noise and the aliasing effect of the pixel accuracy of the edge detection. For noiseless aliased lines, the DE is still greater than 74.6. IV. CONCLUSION The proposed multiview camera-calibration framework offers a generic, adaptive, and intuitive B-spline distortion model which can deal with both parametric and nonparametric distortions. This model is estimated using an iterative surface fitting framework with data redundancy provided from multiple views, and is naturally refined within the whole nonlinear camera-calibration process. Experimental results have shown that the proposed framework corrects standard lens distortions with better accuracy than two other existing multiview methods. Unlike the other two methods, it also corrects nonparametric distortions, but with slightly less precision. REFERENCES [1] O. Faugeras, Q.-T. Luong, and C. B. T. Papadopoulo, The Geometry of Multiple Images. Cambridge, MA: MIT Press, 2001, pp. 173–245. [2] R. Tsai, “A versatile camera calibration technique for high-accuracy 3D machine vision metrology using off-the-shelf tv cameras and lenses,” IEEE J. Robot. Autom., vol. RA-3, no. 4, pp. 323–344, Aug. 1987. [3] Z. Zhang, “A flexible new technique for camera calibration,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 22, no. 11, pp. 1330–1334, Nov. 2000. [4] X. Meng and Z. Hu, “A new easy camera calibration technique based on circular points,” Pattern Recogn., vol. 36, pp. 1155–1164, 2003. [5] M. Pollefeys and L. V. Gool, “Stratified self-calibration with the modulus constraint,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 21, no. 8, pp. 707–724, Aug. 1999. [6] A. Basu and S. Licardie, “Alternative models for fish-eye lenses,” Pattern Recogn. Lett., vol. 16, pp. 433–441, 1995. [7] F. Devernay and O. Faugeras, “Straight lines have to be straight,” Mach. Vis. Applicat., vol. 13, pp. 14–24, 2001. [8] S. B. Kang, “Radial distortion snakes,” in Proc. IAPR Workshop Mach. Vis. Applicat., 2000, pp. 603–606. [9] R. Mohr, B. Boufama, and P. Brand, Eds., Accurate Projective Reconstruction. New York: Springer-Verlag, 1993, vol. 825, Lecture Notes In Computer Science, pp. 257–276. [10] A. Goshtasby, “Correction of image deformation from lens distortion using Bezier patches,” CVGIP Image Understanding, vol. 47, no. 3, pp. 385–394, 1989.
Feedback-Stabilized Minimum Distance Maintenance for Convex Parametric Surfaces Volkan Patoglu and R. Brent Gillespie Abstract—A new minimum-distance tracking algorithm is presented for moving convex bodies represented using tiled-together parametric surface patches. The algorithm is formulated by differentiating the geometric minimization problem with respect to time. This produces a hybrid dynamical system that incorporates dependence on rigid body motion, surface shape, and surface boundary interconnectedness. The minimum distance between a pair of previously identified closest features is found by feedback stabilizing the dynamical equations and numerically solving the resulting closed-loop system equations. Maintenance of the minimum distance and the associated closest points during motion is achieved through the action of a feedforward controller and a switching algorithm. The feedforward controller simultaneously accounts for surface shape and motion while the switching controller triggers updates to the extremal feature pair when extremal points on one body cross between Voronoi regions of the other body. In contrast to previously available minimum distance determination algorithms, attractive properties of the new algorithm include a means of dethat maintains stability under a given distermining the highest gain cretization scheme and a large and easily characterized basin of attraction of the stabilized closest points. These properties may be used to achieve higher computational efficiency. Simulation results are presented for various planar and spatial systems composed of a body and point or composed of two bodies. Index Terms—Closest point determination, collision detection, haptic rendering, minimum distance tracking, parametric surfaces.
I. INTRODUCTION Collision detection is an essential technology in many applications, including virtual environments, computer animation, and robot simulation. A collision detector finds the points in time at which geometric objects first make contact with one another. It might further be called upon to report the minimum distance between a set of objects and to produce the corresponding pair of closest points. Desirable properties of a collision detector include computational efficiency, ease of implementation, Manuscript received September 20, 2003; revised June 22, 2004. This paper was recommended for publication by Associate Editor N. Amato and Editor H. Arai upon evaluation of the reviewers’ comments. This work was supported by the National Science Foundation under Award IIS:PECASE 0093290. The authors are with the Department of Mechanical Engineering, University of Michigan, Ann Arbor, MI 48109 USA (e-mail: [email protected]; [email protected]). Digital Object Identifier 10.1109/TRO.2005.847569
1552-3098/$20.00 © 2005 IEEE
1010
and broad applicability. Computational speed is critical, as a collision detector generally shares computational resources with dynamic simulation, collision response, and graphic or haptic rendering algorithms. Computational efficiency is at a particular premium in virtual environments with haptic rendering, where collisions must be found and the entire dynamic response computed in real-time at rates in excess of 1 kHz. To increase computational efficiency, it is very common to handle the collision detection problem in two parts: a broad phase which involves a coarse global search for potentially interacting surfaces and a narrow phase, which is initialized by the broad phase and is usually based on a fast local optimization scheme [1]. Local operations, if they can be used to improve a proposed solution by some form of gradient descent operation, can be made quite fast. Thus, the use of a narrow-phase algorithm contributes to efficiency, especially when the distance problem is called not just once for a particular pair of objects but at each time step in order to track the evolution of the minimum distance during simulation. Additionally, restriction to convex objects or features is often made in the narrow phase, since in such case the distance problem admits fast, iterative solution by convex optimization. Our interest in this paper lies primarily in narrow-phase algorithms for parametric surfaces. Most narrow-phase collision-detection algorithms available to date are applicable only to polyhedral objects or polyhedral approximations of continuously defined objects, called polygonal meshes [2]–[4]. Less well developed are collision-detection algorithms that operate directly on objects modeled with parametric surfaces. Parametric surfaces are used in computer-aided design (CAD) and computer-aided engineering (CAE) tools, where the surface representation must provide smoothness and continuity independent of the resolution of a particular rendering. One important type of parametric surface is the nonuniform rational B-spline (NURBS), which has the advantages of compact representation, high-order continuity, and easily computable surface derivatives and normals [5]. Although polygonal meshes and other indirect methods [6], [7] can be successfully used to convert a parametric model into a polyhedral model for some applications, there also exist cases in which the polyhedral approximations grow very large in the number of polygons or the intermediate representations fail to approximate surfaces with high curvature. Thompson et al. contribute a tracking-type closest point algorithm for nonpolyhedral models. Their narrow-phase algorithm is based on Newton’s method. After initializing the algorithm with the closest point, it maintains or “tracks” the closest point on the body surface [8]. Extensions to this work include [9], which handles a moving surface, and [10], which makes use of higher order derivatives and tangent plane projections at singularities. Finally, in [11], this approach is generalized to surface-to-surface interactions and combined with the “velocity formulation,” which keeps track of the exact extremal distance during contact and penetration as surfaces move, given exact initial conditions. In this paper, we present an efficient algorithm suitable for collision detection between objects modeled with parametric surfaces. Our algorithm makes direct use of the parametric surface representations and thus eliminates the need for polygonal meshes. Its efficiency derives from its full use of spatial and temporal coherence. It maintains two points on a pair of convex parametric surface patches, taking into account the motion of the bodies to which those surface patches belong. The motion of each closest point is found as a function of the motion of both bodies and the shape of both surfaces. Our algorithm is based on the formulation of a control problem which is then solved with the design of a feedback-stabilizing controller. We differentiate the geometric minimization problem to form the differential kinematics and then find and track the minimum
IEEE TRANSACTIONS ON ROBOTICS, VOL. 21, NO. 5, OCTOBER 2005
distance by driving to zero the projections of a candidate minimum distance vector onto the surface tangents at the pair of candidate closest points (witness points). The witness points are moved using a control signal comprising a feedforward term that embodies the effects of object motion and object shape and a feedback term containing the projection errors. Thanks to the feedback term in the controller, our algorithm has a large basin of attraction, that is, it does not require exact initial conditions as required by other tracking methods. In previous work [12], we presented a feedback-stabilized minimumdistance tracking algorithm for general convex parametric surfaces but not surface patches. In this paper, we extend those results into a hybrid dynamical control system so that we can handle parametric convex surfaces built from tiled-together convex surface pathes. Consideration of parametric surfaces by themselves is not quite sufficient since, within a parametric modeling environment, objects are generally modeled using collections of tiled-together surface patches. Each surface patch is bordered by curve segments formed at the intersection of two adjacent surface patches and each curve segment is bordered in turn by points formed at the intersection of three or more adjacent surface patches. These surface patches, curve segments, and points of parametric objects may be termed faces, edges, and vertices, respectively, or, generally: features. They correspond to planar surface patches, line segments, and points of polyhedral objects. For a parametric object formed using tiled-together surface patches, a Voronoi diagram also exists [13], [14]. Note that determination of Voronoi regions for curved objects can be computationally expensive; however, Voronoi diagrams are generally computed numerically before the simulation is initiated. Consequently, this step does not affect the real-time performance of Voronoi-based algorithms. In this paper, we restrict the surface patches to be convex and, for now, we also require the bodies to be convex. That is, the convex surface patches shall be oriented and joined together at their boundaries in such a way that a line joining any two points in the interior of the compact body will be wholly contained in that body. While defining convexity for a body is straightforward even when it is a tiled body, defining convexity for a surface patch requires special consideration. Like the definition for a convex space curve [15], we have defined a convex surface patch as any patch cut from a compact convex body. Convexity of the surface patch thus depends on convexity of the body from which it is cut and not on the curvature of the bounding curves that lie in the surface. If each of the two surface patches in question are both convex and the closest points lie within the Voronoi regions of each opposing surface, then there exists a unique minimum distance whose endpoints (closest points) lie within the surface patches [3], [4]. Under our algorithm, this unique minimum solution enjoys a large basin of attraction. Thus, large initialization errors are tolerated. A local guarantee follows from the asymptotic stability of the system formed by wrapping the minimum distance tracking problem with a stabilizing feedback controller. Stability of this dynamical control system may be preserved through discretization, even when explicit integration routines are used. Once convergence to the unique minimum distance between convex surfaces is achieved, the algorithm tracks the minimum distance making primary use of the feedforward term. If not only all surface patches comprising an object are convex, but the objects taken pairwise into consideration are also convex (each volume forms a convex set of points), then the uniqueness of the minimum distance can be extended to the object pairs. If, in addition, any two surface normals taken from a single surface patch never span more than 180 , then the Voronoi diagram can be used to further advantage to create efficient algorithms to find the unique minimum distance between object pairs. The 180 restriction is equivalent to requiring that the Gauss map of the surface patch
IEEE TRANSACTIONS ON ROBOTICS, VOL. 21, NO. 5, OCTOBER 2005
1011
9 9 f
1R 9 9
f
Define scalars u , v as the projections of onto the tangents u and v of surface patch and similarly define r , s as the projections onto the tangents s and r of surface patch as follows: of
f 1R
h
h
h
1 1R 1 f 9u =1 u v 1 9 =11R 1 fv 1 1R 1 h 9r =1 r 1 1R 1 h : 9s =1 s
h(
u
can be fit into an open hemisphere [16]. This stipulates that even the sphere and ellipse are to be tiled into at least two surface patches. Virtually without modification, the Lin–Canny algorithm, based on the Voronoi diagrams, may be applied to convex parametric objects to find the closest features. Computational efficiency follows from the incorporation of the Lin–Canny closest feature algorithm since now pairs of whole objects may be considered rather than all possible pairs of surface patches. In this paper, we present a discrete switching algorithm based on Lin–Canny that extends the applicability of our narrow phase to whole convex objects. We show how the discrete switching controller acting on features and the continuous controller acting on witness points within surface patches may be combined into a hybrid dynamical system to effectively extend the basin of attraction of the unique minimum distance. The body of the paper begins in Section II with the presentation of the proposed dynamic model. Controller design and analysis takes place in Section III. The switching algorithm that generalizes the method to a collision detector that may be used on objects composed of convex surface patches will be presented in Section IV. In Section V, simulation results are presented, and, finally, Section VI concludes the paper. II. MODELING Let there exist a parametric representation for the surface patch shown in Fig. 1. Use to denote a position vector to a point on the surface patch, and use u; v to refer to the mapping from
