Adaptive Compliance Control of Flexible Link Manipulator in Unknown Environment

Abstract

The present work proposes an Adaptive Compliant Control scheme based on a closed-form output-redefined and perturbed dynamic model of a Single-link Flexible Manipulator (SLFM) in Unknown Environment. The control scheme is composed of inner and outer controllers. The inner control is designed based on Two-Time Scale Adaptive Robust Control (TTARC) to ensure fast and precise motion control, while the outer control is based on the impedance dynamics aiming to offer a desired compliant behavior in constrained motion. External force is estimated based on the extended Kalman Filter (EKF). The stability of the closed-loop system is verified through Lyapunov theory. The effectiveness of the overall control scheme is verified through simulation.

This work was supported by the National Natural Science Foundation of China (Nos 51675471 and 51375432) and Science Fund for Creative Research Groups of National Natural Science Foundation of China (No. 51821093), and also supported by the Fundamental Research Funds for the Central Universities (2019QNA4002).

1 Introduction

Collaborative robots could meet the human’s demand further by extending their application fields since traditional robots have limitations on handling complex products, being suitable for unstructured environments, and guaranteeing human safety. The expansion of collaborative robots could allow industrial robotics to be more attractive, with an estimation of 4 millions in 2022 [4]. Collaborative robots are characterized by their safety and flexibility, while still facing problems to be solved, such as accurate dynamic modeling and precise motion control. Most of collaborative robots designed in literature are usually equipped with rigid links, which cause hard collision problem during interactive operation with humans [2]. Considering the core issue of safety and compliance, manipulators with flexible joints are recently presented [6]. Besides, if the manipulator link has flexible structure or is operating at high speed, the manipulator will exhibit flexible characteristics rather than rigid behavior. Therefore, the high performance control of flexible link manipulator (FLM) needs to be further developed for wide application of collaborative robots by handling the following well-known addressed problems: (i) link vibration, (ii) system nonlinearity, (iii) inevitable uncertainties, and (iv) instability in the internal dynamics.

Besides passive [10] and active compliant controls [16], Impedance control has been verified as best alternative way to achieve compliance behavior of the robot through regulation of its inertia, stiffness, and damping [13]. Because of modeling uncertainties, both unknown disturbances and environments, robust [5], adaptive [8, 11], and learning technique [14] controls have been investigated and used to enforce the basic impedance control of robot manipulators. Most of the above different impedance control techniques are widely implemented on rigid, parallel rigid, and flexible joint robots. However, research on the implementation of compliant control on Cobots with flexible link are limited to adaptive impedance [1], dynamic hybrid position/force [9], backstepping approach [7] controls, and composite impedance control based singular perturbation (SP) theory [3]. Unfortunately, most of control designs are based on complex dynamics equations with hard task either to measure the vibration states or to retrieve them through accurate sensors.

The current research aims firstly to develop an explicit dynamics model for a SLFM system with high order vibration modes based on the combined Assumed Modal and Lagrange formulation methods. Then, a redefined closed-form dynamic model with both parametric and nonlinear uncertainties under necessary conditions for stability of the internal dynamics is proposed. Secondly, the control scheme comprises an inner control designed based on singular-perturbed and two-time scale sub-system dynamics. The outer control is designed based on impedance dynamics to enable the robot with compliance characteristic for the safety of both robot and unknown environment, during constrained motion. The unknown environment parameters includes contact force, stiffness and equilibrium position. The contact force is estimated by a designed force observer based on the extended Kalman filter (EKF) algorithm.

2 Dynamic of FLM

The SLFM in Fig. 1 consists of thin flexible link regarded as an Euler-Bernoulli beam of length l and a deflection w derived from the assumed modal method.

$$\begin{aligned} w(x,t) = \sum _{i=0}^{\infty }\phi _i(x)q_i(t) \end{aligned}$$

(1)

where $\phi _i(x)$ and $q_i(t)$ are respectively link i-th mode shape function and its associated time generalized coordinate. The link, at one end (i.e., the hub), is driven by a rotary actuator and the other end attached to a payload attached while interacting unknown environment. In addition, the following assumptions are considered: (i) the link material property follows Hook’s law; (ii) the link deflection is small; (iii) no longitudinal stiffness and length’s variation are considered in the link; (iv) the environments is compliant and described by a massless spring.

Fig. 1.

Schematics of a rotary SLFM

By only selecting the first vibration mode while considering higher-order vibration modes as system uncertainties, the FLM dynamic model with uncertainties is given by [17]

$$\begin{aligned} {\left\{ \begin{array}{ll} a_0 \ddot{\theta } + a_1 \ddot{q}_1 + (\kappa _2+b_v) \dot{\theta } + A_f \tanh (\lambda _v \dot{\theta }) + \varDelta _n = \kappa _1 u - \tau _{\theta ,e}\\ a_1\ddot{\theta } + \ddot{q}_1 + 2\xi _1\omega _1\dot{q}_1 + \omega _1^2 q_1 = - \tau _{q,e} \\ \end{array}\right. } \end{aligned}$$

(2)

where $a_0 = J_h +ml^2 + \rho A_bl^3/3$, $a_i = \rho A_b \int _{0}^{l} x\phi _idx + ml\phi _{ie}$, $\varDelta _n = \sum _{i=1}^{\infty }\left( a_i\ddot{q}_i\right) - a_1\ddot{q}_1$, $i=1,2,...,n$. $\tau _{\theta ,e}=J_{\theta }^T(\theta ,q)f_e$ and $\tau _{q,e} = J_q^T(\theta ,q)f_e$ are joint and flexible body frame joint torques, respectively, due to contact force. $\kappa _1$ and $\kappa _2$ are dc-motor parameters, $A_f$ and $b_v$ are the unknown coefficient of Column and viscous friction torques, respectively, and $\lambda _v$ is a large positive coefficient.

According to the above assumptions and to the link deflection (1), the total tip-point angle of the link is given by $y_t(l,t) = \theta + \alpha (t)$ where $\alpha (t) = \arctan \left( \sum _{i=0}^{\infty }\bar{\phi }_{ie}q_{i}(t)\right)$, with $\bar{\phi }_{ie}=\phi _{ie}/l$, and $\phi _{ie}=\phi (l)$, in which the vibration mode of link can be expressed as

$$\begin{aligned} q_{1} = \bar{\phi }_{1e}^{-1} \alpha + \bar{\phi }_{1e}^{-1} \varDelta _{q} \end{aligned}$$

(3)

where $\varDelta _{q}$ is the approximation error of tip-end deflection of link, which is given by $\varDelta _{q} = q_{1} \bar{\phi }_{1e} - \arctan \left( \sum _{i = 1}^{\infty }\bar{\phi }_{ie}q_{i}\right)$. Substituting (3) into (2) and expressing the results in term of system parameters gives

$$\begin{aligned} {\left\{ \begin{array}{ll} \ddot{\theta } = \zeta _1 u + \zeta _2 \alpha + \zeta _3 \dot{\theta } + \zeta _4 \dot{\alpha } + \zeta _9 \tanh (\lambda _v \dot{\theta }) + \zeta _{11}f_e + \zeta _{13} \\ \ddot{\alpha } = \zeta _5 u + \zeta _6 \alpha + \zeta _7 \dot{\theta } + \zeta _8 \dot{\alpha } + \zeta _{10} \tanh (\lambda _v \dot{\theta }) + \zeta _{12}f_e + \zeta _{14} \end{array}\right. } \end{aligned}$$

(4)

where $\zeta _{1}=\frac{\kappa _{1}}{a_{0} - a_{1}^{2}}$, $\zeta _{2}=\frac{a_{1}\omega _{1}^{2}\bar{\phi }_{1e}^{-1}}{a_{0} - a_{1}^{2}}$, $\zeta _{3}= - \frac{\kappa _{2}+b_v}{a_{0} - a_{1}^{2} }$, $\zeta _{4}=\frac{2a_{1}\xi _{1}\omega _{1}\bar{\phi }_{1e}^{-1}}{a_{0} - a_{1}^{2} }$, $\zeta _{5}=-\frac{a_{1}\kappa _{1}\bar{\phi }_{1e}}{a_{0} - a_{1}^{2}}$, $\zeta _{6}=- \frac{a_{0}\omega _{1}^{2}}{a_{0} - a_{1}^{2}}$, $\zeta _{7}=\frac{a_{1}\bar{\phi }_{1e}}{a_{0} - a_{1}^{2}}(\kappa _{2}+b_v)$, $\zeta _{8}=- \frac{2a_{0}\xi _{1}\omega _{1}}{a_{0} - a_{1}^{2}}$, $\zeta _{9}=-\frac{1}{a_{0} - a_{1}^{2}}A_f$, $\zeta _{10}=\frac{a_1\bar{\phi }_{1e}}{a_{0} - a_{1}^{2}}A_f$, $\zeta _{11} = \frac{a_1J_q^T-J_{\theta }^T}{a_0-a_1^2}$, $\zeta _{12} = \frac{(a_1J_{\theta }^T-a_0J_q^T)\bar{\phi }_{1e}}{a_0-a_1^2}$, $\zeta _{13}=\frac{a_1}{a_0-a_{1}^{2}}\varDelta _{1}-\frac{1}{a_0-a_{1}^{2}}\varDelta _{2}$, $\zeta _{14}=-\frac{a_0\bar{\phi }_{1e}}{a_0-a_{1}^{2}}\varDelta _{1}+\frac{a_1\bar{\phi }_{1e}}{a_0-a_{1}^{2}}\varDelta _{2}$. and $\varDelta _{1}=\bar{\phi }_{1e}^{-1}[\ddot{\varDelta }_{q}+ 2\xi _{1}\nu _{1}\dot{\varDelta }_{q}+ \nu _{1}^{2}\varDelta _{q}]$, $\varDelta _{2}= \bar{\phi }_{1e}^{-1}{a}_{1}\ddot{\varDelta }_{q}+\varDelta _{n}$. $\zeta _{13}$ and $\zeta _{14}$ are regarded as the nonlinear uncertainty of FLM, including modeling error from neglected high-order vibration modes, unmodeled complex friction torque, approximation error of tip deflection and unknown disturbances, etc.

3 Control Design

The proposed compliance control strategy comprises two main controllers: an Impedance Dynamics-based Control and the Two-Time Scale Adaptive Robust Control (TTARC) as outer and inner controllers Fig. 2. The feedback for both controllers is realized by an output redefined function. During constrained motion, a force observer estimates the parameters of the environments. In the following, each control part is designed, as well as the stability of the closed-loop system is verified.

Fig. 2.

Compliance control scheme of SLFM

3.1 Output Redefined Model

Consider a new output redefine function $y_{\varGamma } \triangleq \theta + \varGamma \alpha$. Taking use of (4), the second derivative of $y_{\varGamma }$ helps to obtain a refined dynamics model.

$$\begin{aligned} {\left\{ \begin{array}{ll} \ddot{y}_{\varGamma } = \phi _r^T \vartheta _r + \varDelta (t) \\ \ddot{\alpha } = \phi _f^T \vartheta _f + \varDelta _{\alpha } \end{array}\right. } \end{aligned}$$

(5)

where $\vartheta _r = [\zeta _1+\varGamma \zeta _5, \zeta _2+\varGamma \zeta _6,\zeta _3+\varGamma \zeta _7,\zeta _4+\varGamma \zeta _8, \zeta _9+\varGamma \zeta _{10},\zeta _{11}+\varGamma \zeta _{12},d_n]^T$ is the vector parameters, $\phi _r=[u,\alpha ,\dot{\theta },\dot{\alpha },\tanh (\lambda _v\dot{\theta }),f_e,1]^T$ its corresponding regressor. $\vartheta _f=[\zeta _5,\zeta _6,\zeta _7,\zeta _8,\zeta _{10},\zeta _{12},\varDelta _{\alpha }]^T$ and $\phi _f=\phi _f$ are the vector parameters of the flexible dynamics and its corresponding regressor, respectively. $\varGamma >0$ and $\varGamma \in [0,\varGamma ^*]$. $\varGamma ^*$ is a critical value above which the internal dynamics becomes unstable

3.2 Two-Time Scale Adaptive Robust Control Design

The dynamics (5) naturally have a higher frequency in the dynamics associated with the vibration of flexible link than in the one associated with the movement of rigid joint, especially during the motion and/or in the case of high stiffness system. Therefore, (5) can be divided into two subsystems, slow and fast, and the control design can be reduced to slow and fast slow and fast controllers.

Slow Dynamics: actual tip-trajectory $y_{\varGamma }$ tracks a reference tip-trajectory given by $y_r$ with a tracking error $e_r \triangleq y_{\varGamma }-y_d$. Consider a sliding mode function $s_r$ such that $s_r \rightarrow 0$ occurs when $e_r \rightarrow 0$ as $t \rightarrow \infty$:

$$\begin{aligned} s_r = \dot{e}_r + k_1 e_r = \dot{y}_{\varGamma }-x_{2eq} \end{aligned}$$

(6)

with $x_{2eq} \triangleq \dot{y}_d - k_1e_r$, $k_1$ a positive value. According to (5) and (6) the time derivative of $s_r$ gives the error dynamic equation.

$$\begin{aligned} \dot{s}_r = \ddot{y}_{\varGamma } - \dot{x}_{2eq} = K_u u + \phi _{ra}^T\hat{\vartheta }_{ra} - \dot{x}_{2eq} - \phi _{r}^T\tilde{\vartheta }_{r} + \tilde{d}(t) \end{aligned}$$

(7)

where $K_u = \hat{\zeta }_1 + \varGamma \hat{\zeta }_5$, $\phi _{ra} = [\alpha ,\dot{\theta },\dot{\alpha }, \tanh (\lambda _v \dot{\theta }),f_e,I]^T$ is the regressor vector, and $\tilde{\vartheta }_r = \hat{\vartheta }_r - \tilde{\vartheta }_r$ is the system parameter estimation error, and

$\hat{\vartheta }_{ra} = [\hat{\zeta }_{2} + \varGamma \hat{\zeta }_{6}, \hat{\zeta }_{3} + \varGamma \hat{\zeta }_{7}, \hat{\zeta }_{4} + \varGamma \hat{\zeta }_{8}, \hat{\zeta }_{9} + \varGamma \hat{\zeta }_{10},\hat{\zeta }_{11} + \varGamma \hat{\zeta }_{12},d_n ]^T$ and $\tilde{d}_t(t)$ are respectively the estimated parameter vector and nonlinear uncertainty term, which are both assumed to be bounded and satisfy the following inequalities.

$$\begin{aligned} \varOmega _{\vartheta _{r}} \triangleq \{\vartheta _{r,i,\min } \le \vartheta _{r,i} \le \vartheta _{r,i,\max }\},~\text {and}~ \varOmega _{\varDelta } \triangleq | \tilde{d}_t(t)| \le \varrho _0 \end{aligned}$$

(8)

where $\vartheta _{r,i,\min }$ and $\vartheta _{r,i,\max }$ are known lower and upper bound vectors of $\vartheta _{r}$, and $\varrho _0$ is a known constant value. $\hat{\vartheta }_r$ can be estimated through the parameter adaptation law

$$\begin{aligned} \dot{\hat{\vartheta }}_{r} = \text {Proj}_{\hat{\vartheta }_{r}}\left( \varGamma _{r}\phi _{r}s_r\right) \end{aligned}$$

(9)

where $\varGamma _{r}$ is chosen as a symmetric positive definite adaptation rate matrix, and $\text {Proj}_{\bullet }$ is the projection mapping defined as

$$\begin{aligned} \text {Proj}_{\hat{\vartheta }_{r}}(\bullet ) = {\left\{ \begin{array}{ll} 0 &{} \text {if}\quad \hat{\vartheta }_{r,i} = \hat{\vartheta }_{r,i,\max } ~\text {and}~ \bullet > 0 \\ 0 &{} \text {if}\quad \hat{\vartheta }_{r,i} = \hat{\vartheta }_{r,i,\min } ~\text {and}~ \bullet < 0 \\ \bullet &{} \text {otherwise} \end{array}\right. } \end{aligned}$$

(10)

such that the following property is satisfied:

$$\begin{aligned} \begin{aligned} (i)&\quad \hat{\vartheta }_{r} \in \varOmega _{\vartheta _{r}} \triangleq \{\hat{\vartheta }_{r}:\vartheta _{r,\min } \le \hat{\vartheta }_{r} \le \vartheta _{r,\max }\},\quad \forall ~t \\ (ii)&\quad \tilde{\vartheta }_{r}^T \left[ \varGamma _{r}^{-1}\text {Proj}_{\hat{\vartheta }}\left( \varGamma _{r} \phi _{r}s_r\right) - \phi _{r}s_r\right] \le 0,\quad \forall ~t \end{aligned} \end{aligned}$$

(11)

Let the control input be synthesized as follows

$$\begin{aligned} u = u_{slow} + u_{fast} = u_{da} + u_{ds} + u_{fast} \end{aligned}$$

(12)

where $u_{da}$ is the model compensation control law including the contact force model compensation law given by $u_{da} = K_u^{-1}(-\phi _{ra}^T\hat{\vartheta }_{ra} + \dot{x}_{2eq})$, the robust control law $u_{ds} = K_u^{-1}u_s$, and the fast controller law $u_{fast} = K_u^{-1} u_f$, with $u_s$ and $u_f$ the control laws to be synthesized later.

The substitution of (12) into (7) leads to following output tracking error dynamics considered as closed-form slow subsystem dynamics:

$$\begin{aligned} \dot{s}_r = - \phi _{r}^T\tilde{\vartheta }_{r} + \tilde{d}(t) + u_s + u_f \end{aligned}$$

(13)

Fast Dynamics: Consider a new variable $\eta$ such that $\eta = \alpha /\mu ^2$ and by taking (12) into second equation in (5) gives

$$\begin{aligned} \begin{aligned} \mu ^2\ddot{\eta } = \,&\hat{\zeta }_5K_{u}^{-1}( -\phi _{rb}^T\hat{\vartheta }_{rb} + \dot{x}_{2eq} + u_s + u_f) + [\hat{\zeta }_6 - \hat{\zeta }_5K_u^{-1}(\hat{\zeta }_2 + \varGamma \hat{\zeta }_6)]\alpha \\ {}&+ \phi _{fb}^T\hat{\vartheta }_{fb} - \phi _{f}^T\tilde{\vartheta }_{f} \end{aligned} \end{aligned}$$

(14)

where $\mu$ is the singular perturbation parameter, $\phi _{rb} = [\dot{\theta },\dot{\alpha }, \tanh (\lambda _v \dot{\theta }),f_e,1]^T$, $\vartheta _{fb} = [\zeta _7,\zeta _8,\zeta _{10},\zeta _{12},\varDelta _{\alpha }]^T$, $\vartheta _{rb} = \big [{\zeta }_{3} + \varGamma {\zeta }_{7}, {\zeta }_{4} + \varGamma {\zeta }_{8}, {\zeta }_{9} + \varGamma {\zeta }_{10},{\zeta }_{11} + \varGamma {\zeta }_{12},d_n \big ]^T$, $\phi _{fb} = \phi _{rb}$, $\phi _f=\phi _r$ , and $\vartheta _{f} = [\zeta _5,\zeta _6, \zeta _7,\zeta _8,\zeta _{10},\zeta _{12},\varDelta _{\alpha }]^T$.

By definition of the following new variables $K_{cl} = \min \{\hat{\zeta }_6 - \hat{\zeta }_5K_u^{-1}(\hat{\zeta }_2 +\varGamma \hat{\zeta }_6)\}$, $\mu ^2 = |K_{cl}|^{-1}$, and $K_{clf} = \mu ^2 K_{cl}$, (14) shortly becomes

$$\begin{aligned} \mu ^2\ddot{\eta } = K_{clf}\eta + \bar{N}_1(\dot{\theta },f_e) + \varDelta _{N}(t) + N_2 u_s + N_2 u_f \end{aligned}$$

(15)

where ${N}_1(\dot{\theta },f_e) = \hat{\zeta }_5K_{u}^{-1}( -\phi _{rb}^T\hat{\vartheta }_{rb} + \dot{x}_{2eq}) + \phi _{fb}^T\hat{\vartheta }_{fb} - \phi _{f}^T\tilde{\vartheta }_{f} = \bar{N}_1(\dot{\theta },f_e) + \varDelta _{N}(t)$, with $\bar{N}_1(\dot{\theta },f_e)$ and $\varDelta _{N}(t)$ being slow and fast time-variant parts of $N_1(\dot{\theta },f_e)$ respectively.

Moreover, by letting $\mu =0$ into (15), the invariant manifold

$$\begin{aligned} \eta _s = -K_{clf}^{-1}[\bar{N}_{1s}(\dot{\theta },f_e) + N_{2}u_s] \end{aligned}$$

(16)

is designed so that for canceling out effect of force contact in the fast time-varying part of $N_1(\dot{\theta },f_e)$, the following new variables $\eta _1 \triangleq \eta - \eta _s$ and $\eta _2 \triangleq \mu ^{-1}\dot{\alpha }$ can be defined as fast subsystem state variables associated to the following fast subsystem dynamics

$$\begin{aligned} \mu \dot{\eta }_2 = K_{clf}\eta _1 + N_2 u_f + \varDelta _{N}(t) \end{aligned}$$

(17)

By introducing a new time-scale variable $\varsigma = t/\mu$, (17) can be expressed in closed-form as

$$\begin{aligned} d \bar{\eta }/d\varsigma = A_f \bar{\eta } + B_f u_f + \varDelta _f \end{aligned}$$

(18)

where $\bar{\eta } = [\eta _1,\eta _2]^T$, $A_f = \begin{bmatrix}0 &{} 1 \\ K_{clf} &{} 0\end{bmatrix}$, $B_f = [0, N_2]^T$, and $\varDelta _f = [0, \varDelta _N(t) ]^{T}$.

Control Design for Slow Subsystem: In completion of the control law (12), the robust control law of slow subsystem $u_{s}$ is designed as

$$\begin{aligned} u_{s} = -k_r~s_r -S(hsgn(s_r)) \end{aligned}$$

(19)

where $k_r$ is the feedback gain to be designed during the closed-loop analysis. The nonlinear term $S(hsgn(s_r))$ is defined such that the following properties are satisfied:

$$\begin{aligned} \begin{aligned} (i)&\quad -s_r~S(hsgn(s_r)) \le 0 \\ (ii)&\quad s_r[-S(hsgn(s_r))- (\phi _r^T \tilde{\vartheta }-\tilde{d}_t)] \le \epsilon (t) \end{aligned} \end{aligned}$$

(20)

where $\epsilon (t)$ is a bounded time-varying scalar, i.e. $0 < \epsilon (t) \le \epsilon _M$, with $\epsilon _M > 0$.

Control Design for Fast Subsystem: The control law $u_f$ aims to suppress effectively the vibration within the link and especially at the tip link. $u_f$ is synthesized as a state feedback control based on fast subsystem [15]

$$\begin{aligned} u_f = - \mu _f K_f \bar{\eta } \end{aligned}$$

(21)

where $K_f = [K_{f_1}~K_{f_2}]$ is the feedback gain to be synthesized by solving the closed-loop characteristic equation.

$$\begin{aligned} s^2 + \left( N_2K_2\right) s + (N_2K_1-K_{clf}) = s^2 - 2 p_{f_d}s + p_{f_d}^2 \end{aligned}$$

(22)

where $p_{f_d}$ is the desired pole. By identification, the feedback gains are given by $K_{f_1} = \frac{p_{f_d}^2 + K_{clf}}{N_2}$ and $K_{f_2} = \frac{- 2p_{f_d}}{N_2}$ and computed after free placement of $p_{f_d}$.

3.3 Stability of the Controller

Lyapunov Stability of the Closed-Loop Slow Subsystem: The Lyapunov candidate for closed-loop slow subsystem and its time derivative are given in (23)

$$\begin{aligned} {\left\{ \begin{array}{ll} V_1 = \dfrac{1}{2}s_r^2,~\text {with}~V_1 \ge 0~\forall ~s_r\ne 0 \\ \dot{V}_1 = s_r~\dot{s}_r \le -k_r~s_r^2 + \epsilon _M + l_{\psi } \end{array}\right. } \end{aligned}$$

(23)

where $\epsilon _M$ is defined in (20) and $l_{\psi } = ||s_r~u_f||$. The passivity condition of the closed-loop slow subsystem relies in proper design of $\epsilon _M$ and $l_{\psi }$. Expressing (23) in term of $V_1$ as in (24)

$$\begin{aligned} \dot{V}_1 + 2k_rV_1 \le \left( \epsilon _M + l_{\psi }\right) \end{aligned}$$

(24)

and multiplying both side of (24) by $e^{-2k_r\,t}$, and by finally integrating over time leads to

$$\begin{aligned} V_1(t) =e^{-2 k_r\,t} V_1(0) + \dfrac{\epsilon _M + l_{\psi }}{2k_r}\left( 1-e^{-2 k_r\,t} \right) \end{aligned}$$

(25)

For fast convergence in the transient part of (25), $k_r$ is desirable as large as possible. The comparison Lemma [12] helps to write

$$\begin{aligned} |s_r(t)|^2 \le e^{-2 k_r \,t}\,|s_r(0)|^2 + \frac{\epsilon _M + l_{\psi }}{2k_r} \left( 1-e^{-2 k_r\,t} \right) \end{aligned}$$

(26)

so one can easily shows that $s_r(\infty ) \rightarrow 0$ for large $k_1$ and $k_r$, and very small $\epsilon _M$ and $l_{\psi }$. Small $l_{\psi }$ also implies playing with $\mu _f$ in (21).

In case of $\varDelta (x,t) \ne 0$, the asymptotic output tracking error can be achieved by choosing a new Lyapunov candidate

$$\begin{aligned} V_2 = V_1 + \frac{1}{2}\tilde{\vartheta }_r^{T} \varGamma _{r}^{-1}\tilde{\vartheta }_r \end{aligned}$$

(27)

where $V_2 \ge 0$ for $s_r= 0$, and $\tilde{\vartheta }_r= 0$, and $V_2 > 0$ $\forall ~s_r \ne 0$, $\tilde{\vartheta }_r \ne 0$, since $\varGamma _{r} \ge 0$ and $V_1\ge 0$. The time derivative of (27) gives

$$\begin{aligned} \dot{V}_2 = \dot{V}_1 + \frac{\partial V_a}{\partial \tilde{\vartheta }}\dot{\hat{\vartheta }}_r \le -k_r~s_r^2 + l_{\psi } \end{aligned}$$

(28)

where $\dot{\tilde{\vartheta }}_r = \dot{\hat{\vartheta }}_r$, since $\vartheta _{r}$ is constant vector. Therefore, the passivity condition relies in proper design of $l_{\psi }$. If $l_{\psi } \rightarrow 0$ $\forall ~s_r$, then $\dot{V}_2 \le 0$. In addition, according to Barbalat’s lemma [12], $\dot{V}_2 \rightarrow 0$ while $t \rightarrow \infty$ because (i) $V_2$ is lower bounded (27), (ii) $\dot{V}_2 \le 0$, and (iii) $\dot{V}_2$ is uniformly continuous in time. Therefore, one can concludes that $s_r$ is asymptotically stable, which according to (6), implies that $e_r = \exp \left( -k_1 t\right)$, hence $e_r(\infty ) = 0$ since $k_1 > 0$.

Stability of the Closed-Loop Fast Subsystem: The closed-loop internal dynamic is obtained by substitution of fast control input (21) and fast state variables $\bar{\eta }$ into internal dynamic (15):

$$\begin{aligned} \ddot{\alpha } + 2\xi _f \omega _f \dot{\alpha } + \omega _f^2 \alpha = \mu ^2 \omega _f^2 \eta _s + \varDelta _{N} \end{aligned}$$

(29)

where $\omega _{f}^2 = \frac{1}{\mu }(\mu _fN_2K_{f_1} - \bar{K}_{clf})^{1/2}$ and $\xi _f = \frac{\mu _f N_2K_{f_2}}{2 \mu \omega _f}$, respectively frequency and damping ratio of the fast closed-loop internal dynamics, all depending on $\varGamma$, desired poles $p_{f_d}$, and system parameters $\zeta _{i}$. Because of stability analysis, let define new variables $\sigma = [\alpha ^T, \dot{\alpha }^T]^T$ such that (29) can be written as

$$\begin{aligned} \dot{\sigma } = A_{\sigma } \sigma + d(t) \end{aligned}$$

(30)

where $A_{\sigma } = \begin{bmatrix} 0 &{} 1 \\ -\omega _f^2 &{} -2\xi _f\omega _f \end{bmatrix}$, $d(t)= [0,~ N_3]^T$, $N_3 = K_a \eta _s + \varDelta _s(t)$, and the gain $K_a = \mu _f N_2K_{f_1}$.

The stability of inner closed-loop dynamics (29) requires the real part of the pole $P_d$ of (30) to be negative while keeping small disturbance d(t). The selection of stability region is then a trade off between small $\mathbb {R}_e(A_{\sigma })$, small $K_a$, and small $1/K_u$ as shown in Fig. 3 under varying $\varGamma$ and $P_{d_f}$. It can be seen that the smaller is the desired fast dynamics pole $P_{d_f}$, the more the pole $P_d$ is close to zero, the more the dynamics (30) is unstable, and the larger is $1/K_u$, even though $K_a$ is getting smaller. Thus, $\varGamma \in [0.3,0.7]$ is suitable range for internal dynamics stability and further good control performance.

Fig. 3.

Selection of output redefine parameter $\varGamma$

3.4 Impedance Control Design

In order to prescribe a robot dynamic behavior during constrained motion, the impedance dynamics is given by

$$\begin{aligned} M_d(\ddot{y}_r-\ddot{y}_d) + B_d(\dot{y}_r-\dot{y}_d) + K_d(y_r-y_d) = f_e \end{aligned}$$

(31)

where $M_d>0$, $B_d>0$, and $K_d>0$ are respectively desired impedance inertia, damping and stiffness. $y_r$ and $y_d$ are reference and desired tip-trajectories in joint-space. $f_e$ is the contact force between the robot end-effector and the unknown compliant environment given by

$$\begin{aligned} f_e = K_e(y-y_e)l \end{aligned}$$

(32)

where $K_e$ is environment stiffness, y is the actual position of the robot end-effector, and l is the length of robot link. $K_e$, $y_e$ and $f_e$ are the environment parameters considered to be unknown.

Stability Analysis of the Outer Closed-Loop Dynamics: the outer closed-loop dynamics is a combination of the impedance dynamics (31), the interaction force (32), and the inner closed-loop dynamics (13)

$$\begin{aligned} {\left\{ \begin{array}{ll} M_d(\ddot{y}_r-\ddot{y}_d) + B_d(\dot{y}_r-\dot{y}_d) + K_d(y_r-y_d) = K_e(y-y_e)l \\ \dot{s}_r = - \phi _{r}^T\tilde{\vartheta }_{r} + \tilde{d}(t) + u_s + u_f \end{array}\right. } \end{aligned}$$

(33)

Another Lyapunov candidate is chosen as

$$\begin{aligned} V_3 = V_2 + \frac{1}{2}M_d(\dot{y}_d-\dot{y}_r)^2 + \frac{1}{2}K_d(y_d-y_r)^2 + \frac{1}{2}K_e(y-y_e)^2 \end{aligned}$$

(34)

where $V_2$ is predefined in (27) and has been shown to be positive-definite. $V_3 \ge 0$ for all $s_r$, y, $y_d$, $y_r$, $\dot{y}$, $\dot{y}_d$, and $\dot{y}_r$ null and non-null. The time derivative $\dot{V}_3$ is obtained as

$$\begin{aligned} \dot{V}_3 = \dot{V}_2 - B_de_d^2 + f_e\dot{y}_d + f_e\dot{e}_r \end{aligned}$$

(35)

with $\dot{V}_2 \le 0$, $B_d>0$. Thus the passivity of the outwer closed-loop dynamics holds for $f_e\dot{y}_d\le 0$ and $f_e\dot{e}_r\le 0$, i.e. (i) small contact force (use of soft environments - small $K_e$); (ii) accurate inner motion control ($e_r\approx 0$); and (iii) the use of a passive or compliant environment.

Environment Parameters Estimation: The contact force $f_e$ is estimated using force observer based on EKF algorithm (36)

$$\begin{aligned} {\left\{ \begin{array}{ll} \hat{\upsilon }_k^- = \hat{\upsilon }_k^- + T_s~f(\hat{\upsilon }_{k-1}, u_{s,k}) \\ \hat{P}_k^- = \hat{F}_{k-1} \hat{P}_{k-1} \hat{F}_{k-1}^T + \hat{H}_{k-1} \tilde{N}_{k-1} \hat{H}_{k-1}^T \\ K_k = \hat{P}_k^- C_{\upsilon ,k}^T(C_{\upsilon ,k} \hat{P}_k^- C_{\upsilon ,k}^T + \tilde{W}_{k})^{-1} \\ \end{array}\right. } {\left\{ \begin{array}{ll} \hat{\upsilon }_k^+ = \hat{\upsilon }_k^- + K_k~\left( \tilde{y}_k - C_{\upsilon ,k}\hat{\upsilon }_k^-\right) \\ \hat{P}_k = \left( I - K_k C_{\upsilon ,k}\right) \hat{P}_k^- \\ f_{e_k} = \hat{\upsilon }_k^+(\text {end}) \end{array}\right. } \end{aligned}$$

(36)

where $v\triangleq [\dot{\theta }, \theta , \dot{\alpha }, \alpha , f_e]^T$, f is the state transition function based on the dynamics equation (4), $(\hat{\upsilon }_k^-,\hat{P}_k^-)$ and $(\hat{\upsilon }_k, \hat{P}_k)$ are the predicted and estimated states and their associated covariance, respectively. $K_k$ is the Kalman filter gain to correct the prediction on time step k.

Finally, by defining a threshold value for the contact force $f_e^*$, such that at instance $|\hat{f}_e| > f_e^*$ the contact can be detected. Thus, the actual position y at that instance is considered as the estimated rest position of the environment $\hat{y}_e$. The environment stiffness can be computed based on the Eq. (32).

4 Simulation Results

The effectiveness of the proposed controller is verified on a SFLM with model parameters given in [17], tracking from its tip, a desired square wave trajectory given by $y_d=[0~-~40]^o$, with $\dot{y}_{d,\max }=80^{o/s}$ and $\ddot{y}_{d,\max }=240^{o/s^2}$. Table 1 shows control parameters set for good control performance such as accurate inner motion control, robustness, and fast response.

Table 1. Control parameters setup

For given a compliant soft environment of $K_e=2$ N/m, located at $y_e=25^o$ and $y_e=15^o$, for a period of 1.35 s, from 5.5 to 7.4 s and from 9.415 to 11.4 s respectively, Fig. 4 shows effect of stiff robot by setting up the desired impedance stiffness to $K_d=5$ N/m. The robot’s end-effector is able to penetrate within the environment, and damages the environment with low contact force $\hat{f}_{e,ss}\approx 0.189$ N. Therefore, for safety of soft environment, a small value of $K_d$ is suitable to maintain the robot end-effector at the equilibrium position of environment. Moreover, for given stiff compliant environment of $K_e=20$ N/m, setting small $K_d=2$ N/m (soft robot), prevents robot end-effector to penetrate the environment and keep itself safe from hard collision. In addition, for high environment stiffness the contact force becomes larger $\hat{f}_{e,ss}\approx 0.5$ N and the transient response could suffer from large overshoot.

Fig. 4.

Numerical simulation results of Compliant control of SLFM

5 Conclusion

In this paper, an adaptive compliance control of a SLFM in unknown environment has been proposed in term of inner and outer controllers. The inner controller has been designed firstly based on an output-redefined and singular-perturbed slow dynamics for precise tip-trajectory tracking and secondly based on two-time scales fast dynamics to suppress tip-link vibrations. Lyapunov theory has been used to point out necessary passivity conditions for both closed-loop inner and outer dynamics. The unknown environment parameters have been estimated through two force observer based on EKF algorithm and compare with RLSE method. System parameters have been updated through online adaptation law. The effectiveness of the proposed controller has been verified through numerical simulation.