Path Planning Strategy for a Manipulator Based on a Heuristically Constructed Network

Abstract

Collision-free path planning of manipulators is becoming indispensable for space exploration and on-orbit operation. Manipulators in these scenarios are restrained in terms of computing resources and storage, so the path planning method used in such tasks is usually limited in its operating time and the amount of data transmission. In this paper, a heuristically constructed network (HCN) construction strategy is proposed. The HCN construction contains three steps: determining the number of hub configurations and selecting and connecting hub configurations. Considering the connection time and connectivity of HCN, the number of hub configurations is determined first. The selection of hub configurations includes the division of work space and the optimization of the hub configurations. The work space can be divided by considering comprehensively the similarity among the various configurations within the same region, the dissimilarity among all regions, and the correlation among adjacent regions. The hub configurations can be selected by establishing and solving the optimization model. Finally, these hub configurations are connected to obtain the HCN. The simulation indicates that the path points number and the planning time is decreased by 45.5% and 48.4%, respectively, which verify the correctness and effectiveness of the proposed path planning strategy based on the HCN.

1. Introduction

Due to unique operational flexibility, manipulators are widely used in space exploration and on-orbit operation [1,2,3]. To execute tasks quickly and accurately, manipulators should be able to plan a path independently [4]. During the process, obstacles should be taken into consideration for the safety of manipulators. Many scholars have studied this problem in recent years.

On collision-free path planning of a manipulator, Dijkstra algorithm and A-star (A*) algorithm are typically used algorithms [5,6,7]. Because the heuristic function introduced by A* algorithm can indicate search direction for the points selection, A* algorithm has higher efficiency than Dijkstra algorithm [8]. A* algorithm has been continuously improved by many scholars since it was created. Trovato et al. [9] proposed an improved differential A* algorithm, which can achieve better real-time performance than the traditional A* algorithm. Das et al. [10] proposed an online path planning of mobile robots in a grid-map environment using a modified real-time A* algorithm, which can minimize time, energy, and distance while searching the path. Fu et al. [11] proposed an improved A* algorithm, which can obtain a shorter and smoother path. The above research aims at improving some characteristics of the A* algorithm, such as path search and reconstruction. All of them search paths through grid-based maps by default. The grid-based map is constructed by decomposing the work space of a manipulator into interconnected and non-overlapping grids according to a certain granularity [12,13]. However, the side length of grids is artificially set in this process. If the side length of grids is small, the occupied storage space during path search will increase exponentially. Therefore, in the collision-free path planning of a manipulator based on A* algorithm, it is necessary to avoid being entangled with these issues caused by using a grid-based map.

The essence of building a grid-based map is to represent the work space of the manipulator, namely all possible positions and orientations of the manipulator [14]. Besides the grid method, the visibility graph method, free space method, and Voronoi diagram method are commonly used methods for work space representation in path planning [15]. Visibility graph method uses polygons (two-dimensional space) or polyhedrons (three-dimensional space) to characterize obstacles [16,17]; free space method characterizes the obstacle space and free space by mapping the collision-free path planning problem to joint space [18,19]; Voronoi diagram method divides the planning space into several regions, each containing only one obstacle, to realize the representation of the work space [20,21,22]. As we can see, the above three algorithms represent the work space by constructing an irregular network. Compared with the grid-based map, they can better fit obstacle information and occupy less storage space. However, when the path planning of a manipulator is carried out in the joint space, the obstacles are not continuous when they are mapped to the joint space of the manipulator. Considering the advantages of an irregular network, a path planning strategy based on the heuristically constructed network (HCN) will be proposed in this paper, which uses a connected irregular polygon instead of a regular grid to represent the work space.

In the process of a network construction, the key step is nodes determination [23,24,25]. Legowski et al. [26] mapped the path points in the task space to the joint space using inverse kinematics, but this method takes much more time and computational memory on inverse kinematics of manipulators. Qureshi et al. [27] proposed the potential function RRT*-based that incorporates the artificial potential field algorithm in RRT*, which artificially set the number of nodes and selected these nodes randomly. Gang et al. [28] proposed a node enhancing method to optimize the path searched by PRM algorithm. Matteo et al. [29] generated a cloud of safe via points around the workpiece and then searched the suboptimal safe path between the two positions based on graph theory and the Dijkstra algorithm. They consume undesired memory as well as time due to the number of nodes being artificially generated and the nodes being selected randomly [30]. In practice, computing resources and storage may be restrained in special application scenarios. Teleoperation is one of the common operations in space exploration and on-orbit operations. Due to limitations such as the data transfer window, the amount of the data transfer should be reduced as much as possible. In this paper, the nodes of networks will be selected scientifically, and it contains two keys: determining the nodes number and selecting the nodes. To avoid unwarranted nodes, the performance of nodes needs to be evaluated. Based on these nodes, we will obtain more efficient memory utilization.

In summary, a network that simplifies the amount of data is studied in this paper to represent the work space of manipulators. To achieve the goal, it is necessary to determine nodes reasonably. For this reason, this paper puts forward a path planning strategy based on the heuristically constructed network (HCN) and proposes a strategy to construct a HCN. The process of the HCN construction consists of three phases: nodes number determination, nodes selection, and nodes connection. Considering the connection time and connectivity of HCN, the nodes number is determined first. Then, the selection of nodes is decomposed into two processes: work space division and nodes optimization, where these nodes are called hub configurations. In this paper, considering the connection time and connectivity of HCN, the nodes number is determined first. Three indexes are established to evaluate the quality of hub configurations. Based on these indexes, considering constraints of nodes number and joint angle limits, K-means algorithm and genetic algorithm are used to select hub configurations optimally. Finally, these hub configurations are connected to obtain the HCN. The contributions of this paper mainly include the following points:

(1)

For the storage and computational resources problems caused by randomly generated nodes, a path planning strategy based on the heuristically constructed network (HCN) and a construction strategy for HCN are put forward to represent the work space for collision-free path planning. This operation can reduce the amount of data storage and data transmission, which is more suitable for resource-constrained conditions such as space exploration and on-orbit operation.

(2)

The nodes number of HCN is determined by quantitatively describing the connection time and connectivity of the network. The model of work space division and a hub configurations optimization model are established and solved to select the nodes. The above work can realize the heuristic selection of points. Based on these nodes, the HCN can get more efficient memory utilization.

(3)

By considering the similarity among the various configurations within the same region, the dissimilarity among all regions, and the correlation among adjacent regions, the evaluation indexes of regional properties are established for the work space division. These regions obtained can get better performance for connectivity, which can be used as the basis for hub configuration optimization.

The rest of the paper is organized as follows. In Section 2, the network-based collision-free path planning problem of manipulators is analyzed. In Section 3, the construction strategy for HCN is introduced, and then the work space of manipulators is represented by constructing HCN. In Section 4, simulations are carried out to verify the effectiveness of the proposed algorithm. In Section 5, the research results of this paper are presented.

2. Network-Based Collision-Free Path Planning Problem of Manipulators Analysis

The network-based collision-free path planning of manipulators can be described as follows, and each node in the network represents a configuration of the manipulator. Definitions of frequently used variables are as follows (

Table 1. Definitions of some variables.

Variable	Definition
n	the degrees-of-freedom (DOF) of a manipulator
qi	the angle of the ith joint, i = 1,2, …, n
$\mathit{P}={\left[q_1,\dots ,q_n\right]}^T\in {\Re }^n$	a configuration, i.e., joint angles of the manipulator
$\mathit{M}=\{{\mathit{P}}_1,{\mathit{P}}_2,\dots ,{\mathit{P}}_m\}$	a set consisting of m configurations
$\mathit{H}\in {\Re }^n$	a hub configuration
$\mathit{P}\mathit{H}=\{{\mathit{H}}_1,{\mathit{H}}_2,\dots ,{\mathit{H}}_p\}$	a set consisting of p hub configurations
${\mathrm{\Omega }}_i=\{{\mathit{P}}_1,\dots ,{\mathit{P}}_{N\_g_i}\}$	$\mathrm{a} \mathrm{set} \mathrm{consisting} \mathrm{of} N\_g_i$ configurations, i = 1,2, …, p
${\mathit{w}}_i={\left[q_{\mathit{w}1},\dots ,q_{\mathit{w}n}\right]}^{\mathrm{T}}\in {\Re }^n$	$\mathrm{the} \mathrm{center} \mathrm{configuration} \mathrm{in} \mathrm{the} \mathrm{region} {\mathrm{\Omega }}_i$, i = 1,2, …, p
$\mathit{P}C=\{{\mathit{w}}_1,{\mathit{w}}_2,\dots ,{\mathit{w}}_p\}$	a set consisting of p center configurations

):

Suppose a set $\mathit{M}$ contains $m$ configurations, and these configurations do not collide with obstacles. There are $p\left(p\le m\right)$ configurations from the set $\mathit{M}$ that can minimize Equation (1). The $p$ configurations are called hub configurations, and they play a similar role as bus stops. Let $\mathit{P}\mathit{H}$ be the set of $p$ hub configurations. It is assumed that each configuration of $\mathit{M}$ can only be transformed from one hub configuration, where the transformation between two configurations refers to the existence of a collision-free path between them. Then, the selection of hub configurations can be expressed as Equation (1).

$$\begin{array}{l}\min \left({\displaystyle \sum _{i=1}^m{\displaystyle \sum _{j=1}^m{x}_i{a}_{ij}\mathrm{dist}({\mathit{P}}_i,{\mathit{P}}_j)}}\right)\\ \mathrm{s}.\mathrm{t}.{\displaystyle \sum _{i=1}^m{a}_{ij}}=1,\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}j=1,2,\dots ,m\\ \hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}{\displaystyle \sum _{i=1}^m{x}_i}=p,\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}p\le m\hspace{0.17em}\end{array}$$

(1)

where ${\mathit{P}}_i\left(i=1,\dots ,m\right)$, ${\mathit{P}}_j\left(j=1,\dots ,m\right)$∈$\mathit{M}$; $x_i$ denotes whether the configuration ${\mathit{P}}_i$ is a hub configuration; dist (${\mathit{P}}_i$,${\mathit{P}}_j$) denotes the distance between configuration ${\mathit{P}}_i={\left[q_{i1},\dots ,q_{in}\right]}^{\mathrm{T}}$ and ${\mathit{P}}_j={\left[q_{j1},\dots ,q_{jn}\right]}^{\mathrm{T}}$ in the joint space, is calculated as Equation (2); $a_{ij}$ denotes whether the configuration ${\mathit{P}}_i$ is transformed from configuration ${\mathit{P}}_j$; and

$$\mathrm{dist}\left({\mathit{P}}_1,{\mathit{P}}_2\right)={\displaystyle \sum _{i=1}^n\left|q_{1i}-q_{2i}\right|}$$

(2)

$$a_{ij}=\{\begin{array}{l}1,\hspace{0.17em}\mathrm{configuration}\hspace{0.17em}{\mathit{P}}_i\hspace{0.17em}\mathrm{can}\mathrm{be}\mathrm{transformed}\mathrm{from}\mathrm{configuration}\hspace{0.17em}{\mathit{P}}_j\hspace{0.17em}\hspace{0.17em}\hfill \\ 0,\hspace{0.17em}\mathrm{otherwise}\hfill \end{array}$$

(3)

$$x_i=\{\begin{array}{l}1,\hspace{0.17em}\mathrm{cconfiguration}\hspace{0.17em}{\mathit{P}}_i\hspace{0.17em}\mathrm{is}\mathrm{the}\mathrm{hub}\mathrm{configuration}\hfill \\ 0,\hspace{0.17em}\mathrm{otherwise}\hfill \end{array}$$

(4)

Since it is assumed that each configuration of $\mathit{M}$ can only be transformed from one hub configuration, the above problem (Equation (1)) can be transformed as shown in Figure 1. The configurations that can be transformed from the same hub configuration are stored in one set. Furthermore, the determination of hub configurations can be transformed into dividing the $m$ configurations into $p$ groups and selecting one configuration in each group as a hub configuration. The sum of the distance from the hub configuration to the rest of the configurations in the same group is minimum. The problem can be derived as

$$\min \left({\displaystyle \sum _{i=1}^p{\displaystyle \sum _{{\mathit{P}}_j\in {\Omega }_i}\mathrm{dist}({\mathit{H}}_i,{\mathit{P}}_j)}}\right)$$

(5)

where ${\mathrm{\Omega }}_i\left(i=1,\dots ,p\right)$ denotes a set after the division of the set $\mathit{M}$, and ${\mathit{H}}_i\left(i=1,\dots ,p\right)$ denotes the hub configuration in set ${\mathrm{\Omega }}_i\left(i=1,\dots ,p\right)$.

According to the above ideas, the joint space path planning from ${\mathit{P}}_{\mathrm{initial}}$ to ${\mathit{P}}_{\mathrm{goal}}$ can be divided into three segments: planning path from the ${\mathit{P}}_{\mathrm{initial}}$ to the ${\mathit{H}}_{\mathrm{initial}}$ in the set ${\mathrm{\Omega }}_{\mathrm{initial}}$, planning path from the ${\mathit{H}}_{\mathrm{initial}}$ in the set ${\mathrm{\Omega }}_{\mathrm{initial}}$ to the ${\mathit{H}}_{\mathrm{goal}}$ in the set ${\mathrm{\Omega }}_{\mathrm{goal}}$, and planning path from the ${\mathit{H}}_{\mathrm{goal}}$ in the set ${\mathrm{\Omega }}_{\mathrm{goal}}$ to the ${\mathit{P}}_{\mathrm{goal}}$, as shown in Figure 2. ${\mathit{P}}_{\mathrm{initial}}$ and ${\mathit{P}}_{\mathrm{goal}}$ represent the start configuration and the end configuration of path planning, respectively. ${\mathrm{\Omega }}_{\mathrm{initial}}$ and ${\mathrm{\Omega }}_{\mathrm{goal}}$ denotes the set that ${\mathit{P}}_{\mathrm{initial}}$ and ${\mathit{P}}_{\mathrm{goal}}$ belong, respectively, in the above manner. ${\mathit{H}}_{\mathrm{initial}}$ and ${\mathit{H}}_{\mathrm{goal}}$ denote the hub configuration in the set ${\mathrm{\Omega }}_{\mathrm{initial}}$ and ${\mathrm{\Omega }}_{\mathrm{goal}}$, successively. Each segment of the path can be obtained by existing collision-free path planning algorithms.

As mentioned above, the network construction strategy consists of three parts: determining hub configurations number, selecting hub configurations, and connecting hub configurations. Additionally, the selection of hub configurations should be based on the work space division.

3. Construction of HCN

To characterize work space in the collision-free path planning of manipulators, this section proposes a construction strategy for HCN. Additionally, the determination of hub configurations number, selection of hub configurations, and connection of hub configurations are carried out.

For an n-DOF manipulator, the work space of manipulators can be expressed as

$$\mathit{M}=\{{\mathit{P}}_1,{\mathit{P}}_2,\dots ,{\mathit{P}}_m\}$$

(6)

3.1. Determination of Hub Configurations Number

The nodes of the HCN are hub configurations, and the number of hub configurations is equal to the number of groups, as mentioned in Section 2. With the number of groups increasing, the number of hub configurations increases, which could increase the connection time of HCN.

The connection of HCN is mainly realized by the existing path planning algorithms, and the time complexity of algorithms can reflect the magnitude of the program execution time changing with the input scale. Therefore, the paper chooses the time complexity of algorithms to measure the connection time of HCN. If the time of the basic operation execution is considered as unit time 1, the overall running time can be simply expressed as time complexity. The time complexity of existing path planning algorithms often appears as $O(\log n)$, $O(n)$, $O(n\log n)$, $O(n^2)$, and $O(n^3)$. Their size relationship is $O(\log n)<O(n)<O(n\log n)<O(n^2)<O(n^3)$. To select the number of groups that can meet most of the usage requirements, a function $h_1(p)$ is generated as

$$h_1(p)=\frac{p!}{2!(p-2)!}{N}_{ele\_total}{}^3=\frac{p(p-1)}{2}{N}_{ele\_total}{}^3=\frac{N_{ele\_total}{}^3{p}^2-N_{ele\_total}{}^{3}p}{2}$$

(7)

where $N_{ele\_total}$ represents the sum of the link number of the manipulator and the number of obstacles; two configurations are selected among $p$ hub configurations to plan path, and then the number of the possible situation is $\raisebox{1ex}{$p!$}\!\left/ \!\raisebox{-1ex}{$2!(p-2)!$}\right.$.

As the number of groups increases, the regions are more elaborate, and the difference among configurations in each region gradually decreases, which means the connectivity of HCN can be better.

To reflect the relationship between the number of groups and the configuration properties in each region, the error square sum is chosen to measure the relationship and generate a function $h_2(p)$ as

$$h_2(p)={\displaystyle \sum _{i=1}^p{\displaystyle \sum _{\mathit{P}\in {\mathrm{\Omega }}_i}\mathrm{dist}(\mathit{P},{\mathit{w}}_i)}}$$

(8)

where ${\mathrm{\Omega }}_i=\left\{{\mathit{P}}_1,\dots ,{\mathit{P}}_{N\_g_i}\right\}\left(i=1,\dots ,p\right)$ represents the $i$th region; $\mathit{P}\in {\Re }^n$ represents any one configuration in the region ${\mathrm{\Omega }}_i$; and ${\mathit{w}}_i={\left[q_{\mathit{w}1},\dots ,q_{\mathit{w}n}\right]}^{\mathrm{T}}$ can be calculated as

$${\mathit{w}}_i=\frac{1}{N\_g_i}{\displaystyle \sum _{j=1}^{N\_g_i}{\mathit{P}}_j}$$

(9)

As the number of groups increases, the performance reflected by the $h_1(p)$ increases, and the performance reflected by the $h_2(p)$ decreases. To consider $h_1(p)$ and $h_2(p)$ comprehensively, a function $h(p)$ is built as

$$h(p)={\mu }_1{h}_1(p)+{\mu }_2{h}_2(p)$$

(10)

where ${\mu }_1$ and ${\mu }_2$ are weighting coefficients, which are set according to the actual requirements. When storage is sufficient, the value of ${\mu }_2$ can be set relatively small; when computing resources are sufficient, the value of ${\mu }_1$ can be set relatively small.

Select the $\overline{p}$ corresponding to the minimum of the $h(p)$ as the number of groups, and the hub configurations number can be determined.

3.2. Selection of Hub Configurations

3.2.1. Establishment of Evaluation Indexes for Region Connectivity

The result of the work space division is some regions. Additionally, the properties of the regions influence the performance of the HCN. The HCN connection can be affected by the internal attribute of the region which the configuration belongs to, the attribute of adjacent regions, and the attribute of non-adjacent regions. Considering the above, three indexes are established to evaluate the regions.

• The similarity index $R_1$: This index is calculated by the mean of the total distance from all configurations to the corresponding regional center. It represents the dispersion of configurations. The smaller the $R_1$, the higher the similarity of configurations corresponding to the same region.

  $$\hspace{0.17em}{R}_1=\frac{1}{p}{\displaystyle \sum _{i=1}^p{r}_i},r_i=\frac{1}{N\_g_i}{\displaystyle \sum _{\mathit{P}\in {\mathrm{\Omega }}_i}\mathrm{dist}(\mathit{P},{\mathit{w}}_i)}$$

  (11)
• The dissimilarity index $R_2$: This index is calculated by the mean distance between the center configuration of two regions. It reflects the dissimilarity between these regions. The higher the $R_2$, the higher the difference between regions.

  $$R_2=\frac{2}{p^2-p}{\displaystyle \sum _{i=1}^{p-1}{\displaystyle \sum _{j=i+1}^p\mathrm{dist}({\mathit{w}}_i,{\mathit{w}}_j)}}$$

  (12)
• The correlation index $R_3$: This index is calculated by the spatial statistical correlation matrix corresponding to the spatial information contained in these regions. It reflects the adjacency relation of regions composed of some configurations represented by the sequence of joint angles in the manipulator joint space. The smaller the $R_3$, the higher the quality of regions.

  $$R_3=\frac{p}{{\displaystyle \sum _{i=1}^p{\displaystyle \sum _{j=1}^p{\omega }_{ij}}}}\cdot \frac{{\displaystyle \sum _{i=1}^p{\displaystyle \sum _{j=1}^p{\omega }_{ij}{({\mathit{w}}_i-\overline{\mathit{w}})}^{\mathrm{T}}({\mathit{w}}_j-\overline{\mathit{w}})}}}{{\displaystyle \sum _{i=1}^p{({\mathit{w}}_i-\overline{\mathit{w}})}^{\mathrm{T}}({\mathit{w}}_i-\overline{\mathit{w}})}}=\frac{{\displaystyle \sum _{i=1}^p{\displaystyle \sum _{j\ne i}^p{\omega }_{ij}{({\mathit{w}}_i-\overline{\mathit{w}})}^{\mathrm{T}}({\mathit{w}}_j-\overline{\mathit{w}})}}}{S^2{\displaystyle \sum _{i=1}^p{\displaystyle \sum _{j\ne i}^p{\omega }_{ij}}}}$$

  (13)

  where ${\omega }_{ij}$ represents the element of row i and column j in matrix $\mathit{W}$; $S^2=\frac{1}{p}{\displaystyle \sum _{i=1}^p{({\mathit{w}}_i-\overline{\mathit{w}})}^{\mathrm{T}}({\mathit{w}}_i-\overline{\mathit{w}})}$; $\overline{\mathit{w}}=\frac{1}{p}{\displaystyle \sum _{i=1}^p{\mathit{w}}_i}$.

The matrix $\mathit{W}$ indicates the adjacency relationship of $p$ regions, as shown in Equation (14).

$$\mathit{W}=\left[\begin{array}{cccccc}{\omega }_{11}& {\omega }_{12}& \cdot & \cdot & \cdot & {\omega }_{1p}\\ {\omega }_{21}& {\omega }_{22}& \cdot & \cdot & \cdot & {\omega }_{2p}\\ \cdot & \cdot & & & & \cdot \\ \cdot & \cdot & & & & \cdot \\ \cdot & \cdot & & & & \cdot \\ {\omega }_{p1}& {\omega }_{p2}& \cdot & \cdot & \cdot & {\omega }_{pp}\end{array}\right]$$

(14)

where ${\omega }_{ij}$ denotes the adjacency relationship between the ith region and the jth region, and

$${\omega }_{ij}=\{\begin{array}{l}1\hspace{0.17em},\hspace{0.17em}\mathrm{when}\mathrm{region}i\mathrm{and}\mathrm{region}j\mathrm{are}\mathrm{adjacent}\hfill \\ 0\hspace{0.17em},\hspace{0.17em}\mathrm{else}\hfill \end{array}$$

(15)

Based on definitions of the above three indexes, the similarity index $R_1$, the dissimilarity index $R_2$, and the correlation index $R_3$ are used to evaluate the properties of regions. When the connectivity of HCN is good, $R_1$, $R_2$, and $R_3$ are close to a fixed value.

3.2.2. Division of Work Space Based on K-Means Clustering Algorithm

Based on the above, the model of work space division can be built.

Suppose $p$ is a known positive integer as the number of groups, and a set $\mathit{M}$ contains $m$ configurations. Divide the set $\mathit{M}$ into $p$ subset ${\mathrm{\Omega }}_1,\dots ,{\mathrm{\Omega }}_p$ to minimize the value of the objective function. The objective function is

$$f\left({\mathrm{\Omega }}_1,\dots ,{\mathrm{\Omega }}_p\right)={\displaystyle \sum _{i=1}^m\underset{C\in \mathit{P}C}{\min }\mathrm{dist}({\mathit{P}}_i,\mathit{w})}$$

(16)

where the set $\mathit{P}C$ contains $p$ center configurations ${\mathit{w}}_1,\dots ,{\mathit{w}}_p$; $\mathit{w}\in \mathit{P}C$ denotes any one of configurations in the set $\mathit{P}C$.

K-means clustering algorithm is chosen to solve the work space division model, considering that K-means clustering algorithm can solve clustering problems with the linear magnitude of time complexity [31,32,33].

The process of work space division using K-means clustering algorithm is as follows.

Step 1: Select $p$ initial center configurations ${\mathit{w}}_1,\dots ,{\mathit{w}}_p$ randomly to form a set $\mathit{P}C$, denoted as $\mathit{P}C=\left\{{\mathit{w}}_1,\dots ,{\mathit{w}}_p\right\}$. Generate $p$ sets, and denote them as ${\mathrm{\Omega }}_1,\dots ,{\mathrm{\Omega }}_p$. Put initial center configurations ${\mathit{w}}_i(i=1,\dots ,p)$ into ${\mathrm{\Omega }}_i(i=1,\dots ,p)$ in order.

Step 2: Choose one configuration $\mathit{P}\in (\mathit{M}-\mathit{P}C)$, and calculate its distance to each configuration in the set $\mathit{P}C$.

Select the configuration $\mathit{w}\in \mathit{P}C$ corresponding to the minimum distance, and put the configuration $\mathit{P}$ into the set $\mathrm{\Omega }$ corresponding to the configuration $\mathit{w}$.

Step 3: Repeat Step 2. When all the configurations of the set $(\mathit{M}-\mathit{P}C)$ are traversed, go to Step 4.

Step 4: Recalculate the center configuration of each region with Equation (17). Update the set $\mathit{P}C=\left\{{\mathit{w}}_1,\dots ,{\mathit{w}}_p\right\}$.

$${\mathit{w}}_i={\displaystyle \sum _{\mathit{P}\in {\mathrm{\Omega }}_i}\frac{\mathit{P}}{N\_g_i}}(i=1,\dots ,p)$$

(17)

where $\mathit{P}\in {\mathrm{\Omega }}_i$ denotes any one configuration in the set ${\mathrm{\Omega }}_i$.

Step 5: Use indexes $R_1$, $R_2$, and $R_3$ to evaluate ${\mathrm{\Omega }}_1,\dots ,{\mathrm{\Omega }}_p$.

Step 6: Judge whether indexes $R_1$, $R_2$, and $R_3$ meets the threshold setting. If so, the process of work space division is over. Otherwise, go to Step 2.

Based on the above steps, the optimized ${\mathrm{\Omega }}_1,\dots ,{\mathrm{\Omega }}_p$ can be obtained.

3.2.3. Hub Configurations Optimization

To select hub configurations, a hub configurations optimization model is established based on comprehensive quality index for joint travel and planning time and constrains including joint angles ranges. Then, the genetic algorithm is used to solve the model.

Suppose a set $\mathrm{\Omega }$ contains $N\_g$ configurations, and there is a configuration $\mathit{H}$ from the set $\mathrm{\Omega }$ to minimize the function. The objective function $U(\mathit{H})$ can be expressed as

$$U(\mathit{H})={\displaystyle \sum _{i=1}^{N\_g}u\left(\mathit{H},{\mathit{P}}_i\right)}$$

(18)

where ${\mathit{P}}_i(i=1,\dots ,N\_g)\in \mathrm{\Omega }$; $u\left(\mathit{H},{\mathit{P}}_i\right)={\lambda }_{1}D\left(\mathit{H},{\mathit{P}}_i\right)+{\lambda }_{2}T\left(\mathit{H},{\mathit{P}}_i\right)$; $u\left(\mathit{H},{\mathit{P}}_i\right)$ denotes the synthesis of joint travel and planning time from configuration $\mathit{H}$ to configuration ${\mathit{P}}_i$ and represents the comprehensive quality index for joint travel and planning time; $D\left(\mathit{H},{\mathit{P}}_i\right)$ denotes the joint travel from configuration $\mathit{H}$ to configuration ${\mathit{P}}_i$; $T\left(\mathit{H},{\mathit{P}}_i\right)$ denotes the planning time from configuration $\mathit{H}$ to configuration ${\mathit{P}}_i$; and ${\lambda }_1$ and ${\lambda }_2$ are weighting coefficients for joint travel and planning time, respectively, ${\lambda }_1+{\lambda }_2=1$, and $0\le {\lambda }_1,{\lambda }_2\le 1$. When storage is sufficient, the value of ${\lambda }_1$ can be set relatively small; when computing resources are sufficient, the value of ${\lambda }_2$ can be set relatively small.

Any produced configuration during the optimization process should meet the limits of joint angle ranges.

$$\{\begin{array}{c}{q}_1\in \left[q_{\min \_1},q_{\max \_1}\right]\\ q_2\in \left[q_{\min \_2},q_{\max \_2}\right]\\ \cdot \\ \cdot \\ \cdot \\ q_n\in \left[q_{\min \_n},q_{\max \_n}\right]\end{array}$$

(19)

where $[q_{\min \_i},q_{\max \_i}](i=1,\dots ,n)$ is the joint angle range of the $i$th joint.

Based on the above mentioned, the selection of hub configurations can be converted into the minimization of the comprehensive quality index for joint travel and planning time under the constrain of joint angle ranges. Then, the above optimization can be described as

$$\begin{array}{l}\min U(\mathit{H})={\displaystyle \sum _{i=1}^{N\_g}u\left(\mathit{H},{\mathit{P}}_i\right)}\\ \mathrm{s}.\mathrm{t}.\{\begin{array}{c}{q}_1\in \left[q_{\min \_1},q_{\max \_1}\right]\\ q_2\in \left[q_{\min \_2},q_{\max \_2}\right]\\ \cdot \\ \cdot \\ \cdot \\ q_n\in \left[q_{\min \_n},q_{\max \_n}\right]\end{array}\end{array}$$

(20)

Genetic algorithm is a widely used optimization-problem-solving method, and it combines generation and test to simulate Darwin’s biological evolution theory and genetic mechanism for optimal solution search. Compared with the traditional optimization methods (enumeration method, heuristic method, etc.), genetic algorithm is based on biological evolution and has good convergence. When the accuracy of calculation is met, the calculation time is smaller, and the robustness is higher. Then, based on the optimization model, the genetic algorithm is chosen to optimize the hub configuration of each region.

The selection of hub configuration in a region $\mathrm{\Omega }$ is taken as an example to illustrate the process.

Step 1: Initialization. When a hub configuration is selected optimally, search space is the joint space represented by $N\_g$ configurations in the set $\mathrm{\Omega }$. Each configuration can be expressed as

$$\mathit{H}={\left[q_1,\dots ,q_n\right]}^{\mathrm{T}}\in {\Re }^n$$

(21)

Then, the encoding mechanism is determined as float point number encoding. Set the generation of evolution to $t=0$, set the maximum generation of evolution, and set the range of joint angles.

Initial population has $N_{pop}$ configurations; each individual has a structure as in Equation (21).

Step 2: Individual evaluation. Fitness function can be determined by the objective function, as Equation (20). Set the weight coefficients according to the requirements in the actual application process.

$$\begin{array}{l}\underset{\mathit{H}\in \mathrm{\Omega }}{min}U(\mathit{H})={\displaystyle \sum _{i=1}^{N\_g}u\left(\mathit{H},{\mathit{P}}_i\right)}\\ u\left(\mathit{H},{\mathit{P}}_i\right)={\lambda }_{1}D\left(\mathit{H},{\mathit{P}}_i\right)+{\lambda }_{2}T\left(\mathit{H},{\mathit{P}}_i\right)\end{array}$$

(22)

Step 3: Selection. The selection operator is applied to all configurations of the region $\mathrm{\Omega }$, and the optimized configuration can be directly inherited to the next generation. The roulette wheel selection method is used to select a configuration, that is, the configuration with the highest fitness is selected, and the selection probability of each configuration is proportional to the fitness value.

The selection probability of each configuration can be calculated as

$$p_{\chi }=\frac{U\left({\mathit{H}}_{\chi }\right)}{{\displaystyle \sum _{j=1}^{N_{pop}}U({\mathit{H}}_j)}}(\chi =1,\dots ,N\_pop)$$

(23)

Then, the cumulative probability can be calculated as

$$g_{\chi }={\displaystyle \sum _{j=1}^{\chi }{p}_j}(\chi =1,\dots ,N\_pop)$$

(24)

Then, random numbers $r_{11},\dots ,r_{1N\_pop}$ are generated between [0,1]. The interval needs to be determined sequentially, which each random number $r_{1k}(k=1,\dots ,N\_pop)$ belongs to. When $g_{\chi -1}\le r_{1k}\le g_{\chi }$, the configuration ${\mathit{P}}_{\chi }$ can be selected.

Step 4: Crossover. The crossover operation is applied to all configurations of the region. Select the parent according to the cross probability $P_C\in (0,1]$, and produce a new configuration based on the crossover operator $c\in (0,1]$. The linear crossover is used, that is, every part of the father and mother is linearly crossed to produce new configurations.

Then, random numbers $r_{21},\dots ,r_{2N\_pop}$ are generated between [0,1]. When $r_{2k}(k=1,\dots ,N\_pop)>P_C$, the current individual is selected as father, and the next individual is selected as mother for crossover operation.

$$\{\begin{array}{c}{\mathit{P}}_{father}^{ne\mathit{w}}=c\times {\mathit{P}}_{father}+(1-c)\times {\mathit{P}}_{mother}\\ {\mathit{P}}_{mother}^{ne\mathit{w}}=(1-c)\times {\mathit{P}}_{father}+c\times {\mathit{P}}_{mother}\end{array}$$

(25)

Step 5: Mutation. The mutation operation is applied on all configurations of the region. Select the variant according to the mutation probability ${\mathit{P}}_{\mathit{M}}\in (0,1]$, and mutate the joint angle based on the mutation operator $m\in (0,1]$.

Then, random numbers $r_{31},\dots ,r_{3N\_pop}$ are generated between [0,1]. When $r_{3k}(k=1,\dots ,N\_pop)>{\mathit{P}}_{\mathit{M}}$, the current individual is selected for mutation operation. The random integer $r_{random\_1}$ is generated between (0,1], and the random number $r_{random\_2}$ is generated between (0,1]. The random numbers $r_{random\_1}$ and $r_{random\_2}$ are used for mutation.

$$\{\begin{array}{c}{\mathit{P}}_{}^{ne\mathit{w}}=\mathit{P}+m\times ({\mathit{P}}_{\max }-\mathit{P})\times r_{random\_2},\hspace{0.17em}\hspace{0.17em}{r}_{random\_1}(\hspace{0.17em}\hspace{0.17em})\%2=0\\ {\mathit{P}}_{}^{ne\mathit{w}}=\mathit{P}-m\times ({\mathit{P}}_{\max }-\mathit{P})\times r_{random\_2},\hspace{0.17em}\hspace{0.17em}{r}_{random\_1}(\hspace{0.17em}\hspace{0.17em})\%2\ne 0\end{array}$$

(26)

where $r_{random\_1}(\hspace{0.17em}\hspace{0.17em})\%2$ denotes the remainder of the integer $r_{random\_1}$ divided by 2; ${\mathit{P}}_{\max }={\left[q_{\max \_1},\dots ,q_{\max \_n}\right]}^{\mathrm{T}}$.

Step 6: Judgment of the termination condition. Determine whether the generation of evolution reaches the maximum that was set in Step 1. If the condition is satisfied, the configuration with the greatest fitness in the evolution process is taken as the optimal situation, that is, the hub configuration of this region. Otherwise, turn to Step 2.

By performing the above optimization process in region ${\mathrm{\Omega }}_1,\dots ,{\mathrm{\Omega }}_p$ obtained by Section 3.2.2, the optimized $p$ hub configurations can be obtained heuristically.

3.3. Connection of Hub Configurations

After hub configurations selection, the HCN can be constructed by connecting these hub configurations. The connection of hub configurations is achieved using existing path planning algorithms to find a feasible path among the hub configurations of each region and saving these paths. In this subsection, a strategy is proposed for connecting hub configurations.

First, this paper will attempt to connect hub configurations in sequence. If the feasible path can be found, then save it as the edge of the HCN. Otherwise, connect it to the hub configuration of the adjacent region, and then try to connect the selected hub configuration with the target hub configuration, as shown in Figure 3.

In Figure 3, black solid ellipses denote obstacles, dotted lines indicate the region boundary, black dots indicate the hub configuration of each region, the pentagram represents the target hub configuration ${\mathit{H}}_b$, and the black solid line indicates the searched path. The region boundary is obtained by the envelope of these sets ${\mathrm{\Omega }}_1,\dots ,{\mathrm{\Omega }}_p$. As shown in Figure 3, the hub configuration ${\mathit{H}}_a$ to ${\mathit{H}}_b$ cannot be reached directly due to the obstacles. Then, the ${\mathit{H}}_a$ is connected with the adjacent region hub configuration ${\mathit{H}}_c$. Next, the ${\mathit{H}}_c$ is connected with ${\mathit{H}}_b$, and the optimal path needs to be selected as the side of ${\mathit{H}}_a$ to ${\mathit{H}}_b$ according to joint travel and planning time.

The ideas of this paper are summarized in Figure 4.

As shown in Figure 4, the process of HCN construction is as follows. Firstly, the connection time function of HCN and the configuration properties function in each region are generated to evaluate the influence of the number of regions on the properties of the HCN. Secondly, three indexes are proposed to evaluate the properties of the regions, which represent the similarity among the various configurations within the same region, the dissimilarity among all regions, and the correlation among adjacent regions, respectively. Thirdly, a work space division model is designed and solved. Further, based on the above three sections, the number of regions and the regions can be selected. Next, a hub configuration optimization algorithm is solved based on genetic algorithm. Finally, a strategy for connecting hub configurations is proposed.

4. Simulation and Discussion

To verify the effectiveness of the proposed construction strategy of HCN, this paper uses the 3-DOF manipulator for simulation verification, as shown in Figure 5, which is convenient to show work space representation. Its DH parameters are shown in

Table 2. DH parameter of the manipulator.

Link i	α (°)	A (mm)	θ (°)	d (mm)
1	0	0	0	1000
2	90	0	−90	0
3	0	550	0	0
E	90	0	0	550

. Additionally, the obstacles setting is shown in

Table 3. Obstacle Setting.

Obstacle	Center (mm)	Radius (mm)
1	[−400; 400; −400]	300

.

Set the range of joint angles to be ${\theta }_1\in \left[-\pi ,\pi \right]\left(\mathrm{rad}\right)$, ${\theta }_2\in \left[-\pi /6,\pi /2\right]\left(\mathrm{rad}\right)$, ${\theta }_3\in \left[-\pi /3,\pi /3\right]\left(\mathrm{rad}\right)$. Additionally, the step size is set to $\epsilon =\pi /18\left(\mathrm{rad}\right)$, which is used to obtain the work space. Then, the work space of the manipulator in Cartesian space can be shown in Figure 6. Figure 6a shows the three-dimensional workspace of the manipulator, and Figure 6b shows the top view of the three-dimensional workspace, as shown in Figure 6a. It is worth noting that the circular vacancy in Figure 6b is caused by the structure and joint limit of the manipulator, not obstacles.

This paper uses the function $h\left(p\right)$ and set ${\mu }_1=0.1,{\mu }_2=1$ to determine the number of hub configurations. At the same time, the work space of manipulators is divided based on the evaluation indexes for properties of regions. As shown in Figure 7, $p$ is determined as 21. Additionally, the results of space dividing are shown in Figure 8. Figure 8 shows a separate display of the three-dimensional envelope surface of the 21 regions.

Then, hub configurations are selected in each region based on genetic algorithm. The selection result is shown in Figure 9. Figure 9 shows the overall display of the three-dimensional envelope surface of the 21 regions in the joint space, and the red dots indicate the hub configurations selected by each region.

By connecting these hub configurations, the HCN can be shown in Figure 10. In Figure 10, the blue lines indicate the path between hub configurations.

In the case that the manipulator parameters and the working environment of manipulators are mentioned above, 100 different path planning tasks are generated randomly. This paper uses the A* algorithm based on the grid map and the A* algorithm based on the HCN to perform these path planning tasks. The comparison of planning results is shown in Figure 11 and

Table 4. The comparison between the experiments.

		Experiment 1	Experiment 2	Lower than Experiment 1
Planning time/(s)	Max	33.994	0.435	0.992646
	Min	0.219	0.172	0.002571
	Average	2.585619	0.26566	0.484
Path points	Max	39	23	0.615385
	Min	20	10	0.30303
	Average	29.19	15.93	0.455

.

In Table 4, columns 3 and 4, respectively, show the experimental results of the A* algorithm based on the grid map and the HCN for manipulator path planning, and column 5 shows the comparison results of the two experiments. In Figure 11, Figure 11a shows the comparison of planning time, and Figure 11b shows the comparison of path points number. As we can see, through planning time comparison, the usage of the HCN can shorten the planning time by 48.4%. Through the comparison of the number of path points in the path, the usage of the HCN can reduce the path point by 45.5%. The planning step length of the manipulator is the same, and the number of path points in the path can also reflect the length of the path. In summary, the correctness and effectiveness of the proposed strategy of HCN construction are verified.

5. Conclusions

For the collision-free path planning of manipulators, this paper proposes a path planning strategy based on the heuristically constructed network (HCN) and a construction strategy for HCN. First, the number of hub configurations was determined. Secondly, the evaluation indexes for properties of regions were established, and the work space was divided based on these indexes. Next, a hub configuration optimization model was established, and the hub configurations were obtained by solving the model. Finally, the HCN was constructed for the path planning of manipulators. To validate the strategy proposed in this paper, a 3-DOF manipulator was used for simulation. The simulation result shows that the proposed strategy can greatly improve the efficiency of collision-free path planning of manipulators. The contributions of this paper mainly include the following points:

• A path planning strategy based on the heuristically constructed network (HCN) and a construction strategy for HCN are put forward to represent the work space for collision-free path planning, which aims at dealing with the storage and computational resources problems caused by randomly generated nodes.
• By considering the relationship among different configurations, the work space division based on evaluation indexes of regional properties can realize the heuristic determination of nodes number, and the selection of hub configurations can realize the heuristic search for network nodes, which can make searching a path based on this network faster and use fewer path points.
• The use of HCN in collision-free path planning of manipulators can reduce the average number of path points of the path by 45.5% and reduce the average time of path planning by 48.4%, which greatly improves the efficiency of path planning under resource constraints.

The proposed network and strategy only carry out simulation verification with 3-DOF manipulator and A* algorithm, but its concept can be extended to other manipulators and path planning algorithms. Furthermore, by designing dynamic adjustment strategies, the proposed strategy can be used for dynamic and unknow situation.