An Active Obstacle Avoidance Method

Abstract

Swarm robots often encounter dynamic obstacles when performing tasks, such as moving objects in the scene or other individuals in the robot group. The traditional passive obstacle avoidance method makes the robots take emergency avoidance behaviour when it is about to encounter obstacles. Hoverer this may destroy the group cooperation behaviour, thereby affecting the efficiency of the system. Active obstacle avoidance perceives a dynamic target and predicts the movement of the target and takes the initiative to avoid obstacles, minimizes the impact of obstacle avoidance on the system’s cooperative behaviour. Considering that the defects in the structural design of swarm robots and the avoidance strategy of swarm robots, it is necessary to focus on active obstacle avoidance of swarm robots that is based on the prediction of dynamic targets. An improved obstacle avoidance method is therefore proposed, which enables robots to avoid both static and dynamic obstacles.

This work is supported by the projects of National Natural Science Foundation of China (No. 61873192), the Quick Support Project (No. 61403110321), the Innovative Projects (No. 20-163-00-TS-009-125-01; 21-163-00-TS-011-011-01; 2021-JCJQ-LB-010-11), and the Key Pre-Research Project of the 14th-Five-Year-Plan on Common Technology (No. 50912030501). Meanwhile, this work is also partially supported by the Fundamental Research Funds for the Central Universities, as well as the project of Shanghai Key Laboratory of Spacecraft Mechanism (18DZ2272200). All these supports are highly appreciated.

1 Introduction

Due to swarm robots’ abilities of robustness, flexibility and scalability, more and more scholars pay attention to swarm robots. When performing the tasks, swarm robots usually encounter dynamic obstacles, such as moving objects or other individuals in the robot group. Therefore, in order to complete the tasks successfully, it is necessary for the swarm robots to avoid obstacles.

There are several common obstacle avoidance methods for robots, such as Artificial Potential Field, VFH, Reciprocal Velocity Obstacle, etc.

Khabit proposed the concept of Artificial Potential Field [1]. Its disadvantage is that the gravitational force is very large when robot is far away from the target point, thus the relatively small repulsive force can’t be completely offset, which leads to collision.

Fiorini and Shiller proposed the concept of Velocity Obstacle (VO) at 1998 [2]. This method is applied for dynamic obstacle. Under the premise of observing dynamic obstacles, the robot calculates the set of velocities that will eventually cause a collision. The robot is supposed to adapt a new velocity that isn’t belong to the set. Dynamics isn’t considered in VO.

Borenstein et al. proposed the Vector Field Histogram (VFH), VFH permits the detection of unknown obstacles and avoids collisions while simultaneously steering the mobile robot towards the target [3]. However, VFH doesn’t take the size and dynamics of robot into consideration and regards robot as a point. Iwan et al. proposed a method based on VFH and it is called VFH+, which takes size, trajectory and dynamics of robots in to consideration [4]. It doesn’t take movable obstacle into consideration, which may lead to collision.

Fox et al. divided obstacle avoidance methods into two categories, global planning and local planning [5]. The former applies for static obstacle while the latter applies for dynamic obstacle. In addition, Fox also proposed Dynamic Window Approach (DWA), which considered the kinematics of the robot, leading to selection of velocity more reasonable. However, it may lead to collision because it focuses on the obstacles on the trajectory of the robots, while the obstacles near the trajectory may lead to collision.

Piyapat et al. proposed a method based on Dynamic Window Approach which is called Field Dynamic Window Approach (F-DWA). It solves the problem that obstacles near the trajectory of robots may lead to collision. However, F-DWA focus single robot, it doesn’t consider collision avoidance of multiple robots [6].

Zhang Zhiwen et al. proposed a method that combines improved A* algorithm with Dynamic Window [7]. It achieves real-time dynamic obstacle avoidance and performs well in path planning. However, the method applies for collision avoidance and path planning of single robot, while there exists multiple robots that need to avoid collision, it fails.

Jur Van den Berg et al. proposed Reciprocal Velocity Obstacle that is based on the Velocity Obstacle [8]. This method calculates the set of velocities that make the robot avoid collision with others by linear programming, then the robot selects the optimal velocity in the set. It is simple and fast but it doesn’t consider dynamics.

Some of the above method focus on single robot’s collision avoidance such as method proposed by Zhang Zhiwen and Piyapat. Others ignore dynamics such as the method proposed by Michele. Those method used in swarm robots will lead to collision or destruction of swarm robots’ formation. Therefore a method is proposed in this article, which aims to avoid collision. In this method, both static obstacles and dynamic ones are considered.

2 Proposed Method Based on ORCA

Based on Reciprocal Velocity Obstacle, Van den berg and so on proposed Optimal Reciprocal Collision Avoidance (ORCA) [9], which works better than Reciprocal Velocity Obstacle.

The core of ORCA is to establish a velocity set. Each velocity in the set makes sure that robots would be free from collision with other robots and static obstacles. The set is an area of the two-dimensional velocity plane and the area is defined by equations caused by other robots and static obstacles. A robot is supposed to select a velocity in the area as its new velocity in the next iteration.

However, there also exists limitation in ORCA. Dynamic obstacles in ORCA only refers to robots that are adherence to ORCA. Therefore, ORCA doesn’t work well if there exist robots that aren’t adherence to ORCA.

It’s necessary to take robots that aren’t adherence to ORCA into consideration and it is what the proposed method achieves in this article.

2.1 Establishing the Velocity Set

Taking the case of two robots as an example. Robot A and Robot B is described by the equation as follows, see Fig. 1(a):

$$\begin{aligned} \textit{D}({\textbf {p}},r)=\{{\textbf {q}}|\ ||{\textbf {q}}-{\textbf {p}}||<r\}. \end{aligned}$$

(1)

Fig. 1.

Configuration, velocity obstacle and ORCA of robots.

To establish the velocity set, the first step is to establish the velocity obstacle. For robot A and robot B, velocity obstacle $\textit{VO}^\tau _{A|B}$ is defined by the equation as follows, see Fig. 1(b):

$$\begin{aligned} \textit{VO}^\tau _{A|B}=\{{\textbf {v}}|\ \exists \textit{t} \in [0, \tau ]\,{:}{:}\,\textit{t}{} {\textbf {v}} \in \textit{D}({\textbf {p}}_{\textit{B}}-{\textbf {p}}_{\textit{A}}, r_{\textit{A}}+r_{\textit{B}}) \}. \end{aligned}$$

(2)

The next step is to establish the velocity set. If in the next time of $\tau$, robot A and B will not collide at their maximum velocity, both robot A and B don’t have to choose a velocity through ORCA. If robot A and B will collide in the next time of $\tau$, vector u exists and vector u is used to establish the velocity set. Vector u is defined as follows, geometric interpretation is displayed in Fig. 1(c):

$$\begin{aligned} \mathbf {u}=\left( \underset{\mathbf {v} \in \partial V O_{A \mid B}^{\tau }}{\arg \min }\left\| \mathbf {v}-\left( \mathbf {v}_{A}^{\mathrm {opt}}-\mathbf {v}_{B}^{\mathrm {opt}}\right) \right\| \right) -\left( \mathbf {v}_{A}^{\mathrm {opt}}-\mathbf {v}_{B}^{\mathrm {opt}}\right) . \end{aligned}$$

(3)

Vector n is an outer normal vector at the boundary point $\textit{v}_{\textit{A}}^{\textit{opt}}-\textit{v}_{\textit{B}}^{\textit{opt}}+{\textbf {u}}$ of $\textit{VO}^\tau _{A|B}$. Vector u is the smallest change of velocity for robot A and B to be away from collision in next time $\tau$. As robot A and B have the same status, each of robot A and B take half the responsibility to change their velocity. Therefore, velocity of robot A is supposed to add 0.5u and velocity of robot B is supposed to add −0.5u. Finally, the velocity set of robot A is calculated by Eq. (4), see Fig. 1(c):

$$\begin{aligned} ORCA_{A \mid B}^{\tau }=\left\{ \mathbf {v} \mid \left( \mathbf {v}-\left( \mathbf {v}_{A}^{\mathrm {opt}}+\frac{1}{2} \mathbf {u}\right) \right) \cdot \mathbf {n} \ge 0\right\} . \end{aligned}$$

(4)

In the proposed method, a dynamic obstacle which is adherence to ORCA is considered, the velocity set using $ORCA_{A \mid DO}^{\tau }$ to denote, and it is calculated by Eq. (5).

$$\begin{aligned} ORCA_{A \mid DO}^{\tau }=\left\{ \mathbf {v} \mid \left( \mathbf {v}-\left( \mathbf {v}_{A}^{\mathrm {opt}}+ \mathbf {u}\right) \right) \cdot \mathbf {n} \ge 0\right\} . \end{aligned}$$

(5)

The difference between Eq. (4) and Eq. (5) is the coefficient of $\mathbf {u}$. In ORCA, it ignores the dynamic obstacles that aren’t adherence to ORCA. The dynamic obstacle towards a certain robot in ORCA means other robots, therefore each robot takes half responsibility to avoid collision. While in the proposed method, the dynamic obstacles includes other robots that aren’t adherence to ORCA. Therefore robot in the proposed method is supposed to take full responsibility to avoid collision when facing dynamic obstacle that aren’t adherence to ORCA.

As for robot B, the velocity set $ORCA_{B \mid A}^{\tau }$ is defined symmetrically, geometric interpretation is shown in Fig. 1(c).

The final velocity set is the intersection of velocity set caused by other robots, dynamic obstacles and its maximum velocity if there exists a lot of robots, such as robot A, B, C, etc. and dynamic obstacle 1, dynamic obstacle 2 etc. Using $\textit{ORCA}^{\tau }_{\textit{A}}$ to denote the final velocity set. It is described by Eq. (6):

$$\begin{aligned} \begin{aligned} \textit{ORCA}^{\tau }_{\textit{A}} = \textit{D}({\textbf {0}}, \textit{v}_{\textit{A}}^{max}) \cap \textit{ORCA}^{\tau }_{\textit{A} | B} \cap \textit{ORCA}^{\tau }_{\textit{A} | C} \cap \dots \\ \cap \textit{ORCA}^{\tau }_{\textit{A} | DO1} \cap \textit{ORCA}^{\tau }_{\textit{A} | DO2} \cap \dots . \end{aligned} \end{aligned}$$

(6)

2.2 Choose the New Velocity

The velocity set of robot A is calculated by Eqs. (4) and (6) and the next step is to choose a velocity that closes to the prefer velocity of A in the velocity set. The new velocity is chosen through Eq. (7):

$$\begin{aligned} \mathbf {v}_{A}^{\text{ new } }=\underset{\mathbf {v} \in O R C A_{A}^{\tau }}{\arg \min }\left\| \mathbf {v}-\mathbf {v}_{A}^{\text{ pref } }\right\| , \end{aligned}$$

(7)

where $\mathbf {v}_{A}^{\text {pref}}$ is the velocity robot A prefers to select. To simplify, $\mathbf {v}_{A}^{\text {pref}}$ and $\mathbf {v}_{A}^{\text {opt}}$ are set as $\mathbf {v}_A$. Finally robot A selects a new velocity that guarantees it be away from collision.

2.3 Combined with Kalman Filter

In ORCA, the most important information is the position of robots. By using Kalman filter, more accurate position information of robots can be obtained. Kalman filter mainly processes position information of robots, the processed information is converted to ORCA to calculate the new velocity set. Considering that position information matters in ORCA, Kalman filter [10] can be simplified in the proposed method. And the simplified Kalman filter is used to improve the accuracy of robots’ position information and it is described by Eq. (8):

$$\begin{aligned} \begin{aligned} \hat{x}_{n}&=\hat{x}_{n-1}+K_{n}\left( y_{n}-\hat{x}_{n-1}\right) , \\ p_{n}&=\left( 1-K_{n}\right) p_{n-1}, \\ K_{n}&=\frac{p_{n-1}}{p_{n-1}+\sigma ^{2}}, \end{aligned} \end{aligned}$$

(8)

where $\hat{x}_{n}$ is the current estimate, $\hat{x}_{n-1}+K_{n}$ is the former estimate, $K_{n}$ is Kalman gain, $y_{n}$ is the measurement value of sensor, $p_{n-1}$ is based on the state of the previous moment to find the variance of the current state, $p_{n}$ is the updated variance, $\sigma ^{2}$ is the variance of the sensor measurement.

3 Simulations Between ORCA and Proposed Method

The simulation is conducted in the Virtual Robot Experiment Platform (VREP). And there are three scenes conducted in VREP.

3.1 Setup

All the parameters of the robots in the scenes in VREP are the same as the physical robots. The mass of each wheel is 0.2 kg and principal moment of inertia is $\mathrm{9\times 10^{-5}\,kg{\cdot }m^2}$. The maximum torque is 10 $\mathrm{N{\cdot }m}$. The mass of the whole robot is 1.1 kg. The maximum velocity of the robot is 0.94 m/s. The friction coefficient is set as default. Both x-coordinate and y-coordinate of robots are disturbed by Gaussian noise with variance of 1 $\mathrm{cm^{2}}$ and mean value of 0 cm.

3.2 Scenes

There are several scenes to show the effect of method to achieve collision avoidance. All of the scenes are created in VREP.

Scene 1 aims to show the collision avoidance among robots. Twelve robots are distributed on a circle with the radius of 2 m, which is centred on the coordinate origin. In this scene, the task of the robots is to approach the origin and rotate around the origin for a certain period time when the distance between the origin and the robot is less than 1.1 m. Then each robot is supposed to move back to the initial position, see Fig. 2(a). Scene 2 aims to show robots using the proposed method to go through a narrow aisle to arrive target position, see Fig. 2(b).

Fig. 2.

Scenes of simulations.

Scene 3 aims to show the collision avoidance among robots, static obstacles and dynamic ones. The task of the robots is the same as it in scene 1, though some parameters differ, such as the initial position and the center of the rotation, see Fig. 2(c).

3.3 Effectiveness

The proposed method is both applied in the three scenes and ORCA is applied in scene 3.

In the scene 1, as envisaged, the proposed method successfully makes robots approach the target and rotate around it without collision. It shows the proposed method performs well in collision avoidance among robots that are adherence to ORCA, see Fig. 3(a).

In the scene 2, it shows that the proposed method enables robots to avoid collision with static obstacles, see Fig. 3(b). In the scene 3, it shows that the proposed method successfully makes each robot in the scene complete task without collision with other robots and dynamic object, see Fig. 4(a). In this scene, the dynamic object aren’t adherence to ORCA, which means they are highly possible to collision with robots. Therefore, they don’t take responsibility of collision avoidance. In ORCA, each robot is responded for half responsibility to avoid collision. When facing the dynamic obstacles, it is very likely to happen collision between robots and dynamic obstacles if robots take only half responsibility to avoid collision, see Fig. 4(b).

Fig. 3.

Scene 1 and scene 2 of simulations.

Fig. 4.

Scene 3 of simulations. (Color figure online)

In simulation, the proposed method is able to solve the scenes like scene 3 with some limits that the velocitie of dynamic obstacles is supposed to have the same maximum velocity as robots. If dynamic obstacles’ maximum velocity is faster than robots’, there will be collision between them. Collision happens in scene 3 with ORCA beacause Gaussian noise disturb position information of robots and there are robots in scene 3, which aren’t adherence to ORCA. All trajectories of scenes 1, 2 and 3 are displayed in Fig. 5 and Fig. 6.

Fig. 5.

Trajectories of scene 1, 2.

Fig. 6.

Trajectories of scene 3.

4 Conclusion

A collision avoidance method based on ORCA is proposed in this article. The Kalman filter used in the method is supposed to make ORCA better to be applied in simulation and is able to play a role in experiment. The proposed method Combines it with the improved ORCA, which is able to predict the position of dynamic obstacle and therefore helps the robots to avoid collision with dynamic obstacles that aren’t adherence to ORCA. In a word, this study makes contribution of collision avoidance of multi-robot navigation especially in the environment with dynamic obstacles. Future works can focus on the point that how to make robots avoid collision with dynamic obstacles whose velocity is higher than the maximum velocity of the robot and strengthen robot’s ability of robustness towards larger disturbance of position.