Introduction
In the field of robotics, the "findpath" problem is a fundamental challenge: determining a collision-free trajectory for a robot moving from a start configuration to a goal configuration within a cluttered environment. While this problem is relatively straightforward for point-robots, it becomes significantly more complex for rigid bodies capable of rotation. When an object rotates, the physical space it occupies changes non-linearly, rendering simple bounding-box or bounding-circle approximations either insufficient or overly conservative.
This paper, originally published in 1983 by Rodney Brooks and Tomas Lozano-Perez, introduces a robust method for solving the findpath problem for polygonal objects with three degrees of freedom (2D translation and 1D rotation). The approach combines the conceptual power of Configuration Space (C-space) with a recursive cell subdivision strategy to achieve a resolution-complete solution.
Methodology
Configuration Space (C-space) Representation
The core of the algorithm lies in transforming the physical workspace into a Configuration Space. In C-space, the robot is represented as a single point, while the obstacles are "expanded" to account for the robot's geometry. For a fixed orientation, these C-space obstacles ($CO_A(B)$) are convex polygons. However, as the robot rotates, these obstacles sweep out complex, curved 3D surfaces in the $(x, y, \theta)$ space.
Recursive Cell Subdivision
To manage the 3D C-space, the algorithm employs a recursive subdivision technique. The space is partitioned into "rectangloid" cells, each classified into one of three categories:
- Empty: The cell contains no part of any obstacle.
- Full: The cell is entirely contained within an obstacle.
- Mixed: The cell contains both free space and obstacle boundaries.
Iterative Search and Refinement
The algorithm utilizes the A search algorithm to find a path through the cells. It first attempts to find a path consisting solely of Empty cells. If no such path exists, it identifies a path that includes Mixed* cells. These Mixed cells are then subdivided into smaller rectangloids, re-classified, and the search is repeated. This "lazy evaluation" ensures that high-resolution subdivision only occurs where it is necessary to find a valid path.
Key Insights
Constraint Linearization
A major contribution of this work is the handling of the non-linear constraints introduced by rotation. The authors developed a method to treat the curved surfaces of 3D C-space obstacles as a series of linear constraints that are valid over small intervals of rotation ($\Delta\theta$). By linearizing these constraints, the algorithm can use efficient geometric tests to classify cells as Empty, Full, or Mixed.
Search Strategy and the "Attractive" Cell Trap
The authors observed that A* search is naturally biased toward smaller cells. If the algorithm refines the area near the starting point first, it may waste significant computational resources exploring high-resolution local details instead of progressing toward the goal. To mitigate this, the global search is decomposed into local subproblems, which are solved in reverse order (from goal to start). This strategy prevents the search from becoming bogged down in the high-resolution clusters typically found at the beginning of a search.
Results and Discussion
The algorithm was evaluated on complex polygonal environments, including the classic "Piano Mover's" problem. In difficult cases, the final C-space representation involved over 1,000 cells and 2,000 connectivity arcs. While execution times on 1980s hardware (MIT Lisp Machine) ranged from seconds for simple problems to tens of minutes for complex ones, the method successfully solved path-planning tasks that were previously considered intractable for general-purpose planners.
Limitations and Future Directions
Despite its success, the subdivision approach faces challenges with higher dimensionality. Extending the method to 6D (3D translation and 3D rotation) would lead to an exponential increase in the number of cells. Future work suggested by the authors includes hybrid approaches that combine fast, approximate path-finding with this subdivision method for final verification and refinement, as well as the development of better heuristics for cell-splitting to improve convergence speed.
Summary updated