Optimal Paths for Polygonal Robots in SE(2)

Robots operating in human-based environments must be able to plan and execute trajectories that avoid collision and satisfy robot kinematics and power constraints. Consider a planar robot defined by a polygonal hull in the special Euclidean group SE(2) that must navigate a cluttered environment. Once obstacles are detected, the free space that the robot can traverse without collision must be identified in the subset of the configuration space. Once this free space is modeled, a collision-free optimal trajectory is found.

The proposed process of defining an optimal trajectory can be summed up by first identifying and representing the occupied configuration space, and second, defining a viable homotopy class (defined by set of paths that can be deformed continuously into each other while not intersecting obstacles). This is done by first characterizing a seed path using sample based or flood fill path finding algorithms, then modeling the free space using sets of linear constraints. Third, obtain the optimal, final trajectory based on the specified objective function for the polygonal robot. To define the free space within the configuration space, the first step is to account for both the presence of obstacles and robot geometry. To do this, a Minkowski sum is used to determine how close the polygonal robot can come to known obstacles without collision. Agarwal et al. [1] as well as Leven and Sharir [2] consider the Minkowski sum of polygons, adding together the vertices, which they state has complexity $O(mn log(mn))$ for m obstacle vertices and n object vertices. It is noteworthy that calculation time-scales with object complexity. Varadhan and Manocha [3] find obstacle point clouds, and then find the convex hull to construct polyhedral regions. They state that convex polyhedral sum can have $O(n2)$ complexity, but nonconvex can have up to $O(n6)$⁠. Despite being challenged by degenerate models and convex decomposition, these approaches are able to approximate 3D Minkowski sum of polyhedral and use it for motion planning in translational degrees-of-freedom. Wu et al. [4] use a slope diagram to perform the sum as opposed to using vertex addition methods with the convex hull. They state that their method is 3–12 times faster, and plan to extend their algorithm to higher dimensions. Lien [5] shows that using points on the surfaces of two models to perform the Minkowski sum is easier than generating an equivalent representative mesh. However, they state that in this method, more points may be generated than required. All of these methods perform well; however, in this work, we will focus on path planning in the robot configuration space; hence, obstacles must be transformed from $ℝ2$ into SE(2). Of the stated methods, this is done most simplistically using a point cloud representation of obstacles. Once the obstacles have been represented in the robot configuration space C, an initial path (seed path) can be found using common methods such as a flood fill algorithm, rapidly exploring random trees (RRT, RRT-Connect among others) [6,7].

Given a representation of obstacles in the configuration space, an initial suboptimal path must be found. There are many sample-based planners such as rapidly exploring random tree, that provide quick, collision-free, suboptimal solutions, and other planners such as flood fill algorithms Dijkstra or A-star, which take more time to compute but provide shortest path solutions with global guarantees. However, it is often the case that while a quick solution is desired, path optimality must also be considered for robot navigation. This generalized optimality could be trajectories that minimize distance traveled, jerk, or another metric. In a cluttered environment, there may be multiple homotopy classes. Graph search algorithms will explore all homotopy classes and report the globally shortest discrete path, while sample-based methods will randomly select a homotopy class with no guarantee of it containing the globally shortest path as stated in Ref. [8]. Therefore, when optimizing a trajectory based on an initializing seed path, if the optimum trajectory must be the global optimum, then a graph search method should be used. When a sample-based planner is used, the resultant optimal trajectory will be the optimal for that homotopy class.

An alternate but notable method to address this problem of finding an optimal trajectory in an obstacle filled environment is using a mixed integer quadratic program (MIQP) [9], which allows for planning in nonconvex regions, and paths are required to lie outside obstacles (modeled by planes that define its convex hull). While this method is effective, limitations are that obstacles must be modeled by their convex hull, and the optimization problem becomes computationally expensive for many obstacles. In addition, the resulting paths may not be unique, equivalently stated, there may be multiple local minimum producing many solutions. Another consideration is that ordinary range finder sensors return a point cloud for obstacles and occupancy maps are constructed based on these point clouds; hence, methods that leverage this representation of obstacles with less overhead computation are favored in practice.

Hence, we will focus on methods that rely on obstacles represented by point clouds, a given seed path, and a representation of free space to realize an optimal trajectory. In order to effectively model the free space for a quadratic program, linear constraints must be used to represent the free space. This can be done by determining a set of overlapping convex corridors that together define the traversable free space, and can be used directly in the optimization problem in order to find the optimal trajectory. To find such corridors, Rodriguez and Mason [10] use set theory to characterize the free space a robot can traverse, proving that the existence of smooth paths can be guaranteed if the initial path is homotopic to a piecewise smooth path and if the free space is a manifold with a boundary, then the smooth path lies entirely in the interior of the free space. Another method is work done by Deits and Tedrake [11], where they propose a method to expand ellipsoids using an iterative regional inflation by semidefinite (IRIS) program to expand an ellipsoid in a free region until the intersection of the ellipsoid with obstacle points. These intersections produce tangents to the ellipsoid which are then used to construct a set of hyperplanes defining a polytope. Iterating this method while optimizing their growth of the ellipsoid, they then find the largest ellipsoid that fits in a polytope defining the free space, with the condition that the volume does not decrease between iterations. Liu et al. [12] proposed a similar but faster method for inflating polygons around path chords and using the interior of the polygons as the free convex corridor space. These polygon shapes are determined a priori by expanding in preset directions. Our proposed method builds on these existing methods by defining the free region surrounding seed-path chords using polytopes that are constructed based on the relative orientation of the closest obstacle points to the chord of interest. While our method finds polytopes around chords whose final shape is determined online as in IRIS, our method does not include an optimization loop as IRIS does making our method more tractable in some cases. Our method may underperform the Liu method with respect to computational speed, but our final convex corridor polytope shape is not defined a priori, allowing us to better adapt and define the usable free space. Similar to both the IRIS and Liu methods, our proposed method of characterizing free space in a homotopy class expands to any linear high-dimensional configuration space.

Once the convex corridor has been obtained, a quadratic program can be used to find the optimal trajectory. Again, the main advantage of this method over MIQPs is the inherent limitation of MIQP when the number of obstacles is very large as each obstacle must be modeled with its convex hull. In contrast, the methods presented in Refs. [11] and [12] are more likely to produce and require fewer hyperplanes. And in these methods, piecewise polynomials are found by minimizing a derivative of the trajectory, and applying equality constraints at piecewise polynomial connections for position and derivatives, and in Watterson and coworkers, using convex corridors as inequality constraints [12–14].

Robot kinematics and dynamics play an important role in objective function selection. Work considering nonholonomic robots for sample-based planning and shortest path trajectories include [15–17]. Similar control strategies to these were developed for holonomic systems in Ref. [18]. We argue that in each of these cases, if the desired trajectory is smooth, unique, and can be modeled by an objective function, then the proposed approach lends itself to generic robotic platforms and integration with more complex control strategies. The main contribution of this work lies in the simple yet novel method of convex corridor generation. We find the traversable free space for trajectory planning with adaptable polytopes that yield overlapping convex regions, and the corridor generation algorithm can be applied in any n-dimensional space a problem requires. Our key contributions are:

1. (1)

   By representing obstacles as point clouds in the robot configuration space, we are able to represent the free space using overlapping convex polytopes, where every hyperplane of a polytope is defined at respective obstacle points which lie in that hyperplane.

2. (2)

   As opposed to graph-based search methods, our approach obtains smooth trajectories that minimize velocity, acceleration, jerk, or any other specified objective. These paths are relevant to the particular robots dynamic, kinematic, or power constraints.

3. (3)

   Our polytope construction can be calculated in linear time similar to the Liu method [12], which expands set polytopes, but with our ability to adapt the polytope construction, we recover more free space, which approaches the slower yet more free space recovering IRIS method [11].

4. (4)

   We find the path that satisfies the objective function while remaining in the overlapping convex regions.

The rest of this paper is organized as follows: In Sec. 2, we formally describe the problem and an overview of our approach. We then describe how we represent obstacles in the configuration space, plan a seed path, determine the convex corridors, generate a smooth trajectory, and control to follow the output trajectory. In Sec. 3, we present results tested on the KUKA youBot platform.

We consider a polygonal robot navigating on a plane. The configuration space of the robot, in the absence of obstacles, is $SE(2)≡ℝ2×S1$⁠. To solve the trajectory generation and control problem, we propose a solution pipeline where we define the convex hull of the robot, describe the obstacles as a point cloud in $ℝ2$⁠, build the corresponding obstacles in the configuration space, SE(2), and use A-star (or sample based planner) to obtain a seed path in the free configuration space. This path is then used to generate convex corridors in the configuration space, which in turn are used, in conjunction with the seed path, to find an optimal path in the same homotopy class. While obstacles could be represented as polytopes and the optimal trajectory generation could be solved in this manner using a mixed integer quadratic program [9], the advantage of considering obstacles as point clouds lies in the case when the obstacles' full boundary in the environment are unknown and only local sensor readings and an occupancy map are available. By finding a seed path in the configuration space, we establish a homotopy class (region of space where there is a continuous transformation between all paths in that region that do not collide with obstacles), and the set of constraints which define this free space may be far fewer less than the constraints required to describe the polytopes of all obstacles in the environment. After solving for the optimal trajectory in this homotopy class using the convex corridors, we then compute control inputs for the robot to follow the resultant trajectory in SE(2). The proposed solution framework is summarized in Algorithm 1. Line 1 defines the robot convex hull. Line 1 represents the obstacles in the robot configuration space and computes the Minkowski sum of the robot and obstacle points. Line 1 then finds a seed path using a sample-based planner given start and goal points $ρs,ρg$⁠. Line 1 finds the convex corridors; this algorithm is expanded in Algorithm 2. Line 1 then solves the optimization problem to produce a desired trajectory.

Algorithm 1: Optimal Navigation Planning

Input: Robot start and goal states: $ρs,ρg$⁠, Obstacles Points: $o⊂ℝ2$

Output: Trajectory, $τ(t)$⁠, in SE(2)

1 $R¯$ = Robot_Convex_Hull;

2 $M¯$ = Compute_Minkowski_Points(⁠$R¯,o$⁠): “extrude” obstacles to SE(2) and perform Minkowski sum;

3 $P¯$ = Compute_Seed_Path(⁠$M¯,ρs,ρg$⁠);

4 A_set = Generalized_Convex_Corridor_Decomp(⁠$P¯,o$⁠);

5 $τ(t)$ = Find_Optimal_Traj_QP(⁠$Aset,ρs,ρg,t0,tf$⁠)

In order to determine the free space within C, obstacles must be represented in C using a Minkowski sum. The resultant representation of free space allows for the modeling of the robot as a point in the free space. A simplistic way to perform this sum is to represent the obstacle as a point cloud in $ℝ2$⁠, then represent the pertinent points on the robot hull by creating a set of vectors from the centroid of the robot to points on its convex hull with the lower limit of points being the number of robot vertices. This set of vectors from points on the robot hull to the robot center emanating from the same point produce a set of points will denote as $R¯$⁠, which are shown as circles in the bottom left of Fig. 1. The addition of obstacle and robot hull point clouds: $o⊕R¯$ provides an approximation to the Minkowski sum for a given robot orientation with complexity O(mn) for m obstacle points and n robot hull points, as shown in Fig. 1. If this is then repeated for varying robot orientations, it provides an approximation of SE(2) in $ℝ3$ as shown in Fig. 2 and this defines the free space within C. In order to successfully solve our problem in $ℝ3$⁠, it is required that obstacles and goals be repeated for $θ∈(−3π:3π)$ with the start pose defined in $θ∈(−π:π)$⁠. This is done in order to use linear constraints, which are necessary to build our corridors. If configuration space is bounded between $(−π:π)$⁠, we would limit our solutions as the real robot would be able to continue to rotate beyond these bounds as $θ∈S1$⁠. After repeating our goals three times, we then find trajectories to each goal and select the best from some heuristic. It is worthy to note that for very cluttered environments, it may be advantageous to repeat goals further (for multiple, confined rotations); but there is a direct trade-off between solving for multiple goals and computation time.

After representing obstacles in $ℝ2$ in SE(2), we use either the A-star graph search algorithm to obtain a sample path, or a sample-based planner such as RRT-Connect to find a viable seed path. When there are multiple homotopy classes that can be chosen, A-star will provide the group that will contain the global minimum path. In large maps, this may be intractable and a sample-based planner is favorable. RRT-Connect is a good choice due to its dual nature of being sample based yet greedy. In RRT-Connect, two trees are grown simultaneously from the start and goal poses, and in each iteration, the trees alternate between growing in a sample-based direction and trying to greedily connect the tree branches. Hence, in environments with few obstacles, this method performs similarly to other greedy method, but in a cluttered environment, it behaves as any other sample-based planner. When using a sample-based planner, we use the open motion planning library to implement RRT-Connect and use the Minkowski obstacle point cloud described in Sec. 2.1, and the point cloud library to check for collisions. Given an initial, nonoptimal seed path from the sample-based planner, we define convex regions that can be represented by linear constraints. By expressing the convex regions as linear constraints, quadratic programming is then used to compute a globally optimal trajectory within the same homotopy class as the seed path.

Algorithm 2: Generalized Convex Corridor Decomposition.

Input: Seed Path Points: $pκ×3$⁠, Obstacle-Robot Minkowski Points: $M¯m×3$

Output: Corridor Hyperplanes A_set

1 Initialize(A_set);

2 foridx = 1:(κ-1) do

3  $Γ¯$ = Find_Planes_Given_Chord(⁠$pidx,pidx+1$⁠);

4  $L¯$ = Rank_Obstacle_Points(⁠$Γ¯$⁠, $M¯$⁠);

5  A_chord = Construct_Corridor(⁠$L¯, M¯$⁠);

6  A_set.append(A_chord)

7 end

8 returnA_set

We propose an algorithm that finds convex corridors given an initial collision free path and an obstacle point cloud. Line 2 computes the planes that represent the chord line between consecutive seed path points. Line 2 then ranks obstacle points using the summed distances from the two planes describing the chord line. Line 2 then constructs the convex corridor using the ranked obstacle points as discussed in the next paragraph and shown in Fig. 3. Algorithm 2 scales immediately to n-dimensions as it relies on the fact that affine hyperplanes in n dimensional Euclidean space are described as $a1x1+⋯+anxn=b$ and separate the space into halves. The intersection of two n-dimensional hyperplanes defines a line in n-dimensions, given hyperplane normals are not aligned. The algorithm also relies on the properties that the dot product and two-norm are defined in n-dimensions for vectors $g$ and $h$⁠: $g·h=∑i=1ngihi$ and $||g||=(∑i=1ngi2)0.5$⁠, respectively.

Geometrically, our algorithm can be represented as shown in Fig. 3. Given a seed path, every path chord is represented by two planes that can be obtained by finding the normal component of a random vector with the chord to obtain a plane. The second plane is obtained by finding a vector that lies in the first plane and is perpendicular to the chord vector; this vector defines the normal for the second plane. The intersection of these hyperplanes describes a line in three-dimensions, and obstacle points can be ranked by their summed distances from these planes. After sorting, the obstacle points are then iterated through, and vectors are found from the closest point on the chord to the obstacle points and used to create planes that contain the obstacle point. Obstacle points that remain on the same side of this plane as the chord remain for further corridor creation while the rest are discarded from that chords consideration. This process is repeated for κ path points (⁠$κ−1$ times). A set is then returned where every element of the set contains list of planes for a respective chord. Generalization of the algorithm to any n-dimensional space is straightforward.

In order to show that this algorithm will produce overlapping convex regions, we must note that this algorithm satisfies the following constraints:

1. (1)
   For consecutive seed path points $pi,pi+1$⁠, points along this chord $pi,i+1$ and k constraint planes A for this chord

   $Akpi,j<0 ∀k$

   (1)
2. (2)
   For a given chord with k constraint planes A, and m obstacle points $O$

   $∃k∋AkOm≥0$

   (2)
3. (3)
   For k planes and m obstacle points

   $k≤m$

   (3)

The first constraint ensures that the chord lies within the polytope where planes are defined by normals that point away from the chord. The second constraint requires that obstacles either sit on a plane or outside of at least one plane. The third constraint states that there may be fewer planes than obstacle points, as obstacle points that are far away may be negated by constraints constructed with closer points. But the equality in the third constraint is derived from the fact that if the obstacles are placed in a perfect circle/sphere around the chord, then a plane must be defined for every obstacle point, hence the equality. By satisfying all three constraints, this ensures that the convex regions overlap, as constraint in Eq. (1) is satisfied for each chord. Then since Eq. (2) is satisfied, the combined convex region will be obstacle free.

The algorithm components that heavily contribute to computational complexity for m obstacle points in n-dimensional space are finding obstacle points distances from the planes with complexity O(nm), and ranking these distances using the heapsort algorithm with complexity $O(m log(m))$⁠. As $m≫n$⁠, the driving complexity is the ranking step, the algorithm complexity is driven by the sorting and running this for k chords results in an overall complexity of $(κ−1)O(m log(m))$⁠.

Figures 4 and 5 show the seed path as sparsly connected arrows, where orientation is indicated using both arrow direction and height (where z-axis is orientation $θ∈(−3π,3π)$⁠), demonstrating embedding in $ℝ3$⁠. Densly connected arrows indicate the optimal path using this same convention; it can be seen that the resultant optimal trajectories are smooth and minimize distance traveled for the given objective function. In Figs. 4–7, the rectangles represent instances of the polygonal robots pose in the plane while following the optimal path.

Figure 6 shows the method working in more scenarios that require navigation through tight corridors as well as open space. This was implemented on a youBot base shown in Fig. 7(b).² In Fig. 8, points represent seed path points, the dashed path represents the desired trajectory, and solid lines represents the odometry using the control law presented in Eq. (9) for the experiment shown in Fig. 7. In Fig. 8, the trajectories are over normed time. This is scaled as in Ref. [9] to meet the desired trajectory traversal rate using the normed control time represented by $tc=(t−t0)/tscale$ where t is the current time, t₀ is the start time, t_scale is the scaling.

One limitation to this method is sensitivity to scale $(x,y,θ)$⁠; if scale of θ is much smaller than that of x and y, the system will tend not to use the paths that require configurations that allow for passage through tight corridors. But if the scale of θ is much greater than that of x, y, the system will tend to come very close to obstacles in shortest distance objectives. We found in experimentation that a ratio of 1/5 allowed for leveraging of tight corridors yet maintained respectable distances and this would need to be tuned on an application basis. Our method is preferable in scenarios where obstacles are partially occluded, and the free traversable space is to be maximized quickly. We show that our method extends immediately to higher dimensions. We note that what drives complexity is the number of obstacles points, and for range finding sensors that detect a countable number of points, this method is appropriate.

To compare the proposed method to the leading methods in Refs. [11] and [12], we implemented all three methods in robot operating system and examined their output for a single chord defined from start $x=0,y=−0.3$ to goal $x=0,y=0.3$⁠. The simulation units are consistent for comparison, and are considered to be meters. For this chord, we generated obstacles within the world bounds of $xw=±1 m,yw=±1 m$ and discretized the space with step size of $0.02 m$ in both x and y. We visualize this in robot operating system-rviz, shown in Fig. 9. Figure 9(a) shows the IRIS methods' free, convex space, Fig. 9(b) shows the proposed methods free convex space for this chord, and Fig. 9(c) shows the free space generated by the Liu method in Ref. [12]. It is important to note that the Liu method performs ray tracing, and subdivides a chord into sequential convex regions defined by co-linear ray intersections. Hence, while the Liu method can be compared in terms of free space, it cannot determine the convex space around a single chord as in the proposed method and IRIS method [11]. In Table 1, we calculate the free space for the system shown in Fig. 9 for 10, 30, and 50 obstacles each for 100 iterations. By dividing each respective calculated free space by the IRIS method, we obtain a scale free relation that allows for comparison of free space between all methods, and convex space between IRIS and the proposed method.

Table 1 shows the free space comparison. It is important to note that the world size remains constant while the number of obstacles increases for each trial (100 trials for each number of obstacles). Our proposed method is consistently about 81% (with less than 23% standard deviation) of the free convex space calculated by IRIS. The Liu method computes less free space than the proposed and IRIS methods in open environments with few obstacles, but as the environment becomes more cluttered, the ray-tracing feature of the Liu method allows it to find more free space. It is important to note that the free space found in the Liu method in these instances does not translate to usable convex free space required for optimization (as shown in Fig. 9(c)). In addition, the Liu method's ray tracing makes it inherently sensitive to world discretization and dimensionality. The IRIS method finds the maximum convex polytope that can contain the largest ellipsoid. As the proposed method formulation is to expand a polytope around the chord, there are instances when this convex volume will exceed the IRIS method (as it solves a similar but different problem). It must be noted that the key difference between the proposed method and the IRIS method is that they solve different problems, with the main goal of the proposed method being to find the maximum usable free space around a given chord and the goal of IRIS to find the polytope associated with maximum volume ellipsoid in the region. Through this analysis, we demonstrate the proposed methods' effectiveness in the given application, as well as the comparability of the computed free space to existing methods.

In this work, we presented a method of defining convex corridors that specify free space and provide linear constraints required for finding the optimal trajectory for polygonal robots in cluttered environments. By representing obstacles as point clouds, this method combines well with off the shelf range sensors. Obstacle points are represented in the robot configuration space and by using a pointwise Minkowski sum, the occupied regions are defined for every orientation of the polygonal robot. To perform covering of SE(2) in $ℝ3$⁠, obstacles are then repeated between $θ∈(−3π,3π)$⁠. Using a graph search or sample-based planner (in results we use a graph search), we are able to obtain an initial seed path. By using obstacle points to define hyperplanes, we define polytopes that enclose the chords of the seed path and inherently overlap, producing a chain of overlapping polytopes and hence convex regions. These chains of overlapping convex regions then allow us to perform a quadratic program and obtain a trajectory that minimizes a desired objective such as velocity, which would produce smooth, short trajectories, or any higher derivative desired the selection of which is driven by robot dynamics, kinematics, or power constraints. Finally, we compare our method to leading methods and show that while our method is competitive in free space computation, our method best serves the goal of quickly finding consecutive overlapping convex regions. Advantages of this methodology are that first, adaptable polytopes can be constructed that maximize the representation of free space within the configuration space of a robot and this does not require an optimization step to determine the polytopes as in Ref. [11]. Second, this overall methodology is scalable for many obstacles as opposed to MIQP methods. Disadvantages of the proposed method are that first, as there is no direct relation between the translational and rotational units, the selection of the scaling between the x-axis, y-axis, and θ-axis must be selected based on performance. Second, without prior knowledge of the arrangement of obstacles and the seed path, a naive approach is to rank the obstacle points for each chord; this can become computationally intensive depending on the number of obstacles points. In future work, we plan to extend this method to SE(3), by representing SE(3) in higher dimensions that allow for linear constraints. Also, in future work, we will investigate ways to improve computational performance of the method by considering autonomous ways to determine if re-ranking of points is required from chord to chord to increase computational efficiency.

• Defense Advanced Research Projects Agency (Grant No. HR001151626/HR0011516850).

• National Science Foundation (Grant Nos. DGE-1321851, IIS-1328805, IIS-1426840, NSF-1138847, and NSF-1328805, NSF-IIP-1439681).

A_chord =

planes that define convex region around single chord

A_set =

combined set of chord planes from each chord

C =

robot configuration space

$e(t)$ =

robot pose error

J =

objective function

L =

omni-directional wheel velocity matrix

$L¯$ =

ranking list of obstacle points based on distance from chord

$M¯$ =

pointwise approximation of Minkowski sum

o =

obstacle points

O =

computational complexity

$P¯$ =

set of seed path points

p_i =

seed path points

Q =

time convolution matrix

$R$ =

robot frame

$R¯$ =

constructed points based on robot hull for Minkowski sum approximation

r_i =

obstacle distance to chord plane

$ℝn$ =

real coordinate space of n-dimension

$S1$ =

unit circle

SE(n) =

special Euclidean group for nth dimension

t =

time (s)

T_i =

allotted traversing time for chord segment (s)

$uR$ =

robot control input in robot frame

v_i =

robot linear velocity

$W$ =

world frame

x, y =

coordinates for planar position

α =

trajectory coefficients

$Γ¯$ =

dual planes that define chord line

θ =

coordinates for rotational position

κ =

chord index

$ρ(t)$ =

robot pose as function of time

$τ(t)$ =

trajectory vector

$ϕi$ =

vectors correlating path chord and obstacles

ω_j =

robot angular velocity