The window corner algorithm for polyhedral robot and assembly sequence path planning

摘要

We present a complete and exact approach for planning a feasible path for an arbitrary polyhedral subassembly (robot), R, translating amongst arbitrary polyhedral obstacles, Phi. This is the Feasible Path problem. Our algorithm can be used to determine whether a pair of non-convex subassemblies can be separated by a sequence of fine-motion and, possibly, contact-constrained translations, even when the amount of free space tends to be severely limited. The Shortest Path problem entails finding the shortest feasible path. We present the 2D Window Corner (WC) algorithm, which is a novel solution to both problems for the case of translational paths in two dimensions, followed by the 3D WC algorithm, for feasible paths. The Polyhedral Cone Representation (PCR) is introduced to efficiently represent the geometric constraints between the boundaries of R and Phi, and as a basis for fast collision avoidance checks. We introduce the condept of WC's in the PCR and this results in reducing the search space. The Polyhedral Cone Obstacle Representation (PCOR) transforms the problem, into that of a point moving amongst a collection, O(m), of convex obstacles. The PCOR is a constructive representation of the set of contact configurations between R and Phi. In comparison, the worst-case number of vertices in the B-rep. of the C-space obstacles is O(m(exp 2)) in 2D and O(m(exp 3)) in 3D, and requires the same order of time to construct. In addition, contact-constrained, feasible motions, which lie on surfaces without interior points will not be accessible in a typical B-rep. of the C-space obstacles. In two dimensions, a Window Graph with k nodes and O(k(exp 2)) edges, is built, and the shortest path found, in O(km log(m)) time where m is the product of the number of vertices describing the robot and obstacles respectively, k = O(m) in the worst case and 1 less than or equal to k less than or equal (m 2). The nodes of the W-Graph correspond to a subset of the convex vertices of the C-space representation. In 3D, the WC algorithm constructs a Window Edge (WE)-Tree and solves the Feasible Path problem, in worst-case time O(m(exp 5)). Our approach finds WE, which correspond to a subset of the convex edges of the C-space boundary, and traces a path through WC. We have implemented and tested the WC algorithms and examples from robot-indepenent assembly sequence planning are presented in this paper. The 3D WC algorithm searches a finite search space, made possible by the use of Window Corners, and both the 2D and 3D versions are attractive for rapid implementation in robotic and assembly sequence path planning domains.