Path-Following and Collision-Avoidance Controls of a Robot in a Large Building with One or More Elevators

Abstract

For planning a robot’s path inside a large building with one or more elevators, we develop a topological map, called the building Voronoi graph. Using the building Voronoi graph, the robot finds the shortest path to the goal and follows the path. In the case where the robot detects an object with arbitrary shapes (e.g., human) while following the path, the robot avoids the object utilizing reactive control laws. The proposed reactive collision-avoidance control is unique in considering collision avoidance with map environments as well as (moving or static) objects having arbitrary shapes. As far as we know, our paper is novel in addressing how to make the robot follow the building Voronoi graph, while avoiding collision with map environments as well as objects with arbitrary shapes.

1. Introduction

This article addresses the path planing and control of a robot in a large building with one or more elevators. Suppose that the robot is ordered to visit another place (goal) in a large building. Once a goal point in the building is set, then the robot needs to move until it reaches the goal point. This scenario is important for the operation of many indoor robots, such as home robots [1,2], service robots [3,4] or nursing robots [5,6].

There may be a case where the goal is at a floor which is different from the floor where the robot is located. In order to handle this case, the robot needs to take an elevator. Humans effortlessly take an elevator, since elevators are designed for easy use by human operators. However, programming a robot to accomplish this task is surprisingly challenging [7].

Many papers [7,8,9] have addressed how to make a robot use an elevator. It is assumed that the robot can utilize elevators to move from one floor to another floor using the vision-based manipulating approach in [7,8]. How to make the robot operate the elevator buttons is not within the scope of our paper.

This paper considers the case where the robot moves in a known building environment. Therefore, the robot can locate itself using Monte–Carlo localization [10] or RFID tags [11,12]. By deploying multiple RFID tags in the building, one can localize the robot with an RFID antenna and RFID reader [12]. The references [11,12] showed that localization systems using RFID tags can achieve high estimation accuracy in real indoor scenarios. How to localize the robot in a known building is not within the scope of our paper.

In [13], the authors addressed how to produce a graph-based indoor network including floor-level and non-level paths from Industry Foundation Classes (IFC)-based building models. Ref. [13] required only the geometric information of IFC data models to automatically identify indoor space boundaries as well as to produce six categories of indoor paths. However, Ref. [13] did not address tracking controls of a robot based on the graph-based indoor network.

A Voronoi graph has been widely used in robotics [14,15,16,17,18,19,20]. A 2D Voronoi graph is a partition of the plane into regions close to each of a given set of sites. In the Voronoi graph, a Voronoi edge is the set of points which are equidistant from the two closest obstacles. According to this definition, a Voronoi graph provides a connected curve, which is safe and suitable for a robot path. In our paper, our robot uses a Voronoi graph as its path, since the graph provides the safest path which is the farthest from all obstacles.

For planning the robot’s path inside a large building with one or more elevators, this article addresses a topological map, called the building Voronoi graph. The building Voronoi graph consists of Voronoi vertices and Voronoi edges. The building Voronoi graph is generated as a contraction of a large building map, which is assumed to be known a priori.

In the building Voronoi graph, each floor is modeled utilizing the 2D Voronoi graph, and utilizing elevators is modeled as edges connecting distinct 2D Voronoi graphs. One sets the weight of an edge in the graph, based on the travel time inside an elevator and the traversal time of a passage. Using the building Voronoi graph, the robot can obtain a safe path to the goal.

Since the building Voronoi graph is a contraction of the entire building map (e.g, grid map), one can reduce the map size required for building exploration. Using the building Voronoi graph, one only needs to calculate the shortest path along the Voronoi graph, instead of the entire grid map. Therefore, one can improve the path-finding efficiency considerably, compared to the case where one searches for the shortest path utilizing all grid cells in the grid map.

In practice, the robot needs to use an elevator while moving to reach the goal. Moreover, the robot needs to spend some time while staying inside an elevator. One thus sets the weight of a Voronoi edge based on the travel time in an elevator and the traversal time of a passage. Using the weighted Voronoi graph, the robot finds the shortest path to the goal and follows the path.

This paper considers a scenario where the robot detects and identifies an object utilizing on-board sensors. For instance, the robot can detect a nearby object utilizing its camera or Lidar. For object identification in the camera image, one can utilize real-time object identification methods, such as you only look once (YOLO) [21,22,23] or Fast R-CNN [24]. For real-time object identification using Lidar, one can apply classification approaches in [25,26]. The classifier in [25,26] classifies objects into car or pedestrian. How to identify objects using on-board sensors is not within the scope of our paper. If the robot detects an object (e.g., a human) while following a Voronoi edge, then it avoids the object utilizing reactive control laws.

There are many papers on collision-avoidance control laws in 2D environments [27,28,29]. The authors of [27] addressed the dynamic window approach for collision avoidance. collision-avoidance controls in [27,28,29] did not consider the velocity of a moving object. In other words, collision-avoidance controls in [27,28,29] designed the robot’s path assuming that all obstacles are static. Hence, collision-avoidance controls in [27,28,29] may not be suitable for avoiding a fast moving object. In our paper, we develop a reactive collision-avoidance control considering the velocity of a moving object explicitly. This distinguishes our paper from other papers which did not consider moving obstacles.

For collision avoidance in the literature, obstacles were simplified as spheres or bounding boxes in [30,31,32,33]. However, simplifying an object shape is not desirable, especially in the case where an object has arbitrary shapes. Our article develops reactive collision-avoidance controls which do not simplify an object shape. As far as we know, our paper is novel in addressing reactive collision-avoidance controls, which can avoid objects with arbitrary shapes.

To the best of our knowledge, no paper in the literature handled how to make a robot follow the Voronoi graph while avoiding collision with other objects. Our paper integrates the proposed reactive collision-avoidance control with a path-tracking control, so that the robot can follow the Voronoi graph while avoiding collision with static map environments or (moving or static) objects with arbitrary shapes.

To the best of our knowledge, the novelties of our paper can be summarized as follows:

• This paper addresses how to make the robot follow the building Voronoi graph, while avoiding collision with map environments as well as (moving or static) objects with arbitrary shapes.
• This paper proposes a reactive collision-avoidance control, which achieves collision avoidance with map environments as well as (moving or static) objects with arbitrary shapes.

We verify the effectiveness of the proposed approaches utilizing MATLAB simulations.

This paper is organized as follows. Section 2 reviews the literature on robot path planning. Section 3 presents definitions and assumptions in this paper. Section 4 presents the path planner utilizing the building Voronoi graph. Section 5 introduces the reactive control for collision avoidance. Section 6 discusses how to make the robot follow the Voronoi path while avoiding collision. Section 7 provides MATLAB simulations to verify the effectiveness of the proposed approaches. Section 8 provides conclusions.

2. Literature Review on Robot Path Planning

Various papers exist on the path planning of robots. 3D path planning was introduced in [34], so that a robot can arrive at its goal as fast as possible, while not colliding with 3D obstacles in the environments. Many path-planning planning algorithms, such as RRT-star or RRT-star-smart [35,36,37], used random sampling to generate a path in environments without grid cells. Three-dimensional path planning is distinct from path planning in a building structure with elevators, which is handled in our paper.

In the literature on robotics, the A-star algorithm, Theta-star algorithm, Dijkstra algorithm, and Voronoi graph were widely used for path planning of various robots [38,39,40,41]. The A-star algorithm shows better performance than the Dijkstra algorithm by utilizing heuristics to guide its search towards its goal. The Theta-star algorithm in [38,42] is a variant of the A-star algorithm, propagating information along grid edges without constraining the paths to the grid edges. A-star, Theta-star, or Djkstra algorithms [39,40,41] require that the workspace is composed of multiple grid cells. In order to generate a Voronoi graph in grid maps, one can apply brushfire algorithms [43], which build the distance map at each grid cell.

However, as the workspace size increases, the number of grid cells required increases. As the building size increases, the required grid map size increases as well. Therefore, computing a path utilizing A-star, Theta-star, and brushfire algorithms, or Djkstra algorithms in a large building with many floors, may not be feasible due to the large computational load. Therefore, we argue that grid maps are not desirable in the case where one needs to build a path in a large building with many floors.

This inspired us to build a Voronoi graph for representing a large building with many floors. In Robotics, a Voronoi graph has been used as a topological map of a workspace [14,15,16,17,18,19,20]. In [44], multiple robots generate a Voronoi graph as a topological map of the environment, while dropping indexed markers at Voronoi vertices. The references [45,46,47] presented robot localization solutions as a robot explores a 2D environment by moving along Voronoi edges. In [45,46,47], a Voronoi graph is generated by a robot, which moves along Voronoi edges based on range sensor measurements. However, due to sensing errors or obstacles in the environment, the robot with range sensors may have difficulty in generating an accurate Voronoi graph of the environment.

In our paper, the building Voronoi graph is built based on a large building map (e.g., grid map), which is assumed to be known a priori. Hence, our Voronoi graph is robust to sensing errors or obstacles in the environment. This distinguishes our paper from other papers [45,46,47] on Voronoi graphs.

Voronoi path planning based on two steps was introduced in [48]. In the first step, the safest areas in the environment are extracted by means of a Voronoi graph. In the second step, a fast marching approach was applied to the Voronoi extracted areas in order to obtain the shortest path. In [49], the authors introduced an algorithm based on a Voronoi graph in order to compute an optimal path between a start and a goal in the presence of simple disjoint polygonal obstacles. However, in practice, obstacles can have various shapes, other than simple polygonal shapes. Our paper addresses the building Voronoi graph which is generated based on a large building map with arbitrary shapes.

In practice, the robot needs to use an elevator which yields the shortest path to the goal. Moreover, the robot needs to spend some time while staying inside an elevator. We thus set the weight of a Voronoi edge based on the travel time in an elevator and the traversal time of a passage.

We address path planning based on the building Voronoi graph, which represents a building structure with elevators. To the best of our knowledge, our paper is novel in the path planning and tracking control of a robot based on a building Voronoi graph. In addition, as far as we know, our paper is novel in addressing how to make the robot follow the Voronoi graph, while avoiding collision with other objects or static map environments. Moreover, our paper is unique in addressing reactive collision-avoidance controls, which can avoid objects with arbitrary shapes.

3. Definitions and Assumptions

3.1. Building Voronoi Graph

A graph G is defined as a set $G=\left(V\right(G),E(G\left)\right)$, where $V\left(G\right)$ is the vertex set and $E\left(G\right)$ is the edge set [50]. This article addresses the building Voronoi graph G, as a topological map of a building.

Consider a building with L floors. $\mathsf{\pi }:V\left(G\right)\to \{1,2,\dots ,L\}$ is an index, and the graph $G^{\mathsf{\pi }}=(V\left(G\right),E\left(G\right),\mathsf{\pi })$ implies that vertex i has index j if $\mathsf{\pi }\left(i\right)=j$. The indexing $\mathsf{\pi }$ induces a subgraph on G as follows: $G_i^{\mathsf{\pi }}=(V\left(G_i^{\mathsf{\pi }}\right),E\left(G_i^{\mathsf{\pi }}\right))\subset G$ where $V\left(G_i^{\mathsf{\pi }}\right)=\{j\in V\left(G\right)|\mathsf{\pi }\left(j\right)=i\}$ and $E\left(G_i^{\mathsf{\pi }}\right)=\{(j,k)\in E\left(G\right)|\mathsf{\pi }\left(j\right)=\mathsf{\pi }\left(k\right)=i\}$.

$G_i^{\mathsf{\pi }}$ is used as a graph of the i-th floor in the building. $G_i^{\mathsf{\pi }}$ is a 2D Voronoi graph, representing the topological map of the i-th floor.

Using elevators in the building, a robot can move from one floor to another floor. Therefore, one introduces a link which is associated with an elevator. A link is defined as follows. An edge $(i,j)\in E\left(G\right)$ is a link if $\mathsf{\pi }\left(i\right)\ne \mathsf{\pi }\left(j\right)$. Let $G_l\subset G$ define the graph indicating all links. $G_l=(V\left(G_l\right),E\left(G_l\right))$ where $V\left(G_l\right)=\{i\in V\left(G\right)|\exists j\in V\left(G\right)$ such that $(i,j)\in E\left(G\right)\mathrm{and}\mathsf{\pi }\left(i\right)\ne \mathsf{\pi }\left(j\right)\}$ and $E\left(G_l\right)=\{(i,j)\in E\left(G\right)|\mathsf{\pi }\left(i\right)\ne \mathsf{\pi }\left(j\right)\}$.

Figure 1 shows a building Voronoi graph associated with a building with three floors. In Figure 1, links are plotted with dotted curve segments. In Figure 1, there are two elevators. One elevator is represented with links to the left of the graph, and the other elevator is represented with links to the right of the graph.

3.2. Robot Motion and Obstacle Avoidance

Let $L(\mathbf{A},\mathbf{B})$ define the line segment whose two end points are $\mathbf{A}$ and $\mathbf{B}$, respectively. Considering an arbitrary angle $\mathsf{\psi }$, let the rotation matrix be defined as

$$\begin{array}{c}\hfill R(\mathsf{\psi })=\left(\begin{array}{cc}cos(\mathsf{\psi })& -sin(\mathsf{\psi })\\ sin(\mathsf{\psi })& cos(\mathsf{\psi })\end{array}\right).\end{array}$$

(1)

Let $\mathbf{q}\in {\mathcal{R}}^2$ define the 2D coordinate of the robot. Let $\mathbf{v}=\dot{\mathbf{q}}\in {\mathcal{R}}^2$ indicate the robot’s velocity vector.

This article handles discrete-time systems. Considering a variable $\mathbf{C}$, $\mathbf{C}\left(k\right)$ indicates $\mathbf{C}$ at sample-step k. For instance, the robot at sample-step k is located at $\mathbf{q}\left(k\right)\in {\mathcal{R}}^2$.

This manuscript considers a differential drive robot with two wheels (left wheel and right wheel). The robot’s motion model is

$$\begin{array}{c}\hfill \mathbf{q}(k+1)=\mathbf{q}\left(k\right)+\mathbf{v}\left(k\right)\ast dt,\end{array}$$

(2)

where $dt$ indicates the sample duration in discrete-time systems. By controlling the rotation speed of each wheel, one can set $\mathbf{v}\left(k\right)\in {\mathcal{R}}^2$.

The average speed of two wheels determines the forward speed $\parallel \mathbf{v}\left(k\right)\parallel$. Furthermore, the difference between the right wheel speed and the left wheel speed makes the robot change its heading angle. Detailed dynamic models of a differential drive robot appeared in [51].

Once a target speed and a target heading angle are set, two wheels rotate to converge to the target speed and the target heading. It is assumed that one can control the speed of each wheel, so that a target speed and a target heading can be reached within $dt$ seconds. The robot’s maximum speed is $s_{max}$, i.e., $\parallel \mathbf{v}\left(k\right)\parallel \le s_{max}$ for all k. This implies that the wheel speed cannot exceed $s_{max}$ due to the hardware limit of the robot.

This manuscript considers the case where the robot moves in a known map. Hence, the robot can locate itself using Monte—Carlo localization [10] or RFID tags [11,12]. How to localize the robot is not within the scope of this article. Since the robot can locate itself, it can access $\mathbf{q}\left(k\right)$ and $\mathbf{v}\left(k\right)$ in (2) at each sample-step k.

The robot detects and identifies an object utilizing on-board sensors. The robot identifies a nearby object utilizing its camera [21,22,23,24] and Lidar [25,26,52,53]. How to identify objects using on-board sensors is not within the scope of our paper.

Suppose that an object, such as a human, is detected by the robot. The robot measures 2D points on the object’s boundary utilizing on-board sensors. For instance, Lidar sensors can be used for this boundary measurement.

We consider an object with arbitrary shapes. As sensor rays detect an object’s boundary, multiple points on the object are detected by the rays. Every detected point is termed the objectBoundaryPoint $\mathbf{O}\in {\mathcal{R}}^2$.

The robot can measure an objectBoundaryPoint $\mathbf{O}\in {\mathcal{R}}^2$ and its velocity using its on-board sensors, such as Lidar [52,53]. Since the robot can locate itself, it can access $\mathbf{q}\left(k\right)\in {\mathcal{R}}^2$ and $\mathbf{v}\left(k\right)\in {\mathcal{R}}^2$ in (2) at each sample-step k. Therefore, the robot can access the position and velocity of an objectBoundaryPoint in real time. Let ${\mathbf{v}}_o\left(k\right)\in {\mathcal{R}}^2$ denote the velocity of the objectBoundaryPoint at sample-step k. Note that each objectBoundaryPoint $\mathbf{O}$ has its associated velocity ${\mathbf{v}}_o\left(k\right)$.

At sample-step k, the robot is in near-collision with an objectBoundaryPoint, in the case where the relative distance between the robot and the objectBoundaryPoint is less than a certain variable, say $r\left(k\right)>0$. Here, $r\left(k\right)$ represents the safety margin for the robot at sample-step k.

Note that $r\left(k\right)$ depends on sample-step k and is set considering the object’s speed $\parallel {\mathbf{v}}_o\left(k\right)\parallel$. $r\left(k\right)$ is set under

$$\begin{array}{c}\hfill r\left(k\right)=r_0+\mathsf{\mu }\ast \parallel {\mathbf{v}}_o\left(k\right)\parallel .\end{array}$$

(3)

Here, $r_0$ indicates the safety margin, in the case where an object’s speed is zero. Furthermore, $\mathsf{\mu }>0$ is a tuning parameter for weighting the effect of object speed. In the case where there is a fast object, $r\left(k\right)$ increases using (3). This implies that if there is a fast object, then the safety margin increases using (3).

4. Path Planner Utilizing a Building Voronoi Graph

This section presents the path planner utilizing a building Voronoi graph. First, one presents how to generate $G_i^{\mathsf{\pi }}$ for all $i\in \{1,2,\dots ,L\}$. Each floor is represented as a plane with obstacles.

Let mapPoints denote the 2D points associated with static obstacle boundaries in a known 2D map. Since the robot is localized in a known map, it can derive the relative position of a mapPoint while it moves. The robot needs to avoid colliding with mapPoints while it moves. Moreover, the robot needs to avoid colliding with objects, such as humans, in the building environment.

At every floor, map boundaries are represented as mapPoints in the workspace. For instance, in a grid map, a mapPoint can be a grid cell presenting the map. MapPoints can be obtained utilizing the map of each floor in the building. It is assumed that mapPoints are known a priori.

Using mapPoints at the i-th floor ($i\in \{1,2,\dots ,L\}$), we generate $G_i^{\mathsf{\pi }}$. Suppose that $N^i$ mapPoints exist at the i-th floor ($i\in \{1,2,\dots ,L\}$). Let ${\mathbf{O}}_n^i$ define the position of n-th mapPoint at the i-th floor. Here, $n\in \{1,2,\dots ,N^i\}$.

Suppose that one can access the map of every floor, representing mapPoints on the associated plane. At the i-th floor, one generates a Voronoi graph $G_i^{\mathsf{\pi }}$ as follows.

In MATLAB, the $voronoi$ function returns the set of Voronoi edges associated with a given seed points (mapPoints in this paper). The Voronoi edges are all points in the plane, which are equidistant to the two nearest seed points (mapPoints in this paper). A Voronoi vertex is a point where more than two Voronoi edges meet.

In the case where mapPoints are generated from the sensing measurements of a robot, the mapPoints may form a cluster. In the case where mapPoints are clustered, one may generate a Voronoi vertex which exists inside the clustered mapPoints.

See Figure 2 for an illustration. Five mapPoints exist in total. In this figure, the left three mapPoints are generated from the boundary of an obstacle, say A. Furthermore, the right two mapPoints are generated from the boundary of another obstacle, say B. In Figure 2, a curve segment indicates the Voronoi edge generated utilizing mapPoints. Furthermore, a circle indicates a Voronoi vertex. At the center of three mapPoints on the left side, there exists a Voronoi vertex. A Voronoi edge leading to this vertex cannot be utilized as a path for a robot, since this vertex exists on an obstacle boundary. One thus removes Voronoi vertices which are too close to mapPoints. Then, Voronoi edges leading to the removed vertices also disappear. In Figure 2, red bold curve segments indicate the remaining Voronoi edges, after removing Voronoi vertices which are too close to mapPoints.

For instance, we depict a map which is composed of mapPoints. In Figure 3, mapPoints are plotted with yellow points. Blue curve segments in this figure indicate the trimmed Voronoi graph, which is obtained after removing vertices that are too close to mapPoints. Figure 4 is the enlarged figure of Figure 3. See that mapPoints are clustered around map boundaries. In simulations, we removed a Voronoi vertex, whose distance from a mapPoint is less than a certain distance, say $\mathsf{\Delta }$ (5 m in MATLAB simulations).

Figure 3 shows that we still have Voronoi edges which are not necessary for robot path planning. In fact, we can trim a Voronoi edge which crosses the workspace boundary. In addition, we can trim a Voronoi vertex which exists outside the workspace. Furthermore, we can remove Voronoi edges which lead to workspace corners [54]. The authors of [54] considered the case where the robot builds a Voronoi diagram using range sensor measurements. In this case, edges leading to workspace corners can be easily detected using on-board sensors on the robot. However, in our paper, the Voronoi graph is built using the building map, not using range sensor measurements.

Trimming additional edges consumes the additional computational load. Considering computational load, we only removed a Voronoi vertex, whose distance from a mapPoint is less than $\mathsf{\Delta }$.

It is acknowledged that a Voronoi graph does not provide the shortest path between two points. This article uses the Voronoi graph as the robot’s path, since it provides the safest path for the robot. Any Voronoi edge in the workspace can be used as the robot’s path segment.

At the i-th floor ($\forall i\in \{1,2,\dots ,L\}$), we generate a trimmed Voronoi graph and set it as the topological map $G_i^{\mathsf{\pi }}$. Each edge in the trimmed Voronoi graph is weighted using its edge length and its associated passage width.

Consider the trimmed Voronoi graph on the i-th floor. Suppose that a Voronoi edge connects between two vertices, say $V_m$ and $V_n$. We next present how to set the weight of this edge. Let ${\mathbf{V}}_m\in {\mathcal{R}}^2$ and ${\mathbf{V}}_n\in {\mathcal{R}}^2$ define the positions of $V_m$ and $V_n$, respectively. The length of this edge is $\parallel {\mathbf{V}}_m-{\mathbf{V}}_n\parallel$. The traversal time of this edge is conjectured as $\frac{\parallel {\mathbf{V}}_m-{\mathbf{V}}_n\parallel }{s_{max}}$. Recall that the robot’s maximum speed is $s_{max}$, i.e., $\parallel \mathbf{v}\left(k\right)\parallel \le s_{max}$ for all k. Therefore, the weight of an edge $e=(V_m,V_n)$ is set as

$$\begin{array}{c}\hfill weight\left(e\right)=\frac{\parallel {\mathbf{V}}_m-{\mathbf{V}}_n\parallel }{s_{max}}.\end{array}$$

(4)

See that this weight is set considering the time required to traverse the edge, as the robot moves with its maximum speed $s_{max}$.

We further generate a link edge connecting between distinct floors. Recall that a robot can move from one floor to another utilizing elevators. We generate a Voronoi vertex at a workspace position where an elevator exists.

Recall that a Voronoi edge $(i,j)\in E\left(G\right)$ is a link if $\mathsf{\pi }\left(i\right)\ne \mathsf{\pi }\left(j\right)$. The weight of a link is set considering the travel time inside the associated elevator. Suppose that a link is $(i,j)\in E\left(G_l\right)$. Then, we set the weight of this link as $\delta \ast \parallel i-j\parallel$. Here, $\delta >0$ is a constant. The weight of a link is set, considering the fact that as the floor difference between two floors increases, it takes more time to make the elevator move between the two floors.

Once we obtain all weighted edges in the building Voronoi graph, we then generate the adjacency graph based on the weighted edges. We search for a vertex which is closest to the robot’s initial position, and set the vertex as $V_s$. In addition, we search for a vertex which is closest to the goal point, and set the vertex as $V_t$.

We obtain the shortest path from $V_s$ to $V_t$ utilizing the adjacency graph. For finding the shortest path in a graph, Dijkstra’s algorithms [55] can be used. The building Voronoi graph is generated considering the entire building, including all elevators. Thus, $V_s$ and $V_t$ can exist on distinct floors. If necessary, the robot uses an elevator, which can lead to the goal as fast as possible.

Let $P_V=(V^1,V^2,..V^Q)$ define the sequence of Voronoi vertices along the shortest path from $V_s$ to $V_t$. Here, $V^1=V_s$, and $V^Q=V_t$. In total, Q vertices exist along this path.

Note that we obtain the shortest path along the building Voronoi graph, instead of the entire workspace (e.g., grid map). Therefore, one can improve the path-finding efficiency considerably, compared to the case where one searches for the shortest path utilizing all cells in the workspace. Since the building Voronoi graph is a contraction of the entire building map (e.g., grid map), one can reduce the map size required for building exploration.

Once the Voronoi path $P_V$ is built, the robot follows the path until it reaches $V_t$. How to make the robot follow $P_V$ is discussed in Section 6.

5. Reactive Control for Collision Avoidance

While the robot moves along $P_V$, it needs to avoid colliding with objects, such as humans. Suppose the robot detects an object’s surface utilizing local ranging sensors, such as Lidar. Let $r_m$ denote the maximum sensing range of the local sensors. Recall that the robot measures objectBoundaryPoints on the surface of an object with arbitrary shapes. One introduces a reactive collision-avoidance control, which makes the robot avoid colliding with every objectBoundaryPoint.

5.1. Collision Prediction within $k_p$ Sample-Steps in the Future

The robot’s velocity command at each sample-step is set for avoiding a collision with an object detected at the moment. In other words, the robot’s velocity command is set at every sample-step.

Suppose that the robot’s velocity vector at sample-step k is $\mathbf{v}\in {\mathcal{R}}^2$. We address how to evade the situation where the robot and an objectBoundaryPoint, say $\mathbf{O}\in {\mathcal{R}}^2$, collide between sample-step k and $k+k_p$. Here, $k_p>0$ indicates the prediction window size, set by the robot’s operator.

${\mathbf{v}}_o\left(k\right)\in {\mathcal{R}}^2$ denotes the velocity of $\mathbf{O}\in {\mathcal{R}}^2$ at sample-step k. In the collision prediction, one assumes that $\mathbf{O}$ maintains its velocity as ${\mathbf{v}}_o\left(k\right)$ between sample-step k and $k+k_p$.

This assumption is required, since the robot at sample-step k needs to set its velocity vector, considering the predicted motion of an object. This assumption is trivial as we set $k_p=1$. However, $k_p=1$ may lead to collision of the robot, since the prediction window size is too short.

Let objectCircle define the circle with radius $r\left(k\right)$, centered at an objectBoundaryPoint $\mathbf{O}\left(k\right)\in {\mathcal{R}}^2$. Here, $r\left(k\right)$ denotes the safety margin and is defined in (3). Note that $r\left(k\right)$ is set considering $\parallel {\mathbf{v}}_o\left(k\right)\parallel$.

No collision happens between sample-step k and $k+k_p$ if

$$\begin{array}{c}\hfill \parallel (\mathbf{q}\left(k\right)+\mathbf{v}\ast u\ast dt)-(\mathbf{O}\left(k\right)+{\mathbf{v}}_o\left(k\right)\ast u\ast dt)\parallel >r\left(k\right)\end{array}$$

(5)

for all $0\le u\le k_p$. Rearranging (5), we obtain

$$\begin{array}{c}\hfill \parallel \mathbf{q}\left(k\right)+(\mathbf{v}-{\mathbf{v}}_o\left(k\right))\ast u\ast dt-\mathbf{O}\left(k\right)\parallel >r\left(k\right)\end{array}$$

(6)

for all $0\le u\le k_p$. As one increases u in (6) from 0 to $k_p$, $\mathbf{q}\left(k\right)+(\mathbf{v}-{\mathbf{v}}_o\left(k\right))\ast u\ast dt$ generates the straight line segment $L(\mathbf{q}\left(k\right),\mathbf{q}\left(k\right)+(\mathbf{v}-{\mathbf{v}}_o\left(k\right))\ast k_p\ast dt)$.

Utilizing (6), Lemma 1 follows.

Lemma 1.

The robot’s velocity vector is $\mathsf{v}\mathsf{\in }{\mathcal{R}}^2$. Assume that an objectBoundaryPoint maintains its velocity as ${\mathsf{v}}_o\left(k\right)\in {\mathcal{R}}^2$ between sample-step k and $k+k_p$. Assume that $\mathsf{q}\left(k\right)\in {\mathcal{R}}^2$ is outside the objectCircle centered at $\mathsf{O}\left(k\right)$. Between sample-step k and $k+k_p$, the robot does not collide with the objectBoundaryPoint, as long as $L(\mathsf{q}\left(k\right),\mathsf{q}\left(k\right)+(\mathsf{v}-{\mathsf{v}}_o\left(k\right))\ast k_p\ast dt)$ does not cross the objectCircle centered at $\mathsf{O}\left(k\right)$.

Under Lemma 1, the robot can anticipate a collision within $k_p$ sample-steps in the future. The robot examines if its velocity vector $\mathbf{v}$ meets the following safety requirement: $L(\mathit{q}\left(k\right),\mathit{q}\left(k\right)+(\mathit{v}-{\mathit{v}}_o\left(k\right))\ast k_p\ast dt)$ does not cross the objectCircle, centered at $\mathit{O}\left(k\right)$.

If $L(\mathbf{q}\left(k\right),\mathbf{q}\left(k\right)+(\mathbf{v}-{\mathbf{v}}_o\left(k\right))\ast k_p\ast dt)$ does not cross the objectCircle centered at $\mathbf{O}\left(k\right)$, then this implies that the robot’s velocity vector $\mathbf{v}$ avoids collision within $k_p$ sample-steps in the future.

If $L(\mathbf{q}\left(k\right),\mathbf{q}\left(k\right)+(\mathbf{v}-{\mathbf{v}}_o\left(k\right))\ast k_p\ast dt)$ does not cross the objectCircle centered at $\mathbf{O}\left(k\right)$, then the robot utilizes $\mathbf{v}$ as the velocity direction for a single sample duration. Note that the robot’s velocity vector is reset at every sample-step.

For collision avoidance between sample-step k and $k+k_p$, Lemma 1 uses the following assumption: Between sample-step k and $k+k_p$, an objectBoundaryPoint maintains its velocity as ${\mathsf{v}}_o\left(k\right)\in {\mathcal{R}}^2$.

This assumption implies that an objectBoundaryPoint maintains its velocity as ${\mathbf{v}}_o\left(k\right)\in {\mathcal{R}}^2$ for $k_p\ast dt$ seconds. As the sampling interval $dt$ decreases, $k_p\ast dt$ decreases. Thus, the above assumption becomes easier to be satisfied. Hence, it is desirable that $dt$ is as small as possible for satisfying the assumption in Lemma 1.

5.2. Find a Safe Velocity Vector of the Robot

If $L(\mathbf{q}\left(k\right),\mathbf{q}\left(k\right)+(\mathbf{v}-{\mathbf{v}}_o\left(k\right))\ast k_p\ast dt)$ crosses the objectCircle centered at $\mathbf{O}\left(k\right)$, then the robot must calculate a safe velocity vector. Algorithm 1 is used to make the robot avoid colliding with objects as well as static map environments. Let $obsBoundaryPointSet$ denote the set of objectBoundaryPoints measured at sample-step k. Furthermore, let $mapPointSet$ denote the set of mapPoints measured at sample-step k. Recall that mapPoints denote the 2D points associated with static obstacle boundaries in a known 2D map.

Let $PointSet$ denote the union of $obsBoundaryPointSet$ and $mapPointSet$. Let $detectedPoint$ indicate a point in $PointSet$.

As the safety requirement is not satisfied for at least one $detectedPoint\in PointSet$, the robot under Algorithm 1 calculates its velocity vector satisfying the safety requirement for every $detectedPoint\in PointSet$. Once a velocity direction which satisfies the safety requirement is found, then the robot utilizes the found velocity vector as its velocity for a single sample-step.

In Algorithm 1, $colNum=col({\mathbf{v}}_c,PointSet)$ returns the number of $detectedPoints\in PointSet$ colliding with a candidate heading vector ${\mathbf{v}}_c$. For collision prediction within $k_p$ sample-steps in the future, we use the safety requirement. If $colNum==0$, then Lemma 1 proved that moving with velocity ${\mathbf{v}}_c$ assures collision with every $detectedPoint\in PointSet$ within $k_p$ sample-steps in the future. Thus, if $colNum==0$, then one can obtain a safe velocity vector avoiding collision with every $detectedPoint\in PointSet$. Therefore, one sets ${\mathbf{v}}_c$ as the robot’s velocity vector at sample-step $k+1$.

On the other hand, if $colNum\ne 0$, then one cannot derive a safe velocity vector which evades colliding with every $detectedPoint\in PointSet$ within $k_p$ sample-steps in the future. In this situation, one finds a velocity vector with the minimum collision probability by finding a heading vector which collides with the minimum number of $detectedPoints$.

Algorithm 1 shows how to obtain a safe velocity vector at sample-step $k+1$. Let $v_0$ denote a safe velocity vector at sample-step $k+1$. Let $\mathsf{\psi }\left(k\right)$ define the robot’s heading angle at sample-step k. Under (2), one derives

$$\begin{array}{c}\hfill \mathbf{v}\left(k\right)={[cos(\mathsf{\psi }\left(k\right))\ast \parallel \mathbf{v}\left(k\right)\parallel ,sin(\mathsf{\psi }\left(k\right))\ast \parallel \mathbf{v}\left(k\right)\parallel ]}^T.\end{array}$$

(7)

Recall that the robot’s speed is limited by $s_{max}$. Therefore, $\parallel \mathbf{v}(k+1)\parallel$ is limited as

$$\begin{array}{c}\hfill 0\le \parallel \mathbf{v}(k+1)\parallel \le s_{max}.\end{array}$$

(8)

Algorithm 1 finds a safe speed utilizing the following FOR loop: for $v_0=0$, $v_0=v_0+\frac{s_{max}}{N}$, while $v_0\le s_{max}$. Here, N is a tuning parameter indicating the search precision. As one increases N, one can increase the precision of the speed search. However, increasing N increases the computational load. In simulations, we use $N=10$. This implies that the speed search is performed at $N=10$ distinct speeds.

In Algorithm 1, the FOR statement indicates that a velocity vector with low speed ($v_0=0$) is examined before examining a velocity vector with high speed ($v_0=s_{max}$). In this way, the robot does not accelerate abruptly. This is desirable considering the human safety in the building.

Algorithm 1 Find a safe velocity vector at sample-step $k+1$

1: The current sample-step is k;  2: $obsBoundaryPointSet\leftarrow$ the set of objectBoundaryPoints measured at sample-step k;  3: $mapPointSet\leftarrow$ the set of mapPoints measured at sample-step k;  4: $PointSet=obsBoundaryPointSet\bigcup mapPointSet$;  5: if $col\left(\mathbf{v}\right(k),PointSet)$==0 then  6:    Set $\mathbf{v}\left(k\right)$ as the robot’s velocity vector at sample-step $k+1$;  7:    Return $\mathbf{v}\left(k\right)$, and this algorithm is finished;  8: end if  9: $minPoints=\infty$; 10: for $v_0=0$, $v_0=v_0+\frac{s_{max}}{N}$, while $v_0\le s_{max}$do 11:    for ${\mathsf{\psi }}_0=0$, ${\mathsf{\psi }}_0={\mathsf{\psi }}_0+\frac{\mathsf{\pi }}{N}$, while ${\mathsf{\psi }}_0<\mathsf{\pi }$ do 12:      ${\mathbf{v}}_c^1=R\left({\mathsf{\psi }}_0\right)\ast \mathbf{v}\left(k\right)$; 13:      $colNu{m}_1=col({\mathbf{v}}_c^1,PointSet)$; 14:      if $colNu{m}_1==0$; then 15:         Set ${\mathbf{v}}_c^1$ as the robot’s velocity vector at sample-step $k+1$; 16:         Return ${\mathbf{v}}_c^1$, and this algorithm is finished; 17:      end if 18:      if $colNu{m}_1<minPoints$; then 19:         $minPsi={\mathsf{\psi }}_0$; $minV=v_0$; $minPoints=colNu{m}_1$; 20:      end if 21:    end for 22:    for ${\mathsf{\psi }}_0=-\frac{\mathsf{\pi }}{N}$, ${\mathsf{\psi }}_0={\mathsf{\psi }}_0-\frac{\mathsf{\pi }}{N}$, while ${\mathsf{\psi }}_0>-\mathsf{\pi }$ do 23:      ${\mathbf{v}}_c^2=R\left({\mathsf{\psi }}_0\right)\ast \mathbf{v}\left(k\right)$; 24:      $colNu{m}_2=col({\mathbf{v}}_c^2,PointSet)$; 25:      if $colNu{m}_2==0$; then 26:         Set ${\mathbf{v}}_c^2$ as the robot’s velocity vector at sample-step $k+1$; 27:         Return ${\mathbf{v}}_c^2$, and this algorithm is finished; 28:      end if 29:      if $colNu{m}_2<minPoints$; then 30:         $minPsi=-{\mathsf{\psi }}_0$; $minV=v_0$; $minPoints=colNu{m}_2$; 31:      end if 32:    end for 33: end for 34: ${\mathbf{v}}_c=R\left(minPsi\right)\ast minV\ast {[1,0]}^T$; 35: Return ${\mathbf{v}}_c$;

Algorithm 1 finds a safe heading angle, utilizing the following FOR loop: FOR ${\mathsf{\psi }}_0=0$, ${\mathsf{\psi }}_0={\mathsf{\psi }}_0+\frac{\mathsf{\pi }}{N}$, while ${\mathsf{\psi }}_0<\mathsf{\pi }$. This FOR loop implies that a safe heading angle is found in the clockwise direction starting from the robot’s heading. Once a safe heading angle is not derived using this clockwise direction search, then Algorithm 1 finds a safe heading angle utilizing the following FOR loop: FOR ${\mathsf{\psi }}_0=-\frac{\mathsf{\pi }}{N}$, ${\mathsf{\psi }}_0={\mathsf{\psi }}_0-\frac{\mathsf{\pi }}{N}$, while ${\mathsf{\psi }}_0>-\mathsf{\pi }$. This FOR loop implies that a safe heading angle is found in the counter clockwise direction starting from the robot’s heading.

If a candidate heading vector ${\mathbf{v}}_c\in {\mathcal{R}}^2$ and every $detectedPoint\in PointSet$ satisfies the safety requirement, then the candidate heading vector ${\mathbf{v}}_c$ is a safe velocity vector which evades collision. Therefore, one sets ${\mathbf{v}}_c$ as the robot’s velocity vector at sample-step $k+1$, and Algorithm1 is finished.

In Algorithm 2, $maxColNum$ is set considering the computational load of the algorithm. By setting $maxColNum$ as a small number, one does not have to examine the safety requirement for every $detectedPoint$ in $PointSet$. In simulations, one uses $maxColNum=100$.

Algorithm 2$col({\mathbf{v}}_c,PointSet)$

1: $colNum=0$;  2: for  $detectedPoint\in PointSet$do  3:    if ${\mathbf{v}}_c$ and $detectedPoint$ do not satisfy the safety requirement; then  4:      $colNum=colNum+1$;  5:    end if  6:    if $colNum>maxColNum$ then  7:      break;  8:    end if  9: end for 10: Return $colNum$;

6. Path-Following Control Laws While Avoiding Collisions

This section discusses how to make the robot follow the Voronoi path $P_V=(V^1,V^2,∆V^Q)$ while avoiding collision with other objects or static map environments. Before presenting the control laws, one discusses how to make the robot approach a wayPoint, while keeping away from collision.

6.1. Control Laws for Approaching a wayPoint

One discusses how to set the robot’s velocity vector $\mathbf{v}\left(k\right)\in {\mathcal{R}}^2$ so that it approaches a wayPoint, while keeping away from colliding with other objects or static map environments. Let $\mathbf{W}\in {\mathcal{R}}^2$ define the robot’s wayPoint, so that the robot arrives at the wayPoint within one sample-step. Under (2), one defines $v\left(k\right)=\parallel \mathbf{v}\left(k\right)\parallel$.

The robot’s desired heading direction is set as

$$\begin{array}{c}\hfill \mathbf{h}=\frac{\mathbf{W}-\mathbf{q}\left(k\right)}{\parallel \mathbf{W}-\mathbf{q}\left(k\right)\parallel }.\end{array}$$

(9)

In order to arrive at $\mathbf{W}$ at sample-step $k+1$, the desired speed of the robot is

$$\begin{array}{c}\hfill v_D=\frac{\parallel \mathbf{W}-\mathbf{q}\left(k\right)\parallel }{dt}.\end{array}$$

(10)

In the case where

$$\begin{array}{c}\hfill \frac{\parallel \mathbf{W}-\mathbf{q}\left(k\right)\parallel }{dt}<s_{max},\end{array}$$

(11)

$\mathbf{W}\in {\mathcal{R}}^2$ is reachable within one sample-step.

In the case where (11) is satisfied, the robot sets its speed to $v_D=\frac{\parallel \mathbf{W}-\mathbf{q}\left(k\right)\parallel }{dt}$. In the case where (11) is not satisfied, the robot increases its speed to its maximum speed $s_{max}$. Hence, the robot’s velocity vector ($\mathbf{v}\left(k\right)$ in (2)) is set as

$$\begin{array}{c}\hfill {\mathbf{v}}_g=\frac{min(v_D,s_{max})\ast (\mathbf{W}-\mathbf{q}\left(k\right))}{\parallel \mathbf{W}-\mathbf{q}\left(k\right)\parallel }.\end{array}$$

(12)

Using ${\mathbf{v}}_g$ in (12), the robot approaches the wayPoint. In (12), $min(v_D,s_{max})$ presents the speed command of the robot. Furthermore, $\frac{(\mathbf{W}-\mathbf{q}(k\left)\right)}{\parallel \mathbf{W}-\mathbf{q}\left(k\right)\parallel }$ presents the heading command of the robot.

In the case where ${\mathbf{v}}_g$ in (12) satisfies the safety requirement in Section 5.1, ${\mathbf{v}}_g$ is utilized as the robot’s velocity vector $\mathbf{v}\left(k\right)$. However, in the case where ${\mathbf{v}}_g$ does not satisfy the safety requirement, one utilizes Algorithm 1 to obtain the robot’s velocity vector for avoiding collision with other objects or static map environments.

6.2. Control Laws for Tracking the Voronoi Path $P_V$

The idea of the proposed path following is to set the robot’s wayPoint at every sample-step k, and make the robot approach the wayPoint utilizing the control in Section 6.1. The robot’s wayPoint is set so that it tracks the designated path as the robot follows its wayPoint.

One discusses how to set the robot’s wayPoint at every sample-step k. One parameterizes the designated path, so that $V^1$ has arc-length 0 and $V^Q$ has the longest arc-length. Let $ar{c}^p$ define the arc-length associated with a point, say p, on the path.

Let closestPnt, say $C\left(k\right)$, define the point on the Voronoi path $P_V$, which is closest to the robot at sample-step k. $C\left(k\right)$ can be measured by the robot, since the robot localizes itself while accessing the Voronoi path $P_V$. The arc-length associated with $C\left(k\right)$ is $ar{c}^{C\left(k\right)}$.

One draws a circle centered at $C\left(k\right)$, whose radius is $s_{max}\ast dt$. Let pathCircle define this circle for convenience. Since the maximum speed of the robot is $s_{max}$, the maximum movement distance within one sample interval $dt$ is the circle radius $s_{max}\ast dt$.

Suppose that the Voronoi path crosses the pathCircle at distinct points, say $p_1\left(k\right)$,$p_2\left(k\right)$,…$p_{end}\left(k\right)$. The index order of these points is set, satisfying that $ar{c}^{p_l}\le ar{c}^{p_j}$ where $l<j$. This implies that $ar{c}^{p_1}\le ar{c}^{p_2}\dots \le ar{c}^{p_{end}}$. Thus, $p_1\left(k\right)$,$p_2\left(k\right)$,…$p_{end}\left(k\right)$ exist along the Voronoi path in this order. In Figure 5, $p_1\left(k\right)$ and $p_2\left(k\right)$ exist along the Voronoi path in this order.

Suppose that the Voronoi path crosses the pathCircle at distinct points, to be defined as $p_1\left(k\right)$,$p_2\left(k\right)$, …,$p_{end}\left(k\right)$ in this order. One then sets $p_{end}\left(k\right)$ as the robot’s wayPoint at sample-step k. In Figure 5, $p_2\left(k\right)$ is selected as the robot’s wayPoint. Then, the robot approaches the wayPoint under the control laws in Section 6.1.

At each sample-step k, the robot sets its wayPoint as $p_{end}\left(k\right)$ and sets its velocity vector as (12). Lemma 2 proves that this control makes the robot follow $P_V$.

Lemma 2.

Suppose that the Voronoi path crosses the pathCircle at distinct points, to be defined as $p_1\left(k\right)$,$p_2\left(k\right)$,…,$p_{end}\left(k\right)$ in this order. At each sample-step k, the robot sets its wayPoint as $p_{end}\left(k\right)$ and sets its velocity vector as (12). This control makes the robot follow the Voronoi path $P_V$.

Proof. 

Suppose that the Voronoi path crosses the pathCircle at distinct points $p_1\left(k\right)$, $p_2\left(k\right)$, …,$p_{end}\left(k\right)$ in this order. As the robot moves at every sample-step k, its wayPoint is set to $p_{end}\left(k\right)$. $\mathbf{q}\left(k\right)$ is the robot’s position at sample-step k. Let ${\mathbf{p}}_{end}\left(k\right)\in {\mathcal{R}}^2$ denote the 2D position of $p_{end}\left(k\right)$ for convenience. If $\parallel \mathbf{q}\left(k\right)-{\mathbf{p}}_{end}\left(k\right)\parallel >s_{max}\ast dt$, then the robot heads to $p_{end}\left(k\right)$ with speed $s_{max}$ under (12). If $\parallel \mathbf{q}\left(k\right)-{\mathbf{p}}_{end}\left(k\right)\parallel \le s_{max}\ast dt$, then the robot heads to $p_{end}\left(k\right)$ with speed $\frac{\parallel \mathbf{q}\left(k\right)-{\mathbf{p}}_{end}\left(k\right)\parallel }{dt}$ under (12). In this case, the robot reaches $p_{end}\left(k\right)$ at the next sample-step $k+1$.

The Voronoi path crosses the pathCircle at distinct points, say $p_1\left(k\right)$,$p_2\left(k\right)$,…$p_{end}\left(k\right)$, in this order. Under (12), the robot moves towards $p_{end}\left(k\right)$ at each sample-step k. As the robot moves toward $p_{end}\left(k\right)$, it follows the Voronoi path, since $ar{c}^{p_1\left(k\right)}\le ar{c}^{p_2\left(k\right)}\dots \le ar{c}^{p_{end}\left(k\right)}$. □

Once $V^Q$ is inside the pathCircle centered at $C\left(k\right)$, $V^Q$ is selected as the wayPoint. Then, the robot heads towards the wayPoint under (12).

Suppose that one has an object which is too close to $V^Q$, and that the robot’s wayPoint is set as $V^Q$. In this case, the robot stops moving and it is assumed that the path following is finished. This is inevitable, since $V^Q$ is too close to an object.

One may have a situation where an object exists close to the robot’s wayPoint $p_{end}\left(k\right)$, which is not identical to $V^Q$. In this situation, the robot cannot approach $p_{end}\left(k\right)$, due to the collision-avoidance control in Algorithm 1. The robot may become stuck in this situation.

In order to resolve this problem, one updates the pathCircle radius from $s_{max}\ast dt$ to $\mathsf{\eta }\ast s_{max}\ast dt$, where $\mathsf{\eta }>1$ is a constant, if $p_{end}\left(k\right)$ is too close to an object. In this way, the robot can avoid being stuck close to an object.

7. MATLAB Simulations

MATLAB simulations are presented to verify the effectiveness of the proposed approaches. The robot’s speed is 2 m/s. The sample duration is set as $dt=5$ s, and $k_p=1000$ is utilized in the reactive collision evasion control. One uses $\delta =2$ s as the constant associated with the maneuver of an elevator.

Recall that the robot is near-collision at the moment when the distance between the robot and any $detectedPoint$ is shorter than the safety margin $r\left(k\right)$. In (3), one sets $r_0=2$, and $\mathsf{\mu }=0.01$. In MATLAB simulations, a true collision happens at the moment when the distance between the robot and any $detectedPoint$ is shorter than 1 m, which is shorter than the safety margin $r\left(k\right)$. A MATLAB simulation ends when a true collision happens.

As the maximum sensing range of the robot, one sets $r_m=20$ m. One further simulates the measurement noise in the robot’s local range sensors. In Algorithm 1, every $detectedPoint\in PointSet$ is perturbed by adding a Gaussian noise with mean 0 and standard deviation 0.1. In this way, one simulates the case where the robot’s local range sensors have some measurement noise.

In practice, the robot’s motion is perturbed due to environmental effects, such as wheel slippage. Therefore, instead of (2), the robot’s motion model is set as

$$\begin{array}{c}\hfill \mathbf{q}(k+1)=\mathbf{q}\left(k\right)+\mathbf{v}\left(k\right)\ast dt+\mathbf{n}\left(k\right),\end{array}$$

(13)

where $\mathbf{n}\left(k\right)$ denotes the process noise at sample-step k. It is assumed that $\mathbf{n}\left(k\right)$ has a Gaussian distribution with mean 0 and standard deviation 0.1. By setting $\mathbf{n}\left(k\right)$, one simulates the process error in the robot’s motion command.

One considers the case where the robot moves from one floor $\mathsf{\alpha }$ to another floor $\mathsf{\beta }$. Recall that the robot can utilize elevators to move from one floor to another floor using the vision-based manipulating approach in [7,8].

The robot’s initial position is $(200,150)$ at $\mathsf{\alpha }$ floor. At $\mathsf{\alpha }$ floor, we have multiple objects, which do not move. We have an elevator which connects $(370,120)$ of $\mathsf{\beta }$ floor with $(370,120)$ of $\mathsf{\alpha }$ floor. The goal of the robot is $(350,470)$ at $\mathsf{\beta }$ floor. At $\mathsf{\beta }$ floor, we have multiple objects.

The designated path is built using the building Voronoi graph (blue line segments in the following figures). Then, the robot moves along the designated path while utilizing reactive collision-avoidance control in Section 5 if necessary.

We run 20 Monte–Carlo (MC) simulations to verify the robustness of the proposed approaches. The robot reaches its goal without collision in all MC simulations.

7.1. Handling Circular Objects

We first consider a case where multiple objects exists, such that each object has a circular shape. Figure 6 shows the map of $\mathsf{\alpha }$ floor with a static circular object. MapPoints are plotted with yellow points. The boundary of each static object is depicted with black circles. The robot plans the path from $(200,150)$ to the elevator location $(370,120)$. Considering one MC simulation, the robot’s location at every $dt$ seconds is depicted as green diamonds.

Figure 7 is the enlarged figure of Figure 6. See that the robot’s path is slightly deviated from Voronoi edges (blue line segments), in order to keep away from colliding with an object.

At $\mathsf{\beta }$ floor, we have multiple objects, among which four objects move with velocity $(-0.1,0.7)$ in m/s. Consider the map of $\mathsf{\beta }$ floor with moving circular objects. Considering one MC simulation, Figure 8 indicates the robot’s location (green diamonds) at every $dt$ seconds. The boundary of an object at every $dt$ seconds is depicted with black circles. The robot plans the path from the elevator position $(370,120)$ to the goal $(350,470)$. Figure 9 is the enlarged figure of Figure 8.

Considering the scenario in Figure 8, Figure 10a shows the relative distance between the robot and its closest $detectedPoint$ at each sample-step. Since the robot’s maximum sensing range is 20 m, the relative distance is bounded above by 20 m. A collision occurs at the moment when the distance between the robot and any $detectedPoint$ is shorter than 1 m. See that no collision happens during the robot maneuver. Recall that $C\left(k\right)$ defines the point on the path $P_V$, which is closest to the robot at sample-step k. Figure 10b indicates the relative distance between the robot and $C\left(k\right)$ at each sample-step k. See that the robot keeps following the path $P_V$.

7.2. Handling Various Objects with Arbitrary Shapes

We next consider a case where each object has various shapes, other than a circle. Considering one MC simulation of $\mathsf{\alpha }$ floor (static objects), Figure 11 shows the robot’s location (green diamonds) at every $dt$ seconds. The boundary of each object with arbitrary shapes is depicted with black colors. The robot plans the path from $(200,150)$ to the elevator location $(370,120)$. Figure 12 is the enlarged figure of Figure 11. See that the robot’s path slightly deviates from Voronoi edges (blue line segments), in order to keep away from colliding with an object.

At $\mathsf{\beta }$ floor, one has multiple objects, among which four objects move with velocity $(-0.1,0.7)$ in m/s. Considering one MC simulation of $\mathsf{\beta }$ floor with moving objects, Figure 13 shows the robot’s location (green diamonds) at every $dt$ seconds. The boundary of each object at every $dt$ seconds is depicted with black circles. The robot plans the path from the elevator position $(370,120)$ to the goal $(350,470)$. Figure 14 is the enlarged figure of Figure 13.

Considering the scenario in Figure 13, Figure 15a shows the relative distance between the robot and its closest $detectedPoint$ at each sample-step. Note that a collision occurs at the moment when the distance between the robot and any $detectedPoint$ is shorter than 1 m. See that no collision happens during the robot maneuver. Figure 15b shows the relative distance between the robot and $C\left(k\right)$ at each sample-step k. See that the robot keeps following the path $P_V$.

8. Conclusions

This article addresses the path planing of a robot in a large building with many floors. The robot needs to arrive at the goal in the building safely. The robot can utilize elevators to move from one floor to another. Using the building Voronoi graph as a topological map, the robot derives a safe path to the goal. The weight of a Voronoi edge is set, based on the travel time inside an elevator and the traversal time of a passage. The robot then finds the shortest path to the goal, based on the weighted Voronoi graph. In the case where the robot detects an object with arbitrary shapes, such as a human, while moving along a Voronoi edge, the robot avoids the object utilizing reactive control laws.

In summary, we addressed how to make the robot follow the building Voronoi graph, while avoiding collision with map environments as well as objects with arbitrary shapes. This paper proposed a reactive collision-avoidance control, which achieves collision avoidance with map environments as well as objects with arbitrary shapes. We integrated the proposed reactive collision-avoidance control with a path-tracking control, so that the robot can follow the Voronoi graph, while still avoiding collision with static map environments or objects with arbitrary shapes.

As far as we know, our paper is novel in the path planning and control of a robot utilizing the building Voronoi graph representing a large building structure with many elevators. Furthermore, the proposed reactive collision-avoidance control is novel in considering collision avoidance with map environments as well as (moving or static) objects with arbitrary shapes. This paper verifies the effectiveness of the proposed approaches utilizing MATLAB simulations. In the future, we will demonstrate the performance of the proposed approaches utilizing experiments with a real robot.

The proposed control scheme can be extended to the case where multiple robots move in the building. Each robot can share the building Voronoi graph and use the graph as its topological map. A robot can be considered as a moving obstacle to another robot. A robot can share its position and velocity information with its nearby robot using local communication. Then, the proposed reactive collision-avoidance control laws can be used for avoiding collision among multiple robots.