Human experience–inspired path planning for robots

Abstract

In this article, we present a human experience–inspired path planning algorithm for service robots. In addition to considering the path distance and smoothness, we emphasize the safety of robot navigation. Specifically, we build a speed field in accordance with several human driving experiences, like slowing down or detouring at a narrow aisle, and keeping a safe distance to the obstacles. Based on this speed field, the path curvatures, path distance, and steering speed are all integrated to form an energy function, which can be efficiently solved by the A* algorithm to seek the optimal path by resorting to an admissible heuristic function estimated from the energy function. Moreover, a simple yet effective fast path smoothing algorithm is proposed so as to ease the robots steering. Several examples are presented, demonstrating the effectiveness of our human experience–inspired path planning method.

Keywords

Path planning human experience A* search algorithm path smoothing

Introduction

The path planning, which refers to designing a path for navigating an agent toward a desired location from a start position, has been widely applied in robotics community and other real application fields,¹ such as window cleaning,² exploration of Mars, and video game.³ Among these applications, path planning for service robots plays a central role in complex indoor and outdoor environments.⁴

In general, path planning is to find a sequence of location transition actions that transform a start position to a goal position, where each transition action has an associated cost, and the sum of costs of all transition actions represents some measurement for the path. Designing transition actions for path planning generally concerns on following aspects: (i) travel time between the start position and the goal position (also referred as the time factor); (ii) energy spent of an agent traveling a path; (iii) robots do not collide with other objects; and (iv) smoothness of a path is desired to ease steering of robots. Currently, main path planning methods aim at designing an optimization model which considers one or more of the above-mentioned aspects, and then performing a minimization procedure to achieve an optimal path.^1,4

–7 For example, the shortest path, minimum time-consuming path, minimal energy cost, and coverage path planning are, respectively, studied in the literature^6,8,9 for a given navigation task. Path planning involves four developing stages: graph-based methods (e.g. Dijkstra, Voronoi,¹⁰ A* and its variants^5,11,12), artificial potential method,^7,13 probabilistic methods (e.g. probabilistic roadmap method (PRM),¹⁴ rapidly exploring random tree (RRT)^15,16), and machine learning-based methods.^17,18 Tthe developing process of path planning is shown in Figure 1.

Figure 1.

The developing process of path planning.

While existing methods give available solutions for practical applications, neither of them has considered the safety of navigation. In practice, the safety requirement is very important. For example, a relatively narrow space between a robot and persons may easily cause collision between them especially when persons are playing regular activities (e.g. hand waving), resulting in an uneasy environment for humans. Therefore, a safety requirement is explicitly considered along with the distance and smoothness of a path for service robots in this article.

Our scheme is partly inspired by the human experience, because humans seem to be experts in path selection and could well analyze and synthesize diverse factors (e.g. distance, smoothness, and safety) in mind to make a proper decision for path planning. We therefore design a new path planning algorithm for robots by learning from human experience especially in terms of safe driving. Take the scene in Figure 2 as an example, where a service robot is ordered to fetch several cups of coffee for participants of a party. While a traditional algorithm may plan the path P_s, the human experience–inspired algorithm tends to select the path P_o which should be more safe although taking a little longer distance.

Figure 2.

Illustration of human experience–inspired path planning. In this scene, a service robot carrying several cups of coffee on a plate is serving at a party and is required to traverse from the current position S to the target position G. The shortest path P_s is selected by the traditional algorithms, and the path P_o is selected by the human experience–inspired algorithm.

In fact, a large number of robots has been built that explicitly mimic biological navigation behaviors for obstacle avoidance, such as the ones emulating an annual migration of seabirds^19,20 and ant-like behavior navigation model.²¹ Inspired by the social interactions in human crowds or animal swarms, Savkin and Wang²² proposed an efficient obstacle avoidance algorithm in dynamic environments by integrating representation of the information about the environment. Typically, the development of robotics has been directly or indirectly affected by human’s experiences and behaviors.^23
–25 However, these methods^24

–27 mainly focus on the motion modeling of robot’s body parts (e.g. arms) and the interactive applications between robots and humans. In this article, we emphasize and learn from human driving experiences. Let’s take some discussion on the human’s driving experiences. Generally, a person prefers reducing the driving speed when passing neighboring areas of obstacles, while speeding up in wide areas with safe enough distance to obstacles. Furthermore, in front of a very narrow aisle, a person like to select another wide path even with a little longer distance. Yet, she/he may determine to continue the narrow path if the wide one is too much long compare with the narrow one. Passive human walking has been studied by many researchers,²⁸ and it simulates human’s behavior by considering the potential energy consuming when human walking. In a word, path planning by humans is not only related to the time cost, path distance, and ease steering but also relevant to the safety.

Inspired by human’s experiences, we develop a safety-related path planning method. We construct a speed field that can efficiently simulate slowing down and detouring behaviors of humans around obstacles and then integrate the distance and smoothness factors with the speed field to form a human experience–inspired energy function model. Based on this model, the optimal path can be efficiently achieved by the well-known A* algorithm. Specially, the lower bound of the energy function, namely the admissible function in A* algorithm, can be easily estimated. We also propose a fast yet effective post-processing to smooth paths generated by the A* method. In summary, the main contributions of this article include:

A human experience–inspired path planning algorithm that considers the safety factor is presented.

A dynamic speed field is constructed to control robot’s speed, which can also adapt to dynamic scenes.

An effective post-processing approach is utilized to smooth paths for ease steering of robots.

The rest of this article is organized as follows. In “Related work” section introduces some related works. In “The proposed model” section, an energy functional model of path planning is presented to simulate human’s experience, and speed field computation and a practical feasible curvature computation scheme are also detailed. In “Path planning scheme” section, A* algorithm is applied to the energy function to search an optimal path, and a path smoothing method is also presented. Several path evaluation criteria are introduced in “Path evaluation” section. Experimental results are shown in “Implementation and results” section. Finally, conclusions are made in “Conclusions and future works” section.

Related work

Path planning has been widely studied by robot communities and other researchers, and plenty of methods have been proposed to deal with practical problems. In this section, we mainly focus on representative path planning algorithms in two dimensional scenarios, and more references can be found in the study by LaValle.¹

Visibility graph^29,30 views obstacles in configuration spaces as polygons and then constructs a graph using the start position, the goal position, and vertices of polygons. Then the path is obtained by graph search methods, for example, Dijkstra’s algorithm.

Artificial potential field (APF)^7,13,31,32 is another important method for path planning in robots. In general, obstacles are modeled as repulsive fields in APF, while the goal position is considered as an attractive field. Thus, the repulsive fields avoid agents approaching to obstacles, and the attractive fields move agents to the goal. Paths generated by APF are generally smooth, but the main drawback of APF is that a local minimizer is trapped possibly.

Probabilistic path planning algorithm is an effective method, and its two representative methods are the PRM¹⁴ and the RRT.^15,16,33 The basic idea of probabilistic path planning is to randomly select non-collision points in free motion space and then connect them to get a path. Probabilistic methods are probabilistically complete, and the path generated by them for the same problem is also not unique.

A* method^5,11 is a widely used path planning method since it has many good properties: (1) given start and goal positions in a scene, the path achieved by A* is unique; (2) in a finite time, A* can always return a result even there is no solution; and (3) a good admissible function can lead to an acceptable time-consuming even for a large map. Now many researchers have developed various variants of A* to deal with different situations and tasks, such as D* Lite,³⁴ any-angle A*,³ and Field D*.¹²

Gradient-based methods^35,36 are a two-step algorithm of designing an optimal path. It first uses a straight line to connect the start and target points (even pass through obstacle regions) and then takes points out of obstacles in the gradient directions. Generally, the raw paths generated by the process are not smooth, thus a post smoothing operation is desired.

When an agent faces a completely unknown environment and equips with some sensors, it requires to deal with all kinds of situations according to the sensed information. In the case, machine learning-based methods^17,18 are a good technology to deal with path planning tasks.

Comparing with other path planning algorithms, A* algorithm is a deterministic and flexible approach, and it is very convenient to search an optimal solution for a concrete path planning problem. In this article, we propose using the A* algorithm to optimize the proposed path planning model.

Our method is partially similar to Dolgov et al.’s method,¹⁰ and both methods consider the practical safe requirement, but their method does not consider the distance factor and causes the generated paths often along with central lines of roads. As compared, our method can flexibly adjust the path by controlling parameters of the speed field.

The proposed model

In this section, we introduce the energy function of our path planning method according to human experiences.

Energy function

The driving path of a person is related to both the smoothness and the psychological speed of the path. Generally, people prefer driving on a wide road with high speed, which motivates us to design the energy function E inversely proportional to the speed v. However, when v is too small, $\frac{1}{v}$ would tend to infinity and be the dominant contribution for path planning, which is not desired in practice. Therefore, we set

E \propto M - v

where M > 0 is a large enough constant.

On the other hand, humans are prevailing to smooth paths with little turnings. An ideal case is that the driving road is almost straight. In fact, the steering experience implies two factors: the driving time is as short as possible, and the driving road is as straight and smooth as possible. Let s be the driving distance, and κ is the path smoothness parameter. We also consider that the energy function E is proportional to the distance s and the parameter κ.

According to the above analysis, the energy cost along a path C is defined as follows

E (C) = λ_{1} s (C) + λ_{2} κ (C) + λ_{3} \int_{C} M da - v (C)

where da represents the line element; λ₁ > 0, λ₂ > 0, and λ₃ > 0 are three trade-off parameters. The energy function relates to two initial conditions, the start position p_s and the target position p_g. The inequality v ≤ V_max is also required, where v and V_max are the scalar values of velocity v(p) and maximal velocity, respectively. We then denote them as the speed and the maximal speed, respectively. κ(C) represents the sum of curvatures along a path C, and v(C) represents the speed sum along a path C.

The objective of this article is to seek an optimal path C* which minimizes the energy cost function defined in equation (2). We also denote the minimal energy as E(C*). Supposing a path $C = {p_{1} = p_{s}, p_{2}, \dots, p_{n} = p_{g}}$ , then the corresponding discrete energy function is defined as

\begin{array}{l} E (C) = & λ_{1} \sum_{i = 1}^{n - 1} ∥ p_{i + 1} - p_{i} ∥ + λ_{2} \sum_{i = 1}^{n} κ (p_{i}) \\ + λ_{3} \sum_{i = 1}^{n} (M - v (p_{i})) \end{array}

where κ(p) and v(p) are the curvature and speed at point p, respectively.

So far, we have three unknowns s(C), κ(p), and v(p). In the following sections, we give methods to determine the last two quantities. The distance s(C) can be determined by the algorithm automatically.

In the following two subsections, we mainly give details how to compute the speed field v(p) and the computation of curvatures κ(p) at every point p. Here, we just concern the scalar values of v(p) and κ(p).

Dynamic speed field

We assume the map for a robot traversing is a white and black image. The black patches represent obstacles that robots cannot reach, and white parts are available areas that robots can reach.

As we have pointed out, the driving speed of humans is related to driver’s psychology. The speed is high if there are no obstacles around, otherwise it is slow. Thus, the speed is related to the distance between vehicles and obstacles. Therefore, we use the Laplacian equation defined in equation (4) to estimate an impact coefficient map u for an input map. Then u can be viewed as a smooth control function which can simulate the speed field with the maximal speed V_max

Δ u (x, y) = \frac{\partial^{2} u}{\partial x^{2}} + \frac{\partial^{2} u}{\partial y^{2}} = 0

where u(x, y) has the following approximation scheme

\begin{array}{l} u^{k} (x, y) = & \frac{1}{4} (u^{k} (x + 1, y) + u^{k} (x - 1, y) \\ + u^{k} (x, y + 1) + u^{k} (x, y - 1)) \end{array}

where k is the iterative step. The initial conditions are boundaries of obstacles with value 0. The coefficient map is calculated by equation (5) using iterative optimization. In our experiments, the number of iterations is set to 40.

Assuming the maximal speed of a robot as V_max, the speed field of an input map for a robot is defined as follows

\tilde{v} (x, y) = ω V_{max} \cdot u (x, y)

where 0 < ω ≤ 1 is a trade-off coefficient which can control the speed of a robot further.

In practical situations, it is required that the speed of robots is not too slow even it is turning. Thus, the speed field is finally defined by

v^{'} (x, y) = max (\tilde{v} (x, y), ε V_{max})

where 0 < ε < 1 is a constant that control the minimal speed of robots. Essentially, the truncated definition ensures the speed of a robot not too small. Due to the smoothness of Laplacian interpolation, there is not enough discrimination for neighbors of a point in speed map v′. Therefore, a logarithm transformation is performed, and the final speed map is $v (x, y) = log (v (x, y))$ .

Dynamic updating

We have presented a static speed field in scenes as earlier, but in practice, dynamic scenes are ubiquitous (e.g. diverse activities of humans) and thus a dynamic updating scheme is required for modern robots with sensors.

For a given map with obstacles O, we first compute the Voronoi diagram V_d of O. If robots detect a new obstacle O_s, we then update V_d for O_s locally. Denote C_s the Voronoi cell of O_s, then the speed field v(x, y) is also updated locally in C_s by the Laplacian equation (5) with zero boundaries of O_s.

Curvature computation

As shown in Figure 3, p and q are two points on a curve (blue solid curve), and q is also a neighbor point of p. l′₁ and l₂ (two green vectors in Figure 2) are two unit tangent lines at p and q, respectively. Translating l′₁ to q gets another vector l₁, and $δ T = l_{1} - l_{2}$ (red vector) is the difference between l₁ and l₂. From the view of tangent, the curvature κ(p) at p has the following definition

κ (p) = lim_{q \to p} \frac{δ T}{∥ p - q ∥}

where ∥ p − q ∥ is the Euclidean distance of p and q. In this article, we use tangents to approximate curvatures.

Figure 3.

Curvature definition by tangent. l′₁ and l₂ are the unit tangents of p and q, and translating l′₁ to q generates the purple vector l₁. δ T is the difference between l₁ and l₂.

Supposing a robot has been moved i steps which generates i points in the motion space, which are denoted by ${p_{1}, \dots, p_{i - 1}, p_{i}}$ . According to the definition in equation (8), the curvatures can be approximated at these discrete points by

κ (p_{i}) = \frac{2 ∥ \vec{\bar{p_{i - 1}} p_{i}} - \vec{p_{i} p_{g}} ∥}{∥ \bar{p_{i - 1}} p_{g} ∥}

where $\vec{p q} = q - p$ represents a vector pointing from p to q, and ∥ pq ∥ is the Euclidean distance between p and q. It is worthy to pointing out that $\bar{p_{i}}$ is the weighted mean value of previous points $p_{1}, p_{2} \dots, p_{i}$ ,…that is

\bar{p_{i}} = \frac{1}{2} (p_{i} + \bar{p_{i - 1}})

A benefit of the definition of $\bar{p_{i}}$ is that it measures the amount of $\bar{p_{i}}$ deviated from the previous short time path.

Path planning scheme

Based on the proposed model, we search a path by using the A* method that can navigate a robot from any initial position to the target position avoiding collisions.

Formulation in motion space

In this article, robots are assumed to be disk-shaped objects, and obstacles are convex or concave polygons. Assuming a robot centered at _x ∈ R² with radius R > 0 operates in a closed compact space S ⊂ R², and M polygonal obstacles ${O_{i}}_{i = 1}^{M}$ also locate in S. We define the free motion space F of a robot as follows

F = {x \in S : \bar{B (x, R)} \subset S \ \cup_{i = 1}^{M} O_{i} \oplus B (\partial O_{i}, R) \cup B (\partial S, R)}

where B(x, r) is an open ball $B (x, R) = {y \in S : ∥ y - x ∥ < R}$ , $\bar{B (x, R)}$ represents the closure, ∥ ⋅ ∥ denotes the 2-norm, ∂S is the boundary of S, and ⊕ is the dilation operation along every obstacle’s boundary. We also assume that the distance of any two polygonal obstacles in F satisfies

d_{M} (p_{i}, p_{j}) > 2 R, \forall i \neq j

where $d_{M} (p_{i}, p_{j})$ is the minimal distance of two sets, that is $d_{M} (p_{i}, p_{j}) = min {∥ x - y ∥ : x \in p_{i}, y \in p_{j}}$

Overall, a robot in theory can walk freely in its free motion space, however, in practice, a more wider space is needed for robots especially when they traverse some obstacles. For example, in Figure 2, rail-mounted robots in an unmanned dinning room generally preserve a certain distance from tables and chairs for safety considerations.

A*-based path planning

Given a target location p_g ∈ F and a robot locates at p_s ∈ F, we need to find a path that can navigate a robot from p_s to p_g without collisions. The task can be accomplished by getting a position vector $P_{n} = (p_{1}, p_{2}, \dots, p_{n})$ such that p₁ = p_s and p_n = p_g.

The A* method selects a path that minimizes $f (p) = g (p) + h (p)$ , where p is the last position of a path, and g(p) is the cost of the path from the start position p_s to p, and h(p) is an admissible heuristic function that measures the lower bound of the cost from p to the target position p_g. The key of A* search algorithm is to design an admissible heuristic function h(p), where the use of h(p) is to avoid overestimating the actual cost to arrive at the target position. In this article, we define the heuristic function h(p) as follows

h (p) = λ_{1} ∥ p - p_{g} ∥ + λ_{2} κ (p) + λ_{3} (M - v (p))

where ∥ ⋅ ∥ is the 2-norm. The heuristic function h(p) designed here is admissible obviously.

On the other hand, if let p_k = p, then the function g(p) is defined as

g (p) = λ_{1} s_{s} (p) + λ_{2} \sum_{i = 1}^{k} κ (p_{i}) + λ_{3} \sum_{i = 1}^{k} (M - v (p_{i}))

where s_s(p) is the length of a polyline ${p_{1}, p_{2}, \dots, p_{k}}$ .

Based on aforementioned formulations, the optimal path can be produced using the standard A* algorithm.

Path smoothing

In most cases, the raw paths generated by A* algorithm are possibly not smooth enough, even the smoothness has been considered. Therefore, a post smooth process is performed to ease driving for robots. Specifically, the path is smoothed by minimizing an energy function under following constraints: (1) paths are smooth enough, (2) point positions in the smoothed path after smoothing are not too far away from original corresponding ones, and (3) smoothed path should also have a strong safe distance to the obstacles.

Assuming the path $C = {p_{1} = p_{s}, p_{2}, \dots, p_{n} = p_{g}}$ is a polyline, and $C_{s} = {q_{1} = p_{s}, q_{2}, \dots, q_{n - 1}, q_{n} = p_{g}}$ is its smoothed version. Then, the optimal energy function for path smoothing is defined as follows

\begin{array}{l} E (C_{s}) = & \sum_{i = 2}^{n - 3} \frac{∥ q_{i} - B_{i} ∥^{2}}{2} + β_{1} \sum_{i = 2}^{n - 1} \frac{∥ q_{i} - p_{i} ∥^{2}}{2} \\ + β_{2} \sum_{i = 2}^{n - 1} F_{rep} (q_{i}) \end{array}

where β₁, β₂ > 0 both are real numbers, $q_{i}^{b}$ is an attractive point, and $F_{rep} (q_{i})$ is the repulsive function defined in APF,¹³ that is

F_{rep} (q_{i}) = {\begin{matrix} \frac{1}{2} {(\frac{1}{ρ (q_{i}, p_{obs})} - \frac{1}{ρ_{0}})}^{2} & ρ (q_{i}, p_{obs}) \geq ρ_{0} \\ 0 & ρ (q_{i}, p_{obs}) < ρ_{0} \end{matrix}

where ρ(·, ·) is the distance function, p_obs represents obstacles, and ρ₀ > 0 is a constant given by users. The second term of equation (14) guarantees that smoothed paths cannot deviate from original ones too far. The third term guarantees the strong safe distance. The first term attracts the point q_i to a Bezier point B_i, which can control the smoothness of the path. Specially, B_i is a Bezier point interpolated by four neighbor points $(q_{i - 2}, q_{i - 2}, q_{i + 1}, q_{i + 2})$ of q_i

B_{i} = \frac{1}{8} q_{i - 2} + \frac{3}{8} q_{i - 1} + \frac{3}{8} q_{i + 1} + \frac{1}{8} q_{i + 2}

This formula indicates that B_i is from a cubic Bezier curve. Since Bezier curves are generally smooth, we can desire paths processed by this method are also smooth. In fact, Bezier-based smoothing path generation methods are extensively used in path planning.^37,38

Using the gradient descent algorithm, the energy functional shown in equation (14) can be optimized by the following iterative scheme

q_{i}^{k + 1} = B_{i}^{k} + λ_{1} p_{i} + α_{i}^{k} (q_{i}^{k} - O_{i}^{k}) / (1 + λ_{1})

where $α_{i}^{k}$ is defined as

α_{i}^{k} = {\begin{array}{l} α (\frac{1}{ρ (q_{i}^{k}, O_{i}^{k})} - \frac{1}{ρ_{0}}) \frac{1}{{(ρ (q_{i}^{k}, O_{i}^{k}))}^{3}} & ρ (q_{i}^{k}, O_{i}^{k}) \leq ρ_{0} \\ 0 & ρ (q_{i}^{k}, O_{i}^{k}) > ρ_{0} \end{array}

where $O_{i}^{k} \in p_{obs}$ represents the closest point to $p_{i}^{k}$ .

Path evaluation

Path Evaluation is very important in path planning study. In the previous work, researchers usually used the following three criteria to measure a path’s quality: the running time T of algorithm, the number of turning points TPN, and the length of a path PL. However, all three aspects do not related to safety. Thus we design following two criteria to measure safety:

the minimal distance between a path P and obstacles O: $MD = min {∥ x - o ∥ : x \in P, o \in O}$ .

the safety coefficient of a path P and obstacles O: $SC = \frac{\sum_{x \in P} d (x, O)}{PL}$ , where $d (x, O) = min{∥ x - o ∥ : o \in O}$ , and O represents obstacles.

MD reflects the worst case of a path passing through obstacle regions. SC is used to measure the safety performance which is not considered by previous works to the best of our knowledge. In practice, humans generally desire the path length PL as short as possible and distances to obstacles as long as possible. Therefore, a bigger SC reports a better safety performance.

Implementations and results

In this section, we give implementation details of our algorithm and show the results from our approach. All examples shown in this article are performed on a laptop running on a 3.40 GHz laptop computer with 4 GB of memory. Generally, the path planning algorithm, including A* search and path smoothing, can be accomplished in 0.3 s for an indoor layout map (1026 × 800) and the Paris rendering map (907 × 728) shown in Figures 5 and 6, respectively.

In our implementation, the A* method searches eight directions (8-neighbors) of current position each time, and therefore the step length of every action is 1 or $\sqrt{2}$ . We normalize curvatures and speeds of all positions into an interval [0.0, 1.0] to eliminate the influence of different dimensions. Furthermore, we set ω = 0.8 and ε = 0.1. If without statement, in path smoothing module, we set α = 3 and ρ₀ = 10. In path planning module, we set $λ_{1} = 0.4, λ_{2} = 0.4$ , and λ₃ = 0.6 if without special statement.

The proposed optimal path planning algorithm is built on the platform of Visual C++ with Qt 5.3.2 GUI. In the system as shown in Figure 4, we implement the shortest path method and our method, respectively, which are both optimized by the A* search algorithm. On the left working area of the system interface, maps and paths are shown, and the start and target positions S and G can be chosen interactively. On the right panel of the system interface, several widgets are designed to adjust parameters. Once a start position and a target position are chosen, we can choose an algorithm in the process menu to generate a path. For example, in Figure 4, we use our method to generate a path (green curve) that connects S and G.

Figure 4.

The interface of our optimal path planning system. The green and blue curves represent our optimal path and the shortest path, respectively.

Experimental results

For given start and target nodes p_s = 248, 110 and p_g = 669, 653, the example for an indoor scene is shown in Figure 5. The result indicates that our path planning method is feasible for indoor environments. The path (green curve in Figure 5) generated by our method is smooth and extends along boundaries of obstacles, which is very similar to human’s walking habit. Furthermore, the path indeed has a certain distance with boundaries of obstacles, which guarantees strong safety for robot steering. Therefore, it is suitable for robot steering along the path.

Figure 5.

Illustration of path planning by our method at an indoor layout scene.

We have also tested the path planning on large-scale scenes. We download a three-dimensional (3-D) Paris model from Internet, and then the model is projected onto the x − z plane orthogonally. The projection is shown in the left of Figure 6. Then the projection is binarized with color clustering operations and manual refinement to extract the transportation road networks of Paris. As shown in Figure 6, red dots indicate the start position (p_s = 154, 579) and the target position (p_g = 853, 159), and the green and blue curves represent our optimal path and the shortest path, respectively. Note that in this example, the path smoothing is not performed since the width of the road networks is very thin. Note that our path (green curve in Figure 6) contains a long and straight segment comparing with the shortest path (red curve in Figure 6).

Figure 6.

Path planning on a large-scale scene. The left is the render map of Paris 3-D model with several path planning results, where the green and blue curves represent our optimal path planning result and the shortest path planning result, respectively. The right-top and the right-bottom are two local zoom-in images.

Path evaluation

We evaluate different paths generated by different path planning algorithms, including RRT method,¹⁶ practical method,¹⁰ fuzzy logic (FL) method,³⁹ and our method. RRT¹⁶ and the practical search (PS) method¹⁰ are implemented using Matlab [version 2010], and the FL method³⁹ is implemented by C++. Note that in Figure 7, we set λ₃ = 2.0. Paths generated by other methods are smoothed except for RRT, thus a simple mean value filter is used for RRT. Path planning results of these algorithms in maps with size 500 × 500 are shown in Figure 7.

Figure 7.

Path planned by different methods. (a) RRT¹⁶; (b) practical method¹⁰; (c) our path planning result; (d) RRT¹⁶; (e) practical method¹⁰; (f) fuzzy logic³⁹; and (g) our path planning result. RRT: rapidly exploring random tree.

To evaluate path’s quality, five different criteria (see “Path evaluation” section) are computed for paths generated by different algorithms, and the statistical results are reported in Tables 1 and 2. Since RRT is a random algorithm, it is tested ten times in the two static maps, and only one of the results are shown in Figure 7. As shown, RRT method¹⁶ has less turning points comparing to other three algorithms but performs worse with respect to other criteria, especially getting lower safe coefficient. FL method³⁹ shows a mediocre performance for these criteria. The practical method¹⁰ and our method have a similar safe performance, but our method always have a shorter path length than the practical method. In summary, the method proposed in this article can balance different requirements.

Table 1.

Statistical results of paths are shown in Figure 7(a) to (c), where — means the algorithm fails for the map with given start and target nodes.

	T (s)	TPN	PL	MD	SC
RRT	3.088	436	541.75	0.307	9.224
FL	—	—	—	—	—
PS	0.062	573	665.439	10.6538	20.0372
Human experience–inspired path planning method	0.107	564	590.783	9.07	14.6226

RRT: rapidly exploring random tree; FL: fuzzy logic; PS: practical search; TPN: number of turning points; PL: length of a path; MD: minimal distance; SC: safety coefficient.

Table 2.

Statistical results of paths are shown in Figure 7(d) to (g).

	T (s)	TPN	PL	MD	SC
RRT	2.27	461	617.1	6.082	25.374
FL	1.14	508	630.46	15.94	29.98
PS	0.01	524	637.193	28.6314	32.95
Human experience–inspired path planning method	0.109	527	621.276	22.6872	33.8662

RRT: rapidly exploring random tree; FL: fuzzy logic; PS: practical search; TPN: number of turning points; PL: length of a path; MD: minimal distance; SC: safety coefficient.

We have also analyzed influence of different parameters λ₁, λ₂, and λ₃ for optimal path planning, as shown in Figure 8. In this example, we generally set λ₁ = 0.01, λ₂ = 0.01, and λ₃ = 0.01 if without special explanation. The paths shown in Figure 8(a) to (d) tend to be a shorter one with increasing λ₁, which agrees with our model designing. With the increasing of λ₂, paths shown in Figure 8 (e) to (h) have obvious variations. In fact, since curvature characterizes local shapes of geometry, and it is also very sensitive, thus different λ₂ lead diverse results. There is a little special for λ₃. When it is bigger than λ₁ and λ₂, paths shown in Figure 8 (j) to (l) do not show obvious variations. Therefore λ₃ is not sensitive if it has a bigger value. Hence choices of λ₁, λ₂, and λ₃ are pivotal for concrete applications. We could set a larger λ₁ for a shorter distance requirement. Note that λ₂ controls the weight of curvatures of paths which are a local geometrical features. Therefore, we advise a large λ₂ if a local smoothness is desired. However, a very large λ₂ is not a good choice for smoothness (e.g. Figure 8(g) and (h)). Generally, we suggest a large value of λ₃ and a similar value of λ₁ and λ₂ for a safe path application.

Figure 8.

The path planning results by our method with different parameters. (a) λ ₁ = 0.01; (b) λ ₁ = 0.05; (c) λ ₁ = 0.5; (d) λ ₁ = 3; (e) λ ₂ = 0.01; (f) λ ₂ = 0.05; (g) λ ₂ = 0.5; (h) λ ₂ = 3; (i) λ ₃ = 0.01; (j) λ ₃ = 0.05; (k) λ ₃ = 0.5; and (l) λ ₃ = 3.

Dynamic scenes

A dynamic path planning example is tested, and the result is shown in Figure 9. A path is generated and connects S and G in a static scene shown in Figure 9(a). But in Figure 9(b), when robots move to the node M, a new obstacle H is detected, and then the speed field is updated, and the path is also re-computed. The final path is shown in Figure 9(b). The example illustrates that the dynamic updating scheme is useful for dynamic scenes.

Figure 9.

Illumination of path planned in dynamic scene by our method. Compared with (a), a new obstacle H appears in (b).

Examples shown in this section demonstrate that our optimal path planning algorithm for robots is practically feasible. Specially, these results also validate that our model can simulate human’s experiences.

Conclusions and future works

In this article, we propose a model to simulate human’s path planning. Specially, the safety of robot navigation is considered. The major technical contributions of the article include the following aspects:

A dynamic speed field is proposed, which is incorporated with multiple factors, such as safety, path curvatures and path distance, to model human driving experiences. By adapting parameters (λ₁, λ₂, λ₃), the model can generate diverse paths according to specific requirements.

An efficient but easy-to-implement path smoothing algorithm is presented. The smoothed path is more suitable for steering of robots.

Experiments on indoor scene and large-scale outdoor scene have demonstrated the effectiveness of our path planning method.

In the future, we would like to develop our method in several aspects, such as smartly selecting parameters λ₁, λ₂, and λ₃, and adapting to usual maps (e.g. maps with non-smoothed obstacles). We also would like to develop our method assisted by scene sematic analysis especially identifying the static objects and persons.

Footnotes

Acknowledgements

We would like to give our thanks to the artist Johnny who shares the 2-D indoor layout map (Figure 5), and Konstantin who shares the 3-D Paris model ().

Declaration of conflicting interests

The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding

The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: This project was partially supported by the Natural Science Foundation of China (no. 61672544, 61725204, 61661130156), Guangdong Natural Science Foundation (no. 2015A030311047), Tip-top Scientific and Technical Innovative Youth Talents of Guangdong special support program (no. 2016TQ03X263), the Fundamental Research Funds for the Central Universities (no. 11617348), Beijing Higher Institution Research Center of Visual Media Intelligent Processing and Security, and Foundation of Key Laboratory of Machine Intelligence and Advanced Computing of the Ministry of Education (no. MSC-201701A), and Shenzhen Innovation Program (no. JCJY2015040114529008).

References

LaValle

. Planning algorithms. Cambridge: Cambridge University Press, 2006.

Farsi

Ratcliff

Johnson

. Robot control system for window cleaning. In: Proceedings of American control conference, Baltimore, Maryland, 29 June–1 July 1994, pp. 994–995.

Yap

Burch

Holte

. Any-angle path planning for computer games. In: Proceedings of the seventh AAAI conference on artificial intelligence and interactive digital entertainment, Stanford, California, USA, 10–14 October 2011, pp. 201–207. AIIDE’11, AAAI Press.

Liu

. Robotic online path planning on point cloud. IEEE Trans Cybern 2015; 46(5): 1217–1228.

Peter

Nilsson

Raphael

. A formal basis for the heuristic determination of minimum cost paths. IEEE Trans Syst Sci Cybern 1968; SSC–4(2): 100–107.

Mei

. Energy-efficient motion planning for mobile robots. In: IEEE international conference on robotics and automation (ICRA), New Orleans, LA, USA, 26 April–1 May 2004, pp. 4344–4349.

Khansari-Zadeh

Khatib

. Learning potential functions from human demonstrations with encapsulated dynamic and compliant behaviors. Auton Robot 2015; 41(1): 45–69.

Goldberg

Harrelson

. Computing the shortest path: A* search meets graph theory. In: SIAM symposium on discrete algorithms (SODA), Vancouver, Canada, 23–25 January 2005, pp. 156–165.

Galceran

Carreras

. A survey on coverage path planning for robotics. Robot Auton Syst 2013; 61(12): 1258–1276.

10.

Dolgov

Thrun

Montemerlo

. Practical search techniques in path planning for autonomous driving. In: Proceedings of the first international symposium on search techniques in artificial intelligence and robotics (STAIR-08), Chicago, USA, 13–14 July 2008, AAAI.

11.

Rina

Pearl

. Generalized best-first search strategies and the optimality of A*. J ACM 1985; 32(3): 505–536.

12.

Nash

. Any-angle path planning. PhD Thesis, Department of Computer Science, University of Southern California, 2012.

13.

Koren

Borenstein

. Potential field methods and their inherent limitations for mobile robot navigation. In: IEEE international conference on robotics and automation (ICRA), Sacramento, USA, 9–11 April 1991, pp. 1398–1404.

14.

Kavraki

Svestka

Latombe

. Probabilistic roadmaps for path planning in high-dimensional configuration spaces. IEEE Trans Robot Autom 1996; 12(4): 566–580.

15.

Lavalle

. Rapidly-exploring random trees: a new tool for path planning. Technical report, Computer Science Dept, Iowa State University, 1998.

16.

Jaillet

Cortes

Simeon

. Transition-based RRT for path planning in continuous cost spaces. In: International conference on intelligent robots and systems (IROS), Nice, France, 22–26 September 2008, pp. 2145–2150.

17.

Willms

Yang

. An efficient dynamic system for realtime robot-path planning. IEEE Trans Syst Man Cybern B (Cybernetics) 2006; 36(4): 755–766.

18.

Otte

. A survey of machine learning approaches to robotic path-planning. Cs.Colorado.Edu, 2015.

19.

Franz

Mallot

. Biomimetic robot navigation. Robot Auton Syst 2000; 30: 133–153.

20.

Milford

Schulz

. Principles of goal-directed spatial robot navigation in biomimetic models. Philos Trans Royal Soc B 2014; 369(1655). DOI: 10.1098/rstb.2013.0484.

21.

Mathews

Lechon

Calvo

JMB

. Insect-like mapless navigation based on head direction cells and contextual learning using chemo-visual sensors. In: IEEE/RSJ international conference on intelligent robots and systems, St. Louis, USA, 10–15 October 2009, pp. 10–15.

22.

Savkin

Wang

. Seeking a path through the crowd: robot navigation in unknown dynamic environments with moving obstacles based on an integrated environment representation. Robot Auton Syst 2014; 62(10): 1568–1580.

23.

Chen

Sun

. Moving groups of microparticles into array with a robot-tweezers manipulation system. IEEE Trans Robot 2012; 28(5): 1069–1080.

24.

Xiao

Zhang

Beck

. Human-robot interaction by understanding upper body gestures. Presen Teleop Virt Environ 2014; 23(2): 133–154. DOI: 10.1162/PRES-a-00176.

25.

Zhang

Beck

Thalmann

. Human-like behavior generation based on head-arms model for robot tracking external targets and body parts. IEEE Trans Cybern 2015; 45(8): 1390–1400. DOI: 10.1109/TCYB.2014.2351416.

26.

Zhang

. Neural-dynamic-method-based dual-arm cmg scheme with time-varying constraints applied to humanoid robots. IEEE Trans Neural Netwk Learn Syst 2015; 26(12): 3251–3262. DOI: 10.1109/TNNLS.2015.2469147.

27.

Zhang

Lin

. Tri-criteria optimization-coordination-motion of dual redundant robot manipulators for complex path planning. IEEE Trans Control Syst Technol 2017; 99: 1–13. DOI: 10.1109/TCST.2017.2709276.

28.

Corral

Meneses

Castejon

. Forward and inverse dynamics of the biped pasibot. Int J Adv Robot Syst 2014; 11(7): 109. DOI: 10.5772/58537.

29.

Lozano-Pérez

Wesley

. An algorithm for planning collision-free paths among polyhedral obstacles. Commun ACM 1979; 22(10): 560–570. DOI: 10.1145/359156.359164.

30.

Mark

Marc

Mark

. Computational Geometry, 2nd ed. Berlin, Heidelberg: Springer-Verlag, 2000.

31.

Kuric

Bulej

Saga

. Development of simulation software for mobile robot path planning within multilayer map system based on metric and topological maps. Int J Adv Robot Syst 2017; 14(6). DOI: 10.1177/1729881417743029.

32.

Arslan

Koditschek

. Exact robot navigation using power diagrams. In: IEEE international conference on robotics and automation (ICRA), Stockholm, Sweden, 16–21 May 2016, pp. 1–8.

33.

Viseras

Losada

Merino

. Planning with ants: efficient path planning with rapidly exploring random trees and ant colony optimization. Int J Adv Robot Syst 2016; 13(5). DOI: 10.1177/1729881416664078.

34.

Koenig

Likhachev

. D*lite. In: Eighteenth national conference on artificial intelligence, 28 July–1 August 2002, pp. 476–483. Menlo Park, CA, USA: American Association for Artificial Intelligence, ISBN: 0-262-51129-0.

35.

Zucker

Ratliff

Dragan

. Chomp: Covariant Hamiltonian optimization for motion planning. Int J Robot Res 2013; 33(9–10): 1164–1193.

36.

John

Yan

Jonathan

. Motion planning with sequential convex optimization and convex collision checking. Int J Robot Res 2014; 33(9): 1251–1270.

37.

Choi

Renwick

Gabriel

. Path planning based on Bézier curve for autonomous ground vehicles. In: Proceedings of the advances in electrical and electronics engineering—IAENG special edition of the world congress on engineering and computer science, San Francisco, California, 22–24 October 2008, pp. 158–166.

38.

Jolly

Sreerama

Vijayakumar

. A bezier curve based path planning in a multi-agent robot soccer system without violating the acceleration limits. Robot Auton Syst 2009; 57(1): 23–33.

39.

Hong

Nakhaeinia

Karasfi

. Application of fuzzy logic in mobile robot navigation. In: Dadios

(ed.) Fuzzy Logic-Controls, Concepts, Theories and Applications. 2012, ISBN: 978-953-51-0396-7.