Power in Numbers: Primitive Algorithm for Swarm Robot Navigation in Unknown Environments

Figure3illustrates the outline of the navigation problem considered in this study. As shown in Fig.3,we consider the navigation of$n\in\mathbb{N}$robots in a two-dimensional plane$W\in\mathbb{R}^{\mathrm{2}}$within an unknown environment. The plane$W$contains a finite number of static closed zones$R_{1},R_{2},\cdots,R_{n}(n\in\mathbb{N})$with finite perimeters, within which the robots cannot move (hereafter referred to as impassable zones). In$W$,there exist two points outside the impassable zones: a start point$S=[x_{s},y_{s}]^{\mathrm{T}}$and a goal$G=[x_{g},y_{g}]^{\mathrm{T}}$. At the beginning of navigation, it is assumed that at least one path$Q$from the start to the goal does not pass through the closed zones$R_{1},R_{2},\cdots,R_{n}$.

Here, the control objective is for at least one of the$N$robots starting from$S$to reach the goal$G$within a finite time. We set the position of a robot$i~{}(i=1,2,\cdots,N)$at time$t$as$r_{i}(t)=[x_{i}(t),y_{i}(t)]^{\mathrm{T}}$,and then the control objective is formulated as follows:

Subsequently, we describe the proposed algorithm. Each robot is sequentially placed one at a time at the start location with a sufficiently large time interval, and then begins the navigation. The robots have the following two functions:

1. Generate a path from the current location to the goal in real-time, avoiding a circular zone with radius$\epsilon$(circumferential radius) centered at the positions of all other robots.

2. Move along the path generated in the above function. Here, the circular zones with the radius$\epsilon$generated by each robot are represented as virtual potential fields. Specifically, each robot generates a path to the goal that avoids the virtual potential fields created by surrounding robots and moves along this path. The specific implementation of the algorithm based on virtual potential fields is explained in SectionIV-C.

Thereafter, we explain the overall algorithm for the system. When the robot’s position is within$R_{1},R_{2},\cdots,R_{n}$,the robot gets stuck and creates a circular closed region of radius$\epsilon$. As shown in Fig.3,the$i$-th robot,$r_{i}$,obtains the coordinates of other robots and the goal, and generates a path$P_{i}$to the goal that avoids circular closed zones$O_{1},O_{2},\cdots,O_{i-1}$centered on the coordinates of other robots, with the circumferential radius$\epsilon$. Because the robots cannot observe the terrain, they initially gets stuck on the path to the goal. As this process continues with multiple robots, the impassable zones are filled by stuck robots, allowing subsequent robots to bypass the earlier robots. Consequently, the robots can eventually avoid the impassable zones and reach the goal.

An example of the navigation environment is shown in Fig.5. The simulations are conducted using Python 3. The entire environment is a square on a 2D plane with sides of length 60 units. The environment is divided into a$60\times 60$grid, with each cell set to either a passable or impassable state for the robots. In Fig.5,the light blue cells represent areas where robots cannot move, circles represent robots, the cross mark is the start point, and the star represents the goal. Robots are treated as points, and robot-to-robot contact is not considered. Each robot moves while generating a potential field. The purple lines represent the contours of the potential fields created by the robots. If the robot enters a light blue cell, it is considered stuck and immobilized.

The start point and goal are positioned diagonally at coordinates$[15.5,15.5]^{\mathrm{T}}$and$[44.5,44.5]^{\mathrm{T}}$,respectively. After a robot gets stuck, a new robot is placed at the starting position, facing the goal. The simulation ends when the robot’s center coordinates are within the robot’s radius from the goal, indicating the robot has reached the goal, or after$10000\textrm{s}$. The simulation calculation period is set to$0.1\textrm{s}$,and the value of the grid width is$1$.

To generate the terrain, we use Perlin noise, which is commonly used for creating natural and random terrain representations[24]. Perlin noise generates a two-dimensional scalar function$f(x,y)$defined on the$xy$plane. Using a uniformly distributed random real number$a$in the range of$-1$to$1$,each grid is classified as either a traversable or non-traversable area based on the value of$f$at the center of the grid. Specifically, if the value of$f$at the center of a grid is greater than$a$,the grid is designated as a non-traversable area. Conversely, if the value of$f$is less than or equal to$a$,the grid is designated as a traversable area. However, a$10\times 10$square zone centered around the start point and goal positions is always designated as a traversable area to ensure the presence of a viable path. To verify the existence of a path from the start point to the goal that only passes through traversable areas, we use the A* algorithm[25]. Only terrains where such a path exists are utilized for the simulation.

We implement the proposed algorithm inspired by the bug algorithm[6]. The robots employ global sensing to determine the direction to the goal and local sensing to evaluate the strength and gradient of the potential field at their current location. Each robot generates a physical quantity with wave-like properties, such as light, sound, or radio waves, with its center as the source. This allows the robot to form a potential field and sense the potential fields created by other robots, guiding its movement. The strength of the potential field decays according to the$r^{2}$law with respect to the distance$r$from the potential source. Specifically, we define the potential field strength for the robot positioned at$[x_{r},y_{r}]^{\mathrm{T}}$as follows:

where$c$is a constant parameter. In the numerical simulations described in SectionIV,the constant parameter$c$is set to a value of$1$. Moreover, each robot has a certain threshold$g_{th}$for the intensity of the potential. It evaluates the distance from surrounding robots by comparing the potential intensity$g$it senses on the spot with this threshold value. Specifically, the threshold$g_{th}$adjusts the circumference radius$\epsilon$that each robot uses to bypass other robots.

Fig.6shows an overview of the robot’s operation. The robot dynamically switches between two movement modes: mode 1 and mode 2. In mode 1, the robot moves directly in the direction of the goal, while in mode 2, it moves along the contour lines of the surrounding potential field. In mode 2, the robot adjusts its movement to align with the fixed threshold$g_{th}$of the measured potential strength, moving perpendicular to the potential gradient. By setting this threshold below a certain value, the robot maintains a distance from other robots that at least equals the robot’s radius, thereby avoiding collisions. This distance is referred to as the circumferential radius. To simplify the simulation implementation, we assume that in mode 2, the robot moves directly perpendicular to the gradient. Additionally, there are two possible perpendicular directions to the gradient: clockwise and counterclockwise. In this study, the robot is always configured to choose the direction with the smaller angle relative to the goal direction. Hence, the robot’s behavior consists of three types of movements: moving straight, turning clockwise, and turning counterclockwise. When the angle difference between the goal direction and the robot’s current direction is less than or equal to$\pi/36\text{rad}$,the robot moves straight at a speed of$1\text{/s}$. Otherwise, the robot turns in the direction that reduces the angle with a turning speed of$\pi/6\text{rad/s}$.

The condition for switching from mode 1 to mode 2 is when the strength of the potential field at the current position of the moving robot$g$reaches the threshold$g_{th}$,and the angle difference$\theta_{fg}$between the goal direction and potential field gradient direction satisfies$\theta_{fg}\leq\pi/2\textrm{rad}$. This condition indicates that the mobile robot avoids the stuck robot when it has gauged that the distance to the stuck robot$d$is less than or equal to the circumferential radius$\epsilon$,and it is in the direction of the goal. On the other hand, the condition for switching from mode 2 to mode 1 is when the angle difference$\theta_{rg}$between the robot’s direction and goal direction satisfies$\theta_{rg}<\pi/2\textrm{rad}$and the angle difference$\theta_{fg}$between the potential field gradient direction and goal direction satisfies$\theta_{fg}>\pi/2\textrm{rad}$. This is to ensure that the moving robot avoids the potential field and heads towards the goal.

An example of the results is shown in Fig.7. In this simulation, the threshold for the robot’s potential strength is$g_{th}=1$and the minimum path width is$3$. From Fig.7,it is evident that multiple robots getting stuck creates impassable zones represented by the potential field, allowing subsequent robots to reach the goal. This demonstrates that there are conditions under which the robots can successfully navigate unknown environments using potential fields. The movie of this simulation can be viewed in Movie1.mp4 in the Supplemental Materials. However, as shown in Fig.7,there are also instances where the robots fail to reach the goal. The threshold for the robot’s potential strength is$g_{th}=1$and the minimum path width is$2$. Therefore, the robots may not be capable of handling all terrains under the given conditions. Multiple cases where navigation failed occurred when the narrow paths were blocked by the potential field created by the robot swarm itself. Specifically, the successful arrival of the robot at the goal is significantly influenced by the circumferential radius. Therefore, in SectionV-A,we examine how the circumferential radius and terrain affect the probability of successful navigation. These simulation videos can be viewed in the Supplemental Materials (Movie1.mp4 and Movie2.mp4).

As described in SectionIV-C,the robot’s circumferential radius$\epsilon$is adjusted by a constant threshold$g_{th}$of potential strength. By setting the potential field strength threshold$g_{th}$to$1$,$1/1.5^{2}$,$1/2^{2}$,$1/2.5^{2}$,$1/3^{2}$,$1/3.5^{2}$,$1/4^{2}$,$1/4.5^{2}$,and$1/5^{2}$,the circumferential radius$\epsilon$was varied to be 1, 1.5, 2, 2.5, 3, 3.5, 4, 4.5, and 5 times the grid width for simulations. Additionally, for each terrain with minimum path widths of 1, 1.5, 2, 2.5, 3, 3.5, 4, 4.5, and 5 times the grid width, 100 different terrains were generated. The percentage of terrains where the robots successfully reached the goal was determined for each circumferential radius.

The simulation results are shown in Fig.8. The horizontal axis represents the multiplier of the circumferential radius relative to the grid width, and the vertical axis represents the multiplier of the minimum path width relative to the grid width. Each cell indicates the success rate. Fig.8shows that the probability of reaching the goal is larger when the minimum path width (value on the vertical axis) is larger than the robot’s radius of circumference (value on the horizontal axis), while the probability is smaller when the minimum path width is smaller than the robot’s radius of circumference. Specifically, we determine that the probability of reaching the goal tends to depend on the relationship between the value of the minimum path width and the value of the radius of circumference of the robot. This follows from the fact that when the radius of the circumference of the robot is larger than the minimum path width, the potential field generated by the stuck robot blocks the path to the goal. As described in SectionIII,the robot can be guaranteed to reach the goal by setting the robot’s radius of circumference$\epsilon$to a value smaller than the minimum path width$l$of the path to the goal in order to achieve successful navigation. Thus, we have demonstrated the efficacy of the implementation of BYCOMS with potential fields.

We examined the behavior of the proposed system when noise was added to the measurements of the potential field’s strength and gradient direction. In order to introduce noise in the potential field’s strength, a uniformly random value in the range of$0.8$to$1.2$was multiplied by the measured value to obtain the new measurement. To introduce noise in the gradient direction of the potential field, a uniformly random value in the range of$-\pi/6$to$\pi/6\textrm{rad}$was added to the gradient direction to obtain the new direction. The terrain used was the same as that described in SectionV-A. The reachability probabilities considering the sensing noise are shown in Fig.9. In particular, we can observe that the probability of reaching the goal depends on the relationship between the radius of circumference of the robot and the minimum path width of the goal path, as well as the result without considering noise (Fig.8). Compared to the results shown in Fig.8,the probability of reaching the goal does not change significantly under most conditions. The increase in the probability of reaching the goal with noise can be attributed to the randomness of the noise having a positive effect on the probability of reaching the goal by chance. Thus, the simulation results demonstrate the robustness of BYCOMS against sensing noise.

Fig.10shows an overview of the developed robot. The robot consists of an upper microphone array for sound field gradient sensing and a lower crawler unit for movement. The dimensions of the robot are as follows:$213\textrm{mm}$in length,$215\textrm{mm}$in width, and$117.5\textrm{mm}$in height. The microphone array is unitized in an acrylic plate case with a diameter of$256\textrm{mm}$,centered around the middle of the robot. The weight of the robot is$1.78\textrm{kg}$. The robot is controlled by an ESP32-WROOM-32E (Espressif Systems) microcontroller. In addition, it is equipped with motors and a crawler unit (Devastator Tank, DFROBOT) for movement, a motor driver (ROB-14450, SparkFun), and a mobile battery (DE-C17L-5000, Elecom). To generate the sound field, the robot has a speaker (S0266, DAYTON AUDIO) connected to a sound signal amplifier module (BOB-11044, SparkFun) positioned at the center on the top of the robot. For gradient measurement, it is equipped with six condenser microphones (BOB-12758, SparkFun) placed at equal intervals on a circle with a radius of$100\textrm{mm}$. The structural components of the robot are made of$3\textrm{mm}$laser-cut acrylic sheets and 3D printed PLA parts. Additionally, the robot has five infrared reflective markers on the top, which allow an optical motion capture system (Optitrack) to log the robot’s position and orientation.

In the proposed method using acoustic fields, robots measure the gradient direction of the acoustic field at their current location. In this experiment, we adopt a method of measuring the gradient direction by placing multiple microphones on a circumference and using the differences in their measured values. Consider a case where 6 microphones$M_{1},M_{2},\cdots,M_{6}$are evenly spaced on a circle of radius$r$that shares its center with the robot. We employ$xy$coordinates where the center of the robot is considered the origin, and$M_{1}$is at$(r,0)$. Let the measured sound intensities at each microphone be$V_{1},V_{2},\cdots,V_{6}$. The sound intensity at the center of the robot is estimated as the average of all microphone measurements, expressed by the following equation:

Moreover, let$\bm{m_{n}}$be the position vector of the$n$-th microphone relative to the center of the robot. The estimated gradient of the acoustic field obtained by the robot is expressed by the following equation:

The robot normalizes this$\bm{grad}$and uses only the direction of the acoustic field gradient as the estimate.

The overview of the robot control method is shown in Fig.11. As described in SectionIV,the robot is implemented with mode 1, which moves directly to the goal, and mode 2, which moves along the sound field. In mode 1, the robot uses the angle difference$\theta_{1}(-\pi<\theta_{1}\leq\pi)$between the direction of the goal and its own direction, and the control input is$u=K_{p}\theta_{1}$. The robot was controlled by applying a voltage of$v_{m}+u$to the left motor and$v_{m}-u$to the right motor, with$v_{m}$as the reference value of the voltage applied to the motors. In mode 2, the robot measures the gradient direction of the sound field and sets the target direction as the direction rotated by$\pi/2$from the gradient. It calculates the angle difference$\theta_{2}(-\pi<\theta_{2}\leq\pi)$between this target direction and its own direction. Additionally, the target value of the magnitude of the microphone measurements is set as$V_{target}$,and the difference between$V_{target}$and average of all microphone measurements$V_{ave}$is calculated as$V_{e}=V_{ave}-V_{target}$. Using these values, the control input is$u=K_{p}(\theta_{2}+\ Alpha V_{e})$.$\ Alpha$is the weighting coefficient for$\theta_{2}$and$V_{e}$. As in mode 1, the robot was controlled by applying a voltage of$v_{m}+u$to the left motor and$v_{m}-u$to the right motor. The switching conditions between the two modes are as follows. The condition for switching from mode 1 to mode 2 is$V_{ave}>V_{target}$and the angle between the direction of the goal and$\bm{grad}$is less than$\pi/2$. The condition for switching from mode 2 to mode 1 is the angle between the direction of the robot and the goal is less than$\pi/2$while that between$\bm{grad}$and the goal direction is greater than$\pi/2$,or when$V_{ave}$significantly decreases below the lower threshold value$V_{min}$. The latter is set to account for situations where$\bm{grad}$cannot be measured because the robot has moved too far from the sound source in mode 2. The parameters for this experiment are$K_{p}=100$,$\ Alpha =0.0001$,$V_{target}=22000$,and$V_{min}=15000$.

In our proposed navigation method, each robot must emit sound while also listening for sound signals from other robots. Theoretically, if all the robots emit the same frequency, the robot’s own sound would be heard equally by all its microphones, thus not affecting the calculation of its gradient direction. However, in practical scenarios, measurement errors might cause the robot’s own sound to interfere with gradient measurement. Additionally, vibrations transmitted through the robot’s structural components could introduce significant noise. Therefore, we vary the sound frequency for each robot in our experiments. Each robot can isolate the sounds emitted by others using fast fourier transform (FFT)[26]. The robot extracts the sound of other robots’ frequencies from the measured sound and uses the sum of their amplitude spectra as the microphone measurements. This processed data is then used to calculate the gradient as described in SectionVI-B.

We validated the effectiveness of the proposed method in real-world environments through experimental verification with three developed robots. The experimental environment is shown in Fig.12. An acrylic board with reflective markers was placed at the goal. To emulate an unknown environment for the robots, aluminum frames were set up as obstacles on the way to the goal. The later robot starts moving from the starting point after the earlier robot gets stuck on an obstacle. Additionally, each of the three robots emitted sine wave sounds from their speakers at frequencies of$224\mathrm{Hz}$,$320\mathrm{Hz}$,and$512\mathrm{Hz}$,respectively.

The overview of the system controlling the robot swarm is shown in Fig.13. First, the position and orientation of the robots and the position of the goal are measured using the motion capture system and sent to the main PC. On the PC, all the data are processed as topics using Robot Operating System (ROS)[27]. Based on the sensor data obtained by the motion capture system, the direction of the goal from each robot’s perspective is calculated and sent to each robot via WiFi. The robots then move based on the transmitted goal direction data, and the locally sensed sound field gradient. Additionally, the measured sound field gradient data is sent to the main PC, where the data is stored.

In this study, we conducted two experiments in which the goal was obstructed by two types of obstacles of different lengths. The experimental results are shown in Fig.14and Fig.15. In Fig.14,where a short obstacle was installed, it can be observed that the first robot got stuck on the aluminum frame, while the second robot avoided both the sound field of the first robot and the obstacle, thus reaching the goal. In Fig.15,with a long obstacle installed, the second robot similarly bypassed the first robot, but it also got stuck owing to the presence of an obstacle in its path. However, the third robot, guided by the combined sound fields of the first and second robots, successfully avoided the obstacle and reached the goal.

Fig.14and Fig.15show the positions of the robots and the goal, measured gradient directions, and microphone measurements. Blue dots represent Robot 1, red dots represent Robot 2, green dots represent Robot 3, stars indicate the goal, arrow directions show the measured gradient directions, and arrow lengths represent the average microphone measurements. However, in Fig.15,after Robot 1 got stuck, its position coordinates were mistakenly identified as those of Robot 3. This was owing to the rigid body definition based on the detachment of multiple markers on the robot. From Fig.14and Fig.15,it can be observed that when robots are close to each other, they can measure the direction of each others sound fields. However, when the robots are slightly apart, they do not measure as well. This is probably because the difference in microphone measurements becomes small, and the influence of noise, such as echoes and motor vibrations becomes significant. Consequently, it can be considered that in this experiment, the P control based on the average microphone measurement has a greater influence on the robot movement than the gradient direction. Therefore, the robots based on the sound field were shown to have the capability to traverse unknown environments even in real-world experiments. These experiments videos can be viewed in the Supplemental Materials (Movie3.mp4 and Movie4.mp4).