A Path Planning Algorithm Using Generalized Potential Model for Hyper-Redundant Robots with 2-DOF Joints

This paper proposes a potential-based path planning algorithm of articulated robots with 2-DOF joints. The algorithm is an extension of a previous algorithm developed for 3-DOF joints. While 3-DOF joints result in a more straightforward potential minimization algorithm, 2-DOF joints are obviously more practical for active operations. The proposed approach computes repulsive force and torque between charged objects by using generalized potential model. A collision-free path can be obtained by locally adjusting the robot configuration to search for minimum potential configurations using these force and torque. The optimization of path safeness, through the innovative potential minimization algorithm, makes the proposed approach unique. In order to speedup the computation, a sequential planning strategy is adopted. Simulation results show that the proposed algorithm works well compared with 3-DOF-joint algorithm, in terms of collision avoidance and computation efficiency.


Introduction
Path planning of a robot is to determine a collision free trajectory from its original location and orientation (called  2008)). A point in a c-space indicates a configuration of a robot which is usually encoded by a set of the robot's parameters; e.g., joint angles between the robot links. Thus, path planning of articulated robots is reduced to the problem of planning a path from the start to goal point in free space of the c-space. Unlike c-space based approaches, geometric algorithms directly use spatial occupancy information of the workspace (w-space) to solve path planning problem (Khosla, P. & Volpe, R. In general, the potential function used to model the environment can be a scalar function of the distances between boundary points of the robot and those of obstacles. The gradient of such a scalar function, i.e., the repulsive force between the robot and obstacles, can be used to move the former away from latter making potential-based methods simple. The Newtonian potential which is inversely proportional to the distance between two point charges is used in (Chuang, J.H. & Ahuja N. (1998)) for path planning in the 2-D space. The approach computes, similar to that done in electrostatics, repulsive force and torque between robots and obstacles in the workspace. An algorithm is developed to compute a safe and smooth object path by minimizing the potential function locally for obstacle avoidance, while the gross robot movement is subject to the constraints derived from the topology of the path given a priori. Since the potential is minimized for the obstacle avoidance purpose only, its local minima do not cause a problem in the path planning. The approach is generalized to the 3-D space for the path planning of rigid objects in (Tsai, C In Reference (Lin C.C., Pan, C.C. & Chuang, J.H. (2004)), a potential-based path planning algorithm was proposed for articulated robots with 3-DOF joints, spherical joints, as shown in Fig. 1(a). Because the joint has three DOFs, the robot can take full advantage of the proposed algorithm in potential minimization. In this paper, an improved algorithm is proposed for a robot with 2-DOF joints. While a robot with active 3-DOF joints is rare in practice, a robot with 2-DOF joints is often seen in many kinds of applications. The Hooke joint, which has two orthogonal rotation axes as shown in Fig. 1(b), is adopted in this paper to establish the connection between two links.
In this paper, the generalized potential field model presented in (Chuang, J.H. (1998)), is reviewed in next section, is adopted to model the workspace for the path planning of articulated robots. The proposed approach uses one or more guide planes (GPs) among obstacles in the free space as final or intermediate goals in the workspace for the articulated robot to reach. These GPs provide the articulated object a general direction to move forward and also help to establish certain motion constraints for adjusting robot configuration for minimum potential during path planning, as discussed in Section 3. An implementation of the proposed approach based on a sequential planning strategy is discussed. In Section 4, simulation results are presented for path planning performed for robots in different 3-D environments. Section 5 gives conclusions of this work. In this paper, the generalized potential field proposed in (Chuang, J.H. (1998)) is used to modal the 3-D environment by representing object/manipulators by charged sample points, and obstacles by charged polygons, respectively. The main advantages of using such a free space model include: (i) the repulsion among object, manipulators, and obstacles, in forms of force and torque, can be derived in closed form, (ii) the planned object/manipulator paths are spatially smooth and (iii) the collision avoidance of these paths are guaranteed if the above charged points are adequate.
In (Chuang, J.H. (1998)), the potential function is assumed to be inversely proportional to the distance between two point charges to the power of an integer (m). Thus, the potential field at point r due to a polygon S in the 3D space can be evaluated as , and integer m is the order of the potential function. In particular, it is shown that the repulsive force exerted on a point charge due to a 3-D polygon can be obtained analytically by evaluating the gradient of the potential function (for m=3) with (subscript i is omitted(see Appendix)) where the x i -y i -z coordinate system is determined by the right-hand rule for each edge i of the polygon such that z is measured along the normal direction of S, and x i is measured along edge i of the polygon, respectively, and  is a constant. Hence, if a robot is represented by charged points and obstacles are represented by charged polygons. The repulsive force exerted on the robot due to obstacles can be denoted as where m is the number of charged points, n is the number of obstacle polygons, and The discussion for repulsive torque is similar and is omitted for brevity.

Path Planning Algorithm
In this section, the application of the potential model reviewed in the previous section to path planning of articulated robots with 2-D joints will be discussed. For a rough description of the path of an articulated robot, the proposed approach identifies one or more guide planes (GPs) as its final or intermediate goals in the 3-D workspace (see Fig. 2). The generation and selection of initial GPs will be discussed next. The GPs are polygons among obstacles in the free space, providing the articulated robot a general direction to move forward. As for a description of the configuration of the articulated robot, an n-link articulated robot is represented by its leading tip and the n-1 joints, as shown in Fig. 2. A sequential and collision free traversal of a given sequence of GPs by these control points (and the links) is regarded as a global solution of the path planning problem of the articulated robot. In the planning procedure discussed next, intermediate GPs will also be added along the path each time when a given GP is not directly reachable.

Generation and Selection of initial GPs
In the proposed algorithm, the GPs provide the articulated robot a general direction to move forward. The selection of the initial GPs may base on (i) their density along the passage, (ii) the visibility between two adjacent GPs, or (iii) the angular variation of two adjacent GPs. For the examples considered in this paper, the initial GPs are selected arbitrarily and the algorithm seems to work reasonably well in terms of the sensitivity of the planned path to the selection of initial GPs. In general, these initial GPs can also be obtained from the Generalized Cylinder (GC) ( Fig. 3(a) shows a passage which has an approximate rectangle crosssection, and an axis of its GC representation. Often, there is no limit on the number of cross-sections while their shapes are not specified in advance explicitly GC representation (see Fig. 3  On the other hand, the shape of the GPs is usually not critical in the proposed approach as long as the GPs are confined in the free space. Thus, mainly the normal vectors of the GPs, e.g., obtained from the direction of the GC axis are important. Such an axial representation can also be derived from (i) the global navigation function as in ( While the idea of initial GPs can be regarded as a simplified version of global navigation functions which lead the robot to its final goal, the intermediate GPs added in Step 1 of Articulate_ Robot_ to_GP is derived from a "local" navigation (potential) function in the proposed approach. Instead of leading the robot to the final goal (point), the potential field attracts the robot to an intermediate goal (plane), i.e., one of initial GPs. Thus, depending on its current location, the robot is allowed to reach an intermediate goal plane at various locations via various paths. Such flexibility in generating a global forward movement, compared to the traversal of the single path derived from a global navigation function, will hopefully reduce the number of collisions needed to be corrected, and thus the intermediate robot configuration, during the path planning.

Basic Procedure of Path Planning
In this paper, the proposed path planning approach derives a series of minimum potential configurations along the path of an articulated robot by locally adjusting its configuration for minimum potential using the results reviewed in Sec. 2. Assuming that a guide plane GP1 is given as an intermediate goal, the basic path planning procedure for moving the leading tip, p', of an articulated robot onto GP1 include (see Fig. 4): Translate links of the articulated robot to move p' toward the GP1. If p' can not reach GP1 directly, e.g., due to collision, an intermediate plane GP'1 is inserted. (Fig. 4(a)) Search for the minimum potential configuration of the articulated robot for 1 P G p   by repeatedly executing: Search for the minimum potential configuration of the articulated robot with lnk1 fixed in orientation by sliding p on GP'1 (see Fig. 4(b)). (ii) Search for the minimum potential configuration of the articulated robot with p fixed in position (see Fig. 4(c)). (iii) Repeat (i) and (ii) until p reaches GP1.
In general, there are different ways to change the configuration of the articulated robot to move p' toward GP1. A simple translation of all links is adopted in (i) as a preliminary implementation of our algorithm, as shown in Fig. 4(a). The direction of the translation is determined such that p  is in the direction of the attractive force ' p F experience by p' due to GP1. A collision check is performed for such a translation. If collision occurs, the distance of the translation is halved until the translation is collision-free. Accordingly, a new GP is inserted, e.g., GP'1. No configuration improvement to reduce the repulsive potential is considered at this stage.
As for the search for the minimum potential configuration of the articulated robot in (ii), since lnk1 has five DOFs, i.e., two for its location for 1 P G p   and three for its orientation, the associated constrained optimization problem is divided into two iterative univariant optimization procedures, as in (ii-a) and (ii-b). In (ii-a), lnk1 is fixed in its orientation (see Fig. 4(b)) as p slides on GP'1 to search for the minimal potential configuration and other links are sequentially adjusted in orientation, staring from lnk 2 . In (ii-b), lnk 1 is adjusted in orientation while fixed in position (see Fig. 4(c)) and the procedure for adjusting the rest links is similar to that in (ii-a), except only two DOFs for orientation. For a particular GP, say GP' 1 in Fig. 4, (ii-a) and (ii-b) are repeatedly performed until negligible change in the configuration of the articulated robot is obtained. Then, another intermediate GP which is closer to GP1 is obtained with (i) and the process repeats. The path planning algorithm, as summarized below, ends as the leading tip reaches the given GP1 or exits abnormally for an infeasible problem. Detailed implementation of the algorithm is presented in the next subsection.

Algorithm Articulated Robot to GP
Step 0 Initialize 0    and 1 GP GP  . Step Step 2 Translate lnk1, by sliding p on GP, and adjust the orientations of the rest links to minimize the potential. Step 6 Exit and report that GP1 is unreachable.
For path planning involving multiple GPs, the above algorithm will be executed for each of them sequentially. It is assumed that the planning for a GP starts as the planning of the previous GP is accomplished. The path planning ends as the leading tip of the articulated robot reaches the goal, which is usually a (goal) GP in the path planning problems considered in this paper.

Initialization and Step 1 of Algorithm
In the initialization of Articulated_Robot_to_GP, the initial step size, δ 0 , is arbitrarily chosen, i.e., δ 0 = 10% of workspace size. Generally, the number of robot configurations and the computation time depends on δ0. For larger δ 0 , fewer configurations will need to be calculated along the path if there is no collision. On the other hand, if δ 0 is too big, the computation time may actually increase because collisions may occur frequently. A threshold, i.e., δ min = 1% of workspace size, is established to set a lower bound of the magnitude of allowed translation. A translation which requires a smaller movement, δ < δ min , indicates an infeasible path planning problem.
In Step 1 of algorithm, there are different ways to move p' toward GP1. For example, p' can be moved toward GP1 in the normal direction of GP1. In our simulation, the articulated robot is translated, as a rigid object, to move the leading tip from p' to p by an distance δ along the direction of attraction, ' p F , as shown in Fig. 5(a). In the far field, ' p F has a near spherical symmetry and p' is attracted toward GP1 as if GP 1 is a point attractor. In the near field, ' p F will lead p' toward GP1 approximately in the normal direction of GP 1 . In practice, since δ 0 is finite instead of infinitely small, p' may reach GP1 at different locations from various directions.
While no configuration improvement to reduce the repulsive potential is considered in the above translation procedure, Steps 2 and 3 minimize the potential through constrained optimization procedures. To that end, an intermediate GP, GP'1, is added along the path to serve as a geometric constraint for successive adjustments of the configuration of lnk 1 . Again, as long as , the choice of the orientation of GP' 1 is not unique. For example, GP' 1 can be oriented such that it is parallel with GP1. A more reasonable approach is adopted in our simulation wherein GP' 1 is chosen to be perpendicular to ' p F , as shown in Fig. 5(b). Thus, the direction of translation to minimize the potential in Step 2, as discussed next, is perpendicular to that of the translation in the next execution, if necessary, of Step 1.

Adjusting the leading tip position in Step 2
After the leading tip reaches p, as shown in Fig. 4(b), one of the univariant procedures to minimize the repulsive potential, which allows lnkn to adjust its location but not its orientation, is performed in Step 2 of Articulated_Robot_to_GP. In order to speed up the computation, a sequential planning strategy (Gupta, K. K. (1990)) which plans the motion of links sequentially is adopted in Step 2. Accordingly, the procedures of Step 2 first minimize the potential of lnk1 whose translation is constrained in two dimensions by GP. As for the motion of lnki (i >1), as in (Gupta, K. K. (1990)), it is assumed that all links up to lnk i-1 has been planned, i.e., one end of lnk i is fixed in position. Therefore, only the orientation of lnk i , which has two degrees of freedom, need to be adjusted for minimum potential. Consider the forces exerted on lnk 1 , as shown in more detail in Fig. 6, since the minimization is constrained by p GP  only the projection of the resultant force experienced by lnk 1 on GP is taken into account. Let f 1 be the repulsive force exerted on lnk 1 due to the repulsion between lnk 1 and the obstacles. For a univariant minimization approach, only one variable is adjusted at a time. To determine the direction in which lnk 1 should translate to slide p on GP to reduce the repulsive potential, the projection of the resultant force exerted on lnk 1 along an arbitrary / / u GP  , f 1u is calculated.
A gradient-based binary search for the minimum potential location of p along u  can then be performed. The initial step of sliding is arbitrarily chosen as 10% of the workspace size.
If the movement of lnk 1 along u  or f 1,u is negligible, e.g., the movement is smaller than 1% of workspace size, another minimization of potential along Step 2 ends when two consecutive binary searches along the two orthogonal directions result in negligible movement of lnk 1 . In this step, since the orientation of lnk 1 is not changed, lnk 2 can be connected directly, through pure translation, to lnk 1 before the potential minimizations through rotations with respect to the two orthogonal axes of the 2-DOF joint take place. As for lnk i , 3  i , since the previous potential minimization may have changed the orientation of lnk i-1 , more involved connection process, other than the direct connection, as used for lnk 2 , will need to be performed to ensure the intactness of the robot, as discussed next. Once the optimal configuration of lnk 1 is determined, the procedure for adjusting the rest links, except lnk 2 , is similar to that in Step 3, as discussed later.
Consider joint J i which connects two links, lnk i and lnk i-1 , as shown in Fig. 1(b). Assume R i-1 represents the rotation axis of lnk i-1 after a potential minimization procedure while R i A is the rotation axis of lnk i (which is to be connected to lnk i-1 such that a potential minimization for lnk i can take place), as shown in Fig. 7. Because of the constraint of the 2-DOF joint (R i ⊥ R i-1 ), lnk i can not be connected directly to lnk i-1 through pure translation if the orientation of lnk i-1 has been changed due to potential minimization. Let S i be the skeleton axis of lnk i . It is obviously that two rotation axes are not always orthogonal if two links are connected arbitrarily. Such a configuration of these two links is illegal for a robot with 2-DOF joints. Therefore, R i should be moved from R i A to R i B and then orthogonal to R i-1 . R i A can be determined as following. Assume all possible positions of R i are lying on Plant P if lnk i rotates about the axis S i .  1 i R is the projection of R i-1 on P. If a vector R i B of P is orthogonal to  1 i R , it will be orthogonal to R i-1 too. Thus, lnk i should rotate about S i by θ to let R i moves to R i B . After the previous adjusting lnk i orientation, the two links are connected legally with a 2 DOF joint. And, then, the further potential minimization can take place.

Adjusting joint angle in Step 3
Once the minimum potential position of lnk n is determined with Step 2 of Articulated_Robot_to_GP, another univariant procedure, which allows lnk 1 to adjust it sorientation with leading tip p fixed in position is performed to reduce the potential further, as shown in Fig. 4(c). Under such a constraint, lnk 1 can rotate with respect to p to reduce the repulsive potential. The direction in which lnk 1 should rotate is determined by the repulsive torque experienced by lnk 1 with respect to p. i.e., less than 0.5°. In this step, the rest links ( should be connected to lnk i-1 legally, when the orientation of the lnk i-1 is determined. The connection procedure is the same one in Step 2. Then, since there are only two rotation axes of a joint, rest links from lnk 2 to lnk n adjust their orientation along the two axes, R i and R i-1 , of J i .

Simulation Results
In this section, simulation results are presented for path planning performed on Pentium III (800MHz) personal computer for articulated robots in 3-D environments. Fig.  8(a) shows the initial configuration of the path obtained for a 3-link articulated robot in a 1-GP workspace wherein the GPs are shown as black polygons. Fig. 8(b) shows the complete trajectory of the articulated robot which reaches the final GP safely. Since collisions occur frequently near the 90 degrees turn of the passage for the δ0 chosen, more configurations of the articulated robot are planned near the turn than those near start and goal. Due to the repulsive potential model, the trajectory of the articulated robot is smooth and lies near the middle of the workspace. The simulation takes a total of 42.466 seconds to plan the 14-configuration collision-free path.
Figs.9(a)(b) show the initial configuration and the trajectory, respectively, of the path obtained for another 3-link articulated robot in a 6-GP workspace. The simulation takes 91.34 seconds to planning the 23configuration path. Since the twisted passage shown in Fig. 9 is a more narrow case than the case shown in Fig. 8, a longer time is needed to plan more robot configurations for the path. The planned path is also smoother spatially.

A Comparison Between 2-DOF Joints and 3-DOF Joints
In this section, the simulation results of the 2-DOF-joint algorithm are compared with the ones of 3-DOF-joints algorithm. In order to show the difference of paths derived by the two algorithms clearly, the third configurations of robots with 2-DOF joints and 3-DOF joints of Fig. 2 are shown in Figs. 13(a)(b), respectively. In Fig. 13(b), the second link is twisted to minimize the configuration potential due to the additional DOF, rotating with respect to the skeleton. In general, the algorithm with 2-DOF joints takes more time than a 3-DOF one to plan a collision-free path because the 2-DOF robot has less DOFs to minimize the potential. Figs. 14(a)(b) show trajectories of the example in Fig. 9 of the algorithms with 2-DOF joints and 3-DOF joints, respectively. While Fig. 14(a) takes 91.34 seconds to planning a 24-configuration path, Fig. 14(b) takes 88.16 seconds to planning an 18-configuration path. Since there are more motion constrain for the 2-DOF-joint robot, its algorithm plans more intermediate configurations to archive collision free. Since the 3-DOF-joint robot has additional DOFs to minimize potential than the 2-DOF one, the planned trajectory is more smoothly. As shown in Figs. 14, the difference between two trajectories of two algorithms is not significant.

Conclusion
In this paper, a potential-based path planning of articulated robots with 2-DOF joints is proposed. Since a robot with 2-DOF joints is more trivial than 3-DOF joint robots in practice, the proposed algorithm is more adoptable than the 3-DOF-joint algorithm. In the proposed algorithm, a set of initial GPs are used to guide the robot moving toward. It is assumed that such a sequence of GPs to be traversed by an articulated robot is given in advance in the workspace providing the general direction of the path. According to the motion constraints established by the GPs, the proposed approach derives the path for the articulated robot by adjusting its configurations at different locations along the path to minimize the potential using the force and torque between links and obstacles. Simulation results show that the planned path is similar to the planned path derived by the 3-DOF-joint algorithm and the computation time does not increase significantly.

Appendix
In this section, the generalized potential model (Chuang, J.H. (1998)) is reviewed. Consider a planar surface S in the 3-D space, the direction of its boundary, S  , is determined with respect to its surface normal, n , by the right-hand rule, n l u  , where uˆand lˆ are along the (outward) normal and tangential directions of S  , respectively. For the generalized potential function, the potential value at r is defined as (1). The Newtonian potential (m=1) is harmonic in the 3-D space and can not prevent the robot from colliding the obstacles, and thus can not be used for collision avoidance. The basic procedure to evaluate the potential at r can be summarized as follows: i. Write the integrand of the potential integral over S as surface divergence of some vector function.
ii. Transform the integral into the one over S  based on the surface divergence theorem. iii. Evaluate the integral as the sum of line integrals over edges of S  .
Without the loss of generality, it is assumed that   0       r r n d (8) which is equal to the distance from r to Q. For (i), we have where P is the position vector of rʹ with respect to the projection of r on Q, r Q , and  (10) Note that if r Q is inside S, fm(R) will becomes singular for some rʹʹ = r Q , (and R = d). Let Sε denote the intersection of S and a small circular region on Q of radius ε and centered at r Q , the potential due to S can be evaluated as with Rand R + equal to the distances from r to the two end points of Ci, respectively. Thus, the repulsive force exerted on a point charge due to S can be found analytically by evaluating the gradient of the following function at some (x, y, z)ʹs.