Stiff and safe task-space position and attitude controller for robotic manipulators

This paper proposes a stiff and safe task-space position and attitude control scheme for robotic manipulators. This study extends the work of Kikuuwe et al’s. (2006) velocity-bounding proxy-based sliding mode control by explicitly addressing the attitude part. The proposed controller has a Jacobian-based structure, which realizes smooth trajectories when the desired attitude is far rotated from the actual attitude. It also imposes arbitrary magnitude limits on the end-effector velocity, angular velocity, and each actuator force without sacrificing a stiffness, which is the same level as a high-gain PID position control below the limits. The benefit of the proposed controller becomes apparent after the robot yields to external forces due to force saturations, when the robot makes contact with obstacles. In such a situation, if the external forces disappear, the controller generates overdamped resuming motion from large tracking errors. The proposed controller can be expected to enhance the safety of robotic applications for the human–robot interaction. The proposed method is validated by experiments employing a six-degree of freedom industrial manipulator.


Introduction
When the robotic manipulators work in three dimensional task space, the robot's trajectory is usually described by task-space coordinate vectors that contain a position vector and attitude of the robot end-effector. The control of the robot end-effector is usually performed by torque commands to each actuator. In order to determine the actuator torque command, one method is to use the inverse kinematics and the joint angle controllers and another one is to use the transpose Jacobian and the forward kinematics [1]. A weak point of the control schemes using the inverse kinematics is that it can produce excessive large torques in the the vicinity of singular configurations. In such situation, a control scheme based on the transpose Jacobian avoids the problem numerically [2].
Robotic manipulators are usually affected by nonlinear factors such as inertia and joint friction. To suppress such nonlinear factors, a stiff position controller is necessary for tracking accuracy. The proportional-derivative (PD) and proportional-integral-derivative (PID) position control are widely used for the robotic manipulator as a stiff position controller [3]. It can be extended to a Jacobianbased control scheme using the errors between the actual position/attitude and the desired position/attitude [4,5], in which the proportional gain sets the robot stiffness.
While the position error between the desired position and actual position of robot end-effector is simply computed as a vector difference, the attitude error is not straightforward. Some previous researchers have studied the PD position controller combined with a Jacobianbased control scheme for tracking the desired position/ attitude in three-dimensional task space. One example is Luh et al. 's [6] controller. Their control scheme can be seen as a PD-type controller combined with dynamics compensations, in which the attitude error is represented by a rotation matrix. Khatib et al. 's [7] used the attitude Open Access error in a PD-type controller based on three Euler angles. The controllers employing Euler angles are not free from the representational singularity [8]. Caccavale et al. [9] proposed a PD-type controller using the attitude error in terms of an unit quaternion, which has no representational singularities. This control framework has been also applied to force control schemes [10] as a low-level controller.
One drawback of the PD-type controllers is that it can cause undesirable trajectories when a desired attitude is far rotated from an actual attitude. It may lead to a trouble with the robot configuration. Moreover, the controller with high gains produces excessively high speed from the large position and attitude errors. Such behaviors usually result in large oscillations and overshoots, which are not suitable for the robot that performs tasks with humans in the same workspace.
For improving the safety of the stiff position controller, Kikuuwe and his colleagues proposed a proxy-based sliding mode control (PSMC) [11][12][13], which is an extension of PID position controller and also can be seen as an approximation of a sliding mode controller (SMC). The advantage of PSMC is that it produces overdamped motion from large position error after the actuator force saturation, but its tracking accuracy is the same level as a high-gain PID control as long as the actuator force is not saturated. It has been applied to many applications, such as rehabilitation robots [14][15][16], passive actuators [17], pneumatic actuators [18], motion platforms [19][20][21] and bilateral master-slave system [22]. After initial disclosure of PSMC, they proposed a velocity bounding PSMC (VB-PSMC) for complementing the original PSMC [23]. It imposes an arbitrary magnitude limit on velocity without sacrificing tracking accuracy below the limit. An extension of VB-PSMC into the three-dimensional task space has not been considered in the previous work where an attitude control is not trivial especially.
This paper proposes a stiff and safe task-space controller for robotic manipulators, which is an extension of VB-PSMC with addressing the attitude part using the unit quaternion. The proposed controller has a Jacobian-based structure, which does not produce excessively large speed in the vicinity of singular configurations. It imposes arbitrary magnitude limits on the end-effector velocity, angular velocity and each actuator force without sacrificing its stiffness, which is the same level as a high-gain PID position control below the limits. The additional benefit of the controller appears after the robot yields to external forces due to the actuator force saturation, when the robot makes contact with obstacles. In such situation, if the external forces disappear, the controller generates overdamped resuming motion from the large tracking errors. This behavior looks similar to the compliant behavior that is realized by an impedance control for the robotic manipulators [24][25][26], but its tracking accuracy is the same level as the high-gain PID position control during normal operation. The proposed controller can be expected to be useful in a robotic application, when the robot works in the workspace shared with the human.
The rest of this paper is organized as follows. "Background" section prepares mathematical preliminaries and an overview about VB-PSMC. "Robot kinematics" section discusses robot kinematics and a Jacobian-based structure. "Task-space controller" section presents a new taskspace controller and its implementation by implicit Euler discretization, which avoids chattering problem that is the inherent problem of SMC. "Experiment" section presents experimental results. "Conclusion" section provides the concluding remarks.

Mathematical preliminaries
In this paper, R denotes the set of all real numbers and D n denotes the set of all n × n diagonal matrices of which diagonal elements are all strictly positive. Symbols in boldfaces denote a vector or a matrix and the symbol o and I denote the zero vector and identity matricx of appropriate dimensions, respectively.
Let us define scalar functions sgn(·) and sat(·) as follows: where z ∈ R . These functions are illustrated in Fig. 1 and they are related to each other as follows: which is proved by Kikuuwe et al. 's [12]. By using the functions sgn(·) and sgn(·) , we define Sgn(·) and Sat(·) , which are the element-wise vector versions of sgn(·) and sat(·) as follows: (2) y ∈ sgn(z − y) ⇐⇒ y = sat(z) ∀y, z ∈ R,  (1) where z = [z 1 , z 2 , . . . , z n ] T ∈ R n . Because the function sgn(·) is a set-valued function, the function Sgn(·) is a setvalued vector function. Based on the relation (2), Sgn(·) and Sat(·) also have a relationship as follows: which can be used in the following relation: where {Y , Z} ∈ D n and {y, z} ∈ R n . Since Sgn(Yz) is equivalent to Sgn(z) , this relation holds true. It will be used in the derivation in "Task-space controller" section.

Velocity bounding proxy-based sliding mode control
The concept of VB-PSMC was developed from PSMC, Both PSMC and VB-PSMC can be explained by an imaginary dynamical system that includes a massless virtual object (proxy) illustrated in Fig. 2. In this system, the proxy is connected to a controlled object through a PIDtype virtual coupling (PID controller) and the other side is connected to the given desired position through a sliding mode-like set-valued controller. Based on the imaginary dynamical system in Fig. 2, the control algorithm of VB-PSMC can be derived from the analytical solution of a differential algebraic inclusion.
Let p s ∈ R and p d ∈ R denote the actual position and the desired position of the controlled object, respectively. p x ∈ R denotes the proxy's position. Then, the PID-type virtual coupling produces the force f PID ∈ R , which can be determined as follows: where and L, K, and B are positive real numbers, which represent the integral, proportional, and derivative gains, respectively. These parameters should be chosen so that p s is controlled to follow p x . On the other side of the proxy, there is a difference between PSMC and VB-PSMC. The original PSMC involves a force f SMC ∈ R produced by a sliding mode-like set-valued controller, which is a slmple version of sliding mode controller (SMC) presented in Slotine et al. 's work [27]. The force f SMC ∈ R can be determined as follows: where F and H are positive real numbers for control parameters. The set σ x = 0 is a sliding manifold in the proxy's state space {p x ,ṗ x } and the control law (8) attracts the proxy's states toward the sliding manifold.
One potential risk of the control law (8) in the PSMC is that it can produce large velocity before the proxy's states reach to the sliding manifold. In order to complement the drawback, VB-PSMC involves the following controller:

Fig. 2 Physical interpretation of PSMC and VB-PSMC
where V and A are positive real numbers. The control law (9) can be seen as a set-point controller that employs a sliding manifold as s x = 0 . It implies that ṗ x is equal to or smaller than the parameter V because the right-hand side of (9b, c) is always smaller than V. Therefore, a magnitude of ṗ x is bounded in the parameter V as long as s x stays on the sliding manifold. As a result, when the set-point controller satisfies |ṗ x | ≤ V , it is equivalent to SMC. The imaginary dynamical system shown in Fig. 2 satisfies mp x = f SMC − f PID where m is the mass of the proxy. By setting m = 0 , (6) and (9) can be written in the following equation: which is a continuous-time representation of the onedimensional VB-PSMC. As has been detailed in Kikuuwe et al. 's work [23], taking the implicit Euler discretization of the continuous-time expression (10) and the application of (5) result in the following discrete-time representation: where k is the integer representing a discrete-time index, T is the timestep size and ▽ denotes the backward-difference operator, which is defined as ▽z(k) = z(k) − z(k − 1) and

Robot kinematics
This section presents a task-space coordinate using an unit quaternion and a Jacobian-based structure based on a transpose Jacobian matrix. The structure will be combined with a proposed controller to produce smooth trajectories when the desired attitdue is far rotated from the actual atttidue.

Task-space coordinate
The kinematics of robotic manipulators is the relation among joint variables q s ∈ R 6 , a position vector p s = [p x , p y , p z ] T ∈ R 3 and attitude. While the end-effector position is easily obtained by the position vector, several representations exist to represent the attituade. Let us consider an unit quaternion, which is composed of four elements for describing the attitude without a representational singularity resulted in an Euler angle representation. The unit quaternion representation of the attitude can be expressed as follows: where η s ∈ R denotes the scalar part, α s = [α x , α y , α z ] T ∈ R 3 denotes the vector part, in which θ s is a rotation angle around the unit vector of axis µ s ∈ R 3 . Note that the scalar part and vector part are constrained by the following equation: which implies that both [η s ,α T s ] T and [−η s , −α T s ] T describe the same attitude. If the rotation angle is set to the range {−180 • ≤ θ s ≤ 180 • } , then the scalar part is always positive and its representation of the attitude is unique. Based on the unit quaternion, the task-space coordinate can be expressed as the actual vector P s = [p T s ,α T s ] T ∈ R 6 and the desired vector where p d ∈ R 3 denotes the desired position vector and α d ∈ R 3 denotes the vector part of the unit quaternion representing the desried attitude . While the end-effector position error is simply computed as the vector difference p e = p s − p d , the attitude error is not straightforward. The unit quaternion representation of the attitude error can be expressed as follows: where the symbol • denotes the quaternion product and α d is the conjugate of the quaternion representing the desired attitude. It can be written as follows: where θ e denotes the rotation angle between the desired attitude and actual attitude and µ e ∈ R 3 denotes the unit vector of axis for θ e . If α e goes to zero, the quatanion (15) goes closer to the identity quaternion [1, 0, 0, 0] T . Thus, the position and attitude errors are represented by the following vector: which is referred to as an error vector P e ∈ R 6 where α e ∈ R 3 denotes the attitude error, (·) denotes the kinematics function for formulating the error vector with respect to the desired vector P d and the joint variables q s .

Jacobian-based structure
By using the kinematics function (·) , the relation between the error vector P e and joint variables can be written as follows: where J (P d , q s ) ∈ R 6×6 denotes the Jacobian matrix defined as follows: In the use of the relation (17), the force relationship between task-space and joint space can be written in the following equation: where δW denotes the virtual work and f c ∈ R 6 denotes a force vector and τ c ∈ R 6 denotes actuator force. The force vector and the actuator forces should be statically equated with each other.
Based on the relationship (19), a Jacobian-based structure can be written as follows: where f c is determined by the proposed controller using the error vector P e as the input of the controller, which will be discussed in "Task-space controller" section specifically.

Task-space controller
This paper considers a torque-commanded manipulator having six-degrees-of-freedom, which can be described in the following form: where M(q s ) ∈ R 6×6 denotes a symmetric positive-definite matrix that represents the inertia, τ g ∈ R 6 denotes gravity compensation torques, τ c ∈ R 6 represents actuator force generated from the output of the controller, h ∈ R 6 denotes forces applied to the object from external forces and d ∈ R 6 denotes the sum of forces resulted from all unmodeled factors. In order to suppress the influence of h and d , the high-gain PID position/attitude controller is necessary mentioned in "Introduction" section.
In the use of the torque-commanded manipulator, one problem of the high-gain PID position/attitude controller is that it produces excessively high speed and overshoot when the desired position/attitude are far separated from the actual position/attitude. The cascade control scheme is widely used in the industrial robot [28,29]. Simple torque and velocity limiters in the cascade control scheme does not provide direct solutions of the problem. This is because the cascade controller is joint based controller, of which the magnitude limits on the velocity/angular velocity cannot be achieved in task space directly. In addition, it does not guarantee overdamped and smooth motion below the limits.
Another imaginable solution to produce a damped response is to use a very high velocity feedback gain. It can magnify the noise in the velocity measurements and the magnified noise will deteriorate the tracking accuracy of the controller during normal operation.

Extension of VB-PSMC
For this reason, let us consider an extension of VB-PSMC into the three-dimensional task space. We consider the following controller as a direct extension of (10): 23) a � = (P x − P e )dt, P x ∈ R 6 denotes a proxy's vector, {K , L, B} ∈ D 6 denote control gain matrices, {V , F } ∈ D 6 , and A and H are positive real numbers, respectively. This is the continuous-time representation of an extension of VB-PSMC into the three-dimensional task space, in which f c is a force vector that should be statically equivalent to actuator forces. Note that (22) is an algebraic inclusion including discontinuity, which should be solved in the implementation.

Discrete-time implementation
For the implementation of the proposed algorithm, the continuous-time representation (22) must be approximated by a discrete-time representation with preserving its feature. Based on the implicit Euler discretization, the discrete-time representation of (22) can be rewritten as follows: Here, one can see that a(k) in (24b) and (24c) are an unknown vector. By setting the parameter A = 1/(T + H) , (24c) is written as follows: Note that (24d) is an algebraic inclusion with respect to the unknown a(k) . In order to solve the algebraic inclusion, we now express (24e) as follows: Substituting (26b) into (24d) yields: and substituting (26a) into (27) obtains as follows: where Based on (5), (28) can be expressed as follows: of which the discontinuities are eliminated by using the application of (5) and f c (k) is a force vector that should be applied to the robot end-effector.

Force limiter
For situations where actuator forces should be limited before the force vector is saturated in F , f * α (k) is used as a force vector, which can be bounded by using a transposed Jacobian matrix (18) as follows: where ζ(k) ∈ R denotes a scaling factor and in which C ∈ D 6 denotes a matrix for the magnitude limits on actuator forces, C n and τ * cn (k) (n ∈ {1, 2, . . . , 6}) denote the n-th diagonal elements of C and n-th elements of τ * c (k) , respectively. It realizes the statical equivalence of the pairs {τ * c (k), f * α (k)} and {τ c (k), f α (k)} . By using the scaling factor, the force vector f c (k) can be written as follows: which is saturated when τ c (k) is saturated in C . It results that the magnitue of f * α (k) is decreased by reducing the magnitue of a(k) according to (26b). This is especially useful to improve safety when the robot yields to external forces by the actuator force saturations. In addition, f c (k) is also limited by F itself when actuator forces are not saturated in C from the large tracking errors.

Discrete-time control algorithm
In conclusion, based on (24a), (25a), (25b), (26b), (29), (31) and (33), the numerical solution of (24) is obtained as follows: where τ c (k) is the output of the controller that must be sent to the robot actuators. The whole sysytem is illustrated as the block diagram shown in Fig. 3. This is the discrete-time control law of an extension of VB-PSMC combined with the Jacobian-based structure.

Behavior of controller
When removing the velocity limit from the proposed controller (34) (i.e., setting V → ∞I ), it can be expressed as follows: which can be seen as the controller based on PSMC.
In such a framework, smooth and overdamped motions from large tracking errors are realized by setting H to be appropriately large, which is mentioned in the previous paper [12] in detail. Note that (34) is equivalent to (35) as long as all elements of u * (k) is smaller than their correspondent diagonal elements of V . (34j) PSMC is an extension of the conventional PID control. By setting F → ∞I and H = 0 , the controller (35) behaves as follows:

Fig. 5
Results of Experiment I. The end-effector position p s and attitude α s and each joint torque during set-point controllers under cN1, cN2, cN3 and cN4where DT is desired trajectories, which are pointed in Fig. 4 which can be seen as the PID-type controller with the torque limiter using the scaling factor. The above derivation shows that the proposed controller (34) is advantageous over the controller (36) in the choices of V , F and H, which can be set according to safety regulations.

Experiment
The proposed controller (34) was experimentally tested by using a 6-DOF industrial manipulator MOTOMAN-UPJ (Yaskawa Electric Corporation), which is shown in Fig. 4. It was controlled through a PC running the ART-Linux operating system. This manipulator has six joints, which consist of AC servo motors integrated with harmonic drive gearings. In the experiment, the sampling interval was set to be T = 0.001s and six joints were used, in which each joint angles were measured by optical encoders attached to the actuators. For the comparison, the following controllers were used: • cN1: The proposed controller (34) with the following settings: (37a) K = diag(K 1 , K 1 , K 1 , K 2 , K 2 , K 2 ) which were chosen through trial and error. Its optimal design of these parameter values are subject to further research. • cN2: cN1 with setting matrices as follows: • cN3: cN1 with setting matrices as follows: (37b) L = diag(L 1 , L 1 , L 1 , L 2 , L 2 , L 2 )  • cN4: cN1 with setting matrices as follows: which can be seen as the controller based on PSMC (35).
• cSPID: a PID-type controller combined with the Jacobian-based structure (20) and a simple torque limiter: where parametric matrices were set as the same as cN4.

Experiment I: large position and attitude errors
In order to show the effectiveness of the proposed controller (34), a comparison among cN1, cN2, cN3 and cN4 was performed to show their trajectories from large attitude error. The desired vector P d was changed discontinuously among the three configurations shown in Fig. 4.
To be more specific, the initial vector P s was chosen as configuration A in Fig. 4. The desired vector P d was discontinuously changed to configuration B at t = 3s , then to configuration C at t = 7s , and finally, again to configuration A at t = 13s.
The results show that cN1, cN2, cN3 and cN4 produce overdamped motion without overshoot from large position/attitude errors shown in Fig. 5. In Period A in Fig. 5, one can see that the controllers produce smooth trajectories from large attitude error, which is visible as errors in {α x , α z } . In addition, their velocity and angular velocity are bounded at the values specified by the matrix V . Such behavior is correspond to the feature of VB-PSMC mentioned in "Background" section.

Experiment II: comparison between cN4 and cSPID
Second set of experiments were performed to compare cN4 with cSPID. The desired vector P d was changed between the two configurations shown in Fig. 6. The results are shown in Fig. 7. At t = 2s , the robot end-effector started to accelerate due to the large tracking errors. (40) cSPID produced high speed as expected, which leads to large oscillations and overshoots. The results indicate that cN4 and cSPID have distinctly different transient responses after acceleration. Specifically, the result from cN4 shows that the actual position/attitude exponentially move toward the desired position/attitude. It leads to smooth and overdamped motion, which can be adjusted by the parameter H. This property can be considered beneficial for safety when the desired position/attitude are far separated from the actual position/attitude.

Experiment III: human-robot interaction
In the final set of experiments, the robot was moved by the experimenter's hand through the robot's link that is connected to the 5th joint. In the experiment, a comparison between cN1 and cSPID was performed to show the effectiveness of cN1. It was started with the initial configuration F shown in Fig. 8. The experiment results are shown in Fig. 9. In Period B, the experimenter applied a torque smaller than 5 Nm to the robot's link, which appears as a reaction torque generated by the 5th joint. In Period C, the experimenter applied a torque larger than 5 Nm to the robot's link. After Period C, the experimenter released his hand from the robot. Fig. 9 shows that, in Period B, the robot did not move to any direction while the external force is applied to the robot link. It can be seen that the controller cN1 and cSPID resist the external force under the actuator force limiter. In Period C, the robot yields to external force due to the actuator force saturation where cN1 and cSPID produce distinctly different motion. The result produced by cSPID shows that the robot does not move toward the direction of the external force, while the 5th joint yields to external force shown in Figs. 8b and 9. This motion leads to the undesirable robot configuration. In addition, it produces excessively high speed after Period C due to large tracking errors. With the controller cN1, in contrast, the robot moves toward the direction of the external force shown in Figs. 8a and 9. In this Period, the force vector is saturated when 5th joint torque is saturated, which is consistent with the point discussed in "Force limiter" section. It is a preferable response when the robot should be moved by external environments. After Period C, the controller cN1 produces overdamped resuming motion from the large tracking errors. These behaviors can be seen as a main advantage of the proposed controller.

Conclusion
This paper has proposed a new task-space controller for robotic manipulators. It can be seen as an extension of VB-PSMC to deal with position and attitude in the threedimensional task space. The proposed controller has a Jacobian-based structure, which realizes smooth trajectories when the desired attitude is far rotated from the actual attitude. It also imposes arbitrary magnitude limits on the end-effector velocity, angular velocity and each actuator force without sacrificing the stiffness, which is the same level as a PID-type controller below the limits.
This controller was tested through experiments employing a 6-DOF industrial manipulator equipped with harmonic drive gearings. The results show that it produces high tracking accuracy and safe behavior as the advantage inherited from VB-PSMC. In addition, the force vector is saturated when actuator forces are saturated by external