Trajectory tracking control of 7-DOF redundant robot based on estimation of intention in physical human-robot interaction

Abstract

Collaborative robot has been widespread application prospect, such as homes, manufacturing, and health-care etc. In physical human-robot interaction, the external force appears inevitably in contact with environment or human, especially the interactive tasks such as trajectory tracking requirements and force compliance control. In this article, a method based on interaction intention estimation, which solve the problem of trajectory tracking accuracy and force compliance control in the same direction for the 7-DOF robot, is proposed. The increased virtual force depended on the manipuility performance index and inverse kinematic solution used the kinematic decoupling method based on the redundant angle avoid the singularity of redundant robot. Then, based on interactive intention estimation, a control strategy of variable impedance sliding mode theory in the presence of virtual force and contact force is proposed to achieve the trajectory tracking. We adopted hyperbolic tangent function to alleviate the chattering problem caused by switch function and validated the control system stability by Lyapunov theorem. Finally, Matlab simulations exhibit a 97.8% of high tracking accuracy amid the external force is 43% less than variable impedance parameters. It is therefore proved that the proposed method can achieve asymptotic tracking and the compliant behavior in physical human-robot interaction.

Introduction

In recent years, robots safely and reliably coexist, cooperate with humans, share spaces and tasks with humans have captured the attention of researchers and industries. In physical human-robot interaction (pHRI), robot is required for accomplishing a task or a mission, such as trajectory tracking, the external forces and torques appear inevitably in contact with environments or with humans. Dealing with external forces and torques are important to support compliant interactions between robot and humans or environments in achieving a stable and safe cooperating task. In the presence of external forces, human and robot may agree upon the goal they are attempting to reach, nevertheless they might disagree on the trajectory that should be tracked. Especially for redundant robot (Degrees of freedom >6), recent research has focused on the fact that inverse kinematics (IK) solutions and self-motion. Aristidou et al. ¹ presented a comprehensive review of the IK problem and the solutions which include analytical and numerical, data-driven and hybrid methods from the computer graphics point of view. Where analytical method consists of a geometric and algebraic method. The analytical solution is suitable for the manipulators with less DOF and when DOF increases the solution gets complex. A Numerical method is an iterative method based on inverse Jacobian, pseudo-inverse Jacobian or Jacobian transform. ² proposed a numerical method of IK based on inverse Jacobian for 5-DOF manipulator but concluded that when the inverse of Jacobian is zero, the singularity occurs. Reference^3–5 focused on how to obtain all feasible inverse kinematic solutions in the global configuration space where joint movable ranges are limited. A method was proposed for determining the continuous joint angle vector by selecting the inverse solution from discrete multiple solutions to the continuous end path of the mechanical arm in. ⁶ Whereas, the literature of IK problem based on the dynamics constraint which there exist external force is relatively less. Ficuciello et al.^7,8 presented compliant behavior at the end effector according to a decoupled impedance dynamics and used DCI (dynamic conditioning index) to optimize IK problem for redundancy. Nevertheless, inverse kinematic under the external force for the redundant manipulator might suffer from singularity problem due to the Jacobian matrix, that was not mentioned.

After generating a joint trajectory using IK, the second challenging task is the manipulator motion control design and implementation. Different control algorithms have been proposed and implemented in the robotics field for the robust performance of manipulator. Nageshrao et al. ⁹ proposed an optimal passive control for the 2-DOF manipulator by using the energy balance theory. Tang et al. ¹⁰ proposed a learning-based adaptive optimal control method, which is used to solve the tracking problem of n-link robots. As a useful tool to deal with disturbances, sliding mode control (SMC) technique, which benefits from its strong robustness to overcome parametric uncertainties and system nonlinearities, has been applied in the field of robot control. As part of that, sliding mode control requires the approaching motion (non-sliding mode) reach the switching surface selected by the switching function in finite time, so that the tracking error converges asymptotically to zero. There are some insightful papers that address the stabilization and tracking control problems by using SMC technique. Zhu et al. ¹¹ used the sliding mode control theory applied to the robot end-effector, and it has the advantages of globally asymptotically stable when there exists a bounded disturbance. Saleh and Fairouz ¹² investigated a robust adaptive second-order SMC method for tracking problem of a class of uncertain linear systems with matched and unmatched disturbances. In,^13,14 sliding mode control methods were proposed to solve the problem of optimal trajectory tracking control of reconfigurable robots or amphibian robot. However, the slide mode control also has some drawbacks. SMC consist of reaching and sliding phase in which system reach the sliding surface and maintained on it, but because of nonlinearities high switching gains are required which makes the system go back and forth of the desired surface introducing chattering in the system response. ¹⁵ To effectively eliminate the chattering, in, ¹⁶ the switching term, made up of symbolic function, is approximated by fuzzy system to make adaptive adjustment of the sliding mode control’s switching gains. Nicolis et al. ¹⁷ proposed a teleoperation framework that relied on sliding mode control to achieve desired master and slave impedance and discussed the robust absolute stability conditions in presence of variable communication delays. Nevertheless, the main drawbacks lie on the difficulty in tuning a priori the sliding mode gains, the necessity of a force sensor on the master device, and partially, static friction compensation. Jung ¹⁸ introduced neural network at the trajectory level by forming the RCT (reference compensation technique) scheme to achieve the effect of selecting the appropriate gain of the sliding mode control indirectly. Yet, a sign function was used as the transformation function easily to cause chattering, also the force control for tracking trajectory was not considered.

Robotic control technology to match the target relations between force and displacement has been proposed in impedance control. Since the early innovative works on impedance and compliance control in,^19,20 the impedance control has been studied by many scholars. The constant impedance parameters, as a common method, might bring performance limitations in conducting cooperative works between robot and human or unstructured environment. In light of this, impedance control using the variable parameters, which had the advantage of decreasing the need for precise mathematical modeling of the robot and environment, was proposed. Ficuciello et al. ²¹ suggested the modulation scheme of the impedance parameters for redundant manipulator in Cartesian space to improve the control performance during the physical interactions between human and robot. In addition, a nonlinear model reference adaptive impedance controller was presented to achieve compliance in human-robot interaction by Sharifi et al. ²² The results were obtained through simulations used 2-DOF robot which end-effector move in the plane of x-z. As is known to all, the dynamic equations are relatively simplified for control methods based on the Lagrangian models. Xiong et al. ²³ proposed and verified a adaptive dynamic approach on the basis of the virtual decomposition Cartesian impedance control for the flexible joint manipulator in physical human-robot interaction. Motivated by the human versatility, Li et al. ²⁴ presented a robot controller that had feedforward force and impedance adaptation compensation for the interaction with the environment. In, ²⁵ the authors introduced an IC-PITD (impedance control physically interactive trajectory deformations) control technique for physically interactive trajectory deformation which allowed the human to modulate both the actual and desired trajectories of the robot. The aforementioned methods were intended to solve the problem of how to obtain the appropriate variable impedance parameters to ultimately achieve the desired interaction motion planning task, but they did not deal with the trajectory tracking requirements and force compliance control achieved in the same direction for the redundant robot.

This article aims to solve the problem of trajectory tracking accuracy and force compliance control in the same direction for the 7-DOF robot in physical human-robot interaction. Compared with previous studies, the main contributions of this article are threefold.

1. A method based on interaction intention estimation is proposed to meet the requirements of trajectory tracking and force compliance in the same direction for the 7-DOF robot in physical human-robot interaction. In this method, the increased virtual force depends on the manipulability performance index, judged by the contact force, velocity, normalized position increment and joint angle increment, and combine with the kinematic decoupling method based on the redundant angle to achieve effective tracking of the trajectory, as well as to avoid the singularity of a numerical method and complexity of an analytical solution for redundant robot.

2. On the basis of interactive intention estimation, a control method based on variable impedance sliding mode theory in the presence of virtual force and contact force, in which impedance parameters tuning used fuzzy inference rule and approach law adopted the hyperbolic tangent function to alleviated the chattering problem caused by switch function, is presented.

3. We validate the stability of control system by the theoretical calculation with the Lyapunov function and verify the control effects. For 7-DOF redundant robot, our controller not only can obtain the continuous joint angle that satisfies the trajectory tracking but also render the compliant behavior in the trajectory tracking.

This article is structured as follows: Section 2 introduces the kinematics and dynamics model of redundant robot. Section 3 suggests the controller of variable impedance sliding mode and stability proof in Cartesian coordinates. Section 4 shows the simulation results, carried out in the Matlab environment and Robotics Toolbox while robot is modeled in SolidWorks, to confirm the effectiveness of the proposed controller. Finally, Section 5 draws the conclusions and future works.

Inverse kinematics and dynamics modeling of the physical human-robot interaction

System design and inverse kinematics model

The robotic manipulator is a 7-DOF rotating-joint type robot and the Computer-Aided Design (CAD) model is presented in Figure 1. SolidWorks is used to get the virtual prototype model of the actual system which is kept vertical up in the original state. The axis configuration is shown in Figure 1(a). For kinematics, the joint coordinate frames on the basis of Denevit-Hartenberg (DH) rules are presented in Figure 1(b). From Figure1, it can be seen that rotation axes of the first three joints, named as S (Shoulder), intersect at a point, as well as the last three joints which named as W (Wrist) and the joint 4 named as E (elbow) connects the two parts. The simplified manipulator structure is shown as Figure 2.

Figure 1.

The structural scheme of the 7-DOF redundant manipulator. (a) seven rotational joints of the 7-DOF redundant manipulator. (b) each of the joints coordinate frames based on the D-H rules.

Figure 2.

Simplified model of the 7-DOF manipulator; The wrist joint and the shoulder joint can be regarded as two Spherical joints, with joint four as rotating joint, the redundant angle β is defined as the angle between arm plane determined by point S, E, W and plane determined by point X₀, S, W.

If the position and orientation of the end-effector are given, the pose of point W is uniquely determined. The position of point S is not changed in the case of base coordinate. So, the only point that can be changed is E, which is rotating around the line SW in view of the fixed position of S and W. The motion of point E, called self-motion of the manipulator, causes the multiple inverse kinematics solutions.

In this article, the kinematic decoupling method based on redundant angle is presented. We firstly define the new coordinate system C to calculate the position of point E. X₀ is the x-axis orients in base coordination frame. The origin of the new coordinate system C is the vertical point from E to vector SW, Z-axis is defined from point C to W, Y-axis goes through point C and is the normal to a plane composed of point X₀, S, W, and X-axis depends on the right-hand rule. Thus, we can compute the three unit axes of the new coordinate system C as follows:

$$\widehat{\mathrm{CZ}}=\frac{\overrightarrow{\mathrm{SW}}}{|\overrightarrow{\mathrm{SW}}|},\widehat{\mathrm{CY}}=\frac{\overrightarrow{\mathrm{SW}}\times \overrightarrow{S{X}_0}}{|\overrightarrow{\mathrm{SW}}\times \overrightarrow{S{X}_0}|},\widehat{\mathrm{CX}}=\frac{\widehat{\mathrm{CY}}\times \widehat{\mathrm{CZ}}}{|\widehat{\mathrm{CY}}\times \widehat{\mathrm{CZ}}|}$$

(1)

Then, redundant angle β is defined as the angle between two planes consist of point S, E, W and point X₀, S, W.

$$\mathrm{\beta }=\mathrm{atan}2({\mathrm{\beta }}_{\mathrm{Y}},{\mathrm{\beta }}_{\mathrm{X}})$$

(2)

Where β_X, β_Y denote that $\overrightarrow{\mathrm{SE}}$ are projected onto $\widehat{\mathrm{CX},}\widehat{\mathrm{CY},}$ respectively. So, we can get the position of elbow E when β is given.

For kinematic decoupling method, which consists inverse position and inverse orientation, the position of the end-effector actually depends on the first four joints as well as the orientation is the last three joints. From Figure2 we can see that θ₂ and α have the same numerical value exclude that θ₂ has direction and it is the same with θ₄ and γ. According to the geometric mapping, we can calculate θ₂ and θ₄ as follows:

$${\theta }_2=\pm \frac{\overrightarrow{\mathrm{SE}}\times \overrightarrow{S{X}_0}}{|\overrightarrow{\mathrm{SE}}\times \overrightarrow{S{X}_0|}},{\theta }_4=\pm \arccos \frac{\overrightarrow{\mathrm{SE}}\times \overrightarrow{\mathrm{EW}}}{|\overrightarrow{\mathrm{SE}}\times \overrightarrow{\mathrm{EW}}|}$$

(3)

θ ₂ has two solutions, so θ₁ has two corresponding solutions. θ₁ can be calculated by the projection of $\overrightarrow{\mathrm{SE}}$ in the plane x₀oy₀:

$${\theta }_1=\{\begin{array}{c}\arctan \frac{E_y}{E_x},\hfill \\ \multicolumn{1}{c}{\arctan \frac{E_y}{E_x}+\pi ,}\end{array}\begin{array}{c}{\theta }_2>0\hfill \\ \multicolumn{1}{c}{{\theta }_2<0}\end{array}$$

(4)

where E_y and E_x are y-axis and x-axis components of $\overrightarrow{\mathrm{SE}}$ , respectively. At the same time, θ₃ is obtained by the projection of ${\overrightarrow{\mathrm{EW}}}^2$ in the plane x₂oy₂.

$${\theta }_3=\{\begin{array}{c}\arctan \frac{{\overrightarrow{\mathrm{EW}}}_y^2}{{\overrightarrow{\mathrm{EW}}}_x^2},\hfill \\ \multicolumn{1}{c}{\arctan \frac{{\overrightarrow{\mathrm{EW}}}_y^2}{{\overrightarrow{\mathrm{EW}}}_x^2}+\pi ,}\end{array}\begin{array}{c}{\theta }_4>0\hfill \\ \multicolumn{1}{c}{{\theta }_4<0}\end{array}$$

(5)

where ${\overrightarrow{\mathrm{EW}}}_y^2$ and ${\overrightarrow{\mathrm{EW}}}_x^2$ respectively are y-axis and x-axis components.

The last three joint rotations of the manipulator can be defined as follows: rotating around the z-axis of coordinate 4 (θ₅), then rotating around the y-axis (θ₆), and finally rotating around the z-axis (θ₇), the total rotation matrix can be obtained because that the rotation matrix $R_4^0$ can be computed by forward kinematics and the rotation matrix of end-effector ( $R_7^0$ ) is given.

$$R_7^4={\left(R_4^0\right)}^{-1}{R}_7^0=\left[\begin{array}{ccc}{r}_{11}\hfill & r_{12}\hfill & r_{13}\hfill \\ \multicolumn{1}{c}{r_{21}}& r_{22}\hfill & r_{23}\hfill \\ \multicolumn{1}{c}{r_{31}}& r_{32}\hfill & r_{33}\hfill \end{array}\right]=\left[\begin{array}{ccc}*\hfill & *\hfill & c_5{s}_6\hfill \\ \multicolumn{1}{c}{*}& *\hfill & s_5{s}_6\hfill \\ \multicolumn{1}{c}{-s_6{c}_7}& s_6{s}_7\hfill & c_6\hfill \end{array}\right]$$

(6)

Where c, s and subscripts denote cosine, sine and the corresponding joint angles, respectively. Compare elements of two matrices refer to equation (6), we can obtain θ₅, θ₆, θ₇:

$$\begin{array}{c}{\theta }_5=\arctan 2(\frac{r_{23}}{s_6},\frac{r_{13}}{s_6})\hfill \\ \multicolumn{1}{c}{{\theta }_6=\arctan 2(\pm \sqrt{1-r_{33}^2},r_{33})}\\ \multicolumn{1}{c}{{\theta }_7=\arctan 2(\frac{r_{32}}{s_6},-\frac{r_{31}}{s_6})}\end{array}$$

(7)

Dynamics of the 7-DOF robot manipulator

In this article, a series 7-DOF flexible joint robotic arm is modeled in the joint space:

$$\begin{array}{l}{M}_q\left(q\right)\stackrel{··}{q}+C_q\left(q,\stackrel{.}{q}\right)\stackrel{.}{q}+G_q\left(q\right)+F\left(\stackrel{.}{q}\right)=\tau -J^T\left(q\right)f_{ext}\\ B\stackrel{··}{\theta }+K\left(\theta -q\right)={\tau }_{motor}\\ \tau =K\left(\theta -q\right)\end{array}$$

(8.1) (8.2) (8.3)

Where q= [q₁; q₂; . . .; q_n] ^T is the joint position vector and n is the number of joints. In equation (8.1), on the left-hand side, the well-known rigid body dynamics with $M_q\left(q\right),C_q(q,\stackrel{·}{q})\stackrel{·}{q},G_q\left(q\right)$ and $F\left(\stackrel{·}{q}\right)$ denote the inertia matrix, centripetal and Coriolis vector, gravity vector and friction term. On the right-hand side has the torque from the rigid body system, denoted also by τ. $J\left(q\right)$ is the Jacobian matrix and $f_{\mathrm{ext}}$ denotes the external force acting on the robot. According to Lagrange method

$$L=K-P;\frac{d}{\mathrm{dt}}\frac{\partial L}{\partial q}-\frac{\partial L}{\partial q}=\mathrm{\tau }$$

(9)

Where K, P denote the kinetic energy, potential energy of the system, respectively. Thus, we can derive

$$\begin{array}{c}{M}_q\left(q\right)=\sum _{i=1}^n[m_i{J}_v^{i^T}{J}_v^i+J_{\omega }^{i^T}{R}_i{I}_i{R}_i^T{J}_{\omega }^i]\hfill \\ \multicolumn{1}{c}{C_q(q,\stackrel{·}{q})=\sum _{i=1}^n\sum _{j=1}^n{c}_{\mathrm{ijk}}{\stackrel{·}{q}}_j}\\ \multicolumn{1}{c}{G_q\left(q\right)=\frac{\partial P}{\partial q}}\end{array}$$

(10)

Where $J_v^i,J_{\omega }^i,R_i,m_i$ , and $c_{\mathrm{ijk}}$ represent position Jacobian matrix, orientation Jacobian matrix, rotation matrix of each joint coordinate frame relative to the base coordinate system, joint mass and Christoffel symbol, respectively.

The last two equations (8.2) and (8.3) represent the torque model of flexible joint manipulator and the structural scheme is shown as Figure 3. Figure 3(a) presents the block diagram of the flexible joint manipulator and the simplified model representation is shown in Figure 3(b). The motors and the links of the 7-DOF robotic arm are connected by springs. B, K, and ${\tau }_{\mathrm{motor}}$ respectively are the positive definite motor inertia matrix, stiffness matrix and the motor input torque.

Figure 3.

The schematic diagram of the 7-DOF flexible manipulator. (a) Block diagram of the flexible joint manipulator. (b) Simplified model representation of the flexible joint manipulator.

We adopt the inverse kinematics solution to get the joint position by the commanded trajectory x_c (see Figure 4). Combine the linkage dynamics equation (8.1) to obtain the torque τ, then use the equations (8.2) and (8.3), taking K as given, the motor rotating angle θ and torque ${\tau }_{\mathrm{motor}}$ can be obtained. Therefore, under the drive of the motor, we can get the current actual trajectory information by the forward kinematics.

Figure 4.

Block diagram representation of the proposed control scheme.

To track a desired reference trajectory in Cartesian coordinate system, the first dynamics refer to equation (8.1) are transformed and expressed as in Cartesian space ²⁶ :

$$M_x\left(q\right)\stackrel{··}{x}+C_x(q,\stackrel{·}{q})\stackrel{·}{x}+G_x\left(q\right)+f_{\mathrm{fric}}+f_{\mathrm{ext}}=f$$

(11)

Where x denotes the position/orientation vector of the end-effector. M_x, C_x, G_x respectively are corresponding to inertial matrix, coriolis and centrifugal matrix, gravitational force vector in Cartesian coordinates. The relationship between x and q is given by

$$\stackrel{·}{x}=J\left(q\right)\stackrel{·}{q},\stackrel{··}{x}=J\left(q\right)\stackrel{··}{q}+\stackrel{·}{J}\left(q\right)\stackrel{·}{q}$$

(12)

The relations between the coefficient matrices of equations (8) and (11) for 7-DOF redundant manipulator with known Jacobian matrix $J\left(q\right)$ are:

$$\begin{array}{c}{M}_x\left(q\right)={\left(J^{+}\right)}^{T}M{J}^{+},G_x\left(q\right)={\left(J^{+}\right)}^{T}G\hfill \\ \multicolumn{1}{c}{C_x\left(q\right)={\left(J^{+}\right)}^T[C-M{J}^{+}\stackrel{·}{J}]J^{+},f={\left(J^{+}\right)}^T\tau }\end{array}$$

(13)

For the convenience, in the subsequent design, M, C, G denote $M_q\left(q\right),C_q(q,\stackrel{·}{q}),G_q\left(q\right)$ , respectively. Where $J^{+}$ denotes the pseudo inverse Jacobian matrix while the Jacobian $J$ is derived from the position and orientation vectors of the end-effector.

Controller design

The compliance is used in the robot and human/environment safety interactions while the redundant robot is under the external force. Figure 4 shows the control scheme of 7-DOF redundant robot in physical human-robot interaction. The external force is consist of virtual force based on interactive intention and human contact force. The error between the actual trajectory x and the commanded trajectory x_c may be caused under the external force. Thus, we use the sliding mode control technology to carry out the tracking trajectory optimization. Meanwhile, the force compliance in real-time is realized by the variable impedance parameters tuning combined with the fuzzy rules as to obtain the command trajectory. Then the inverse kinematics based on redundant angle is derived, the actual trajectory can be calculated by between kinematics and dynamic of the flexible-joint manipulator.

Virtual force estimation of interactive intention

Considering that human improperly operation or other special responding easily cause singular configuration which is reflected in the Jacobian matrix J(q), whereas the Jacobian matrix J(q) and its pseudo inverse are used to map velocities, forces/torques and robot dynamics from the configuration space to the task space. Thus, we put an additional virtual force which is imposed on the original system to ensure the desired trajectory, as well as to avoid unacceptable behavior at singular configurations on account of the ill-conditioned Jacobian. The scheme of the virtual force estimation based on interactive intention is shown in Figure 5.

Figure 5.

Virtual force estimation of interactive intention.

According to the state information of the robot’s velocity and the interaction force in real time, when the force and velocity are in the same direction, the intention is determined as acceleration, otherwise the intention is determined as deceleration. The greater the force is, the stronger the intention is. Let $w\left(q\right)=\sqrt{\left|J{J}^T\right|}$ , namely Jacobian maximum eigenvalue, represents the manipulability performance index of the 7-DOF manipulator which is used to determine the performance constraints. If the robot tends to accelerate, when the normalized position increment ( $\frac{\mathrm{\Delta }x}{x}$ ) is greater than the normalized joint angle increment ( $\frac{\mathrm{\Delta }q}{q}$ ) at the adjacent moment, we can derive the $\mathrm{\Delta }w\left(q\right)>0$ due to $\mathrm{\Delta }J>0$ refer to equation (12). That is to say, at singular or near singular configurations, some of these mappings can be unfeasible because the Jacobian is ill conditioned. Similarly, the other cases in the Figure 5 can be obtained. In this article, we define the performance measure B(q), which is calculated using equation (16), to prevent the robot from reaching such configuration, otherwise B(q) = 0.

Considering an interaction at the end-effector, the virtual force model equivalent to the nonlinear spring system can be integrated into the desired dynamic behavior refer to equation (17). The virtual force f_v can be achieved as follows:

$$f_v=z\left(w\right)·B\left(q\right)$$

(14)

$$z\left(w\right)=\{\begin{array}{c}\lambda (\frac{1}{w\left(q\right)-w_{\mathrm{cr}}}-\frac{1}{w_{\mathrm{th}}-w_{\mathrm{cr}}}),\hfill \\ \multicolumn{1}{c}{0,}\end{array}\begin{array}{c}w\left(q\right)\le w_{\mathrm{th}}\hfill \\ \multicolumn{1}{c}{\mathrm{else}}\end{array}$$

(15)

$$B\left(q\right)=\{\begin{array}{c}0,\hfill \\ \multicolumn{1}{c}{\max \mathrm{\Delta }w·\mathrm{sign}\left(v\right),}\end{array}\begin{array}{c}\mathrm{\Delta }w\left(q\right)\le 0\hfill \\ \multicolumn{1}{c}{\mathrm{else}}\end{array}$$

(16)

Where z(w) is equivalent to deformation of the spring and B(q) which comes from Figure 5 is the spring constant vector. The non-negative scalar z(w) is activated when the index w(q) crosses the threshold w_th. Moreover, z(w) increases asymptotically if the index approaches the lowest critical value w_cr. Thus, the critical value w_cr is selected to guarantee that the robot never reaches a singularity and that the Jacobian matrix has full rank for calculating the inverse differential kinematics. For a low positive gain λ, the virtual force f_v is realized by the operator as a very compliant spring when w(q) ≤w_th.

Variable impedance parameters tuning

In real robotic system, the object usually suffers from the external force when moving in a task space. Therefore, the compliance control is widely used in the robot and human/environment safety interaction. In this article, the external force is consist of virtual force and human contact force. The impedance dynamics between the external force f_ext and position error e_x are given by:

$$m{\stackrel{··}{e}}_x+b{\stackrel{·}{e}}_x+k{e}_x=f_{\mathrm{ext}}$$

(17)

Where m, b, and k respectively are impedance coefficients of the mass, damping and stiffness. According to equation (17), we define that position error $e_x=x_c-x$ , where x_c is the commanded trajectory of contact position, x is the actual motion position of the end-effector (see Figure 6).

Figure 6.

The impedance dynamics structure.

As well known, the impedance model depends on the choice of the impedance coefficients. Considering the stability and reliability of the system, the three parameters should be defined by positive diagonal matrices, given that all poles of the second-order differential equation must be on the left half-plane. Moreover, the coefficients of the impedance controller need to be adjusted in real time. To be simplified, we chose a constant for the mass coefficient m. To achieve compliance, the impedance parameter b is designed based on fuzzy rules by judging the intention of acceleration or deceleration for the next moment (see Figure 5). Thus, the velocity and human contact force are used as the input variables of the fuzzy controller. According to the fuzzy inference system (FIS), the language values of input and output variables are defined as: {NB, NS, Z, PS, PB}, {S, SM, M, BM, B} refer to. ²⁷ The purpose of this definition is to facilitate the comparison between the control method of the increased virtual force based on interactive intention in this article and that of the single variable impedance parameters. Five triangular membership functions are used to represent the input and output variables. The control surface of is shown in Figure 7.

Figure 7.

Control surface of b.

In view of the great influence of stiffness parameter k on the system, we can obtain the parameter k by the transfer function of the equation (17) compared with a typical second-order system:

$$G_1\left(s\right)=\frac{1}{m{s}^2+\mathrm{bs}+k}=\frac{{\omega }_n^2}{s^2+2\xi {\omega }_{n}s+{\omega }_n^2}$$

(18)

Then the stiffness parameter k is obtained

$$k=\frac{b^2}{4m{\xi }^2}$$

(19)

ξ denotes the damping ratio which reflects the convergence of the system. In order to prevent the system overshooting, we choose ξ ≥ 1. Then, the stiffness parameter k is obtained.

Impedance sliding mode controller design for trajectory tracking

In the nonlinear controller type, sliding mode control technique is very useful for the controller designing of the system with matching perturbation uncertainty. In this article, the application of a impedance sliding mode-based approach to motion control of the 7-DOF redundant robot is discussed. As is well known, the control target is to keep the actual trajectory x consistent with the desired trajectory x_d. According to equation (17), the impedance error e_x should be zero if f_ext equals zero. Substituting $e_x=x_c-x_d$ into equation (17), yields

$$\begin{array}{c}m({\stackrel{··}{x}}_c-{\stackrel{··}{x}}_d)+b({\stackrel{·}{x}}_c-{\stackrel{·}{x}}_d)+k(x_c-x_d)=f_{\mathrm{ext}}\hfill \\ \multicolumn{1}{c}{x_d\left(0\right)=x_c\left(0\right),{\stackrel{·}{x}}_d\left(0\right)={\stackrel{·}{x}}_c\left(0\right)}\end{array}$$

(20)

When the trajectory of the end-effector does not achieve the ideal tracking case, it means that the small position deviation will generate a great force or torque. Consider of the insensitivity of dynamic compensation system to disturbance and uncertainty in case of sliding mode control, the open loop dynamic system refers to equation (11), if there is no controller for implementing trajectory tracking, can be written in Cartesian coordinates as

$$M_x\left(q\right)\stackrel{··}{x}+C_x(q,\stackrel{·}{q})\stackrel{·}{x}+G_x\left(q\right)+f_{\mathrm{ext}}+f_{\mathrm{fric}}+\mathrm{\Delta }(q,\stackrel{·}{q},\stackrel{··}{q})=f$$

(21)

Where $\mathrm{\Delta }(q,\stackrel{·}{q},\stackrel{··}{q})$ denotes the unknown perturbation terms, M_x, C_x, G_x are given by equation (11). For the sliding mode controller, the following manifolds are defined:

$$\{\begin{array}{c}\begin{array}{c}\multicolumn{1}{c}{e\left(t\right)=x_d\left(t\right)-x\left(t\right)}\\ \multicolumn{1}{c}{{\stackrel{·}{x}}_r\left(t\right)={\stackrel{·}{x}}_d\left(t\right)+\mathrm{ce}\left(t\right)}\end{array}\hfill \\ s\left(t\right)={\stackrel{·}{x}}_r\left(t\right)-\stackrel{·}{x}\left(t\right)=\stackrel{·}{e}\left(t\right)+\mathrm{ce}\left(t\right)\hfill \\ \stackrel{·}{s}\left(t\right)=-\mathrm{Ks}-\eta \tanh \frac{s}{\epsilon }\hfill \end{array}$$

(22.1) (22.2) (22.3) (22.4)

Where $e\left(t\right),s\left(t\right),\stackrel{·}{s}\left(t\right)$ represent the position tracking error, sliding surface, approach law, respectively. $c$ is positive constant which determines the convergence speed of the sliding surface. Both K and $\eta$ are greater than zero, $\mathrm{\Delta }(q,\stackrel{·}{q},\stackrel{··}{q})\le \eta$ . When the model uncertainty or interference is large, it is necessary to set the gain of the switching term caused by sign function to be large. However, large gain will cause obvious chattering. Here the continuous hyperbolic tangent function is designed to reduce the chattering phenomenon caused by switch function. The hyperbolic tangent function has the following properties ²⁸ :

• Property 1. For any real value x, it is deduced by,

$$\begin{array}{cc}\mathrm{xtanh}\left(\frac{x}{\epsilon }\right)=\left|\mathrm{xtanh}\left(\frac{x}{\epsilon }\right)\right|=\left|x\right|\left|\tanh \left(\frac{x}{\epsilon }\right)\right|\ge 0;\hfill & (\epsilon >0)\hfill \end{array}$$

(23)

• Property 2. For any real value x, there is a inequality expression

$$\begin{array}{cc}0\le \left|x\right|-\mathrm{xtanh}\left(\frac{x}{\epsilon }\right)\le \mu \epsilon ;\hfill & (\mu =0.2785,\epsilon >0)\hfill \end{array}$$

(24)

• Property 3. For $f,V\in [0,+\infty )\in R,$ if $\stackrel{·}{V}\le -\partial V+f$ , then

$$\begin{array}{cc}V\left(t\right)\le e^{-\alpha (t-t_0)}V\left(t_0\right)+\underset{t_0}{\overset{t}{\int }}{e}^{-\alpha (t-\tau )}f\left(\tau \right)d\tau ;\hfill & (t\ge t_0\ge 0,\partial \in R)\hfill \end{array}$$

(25)

From equations (22.1) and (22.3), we can get:

$$\stackrel{·}{x}=-s+{\stackrel{·}{x}}_d+\mathrm{ce}$$

(26)

Then, by using (21) and (26), we have

$$M_x\stackrel{·}{s}=M_x({\stackrel{··}{x}}_d+c\stackrel{·}{e})+C_x({\stackrel{··}{x}}_d+\mathrm{ce})+G_x\left(q\right)+f_{\mathrm{ext}}+f_{\mathrm{fric}}+\mathrm{\Delta }(q,\stackrel{·}{q},\stackrel{··}{q})-C_{x}s-f$$

(27)

Substituting equations (22.2) and (22.4) into (27), we design the control law:

$$f=M_x\left(q\right){\stackrel{··}{x}}_r+C_x(q,\stackrel{·}{q}){\stackrel{·}{x}}_r+G_x\left(q\right)+f_{\mathrm{ext}}+f_{\mathrm{fric}}+\mathrm{Ks}+\eta \tanh \frac{s}{\epsilon };K>0,\mathrm{\epsilon }>0$$

(28)

Where ${\stackrel{.}{x}}_r$ is defined by equation (22.2). We can adjust the gradient of the hyperbolic tangent function through $\epsilon$ which is more than zero refer to (28).

By using (21), (22.3), and (28) the closed-loop dynamic system during sliding mode controller can be calculated as

$$M_x\left(q\right)\stackrel{·}{s}+C_x(q,\stackrel{·}{q})s+\mathrm{Ks}+\eta \tanh \frac{s}{\epsilon }-\mathrm{\Delta }(q,\stackrel{·}{q},\stackrel{··}{q})=0$$

(29)

Equation (29) expresses the regulation of controller parameter $K,c,\eta ,\epsilon$ to realize the trajectory tracking and the stability proof is provided in the next section.

Lyapunov stability proof

To ensure that the system converges to the sliding surface s = 0 in the human-robot interaction, we use a positive definite Lyapunov candidate functions

$$V=\frac{1}{2}{s}^T{M}_x\left(q\right)s$$

(30)

Taking the time derivative of V yields:

$$\stackrel{·}{V}=s^T{M}_x\left(q\right)\stackrel{·}{s}+\frac{1}{2}{s}^T{\stackrel{·}{M}}_x\left(q\right)s$$

(31)

The inertia matrix M_x is positive definite and ${\stackrel{·}{M}}_x\left(q\right)-2C_x(q,\stackrel{·}{q})$ is skew symmetric. Using equation (29) into (31), we get

$$\stackrel{·}{V}=s^T{M}_x\left(q\right)\stackrel{·}{s}+s^T{C}_x\left(q\right)s=s^T[M_x\left(q\right)\stackrel{·}{s}+C_x\left(q\right)s]=s^T[-\mathrm{Ks}-\eta \tanh \frac{s}{\epsilon }+\mathrm{\Delta }(q,\stackrel{·}{q},\stackrel{··}{q})]$$

(32)

Using Property 1 and Property 2, according to equation (31) and equation (32), the above equation can be rewritten as:

$$\begin{array}{c}\multicolumn{1}{c}{\stackrel{·}{V}\le -s^T\mathrm{Ks}+\eta \mathrm{\mu }\mathrm{\epsilon }\le -{\mathrm{\lambda }}_{\min }\left(K\right)s^{T}s+\eta \mathrm{\mu }\mathrm{\epsilon }}\\ =-2\frac{{\mathrm{\lambda }}_{\min }\left(K\right)}{{\mathrm{\lambda }}_{\max }\left(M_x\right)}·\frac{1}{2}{\mathrm{\lambda }}_{\max }\left(M_x\right)s^{T}s+\eta \mathrm{\mu }\mathrm{\epsilon }\le -2\mathrm{\lambda }V+b\hfill \end{array}$$

(33)

Where $\lambda ={\lambda }_{\min }\left(K\right)/{\lambda }_{\max }\left(M_x\right)$ which ${\lambda }_{\max }\left(M_x\right),{\lambda }_{\min }\left(K\right)$ respectively are the max and minimum eigenvalue of $M_x,K$ , and $b=\eta \mu \epsilon$ .

Using Property 3, equation (33) is derived as:

$$V\left(t\right)\le e^{-2\lambda (t-t_0)}V\left(t_0\right)+b{e}^{-2\lambda t}\underset{t_0}{\overset{t}{\int }}{e}^{2\lambda \tau }d\tau =e^{-2\lambda (t-t_0)}V\left(t_0\right)+\frac{b}{2\lambda }(1-e^{-2\lambda (t-t_0)})\le \frac{b}{2\lambda }$$

(34)

Thus, equation (33) is replaced in equation (34) to obtain:

$$\lim \stackrel{·}{V}\left(t\right)\le -2\lambda V+b\le -2\lambda \frac{b}{2\lambda }+b=0$$

(35)

By choosing c to satisfy the stability of the sliding surface s, we ensure that $\stackrel{·}{V}$ is negative definite. Then applying the Lyapunov direct method to non-autonomous systems, the global unified asymptotic stability of s = 0 is obtained. Therefore, it is proved that the approach law composed of hyperbolic tangent can guarantee the sliding surface s = 0 and the stability of the system.

Numerical simulation and results analysis

To verify the availability of the controller proposed above, the application of the satellite component assembly simulation is mentioned. Figure 8(a) displays the serial robot manipulator in real time, and Figure 8(b) shows simulation structure used Simulink-Matlab and Robot Toolbox. The detailed parameters of the manipulator are presented in Tables 1 and 2.

Figure 8.

Simulation scenario. (a) Serial robot manipulator used Robot Toolbox in real time. (b) Simulation structure used Simulink-Matlab.

Table 1.

D-H parameters of the 7-DOF redundant manipulator.

Frame	Link	αi-1(°)	ai-1(m)	di(m)	θi(°)
0–1	1	0	0	0	θ1
1–2	2	−90	0	0	θ2
2–3	3	90	0	0.4404	θ3
3–4	4	−90	0	0	θ4
4–5	5	90	0	0.436	θ5
5–6	6	−90	0	0	θ6
6–7	7	90	0	0	θ7

Table 2.

Parameters of the robot.

Parameter	Description	Value
m1 ~ m7	Masses of links	[0.897,2.4056,1.7153,1.5608,1.0399,0.60356,0.60965] kg
[I1xx I1yy I1zz]	Inertia of link 1	[0.0016897,0.0037456,0.0031901] kg·m2
[I2xx I2yy I2zz]	Inertia of link 2	[0.017898,0.003637,0.017746] kg·m2
[I3xx I3yy I3zz]	Inertia of link 3	[0.009861,0.0031381,0.010596] kg·m2
[I4xx I4yy I4zz]	Inertia of link 4	[0.010574,0.010543,0.0015508] kg·m2
[I5xx I5yy I5zz]	Inertia of link 5	[0.0051194,0.0053812,0.001164] kg·m2
[I6xx I6yy I6zz]	Inertia of link 6	[0.001229,0.00073682,0.00086092] kg·m2
[I7xx I7yy I7zz]	Inertia of link 7	[0.00086225,0.00083692,0.00061048] kg·m2

The simulation procedure is that carried by human and robot, the object moves along an accelerating—uniform—decelerating trajectory x_d under teaching. The trajectory command by the user is given by

$$x_d=\{\begin{array}{cc}0.5285+\frac{1}{2}a{t}^2;\hfill & t\le 2\hfill \\ \multicolumn{1}{c}{0.5285+\mathrm{at};}& t\le 6\hfill \\ \multicolumn{1}{c}{0.5285-\frac{1}{2}a{t}^2+8\mathrm{at}-24a;}& t>6\hfill \end{array}$$

(36)

The starting position of the end-effector is x_d (0) = x_c (0) = [0.5285, −0.5285, 0.3114]m, a = 0.005 $,{\stackrel{·}{x}}_d\left(0\right)={\stackrel{·}{x}}_c\left(0\right)=[0,0,0]$ . The parameters of the sliding mode controller are selected based on the principles that RMSE(Root Mean Square Error) is minimal, that is K = $c$ = diag[50,50,50], $\epsilon =0.2,\eta =0.5$ . The mass coefficient of the impedance model m = diag [20, 20, 20], damping ratio ξ=1. The damping coefficient b is designed based on fuzzy rules in Figure 7, then the stiffness parameter k is obtained by equation (19). To make the simulation approach to actual situation, the joint friction parameters are selected by coulomb friction $f_{\mathrm{fric}}=0.01\mathrm{sgn}\left(\stackrel{·}{q}\right)$ . The simulation results are presented in Figure 9.

Figure 9.

Simulation results of 7-DOF redundant robot. (a) Seven joint angles when the redundant angle is fixed value (β = 45°). (b) Seven joint angles when the redundant angle based on virtual force, (c) joint angle velocities with different damping parameters, the inset one is the case of damping parameter b = 15 Ns/m and other one is performance optimization based on velocity constraint, joint velocities are close to zero in the end. (d) variable impedance parameters k and b versus time. (e) external forces versus time, the thin dashed lines represent the case of fixed impedance parameters, the solid lines mean the stiffness parameter k is adjusted by fuzzy rule, the biggest external force is −0.99486 N in z-axis, the bold dashed lines denote the case of increased virtual force, the biggest external force is −0.56348 N in z-axis. (f) the end point motions of three cases in constant b, variable impedance parameters used fuzzy inference rule, and increased virtual force.

Figure 9(a) and (b) show seven joint angles of the robot. For redundant robot, there exist infinite inverse kinematics solutions, redundant angle can be computed presented in Section 2. If redundant angle is fixed value (β = 45°) in the continuous trajectory tracking, from the Figure 9(a), joint q₃ and q₇ exceed the joint limit and a mutation occurs at ± 180°. Figure 9(b) represents the results for redundant angle based on the increase virtual force. Joint q₃ and q₇ are completely within the limit in this setting and avoids joint overrun.

As shown in Figure 9(c), there are two graphs. As well known, it is reasonable that joint velocity change slowly in the process of teaching. The inset one is the case of constant damping parameter b = 15 Ns/m, it is obvious that the joint velocities are not equal to zero in the end of time, so this can easily lead to collisions. The other one is performance optimization based on interactive intention presented in this paper, the joint velocities are close to zero at last. The inset graph of Figure 8(d) represents corresponding damping parameter b is variable according to the fuzzy inference rule based on the estimation of interactive intention, thus, the stiffness parameter k (Figure 9(d)) is obtained by equation (19).

The forces by human versus time correspond to three cases are drawn in Figure 9(e). The thin dashed lines represent the case of fixed impedance parameters, which would lead to the instability of the system and inaccurate tracking (see Figure 9(f)). In order to improve the cooperation ability of the robot in the interaction process, the robot must have ability to adjust its own characteristics by variable impedance parameters and virtual force constraint. It is seen that the human force is augmented as the stiffness parameter k (see Figure 9(d)) increased, which can result in deviations of trajectory tracking in Cartesian space such as y and z orientation. This, in turn, will generate the contact forces in y and z axis according to the impedance model. Thus, the stiffness parameter k is adjusted by fuzzy rules to reduce the interaction force and the accuracy of the trajectory tracking is better compared to that fix b. The bold dashed lines denote the case of increased virtual force in this paper, the biggest external force (–0.56348 N) in z-axis, is 43% less than variable impedance parameters (corresponding −0.99486 N) marked solid lines.

It is noted from Figure 9(f) that in three cases, that is constant b, variable impedance parameters used fuzzy rules showed in Figure 9(d), increased virtual force mentioned in this paper, the end-effector motions to move along the desired trajectory are plotted in Cartesian space. We can see that the tracking performance in virtual force constraint in this paper, which is better compare to the other two cases, is satisfactory and the proposed controller has the ability to guarantee the effectiveness of tracking. In addition, tracking accuracy is achieved by 97.8% when the endpoint of trajectory is 0.5885 m compared to 0.576 m in the ideal position at this time.

By analyzing the above simulation results, it is known that if the desired trajectory is planned in the space, meanwhile the appropriate redundant angle under the virtual force is chosen to avoid the mutation of the joint angle, we can achieve better tracking and the lower contact force. Consequently, the proposed controller combined with performance optimization based on interactive intention can make the system output remain in the corresponding predefined constraint region.

Conclusion

In summary, a control method based on variable impedance sliding mode theory is proposed to solve the problem of trajectory tracking accuracy and force compliance control in the same direction in physical human-robot interaction. The control system was established by the kinematic decoupling method utilized redundant angle for inverse kinematics solution as well as by the impedance sliding mode control law on the basis of the virtual force. It shows that the robot has an improved performance on trajectory tracking and force compliance. Meanwhile, to alleviate the chattering problem caused by the sign function in existing controllers, we replace the sign function with the hyperbolic tangent function to accomplish the soft switch control and the asymptotic tracking is proven using Lyapunov stability theorem.

Simulations with Matlab software exhibit an improved performance, namely, tracking accuracy is achieved by 97.8% and the external force is 43% less than variable impedance parameters. It is proved that the proposed method not only can obtain the continuous joint angle that satisfies the trajectory tracking for redundant robot but also render the compliant behavior in the human-robot interactions. Consequently, the controller is potential widely used for physical human-robot interactions, typically like teaching where the impedance of the robot’s end-effector plays a key role. Since the tracking trajectory is the basic and important study on manipulator control, it reflects the control performance of the manipulator. However, it is noted that the effect from load on the end-effector of manipulator on tracking performance should not be ignored. In the future, based on the tracking trajectory, the force compliance control with load should be considered. Also, practical experimental verification on simulation effect will be necessary for the near future.