Sage Journals: Discover world-class research

Abstract

Path planning under uncertainty in an unknown environment is an arduous task as the resulting map has inaccuracies and a safe path cannot always be found. A path planning method is proposed in unknown environments towards a known target position and under pose uncertainty. A limited range and limited field of view range sensor is considered and the robot pose can be inferred within certain bounds. Based on the sensor measurements a modified map is created to be used for the exploration and path planning processes, taking into account the uncertainty via the calculation of the guaranteed visibility and guaranteed sensed area, where safe navigation can be ensured regardless of the pose-error. A switching navigation function is used to initially explore the space towards the target position, and afterwards, when the target is discovered to navigate the robot towards it. Simulation results highlighting the efficiency of the proposed scheme are presented.

Keywords

Mobile robots guidance navigation and control exploration motion planning autonomous robotic systems

Introduction

Autonomous navigation of mobile robots is an area of research with increasing interest over the years.¹ Tasks such as area coverage (exploration),^2

–5 surveillance,⁶ search and rescue missions require that the robots move efficiently in the environment, avoiding obstacles during motion and keeping under consideration the robots’ physical constraints.

The majority of research on motion planning in the past few decades focused on known static environments,⁷ relying on principles such as the artificial potential fields,⁸ the vector field histogram,⁹ probabilistic roadmaps¹⁰ and rapidly exploring random trees (RRT).¹¹ In later years, the dynamic window approach¹² has emerged based on the necessity of navigating in dynamic¹³ or uncertain^14,15 environments, where most popular navigation methods can be inefficient.¹⁶ Navigation in this case is based on local real-time obstacle avoidance, where onboard sensors can provide information regarding the environment in the robot’s neighbourhood.¹⁷

While on entirely unknown environments, a similar approach can be employed¹⁸; the sensorial information can be utilized for an online map building process and the exploration process can be involved in the navigation. In classic exploration strategies,¹⁹ the robot is considered to move towards areas that provide new information about the environment, considering perfect knowledge of the position of the robot. One of the first methods is the frontier exploration method, where a frontier is the boundary between the explored and the unexplored space.

With the identification of the frontier candidates for exploration, a selection must be made. Yamauchi²⁰ in his work proposed movement closest to the robot frontier. The MinDist approach, as it is referred in the literature, has been similarly used in the works of Santosh et al.²¹ Gonzalez-Banos and Latombe²² instead proposed a cost function that involves the utility of a frontier. Similar approach has been presented by Burgard et al.²³ In both cases, the cost function produces a new target point on the selected frontier and a path planning—usually shortest path—method is used to guide the robot towards the selected point in the frontier. In Haumann et al.^24,25 instead of selecting a frontier, the authors propose a control law, where all candidate frontiers have an impact on the movement and the frontier exploration is coupled directly with the path planning.

While recent sensors, such as Light Detection and Ranging (LIDAR) and vision-based systems, provide accurate environment information measurements, pose information given by Simultaneous Localization and Mapping (SLAM)^26,27 techniques contain some uncertainty. Navigation becomes an issue, as the resulting map becomes inaccurate and this must be taken into account for the motion planning phase. Because of this, integrated exploration strategies have emerged, where effort is made to reduce the imposed uncertainty. Sim and Roy²⁸ proposed a method, where the uncertainty is reduced by evaluating the information gain of candidate’s future poses and selecting the optimal one. Similar stochastic method has appeared in the work of Vallvé and Andrade-Cetto.²⁹ These methods reduce the pose error at the expense of computational cost, as they require to compute possible future poses and find in the computed space the optimal one at each step. A much simpler solution for the reduction of the pose uncertainty is the relocalization of the robot either through loop closures³⁰ or revisiting known positions.³¹

In the aforementioned integrated exploration strategies, there are some inherent drawbacks. Most strategies calculate the new position in discrete space which is suboptimal³² and any local planners that are utilized to guide robots between path points³³ are not modified to account for the uncertainty resulting into control actions that might be unsafe when the robot moves close to obstacles.

The authors aim to provide a solution to the problem of navigating a mobile robot in an unknown environment with a known target position and under uncertainty. Here, the robot is equipped with a limited field of view and range sensor, whereas in other reported research efforts³⁴ an omnidirectional sensor was considered. The robot state is augmented with the orientation apart from the position, and pose uncertainty is introduced, that can be inferred within certain bounds. The approach utilized in this work expands the concept of the classic exploration, where instead of improving the SLAM procedure, the uncertainty is taken into account via the transformation of the sensor readings to create a modified map, where safe navigation of the robot can be ensured.

Specifically, the contribution lays within the calculation of the guaranteed visibility and from it the derivation of the guaranteed sensed area, subspaces of the initial instantaneous and aggregated sensed areas, respectively. While the target position is not within the guaranteed sensed area, the exploration phase occurs, where the maximization of a navigation function is utilized to guide the robot towards it through frontier-based exploration of the unexplored space. Frontier selection is done via the minimization of a cost function that involves the utility of the frontier towards the desired navigation and the vicinity of it to the robot. When the target position is discovered, the control law switches to a distance from target-based navigation function to reach it through a gradient ascend control law.

The article is structured as follows: (a) mathematical preliminaries are provided along with the problem formulation and the definitions and computations of the guaranteed visibility and guaranteed sensed area, followed by (b) the derivation of the control law, (c) simulation studies outlining the efficiency of the proposed method and (d) concluding remarks.

Problem Formulation

Mathematical Preliminaries

Consider a path-connected topological space $A \subset ℝ^{2}$ . The boundary of $A$ is denoted as $\partial A$ , ${B_{n}}$ , $n \in ℤ^{+}$ denotes a collection of n disjoint subspaces or ${B_{n}} ≜ \cup_{i = 1}^{n} B_{i}$ and for the given m-pair of points $a_{i}, b_{i} \in A$ , $i = 1, \dots, m$ , the collection of the m-line segments connecting a_i and b_i is denoted as ${a_{m}, b_{m}}$ . Spaces $A, B$ are considered disjoint if $A \cap B = \emptyset$ .

The Minkowski sum of two spaces $A, B$ can be defined as the space given by $A \oplus B = {a + b | a \in A, b \in B}$ , while the Minkowski difference can be defined as the space given by $A ⊖ B = {c \in C | c \oplus B \subseteq A}$ .

Given the collection of all paths {γ_k} that connect two arbitrary points $p_{1}, p_{2} \in A$ , the length of the shortest path defines the geodesic metric $d_{g} (p_{1}, p_{2})$ and the resulting path is called the geodesic path.

Definition 1

Let us consider $r \in A$ and a subspace $B \subseteq A$ as shown in Figure 1(a). Then the geodesic Hausdorff distance is defined as the minimum geodesic distance of all points $q \in B$ from r, that is

$H_{g} (r, B) ≜ min_{q \in B} d_{g} (r, q)$

Figure 1.

Path connected space (a) and visibility subspace from an arbitrary position (b).

Definition 2

Consider a point $r \in A$ , the visibility subspace of $A$ from r, shown in Figure 1(b), is defined as a subset $A^{v} (r; R)$ , containing all points q, so that the geodesic path connecting r and q is a straight line and has length less than or equal to R > 0, that is

\begin{array}{l} A^{v} (r; R) = {q \in A; d_{g} (r, q) = ∥ r - q ∥ \leq R, \\ \land r + λ (q - r) \in A, \forall λ \in [0, 1]} \end{array}

$I_{n \times m}$ and $O_{n \times m}$ denote the n × m identity and zero matrix, respectively, while $L [\partial A_{k}]$ denotes the length of the boundary segment $\partial A_{k}$ .

Problem Statement

Let a path connected space, $Ω \subset ℝ^{2}$ be the unknown area of interest. Let $x = {[\begin{matrix} r, & θ \end{matrix}]}^{T} = {[\begin{matrix} (r_{x}, & r_{y}), & θ \end{matrix}]}^{T}$ be the robot’s current state vector, where $r \in Ω$ and $θ \in ℝ$ be the position and orientation, respectively and p_t ∈ Ω be a goal position. The robot is equipped with a range sensor of circular sector pattern $C_{s} (r; θ; ψ)$ , with a sensing limit R and a field of view angle ψ, centered around its current heading, defined as the intersection of two semi-planes

C_{s} (r; θ; ψ) \overset{Δ}{=} r + {[\begin{matrix} - {tan}^{- 1} (θ + \frac{ψ}{2}) & 1 \\ {tan}^{- 1} (θ - \frac{ψ}{2}) & - 1 \end{matrix}] p \leq [\begin{matrix} 0 \\ 0 \end{matrix}]}

At any time instance, a sector visibility subspace

S (t) = Ω^{v} (r; R) \cap C_{s} (r; θ, ψ)

created by the range sensor is defined, while $A = \cup_{t} S (t) \subseteq Ω$ is the aggregated sensed area; it is apparent that $S \subseteq A$ .

The following kinodynamic robot model is assumed

[\begin{matrix} \dot{r} \\ \dot{θ} \end{matrix}] = [\begin{matrix} u \\ ω \end{matrix}], u \in ℝ^{2}, ω \in ℝ

a commonly used^4,35,36 simplified version of the Dubin’s car model that incorporates both the position and the orientation of a robot into the robot dynamics.

Under the assumption of noisy position and orientation measurements the robot’s state vector $\tilde{x} = {[\begin{matrix} \tilde{r}, & \tilde{θ} \end{matrix}]}^{T} = {[\begin{matrix} ({\tilde{r}}_{x}, & {\tilde{r}}_{y}), & \tilde{θ} \end{matrix}]}^{T}$ is assumed to be within a set $\tilde{E}$ defined as $\tilde{E} = x \oplus E$ (Figure 2) where

E = {x \in ℝ^{3} : [\begin{matrix} r_{x} & r_{y} \end{matrix}] [\begin{matrix} r_{x} \\ r_{y} \end{matrix}] \leq ε_{d}, | θ | \leq ε_{θ}}

Figure 2.

Visualization of the areas $E$ and $\tilde{E}$ .

A switching objective function is formulated, where subsets of spaces $S, (A)$ —namely, $\tilde{S}, (\tilde{A})$ —are computed that take into account the uncertainty and ensure safe robot operation.

Guaranteed visibility and guaranteed sensed area

The imposed uncertainty affects the navigation by incorrect estimation on the created global map of the sensed area boundaries which can be described by a collection of l disjoint segments, ${\partial S_{l}^{o}} \subset \partial Ω$ . To amend for the uncertainty, the aim is to define a new visibility subspace, called the guaranteed visibility $\tilde{S} \subseteq S$ —and consequently the guaranteed sensed area derived from this subspace $\tilde{A} \subseteq A$ —where safe navigation for the robot can be ensured. For this reason, initially given the collection ${\partial S_{l}^{o}}$ of the sensed area boundaries, and localization uncertainty, the boundary uncertainty space $C$ must be defined.

All range sensor measurements can be described in the local frame by a pair of polar coordinates $(d_{p}, ψ_{p})$ , d_p ∈ (0, R), $ψ_{p} \in [- \frac{ψ}{2}, \frac{ψ}{2}]$ . The sensed cloud of points, expressed in a global frame can be given by

\begin{matrix} p = \tilde{r} + [\begin{matrix} \cos \tilde{θ} & - \sin \tilde{θ} \\ \sin \tilde{θ} & \cos \tilde{θ} \end{matrix}] [\begin{matrix} d_{p} \cos ψ_{p} \\ d_{p} \sin ψ_{p} \end{matrix}] \\ = \tilde{r} + \tilde{R} (\tilde{θ}; d_{p}; ψ_{p}), \forall ψ_{p} \end{matrix}

Two additional spaces are introduced, namely, ${\tilde{E}}_{r}$ and ${\tilde{E}}_{θ}$ derived from projections of space $\tilde{E}$ .

\begin{array}{l} {\tilde{E}}_{r} = {\tilde{r} \in ℝ^{2} : ∥ \tilde{r} - r ∥ \leq ε_{d}} \\ {\tilde{E}}_{θ} = {\tilde{θ} \in [θ - ε_{θ}, θ + ε_{θ}]} \end{array}

The locus $C_{\tilde{θ}}$ of a sensor measurement (d_p, ψ_p) given orientation uncertainty can be given from:

C_{\tilde{θ}} = {\tilde{r} + \tilde{R} (\tilde{θ}; d_{p}; ψ_{p}) | \tilde{θ} \in {\tilde{E}}_{θ}}

It is apparent that the locus forms a circular arc around point $\tilde{r}$ and angle ε_θ. Lastly, considering the additional position uncertainty creates locus $C_{\tilde{r}}$ that can be calculated from

C_{\tilde{r}} = C_{\tilde{θ}} \oplus {\tilde{E}}_{r}

From equations (4)—and (6) the instantaneous visible uncertainty space $C^{o} (t)$ and the cumulative boundary uncertainty space $C (t)$ can be retrieved as

\begin{matrix} C^{o} (t) = \cup_{i = 1}^{l} \partial S_{i}^{o} \oplus C_{\tilde{r}} \\ C (t) = \underset{τ}{\cup} C^{o} (τ) \end{matrix}

It should be noted that while for the initial collection $\partial S_{i}^{o} \cap \partial S_{j}^{o} = \emptyset$ , i ≠ j, it may occur for some boundaries that $(\partial S_{i}^{o} \oplus C_{\tilde{r}}) \cap (\partial S_{j}^{o} \oplus C_{\tilde{r}}) \neq \emptyset$ , i ≠ j.

With the definition of $C$ , $\tilde{S}$ can be derived as the current sector visibility subspace of space $S \ C$ , that is:

\tilde{S} = {(S \ C)}^{v} (\tilde{r}; R - ε_{d}) \cap C_{s} (\tilde{r}; \tilde{θ}; ψ - 2 ε_{θ})

where R and ψ are reduced to R − ε_d and ψ − 2ε_θ to amend for the uncertainty.

The above process is summarized in Figure 3. In Figure 3(a) the initial sensed area $S$ with the sensed area boundaries ${\partial S_{l}^{o}}$ can be seen, in conjunction with a visualization of the position (red area) and orientation (green area) uncertainty. From ${\partial S_{l}^{o}}$ , four arbitrary points ${(d_{p}, ψ_{p})}_{i}$ , i = 1, …, 4 are selected for the calculation of spaces $C_{\tilde{θ}}$ and $C_{\tilde{r}}$ . In Figure 3(b) the resulting spaces $\partial S_{i}^{o} \oplus C_{\tilde{r}}$ , i = 1, …, 3 are seen, where it is apparent that $(\partial S_{2}^{o} \oplus C_{\tilde{r}}) \cap (\partial S_{3}^{o} \oplus C_{\tilde{r}}) \neq \emptyset$ .

Figure 3.

Visualization of the sensed space with the pose uncertainty (a), and boundary uncertainty space with the guaranteed visibility and the various boundaries (b).

The boundary $\partial \tilde{S}$ can be decomposed into four individual disjoint collections of segments (Figure 3(b)): (a) a collection of l-segments that belong to visible boundary uncertainty space ${\partial {\tilde{S}}_{l}^{o}} \subseteq \partial C$ , (b) a collection of k-circular arcs ${\partial {\tilde{S}}_{k}^{c}}$ created by the limited visibility range, (c) a collection of m-line segments ${\partial S_{m}^{ℓ}}$ created by visibility constraints that may be alternatively denoted as ${a_{m}, b_{m}}, ∥ a_{m} - \tilde{r} ∥ < ∥ b_{m} - \tilde{r} ∥$ and (d) two line segments ${\partial {\tilde{S}}_{2}^{v}}$ created by the limited field of view of the sensor, denoted as ${\tilde{r}, c_{2}}$ .

The cumulative guaranteed sensed area, $\tilde{A}$ , can be then derived as

\tilde{A} = [(\underset{τ}{\cup} \tilde{S} (τ)) \ C] \cup [(\underset{τ}{\cup} \tilde{S} (τ)) \cap \partial C]

Exploration and Navigation Objective

With the definition of $\tilde{S}$ the objective of the robot is to:

Maximize the following function

H (\tilde{x}; p_{t}) = \int_{\tilde{S}} f (p) φ (p) d p, i f p_{t} \notin \tilde{S}

during exploration phase,

2. Maximize the function,

H (\tilde{x}; p_{t}) = \frac{1}{∥ p_{t} - \tilde{r} ∥}, i f p_{t} \in \tilde{S}

during navigation to the goal position, where: $f (p) : \tilde{S} \to ℝ^{+}$ is the performance function and $φ (p) : \tilde{S} \to ℝ^{+}$ the weighting function.

These performance and navigation functions are spatially varying, and their selection navigates the robot closer to the target area.

Path Planning under Uncertainty

During the exploration phase, since the target has not been within the robot’s cumulative guaranteed sensed area $\tilde{A}$ , the robot attempts to move closer to it while at the same time exploring the unknown environment

Control Law Derivation

Theorem 1

Consider a robot with a sensing pattern of a circular sector with field of view angle ψ and range R, governed by its kinodynamics (equation (2)). If $p_{t} \notin \tilde{S}$ , the control law that maximizes in a monotonic manner the objective function shown in equation (10) is given by

\begin{array}{l} [\begin{matrix} u \\ ω \end{matrix}] = \sum_{i = 1}^{k} \int_{\partial S_{i}^{c}} f (p) φ (p) {\frac{\partial p}{\partial \tilde{x}}}^{T} |_{p \in \partial {\tilde{S}}_{i}^{c}} n d p \\ + \sum_{i = 1}^{m} \int_{0}^{1} f_{i}^{1} (υ) φ_{i}^{1} (υ) {\frac{\partial p}{\partial \tilde{x}}}^{T} |_{p \in \partial {\tilde{S}}_{i}^{ℓ}} υ n d υ \\ + \sum_{i = 1}^{2} \int_{0}^{1} f_{i}^{2} (υ) φ_{i}^{2} (υ) {\frac{\partial p}{\partial \tilde{x}}}^{T} |_{p \in \partial {\tilde{S}}_{i}^{v}} υ n d υ \end{array}

where $f_{i}^{1} = f (a_{i} + υ (b_{i} - a_{i}))$ , $φ_{i}^{1} = φ (a_{i} + υ (b_{i} - a_{i}))$ , i = 1, …, m, $f_{i}^{2} = f (\tilde{r} + υ (c_{i} - \tilde{r}))$ , $φ_{i}^{2} = φ (\tilde{r} + υ (c_{i} - \tilde{r}))$ , i = 1, 2 and

\begin{matrix} \frac{\partial p}{\partial \tilde{x}} |_{p \in \partial {\tilde{S}}_{i}^{c}} = [\begin{matrix} 1 & 0 & - R sin (ϕ_{i} + \tilde{θ}) \\ 0 & 1 & R cos (ϕ_{i} + \tilde{θ}) \end{matrix}] \\ \frac{\partial p}{\partial \tilde{x}} |_{p \in \partial {\tilde{S}}_{i}^{ℓ}} = [\begin{matrix} - \frac{∥ b_{i} - a_{i} ∥}{∥ \tilde{r} - a_{i} ∥} υ I_{2 \times 2} | O_{2 \times 1} \end{matrix}] \\ \frac{\partial p}{\partial \tilde{x}} |_{p \in \partial {\tilde{S}}_{i}^{v}} = [\begin{matrix} 0 & 0 & - υ ∥ \tilde{r} - c_{i} ∥ sin (ϕ_{i} + \tilde{θ}) \\ 0 & 0 & υ ∥ \tilde{r} - c_{i} ∥ cos (ϕ_{i} + \tilde{θ}) \end{matrix}] \end{matrix}

Proof

For the remainder of this proof, for notation simplicity, the arguments of functions f and φ will be omitted. By differentiating equation (10) with respect to $\tilde{x} = [\tilde{r}, \tilde{θ}]^{T}$ and using the Leibniz integral rule

\frac{\partial H}{\partial \tilde{x}} = \int_{\partial \tilde{S}} f φ {\frac{\partial p}{\partial \tilde{x}}}^{T} n d p

where n is the outward unit normal vector to $\partial \tilde{S}$ .

From the decomposition of the boundary $\partial \tilde{S}$ noted in the previous section, $\partial \tilde{S}$ can be written as

\partial \tilde{S} = \cup_{i = 1}^{l} \partial {\tilde{S}}_{i}^{o} + \cup_{i = 1}^{k} \partial {\tilde{S}}_{i}^{c} + \cup_{i = 1}^{m} \partial {\tilde{S}}_{i}^{ℓ} + \cup_{i = 1}^{2} \partial {\tilde{S}}_{i}^{v}

Equation (13) is thus transformed to

\begin{array}{l} \frac{\partial H}{\partial \tilde{x}} = \sum_{i = 1}^{l} \int_{\partial {\tilde{S}}_{i}^{o}} f φ {\frac{\partial p}{\partial \tilde{x}}}^{T} n d p + \sum_{i = 1}^{k} \int_{\partial {\tilde{S}}_{i}^{c}} f φ {\frac{\partial p}{\partial \tilde{x}}}^{T} n d p \\ + \sum_{i = 1}^{m} \int_{\partial {\tilde{S}}_{i}^{ℓ}} f φ {\frac{\partial p}{\partial \tilde{x}}}^{T} n d p + \sum_{i = 1}^{2} \int_{\partial {\tilde{S}}_{i}^{v}} f φ {\frac{\partial p}{\partial \tilde{x}}}^{T} n d p \end{array}

The Jacobian matrix $\partial p / \partial \tilde{x} = [\partial p / \partial \tilde{r} \partial p / \partial \tilde{θ}]$ is calculated for each term of equation (15). The first term is zero

\partial p / \partial \tilde{x} |_{p \in \partial {\tilde{S}}_{i}^{o}} = O_{2 x 3}

For the second term, $\partial p / \partial r |_{p \in \partial {\tilde{S}}_{i}^{c}}$ , it can be shown that any point laying on the i th circular arc can be given from

p = \tilde{r} + R [\begin{matrix} cos (ϕ_{i} + \tilde{θ}) \\ sin (ϕ_{i} + \tilde{θ}) \end{matrix}]

where ϕ_i is an angle parameter defining each point. Applying the above equation into the Jacobian yields

\frac{\partial p}{\partial \tilde{x}} |_{p \in \partial {\tilde{S}}^{c}} = [\begin{matrix} 1 & 0 & - R sin (ϕ_{i} + \tilde{θ}) \\ 0 & 1 & R cos (ϕ_{i} + \tilde{θ}) \end{matrix}]

For the third term, p can be expressed as

p = a_{i} + υ (b_{i} - a_{i}), υ \in [0, 1], i = 1, \dots, m

Since p is dependent only on the position of the robot $\tilde{r}$ and not on its orientation $\partial p / \partial \tilde{θ} = O_{2 x 1}$ . The term $\partial p / \partial \tilde{r}$ can be computed by differentiating equation (18) as

\frac{\partial p}{\partial \tilde{r}} = υ \frac{\partial b_{i}}{\partial \tilde{r}}, υ \in [0, 1]

Considering $\partial b_{i} / \partial \tilde{r}$ , infinitesimal movement of point $\tilde{r}$ will give point b_i a velocity ν_b that can be analysed into an angular component $ν_{b}^{a}$ created by a possible rotation of $\tilde{r}$ around point a_i and a translational component $ν_{b}^{t}$ along the direction of vector $\vec{a_{i} b_{i}}$ . The translational component $ν_{b}^{t}$ is neglected as the boundary is mainly affected by the rotational movement around a_i, which yields

\frac{\partial p}{\partial \tilde{r}} |_{\tilde{r} \in \partial {\tilde{S}}^{ℓ}} = - \frac{∥ b_{i} - a_{i} ∥}{∥ \tilde{r} - a_{i} ∥} υ I_{2 \times 2}

and the Jacobian can thus be given from

\frac{\partial p}{\partial \tilde{x}} |_{p \in \partial {\tilde{S}}^{ℓ}} = [\begin{matrix} - \frac{∥ b_{i} - a_{i} ∥}{∥ \tilde{r} - a_{i} ∥} υ I_{2 \times 2} & O_{2 \times 1} \end{matrix}]

For the fourth term similarly, p can be expressed as

p = \tilde{r} + υ ∥ \tilde{r} - c_{i} ∥ [\begin{matrix} cos (ϕ_{i} + \tilde{θ}) \\ sin (ϕ_{i} + \tilde{θ}) \end{matrix}], υ \in [0, 1], i = 1, 2

As mentioned, the two line segments ${\tilde{r}, c_{2}}$ are the limits caused by the reduced field of view of the sensor, while the position of the robot is the intersection point of these lines (the center of the robot coincides with the sensing origin). Unlike the case of ${\partial S_{m}^{ℓ}}$ , rotational movement of the robot about its axis would affect the boundary, as it would shift the field of view towards an unknown area. However, pure translation could either possibly result in no new gain of information (moving forward for example) or a backwards movement would risk the robot hitting an obstacle because of the sensing pattern. For this reason, the term $\partial p / \partial \tilde{r}$ is neglected and only the robot’s rotation is taken into account, leading to the Jacobian

\frac{\partial p}{\partial \tilde{x}} |_{p \in \partial {\tilde{S}}^{v}} = [\begin{matrix} 0 & 0 & - υ ∥ \tilde{r} - c_{i} ∥ sin (ϕ_{i} + \tilde{θ}) \\ 0 & 0 & υ ∥ \tilde{r} - c_{i} ∥ cos (ϕ_{i} + \tilde{θ}) \end{matrix}]

Summarizing the above analysis, equation (15) takes the form

\begin{array}{l} \frac{\partial H}{\partial \tilde{x}} = \sum_{i = 1}^{k} \int_{\partial S_{i}^{c}} f φ {\frac{\partial p}{\partial \tilde{x}}}^{T} |_{p \in \partial {\tilde{S}}^{c}} n d p \\ + \sum_{i = 1}^{m} \int_{0}^{1} f_{i}^{1} φ_{i}^{1} {\frac{\partial p}{\partial \tilde{x}}}^{T} |_{p \in \partial {\tilde{S}}^{ℓ}} υ n d υ \\ + \sum_{i = 1}^{2} \int_{0}^{1} f_{i}^{2} φ_{i}^{2} {\frac{\partial p}{\partial \tilde{x}}}^{T} |_{p \in \partial {\tilde{S}}^{v}} υ n d υ \end{array}

where $f_{i}^{1} = f (a_{i} + υ (b_{i} - a_{i}))$ , $ϕ_{1} = ϕ (a_{i} + υ (b_{i} - a_{i}))$ , $f_{i}^{2} = f (\tilde{r} + υ (c_{i} - \tilde{r}))$ , $ϕ_{2} = ϕ (\tilde{r} + υ (c_{i} - \tilde{r}))$ and the respective Jacobians are given in the from of equations (17), (19) and (20).

Using $\frac{\partial H}{\partial \tilde{x}}$ from equation (12), as the control input of the robot results to the monotonic maximization of equation (10), since

\frac{d H}{d t} = \frac{\partial H}{\partial \tilde{x}} \frac{d \tilde{x}}{d t} = {‖ \frac{\partial H}{\partial \tilde{x}} ‖}^{2} \geq 0 □

As mentioned in problem statement section, this control input is applied to the robot until the target area is discovered, at which point the control law switches to a navigation function based on the shortest distance to target, $d_{g} (\tilde{r}, p_{t})$ and the gradient descent law constructs the final segment of the path.

Exploration Frontier Selection

Having calculated the control law, functions f(p) and φ(p) should be selected in an intelligent manner in order to encapsulate the need not only to explore the area but also the preferred movement towards the target. As mentioned, the overall scheme is based on a frontier exploration method. For this reason a suitable cost function should initially be formulated for frontier exploration selection.

Boundary $\partial \tilde{A}$ is initially decomposed into, (a) part of the boundary uncertainty space, ${\partial {\tilde{A}}_{l}^{o}} \subseteq C$ , and free boundaries ${\partial {\tilde{A}}_{k}^{f}}$ . It should be noted that from the moment that $\tilde{A}$ is partly the aggregated union over time of $\tilde{S}$ , a single free boundary $\partial {\tilde{S}}_{k}^{f}$ can be any or a combination of the various boundaries as mentioned in control law derivation section. The various line segments (visibility constraints or field of view limits) having no intermediate physical interpretation could be either treated as frontiers (maximizing the overall potential frontiers for exploration) or parts of the boundary uncertainty space (a more conservative option). In this case they are treated as possible frontiers and the resulting the frontiers given by ${\partial {\tilde{A}}_{k}^{f}}$ .

The frontier selection scheme should take into account: (a) the proximity of the frontier to the target, (b) the proximity of the robot to the frontier and (c) the accessibility to new unexplored areas.

To implicate the proximity to target the introduction of the complimentary unexplored space $W$ , defined as

W = [ℝ^{2} \ (\tilde{A} \cup C)] \cup [\partial \tilde{A} \ \partial C]

that comprises a collection of simply connected disjoint subspaces. The frontier search is then limited to those frontiers that are boundaries of the disjoint subspace $W_{d} \subset W$ that contains the target. In Figure 4 an illustration is given, where spaces $\tilde{A}$ , $C$ and $W$ are seen. After the selection of $W_{d}$ , the frontier search is limited to frontiers $\partial {\tilde{A}}_{1}$ and $\partial {\tilde{A}}_{1}$ only, since $\partial {\tilde{A}}_{3} \subseteq W_{d}$ . The geodesic Hausdorff distance $H_{g} (p_{t}, \partial {\tilde{A}}_{k}^{f})$ of a frontier from the target within $W_{d}$ will be used. This distance given the existing information about the explored area relates with the distance the robot will need to traverse in the unknown area to reach the target. Furthermore in space $\tilde{A}$ the geodesic Hausdorff distance of the robot from a frontier $H_{g} (\tilde{r}, \partial {\tilde{A}}_{k}^{f})$ is calculated, which estimates the cost of moving towards a frontier. Lastly, frontier length is taken into account in the cost function which takes the following form

\begin{array}{l} \partial {\tilde{A}}_{c}^{f} = \underset{j}{arg min} (w_{1} L {[\partial {\tilde{A}}_{j}^{f}]}^{- 1} \\ + w_{2} H_{g} (p_{t}, \partial {\tilde{A}}_{j}^{f}) + w_{3} H_{g} (\tilde{r}, \partial {\tilde{A}}_{j}^{f})) \end{array}

where $w_{i} \in [0, 1], i = 1, 2, 3$ are weights assigned to each part of the cost function. It should be noted that equation (23) is evaluated constantly in conjunction with the control law.

Figure 4.

Visualization of areas $W$ , $\tilde{A}$ and the possible frontiers for exploration.

Performance and weighting functions selection

Performance function f(p) implicates the exploration process into the objective given by equation (10) and weighting function φ(p) implicates the navigation towards the desired position, while both the performance and weighting functions are selected so as to be area independent. The performance function will be defined as

f (p) = \frac{1}{H_{g} (p, \partial {\tilde{A}}_{c}^{f}) + 1}

This selection ensures that areas near the exploration frontier will be of greater importance than areas further away from it. By ignoring the weighting function, by intuition the robot would move towards the middle area of the frontier expanding it in a uniform manner. This alone could lead the robot to expand the neighbourhood of the frontier that will be further away from the target and thus potentially fail to reach it. To avoid this the weighting function φ(p) is defined as

φ (p) = \frac{1}{d_{g} (y, p_{t}) + 1}

y = \underset{y \in {\tilde{A}}_{c}^{f}}{arg min} H_{g} (p_{t}, \tilde{\partial} A_{c}^{f})

It must be noted that $d_{g} (y, p_{t})$ refers to space $W$ . This selection gives greater importance in neighbourhoods of $\partial A_{c}^{f}$ that are closer to the target than neighbourhoods further away from it. In this way, instead of expanding a frontier in a uniform manner, the frontier will be expanded towards the target area, thus guaranteeing the target’s discovery.

Simulation studies

The efficiency of the proposed scheme is verified through two different simulation scenarios. Two different areas for navigation were created that are depicted in Figure 5, where for visualization purposes the initial (green dot) and the target position (black dot) are illustrated.

Figure 5.

Ω-sample areas for navigation.

In the first scenario (Figure 5(a)) the rectangle encapsulating the convex hull of Ω is of 14 m × 12 m. The robot has a range sensor of R = 1.6 m and ψ = 1.047 rad, while the error bounds of equation (3) are given from ε_d = 0.05 m, ε_θ = 0.087 rad. At each time instant, the robot moves according to control law (12) with a maximum translational velocity of ν = 0.1 m/s and angular velocity of ω = 0.1 rad/s. The weights of equation (23) are selected as w₁ = 0.8, w₂ = 0.6 and w₃ = 0.4. Boundaries of $\tilde{C}$ and $\tilde{A}$ at each step are archived using an OctoMap³⁷ method with a grid resolution of 0.02 m.

In Figure 6, the evolution of the navigation towards the target area is seen, where the ‘light grey’ area depicts the unknown space, the guaranteed sensed area corresponds to ‘light blue’ and the boundary uncertainty space $C$ is depicted from the ‘dark grey’ area. Boundaries of the boundary uncertainty space $\partial C$ are depicted with black, while the frontiers are depicted in red, and blue depicts the selected frontier given from equation (23). As seen in Figure 6(a) and (b), despite the limited field of view of the sensor and the orientation of the robot, it is able to move efficiently in exploring the selected frontier. As seen in Figure 6(b)–(d), equation (23) is able to select the optimal frontier to explore and is capable of adapting to changes in the existing frontier. In Figure 6(e) the switching to the shortest path towards target takes effect as the target is within the explored space. As seen in Figure 6(f) the resulting path is sufficiently far from the boundary uncertainty space to account for safe and fast navigation without danger of collision, despite localization errors.

Figure 6.

Evolution of the robot navigation towards the target location with respect to the actual area [First scenario].

In the second scenario (Figure 5(b)) the area under investigation is of 14 m × 14 m. The robot’s range sensor is defined by R = 2 m and ψ = 1.4 rad, while the error defined by space (3) has parameters ε_d = 0.05 m, ε_θ = 0.175 rad. Maximum translational velocity of the robot is selected as ν = 0.2 m/s and maximum angular velocity as ω = 0.2 rad/s. The weights of equation (23) are kept the same as in the first scenario while the grid resolution is kept at 0.02 m. In Figure 7(a)–(f) the evolution of the navigation towards the target area is seen, where the colour coding is unchanged. It should be noted that the more clustered environment of this scenario and the larger bounded error in orientation results in significantly larger areas of $C$ . Despite this the robot is able to discover and reach the target position (Figure 7(f)) efficiently.

Figure 7.

Evolution of the robot navigation towards the target location with respect to the actual area [Second scenario].

Conclusions

In this article a novel method for navigation in unknown environments by a mobile robot with pose (position/orientation) uncertainty is presented. The robot is equipped with a ranged sensor with limited sensing range and field of view while its position/orientation measurements can be inferred within certain bounds. Taking into account a target location in the unknown area and the sensed boundaries, the robot proceeds to find the guaranteed visibility $\tilde{S}$ and guaranteed sensed area $\tilde{A}$ , where safe navigation is ensured, given the bounded localization error and the sensed boundaries of the area. Within that, it selects a suitable frontier for exploration via minimization of a cost function. A control law is implemented that moves the robot along the direction that maximizes an objective function that implicates the exploration towards the unknown area near the target. As soon as the target area is detected, the motion control law switches over to the shortest length navigation function. Simulation results that prove the efficiency of the proposed scheme are presented.

Footnotes

Authors’ note

A shorter version appeared in the proceedings of the IFAC 2017 World Congress.

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 work has received funding from the European Union Horizon 2020 Research and Innovation Programme under the Grant Agreement No. 644128, AEROWORKS.

ORCID iD

Ioannis Arvanitakis

Anthony Tzes

References

Garcia

Jimenez

De Santos

. The evolution of robotics research. IEEE, Robot Autom Mag 2007; 14(1): 90–103.

Stergiopoulos

Thanou

Tzes

. Distributed collaborative coverage-control schemes for non-convex domains. IEEE Trans Autom Control 2015; 60(9): 2422–2427.

Kantaros

Thanou

Tzes

. Distributed coverage control for concave areas by a heterogeneous robot-swarm with visibility sensing constraints. Automatica 2015; 53: 195–207.

Stergiopoulos

Tzes

. Spatially distributed area coverage optimisation in mobile robotic networks with arbitrary convex anisotropic patterns. Automatica 2013; 49(1): 232–237.

Hoy

Matveev

Savkin

. Algorithms for collision-free navigation of mobile robots in complex cluttered environments: a survey. Robotica 2015; 33(3): 463–497.

Galceran

Carreras

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

LaValle

. Planning Algorithms. Cambridge, UK: Cambridge University Press, 2006.

Khatib

. Real-time obstacle avoidance for manipulators and mobile robots. Int J Robot Res 1986; 5(1): 90–98.

Borenstein

Koren

. The vector field histogram-fast obstacle avoidance for mobile robots. IEEE Trans Robot Autom 1991; 7(3): 278–288.

10.

Kavraki

Svestka

Latombe

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

11.

Kuffner

LaValle

. RRT-connect: an efficient approach to single-query path planning. In: IEEE international conference on robotics and automation, Vol. 2, San Francisco, CA, USA, 24–28 April 2000, pp. 995–1001. IEEE.

12.

Brock

Khatib

. High-speed navigation using the global dynamic window approach. In: Hadi

(ed) IEEE International Conference on Robotics and Automation, Vol. 1, Detroit, MI, USA, 15–19 May 1999, pp. 341–346. IEEE.

13.

Kyriakopoulos

Saridis

. An integrated collision prediction and avoidance scheme for mobile robots in non-stationary environments. In: Giuseppe

(ed) Proceedings 1992 IEEE international conference on robotics and automation, Vol. 1, Nice, France, 12–14 May 1992, pp. 194–199. IEEE.

14.

Du Toit

Burdick

. Robot motion planning in dynamic, uncertain environments. IEEE Trans Robot 2012; 28(1): 101–115.

15.

Loizou

Tanner

Kumar

. Closed loop motion planning and control for mobile robots in uncertain environments. In: Douglas

A. Lawrence

(ed) 42nd IEEE international conference on decision and control, Vol. 3, Maui, HI, USA, 9–12 December 2003; pp. 2926–2931. IEEE.

16.

Valero-Gomez

Gomez

Garrido

. The path to efficiency: Fast marching method for safer, more efficient mobile robot trajectories. IEEE Robot Autom Mag 2013; 20(4): 111–120.

17.

Lopez-Padilla

Murrieta-Cid

Becerra

. Optimal navigation for a differential drive disc robot: a game against the polygonal environment. J Intell Robot Syst 2016; 89: 1–40.

18.

Tovar

Murrieta-Cid

LaValle

. Distance-optimal navigation in an unknown environment without sensing distances. IEEE Trans Robot 2007; 23(3): 506–518.

19.

Juliá

Gil

Reinoso

. A comparison of path planning strategies for autonomous exploration and mapping of unknown environments. Autonomous Robot 2012; 33(4): 427–444.

20.

Yamauchi

. A frontier-based approach for autonomous exploration. In: IEEE international symposium on computational intelligence in robotics and automation, Monterey, CA, USA, 10–11 July 1997, pp. 146–151. IEEE.

21.

Santosh

Achar

Jawahar

. Autonomous image-based exploration for mobile robot navigation. In: Stefan

Gaurav

S Sukhatme

(eds) IEEE international conference on robotics and automation, Pasadena, CA USA, 19–23 May 2008, pp. 2717–2722. IEEE.

22.

Gonzalez-Banos

Latombe

. Navigation strategies for exploring indoor environments. Int J Robot Res 2002; 21(10–11): 829–848.

23.

Burgard

Moors

Stachniss

. Coordinated multi-robot exploration. IEEE Trans Robot 2005; 21(3): 376–386.

24.

Haumann

Listmann

Willert

. Discoverage: a new paradigm for multi-robot exploration. In: Vijay

(ed) IEEE international conference on robotics and automation (ICRA), Anchorage, AK, USA, 3–8 May 2010, pp. 929–934. IEEE.

25.

Haumann

Breitenmoser

Willert

. Discoverage for non-convex environments with arbitrary obstacles. In: Yuan

F. Zheng

(ed) IEEE international conference on robotics and automation (ICRA), Shanghai, China, 9–13 May 2011, pp. 4486–4491. IEEE.

26.

Durrant-Whyte

Bailey

. Simultaneous localization and mapping: part I. IEEE Robot Autom Mag 2006; 13(2): 99–110.

27.

Bailey

Durrant-Whyte

. Simultaneous localization and mapping (SLAM): Part II. IEEE Robot Autom Mag 2006; 13(3): 108–117.

28.

Sim

Roy

. Global a-optimal robot exploration in SLAM. In: Rüdiger

(ed) Proceedings of the 2005 IEEE international conference on robotics and automation, Barcelona, Spain, 18–22 Apri 2005, pp. 661–666. IEEE.

29.

Vallvé

Andrade-Cetto

. Potential information fields for mobile robot exploration. Robot Auton Syst 2015; 69: 68–79.

30.

Stachniss

Grisetti

Burgard

. Information gain-based exploration using Rao-Blackwellized particle filters. In: Robotics: Science and systems, Vol. 2, 2005, pp. 65–72.

31.

Sim

Little

. Autonomous vision-based robotic exploration and mapping using hybrid maps and particle filters. In: Sebastian

(ed) Image Vis Comput, Vol. 27, Cambridge, MA, USA, 8–11 June 2009, pp. 167–177. IEEE.

32.

Indelman

Carlone

Dellaert

. Planning in the continuous domain: a generalized belief space approach for autonomous navigation in unknown environments. Int J Robot Res 2015; 34(7): 849–882.

33.

Valencia

Morta

Andrade-Cetto

. Planning reliable paths with pose SLAM. IEEE Trans Robot 2013; 29(4): 1050–1059.

34.

Arvanitakis

Giannousakis

Tzes

. Mobile robot navigation in unknown environment based on exploration principles. In: Mario

(ed) IEEE conference on control applications (CCA), Buenos Aires, Argentina, 19–22 September 2016, pp. 493–498. American Control Conference, 2008.

35.

Laventall . Coverage control by robotic networks with limited-range anisotropic sensory. American Control Conference, 2008, pp. 2666–2671. IEEE.

36.

Francis

Bruce A

Maggiore

. Flocking and rendezvous in distributed robotics 2016; Springer.

37.

Hornung

Wurm

Bennewitz

. Octomap: an efficient probabilistic 3d mapping framework based on octrees. Auton Robot 2013; 34(3): 189–206.

Synergistic exploration and navigation of mobile robots under pose uncertainty in unknown environments

Abstract

Keywords

Introduction

Problem Formulation

Mathematical Preliminaries

Definition 1

Definition 2

Problem Statement

Guaranteed visibility and guaranteed sensed area

Exploration and Navigation Objective

Path Planning under Uncertainty

Control Law Derivation

Theorem 1

Proof

Exploration Frontier Selection

Performance and weighting functions selection

Simulation studies

Conclusions

Footnotes

Authors’ note

Declaration of conflicting interests

Funding

ORCID iD

References