Advances in Modelling, Monitoring, and Control for Complex Industrial SystemsView this Special Issue
Research Article | Open Access
Jae-Han Park, Tae-Woong Yoon, "Maximizing the Coverage of Roadmap Graph for Optimal Motion Planning", Complexity, vol. 2018, Article ID 9104720, 23 pages, 2018. https://doi.org/10.1155/2018/9104720
Maximizing the Coverage of Roadmap Graph for Optimal Motion Planning
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.
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 . The probabilistic roadmap (PRM)  and rapidly exploring random trees (RRT)  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 , Gaussian random sampling , the bridge test , and visibility PRM  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  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
The configuration space is the space of all possible configurations of the system, where n is the number of degrees of freedom (DOF) of the system and is n-dimensional Euclidean space . In the formulation of the configuration space, the robot can be abstracted as a point . Free configuration is defined as one where the robot does not intersect with any obstacle in the workspace. The set of all free configurations is defined as the free configuration space , and it can be determined by checking for collisions between obstacles and the robot in the workspace. Every workspace obstacle is mapped as an obstacle in the configuration space . Figure 1 shows the mapping relations between the workspace and the configuration space for a two-link articulated robot.
The problem of motion planning requires as solution a feasible path in the free configuration space for a specified initial motion and a goal. The path planning problem can then be defined as follows:
Definition 1 ((path planning problem) ). Given a set of collision-free configurations , and initial and goal configurations , find a continuous curve , where and .
In this study, we employ the roadmap 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) ). A union of 1D curves is a roadmap if, for all and in that can be connected by a path, the following properties hold:(1)Accessibility: there exists a path from to some ,(2)Departability: there exists a path from to ,(3)Connectivity: there exists a path in between and .
The result of the roadmap method is a graph consisting of a set of nodes and a set of edges . Edge is created if the line segment between nodes and persists in , i.e., .
To obtain a feasible path for the robot’s motion using information from the roadmap graph, the three properties in Definition 2 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, the covering region of node for a certain neighbor radius , is defined as follows:
Let be the family of sets of ; subsequently, the roadmap coverage for neighbor radius is defined as the union of s as follows.
Definition 3 (roadmap coverage). Roadmap coverage for roadmap is the region of the union of the family of sets :
Figure 2 shows an example of a roadmap and its coverage for a configuration space.
According to the properties in Definition 2, 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 the optimal roadmap as the graph that maximizes the volume of roadmap coverage as follows.
Definition 4 (optimal roadmap). The optimal roadmap is the graph that maximizes the volume of roadmap coverage within the finite free configuration space:
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 be . Therefore, (3) can be written aswhere , and is the gamma function. The problem of designing the optimal roadmap can be transformed into a minimization problem as follows:
Moreover, the upper bound of , which is the sum of the intersection terms, can be expressed as follows [Section A.1]:where is a finite constant that depends on the number of nodes N.
This upper bound on can be applied to the following suboptimal roadmap problem.
Definition 5 (suboptimal roadmap). The suboptimal roadmap is the graph that minimizes the upper bound of in (7) such that
3. Coverage Maximization Algorithm
Let be the volume of the region of intersection between nodes, . By replacing the constraints with the collision detection function , the optimization problem in Definition 5 can be represented as follows:where is the collision detection function defined as
The volume of the region of intersection between n-dimensional hyperspheres with respect to the neighbor radius iswhere . See [Section A.2] for details.
The solution of this minimization problem is a node set that makes the gradient of to zero, i.e., .
Let be the stacked vector of N nodes. Using the steepest descent method , the algorithm to minimize (9) can be described as follows:where is the iteration index, and is the step size of each iteration. By the penalty parameter , the Lagrange multiplier can be determined by the following updating formula :where the penalty parameter is chosen to satisfy .
If the distance between nodes and is greater than the neighbor radius , the volume of the region of intersection becomes zero, i.e., . Also for node is valid only when is located within . Thereby, as shown in Figure 3, we define the neighboring node set located within the neighbor radius asFurthermore, using the decomposition method [28–30], the process in (13) can be transformed into a decentralized form that is processed at each node as follows:
in (16) is, however, not available because is discontinuous. In such cases, the gradient of can be estimated stochastically through response surface method (RSM) [31–33]. To apply the RSM algorithm, we define the sensing node as follows.
Definition 6 (sensing node). A sensing node is a point evenly sampled by Poisson-disk sampling  on , which is the surface of an n-dimensional hypersphere of radius . The sensing nodes detect the deformation of the configuration space around each node. The set of sensing nodes can be expressed aswhere and denote the sampling radius of the Poisson disk and the number of sensing nodes, respectively.
Let be a matrix composed of sensing nodes and be a stacked vector representing sensing information. If the sensing nodes are selected symmetrically such that , the estimated gradient can be calculated by the RSM as follows:where is the pseudoinverse matrix of and is a nonsingular matrix. See [Section A.3] for details.
represents the interaction between two nodes and , and it is a function of the distance . The gradient of can be described as follows :
In this algorithm, the major variable influencing the amount of computation needed is the number of nodes N. 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 nodes N are estimated as quadratic for the Laplacian matrix and linear 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 and constant , 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 . 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 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 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 . 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 should be considered.
Let the vector be the stacked vector of sensing vector at each node, and let the Laplacian matrix for be as follows:With the Lagrange multipliers set to , the decentralized differential equation shown in (22) can be integrated for as follows [Section A.4]:where is the Kronecker product operator and is a k-dimensional identity matrix.
Let be the whole-state variable in equilibrium, and let and be the entire sensing vector and the Laplacian matrix at that time, respectively. Subsequently, the differential equation at equilibrium is
In (25), the condition that renders the whole-state vector in equilibrium isThe cases of states of equilibrium that can meet such a condition can be summarized as follows.
Remark 7 (equilibrium conditions of suboptimal roadmap problem). The conditions of equilibrium states for the suboptimal roadmap algorithm are classified into three cases as follows:
Case 1 (). All nodes and their corresponding sensing nodes are in . Furthermore, no neighbor node exists because the neighbor radius is small for , such that . Figure 5(a) shows an example of this case.
Case 2 (). In this case, is a null-space element of the matrix , and all sensing nodes are in . If the graph is formed by a single connected component, the null-space elements are , where . Such states are rare in dynamical processes. Thus, herein, we do not consider this type of equilibrium.
Case 3 (). 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:
The maximum rank of the Laplacian matrix is n−1 . Although information concerning the Laplacian matrix and sensing vector in equilibrium state is given, the state vector cannot be calculated using (27). However, if the graph is a single connected component such as [36, 37], the state vector can be obtained with some additional constraints.
If a constraint such as is added, the state vector in equilibrium can be calculated as follows:where the symbol “+” indicates the pseudoinverse matrix and is an N-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 Figure 6.
Example 8 (two-node roadmap in 1D space). Obtain the equilibrium point for , , , , and .
The sensing node matrix is and its pseudomatrix is . The configuration space is symmetric to the origin. Thus, a constraint on the center of mass can be added, such as . Subsequently, the equilibrium point can be calculated using (28) as follows:
Hence, the roadmap at equilibrium is as shown in Figure 7.
Figure 8 shows the phase portrait of the optimization process for various initial conditions. As shown in the phase portrait, the states of converge for all initial conditions to or , as calculated in (29). In case of convergence to , the sensing vector is .
Figure 9 shows the phase portrait of example 1 for , 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.
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 thatis 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, the internal repulsion is defined as the quantitative information of the Laplacian matrix.
Definition 9 (internal repulsion). The internal repulsion is the sum of the repulsion factors between neighbor nodes in the graph. It can be defined as the entry-wise 1-norm of the Laplacian matrix as follows:
In the Laplacian matrix, the diagonal element is the sum of the row elements except itself, such that . Considering (23), the internal repulsion can be represented as follows:
Equilibrium in Case 1 can occur when the neighbor radius is small in comparison with the volume of the free configuration space, such that . Thus, whether the Laplacian matrix is a zero matrix is intimately related to the volume of the free configuration space and neighbor radius . 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.
Figure 10 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).
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 for , the internal repulsion regulation can be achieved. See [Section A.5] for proof.(1)Repulsion function condition: where is added to the repulsion function as a variable because it is the control value in internal repulsion regulation, and is the expectation of neighbor distance, i.e., .(2)Regulation controller condition: where is controller gain, is the desired internal repulsion value, and is the internal repulsion by the expectation of neighbor distance, i.e., .
In the suboptimal roadmap algorithm, the gradient of the repulsion function satisfies Condition (1) because it is calculated from (21) as follows:Thus, the internal repulsion can be regulated to the desired value using the controller of Condition (2). Such a controller in (34) can be implemented as a discrete-time system as follows:where is the sampling time or step size of the discrete-time system, and is the estimation of that can be calculated as the average of for intervals as .
Figure 12 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 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 13 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.
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.
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) . 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.
The first step in applying the motion-planning algorithm is to create the collision-detection function . We implemented it using the oriented bounding box (OBB) method, which yields the best efficiency in terms of both accuracy and computation time . 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 nodes N, we varied N from 50 to 300 and set the step size and the Lagrange multiplier to and , respectively. The number of sensing nodes and the radius of the sensing node were set to and , respectively, and the desired internal repulsion was set to .
Figure 17 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 18 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.
Figures 19 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 21 shows comparison graphs of the results of motion planning using different algorithms—the suboptimal roadmap, PRM, and RRT—with respect to the number of nodes N. 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.
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.
A.1. Upper Bound of Intersection Volume
Theorem A.1 (upper bound of intersection volume). In a family of sets composed of N sets, the upper bound of the intersection volume can be represented by the multiplication of the constant and the sum of the intersection volume of two sets as follows:where is a finite constant related to number of sets .
Proof. Let the sum of intersection volumes of elements in besubsequently, the intersection volume meets the following condition:Because by Lemma A.2, the following inequality is met for :further, the following inequality is also satisfied for :Hence, the intersection volume satisfies the inequality of (A.1).
Lemma A.2. meets the following inequality for :
Proof. Let the k-th intersection set in be and its volume be ; therefore, can be expressed as follows:In , as for , can be represented as Thus, can be expressed by the sum of terms of as follows:Let be the sum of that is applied to the same ; therefore, can be reformulated by the sum of terms of as follows: where .
can be represented by the inner product of two vectors and as follows:Because , is satisfied by the Cauchy–Schwarz inequality. andHence, the inequality in (A.6) is satisfied as follows:
A.2. Volume of Region of Intersection between -Dimensional Hyperspheres 
The volume of the n-dimensional hypersphere can be calculated by integrating the volume of an n−1-dimensional hypersphere along a certain axis. As shown in Figure 23, the region of intersection between spheres can be calculated by integrating the volume of the n−1-dimensional sphere from to along a certain axis. The volume of the n−1-dimensional hypersphere of radius iswhere is the gamma function .
To obtain the volume of the n-dimensional hypersphere of radius , the n−1 hypersphere is integrated for radius on . Thus, the integral of the yellow region in Figure 23 can be calculated by integrating the volume of the n−1 sphere as follows:
In Figure 23, the yellow area calculated by the integration is the half-region of the area of intersection; thus, the volume of intersection is twice the calculated volume. Further, and for ; thus, the volume of the region of intersection between the n-dimensional hyperspheres can be summarized as follows:
A.3. Gradient Estimation of the Collision-Detection Function
Function checks for collisions in the workspace for configuration . As is a discontinuous function, its gradient cannot be calculated. The gradient of can be estimated stochastically using the response surface method (RSM) [31–33].
In the RSM, for , , is assumed to be a linear model in the neighborhood of as follows:The gradient of the function above, which is an estimated gradient of at is
In the RSM algorithm, can be estimated using p design points around node ; here, the sensing nodes of correspond to the design points, i.e., . By stacking the information of design points, (A.17) can be represented in matrix form as follows:
Using the sensing vector at node , the parameter is estimated by the least-squares algorithm as follows :where and .
The matrix isif , then ; therefore, (A.21) becomesHence, the estimated gradient of is
A.4. Derivation of the Differential Equation for the Entire Optimization Variable
Let the Lagrange multipliers be ; then, the decentralized optimization algorithm can be formulated as follows:where is the distance between nodes , ; is the set of neighbors of ; and is a function defined in (21).
The equations of iteration performed at every node can be integrated such thatLet be the stacked vector of N sensing vectors ; then, using the Laplacian matrix , (A.28) can be reformulated for the entire variable as follows: