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

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.


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].
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][3][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][8][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][21][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.

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.

Hierarchical Control Structure
When a robot is controlled in the operational space control framework, the task space dynamics is described as follows: [30] and 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]. and 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], where J c and J t are the Jacobians corresponding to the constraint and primary tasks, respectively.The matrix Jc 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]: where Using the equations of motion in the constraint and task spaces, the control forces F c and F t can be formulated as follows: If the two equations are combined, the control torque for the hierarchical task set is 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.

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 fc is modified to f i c as follows: where and The intermediate values are defined by 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: where 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 where Hyejin Han and Jaeheung Park: Robot Control near Singularity and Joint Limit Using a Continuous Task Transition Algorithm

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 fc and ft in (12), which correspond to constraint and task.During transition, the intermediate desired values of the control inputs, f i c and f i 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.

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.Considering the task of avoiding the joint limit, the torque command is computed as where 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: 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: and 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 On the other hand, the 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: where 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 .

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 .
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.
The primary task x t is then decomposed into two tasks as follows: x t,ns = U T ns x t and x t,s = U T s x t (31) The corresponding Jacobians are defined as Accordingly, task x t,ns and task x t,s are at the same priority level and the control torque is computed as where and 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 where and 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.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 o 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 o , and then the position control task x p : where and The intermediate values are defined as follows: (42)

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.
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.

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, where  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.
Accordingly, this step can only be carried out if the matching procedure was already performed for the first error image.Therefore, only areas that were not removed during the first matching procedure are extended by corresponding areas of the subsequent error images.Otherwise, the noise (falsely detected areas) would cause an enlargement of incorrectly detected areas.The red short dashed rectangles in Figure 8 mark 2 examples of such corresponding areas.Resulting areas that are too large are removed from the error images I n and I n+1 .This is indicated by the areas in the right lower corner of error image I n in Figure 8.As can be seen, the resulting error image I n from Figure 8 is used as input (error image I n ) in Figure 7. Without the extension of the areas, the midmost candidate in Figure 7 would have been rejected.
As some real moving objects are sometimes not detected in an error image as a result of an inaccurate optical flow calculation or (radial) distortion, the temporal matching would fail.already be the case if only one area in one error image is missing.Thus, candidates that were detected once in 3 temporal succeeding error images and 4 greyscale images (original images), respectively, are stored for a sequence of 3 error images subsequent to the image where the matching was successful, cf. Figure 9(a).Their coordinates are updated for the succeeding error images by using the optical flow data.As a consequence, they can be seen as candidates for moving objects in the succeeding images, but they are not used within the matching procedure as input.If within this sequence of images a corresponding area is found again, it is stored for a larger sequence of images (more than 3) and its coordinates are updated for every succeeding error image.The number of sequences depends on the following condition: where c is the number of found corresponding areas and c is the number of missing corresponding areas for one candidate starting with the image where the candidate was found again.If ξ < 0 ∨ ξ > 10, the candidate is rejected.Moreover, the candidate is no longer stored if it was detected again in 3 temporal succeeding images.In this case, it is detected during the matching procedure.
An example concerning to this procedure is shown in Figure 9(b).As one can imagine, error image I n in Figure 9(a) is equivalent (except area-extension) to I n+1 in Figure 7, whereas error image I n in Figure 9(b) is equivalent to I n+2 in Figure 9(a).
For a further processing of the data, only the position (shown as small black crosses in the left lower corners of the rectangles in Figures 7 and 9) and size of the rectangles marking the candidates are of relevance.Thus, for every error image the afore mentioned information is stored for candidates that were detected during the matching procedure, for candidates that were detected up to 3 error images before and for candidates that were found again (see above).On the basis of this data, candidates that are very close to each other are combined and candidates that are too large are rejected.

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.

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 Singular value and activation parameter of the singular direction when the robot is controlled near both singularity and joint limit.Position of the end-effector when the robot is controlled near both singularity and joint limit.
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.

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.

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

Figure 2 .
Figure 2. The activation parameter for a joint limit avoidance task.

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

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

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

Figure 9 .
Figure 9. Preventing rejection of candidates for moving objects that were detected only in a few sequences.(a) Storage of candidates for which a further matching fails.These candidates are marked by a blue dotdashed rectangle.The green dashed rectangle marks a candidate for which a corresponding area was found again and the red short-dashed rectangle marks a candidate with successful matching.(b) Storage of candidates for which a corresponding area was found again.The 2 areas drawn with transparency in error image I n indicate the position of the candidates, but they are not part of the error image.

Figure 16 .
Figure16.Joint angle and activation parameter of the second joint when the robot is controlled near both singularity and joint limit.
sponsored by the Basic Science Research Program (no.2010-0005799) and by the Global Frontier R&D Program on <Human-centered Interaction for Coexistence> funded by the National Research Foundation of Korea grant funded by the Korean Government(MSIP) (no.2011-0032014).