Motion planning and control of redundant manipulators for dynamical obstacle avoidance

: This paper presents a framework for the motion planning and control of redundant 1 manipulators with the added task of collision avoidance. The algorithms that were previously studied 2 and tested by the authors for planar cases are here extended to full mobility redundant manipulators 3 operating in a three-dimensional workspace. The control strategy consists of a combination of off-line 4 path planning algorithms with on-line motion control. The path planning algorithm is used to 5 generate trajectories able to avoid ﬁxed obstacles, detected before the robot starts to move; it is based 6 on the potential ﬁelds method combined with a smoothing interpolation that exploits Bézier curves. 7 The on-line motion control is designed to compensate for the motion of the obstacles and to avoid 8 collisions along the kinematic chain of the manipulator; it is realized by means of a velocity control 9 law based on the null space method for redundancy control. A term of the control law takes into 10 account the speed of the obstacles as well as their position. In order to test the algorithms, a set of 11 simulations are presented: the robot KUKA LBR iiwa is controlled in different cases, where ﬁxed 12 or dynamic obstacles interfere with its motion. Simulations are also used to estimate the required 13 computational effort in order to verify the transferability to a real system. 14

. Example of path planning in 3D space with two obstacles.
following the minimum potential path towards the goal. As shown in Figure 1 The end-effector trajectory can be found by numerical integration of the resulting velocity: x e = v rep + v att x e (t + dt) = x e (t) +ẋ e (t)dt Then, an interpolation with a fifth order polynomial law is used to generate a timed motion 78 law. Nevertheless, the trajectory resulting from equations (1) and (2)  obstacles. An interpolation procedure that exploits a smoother type of curve, e.g a Bézier curve, can be 85 used to solve the problem. More in detail, given n + 1 points P 0 , P 1 , . . . , P n , where n is the power of 86 the Bezeir curve, the latter is defined as: Without any loss of generality, let's consider a third order Bézier curve: Being points P 0 and P 3 coincident with the starting and goal points respectively, the fitting 89 procedure seeks for points P 1 and P 2 generating the curve B that best approximates the original one 90 with a least-square metric. A closed-form solution to the problem can be found manipulating equation

91
(4) as follows: If the curvilinear abscissa s is discretized in m samples, the trajectory x e becomes a set of m points.

93
Thus, equation (5) can be written m times for the points B j , j = 1 . . . m, assuming the form: where:  A closed-form solution for the optimal set of coefficients C 2 can be easily found manipulating 96 equation (6) and substituting the matrix N with the analogous matrix X that is built with the points 97 belonging to the trajectory x e found with the potential field algorithm: where † represents the pseudoinverse operator according to the Moore-Penrose definition. The result 99 of the smoothing procedure is shown in Figure 2, where the best-fit 3 rd order Bézier curve is plotted velocity, thus is feasible to be assigned to a real manipulator. Bézier curves of higher order have been 105 also tested, but they suffer from too sharp curvatures and high computational times, thus they are not 106 suitable for the purpose.

107
When the potential fields approach is used in path planning a particular condition must be taken into predefined. The effect of the deviatoric velocity is to bring the end-effector out of the stagnation line.

116
Obviously, for each one of the selected directions the resulting path is different. Thus, the shortest one 117 is considered for further steps. As an example, Figure 4b shows the trajectories obtained with each one

126
Starting from the results obtained by the authors in [20], the mentioned algorithms are implemented in the present work for a redundant manipulator with full mobility, i.e. the KUKA LBR iiwa 14 R820 robot. The kinematic scheme of the manipulator is shown in Figure 5. The pose of the end-effector in the Cartesian space is defined by the vector x = [x y z α β γ] T , where the last three components are the Euler angles according to the ZYZ convention. The seven rotations related to the revolute joints of the serial kinematic chain form the joint space position vector, defined as q = [θ 1 θ 2 θ 3 θ 4 θ 5 θ 6 θ 7 ] T . Thus, the kinematic chain of the manipulator has a redundant degree of freedom with respect to the task. Besides the end-effector E, a total of 13 control points (A, B, C, A 1 , A 2 , A 3 , A 4 , B 1 , B 2 , B 3 , B 4 C 1 , C 2 ) belonging to the kinematic chain characterizes the manipulator, as shown in the bottom picture of Figure 5. The forward kinematics of the manipulator can be described by: In the previous equations f represents the expression of the position forward kinematics and J is the (6 × 7) analytic Jacobian composed by the position and orientation Jacobian matrices J p and J o , each one of dimensions (3 × 7); the velocity vectorẋ is composed by the position vectorẋ p and the angular velocity ω, whose expressions are given by: Due to redundancy, the inverse position kinematics problem is not uniquely defined, but it can be 127 workedout by the following differential formulation: The terms of equation (13) are defined as follows:q 0 is the joint null space velocity, whose effect The inverse kinematic problem formulated in equation (13) suffers from numerical problems due to 132 the inversion of the Jacobian matrix when the manipulator is near to a singular configuration, i.e.

133
when the singular values of J tend to zero. In order to avoid singularity problems, the use of the well 134 known damped least-square method was applied [26]. Such method consists in the substitution of the 135 pseudoinverse of the Jacobian J † by: where λ represents a damping factor that confers a better numerical conditioning to the inversion 137 problem. The value of λ should be a compromise between accuracy (low value) and numerical 138 robustness (high value). An efficient way to determine λ is to define it as a function of the smallest 139 singular value s min of J: In this way the effect of the approximation vanishes when the smallest singular valuer is greater than 141 a threshold , where λ → λ max when s min → 0, being λ max and tunable parameters of the algorithm.  the one related to the damped least-square approximation for the inversion of the Jacobian matrix.

161
Nevertheless, the CLIK control law can be exploited to recover the error.

162
In terms of equations, the following expressions (17) and (18) must be imposed in order to assign the 163 two velocity tasks above described:

164
Jq =ẋ e (17) where J 0 represents the Jacobian matrix associated to the velocity of the point P r .

165
Thus, equation (13) can be modified in [28,29]: where N * = I − J * J,ẋ c =ẋ e + Ke is the corrected end-effector velocity, K a positive-defined gain where the translation error is given by the (3 × 1) vector e t and the orientation error is given by the (3 × 1) vector e o . The end-effector position is expressed by the (3 × 1) position vector p, whereas its orientation by the (3 × 3) rotation matrix R = [n s a], with n, s, a being the unit vectors of the end-effector frame. The first term of equation (19) guarantees the exact velocity of the end effector with minimum joints speed. The second term drives the motion of the point P r of the robot, satisfying the collision avoidance additional task. The choice ofẋ 0 is a critical point of the algorithm. The proposed strategy is to changė x 0 according to the distance from the obstacle. To avoid a discontinuity and give smoothness to the motion, two weighting coefficients, a h and a v , are introduced: Thus, a nominal repulsive velocity v rep is modulated by a v as a function of the distance d o , whereas x 0 .
x e .
-k v x obs .

E moving obstacle
In order to improve the ability of the algorithm to avoid moving obstacles, a modification of the 178 control law expressed by equation (22) where, as described in Figure 8,ẋ obs is the obstacle velocity,ẋ 0 is the repulsive velocity along the 185 direction of d 0 , k v is a gain term andẋ * 0 is the modified repulsive velocity applied to the end-effector in 186 combination with the planned velocityẋ e . As a consequence, the direction of the repulsive velocity is 187 modified, whereas its magnitude doesn't change; furthermore, for k v = 0 the effect vanishes.

189
The algorithms presented in the previous sections were tested by Matlab simulations over a wide  set of parameters is used (Table 1)   In the first example the robot is fixed while a dynamic obstacle is interfering with the end-effector.

197
The obstacle moves linearly in the horizontal plane along y direction, with a speed of 0.25 m/s.   (t = 0.8 s), which however is quickly recovered by the control. In a similar case where the task could be 215 performed without speed saturation, the absence of motion of the end-effector would be guaranteed. The third and more complex example presents two obstacles ( Figure 13). The first, fixed, is located 218 under the linear path of the robot. Since its proximity to the path is less than the radius r of the 219 end-effector's region of influence, the off-line trajectory planner generates a Bézier curve that avoids 220 the obstacle by passing over it. Once the trajectory is assigned and the motion of the manipulator starts, 221 a second obstacle, dynamic, moves linearly at the speed of 0.25 m/s along a path that intersects the 222 kinematic chain of the robot. The effect of the second obstacle is visible in Figure 14, where a sudden 223 change of the velocity profiles is visible at approximately t = 0.9 s. Despite the interference of the two 224 obstacles and the speed saturation for some joint, the control is able to execute the task.

226
The simulations presented in the previous section are intended to verify the correctness of the 227 method and to investigate the computational effort needed to implement the related algorithms in a 228 real system. As a matter of fact, the final objective of this research is to transfer the control framework to a real system, now under construction in laboratory, where the robot will be equipped with a