Robot Control near Singularity and Joint Limit Using a Continuous Task Transition Algorithm

Abstract

When robots are controlled in the task space, singularities and joint limits are among the most critical and difficult issues that can arise. In this paper, we propose a new approach for the robots to operate in the regions near singularities and joint limits using the operational space control framework. Specifically, a continuous task transition algorithm called the intermediate desired value approach is applied to the hierarchically structured controller in the operational space control framework. In this approach, new tasks are defined for dealing with singularities and joint limits, and the tasks are activated or deactivated using the continuous task transition algorithm to guarantee the continuous execution of the tasks during the execution of the main task. The proposed approach is implemented on a 6-DOF manipulator called Roman-MD. The experimental results demonstrate its performance near the singular regions and joint limits.

1. Introduction

The workspace of robots is limited by singularities and joint limits. When robots are controlled by task space control algorithms, it is necessary to handle singularities and joint limits during the execution of tasks so as to perform the tasks successfully (Figure 1) [1].

Figure 1.

When robots operate in a human environment they can encounter singularities and joint limits during the execution of their tasks.

There have been various approaches to operating robots near singularities and joint limits in task space control. To deal with singularities in the kinematic control, a damped least-square inverse Jacobian matrix was proposed to resolve the singularity problem [2–4]. It provided an approximate solution when the robot was close to a singular position. Later, the transpose of the Jacobian was used instead of the inverse, which also approximates the solution [5, 6]. In addition, a redundancy-control method based on a task-priority strategy was proposed as a means of kinematic control [7–9]. This approach allows robots to move near or even pass through singular configurations. Moreover, a method is proposed to control the robot to stop and switch its controller when it is detected that the robot is near a singular region in a kinematic control scheme [10, 11]. On the other hand, there have been few studies on this issue in the framework of operational space control. Some of the solutions were specifically designed for the singularity types on PUMA560 robots [12, 13].

Meanwhile, to overcome the problem of joint limits, a method was proposed based on a weighted least norm solution for a redundant robot in the kinematic control [14, 15]. The weighted least norm solution was also compared with the gradient projection approach [16], showing that the weighted least norm method can avoid unnecessary self-motion and oscillation when the correct magnitude of self-motion is used. In line with redundancy resolution algorithm, quadratic programming (QP) methods have been proposed in the kinematic control [17] and the QP-based dynamical system approach has been developed [18].

For the operational space control framework, a typical solution is to use the classical potential field approach [19]. However, it is often difficult to select appropriate gains because the selection depends on the configuration and the controller of the existing tasks. Moreover, it was pointed out that the potential field methods have some problems such as local minimum and oscillations [20–22].

A solution for dealing with unilateral constraints, including both singularity and joint limit, was proposed for the kinematic control [23, 24], which modifies the null-space projection matrices for continuous control law during the transition among multiple tasks. This approach was further developed to be applied in the operational space control framework [25]. Similarly, a method using continuous null space projections was proposed for torque-controlled robots [26]. Both of the methods are based on the modification of the null space projection matrix and limited in the use of multiple hierarchical structures.

Recently, a task transition approach called the intermediate desired approach was proposed to deal with task transition among multiple tasks in kinematic control [27] and was further developed to be used in the operational space control framework [28]. This approach is different from the previously mentioned approaches [25, 29] in that the desired values are modified while the control structure remains the same. Additionally, multiple constraints and tasks can be performed with multiple levels of priorities, which is important when dealing with joint limit avoidance, singularities and tasks in different priorities. In this paper, the problems of singularity and joint limit avoidance are resolved by this intermediate desired value approach in the operational space control framework.

The main contribution of this paper compared to our previous works is that the present study provides complete details pertaining to issues relating to singularity and joint limit tasks during the implementation of the intermediate desired value approach in the operational space while also demonstrating its feasibility via its performance on a physical robot.

The main idea of using the intermediate desired value approach when dealing with singularities and joint limits is first to define the tasks corresponding to the singularities and joint limits, and then to insert or remove these tasks while the main task of the robot is executed. The intermediate desired value approach provides a smooth transition among multiple tasks with or without a hierarchy. Specifically, constraint tasks, such as joint-limit-avoidance tasks, are provided with high priority over the primary task in order to conduct tasks accurately and to maintain uninterrupted transitions with a continuous task transition algorithm.

This approach is implemented both in simulation and on a physical robot which is a 6-DOF manipulator called Roman-MD. In the experiments of joint limit avoidance, the proposed approach is compared with the potential field approach [19]. Then, the operation of the robot is demonstrated approaching near singularity and coming back to the initial position. Finally, the control of the robot is performed in the configuration near both joint limit and singularity.

The paper is organized as follows. First, the task transition algorithm in the operational space control framework is summarized in Section 2. Second, stability during transitions is discussed in Section 3. Singularity and joint limit avoidance using the task transition algorithm are presented in Section 4 and 5, respectively. The experimental results are plotted and discussed in Section 6. The paper is concluded in Section 7.

2. Continuous Task Transition Approach

We approach the problems of handling singularities and joint limits by applying a continuous transition algorithm [27]. In the following sub-sections, we first recall a hierarchical control structure in the operational space control framework and then introduce a continuous task transition approach.

2.1. Hierarchical Control Structure

When a robot is controlled in the operational space control framework, the task space dynamics is described as follows: [30]

${\mathbf{\Lambda }}_t{\ddot{x}}_t+{\mathit{\mu }}_t+p_t={\mathit{F}}_t\mathrm{,}$

(1)

and

$\begin{array}{ll}{\mathbf{\Lambda }}_t\hfill & =(J_t{A}^{-1}{J}_t^T{)}^{-1}\hfill \\ {\mathit{\mu }}_t\hfill & ={\mathbf{\Lambda }}_t(J_t{A}^{-1}\mathit{b}-{\dot{J}}_t\dot{q})\hfill \\ {\mathit{p}}_t\hfill & ={\mathbf{\Lambda }}_t{J}_t{A}^{-1}\mathit{g}\mathrm{.}\hfill \end{array}$

(2)

Here, q, A, b and g are the vectors of the joint angles, the joint space mass/inertia matrix, the Coriolis/centrifugal torque, and the gravity torque, respectively. The terms J_t, Λ_t, µ_t, p_t and F_t are the Jacobians corresponding to the task x_t, the task space inertia matrix, the vector of Coriolis/centrifugal forces, the vector of gravity forces, and the control force for this task. The control torque for control of the task can then be composed using the task dynamics [30].

$\begin{array}{ll}{\mathbf{\Gamma }}_t\hfill & =J_t^T{\mathit{F}}_t=J_t^T\hspace{0.17em}{\mathbf{\Lambda }}_t{\mathit{f}}_t^{*}+{\mathit{\mu }}_t+{\mathit{p}}_t\hfill \\ \hfill & =J_t^T{\mathbf{\Lambda }}_t\hspace{0.17em}{\mathit{f}}_t^{*}+{\mathit{\eta }}_t+{\mathit{\zeta }}_t=J_t^T{\mathbf{\Lambda }}_t{\tilde{f}}_t\mathrm{,}\hfill \end{array}$

(3)

and

${\mathit{\eta }}_t={\mathbf{\Lambda }}_t^{-1}{\mathit{\mu }}_t\mathrm{,}{\mathit{\zeta }}_t={\mathbf{\Lambda }}_t^{-1}{\mathit{p}}_t\mathrm{.}$

(4)

In these equations, f*_t is the control input for the unit mass system for the task x_t.

At this point, when the robot has a constraint task, such as a joint limit avoidance task, we consider the constraint task as x_c and the primary task as x_t in a hierarchical structure. In other words, the command torque for the primary task is controlled in the null space of the constraint task as follows [30],

$\begin{array}{ll}\mathbf{\Gamma }\hfill & ={\mathbf{\Gamma }}_{constraint}+{\mathbf{\Gamma }}_{task}\hfill \\ \hfill & =J_c^T{\mathit{F}}_c+N_c^T{\mathbf{\Gamma }}_t\hfill \\ \hfill & =J_c^T{\mathit{F}}_c+N_c^T(J_t^T{\mathit{F}}_t)\hfill \end{array}$

(5)

$\begin{array}{ll}{N}_c^T\hfill & =I-J_c^T{\bar{J}}_c^T\hfill \\ {\bar{J}}_c^T\hfill & =A^{-1}{J}_c^T{\mathbf{\Lambda }}_c\hfill \end{array}$

(6)

where J _c and J _t are the Jacobians corresponding to the constraint and primary tasks, respectively. The matrix J̄ _c is the dynamically consistent inverse of the Jacobian J _c . The terms F_c and F_t are the control forces corresponding to x_c and x_t, respectively. The term N ^T _c is the null space projection matrix of the constraint task. The dynamics in the constraint and task space can then be derived [30]:

${\mathbf{\Lambda }}_c{\ddot{x}}_c+{\mathit{\mu }}_c+{\mathit{p}}_c={\mathit{F}}_c$

(7)

${\mathbf{\Lambda }}_{t|c}{\ddot{x}}_t+{\mathit{\mu }}_{t|c}+{\mathit{p}}_{t|c}={\mathit{F}}_t$

(8)

where

$\begin{array}{ll}{\mathbf{\Lambda }}_{t\hspace{0.17em}c}\hfill & =(J_t{N}_c{A}^{-1}{N}_c^T{J}_t^T{)}^{-1}\hfill \\ {\mathit{\mu }}_{t\hspace{0.17em}c}\hfill & ={\mathrm{\Lambda }}_{t\hspace{0.17em}c}(J_t{A}^{-1}b-{\dot{J}}_t\dot{q})\hfill \\ {\mathit{p}}_{t\hspace{0.17em}c}\hfill & ={\mathrm{\Lambda }}_{t\hspace{0.17em}c}{J}_t{A}^{-1}\mathit{g}\mathrm{.}\hfill \end{array}$

(9)

Using the equations of motion in the constraint and task spaces, the control forces F_c and F_t can be formulated as follows:

${\mathit{F}}_c={\mathbf{\Lambda }}_c{\mathit{f}}_c^{*}+{\mathit{\mu }}_c+{\mathit{p}}_c={\mathbf{\Lambda }}_c{\tilde{f}}_c$

(10)

${\mathit{F}}_t={\mathbf{\Lambda }}_{t\hspace{0.17em}c}{\mathit{f}}_t^{*}+{\mathit{\mu }}_{t\hspace{0.17em}c}+{\mathit{p}}_{t\hspace{0.17em}c}={\mathbf{\Lambda }}_{t\hspace{0.17em}c}{\tilde{f}}_t\mathrm{.}$

(11)

If the two equations are combined, the control torque for the hierarchical task set is

$\mathbf{\Gamma }=J_c^T{\mathbf{\Lambda }}_c{\tilde{f}}_c+N_c^T{J}_t^T{\mathbf{\Lambda }}_{t\hspace{0.17em}c}{\tilde{f}}_t\mathrm{.}$

(12)

When a constraint with a high priority is added or removed, the torque command switches between (3) and (12). The task transition approach is introduced in the next section to resolve the issue of a discontinuity in such cases.

2.2. Task Transition Approach for both Prioritized Tasks and Non-prioritized Tasks

The continuous task transition algorithm known as the intermediate desired value approach is applied for a smooth transition when new tasks are inserted or removed. A joint limit avoidance task, for example, can be created with a high priority level while a primary existing task is executed. Therefore, we can revise equation (12) by applying the intermediate desired value approach.

In addition, for dealing with singularity, some parts of an existing task can be selected for removal, in which case the task removal or insertion transition is at the same hierarchical level as the existing task. Moreover, we need an algorithm to deal with operations that avoid both joint limits and singularities simultaneously.

For one typical example, we consider the execution of one constraint task with two tasks with equal priorities: x_c, x_t1 and x_t2. By applying the intermediate desired value approach, the term f˜ _c is modified to f˜ ⁱ _c as follows:

$\mathbf{\Gamma }=J_c^T{\mathbf{\Lambda }}_c{\tilde{f}}_c^i+N_c^T{J}_t^T{\mathbf{\Lambda }}_{t\hspace{0.17em}c}{\tilde{f}}_t^i$

(13)

where

$\begin{array}{l}{J}_t=\left(\begin{array}{c}{J}_{t1}\\ J_{t2}\end{array}\right)\mathrm{,}{\tilde{f}}_t^i=\left(\begin{array}{c}{\tilde{f}}_{t1}^i\\ {\tilde{f}}_{t2}^i\end{array}\right)\hfill \end{array}$

(14)

and

$\begin{array}{l}\hspace{0.17em}{N}_c^T=I-J_c^T{\bar{J}}_c^T\hfill \\ {\mathbf{\Lambda }}_{t\hspace{0.17em}c}^{-1}=J_t{N}_c{A}^{-1}{N}_c^T{J}_t^T\mathrm{.}\hfill \end{array}$

(15)

The intermediate values are defined by

$\begin{array}{ll}{\tilde{f}}_c^i\hfill & =h_c{\tilde{f}}_c+(1-h_c)J_c{A}^{-1}{\mathbf{\Gamma }}_{[/c]}^{*}\hfill \\ {\tilde{f}}_{t1}^i\hfill & =h_{t1}{\tilde{f}}_{t1}+(1-h_{t1})J_{t1}{A}^{-1}{\mathbf{\Gamma }}_{[/t1]}^{*}\hfill \\ {\tilde{f}}_{t2}^i\hfill & =h_{t2}{\tilde{f}}_{t2}+(1-h_{t2})J_{t2}{A}^{-1}{\mathbf{\Gamma }}_{[/t2]}^{*}\mathrm{.}\hfill \end{array}$

(16)

These equations provide a continuous transition among prioritized tasks when the task x _c is inserted at a higher priority. The activation parameters h_c, h_t1 and h_t2 change from 0 to 1 continuously for a smooth transition when the task is inserted, whereas they change to 0 when the task is removed. The term Γ*_[/n] denotes the torque input using the intermediate desired value excluding the n task:

${\mathrm{\Gamma }}_{[/c]}^{*}=J_t^T{\mathrm{\Lambda }}_t{\tilde{f}}_t^i$

(17)

where

$\begin{array}{ll}{\tilde{f}}_{t1}^i\hfill & =h_{t1}{\tilde{f}}_{t1}+(1-h_{t1})J_{t1}{A}^{-1}{J}_{t2}^T{h}_{t2}{\mathrm{\Lambda }}_{t2}{\tilde{f}}_{t2}\hfill \\ {\tilde{f}}_{t2}^i\hfill & =h_{t2}{\tilde{f}}_{t2}+(1-h_{t2})J_{t2}{A}^{-1}{J}_{t1}^T{h}_{t1}{\mathrm{\Lambda }}_{t1}{\tilde{f}}_{t1}\mathrm{.}\hfill \end{array}$

(18)

In the equations above, in order to compute Γ*_[/c], we need to consider task x_t1 and x_t2, which have the same priority level. On the other hand, in the cases of Γ*_[/t1] and Γ_[/t2], we take into account task transitions with priority. Therefore, Γ*_[/t1] can be computed as

${\mathbf{\Gamma }}_{[/t1]}^{*}=J_c^T{\mathbf{\Lambda }}_c{\tilde{f}}_c^i+N_c^T{J}_{t2}^T{\mathbf{\Lambda }}_{t2|c}{\tilde{f}}_{t2}^i$

(19)

where

$\begin{array}{ll}{\tilde{f}}_c^i\hfill & =h_c{\tilde{f}}_c+(1-h_c)J_c{A}^{-1}{J}_{t2}^T{h}_{t2}{\mathbf{\Lambda }}_{t2}{\tilde{f}}_{t2}\hfill \\ {\tilde{f}}_{t2}^i\hfill & =h_{t2}{\tilde{f}}_{t2}+(1-h_{t2})J_{t2}{A}^{-1}{J}_c^T{h}_c{\mathbf{\Lambda }}_c{\tilde{f}}_c\mathrm{.}\hfill \end{array}$

(20)

3. Stability During Transition

The proposed approach uses the operational space control framework with a hierarchical structure having priorities. The control structure is not modified in this approach, so the controlled system is stable if not in transition. The modelling uncertainties may affect the performance of the system, but these are mostly overcome by the feedback on the unit-mass system in the operational space control framework.

Now, the stability during transition is discussed in this section. The intermediate desired value approach is applied to the control inputs f˜ _c and f˜ _t in (12), which correspond to constraint and task. During transition, the intermediate desired values of the control inputs, f˜ ⁱ _c and f˜ ⁱ _t , are computed using (16). Each intermediate desired value of the control input in (16) is composed of two parts: the control input of the constraint or task itself and the effect from the control of other constraints or tasks. They are scaled down by h_c or h_t, and (1 – h_c) or (1 – h_t), respectively. As the activation parameter decreases, the control of the constraint or task itself diminishes. That is, the DOF of the system used for the control of the corresponding constraint or task is gradually used by the other constraints or tasks.

Considering this constitution of the sub-systems, their stability depend on each controller with the control input of the constraint or task itself. In this paper, a PD control is designed for each sub-system, which behaves as a unit mass system by using the operational space control framework if not in transition [31]. The proportional and derivative gains can be chosen to have critically damped responses for each unit mass system. Then, when the task is in transition, the effect of using the activation parameters is to decrease the proportional and derivative gains with the same ratio. Therefore, the response of each system in transition becomes over-damped with a lower bandwidth. Since the system becomes over-damped, the system is still stable with the comprised bandwidth of the task.

4. Joint Limit Avoidance

As stated in the Introduction section, there has been much research on implementing stable operation near joint limits or on avoiding joint limits. The task transition approach can also be very effective in the application of a joint limit avoidance scheme in the operational space control framework.

We need to manage the robot so that it moves within the range of joint movement while the main task x_t is performed. This can be accomplished by inserting a joint limit avoidance task as a new task with a higher priority level than the other tasks when one of the joints comes close to its lower or upper joint limits. We set the joint limit buffer regions considering each joint limit, and a new task of avoiding the joint limit is created within this buffer region considering the range of joint motion [27]. In this buffer region, we treat the formation and destruction of joint limit avoidance task as cases of task insertion and removal, respectively. Consequently, we need a smooth task transition during the performance of these tasks.

Figure 2.

The activation parameter for a joint limit avoidance task.

Considering the task of avoiding the joint limit, the torque command is computed as

$\mathbf{\Gamma }=J_{jl}^T{\mathbf{\Lambda }}_{jl}{\tilde{f}}_{jl}^i+N_{jl}^T{\mathbf{\Gamma }}_t$

(21)

where

$\begin{array}{ll}{\tilde{f}}_{jl}^i\hfill & =h_{jl}{\tilde{f}}_{jl}+(1-h_{jl})J_{jl}{A}^{-1}{J}_t^T{\mathbf{\Lambda }}_t{\tilde{f}}_t\hfill \\ {\mathbf{\Gamma }}_t\hfill & =J_t^T{\mathbf{\Lambda }}_{t\hspace{0.17em}jl}{\tilde{f}}_t\mathrm{.}\hfill \end{array}$

(22)

In these equations, J _jl , F _jl and N^T _jl are the Jacobian, the control force for joint limit avoidance, and its null-space projection matrix, respectively, while Γ _t is the torque for the main task. For example, if the fourth joint is in the buffer region of its joint limit, J _jl is defined as follows for a 6-DOF manipulator:

$J_{jl}=\left[000100\right]$

(23)

Moreover, to maintain smooth operation while the joint limit avoidance task is active or inactive, the activation parameter of the joint limit avoidance task varies continuously between 0 and 1 as a function of the corresponding joint angle. The activation parameter increases as the corresponding joint angle approaches the joint limit. In this experiment, the activation function in an earlier study [27] is used, as plotted in Figure 2. Specifically, the activation function h_i is designed as follows:

$h_i=\{\begin{array}{ll}1.0,\hfill & \text{\hspace{1em}\hspace{1em}if}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}{q}_i\hspace{0.17em}\ge {\bar{q}}_i\hfill \\ f(q_i)\mathrm{,}\hfill & \text{\hspace{1em}\hspace{1em}if}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}{\tilde{q}}_i<q_i<{\bar{q}}_i\hfill \\ \mathrm{0.0,}\hfill & \text{\hspace{1em}\hspace{1em}if}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}{\underset{˜}{q}}_i\le q_i\le {\tilde{q}}_i\hfill \\ g(q_i)\mathrm{,}\hfill & \text{\hspace{1em}\hspace{1em}if}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}{\underset{\_}{q}}_i<q_i<{\underset{˜}{q}}_i\hfill \\ \mathrm{1.0,}\hfill & \text{\hspace{1em}\hspace{1em}if}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}{q}_i\le {\underset{\_}{q}}_i\hfill \end{array}$

(24)

and

$\begin{array}{ll}f(q_i)\hfill & =0.5+0.5sin\left(\frac{\pi }{\beta }(q_i-{\tilde{q}}_i)-\frac{\pi }{2}\right)\hfill \\ g(q_i)\hfill & =0.5+0.5sin\left(\frac{\pi }{\beta }(q_i-{\underset{˜}{q}}_i)+\frac{\pi }{2}\right)\mathrm{.}\hfill \end{array}$

(25)

The ith joint angle, q_i, is constrained by the lower limit q _i and the upper limit q̄_i, and β denotes the size of the activation buffer

$\begin{array}{ll}{\tilde{q}}_i\hfill & ={\bar{q}}_i-\beta \hfill \\ {\underset{˜}{q}}_i\hfill & ={\underset{\_}{q}}_i+\beta \mathrm{.}\hfill \end{array}$

(26)

On the other hand, the activation parameter for the primal task is maintained at 1 throughout the execution of the task because it is always fully activated.

The size of the buffer region can be determined depending on the specific task. If the size is large, the transition will be more smoothly conducted. However, the normal workspace would be smaller and the transition time would be longer. The size is set to approximately 10% of the joint range, 0.15rad, in this experiment.

Note that the dimension of the Jacobian for the joint limit depends on how many joints are in the buffer region. The activation parameters for each joint limit task, which correspond to each row in J _jl , have different values because the joints are at different points in their buffer regions. For example, if the first and fourth joints are in transition for a 6-DOF robot, we have two joint limit avoidance tasks x_jl1 and x_jl2 at the same priority level. The Jacobian can be formulated as follows:

$J_{jl}=\left(\begin{array}{c}{J}_{jl1}\\ J_{jl2}\end{array}\right)$

(27)

where

$\begin{array}{ll}{J}_{jl1}\hfill & =\left[100000\right]\hfill \\ J_{jl2}\hfill & =\left[000100\right]\mathrm{.}\hfill \end{array}$

(28)

The transitions among these joint limit avoidance tasks can be regarded as non-prioritized tasks in equation (13). Above all, the joint limit avoidance task x_jl needs to have a higher priority than the primal task x_t.

5. Singularity Avoidance

In this section, we discuss the implementation of singularity avoidance using the intermediate desired value approach in the operational space control framework. The idea is to deactivate the task along the singular direction near the singularity. The singular value decomposition (SVD) is used on the inverse of the task space inertia matrix to identify the singular directions and to define the task associated with it. The activation parameter for continuous transition is designed to change based on the singular value which is lower than a certain threshold.

First, singularity is constantly monitored by the singular value decomposition (SVD) of the inverse of the inertia matrix of the primary task x_t.

${\mathbf{\Lambda }}_t^{-1}=J_t{A}^{-1}{J}_t^T=U_t{\mathbf{\Sigma }}_t{U}_t^T$

(29)

Here, Σ _t is a diagonal matrix with singular values, and U _t is a rotation matrix [30, 32].

When one of the singular values is less than a threshold, the corresponding direction is considered as a singular direction. Therefore, U _t is decomposed into two parts: U _ns for non-singular directions and U _s for the singular direction.

$U_t=\left[U_{ns}{U}_s\right]$

(30)

The primary task x_t is then decomposed into two tasks as follows:

${\mathit{x}}_{t\mathrm{,}ns}=U_{ns}^T{\mathit{x}}_t\text{\hspace{1em}and\hspace{1em}}{\mathit{x}}_{t\mathrm{,}s}=U_s^T{\mathit{x}}_t$

(31)

The corresponding Jacobians are defined as

$J_{t\mathrm{,}ns}=U_{ns}^T{J}_t\text{\hspace{1em}and\hspace{1em}}{J}_{t\mathrm{,}s}=U_s^T{J}_t\mathrm{.}$

(32)

Accordingly, task x_t,ns and task x_t,s are at the same priority level and the control torque is computed as

$\mathbf{\Gamma }=J_t^T{\mathbf{\Lambda }}_t{\tilde{f}}_t^i$

(33)

where

$\begin{array}{l}{J}_t=\left(\begin{array}{c}{J}_{t\mathrm{,}ns}\\ J_{t\mathrm{,}s}\end{array}\right)\mathrm{,}{\tilde{f}}_t^i=\left(\begin{array}{c}{\tilde{f}}_{t\mathrm{,}ns}^i\\ {\tilde{f}}_{t\mathrm{,}s}^i\end{array}\right)\hfill \end{array}$

(34)

and

$\begin{array}{ll}{\tilde{f}}_{t\mathrm{,}ns}^i\hfill & =h_{t\mathrm{,}ns}{\tilde{f}}_{t\mathrm{,}ns}+(1-h_{t\mathrm{,}ns})J_{t\mathrm{,}ns}{A}^{-1}{J}_{t\mathrm{,}s}^T{h}_{t\mathrm{,}s}{\mathbf{\Lambda }}_{t\mathrm{,}s}{\tilde{f}}_{t\mathrm{,}s}\hfill \\ {\tilde{f}}_{t\mathrm{,}s}^i\hfill & =h_{t\mathrm{,}s}{\tilde{f}}_{t\mathrm{,}s}+(1-h_{t\mathrm{,}s})J_{t\mathrm{,}s}{A}^{-1}{J}_{t\mathrm{,}ns}^T{h}_{t\mathrm{,}ns}{\mathbf{\Lambda }}_{t\mathrm{,}ns}{\tilde{f}}_{t\mathrm{,}ns}\mathrm{.}\hfill \end{array}$

(35)

In the following experiment of controlling the end-effector as a main task, the orientation task is given a higher priority level than the position. This is because the singular values and the corresponding directions have more intuitive physical meaning when the position and orientation tasks are separately considered [33]. Thus, it is proper to divide the tasks initially into two main parts: the position and the orientation.

Herein, a hierarchical structure, which is similar to a constraint-task structure, is applied and the task of controlling the end-effector is divided into two parts (13). The subscripts o and p indicate the orientation task and the position task, respectively. When the singular direction comes up from the position task, the control torque can be formulated as

$\mathbf{\Gamma }=J_o^T{\mathbf{\Lambda }}_o{\tilde{f}}_o^i+N_o^T{J}_p^T{\mathbf{\Lambda }}_{p\hspace{0.17em}o}{\tilde{f}}_p^i$

(36)

where

$\begin{array}{l}{J}_p=\left(\begin{array}{c}{J}_{p\mathrm{,}ns}\\ J_{p\mathrm{,}s}\end{array}\right)\mathrm{,}{\tilde{f}}_p^i=\left(\begin{array}{c}{\tilde{f}}_{p\mathrm{,}ns}^i\\ {\tilde{f}}_{p\mathrm{,}s}^i\end{array}\right)\hfill \end{array}$

(37)

and

$\begin{array}{cc}\hfill {\tilde{f}}_o^i\hfill & =h_o{\tilde{f}}_o+(1-h_o)J_o{A}^{-1}{\mathbf{\Gamma }}_{[/o]}^{*}\hfill \\ {\tilde{f}}_{p\mathrm{,}ns}^i\hfill & =h_{p\mathrm{,}ns}{\tilde{f}}_{p\mathrm{,}ns}+(1-h_{p\mathrm{,}ns})J_{p\mathrm{,}ns}{A}^{-1}{\mathbf{\Gamma }}_{[/p\mathrm{,}ns]}^{*}\hfill \\ {\tilde{f}}_{p\mathrm{,}s}^i\hfill & =h_{p\mathrm{,}s}{\tilde{f}}_{p\mathrm{,}s}+(1-h_{p\mathrm{,}s})J_{p\mathrm{,}s}{A}^{-1}{\mathbf{\Gamma }}_{[/p\mathrm{,}s]}^{*}\mathrm{.}\hfill \end{array}$

(38)

The activation parameter for task x_t,ns is kept at 1 because we want to execute the task fully along the non-singular direction. Thus, task x_t,s along the singular direction is activated or deactivated by the activation parameter which is designed as a function of the corresponding singular value. The activation parameter for the singular value starts with 1 at the threshold value and then decreases as the singular value decreases such that the control of the task in that direction will be progressively deactivated. One example of such an activation parameter is plotted in Figure 3, where the threshold is set to 0.02 and decreases until the value reaches 0.01. These values and the shape of the function can be designed by considering the physical intuition of the inertial property of the manipulator.

Figure 3.

The activation parameter of the singular direction task is designed as a function of the singular value by singular value decomposition (SVD)

At this point, when we consider both a joint limit and a singularity simultaneously, the joint limit avoidance task has a higher priority. Moreover, as already stated, task x_t is initially decomposed into the orientation task x₀ and the position task x_p when singularity avoidance is considered. Hence, the order of task priorities is the joint limit, the task for orientation control x₀, and then the position control task x_p:

$\mathbf{\Gamma }={\mathbf{\Gamma }}_{jl}^i+{\mathbf{\Gamma }}_o^i+{\mathbf{\Gamma }}_p^i$

(39)

where

$\begin{array}{ll}{\mathbf{\Gamma }}_{jl}^i\hfill & =J_{jl}^T{\mathrm{\Lambda }}_{jl}{\tilde{f}}_{jl}^i\hfill \\ {\mathbf{\Gamma }}_o^i\hfill & =(J_o{N}_{jl}{)}^T{\mathbf{\Lambda }}_{o\hspace{0.17em}jl}{\tilde{f}}_o^i\hfill \\ {\mathbf{\Gamma }}_p^i\hfill & =(J_p{N}_{o\hspace{0.17em}jl}{N}_{jl}{)}^T{\mathbf{\Lambda }}_{p\hspace{0.17em}o\hspace{0.17em}jl}{\tilde{f}}_p^i\hfill \end{array}$

(40)

and

$\begin{array}{l}{J}_p=\left(\begin{array}{c}{J}_{p\mathrm{,}ns}\\ J_{p\mathrm{,}s}\end{array}\right)\mathrm{,}{\tilde{f}}_p^i=\left(\begin{array}{c}{\tilde{f}}_{p\mathrm{,}ns}^i\\ {\tilde{f}}_{p\mathrm{,}s}^i\end{array}\right)\mathrm{.}\hfill \end{array}$

(41)

The intermediate values are defined as follows:

$\begin{array}{ll}\hfill {\tilde{f}}_{jl}^i\hfill & =h_{jl}{\tilde{f}}_{jl}+(1-h_{jl})J_{jl}{A}^{-1}{\mathbf{\Gamma }}_{[/jl]}^{*}\hfill \\ \hfill {\tilde{f}}_o^i\hfill & =h_o{\tilde{f}}_o+(1-h_o)J_o{A}^{-1}{\mathbf{\Gamma }}_{[/o]}^{*}\hfill \\ {\tilde{f}}_{p\mathrm{,}ns}^i\hfill & =h_{p\mathrm{,}ns}{\tilde{f}}_{p\mathrm{,}ns}+(1-h_{p\mathrm{,}ns})J_{p\mathrm{,}ns}{A}^{-1}{\mathbf{\Gamma }}_{[/p\mathrm{,}ns]}^{*}\hfill \\ {\tilde{f}}_{p\mathrm{,}s}^i\hfill & =h_{p\mathrm{,}s}{\tilde{f}}_{p\mathrm{,}s}+(1-h_{p\mathrm{,}s})J_{p\mathrm{,}s}{A}^{-1}{\mathbf{\Gamma }}_{[/p\mathrm{,}s]}^{*}\mathrm{.}\hfill \end{array}$

(42)

6. Experimental Results

The proposed approach was implemented on a 6-DOF manipulator called Roman-MD (Figure 4). The robot is controlled using software called RoboticsLab [34]. The servo rate was set to 500 Hz when reading the encoders and sending the torque commands to the motors. In the following experiments, the control input for the tasks is composed by PD control.

${\mathit{f}}^{*}=k_p({\mathit{x}}^d-\mathit{x})-k_v\dot{x}\mathrm{,}$

(43)

where x^d, x and ẋ are the desired value, the current state and the derivative of the current state of the task. The terms k _p and k _v are the proportional and derivative feedback gains, respectively. Because each task becomes a unit mass system using the nonlinear feedback linearization by the operational space control framework, the gains can be selected based on the task specifications. In this paper, the closed-loop frequency and damping ratio were chosen as 20.0 rad/sec and 1.0, respectively, to be a critically damped system. The gains k_p and k_v are then 400.0 and 40.0 because it is a unit mass system.

Figure 4.

Joint configuration of Roman-MD and the Cartesian coordinates of a robot system.

6.1. Joint Limit Avoidance Task

In this experiment, the position of the end-effector is controlled to move to the lower-right direction and come back to the initial position while the orientation of the end-effector is controlled to be the initial orientation, as illustrated in Figure 5. The first joint limit avoidance task is inserted while the positioning task is performed.

In order to avoid the joint limit, the activation parameter of the first joint increases at around three seconds, as the first joint enters its joint limit buffer region, as shown in Figure 6.

In this experiment, the upper and lower limits for all joints are set to 0.78rad and −0.78rad, respectively. These chosen values are smaller than necessary to show the performance of the proposed algorithm effectively. Furthermore, the size of the buffer region is 0.15rad, and the activation parameter in Figure 2 is used.

The experiment demonstrates that the joint limit task is activated and deactivated without any abrupt change in the control of the end-effector (Figure 7). The position of the robot cannot be controlled as desired when the constraint task of joint limit avoidance is activated.

In the next experiment, we compared the potential field approach and the proposed task transition approach for joint limit avoidance through the simulation using the 6-DOF manipulator. The control input f_i using the potential field approach [19] is,

$f_i=\{\begin{array}{ll}\eta \left(\frac{1}{{\underset{\_}{\rho }}_i}-\frac{1}{{\underset{\_}{\rho }}_{i(0)}}\right)\frac{1}{{\underset{\_}{\rho }}_i^2}\mathrm{,}\hfill & \text{\hspace{1em}\hspace{1em}if\hspace{1em}}{\underset{\_}{\rho }}_i\le {\underset{\_}{\rho }}_{i(0)}\hfill \\ \mathrm{0.0,}\hfill & \text{\hspace{1em}\hspace{1em}if\hspace{1em}}{\underset{\_}{\rho }}_i>{\underset{\_}{\rho }}_{i(0)}\hfill \\ -\eta \left(\frac{1}{{\bar{\rho }}_i}-\frac{1}{{\bar{\rho }}_{i(0)}}\right)\frac{1}{{\bar{\rho }}_i^2}\mathrm{,}\hfill & \text{\hspace{1em}\hspace{1em}if\hspace{1em}}{\bar{\rho }}_i\le {\bar{\rho }}_{i(0)}\hfill \\ \mathrm{0.0,}\hfill & \text{\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}if\hspace{1em}}{\bar{\rho }}_i>{\bar{\rho }}_{i(0)}\hfill \end{array}$

(44)

$\begin{array}{ll}{\underset{\_}{\rho }}_i\hfill & =q_i-{\underset{\_}{q}}_i\hfill \\ {\bar{\rho }}_i\hfill & ={\bar{q}}_i-q_i\mathrm{.}\hfill \end{array}$

(45)

Figure 5.

Snapshots from the joint limit avoidance experiment.

Figure 6.

Joint velocity, joint angle and activation parameter of the first joint during the joint limit avoidance experiment.

Figure 7.

Plots of end-effector position during the joint limit avoidance experiment.

Figure 8.

Simulation: execution of the joint limit task using the potential field approach and the proposed continuous control approach.

Figure 9.

Fourth joint angle and end-effector position during the joint limit avoidance simulation using the potential field approach.

Here, ρ _i(0) and ρ̄_i(0) are the distance limit of the potential field influence. The ith joint angle is q_i, and q _i and q̄_i represent the lower limit and the upper limit.

In this simulation, the desired position is set out of the joint range. While the task is performed, the fourth joint reaches its limit buffer region (Figure 8). In the case of the potential field approach, the constant gain η was chosen as 0.5, and the distances ρ _i(0) and ρ̄_i(0) are 0.08 and −0.08, respectively.

As shown in Figure 9 and Figure 10, the potential field approach, in comparison with the proposed approach, can easily cause oscillation and instability unless the gains are carefully tuned. Although the oscillatory response of the potential field approach shown in this experiment can be improved by changing the parameters of η and ρ _i(0) at the specific configuration, it is not easy to find suitable values for all the configurations of the robot [21, 22]. The oscillatory and unstable response is shown in this experiment to point out the typical problem of using the potential field approach in the application of joint limit avoidance.

Figure 10.

Fourth joint angle and end-effector position during the joint limit avoidance simulation using the intermediate desired value approach.

6.2. Handling Singularity

The experiment of controlling the robot near singularity is presented in this section. As noted in Section 5, the robot can deal with singularities by deactivating the task along the singular direction.

In this experiment, the end-effector is commanded to move from the upper-right to the lower-left position and come back to the initial position (Figure 11). The position of the end-effector cannot reach the desired position exactly because the deactivated direction is not fully controlled.

The activation parameter of the task is designed as a function of the singular value by singular value decomposition (SVD), and the buffer of the singular values is identical to that plotted in Figure 3.

Figure 12 and Figure 13 show the data plots from this experiment. The activation parameter for the singular direction starts to decrease at σ = 0.02 (around 4 seconds) and the positioning task becomes smoothly deactivated along the singular direction. Because the singular direction is on the y-z plane, the position in the z-direction does not follow the desired position trajectory when the position task in the singular direction is in transition region.

Figure 11.

Experiment of the singularity avoidance.

Figure 12.

Activation parameter and singular value during the experiment of handling singularity.

Figure 13.

Plots of position y and position z during the experiment of handling singularity.

6.3. Joint Limit and Singularity Avoidance

In this section, related to Section 5, the case is demonstrated in which both the joint limit and singularity are handled when controlling the end-effector of the robot as a primary task. As shown in Figure 14, the position of the end-effector is commanded to move toward the desired position and come back to the initial position.

As shown in Figure 15, when the singular value enters the buffer region at approximately three seconds, the activation parameter for the singular direction decreases according to the activation function of the singular value as designed. Approximately 1 second later, the task of avoiding the fourth joint limit is added to the commanding task, as shown in Figure 16. The position of the end-effector during the experiment is plotted in Figure 17. It should be noted that the position of the end-effector did not follow the desired trajectory completely near singularity and joint limit. The proposed algorithm executed the primary task of controlling the end-effector without abrupt change in motion near the singularity while avoiding the joint limit.

Figure 14.

Joint limit and singularity avoidance experiment.

Figure 15.

Singular value and activation parameter of the singular direction when the robot is controlled near both singularity and joint limit.

Figure 16.

Joint angle and activation parameter of the second joint when the robot is controlled near both singularity and joint limit.

Figure 17.

Position of the end-effector when the robot is controlled near both singularity and joint limit.

7. Conclusion

In this paper, we propose a control strategy for dealing with singularities and joint limits using the intermediate desired value approach in the operational space control framework. The intermediate desired value approach in the operational space control framework enables an effective and stable transition when tasks are inserted and removed. Controlling the robot near singularity is implemented by deactivating the task along the singular direction. In addition, the transition algorithm is effectively applied to both the insertion and the removal of the joint limit task with a hierarchical control structure. The experimental results demonstrate that the robot is successfully controlled when it approaches its joint limits and a singular region using the proposed approach. The proposed approach in this paper demonstrated the robot operation with only motion tasks. Our future work is directed toward generalizing this control approach to deal with tasks involving both motion and contact so that it can be used in robot-environment interaction.