Dynamic obstacle avoidance and trajectory planning of five-axis redundant industrial manipulator

2.1. Forward kinematics analysis of manipulator

Five axis redundant industrial manipulator mechanism consists of six joints, of which four are rotary joints and two are moving joints with the same direction. The pose of the manipulator end is determined by these six joints. According to the D-H rule [6], the coordinate system of each link joint of the robotic arm is established, and the pose of each joint is described by the four parameters $a_{i-1}$, ${\alpha }_{i-1}$, $d_i$, and ${\theta }_i$. The four parameters are specifically defined as follows: $a_{i-1}$ is the distance from $z_{i-1}$ to $z_i$ along the $x_{i-1}$ axis; ${\alpha }_{i-1}$ is the angle of rotation from $z_{i-1}$ to $z_i$ around the $x_{i-1}$ axis; $d_i$ is the distance from $x_{i-1}$ to $x_i$ along the $z_i$ axis; ${\theta }_i$ is the angle of rotation from $x_{i-1}$ to $x_i$ around the $z_i$ axis.

Then, the relative pose of the connecting rod $i$ in the coordinate system $\left\{i-1\right\}$ can be expressed as:

The parameters of five axis redundant industrial manipulator are as Table 1.

Substituting the data in Table 1 into Eq. (1), the transformation matrix ${}_i{}^{i-1}\mathrm{}A=\left(i=\mathrm{1,2},...,n\right)$ between adjacent links of the robotic arm can be obtained, and the coordinate transformation matrix of the end of the robotic arm relative to the base can be obtained:

After replacing the two-angle sum formula $c_{25}=\mathrm{c}\mathrm{o}\mathrm{s}\left({\theta }_2+{\theta }_5\right)=c_2{c}_5-s_2{s}_5$ and $s_{25}=\mathrm{s}\mathrm{i}\mathrm{n}\left({\theta }_2+{\theta }_5\right)=c_2{s}_5+s_2{c}_5$, the calculation result is Eq. (3):

2.2. Inverse kinematics analysis of manipulator

Inverse kinematics of the manipulator is to solve the corresponding joint variables through the pose of the end of the manipulator [7]. But the inverse kinematics equation is really a nonlinear problem, in the process of solving, it needs to couple multiple angles, especially the five axis redundant manipulator studied in this paper, there are infinitely many inverse solutions, so we need to choose a group of inverse solutions according to the actual situation. The coordinate system of each joint of five axis redundant industrial manipulator is as Fig. 1.

Fig. 1Coordinate system of each joint of five axis redundant industrial manipulator

It can be seen from Fig. 1 that when ${\theta }_2$ changes, $d_3$, $d_4$, and $d_6$ form a plane triangle. The parameters of the triangle are also related to ${\theta }_2$ and ${\theta }_5$. That is to say, ${\theta }_2$, ${\theta }_5$, $d_3$, $d_4$ 4 parameters always have a certain functional relationship, which can be solved by closed solution method and geometric method [8]. This paper takes the closed solution method as an example to solve it.

Solving the matrix equation ${\left({}_1{}^0\mathrm{}A\right)}^{-1}{}_6{}^0\mathrm{}T={}_2{}^1\mathrm{}A{}_3{}^2\mathrm{}A{}_4{}^3\mathrm{}A{}_5{}^4\mathrm{}A{}_6{}^5\mathrm{}A$ can be obtained:

Solving the matrix equation ${\left({}_1{}^0\mathrm{}A{}_2{}^1\mathrm{}A\right)}^{-1}{}_6{}^0\mathrm{}T={}_3{}^2\mathrm{}A{}_4{}^3\mathrm{}A{}_5{}^4\mathrm{}A{}_6{}^5\mathrm{}A$ can be obtained:

Solving the matrix equation ${\left({}_1{}^0\mathrm{}A{}_2{}^1\mathrm{}A{}_3{}^2\mathrm{}A{}_4{}^3\mathrm{}A\right)}^{-1}{}_6{}^0\mathrm{}T={}_5{}^4\mathrm{}A{}_6{}^5\mathrm{}A$ can be obtained:

Solving the matrix equation ${\left({}_1{}^0\mathrm{}A{}_2{}^1\mathrm{}A{}_3{}^2\mathrm{}A{}_4{}^3\mathrm{}A{}_5{}^4\mathrm{}A\right)}^{-1}{}_6{}^0\mathrm{}T={}_6{}^5\mathrm{}A$ can be obtained:

Eqs. (4), (5), (6) and (7) are the analytical solutions of the joint variables ${\theta }_1$, ${\theta }_2$, ${\theta }_5$, and ${\theta }_6$ respectively. It can be seen that ${\theta }_1$ can be obtained directly from the end pose of the robotic arm, but it needs to be calculated according to the range of motion and the principle of minimum motion are selected. The values of ${\theta }_2$ and ${\theta }_5$ are related to $d_3+d_4$, and the value of ${\theta }_6$ is determined by ${\theta }_1$, ${\theta }_2$, and ${\theta }_5$. Therefore, a set of inverse solutions can be obtained by determining the values of the four joint variables ${\theta }_2$, ${\theta }_5$, $d_3$, and $d_4$.

Taking into account the actual working conditions of the robotic arm, the joint 5 is close to the working area and the environment is harsh, and $d_6$ is much smaller than $d_3+d_4$. The movement in the $x$ direction is mainly completed by the joints 3 and 4, and the ${\theta }_2$ is more complex due to the complicated calculations. The efficiency can be calculated after determining ${\theta }_5$ and $d_3+d_4$. Therefore, the following provisions can be made:

(1) Joint 5 was kept as motionless as possible, and joint 3 and joint 4 were the main motion joints.

(2) Ensure that joint 3 and joint 4 each joint changes minimum.

In the process of solving the inverse motion, the Eq. (8) of the functional equation about $d_3+d_4$ is constructed according to the Eq. (5):

Determine the value of $d_3+d_4$ to fit the range of motion of joint 5, and then solve for ${\theta }_5$ and ${\theta }_2$. $d_3$ and $d_4$ are calculated according to Eq. (9):

In Eq. (9), $d_{30}=\text{1475}$ is the $d_3$ value of the initial posture of the robotic arm, $d_{40}=\text{3825}$ is the $d_4$ value of the initial posture of the robotic arm, ${\left(d_3+d_4\right)}_0=\text{5300}$ is the $d_3+d_4$ value of the initial posture of the robotic arm, 2300 and 1750 are the maximum allowable values of $d_3$ and $d_4$ respectively the amount of change. The specific solution process of ${\theta }_2$, ${\theta }_5$, $d_3$, $d_4$ is as Fig. 2.

Fig. 2Reverse solution flow chart of manipulator

In Fig. 2, 7325 and 3275 are the maximum and minimum values of $d_3+d_4$ respectively. When using the above method to calculate, because the working point is selected in the working space, there is no situation where ${\theta }_2$ and ${\theta }_5$ have no solution. When solving Eq. (8), two solutions may be obtained. The range of motion of $d_3$ and $d_4$ is preferentially selected, that is, the value in the interval [3275, 7325], otherwise the minimum change value is selected according to the $d_3+d_4$ corresponding to the last operating point. $c_5$ is a monotonic function in the interval [0°, 90°], and the value of ${\theta }_5$ is unique. Eq. (6) is a transcendental equation. When solving, you need to replace $c_2$ and $s_2$ with $u=\mathrm{t}\mathrm{a}\mathrm{n}\frac{\theta }{2}$. $u$ may have two values. After solving ${\theta }_2$ by $c_2$ the choice is made according to the range of motion and the principle of minimum motion.

2.3. Dynamic analysis of manipulator

The dynamic modeling of the manipulator is an essential part of the dynamic model to achieve the control purpose. The accuracy of the model has a direct impact on the trajectory planning and control accuracy of the manipulator. Newton Euler method can solve kinematics parameters and joint torque respectively through extrapolation and interpolation iteration. The whole calculation process is not only fast in calculation and easy to convert program language, but also can solve system dynamics problems with high degrees of freedom such as mechanical arm. First, the forward recursion of the manipulator is solved. The velocity and acceleration of each point are calculated in Eq. (10):

where, ${\omega }_{i+1}$ represents angular velocity; ${\stackrel{\mathrm{*}}{\omega }}_{i+1}$ is the angular acceleration; ${\stackrel{\mathrm{*}}{v}}_{i+1}$ represents linear acceleration; ${{\stackrel{\mathrm{*}}{v}}_c}_{{\mathrm{}}_{i+1}}$ is the linear acceleration at the center of mass.The resultant force and moment at the center of mass of the connecting rod shall be calculated according to Eq. (11):

where, ${}_{\mathrm{}}{}^i\mathrm{}{F}_i$ represents the resultant force at the center of mass of the connecting rod; $N_i$ represents the resultant moment at the center of mass of the connecting rod. The expression of the equation of reverse recursion is shown in Eq. (12):

where, $f_i$ represents the acting force at the connecting rod joint; $N_i$ represents the moment balance equation at the connecting rod joint; $N_i$ represents the joint moment of the connecting rod; ${\tau }_i$ represents the driving torque at the joint. $Z_i$ in Eqs. (10), (11) and (12) represents the vector of axis $\left\{i\right\}$ in coordinate system $Z$; ${}_{\mathrm{}}{}^{i+1}\mathrm{}{R}_i$ represents the rotation matrix in coordinate system $\left\{i\right\}$ relative to coordinate system $\left\{i+1\right\}$; ${}_{\mathrm{}}{}^i\mathrm{}{P}_{i+1}$ and ${}_{\mathrm{}}{}^i\mathrm{}{r}_i$ represent the position vector of the origin of coordinate system $\left\{i\right\}$ in coordinate system $\left\{i+1\right\}$ and the position variable of the center of mass of the connecting rod; $I_{c_i}$ represents the inertia tensor of the member; $f_n$, $n_n$ represents the force and torque acting on the actuator at the end of the mechanical arm.

3.1. Collision detection based on AABB hierarchical bounding box tree

There are all kinds of obstacles in the manipulator workspace. In order to ensure the safe operation of the manipulator and avoid unnecessary losses, collision detection is needed. In this paper, we need collision information in a given state, so we need discrete collision detection algorithm. AABB based hierarchical bounding box tree algorithm is one of the most commonly used discrete collision detection algorithms. Because of its low construction difficulty, small storage capacity, simple intersection test, and less calculation of bounding box update when the object rotates, it has been deeply studied and widely used [9]. The flow chart of AABB based hierarchical bounding box tree collision detection algorithm is as Fig. 3.

Fig. 3AABB based hierarchical bounding box tree collision detection algorithm flow

The construction of AABB bounding box is to surround an object with one or more AABB bounding boxes according to its geometric characteristics. Axis aligned bounding box (AABB) is a six sided box like cuboid in three-dimensional space (a rectangle with four sides in two-dimensional space), and its face normals are parallel to the given coordinate axis. By integrating bounding box into tree structure, namely hierarchical bounding box technology (BVH), the time of collision detection can be reduced [10]. Among them, the original bounding box set corresponds to multiple leaf nodes in the hierarchical tree, so leaf nodes can form multiple small sets, and can be limited by a larger bounding volume. This process can be executed recursively. Finally, a hierarchical tree structure will be constructed and an independent bounding box will be presented at the root node. The AABB hierarchical tree structure based on five objects is as Fig. 4.

Fig. 4AABB hierarchical tree structure based on five objects

When using hierarchical structure, if the parent node does not intersect in the collision test process, it is unnecessary to detect the child node, so as to improve the speed of collision detection. After the bounding box and its hierarchical tree structure are constructed, collision detection can be started. The first step is to exclude objects that are obviously disjoint. This process is called preliminary detection. Then, the possible intersecting objects are further detected, which is called detailed detection. In the detailed detection stage, the collision detection method between bounding boxes [11] is used to recursively detect whether there is a collision between nodes in the hierarchical tree (called the step-by-step refinement stage), until the leaf node of the tree, and further detect whether there is a collision between objects in the leaf node (the object surrounded by the bounding box) (called the accurate intersection stage).

3.2. Obstacle avoidance planning based on harmony search algorithm

Harmony search algorithm is equivalent to the music creation optimization problem, all kinds of musical instruments are equivalent to independent variables, the tone of musical instruments is equivalent to the value range of independent variables, and the evaluation index is equivalent to the objective function [12]. The basic harmony algorithm flow is as Fig. 5.

Fig. 5Flow chart of basic harmony algorithm

The steps of basic harmony search algorithm are as follows:

(1) The objective function and constraints of the optimization problem are determined, and the parameters of the algorithm are set.

The objective function and constraint conditions of the optimization problem are as follows:

Objective function: $\mathrm{m}\mathrm{i}\mathrm{n}f\left(X\right),X=\left(x_1,x_2,...,x_n\right)$.

Constraints: $G_i\left(X\right)\le 0,i=\mathrm{1,2},...,m$.

(2) Initialize harmony memory:

For the solution of a function optimization problem, the harmony memory bank is usually expressed in the form of a matrix [13]. For a function to be optimized whose independent variable is $n$ dimensional, its harmony memory bank can be expressed as:

The initialization of the harmony memory bank is to randomly select the $HMS$ group of harmony vectors from the solution space of the objective function and calculate the respective objective function values to determine the matrix $HM$ [14].

(3) Creating new harmony:

This is the most critical part of the algorithm. Creating a new harmony is to generate a new solution vector [15]. There are three ways to generate new solution vectors: learning from harmony memory, fine-tuning after learning and randomly selecting from solution space. The generation formula is as follows:

where, $ran{d}_1$ is a random value with a size between 0 and 1, and $HMCR$ represents the probability of learning from the memory bank. If $ran{d}_1>HMCR$, the value is randomly selected from the corresponding solution space. The purpose of this is to prevent the search from falling into the local optimum or local convergence [16].

After learning and generating $x_i^{new}$ in the harmony search algorithm of the harmony memory bank, there is a probability of $PAR$ to fine-tune it. The fine-tuning formula is as follows:

where, $ran{d}_2$ is a random value with a size between 0 and 1, $b_w$ is the width of fine-tuning, and $PAR$ is the probability of fine-tuning.

(4) Update harmony memory.

The objective function value of the new solution vector generated in the previous step is compared with the worst objective function value in the memory. If the function value of the new solution vector is better than the worst function value, the vector corresponding to the worst function value is removed from the memory, and the new solution vector is put into the memory.

(5) Check whether the end condition is met. If not, return to step 3 to continue. Otherwise, end. The end condition can be in many forms, and the algorithm is usually terminated by setting the maximum number of iterations.

Due to the strong global convergence performance of harmony search algorithm, this paper uses harmony search algorithm to plan the obstacle avoidance path of the manipulator, and transforms the obstacle avoidance path planning problem into the problem of finding the minimum difference between the current value and the target value of each joint of the manipulator [17]. The main steps of using harmony search algorithm to search obstacle avoidance path are as follows:

Step 1: Use $f\left(\theta \right)=\sum {\left({\theta }_{t\mathrm{a}\mathrm{r}\mathrm{g}et}-{\theta }_{current}\right)}^2$ as the objective function and collision detection as the constraint condition to initialize the parameters of the algorithm.

Step 2: Initialize the harmony memory bank $HM$. The solution vector is randomly selected in a small neighborhood $\left[{\theta }_s,{\theta }_s+\epsilon \right]$ from the initial joint value ${\theta }_s$ of the robot arm to the target joint value ${\theta }_t$, and the corresponding function value is calculated and then filled into the memory and sound library $HM$ for initialization.

Step 3: Generate a new solution vector. There are three generation methods: randomly selected from the harmony memory bank $HM$, fine-tuned the solution vector selected from $HM$, and randomly selected within a small neighborhood $\left[{\theta }_c,{\theta }_c+\epsilon \right]$ from the current state ${\theta }_c$ of the robot arm to the target state ${\theta }_t$.

Step 4: Update the harmony memory bank $HM$. According to the constraints, if the generated new solution vector will cause the obstacle to collide with the robotic arm, it will be discarded directly. Otherwise, the function value of the new solution vector will be compared with the worst function value in the memory bank. If the former is worse than the latter, the new solution vector is used to replace the solution vector corresponding to the latter.

Step 5: Check whether the target state is searched or the maximum number of iterations is reached. If not, continue to search. Otherwise, return to step 3 to continue. Through the above steps, the dynamic obstacle avoidance and trajectory planning of five axis redundant industrial manipulator are realized.

4.1. Experimental environment and data

In order to verify the effectiveness of the five-axis redundant industrial manipulator dynamic obstacle avoidance and trajectory planning simulation research. Using a factory’s five-axis redundant industrial robotic arm as a prototype for simulation, the experiment was run on a computer with Intel(R) Core(TM) i3-2120 CPU @ 3.30 GHz, 4.00 GB memory, 32-bit Window 7 operating system, five-axis the simulation of the redundant industrial manipulator runs on Microsoft Visual C++ 6.0 software, and the structure model is established in the software as Fig. 6.

Fig. 6Five axis redundant industrial manipulator model

Due to the different paths of the dynamic obstacle avoidance and trajectory planning algorithm of the five axis redundant industrial manipulator, in order to improve the credibility of the experimental results, the experimental data in this paper is the average value of 100 times of repeated operation under the same conditions. The reference [4] algorithm, the reference [5] algorithm and the proposed algorithm are used to compare the planning success rate, the speed of convergence to the optimal value and the planning time index of different algorithms.

4.2. Comparison of success rate of obstacle avoidance planning

The success rate of obstacle avoidance planning refers to the percentage of the number of experiments to find a feasible path in 100 simulation experiments with the same planner parameters in the same planning time. In the same obstacle avoidance planning time, the comparison results of the reference [4] algorithm, the reference [5] algorithm and the proposed algorithm are as Fig. 7.

Fig. 7Comparison results of success rate of obstacle avoidance planning with different algorithms

According to Fig. 7, when the obstacle avoidance planning time reaches 30 s, the average success rate of the reference [4] algorithm is 63 %, the average success rate of the reference [4] algorithm is 48 %, and the average success rate of the proposed algorithm is 92 %. Therefore, the success rate of the proposed algorithm is high.

4.3. Comparison of path cost convergence speed

On this basis, the convergence speed of the proposed algorithm is further verified. With the change of time, the path cost of the reference [4] algorithm, the reference [5] algorithm and the proposed algorithm converges to the optimal value as Fig. 8.

Fig. 8Comparison results of path cost convergence of obstacle avoidance planning with different algorithms

As can be seen from Fig. 8, with the increase of obstacle avoidance planning time, the path cost of different algorithms gradually converges to the theoretical optimal path generation value. However, compared with the reference [4] algorithm and the reference [5] algorithm, the path cost of the proposed algorithm is closer to the theoretical optimal path generation value, and the path cost of obstacle avoidance planning converges faster.

4.4. Comparison of obstacle avoidance planning time

In order to further verify the obstacle avoidance planning time of the five axis redundant industrial manipulator, the algorithm in reference [4], the algorithm in reference [5] and the proposed algorithm are calculated according to Eq. (13) and compared. The calculation results are shown in Table 2:

where, ${\sigma }_i$ is the obstacle avoidance sensitivity coefficient of each joint, $v$ is the obstacle avoidance speed of each joint, $h_i$ is the planned route distance for obstacle avoidance of each joint, $N$ is the number of joints, $i=\mathrm{1,2},...,6$.

According to the data in Table 2, when the number of joints is 6, the average obstacle avoidance planning time of the reference [4] algorithm is 29.7 s, the average obstacle avoidance planning time of the reference [5] algorithm is 20.8 s, and the average obstacle avoidance planning time of the proposed algorithm is 18.9 s. Therefore, compared with the reference [4] algorithm and the reference [5] algorithm, the obstacle avoidance planning time of the proposed algorithm is shorter.