(Microsoft Word - \335\321\307\323 \346\307\323\343\307\301124-136) Al-Khwarizmi Engineering Journal Al-Khwarizmi Engineering Journal,Vol. 13, No. 4, P.P. 124- 136 (2017) Applying A* Path Planning Algorithm Based on Modified C-Space Analysis Firas A. Raheem* Asmaa A. Hussain** *,* *Department of Control and Systems Engineering/ University of Technology *Email: 60124@uotechnology.edu.iq **Email: asmaa.alshawi@yahoo.com (Received 25 October 2016; accepted 7 March 2017) https://doi.org/10.22153/kej.2017.03.007 Abstract In this paper, a modified derivation has been introduced to analyze the construction of C-space. The profit from using C-space is to make the process of path planning more safety and easer. After getting the C-space construction and map for two-link planar robot arm, which include all the possible situations of collision between robot parts and obstacle(s), the A* algorithm, which is usually used to find a heuristic path on Cartesian W-space, has been used to find a heuristic path on C-space map. Several modifications are needed to apply the methodology for a manipulator with degrees of freedom more than two. The results of C-space map, which are derived by the modified analysis, prove the accuracy of the overall C-space mapping and construction, and then a successful and guaranteed path from a start to goal configuration has been obtained without any collision probability. The results had been achieved by (Matlab R2015a) software, which run on Intel (R) Core (TM) i3-3120M CPU. Keywords: Configuration Space, Degrees of Freedom, Topology, Path Planning. 1. Introduction Due to the importance of path planning and obstacle avoidance problems in practical areas, there are more researches and studies about it in the last decades. In the classical mechanics, the configuration space (C-space) represent very important tool to describe and analyze the motion planning for many important systems, especially in robotics [1]. To understand the C-space, first it is important to know what the term of configuration means in robotic systems. It means a complete description, which determines every point position on the robot uniquely. So, the C-space of a robot typifies the space of all configurations, which are processed by the robot. As the robot has n degrees of freedom, the C-space for this robot is n- dimensional [2]. C-space deals with static obstacles, or dynamic obstacles with slow motion. Also, the shape and dimensions of each obstacle must be known before finding C-space construction if the path planning that will be used is off-line. From the first time the C-space was defined [3], it played very important role in robotic systems especially in path finding problem. Then, the usage of C-space continue in many researches and applications. Until today, more analysis and modifications with many type of robots are applied on C-space [4- 7]. Path planning for robot arm requires optimized global path generation, in which the robot arm can avoid any obstacle in the Cartesian W-space. The feasibility of path planning depends on some factors: map accuracy, number of obstacles and current configuration of the robot. There are many approaches and algorithms have been proposed to solve the path planning problem. This problem is related with finding the shortest path between the start and goal configurations. The most known Firas A. Raheem Al-Khwarizmi Engineering Journal, Vol. 13, No. 4, P.P. 124- 136 (2017) 125 algorithms that are being used to find shortest path are graph search algorithms. These algorithms are based on node-edge marking [8]. Graph search methods speculate the cost and generate a sequence of segments that extent from the start point to the goal point through vertices [9]. One of the graphical search methods is the A* algorithm. It is a graph, heuristic and greedy search algorithm, that means the determined solution is approximated not optimal (it is Best-First Search) [10]. The main advantage of A* that it can be modified easily and by change the weights of the traversal costs, it can be extended [11]. Usually A* algorithm is applied to find the path on Cartesian W-space, while in this paper, a different approach adopted by the application of A* algorithm on C-space analysis, which includes a modified derivation based on geometrical analysis to find the construction of C- space for two revolute joints planar robot arm. 2. Modified C-Space Derivation For a manipulator, the space that represented by the variable parameters of joints, either slide position or rotation angles, is called C-space. It includes two main regions: C-free and C-obstacle, in which the second region is a prohibitive because it represents collision between the robot arm and obstacles and between the links of the robot arm [4]. The geometrical representation of the obstacle (s) can be denoted as �, so the configuration space obstacles will be as below [1]: ���� = { : �� ∩ � ≠ ∅} … �1 While C-free can be represented as below [1]: ����� = { : �� ∩ � = ∅} … �2 Where: ���� : C-space obstacles : Robot configuration �� : The set of points of the space confined by the robot at configuration The C-space as overall can be defined as [1]: ������ = ���� ∪ ����� … �3 The mathematical study of the surface properties, which are do not change if this surface has been applied for certain deformations, such as bending or stretching, is called topology of space. The topology for robot arm with two revolute joints can be described as [12]: �� ∗ �� = ! … �4 The above equation means that the C-space for two revolute joints robot arm is a torus (with no joint limits for the joints). Where �� is the circle description, and ! is the 2-dimensional torus surface [12]. Each joint angles pair is corresponded by a unique point on the torus. (a) 2R Robot Arm (b)2Torus (c)Sample representation Fig. 1. Topologically representation of the 2- dimensional C-space [12]. To find the mapping of C-space, there are some basic shapes of the obstacle(s) will be analyzed. 2.1. C-Space Construction for Point Obstacle Mathematically, the simplest obstacle may be found in the workspace is the point. In this paper, a modified derivation for C-space analysis will be discussed, in which the collision checking based on geometrical analysis. 1) First Link-Point Obstacle A collision between the first link and the point obstacle will acquire if the following condition achieve: #��� = #� $%& '� ≥ ) $%& *� ≥ % … �5 Where: d��� = -./ 0./ ; #��� =tan .� - 0 ; … �6 '�, *�: First link end effector coordinate ), %: Point obstacle coordinate Firas A. Raheem Al-Khwarizmi Engineering Journal, Vol. 13, No. 4, P.P. 124- 136 (2017) 126 2ᶿ Point Obstacle First Link Reachability Circle 1ᶿ obsᶿ Fig. 2. The first link with the point obstacle geometrical analysis. For Fig. 2, the coordinate of point obstacle is inside the first link reachability circle. If the slope of the first link equal to the slope of the point obstacle, the collision is acquire. 2. Second Link-Point Obstacle The following mathematical derivation explains the condition of collision between the second link and point obstacle: 7 = 8 − #! … �7 ; = #��� − #� … �8 By sine's law: => ?@A B = CDEF ?@A G H = sin.� =>∗?@A G CDEF … �9 While, L7 + ; + HN = 8 … �10 There is a collision between the robot and the point obstacle, where (7, ; and H) are the angles used for the analysis as shown in Fig. 3. Point Obstacle Fig. 3. The second link with the point obstacle geometrical analysis. These collision conditions are applied for any position of the point obstacle inside first or second link, not only at the end effector for each of them. 2.2. C-Space Construction for Line Obstacle The robot links can be handled as two lines, with regardless of their width. In our research, the modified derivation of C-space analysis includes two analyses for checking collision. Analysis One To implement the checking, there are several constraints can be put: 1) First Link – Line Collision In Fig. 4, if the first joint angle, at specific configurations, has values locate at the range of angles of end points for line obstacle, this may cause collision between the first link and the obstacle: #P� ≤ #� ≤ #P! … �11 Where: #PR : The angles of line obstacle end points, i = 1, 2. It is important to take in consideration that the length of the first link equal or more than the distance between the robot's base and the line obstacle, as in the following equation: S'�, *�T ≥ U'=V , *=VW … �12 S'�, *�T : The coordinate of first link end effector S'=V , *=VT : Matrix of line obstacle points j=1, 2, …, number of line obstacle points (a) Horizontal Line (b) Vertical Line (c) Sloping Line Fig. 4. Cases of first link – line obstacle collision. 1) Second Link – Line Collision As it was done in checking collision with point obstacle, the geometrical analysis can be used to find the collision angle between the second link and the line obstacle. Equations (5, 6, 7, 8, and 9) can be used but it is applied for the intersection point between the second link and the line obstacle. Firas A. Raheem Al-Khwarizmi Engineering Journal, Vol. 13, No. 4, P.P. 124- 136 (2017) 127 Fig. 5 illustrate the situations of collision between the second link and the line obstacle. obsᶿ obsᶿ obsᶿ obsd obsd (a) Horizontal Line (b) Vertical Line (c) Sloping Line Fig. 5. Cases of second link – line obstacle collision. Analysis Two Another method for checking collision by checking the equality of each link slop with the slop of each point in the line obstacle: For the first link: X>./ Y>./ = XZ[ ./ YZ[./ … �13 For the second link: X\.X> Y\.Y> = XZ[.X> YZ[.Y> … �14 An important condition must be considered for equations (13) and (14): the length of each link should be more than or equal to the distance of the robot's base to that point (for equation 13) or from the first link end effector to the point (for equation 14). 2.3. C-Space Construction for Polygonal Shapes Obstacle Any two dimensions shape formed by straight lines, is called polygon. It has three lines (such as triangle), four lines (such as square, rectangle, rhombic… etc.), or more [13]. So the same methods that have been used for line obstacle can be used here. 2.4. C-Space Construction for Circle Obstacle For a circle that is known in center ('� , *� ) and radius (rad), the collision can be checked by the following modified derivation of C-space analysis: The distance from the center of the circle to each point on the first and second link will be calculated: For the first link: &��] = ^�'� − ] ∗ '� ! + �*� − ] ∗ *� ! … (15) For the second link: &!�] = ^�'� − _! ! + �*� − !̀ ! … �16 _! = '� + ] ∗ a! ∗ cos�#� + #! … �17 !̀ = *� + ] ∗ a! ∗ sin�#� + #! … �18 d� = minS&��] T … �19 d! = minS&!�] T … �20 Where: ] = 0.1, 0.2, … , � It is important to denote that ] is the resolution step of deviation of each link, while � is the total length of link. Theoretically, the step ] may be taken 0.1, smaller than 0.1, or biggest. If the following condition verified, that means there is a collision between the circle obstacle and the robot arm: dR ≤ g$& … �21 h = 1, 2. The distances &R can be implemented by the form of deviation as below: &R� = S&R�, &R!, … , &Ri T … �22 j: Number of steps of deviation (]). Fig .6 show the distances that used in the analysis for checking collision with circle obstacle. )cy,cx( Fig. 6. Geometrical analysis of circle obstacle. 3. A* Algorithm A* algorithm is most familiar search algorithm that is used to find the shortest path. It represent an extension of Dijkstra's algorithm. The usage of heuristic is what make the A* different from another graph search algorithms. The heuristic provides an estimated distance from a current node to the goal node, it denoted by h(n), also the A* take in account the cost from the start node to the current node, and it is denoted as g(n). The cost function is determined as below [9]: f(n) = g(n) + h(n) … �23 Firas A. Raheem Al-Khwarizmi Engineering Journal, Vol. 13, No. 4, P.P. 124- 136 (2017) 128 For C-space map, each node represented by a pair of joint angles, n: (#�, #! ). The A* algorithm includes two lists: OPEN list (O_List) and CLOSED list (C_List). It choose the nodes that have minimum cost functions to create a path from start node to goal node. The fulfillment of this algorithm on C-space map can be explained briefly by the following flowchart: C-Space Construction Discretizing C-space map Initialization of Start Configuration End Remove n_best at which f(n_best)<=f(n) from O-List to C_List End Expand all n_best neighbor nodes that not in C_List Add current node to O_List If g(n_best)+Path cost