Obstacle Avoidance and Multitarget Tracking of a Super Redundant Modular Manipulator Based on Bezier Curve and Particle Swarm Optimization

Abstract

A super redundant serpentine manipulator has slender structure and multiple degrees of freedom. It can travel through narrow spaces and move in complex spaces. This manipulator is composed of many modules that can form different lengths of robot arms for different application sites. The increase in degrees of freedom causes the inverse kinematics of redundant manipulator to be typical and immensely increases the calculation load in the joint space. This paper presents an integrated optimization method to solve the path planning for obstacle avoidance and discrete trajectory tracking of a super redundant manipulator. In this integrated optimization, path planning is established on a Bezier curve, and particle swarm optimization is adopted to adjust the control points of the Bezier curve with the kinematic constraints of manipulator. A feasible obstacle avoidance path is obtained along with a discrete trajectory tracking by using a follow-the-leader strategy. The relative distance between each two discrete path points is limited to reduce the fitting error of the connecting rigid links to the smooth curve. Simulation results show that this integrated optimization method can rapidly search for the appropriate trajectory to guide the manipulator in obtaining the target while achieving obstacle avoidance and meeting joint constraints. The proposed algorithm is suitable for 3D space obstacle avoidance and multitarget path tracking.

1 Introduction

Super redundant manipulators are widely applied in medical services, equipment maintenance, and post earthquake assistance [1, 2]. They can be divided into two structural types, namely, soft manipulators with unlimited degrees of freedom [3, 4] and rigid manipulators with limited degrees of freedom [5, 6]. Soft manipulators have high flexibility but their movement mechanism is complex, making them difficult to accurately control under a high load capacity [3]. Rigid manipulators are mostly composed of joint modules in series. Their entire structure is easy to maintain and is robust to external influences because of their high bearing capacity and moderate flexible bending ability; thus, they have been widely studied in practice [4, 5]. The complexity of robot motion is that the task is given in the Cartesian coordinate, and the motion is realized in the joint space. With the increase in degrees of freedom, the inverse kinematics (IK) of redundant manipulator is typical, and the calculation load in the joint space immensely increases [7, 8].

The IK and path planning of a super redundant manipulator is usually solved through optimization, where obstacle avoidance and limit constraints at the joints are handled implicitly as boundary conditions for the numerical algorithm, enabling complex task planning to be implemented with expensive computational cost [9, 10]. This paper aims to reduce the complexity of optimization calculation. Niu et al. [11] presented a projection strategy and region clipping method to reduce the computational complexity. Godage et al. [12] developed a mode shape function-based kinematic model of continuum manipulators and efficiently yielded accurate solutions. Ananthanarayanana et al. [13] proposed analytical equations to determine the first two and last three joint angles and a numerical technique to solve the remaining joint angles. Aristidou et al. [14] presented a heuristic iterative method called forward and backward reaching IK to solve the IK problem under different scenarios. David et al. [15] proposed a tip following method using a sequential quadratic programming optimization approach to navigate the robot. Maria et al. [16] presented a follow-the-leader deployment strategy for continuum robots with extensible sections along 3D paths. Tappe et al. [17] proposed a tip following method adaptable to binary actuation with optimized switching sequences. Xiong et al. [18] developed a tip following method with constraints on the bending angles. The joint position is located on the axis of the snake arm to reduce the configuration deviation of the entire robot. Palmer et al. [19] presented an approach to control the active action of coiling a continuum arm using tip following navigation. Xu et al. [20] proposed a modified fitting method based on a mode function to solve the IK of hyper-redundant manipulator. Xie et al. [21] presented a novel follow-the-leader method for rigid-backbone serpentine manipulator with high location precision and less computation time. Collins et al. [22] proposed an integrated, scalable optimization framework that uses particle swarm optimization (PSO) to produce approximate solutions to the hyper-DOF path planning and IK problems. Bulut et al. [23] proved that taking the whole arm as discrete links is an efficient way for real-time path planning. The abovementioned studies simplify the problem from the IK perspective.

In this paper, an integrated method is proposed to control a super redundant manipulator when entering an aircraft fuel tank with internal obstacles. The integrated optimization problem of path planning and trajectory tracking is established on the basis of Bezier curve and PSO [24,25,26]. The Bezier curve has an extremely strong graphic description ability by changing the position of several control points [27, 28]. PSO has good convergence with small calculation time for most optimization problems [29, 30]. The optimal results are one feasible path to avoid obstacles with a series of discrete path points and a discrete path tracking of super redundant manipulator guided by using a follow-the-leader strategy. The relative angle between two discrete path sections is limited to reduce the fitting errors of the connecting rigid links and smoothen the tracking during grasping or monitoring.

The remainder of this paper is organized as follows: Section 2 presents the manipulator model. Section 3 discusses the methodology for following the discrete Bezier path points of hyper redundant manipulators. Section 4 describes the path planning optimization problem. Section 5 extends the proposed method to different constraints and multitarget tracking. The simulation results under different situations are presented. Section 6 provides the conclusion.

2 Manipulator Parameters

The super redundant manipulator is mainly composed of a moving base, link model, joint, and end effector, and its overall structure is shown in Figure 1. The target working space of the robotic manipulator is a 120 × 80 × 30 cm³ rectangular fuel tank. The bottom surface has a circular entrance with a diameter of 10 cm. The base of the manipulator is mounted on a vertical guide rail that can realize the feed movement of the robot manipulator in the vertical direction. Each joint has two degrees of freedom, namely, bending angle $\theta_{i}$ between the two adjacent links and rotational angle $\varphi_{i}$ around each link axis. The base provides the feed displacement for the robot manipulator to move up and down. The robot manipulator has 2n+1 degrees of freedom (n is the number of modular joints), and the other main parameters are shown in Table 1.

Figure 1

Redundant modular manipulator structure

Table 1 Main parameters of the modular manipulator

The number of robot manipulator, link length, maximum bending angle of every joint, and the distance and speed of the base movement can be designed. The maximum distance of the base movement can send the entire robotic manipulator into the fuel tank and is defined as the manipulator’s length.

3 Discrete Trajectory Tracking and Control

3.1 Bezier Curve Fitting

A Bezier curve has an extremely strong graphic description ability that can smoothly connect position points and has convexity. Changing the positions of control points can ensure that the formed path does not collide with obstacles, as shown in Figure 2.

Figure 2

Fourth-order Bezier curve

The expression of m-order Bessel curve can be written as

$$B(t) = \sum\limits_{i = 0}^{m} {P(i)C_{i,m} (t), \, t \in [0,{ 1]}} ,$$

(1)

where $B(t)$is the position of discrete points at time t (the point on the Bezier curve moves from the start point to the end point while t changes from 0 to 1), $P(i)$ is the position of control points, and $C_{i,m}$ is the coefficient of fitting curve

$$C_{i,m} (t) = \frac{m!}{(m - i)!i!}t^{i} (1 - t)^{m - i} , \, i{ = 0,1,} \cdots ,m,$$

(2)

where m is the order of Bessel curve. When m = 1, the Bezier curve is a straight line; when m = 2, the Bezier curve is a parabola; and when m ≥ 3, the curve is a high-order Bezier curve. The greater the order of m is, the more the flexible the Bezier curve is, but the more the computing resources will be. The fourth-order Bezier curve is selected to avoid obstacles.

3.2 Follow-the-Leader Strategy

All joints should be far from the obstacle avoidance trajectory to move the robotic manipulator along the optimized path because the numerical optimal path generated by this method is a series of spatially discrete points, as shown in Eq. (1). However, we cannot find the analytical solution derived by the geometric method because the trajectory is discretized into Bezier path points. A given joint position p_i+1 consistently coincides with the discrete path point in front of it when it is an end effector. The positions of subsequent joints can be deduced from the link length constraint and are supposed to be located on the line connected by two adjacent path points, as shown in Figure 3.

Figure 3

Joint position of adjacent robot manipulators

The previous joint position p_i is located on one circle with p_i+1 as the center and link length as the radius. The intersection of this circle and line $m_{i - 1} m_{i}$ is point p_i, where m_i−1 and m_i are the two adjacent path points. The distance of p_i from p_i+1 satisfies the length constraints $le = \overrightarrow {{\left| {\varvec{p}_{{\varvec{i}}} \varvec{p}_{{{\varvec{i + }} 1}} } \right|}}$. Let $\overrightarrow {{\varvec{w}}} = \overrightarrow {{{\varvec{m}}_{{{\varvec{i - }} 1}} {\varvec{m}}_{{\varvec{i}}} }}$, then $\lambda \overrightarrow {{\varvec{w}}} = \overrightarrow {{{\varvec{m}}_{{{\varvec{i - }} 1}} {\varvec{p}}_{{\varvec{i}}} }}$, $(0 < \lambda < 1 )$. The vector theorem states that

$$\lambda \overrightarrow {\varvec{w}} = \overrightarrow {{{\varvec{m}}_{{{\varvec{i - }} 1}} {\varvec{p}}_{{{\varvec{i + }} 1}} }} - \overrightarrow {{{\varvec{p}}_{{\varvec{i}}} {\varvec{p}}_{{{\varvec{i + }} 1}} }} ,$$

(3)

then

$$\lambda { = }\frac{{\overrightarrow {{{\varvec{m}}_{{{\varvec{i - }} 1}} {\varvec{p}}_{{{\varvec{i + }} 1}} }} - \overrightarrow {{{\varvec{p}}_{{\varvec{i}}} {\varvec{p}}_{{{\varvec{i + }} 1}} }} }}{{\overrightarrow {{\varvec{w}}} }}.$$

(4)

Thus, the position of $p_{i}$ is

$$\overrightarrow {{{\varvec{p}}_{{\varvec{i}}} }} = \overrightarrow {{{\varvec{m}}_{{{\varvec{i - }} 1}} }} + \lambda \overrightarrow {{\varvec{w}}} .$$

(5)

The following joint position p_i−1 is determined from p_i in the same manner. The motion of manipulator is guided by the tip following strategy, and joint position update is derived using the geometric method. However, unavoidable errors occur when fitting the continuous curve using the rigid link. The distance between two discrete path points should be as short as possible to reduce the fitting error. As shown in Figure 3, the smaller the distance between m_i−1 and m_i is, the closer the joint distance from the fitting curve. However, the large number of path points decrease the computation efficiency. Therefore, the interval of discrete points should be accordingly selected.

3.3 Control Variables

Assuming that the obstacle avoidance trajectory has been obtained, the trajectory is usually expressed in the inertial coordinate system OXYZ, and the space path is discretized. We can obtain the position of every joint by using the follow-the-leader strategy in Section 3.2. Thus, the relative position of two links is determined, as shown in Figure 4. The control joint angles are deduced by the relative position changes in the two adjacent joints at the inertial space. This condition is because the control angles of joint i change the position of joint i+1, thereby avoiding the IK calculation of the entire manipulator in the joint space. The control parameters of joint angles $\theta_{i}$ and $\varphi_{i}$ and base displacement can be calculated as follows:

Figure 4

Control angles of a single link

$$\theta_{i} = \arccos \left( {\frac{{\overrightarrow {{{\varvec{p}}_{{{\varvec{i}} - 1}} {\varvec{p}}_{{\varvec{i}}} }} \cdot \overrightarrow {{{\varvec{p}}_{{\varvec{i}}} {\varvec{p}}_{{{\mathbf{i + }} 1}} }} }}{{\left| {\overrightarrow {{{\varvec{p}}_{{{\varvec{i}} - 1}} {\varvec{p}}_{{\varvec{i}}} }} } \right|\left| {\overrightarrow {{{\varvec{p}}_{{\varvec{i}}} {\varvec{p}}_{{{\varvec{i + }} 1}} }} } \right|}}} \right),$$

(6)

$$\varphi_{i} { = }\arctan 2(y_{i + 1} - y_{i}^{{}} ,x_{i + 1} - x_{i}^{{}} ),$$

(7)

$$\left\{ \begin{aligned}& \dot{s}{ = }\frac{ 1}{\tau } (Z_{{}}^{t} - Z_{{}}^{t - \tau } ) ,\hfill \\ & s{ = }\int_{0}^{t} {\dot{s}} {\text{d}}t, \hfill \\ \end{aligned} \right.$$

(8)

where p_i+1(x_i+1, y_i+1, z_i+1), p_i(x_i, y_i, z_i), and p_i−1(x_i−1, y_i−1, z_i−1) are the position coordinates of joints i+1, i, and i−1 on the spatial trajectory, respectively, $Z_{{}}^{t}$ is the base position at time t, $Z_{{}}^{t - \tau }$ is the base position at previous moment $t - \tau$, $\tau$ is the sample period, $\dot{s}$ represents a certain speed of the base movement, and s is the base displacement. Base motion is restricted along the Z axis.

The rotation angle has no limit because it is easily designed to move 180° left and right. The bending angle of adjacent links is the important design parameter for hyper redundant manipulator and depends on its flexibility. The feed distance and speed of base movement determine the manipulator position. The possible workspace is extremely small, and the height is extremely short to allow the manipulator for immensely extending inside. These physical constraints of the manipulator must be considered in path planning.

4 Obstacle Avoidance Path Planning

4.1 Obstacle Checking

In the real environment, obstacles are usually irregular in shape and difficult to describe with mathematical formulas. Therefore, a regular envelope is used to approximate the outline of obstacles. Although this method expands the volume of obstacles, it simplifies the obstacle description to improve the planning efficiency and avoid the collision of the manipulator with obstacles during the movement. The envelope radius of the obstacles includes the radius of the robot link. Let the coordinates of p_i and p_i+1 be (x_i, y_i, z_i) and (x_i+1, y_i+1, z_i+1), respectively. The linear parameter equation of robot link i can be expressed as

$$\left\{ \begin{aligned} x = (1 - k)x_{i} + kx_{i + 1} , \hfill \\ y = (1 - k)y_{i} + ky_{i + 1} , \hfill \\ z = (1 - k)z_{i} + kz_{i + 1} , \hfill \\ \end{aligned} \right.$$

(9)

where $0 < k < 1$, (x, y, z) is the arbitrary point on link for the obstacle avoidance, and the distance of arbitrary point on link from the obstacle center should satisfy

$$\left( {x - a} \right)^{2} + \left( {y - b} \right)^{2} + \left( {z - c} \right)^{2} > (r + \frac{d}{2})^{2} ,$$

(10)

where (a, b, c) are the coordinates of the spherical center of the obstacle, r is the radius of the spherical obstacle, and d is the diameter of robot link. Solving Eqs. (9) and (10) simultaneously yields the quadratic equation for parameter k:

$$\begin{aligned}& \left[ {(1 - k)x_{i} + kx_{i + 1} - a} \right]^{2} + \left[ {(1 - k)y_{i} + ky_{i + 1} - b} \right]^{2} + \hfill \\& \left[ {(1 - k)z_{i} + kz_{i + 1} - c} \right]^{2} = (r{ + }\frac{d}{2})^{2} . \hfill \\ \end{aligned}$$

(11)

Figure 5 shows the three typical relative positions of p_ip_i+1 with obstacle G to represent the collision situations in path planning. The manipulator does not collide with the obstacle when k has no solution (first scenario in Figure 5). The manipulator does not collide with the obstacle when k>1 or k<0 (second scenario in Figure 5). The manipulator collides with an obstacle when 0≤k≤1 (third scenario in Figure 5).

Figure 5

Relative positions of links and obstacle

4.2 Integrated Optimization Problem

This section provides the integrated optimization problem of obstacle avoidance path planning with kinematic constraints in path tracking control. The optimization aims to minimize the maximum joint bending angles of the manipulator at each moment. The optimization problem should satisfy the following constraints: the joint angle does not exceed the maximum bending angle, the robotic manipulator does not collide with any obstacles inside, and the manipulator is working in an effective workspace. The mathematical expression of the integrated optimization problem is written as follows:

$$\begin{aligned} &J = \hbox{min} (\hbox{max} \theta_{j} (P)), \hfill \\& \left\{ \begin{aligned}& \theta_{j} < \theta_{\text{max} } ,j = 1, \ldots ,\;num, \hfill \\& k > {\text{ 1 or }}k < 0, \hfill \\& x_{i} ,y_{i} ,z_{i} \in W \in i = 1, \ldots ,M. \hfill \\ \end{aligned} \right. \hfill \\ \end{aligned}$$

(12)

Here, the optimization variable is the control point positions of Bezier curve $P_{i} ,\;i = 1, 2, 3$, where $\theta_{\text{max} }$ indicates the maximum bending angle of the robot manipulator in path tracking, num indicates the number of joints, M represents the number of discrete points on the path, W indicates the working space of the robot manipulator, and $(x_{i} ,y_{i} ,z_{i} )$ are the positions of arbitrary point on the manipulator. $k > {\text{ 1 or }}k < 0$ indicates that obstacle avoidance conditions are satisfied.

PSO, which is a bionic intelligent algorithm [22], is used to solve the above optimization problem. Each particle is a potential solution evaluated by fitness value J in Eq. (12). For a group of N particles, each particle has M dimensions. For the ith particle, its position and speed are X_i= [x_i,1, x_i,2, …, x_i,M] and V_i= [v_i,1, v_i,2, …, v_i,M], respectively. In the optimization, the particle obtains the individual optimal value P_besti and group’s optimal value G_best in accordance with the fitness value reflected in each optimization step. It judges whether to adjust in accordance with the fitness value and then gradually moves toward the optimal particle. The position and velocity of the particles are updated as follows:

$$\left\{ \begin{aligned}& v_{i,j}^{h + 1} = \omega \cdot v_{i,j}^{h} + c_{1} r_{1}^{h} (p_{i,j}^{h} - x_{i,j}^{h} ) + c_{2} r_{2}^{h} (p_{g,j}^{h} - x_{i,j}^{h} ), \hfill \\& x_{i,j}^{h + 1} = x_{i,j}^{h} + v_{i,j}^{h + 1} , \hfill \\ \end{aligned} \right.$$

(13)

where subscripts i and j represent the particle number and particle dimension, respectively, $p_{i,j}^{h}$ represents the optimal solution of the jth dimension of particle i in the hth optimization step, $p_{g,j}^{h}$ represents the optimal solution of the jth dimension of all particles in the hth optimization step, c₁ and c₂ are the learning factors, which are taken as 2, $r_{1}^{h}$ and $r_{ 2}^{h}$ are random numbers between 0 and 1, and $\omega$ is the weight coefficient. The larger the value is, the stronger the global search ability will be. The smaller the value is, the stronger the local search ability will be, where $\omega \;{ = 0} . 8$ in this study.

4.3 Obstacle Avoidance Path Generation

The steps of obstacle avoidance path generation are given as follows.

Step 1: Initialize the particle positions and velocities of the Bezier curve. Update the tracking position of the manipulator and calculate the control angles and base displacement in the initial configuration.

Step 2: Determine whether the initial curve path meets the optimization constraints. Go to Step 7 when they are met, otherwise go to Step 3.

Step 3: Update the positions and velocities of the particles by using Eq. (13) and obtain the path points by using Eq. (1).

Step 4: Compare the fitness values ​​of the particles before and after the update by using Eq. (12). The individual optimal solution of the particles is refreshed when the fitness values ​​of the particles after the update are better than the fitness values of the particles before the update. Otherwise, no refresh is required.

Step 5: Compare the updated particle fitness value with the group’s optimal particle fitness value. Refresh the particle group’s optimal solution when the former is better than the latter. Otherwise, no refresh is required.

Step 6: Update the tracking position of the manipulator and calculate the control angles and base displacement for every optimal result. Determine whether the Bezier curve corresponding to the group’s optimal solution meets the optimization constraints in Eq. (12). Specifically, max $\theta_{i,\text{max} } < \theta_{\text{max} }$, the manipulator does not collide with the obstacle, and the positions of every link does not exceed the workspace. Go to Step 7 when these constraints are satisfied. Otherwise, return to Step 3.

Step 7: Store the optimal solution of the group (that is, the positions of the control points satisfying obstacle avoidance and joint constraints) and generate a series of discrete Bezier path points.

5 Simulation Results and Discussion

In this simulation, the working space of fuel tank W is 120 × 80 × 30 cm³, and the robotic manipulator enters the fuel tank with a radius of 5 cm. Obstacle radius r = 1 cm, the minimum safety distance between the robotic manipulator, and the obstacle is 1 cm. Six groups of obstacles are distributed in the form of an array in the fuel tank, and the positions of obstacle center of every column are shown in Figure 6.

Figure 6

Working space and obstacle distribution

Obstacle 1 = [30, −30, 10; 30, −20, 10; 30, −10,10; 30, 0, 10; 30, 10, 10; 30, 20, 10; 30, 30, 10];

Obstacle 2 = [−30, −30, 10; −30, −20, 10; −30, −10, 10; −30, 0, 10; −30, 10, 10; −30, 20, 10; −30, 30, 10];

Obstacle 3 = [0, −30, 10; 0, −20, 10; 0, −10, 10; 0, 0, 10; 0, 10, 10; 0, 20, 10; 0, 30, 10];

Obstacle 4 = [50, −30, 20; 50, −20, 20; 50, −10, 20; 50, 0, 20; 50, 10, 20; 50, 20, 20; 50, 30, 20];

Obstacle 5 = [−50, −30, 20; −50, −20, 20; −50, −10, 20; −50, 0, 20; −50, 10, 20; −50, 20, 20; −50, 30, 20];

Obstacle 6 = [0, −30, 20; 0, −20, 20; 0, −10, 20; 0, 0, 20; 0, 10, 20; 0, 20, 20; 0, 30, 20].

As shown in Figure 6, the black balls are the obstacle avoidance envelopes, and the red balls inside are the real obstacles. The typical manipulation of this manipulator is going into the tank and is reversed at 180° to pick up the foreign matter at the bottom of the tank or with a camera on the end effector to monitor the cracks on the tank wall.

5.1 Arbitrary Target Arrival in Space

Given robot manipulator length le = 9 cm, number of sections num = 12, maximum relative angle $\theta_{\text{max} } { = 30}^{\text{o}}$, robot manipulator diameter d = 4 cm, and the target position is (50, 30, 0). The simulation results are as follows: the trajectories and bending angles before and after optimization are shown in Figure 7, and a possible trajectory can be generated before optimization. However, the bending angles exceed the limit of 30° in some cases. The trajectory is divided into 60 portions, and the manipulator moves each portion at every sample time. The final optimal trajectory is shown in Figure 8 under different perspectives. As shown in Figure 8, the red line is the manipulator link, the green line is the obtained track, and they basically coincide. The discrete rigid links can fit the continuous track with small errors. These errors depend on many factors, such as link length, bending angle, and length of each portion. The resulting control angles and base movement are shown in Figures 9 and 10.

Figure 7

Trajectory and bending angles

Figure 8

Manipulator tracking result

Figure 9

Bending angles and base position change

Figure 10

Rotational angles and base displacement

All physical constraints are satisfied. The bending angle are within the limits of $\theta_{\text{max} } { = 30}^{\text{o}}$. The base displacement change depends on the distance between two adjacent path points in every sample time.

5.2 Multitarget Path Tracking

Multipoint tracking is conducted to illustrate the generality of the proposed algorithm. The manipulator needs to move to the next position for performing subsequent tasks after completing the task at the previous position. Given that the manipulator passes through the two target points, one curve is determined by two points, namely, start and end points. Thus, multiple sections of obstacle avoidance paths for multipoint tasks should be smoothly connected to guide the multipoint tracking.

As shown in Figure 11, two fourth-order Bezier curves are found for two-target tracking. End point P_1,4 of the former curve B₁ is the start point of the latter curve B₂. The two curves are smoothly connected at P_1,4, that is, the two curves are continuous and differentiable at P_1,4. The derivation of Eq. (1) can be expressed as

Figure 11

Smooth connection of the two fourth-order Bezier curves

$$\dot{B}(t) = \sum\limits_{i = 0}^{m} {P(i)\dot{C}_{i,m} (t), \, t \in [0,{ 1]}} .$$

(14)

On the basis of continuous derivable condition, we have

$$\dot{B}_{1} (1) = \dot{B}_{2} (0).$$

(15)

Expanding and simplifying Eq. (15) yields

$$4\left( {P_{1,4} - P_{1,3} } \right) = 4\left( {P_{2,1} - P_{2,0} } \right),$$

(16)

then

$$P_{1,4} { + }P_{2,0} { = }P_{1,3} { + }P_{2,1} ,$$

(17)

Considering that P_1,4 = P_2,0, Eq. (17) can be written as follows:

$$P_{1,4} { = }\frac{ 1}{ 2}\left( {P_{1,3} { + }P_{2,1} } \right).$$

(18)

As shown in Eq. (18), control point P_1,3 of curve B1, connection point P_1,4 of the two curves, and second control point P_2,1 of curve B₂ should be on the same straight line. The connection point of the curve should be the midpoint of the two control points to ensure the smooth connection of two fourth-order Bezier curves.

Considering the smooth connection condition of the fourth-order Bezier curve, only five control points need to be optimized for two-target path tracking. On the basis of the above theory, the simulation is given, the starting point is set to (0, 0, 0), the first target point is (20, 15, 15), and the second target point is (50, 40, 0). The simulations are shown in Figures 12, 13, 14, 15. The two pink asterisks represent the two targets. The end effector is passing through every path point to ensure accurate target tracking. The optimal trajectory is suppressed with small bending angles, the two sections of the trajectory are smoothly connected by target points, and all physical constraints are satisfied. For a long track, the modules of manipulators should be increased to lengthen them for obtaining the target points.

Figure 12

Trajectory and bending angles

Figure 13

Robot manipulator tracking result

Figure 14

Bending angles and base position change

Figure 15

Rotational angles and base position

5.3 Relaxing Design Conditions

In this section, we will relax the design conditions to obtain large curvature motion. For the super redundant manipulator, the bending angles are the critical design parameters because they determine the curvature of the designed trajectory and make the robot motion to be flexible or obtain steep motion in strictly limited environment. In this simulation, the starting point is given as (0, 0, 0), the position of the first target point is (0, 0, 15), and the second target point is (−40, 30, 0). The manipulator needs to cross between the two obstacles with a large curvature because they are located at (0, 0, 10) and (0, 0, 20). This manipulator cannot reach this point with the former bending angle limits. Here, the maximum bending angle is enlarged to 60°. The simulation results are shown in Figures 16, 17, 18.

Figure 16

Robot manipulator tracking result

Figure 17

Rotational angles and base position

Figure 18

Bending angles and base position change

The simulation results show that the proposed method can be easily extended to the manipulator with different physical constraints and different target points. The links of the manipulator fit well with the trajectory to ensure the smooth transition motion and tracking accuracy. The bending angles are limited to satisfy the physical constraints.

6 Conclusions

This paper presents an integrated optimization method to solve path planning and trajectory tracking simultaneously. Metaheuristic optimization is used to choose the optimal solution within the physical constraints in the following trajectory of a given hyper-redundant manipulator. The innovations of this paper are as follows: (1) The motion of manipulator is guided by using a tip following strategy, and the joint position update is derived by using a geometric method. (2) The control joint angles are calculated in the inertial space, thereby avoiding the complex IK calculation. (3) The proposed method is suitable for multitarget tracking and effective for obstacle avoidance in 3D spaces.

The limitations of the proposed numerical optimization problem are as follows: (1) The feasible solution of particle filter numerical optimization problem is related to the selection of initial values. Thus, giving reasonable initial solutions are necessary. (2) The obstacle avoidance algorithm is based on the obstacles with spherical profile. The obstacle checking algorithm should be changed accordingly if the obstacle is a hole or other shapes. Future studies should include the adaptive change of parameters in the PSO algorithm to accelerate the convergence speed under strict constraints.