Interactive Heuristic D * Path Planning Solution Based on PSO for Two-Link Robotic Arm in Dynamic Environment

This paper is devoted to find an intelligent and safe path for two-link robotic arm in dynamic environment. This paper focuses on computational part of motion planning in completely changing dynamic environment at every motion sample domains, since the local minima and sharp edges are the most common problems in all path planning algorithms. In addition, finding a path solution in a dynamic environment represents a challenge for the robotics researchers, so in this paper, a proposed mixing approach was suggested to overcome all these obstructions. The proposed approach methodology for obtaining robot interactive path planning solution in known dynamic environment utilizes the use of modified heuristic D-star (D*) algorithm based on the full free Cartesian space analysis at each motion sample with the Particle Swarm Optimization (PSO) technique. Also, a modification on the D* algorithm has been done to match the dynamic environment requirements by adding stop and return backward cases which is not included in the original D* algorithm theory. The resultant interactive path solution was computed by taking into consideration the time and position changes of the moving obstacles. Furthermore, to insure the enhancement of the final path length optimality, the PSO technique was used. The simulation results are given to show the effectiveness of the proposed method.


Introduction
The words "path planning" refer to find a path solution in robotics where the process is totally or sub automated.It is used to indicate the type of computational process for moving a robot from one place to another with respect to obstacles.In other words, it represents the search for initial feasible path which is considered as the first step to solve the path planning problem and includes finding a path for a robot that must move from initial given start point to the goal point which is also given as the destination position.The environment usually contains a fully de-fined set of obstacles.Therefore, the robot does not collide with any of the obstacles [1].Generally, obstacles cannot always be static.
When the obstacle changes its location over time, it called dynamic.Dynamic environment represents a revolution and important side in modern automation.
It redounds in many important diverse applications like air and sea traffic control negotiating, freeway traffic, intelligent vehicles, automated assembly, and automated wheelchairs.Because of existence the moving objects, the robot needs to make decision quickly for avoiding possible collisions [2] [3].There are three types of dynamic environment.The first type is called a known dynamic environment when all the information regarding the obstacles motions, sizes, shapes, and locations is completely known.The second type is called a partially known dynamic environment when not all information about the obstacles exists at the planning time.In this state, it needs to calculate the robot motion according to insufficient information about the environment.It is important to know when the robot's mission is to complete a map of the environment by scanning the area to make sure if there is a new object found.Finally, when there is nothing to know about the obstacles completely, it's called the totally unknown and dynamic environment and this needs some intelligent methods for sensing and making a proper decision [3].
Due to the rapid increase in the use of robots in different life and industry applications, path planning in dynamic environment is still considered as an actual challenge and needed to be solved interactively for both mobile and manipulator robots.The proposed method in this paper ensures finding an intelligent, proper, safe and relatively optimal path depending on the dynamic environment complexity.The structure of this paper contains seven sections in addition to the introduction, Related Research work, two-link robot arm kinematics, free Cartesian space analysis, proposed method, simulation results, and conclusions.

Related Research Work
To overcome the dynamic environment challenges, many path planning algorithms have been used in the literature.Here we started with Anthony Stentz [4] whom used The D* algorithm which is represented the dynamic A*, it aims to create a short path in real-time by incrementally reforming paths for the robot's state as a new information is found.The D* extension which concentrate the repairs of significantly reduction the whole time required to subsequent and calculate the initial path re-planning operations which this paper aims to, followed by Yueshi Shen and Knut Huper [5] who suggested using a method to calculate F. A. Raheem, U. I. Hameed the optimal joint trajectories for multi-DOF manipulator robot performing constrained motion task.The path planning is completed in two stages, firstly converting the variational calculus problem into a finite dimensional optimization problem, using Newton's method to compute the joint trajectory's inter-mediate points.Secondly, interpolating these points to form the joint curve and adjust it to ensure that the motion constraint will be satisfied throughout the entire trajectory.These calculations are easier to apply for multi-DOF manipulators than the Tow-Link robot arm because multi-DOF manipulators are more flexible.
Many other path planning algorithms were applied for solving dynamic environment problems to find an optimal path for robots in an environment that is only partially known and continuously changing.A method was suggested for generating a collision-free near-optimal path for an autonomous mobile robot in a dynamic environment containing moving and static obstacles using the neural network and fuzzy logic with a genetic algorithm.The neural network has a limited data size therefore, it cannot solve path in bigger environment furthermore, the simulation results show that the method is efficient and gives near-optimal and not optimal path reaching the target position of the mobile robot [6].An improved virtual force field (VFF) algorithm approach was proposed for the real-time robot path planning, where the environment is unknown and changing.An area ratio parameter with the consideration of the robot size and obstacles was used.Furthermore, a fuzzy control module was added to deal with the problem of obstacle avoidance in dynamic environments.Due to the local minimum problem and the higher computational complexity, the path could not reach optimality [7].
From the same point of view, an algorithm for specifying Cartesian constraints with a single and dual arm operation is proposed based on Rapidly-Exploring Random Tree (RRT).One parameter is used to be the common in all RRT based on planers is the range.The range represents a threshold for the samples drawn newly and their small distance values after that those will add to the tree.Range value will be affective on the planning time which means that it should be selected carefully since the higher values causing short time planning which is advantage but as a result, it also cause a tensive motions and the smaller values give a long time planning and it composed more points in the tree.Also, more checking will be needed for the collision that may occur in the space; therefore it needs extra collision checking [8].
Most of the existing literature in dynamic environment considers only the traditional path planning algorithms and there are only a few studies that consider the heuristic algorithms like Anirudh Vemula [9] whom uses a heuristic based graph search planning algorithm that uses adaptive dimensionality consider time dimension corresponding to region where potential dynamic obstacle collisions can occur.Their results show the approach can only model simple interactions and fail to generalize for complex crowded settings.
Inspired by the above discussion, the contribution of the proposed method in DOI: 10.4236/wjet.2019.71005F. A. Raheem, U. I. Hameed this paper can be summarized in the following points: 1) Multi free Cartesian space analysis has been computed at each motion sample to identify the feasible workspace area.
2) Modifying the heuristic D* algorithm by adding stop case and return backward case for taking into consideration the obstacles position and time which does not used before.These modification cases not included in the original D* theory.
3) Finding a path solution utilizing the proposed method in this paper that use the modified heuristic D* algorithm based on the PSO optimization technique in known dynamic environment.4) Applying the proposed method to simulate the path solution for the two-link robot arm as case study, which is more difficult than the mass-point case study usually used in path planning researches.
5) The resulting path solution from this proposed method can solve the problem of finding an optimal path in dangerous, harsh, toxic environments and inaccessible areas.

Two-Link Robot Arm Kinematics
Kinematics is the science of motion that treats the subject without regard to the forces that cause it.Two types of kinematics problems can be classified; the first one is the forward kinematics problem which is concerned with the relationship between the individual joints of the robot manipulator and the position and orientation of the tool or end-effector [10].The second is the inverse kinematics problem of the serial manipulators has been studied for many decades.It is needed in the control of manipulators.Solving the inverse kinematics is computationally expensive and generally takes a very long time in the real-time control of manipulators.Tasks to be performed by a manipulator are in the Cartesian space, whereas actuators work in joint space.Cartesian space includes orientation matrix and a position vector.However, joint space is represented by joint angles.The conversion of the position and orientation of a manipulator end-effector from Cartesian space to joint space is called an inverse kinematics problem [11].The planar manipulator is shown in Figure 1(a) to illustrate the 2-DOF.The planar manipulators have 1 l and 2 l as their link lengths and 1 θ and 2 θ represent joint angles with x and y as task coordinates.
The forward kinematics equations are shown below: ( ) ( ) In 2-DOF planner manipulator, the Geometric Solution Approach of inverse kinematics is used for obtaining the joint variables for all the points in Cartesian space regardless their orientation.The calculations of 1 θ and 2 θ for the 2-DOF manipulator could be found by applying the following equations.Utilizing the cosines law, where the angle is given by: To solve 2 θ , it has to use the 1 cos − function.Although it is more benefical to use 1 tan − for numerical solution, in the software implementation, we will use the function tan −1 (b/c) which is ATAN2 (b, c).This function has a uniform accuracy over the range of its arguments, returns a unique value for the angle depending on the signs of b and c, and gives the correct solution if b and/or c is zero [11].
Subsequently, the two conceivable solutions for 2 θ can be written as: At last, 1 θ can be solved by: ( ) where 1 l and 2 l are lengths of the first and the second links, respectively.

Free Cartesian Space Analysis
The result of inverse kinematics has been used to build and analyze the workspace in this thesis.The specific points that the end-effector can reach, can be defined as free Cartesian space (FCS).The angles ( 1 θ and 2 θ ) which were found by inverse kinematics are related to these specific points directly.The shape and volume of the FCS in an environment, which contains many obstacles, are changed depending on the number of obstacles, position and size in addition to the joints limits.In this paper, Table 1 shows the modeling and simulation.These restrictions impact and limit movement of manipulator and additionally split up the workspace into a reachable region and an inaccessible one.To find all possible solutions, we should compute the FCS and that done was by analyzing the environment's points.The center point and the diameter of the obstacles are the basic issues upon which the checking function depends.Moreover, there are three possible states for each point in the whole environment.
Firstly and as shown in Figure 1(b), there are two solutions to each point, the elbow up solution and the elbow down solution.But in the second state, there is just one solution, either elbow up or elbow down, and in the third one, there is no solution because the point lies out of the reachable area of the manipulator or on an obstacle, or the one or two links collide with an obstacle [12].
The resulted areas are separated into three parts, the first one contain the points which can be reached by the elbow up solution, and the second area contains the points which can be reached by the elbow down solution, and the third one represents the unreachable points that contain points outside the accessible range of manipulator workspace, points in obstacle's body and points that make collision with the obstacles.The biggest factor that affects the free Cartesian is the length of the arm link [12].

Proposed Method
where f is objective function and g is the estimated cost from start point while ℎ is the cost to the goal.
The velocity of the moving obstacle is calculated by: where x ∆ represents the change of distance in X-axis, y ∆ represents the change of distance in Y-axis and s T represents the sample time which assumed to be 0.01 sec.
After finding the D* path, the PSO optimization technique is used for enhancing the path, removing sharp edges and making it the shortest because the D* path has stairs shape sometimes.The principle of PSO technique depends on generated specified number of particles in random positions in the specified workspace, also the velocity of those particles is nominated randomly, each particle has a memory that stores all the best position have been visited before, in addition to the fitness in that position which has been improved over time [13].
In each iteration of PSO technique, the ij pos and , t i j vel vectors of particle i is modified in each dimension j in order to lead the particle i toward either the personal best vector ( ij pbest ) or the swarm's best vector The position of each particle (the point that is chosen from D* path) is updated by using the new velocity for that particle, according to equation below: where 1 c and 2 c are the cognitive coefficients ( 1 2 4 c c + ≤ ), and 1, j r and A large inertia weight (w) eases the local when its value is large and eases the global search when its value is small [14].
The first step to operate the PSO is to set the parameters of PSO

First Environment
Here we have applied the two-Link robot arm as a case study.The arm length is equal to the half of the environment with 50 cm for each link and the arm has 360 degrees.The obstacle motion behavior is shown in Figure 4. Since the multi FCS is applied as clarified in Figure 5, where the column (5(a)) represents the elbow down FCS analysis at some selected motion samples; 1, 4, 5, 7, and 8, while in the column (5(b)) and (5(c)) represents the elbow up FCS analysis and the gathering between elbow up and elbow down FCS analysis respectively at the same motion samples.In each FCS figure the colored area (blue, red or green) is represented the free and safe area for moving and the white area is represented the unreachable area.Therefore, it has calculated the elbow up and elbow down FCS to enable the robot from making decision which free space is the suitable for moving.After computing these calculations the robot arm could move more freely because it could detect with each FCS the free area to move on.Figure 6 DOI: 10.4236/wjet.2019.71005and change in these angles for the optimized path respectively.The same procedure will be done for the second environment.
These environments featured the interactive style motion where the robot should make a decision and must be stopped in some situations if it is needed, in this case, the stop state and the backward motion guarantee a collision free path.
Table 2 clarifies the environment information.Therefore, the robot can take a proper decision for selecting the next points in a free and intelligent path inside the permitted motion area.Figure 12   and the changes in these angles during the optimized path respectively.Also, Table 2 clarifies the environment information.

Second Environment
In comparison with the nearest research literature [4] that try to solve the same problem for mobile robot using the D* heuristic method, our proposed method has different results because of our original modification that applied on D* algorithm which guarantees the interactivity and safety of the path and because of finalizing the path using PSO optimization technique.

Conclusion
This paper proposed an optimized heuristic approach for solving the problem of two-link robot arm path planning in known position and time changes dynamic environment.This approach utilizes the use of heuristic D* algorithm for finding How to cite this paper: Raheem, F.A. and Hameed, U.I. (2019) Interactive Heuristic D* Path Planning Solution Based on PSO for Two-Link Robotic Arm in Dynamic F. A. Raheem, U. I. Hameed

DOI: 10 .Figure 1 .
Figure 1.(a) Two degrees of freedom manipulator; (b) The configurations of two-link arm elbow up and elbow down. .4236/wjet.2019.71005 : D* Algorithm Based on PSOAfter applying the D* algorithm to find the non-interactive path solution in known dynamic environment and studying the produced path, it has been proposed to make the environment interactive considering the position and time of the obstacles to enhance the path planning.Employing the D* algorithm in the known obstacle position and time dynamic environment, to find the robot path needs to build a map with its limitation, start and target locations and the obstacles locations and dimensions.The proposed planning for the two-link robot arm in dynamic environment is shown in Figure2.In the interactive path solution of known dynamic environment, in each move of the robot from node to node in each motion sample the free Cartesian space (FCS) will be calculated.As a result, the movement range of the robot will be defined and described accurately.It is a more intelligent behavior having multiple FCS with each motion sample the obstacles move.The new FCS, which was calculated, gives an indication to find the feasible area for the robot to move on.This process takes time but gives an accurate decision for the environment and high accuracy to the robot movement in the environment.Since the process is off-line and after constructing the environment, the algorithm will start its work when the robot firstly stands on the target node and starts the initial expand to its 8-connected neighbor.Each node will have an initial cost function where the robot will choose the least cost function node to move on and put it in the closed list.The other nodes will go to the open list DOI: 10.4236/wjet.2019.71005

Figure 2 .
Figure 2. Proposed planning for the two-Link robotic arm in dynamic environment.

r
are random real numbers between [0, 1], the inertia weight w controls the particle momentum.The value ofij vel is clamped to the range [ max max , vel vel − ] to reduce the probability of leaving the search space by the particle.If the search space is de-DOI: 10.4236/wjet.2019.71005F. A. Raheem, U. I. Hameed fined by the bounds [

( 1 c
= 1, 2 c = 3, w = 1) which are found by a trial and error technique to reach the best results.Now, the D* path enters the PSO work, which will represent the D* path as the search space and choose the optimal points from it taking into account the general shape for the D* path and without distorting the original form.After determining the PSO search space which is the D* path matrix, the search process begins with defining the number of via points (the points that is chosen from D* path) that search for the new points according to the search space dimension and the PSO parameters.These via points are chosen randomly from the search area (D* path) and will represent the position parameters in the PSO equation.The cubic spline interpolation equation has been used to connect the best chosen points from the PSO optimization technique to generate the corresponding smoothed path.The cost function of PSO, which is the path length, is computed.Then the elementary velocity is set, and updating the local and global parameters are the next step.This procedure is reiterated until the given points that are looked over the path are finished.The next step is updating the velocity and position equations, for converging the via points into the best cost, and also updating the local and global parameters.The procedure is repeated for the number of specified populations and the entire process is repeated for a number of specified iterations.The global values that construct the shortest path are then allocated.The entire process flowchart is represented in Figure3.After getting the smoothed path, the robot will move also interactively with position and time on the new PSO path.

Figure 3 .
Figure 3. Proposed D* via Point flowchart for position and time consideration dynamic environment.

Figure 8
Figure 8 shows the D* based on PSO path planning process from the initial state in part (a) where the end-effector is on the start location to the part (f) where the process has completed by reaching goal location.At part (b) where the motion sample equal 7 the arm has restarted backward to safe itself from collision.In addition, Figure 9 and Figure 10 represent the two link robot arm joint angles

For
Figure 7. (a) Final D* and PSO path and (b) the path cost function.

Figure 8 .Figure 9 .
Figure 8. Two-link robot arm result for the D* based on PSO path.

Figure 10 .
Figure 10.The change in joint angles for the optimized path.
Figure 11.FCS for the D* path and PSO path for the second environment where (a) represents elbow down, (b) represents elbow up and (c) represents gathering between elbow up and elbow down.

Figure 12 .
Figure 12.The two-link robot arm result for the D* path for the second environment.

Figure 13 .Figure 14 .
Figure 13.(a) Final D* and PSO path and (b) the path cost function for the second environment.

Figure 16 .
Figure 16.The change in joint angles for the optimized path of the second environment

Table 1 .
Theoritical range of the two-link robotic arm case study.