# Maximizing the Coverage of Roadmap Graph for Optimal Motion Planning

Authors: Jae-Han Park; Tae-Woong Yoon
Journal: Complexity
 (2018)

---

## Abstract

Automated motion-planning technologies for industrial robots are critical for their application to Industry 4.0. Various sampling-based methods have been studied to generate the collision-free motion of articulated industrial robots. Such sampling-based methods provide efficient solutions to complex planning problems, but their limitations hinder the attainment of optimal results. This paper considers a method to obtain the optimal results in the roadmap algorithm that is representative of the sampling-based method. We define the coverage of a graph as a performance index of its optimality as constructed by a sampling-based algorithm and propose an optimization algorithm that can maximize graph coverage in the configuration space. The proposed method was applied to the model of an industrial robot, and the results of the simulation confirm that the roadmap graph obtained by the proposed algorithm can generate results of satisfactory quality in path-finding tests under various conditions.


---

## Body

## 1. Introduction

Automatic motion planning for robots is an important technology for next-generation manufacturing systems, such as Industry 4.0. Sampling-based algorithms have been widely used in robot motion-planning problems because they are efficient at providing reasonable solutions [1–4]. Sampling-based algorithms determine feasible paths for the robot’s motion using information from a graph that consists of randomly sampled nodes and connected edges in the given configuration space. Such randomized approaches have a strong advantage in terms of quickly providing solutions to complex problems, such as in a high-dimensional configuration space. However, sampling-based methods guarantee probabilistic completeness, and thus they require a large number of sampling nodes to find paths for the motion of the robot in arbitrary cases [2]. Theprobabilistic roadmap (PRM) [3] andrapidly exploring random trees (RRT) [5] are representative of such sampling-based motion-planning algorithms.The PRM method is composed of learning and query phases. In the query phase, collision-free paths for the motion of the robot are obtained by using information from a graph known as the roadmap [2, 3]. The learning phase is the process of constructing a graph that represents the morphology of the given configuration space. In this phase, it is important to construct the graph appropriately to obtain the shortest path for the collision-free motion of the robot. Developments have recently been reported on the optimality of sampling-based algorithms [6–9]. For example, the PRM∗ algorithm provides criteria for connecting edges that can guarantee asymptotic optimality in terms of the probability of finding path.An important factor to consider when constructing a graph for optimal motion planning is the node sampling strategy used. Sampling-based algorithms were originally designed with a random sampling of uniform distribution [2, 10]. However, a few drawbacks have been discovered in these uniform sampling strategies, such as the narrow passage problem and failure to capture the true connectivity of spaces of arbitrary shapes [9–11]. Thus, various sampling schemes, such as the Voronoi diagram [11], Gaussian random sampling [12], the bridge test [13], and visibility PRM [14] algorithms, have been proposed to overcome drawbacks in the uniform sampling method. Nevertheless, the effect of sampling strategies on optimal motion planning remains unclear. The node sampling strategy that fully captures the shape of the free configuration space remains an important problem in optimal motion planning [9, 10]. In the query phase of the roadmap method, the shortest path is obtained as the optimal path using graph information. Thus, constructing an appropriate graph is closely related to the problem of optimal motion planning.Another area highly relevant to our work here is research on optimization techniques. In recent years, with growing interest in machine learning and Artificial Intelligence, techniques of distributed optimization that can handle large amounts of data and high-dimensional variables have emerged as an important issue. In [15, 16], distributed optimization techniques were considered for multi-agent systems in complex networks. Secure and resilient techniques [17–19] have also been developed to guarantee the stability of distributed algorithms in adversarial environments. As highly relevant to our work here, coverage maximization was studied using methods of distributed optimization in applications of sensor networks [20–22].This paper proposes an algorithm to optimize the roadmap graph that can cover arbitrary morphologies of the free configuration space to maximize coverage. Our previous study [23] considered an adaptation algorithm for geometric graphs in an intuitive manner. However, in this study, we reanalyze that adaptive graph algorithm and present its results in terms of the maximization of graph coverage. We found that the optimization algorithm updates the positions of the nodes along the direction of expanding the region of covering until it reaches equilibrium, where expansion and contraction factors are balanced. The results of a simulation to test the proposed optimization algorithm show that it can construct a suitable graph to describe arbitrary configuration spaces, and the resulting graph can generate paths of shorter length for various test queries.


## 2. Problem Statement

Theconfiguration spaceQ⊂Rn is the space of all possible configurations of the system, wheren is the number of degrees of freedom (DOF) of the system and Rn isn-dimensional Euclidean space [1]. In the formulation of the configuration space, the robot can be abstracted as a point q∈Q.Free configuration is defined as one where the robot R(q) does not intersect with any obstacle in the workspace. The set of all free configurations is defined as thefree configuration spaceQfree⊂Q, and it can be determined by checking for collisions between obstacles and the robot in the workspace. Every workspace obstacle WOi is mapped as an obstacle in the configuration space QOi=q∈Q∣R(q)∩WOi≠φ [2]. Figure 1 shows the mapping relations between the workspace and the configuration space for a two-link articulated robot.Figure 1
Mapping relations between the workspace and the configuration space.The problem of motion planning requires as solution a feasible path in the free configuration space for a specified initial motion and a goal. Thepath planning problem can then be defined as follows:Definition 1 ((path planning problem) [1]).
Given a set of collision-free configurationsQfree⊂Q, and initial and goal configurations qinit,qgoal∈Qfree, find a continuous curve σ∈Σ=t∣t:[0,1]→Qfree, where σ(0)=qinit and σ(1)=qgoal.In this study, we employ theroadmap method for path planning. The roadmap represents the configuration space topologically by a network of one-dimensional (1D) curves. It is defined as follows.Definition 2 ((roadmap) [2]).
A union of 1D curves is aroadmapRM if, for all qinit and qgoal in Qfree that can be connected by a path, the following properties hold:(1)
Accessibility: there exists a path from qinit∈Qfree to some q′init∈RM,(2)
Departability: there exists a path from q′goal∈RM to qgoal∈Qfree,(3)
Connectivity: there exists a path in RM between q′init and q′goal.The result of the roadmap method is a graphG=(V,E) consisting of a set of N nodes V=qi∈Qfree∣∀i∈{1,⋯,N} and a set of edges E=eij=(qi,qj)∣qi,qj∈V. Edge eij is created if the line segment between nodes qi and qj persists in Qfree, i.e., tqi+(1-t)qj∈Qfree∀t∈[0,1].To obtain a feasible path for the robot’s motion using information from the roadmap graph, the three properties in Definition2 need to be met. Furthermore, for optimal planning, the best roadmap must be determined among a set of feasible roadmaps. We now employ roadmap coverage as a performance index to assess the optimality of the roadmap graph.To describe roadmap coverage, thecovering region of nodeqi for a certainneighbor radiusrη, is defined as follows:(1)Bi=Bqi,rη=q∈Rn∣q-qi2≤rη2,qi∈V.LetB=B1,B2,⋯,BN be the family of sets of Bi; subsequently, theroadmap coverage for neighbor radius rη is defined as the union of Bis as follows.Definition 3 (roadmap coverage).
Roadmap coverage C for roadmap G is the region of the union of the family of sets B:(2)CG=⋃B=B1⋃B2⋃⋯⋃BN.Figure2 shows an example of a roadmap and its coverage for a configuration space.Figure 2
Example of a roadmap and its coverage.Letμ(·) be the Lebesgue measure; by the inclusion–exclusion principle [25, 26], the volume of roadmap coverage μ(C)can be represented as follows:(3)μCG=μ⋃B=∑i=1N-1i-1Si,where Si=∑1≤j1<j2⋯<ji≤Nμ(Bj1∩Bj2∩⋯∩Bji).According to the properties in Definition2, the optimality of the roadmap can be considered to be the capability of finding paths for an arbitrarily set initial point and goal in the configuration space. To satisfy this requirement, roadmap coverage should cover the entire free configuration space. As a quantitative representation of this performance index, we define theoptimal roadmap as the graph that maximizes the volume of roadmap coverage as follows.Definition 4 (optimal roadmap).
Theoptimal roadmapG∗ is the graph that maximizes the volume of roadmap coverage within the finite free configuration space:(4)G∗=maximizeV=q1,q2,⋯,qNμCGsubject  toqi∈Qfree,i=1,⋯,N.The volume of roadmap coverage is primarily related to the geometric structure of the graph. It is complicated to calculate, especially in case the configuration space is high dimensional and the roadmap consists of a large number of nodes. Thus, an alternative performance index that uses the upper or lower bound for the volume of roadmap coverage is needed to render the optimization problem solvable.Let the sum of terms consisting of two or more intersections beI(B)=∑i=2N-1iSi. Therefore, (3) can be written as(5)μCG=μ⋃B=∑i=1N-1i-1Si=S1+∑i=2N-1i-1Si=∑i=1NμBi-IB,where ∑i=1NμBi=N·Γn/2+1-1π·rηn, and Γ(x) is the gamma function. The problem of designing the optimal roadmap can be transformed into a minimization problem as follows:(6)G∗=minimizeV=q1,q2,⋯,qNIBsubjecttoqi∈Qfree,i=1,⋯,N.Moreover, the upper bound ofI(B), which is the sum of the intersection terms, can be expressed as follows [Section A.1]:(7)IB≤κ·S2=κ·∑1≤i<j≤NμBi∩Bj,where κ=2/N(N-1)·∑i=2N∐j=2iNj is a finite constant that depends on the number of nodesN.This upper bound onI(B) can be applied to the followingsuboptimal roadmap problem.Definition 5 (suboptimal roadmap).
Thesuboptimal roadmapG^∗ is the graph that minimizes the upper bound of I(B) in (7) such that(8)G^∗=minimizeV=q1,q2,⋯,qN∑1≤i<j≤NμBi∩Bjsubjecttoqi∈Qfree,i=1,⋯,N.


## 3. Coverage Maximization Algorithm

LetΦ(qi,qj) be the volume of the region of intersection between nodes, μ(Bi∩Bj). By replacing the constraints qi∈Qfree with the collision detection function s(qi), the optimization problem in Definition 5 can be represented as follows:(9)G^∗=minimizeV=q1,q2,⋯,qN∑1≤i<j≤NΦqi,qjsubjecttosqi=0,i=1,⋯,N,where s(qi) is the collision detection function defined as (10)sqi=1collisiondetected0otherwise.The volume of the region of intersection between n-dimensional hyperspheres with respect to the neighbor radiusrη is(11)Φqi,qj=2·∫dij/2rη/2πn-1Γ1+n-1/2rη/22-t2n-1dt0≤dij<rη0dij≥rη,where dij=qi-qj2. See [Section A.2] for details.Using theLagrange multiplierλ, the optimization problem in (9) can be transformed as follows [27]:(12)G^∗=minimizeH,where H=∑1≤i<j≤NΦ(qi,qj)+∑i=1Nλis(qi).The solution of this minimization problem is a node set that makes the gradient ofH to zero, i.e., ∇H=∑1≤i<j≤N∇Φ(qi,qj)+∑i=1Nλi∇s(qi)=0.Letx=[qi]∈RnN be the stacked vector ofN nodes. Using the steepest descent method [27], the algorithm to minimize (9) can be described as follows:(13)xk+1=xk-ε∇Hk,∇Hk=∑1≤i<j≤N∇Φqik,qjk+∑i=1Nλik∇sqik,where k is the iteration index, and ε>0 is the step size of each iteration. By the penalty parameter c[k], the Lagrange multiplier λi[k] can be determined by the following updating formula [27]:(14)λik+1=λik+cksqik,where the penalty parameter is chosen to satisfy c[k+1]≥c[k].If the distancedij=qi-qj2 between nodes qi and qj is greater than the neighbor radius rη, the volume of the region of intersection becomes zero, i.e., Φ(qi,qj)=0. Also ∇Φ(qi,qj) for node qi is valid only when qj is located within rη. Thereby, as shown in Figure 3, we define theneighboring node set located within the neighbor radius as(15)ηi=qj∈V∣qj-qi2≤rη,j≠i.Furthermore, using thedecomposition method [28–30], the process in (13) can be transformed into a decentralized form that is processed at each node as follows:(16)qik+1=qik-ε∑qjk∈ηi∇Φqik,qjk-ελik∇sqik.Figure 3
Nodes neighboringqi within radius rη [23].∇ s ( q i ) in (16) is, however, not available because s(qi) is discontinuous. In such cases, the gradient of s(qi) can be estimated stochastically throughresponse surface method(RSM) [31–33]. To apply the RSM algorithm, we define thesensing nodeΔq∈Rn as follows.Definition 6 (sensing node).
Asensing nodeΔq is a point evenly sampled by Poisson-disk sampling [34] on ∂BrS=q∈Rnq2=rs, which is the surface of an n-dimensional hypersphere of radius rs. The sensing nodes detect the deformation of the configuration space around each node. The set of sensing nodes S can be expressed as(17)S=Δql∈∂BrS∣l=1,⋯,ns,Δqi-Δqj2>rpfor∀i,j∈1,⋯,ns,where rp and ns denote the sampling radius of the Poisson disk and the number of sensing nodes, respectively.Figure4(a) shows an example of sensing nodes sampled on the surface of a sphere, and Figure 4(b) shows a node and its sensing nodes in 3D configuration space.Figure 4
Example of sampled sensing nodes and a node in 3D configuration space. (a) Sensing nodes sampled on∂BrS(rs=5). (b) A node and its sensing nodes.
(a) (b)LetM=[ΔqlT]∈Rns×n be a matrix composed of sensing nodes and wi=[s(qi+Δql)]∈Rns be a stacked vector representing sensing information. If the sensing nodes are selected symmetrically such that ∑l=1nsΔql=0, the estimated gradient ∇s^(qi) can be calculated by the RSM as follows:(18)∇sqi≈∇^sqi=∇s^qi=M+wi,where M+=(MTM)-1MT is the pseudoinverse matrix of M and MTM is a nonsingular matrix. See [Section A.3] for details.Φ ( q i , q j ) represents the interaction between two nodes qi and qj, and it is a function of the distance dij=qi-qj2. The gradient of Φ(qi,qj) can be described as follows [35]:(19)∇Φqi,qj=∂Φdij∂dij·qi-qjdij.Then, it follows from (11) that(20)∂Φdij∂d=-2π/4n-1Γ1+n-1/2rη2-dij2n-10≤dij<rη0dij≥rη.It is noteworthy that ∂Φ(dij)/∂d≤0. We now set a nonnegative value of g(dij) as(21)gdij=-1dij·∂Φdij∂dij0<dij<rη0otherwise.Subsequently, the distributed optimization algorithm in (16) can be reformulated as follows:(22)qik+1=qik+ε∑qjk∈ηigdijkqik-qjk-ελikM+wik.In this algorithm, the major variable influencing the amount of computation needed is the number of nodesN. The main computational tasks of the optimization process are the construction of the Laplacian matrix and the computation of the sensing vector. In the original optimization process in (13), computational complexities according to the number of nodesN are estimated as quadratic O(N2) for the Laplacian matrix and linear O(N) for the sensing vector, respectively. However, as this algorithm can be decomposed as shown in (16), if a many-core processor such as a General-Purpose computing on Graphics Processing Unit (GPGPU) is available, (22) can be computed at each node in a separate processor in parallel. Thus, if the optimization algorithm can be executed by using parallel computation, the computational complexity of the Laplacian matrix and the configuration of the sensing vector are linear O(N) and constant O(c), respectively.The algorithm in (22) is an iterative process that can converge to a suboptimal roadmap by detecting the boundary of the state of collision of the configuration space through the sensing vector wi. Even if the morphology of the configuration space changes, the structure of the roadmap graph can adapt to it accordingly because the optimization process is continuously performed using the sensing node. In the general optimization algorithm, the Lagrange multiplier λi[k] is updated at every iteration. This is suitable in fixed configuration spaces, but it is difficult to apply to changeable spaces because if the space changes, the updated λi[k] becomes useless and needs to be recalculated to obtain the initial conditions. We set the Lagrange multiplier to a constant value λ as an iteration gain. Further, we set the same value for each node such that λ1=λ2=⋯=λN=λ. The results of a simulation show that the algorithm works well with a fixed λ even with a changing configuration space.


## 4. Equilibrium State Analysis

This section analyzes the states of equilibrium of the algorithm in (22). For the equilibrium analysis, the differential equation for the vector x=[qi]∈RnN should be considered.Let the vectorw=[wi]∈RnsN be the stacked vector of sensing vector wi at each node, and let theLaplacian matrixL=[Lij]∈RN×N for g(dij)≥0 be as follows:(23)L=Lij,Lij=-gdiji≠j∑k=1,i≠kNgdiki=j.With the Lagrange multipliers λi set to λ1=λ2=⋯=λN=λ, the decentralized differential equation shown in (22) can be integrated for x∈RnN as follows [Section A.4]:(24)xk+1=xk+εLk⊗Inxk-ελIN⊗M+wk=InN+εLk⊗Inxk-ελIN⊗M+wk,where ⊗ is theKronecker product operator and Ik∈Rk×k is ak-dimensional identity matrix.Letx∗ be the whole-state variable in equilibrium, and let w∗ and L∗ be the entire sensing vector and the Laplacian matrix at that time, respectively. Subsequently, the differential equation at equilibrium is(25)x∗=InN+εL∗⊗Inx∗-ελIN⊗M+w∗=x∗+εL∗⊗Inx∗-ελIN⊗M+w∗.In (25), the condition that renders the whole-state vector x in equilibrium is(26)εL∗⊗Inx∗-ελIN⊗M+w∗=0.The cases of states of equilibrium that can meet such a condition can be summarized as follows.Remark 7 (equilibrium conditions of suboptimal roadmap problem).
Theconditions of equilibrium states for the suboptimal roadmap algorithm are classified into three cases as follows:
Case  1 (L∗=0,w∗=0). All nodes and their corresponding sensing nodes are in Qfree. Furthermore, no neighbor node exists because the neighbor radius rη is small for μ(Qfree), such that μ(Qfree)>N·μ(B(qi,rη)). Figure 5(a) shows an example of this case.
Case  2 (L∗≠0,x∗∈Null(L∗⊗In),w∗=0). In this case, x is a null-space element of the matrix L∗⊗In, and all sensing nodes are in Qfree. If the graph is formed by a single connected component, the null-space elements are x=[qi], where q1=q2=⋯=qN [36]. Such states are rare in dynamical processes. Thus, herein, we do not consider this type of equilibrium.
Case  3  (L∗≠0,x∗∉Null(L∗⊗In),w∗≠0). The neighbor nodes and information regarding the sensing nodes exist. In this case, the equilibrium is a point, and this is the desirable solution to the suboptimal roadmap problem. Figure 5(b) shows an example of this case. In this state, the expanding factor between the neighbor nodes and the factor of sensing nodes that detect the boundary of collision are in balance as follows:(27)L∗⊗Inx∗=λIN⊗M+w∗.Figure 5
Examples of states of equilibrium: The yellow region shows the coverage of the roadmap. (a) An ill-structured roadmap. (b) A properly constructed roadmap.
(a) (b)The maximum rank of the Laplacian matrix isn−1 [37]. Although information concerning the Laplacian matrix L∗ and sensing vector w∗ in equilibrium state is given, the state vector x∗ cannot be calculated using (27). However, if the graph is a single connected component such as rank(L∗)=n-1 [36, 37], the state vector x∗ can be obtained with some additional constraints.If a constraint such as∑i=1Nqi=0 is added, the state vector in equilibrium can be calculated as follows:(28)x∗=L∗1N·1NT⊗In+λIN⊗M+w∗0,where the symbol “+” indicates the pseudoinverse matrix and 1N is anN-dimensional vector.We now verify this result through a simple example of a roadmap with two nodes in a 1D configuration space as shown in Figure6.Figure 6
Structure of roadmap for Example8.Example 8 (two-node roadmap in 1D space).
Obtain the equilibrium pointx∗=x1∗x2∗ for Qfree=x∈R∣-1≤x≤1, rs=0.6, λ=1, L∗=1-1-11, and w∗=1001T.The sensing node matrix isM=0.6-0.6 and its pseudomatrix is M+=0.8333-0.8333. The configuration space is symmetric to the origin. Thus, a constraint on the center of mass can be added, such as (x1+x2)/2=0. Subsequently, the equilibrium point can be calculated using (28) as follows:(29)x∗=1-1-111/21/2+1001⊗0.8333-0.833310010=0.4167-0.4167Hence, the roadmap at equilibrium is as shown in Figure7.Figure 7
State of equilibrium for Example8.Figure8 shows the phase portrait of the optimization process for various initial conditions. As shown in the phase portrait, the states of x converge for all initial conditions to x∗=0.4167-0.4167T or x∗=-0.41670.4167T, as calculated in (29). In case of convergence to x∗=-0.41670.4167T, the sensing vector is w∗=0110T.Figure 8
Phase portrait of the optimization process of Example8. (a) State trajectories for each initial condition. (b) Cost graph of functionH. (c) Contour graph of the cost function. (d) Cost graph.
(a) (b) (c) (d)Figure9 shows the phase portrait of example 1 for rη=0.6, where the neighbor radius is small for the volume of the configuration space. In this case, the equilibrium condition of the optimization process is the first case of Remark 7. The state of equilibrium is not a point but a region here, as shown by the two symmetrical triangles in Figure 9.Figure 9
Phase portrait of the optimization process of Example8 for rη=0.6. (a) State trajectories for each initial condition. (b) State trajectories and contour graph. (c) Contour graph. (d) Cost graph of functionH.
(a) (b) (c) (d)


## 5. Regulation of Internal Repulsion

From the differential equation in (24), the types of equilibrium can be summarized into three cases. We also confirmed that the equilibrium of Case 3, such that(30)L∗≠0,x∗∉NullL∗⊗In,w∗≠0,is the desirable solution to the suboptimal roadmap problem. To construct the appropriate roadmap, the Laplacian matrix must not be a zero matrix. To investigate this issue, theinternal repulsion is defined as the quantitative information of the Laplacian matrix.Definition 9 (internal repulsion).
Theinternal repulsionP is the sum of the repulsion factors between neighbor nodes in the graph. It can be defined as theentry-wise 1-norm of the Laplacian matrix as follows:(31)P=12L1=12vecL1=12∑i=1N∑j=1NLij.In the Laplacian matrix, the diagonal element is the sum of the row elements except itself, such thatLii=∑i=1,i≠jNLij. Considering (23), the internal repulsion can be represented as follows:(32)P=12∑i=1N∑j=1NLij=12∑i=1N2∑j=1,i≠jNgdij=∑i=1N∑j=1,i≠jNgdij.Equilibrium in Case 1 can occur when the neighbor radius is small in comparison with the volume of the free configuration space, such thatμ(Qfree)>N·μ(B(qi,rη)). Thus, whether the Laplacian matrix is a zero matrix is intimately related to the volume of the free configuration space μ(Qfree) and neighbor radius rη. Therefore, to determine the properties of convergence of the algorithm, we observe the steady-state value of internal repulsion with respect to various neighbor radii.Figure10 shows the maximum and minimum values of internal repulsion in the steady state for various neighbor radii. Figure 11 shows the structure of the roadmap in the steady state for some cases. In the internal repulsion graph, the maximum and minimum values are identical when the neighbor radius is smaller than 57. In this case, the internal repulsion and the nodes remain in a certain state, which means that the structure and the state of the roadmap converge to equilibrium. However, when the internal repulsion converges to zero, in the case of a neighbor radius smaller than 40, this means that the roadmap does not encompass the entire free configuration space, as shown in Figures 11(a) and 11(b). When the neighbor radius is 47, as shown in Figure 11(c), the structure of the roadmap encompasses the entire free configuration space well because the state of the roadmap converges with the appropriate internal repulsion, such as in the equilibrium of Case 3. When the neighbor radius is larger than 57, the maximum and minimum values are different. This means that the state of the nodes and the internal repulsion oscillate without converging. In this case, the algorithm cannot properly construct the roadmap because the state does not converge to equilibrium, as shown in Figure 11(d).Figure 10
Graph of internal repulsion with respect to neighbor radiusrη.Figure 11
States of roadmap after 100 iterations. (a)rη=10 (no action); (b) rη=30 (ill-structured graph); (c) rη=47 (well-structured graph); (d) rη=60 (does not converge).
(a) (b) (c) (d)The deformation of the configuration space can induce changes in the volume of the free configuration space. When the volume of the free configuration space changes, the roadmap may no longer be constructed properly because the neighbor radius may be large or small relative to the changed volume of the free configuration space. Thus, the parameter of the neighbor radius should be adjusted with respect to the volume of the free configuration space. Generally, the volume of the free configuration space is difficult to calculate. However, the internal repulsion is highly related to it and is computable by executing the algorithm. With the regulation of internal repulsion by a desirable value, the roadmap can be constructed stably regardless of the volume of the free configuration space. To regulate internal repulsion, the controller that adjusts the neighbor radius should satisfy the following conditions.Remark 10 (conditions for internal repulsion regulation).
If the repulsion function and the regulation controller meet the following conditions forL≠0, theinternal repulsion regulation can be achieved. See [Section A.5] for proof.(1)
Repulsion function condition:(33)∂grη,ρij∂rη>0∃ρij,ρij<rη,whererη is added to the repulsion function as a variable because it is the control value in internal repulsion regulation, and ρij is the expectation of neighbor distance, i.e., ρij=E[dij].(2)
Regulation controller condition:(34)rη=GIRR∫ePdt+rηt0,eP=Pd-Pρ,whereGIRR>0 is controller gain, Pd is the desired internal repulsion value, and Pρ is the internal repulsion by the expectation of neighbor distance, i.e., Pρ=∑i=1N∑j=1,i≠jNg(rη,E[dij]).In the suboptimal roadmap algorithm, the gradient of the repulsion function satisfiesCondition (1) because it is calculated from (21) as follows:(35)∂g∂rη=4dijπ/4n-1Γ1+n-1/2rηrη2-dij2n-30<dij<rη0otherwise.Thus, the internal repulsion can be regulated to the desired value using the controller ofCondition (2). Such a controller in (34) can be implemented as a discrete-time system as follows:(36)rηk=GIRR·∑l=k0kε·Pd-∑i=1N∑j=1,i≠jNgrηl,ρ-ijl+rηk0,where ε is the sampling time or step size of the discrete-time system, and ρ-ij[k] is the estimation of ρij that can be calculated as the average of dij for TR intervals as ρ-ij[k]=(1/(TR+1))∑j=k-TRkdij[j].Figure12 shows the efficacy of internal repulsion regulation in the case of a sudden change in free configuration space. At the initial condition, no obstacle exists and the free configuration space ratio μ(Qfree)/μ(Q) is 100%. After the process reaches the state of equilibrium, some obstacles are added so that the volume of the free configuration space is reduced to 69.3%. The graphs of the internal repulsion for this situation are depicted in Figure 12 with respect to the time series. The green line represents the fixed neighbor radius and the blue line corresponds to the internal repulsion regulation. As shown in the graph, if no obstacles exist in the configuration space, both roadmaps converge to a similar structure. However, after the addition of obstacles into the configuration space, the roadmap with the fixed neighbor radius cannot converge and exhibits unstable oscillation, as depicted by the green line. However, the blue line graph shows that the roadmap converges to equilibrium by regulating the internal repulsion uniformly regardless of the volume of the free configuration space.Figure 12
Graphs of internal repulsion for a change in the configuration space with a fixed neighbor radius (rη=50) and with internal repulsion regulation (Pd=700).Figure13 shows an overall diagram of the optimization algorithm with internal repulsion regulation. This system features two types of differential equations. One is the differential equation used to maximize coverage that can construct an appropriate graph against deformations in the configuration space, and the other is that for internal repulsion regulation of the roadmap regardless of the volume of the free configuration space. With these differential equations, the suboptimal roadmap algorithm can construct the proper roadmap graph that covers arbitrary shapes of the free configuration space.Figure 13
Overall structure of the suboptimal roadmap algorithm with internal repulsion regulation.The suboptimal roadmap algorithm can be optimized by dividing its complex procedures using parallel computing techniques, such as multithreading or GPU processing. Thus, (22) is computed concurrently at each node. Figure 14 shows the flowchart for processing the suboptimal roadmap algorithm with internal repulsion regulation. The four procedures in the red border are performed in parallel at each node.Figure 14
Flowchart for processing the suboptimal roadmap algorithm with internal repulsion regulation.


## 6. Case Study

To verify the effectiveness of the proposed method, we applied the suboptimal roadmap algorithm to a robot system in a car manufacturing assembly line. The target robot was the MPX3500, a painting robot system of the Yaskawa Electric Corporation (YEC) [24]. As shown in Figure 15, the MPX3500 robot had six axes; the S, L, and U axes rendered translation motions of the end effector. The other axes—R, B, and T—provided orientation motions. In conventional industrial robots, these axes are located around the end effector. In this application, only three axes—the S, L, and U axes—are considered as the orientation-related motions of the end effector do not affect motion related to collisions. Hence, the configuration space of the MPX3500 was a 3D space, and the constructed roadmap could be visualized with a geometrical representation.Figure 15
YEC-MPX3500 painting robot system [24]. (a) MPX3500 robot. (b) Scene of its operation at an automobile painting line.
(a) (b)The first step in applying the motion-planning algorithm is to create the collision-detection functions(q). We implemented it using theoriented bounding box (OBB) method, which yields the best efficiency in terms of both accuracy and computation time [38]. The suboptimal roadmap algorithm was applied to plan the motion of the MPX3500 robot in the automobile manufacturing line. Figure 16 shows the simulation environment, where a model of a car and some obstacles were placed. To observe the performance of the constructed roadmap with respect to the number of nodesN, we variedN from 50 to 300 and set the step size and the Lagrange multiplier to ε=0.01 and λ=25, respectively. The number of sensing nodes and the radius of the sensing node were set to ns=100 and rs=5, respectively, and the desired internal repulsion was set to Pd=100.Figure 16
Workspace of an automobile manufacturing line. The car model and some obstacles are placed in the simulation environment.Figure17 shows the state of the roadmap for the iterations of the suboptimal roadmap algorithm for 150 nodes. Figure 17(a) displays the initial state of the roadmap, in which 150 nodes were scattered randomly in the configuration space. Figures 17(b) and 17(c) show the states of the roadmap after the first and the fifth iterations, respectively. These states are the initial steps of the execution of the algorithm. Figures 17(d), 17(e), and 17(f) show the states of the roadmap after the 25th, 50th, and 100th iterations, respectively. After 25 iterations, no considerable changes were observed in the state of the roadmap, which indicates that the iteration process might have arrived at a minimum state.Figure 17
Operating procedures of the suboptimal roadmap algorithm in the configuration space (150 nodes). (a) Initial state. (b) 1st iteration. (c) 5th iteration. (d) 25th iteration. (e) 50th iteration. (f) 100th iteration.
(a) (b) (c) (d) (e) (f)Figure18 shows roadmaps of the suboptimal roadmap algorithm with respect to the number of nodes after 100 iterations. As shown in the figures, the appearances of the roadmap graphs are similar. However, the density of nodes is different with respect to the number of nodes. Although the quality of the results of planning depends on the density of nodes, near-optimal solutions were obtained even for a low density of nodes because the nodes were dispersed evenly.Figure 18
Roadmaps constructed by the suboptimal roadmap algorithm with respect to the number of nodes after 100 iterations. (a)N =50; (b)N =100; (c)N =150; (d)N =200; (e)N =250; (f)N =300.
(a) (b) (c) (d) (e) (f)Figures19 and 20 show examples of the results of motion planning using the optimal roadmap, PRM∗, and RRT algorithms in the test environment. Figure 19(a) shows the initial motion and (b) shows the goal motion of the robot. (c) and (d) present the compared paths obtained by the three methods in the configuration space. Figure 20 shows the three trajectories of the robot’s motion according to the planned path in Figure 19 in the workspace. As shown in the figures, the paths obtained by the suboptimal roadmap method were the shortest for the two workspaces; those obtained by the PRM∗ algorithm were the next shortest, and the RRT algorithm offered suboptimal results, such as paths involving detours, as confirmed in Figures 19(c) and 19(d).Figure 19
Comparison of the results of motion planning in the test environment. (a) Initial configuration. (b) Goal configuration. (c), (d) Motion paths obtained by the proposed method (red), PRM∗ (magenta), and RRT (blue) algorithms in the configuration space.
(a) (b) (c) (d)Figure 20
Trajectories of the robot’s motion in the workspace with respect to the planning results of Figure19. (a) Proposed method. (b) PRM∗. (c) RRT.
(a) (b) (c)Figure21 shows comparison graphs of the results of motion planning using different algorithms—the suboptimal roadmap, PRM∗, and RRT—with respect to the number of nodesN. Using roadmaps with varying number of nodes, each calculation was performed 10 times to plan for 100 pairs of motions of arbitrary initial points and goals. For all roadmaps, the results of the suboptimal roadmap algorithm yielded the lowest value of accumulated length of the planned path. This indicates that the paths obtained using the proposed method were closest to the optimal results for the given motion-planning problem. Figure 22 shows the graph of the cumulative length of the paths obtained with the suboptimal roadmap and the PRM∗ algorithm with respect to the number of nodes. As shown in the figure, the graph of the suboptimal roadmap algorithm is always lower than that of the PRM∗. This indicates that better motion-planning results can be obtained with the suboptimal roadmap algorithm with a small number of nodes. Moreover, the ratios of decrement of the suboptimal roadmap graphs were smaller than those of PRM∗. This means that the motion-planning performance of the suboptimal roadmap algorithm was less sensitive to the number of nodes than current sampling-based methods.Figure 21
Results of comparison of path qualities obtained using the proposed method, PRM∗, and RRT algorithms with respect to the number of nodes. (a)N =50; (b)N =100; (c)N =150; (d)N =200; (e)N =250; (f)N =300.
(a) (b) (c) (d) (e) (f)Figure 22
Results of evaluation of the total cumulative path of the proposed method and PRM∗ algorithm with respect to the number of nodes in the test environment.


## 7. Conclusion

This paper considered the problem of constructing a proper roadmap graph that fully encompasses the arbitrary morphologies of the free configuration space. To this end, the coverage of the graph was defined as a performance index on the optimality of a graph constructed by the sampling-based algorithm. We then proposed an optimization algorithm that can maximize graph coverage in the configuration space. Because of the computational complexity of coverage in practice, the proposed algorithm was designed using the concept of the suboptimal roadmap that minimizes the upper bound of the volume of intersection of the graph-covering region. Equilibrium conditions for the optimization algorithm were derived from the iteration equation. Among the results, we found the equilibrium condition that can construct an appropriate roadmap in an arbitrary configuration space. Furthermore, a method for regulating internal repulsion was proposed to utilize the suboptimal roadmap algorithm regardless of changes to the volume of the free configuration space. The convergence of the algorithm was found to be highly relevant to the volume of the free configuration space, accompanied by the number of nodes and the neighboring radius. Thus, to represent these relations quantitatively, internal repulsion was defined as the sum of repulsion factors between neighboring nodes in the graph. Further, we proposed an integrator-type controller that adjusts the neighboring radius to regulate internal repulsion according to the desired value. The feasibility of the proposed method was confirmed through a case study involving a robot. We applied the method to an industrial robot used in car manufacturing assembly lines. The results of the case study confirmed that the roadmap graph obtained by the proposed algorithm can produce more optimal results for paths of motion of the robot in the simulation environment.



---

*Source: 9104720-2018-11-08.xml*