A New Online Random Particles Optimization Algorithm for Mobile Robot Path Planning in Dynamic Environments

A new algorithm named random particle optimization algorithm (RPOA) for local path planning problem of mobile robots in dynamic and unknown environments is proposed. The new algorithm inspired from bacterial foraging technique is based on particles which are randomly distributed around a robot. These particles search the optimal path toward the target position while avoiding themoving obstacles by getting help from the robot’s sensors.The criterion of optimal path selection relies on the particles distance to target and Gaussian cost function assign to detected obstacles. Then, a high level decision making strategy will decide to select best mobile robot path among the proceeded particles, and finally a low level decision control provides a control signal for control of considered holonomic mobile robot. This process is implemented without requirement to tuning algorithm or complex calculation, and furthermore, it is independent from gradient base methods such as heuristic (artificial potential field) methods. Therefore, in this paper, the problem of local mobile path planning is free from getting stuck in local minima and is easy computed. To evaluate the proposed algorithm, some simulations in three various scenarios are performed and results are compared by the artificial potential field.


Introduction
Application of mobile robots in industries, military activities, and indoor usages has been remarkably growing recently.In addition, in some missions such as hazardous or hardly accessible territorial, where humans could be in danger, mobile robots can carry out tasks such as nuclear plants, chemical handling, and rescue operations.
One of the most challenging subjects in mobile robot's research is path planning.The path planning problem is usually defined as follows.Given a robot and a description of an environment, plan a path between two specific locations.The path must be collision-free (feasible) and satisfy certain optimization criteria minimum traveling distance [1].Researchers have developed and used different concepts to solve the robot path planning problem.These concepts have been categorized into the following subclasses: the graphical approaches, the classical techniques and the heuristic approaches as well as the use of the potential field methods.
Graphical methods are more often than not developed on the principles of geometry and they are dominantly known for their limitation to static environment [2].Graphical techniques such as voronoi diagram, spatial planning, transformed space, cell decomposition, and configuration space have been proposed and extensively used in the past few decades.One of the well-known graphical approaches is the Configuration Space method used by [3][4][5][6][7][8][9][10].
Another class of path planning approaches is the heuristic-based technique.Path planning in this context consists of every state as a possible position for a robot to move to and the transitions between these states are the actions the robot takes from the known information of the surrounding environment.The information that is valued the most by the algorithm is the cost of transitions from the current state to the goal state.The path taken is optimal if the sum of transition costs which are labeled global costs, also known as edge costs, is a global minimal.Global costs consist of all known paths for the robot at a specific time.The algorithm also has to be complete.This technique is based on inferential and evolutionary computation.There has been several heuristic techniques in literatures such as potential field, virtual force,  * algorithm,  * algorithm, genetic algorithm, memetic algorithm, and simulated annealing.
Artificial potential fields (APFs) for collision avoidance have attracted many researchers in the field of robotics.The idea of virtual forces acting on a robot has been proposed by Andrews and Hogan [21] and Khatib [22].This method is based on the construction of a potential function around the obstacles, deriving repulsive forces to the robot to avoid them; on the contrary, an attractive potential function was defined with its center at the end position.In these approaches the target applies an attractive force to the robot while obstacles exert repulsive forces on the robot.The resultant force (sum of all forces) determines the direction and speed of movement.A generalized potential field method that combines global and local path planning has been introduced by Krogh and Thorpe [23].
Path planning on other hand can be divided into two groups according to the assumptions about the information available for planning.In path planning with complete information perfect knowledge about the robot and the environment are considered, shapes of obstacles are computed algebraically, and path planning is implemented offline.In path planning with incomplete information or sensor-based planning, the obstacles can be of arbitrary shape and the input information is, in general, local information from a range detector or a vision sensor [24].Path planning could be either local or global.Local path planning means that path planning is done while the robot is moving; in other words, the algorithm is capable of producing a new path in response to environmental changes [25].On the other, global path planning requires that environment to be completely known and all the features present within the terrain remain static and the algorithm generates a complete path from the start point to the destination point before the robot starts its motion.
In this paper, a local path planning problem in an unknown environment for a holonomic mobile robot in the presence of mobile obstacles is addressed.In order to face this problem several approaches have been proposed in literatures.One of the most general techniques is based on the utilization of artificial potential fields (APFs).As mentioned above, this concept was introduced by Khatib [22], as a realtime obstacle avoidance method.The major drawback of this approach is that the robot may get stuck in local minima.To cope with this problem, Vadakkepat et al. introduced a new technique named evolutionary artificial potential field (EAPF) to address the path planning scenario with mobile obstacles.Genetic algorithm was employed to derive optimal potential field functions.In addition, an escape-force algorithm was performed to avoid being trapped in the local minima [26].Ge and Cui [27] proposed a new potential field method for motion planning of a mobile robot in a dynamic environment.The new potential functions take into account not only the relative positions of the robot with respect to the target and obstacles, but also the relative velocities of the robot with respect to the target and obstacles.Then, Poty et al. [28] merged the approach proposed in [27] and the fractional potential for dynamic motion planning of mobile robot.The fractional potential was utilized to characterize danger zone and risk coefficient of each obstacle.Computer simulations demonstrated that mobile robot avoided obstacles and reached the target successfully.Recently, Munasinghe et al. [29] proposed the velocity dipole field and its integration with the conventional potential field to form a new realtime obstacle avoidance algorithm.Unlike the radial field lines of conventional potential field, the velocity dipole field has elliptical field lines that navigate a robot more skillfully.It is useful to skillfully guide the robot around obstacles, quite similar to the way humans avoid moving obstacles.Conn and Kam in [30] proposed a method where the mobile obstacles can be considered as stationary in the extended world by assuming time as one of the dimensions.However, this method required the mobile obstacle trajectory to be known which is not applicable in real world.
The organization of this paper is as follows.In Section 2, properties and assumptions of path planning in this proposal are described.Section 3 introduces the proposed RPO algorithm for local path planning of mobile robots.Simulation and numerical results are provided in Section 4. Finally, in Section 5 we draw some conclusions and discuss about the proposed path planning method.

System Modeling and Problem Formulation
In path planning problem of mobile robots, obstacle avoidance and target seeking are two fundamental behaviors that must be controlled.On the other hand, path planning of mobile robots in the unknown environments with fixed/moving target is one of the difficulties in this area.As shown in Figure 1, the robot is controlled to track fixed/moving target while avoiding the moving obstacle.The obstacles and the target are moving arbitrarily during the mission.In addition, moving obstacles positions are also unknown unless they are within the robot's sensor detection range.Sensor detection range is defined according to allowed sensory distance to obstacles.A Gaussian function is assigned to each detected obstacle.Furthermore, in mobile robotics, the most common approach is to assume that the robot is holonomic (omnidirectional) and by further assumption it is simply a point, so the configuration space is reduced to a 2D representation with ,  axis.Since we have reduced the robot to a point, we must inflate each obstacle by the size of the robot's radius to compensate.
Differential equation of robot's motion is described by q () = (), where  ∈  2 is control input and () = [(), ()]  ∈  2 is the robot's position at time  and assumed to be known all along the mission, and  0 ( 0 ) = [ 0 (),  0 ()]  ∈  2 is the robot's initial position.The target position is also defined by

Robot Path Planning in Dynamic Environments
In this section, an algorithm inspired from bacterial foraging mechanism is designed for local path planning of a mobile robot in dynamic environment.First, we briefly review a bacterial foraging technique and then the random particle algorithm is proposed.

Bacterial Foraging Optimization
Algorithm.Bacterial foraging theory is based on the assumption that animals search for and obtain nutrients in a way that maximizes their energy intake  per unit time  spent foraging [31].Hence, they attempt to maximize their long-term average rate of energy intake.
The main target in this algorithm for each bacterium is to find the minimum (),  ∈   without considering the gradient, ∇(), where  is the position of a bacterium, and () is an attractant-repellent profile from the environment.() < 0, () = 0, and () > 0 represent that the bacterium at location  is in nutrient rich, neutral, and noxious environments, respectively.In fact in this algorithm we deal with a nongradient optimization algorithm.Chemotaxis is a foraging behavior that implements a type of optimization where bacteria try to climb up the nutrient concentration (i.e., lower and lower values of ()) and avoids being at positions  where () ≥ 0 [1].
According to this algorithm, th bacterium at the position  takes a chemotaxis step  with the step size () in the random direction and calculate the cost function () at each step.If the cost function of the new position (,  + 1) that is, ((,  + 1)), is smaller than the ((, )), then another step size () in the same previous direction will be taken.This process in the direction of lower cost function will be continued until the maximum number of steps (  ) is reached.Then after each   chemotaxis step, the least healthy bacteria as stated by the cost function ((, )) are replaced by the copies of healthy ones.This procedure is called reproduction step, and it is followed by the eliminationdispersal ( ed ) event.For each elimination-dispersal event each bacterium in the population is subjected to eliminationdispersal with probability  ed .

The Random Particle Path Optimization Algorithm (RPOA).
The proposed random particle optimization (RPO) algorithm is based on particles randomly distributed around a robot.In initial position of mobile robot ( 0 ( 0 )), ( = 1, 2, . . ., ) virtual particles are generated and distributed randomly on a circle with radius of () around the robot position.Each particle position () in time  could be defined as (  () ∈  2 ) and the next position is calculated as where Δ() ∈  2 is a unit length random vector which is used to define the direction particle in each time.These particles in time  +  search the optimal path toward the target position while avoiding the moving obstacles by helping the robot's sensors.Then the best particle which has found the most optimal path among other ones is chosen and the mobile robot goes to the position of selected particle and it will continue again and again until the target is reached by the MR.For choosing the best particle, two different strategies are combined.Distance norm to target position and cost function (  ()),  ∈  2 which is an attractant-repellent profile from the environment and according to section two is defined as Gaussian function.

Decision Making Strategy of Finding the Optimal Particle.
When the mobile robot arrives at the moving obstacles, its sensors detect obstacles.Then, the algorithm virtually assigns a repellent Gaussian cost function to the obstacle by the following formula: where  obstc ,  obstc are constant values defining the height and the width of the repellent, respectively.  ∈  2 denotes the obstacle position detected by the robot's sensor.Note that, during the algorithm, whenever an obstacle is within the robot's sensor range,  obstc has a value unless otherwise its value is zero.In other words, repellent cost function all through the mission is defined as In which,  is a robot's sensor range.Since the target position is always known, we also assign an attractant Gaussian where  goal ,  goal are constant values defining the height and the width of the attractant, respectively.Total cost function is defined as sum of repellent and attractant cost functions as The objectives of path planning are to simultaneously (1) move the robot on the path that under the minimum time and cost arrive to the target while seeking it and (2) avoid robot to collide with unknown obstacles moving in the environment.These objectives have to be achieved in an unknown environment with online computation.In other words, this paper presents an online algorithm for path planning mobile robots in unknown environments without having prior information of obstacles and domain.

High Level Decision Control.
High level decision control designed for path planning mobile robot is based on artificial random particles in which at time  ≥ 0 artificial particles are placed at the position of robot and at time + ≥ 0 where  is a small time, they are randomly located over the circle with radius () around the mobile robot.In this one-step-ahead algorithm, the particle which could find the best path at time  +  (whereas our objectives mentioned in Section 3.2 are satisfied) is selected as a best particle and robot with a low level decision control signal (it will be discussed at Section 3.2.3)will put on the point found by the best particle, and again from the new mobile position, a group of artificial particles are distributed and the process goes on again and again.Hence, artificial particles generated in the main CPU of mobile robot along with the information given from the robot sensors could search mobile surroundings and determine the robot's optimal path for one step ahead.A decision making strategy designed for finding the best particle among the others is based on both distance error to target (   ( + )) and cost function error    ().If   (),   ( + ) represent the distance from particle  to the target at time  and  + , and is defined as   () = ‖  () −   ()‖ 2 ,   ( + ) = ‖  ( + ) −   ()‖ 2 , respectively, each particle distance error and cost function error will be described as In time +, all the    (), for  = 1, 2, . . .,  are computed and then particles are sorted in order of ascending error    () in a vector  sort such that the particle with the lowest distance error is at top.The first priority in this technique is to glide the robot to the target.Hence, particles are considered as fit ones that put on the higher part of vector  sort .Furthermore, among these fit particles, one is chosen that has negative error cost function    () with priority to the lowest distance error, because they must be kept away from obstacles, and having    () > 0 according to (3) represents that particles are in the region of obstacles.In fact, in this decision making strategy for finding the best particle, a search on distance error    () from top to bottom is performed and the first particle with    () < 0 is selected as the best particle.By using these two techniques together, objectives are also satisfied.Note that, according to (3)-( 5) when an obstacle is not in the robot's sensor range,  obstc is zero and as a result −2 goal <    () < 0. In this situation, robot freely moves toward target by choosing the first element of vector  sort since all of them have    () < 0. But, when robot's sensor detect an obstacle,  obstc gets a value and consequently, −2 goal <    () < 2 obstc .In this case, the first particle, in the ascended ordered of    (), which has the negative error    () is selected as the best one.A modification could be performed on this strategy.In the above strategy, we have omitted particles with    () > 0 because they are in the region of detected obstacle.But to find the optimal path, we could let particles to somewhat approach to the obstacle in order to find that optimal path.Actually if a particle fall into the region of a detected obstacle, it will not be omited, but we allow that particle to approach nearby the obstacle to find the new optimal path.For this aim, we introduce a new parameter  which is a coefficient of  obstc in decision equation    () <  and let the particle located in the obstacle's region not to be omitted.

Low Level Decision Control.
Low level decision control of mobile robot is based on each time step.Control law is chosen as follows.
where  first () is the mobile robot position at time , and   best ( + ) is a certain particle position at time  +  which has the optimal position and is supposed to be selected by the algorithm and the robot will go through this particle position.To explicit this control law and to compare with classical control law such as PI or PD, it seems it operates as a P controller.

Simulation Results
In this section, we apply the proposed RPO algorithm on a mobile robot to find an optimal path for the robot to reach its target in the presence of obstacles.This is an online algorithm.In fact, we did not need any information and prior computation from the environment in which the robot performs its task.The only parameters that are obtained from the environment are the target position, robot position, and data from obstacle detection sensors which practically can be obtained from the sensors installed on the robot.All of the other parameters are selected by the operator.Then, the proposed algorithm is computed in the main CPU of the mobile robot, and it helps the robot to find optimal path.The number of obstacles and particles can effect on the real-time performances, but with high speed CPUs there is no problem in real-time performance.
To evaluate our results, in this section three different scenarios have been set up.In the first scenario, obstacles and target are assumed to be fixed.In the second one, the algorithm has been run on a dynamic environment with fixed target and unknown moving obstacles, and in the third one, both moving target and obstacles are evaluated.Furthermore, to assess the performance of the algorithm, a comparison to artificial potential field (APF) method is presented in both scenarios.
The algorithm parameters in simulation are set as shown in Table 1.
As mentioned in Section 3.2.2, an attractant artificial Gaussian function was donated to the target all through the mission.On the other hand, when an obstacle is detected by mobile robot sensors, a repellent artificial Gaussian function is assigned to the obstacle.Then, the total cost function is computed as a sum of attractant and repellents Gaussian functions.The parameters selected for the Gaussian cost functions are as in Table 2.The parameters for simulation are defined arbitrarily, but in real situation the dimension of obstacles can be estimated by some sophisticated devices like radars and so forth.

Fixed Obstacles and Target.
In the first scenario, all the obstacles and the target are assumed to be fixed by the position given in Table 3.Note that obstacles positions are unknown and are detected by the robot's sensors.
The result has been shown in Figure 1.The red circles around the mobile robot show the robot sensor range.Whenever, an obstacle is detected by robot's sensor, the algorithm dedicates a Gaussian function to it and makes the robot change its path to avoid it while it remains on its optimal path.Figure 2 shows these Gaussian functions used by the algorithms.To compare the proposed RPO algorithm by the conventional artificial potential field technique, a simulation has been performed on the same condition.Equations ( 8) and ( 9) are the most used potential field formula.In the AFP method, we suppose that all of the obstacles are unknown until they are within the robot's sensor range, and target positions are known, and attractive potential function is defined as where  = 0.2 is some positive constant scaling factor from [22], and the repulsive potential function is defined as where   is a positive constant, and it represents allowance distance to obstacle.In this simulation,   is selected same as  sensor to make both algorithms the same. = 10 is a positive scaling factor.  () is defined as   () = (‖() −  obstc ()‖).
Figure 3 shows potential filed considered for this simulation.
Figure 4 shows paths ran by the both algorithm to reach to the target.The red line shows the path that robot has followed due to the presented algorithm and the blue one refers to the path generated by using the artificial potential field (APF) method.Total virtual force exerted to the robot is also shown in Figure 5.As we expected, in AFP target total force is zero.An important drawback of APF method is getting stuck in local minima.In some certain conditions, the potential field method results in minima other than the target in the plane which absorb the robot, so the robot may get stuck in these minima and could not reach the target.Producing attracting points other than the targets is an inherent drawback of potential field based methods.In this simulation, if we change forth obstacle position to [, ]  = [4,4]  , in AFP algorithm, mobile robot will stuck in local minimum as shown in Figure 6 while mobile robot can find its optimal path to target by the RPO algorithm.

Random Moving Obstacle and Fixed
Target.In the second scenario the robot should reach to a fixed target on a dynamic environment with unknown moving obstacles.There are six unknown obstacles in the environment which move randomly.The red circles around the mobile robot show the robot sensor range.Since in this case we deal with moving obstacles, we increase sensor detection range to  sensor = 2 × 1.2 m so that the collision avoidance of mobile robot with moving obstacles is guaranteed.  is selected same as  sensor to make both algorithms the same, and other parameters are the same as the previous one.Figure 7 shows the obtained results.Although the proposed algorithm is online and one step ahead but it generates a near optimal path while the APF generated path is much longer.Generated paths by both algorithms as well as virtual force exerted on robots by the AFP are also shown in Figure 8.In this section, if one of the obstacles positions rotates around the goal, in AFP algorithm, mobile robot will stuck in local minimum and with the constant rate rotate around the target and never gets it while the proposed RPO algorithm can find its optimal path to target by the RPO algorithm.These results are shown in Figure 9.

Random Moving Obstacle and Target.
In the third scenario the robot should reach to a moving target on a dynamic environment with unknown moving obstacles and a moving target.There are six unknown obstacles in the environment which move randomly.Sensor detection range is  sensor = 2 m, and   is selected same as  sensor to make both algorithms the same.Other parameters are the same as previous simulations.Figure 10 shows the obtained results.The red circles around the mobile robot at the target show the robot sensor range.The green and violet circles represent paths ran by moving target and obstacles, respectively.Exerted virtual force by the APF and spanned paths by both algorithms are shown in Figure 11.

Conclusion
Local path planning of mobile robots was addressed in this paper.In comparison to artificial potential field method, the RPO gives much optimal solution in shorter time.Furthermore, it is free from some drawbacks such as trapped states or gradient of potential functions which could provide impulsive force to the robot.

Figure 1 :
Figure 1: The proposed RPO algorithm in fixed obstacle and target.

Figure 2 :Figure 3 :
Figure 2: Gaussian cost function used by the algorithm for obstacles and target in RPO.

Figure 6 :
Figure 6: Fixed obstacle and goal position.(red) The proposed RPO algorithm, (blue) APF algorithm got stuck in local minimum.

Figure 7 :
Figure 7: Moving obstacle and fixed goal position.

Figure 8 :
Figure 8: (a) Spanned paths by the robot with two algorithms.(b) Exerted virtual force to the robot in APF method.

Figure 9 :
Figure 9: (a) Moving obstacle and fixed goal position.(b) Spanned paths by the robot with two algorithms.

Figure 11 :
Figure 11: (a) Spanned paths by the robot with two algorithms.(b) Exerted virtual force to the robot in APF method.