Cylindrical Manipulator Path Planning Among Static Obstacles Using Artificial Potential Fields

Abstract In this paper, path planning for cylindrical manipulator of 4-DOF is studied. Another view point is presented for using so called ‘artificial potential fields’ which is used as the base of searching for new and safe points in the manipulator’s workspace among static obstacles. Three vectors are used for safe manipulator navigation. The first vector is determined between the end-effector and the goal points and it is used to attract the end-effector to the goal point. While the second and third vectors are computed from points defined on the obstacle and the end-effector. These two vectors are used to repel the end-effector and the arm from the boundaries of the obstacles. In this work, the obstacles are suggested to have a cylindrical shape with different sizes. Displacement detections between the manipulator (its end-effector and arm) and the obstacles are used as sensors for collisions impending. A random movement is suggested for joint two to avoid contacting between the arm and obstacles. At the off – line path mode, all path points are determined by the presented method and some of them are updated, if an arm collision is detected, then joint variables are calculated at each point. In real mode, these joint variables are fed to a simple real control system to make the manipulator tracks the found path. The method gives a safe path for undertaken manipulator. An experimental work is also presented.

. " " . . is that the thermal resistance in the configuration space for all different orientations of the robot can be superimposed and this reduce a search on R n xSO(n) to one on R n followed by a search on SO(n).
Artificial potential field is not applied only to planning paths for serial manipulators; it has been applied to collision avoidance for humanoid robot arms by Sahara A, and Anzai [7] named "CAHRA". There is a difficulty when robot developers make motions of humanoid robot arms, because the right and left arms may collide each other. CAHRA uses a potential method with very small computational cost and can avoid collision between its arms in 97% without being nervous. Shimoda S. and Iagnemma K. [8] propose a potential field-based method for high speed navigation of unmanned ground vehicles (UGVs) on uneven terrain. The potential field is generated in the two-dimensional "trajectory space" of the UGV path curvature and longitudinal velocity. The proposed method is subjected to local maximum problems, rather than local minimum. The presented method succeeded in navigating a UGV between pre-defined waypoints at high speed, while avoiding unknown hazards.
Pervious approaches have made the path planning with artificial potential fields highly complex by employing distance functions that cause many local minima in the search methods for global minimum (the goal point). In this paper instead of using functions, vector methods have been used for searching for new and safe points in the manipulator's workspace and avoiding collision with the obstacles that are found in the workspace. Three vectors are suggested. The first one is calculated between the end-effector and the goal points and it is used to attract the end-effector to the goal point, the second one is produced between the probable contacting point on the obstacle's surface in the direction of the first vector and the end-effector point and it is used to repel the end-effector from the obstacle boundaries. Finally, third vector is found between the knowing (reference) point on the obstacle and the end-effector and it is used to repel the arm from the obstacle boundaries. Collision detection is a vital part of any path planner. Furthermore, because path planners spend most of their time on collision or distance queries, the efficiency of the collision detection algorithm will greatly affect the overall efficiency of the planner [9]. In this work, the collision detection between the manipulator (its end-effector and arm) and the surrounding obstacles is sensed by determining the distances between them, and this is done by finding the nearest point on the obstacle's surface that collision would occur with it in the direction of the first vector and find the distance between it and the end-effector. For the arm, the third vector is used to find the distance perpendicular to the arm from the obstacle's reference point. For multi-obstacles, the distances between the hand and each obstacle are found in the direction of the first vector and to reduce the computations, the above procedure is done for the closer one. In the beginning of the presented method, the straight line from the starting and goal configurations is checked, and if a part of the line is found to be outside the manipulator's workspace, a new goal point is proposed and named a virtual goal point that connects the start and goal points with straight lines within the workspace. This paper is organized as follows: kinematics and workspace of the manipulator are presented first while method principles and vector computations are introduced secondly. Then, collision detections and virtual goal point are explained. Focus on how joint variable computations and obstacle representations are done followed by Experimental works and results. Finally, conclusions are given.

2-1 Manipulator Forward Kinematics:
The coordinate frame assignment is depicted in Fig (1). Details about definitions of the coordinates and Denavit-Hartenberg (DH) parameters can be found in [10]. The current cylindrical manipulator has a 4 = 0 which differs from that is in [10] and this is done to reduce some computations. Following the DH methodology, the general transformation matrix (forward kinematics), which expresses the position and orientation of the gripper with respect to the arm's base (frame 0), is given as:   which is useful in pervious sections. In matrices (1) and (2), Cq i & Sq i denote cos(q i ) and sin(q i ) respectively, and a 2 , a 3 , d 1 , d 4 are constants and depend on manipulator dimensions and geometry. The joint variables are: the relative angles between links one and two q 1 , and between links three and four q 4 and links two and three extensions q 2 ,& q 3 .

Fig (1) Reference Coordinate Assignments and the Built in-House 4-link 4-DOF Cylindrical Manipulator
The built in-house cylindrical manipulator End-effector (gripper) x 4

2-2 Workspace:
The total workspace of a manipulator is defined as the space reachable by a reference point in the hand [11]. The forward kinematics DH conversion can be written as: where 0 n R is a (3x3) rotational matrix and (q) is a (3x1) translational vector. The position vector of a point on the end-effector of the manipulator can be written in the terms of joint coordinates as: ..(4) where X is a position vector in Cartesian space, q R n , q is a vector of joint variables and n is the number of DOF. For the current cylindrical manipulator, Eq (4) can be written as: ], for i, j: 1 n; i j. By substituting each set into Eq (5) and varying the third one, apart of the workspace boundary is drawn. Fig (2) shows the drawn workspace.

3-Method Principles:
In traditional artificial potential field methods, potential field U is assumed around the robot. When the robot is close to obstacles, the potential becomes large, and when the robot is close to goal point, the potential becomes smaller. Then the robot moves to the direction F which is the sum of an attraction vector F attract (x) to the goal and repulsion vector(s) F rep (x) from obstacles at the current position (see Fig (3)). In other wards, the robot moves to the direction where the potential U is smaller. In this work, instead of using potential function which can be a function of distance (for F attract & F rep ), a vector is used so that there will not be need of an optimization search method that can easily trap into local minima. The vector that attracts the end-effector to the goal configuration is computed between the end-effector point (the origin point of frame 4) and the goal point, it always directs to goal point, as following: are coordinate vectors of the goal and current end-effector points. To repel the gripper from obstacle's boundaries, a second vector is used. This vector is defined between the end-effector point and the probable collision point on the obstacle's surface in the direction of the first vector and it directs from the obstacle to the gripper. Eq. (7) gives the repel vector. (7) is computed and starts affecting the solution when a certain distance between the two points reaches a given value. That is: where D a is the distance between P O & P h and is given by Eq. (9) and is the influencing distance of the obstacle.
..(9) For each obstacle found in the manipulator's workspace and satisfies Eq (8), there will be a repel vector. After each vector is computed (including the third vector in the next section), the resultant of Eqs (6) & (7) is calculated and a new and save point can be found in the direction of the resultant as shown: .. (11) where R is the resultant vector, Ph)new is the new point (position) of the gripper, and i is a scalar determines the step size. It is important that i be small enough that the robot is not allowed to jump into obstacles while being large enough that it does not require excessive computation time. Therefore, i is considered to have variable values depending on the distance to the nearest obstacle (Eq (9)) and empirical basis.

4-1 Collision Detections:
A collision occurs when the robot contacts an obstacle in its workspace. If this workspace, that is, the Cartesian space in which the robot moves, is denoted by W and the obstacle region, the set of all points in W that belong to an obstacle i, is denoted by i, i.e. i W. Also, denoting the Cartesian space by X, the manipulator by and the sub set of the workspace that is occupied by the end-effector by (X). The set of Cartesian space for which the robot collides with an obstacle is referred to as the space obstacle and it is defined by: .. (13) in which = i . The remaining portion of the Cartesian space is called the set of collision-free space and is simply the set: Note that the definition of space obstacle in Eq (13) includes arm-collisions without selfcollisions, since; the undertaken cylindrical manipulator does not self-collide. The collision detections that are presented below are true if the obstacles, which have cylindrical shapes, stand with its base parallel to the x-y plane of manipulator reference frame (frame 0).

4-2 End-Effector Collision Detection:
The parametric equations of the line that joints P g & P h are calculated as: where t is a parameter. From Fig (4), the following computations can be done: where D hg , D Og are distances between P g & P h and P g & P rO respectively, P rO is the obstacle's reference point, is the angle between are the distance between a point (P C ) on h g P P to P g and the smallest distance from P C to the obstacle's ( i ) reference point respectively and P C is the coordinates of nearest point to P rO . Now, if P C i , this means that the end-effector is going to have collision with i in its direction toward P g . Therefore, the closer point on the i surface to the end-effector must be found. This is done by finding the parametric equations of the line that extends between P C & P h : where [ X] hC = X g -X C . By changing t in step of (0.01), a new point P(k) on the h C P P is determined and if P(k) i , a new point P(k+1) is found and so on until a point P(k+m) is computed that i , then, P(k+m-1) is an approximation of the closer point which is denoted by P O .

4-3 Arm Collision Detection:
From a point above P h directly at distance of a 3 (Fig (5)), denoted by P rh and P rO on i a vector is computed as: .. (18) where [ X] rh.rO = X rO -X rh ., and from P rh and the original point of frame (2), P o2 , another vector is noted as: where [ X] rh.o2 = X o2 -X rh . P o2 is given by the first three elements in last column in Eq (2). Fig  (5 , D rO.h is the distances between P rh & P rO , D rO.R is the perpendicular distance between P rO and the arm and D real is the projection of D rO.R on the x 0 -y 0 plane. After the shortest distance between the obstacle reference point and the manipulator arm is determined, it must be ensured that the point which locates above or down P rO with distance equal to z = (z h -z rO ), i.e. (x rO , y rO , z), i , Fig (5), if it does, and D real be R Ovi , q 2 must be actuated to avoid collision of the arm, where R Ovi = radius of i + F and F is a scalar. The movement of q 2 is suggested to be randomly as follows: A random step from q 2 is obtained by randomly adding and subtracting a small fixed constant u to q 2 at same time: where i = 1,2,…,m. At each direction, the arm collision is detected and if there is detection, Eq (21) is repeated with a step of 2u and so on until a collision in one direction is not detected. Then, that side is taken as a recommended movement for q 2 .
The above procedures for collision detections are repeated for every obstacle found inside the workspace. For multi-obstacles, the collision computations become very expensive in time and some computations are useless. To reduce the computations and save time, it has been suggested to have collision calculations in the direction of the first vector  Fig (6a

4-4 Virtual Goal Point:
When a part of the line that joining P h & P g , g h P P lies outside the manipulator workspace, the presented path planning approach fails to give a path because the points on the produced path W , and to avoid that, a so called virtual goal point is suggested. This point is connected to Ph from side and with P g from other side (Fig (7)). The lines that join the three points must be inside the workspace. For the current cylindrical manipulator, to find out if a part of g h P P lies outside the workspace, Eq (15) is used by eliminating the z-coordinate part and changing t at step of 0.1, is < r ib a part of the line lies outside the workspace where r ib is the radius of the inside workspace boundary. From Fig (7), the following computations can be done: where m is the mid point of g h P P . The parametric equations of lines that connect points: where B is a point on the outside workspace boundary, V g is the virtual goal point that produced after each changing in t m , and X = X B -X m , X = X Vg -X h , X = X vg -X g respectively. At each changing of t m , a new V g is determined at which the lines g h V P and g g V P must be checked if they are W and this is done as with g h P P using the last two parametric equations in Eq (24). In addition, V g must i , therefore, V g will be proper if: If conditions (25) are not satisfied for each value of t m , then point m is replaced by: ..(27) where 0 < n d < 1 is a random quantity. When the virtual goal point is presented, the path planning is divided into two parts; first part is from initial P h point to V g (P g ) secondary = V g ) where V g plays the turn of goal point, and second part from V g to P g (P g ) original ) where V g represents the P h point.

5-1 Joint Variables Computations:
The aim of any path planning algorithm is finding the manipulator configurations (joint variables) along the determined path. In the current implementation, three joint variables might be calculated (q 1 , q 2 , and q 3 ) since q 4 is used for changing the orientation of the end-effector only as can be seen from Eq (1). The inverse kinematics (IK) is the inverse problem of finding joint variables in terms of the end-effector's position and orientation. The presented approach gives only the end-effect' position (Eq (11)); therefore, using IK is not possible. To solve this problem, the following simple methods have been used: For q 1 : After P h ) new has been found, two vectors can be drawn from frame (0)  For q 2 : Movement in the z 0 -direction is the responsibility of joint two only. Therefore, its value can be found from Eq (5) as: Always 0 0 q 1 < 90 0 ( i is small) and if there is a division by zero (q 1 = 0 0 ) switching in using of Eq (29) is suggested.

5-2 Obstacle Space:
In last sections, X free is determined among obstacles that are assumed to have cylindrical shapes with different sizes and are located in the manipulator's workspace with their bases parallel to x 0 -y 0 plane. All points that W and i , must be defined to the path planner so that X be known. In this work, i is simply represented by a circle equation with z-coordinate for height as: where (g i , u i ) and R i & H i are the reference point, radius and height of i , respectively. (g i , u i ) is always assumed to be located at the base of i and the sign is used to ensure that all point i be represented.

6-Experimental Work and Results:
In this work, the manipulator movement from an initial configuration to final (goal) one is done among two modes. In the off -line path mode, a path is planned (if a one exists) through the given obstacles. At each point path, the three joint variables (manipulator configuration) are determined and saved in sets. In real-time mode, each saved set is sequentially fed as command signals into the control system so that the end-effector can track the path. Joint actuators, optical sensors and a PC (as a controller) form the control system. More details about the control system and interfacing can be found in [10]. The overall i path planning approaches are illustrated as a flow chart and diagram in Fig (8). Two different sets of initial and goal configurations are used as inputs to the path planner. In set one, calculation of virtual goal point is needed and in set two it is not. The resulted manipulator actions can be seen in Fig (9).

7-Conclusions:
This paper presents a path planning for cylindrical manipulator of 4-DOF based on artificial potential field. Instead of using functions for safe path searching, vectors have been used with distance computations for collision detections. The suggested vector based potential field does not need an optimization search method for searching of a goal point which can easily fall into local minima. It is possible to build X free through the suggested with non parallel base to the x 0 -y 0 plane (arbitrary) by introducing the Euler rotation matrix with respect to frame (0). The proposed collision detections (with little improving) can be used for other obstacle shapes. If the orientation of the end-effector is known (desired), it will be possible to use the proposed path planner with IK or any other improved solutions to plan a path for robotic arms with more DOF. For not very complex mediums of obstacle shapes and numbers, the vector based potential field can be used successfully for path planning.

Diagram of the two modes
Flow chart of off-line path mode Fig (8