1.1 Motion planning and Mixed-Integer programming

In the literature, examples of motion planning are abundant, and regardless of the medium or vehicle used, they may be split into two separate layers: path planning and trajectory planning.

Path planning methods build routes without time parametrization, as in [15] or [16], which define crossover and mutation operators on genetic algorithms, or in [17], which uses a wavelet-based decomposition for easing the computational load of a multiresolution path planner.

Trajectory planning methods, on the other hand, search for routes that respect the movement constraints along time. For example, while in [18] a linear regression model predicts the evolution of human mobility in regions of a megalopolis, [19] uses statistical modeling to investigate the hierarchical structure of accident causes in autonomous vehicles and [20] proposes a proportional-integral-derivative (PID) controller for the real-time robotic stabilization of a robotic arm to act upon a dynamically moving human with a tumor.

However, it is possible to combine both path and trajectory planning by using Mixed-Integer Programming. The MIP approach is a general problem-solving framework that involves both discrete and continuous variables. In the case of agent movement, for example, we may use the former to model the activation of brake mechanisms while the latter calculates the speed and yaw angle in a curve. By constraining the domain of some variables to integers only, it becomes an approach much more general than a Linear Program (LP).

In particular, as in [21], for certain measures only Boolean variables are considered (thus assuming values 0 or 1). This procedure allows the system dynamics to be modeled by transforming propositional logic equations that derive from it into mixed-integer inequalities, which can be computed by the existent mixed programming solvers, such as CPLEX [22], Gurobi [23], Xpress [24], Xpress-MP [25] or ParaXpress [26].

Applications of MIP in the literature are plentiful, whether in terrestrial [1, 5–8, 18, 19], aquatic [27–29], aerial [2, 4, 9, 16, 30–32] or even spatial [33] environments. For instance, the authors propose in [14] an approximate model of aircraft dynamics using linear constraints and they apply a MIP approach to the trajectory planning of airplanes. The model ensures collision avoidance for each aircraft and guarantees the desired hard constraints fulfillment. Then, [34] applies a constraint tightening strategy to obtain a robust solution that guarantees finite-time arrival into an arbitrary target set. In spite of unknown disturbances, the central idea is to hold a “border” for feedback action as time goes by.

We assume in this manuscript that the computation time demanded in trajectory planning grows with the number of binary variables used for obstacle avoidance. This is not always valid, but can be used as a rule-of-thumb for improving computational performance. In [35], such number is used to express the complement of the polytope regions, which associate a unique number to each obstacle. As the sequence of the number powers in base 2 is super increasing, according to [36], any integer can be coded in log₂(N + 1) binary digits, which is the number of binary variables necessary to distinguish between N different regions.

In other words, [35] sets a global limit to the encoding itself. It is important to recall that the direct MIP solution for the trajectory optimization problem in an environment with obstacles is hard because in every timestep each obstacle face introduces non-convexity into the solution space [11–13]. As a consequence, for every obstacle with N_f faces, N_f non-convex constraints shall be added at each timestep to ensure that the desired trajectory remains outside the obstacle. As a result, to obtain further processing speedup, every technique obtained after [35] should involve a pre-processing step, performed separately with respect to the optimization.

For example, [37] proposes a strategy which requires a pre-planned path to define intermediate target sets, known as waypoints. Alternatively, [38] obtains large ellipsoidal regions of convex obstacle-free space in intricate environments with a greedy convex segmentation technique and [31] provides an entirely collision-free path with reduced number of integer variables. In turn, [39] divides large and complex environments into smaller segments through many pre-processing steps, [40] uses convex optimization to obtain target defect areas and [41] builds a convex lifting which partitions the space and descends to convex optimization.

However, we often demand a dynamically-built method in actively-changing scenarios, and this is a special contribution offered here. For instance, [42] develops a resembling approach with a three-stage algorithm: first it computes a collision-free path through the environment, next it generates convex polytopes that contain such route and then it poses a MIP to determine the dynamically feasible path. Yet, we know that peripheral, possibly distant, obstacles are not as significant as those circumvented along a path, which opens way to achieve performance gains in scenarios with many obstacles.

Additionally, it is worth noting that the approaches here presented do not count on pre-processed trajectory segments. But, as it will be clear, the results we achieved introduce a feasible and versatile way of solving the trajectory planning problem, more specifically through obstacle clustering.

2.1 Time minimization

It is possible to transform a variable horizon problem involving time minimization into a fixed horizon one with the use of integer variables. With the assumption of a polytopic set of terminal constraints, (4) in which and .

To perform the time minimization task, the terminal constraint in Eq (3d) can be rewritten by making use of auxiliary binary variables b[j] which are determined as b[j] = 1 if N[k] = j and b[j] = 0 otherwise. The following constraints impose, for some j such that b[j] = 1 and in interplay with the cost function to be redefined in Eq 7, that the agent must be within , where N_s is a fixed maximum horizon that must be larger than than the optimal N[k]: (5) with and corresponding respectively to i-th line of matrix D^m and to the i-th component of vector H^m.

To ensure that the binary variable b assumes the unitary value only once over the horizon N_s, the following constraint is defined: (6)

As a result, the variable-horizon cost function in Eq (2) can be rewritten by using a binary variable vector b to ensure the correctness in a fixed horizon approach: (7) where for j > N[k], i.e. the control is zero for all steps beyond the chosen maximal horizon . This is a consequence of the relaxation of Eqs (3a) and (3c) after N[k] in the minimization of Eq (2).

In Fig 4 there is a scheme that represents the relaxation of the time minimization binary variables in a trajectory planning problem. Note that N^⋆[k = 2] = N^⋆[k = 1] − 1, i.e. the optimal horizon at k = 2 is one unit smaller than the one at k = 1. The constraints in Eq (5) are relaxed for j > N^⋆[k = 1], once , and for j > N^⋆[k = 2], once . To accomplish it, the scalar M_t must be chosen such that , for all admissible x [21]. That is, M_t must be chosen large enough for every x reachable in steps, to serve as a barrier that allows numerical solvers to correctly modulate the domains of action of the continuous variables through the binary variables . Such variables act as sidings that separate the action domain of each continuous instance of the problem and allow the solver to perform global optimization calculations by evaluating multiple local domains at once. Note also that, for k = 2, the binary variable , which marks the arrival to in the last step of the simulation horizon, is clear, so that Eq (7) ≡ Eq (2) in this problem instance.

Download:

• PPT
  PowerPoint slide
• PNG
  larger image
• TIFF
  original image

Fig 4. Constraint relaxation scheme in a variable horizon problem.

For k = 1, the binary variable is set in the horizon N^⋆[k = 1], when . In k = 2, the agent repeats another instance of the same variable horizon problem, which now sets variable , as for N^⋆[k = 2].

https://doi.org/10.1371/journal.pone.0233441.g004

2.2 Obstacle avoidance

The optimization of trajectories in two-dimensional territories, clear of obstacles, presents an inherently convex search space, but the insertion of an obstacle into the region may render the problem not convex. Hence, we describe the commonly adopted remodeling that follows.

As a common requirement in an agent guidance problem, its formulation includes an obstacle avoidance task. Any polytopic obstacle O can be represented by: (8) with and .

We avoid collisions against obstacles by imposing that, at each time step, the position of the agent is outside of at least one face of each obstacle. This is done through the binary variable , which sets if the agent is outside face f of obstacle O at time step k + j.

As we need , we should have (9a) (9b) where and correspond both to the f-th line of matrix S^O and to the f-th element of vector C^O, respectively.

Obstacle avoidance in a trajectory planning problem can be attained by using the strategy of Alg 1. Initially we load the simulation parameters, and while the agent state x[k] does not attain , based on N_s and the positions of the obstacle set R_ob, we assemble the matrices A and B that contain the constraints of the problem, plan the trajectory by solving a traditional MIP problem, and finally evaluate and update the system state.

Alg 1. Closed-loop receding horizon maneuvering with Obstacle Avoidance.

Input:

Output: x[k + 1]

1: Load Simulation Parameters

2: while do

3:  [A, B]←Assemble Problem Matrices(R_ob, N_s)

4:  u[k]← Solve MIP Problem(A, B)

5:  x[k + 1]←A x[k]+ B u[k]

6:  k ← k + 1

7: end while

In Fig 5 there is a flowchart of the main variables produced throughout the different steps of the traditional obstacle avoidance algorithm. The regular data flow is represented by the loop over state x[k] until the next state .

Download:

• PPT
  PowerPoint slide
• PNG
  larger image
• TIFF
  original image

Fig 5. Data flowchart of the closed-loop receding horizon maneuvering.

The agent solves the optimization problem to avoid the obstacle set R_ob and applies the first element u[k] of the control sequence u[k + i], 0 ≤ i ≤ N_s − 1.

https://doi.org/10.1371/journal.pone.0233441.g005

However, as each obstacle partitions the search space into N_f disjoint regions, the overall performance in environments with tens of obstacles is severely deteriorated with regard to the initial obstacle-free case.

2.3 Inter-sample avoidance

In addition to guaranteeing obstacle avoidance in the sampled time steps, inter-sample avoidance is achieved by applying the avoidance constraints from step j + 1 at the preceding timestep j, as proposed by [43]. This imposes additional constraints to the problem, but employs no additional binary variables, as Eq (10) shows. (10)

2.4 Considering fuel expense

In order to cope with the one-norm of the control variables in Eq (7), which penalizes fuel expense, we add a set of auxiliary variables ϵ which are constrained [44]. As a result, in the optimization we obtain a compromise solution between fuel expense and time minimization. Then, we add the following constraints to the problem: (11a) (11b) (11c) (11d)

The previous cost function given by Eq (7), then, reaches its final form in Eq (12) (12) subject to the constraints in Eqs (3a), (3b), (5), (6), (9), (10) and (11), for 0 ≤ j ≤ N_s − 1.

3.1 Graph theory background

In this work, we assume that each obstacle is a static object with known coordinates. Let be the the smallest and largest coordinates of the projection of each obstacle of into the position space. Let also be a territory with target set in an environment around the agent that contains N_ob obstacles in a given clustering region, with a corresponding maximum clustering distance , chosen by the trajectory planner.

Then, can be mapped into an undirected graph , where the order of the graph is and |Ω| indicates the number of obstacle pairs that maintain a distance . As an example, two nodes and corresponding to obstacles O₁ and O₂ in with points p₁ ∈ O₁ and p₂ ∈ O₂ will be connected if (13) where the function r_d(⋅) returns the position of ⋅.

The fulfilling of Eq (13) for each pair of obstacles is recorded by the adjacency matrix , a square matrix formed by N_ob lines in which a_ij = 1 if and only if the vertices (obstacles, in this case) and are connected, and a_ij = 0, otherwise.

Definition 1. Let for the identity matrix I, where ∨ is the element-wise OR operation. Then, if we take for , where is the ‡-th power of matrix , each element if and only if there is a path between the obstacles corresponding to nodes and with length l ≤ r [45].

Definition 2. Let , in which , where N_d is the number of nodes in the graph. Then c_ij = 1, ∀i, j ≤ n if and only if there is a path from to in graph . is known as the connectivity matrix of .

If lines—or columns, as and are symmetrical for undirected graphs—i and j in are equal, then and will have a path to the same nodes. In this case, they will belong to the same connected component, or cluster, as we will name it from now on.

Remark 1. To verify if the obstacles corresponding to nodes and belong to the same cluster, it is enough to compare the i-th and j-th lines (or columns) in C. If they are equal, then the obstacles belong to the same cluster.

As an example, the terrain that contains a set of N_ob = 6 obstacles (Fig 6a), can be represented by the graph in Fig 6b, which is described both by the adjacency matrix and the connectivity matrix in Eq (14). (14)

Download:

• PPT
  PowerPoint slide
• PNG
  larger image
• TIFF
  original image

Fig 6. Example of the graph extracting from a spatially located set of obstacles.

(a): Spatial obstacle distribution. (b): Graph of the obstacle interconnection.

https://doi.org/10.1371/journal.pone.0233441.g006

The resulting clustering procedure is computationally efficient, since in order to obtain the connected components of it is enough to assemble , with a simple set of comparisons between the obstacle positions, and to obtain , with only the use of matrix multiplications.

Alg 2. Copertinence test of two obstacles in the same cluster.

Input:

Output: SameCluster

1: if then

2:  SameCluster ← True

3: else

4:  SameCluster ← False

5: end if

From Eq (13), the interobstacle distance is obtained as the Hausdorff distance between the sets defined by the points inside obstacles O_i and O_j.

Here, as R_ob contains the stacked coordinates in the x- and y-axis of the lower left-hand side and the upper right-hand side corners of every non-intersecting obstacle, to determine the interobstacle distances Δr_o between every two obstacles, it is enough to make a simple set of comparisons and operations with the position of the corners of every obstacle pair. It is worth noting that, as long as the environment does not change and is supposed to be static, Δr_o can be computed offline and used throughout the agent movement, once it depends only on the environment topology. It is also worth remarking that the clustering distance d_c is the generalization of for multiple obstacles, as next section will highlight.

It is possible to check the copertinence of two obstacles to the same cluster with Alg 2, and there is a representation of this clustering strategy in Alg 3. Here, Δr_ao is an array in which each element Δr_ao[i] is the value of the distance between the agent and obstacle O_i. The main idea is to obtain the matrix , matrix compounded by N_c lines and N_ob columns which indicates for the agent position r and the obstacle set position R_ob if two obstacles are neighbors in a given clustering region. The first for loop identifies the innermost clustering region each obstacle belongs to in order to set the respective clustering distances d_c[μ], for each μ-th obstacle. The next two nested for loops build the adjacency matrix and as there are no loops in , we must add to the identity matrix before raising it to (N_ob − 1) − th power, so as to propagate the links that pass through a node and obtain the maximum number of obstacles clustered together in the environment. Next we obtain the connectivity matrix and, as we are only interested in the identification of the connected components of obstacles, we finally set the connectivity of these as 1.

Alg 3. Obstacle clustering.

Input: r, R_ob, R_ic, R_sc, d_ic, d_sc, d_oc, N_ob

Output:

1: Δr_ao← get agent-obstacle distances (r, R_ob)

2: for all i ≤ N_ob do

3:  if Δr_ao[i]<R_ic then

4:   d_c[i]←d_ic

5:  else if Δr_ao[i]<R_sc then

6:   d_c[i]←d_sc

7:  else

8:   d_c[i]←d_oc

9:  end if

10: end for

11: Δr_o← get interobstacle distances (R_ob)

12: for all i ≤ N_ob do

13:  for all j ≤ N_ob do

14:   if Δr_o[i, j]<min(d_c[i], d_c[j]) then

15:    

16:    

17:   else

18:    

19:    

20:   end if

21:  end for

22: end for

23:

24: for all i ≤ N_ob do

25:  for all j ≤ N_ob do

26:   if then

27:    

28:   end if

29:  end for

30: end for

These tools allow us to build a clustering strategy that directly associates the reduction in the computational effort to the trajectory planning problem in the presence of obstacles, through a more refined clustering only for closer obstacles. This is the subject of the next subsection.

3.2 Dynamic obstacle clustering strategy

The approaches proposed by [30, 38] consider the interobstacle relative positions to find convex regions of obstacle free space, which is analogous to considering the interobstacle distances Δr_o to perform the clustering procedure, before optimization per se. In the trajectory planning problem, the computational speedup that these approaches obtain is a consequence of taking into account, in the avoidance modeling, the clusters themselves instead of the obstacles. As those potentially consist in a smaller number regarding these, less binary avoidance variables and constraints will possibly be used, fact which produces a problem that can be solved with less computational effort.

This work adds to this rationale the relative agent-obstacle position Δr_ao. The strategy we adopt segments this distance in regions around the agent, and flexibilizes the effective interobstacle clustering distance d_c in each of these regions. For obstacles in a region closer to the agent, a more subtle clustering with a smaller value of d_c is adopted. For more distant obstacles, a coarser clustering may be used with a greater value of d_c, as these obstacles are not in the immediate future of the agent. In this way, two obstacles can be clustered together while distant from the agent, but they would possibly separate into distinct clusters if the agent approaches them along its trajectory. Once less binary avoidance variables and constraints would be used compared to the single clustering distance case, a smaller total computational effort would be necessary, saving computational time to find the desired trajectory.

To make the exposition easier, here we will consider only polytopic (more specifically, rectangular) obstacles with sides parallel to the axes. Nonetheless, it must be remarked that the technique proposed in the present paper can be extended for any form of polygonal obstacles.

3.2.1 Close obstacles clustering.

This section describes the first clustering strategy this paper proposes. A typical scenario for trajectory planning is shown in Fig 7. Each clustering region is represented as a circle around the agent with the respective clustering radii R_ic, R_sc and R_oc. The clustering distances d_ic, d_sc and d_oc are depicted at the bottom of the figure.

Download:

• PPT
  PowerPoint slide
• PNG
  larger image
• TIFF
  original image

Fig 7. Typical scenario in a trajectory planning problem.

The agent is depicted as a blue dot, the obstacles in green and the target set R_f as a blue square.

https://doi.org/10.1371/journal.pone.0233441.g007

After obtaining the connectivity matrix , which describes the interconnections among close obstacles, the next step is to identify the coordinates of the clusters, as Alg 4 shows. This is done by boxing the convex hull of the connected components of obstacles in the environment, according to the value of d_c chosen in the clustering algorithm. After that, it is just a matter of solving the optimization problem by considering cluster avoidance, with coordinates R_κ, instead of obstacle avoidance, with coordinates R_ob, with the constraints proposed in the previous section.

Alg 4. Cluster coordinates extraction.

Input:

Output: R_κ

1: c ← 1

2: o ← 1

3:

4: while c ≤ N_c do

5:  while o ≤ N_ob do

6:   if then

7:    CH ← convex hull(c, o)

8:    R_κ ← bounding box(CH)

9:   end if

10:   o ← o + 1

11:  end while

12:  c ← c + 1

13: end while

The scheme in Fig 8 represents this strategy with three different agent positions. Initially, the agent estimates a path that goes down the largest cluster, to the east (Fig 8a). However, as the agent starts moving, some obstacles of this cluster enter into the surroundings-zone (Fig 8b) and the algorithm splits it up in two smaller clusters, which frees a corridor for the agent to reach the target (Fig 8c).

Download:

• PPT
  PowerPoint slide
• PNG
  larger image
• TIFF
  original image

Fig 8. Scheme of the close obstacles clustering strategy.

(a): Obstacles are grouped in 7 clusters. (b): Obstacles are grouped in 9 clusters. (c): Obstacles are grouped in 8 clusters.

https://doi.org/10.1371/journal.pone.0233441.g008

We depict the algorithm that represents this strategy in Alg 5. Basically, to the usual problem of trajectory planning, with instructions shown in black, we include an obstacle clustering phase, shown in blue. Initially, we load the simulation parameters, i.e. obstacle positions, clustering distances, and so on, and obtain the Adjacency and Connectivity matrices and . In possession of , it is straightforward to obtain the cluster configuration R_κ and perform a traditional trajectory planning to reach the target region .

Alg 5. Closed-loop receding horizon maneuvering with Close Obstacles Clustering Algorithm.

Input: x[k], R_ob, R_f

Output: x[k + 1]

1: Load Simulation Parameters

2: while do

3:  R_k ← Cluster Obstacles in Neighborhood(r[k], R_ob, d_c, R_ic, R_sc, R_oc)

4:  [A, B]←Assemble Problem Matrices(R_k, N_s)

5:  u[k]← Solve MIP Problem(A, B)

6:  x[k + 1]←A x[k]+ B u[k]

7:  k ← k + 1

8: end while

In Fig 9 there is a flowchart of the main variable data types produced throughout the different steps of the Close Obstacles Clustering algorithm. To the regular flow of an obstacle avoidance algorithm, represented by the feedback on state x[k], we prepend the Clustering step, represented by the green box, which produces the coordinates of the cluster configuration R_κ from the obstacle positions R_ob. Notice that the complete underlying clustering logic described along this section in Alg 2, 3, 4 and 5 is here implicit, and that in Fig 9 both the iterations of the upper minor Clustering loop as the lower major update loop repeat until .

Download:

• PPT
  PowerPoint slide
• PNG
  larger image
• TIFF
  original image

Fig 9. Data flowchart of the closed-loop receding horizon maneuvering with close obstacles clustering algorithm.

https://doi.org/10.1371/journal.pone.0233441.g009

This clustering component deals only with the current positions of the agent and the obstacles, i.e. it affects the present situation of the system.

4.1 Bygone obstacles rebuttal strategy

After optimization, a trajectory between the initial position r[k] and the target set is found. If all the predicted positions of the agent are closer to than a given obstacle whose projection into the position space is given by O_b, then such obstacle may be replaced by a single constraint independent of the obstacle avoidance binary variables, which may then be removed from the optimization problem from k + 1 onwards. In such a case, we first determine a straight-line perpendicular to the segment connecting point p in O_b and point that produces the minimum distance between O_b and , and then we translate this straight-line to pass at the point p. The half-plane defined by this line that contains the current position is guaranteed to contain all predicted positions by construction and defines the hard constraint that can replace the bygone obstacle in the subsequent trajectory, and ensures that the agent does not collide with it.

Therefore, we can replace the obstacle of projection O_b itself (and its binary avoidance variables ) by hard constraints (15) that ensure the agent will be confined to a region away from the obstacle. Here, contains the coefficients of the straight-line perpendicular to and contains the constant terms that ensure that the perpendicular line passes at the point p that is closest to .

The illustrative example in Fig 10 represents the inherent concept to this strategy. Each figure depicts in continuous lines concomitantly both the current and the succeeding positions of the agent and the scheme exhibits the situation after all the movement updates in each step.

Download:

• PPT
  PowerPoint slide
• PNG
  larger image
• TIFF
  original image

Fig 10. Bygone obstacle rebuttal concept.

(a): All the predicted positions of the trajectory lie closer to than the obstacle itself. (b): The straight-line perpendicular to the line that produces the minimum distance between R_f and O_b is represented in black and the hard constraint that replaces the bygone obstacle in the subsequent trajectory is depicted as a red arrow.

https://doi.org/10.1371/journal.pone.0233441.g010

In Fig 11 there is a mixed representation of this approach with the Close obstacles Clustering strategy in three different agent positions, as the agent approaches the target. For clarity purposes, we omit both the clustering regions and the clustering distances.

Download:

• PPT
  PowerPoint slide
• PNG
  larger image
• TIFF
  original image

Fig 11. Scheme of the bygone obstacles rebuttal strategy.

The projection of each bygone obstacle into the position space is represented in gray and its corresponding active hard constraint is drawn as a red arrow. In each figure, there are respectively: (a): 3 bygone obstacles. (b): 6 bygone obstacles. (c): 16 bygone obstacles.

https://doi.org/10.1371/journal.pone.0233441.g011

It is important to note that this approach only considers an obstacle as bygone if the predicted agent-target distances are smaller than the obstacle-target ones in all future steps. This policy works then by reducing the search space through the gradual removal of obstacles at the edges of the environment.

Alg 6. Bygone Obstacles Rebuttal.

Input:

Output: R_ob

1: for each o ≤ N_ob do

2:  O_b ← Get Coordinates(R_ob, o)

3:  if then

4:   

5:   

6:   coefficients ← Translate This Line to Pass at p

7:   Add Hard Constraint(coefficients)

8:   R_ob ← Remove (R_ob, o)

9:   N_ob ← N_ob − 1

10:   o ← o − 1

11:  end if

12: end for

We represent the algorithm that underlies this strategy in Alg 6, where returns the euclidean distance between r[k] and . In case of an obstacle rebuttal, the coordinates of its projection are removed from R_ob and both N_ob and o must be decremented, so that the number of obstacles and the subsequent stacked obstacle are correctly evaluated in the next iteration. The inclusion of this algorithm in the planning process is highlighted in blue in Alg 7. Through the assurance that the agent has moved away from the obstacle and will do so in all the predicted trajectory, it is possible to rebut bygone obstacles while ensuring the existence of a feasible solution to the optimization problem at the next sample times.

Alg 7. Closed-loop receding horizon maneuvering with Bygone Obstacles Rebuttal Strategy.

Input: x[k], R_ob, R_f

Output: x[k + 1]

1: Load Simulation Parameters

2: while do

3:  R_ob ← Rebut Bygone Obstacles(R_ob, R_f, k)

4:  R_k ← Cluster Obstacles in Neighborhood(r[k], R_ob, d_c, R_ic, R_sc, R_oc)

5:  [A, B]←Assemble Problem Matrices(R_k, N_s)

6:  u[k]← Solve MIP Problem(A, B)

7:  x[k + 1]←A x[k]+ B u[k]

8:  k ← k + 1

9: end while

The flowchart in Fig 12 represents the data produced throughout the main steps of the Bygone obstacles rebuttal algorithm. To the previous Clustering algorithm of Fig 9 we prepend a bygone obstacle rebuttal step, represented in blue, which removes the bygone obstacles and adds the respective hard constraints to the problem to be solved thenceforth.

Download:

• PPT
  PowerPoint slide
• PNG
  larger image
• TIFF
  original image

Fig 12. Data flowchart of the closed-loop receding horizon maneuvering with bygone obstacles rebuttal algorithm.

https://doi.org/10.1371/journal.pone.0233441.g012

This strategy replaces the initial N_f+ 1 mixed-integer avoidance constraints of each bygone obstacle with only one hard constraint, and allows for removal N_s N_f binary variables for each bygone obstacle that is replaced by a simple inequality.

4.2 Exterior obstacles contempt strategy

Once the agent obtains a path to the target , if all the predicted positions of the agent lay on the same side of some obstacle whose projection into the position space is given by O_e, then such obstacle may be replaced by the hard constraint corresponding to this very side. The example in Fig 13 illustrates this idea.

Download:

• PPT
  PowerPoint slide
• PNG
  larger image
• TIFF
  original image

Fig 13. Exterior obstacle contempt concept.

(a): After the first step, all predicted positions represented in gray lie above the upper face of the obstacle. (b): It is possible to replace this exterior obstacle in the subsequent trajectory by the hard constraint which corresponds to the red arrow that is drawn from the side of O_e that is collateral to the entire trajectory.

https://doi.org/10.1371/journal.pone.0233441.g013

It is worth emphasizing that, while in Eq (9) f ranges from 1 to N_f, in the case of the exterior obstacle of Eq (16), assumes a single value, which is that of the constraint that is always obeyed along the horizon. (16)

A simple check upon the correspondence of the values that the binary avoidance variables assume for a given obstacle side along the complete prediction horizon would represent a sufficient condition for identifying an obstacle as exterior. However, there are cases in which an obstacle can be exterior and not have all binary variables fixed True to a certain side throughout the whole prediction horizon, due to the redundancy resulting from the overlap of valid regions in the presence of a polygonal obstacle.

For example, at k = k₀+ 1 in Fig 13a, if the solver sets the variable of the valid region to the left-hand side of the obstacle instead of the one above it, the obstacle would not be identified as exterior at k = k₀+ 2 in Fig 13b. In such a case, there are obstacles that could be removed at a certain instant, but would not. In fact, we do not rely only on the binaries to verify the obstacle contempt condition, but in fact we calculate whether the condition of the constraint is respected for each cluster by the predicted positions of the agent at every instant. The choice of which constraint to adopt in the case of the overlap of more than one valid region becomes then a design decision. Thus, as the agent circumvents a cluster and proceeds in its path to the target, the obstacles inside it become exterior and a single hard constraint is added replacing the whole cluster.

In Fig 14 there is a mixed representation of this approach with the Close Obstacles Clustering strategy in three different agent positions.

Download:

• PPT
  PowerPoint slide
• PNG
  larger image
• TIFF
  original image

Fig 14. Scheme of the exterior obstacles clustering strategy.

The exterior obstacles are drawn in yellow and the respective hard constraints are represented by red arrows. (a): 7 clusters are identified, 2 of which contain 3 obstacles that become exterior. (b): 3 clusters which contain the 8 remaining obstacles are identified as exterior. (c): Simulation ends with the 16 obstacles replaced by 10 hard constraints.

https://doi.org/10.1371/journal.pone.0233441.g014

Alg 8. Exterior Obstacles Identification.

Input: U, R_κ, N_c, N_s

Output: ext, current

1: current[1: N_s]←Get Trajectory Prediction(U)

2: max_r_x ← max(Load r_x Coordinates(current))

3: max_r_y ← max(Load r_y Coordinates(current))

4: min_r_x ← min(Load r_x Coordinates(current))

5: min_r_y ← min(Load r_y Coordinates(current))

6: c ← 1

7: while c ≤ N_c do

8:  isLefthand, isBelow, isRighthand, isAbove ← False

9:  if max_r_x < Get Left − hand Side Coordinate(R_κ, c) then

10:   isLefthandSide ← True

11:  else if max_r_y < Get Lower Coordinate(R_κ, c) then

12:   isBelow ← True

13:  else if −min_r_x < −Get Right − handSide Coordinate(R_κ, c) then

14:   isRighthandSide ← True

15:  else if −min_r_y < −Get Upper Coordinate(R_κ, c) then

16:   isAbove ← True

17:  end if

18:  ext[c]←isLefthandSide ∨ isBelow ∨ isRighthandSide ∨ isAbove

19:  c ← c + 1

20: end while

Alg 9. Exterior Obstacles Contempt.

Input: R_ob, N_ob, R_κ, N_c, ext, current

Output: R_ob, N_ob

1: c ← 1

2: while c ≤ N_c do

3:  if ext[c] then

4:   coefficients ← Get Active Exterior Constraint(c, R_κ, current)

5:   Add Hard Constraint(coefficients)

6:   o ← 1

7:   while o ≤ N_ob do

8:    if Is Obstacle Inside Cluster(o, c, R_ob, R_c) then

9:     R_ob ← Remove (R_ob, o)

10:     N_ob ← N_ob − 1

11:     o ← o − 1

12:    end if

13:    o ← o + 1

14:   end while

15:  end if

16:  c ← c + 1

17: end while

The pseudocode that identifies exterior obstacles is represented in Alg 8. It consists basically in the comparison of the sides of each cluster, that were previously identified in the clustering step, with the most extreme predicted trajectory coordinates, either at the left-hand side, below, at the right-hand side or above, where ∨ is the boolean OR operator. Here, ext is an array of boolean variables, in which ext[c] = True if cluster c is exterior. Then, to replace the exterior obstacle it is enough to add the hard constraint that corresponds to the new exterior cluster in the planning problem to be solved thenceforth, with the proper contempt of the obstacles that belong to this cluster from the obstacle set, as Alg 9 shows. Here, function Get Active Exterior Constraint() returns the coefficients of the exterior constraint that replaces the exterior obstacle.

Alg 10. Closed-loop receding horizon maneuvering with Exterior Obstacles Contempt Algorithm.

Input: x[k], R_ob, R_f

Output: x[k + 1]

1: Load Simulation Parameters

2: while do

3:  R_ob ← Rebut Bygone Obstacles(R_ob, R_f, k)

4:  R_k ← Cluster Obstacles in Neighborhood(r[k], R_ob, d_c, R_ic, R_sc, R_oc)

5:  [A, B]←Assemble Problem Matrices(R_k, N_s)

6:  u[k]← Solve MIP Problem(A, B)

7:  R_ob ← Contempt Exterior Obstacles(R_ob, u[k], …, u[k + N_s − 1])

8:  x[k + 1]←A x[k]+ B u[k]

9:  k ← k + 1

10: end while

We represent the pseudocode of this strategy in Alg 10, with the contempt of exterior obstacles highlighted in blue, while the flowchart in Fig 15 represents the data produced throughout the main steps of the Exterior obstacles contempt algorithm.

Download:

• PPT
  PowerPoint slide
• PNG
  larger image
• TIFF
  original image

Fig 15. Data flowchart of the closed-loop receding horizon maneuvering with exterior obstacles contempt algorithm.

To the previous Bygone obstacles rebuttal algorithm of Fig 12, we append an exterior obstacle contempt step, represented in blue, which replaces the exterior obstacles by the respective hard constraints to the problem to be solved from then on.

https://doi.org/10.1371/journal.pone.0233441.g015

Once a cluster is identified as exterior, this strategy replaces the initial N_f+ 1 mixed-integer avoidance constraints of each exterior cluster with only one hard constraint for the whole cluster, and allows for removal N_s N_f binary variables for each cluster that is replaced by a simple inequality.

4.3 Iterative clustering distance tuning

To decrease the number of binary variables in the optimization problem, the Close obstacles clustering strategy may entail the elimination of spaces initially available for navigation. This, in turn, might lead to an infeasible optimization problem, even when in the absence of clustering a feasible trajectory could be determined. In this section we will describe a scheme that circumvents this problem and allows to obtain a feasible trajectory to the final region, when one exists.

To the previous cluster avoidance algorithm in Alg 10 we append an iterative deepening search in the clustering avoidance subproblems, with a non-increasing clustering distance highlighted in blue. In Alg 11, if we initialize the clustering distance d_c with a value large enough, the problem could be infeasible. As a result, the if condition of line 7 would hold value True, d_c would be reduced according to the expression in Eq (17) and the continue statement of line 9 would break the loop execution. As the clustering distance is held as a global variable, in the next loop iteration, the algorithm would make its prediction with the updated value, and the execution would continue until the algorithm was able to find a feasible path to the target set. On the other hand, if the value of d_c did not produce initially an infeasible problem, then a trajectory to the final region would have already been found. (17)

Here, 0 < s_r < 1 is taken as a shrinking rate for d_c. Small values of s_r imply a high cutback on d_c and a smaller number of clustering attempts until the strategy finds a viable path to , potentially in a configuration with many clusters in the environment. On the other hand, high values of s_r entail a gradual reduction on d_c and allow the attainment of a cluster configuration that is closer to the minimal number of clusters, possibly at the expense of a greater number of infeasible solution trials.

Alg 11. Closed-loop receding horizon maneuvering with the Iterative Clustering Distance Tuning Algorithm.

Input: x[k], R_ob, R_f

Output: x[k + 1]

1: Load Simulation Parameters

2: while do

3:  R_ob ← Rebut Bygone Obstacles(R_ob, R_f, k)

4:  R_k ← Cluster Obstacles in Neighborhood(r[k], R_ob, d_c, R_ic, R_sc, R_oc)

5:  [A, B]←Assemble Problem Matrices(R_k, N_s)

6:  u[k]← Solve MIP Problem(A, B)

7:  if Is Problem Infeasible(u[k]) then

8:   d_c ← Shrink Clustering Distance(d_c)

9:   continue

10:  end if

11:  R_ob ← Contempt Exterior Obstacles(R_ob, u[k], …, u[k + N_s − 1])

12:  x[k + 1]←A x[k]+ B u[k]

13:  k ← k + 1

14: end while

The flowchart in Fig 16 represents the data produced throughout the main steps of the Iterative clustering algorithm. To the previous Exterior clustering flowchart of Fig 15 we add the continue statement, represented in blue, which decreases the clustering distance in the avoidance subproblem to be solved thenceforth.

Download:

• PPT
  PowerPoint slide
• PNG
  larger image
• TIFF
  original image

Fig 16. Data flowchart of the closed-loop receding horizon maneuvering with iterative clustering distance tuning algorithm.

https://doi.org/10.1371/journal.pone.0233441.g016

5.1 Random obstacles generation

Consider the uniformly distributed variables b_x, b_y, h_x, h_y, where b_x and b_y are both limited between 0 and 1 and represent relative coordinates of the obstacle center, h_x and h_y are both limited between 0.04 and 1.25 and represent respectively the width and height of the obstacle. Consider also L = [L_x L_y]^T, in which L_x and L_y are both 20 and correspond to the dimensions of the environment.

Let f_0x, f_0y, f_1x, f_1y, B_x, B_y, m_x and m_y be 8 real numbers, where f_0x = f_0y = 0 and f_1x = f_1y = 20 describe respectively the smallest and largest coordinate values in the x and y axes within the region that contains them in the environment, B_x and B_y are both limited between 0 and 20 and identify the coordinates of the center of an obstacle and m_x and m_y account for the border around the environment which defines the territory that contains the random obstacle set, where m_x = m_y = 0.

Then, an obstacle in the environment with sides parallel to the axes and center in [B_x B_y]^T can be randomly generated according to the expression in Eq (22), (22) where B = (f₀ − m)+ (f₁ − f₀+ 2m)b, in which B ∈ {B_x, B_y}, b ∈ {b_x, b_y}, m ∈ {m_x, m_y}, f₀ ∈ {f_0x, f_0y} and f₁ ∈ {f_1x, f_1y}.

For the generation of uniformly distributed pseudorandom numbers, we used the rand function in Matlab environment.

6.1 Unclustered scenario

For comparison purposes, we obtained the average time to calculate the complete trajectory that results from the obstacle avoidance procedure in an unclustered scenario, as [34] proposes. For the Unclustered, Clustering, Bygone, Exterior and Iterative strategies in maps of Fig 17a and 17b we obtained each of these times as an average of 30 simulations. The resulting path for the Unclustered strategy is represented in Fig 18a, with a zoom of the curve drawn among the obstacles in Fig 18b.

Download:

• PPT
  PowerPoint slide
• PNG
  larger image
• TIFF
  original image

Fig 18. Trajectory of the unclustered strategy.

(a): Complete trajectory. (b): Central Obstacles Zoom.

https://doi.org/10.1371/journal.pone.0233441.g018

The average simulation times along each iteration of Alg 1 are monotonically decreasing, as Fig 19 shows with the respective standard deviation. For the unclustered scenario, the average total simulation time, i.e. the average of the summation of all optimization times from k = 0, 1, …, N[k] until reaching the target set at N[k], is t_u ≈ 61.65 s.

Download:

• PPT
  PowerPoint slide
• PNG
  larger image
• TIFF
  original image

Fig 19. Average elapsed time and standard deviation of the simulations in the unclustered scenario.

https://doi.org/10.1371/journal.pone.0233441.g019

6.2 Close obstacles clustering strategy

Algorithm 5 was applied for the results in this subsection. In Fig 20, we can observe the evolution of the clustering strategy as the agent moves towards the goal area. Each figure represents in continuous lines concomitantly both the current and the succeeding positions of the agent and each scheme exhibits the situation after all the movement updates in each step.

Download:

• PPT
  PowerPoint slide
• PNG
  larger image
• TIFF
  original image

Fig 20. Trajectory of the close obstacles clustering strategy in the map of Fig 17a.

(a) Step 1. (b) Step 4. (c) Step 7. (d) Step 8. (e) Step 9. (f) Step 11. (g) Step 12. (h) Step 14.

https://doi.org/10.1371/journal.pone.0233441.g020

Algorithm 3 initially groups the obstacles into 12 clusters, which are depicted in Fig 20a. After the turn to the right-hand side in Fig 20c, the north cluster enters the inner-zone and is split up into 2 clusters (Fig 20d). Once there are still some obstacles of the cluster to the south of the agent in the inner-zone, the algorithm considers d_ic as the active clustering distance and does not merge the clusters to the south and to the southwest of the territory, totaling 13 clusters in step 8.

In Fig 20e, the clusters to the south and to the southwest of the agent are lumped together, and the same also happens to the clusters to the north and to the northwest of the territory in the following steps (Fig 20f and 20g). Once there is no significant change in the environment, the initially planned trajectory is strictly followed, with , where is the predicted solution cost for time k + 1 given k, J^⋆[k + 1] is the optimal cost at time k + 1 and is the ℓ₁-norm of the control effort applied at time step k.

In this approach, the obstacle set is used only as input of the clustering procedure, without any update to its components. This means that the computational load remains approximately fixed throughout the maneuver, except for the natural clustering variations due to the change in the relative position between the agent and the obstacles. This is evident in the representation of the number of obstacles N_ob at the right of every plot in Fig 20a–20h.

With the Clustering strategy, Fig 21a shows the average time spent on each algorithm iteration. The clustering procedure has as main consequence, regarding the computational times along each algorithm iteration, an expressive cutback when compared to the ones of the Unclustered case in Fig 19. By clustering two close obstacles, we remove the need for additional branches made by the branch-and-bound algorithm along the path of an agent that circumvents these obstacles.

Download:

• PPT
  PowerPoint slide
• PNG
  larger image
• TIFF
  original image

Fig 21. Average simulation times with standard deviation.

(a): Clustering strategy in the map of Fig 17a. (b): Bygone obstacles clustering strategy in the map of Fig 17a. (c): Exterior obstacles clustering strategy in the map of Fig 17a. (d): Iterative clustering strategy in the map of Fig 17a. (e): Unclustered strategy in the map of Fig 17b. (f): Iterative clustering strategy in the map of Fig 17b.

https://doi.org/10.1371/journal.pone.0233441.g021

The average clustering time along the whole trajectory of the agent is . The average total simulation time, i.e. the average of the summation of the times of all optimizations realized until the target set was reached is t_c ≈ 3.79 s. The Clustering strategy has an average speedup of 16.26 regarding the Unclustered strategy.

6.3 Bygone obstacles rebuttal strategy

In Fig 22 we have the clustering strategy with the rebuttal of bygone obstacles as the agent moves towards the goal area, i.e. running Alg 7.

Download:

• PPT
  PowerPoint slide
• PNG
  larger image
• TIFF
  original image

Fig 22. Trajectory of the bygone obstacles clustering strategy in the map of Fig 17a.

(a) Step 1. (b) Step 3. (c) Step 5. (d) Step 7. (e) Step 8. (f) Step 9. (g) Step 11. (h) Step 14.

https://doi.org/10.1371/journal.pone.0233441.g022

In the beginning, the trajectory of Fig 22a is the same as the one of Fig 20a, once this strategy only removes obstacles that are already distancing from the agent and will do so thenceforth. This approach gradually replaces these obstacles by simple convex constraints once the agent is predicted to always move away from them, as the drop in N_ob to the right-hand side of Fig 22b–22h shows, with respectively 43, 45 and 50 bygone obstacles, represented in grey in each figure. The evolution of the classification of N_ob can be seen in Fig 23a.

Download:

• PPT
  PowerPoint slide
• PNG
  larger image
• TIFF
  original image

Fig 23. Obstacle classification for the bygone, exterior and iterative obstacle clustering strategies.

(a) Bygone Obstacles Rebuttal strategy. (b) Exterior Obstacles Contempt strategy. (c) Iterative Clustering Distance Tuning strategy (d) Iterative Clustering Distance Tuning strategy in the random obstacles scenario.

https://doi.org/10.1371/journal.pone.0233441.g023

The decrease of the computational load taken as input of the clustering procedure can be seen both in the representation of N_ob, at the right-hand side of every plot along Fig 22a–22h, as in the obstacle classification in Fig 23a for all the simulation steps.

The average time spent on each algorithm iteration is shown in Fig 21b. Along all steps of the trajectory, the average time necessary to identify the bygone obstacles is and the average clustering time is . With the Bygone obstacles rebuttal strategy, the average total simulation time, i.e. the average of the summation of all the optimization times until the target set was reached is t_b ≈ 3.14 s, with an average nominal speedup of S_b ≈ 19.64 with regard to the Unclustered strategy.

6.4 Exterior obstacles contempt strategy

In Fig 24a, the Exterior obstacles clustering strategy (Alg 10) initially identifies exterior obstacles. We represent them in green-yellowish color in the timestep the algorithm identifies them and in yellow color in the subsequent steps.

Download:

• PPT
  PowerPoint slide
• PNG
  larger image
• TIFF
  original image

Fig 24. Trajectory of the exterior obstacles contempt strategy in the map of Fig 17a.

(a) Step 1. (b) Step 2. (c) Step 4. (d) Step 5. (e) Step 7. (f) Step 8. (g) Step 9. (h) Step 14.

https://doi.org/10.1371/journal.pone.0233441.g024

One can see that the exterior obstacle contempt strategy complements the effects of the bygone obstacle rebuttal one. The obstacles that belong to exterior clusters are promptly removed after the first step, and while the agent circumvents both the central and northwestern clusters, some obstacles of these clusters become bygone obstacles before they are treated as exterior ones. See the obstacles in the lower-half of the central cluster and to the left-hand side of the north-west cluster in Fig 24f–24g. As the agent surpasses both clusters, the predicted (future) trajectory stands at the same side of them and turns every remaining obstacle in both clusters into exterior ones. This causes the drop of N_ob observed in k = 9 from Fig 24g on. The maneuver ends with exterior obstacles and bygone obstacles in Fig 24h, and the evolution of the classification of N_ob can be seen in Fig 23b.

Along all steps of the trajectory, the average time required to identify the exterior obstacles is , the average identification time of bygone obstacles is and the average clustering time is . With the exterior obstacles clustering strategy, the average total simulation time, i.e. the average of the summation of all the optimization times until the target set was reached is t_e ≈ 1.61 s, with an average nominal speedup of S_e ≈ 38.33 with respect to the Unclustered strategy.

6.5 Iterative clustering distance tuning

The results for the Iterative Clustering Distance Tuning Strategy in Alg 11 can be seen in Fig 25. The agent initial state is set as x₀ = [0.5 0 3.5 0]^T (i.e. the agent is brought closer to the obstacle set) and the first guess for the clustering distance is increased to , to ensure the agent is initially surrounded by a cluster. The shrinking rate is taken as s_r = 0.75.

Download:

• PPT
  PowerPoint slide
• PNG
  larger image
• TIFF
  original image

Fig 25. Trajectory of the iterative clustering distance tuning strategy in the map of Fig 17a.

(a) Step 1. (b) Step 2. (c) Step 3. (d) Step 4. (e) Step 5. (f) Step 6. (g) Step 7. (h) Step 14.

https://doi.org/10.1371/journal.pone.0233441.g025

Initially, the algorithm groups the obstacles into a cluster configuration that covers the entire territory, which is not shown in Fig 25. Since no movement is possible, as a single cluster surrounds the agent and prevents it from choosing a viable action, the algorithm shrinks the clustering parameters. However, a second infeasible problem arises, which demands another reclustering procedure.

The result is the maneuver of Fig 25a, with the agent moving south to leave the avoidance area of the largest cluster. As the agent begins to circumvent it, after Fig 25b, its central-south obstacles enter into the surrounings-zone. The agent makes a less sharp curve with the new open area between the clusters and the trajectory ends with and .

The average times needed along the first and the second reclustering steps, which comprise the time CPLEX needed to conclude that the problem is infeasible, are . Along all steps of the trajectory, the average time required to identify exterior obstacles is , the average identification time of bygone obstacles is and the average clustering time is .

With the Iterative clustering distance tuning strategy, the average total simulation time, i.e. the average of the summation of all optimization times until the target set was reached is t_i ≈ 1.03 s, with an average speedup of S_i ≈ 59.53 with regard to the results of the Unclustered strategy.

6.6 Iterative clustering distance tuning in the random obstacle scenario

We used the Close, Bygone, Exterior and the Iterative Clustering strategies in another environment, with obstacles randomly generated according to Eq (22) for N_s = 18 steps, [f₀, f₁] = [0, 20] and m = 0. Now there are 50 small uniformly-distributed obstacles in the environment and Fig 26 contains the results. The shrinking rate is kept as s_r = 0.75.

Download:

• PPT
  PowerPoint slide
• PNG
  larger image
• TIFF
  original image

Fig 26. Trajectory of the iterative clustering distance tuning strategy in the map of Fig 17b.

(a) Step 1. (b) Step 2. (c) Step 4. (d) Step 5. (e) Step 6. (f) Step 7. (g) Step 8. (h) Step 15.

https://doi.org/10.1371/journal.pone.0233441.g026

The algorithm adjusts the initial clustering distance 4 times, what takes respectively and in each reclustering operation, which includes the time CPLEX needed to deduce that the problem is infeasible, until the 50 obstacles are arranged in Fig 26a into 7 clusters for d_c ≈ [0.95 1.90 2.85]^T.

With the Iterative clustering distance tuning strategy, the average total simulation time, i.e. the average of the summation of all simulation times until the agent reaches the target set is t_r ≈ 2.63 s, with an average speedup of S_r ≈ 13.43 in relation to the results of the Unclustered strategy in the random scenario. A comparison of the trajectory that these strategies obtained can be seen in Fig 27 and is discussed in detail in the Discussion section.

Download:

• PPT
  PowerPoint slide
• PNG
  larger image
• TIFF
  original image

Fig 27. Trajectory comparison in the map of Fig 17a for the unclustered, clustering, bygone, exterior and iterative strategies.

The proximity in respect to the euclidean distance with the Unclustered path is given in decreasing order by the Clustering, Bygone, Exterior and Iterative strategies.

https://doi.org/10.1371/journal.pone.0233441.g027

7.1 Obstacle classification

The obstacle composition for the cases studied in this work can be seen in Fig 23. The main tendency we can observe is that the more elaborate the strategy, the faster the substitution of binary variables of obstacle avoidance. While the Bygone obstacle clustering strategy in Fig 23a replaces all binary avoidance constraints in k = 14, the addition of the Exterior clustering strategy achieves the same in k = 10 and the adoption of the Iterative tuning heuristics accomplishes it in k = 7. These strategies cooperate in the substitution of binary avoidance variables, and the preponderance of any of them over the others is intrinsically related both to the clustering distances adopted and the obstacle topology in the environment, as well as the order in which they are carried out. However, one practical limitation of these algorithms is the need for full knowledge of the obstacle environment topology upon which they will operate.

7.2 Evolution of the cost function and its prediction

In Fig 28 there is a representation of the cost function evolution, in continuous lines, and its prediction, in dashed lines, along all the simulations presented here.

Download:

• PPT
  PowerPoint slide
• PNG
  larger image
• TIFF
  original image

Fig 28. Evolution of the cost function and its prediction for the clustering, bygone, exterior and iterative strategies.

(a): Clustering strategy in the map of Fig 17a. (b): Bygone obstacles clustering strategy in the map of Fig 17a. (c): Exterior obstacles clustering strategy in the map of Fig 17a. (d): Iterative clustering strategy in the map of Fig 17a. (e): Unclustered strategy in the map of Fig 17b. (f): Iterative clustering strategy in the map of Fig 17b.

https://doi.org/10.1371/journal.pone.0233441.g028

Along all the steps of the Close, Bygone, Exterior and Unclustered strategies, the cost function J^⋆[k] corresponds exactly to the prediction made in the previous step, as the dashed horizontal extensions in the value of show. However, in the Iterative strategy, both for k = 3 in Fig 28d and for k = 6 in Fig 28f, when an obstacle occupies an inner clustering region, the clustering algorithm frees previously clustered space that becomes available for the agent movement, which makes the value of the cost function be less than the prediction previously made.

Such fact was observed here only in the Iterative clustering strategy, but it is worth noting that it is a consequence of the distribution of the d_c values in the clustering regions around the agent. With a fine parameter tuning, the Close strategy itself would identify the same phenomenon.

7.3 Trajectory comparison

In Fig 27 we can compare the trajectories that each strategy obtains in the map of Fig 17a. While the Unclustered strategy optimizes a trajectory that overcomes the obstacles that compose the central cluster through an internal path, the beginning of the Clustering, Clustering + Bygone and Clustering + Bygone + Exterior strategies is exactly the same, moving north to circumvent it.

After surpassing the central cluster at time t = k₀+ 8, the trajectories slightly separate themselves until they rejoin at the lower left corner of , in t = k₀+ 14. In this case, the Exterior strategy takes a path that prioritizes the vertical displacement to the detriment of the horizontal one, whereas the Bygone and the Clustering strategies take paths more similar to a straight line toward the target.

The Iterative clustering strategy in black, on the other hand, chooses the first available path freed in the iterative deepening search among the obstacles. Thereby, the trajectory of the agent overcomes the central cluster underneath.

In Fig 27, the comparison of the paths the Clustering, Bygone, Exterior and Iterative strategies obtained against the one optimized by the Unclustered strategy alone shows that the more comprehensive the changes in the original planning problem, the more the final trajectory potentially diverges from the optimal path. In such case, there is an increment of approximately 2.8%, 2.8%, 2.8% and 21.2% in the the value of the cost function with the adoption of these strategies.

In Fig 29 we have a comparison of the trajectories that the Iterative and the Unclustered strategies optimize in the random scenario of Fig 17b. The Iterative strategy obtains the nozzle-shaped trajectory shown in black, which shows that the path produced can substantially differ from the optimal one, as a consequence of the different constraints that make up the problem. Here, there is an increment of approximately 30.7% in the value of the cost function with the adoption of the clustering strategies.

Download:

• PPT
  PowerPoint slide
• PNG
  larger image
• TIFF
  original image

Fig 29. Trajectory comparison in the map of Fig 17b for the unclustered and iterative strategies.

While the optimal path found by the Unclustered strategy approaches a straight-line among the obstacles, the path that the Iterative strategy obtains initially moves northwards to escape the northwestern cluster.

https://doi.org/10.1371/journal.pone.0233441.g029

We will see in the next subsection what are the outcomes of the adjustments made in the original planning problem, specially regarding the optimization times.

7.4 Computational time

The strategies proposed in this paper have two main effects in the agent trajectory. The first one is their impact on the resulting trajectories, as discussed in the last subsection. The second consequence is the decrease in the optimization times, the main drawback in a Mixed Integer problem and the primary advantage of our approach.

We summarize in Table 2 the resulting computational times of the presented strategies on the map of Fig 17a. For every strategy we represent both the nominal time of the CPLEX optimization—in the second column—as well as the Total time necessary to obtain the complete trajectory—in the seventh one. The latter includes all operations performed during the solution of the computational problem with the only exception of loading it initially into main memory. Additionally, we also identify the aggregated time, i.e. the time of the operations performed along all time steps from the initial position until the agent reaches the target set, that each strategy spends to obtain not only the Clustering configuration, but also the Bygone and Exterior constraints and the overall time spent on Infeasible iterations.

Download:

• PPT
  PowerPoint slide
• PNG
  larger image
• TIFF
  original image

Table 2. Average computational times and speedup for the map of Fig 17a.

https://doi.org/10.1371/journal.pone.0233441.t002

The trajectory of the Relaxed strategy is a straight-line from the initial agent position to the target set. In this case, the agent behaves as if there is no obstacle to be avoided. Naturally, it is not possible to attain a faster approach. Despite not being drawn in Figs 27 and 29, its solution time can be used as a benchmark to obtain the lower-bound on the optimization times.

From the data in Table 2, the main inference is that the extra time spent in the techniques we propose is negligible when compared to the total simulation time. The CPLEX optimization time falls progressively as more elaborate clustering strategies are used because the effect of each new component adds up incrementally to the previous ones.

The Effective speedup column measures the typical behavior of each strategy in practical situations. It is no use to guarantee speedup only in the problem solving phase, the so-called Nominal speedup here, if we add overhead in other steps, transferring computational load to stages not monitored and previous to the main computation. It is necessary to decrease the total solution time, and from Table 2 we can see that the Nominal speedup propagates to other solving phases, which renders an Effective reduction in the computing times of approximately 52, 148, 446 and 996 times for the Close, Bygone, Exterior and Iterative strategies, respectively.

All the strategies presented here offer both substantial Nominal and Effective speedups regarding the Unclustered scenario. Besides that, the Exterior and Iterative strategies also offer simulation times in the same order of magnitude than the lower-bound offered by the Relaxed strategy, which shows the effectiveness of the techniques proposed.

The same results for the Unclustered and Iterative Strategies in the Random environment of Fig 17b are shown in Table 3. Finally, the times spent on reclustering steps in the Iterative strategy are represented in Table 4. represented in Table 4.

Download:

• PPT
  PowerPoint slide
• PNG
  larger image
• TIFF
  original image

Table 3. Average computational times and speedup for the map of Fig 17b.

https://doi.org/10.1371/journal.pone.0233441.t003

Download:

• PPT
  PowerPoint slide
• PNG
  larger image
• TIFF
  original image

Table 4. Average time spent on reclustering iterations.

https://doi.org/10.1371/journal.pone.0233441.t004