Mejoras a los ambientes prácticos de aprendizaje

The path resulting from A* or JPS is normally close to obstacles in order to be “shortest” (e.g.,Figure 2.8). In the real world, when the robot gets close to obstacles, the control error due to disturbances and state estimation drifts could easily result in a crash into nearby obstacles. Hence, the resulting shortest paths are hard to follow due to their proximity to obstacles. A common solution for real world navigation is to plan in the C-space with over-inflated obstacles. Over-inflating can easily block small gaps such as doors, windows and narrow corridors. Thus, this solution is not complete, especially in an obstacle-cluttered indoor environment.

We propose a path perturbation technique to sacrifice path length for safety, resulting in increased clearance of obstacles, while avoiding unnecessary detours [106]. We model the collision cost using an artificial potential field (APF). The traditional APF method

(a) A*. (b) JPS.

Figure 2.8: The search for the shortest path using A* (left) and JPS (right). The output paths have the same lengths. Cells in red show the expanded vertices in the close-set.

in [2, 19, 25, 46, 104] only works for simple convex obstacles and is prone to get stuck in local minima, which makes it hard to use in complex environments. In our proposed method, we perturb a nominal shortest path into an optimal path using a graph search algorithm that considers collision costs and regional constraints. A similar approach demonstrated in [80] uses an iterative method to optimize the optimal safe path. In contrast, our method is more efficient due to its formulation. Our pipeline consists of three steps: first, find the shortest path from the start to the goal using the A* algorithm; second, inflate the path to get a tunnel for perturbation; third, generate the APF and search for the optimal path inside the tunnel.

Figure 2.11 illustrates the effect and importance of this perturbation process in naviga- tion, in which the map is partially unknown. To penalize the path approaching obstacles too closely, we generate a local APF around nearby obstacles (indicated by the colored dots). There are three different paths from the robot’s current position to the goal inside the room: the blue path is the shortest path derived from A*; the green path is the path that avoids both the potential field and obstacles; the magenta path is the optimal path planned using the proposed method. The magenta path goes through the middle of the door without any

-1

0

+1

Case 1:

Case 2:

Case 3:

Figure 2.9: Neighbor Pruning. We draw a3×3×3 voxel grid as three 3×3 2D layers – bottom (-1), middle (0), top (+1). The center node indicated by the blue arrow is currently being expanded. Thenatural neighbors of the current node are marked in white. The pruned neighbors are marked in grey. The blue arrow also shows the direction of travel from its parent which includes three cases: (1) straight, (2) 2D diagonal, and (3) 3D diagonal.

detours and is better than other two paths considering both travel distance and collision cost.

The first step in the proposed pipeline is to find the shortest path, which has already been described in previous sections. In the following paragraphs, we introduce the last two steps.

Tunnel Around Path

A tunnel in configuration space around the given path bounds the perturbation. LetD(r) be the disk with radius r,P be the given path, the tunnel T(D, P) is the Minkowski sum

Case 1:

Case 2:

Case 3:

-1

0

+1

Figure 2.10: Forced Neighbors. When the current node is adjacent to an obstacle (black), the highlightedforced neighbors (light red) cannot be pruned. The red arrow indicates the pair of an obstacle and its correspondingforced neighbor: if the tail voxel is occupied, its head voxel is aforced neighbor. For example, in Case 1, if the voxel (0,1,0) is occupied,(1,1,0) is aforced neighbor. In Case 2, the occupied voxel(0,0,1)results in threeforced neighborsand similarly in Case 3. We omit drawing the symmetric situations with respect to the blue arrow.

of P andD:

T(D, P) =P⊕D. (2.16)

Figure 2.12 shows three examples of tunnels generated using different D(r) from the same path. In general, the tunnel is non-convex. To implement the tunnel constraint in the pseudo-code, we simply augment each cell state with a flag to indicate whether it is inside the tunnel or not.

start goal

Figure 2.11: When planning a path from the robot’s current position to a goal inside the room, we examine three possible paths: the blue one is the shortest path derived from A*; the green one is the safest path that does not intersect with potential field (colored dots) or obstacles; the magenta one is the optimal path planned using our method.

Artificial Potential Fields

In our implementation on a grid map, the APF is discretized and truncated. Denote the truncated potential value of cellu asU(u), it is defined as:

U(u) =      0, d(u)≥dthr F(d(u)), dthr> d(u)≥0 (2.17)

where d(u) is the distance of cell u to the closest obstacle. The potential function F(d) should be a scalar, positive, and monotonically decreasing function of distancedwith domain [0, dthr). One choice that we use in Figure 2.11is an exponential function of orderk:

(a)r= 0.5m. (b)r= 1.0m. (c) r= 1.5m.

Figure 2.12: Our proposed method with different size tunnels, indicated in yellow. Left to right, the tunnel is created using the nominal shortest path (blue) with an increasing radiusr. The perturbed path (magenta) is away from obstacles and is much safer to follow than the nominal path.

with Fmax, k > 0. The parameter dthr determines the maximum distance that is affected by the APF. To improve algorithm efficiency, we only generate the APF for the local map around the robot’s current location inFigure 2.11.

Perturbation

To perturb the path, we need to change the optimization problem in (2.11) to account for a different cost function and a new set of constraints as compared to (2.13). Denote the us, ug as start and goal vertices, wU as the weight of potential value, T(D, Pprior) as the tunnel around the given path Pprior, the path perturbation is to solve the optimal P∗ for the following problem:

arg min P I P 1 +wU·U(s)ds s.t us, ug∈P, P ⊂T(D, Pprior)∩ Xf ree. (2.19)

The line integration in (2.19) is equivalent to the sum of the path length and the weighted value of the potential along the path. Therefore, the modified edge cost is simply the sum of edge length and weighted values of the potentials at the two vertices:

c(u, s) =d(u, s) + 1

2wU(U(u) +U(s)). (2.20)

collision-free. In the corresponding pseudo-code ofAlgorithm 1, we only need to modify the edge cost c(u, s) in line 23 and forbid the successors of the vertex u from going outside of the tunnel. The heuristic function of Euclidean distance is still admissible forwU·U(u)≥0, such that we are able to guarantee the optimality of the planned path. Given the tunnel and APF, we can search for the optimal path using the modifiedAlgorithm 1to observe the results shown inFigure 2.12.

Please note the following two propositions:

1. The perturbed path converges to the local minimum within the tunnel.

2. The computational time increases as tunnel size increases.

We briefly describe these two propositions below. By setting the size of tunnel to infinity, we can get the globally optimal path, but there is a trade-off between the path’s sub- optimality and the algorithm’s run time. In addition, as illustrated in Figure 2.11, it is not desirable to use the globally optimal path to navigate the robot in partially unknown environments.

For our application, we naively choose the tunnel radius r to be an arbitrary constant. An outer loop for iterative path optimization (2.19) can be used to ensure that the result converges to a locally optimal path. As shown inFigure 2.13, after 3 iterations, the optimal path converges to Figure 2.13c. We also extend this approach for anytime re-planning [48], which requires extremely fast computation.