Human-Like Path Planning in the Presence of Landmarks

Abstract

This work proposes a path planning algorithm for scenarios where the agent has to move strictly inside the space defined by signal emitting bases. Considering a base can emit within a limited area, it is necessary for the agent to be in the vicinity of at least one base at each point along the path in order to receive a signal. The algorithm starts with forming a specific network, based on the starting point such that only the bases which allow the described motion are included. A second step is based on RRT*, where each edge is created solving an optimal control problem that at the end provides a human-like path. Finally the best path is selected among all the ones that reach the goal region with the minimum cost.

1 Introduction

Path planning is still a lively research field in robotics and autonomous vehicles, nowadays focusing on some peculiar aspects like the introduction of an accurate dynamic model of the agent, sometimes even including logical rules to govern multi-agent interaction, or a multi-objective cost function. Sampling based planners such as Rapidly-exploring Random Trees RRT [6] has enabled motion planning problem in high-dimensional spaces to be solved more efficiently. RRT incrementally samples the continuous state-space and returns a feasible solution as soon as a path from start to goal is obtained. RRT* has been introduced to tackle the optimal path planning problem and problems with complex task specifications [5]. This approach has been applied to many scenarios while guaranteeing asymptotic optimality. There are many variations of RRT*, such as, in [4] the authors used the same approach combined with a shooting method to obtain feasible but suboptimal solutions for any kino-dynamic system extending to systems with nonlinear dynamics, LQR-RRT* [9] locally linearizes the system in the presence of complex dynamics and applies the linear quadratic regulator. In T-RRT* [3] the evolution of the RRT* algorithm is guided by a transition function favoring the low-cost regions, conserving the asymptotic optimality. These innovative planning techniques can be applied to a context completely different from robotics, where the problem to be solved is finding a path in a complex environment a walking human can follow in order to safely reach a target. With the emergence of human-robot collaboration in the recent years, there is a substantial interest on how humans plan their paths. Among many approaches that cover the human-like path planning some focus especially on finding a cost function that results in a human-like path [1, 7, 10].

This work proposes a path planning algorithm for scenarios where an agent has to reach a goal while always keeping in contact with other humans in the surroundings using a SDR radio. This problem can be reformulated as a multi-objective path planning problem, where the path is selected optimizing a cost function including not only path length, safety, but also distance from the path to the other radios in the surroundings, and the motion of the agent is described using a dynamic model able to generate human-like walking paths. Considering each device can emit within a limited range, the agent must keep in the vicinity of at least one device at each point along the path in order to keep the connection alive. The proposed algorithm is based on RRT* and is composed by two steps. The first step sets up a specific network, based on the starting point of the agent, such that only the emitting devices that allow the motion to the target are included in the network. In the second step, RRT* algorithm is used to generate the desired path within the space defined by the network. Each edge of this path is determined by solving an optimal control problem where the differential constraint is represented by the kinematic model of a unicycle in the space domain, and the objective function has been selected in such a way that a human-like motion is generated [7]. Finally, the best path is selected minimizing an overall cost that includes the cost of the human-like motion and the distance from the signal emitting devices. The proposed path planning method is validated by simulation using a realistic scenario.

2 Proposed Approach

The main idea of the proposed path planning approach is to be able to generate a path that a walking human can follow. It has been shown that the motion of a human can be represented by a simple unicycle model [2]

$$\begin{aligned} {\left\{ \begin{array}{ll} \dot{x}=v\cos \theta \\ \dot{y}=v\sin \theta \\ \dot{\theta }=w \end{array}\right. } \end{aligned}$$

(1)

where x, y are the Cartesian coordinates of the agent defined as a point, $\theta$ is the orientation, v represents linear velocity along the motion while w is the angular velocity. In [7], on the other hand, the authors focus solely on the geometry of the path. In this work we follow this approach, considering the dynamic model with respect to the natural coordinate instead of time as follows

$$\begin{aligned} {\left\{ \begin{array}{ll} \acute{x}=\cos \theta \\ \acute{y}=\sin \theta \\ \acute{\theta }=\sigma \end{array}\right. } \end{aligned}$$

(2)

in which $\sigma =w/v$ is the control variable while $\acute{x},\acute{y},\acute{\theta }$ are the derivatives with respect to the natural coordinate s, taking $s(t)= \int _{0}^{t}v(\tau )d\tau$. For instance, $\acute{x}=dx/ds$.

2.1 Connectivity Zone

The problem considered in this work restricts the agent to move strictly inside the zones defined by the signal emitting bases. Considering the starting point and the ultimate goal region of the agent, a network could be formed preliminarily, restricting this space to the union of the bases that allow a motion from start to goal (Fig. 1(a-b)). Computing the distance from each point along the path to each of the landmarks defined in the network and finding the distance to the nearest landmark is computationally inefficient. Therefore, we represent the Cartesian space as a grid and compute the Distance Transform of this grid (Fig. 1c) with each cell representing the Euclidean distance from the nearest landmark

$$\begin{aligned} DT(\mathbf x )= \min _{LM_j \in LM_{con}} \left| | \mathbf x -LM_j \right| | \end{aligned}$$

where $LM_j$, $j=1,2 \dots n_{conn}$ and $\mathbf x$ represent the coordinates of the landmark and of each cell respectively. As a result computing the distance of the agent from the nearest landmark is converted into a look-up map.

Fig. 1.

Defining the connectivity zone (a) Signal emitting bases, with their emitting radii (b) Network formed using the bases that allow a motion that guarantees the connectivity condition (c) Distance transform computed on the discretized Cartesian space, darker color indicates lower cost (landmarks used are denoted with yellow color) (Color figure online)

2.2 Path Planning

The proposed approach for path planning is based on a sampling based algorithm RRT* [5]. The model defined in (2) is used to describe the motion. The node q is the state of system. Taking $q=[x,y,\theta ]^T$, such that we can define a set $Q=[x_{min},x_{max}]\times [y_{min},y_{max}]\times [\theta _{min},\theta _{max}]$ and $q \in Q$. Connection between two nodes is called an edge such that $e=(q,\tilde{q})$ and it represents the path obtained minimizing a cost function. Finally the cost that is assigned to an edge is denoted by C(e). The algorithm builds a tree $T=(Q,E)$ with the cardinality N defined by the user, where Q is a set of nodes, and the set E defines the collision-free edges that are inside the connectivity zone. Connectivity zone $Q_{conn}$ is defined as the union of signal emitting bases along the network and q is considered in this zone if the corresponding DT(x) is smaller than $r_{max}$, the maximum radius that a base can emit. Tree starts with an initial node $q_{0}=[x_0 y_0 \theta _0]^T \in Q$ , that defines the initial dynamics of the system, and follows with a sequence of steps¹:

• Random Sampling: The random state $q_{rand}$ is sampled from the space $Q_{free}=Q_{conn}\setminus Q_{obs}$ according to a uniform distribution. Where $Q_{obs}$ defines the space occupied by the obstacles.

• Nearest Nodes: Nodes that are in the vicinity of $q_{rand}$ are selected as the ones being in the radii of $\gamma _{ball}=\gamma _{RRT*}(log(n)/n)^{1/d}$ where n is the cardinality of the tree, $d=3$ is the dimension of the state space and $\gamma _{RRT*}$ is selected according to $\gamma _{RRT*} > 2 (1+1/d)^{1/d}(\mu ((Q_{free})/\zeta d)^{1/d})$ where $\mu (Q_{free})$ is the space dimension and $\zeta d$ is the unit ball volume.

• Steering: System is steered from the nearest nodes to $q_{rand}$ and the edge $e_{min}$ that gives out the minimum cost according to (4) is selected if all points along the edge lies in the connectivity zone and are collision free. As a result $q_{rand}$ and $e_{min}$ are added to the tree: $Q=q_{rand} \in Q$ and $E=e_{min}\in E$

• Rewiring: Every time a new node is added to the tree, it is rewired to check if an already existing node in the tree can be reached from this newly added node with a smaller cost.

• Termination and Best Path Selection: After the desired cardinality is reached tree incrementation terminates. For every node sequence $q_0, q_1, \dots , q_n$ that satisfies $q_i \in Q$ where $i=0,1,\dots ,n$ and $2 \le n \le N$, the cost is cumulative over the sequence such that $C(\rightarrow q_n) = \sum \limits _{0}^{n}C(e_i)$. At the end the best path is selected among the node sequences reaching the goal region, $q_n \in Q_{goal}$ with the minimum cost $C(\rightarrow q_n).$

Two nodes are connected minimizing a normalized hybrid energy/goal-based cost function that is proven to generate human-like paths [7]

$$\begin{aligned} J=\frac{1}{2}\int \limits _{0}^{S}\sigma ^2(1+\beta ^T \varGamma ^2)ds \end{aligned}$$

(3)

where $\beta ^T=[\beta _1 \beta _2]$ is a set of parameters and

$$\begin{aligned} (\varGamma ^2)^T=\left[ \frac{(x-x_{i+1})^2+(y-y_{i+1})^2}{(x_{i}-x_{i+1})^2+(y_{i}-y_{i+1})^2} \frac{(\theta -\theta _{i+1})^2}{(\theta -\theta _{i+1})} \right] \end{aligned}$$

$(x_i,y_i,\theta _i)=: \mathbf q _\mathbf i$ and $(x_{i+1},y_{i+1},\theta _{i+1}){=:} \mathbf q _\mathbf i+1$ being the initial and the final nodes to be connected. The cost function for generating the path can be interpreted as penalizing the control effort $\sigma$ with a space-varying weight. Such that, as the agent approaches the goal in the state space the weight on the control effort decreases, similar to how humans plan their path. Cost of each edge includes the cost of the human-like path and the cost of being away from the nearest landmark and is defined as follows

$$\begin{aligned} C(e_i)=c\left( 1+ \frac{\bar{d_i}}{r_{max}} \right) J_i^* \end{aligned}$$

(4)

where $J^*$ represents the optimal value of (3), $\bar{d}$ is the average of the distances from the nearest landmarks along the path computed, and c is a constant.

Fig. 2.

Experiments: allowed zone (cyan), tree nodes (yellow), tree edges (blue), best path (red), goal region (magenta square), landmarks (magenta), discarded landmarks (yellow), obstacles (green circles) (Color figure online)

3 Results

The proposed algorithm has been developed and tested in a MATLAB implementation. State limits are chosen to be $x \in [0,100],y\in [0,100],\theta \in [0,2\pi ]$. Parameters of the objective function are taken as $\beta =[7.55 0.27]$ as given in [7]. A virtual environment is used for verification that comprises 40 landmarks and 4 non-overlapping circular obstacles as represented in Fig. 2(a). We used GPOPS–II [8] software for MATLAB to solve the nonlinear optimization problem (3) for building the edges, taking model in (2) for differential constraints. Simulations are preformed for different numbers of cardinalities (Fig. 2(b-d)). Considering the minimum cost path obtained from a tree of 1500 vertices (Fig. 3) it is possible to see that, the best path selected that is designed to be human-like follows the network. Hence obtaining a human-like motion while maximizing the vicinity to the signal emitting bases.

Fig. 3.

(a) Planning experiment with a tree cardinality of 1500 nodes. (b) Optimal path (solid red) shown on the cost map of the distance from the landmarks (Color figure online)

4 Conclusion

This work presents a RRT-based path planning algorithm that is capable of generating paths that a human can follow while guaranteeing continuous connectivity in the environments populated with signal emitting bases. The approach can be used for various applications extending from search-rescue missions to military applications, where an agent is expected to move human-like and to be able to communicate with the surroundings.