0
Research Papers

# Optimal Paths for Polygonal Robots in SE(2)

[+] Author and Article Information
Monroe Kennedy, III

GRASP Laboratory,
MEAM Department,
University of Pennsylvania,
e-mail: kmonroe@seas.upenn.edu

Dinesh Thakur

GRASP Laboratory,
MEAM Department,
University of Pennsylvania,
e-mail: tdinesh@seas.upenn.edu

M. Ani Hsieh

GRASP Laboratory,
MEAM Department,
University of Pennsylvania,
e-mail: m.hsieh@seas.upenn.edu

Subhrajit Bhattacharya

CSE Department,
Lehigh University,
Bethlehem, PA 18015
e-mail: sub216@lehigh.edu

Vijay Kumar

GRASP Laboratory,
MEAM Department,
University of Pennsylvania,
e-mail: kumar@seas.upenn.edu

1Corresponding author.

Manuscript received September 22, 2017; final manuscript received December 14, 2017; published online February 1, 2018. Assoc. Editor: Andrew P. Murray.

J. Mechanisms Robotics 10(2), 021005 (Feb 01, 2018) (8 pages) Paper No: JMR-17-1313; doi: 10.1115/1.4038980 History: Received September 22, 2017; Revised December 14, 2017

## Abstract

We consider planar navigation for a polygonal, holonomic robot in an obstacle-filled environment in SE(2). To determine the free space, we first represent obstacles as point clouds in the robot configuration space (C). A point-wise Minkowski sum of the robot and obstacle points is then calculated in C using obstacle points and robot convex hull points for varying robot configurations. Using graph search, we obtain a seed path, which is used in our novel method to compute overlapping convex regions for consecutive seed path chords. The resulting regions provide collision-free space useful for finding feasible trajectories that optimize a specified cost functional. The key contribution is the proposed method's ability to easily generate a set of convex, overlapping polytopes that effectively represent the traversable free space. This, in turn, lends itself to (a) efficient computation of optimal paths in $ℝ3$ and (b) extending these basic ideas to the special Euclidean space SE(2). We provide simulated examples and implement this algorithm on a KUKA youBot omnidirectional base.

<>

## References

Agarwal, P. K. , Flato, E. , and Halperin, D. , 2002, “ Polygon Decomposition for Efficient Construction of Minkowski Sums,” Comput. Geom., 21(12), pp. 39–61.
Leven, D. , and Sharir, M. , 1987, “ Planning a Purely Translational Motion for a Convex Object in Two-Dimensional Space Using Generalized Voronoi Diagrams,” Discrete Comput. Geom., 2(1), pp. 9–31.
Varadhan, G. , and Manocha, D. , 2006, “ Accurate Minkowski Sum Approximation of Polyhedral Models,” Graph. Models, 68(4), pp. 343–355.
Wu, Y. , Shah, J. J. , and Davidson, J. K. , 2003, “ Improvements to Algorithms for Computing the Minkowski Sum of 3-Polytopes,” Comput.-Aided Des., 35(13), pp. 1181–1192.
Lien, J. M. , 2008, “ Covering Minkowski Sum Boundary Using Points With Applications,” Comput. Aided Geom. Des., 25(8), pp. 652–666.
LaValle, S. M. , and Kuffner, J. J. , 2001, “ Rapidly-Exploring Random Trees: Progress and Prospects,” Algorithmic and Computational Robotics: New Directions, B. R. Donald, K. M. Lynch, and D. Rus, eds., A K Peters, Wellesley, MA, pp. 293–308.
Kuffner, J. J. , and LaValle, S. M. , 2000, “ RRT-Connect: An Efficient Approach to Single-Query Path Planning,” IEEE International Conference on Robotics and Automation (ICRA), San Francisco, CA, Apr. 24–28, pp. 995–1001.
Hart, P. E. , Nilsson, N. J. , and Raphael, B. , 1968, “ A Formal Basis for the Heuristic Determination of Minimum Cost Paths,” IEEE Trans. Syst. Sci. Cybern., 4(2), pp. 100–107.
Mellinger, D. , Kushleyev, A. , and Kumar, V. , 2012, “ Mixed-Integer Quadratic Program Trajectory Generation for Heterogeneous Quadrotor Teams,” IEEE International Conference on Robotics and Automation (ICRA), Saint Paul, MN, May 14–18, pp. 477–483.
Rodriguez, A. , and Mason, M. T. , 2012, “ Path Connectivity of the Free Space,” IEEE Trans. Rob., 28(5), pp. 1177–1180.
Deits, R. , and Tedrake, R. , 2015, “ Computing Large Convex Regions of Obstacle-Free Space Through Semidefinite Programming,” Springer Tracts in Advanced Robotics, Vol. 107, Springer, Cham, Switzerland, pp. 109–124. [PubMed] [PubMed]
Liu, S. , Watterson, M. , Tang, S. , and Kumar, V. , 2016, “ High Speed Navigation for Quadrotors With Limited Onboard Sensing,” IEEE International Conference on Robotics and Automation (ICRA), Stockholm, Sweden, May 16–21, pp. 1484–1491.
Richter, C. , Bry, A. , and Roy, N. , 2016, “ Polynomial Trajectory Planning for Aggressive Quadrotor Flight in Dense Indoor Environments,” Robotics Research, Springer, Cham, Switzerland, pp. 649–666.
Watterson, M. , and Kumar, V. , 2015, “ Safe Receding Horizon Control for Aggressive MAV Flight With Limited Range Sensing,” IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Hamburg, Germany, Sept. 28–Oct. 2, pp. 3235–3240.
Chitsaz, H. , LaValle, S. M. , Balkcom, D. J. , and Mason, M. T. , 2006, “ Minimum Wheel-Rotation Paths for Differential-Drive Mobile Robots,” IEEE International Conference on Robotics and Automation (ICRA), Orlando, FL, May 15–19, pp. 1616–1623.
Knepper, R. A. , and Mason, M. T. , 2012, “ Real-Time Informed Path Sampling for Motion Planning Search,” Int. J. Rob. Res., 31(11), pp. 1231–1250.
Balkcom, D. J. , and Mason, M. T. , 2002, “ Time Optimal Trajectories for Bounded Velocity Differential Drive Vehicles,” Int. J. Rob. Res., 21(3), pp. 199–217.
Balkcom, D. J. , Kavathekar, P. A. , and Mason, M. T. , 2006, “ Time-Optimal Trajectories for an Omni-Directional Vehicle,” Int. J. Rob. Res., 25(10), pp. 985–999.

## Figures

Fig. 1

Minkowski sum and pointwise representation. With a dense point cloud, the pointwise sum can be used to approximate the Minkowski sum, which is indicated by the dashed line. This dashed line shows how close the robot centroid can approach the obstacle without collision.

Fig. 2

Minkowski pointwise sum for robot centroid in C with convex hull shown in ℝ2 and ℝ3. The triangles represent the initial and goal pose of a polygonal robot, the stacked polytopes represent the Minkowski sum in different robot orientations (here we show three orientations θ0, θ120, θ240). This provides a representation of the SE(2) free configuration space in ℝ3.

Fig. 3

Convex corridor generation: obstacle point ϕ is distance r1, r2 from planes that define seed path chord segment (pi, pi+1) and ϕv=ϕ−pi defines ϕv,⊥ (constraint plane normal). Figures 3(b) and 3(c) show 2D overlapping regions between consecutive chords.

Fig. 4

Simulated trajectory: rectangle is robot hull, orientation is shown with arrows and height. Sparse connected arrows and densly connected arrows are seed path and optimal path, respectively.

Fig. 5

Alternate map with same representation as Fig. 4

Fig. 6

Simulated examples demonstrating robustness in tight corridor scenarios: (a) map 1 and (b) map 2

Fig. 7

Implementation on youBot with known obstacles equivalent to Fig. 6(a)

Fig. 8

Smooth trajectory of Fig. 6(a). Points show the seed path, dashed lines are the desired trajectory and solid lines represent the actual odometry. (a) X position versus normed time, (b) Y position versus normed time, and (c) θ position versus normed time.

Fig. 9

General method comparison of free space: single chord indicated by arrow, 30 generated obstacles and free space characterized by each method: (a) IRIS SDP method, (b) proposed method, and (c) Liu method

## Errata

Some tools below are only available to our subscribers or users with an online account.

### Related Content

Customize your page view by dragging and repositioning the boxes below.

Related Journal Articles
Related Proceedings Articles
Related eBook Content
Topic Collections