This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination. IEEE TRANSACTIONS ON AUTOMATION SCIENCE AND ENGINEERING
1
Homotopy Path Planning for Terrestrial Robots Using Spherical Algorithm Gerardo Diaz-Arango, Héctor Vázquez-Leal, Luis Hernandez-Martinez, María Teresa Sanz Pascual, and Mario Sandoval-Hernandez
Abstract— Autonomous and semiautonomous mobile robots play an important role in making tasks in environments considered hostile or dangerous for a human being. In order to execute many of the required tasks, robots need, in its architecture, a module of navigation with an appropriate path planning algorithm. This paper presents the development and implementation of a methodology for path planning of a mobile robot using a spherical algorithm and homotopy continuation methods (HCMs). The first section is a brief introduction about HCMs. Subsequently, the homotopy path planning method and spherical path tracking algorithm are explained, as well as the upgraded version and its main features. Then, the main contributions of this paper are presented and the effectiveness of the proposed method is proved. Besides, numerical examples, implementation results in multiple platforms including a 32b microcontroller and robot operating system, are displayed. Finally, some favorable results from a comparative between the proposed methodology against sampling-based planners using the open motion planning library are presented. Note to Practitioners—This paper presents a numerical routine for homotopy path planning that can be implemented easily using different programming languages or programs like C, C++, and Maple CAS software, among many others. The aim of this paper is to introduce a numerical algorithm suitable to be implemented in different platforms even with low hardware resources. The main advantage of this method is that it is not probabilistic and it is easy to implement by any engineer with basic knowledge of numerical methods. The numerical simulations show that it is possible to resolve in milliseconds environment maps composed by several hundreds of obstacles. Index Terms— Homotopy continuation methods (HCMs), mobile robotics, open motion planning library, path planning algorithm, robot operating system (ROS), spherical path tracking algorithm.
Manuscript received February 5, 2016; accepted December 3, 2016. The work of H. Vazquez-Leal was supported by the National Council for Science and Technology of Mexico under Grant CB-2010-01 #157024. This paper was recommended for publication by Associate Editor H. Wang and Editor M. Yang upon evaluation of the reviewers’ comments. (Corresponding author: H. Vazquez-Leal.) G. Diaz-Arango, L. Hernandez-Martinez, and M. T. Sanz Pascual are with the Electronics Department, National Institute for Astrophysics, Optics and Electronics, Puebla 72840, México. H. Vazquez-Leal is with the Electronic Instrumentation and Atmospheric Sciences School, Universidad Veracruzana, Veracruz 91000, México (e-mail:
[email protected]). M. Sandoval-Hernandez is with the Doctorado en Ciencia, Cultura y Tecnología, Universidad de Xalapa, Veracruz 91190, México, and also with the Centro de Bachillerato Tecnológico Industrial y de Servicios, Veracruz 91777, México. Color versions of one or more of the figures in this paper are available online at http://ieeexplore.ieee.org. Digital Object Identifier 10.1109/TASE.2016.2638208
Fig. 1.
Main task for an autonomous mobile robot.
I. I NTRODUCTION
R
ECENTLY, mobile robotics has become an important tool for many applications in areas such as artificial intelligence, robotics exploration, and control and computational systems. The main task of mobile robots is navigating in hostile or difficult access environments for a human being while performing a determinate task. In general, mobile robots can be autonomous or semiautonomous depending on the assigned task. Autonomous robots must have in its architecture a navigation module that allows you to locate it and guide its course in an environment so that the robot can travel from point A to point B as shown in Fig. 1. The navigation module mainly consists of a map of the environment and a path planning algorithm [1]. For a terrestrial mobile robot, the map of its environment is a top image of the space in which it develops its tasks. This image can be provided by a satellite within the area of exploration or, in some cases, by sensors integrated in the robot. If the environment map is known, then the planning of a path becomes the main problem. An algorithm for planning collision-free paths is responsible for tracing a trajectory that the robot must follow to avoid colliding with obstacles in its way. These obstacles can be static or temporally static in agreement with the application [2], [3]. Several works about algorithms for planning collision-free paths have been reported. The algorithm bug and the algorithm of potential artificial fields have been demonstrated to be very effective and of rapid execution. The bug algorithm, for example, consists in exploring a path from the point of origin
1545-5955 © 2017 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission. See http://www.ieee.org/publications_standards/publications/rights/index.html for more information.
This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination. 2
IEEE TRANSACTIONS ON AUTOMATION SCIENCE AND ENGINEERING
to the target point avoiding obstacles as insects do [4], [5]. The artificial potential field algorithm mentioned in [6]–[9] considers the robot and the obstacles as electric charges equal, with the purpose of generating repulsion among them; the final point is a charge of opposite sign, which generates an attraction effect. Nevertheless, neither the bug nor the artificial potential fields’ algorithm guarantees finding a successful path. The bug algorithm may fail to find an optimal path, and the potential fields’ algorithm presents local minimums causing false convergence to the solution [7], [8], [10]. There are other algorithms based on visibility graphs; these create a link between all the vertices of each polygonal obstacle, the initial and the goal point. Then, the solution path is formed with the best set of links in terms of length [11], [12]. Some others are the sampling-based planners, such as rapidly exploring random tree (RRT) algorithm, probabilistic road map (PRM) method, kinodinamic planning by interior–exterior cell exploration (KPIECE), and expansive space trees (ESTs) [13], [14]. These algorithms can be combined with heuristic optimization methods as genetic algorithms, neural networks, and ant colony optimization algorithms [13], [15]. The RRT algorithm is the most used in collision-free path planning, and its computing speed makes it an extremely attractive algorithm [16]. However, it provides a tree formed by piecewise linear type paths. The RRT algorithm requires a collision checker strategy and a postprocessing algorithm to smooth the piecewise linear path [13], [14], [17], [18]. Both the algorithms based on graph and sampling-based planners have proven their effectiveness in path planning, but are not always successful. Recently, papers have been published in which a new tool based on homotopy continuation methods (HCMs) is able to get collision-free paths. The homotopy path planning method (HPPM) [1] operates similarly to the potential field algorithm, and it is based on a kind of repulsion effect by the obstacles and a strong attraction due to the goal point. This paper is organized as follows. In Section II, the fundamental concept of the homotopy methods is explained. In Section III, the enhanced version of the HPPM is explained. In Section IV, the hyper-spherical algorithm (SA) used to tracing homotopy curves is presented. The proposed predictor– corrector algorithm is shown in Section V. In Section VI, the technique for reversal effect detection and the proposed method to correct it is presented. Some numerical simulations in multiple platforms are shown in Section VII. A performance comparison between the proposed methodology and SBP algorithms is presented in Section VIII. The results are discussed in Section IX, and finally, the conclusions are drawn in Section X. II. H OMOTOPY C ONTINUATION M ETHODS The HCMs are commonly used to find multiple solutions in nonlinear algebraic equations’ system (NAES) of the form f (x) = 0 :
Rn −→ Rn .
(1)
The HCMs create a perturbation in the system f (x), adding a function G(x) and a homotopy parameter λ. Therefore, the original system becomes a homotopy system that is expressed
Fig. 2.
Family of homotopy curves.
in this way H (x, λ) = λ f (x) + (1 − λ)G(x) = 0
(2)
where H ( f (x)) = H (x, λ) : Rn+1 −→ Rn , x ∈ Rn λ ∈ [0, 1]. The function G(x) has well-known or trivial solutions of such form that the following conditions hold. 1) If λ = 0 and H (x, λ) = 0, then a well-known or trivial solution is obtained. 2) If λ = 1 and H (x, λ) = f (x), then the solution of the original system is obtained. 3) The HCM generates a set of solutions H −1 (0) for the system f (x) = 0 with 0 λ 1. H −1(0) is represented by a continuous curve γ . All the solutions available to solve the homotopy system during the continued deformation from λ = 0 to λ = 1. Form a set of points of n + 1 dimension. These solutions can be mapped like a family of curves γ [1], [19]–[34]. Fig. 2 shows a family of curves γ produced by the continuous homotopy deformation of a system of nonlinear equations. γ1 is a continuous curve from λ = 0 to λ = 1, γ2 is a closed curve, and γ3 diverges to infinity without reaching λ = 1. These curves also show turning points. Commonly, the only interest is the set of intersection points with the shaft λ = 1. The adequate selection of G(x) determines the specific homotopy. In this paper, we propose to use Newton homotopy method that is defined as G(x) = f (x)− f (x 0 ) [1], [25], [26], [32], [34]; now if it is replacing G(x) in (2) H ( f (x), λ) = f (x) − (1 − λ) f (x 0 ) = 0, x ∈ Rn
(3)
where x 0 is the starting point. On the one side, if λ = 0, the homotopy system is reduced to the trivial problem H ( f (x), 0) = f (x) − f (x 0 ) = 0. On the other side, if λ = 1, we obtain the original system H ( f (x), 1) = f (x) = 0. III. H OMOTOPY PATH P LANNING M ETHOD In general terms, HPPM and the potential fields’ algorithm share similar characteristics: 1) there exists a kind of repulsion effect from obstacles and 2) the path tends to reach the target point. Moreover, the method of artificial potential fields is the interpretation of a physical effect, whereas the HPPM considers the contour of obstacles as singularities causing a
This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination. DIAZ-ARANGO et al.: HOMOTOPY PATH PLANNING FOR TERRESTRIAL ROBOTS USING SA
3
Fig. 4. Two-dimensional obstacles’ map normalized. Starting point (0, 0) and target point (1, 1). (a) Obstacles map. (b) Intersection of (9) and (10) at target point. Fig. 3. Representation of obstacles in an environment map. (a) Representation of obstacles considering the robot radius. (b) Obstacle shapes defined in HPPM.
kind of repulsive effect. The HPPM allows planning a path that avoids obstacles, generating a continuous curve from a starting point to a goal point. In this paper, we use the improved version of this method, called EHPPM [1]. The robot moves only in the plane x y, and hence the EHPPM represents each obstacle as follows: W (x, y) =
i=k i=1
pi Obi (x, y)
(4)
where k is the number of obstacles in the map, pi is the repulsion parameter for the i th obstacle, and Obi (x, y) is the equation that defines the shape of each obstacle [1]. Two forms of obstacles, circular and rectangular, are defined. The circular obstacles are represented by Ci (x, y) = (x − x i )2 + (y − yi )2 − r ci2 = 0
(5)
where r ci is the ratio of the circumference and the point (x i , yi ) represents the center of a circular obstacle. Rectangular-type obstacles are defined using an ellipse approximation (x − x i ) cos(ψi ) + (y − yi ) sin(ψi ) 2η Ri (x, y) = α (x − x i ) sin(ψi ) + (y − yi ) cos(ψi ) 2η + β −1 = 0 (6) where η is the value that defines the sharpness of the corners (in this paper, we use η = 2 for all case studies) and the point (x i , yi ) represents the center of a rectangular obstacle with a width of 2α and a height of 2β. Finally, ψi is the angle of rotation with respect to horizontal as shown in Fig. 3(b). Moreover, the representation of the obstacles must consider the size of the robot [1]. For example, consider the holonomic constraints of a robot with differential drive, where the rotation point can be the same center of the vehicle. The environment map can be modified considering an increase in size of the obstacles as shown in Fig. 3(a). Fig. 3(a) shows the new size of obstacles considering the robot size with radius rbot , where the actual size of objects is
represented by the dotted line and the solid line represents the objects with enlarged size. In addition, the workspace of the robot is standardized. If the target point B is (1, 1) and the starting point is (0, 0), the obstacles are located in the space x ∈ [0, 1], y ∈ [0, 1] as described in Fig. 4(a). On the other hand, an equation system is necessary with a single solution at the end point B [1]. This can be formulated as f 1 (x, y) = 0, x, y ∈ R f2 (x, y) = 0, x, y ∈ R
(7) (8)
where f 1 (1, 1) = 0 and f 2 (1, 1) = 0. In this paper, we propose linear equations of the form L 1 (x, y) = −y − m 1 x + (b + m 1 a) = 0
(9)
L 2 (x, y) = −y − m 2 x + (b + m 2 a) = 0
(10)
where m 1 and m 2 are the values of the respective slopes and (a, b) represents the ordered pair of the point of intersection between the straight lines. The lines intersect only at the target point B as depicted in Fig. 4(b). We use this property to create continuous curves of the homotopy and apply them to the collision-free path planning. The HPPM uses the attraction effect toward the point of intersection of the straight lines and a kind of repulsion effect by the obstacles [1]. This method embeds the contour of the obstacle as a singularity in the system of equations. The singularities are added to the expression of the straight line L 2 transforming the equations in f 1 (x, y) = L 1 (x, y) = 0 f 2 (x, y) = L 2 (x, y) + W (x, y) − Q = 0
(11) (12)
where Q = W (a, b). W (x, y) in (12) allows singularizing the function in the outline of the obstacles, whereas Q vanishes this effect at point B. Applying Newton homotopy to (11) and (12), the homotopy equation system is expressed as H1( f 1 (x, y), λ) = f 1 (x, y) − (1 − λ) f1 (x 0 , y0 ) = 0 H= H2( f2 (x, y), λ) = f 2 (x, y) − (1 − λ) f2 (x 0 , y0 ) = 0 (13) where (x 0 , y0 ) is the starting point A. Fig. 5 shows a hypothetical example; in this, the contours of the obstacles in solid line and dotted lines represent the γ curves. Fig. 5(a) presents a homotopy path without the
This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination. 4
IEEE TRANSACTIONS ON AUTOMATION SCIENCE AND ENGINEERING
Fig. 5. Homotopy paths 2-D representation for different cases. (a) Direct trajectory without obstacles. (b) Internal and external curves generated due the obstacles in the map. (c) Successful trajectory for a map with obstacles.
presence of obstacles, while a family of γ curves (internal and external) produced by objects in the environment is presented in Fig. 5(b). In Fig. 5(c), γ0 represents a successful trajectory from point A to point B. On the one side, γ5 and γ7 are closed internal isolated curves that do not reach at the target point; on the other side, γ6 and γ8 are closed external isolated curves that can potentially increase their size to construct connections for new homotopy trajectories. It is important to note that γ1 and γ4 joint to create the successful homotopy trajectory γ0 . In general terms, to each obstacle in the map are associated two γ curves, one internal and the other external to its boundary. The external trajectories can joint to conform homotopy paths from A to B. The successful collision-free curve in an environment with the presence of obstacles can be formed by combining two or more external γ curves, depending on the parameters of repulsion and proximity of obstacles with the direct path [1]. As depicted in Fig. 5(c), the existence of multiple γ curves implies the requirement of a proper tracking technique to prevent jumps between homotopy paths or branches producing a failure in the method [1], [24], [28], [29], [35]. IV. S PHERICAL PATH T RACKING The HCM methods require a proper path following technique to trace the homotopy curves. In this paper, a modified spherical tracking is proposed. The SA is a tool presented in [24] and [28]–[30] with a clear graphical interpretation and straightforward procedure easy to implement for curve tracing using spheres. In order to explain the procedure with simple figures, we will present the procedure using circles; nonetheless, the same principles can be extended to three
Fig. 6.
Intersection of circle with homotopy curve.
Fig. 7.
Spherical tracking for a homotopy curve.
dimensions or more. If a circle of radius r and center O3 is located on the path, its outline must intersect the homotopy curve at least at two points (O2 and O4 ) as shown in Fig. 6. The formulation of a homotopy system has n equations and n + 1 variables that means n system variables plus homotopy parameter λ [24], [28]–[30]. It means that we should include an extra equation representing a hypersphere of n + 1 dimensions to trace the homotopy path. Moreover, we will consider only x and y displacements for path planning. Therefore, after the homotopy formulation, the new transformed problem will described by three variables (x, y, and λ). Subsequently, the hypersphere is reduced to three dimensions, resulting in Si (x, y, λ) = (x − cx )2 + (y − c y )2 + (λ − cλ )2 − r 2 = 0 (14) where r is the radius and (cx , c y , cλ ) is the center of the sphere in the homotopy path. Using (13) and (14), we propose ⎧ ⎪ ⎨ H1 (x, y, λ) = 0 (15) H S = H2 (x, y, λ) = 0 ⎪ ⎩ Si (x, y, λ) = 0. The curve γ is a representation of the family of solutions (13), i.e., γ represents all the points H −1 (0). In the SA, the center of the sphere must be updated in each step of the tracking and resolved for (15). Spherical tracking is subsequently performed as depicted in Fig. 7. The solution of this system is found using the Newton–Raphson (N-R) method. Combining a predictor–corrector scheme and the SA, we obtained an appropriate methodology for spherical tracking. V. P REDICTOR –C ORRECTOR A LGORITHM The predictor–corrector scheme allows the SA to perform a correct tracking of the homotopy γ curve. Fig. 8 illustrates
This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination. DIAZ-ARANGO et al.: HOMOTOPY PATH PLANNING FOR TERRESTRIAL ROBOTS USING SA
5
was proposed ⎛
⎛ ∂H
1
∂λ ⎜ ⎜ = sg ⎝(−1)n+1 det ⎝ ∂∂Hx 2 ∂ρ ∂x
Fig. 8.
Predictor–corrector algorithm.
the behavior of the predictor–corrector scheme. The Euler predictor draws a tangent vector (x p , y p , λ p ) at the point (x i , yi , λi ) of the homotopy path. Then, the predictor point is found in the intersection of the sphere Si with the tangent vector. The N-R method is subsequently applied as corrector to find the intersection point (x i+1 , yi+1 , λi+1 ) between the homotopy path and the sphere of radius r . A. Euler Predictor The Euler predictor algorithm is a powerful tool for homotopy path tracking. This provides a point close to the solution of the homotopy system in the next tracking step. In recent works [27], [30], the effectiveness of the Euler tangent predictor in spherical tracking of homotopy curves has been verified. The Euler predictor point used in SA is expressed as v p (x p , y p , λ p ) = (cx , c y , cλ ) + r
(16)
where (x p , y p , λ p ) is the predictor point, (cx , c y , cλ ) is the center of the sphere, r is the radius of the sphere, and vp is the tangent vector. Further, in this paper, we will show that the center of the sphere and the i th point in the homotopy path are not always equal. The tangent vector is obtained, as explained in [27] and [30]. In this process, it is proposed the dependence of each of the variables with respect to an arbitrary parameter ρ, i.e., (x(ρ), y(ρ), λ(ρ)) and H1(x(ρ), y(ρ), λ(ρ)) = 0 H (x(ρ), y(ρ), λ(ρ)) = (17) H2 (x(ρ), y(ρ), λ(ρ)) = 0. Now, the system of (17) is derived with regard to ρ, using the chain rule ⎛ ⎞ ∂x ⎞ ⎛ ∂H ∂ H1 ∂ H1 ⎜ ∂ρ ⎟ 1 ⎜ ⎟ 0 ⎜ ∂x ∂y ∂λ ⎟ ⎜ ∂y ⎟ (18) ⎟= ⎝ ∂ H2 ∂ H2 ∂ H2 ⎠ ⎜ 0 ⎜ ∂ρ ⎟ ⎝ ∂λ ⎠ ∂x ∂y ∂λ ∂ρ where the vector of partial derivatives (∂(x(ρ), y(ρ), λ(ρ))/∂ρ) is the tangent vector vp . Rearranging the terms ⎛ ⎛ ∂H ⎞ ∂ H1 ⎞ ⎛ ∂ x ⎞ 1 ∂ H1 ∂λ ⎜ ∂λ ⎟ ⎜ ∂x ∂y ⎟ ⎜ ∂ρ ⎟ (19) ⎝ ∂ H2 ⎠ ⎝ ∂ H2 ∂ H2 ⎠ ⎝ ∂y ⎠ = − ∂ρ ∂x ∂y ∂ρ ∂λ in order to obtain the solution of this system, it is necessary to know the value of ∂λ(ρ)/∂ρ; in [36], the next solution
∂ H1 ⎞⎞ ∂y ⎟⎟ ∂ H2 ⎠⎠ ∂y
(20)
where, in this case, n = 2 and sg is a parameter that only takes values +1 or −1 and designates the direction of a predictor vector in forward or backward direction; the sign of the sg is linked to the expressions of the auxiliary straight lines. For lines L 1 and L 2 considering m 1 = m 2 , we know the relationship among (11)–(13) and m 1 , m 2 . To explain this effect, we take the case m 1 = 1 and m 2 = 2 ⎛ ⎞ ∂ H1 ∂ H1 ∂y ⎟ ⎜ det ⎝ ∂∂Hx ⎠ = δ1 − δ2 2 ∂ H2 ∂y ∂x ∂ H1 ∂ H2 δ1 = ∂x ∂y ∂ H1 ∂ H2 δ2 = . (21) ∂y ∂x Assuming δ1 > δ2 in (21), then the sign of the determinant is “+” but if we exchange the value of slopes, i.e., m 1 = 2 and m 2 = 1, it is similar to changing the rows in the matrix, we have ⎛ ∂H ∂H ⎞ 2
⎜ det ⎝ ∂∂Hx 1 ∂x
2
∂y ⎟ ∂ H1 ⎠ = δ 2 − δ 1 . ∂y
(22)
Then, considering δ1 > δ2 in (22), the sign of the determinant is “−.” Note that δ1 > δ2 is only an assumption that is used to explain the change in sign of the determinant related to the slopes m 1 and m 2 . Thence, if m 1 > m 2 , then sg = −1, and if m 2 > m 1 , then sg = +1. Equation (19) can be expressed as ⎛ ∂x ⎞ ⎛ ∂ H ∂ H1 ⎞−1 ⎛ ∂ H1 ∂λ ⎞ 1 − ⎜ ∂ρ ⎟ ⎜ ∂ x ∂y ⎟ ⎜ ∂λ ∂ρ ⎟ (23) ⎝ ∂y ⎠ = ⎝ ∂ H2 ∂ H2 ⎠ ⎝ ∂ H2 ∂λ ⎠ . − ∂ρ ∂x ∂y ∂λ ∂ρ By placing the tangent vector in (16), we obtain ∂(x(ρ), y(ρ), λ(ρ)) (x p , y p , λ p ) = (cx , c y , cλ ) + r ∂ρ
(24)
where (x p , y p , λ p ) is the predictor point, (cx , c y , cλ ) is the center of the i th sphere, r is the radius of the sphere, and (∂(x(ρ), y(ρ), λ(ρ))/∂ρ) is the tangent vector constructed using (20) and (23). B. Newton–Raphson Method In this paper, the N-R method is used to correct the predictor point to a point on the homotopy path γ [1], [29], [30], [32], [36]. The N-R method, commonly used to resolve NAES [37], is an iterative process of local convergence that approximates an initial value to a final value. Because convergence is local, an initial point close to the solution point is required; the
This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination. 6
Fig. 9.
IEEE TRANSACTIONS ON AUTOMATION SCIENCE AND ENGINEERING
N-R corrector in spherical tracking.
Fig. 10.
predictor point fulfills this condition [37]. The N-R corrector is described by (x j +1, y j +1 , λ j +1 ) = (x j , y j , λ j ) − [ J (x j , y j , λ j )]−1 H S (25) where j = 0, 1, 2, . . . n iteration and (x j , y j , λ j ) is the point that is updated in each iteration until arriving to a final value; this final value can be considered to be the solution. [ J (x j , y j , λ j )]−1 is the inverse Jacobian of H S , and H S (x, y, λ) is evaluated in the point (x j , y j , λ j ) for each j th iteration. This iterative method requires an appropriate starting point to reach the solution in a minimum of iterations. Therefore, for the first iteration j = 0, (x j , y j , λ j ) = (x p , y p , λ p ). The N-R method is repeated until it reaches a maximum of iterations jmax or it converges to a solution (x i+1 , yi+1 , λi+1 ) with a minimum margin of error, as shown in Fig. 9. The tolerance criterion is given by H S (x j +1, y j +1 , λ j +1 ) < err.
(26)
In this paper, we propose err = 1 × 10−6 and jmax = 40. Taking as starting point the predictor point (x p , y p , λ p ), the method locates the intersection point (x i+1 , yi+1 , λi+1 ) between the i th sphere Si and the homotopy path after four iterations.
VI. T ECHNIQUE TO D ETECT AND C ORRECT THE R EVERSAL E FFECT Each sphere in the tracking procedure must intersect the homotopy path in two points as depicted in Fig. 6 for two dimensions. The sphere intersects at points O2 and O4 , one in backward direction and the other in forward direction. Nonlinearities in the system H can cause the N-R method to converge to the point O2 , although the predictor point is located close to O4 . This convergence in the backward direction causes a reversal effect in the tracking [28]–[30], [36] procedure. To correct the reversal effect, first of all, we must be able to detect when N-R converges to a point in backward direction. A good technique to detect this is to compare the direction of the normal vector at each point of intersection [29]. Subsequently, a technique able to correct this effect is required. This paper proposes to rotate the predictor point in the forward direction to obtain a point (x rot , yrot , λrot ). Then, the N-R method is applied again taking (x rot , yrot , λrot ) as the starting point.
Strategy for reversal effect detection.
A. Reversal Effect Detection A technique to detect the reversal effect is of vital importance for SA. When the radius of the spheres is very small, it is difficult to differentiate between points in the backward or forward that causes false detections. Therefore, a technique that provides more information to differentiate the points is required. The work in [29] proposes a technique that is very efficient to detect reversal in the tracking. This technique compares the angles of the normal vector at each intersection point. Fig. 10 shows the operation of this technique. To obtain the normal vector, we must evaluate the gradient of the equation of the sphere (14) at the points O2 and O4 ⎛ ⎞ 2(x − cx ) ∇ Sn = ⎝ 2(y − c y ) ⎠. (27) 2(λ − cλ ) A reversal effect is detected in the path when ⎞ ⎞ ⎛ ⎛ 2(x − cx ) 2(x − cx ) ⎜ arccos ⎟ ⎟ ⎜ arccos ⎜ ⎜ ∇ Sn ⎟ ∇ Sn ⎟ ⎜ ⎟ ⎟ ⎜ ) ) 2(y − c 2(y − c y y ⎟ = ⎜ arccos ⎟ ⎜ arccos ⎜ ⎟ ⎟ ⎜ ∇ S ∇ S n n ⎜ ⎜ ⎟ ⎟ ⎝ ⎝ 2(λ − cλ ) ⎠ 2(λ − cλ ) ⎠ arccos arccos ∇ Sn ∇ Sn O2 O4 (28) where the arccos function is used to find the angle with respect to each variable for each normal vector [29]. This technique proves to be effective for detecting the reversal effect, because the normal vectors provide more information than the position of the point. B. Strategy to Correct Reversal Effect When a reversal in the tracking procedure has been detected, an algorithm able to find a new point in forward direction is required. In [29], an algorithm was proposed; it increases the radius of the spheres as a technique to correct the reversal effect. However, this technique could cause jumps from the solution curve to a closed curve, if these are very close. In this paper, we propose to place the predictor point closer to the solution point. For this purpose to do so the predictor point should rotate a θ angle on the surface of the sphere in forward direction. Fig. 11 presents the proposed technique for correcting reversal effect. From (11) and (13), we can represent H1 as H1 = −y − m 1 x + λ(b + m 1 (a − x 0 ) − y0 )(y0 + m 1 x 0 ) = 0 (29)
This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination. DIAZ-ARANGO et al.: HOMOTOPY PATH PLANNING FOR TERRESTRIAL ROBOTS USING SA
7
should be projected in the plane x y Finally, the point Prot and expression (30) can be applied to represent the point in the solution plane H1 x rot x3 x rot Prot = = + (35) yrot y3 yrot ⎞ ⎛ ⎞ ⎛ x rot x rot ⎠ ⎝ yrot ⎠ = ⎝ yrot yrot +m 1 x rot −(y0 +m 1 x 0 ) λrot b+m 1 (a−x 0 )−y0 ⎛ ⎞ x rot ⎝ yrot ⎠ ∈ H −1(0). (36) 1 λrot
Fig. 11. Technique to correct reversal effect and reduce N-R iterations. (a) Predictor point in the original reference plane. (b) Representation of the predictor point rotated in auxiliary reference plane. (c) Predictor point rotated in original reference plane.
where (x 0 , y0 ) is the starting point A, (a, b) is the goal point B, and m 1 is the slope of L 1 . In (29), the expression H1 describes a plane in the 3-D space; therefore, the points that form the set of the homotopy path are a subset of all solution points H1, i.e., H −1 (0) ⊆ H1−1(0). Therefore, if the γ curve belongs to the plane described by H1, then the tangent vector at each point of γ and the predictor point also belong to this plane. From (29), we find the expression y + m 1 x − (y0 + m 1 x 0 ) . (30) λ= b + m 1 (a − x 0 ) − y0 Then the rotation of the predictor point becomes a problem of two dimensions. First, a conversion in 2 − D should be applied to find the point Pp in the reference plane x y , as shown in Fig. 11(b). Fig. 11(a) shows the predictor point in the original plane, Fig. 11(b) shows the predictor point rotated an angle θ in an auxiliary plane, and Fig. 11(c) presents the predictor point rotated an angle θ in the original plane, where O1 = (x 1 , y1 ) O2 = (x 2 , y2 ) O3 = (x 3 , y3 ) O4 = (x 4 , y4 ) Pp = (x p , y p ).
(31)
The points O3 and Pp were projected with respect to the origin, point O3 , of the auxiliary plane. Then, O3 and Pp is set are calculated and using matrix 2-D rotation Prot O3 = (x 3 , y3 ) = (x 3 , y3 ) − (x 3 , y3 ) = (0, 0) Pp
=
(x p , y p )
= (x p , y p ) − (x 3 , y3 ) cos(θ ) − sin(θ ) . sin(θ ) cos(θ )
(32) (33) (34)
This technique can reduce the number of iterations the N-R method required to converge to the solution in the forward direction. As mentioned above, after the Euler predictor step, the N-R method is applied considering a maximum of 40 iterations. After such iterations, if N-R not fulfills the error criteria, then the rotation scheme comes into operation using an angle θ = (π/4) in the clockwise direction. If it fails, the rotation is repeated using the same angle but in the other direction. This backup strategy was able to successfully correct all the reversal issues presented in the case studies of this paper. What is more, such technique aids to solve problems of non-convergence and reduce the computation time. C. Variable Radius The radius of the sphere affects the number of steps of the spherical tracking and, consequently, the computing time [28], [30]. In this paper, the method described in [30] is used, which consists in varying the radius in each step of the spherical tracking using |μ | − |μ i | i+1 (37) ri+1 = r0 1 − (1 − ) exp where ri+1 is the radius of the sphere Si+1 , r0 is the initial radius, is the maximum factor by which to multiply r0 , and μi is the partial derivative of the homotopy parameter with respect to the parameter ρ for the i th step that means μi = (∂λ/∂ρ)i ; in the same way, μi+1 = (∂λ/∂ρ)i+1 . A variable radius in the methodology involves a modification in the reversal effect detection method. Fig. 12(a) shows the method without variable radius, while Fig. 12(b) presents the problem for detection of the reversal effect when using variable radius. The radius variation causes the intersection point between the sphere Si and the homotopy path γ not to match with the center of Si−1 ; in this case, the reversal effect detection fails, as shown in Fig. 12(b). In [28], it is proposed to move the center of the sphere by a linear interpolation between the points Oi−1 and Oi . Fig. 12(c) clearly presents this methodology, where the intersection point between the sphere Si−1 and the curve γ matches with point Oi−1 for i = 3. The center of the sphere Si−1 , represented by point O¯ i , is moved r = ri − ri+1 , as depicted in Fig. 12(c). The new center is calculated with the expression O¯ 3 = (cx , c y , cλ ) = (x i+1 , yi+1 , λi+1 ) + r (x i+1 , yi+1 , λi+1 ) − (x i , yi , λi )
(38)
This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination. 8
IEEE TRANSACTIONS ON AUTOMATION SCIENCE AND ENGINEERING
From (39) and (40), and applying (11) and (12), we obtain f 1 (x, y) = −y − 4x + 5 = 0 f 2 (x, y) = −y − x + 2 + W (x, y) − Q = 0 (41) p1 p2 p3 W (x, y) = + + (42) Ob1 (x, y) Ob2 (x, y) Ob3 (x, y) p2 p3 p1 + + (43) Q = W (1, 1) = Ob1 (1, 1) Ob2 (1, 1) Ob3 (1, 1) where Q = 0.01303, f 1 (1, 1) = 0, and f 2 (1, 1) = 0. For this example, the starting point is (x 0 , y0 ) = (0, 0), the goal point is (a, b) = (1, 1), the initial radius for the spheres of the tracking is r0 = 0.01, and for the first sphere, r1 = r0 . From (13)–(15) and (41), we obtain H1 = f 1 (x, y) − (1 − λ) f1 (0, 0) = 0 H2 = f 2 (x, y) − (1 − λ) f2 (0, 0) = 0 Fig. 12. Detection of reversal effect in spherical tracking. (a) Correct detection for fixed radius. (b) Erroneous detection for variable radius. (c) Correct detection for variable radius. TABLE I S IMULATION PARAMETERS FOR M AP W ITH T HREE C IRCULAR O BSTACLES
S1 = (x − cx )2 + (y − c y )2 + (λ − cλ )2 − (0.01)2 = 0. (44) Removing and rearranging terms, the following system of equations is obtained: H1 = −y − 4x + 5λ = 0 H2 = −y − x + W (x, y) + 2λ + (λ − 1)W0 − λQ+ = 0 S1 = (x − cx )2 + (y − c y )2 + (λ − cλ )2 − (0.01)2 = 0 (45)
where O¯ 3 is the new center and (x i , yi , λi ) and (x i+1 , yi+1 , λi+1 ) are the points O2 and O3 , respectively, as depicted in Fig. 12(c).
to evaluate the terms W0 = W (0, 0) = 0.1. For the first step, (cx , c y , cλ ) = (x 0 , y0 , 0). Then, the predictor point (x p , y p , λ p ) is obtained using the Euler vector predictor as aforementioned. For the first step of tracking, i = 1 and O1 = (x 0 , y0 , 0) = (0, 0, 0). Using (24) ⎛ ∂H ∂H ⎞ 1
VII. N UMERICAL S IMULATIONS In this section, we first present a numerical example that explains the operation of the methodology. Subsequently, a series of simulations on two software platforms (Maple and C++) and performance of the methodology implemented in a microcontroller are presented. Finally, the methodology is proved in robot operating system (ROS), for a Pioneer 3− D X robot. A. Three Circular Obstacles With the Same Radius Let us consider a known map with three circular obstacles represented by the parameters in Table I Ob1 (x, y) = (x − 0.2)2 + (y − 0.2)2 − (0.1)2 = 0 Ob2 (x, y) = (x − 0.5)2 + (y − 0.5)2 − (0.1)2 = 0 Ob3 (x, y) = (x − 0.8)2 + (y − 0.8)2 − (0.1)2 = 0
Continuing the process of Euler’s method ⎛ ∂x ⎞ −1 −4 −1 −13.823 ⎜ ∂ρ ⎟ ⎝ ∂y ⎠ = −0.9215 −0.9215 −5.5292 ∂ρ 2.60770 = (47) 3.39229 ⎞ ⎛ ∂x ⎞ ⎜ ∂ρ ⎟ ⎛ 2.60770 ⎜ ∂y ⎟ ⎟ ⎝ ⎜ v p = ⎜ (48) ⎟ = 3.39229 ⎠ ⎜ ∂ρ ⎟ 2.76462 ⎝ ∂λ ⎠ where μ1 = (∂λ/∂ρ) = 2.76462, and the tangent vector is v p = (0.511895, 0.665913, 0.542699)
(49)
using tangent vector and r1 . The predictor point for the first step is
L 1 (x, y) = −y − 4x + 5 = 0 where m 1 = 4 and m 2 = 1.
1
∂y ⎟ ∂ H2 ⎠ = 2.76462. (46) ∂y
∂ρ (39)
where Obi (x, y) represents each circular obstacle. We propose the system of auxiliary lines L 1 and L 2 as
L 2 (x, y) = −y − x + 2 = 0
∂λ ⎜ = −1(−1)3 det ⎝ ∂∂Hx μ1 = 2 ∂ρ ∂x
(40)
(x p , y p , λ p ) = (0, 0, 0) + 0.01(0.5118, 0.6659, 0.5426) = (0.005118, 0.006659, 0.005426).
(50)
This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination. DIAZ-ARANGO et al.: HOMOTOPY PATH PLANNING FOR TERRESTRIAL ROBOTS USING SA
9
Fig. 14. Representation of the second sphere of the SA on the plane H1 . (a) New center for the second sphere. (b) Predictor point for the next step.
Fig. 13. Representation for the first steps of the path tracking homotopy on the plane H1 . (a) First predictor point. (b) Correcting of predictor point. (c) Second sphere in the follow.
The representation for the first sphere in the plane H1 is shown in Fig. 13(a). Then, we can calculate the intersection point between the first sphere and the γ curve; on the plane H1 , after three N-R iterations, we have the solution O2 = (x 2 , y2 , λ2 ) = (0.005079, 0.006707, 0.005404) as depicted in Fig. 13(b). The process continues to the next sphere ⎛ ∂H ∂H ⎞ 1
∂λ ⎜ = −(−1)3 det ⎝ ∂∂Hx μ2 = 2 ∂ρ ∂x
and then ⎛ ∂x ⎞ ⎜ ∂ρ ⎟ ⎝ ∂y ⎠ = ∂ρ =
−4 −0.912 2.563971 3.439780
1
∂y ⎟ ∂ H2 ⎠ = 2.764620 (51) ∂y
−1 −0.9127
−1
−13.695 −5.478
i.e., the predictor vector is ⎛ ⎞ ∂x ⎞ ⎜ ∂ρ ⎟ ⎛ 2.563971 ⎜ ∂y ⎟ ⎜ ⎟ ⎝ v p = ⎜ ⎟ = 3.439780 ⎠. ⎜ ∂ρ ⎟ 2.739132 ⎝ ∂λ ⎠
(52)
Planned trajectory for three circular obstacles.
Then, r = 0.01 − 0.013644 = 0.003644 and using (38), the new center is calculated and is depicted in Fig. 14(a) O¯ 2 = (cx , c y , cλ ) = O2 + 0.003644 O2 − O1 = (0.0069, 0.00915, 0.00737). (55) The predictor point is located in the same way as for the previous step and shown in Fig. 14(b) (56) v p = (0.503719, 0.675781, 0.538131) (x p , y p , λ p ) = O¯ 2 + 0.0169(0.5037, 0.6757, 0.5381) = (0.013803, 0.018373, 0.014717). (57) In this way, the calculation of the first centers of the spheres is made, and successive centers are found by following the path tracking homotopy. Fig. 15 presents the homotopy path from the initial point A to the target point B. The effect of the negative sign of the parameter p2 causes the homotopy path to pass under the obstacle Ob2 . This direction change effect in the path is explained in the following section. B. Several Configurations of Repulsion Parameters and Auxiliary Straights
(53)
∂ρ For the first step, it is proposed to set μ1 = 2.76462, μ2 = 2.739132, and = 2. The new sphere radius is − |2.76462| |2.73439| = 0.013644. (54) r2 = 0.01 1 − (1 − 2)e Fig. 13(c) presents the sphere S2 with radius r2 .
Fig. 15.
The EHPPM associates a parameter repulsive to each obstacle Obi as expressed in (4). The repulsion parameters determine the distance between the contour of the obstacle and the homotopy path. Fig. 16 shows the effect of the repulsion parameter for a circular object. Fig. 17 shows the variations in the homotopy path when the repulsion parameter of a rectangular obstacle varies. For a rectangular obstacle Ri , the repulsion parameter must be greater than that for a circular obstacle. This can be explained if we consider that the rectangular shape is governed by the power 2η, resulting in huge values when
This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination. 10
IEEE TRANSACTIONS ON AUTOMATION SCIENCE AND ENGINEERING
Fig. 16. Homotopy paths for tree values of the repulsion parameter p0 of the circular obstacle C0 . (a) Homotopy path for p0 = 0.01 and m 2 > m 1 . (b) Homotopy path for p0 = 0.001 and m 2 > m 1 . (c) Homotopy path for p0 = 0.0001 and m 2 > m 1 .
Fig. 17. Homotopy paths for tree values of the repulsion parameter p0 of the rectangular obstacle R0 . (a) Homotopy path for p0 = 100 and m 2 > m 1 . (b) Homotopy path for p0 = 10 and m 2 > m 1 . (c) Homotopy path for p0 = 1 and m 2 > m 1 .
Fig. 19. Several collision-free trajectories for the same scenario caused by changes in the auxiliary straights lines. (a) Collision-free trajectory for m 1 = 1 and m 2 = 1.1. (b) Collision-free trajectory for m 1 = 1 and m 2 = 2. (c) Collision-free trajectory for m 1 = 1 and m 2 = 4. Fig. 18. Upper and lower trajectory with respect to the diagonal line. (a) Collision-free trajectory for m 1 > m 2 . (b) Collision-free trajectory for m2 > m1.
the rectangles functions are evaluated far from its perimeter. Moreover, small values of repulsion parameter prevent the robot from deviating from the shortest path; however, very small values produce jumps from the solution path to closed curves [see Fig. 5(c)]. On the other hand, the sign of the repulsion parameter p0 determines whether the homotopy path passes above or below an obstacle Obi . Fig. 15 shows this effect: for Ob1 with p1 = +0.001, the homotopy path passes above, for Ob3 with p3 = +0.001, the homotopy path passes above, and for Ob2 with p2 = −0.001, the homotopy path passes below. This effect occurs only near the obstacle Obi . Similarly, modifying the expressions of the auxiliary straights lines can cause changes on the homotopy path. The auxiliary straight lines L 1 and L 2 can be modified by changing the value of m 1 and m 2 . Fig. 18(a) shows the upper trajectory for m 1 > m 2 , and Fig. 18(b) shows the lower trajectory for m 2 > m 1 . For the same scenario, five variants with different values of m 1 and m 2 are presented in Fig. 19.
TABLE II E NVIRONMENT M APS AND S IMULATION PARAMETERS OF F IG . 19. Ci R EPRESENTS C IRCULAR O BSTACLES AND Ri R EPRESENTS R ECTANGULAR O BSTACLES
The number of steps in the tracking largely depends on the appropriate choice of auxiliary lines and the repulsion parameters of each obstacle. For example, in Fig. 19(e), a trajectory shorter than that of Fig. 19(c) is displayed. The repulsion parameter and position of each obstacle used in Fig. 19 are depicted in Table II.
This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination. DIAZ-ARANGO et al.: HOMOTOPY PATH PLANNING FOR TERRESTRIAL ROBOTS USING SA
11
Fig. 20. Successful trajectories for a same obstacles map with different auxiliary straight lines. (a) Successful trajectory for m 1 = 1 and m 2 = 4. (b) Successful trajectory for m 1 = 0.1 and m 2 = 0.01.
Fig. 22.
Successful paths for complex indoor environment.
Fig. 23.
Successful path in sceneries with corridors.
Fig. 21. Successful trajectories for a same obstacles map and slight variations of repulsion parameters. (a) Successful trajectory for C1 and C4 with positive repulsion parameters. (b) Successful trajectory for C1 and C4 with negative repulsion parameters.
Fig. 19 presents a qualitative analysis to select the best values for m 1 and m 2 . In this, we can observe that using m 1 = 1 and m 2 = 4 produces a shortest path between two points. For simulations presented in the next sections, these are used as optimal values. C. Indoor Operation Now, we apply the method for path planning in a closed environment. Consider a room in which the objects or furniture are represented by rectangles and circumferences. What is more, the robot should not go through the walls, and then the walls are considered as additional obstacles. Fig. 20(a) shows an environment map for a robot confined into a room, where the initial point and target are in the free space. Fig. 20 shows two different homotopy path configurations for indoor environments. In particular, Fig. 20(b) shows the intersection between the homotopy path and the auxiliary straights, depicting how the methodology is not affected by this type of intersection. Then, we show that the proposed methodology is able to trace a collision-free path between two points in free space inside a closed environment. Furthermore, Fig. 21 shows two different configurations of collision-free paths produced by varying the repulsion parameter of each obstacle. Such figure is result of two different homotopy path configurations produced by a change in the sign of the repulsion parameters of some obstacles, as shown in Table III. We propose the repulsion parameter for circular
obstacles to be pi = 0.0001 and for rectangular obstacles pi = 1. Fig. 22 shows two successful paths, γ0,1 and γ0,2 , generated with different configurations of repulsion parameters of circular and rectangular obstacles. The path γ0,2 was generated using positive parameters for all obstacles, and γ0,1 was generated using positive and negative repulsion pi , as shown in Table IV. Variation of sign provides flexibility to create shortest trajectories, as shown in Figs. 21 and 22. One of the most difficult scenarios for existing path planning algorithms are the maps with corridors, and the next example shows the advantages of the proposed methodology. Fig. 23 shows the ability of the proposed methodology for finding successful paths in few steps for sceneries with corridors. For this environment map, the proposed methodology takes only 80 steps to reach goal point. The positions and size of the obstacles in the map are presented in Table V. D. Performance Simulation and Comparisons for Personal Computer In order to test the robustness of this method, tests for 20 maps that contained only circular obstacles were performed.
This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination. 12
IEEE TRANSACTIONS ON AUTOMATION SCIENCE AND ENGINEERING
TABLE III E NVIRONMENT M APS AND S IMULATION PARAMETERS OF F IGS . 20(a) AND (b) AND 21(a) AND (b). Ci R EPRESENTS C IRCULAR O BSTACLES AND Ri R EPRESENTS R ECTANGULAR O BSTACLES
TABLE IV E NVIRONMENT M APS AND S IMULATION PARAMETERS OF F IG . 22
Radius and centers of the obstacles are randomly chosen into a normalized space. The maximum radius of the obstacles is 0.05 and the minimum is 0.002. The first 20 maps correspond
to 10–200 obstacles with increments of 10 obstacles. For this paper, we have implemented the methodology in a personal computer (PC) (see Table VII) using C++ and Maple.
This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination. DIAZ-ARANGO et al.: HOMOTOPY PATH PLANNING FOR TERRESTRIAL ROBOTS USING SA
13
TABLE V E NVIRONMENT M APS AND S IMULATION PARAMETERS OF F IG . 19
Fig. 25. Collision-free path for a map with 200 circular obstacles. (a) Methodology in Maple. (b) Methodology in C++. TABLE VI S IMULATION PARAMETERS
Fig. 26.
Development card used in the tests.
scenario composed by 200 circular obstacles. As expected, both trajectories are equal, but the computing time for the C++ code is significantly lower. E. 32-b Microcontroller Test
Fig. 24. Computing time of simulations versus number of circular obstacles. (a) PC C++. (b) Maple 13 and ARM.
Table VI shows the simulation parameters used. Fig. 24 shows a graph with computing time obtained versus the number of obstacles in the map for the two programming languages. On the one hand, in general terms, Fig. 24 shows a remarkable decrease in computing time for the methodology when implemented in C++. On the other hand, Fig. 24(a) shows that the variable radius exhibits a better performance than the fixed radius strategy. In addition, Fig. 25 shows the homotopy paths obtained on Maple and C++ for a particular
Considering the implementation of the methodology in small robots, we implemented a version of the methodology in C++. Then, we made tests in a development card that integrates a 32-b ARM microcontroller. The main features of the development card are presented in Table VII. A methodology in ARM platform capable of handling maps with 200 circular obstacles was implemented. Fig. 26 shows the small dimensions of the development card used in the tests. The total time calculation versus the number of obstacles for the methodology with variable radius implemented in Maple 13 and ARM microcontroller embedded system is presented in Fig. 24(b). In Fig. 24(b), a significant improvement of calculation time for the ARM microcontroller with respect to the methodology implemented in Maple 13 is shown. This is an interesting result, considering that Maple runs into a PC with high resources and the C++ code was deployed into lowresource embedded system (see Table VII for a comparison). Similarly, the methods implemented in Maple 13 and C++ are considered the parameters shown in Table VI. Fig. 27 shows successful homotopy path configuration found by the methodology implemented in a microcontroller for 200 obstacles. The microcontroller was tested using 200 obstacles, resulting in a collision-free path depicted in Fig. 27. Such a path is the same as the ones obtained using PC test of Maple and C++, depicted in Fig. 25. Therefore, the microcontroller implementation using C++ is an effective and viable option for embedded systems. Table VII presents the main features
This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination. 14
IEEE TRANSACTIONS ON AUTOMATION SCIENCE AND ENGINEERING
TABLE VIII I NTERESTING F EATURES OF THE ROBOT P IONEER 3-DX
Fig. 27. Collision-free path for 200 obstacles. Points scored by the methodology implemented in microcontroller. TABLE VII M AIN F EATURES OF ARM D EVELOPMENT B OARD AND PC U SED TO P ERFORM S IMULATIONS B OTH U SING 32 B
Fig. 29. Linear and angular displacements of a differential-drive mobile robot for the first steps. (a) Linear displacement. (b) Angular displacement.
Fig. 28. Holonomic restrictions for the robot Pioneer 3-DX. (a) Pioneer 3-DX. (b) Path following by differential-drive mobile robot.
of the development card that make this an attractive platform for small robots. F. Pioneer 3-DX Simulation In order to prove the effectiveness of methodology for real robots, we developed a series of simulations in MobileSim (robot simulator by Adept MobileRobots) and ROS. MobileSim is a software that considers the holonomic and physics restrictions of a real robot, and the results are very reliable. For this paper, we propose to use a robot Pioneer 3P-DX that is a common robot on academic and research fields. Pioneer 3P-DX is a differential-drive mobile robot, and its displacement depends only on linear velocity v(t) and angular velocity w(t), as depicted in Fig. 28. The interesting characteristics of Pioneer 3-DX are depicted in Table VIII. The linear displacement is calculated using the Euclidean distance (dlin ) between two points of the path; in a similar form, we can calculate angular displacement using the difference (dang) between two angles with respect to each linear displacement vector, as shown in Fig. 29(a) and (b), respectively. In general, the linear and angular displacements
Fig. 30. Pioneer 3-DX simulations using MobileSim and ROS for indoor and open environments. (a) Pioneer 3-DX simulation for indoor environment. (b) Pioneer 3-DX simulation for an open environment with 200 obstacles.
are calculated as follows: dlin = (x i − x i−1 )2 + (yi − yi−1 )2 yi − yi−1 dang = φ2 − φ1 ; φi = arctan x i − x i−1
(58) (59)
where (x i , yi ) is a point of the successful path γ0 . Fig. 30(a) and (b) shows the simulations from MobileSim using the parameters of Pioneer 3-DX. For these simulations, we created a ROS package that is responsible for reading the points from the path and interpreting the data for the robot considering the holonomic restrictions. Fig. 30(a) shows the same trajectory for the path λ0,2 in Fig. 22. For this simulation, we consider the radius robot in the dimensions of each obstacle in the map and the indoor scenario is a square room of 20 m per side. The straight lines
This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination. DIAZ-ARANGO et al.: HOMOTOPY PATH PLANNING FOR TERRESTRIAL ROBOTS USING SA
15
are the same, but the start point is A = (6, 2) and goal point is B = (19, 13); the units are in meters. In the other side, Fig. 30(b) shows the same trajectory for the successful path in Fig. 25. The normalized area represents a square field of 20 m per side with A = (0, 0) and B = (20, 20); also the dimensions of the robot were considered for each obstacle. VIII. C OMPARATIVE P ERFORMANCE AGAINST E XISTING A LGORITHMS In order to show the advantages of the proposed methodology against SBP and BUG2 algorithms, we have proposed two case studies. For these cases, we used a PC whose characteristics are described in Table VII under Linux OS. The sampling-based planner algorithms were performed using the open motion library presented in [38], and planner arena (presented in [39]) was used to analyze the benchmark data. The Bug2 algorithm was tested using the Robotics Toolbox and MATLAB 2014b reported in [40]. On the other hand, the proposed methodology was tested using our C++ implementation. In order to unify the conditions for different simulation tools, the robot was considered as holonomic and the work space was normalized. Fig. 31 shows the successful solution paths for each algorithm for two indoor environment maps. Both maps have been designed in order to test the robustness and efficiency of each algorithm. Fig. 31 shows the successful solution paths for each algorithm for two indoor environment maps. We can conclude in both maps that the KPIECE1 and EST are inefficient to obtain a shortest path. On the other hand, HPPM, RRTstar, and PRM show a better performance, being the RRTstar the best in terms of shortest path. The Bug2 algorithm obtains a good solution for map 2, but for map 1, it is inefficient. A fuller analysis of the performance of the algorithms is presented in the following comparative graphs to which 100 runs for each BSP algorithms are performed. Because EHPPM and Bug2 are deterministic nature, it was not necessary to make the same number of runs. For BSP algorithms, we use the default parameter configuration and 10 s of time limit. The minimum and maximum values in Figs. 32 and 33 for computing time and solution length are located at the ends of the thin line. On the one hand, Figs. 32(a) and 33(a) show the computing time for the eight algorithms. Note that for Bug2 algorithm, the spend computing time is lower (approximately 200 ms) for two study cases. For the BSPs, the minimum computing time is obtained by KPIECE1, 0.4821 and 1.0334 s for map 1 and map 2, respectively. However, for the most of BSP algorithms, the computing time is located between 1 and 10 s. RRTstar consumes all the time due to the process of optimizing the path length it contains, whereas for EHPPM, the computing time for map 1 is 27.3 ms and for map 2 is 28.4 ms. That indicates that the EHPPM implementation is dozens of times faster than the best time of BSP and hundreds of times faster than the normal time of a BSP algorithm. Figs. 32(b) and 33(b) present the path length for each planner. For EHPPM and Bug2, the length is constant unlike some of SBPs. The most inefficient are the EST and KPIECE1, and the
Fig. 31. Comparison of solution paths for sampling-based algorithms, BUG2, and EHPPM for two environment maps. (a) Indoor environment map 1. (b) Indoor environment map 2.
shortest path was found by RRTstar algorithm. In this graphs, we can observe that the length of the EHPPM path is close to the shortest and is the second shortest. On the other hand, Figs. 32(c) and 33(c) represent the total of successful solution paths from initial to goal points for each algorithm in the limit time. During the EHPPM simulations, ten reversal problems were detected and solved for map 1 and 18 for map 2, i.e., the strategy to correct reversal guaranteed the success of the EHPPM. IX. D ISCUSSION AND F UTURE W ORK The main results of this paper can be described as follows. 1) It is possible to modify the homotopy trajectory by changing the value and sign of the repulsion parameters and the slope of the auxiliary straight lines. On the one side, we present the use of negative and positive obstacle repulsion parameters to construct homotopy curves that cross between two neighboring obstacles.
This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination. 16
IEEE TRANSACTIONS ON AUTOMATION SCIENCE AND ENGINEERING
2)
3)
Fig. 32. Comparative results for map 1 after 100 runs for each BSP algorithm. (a) Computing time. (b) Path length. (c) Successful paths.
4)
5)
6)
Fig. 33. Comparative results for map 2 after 100 runs for each BSP algorithm. (a) Computing time. (b) Path length. (c) Successful paths.
This characteristic was not presented in the original method in which the homotopy curve is restricted only to cross under or lower a neighboring obstacles’ set [1]. This characteristic allows the EHPPM to be successful to environment maps with narrow corridors. On the other side, we experimentally presented the relationship among the value of the slopes of the straight lines and
7)
the shape of the trajectory, which performs better in terms of length for m 1 = 1 and m 2 = 4. In general terms, we presented a modified hypersphere technique to trace the homotopy paths from EHPPM. The performance tests for C++ and Maple in PC and C++ for a microcontroller exhibit the power of the proposed methodology. On the one side, the C++ for PC presented a high performance in the order of milliseconds even for 200 obstacles. On the other side, the C++ for ARM platform presents low to moderate simulation times for even 200 obstacles. However, for local maps with less than 20 obstacles, the ARM implementation shows a good performance in terms of time, less than 1 s of execution time, i.e., it is proper for real applications despite its low computational resources. Considering the results, the Maple implementation is a poor implementation that can be discarded for practical implementations. We presented a new strategy to avoid the reversion phenomenon that is based on a rotation scheme using an auxiliary plane related to H1. Such strategy proves to be efficient and reliable through all the case studies of this paper. For the first time, we presented the EHPPM methodology as path planner for indoor environments, which is used to solve local maps. The simulations for a Pioneer 3-DX show the potential of this methodology for real applications in academic and research activities. As expected, the simulation allows us to conclude that the total computation time is affected by the complexity of the environment map. In fact, the computation time for each iteration of the N-R method during the correction step depends on the number of obstacles in the environment. This can be understood if we remember that the N-R method needs the calculation of the Jacobian of the system, which increases its size depending on the number of obstacles. The dependence of each N-R iteration with respect to the number of obstacles is shown in Fig. 34(a) and (b). For the methodology in C++ language and implemented in PC and ARM microcontroller, the graph shows a linear dependence. Such linear dependency is not clear for Maple 13 mainly due to the complexity of its internal symbolic/numeric motor and JAVA performance issues. However, it can be said that the proposed methodology in C++ language has a linear dependence with respect to the number of obstacles. Comparative graphs in the previous section show that our proposed methodology is hundreds of times faster than the BSP algorithms and also the length of the path found is the second shortest path. Besides, the EHPPM computing time tends to grow linearly according to the number of obstacles, whereas BSP algorithms grow according to the number of nodes. In the same way, BSP uses smoothing and collision checker schemes, which implies a greater computing time. On the other hand, we can deduce that the correction reversal technique and
This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination. DIAZ-ARANGO et al.: HOMOTOPY PATH PLANNING FOR TERRESTRIAL ROBOTS USING SA
17
Fig. 35. Influence of the straight lines in the successful trajectory. (a) Successful trajectory for m 1 = 1 and m 2 = 4. (b) Successful trajectory for m 1 = 0.1 and m 2 = 0.01.
Fig. 34. Computation time for each iteration of N-R for each map of obstacles. (a) Computing time per N-R iteration versus number of circular obstacles for PC C++ language. (b) Computing time per N-R iteration versus number of circular obstacles for methodology in Maple 13 and ARM microcontroller.
SA make HPPM a very useful troubleshooting tool for route planning without greatly affecting the computing time. 8) Further work is necessary to optimize the Jacobian matrix inverse calculation using numerical and iterative strategies like lower and upper triangular matrix decomposition (LU) and iterative LU. Further work is required to improve the reliability and speed of the proposed tracking algorithm for the EHPPM method. Among the main roads to follow, we can mention the following. 1) Obtaining optimal paths also depends on the value of the repulsion parameters of each obstacle. Then, a preplanning stage could designate if the path must pass over or under the obstacle. This algorithm should be capable of assigning a sign to each repulsion parameter depending on the size, position with respect to a straight line from A to B, and proximity with the neighboring obstacles. 2) The optimal path is sometimes the shortest. However, it is important to correlate the shape of the path with the holonomic restrictions of real robots. It means that a given path can or cannot be followed by a real robot depending on its mechanical constraints. 3) In some cases, as the one in Fig. 35(a), it is not efficient to follow the behavior of the direct path and a more relaxed curve is required, as shown in Fig. 35(b). To do this, it is necessary to develop an algorithm that allows choosing the slope values of the auxiliary straight lines. 4) The computation times in the microcontroller are insufficient for real-time environments. Furthermore, as future work, the implementation of a simultaneous localization
and mapping (SLAM) algorithm is proposed to improve the computing time in the EHPPM. Having a preplanned path A to B, the SLAM algorithm could build a map of obstacles while the robot moves to the target point B [41]. 5) It is important to propose improved methods for the predictor step and variable radius, in order to reduce the number of steps even more during the tracking without causing jumps to isolated curves/branches. This can lead to reduction in the computation time. X. C ONCLUSION In this paper, a modified hyperspherical path following technique was presented to track the homotopy curves of an HPPM. The results show that this implementation is fast enough to be used in real-time applications (milliseconds) using PC of relative low resources. In addition, an implementation into an embedded system exhibits the great potential to construct hardware modules for path planning in reduced spaces. An empirical study of each parameter that affects the behavior of the homotopy path was presented, and an optimal value for them is proposed. An effective strategy to solve the reversal effect and to reduce the number of iterations in the corrector scheme was also proposed. We report the results obtained in multiple software platforms that validate the effectiveness of the methodology. Finally, the comparative results shows that the proposed methodology is faster than some SBP algorithms and the solution path is close to the shortest. D ISCLOSURE P OLICY The authors declare that there is no conflict of interests regarding the publication of this paper. R EFERENCES [1] H. Vazquez-Leal et al., “Exploring collision-free path planning by using homotopy continuation methods,” Appl. Math. Comput., vol. 219, no. 14, pp. 7514–7532, Mar. 2013. [2] Q. Zhao and S. Yan, “Collision-Free Path Planning for Mobile Robots Using Chaotic Particle Swarm Optimization,” Proc. 1st Int. Conf. Adv. Natural Comput., Part III, vol. 3612, Lecture Notes in Computer Science, Changsha, China: Springer-Verlag, 2005, pp. 632–635.
This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination. 18
[3] R. Kala, A. Shukla, and R. Tiwari, “Robotic path planning in static environment using hierarchical multi-neuron heuristic search and probability based fitness,” Neurocomputing, vol. 74, nos. 14–15, pp. 2314–2335, Jul. 2011. [4] N. Buniyamin, W. A. J. Wan Ngah, N. Sariff, and Z. Mohamad, “A simple local path planning algorithm for autonomous mobile robots,” Int. J. Syst. Appl., Eng. Develop., vol. 5, no. 2, pp. 151–158, 2011. [5] A. Yufka and O. Parlaktuna, “Performance comparison of the bug’s algorithms for mobile robots,” in Proc. Int. Symp. Innov. Intell. Syst. Appl. (INISTA), 2009, pp. 416–421. [6] O. Khatib, “Real-time obstacle avoidance for manipulators and mobile robots,” Int. J. Robot. Res., vol. 5, no. 1, pp. 90–98, 1986. [7] M. C. Lee and M. G. Park, “Artificial potential field based path planning for mobile robots using a virtual obstacle concept,” in Proc. IEEE/ASME Int. Conf. Adv. Intell. Mechatronics, vol. 2, Jul. 2003, pp. 735–740. [8] T. Laue and T. Röfer, “A behavior architecture for autonomous mobile robots based on potential fields,” in RoboCup: Robot Soccer World Cup. Berlin, Germany: Springer-Verlag, 2005, pp. 122–133. [9] M. A. P. Castañeda, J. Savage, A. Hernández, and F. A. Cosío, Local Autonomous Robot Navigation Using Potential Fields (Motion Planning). Rijeka, Croatia: InTech, 2008. [10] H. E. E. Cuchango, “Algoritmo para planear trayectorias de robots móviles, empleando campos potenciales y enjambres de partículas activas brownianas,” Ciencia E ingeniería Neogranadina, Bogotá, vol. 22, no. 2, pp. 75–96, Dec. 2012. [11] P. Brass, I. Vigan, and N. Xu, “Shortest path planning for a tethered robot,” Comput. Geometry, vol. 48, no. 9, pp. 732–742, Oct. 2015. [12] O. Salzman and D. Halperin, “Optimal motion planning for a tethered robot: Efficient preprocessing for fast shortest paths queries,” in Proc. IEEE Int. Conf. Robot. Autom. (ICRA), May 2015, pp. 4161–4166. [13] M. Elbanhawi and M. Simic, “Sampling-based robot motion planning: A review,” IEEE Access, vol. 2, pp. 56–77, 2014. [14] S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimal motion planning,” Int. J. Robot. Res., vol. 30, no. 7, pp. 846–894, 2011. [15] C.-B. Moon and W. Chung, “Kinodynamic planner dual-tree RRT (DT-RRT) for two-wheeled mobile robots using the rapidly exploring random tree,” IEEE Trans. Ind. Electron., vol. 62, no. 2, pp. 1080–1090, Feb. 2015. [16] J. J. Kuffner and S. M. LaValle, “RRT-connect: An efficient approach to single-query path planning,” in Proc. IEEE Int. Conf. Robot. Autom., vol. 2, Apr. 2000, pp. 995–1001. [17] S. M. LaValle, Planning Algorithms. Cambridge, U.K.: Cambridge Univ. Press, 2006. [18] S. M. LaValle and J. J. Kuffner, “Rapidly-exploring random trees: Progress and prospects,” in Proc. Workshop Algorithmic Found. Robot., 2000, pp. 293–308. [19] R. C. Melville, L. Trajkovic, S.-C. Fang, and L. T. Watson, “Artificial parameter homotopy methods for the DC operating point problem,” IEEE Trans. Comput.-Aided Des. Integr. Circuits Syst., vol. 12, no. 6, pp. 861–877, Jun. 1993. [20] L. T. Watson, M. Sosonkina, R. C. Melville, A. P. Morgan, and H. F. Walker, “Algorithm 777: HOMPACK90: A suite of Fortran 90 codes for globally convergent homotopy algorithms,” ACM Trans. Math. Softw., vol. 23, no. 4, pp. 514–549, Dec. 1997. [21] L. T. Watson, “Probability-one homotopies in computational science,” J. Comput. Appl. Math., vol. 140, nos. 1–2, pp. 785–807, Mar. 2002. [22] A. Dyes, E. Chan, H. Hofmann, W. Horia, and L. Trajkovic, “Simple implementations of homotopy algorithms for finding DC solutions of nonlinear circuits,” in Proc. ISCAS, vol. 6, Jul. 1999, pp. 290–293. [23] O. Brock and O. Khatib, “Real-time re-planning in high-dimensional configuration spaces using sets of homotopic paths,” in Proc. IEEE Int. Conf. Robot. Autom., vol. 1, Apr. 2000, pp. 550–555. [24] H. Vazquez-Leal et al., “Biparameter homotopy-based direct current simulation of multistable circuits,” Brit. J. Math. Comput. Sci., vol. 2, pp. 137–150, Jul. 2012. [25] K. Ahuja, L. Watson, and S. C. Billups, “Probability-one homotopy maps for mixed complementarity problems,” Comput. Optim. Appl., vol. 41, no. 3, pp. 363–375, Dec. 2008. [26] K. S. Gritton, J. D. Seader, and W.-J. Lin, “Global homotopy continuation procedures for seeking all roots of a nonlinear equation,” Comput. Chem. Eng., vol. 25, nos. 7–8, pp. 1003–1019, Aug. 2001. [27] T. L. Wayburn and J. D. Seader, “Homotopy continuation methods for computer-aided process design,” Comput. Chem. Eng., vol. 11, no. 1, pp. 7–25, 1987.
IEEE TRANSACTIONS ON AUTOMATION SCIENCE AND ENGINEERING
[28] K. Yamamura, “Simple algorithms for tracing solution curves,” in Proc. IEEE Int. Symp. Circuits Syst. (ISCAS), vol. 6, May 1992, pp. 2801–2804. [29] D. Torres-Muñoz, H. Vazquez-Leal, L. Hernandez-Martinez, and A. Sarmiento-Reyes, “Improved spherical continuation algorithm with application to the double-bounded homotopy (DBH),” Comput. Appl. Math., vol. 33, no. 1, pp. 147–161, Apr. 2014. [30] J. M. Oliveros-Muñoz and H. Jiménez-Islas, “Hyperspherical path tracking methodology as correction step in homotopic continuation methods,” Chem. Eng. Sci., vol. 97, pp. 413–429, Jun. 2013. [31] H. Vazquez-Leal et al., “New aspects of double bounded polynomial homotopy,” Brit. J. Math. Comput. Sci., vol. 3, no. 4, pp. 549–566, 2013. [32] H. Jiménez-Islas, “SEHPE: Programa para la solución de sistemas de ecuaciones no lineales mediante método homotópico con seguimiento hiperesférico,” Avances en Ingeniería Química, vol. 6, no. 2, pp. 174–179, 1996. [33] R. Castaneda-Sheissa, H. Vazquez-Leal, A. Yildirim, U. FilobelloNino, A. Sarmiento-Reyes, and L. H. Martínez, “Método modificado de hiperesferas aplicado a homotopías biparamétricas: Simulación de circuitos con transstores bipolares,” in Proc. Int. IBERCHIP Workshop, 2012, pp. 160–163. [34] H. Vazquez-Leal, R. Castaneda-Sheissa, F. Rabago-Bernal, L. Hernández-Martínez, A. Sarmiento-Reyes, and U. Filobello-Nino, “Powering multiparameter homotopy-based simulation with a fast path-following technique,” Int. Scholarly Res. Netw. ISRN Appl. Math., vol. 2011, 2011, pp. 1–7. [35] E. L. Allgower and K. Georg, “The basic principles of continuation methods,” in Introduction to Numerical Continuation Methods, SIAM, Philadelphia, PA, USA, 2003, vol. 45, Series: Classics in Applied Mathematics vol. 45, 2003, pp. 7–16. [36] H. Jiménez-Islas, “Paquete Computacional para la Resolución de Sistemas de Ecuaciones no Lineales,” Inst. Tecnológico de Celaya, Ingeniería Química en Procesos, Celaya, Guanajuato, Mexico, 1988. [37] A. S. Reyes, A Partition Method for Deternination Multiple DC Operating Points. Delft, The Netherlands: Delft Univ. Press, 1994. [38] I. A. Sucan, M. Moll, and L. E. Kavraki, “The open motion planning library,” IEEE Robot. Autom. Mag., vol. 19, no. 4, pp. 72–82, Dec. 2012. [39] M. Moll, I. A. Sucan, ¸ and L. E. Kavraki, “Benchmarking motion planning algorithms: An extensible infrastructure for analysis and visualization,” IEEE Robot. Autom. Mag., vol. 22, no. 3, pp. 96–102, Sep. 2015. [40] P. I. Corke, Robotics, Vision & Control: Fundamental Algorithms in MATLAB. Berlin, Germany: Springer, 2011. [41] H. Durrant-Whyte and T. Bailey, “Simultaneous localization and mapping: Part I,” IEEE Robot. Autom. Mag., vol. 13, no. 2, pp. 99–110, Jun. 2006.
Gerardo Diaz-Arango received the M.Sc. degree in electronics from the National Institute of Astrophysics, Optics and Electronics, Cholula, Mexico, in 2014, where he is currently pursuing the Ph.D. degree. His current research interests include robotics, mechatronics, and nonlinear circuits.
Héctor Vázquez-Leal received the B.Sc. degree in electronic instrumentation engineering from the University of Veracruz, the M.Sc. and Ph.D. degrees in electronic sciences from the National Institute of Astrophysics, Optics and Electronics, Cholula, Mexico, in 2001 and 2005, respectively. He is the author or co-author of 107 research articles published in several prestigious journals. His current research interests include analyticalnumerical methods, nonlinear circuits, robotics, applied mathematics, and wireless energy transfer. Dr. Vázquez-Leal is an editor of the three International journals and a regular invited reviewer of over 40 journals.
This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination. DIAZ-ARANGO et al.: HOMOTOPY PATH PLANNING FOR TERRESTRIAL ROBOTS USING SA
Luis Hernandez-Martinez received the Ph.D. degree in electronic engineering from the National Institute of Astrophysics, Optics and Electronics (INAOE), Cholula, Mexico, in 2001. Since 2001, he has been a Researcher with the Electronics Department, INAOE. His research interests include design automation, nonlinear circuits, and cellular neural networks.
María Teresa Sanz Pascual received the Ph.D. degree in electronic engineering from the University of Zaragoza, Zaragoza, Spain, in 2004. She is currently a Researcher with the Electronics Department, National Institute for Astrophysics, Optics and Electronics, Cholula, Mexico. Her research interests include analog and mixed IC design, integrated sensor readouts, and power conditioning. Dr. Sanz Pascual has been a member of the Electronic Design Group, Department of Electronic Engineering and Communications, University of Zaragoza, since 2008.
19
Mario Sandoval-Hernandez received the M.Sc. degree in electronic sciences from the National Institute of Astrophysics, Optics and Electronics, Cholula, Mexico, in 2001. His current research interests include analyticalnumerical methods, applied mathematics, and robotics.
