Artificial Potential Field Algorithm Implementation for Quadrotor Path Planning

Potential field algorithm introduced by Khatib is well-known in path planning for robots. The algorithm is very simple yet provides real-time path planning and effective to avoid robot’s collision with obstacles. The purpose of the paper is to implement and modify this algorithm for quadrotor path planning. The conventional potential method is firstly applied to introduce challenging problems, such as not reachable goals due to local minima solutions or nearby obstacles (GNRON). This will be solved later by proposed modified algorithms. The first proposed modification is by adding virtual force to the repulsive potential force to prevent local minima solutions. Meanwhile, the second one is to prevent GNRON issue by adding virtual force and considering quadrotor’s distance to goal point on the repulsive potential force. The simulation result shows that the second modification is best applied to environment with GNRON issue whereas the first one is suitable only for environment with local minima traps. The first modification is able to reach goals in six random tests with local minima environment. Meanwhile, the second one is able to reach goals in six random tests with local minima environment, six random tests with GNRON environment, and six random tests with both local minima and GNRON environment.


I. INTRODUCTION
The Artificial Potential field is a path planning algorithm for moving the robot from the initial to the goal point by the artificial potential field method found by Khatib [1]. It provides simple and effective motion planners for practical purpose [2].
On the other side, quadrotors has become interesting topics on researches due to its functional uses such as surveillance robots [14] [15], autonomous target tracking [16], bridge crack detection [17], object inspection [18] and autonomous transportation systems [19]. However, there are only a few researches about potential field algorithm implementation on quadrotor path planning [27].
In 2016, Woods et al. conducted a study of potential field controls to be applied to the quadrotor with Ardrone type [20]. It proposed an extended APF-based algorithm to make quadrotor able to avoid collision with obstacles in aerial space. Unfortunately, the research had not provide any experiment with local minima or obstacle near goal environment. Meanwhile, at the same year, Mac et al using artificial potential field algorithm applied to quadrotor with Ardrone type to simulate it with gazebo simulator in the robot operation system (ROS) software [21]. The study conducts varies tests from known area to unknown obstacles. Yet, the XY-coordinates in the test are on positive axis only; there is no test for coordinates on negative axis. Therefore, there is no guarantee that the result will be the same if the initial, obstacle, and goal position changes to negative axis. Besides, every test was conducted only at the same starting positions.
The random force algorithm was studied by Lee et al to create new potential functions for to solve the problem of symmetrically aligned robot-obstacle-goal (SAROG) and the problem of local minima of the potential field algorithm [22]. By using the new potential function mobile robot can pass local minima and SAROG to reach the goal point. The local virtual target method was investigated by Zou & Zhu to create local virtual target attraction in the artificial potential field algorithm to avoid local minima so that the robot can reach the final point [23]. The concept of virtual obstacles was studied by Liu Chengqing et al. to modify the artificial potential field algorithm to close the local minima [24].
The research conducted and presented in this paper is different from the research previously mentioned. This paper presents the potential field algorithm in which the attractive and repulsive forces have been modified by using virtual potential so that when applied in the quadrotor path planning, the quadrotor will be able to avoid GNRON, local minima, and static and dynamical obstacles quickly.

II. ASSUMPTIONS
These following assumptions are made to simplify the analysis of the research: Assumption 1, the shape, position, and the speed of the robot is known; Assumption 2, the position

III. QUADROTOR CONTROL MODELING
The quadrotor is an unmanned aircraft capable of moving upward by using the thrust generated by the rotation of the four propellers driven by electric motor rotation as shown in Fig. 1 which is the model of Mahony et al. [25]. As an unmanned aircraft, the quadcopter is often used as a means for monitoring, rescuing, and some military purposes. Like helicopters, the quadcopter has some advantages that are it can fly and land vertically in a narrow area. Also, a quadrotor can do rotation or movement easier. Fig. 1 shows that the quadrotor has a body frame (x b , y b , z b ) in the global frame (x, y, z). The quadrotor movement is based on 6 degrees of freedom involving translational and rotation movements of the quadrotor position against the earth frame [y z φ θ ψ] T . Based on the Fig. 1, the non-linear quadrotor model can be made, where δ = φ θ ψ x y zφθψẋẏż T . Thus the models of the quadrotor are as follows, where φ is the angle of the roll, θ is the angle of the pitch, ψ is the angle yaw,φ is the angular velocity on the x-axis,θ is the angular velocity on the y-axis,ψ is the angular velocity on the z-axis,φ is the angular acceleration on the x-axis,θ is the angular acceleration on the y-axis,ψ is angular acceleration on the z-axis.δ 4 =ẋ = δ 10 (4) where,ẋ is the speed on the x-axis,ẏ is the speed on the yaxis, andż is the speed on the z-axis. Then, the next equation of quadrotor model is as follows:δ where, τ x is the torque of the roll, τ y is the torque of the pitch, τ z is the torque of the yaw, I x is the moment of inertia of the x-axis, I y is the moment of inertia of the y-axis, and I z is the moment of inertia of the z-axis. The non-linear model of quadrotor based on the is as follows: whereẍ is the acceleration on the x-axis,ÿ is the acceleration on the y-axis o,z is the acceleration on the z-axis, m is the quadrotor period, and T is the thrust. Some earlier researchers have designed a quadrotor control system by separating the non-linear quadrotor model into four subsystems as has been conducted by Ajmera & Sankaranarayanan [29]. There is four equations of the quadrotor control subsystem namely full-state feedback control system for roll τ x , pitch τ y , yaw τ z , and thurst T subsystems.
Full-state feedback for the roll subsystem is where k 1 , k 2 , k 3 , and k 4 are the constants of the full-state feedback control gain for the roll subsystem.
Full-state feedback for the pitch subsystem is where k 5 , k 6 , k 7 , and k 8 are the constants of the full-state feedback control gain for the pitch subsystem.
Full-state feedback for the yaw subsystem is where k 9 and k 10 are the constants of the full-state feedback control gain for the yaw subsystem.
Full-state feedback for thrust subsystems as follows: where, k 11 and k 12 are the constants of the full-state feedback control gain for the thrust subsystem. From the equation (13) -(16) about the full-state feedback control system, the PD algorithm control as shown in Fig. 2 can be designed. It can be seen that the there are four control subsystems namely roll, pitch, yaw, and thrust controls. Fig. 2 shows that the full-state feedback control system has four outputs namely roll torque for the roll subsystem, pitch torque for the pitch subsystem, yaw torque for the yaw subsystem, and thrust torque for the thrust subsystem. The full-state feedback control system for the roll has four inputs namely the distance, and the speed on the x-axis, the angular position and the angular velocity of the roll. Full-state feedback control system for pitch has four inputs namely the distance on the y-axis, the speed on the y-axis, the angular position and the angular velocity of the pitch. The full-state feedback control system for yaw has two inputs namely the angular position and angular velocity of the yaw. The full-state feedback control system for the thrust has two inputs namely the distance of the z-axis and the speed on the z-axis. It is seen in the Fig. 2 T is converted by using the matrix Γ to gain the rotor speed value Ω = [w 1 w 2 w 3 w 4 ] on the quadrotor system shown in the following equation IV. ARTIFICIAL POTENTIAL FIELD The artificial potential field (APF) algorithm is one of the algorithms used in robot path planning in which its force F AP F is the sum of the attractive potential field F att and the repulsive potential field F rep as shown the following equation: To apply the APF force to the robot, it is inserted into the linear speed equation on the kinematic robot [9]. The desired where the potential attractive equation is partially derived to the x and y-axis as follows: v att The equation of the Khatib's potential attractive U att (x, y) of [1] is as follows: where k a is the potential attractive constant, δ 4 , δ 5 is the position of the robot.
The desired speed equation in the repulsive force where the potential repulsive equations are partially derived to the x and y-axis as follows: The equation of the Sfeir's et al potential repulsive where k r is the potential repulsive constant, r O is the distance limit of potential repulsive influence, and ρ O is the closest distance between the robot and the obstacle as shown in Fig.  3.
The closest distance between the robot and the obstacle ρ O is where x or is the difference of the distance between the robot and the obstacle on the x-axis, and y or is the difference of the distance between the robot and the obstacle on the y-axis which the equation is as follows: The following is the desired speed equation for the APF Repulsive force v rep O on the x and y-axes: v rep www.ijacsa.thesai.org Thus, the speed equations of the x and y-axes in the APF are as follows: v apf The first problem found in this research is applying APF force to the quadrotor. This problem can be solved by substituting the speed equations on the x and y-axes of the APF into the full-state feedback control equations for the roll and pitch subsystems as shown in the following equations: Based on the equation, the block diagram control of artificial potential field as shown in Fig. 4 can be designed. It is seen that there are two artificial potential fields namely the artificial potential field of the x and of the y-axes. The artificial potential field of the x-axis has three inputs: the obstacle data input of the x-axis, goal data input of the x-axis and robot data input of the x-axis, while the artificial potential field of the y-axis has three inputs: obstacle data input of the y-axis, goal data input of the y-axis and robot data input of the yaxis. The output of the artificial potential field algorithm is the vector speed of the x and y-axes. The speed vectors of the x and y-axes are inserted into the PD control system of the quadrotor.

V. GNRON AND LOCAL MINIMA PROBLEM
Artificial potential field algorithm is one of the algorithms in the robot path planning method used to reach the goal point and avoid obstacles by using the magnetic force method that is the attractive force to reach the goal point and repulsive force to avoid obstacles in an unknown environment. The problems appear when it is applied to the environment that has many obstacles such as local minima area as shown in Fig. 5. From the picture, it is seen that Quadrotor stops in local area minima which are formed by two obstacles placed in parallel which has a gap between them in an unknown environment.
In the APF algorithm, there is an area called local minima that is the area where the attractive potential field F att and repulsive potential field F rep has the same value resulting in a zero value of the artificial potential field F AP F . The environment that has the local minima area can be tested with the graph of the algorithmic force as shown in Fig. 6. It is seen that the two obstacles located at points (8.8,8) and (8,8.8) have a repulsive force to reject the quadrotor. It is also seen that in front of the two obstacles there is a basin indicating a local minimum due to the value of artificial potential field artificial force is zero. Point (0, 0) is the initial position of the robot and the value of the artificial potential field is large because the attractive force is proportional to the distance. Fig. 6 shows that point (12,12) has zero value of artificial potential field force because it is the goal position of the robot.
In the APF algorithm there is an area called local minima which is the area where the attractive potential field F AP F and repulsive potential field F rep have the same value resulting in a zero value of the artificial potential field F AP F . To avoid the area, the author proposed a modification of the artificial The equation of virtual potential U vir (x, y) is as follows: where k v is the potential repulsive constant.
The desired speed equation for the virtual potential force v vir O on the x and y-axes is as follows: Thus, the desired speed equation for the repulsive artificial potential field force v rep O on the x and y axes is An environment that has a local minima area can be eliminated by the equation of the repulsive force of the artificial potential field v O on the x and y-axes. The environment using the algorithm is tested with the algorithmic force graph as shown in Fig. 7. It is seen that the two obstacles located at points (8.8, 8) and (8, 8.8) have a repulsive force to reject the quadrotor. It is also seen that in front of the two obstacles there is no basin indicating that there is no local minima. Point (0,0) is the initial position of the robot and the value of the artificial potential field is large because the attractive force is proportional with the distance. The Fig. 7 shows that point (12,12) has zero value of artificial potential field force because it is the goal position of the robot. Besides local minima traps, there is another problem in the artificial potential field algorithm that is Goal Not Reachable when Obstacles are Nearby (GNRON). In this case the point is located near the obstacle causing the quadrotor is unable to reach the goal point as shown in Fig. 8. It is seen that the obstacle located at point (10, 10) and the goal point is located near the obstacle at point (8,8). Fig. 8 shows that the quadrotor stops at the goal point and cannot reach the goal point due to the attractive potential value is the same as the repulsive potential.
GNRON is the area where the goal point is near the obstacle so that the quadrotor cannot reach the goal point because the attractive potential field F att and repulsive potential field F rep have the same value resulting in a zero value of the artificial potential force field F AP F . The goal near the obstacles is tested using the algorithmic force graph as shown in Fig. 9. It shows that the obstacles located at the point (12, 12) has a repulsive force to reject the quadrotor. It is seen that a basin near the obstacle is the robot goal point (10, 10) which has a zero value of artificial potential field. However, near the robot goal point, there is another basin at point (9,9) which has zero value of artificial potential field causing the robot stop at that point and cannot reach the goal.
To reach the goal in a GNRON environment, the artificial potential field algorithm has been modified using the virtual potential force. The environment using the algorithm is tested by the algorithmic force graph shown in Fig. 10. Fig. 10 shows that the obstacle is placed at point (12, 12) has a repulsive force to reject the quadrotor. It is seen that there is a basin near the obstacle. The goal point of the robot at (10, 10) is in the basin and it has a zero value of artificial potential field. By using the modification of artificial potential field and virtual potential force, there is no basin, however, the robot cannot reach the goal because it is obstructed by the obstacle.
Based on the figure of the force graphic of the artificial potential field algorithm a shown in Fig. 10, the artificial Fig. 11. GNRON environment potential field U rep (x, y) algorithm is modified as follows: where ρ G is the distance of the robot to the goal point shown in Fig. 11. It is seen in the figure that the robotic goal point is located near the obstacle so that the quadrotor cannot reach the goal point, hence, the artificial potential field repulsive ρ G is modified.
The distance between the robot and the goal ρ G is where x Gr is the difference of the distance between the robot and the obstacle on the x-axis, and y Gr is the difference of the distance between the robot and the obstacle on the y-axis which equation as follows: The desired speed equation for the artificial potential field repulsive force v rep O on the x and y-axes is as follows: v rep From the new artificial potential field equation, the environment that has a goal point close to the obstacle is tested by algorithmic force graph shown in Fig. 12. In Fig. 12, it is seen that the obstacles located at the point (12,12) have a repulsive force to reject the quadrotor. It shows that there is a basin in front of obstacles. The basin is the area where there is the robot goal point at (10,10) which has an artificial potential field style equal to zero. By using artificial potential field algorithm that has been modified by the author, there is no more basin in front of the robot goal point that causes the robot stop so that the robot can reach the goal.

VI. RESULTS AND DISCUSSION
The experiments presented in the paper are made with Peter Corke's quadrotor model [26], simulated in MATLAB simulation software. An algorithm then is applied to the quadrotor model to see its performances result.
There are three kinds of experiments represented in the paper. First, the original APF proposed by Khatib [1] is tested in MATLAB simulation. The algorithm is tested to local minima and GNRON environment. The performance results are analyzed to verify any found issues. Hence, the algorithm will be modified to solve the issues. There is two types of modified algorithms proposed in this paper. The first proposed In the first experiment, the original APF was tested and applied to the quadrotor in an unknown environment where two static obstacles are located at points (-6.6) and (5, 5) as shown in Fig. 13. Fig. 13 shows that the quadrotor was tested five times by changing the starting and goal positions. The list of starting and goal positions are presented in Table I. In the first two tests, the obstacles, starting point, and destination point are located in a straight line.
The first test has local minima trap near the obstacles. Meanwhile, the second one does not have any local minima trap. The result shows that the quadrotor still able to avoid the local minima trap in the first test and reach goal points in all tests. However, the quadrotor does not move smoothly enough when avoiding the obstacle with local minima trap. In reality, the green line trajectory in the first test can be interpreted as the quadrotor moves back and forth in rapid movements. This can be bad for the quadrotor's safety.  Next, the original APF was applied to the quadrotor in an environment where the goal point is (-4,4) and a local minima area generated by two static obstacles located at point (-0.8, 0.8) and (0.8, 0.8) as shown in Fig. 14. The quadrotor is tested five times by changing the quadrotor starting point, and the list of starting points are shown in Table II. The result shows that the quadrotor reaches the goal point and able to avoid the local minima trap in all tests except in the second test.
The original APF then is applied to the quadrotor in a GNRON environment where there is a goal point located at   2) and a static obstacle located at point (0,0) as shown in Fig. 15. The quadrotor is tested six times by changing the quadrotor starting position as listed in Table III. The result shows that there is no quadrotor to reach the goal point in all tests.
The quadrotor was not able to avoid the obstacle on the second test in the environment with local minima trap because of the straight line positions of start, obstacles, and goal points. Hence, the repulsive force is equal to attractive force or it can be assumed that the total forces happen to the quadrotor is equal to zero. There will be any force to move quadrotor toward the goal point and the quadrotor is trapped in local minima.
The quadrotor was not able to reach goal points located near obstacle in the environment with GNRON because its repulsive force is so much bigger than its attractive force in every test. Hence, the quadrotor is pushed to somewhere near goal points outside the repulsive force area of an obstacle. happen to the quadrotor must not be equal to zero when it has not reached the goal point. To make it happened, a virtual force is added to the repulsive force. This equation contains a certain constant multiplied with the distance between the quadrotor and the obstacle. Hence, local minima can be eliminated.
Next experiment is to test the first proposed modified algorithm. The proposed APF with virtual force was applied to the quadrotor in an environment with local minima area generated from two static obstacles located at the point (-0.8, 0.8 ) and (0.8, -0.8) with a goal point (4,4) as shown in Fig.  16. The quadrotor is tested six times by changing the initial position of the quadrotor as listed in Table IV. Fig. 16 shows that the quadrotor can avoid obstacles which have local minima area and moves toward the goal point in all tests. It also shows that the trajectory of the quadrotor is safe enough, and there is no back and forth rapidly movement.
Then, the APF with virtual force was applied to the quadrotor in a GNRON environment where there was a destination point at a point (2, 2) and a static obstacle at (0, 0) as shown  Regarding previous experiment result's analysis, GNRON environment issue happens because there is no consideration of its distance to goal point in repulsive force. The repulsive force should be smaller when the quadrotor's distance toward the goal is smaller too. Therefore, the repulsive force should be modified by considering the quadrotor's distance to the goal point. This second proposed modification algorithm is called New APF.
The next experiment is the application of the New APF to the quadrotor in an environment where goal point is located www.ijacsa.thesai.org  Table VI. It can be seen in Fig. 18 that the quadrotor avoids the area of local minima generated from two adjacent obstacles and moves toward the goal point in the first test. This result also provides better trajectories than two previous experiments. The trajectory is the smoothest among algorithms. The quadrotor can reach the goal point in all tests. Next, the New APF was applied to the quadrotor in GNRON environment where the goal point is located at point (2, -2) and a static obstacle is located at point (0,0) as shown in Fig. 19. The Fig. 19 shows that the quadrotor is tested six times by changing the initial position of the quadrotor, as seen in Table VII. From Fig. 19, it is seen that the quadrotor can avoid obstacles and can go to the goal point located near the obstacle. The quadrotor reaches targets in all tests.
In further analysis, some improvements can be seen by comparing this result with previous experiment's (APF with virtual force) result. Although it seems that it is safer to be passed by the quadrotor in previous experiment result, the trajectory was not optimal due to its unoccupied space between obstacle and the trajectory. In other words, the trajectory in previous result was safe but was not optimal in its mileage. However, this experiment trajectories are optimal by the mileage yet not really safe to be passed by the quadrotor.  Last but not least, the New APF was applied to the quadrotor in an environment with local minima trap and GNRON. The goal point is located at point (2, -2) and a local minima generated from two static obstacles located at the point ( -0.8, 0.8) and (0.8, -0.8). The quadrotor is tested six times by changing the initial position of the quadrotor as listening in Table VIII. The result is shown as in Fig. 20. It is proved that the quadrotor can avoid local minima traps and to reach the goal point near the obstacle. The quadrotor reaches the target in all tests.
Uniquely, the trajectories made by this experiment result is not only safe enough to be passed by the quadrotor but also provides optimal trajectories by considering the mileage. The trajectories are safe because there is still some distance with the obstacle and the trajectories are smooth enough (no sharp turns or sudden maneuvers). However, the distance with the obstacle is not too far so it is still optimal enough.  This paper presents modified Khatib's potential field algorithm applied to the quadrotor. There are problems related to the Khatib's algorithm that is the attractive force to the goal located near the obstacle is not functioned because there is a repulsive force from the obstacle. The other problem is that the repulsive force used to avoid the obstacle make the quadrotor stop. To overcome the problem, the attractive and repulsive forces of the Khatib's potential field algorithm was modified. With the modification of the algorithm, the quadrotor can quickly avoid the static and dynamic obstacles and the local minima compared to the Khatib's algorithm.

APPENDIX NEW ARTIFICIAL POTENTIAL FIELD
The new artificial potential field U rep (x, y) algorithm is modified as follows. The APF because of the position is,