2.1. Traditional A* algorithm

The A* algorithm is a heuristic global optimal path planning algorithm that enables efficient path planning in an obstacle-aware environment [Reference Zhang, Zhang, Zeng and Li31]. The direction of the A* algorithm’s path search is determined by the cost function. In each round of path search, the cost function value of each child node in the vicinity of the parent node is calculated, and the child node with the smallest cost function value is chosen as the parent node in the next round. The final path result is then searched in this cycle. The cost function is typically expressed as follows:

(1) \begin{equation} F\!\left(n\right)=G\!\left(n\right)+H\!\left(n\right) \end{equation}

where $G(n)$ is the current path cost, which represents the cost of moving from the starting point to the child node, and $H(n)$ is the heuristic function known as the estimated cost, which represents the cost of moving from the child node to the target point.

However, if the traditional A* algorithm is directly applied to the path planning of the robotic arm in 3D environment, the following three issues will arise: (1) The A* algorithm’s calculation speed in 3D environment will be drastically reduced; (2) when a node encounters obstacles, the A* algorithm’s search efficiency is drastically reduced; (3) the traditional A* algorithm treats the moving subject as a point and only considers whether the moving point collides with environmental obstacles. However, the position relationship between the robotic arm’s rod, the end position of the robotic arm, and the obstacles must be carefully considered when planning the robotic arm’s path.

Due to the above-mentioned shortcomings of the traditional A* algorithm in practical applications, this paper proposes an enhanced A* algorithm to apply the motion planning of the robotic arm to the known obstacle environment model. The enhanced A* algorithm first proposes a node collision detection method and then improves the efficiency of obstacle avoidance by refining the node search direction and enhancing the local path optimization.

2.2. Path nodes collision detection

An important factor for ensuring that nodes can search the environment without colliding with obstacles is the collision detection [Reference Wang, Tang, Dinesh and Tong32, Reference Liu, Qu, Xu, Du, Jia, Song and Liu33]. In this paper, all environmental obstacles are viewed as convex polyhedrons that undergo a particular expansion process, and the proposed algorithm can perceive the obstacles after the expansion. Since the search step size is relatively small compared to the polyhedral size of the obstacle, the essence of node and obstacle collision detection is to determine whether the node is located within this polyhedron. The plane ${\Omega} _{j}$ of the convex polyhedron ${\Omega}$ is arbitrarily extracted; the outward normal vector of the extracted plane ${\Omega} _{j}$ is the vector $\overrightarrow{n}$ , and the point $p$ belongs to the plane ${\Omega} _{j}$ . The subsequent definitions are provided first:

(2) \begin{equation} T\!\left(\Omega _{j},x\right)=\overrightarrow{n}\cdot \!\left(x-p\right) \end{equation}

where $x$ is the coordinate position of the current node.

If $T\gt 0$ , it is defined that the node $x$ is located in the front area of the plane ${\Omega} _{j}$ . If $T=0$ , it is defined that point $x$ is located on the plane ${\Omega} _{j}$ . If $T\lt 0$ , it is defined that point $x$ is located in the reverse area of the plane ${\Omega} _{j}$ . For a clear illustration of the above definition, see Fig. 1. The node $x$ is located in the front area of the plane ${\Omega} _{1}$ and plane ${\Omega} _{2}$ in Fig. 1(a), and the node $x$ is located in the front area of the plane ${\Omega} _{1}$ and the reverse area of plane ${\Omega} _{2}$ in Fig. 1(b).

Figure 1.

Discrimination of obstacle planes.

If for any plane ${\Omega} _{i}$ of the convex polyhedron ${\Omega}$ , the following equation exists:

(3) \begin{equation} T\!\left(\Omega _{i},x\right)\lt 0 \end{equation}

Therefore, it means that the node $x$ is in the obstacle.

2.3. Improved A* algorithm

When no obstacles are encountered, the search direction of the enhanced A* algorithm is the vector direction of the current node pointing to the goal point. The current node’s search direction is $\overline{v}$ , the current node is $x$ , and the search step is $d$ . Then, the search is performed on the child node $x'$ :

(4) \begin{equation} x'=x+\overrightarrow{v}\cdot d \end{equation}

If the child node $x'$ interferes with the obstacle, the search cannot continue along the direction $\overrightarrow{v}$ at the node $x$ . Therefore, the search direction of the child nodes needs to be redefined. The front area plane of the current node $x$ is first selected from the obstacle $O_{i}$ . Arbitrarily select a front area plane $\Omega _{j}$ of the obstacle $O_{i}$ , take any edge $l_{k}$ from the plane $\Omega _{j}$ , and take any point $q$ on $l_{k}$ , then the cost value of the point $q$ can be expressed as follows:

(5) \begin{equation} F\!\left(q\right)=g\!\left(x\right)+h\!\left(q\right)+\!\left\| x-q\right\| \end{equation}

where $h(q)$ is the estimated cost value from the point $q$ to the goal point, $g(x)$ is the current cost value of the node $x$ , and $\| x-q\|$ is the path length value from node $x$ to point $q$ .

Then, the point on $l_{k}$ where the smallest value of $F(q)$ exists is noted as $q_{k}$ . $q_{k}$ is denoted as a key node of the local path. Figure. 2 shows the search schematic.

Figure 2.

Schematic diagram of node search.

The node $\hat{x}$ will traverse all the edges on the frontal area plane ${\Omega} _{j}$ , and each edge will generate a key node, as shown in Fig. 3. If there are $n$ boundaries on the front area plane ${\Omega} _{j}, n$ alternative directions will be generated, denoted by $\{\tau _{ij1}^{x_{1}},\tau _{ij2}^{x_{1}}\cdots \tau _{ijn}^{x_{1}}\}$ . The path cost values of $n$ directions are put into the set $c_{ij1}^{x_{1}}$ , and the minimum cost value in the set $c_{ij1}^{x_{1}}$ is selected as the movement direction.

Figure 3.

Key nodes for the current node.

After locating the key node, the enhanced A* algorithm optimizes the local path based on the path nodes and the key node. The local path before the optimization is denoted by $\{K_{s},P_{1},P_{2},P_{3}\cdots P_{n-1},P_{n},K_{g}\}, K_{s},K_{g}$ denote the start point and goal point of the local path, respectively, and $P_{1},P_{2},P_{3}\cdots P_{n-1},P_{n}$ denote the nodes of the path. The local path optimization process is as follows: Starting from the starting point $K_{s}$ , connect $K_{s}$ to $P_{1}$ . If $K_{s}$ and $P_{1}$ do not interfere with the obstacle, connect $K_{s}$ and $P_{2}$ until $K_{s}$ and $P_{m} (k=3,4\cdots,m)$ interfere with the obstacle. Connect $K_{s}$ to $P_{m-1}$ and clear all path nodes between the starting point $K_{s}$ and node $P_{m-1}$ and update the path. Repeat this operation from the node $P_{m}$ until the key node $K_{s}$ is searched. Figure 4 compares the situation before and following path optimization. Local path optimization can effectively reduce the path’s length and number of turns.

Figure 4.

Comparison of paths before and after optimization.

3.1. Rod collision detection of the 6-DOF robotic arm

The robotic arm bars can be viewed as cylindrical features; therefore, it is necessary to determine if each cylindrical feature of the robotic arm bars collides with each environmental obstacle. The position of the robotic arm rod in space can be determined based on the robotic arm’s current pose. The current pose of the robot arm is $X=(x,y,z,\alpha,\beta,\gamma )$ , and the joint angle $Q_{X}=(q_{1},q_{2},q_{3},q_{4},q_{5},q_{6})$ can be obtained from the inverse kinematic model of the robot arm. As shown in Fig. 5(a), for the $ith$ rod of the robot arm, the rod axis line segment is $l_{i}$ , the rod radius is $r_{i}$ , and the shortest distance from the line segment $l_{i}$ to the obstacle $O_{j}$ is denoted by $D(l_{i},O_{j})$ . Then, the distance $d_{ij}$ between the $ith$ rod of the robot arm and the $jth$ obstacle $O_{j}$ is expressed as follows:

(6) \begin{equation} d_{ij}=\!\left\{\begin{array}{l@{\quad}l} D\!\left(l_{i},O_{j}\right)-r_{i} & D\!\left(l_{i},O_{j}\right)\gt r_{i}\\[5pt] \;\;\;\;\;\;\;0 & D\!\left(l_{i},O_{j}\right)\leq r \end{array}\right. \end{equation}

Figure 5.

Distance between the robotic arm joint rod and the obstacle.

In this study, the distance between the robotic arm and the obstacle is calculated as follows: first, the linkage is simplified into spatial line segments, and then, the obstacle is inflated, as depicted in Fig. 5(b). Determine if there is a point of intersection between each linkage segment and the obstacle plane region. If an intersection point exists, the robotic arm will collide with the obstruction. Obtain the distance between the line segments of each link and the line segments comprising the obstacle plane if there is no intersection point. An illustration is provided below. Suppose that the endpoints of the $ith$ link of the robot arm are $q_{1}^{i}=(x_{1}^{i},y_{1}^{i},z_{1}^{i})$ and $q_{2}^{i}=(x_{2}^{i},y_{2}^{i},z_{2}^{i})$ . Then, $ith$ link can be regarded as a line segment $\overline{q_{1}^{i}q_{2}^{i}}$ . Assume that the plane region is enclosed by points $\overrightarrow{p}_{\!\!1}\overrightarrow{p}_{\!\!2}\cdots \overrightarrow{p}_{\!\!n}\overrightarrow{p}_{\!\!1}$ connected in counterclockwise order. Suppose the normal vector of the plane is vector $(A,B,C)$ , the equation of the plane is given below:

(7) \begin{equation} Ax+By+Cz+D=0 \end{equation}

Substitution of the coordinates $q_{1}^{i}=(x_{1}^{i},y_{1}^{i},z_{1}^{i})$ and $q_{2}^{i}=(x_{2}^{i},y_{2}^{i},z_{2}^{i})$ of the endpoints of the segment into the above equation yields:

(8) \begin{equation} d_{1}^{i}=Ax_{1}^{i}+By_{1}^{i}+Cz_{1}^{i}+D \end{equation}

(9) \begin{equation} d_{2}^{i}=Ax_{2}^{i}+By_{2}^{i}+Cz_{2}^{i}+D \end{equation}

If $d_{1}^{i}$ and $d_{2}^{i}$ have the same sign, then there is no intersection of line $\overline{q_{1}^{i}q_{2}^{i}}$ with the plane, and there is no collision between that line and the obstacle. If $d_{1}^{i}$ and $d_{2}^{i}$ have opposite signs, then there is an intersection of line $\overline{q_{1}^{i}q_{2}^{i}}$ with the plane, and it is easy to solve for the location of the intersection. Since the plane region $\overrightarrow{p}_{\!\!1}\overrightarrow{p}_{\!\!2}\cdots \overrightarrow{p}_{n}\overrightarrow{p}_{\!\!1}$ mentioned in this study is only a small part of the whole plane, when there is an intersection of the line segment with the whole plane, there may be a situation where the line segment $\overline{q_{1}^{i}q_{2}^{i}}$ does not intersect with the plane region $\overrightarrow{p}_{\!\!1}\overrightarrow{p}_{\!\!2}\cdots \overrightarrow{p}_{n}\overrightarrow{p}_{\!\!1}$ . Therefore, it is necessary to determine whether the intersection point is in the plane area. To better illustrate the judgment of whether the intersection point is in the plane region $\overrightarrow{p}_{\!\!1}\overrightarrow{p}_{\!\!2}\cdots \overrightarrow{p}_{n}\overrightarrow{p}_{\!\!1}$ , Fig. 6 is depicted below.

Figure 6.

Schematic diagram for judging the intersection point.

Figure 7.

The shortest distance between a line segment and a planar region.

Assume that the intersection point is $o$ . The points $p_{1}p_{2}\cdots p_{n}$ are forming the planar region form the vectors $\overrightarrow{p_{1}p_{2}},\; \overrightarrow{p_{2}p_{3}},\; \overrightarrow{p_{3}p_{4}}$ and $\overrightarrow{p_{4}p_{5}}$ in order, and the vertices form the vectors $\overrightarrow{p_{1}o},\; \overrightarrow{p_{2}o},\; \overrightarrow{p_{3}o},\; \overrightarrow{p_{4}o}$ and $\overrightarrow{p_{4}o}$ with the point $o$ , respectively, and satisfy the following equation:

(10) \begin{equation} \overrightarrow{p_{i}p_{i+1}}\times \overrightarrow{p_{i}o}=m_{i}\quad,\,i=1\sim n \end{equation}

According to the above equation, the vector $\overrightarrow{p_{i}p_{i+1}}$ , formed between the vertices of the plane region and the vector $\overrightarrow{p_{i}o}$ (formed by the vertices and the intersection point) is multiplied.

If the sign of the results of the calculation is the same, then the intersection point is in the plane region, and line segment $\overline{q_{1}^{i}q_{2}^{i}}$ , must collide with the obstacle region. On the contrary, the intersection point is outside the plane region, then the line segment $\overline{q_{1}^{i}q_{2}^{i}}$ will not collide with the obstacle region. When no collision occurs, solve for the distance between line segment $\overline{q_{1}^{i}q_{2}^{i}}$ and line segment $\overline{p_{1}p_{2}}, \overline{p_{2}p_{3}}, \overline{p_{3}p_{4}}, \overline{p_{4}p_{5}}, \overline{p_{1}p_{3}}, \overline{p_{1}p_{4}}, \overline{p_{2}p_{4}}$ and $\overline{p_{2}p_{5}}$ , respectively. The line segments of each link of the robot arm and the obstacle plane are evaluated sequentially to determine if an intersection point exists. If there is no intersection point, calculate the distance between the arm and the obstacle and take the smallest value as the shortest distance.

The detailed calculation of the shortest distance between the rod of the robotic arm and the obstacle is provided below. The shortest distance between the line segment $\overline{q_{1}^{i}q_{2}^{i}}$ and each planar region of the obstacle is solved separately. An example of the shortest distance between the line segment $\overline{q_{1}^{i}q_{2}^{i}}$ and the plane region $\overrightarrow{p}_{\!\!1}\overrightarrow{p}_{\!\!2}\overrightarrow{p}_{\!\!3}\overrightarrow{p}_{\!\!4}\overrightarrow{p}_{\!\!5}\overrightarrow{p}_{\!\!1}$ is shown in Fig. 7. In essence, the goal is to find the minimum value of the shortest distance between the line segment $\overline{q_{1}^{i}q_{2}^{i}}$ and each of line segments $\overline{p_{1}p_{2}}, \overline{p_{2}p_{3}}, \overline{p_{3}p_{4}}, \overline{p_{4}p_{5}}, \overline{p_{5}p_{1}}, \overline{p_{1}p_{3}}, \overline{p_{1}p_{4}}, \overline{p_{2}p_{4}}$ and $\overline{p_{2}p_{5}}$ on the plane region $\overrightarrow{p}_{\!\!1}\overrightarrow{p}_{\!\!2}\overrightarrow{p}_{\!\!3}\overrightarrow{p}_{\!\!4}\overrightarrow{p}_{\!\!5}\overrightarrow{p}_{\!\!1}$ as the shortest distance between the line segment and the plane region. It is noteworthy that the shortest distance between line segment $\overline{q_{1}^{i}q_{2}^{i}}$ and line segments $\overline{p_{1}p_{3}}, \overline{p_{1}p_{4}}, \overline{p_{2}p_{4}}$ and $\overline{p_{2}p_{5}}$ is necessary for this calculation. In general, the obstacle is relatively small compared to the rod of the robotic arm. Consequently, the shortest distance between the line segment of the rod of the robotic arm and the plane region of the obstacle often falls on the line segment that encloses the flat plane region, such as the shortest distance $| pq|$ in Fig. 7(a). However, under certain conditions, the shortest distance between the line segment of the rod and the plane region of the obstacle can lie within the plane region of the obstacle, such as the shortest distance $| pq|$ in Fig. 7(b). Therefore, solving for the shortest distance between rod segment $\overline{q_{1}^{i}q_{2}^{i}}$ and line segments $\overline{p_{1}p_{3}}, \overline{p_{1}p_{4}}$ and $\overline{p_{2}p_{4}}$ can help reduce the calculation errors in a few cases.

Here is an example of solving for the shortest distance between line segment $\overline{q_{1}^{i}q_{2}^{i}}$ and line segment $\overline{p_{1}p_{2}}$ . The coordinates of points $q_{1}^{i}, q_{2}^{i}, p_{1}$ and $p_{2}$ are $(x_{{q_{1}}},y_{{q_{1}}},z_{{q_{1}}}), (x_{{q_{2}}},y_{{q_{2}}},z_{{q_{2}}}), (x_{{p_{1}}},y_{{p_{1}}},z_{{p_{1}}})$ and $(x_{{p_{2}}},y_{{p_{2}}},z_{{p_{2}}})$ , respectively. Assuming $q$ is a point on the line $q_{1}^{i}q_{2}^{i}$ ; then, the coordinates of point $q$ can be described as follows:

(11) \begin{equation} \!\left\{\begin{array}{l} x_{q}=x_{{q_{1}}}+s\!\left(x_{{q_{2}}}-x_{{q_{1}}}\right)\\[5pt] y_{q}=y_{{q_{1}}}+s\!\left(y_{{q_{2}}}-y_{{q_{1}}}\right)\\[5pt] z_{q}=z_{{q_{1}}}+s\!\left(z_{{q_{2}}}-z_{{q_{1}}}\right) \end{array}\right. \end{equation}

When there exists $0\leq s\leq 1, q$ is a point on line segment $\overline{q_{1}^{i}q_{2}^{i}}$ . Conversely, $q$ is a point on the extension of the line segment $\overline{q_{1}^{i}q_{2}^{i}}$ .

Similarly, letting $p$ be a point on the line $p_{1}p_{2}$ , the coordinates of the point $p$ can be described as follows:

(12) \begin{equation} \!\left\{\begin{array}{l} x_{p}=x_{{p_{1}}}+t\!\left(x_{{p_{2}}}-x_{{p_{1}}}\right)\\[5pt] y_{p}=y_{{p_{1}}}+t\!\left(y_{{p_{2}}}-y_{{p_{1}}}\right)\\[5pt] z_{p}=z_{{p_{1}}}+t\!\left(z_{{p_{2}}}-z_{{p_{1}}}\right) \end{array}\right. \end{equation}

When there exists $0\leq t\leq 1, p$ is a point on line segment $\overline{p_{1}p_{2}}$ . Conversely, $p$ is a point on the extension of line segment $\overline{p_{1}p_{2}}$ .

Thus, the distance between points $p$ and $q$ can be expressed as:

(13) \begin{equation} \!\left| pq\right| =\sqrt{\!\left(x_{q}-x_{p}\right)^{2}+\!\left(y_{q}-y_{p}\right)^{2}+\!\left(z_{q}-z_{p}\right)^{2}} \end{equation}

Solving for the shortest distance between line segment $\overline{q_{1}^{i}q_{2}^{i}}$ and line segment $\overline{p_{1}p_{2}}$ is equivalent to solving for the shortest distance between the points $p$ and $q$ . The function can be set up as follows:

(14) \begin{equation} f\!\left(s,t\right)=\!\left| pq\right| ^{2}=\!\left(x_{q}-x_{p}\right)^{2}+\!\left(y_{q}-y_{p}\right)^{2}+\!\left(z_{q}-z_{p}\right)^{2} \end{equation}

Calculate the partial derivative of the function $f(s,t)$ and compute the following equation:

(15) \begin{equation} \!\left\{\begin{array}{l} \dfrac{\partial f\!\left(s,t\right)}{\partial s}=0\\[10pt] \dfrac{\partial f\!\left(s,t\right)}{\partial t}=0 \end{array}\right. \end{equation}

If the solutions $s$ and $t$ of the above equation satisfy the following equation:

(16) \begin{equation} \!\left\{\begin{array}{l} 0\leq s\leq 1\\[5pt] 0\leq t\leq 1 \end{array}\right. \end{equation}

It can be determined that $p$ is on line segment $\overline{p_{1}p_{2}}$ and $q$ is on line segment $q$ . The distance $| pq|$ can be obtained by solving the equations mentioned above.

If Eq. (16) is not satisfied, then it is calculated as follows: If $s\gt 1$ , then the value of $s$ is 1, and the value of point $p$ is $p_{2}$ . If $s\lt 0$ , then the value of $s$ is 0, and the value of point $p$ is $p_{1}$ . The problem mentioned above can be transformed into the shortest distance from point $p$ to line segment $\overline{q_{1}^{i}q_{2}^{i}}$ . Consequently, Eq. (4) is transformed into the following function:

(17) \begin{equation} f\!\left(t\right)=\!\left| pq\right| ^{2}=\!\left(x_{q}-x_{p}\right)^{2}+\!\left(y_{q}-y_{p}\right)^{2}+\!\left(z_{q}-z_{p}\right)^{2} \end{equation}

Solve for the derivative of the function $f(t)$ and compute the following equation:

(18) \begin{equation} \frac{df\!\left(t\right)}{dt}=0 \end{equation}

If $t\gt 1$ , then the value of $t$ is 1, and the value of point $q$ is $q_{1}^{i}$ . If $t\lt 0$ , then the value of $t$ is 0 and the value of point $q$ is $q_{2}^{i}$ . The distance between points $p$ and $q$ can be calculated by Eq. (13), which is also the shortest distance between line segments $\overline{p_{1}p_{2}}$ and $\overline{q_{1}^{i}q_{2}^{i}}$ .

Finally, the method outlined above is applied to calculate the shortest distance between the line segment $\overline{q_{1}^{i}q_{2}^{i}}$ and each line segment of the plane region $\overrightarrow{p}_{\!1}\overrightarrow{p}_{\!2}\overrightarrow{p}_{\!3}\overrightarrow{p}_{\!4}\overrightarrow{p}_{\!5}\overrightarrow{p}_{\!1}$ . The smallest of these distances is then considered the shortest distance between the line segment $\overline{q_{1}^{i}q_{2}^{i}}$ and the plane region $\overrightarrow{p}_{\!1}\overrightarrow{p}_{\!2}\overrightarrow{p}_{\!3}\overrightarrow{p}_{\!4}\overrightarrow{p}_{\!5}\overrightarrow{p}_{\!1}$ . Following the same procedure, the shortest distance between the line segment $\overline{q_{1}^{i}q_{2}^{i}}$ and each plane of the obstacle is calculated, and the smallest value among these distances represents the shortest distance between the line segment $\overline{q_{1}^{i}q_{2}^{i}}$ and the obstacle.

Figure 8.

Overall obstacle avoidance strategy of the robotic arm.

Figure 9.

Path planning in Environment 1. (a) Traditional A*, (b) improved A*

Figure 10.

Path planning in Environment 2. (a) Traditional A*, (b) improved A*

Figure 11.

Path planning in Environment 3. (a) Traditional A*, (b) improved A*

Figure 12.

Path planning in Environment 4. (a) Traditional A*, (b) improved A*

Therefore, if the robot arm pose is $X$ , the distance between the robot arm and the obstacle $O_{j}$ can be described as follows:

(19) \begin{equation} d_{j}^{X}=\min\!\left(d_{ij}|i\in \!\left(1,2,\ldots,n\right)\right) \end{equation}

If the robotic arm rod does not collide with the obstacle, then the following equation exists.

(20) \begin{equation} d_{j}^{X}\geq 0 \end{equation}

3.2. Principle of robotic arm posture adjustment based on artificial potential field method

The artificial potential field method with local optimization properties is employed to adjust the robot arm’s posture and find the local optimal posture at the current position. As posture adjustment is a locally optimal solution procedure, only the repulsive potential energy of the robotic arm rods is taken into account. It is expressed as:

(21) \begin{equation} U_{j}^{X}=\!\left\{\begin{array}{l@{\quad}l} \dfrac{1}{2}k_{r}\!\left(\dfrac{1}{d_{j}^{X}}-\dfrac{1}{d_{0}}\right)^{2}, & d_{j}^{X}\leq d_{0}\\[10pt] 0, & d_{j}^{X}\gt d_{0} \end{array}\right. \end{equation}

where $d_{j}^{X}$ is the distance between the robot arm bar and the obstacle $O_{j}, d_{0}$ is the repulsive range of the obstacle $O_{j}$ , and $k_{r}$ is the gain factor.

(22) \begin{equation} U^{X}=\sum _{j=1}^{n}U_{j}^{X} \end{equation}

Adjusting the posture of the robotic arm yields the minimum value of $U^{X}$ to find the optimal solution. The robotic arm’s repulsive force is calculated as follows:

(23) \begin{equation} f^{X}=\frac{\partial U^{X}}{\partial X}=\sum _{j=1}^{n}\frac{\partial U_{j}^{X}}{\partial X} \end{equation}

where

(24) \begin{equation} \frac{\partial U_{j}^{X}}{\partial X}=\!\left\{\begin{array}{l@{\quad}l} k_{r}\!\left(\dfrac{1}{d_{0}}-\dfrac{1}{d_{j}^{X}}\right)\dfrac{1}{\!\left(d_{j}^{X}\right)^{2}}\dfrac{\partial d_{j}^{X}}{\partial X}, &d_{j}^{X}\leq d_{0}\\[9pt] \;\;\;\;\;\;\;\;\;\;\;\;0, &d_{j}^{X}\gt d_{0} \end{array}\right. \end{equation}

where the nearest point of the robot arm to the obstacle $O_{j}$ is $x_{j}$ and the nearest point on the obstacle is $x_{j}^{o}$ .

The following equation can be obtained:

(25) \begin{equation} d_{j}^{X}=\sqrt{\!\left(x_{j}-x_{j}^{o}\right)^{T}\cdot \!\left(x_{j}-x_{j}^{o}\right)} \end{equation}

Thus, the following equation can be derived:

(26) \begin{equation} \begin{array}{l} \dfrac{\partial d_{j}^{X}}{\partial X}=\dfrac{\!\left(x_{j}-x_{j}^{o}\right)}{\sqrt{\!\left(x_{j}-x_{j}^{o}\right)^{T}\cdot \!\left(x_{j}-x_{j}^{o}\right)}}\dfrac{\partial x_{j}}{\partial X}\\[22pt] \;\;\;\;\;\;\;\;\;\;=\dfrac{\!\left(x_{j}-x_{j}^{o}\right)}{\!\left\| \!\left(x_{j}-x_{j}^{o}\right)\right\| }\dfrac{J_{j}^{*}\partial q}{\partial X}=\dfrac{\!\left(x_{j}-x_{j}^{o}\right)}{\!\left\| \!\left(x_{j}-x_{j}^{o}\right)\right\| }J_{j}^{*}\!\left(J_{X}\right)^{-1} \end{array} \end{equation}

where $J_{j}^{*}$ denotes the Jacobi matrix of the point $x_{j}$ on the robotic arm considering only small displacements and not the pose. $J_{X}$ stands for the Jacobi matrix at the end of the robotic arm.

Therefore, it can be easily obtained that $f^{X}$ is a $6\times 1$ vector. The repulsive potential energy of the robot arm will decrease when its attitude changes in the direction of $f^{X}$ , and only the attitude component of $f^{X}$ needs to be considered to adjust the robotic arm in this paper.

3.3. Overall obstacle avoidance strategy for 6-DOF robotic arm

The robotic arm bars can be viewed as cylindrical features; therefore, it is necessary to determine if each cylindrical feature of the robotic arm bars collides with each environmental obstacle. The position of the robotic arm rod in space can be determined based on the robotic arm’s current pose. To elucidate the overall obstacle avoidance strategy of the robotic arm, the working process of the improved A* algorithm and the artificial potential field method are analyzed in depth. Figure 8 depicts the overall obstacle avoidance strategy.

First, an optimal robotic arm end planning path is determined using the enhanced A* algorithm in the environment model, followed by the determination of the initial pose and search direction. The robot arm begins to move when the joint rod is about to collide with an impediment. If the pose of the robot arm at the node of collision is $X_{0}=(x_{0},y_{0},z_{0},\alpha _{0},\beta _{0},\gamma _{0})$ , only the posture of the robot arm is changed to adjust the spatial position of each bar to avoid the obstacle.

The direction of the robotic arm posture change is described in section 3.2, and its magnitude is as follows:

(27) \begin{equation} \delta X=\!\left(0,0,0,\delta \alpha,\delta \beta,\delta \gamma \right) \end{equation}

Table I.

Parameters set for obstacle avoidance environment

Table II.

Comparison of search parameters before and after the development of the A* algorithm

Figure 13.

Improved A* in case 1. (a) Front view, (b) side view.

The position of the robot arm after the modification is as follows:

(28) \begin{equation} X_{\delta }=\!\left(x_{0},y_{0},z_{0},\alpha _{0}+\delta \alpha,\beta _{0}+\delta \beta,\gamma _{0}+\delta \gamma \right) \end{equation}

Using the robotic arm’s Jacobi matrix, the relationship between the change in arm pose and the change in joint angle is determined and expressed as:

(29) \begin{equation} \delta X=J\cdot \delta Q \end{equation}

Consequently, the following equation can be derived:

(30) \begin{equation} \delta Q=J^{-1}\cdot \delta X \end{equation}

The shortest distance $d_{j}^{X}$ between the robot arm and the obstacle can be solved using the changed joint angle $Q$ . If $d_{j}^{X}\gt 0$ , it is easy to obtain that the robotic arm can avoid the obstacle according to the changed pose. If $d_{j}^{X}\leq 0$ , it indicates that the obstacle cannot be avoided regardless of the robot arm’s orientation. Since excessive posture adjustment of the robotic arm can result in vibrations and abrupt changes, it is necessary to impose certain limits on the amount of attitude adjustment.

The constraints are as follows:

(31) \begin{equation} \!\left\{\begin{array}{l} \!\left| \delta \alpha \right| \leq \varepsilon _{\alpha }\\[5pt] \!\left| \delta \beta \right| \leq \varepsilon _{\beta }\\[5pt] \!\left| \delta \gamma \right| \leq \varepsilon _{\gamma } \end{array}\right. \end{equation}

where $\varepsilon _{\alpha }, \varepsilon _{\beta }$ and $\varepsilon _{\gamma }$ are the constraint values of posture change respectively. The values of $\varepsilon _{\alpha }, \varepsilon _{\beta }$ , and $\varepsilon _{\gamma }$ are 0.2, 0.2, and 0.1, respectively in the obstacle avoidance algorithm of this study.

Figure 14.

Traditional A* in case 1. (a) Front view, (b) side view.

Figure 15.

Improved A* in case 2. (a) Front view, (b) side view.

Figure 16.

Traditional A* in case 2. (a) Front view, (b) side view.

Figure 17.

Improved A* in case 3. (a) Front view, (b) side view.

Figure 18.

Traditional A* in case 3. (a) Front view, (b) side view.

Figure 19.

Traditional A* in case 4. (a) Front view, (b) side view.

Figure 20.

Traditional A* in case 4. (a) Front view, (b) side view.

4.1. Simulation and analysis

4.1.1. Comparison of the improved A* algorithm and the traditional A* algorithm

The traditional A* algorithm has many problems in the path planning of 3D environment, including a large number of search nodes, a lengthy search, and a decrease in computational efficiency as the number of obstacles in the environment increases. To verify the benefits of the improved A* algorithm proposed in this paper for path planning, the improved algorithm and the traditional A* algorithm are simulated and analyzed in four map environments, respectively. As illustrated in Figs. 9, 10, 11 and 12. The coordinates of the path’s start and goal points and the parameters of the obstacles in the environment are displayed in Table I. The search step size of the improved A* algorithm is set to 1. The search terminates when the spatial distance between the search node and the goal point is less than 1.

From the simulation results of the four environments listed above, it can be concluded that the improved A* algorithm proposed in this paper can effectively reduce the number of search nodes in path planning when compared to the traditional A* algorithm. Table II displays the search results of the enhanced A* algorithm and the traditional A* algorithm in four environments. It can be observed that the number of search nodes utilized by the enhanced A* algorithm in various environments has decreased considerably. The proportion of search nodes decreased significantly as the number of obstacles in the environment increased, but the final search path length remained essentially the same. Therefore, the enhanced A* algorithm can effectively improve the search efficiency in 3D environment and significantly reduce the defects of the conventional A* algorithm.

Table III.

Environmental parameter

Table IV.

Parameters set for obstacle avoidance environment

Table V.

Comparing the two algorithms in four distinct situations

Figure 21.

Path planning in Case 1. (a) Front view, (b) side view.

Figure 22.

Path planning in Case 2. (a) Front view, (b) side view.

4.1.3. Simulation of 6-DOF robot arm posture adjustment strategy

The preceding simulation is limited to the path planning of the robotic arm end by the enhanced A* algorithm and does not account for the possibility that the robotic arm’s joint rod will collide with the obstacle. To improve the 6-DOF robotic arm’s overall obstacle avoidance strategy, simulation analysis is performed in the obstacle environment. The simulation is depicted in Figs. 21 and 22, which depict the path planning results of the robot arm with different starting and endpoints. A blue line segment represents the joint rod of the 6-DOF robotic arm, and the environment’s obstacles have been enlarged to be larger than the manipulator’s radius. The parameters of the simulation are presented in Table VI. Following this premise, a total of 1000 starting points and goal points are randomly generated within a space characterized by a radius of 5 units. The coordinates of these points are determined such that the starting point corresponds to the center of the sphere mentioned in Table V, while the goal point corresponds to the center of the sphere as well. The path planning is executed following the proposed overarching obstacle avoidance strategy in two distinct scenarios, and the outcomes are presented in Table VII.

Table VI.

Two simulation conditions

Table VII.

Comparison of the three algorithms in three different cases

Figure 23.

Obstacle avoidance algorithm proposed by Jia.

Figure 24.

Robotic arm obstacle avoidance experiment.

Figure 25.

Obstacle avoidance environment. (a) Path trajectory in Jia’s method, (b) path trajectory in this study.

Figure 26.

Comparison of changes in end position. (a) The algorithm proposed in this paper, (b) the algorithm proposed by Jia.

Figure 27.

Comparison of end-pose changes. (a) The algorithm proposed in this paper, (b) the algorithm proposed by Jia.

Figure 28.

Joint angle changes in the algorithm proposed in this study.

Figure 29.

Joint angle changes in the algorithm proposed by Jia.

In the above simulation, both the traditional A* algorithm and the improved A* algorithm proposed in this paper are used for end position obstacle avoidance, achieving a 100% success rate of obstacle avoidance. However, when the posture adjustment method proposed in this paper is used for the overall obstacle avoidance of the robotic arm, there is a slight reduction in the success rate. This decrease primarily stems from the fact that the traditional and improved A* algorithms for end position obstacle avoidance do not consider collisions between the individual links of the robotic arm and the obstacles. However, when the posture adjustment method is employed for the overall obstacle avoidance of the robotic arm, the posture adjustment strategy, based on the artificial potential field, may lead to the local optimum in certain extreme cases. This can, to some extent, reduce the success rate of the search. The path results of the posture adjustment strategy for a 6-DOF robotic arm based on the improved A* algorithm and the artificial potential field method differ from the path results planned by the improved A* algorithm alone, and the path length and cost time have increased significantly. This is due to the posture adjustment strategy determining whether the obstacle collides with the joint rod and adjusting the original path result to accommodate the manipulator’s movement. The attitude adjustment strategy does not guarantee complete obstacle avoidance as the A* and improved A* algorithms do, but its obstacle avoidance success rate is still quite high. It can be seen that the algorithmically planned path has poor smoothness, and jitter may occur in the manipulator’s trajectory motion. Finally, cubic spline processing is applied to the planned path to increase the stability of the manipulator’s motion.

4.2. Experiment in a real environment

To demonstrate the efficacy of the comprehensive obstacle avoidance strategy, an experimental evaluation is conducted to compare the performance of the six-degree-of-freedom joint obstacle avoidance algorithm, which is based on the A* algorithm proposed by Jia [Reference Jia29], with the algorithm proposed in this study. The primary concept of the algorithm presented by Jia involves mapping the search for the position of the robotic arm in 3D space to the search for angles in joint space. The prescribed procedure is outlined as follows: the six joint angles of the robotic arm are designated to be documented as a six-dimensional array. Subsequently, the initial and target positions in three-dimensional space are determined through inverse kinematics, thereby facilitating the computation of the corresponding initial and target joint angles. In Eq. (1), define $G_{i}(q)=\sum\limits_{m=1}^{i}\| q_{i}[6]-q_{i-1}[6]\|$ , and $H_{i}(q)=\max\limits_{m=1,2\cdots 6}| q_{i}[m]-q_{des}[m]|$ . The flowchart of the six-degree-of-freedom obstacle avoidance algorithm proposed by Jia is shown in Fig. 23.

The algorithm proposed in this study is used to conduct experiments on the AUBO-i10 robotic arm, and the arm’s position at multiple points during its movement is recorded, as shown in Fig. 24. Figure 25(a) depicts the trajectory of the algorithm proposed in this study, while Fig. 25(b) depicts the trajectory of the algorithm proposed by Jia. Compared to the obstacle avoidance algorithm proposed by Jia, this study’s algorithm has a significantly shorter path length in real 3D space.

In this simulation, the search time for Jia’s proposed algorithm is 35.386 s, while the search time for this study’s algorithm is. 9.426 s. The primary reason for this is that the proposed method of this study is based on a three-dimensional positional space, and only three spatial positions must be altered during each search. However, Jia’s method is based on six joint spaces, and each search requires changing six joint angles, which significantly increases the search algorithm’s complexity. Under the same motion time, the search trajectories of the two algorithms are compared, and the changes in the end position, end pose, and joint angle are depicted in Figs. 26, 27, 28, and 29, respectively. According to Figs. 26 and 27, it can be determined that, compared to the algorithm proposed by Jia, the algorithm in this study has a smoother position change and posture change of the robotic arm in the three-dimensional space, resulting in less jitter at the end of the robotic arm in the actual motion space. According to Figs. 28 and 29, it can be determined that, compared to the algorithm proposed by Jia, the joint variation range of the algorithm proposed in this study is greater, and the joints in motion exhibit some jitter. This is because the algorithm in this paper searches in the 3D position space and solves the joint angles using inverse kinematics. In conclusion, the algorithm proposed in this study has superior performance in terms of search time, path length, and end position; however, the smoothing of joint angle changes should be enhanced.