Obstacle Avoidance for Redundant Manipulators Utilizing a Backward Quadratic Search Algorithm

Abstract

Obstacle avoidance can be achieved as a secondary task by appropriate inverse kinematics (IK) resolution of redundant manipulators. Most prior literature requires the time-consuming determination of the closest point to the obstacle for every calculation step. Aiming at the relief of computational burden, this paper develops what is termed a backward quadratic search algorithm (BQSA) as another option for solving IK problems in obstacle avoidance. The BQSA detects possible collisions based on the root property of a category of quadratic functions, which are derived from ellipse-enveloped obstacles and the positions of each link's end-points. The algorithm executes a backward search for possible obstacle collisions, from the end-effector to the base, and avoids obstacles by utilizing a hybrid IK scheme, incorporating the damped least-squares method, the weighted least-norm method and the gradient projection method. Some details of the hybrid IK scheme, such as values of the damped factor, weights and the clamping velocity, are discussed, along with a comparison of computational load between previous methods and BQSA. Simulations of a planar seven-link manipulator and a PUMA 560 robot verify the effectiveness of BQSA.

1. Introduction

The redundancy of manipulators can be used to achieve secondary goals, such as joint limits avoidance [1], obstacle avoidance [2–4], minimization of joint torque [5], etc., without disturbances to the main task [6]. In order to drive redundant manipulators, an appropriate sequence of joint velocities is generated from the desired velocity of the end-effector and the predefined constraints of the inverse kinematics (IK) [7]. However, the underdetermined ability of the Jacobian matrix would result in infinite IK solutions and a probable singularity [8]. This is the primary difficulty in the application of redundant manipulators. Thus, prior literature [9] has proposed abundant methods, including damped least-squares (DLS) [10], gradient projection (GP) [11], weighted least-norm (WLN) [1] and augmented, or extended, Jacobian matrix (AJM or EJM) [7, 12] methods to cope with these difficulties.

Obviously, it is essential to drive manipulators to avoid collisions with obstacles, and this secondary goal has inspired the development of a variety of methods. Some of the prior literature achieved obstacle avoidance kinematically based on the determination of the closest point to the obstacle (called the obstacle avoidance point). A. A. Maciejewski and C. A. Klein [2] calculated the obstacle avoidance point based on a Jacobian matrix and the end-effector velocity, and utilized the GP method to achieve obstacle avoidance with lower priority. F. T. Cheng, T. H. Chen, Y. S. Wang and Y. Y. Sun [13] formulated the IK problem of redundant manipulators considering obstacle avoidance and drift-free criteria using compact quadratic programming (QP). Y. N. Zhang and J. Wang [14] improved the equality-based QP formulation in [13] to create an inequality-based one, and used a dual neural network to generate joint velocities. Recent progress in the kinematical method of obstacle avoidance can be found in [15–17]. Nevertheless, these methods require the identification of the obstacle avoidance point for every calculation step, which is time-consuming and sometimes difficult when the obstacle contour becomes complex. On the other hand, some researchers have resolved the problem at a dynamical level. O. Khatib [3] established an artificial potential field around obstacles and substituted potential forces into the dynamical equation of manipulators. The application of harmonic potential functions can be seen in [4], and some other definitions of potential functions have been provided in recent work [18,19]. However, methods based on artificial potential field achieve obstacle avoidance in a dynamical way and are not applicable when manipulators’ dynamical parameters, such as link masses, positions of mass centres, inertias, etc., are unknown. Thus, a kinematical method for obstacle avoidance involving less calculation burden is eagerly anticipated.

This is the motivation of the present paper. Since the obstacle avoidance point is located between the two end-points of each manipulators’ link, and because the trajectories of all end-points can be easily obtained during calculation, it is naturally expected that the detection and avoidance of possible collisions depend on the relationship between obstacles and the manipulators’ end-points alone. Assuming that all obstacles can be enveloped by different ellipsoids, this paper will reveal the categories of collision based on the property of a derived quadratic function. Utilizing some recent IK algorithms [20–22] dealing with unilateral constraints, a backward quadratic search algorithm (BQSA) is developed to control redundant manipulators moving along desired end-effectors’ trajectories, as well as away from obstacles, by employing a hybrid scheme of GP and WLN methods.

The rest of this paper is arranged as follows. Section 2 presents the geometrical model of the problem and a hybrid IK scheme. Collisions are divided into several categories based on the derived quadratic function, and the basic procedure and determination of parameters of the BQSA are introduced in Section 3. Section 3 also compares the computational load between a previous kinematical method and the BQSA. A seven degree-of-freedom (DOF) planar serial-link manipulator and a PUMA 560 robot are numerically simulated, respectively, to validate the effectiveness of the proposed algorithms in Section 4. Finally, Section 5 presents the conclusions of this paper.

2. Problem Formulation

2.1. Geometric modelling

The geometric model of manipulators and obstacles is depicted in Figure 1. Neglecting the shapes of all links and joints, the manipulators have n DOF with links (line segments in geometry) and joints (dots in geometry) denoted as l _i and O _i (i = 1, 2,…, n), respectively. All obstacles are assumed to be enveloped by k ellipsoids (called obstacle enveloped ellipsoids, OEEs) denoted as Γ _j (j = 1, 2,…, k). The world coordinates are denoted as O _W ,X _W ,Y _W ,Z _W . The manipulator is fastened on a base O₁ and its end-effector O_n+1 moves along the trajectory S. For every time period of calculation, the location and shape of all enveloped ellipsoids and the position of the end-effector O_n+1 are given relation to the world coordinate. Assume that O_n+1 is never in contact with obstacles, which can be assured by the programming of the end-effector's trajectory p _n+1. The IK problem here is to resolve the sequence of all joint angles θ _i and positions p _i (i = 1, 2,…, n) without causing collision with obstacles.

Here, all obstacles are assumed to be enveloped in ellipsoids that have smooth convex contours and can be determined according to nine parameters, including its centre location, axis length and axis direction. Most obstacle shapes (such as points, rectangles and triangles, etc.) can be conservatively included in the ellipsoid, and some can even be obtained from the deformation of an ellipsoid. Take a wall obstacle, for instance, and denote the three ellipsoidal axis lengths as a, b and c, respectively. If a >> b, then the ellipsoidal obstacle approximately deforms into a wall with a as the length, b as the thickness and c as the height. This state of affairs is illustrated in Figure 20. Another benefit of ellipsoidal obstacles is the quadratic property of their analytical expression thanks to which this paper's proposed BQSA can easily detect possible collisions between manipulator links and obstacles with minimal computational burden. This point will be discussed in further detail in Subsection 3.4.

Figure 1.

Geometric model of redundant manipulators and obstacles

Generally, manipulators move and OEEs are shaped in the three-dimensional Euclidean space. The collision between manipulators and OEEs can be avoided by one of the three two-dimensional link-obstacle pairs, which are the projections of the three-dimensional link-obstacle pairs to three mutually orthogonal planes that avoid the collision. This proposition is depicted in Figure 2 and further proved by Lemma 1.

Figure 2.

Collision avoidance between manipulators and OEEs: a three-dimensional and three mutually orthogonal planar situation

Lemma 1. If there is at least one planar-projected pair that does not intersect with the collision area, the collision does not happen between the three-dimensional link-obstacle pair.

Proof. See Appendix 7.1. □

The converse negative proposition of Lemma 1 indicates that, if the collision happens between the three-dimensional link-obstacle pair, all of the three planar-projected pairs are involved in the collision. However, the negative proposition of Lemma 1 is incorrect. Thus, planar obstacle avoidance can be adopted as a conservative way to assure three-dimensional obstacle avoidance, which simplifies the problem to a two-dimensional situation. The following discussions in this paper are conducted in the xoy plane and analytical equations for the i-th link and the j-th obstacle ellipse can be written as

$\begin{array}{cc}{a}_{i}y=b_{i}x+c_i,& x\in \left[x_{i\min },x_{i\max }\right]\end{array}$

(1)

and

$A_j{x}^2+B_{j}xy+C_j{y}^2+D_{j}x+E_{j}y+F_j=0$

(2)

respectively. The end-points of the i-th link are denoted as (x _i , y _i ) and (x_i+1, y_i+1), and parameters of the line segment can be obtained as

$\begin{array}{l}\begin{array}{ccc}{a}_i=x_{i+1}-x_i,& b_i=y_{i+1}-y_i,& c_i=x_{i+1}{y}_i-x_i{y}_{i+1},\end{array}\\ \begin{array}{cc}{x}_{i\min }=\min \left\{x_i,x_{i+1}\right\},& x_{i\max }=\max \left\{x_i,x_{i+1}\right\}.\end{array}\end{array}$

(3)

Parameters of the j-th obstacle ellipse, including A _j , B _j , C _j , D _j , E _j , and F _j , are assumed to be known at every time period of calculation.

2.2. Hybrid scheme of inverse kinematics

Let $p\in R^{m\times 1}$ be the trajectory of the end-effector and $\theta \in R^{n\times 1}$ be the joint angles. The forward kinematics describing the relationship between p and θ can be written as

$p=\mathrm{K}\left(\theta \right)$

(4)

with the initial configuration p (0) = K( θ (0)). The differentiation of Eq. (4) derives the forward kinematics in velocity level as

$\dot{p}=\frac{\partial \mathrm{K}\left(\theta \right)}{\partial \theta }\dot{\theta }=J\left(\theta \right)\dot{\theta }$

(5)

in which vectors $\dot{p}\in R^{m\times 1}$ denotes the end-effector velocity, $\dot{\theta }\in R^{n\times 1}$ denotes the joint velocity and the matrix $J\in R^{m\times n}$ denotes the Jacobian matrix.

The aim of the inverse kinematics is to resolve an appropriate sequence of joint velocity $\dot{\theta }$ to drive the end-effector tracking desired trajectory p and the manipulator satisfying constraints, like obstacle avoidance. Consider the trajectory tracking by the end-effector as the primary task. The inverse kinematical solution with the least squares error [8] for the primary task can be obtained by directly utilizing the pseudo-inverse of the Jacobian matrix as

$\dot{\theta }=J^{†}\dot{p}$

(6)

in which the matrix J ^† ∈R^n×m represents the Moore-Penrose inverse of the Jacobian. As the Jacobian probably becomes ill-conditioned in some manipulator configurations, a damping factor λ > 0 is added to avoid probable Jacobian singularities, which is found using the DLS [10] method as

$\dot{\theta }={\left(J^{\mathrm{T}}J+{\lambda }^2{I}_n\right)}^{-1}{J}^{\mathrm{T}}\dot{p}$

(7)

where the matrix I _n ∈R^n×n represents the n-dimensional identity matrix and the matrix J ^T J +λ² I _n is obviously full-rank and invertible due to the positive damping factor.

The self-motion that arises from the redundancy of the manipulator can be utilized to achieve secondary tasks, such as joint limits avoidance, obstacle avoidance, etc. without any disturbances to the primary task. GP is one such method [11], projecting secondary goals into the null space of the Jacobian matrix, which can be written as

$\dot{\theta }={\left(J^{\mathrm{T}}J+{\lambda }^2{I}_n\right)}^{-1}{J}^{\mathrm{T}}\dot{p}+\mu \left(I_n-J^{†}J\right)\nabla H$

(8)

in which μ > 0 is the gradient rate, $\nabla H$ represents the gradient function of the secondary task and I _n - J ^† J projects the secondary goals into the Jacobian kernel. Nevertheless, the constraints are only satisfied in the null space of the Jacobian, so the secondary tasks cannot always be guaranteed during the manipulation. This drawback inspires another more effective method of achieving secondary goals, called WLN [1]:

$\dot{\theta }=W^{-1}{J}^{\mathrm{T}}{\left(J{W}^{-1}{J}^{\mathrm{T}}+{\lambda }^2{I}_m\right)}^{-1}\dot{p}$

(9)

in which W ∈R^n×n represents the diagonal-weighted matrix. Usually, when secondary goals are satisfied, W = I _n and Eq. (9) are no different to Eq. (7); while some weights in W will increase so as to dampen the approach to the constraint limits, some joints enter the transient interval near the limits. However, the WLN method cannot drive joints away from the limits and will eventually lose some degree of freedom. This defect can be removed by adding the clamping velocity Φ = [φ _i ]_n×1 ∈R^n×1 in the Jacobian kernel, causing the joints that enter the transient interval to be pushed away from the limits. Thus, a hybrid scheme of IK involving DLS, WLN and GP methods, along with clamping velocity, can be written as

$\dot{\theta }=W{J}^{\mathrm{T}}{\left(JW{J}^{\mathrm{T}}+{\lambda }^2{I}_m\right)}^{-1}\dot{p}-W{J}^{\mathrm{T}}{\left(JW{J}^{\mathrm{T}}+{\lambda }^2{I}_m\right)}^{-1}J{W}^{1/2}\Phi +W^{1/2}\Phi$

(10)

Define the weighted Jacobian matrix and joint velocity as

$\begin{array}{cc}{J}_w=J{W}^{1/2},& {\dot{\theta }}_w=W^{-1/2}\dot{\theta }\end{array}$

(11)

and the singular value decomposition (SVD) of J _w as

$J_w=U_w{D}_w{V}_w^{\mathrm{T}}$

(12)

where the matrix U _w ∈R^m×m and V _w ∈R^n×n can be expressed as a row of column vectors

$\begin{array}{cc}{U}_w=\left[\begin{array}{cccc}{u}_{w1}& u_{w2}& \cdots & u_{wm}\end{array}\right],& V_w\end{array}=\left[\begin{array}{cccc}{v}_{w1}& v_{w2}& \cdots & v_{wn}\end{array}\right]$

(13)

and D _w ∈R^m×n is a diagonal matrix

$D_w=\mathrm{diag}\left({\sigma }_{w1},\dots ,{\sigma }_{wr},0,\dots ,0\right)$

(14)

with r ≤ m as the rank of J _w and σ_w1 ≥ σ_w2 ≥… ≥ σ _wr > 0 as the singular values of J _w . Substituting Eqs. (11) – (14) into Eq. (10), the weighted joint velocity can be obtained as

$\begin{array}{c}{\dot{\theta }}_w=V_w{D}_w^{\mathrm{T}}{\left(D_w{D}_w^{\mathrm{T}}+{\lambda }^2{I}_m\right)}^{-1}{U}_w^{\mathrm{T}}\dot{p}-V_w{D}_w^{\mathrm{T}}{\left(D_w{D}_w^{\mathrm{T}}+{\lambda }^2{I}_m\right)}^{-1}{D}_w{V}_w^{\mathrm{T}}\Phi +\Phi \\ ={\displaystyle \sum _{s=1}^r\frac{{\sigma }_{ws}{u}_{ws}^{\mathrm{T}}\dot{p}}{{\sigma }_{ws}^2+{\lambda }^2}{v}_{ws}+}\left({{\rm I}}_n-{\displaystyle \sum _{s=1}^r\frac{{\sigma }_{ws}^2{v}_{ws}{v}_{ws}^{\mathrm{T}}}{{\sigma }_{ws}^2+{\lambda }^2}}\right)\Phi ={\dot{\theta }}_w^C+{\dot{\theta }}_w^N\end{array}$

(15)

in which the velocities ${\dot{\theta }}_w^C$ and ${\dot{\theta }}_w^N$ represent the weighted joint velocities in the column space and null space of the weighted Jacobian, respectively.

The DLS [10] can effectively increase the Jacobian singular robustness and is utilized to avoid a potential Jacobian singularity. Nevertheless, as the non-zero damping factor λ will decrease the accuracy of the main task, which can be indicated as the reprojection error [9], defined as follows

$e=J_w\left(J_w^{†}\dot{p}-{\dot{\theta }}_w\right)={\displaystyle \sum _{s=1}^r\frac{{\lambda }^2}{{\sigma }_{ws}^2+{\lambda }^2}}\dot{p},$

(16)

it is better to take the zero value for the damping factor when the Jacobian matrix does not approach the singularity; when the Jacobian singularity occurs, the value for the damping factor should be positive but as small as possible. The detailed method to determine the value of λ is presented in Lemma 2.

Lemma 2. If the value of the damping factor λ is taken as follows

$\lambda =\{\begin{array}{cc}0,& \text{if }\underset{s=1,2,\dots ,r}{\max }\left\{\Vert u_{ws}^{\mathrm{T}}\dot{p}\Vert /{\sigma }_{ws}\right\}\le \epsilon /r\\ \frac{r}{4\epsilon }\underset{s=1,2,\dots ,r}{\max }\left\{\Vert u_{ws}^{\mathrm{T}}\dot{p}\Vert \right\},& \text{if }\underset{s=1,2,\dots ,r}{\max }\left\{\Vert u_{ws}^{\mathrm{T}}\dot{p}\Vert /{\sigma }_{ws}\right\}>\epsilon /r\end{array},$

(17)

the trajectory tracking task can be bounded by ε which is an arbitrary positive number, e.g.

$\Vert {\dot{\theta }}_w^C\Vert =\Vert {\displaystyle \sum _{s=1}^r\frac{{\sigma }_{ws}{u}_{ws}^{\mathrm{T}}\dot{p}}{{\sigma }_{ws}^2+{\lambda }^2}{v}_{ws}}\Vert \le \epsilon .$

(18)

Proof. See Appendix 7.2. □

In this paper, obstacle avoidance is considered as the secondary task and the WLN method is utilized to enforce this constraint. However, the traditional WLN method [1] will result in the loss of DOF, so the clamping velocity to the kernel of the weighted Jacobian by the GP method [22] is added to the WLN scheme. These two measures are presented in subsection 3.3 in further detail.

It should be noted that there is not always a feasible solution for the IK problem with constraints like joint limits and obstacle avoidance. One of the most significant factors influencing the feasibility of the IK problem is the manipulator's redundancy. Considering an n-DOF manipulator, the primary trajectory tracking mission will cost m DOF, and the rest of n-m DOF represents the manipulator redundancy which can be utilized to achieve secondary goals. In order to guarantee the feasibility of the IK problem, the consumption of DOF by the constraints must be less than n-m, otherwise no optimal solution can be obtained.

3. Backward Quadratic Search Algorithm

3.1. Quadratic category of collisions

According to the problem formulation, obstacle avoidance is modelled as the geometrical relationship between a line segment and an ellipse. Generally, assume $a_i\ne 0$ and substitute Eq. (1) into Eq. (2) to obtain the following quadratic function

$\begin{array}{cc}{f}_{ij}\left(x\right)=a_{ij}{x}^2+b_{ij}x+c_{ij},& x\in \left[x_{i\min },x_{i\max }\right]\end{array}$

(19)

the coefficients of which can be obtained as

$\begin{array}{l}{a}_{ij}=A_j{a}_i^2+B_j{a}_i{b}_i+C_j{b}_i^2, b_{ij}=D_j{a}_i^2+B_j{a}_i{c}_i+E_j{a}_i{b}_i+2C_j{b}_i{c}_i, \\ c_{ij}=C_j{c}_i^2+E_j{a}_i{c}_i+F_j{a}_i^2.\end{array}$

(20)

Furthermore, define Δ _ij and {x_1ij, x_2ij} as

${\mathrm{\Delta }}_{ij}=b_{ij}^2-4a_{ji}{c}_{ij}$

(21)

and

$\begin{array}{cc}{f}_{ij}\left(x_{1ij}\right)=0,& f_{ij}\left(x_{2ij}\right)=0\end{array}$

(22)

respectively. The collision between the i-th link and the j-th obstacle can be determined via the root property of the quadratic function (19), which is dependent on Eq. (21) and Eq. (22), and can be divided into five categories, depicted in Figure 3. The five quadratic categories of collisions can be expressed as follows:

• No collision 1: the line does not intersect the ellipse, and Δ _ij ≤ 0;

• No collision 2: only the extension line intersects the ellipse, and Δ _ij > 0 ∩ f _ij (x_imin) > 0 ∩ f _ij (x_imax) > 0 ∩ df _ij (x_imin)/dx⋅df _ij (x_imax)/dx > 0;

• Collision 1: one crossover point on the line segment, the other being located at the extension line, and Δ _ij > 0 ∩ f _ij (x_imin)⋅f _ij (x_imax) ≤ 0;

• Collision 2: two crossover points on the line segment, and Δ _ij > 0 ∩{f _ij (x_imin) > 0, f _ij (x_imax) > 0}∩ df _ij (x_imin)/dx⋅df _ij (x_imax)/dx < 0;

• Collision 3: the line segment is located inside the ellipse, and f _ij (x_imin) < 0 ∩ f _ij (x_imax) < 0.

Thus, the collision between the link and the obstacle can be determined by calculating the root discriminant Δ _ij of the quadratic equation f _ij = 0 and the function of its derivative values of both end-points of the link. Since the position for each end-point can be conveniently obtained at every calculation step, this method is much easier than that in [2], which requires a search for the obstacle avoidance point and the calculation of the minimum distance.

Figure 3.

Quadratic categories of collisions

3.2. Basic procedure of BQSA

Assume that the initial configuration of the manipulator avoids all obstacles and the trajectory of the end-effector has been appropriately programmed outside all obstacles. The backward quadratic search algorithm (BQSA) combines the proposed hybrid scheme of IK with the root properties of derived quadratic functions f _ij (i = 1, 2,…, n, j = 1, 2,…, k) to generate joint velocity and guarantee obstacle avoidance. To assure the feasibility of the solution, the inequality n - m ≤ k should be satisfied.

Since the end-effector always moves outside obstacles, the collision detection can be conducted from link l _n backward to l_n-1, l_n-2, through to l₁ by determining to which quadratic category the collision belongs. Once a probable collision between the link l _i and the j-th obstacle Γ _j is discovered, updates of joint angles pause until a collision-free manipulator configuration is obtained, which is achieved by setting the weighted matrix W = 0 and actuating the clamping velocity elements φ₁~φ_i+1 in the Jacobian kernel so that collision-free configurations of links l_i+1~l _n will not be destroyed. The clamping velocity element φ_i+1 increases or decreases by θ_i+1 in trials until the conditions of no collision are satisfied; thereafter the position of the end-point O _i and the joint angle θ _i are determined geometrically and the joint angles θ₁~θ_i-1 are computed by IK. After the collision detection for all links and obstacles is complete, the joint angles and manipulator configurations are updated.

Thus, the word ‘ backward ’ refers to the search direction from the end-effector to the first joint, while the word ‘ quadratic ’ refers to the collision detection's dependence on the root properties of quadratic functions. The basic procedure of BQSA is depicted in Figure 4 and can be summarized as follows:

Step 1 (Initialization): Set time t = 0, time step of computation δt, the initial configuration of the manipulator (i.e., the end-effector position) p (0) and generalized coordinates (joint angles) θ (0).

for t = δt, 2δt,…

Step 2 (Joint Velocity Resolution): Set the weighted matrix W = I and resolve the joint velocity $\dot{\theta }\left(t\right)$ directly utilizing the DLS method as follows:

$\dot{\theta }\left(t\right)={\displaystyle \sum _{s=1}^r\frac{{\sigma }_s{u}_s^{\mathrm{T}}\dot{p}\left(t\right)}{{\sigma }_s^2+{\lambda }^2}{v}_s}$

(23)

in which vectors u _s , v _s and parameters σ _s (s = 1, 2,…, r) are obtained from the SVD of the Jacobian matrix J as Eqs. (12) - (14) when the weighted matrix W = I , and the value of the damped factor λ can be computed as Lemma 2.

Step 3 (Backward Quadratic Detection of Collisions): Calculate quadratic conditions and detect collisions.

Compute θ(t)=θ(t−δt)+θ̇(t)δt and p (t) = K(θ(t)); for i = n, n-1, …, 1, j = 1, 2, …, m if No collision 1 or No collision 2 , no collisions occur between the i-th link and the j-th obstacle, flag_ij(t) = 0; else the i-th link and the j-th obstacle collide, flag_ij(t) = 1. end end

Step 4 (Joints Clamping): Actuate the clamping velocity Φ (t) by setting the weighted matrix W = 0 when a collision is detected.

If flag _ij (t) = 0 for all links, jump to Step 5; else while flag _ij (t) = 1 set the weighted matrix W = 0, change θ _i+1 (t) by φ _i+1 in the while loop, detect the collision between l_i and γ _j with the quadratic criteria in Step 3. If collision still occurs, flag _ij (t) = 1; else flag _ij (t) = 0, update the value of θ _i+1 (t) = θ _i+1,tt (t), fix θ_i+1 (t)-θ_n (t) and p _i+1 (t) - p _n+1 (t), geometrically compute p _i (t), resolve θ₁ (t)-θ _i-1 (t) by IK and p ₁ (t)- p _i-1 (t) by the forward kinematics (FK), geometrically compute θ _i (t), jump to Step 5. end end end Step 5 (Update and Output): Update and output θ (t) and p (t). end □

Details of Step 4 (Joints Clamping), including the determination of θ_i+1,tt (t), geometrical computation of p _i (t) and θ _i (t) etc., are discussed further in subsection 3.3 to elucidate the effect of the clamping velocity.

Figure 4.

Basic procedure of the backward quadratic search algorithm

3.3. Discussions on joints clamping

Following the detection of collisions, the BQSA enters into the joints clamping step before updating and outputting the kinematical resolutions. The algorithm makes this choice automatically by setting the value of the weighted matrix, which can be expressed as

$W=\{\begin{array}{cc}0,& {\text{flag}}_{ij}\left(t\right)=0\begin{array}{cc} \text{for}& \text{all }i,j;\end{array}\\ I,& {\text{flag}}_{ij}\left(t\right)=1.\end{array}$

(24)

The weighted matrix W performs the function of a switch: when all the links are clear of all obstacles, W will initiate the main task of trajectory tracking and suppress the secondary task of collision avoidance; when some collision occurs, W will halt the main task and initiate the secondary task.

Elements of the clamping velocity φ₁(t)~φ_i+1(t) are actuated in the Jacobian null space when link l _i becomes involved in a collision with the obstacle, and the remaining elements of the clamping velocity φ_i+2(t)~φ _n (t) are set to zero. Assuming tt = 1, 2, 3,… according to the while loop inner counter in Step 4, the value of θ_i+1(t) is adjusted as

${\theta }_{i+1,tt}\left(t\right)={\theta }_{i+1,tt-2}\left(t\right)+{\left(-1\right)}^{tt}\mathrm{\Delta }\theta$

(25)

where θ_i+1,tt(t) denotes the value of the i+1-th joint angle at the tt-th step in the while loop and the t-th step in the for loop, and θ_i+1,-1(t) = θ_i+1,0(t) incorporates the value of θ_i+1(t) in Step 3. The adjustment Δθ is assigned a minutely positive value as the increment of |θ_i+1,tt(t)| for every computation step in the while loop. Once link l _i is moved away from the obstacle as detected in Step 4, the clamping adjustment of θ_i+1(t) ends and the renewed value of θ_i+1(t) and the clamping velocity element φ_i+1(t) can be obtained as θ_i+1 ^u (t) = θ_i+1,tt(t) and φ_i+1(t) = (θ_i+1 ^u (t) - θ_i+1,1(t))/δt, respectively.

Subsequently, the position of the end-point O _i can be geometrically computed as

$p_i\left(t\right)=p_{i+1}\left(t\right)-l_i{\left[\begin{array}{cc}\cos \left({\displaystyle \sum _{s=1}^i{\theta }_s\left(t\right)}+{\theta }_{i+1,tt}\left(t\right)\right)& \sin \left({\displaystyle \sum _{s=1}^i{\theta }_s\left(t\right)}+{\theta }_{i+1,tt}\left(t\right)\right)\end{array}\right]}^{\mathrm{T}}$

(26)

in a planar case. Note that BQSA will not affect p _i+1(t)~ p _n (t), so the value of p _i+1(t) in Eq. (26) is already set. Moreover, values of θ₁(t)~ θ _i (t) in Eq. (26) are obtained in Step 2 by Eq. (23) and can be updated by the clamping velocity as

$\begin{array}{cc}{\phi }_{i-1}\left(t\right)={\displaystyle \sum _{s=1}^r\frac{{\sigma }_{is}{u}_{is}^{\mathrm{T}}{\dot{p}}_i\left(t\right)}{{\sigma }_{is}^2+{\lambda }^2}{v}_{is}},& {\theta }_{i-1}^u\left(t\right)={\theta }_{i-1}\left(t\right)+{\phi }_{i-1}\left(t\right)\delta t\end{array}$

(27)

in which vectors φ _i-1(t) = [φ₁(t), φ₂(t),…, φ_i-1(t)]^T, θ _i-1(t) = [θ₁(t), θ₂(t),…, θ_i-1(t)]^T, u _is and v _is and parameters σ _is (s = 1, 2,…, r) are supplied by the SVD of the Jacobian matrix J _i , satisfying ${\dot{p}}_i\left(t\right)=J_i{\dot{\theta }}_{i-1}\left(t\right)$ , and the vector θ _i-1 ^u (t) represents the updated value of θ _i-1(t).

Finally, the updated value of θ _i ^u (t) can be geometrically computed as

${\theta }_i^u\left(t\right)=\arccos \frac{〈p_{i+1}\left(t\right)-p_i\left(t\right),p_i\left(t\right)-p_{i-1}\left(t\right)〉}{{\Vert p_{i+1}\left(t\right)-p_i\left(t\right)\Vert }_2{\Vert p_i\left(t\right)-p_{i-1}\left(t\right)\Vert }_2}-{\displaystyle \sum _{s=1}^{i-1}{\theta }_s\left(t\right)}$

(28)

in which the symbol < a , b > denotes the dot product between vectors a and b , and the symbol || a ||₂ denotes the two-norm of the vector a . The clamping velocity element φ _i (t) can be written as φ _i (t) = (θ _i ^u (t) - θ _i (t))/δt, for which θ _i (t) is calculated in Step 2 by Eq. (23). Thus, the clamping velocity Φ (t) can be obtained as

$\Phi \left(t\right)={\left[\begin{array}{cccc}{\left({\displaystyle \sum _{s=1}^r\frac{{\sigma }_{is}{u}_{is}^{\mathrm{T}}{\dot{p}}_i\left(t\right)}{{\sigma }_{is}^2+{\lambda }^2}{v}_{is}}\right)}^{\mathrm{T}}& \frac{{\theta }_i^u\left(t\right)-{\theta }_i\left(t\right)}{\delta t}& \frac{{\theta }_{i+1}^u\left(t\right)-{\theta }_{i+1}\left(t\right)}{\delta t}& 0_{(n-i-1)\times 1}\end{array}\right]}^{\mathrm{T}}.$

(29)

3.4. Discussions on computational load

As mentioned above, the proposed BQSA is motivated primarily by the aim of relieving the computational load. A comparison of computational complexity between traditional kinematical methods for two-dimensional obstacle avoidance and BQSA is discussed in this subsection.

In prior literature [2, 13, 15–17], kinematically redundant manipulators’ obstacle avoidance is dependent on the real-time determination of the obstacle avoidance point, contributing the greater portion of the method's computational load. Without the assistance of extra sensors, like laser range finders, cameras, etc., the search for the obstacle avoidance point that is least far from the obstacles on the links in the structured environment is conducted based on the computation of the Euclidean distance between every point on the obstacles’ surface and manipulator links, for every iteration step. Considering the scenario depicted in Figure 1, assume that the i-th (i = 1, 2,…, n) manipulator link consists of N _i test points and the j-th (j = 1, 2,…, k) obstacle consists of M _j test points, respectively. The position of every test point is assumed to be known, and the computation of the Euclidean distance between every single test point pair results in double multiplication. Moreover, there are ${\displaystyle \sum _{i=1}^n{N}_i}{\displaystyle \sum _{j=1}^k{M}_j}$ test point pairs in total. Thus, the computational complexity of the determination of obstacle avoidance points is $O\left(2{\displaystyle \sum _{i=1}^n{N}_i}{\displaystyle \sum _{j=1}^k{M}_j}\right)$ .

Nevertheless, the BQSA achieves obstacle avoidance without searching for the obstacle avoidance point, but rather by determining the states of collisions based on the position of end-points for every link and the shape of every elliptical obstacle. Assume that the position (x_imin, x_imax) for i = 1, 2,…, n and parameters {A _j , B _j , C _j , D _j , E _j , F _j } for j = 1, 2,…, k is given. According to Eqs. (3) and (16), 22 times of multiplications is consumed to determine the quadratic function f _ij (x). Furthermore, for the computation of the criterion of root Δ _ij and roots {x_1ij, x_2ij}, three further multiplications are required, indicated by Eqs. (21) and (22). Referring to the same scenario consisting of n links and k obstacles, as depicted in Figure 1, it will repeat the above computation processes nk times for every iteration and, hence, the computational complexity of collision detection reaches O (25nk).

Assuming that N = min{N _i , i = 1, 2,…, n } and M = min{M _j , j = 1, 2,…, k }, the following inequalities can be derived as

$2{\displaystyle \sum _{i=1}^n{N}_i}{\displaystyle \sum _{j=1}^k{M}_j}\ge 2NMnk$

(30)

and

$\frac{2{\displaystyle \sum _{i=1}^n{N}_i}{\displaystyle \sum _{j=1}^k{M}_j}}{25nk}\ge \frac{NM}{12.5}.$

(31)

Therefore, the computational complexity of the BQSA will be lower than previous kinematical methods based on the determination of the obstacle avoidance point, as long as NM > 12.5, which is satisfied in most situations. Eq. (30) also indicates that the improvement in computational load for the BQSA increases as the number of test points increases, but has no relationship to the number of links or obstacles. Hence, the relief of computational burden achieved by the BQSA is reasonable for most situations with an arbitrary number of manipulator links and obstacles.

In order to demonstrate the reduction in computational load achieved by the BQSA as compared with previous methods, the cost of computation time versus the number of test points for each obstacle-link pair (i.e., the value of NM) and the total number of links and obstacles (i.e., the value of nk) are respectively calculated using data from statistical experiments. The statistical results are displayed in Figure 5 and Figure 6, respectively. Data in each figure are the averages of 100 repeated tests conducted using a Matlab program run on a Intel(R) Core(TM) i7-4770 CPU @ 3.40 GHz.

Setting nk = 27, Figure 5 illustrates a linear relationship between computation time for the test of obstacle avoidance points and the value of NM, while the computation time for the BQSA is little influenced by the value of NM. Obviously, for most situations (when NM > 100), the BQSA requires less computation time and the benefit of the BQSA for time consumption grows as the value of NM increases. Thus, for most obstacle shapes (excluding points and lines), the BQSA can clearly relieve the computational burden as compared to present kinematical methods proposed in the existing literature. As obstacles’ shapes become more complicated, this benefit in computational load will become even more pronounced.

The computation time consumed by both previous methods and the BQSA, where NM = 600 and the total number of links and obstacles (i.e., the value of nk) is increased, is depicted in Figure 6. Both curves increase linearly as the value of nk increases, and both the computation time and the slope of the obstacle point test are obviously greater for the existing methods than for the BQSA. This phenomenon thereby verifies the relief of computational load by the BQSA from another perspective.

Figure 5.

Computation time versus ‘NM’ when the value of nk is fixed to 27

Figure 6.

Computation time versus ‘nk’ when the value of NM is fixed to 600

4. Numerical Simulations

4.1. A seven-DOF planar manipulator

Since three-dimensional obstacle avoidance can be guaranteed by three mutually orthogonal two-dimensional obstacle avoidance processes, as indicated by Lemma 1, a planar case is numerically simulated to verify the effectiveness of the proposed BQSA. The simulation scenario is depicted in Figure 7, in which a planar seven-link manipulator is expected to move its end-effector along the surface of an elliptic object while avoiding an elliptic obstacle. The shape and location of both the elliptic object and obstacle are assumed to be known before the resolution of IK, and the parameters specification of the simulation are summarized in Table 1. The seven-DOF planar manipulator is obviously redundant since the planar trajectory tracking mission only consumes two DOF. Therefore, the manipulator has five extra DOF to be utilized for the obstacle avoidance task.

Figure 7.

Simulation scenario: a planar seven-DOF manipulator is expected to move its end-effector along a desired trajectory on the surface of an elliptic object, avoiding collisions between the object and an elliptic obstacle

Table 1.

Parameters specification of the simulation of a planar manipulator

Nomenclatures	Specification	Values(unit)
O1, O2, O3, O4, O5	End-points of links	\
OXY	World coordinate	Origin at O1, OX along the horizontal axis and OY along the vertical axis.
l1, l2, l3, l4, l5, l6, l7	Link length	l1=0.2m, l2=0.3m, l3=0.5m, l4=0.5m, l5=0.5m, l6=0.5m, l7=0.7m
A1, B1, C1, D1, E1, F1	Object ellipse	A1= 0.01, B1= 0, C1= 0.04, D1= -0.048, E1= -0.016, F1= 0.0588
A2, B2, C2, D2, E2, F2	Obstacle ellipse	A2= 0.01, B2= 0, C2= 0.09, D2= -0.04, E2= -0.144, F2= 0.0967
Δθ	Joint angle increment	0.01 rad

Depicted as Figure 8, manipulator configurations obtained by BQSA illustrate an avoidance of the elliptic obstacle without deviation from the desired end-effector trajectory. It should be noted that the collision involving the object is unexpected, so the avoidance of the object is also necessary during the simulation. The trajectory tracking is considered the main task, which requires at least two DOF to guarantee tracking accuracy, and the redundancy of the remaining five DOF for the seven-link manipulator is utilized to achieve the secondary task of avoiding collision with both the obstacle and the object.

Figure 8.

Manipulator configurations illustrating the trajectory tracking by the end-effector, as well as obstacle avoidance utilizing BQSA

Figure 9.

Criteria of the quadratic categories of collisions as defined in Figure 3, where possible collisions occur only between link seven and the object, and link six and the obstacle

According to the results of collision detection, possible collisions only occur between link seven and the object, and link six and the obstacle. Figure 9 depicts the value of classification criteria of the quadratic categories, as defined in Figure 3. The manipulator configurations satisfy either Δ ≤ 0 or Δ > 0 ∩ f(x_min) > 0 ∩ f(x_max) > 0 ∩ df(x_min)/dx⋅df(x_max)/dx > 0, which belong to either the no collision 1 or no collision 2 categories, as outlined in Figure 3. The results demonstrated in Figure 9 verify the effectiveness of collision detection based on quadratic categories, and are compatible with the manipulator configurations shown in Figure 8.

Figure 10 compares the desired end-effector trajectory and the practical trajectory. Clearly, the manipulator can achieve an accurate trajectory tracking mission, even though the tracking precision decreases slightly as the manipulator avoids the obstacle. A further illustration of the tracking performance is depicted in Figure 11, which shows that the tracking errors along both the x-axis and y-axis are below 3.5 mm. Figure 11 also illustrates smooth trajectory tracking when the manipulator meets no obstacles, while the end-effector moves non-smoothly as the manipulator encounters and actively avoids obstacles.

Figure 10.

A comparison of the desired end-effector trajectory and the practical trajectory

Figure 11.

Trajectory tracking errors along both the x-axis and y-axis

The seven joint angles are depicted in Figure 12, which displays continuous but clearly non-smooth curves. The non-smooth aspect arises when the manipulator encounters and avoids the obstacles, and indicates a discontinuous property for joint velocities during obstacle avoidance. This phenomenon is compatible with the characteristics of the BQSA, shown as Eqs. (24) and (27).

Figure 12.

Joint angles during the manipulation

4.2. A demonstration of a PUMA 560 robot

In order to verify the feasibility and effectiveness of the BQSA for three-dimensional situation, numerical simulations are conducted on a PUMA 560 robot. The configuration and the Denavit-Hartenberg (DH) coordinates are depicted in Figure 13 and the DH parameters are summarized in Table 2. The robot has six DOF in total, and the trajectory tracking mission costs three DOF. The remaining three DOF can be utilized for the obstacle avoidance task.

Figure 13.

The configuration and the DH coordinates of the PUMA 560 robot

Table 2.

DH parameters of the PUMA 560 robot

Joint number	Torsion angle α(deg)	Link length a (mm)	Joint angle θ (deg)	Link distance d (mm)
1	0	0	θ 1	400
2	90	25	θ 2	0
3	0	315	θ 3	0
4	90	35	θ 4	0
5	-90	0	θ 5	-285
6	90	0	θ 6	-80

During the manipulation task, the robot starts from an initial state with joint angles set as θ = [0; −60; 90; 0; 10; 0] deg and tracks a circular trajectory with 100mm as the radius, located in the YOZ plane by its end-effector. At the top section of the trajectory, the end-effector collides with an ellipsoidal obstacle whose centre is located at (547, 0, 634) mm and the three axis lengths of which equal 20 mm, 100 mm and 60 mm, respectively. By projecting the three-dimensional robot configuration to the XOZ plane, the BQSA achieves the two-dimensional elliptic obstacle avoidance, which is illustrated in Figure 14. Assured by Lemma 1, the robot can succeed in avoiding the ellipsoidal obstacle in the three-dimensional space, as depicted in Figure 15. Moreover, the obstacle avoidance and the effectiveness of the collision detection by the quadratic categories defined in Figure 3 can also be illustrated by the value of criteria shown as Figure 16, in which the data satisfy either Δ ≤ 0 or Δ > 0 ∩ f(x_min) > 0 ∩ f(x_max) > 0 ∩ df(x_min)/dx⋅df(x_max)/dx > 0. This phenomenon is compatible with the collision-free quadratic category and indicates obstacle avoidance.

Figure 14.

PUMA 560 robot configurations illustrating the end-effector trajectory tracking and obstacle avoidance utilizing BQSA, projected from the three-dimensional space to the XOZ plane

Depicted in Figure 17, the comparison of the desired and the practical circular trajectories demonstrates accurate trajectory tracking by the end-effector. However, when the BQSA activates obstacle avoidance, the tracking trajectory becomes a little less smooth and deviates from the desired path. As shown in Figure 18, the tracking error along the x-axis is less than 1 mm, and along the y-axis is less than 4 mm. The largest tracking error, of approximately 20 mm, occurs on the y-axis, which is reasonable given that the BQSA is performed on the XOZ planar projection of the robot. The joint angles shown in Figure 19 have similar shape properties to the curves shown in Figure 12. The non-smooth change occurs when the robot avoids the obstacle, which indicates a discontinuity in joint velocities and accords with the weighted characteristics of the BQSA.

Aiming to illustrate the effectiveness of the BQSA with regard to a wall obstacle, the three axis lengths of the ellipsoidal obstacle are set as 0.1 mm, 100 mm and 10000 mm. The ellipsoid thereby approximates a wall, relative to the robot demo. The three-dimensional configurations depicted in Figure 20 illustrates success in avoiding the wall obstacle, achieved by the utilization of the BQSA.

Figure 15.

PUMA 560 robot configurations illustrating the end-effector trajectory tracking, as well as obstacle avoidance utilizing BQSA

Figure 16.

Criteria of quadratic categories of collisions as defined in Figure 3, where possible collisions occur only between the end-effector and the ellipsoidal obstacle

Figure 17.

A comparison of a desired and a practical end-effector trajectory

Figure 18.

Tracking errors of the end-effector of the PUMA 560 robot along three axes

Figure 19.

Joint angles of the PUMA 560 robot during the manipulation

Figure 20.

Puma 560 robot configurations during wall obstacle avoidance

5. Conclusions

This paper develops a backward quadratic search algorithm (BQSA) for inverse kinematics (IK) of redundant manipulators, with trajectory tracking as the primary task and obstacle avoidance as the secondary task. As the BQSA uses end-points’ positions, which can be easily obtained, rather than the closest point to the obstacle on each link, the computational burden of IK resolution is expected to be reduced. The design of the backward search ensures that the obstacle avoidance by the i-th link does not deform the configurations of those links labelled as larger than i. The hybrid scheme of IK guarantees obstacle avoidance as the effect of the weighted least-norm method and ensures no loss of degree of freedom by the gradient projection method and the clamping velocity. The effectiveness of the BQSA is verified numerically and the relief of computational load by the BQSA is analysed both theoretically and numerically.

Furthermore, although most of the discussions and simulations are conducted on the basis of planar situations, three-dimensional obstacle avoidance can also be achieved by utilizing the BQSA in three mutually orthogonal two-dimensional planes, as indicated by Lemma 1. Numerical simulations of a PUMA 560 robot support the feasibility and effectiveness of the BQSA in the three-dimensional situation.

Since the PUMA 560 robot is widely used in the design of industrial manipulators, applications of the proposed BQSA are promising and eagerly anticipated. Moreover, obstacle avoidance is only a small subset of the secondary tasks that can be achieved by utilizing manipulators’ redundancy. A series of extra constraints, including joint limits, joint torque minimization, etc., are expected to be addressed in the developed hybrid scheme of inverse kinematics.