OBSTACLE AVOIDANCE PLANNING FOR REDUNDANT MANIPULATOR BASED ON VARIATIONAL METHOD

Original scientific paper To reduce the excessive joints movement, this paper proposed a new obstacle avoidance method for a redundant manipulator. In this method, we designed the performance evaluation functional to realize the shortest joints motion path of manipulator, and deduced the gradient vector optimizing index by variational method to make the joint movement minimum during obstacle avoiding. Meanwhile, in order to avoid the obstacle avoidance algorithm failure or joints exceeding their limits, which arose from the great difference between least-norm solution and homogeneous solution of velocity inverse kinematics of the manipulator, this paper used 2-norm continuous function to adjust homogeneous solution factor of gradient projection method dynamically. To verify the validity of the proposed obstacle avoidance method in the paper, simulation experiments were conducted on a 7-DOF redundant manipulator. The results show that, compared to the traditional gradient projection method for obstacle avoidance, the proposed algorithm in this paper has decreased the displacement of joint 1 to joint 6 by 38,3 %, 83,3 %, 3,81 %, 7,85 %, 50,1 % and 45,6 % respectively, and the total displacement of prismatic joints and revolute joints has reduced 62,2 % and 26,4 %. At the same time, the changes of joint 1 to joint 6’s velocity and acceleration between initial time and final time during obstacle avoiding has been decreased 43,2 %, 97,3 %, 2,23 %, 36,6 %, 96,7 %, 72,7 % (velocity) and 91,04 %, 98,28 %, 73,33 %, 98,40 %, 93,86 % and 91,94 % (acceleration) respectively. The test validated that the proposed obstacle avoidance method based on variational method is feasible and practicable.


Introduction
Redundant manipulators have been widely applied in highly sophisticated and Non-structured environment owing to their obvious advantages on manipulability and flexibility.However, there is great uncertainty in the joint moving path of redundant manipulators during obstacle avoiding, so an effective method is needed for solving this problem.
At present, the domestic and foreign scholars have presented many methods for the obstacle avoidance problem of manipulators [1,2,3], such as C-space method, artificial potential field method, Rapidlyexploring random trees (RRT) and probabilistic roadmap method (PRM), etc.The C-space method, proposed by Lozano [2], accomplishes obstacle avoidance through generating path in the C-free space, but it needs excessive complex calculation for redundant manipulators.Yao et al. [4] improved the real-time performance of obstacle avoidance for eggplant harvesting robot by calculating the edge points of obstacles to generate C-space path based on improved C-space method.Zhou et al. [5] obtained optimal path at the aim of path length and safety of manipulators based on the improved RRT algorithm.Liang et al. [6] used the ellipsoid to surround obstacles and calculated the pseudo-distance between the manipulator and ellipsoid surface in obstacle avoiding and obtained good effects.Dasgupta et al. [7] proposed a method taking the path length and safety as the evaluation function to improve the moving path of redundant manipulator.Liegeois [8] proposed a gradient projection method, which can effectively accomplish the real-time obstacle avoidance task.There are many other methods for obstacle avoiding in researching [9,10,11,12,13].
As we know, inverse kinematics is needed for obstacle avoidance planning of manipulator.And the redundant manipulator inverse kinematics is usually obtained by gradient projection method, which includes least-norm solution and homogeneous solution, and the homogeneous solution can be used for optimizing the movement of manipulator joints.Though the inverse solution of redundant manipulators can be rapidly solved by gradient projection method during obstacle avoiding, this method always accumulates errors in the process of iterative integral transformation for solving the joint position and velocity.In addition, the gradient projection method generally optimizes the joints movement of manipulators based on the distance between the joints or links and obstacles, and it may eventually lead to excessive joint motion of manipulators.Meanwhile, the coefficient of homogeneous solution in gradient projection method is usually set as fixed factor during obstacle avoidance path planning.So if the value of this fixed factor is too large or too small during the whole process of obstacles avoidance, the method failure may occur.And it will lead to the fact that the method cannot achieve the desired effects, eventually it would make the manipulator obstacle avoidance failure and the joints exceeding the limits.
To optimize the joint path and avoid the joints to exceed the limits, this paper used variational method to obtain the shortest path index and 2-norm function continuous factor to dynamically adjust the factor of homogeneous solution.We obtained the velocity inverse kinematics by the generalize inverse Jacobian matrix of manipulators, and took the pseudo distance obstacle avoidance index, the joints limit index and the shortest path index which was based on the variational method as the gradient vector of gradient projection method, and eventually accomplished the desired movement with the shortest joint path during obstacle avoidance planning for a redundant manipulator.At the same time, the proposed method avoided the gradient projection method failure and joints exceeding the limits.
The remainder of this paper is organized as follows.Section 2 develops the theory of gradient projection method, dynamically adjusting homogeneous solution factor and the gradient projection obstacle avoidance algorithm based on pseudo distance method.In Section 3, the shortest path index of joints based on variational method is proposed and deduced.To verify the efficiency of the proposed method, simulation experiment on a 7-DOF redundant manipulator is conducted and result analyses are provided in Section 4. Conclusions are presented in Section 5.

Obstacle avoidance of redundant manipulators based on gradient projection method 2.1 Velocity inverse kinematics of redundant manipulators
The Jacobian matrix of redundant manipulator is m×n dimensional non-square matrix and n > m, so there are infinite velocity solutions for arbitrary end-effect or velocity of redundant manipulators.Known the relationship between the end-effect or velocity and joint velocity , is the end-effector velocity and m R q ∈  is the joint velocity [14,15,16,20].Therefore, we can obtain the velocity inverse kinematics through the generalize inverse Jacobian matrix method as follows: where + J is the Moor-Penrose generalize inverse of the Jacobian matrix of manipulator; s q  is the least-norm solution, indicates the desired movement of end-effector; ϕ  is the joint space movement, indicates n-dimensional space arbitrary velocity vector; h q  is the projection of ϕ  in the zero-space; k is the gradient factor of homogeneous solution; I is n-dimensional identity matrix.
The gradient projection method constraints the joint space movement ϕ  by the gradient vector ) (q H ∇ of optimizing index ).

(q H
And if k > 0, the optimizing index ) (q H will get the maximum value, then if k < 0, ) (q H will get the minimum value.
The gradient factor k in velocity inverse kinematics is generally fixed.Considering the fixed gradient projection factor could cause a great difference between the leastnorm solution and homogeneous solution, which may lead to obstacle avoidance failure, this paper uses a 2-norm continuous function as the dynamic gradient projection factor [14,15,16], and it can be represented as: where k α is the adjustment coefficient.The continuous factor k applied in this paper is a 2-norm function combined Jacobian matrix, velocity vector matrix of endeffector in the Cartesian space and performance index function.So this function factor can be adjusted dynamically, which can ensure the continuity of the algorithm.Meanwhile, it can effectively prevent algorithm failure by compensating the difference between least-norm solution and homogeneous solution.

Obstacle avoidance optimizing index of gradient projection method 2.2.1 Joints exceeding limit index
To prevent the joint exceeding its limits, we constraint the manipulator joints in the range of max min i i i q q q < < .Considering the possibility of joint positive and negative displacement, this paper takes the mid-value of joint displacement as joint limit performance index, namely: where N is the number of manipulator joints; m i is the mid-value of the i th joint, i.e.

Pseudo distance obstacle avoidance index
The obstacle avoidance of manipulators should keep the end-effector and links untouch with any obstacles during the manipulator moving.The shape of obstacles is usually irregular, so it's really hard to build accurate obstacle model.To describe the obstacle by mathematics model, this paper uses ellipsoid to surround the obstacles [6] and calculate the pseudo distance between the surface of the ellipsoid equation and the joint or links of manipulators.Then maximize the minimum pseudo distance to realize the manipulator obstacle avoidance.
Assuming the ellipsoid equation for surrounding obstacles [6] is expressed as: where X is the space position vector of manipulator joints and links.So Eq. ( 4) stands for the distance between ellipsoid obstacle and manipulator, the manipulator will not collide the ellipsoid obstacle when Eq. ( 4) is greater than 0. And a, b, c is the length of ellipsoid half shaft; o x , o y , o z is the origin of space coordinates of ellipsoid.
Therefore, the distance between arbitrary point on the manipulator and the ellipsoid surface can be given by: So the necessary condition for obstacle avoidance is all the joints and links meet: S(X i ) > 0, i = 1, 2,..., i.e. the minimum distance S min (X i ) > 0.

The principle of variational method
Variational method is a method for solving extreme value of functional, and has been widely applied in the field of optimal control [17,18,19].The extreme value in variational method can be easily obtained, so it is generally applied to address modern control system problem, such as the minimum energy control, the smallest error control, the shortest time control, the maximum velocity control and the minimum fuel control, etc. [14,15].In this paper, we solve the excessive joint movement during the process of obstacle avoiding by using the variational method.This method takes the total displacements of all joints as objective functional, and works out the gradient vector to make the objective functional and joint path shortest in the obstacle avoidance planning.The gradient vector is the shortest path index for gradient projection method.
The extreme value problem in traditional variational method generally can be described as follows.Assuming there is a functional: where y = y(t), and t is between t 0 and t f , transversal condition is y(t 0 ) = a and y(t f ) = b.
Therefore, the variation of this functional can be represented as: where δy is the variation of y(t).So when the variation of functional δF = 0, i.e.Euler equation 0 , the extreme value can be got in the functional.

Performance functional and equality constraint condition
To restrain the joints excessive movement away from the obstacle in pseudo distance obstacle avoidance method, this paper proposed the shortest path index to ensure the joint displacement minimum in the premise of completing the desired motion task and obstacle avoidance planning.

Performance functional
The single joint displacement of redundant manipulators can be considered as the sum position change of joint within the moving time, and it can be expressed as: where q tc is the joint position of the current sample timing; q tl is the joint position of the previous sample timing.Divide the total joint displacement by the total motion time, and then the average velocity of joint in the sample time can be obtained.
Due to the total motion time of manipulator being fixed, the total joint displacement of the manipulator can be represented as the joints velocity vector functional, and so the performance functional in variational method can be obtained as: where is the joint velocity vector.

Equality constraint condition
In this paper, the minimum extreme value of performance functional Eq. ( 11) is selected as the shortest path index.Assuming the shortest path index is represented as , and then it meets: Eq. ( 12) can be regarded as the equality constraint condition of variational problem, then extreme value of functional Eq. ( 11) can be solved by using the Lagrange multiplier method in the condition of equality constraint Eq. ( 12), the steps are as follows: Construct the auxiliary function: where is undetermined vector multiplier.The extreme value problem of functional Eq. ( 11) under equality constraint Eq. ( 12) can be transformed to the extreme value problem of functional Eq. ( 13) without constraint conditions.
Define Hamilton function as: Then, the auxiliary function Eq. ( 13) can be changed as: Thus, the Euler equation of auxiliary functional Eq. ( 15) can be obtained by using variational method as follows: Assume q(t 0 ) = q 0 , and in the terminal time t f , ] , [ ) ( max min q q t q f ∈ as the boundary conditions. Therefore, combining the Euler Eq. ( 16), we can obtain: From Eq. ( 17), we can obtain the shortest path index by necessary condition of minimum value (Jacobian matrix changes with time).
Therefore, combining Eq. ( 3), Eq. ( 7) and Eq. ( 17), we can obtain the total gradient vector of gradient projection method as follows: where k w , k s and k f are the scale coefficients of three indexes.

Simulation and discussion 4.1 Simulation
To verify the validity of the proposed obstacle avoidance algorithm in the paper, we conducted simulation experiments on a 7-DOF redundant manipulator which has 2 prismatic joints and 5 revolute joints, and the mechanism diagram is shown in Fig. 1.

Figure 1
The mechanism diagram of the 7-DOF manipulator (Notes: d1, d2 are displacements of the 1 st joint and the 2 nd joint, mm.θ3, θ4, θ5, θ6 and θ7 are angular displacement of the 3 rd joint to the 7 th joint, rad; l1, l2, l3, l4, l5, l6 and l7are the link lengths of the manipulator, mm.) In simulation experiments, we planned obstacle avoidance path for the redundant manipulator by the proposed obstacle avoidance method in this paper and the traditional gradient projection method for obstacle avoidance, and compared the experimental results from the two obstacle avoidance methods.The procedure of the proposed algorithm is shown in Fig. 2. We obtained the joint velocity solutions by synthesizing all of the performance optimizing indexes in the iterative calculation of the gradient projection obstacle avoidance method, which are taken as control values to realize the desired end-effect or movement.
The simulation experiment system of the proposed obstacle avoidance method in this paper is shown in Fig. 3.The two obstacle avoidance planning methods are tested in MATLAB.
Fig. 1 shows that the 7 th joint has no effect on the position of end-effector in the Cartesian space, so set the fixed value q 7 = 0, and initial positions of manipulator are set as q 0 = [0,1; 0,

Simulation results discussion
The obstacle avoidance paths of the redundant manipulator from three different views are shown in Fig. 4 and Fig. 5.The joints displacement and the joint velocity changes obtained by the two obstacle avoidance methods are shown in Fig. 6 and Fig. 7.The final position data of every joint of the manipulator by the two methods during obstacle avoidance planning are shown in Tab. 1. Comparing Fig. 4 and Fig. 5, we can see that two obstacle avoidance methods can both make the endeffector reach the desired position with collision-free, but the path length of the proposed obstacle avoidance algorithm in the paper is significantly reduced.
According to Fig. 8, compared to the ordinarily gradient projection method, the proposed obstacle avoidance method in this paper makes the joint acceleration of manipulator vary gentler.And the acceleration change of joint 1 to joint 6 between initial time and final time during obstacle avoiding reduces 91,04 %, 98,28 %, 73,33 %, 98,40 %, 93,86 % and 91,94 % respectively.
From Fig. 6, Fig. 7 and Fig. 8, it is also observed that every joint velocity and acceleration varies more smoothly and continuously within the joint limits during obstacle avoidance planning.Therefore, the proposed algorithm in this paper decreases the joints initial velocity value and the average velocity of manipulator during obstacle avoidance planning, and avoids the joint to exceed its limits.

Conclusion
1) In this paper, a new obstacle avoidance method for redundant manipulators has been proposed based on variational method and gradient projection method.In this new method, the variational method solved the shortest path optimizing index, and this index was applied to gradient projection algorithm for obstacle avoidance to reduce the joints' motion path of manipulator during obstacle avoiding.Simulation experiment results show that it has resolved the excessive joint movement and greatly reduced the joints displacement of the redundant manipulator during obstacle avoiding.Compared with traditional gradient projection method for obstacle avoidance, the maximum reduction of joint displacements reached 83,3 %, and the total displacement of prismatic joints and revolute joints reduced 62,2 % and 26,4 % respectively for the proposed algorithm in this paper.So the new proposed method can avoid unnecessary wastage of energy and improve the efficiency of redundant manipulator.
3) The proposed obstacle avoidance algorithm in this paper, which combined the shortest path index deduced by variational method and 2-norm continuous function gradient projection factor, successfully accomplished the obstacle avoidance in the work space, and avoided algorithm failure and joint exceeding its limits.It is proved that the proposed obstacle avoidance method in this paper is effective and feasible.

Figure 2
Figure 2 The procedure of the proposed obstacle avoidance algorithm

Table 1
Final displacements of joints during obstacle avoiding obtained by the two obstacle avoidance methods