Unified Motion/Force/Impedance Control for Manipulators in Unknown Contact Environments based on Robust Model-Reaching Approach

2021 IEEE/ASME International Conference on
Advanced Intelligent Mechatronics (AIM)

 Unified Motion/Force/Impedance Control for
 Manipulators in Unknown Contact Environments
 based on Robust Model-Reaching Approach
 Yinjie Lin, Zheng Chen, , and Bin Yao,

 Abstract—With the developments of intelligent and au- introducing compliant mechanical elements into the transmis-
tonomous robotic technology, robots are usually designed to sion train, such as the cable-driven manipulators [10], [11], the
confront sophisticated tasks like automated assembly, which series elastic actuators (SEAs) [12] driven exoskeletons, and so
requires both high-speed positioning capabilities and compliance
with unknown contact environments. As we know, a high- on. However, the compliant elements in mechanical structure
performance motion tracking control in free-space can achieve decrease the control performance and stifle the possibility of
efficient and accurate positioning, while impedance or force high-precision motion control [13]. The other one is through
control shows superior performance in terms of sensitive force active compliance control algorithms, such as impedance [14]
and compliance with unknown contact environments. However, or force control. Robotic systems with impedance control are
it is still challenging to achieve both high-performance motion
tracking and compliance within one single control framework, governed to be compliant to the unknown contact environ-
especially in unknown contact environments. To this end, a ments, but they have poor motion tracking performance in
unified motion/force/impedance approach in unknown contact free-space due to model uncertainties [15]. In addition, further
environments is proposed by robust model-reaching control with researches develop the position-based impedance control (i.e.,
dynamic trajectory adaptation. Specifically, the overall control admittance control), which employs the inner motion control
scheme includes two loops: the outer-loop replans the trajectories
of motion and force in real-time to meet the environmental loop to provide motion tracking in free-space. However, it is
constraints, which are estimated by the recursive least square easy to result in instability when the contact environment is
estimation law; in the inner-loop, the robust model-reaching comparatively stiff since the inner motion loop cannot track
control law is proposed to realize a target model, which is too aggressive trajectory [16]. As such, these approaches, ei-
designed to establish a dynamic relationship between the motion ther via impedance/force/admittance control or by introducing
and force tracking error of the replanned trajectories. Then,
by changing the matrices of the target model, the compromise compliant elements into drive train, cannot guarantee both
between motion and force tracking can be achieved, as well as high-performance motion tracking in free-space and compli-
different control objectives. Experiments are conducted on a 7- ance with unknown contact environments.
DOF manipulator to validate the advantages of the proposed As we know, in many tasks like automated assembly, a high-
scheme. performance motion tracking control in free-space can achieve
 Index Terms—Manipulator control, impedance control, force
control, unknown environment, trajectory adaptation. efficient and accurate positioning [17], while impedance or
 force control shows superior performance in terms of sensitive
 force and compliance with unknown contact environments.
 I. I NTRODUCTION
 However, the aforementioned facts make it a prospective and
 In the past decades, motivated by the automation of indus- challenging research topic to achieve both objectives within
tries, motion trajectory tracking control techniques have been one single control framework.
widely developed for mechatronic systems such as motors Practically, for unified control framework, many works
[1], vehicles [2], hydraulics [3]–[6], and especially the robotic designed the motion control, force control, impedance control,
systems [7]–[9]. In this case, manipulators are always designed and hybrid motion/force control [18] respectively, and focused
to have higher stiffness in the mechanical structure as well, on the switching strategies. Ott [15] et al. proposed a unified
which can indirectly increase the bandwidth of the motion impedance and admittance hybrid control scheme to contin-
control. However, different from the “stiffer is better” rule in uously switch and interpolate between impedance and admit-
motion tracking, increasing sets of tasks nowadays need to tance control, which is effective in the known environments.
deal with contact-sensitive objects and automated assembly, On the basis of [15], [19] developed this approach to face
which require both high-speed positioning capabilities and unknown environment via neural network, and [20] further
compliance with unknown contact environments. Therefore, in combined the recursive least square environment estimate law
addition to the motion control, the robotic systems nowadays to face time-varying environment. However, these approaches
give a strong emphasis on the compliance to the unknown have done an innovative work while do not include force
contact environments as well. tracking control, and there is a lack of verification experiments
 Generally, the compliance of the robot system can be in practice.
realized through two kinds of methods. One is by intentionally From another perspective, the unified framework of mo-

978-1-6654-4139-1/21/$31.00 ©2021 IEEE 192

tion/force/impedance control can be regarded as a trade-off lows the accuracy of motion tracking and the contact
between the motion tracking and force regulation problem. compliance to be effectively combined, but also enables
Matinfar et al. [21] formulated the motion tracking and force a smooth transition between these two, especially in the
regulation problem as an optimization problem, which can unknown contact environments.
adjust the target impedance of robots for a wide range of en- The superiority of the scheme is illustrated by comparative
vironments. However, the Linear Quadratic Regulation (LQR) experiments on the Franka Emika Panda 7-DOF manipulators,
was solved off-line and based on the known environment results show the free-space motion tracking accuracy and
dynamics in this study. Ge et al. [22], [23] improved the contact compliance. Furthermore, the 2-DOF continuous mo-
approach in [21] by off-line solving the Adaptive Dynamic tion and force tracking performance are validated by another
Programming (ADP) with the unknown environment’s dynam- experiment.
ics. These effective results were obtained under the assumption The rest of the paper is organized as follows. In Section II,
that the inner motion control loop could track the optimized the problem is formulated with the dynamics of manipulator
reference trajectory well. However, the inner loop cannot track and environment. The unified motion/force/impedance control
too aggressive trajectory for the limited bandwidth. Different in unknown environment is achieved in Section III. In Section
from the motion based approach, Yao et al. [24] proposed IV, the proposed approach is verified experimentally on a 7-
a generalized impedance model which directly establishes DOF manipulator. Finally, a conclusion is drawn in Section
the dynamic relationship between motion tracking error and V.
force tracking error in the control law, so that the motion
control, impedance control, and hybrid motion/force control II. P ROBLEM F ORMULATION
can be realized just by simply changing the matrices in the In this section, the system under study includes a gen-
target model. It is widely used in recent studies, such as the eral n-DOF rigid manipulator dynamics and an unknown
impedance control of [25]. However, the transition between environment dynamics, and the problem of unified mo-
different objectives was not considered in this method, and it tion/force/impedance control for manipulators in unknown
not only required both motion and force reference trajectories contact environments will be stated based on these two dy-
in advance but also required these trajectories to conforms to namics.
unknown environment dynamics, so the environment dynamics
was assumed to be known in advance. A. Manipulator Dynamics
 The above literature review shows that the challenges Firstly, the dynamics of an n-DOF manipulator in joint space
of developing a unified motion/force/impedance control in can be written as:
unknown environment are twofold: firstly, it is difficult to M (q)q̈ + C(q, q̇)q̇ + G(q) + τf + f˜(t) = τ − J T (q)Fext (t)
formulate a unified representation of different controllers; the (1)
second is that the process uncertainties in unknown contact where q ∈ Rn×1 denotes the joint position of manipulator,
environments make it difficult to accurately switch between and q̇, q̈ denote the velocity and acceleration of the robot
different control objectives. Faced with the aforementioned joints respectively; M (q) ∈ Rn×n is the inertia matrix,
challenges, this paper further develops the general model C(q, q̇)q̇ ∈ Rn×1 denotes the Coriolis and centrifugal force
proposed by Yao [24], and realized the unified control of vector, G(q) ∈ Rn×1 represents the gravitational term; τf
manipulator motion/force/impedance in unknown environment is the vector of friction torques; f˜(t) ∈ Rn×1 represents the
through the combination of robust model-reaching control modeling error and external disturbance; τ ∈ Rn×1 is the
law and dynamic trajectory adaptation. The novelties and vector of applied joint torque; Fext (t) ∈ R6×1 is the vector of
contributions of this paper include: external force/moment between the end-effector and unknown
 1) In order to replan the desired trajectory to conform to the environment, and J T (q) ∈ Rn×6 represents the transpose of
 unknown contact environment, the dynamic trajectory the geometric Jacobian matrix.
 adaptation approach is proposed based on the real- As the unified control is designed in Cartesian coordinate,
 time estimated environment dynamics and the kinematic considering the robot kinematics given by
 constraints. x(t) = φ(q) (2)
 2) The generalized model and robust model-reaching con- ẋ(t) = J (q)q̇ (3)
 trol are utilized to formulate a unified representation of
 different controllers based on the replanned trajectories. where x(t) ∈ R6×1 represents the position and orientation of
 The control performance and stability are guaranteed end-effector in the Cartesian space, ẋ(t) denotes the transla-
 and proved even if during the variation of the target tion velocity and angular velocity of end-effector.
 model. Remark 1: It should be noted that the x(t) and ẋ(t) do
 3) A unified framework of motion/force/impedance control not have the time derivative relations here, since the angular
 in unknown contact environments is developed by inte- velocity ẋ(t) is described based on body frame which is
 grating these two. Unlike the traditional position/force directly calculated from geometric Jacobian matrix J (q) and
 switching control, the proposed approach not only al- joint velocity q̇, refer to the [26] for detail.

 193

Differentiating the (3) with respect to time results in equilibrium position; Fe is the contact force/moment between
 ẍ(t) = J˙ (q)q̇ + J (q)q̈ (4) environment and manipulator, i.e., Fe = −Fext .
 Specially, when the stiffness Ke = 0, and the damping De
where ẍ represents the translation acceleration and angular is small enough, the environment dynamics can be seen as
acceleration of end-effector. free-space. Hence, (8) can represent the unknown environment
 Then, substituting the kinematics (2)-(4) into the joint space dynamics both for contact and non-contact situations.
dynamics (1), the dynamic model of the manipulator can be
derived as follows in operational space C. Unified Motion/Force/Impedance Control Problem State-
 ment
 MO (q)ẍ + CO (q, q̇)ẋ + GO (q) + Ff + F̃ = T − Fext (5)
 The problem of unified motion/force/impedance control for
 MO (q) = J †T M J † manipulators in unknown environment dynamics is stated
 CO (q, q̇) = J †T (C − M J † J˙ )J † based on (5) and (8) in this subsection. Suppose that the
 (6) desired motion and force trajectory xd (t), Fd (t) in Cartesian
 GO (q) = J †T G, Ff = J †T τf
 space, the motion and force tracking error can be represented
 F̃ = J †T f˜, T = J †T τ as ep = x(t) − xd (t) and ef = Fext (t) − Fd (t), respectively.
where J † denotes the pseudo-inverse Jacobian matrix. The unified motion/force/impedance control of manipulators
Property 1: MO (q) is symmetric positive definite(s.p.d.) ma- can be stated as that of determining a control law so that the
 0 00 0 00
trix, with kr In×n ≤ MO ≤ kr In×n , where kr and kr denote motion and force errors ep , ef achieve the following target
positive scalars. model,
Property 2: ṀO (q) − 2CO (q, q̇) is a skew-symmetric matrix, Md ëp + Dd ėp + Kd ep = −Kf ef (9)
i.e., 12 ς T ṀO (q)ς = ς T CO (q, q̇)ς, ∀ς ∈ Rn . where Md , Dd , Kd , Kf ∈ Rn×n are desired matrices which
 Let ∆• and ˆ• denotes the modeling error and estimated can be changed with respect to control objectives, and usually
value of • respectively, i.e., ∆• = • − ˆ•. It should be noted chosen as diagonal matrices to get decoupled responses in
that the nominal value usually cannot be exactly known in the Cartesian space.
early identification experiments, only estimated values M̂O , Remark 2: It is important that the desired trajectories xd (t),
ĈO , ĜO , F̂f are available in (5). Fd (t) in target model (9) should conform to the environment
Assumption: The modeling error are bounded by known dynamics (8), and Md , Dd , Kd , Kf should be chosen
bounds, i.e., properly when interacts with environments.
 k∆MO (q)k = MO (q) − M̂O (q) ≤ δMO Remark 3: The target model (9) is actually a trade-off between
 the motion tracking and force tracking of manipulators. The
 k∆CO (q, q̇)k = CO (q, q̇) − ĈO (q, q̇) ≤ δCO motion tracking is emphasized by assigning Kd  Kf , then
 (7) the steady-state motion tracking error ep is relatively small
 k∆GO (q)k = GO (q) − ĜO (q) ≤ δGO whereas the force tracking error ef is large. Otherwise, the
 force tracking can be achieved with Kd  Kf as well.
 k∆Ff k = Ff − F̂f ≤ δFf , F̃ ≤ δ F̃
 Specifically, diversity control objectives can be realized
where k•k denotes a norm of • which is a vector or a for manipulators via the target model (9) if the desired
 p loss of generality, k•k2 is used, i.e., k•k =
matrix; without motion/force trajectory xd (t)/Fd (t), and the matrices Md ,
σmax (•) = λmax (•T •), where σ(•) denotes singular value Dd , Kd , Kf are well settled, such as the motion control, the
of •, λ(•) means the eigenvalue of •. The positive bounds impedance control, the force control, the hybrid motion/force
δMO , δCO , δGO , δFf , δ F̃ are assumed known. control and so on [24].
 As such, to achieve the unified motion/force/impedance
B. Unknown Environment Dynamics control in unknown contact environments, firstly the motion
 In practice, manipulators can obtain approximate environ- and force tracking performance of the manipulator should be
mental information through external sensors (such as cameras), regulated by adjusting the matrices Md , Dd , Kd , Kf in target
but before establishing contact, it is often impossible to fully model (9). Meanwhile, the desired motion and force trajectory
understand the accurate contact point and contact stiffness. in the target model are also required to meet the geometric and
Under the hypothesis of a stable and single contact, the dynamic constraints of the unknown contact environments (8).
unknown environment dynamics in this paper is formulated In the next section, the robust model-reaching based approach
with unknown contact position and unknown contact stiffness. is proposed for these objectives.
The most commonly used environment model is the Kelvin- III. ROBUST M ODEL -R EACHING C ONTROL WITH
Voigt linear model as in [27], where the environments can be DYNAMIC T RAJECTORY A DAPTATION
modeled as a liner damper-spring system as follows,
 The overall control architecture of the closed-loop system
 Ke (x − xe ) + De ẋ = Fe (8) is given in Fig. 1, it includes two loops: in the outer-loop,
where Ke and De represent the stiffness and damping of both the desired motion and force trajectories xd (t), Fd (t) are
the environments respectively, xe denotes the environment replanned according to the environmental constraints which

 194

are estimated by environment estimation algorithm. Mean-
while, with the replanned trajectories xr (t) and Fr (t), the Remark 4: The estimated value x̂e,i in ϕi is gotten when the
target model in the inner-loop is also changed according to manipulator first contacts with environments.
the control objectives and estimated environment dynamics. Remark 5: The environment damping usually cannot be
Finally, the robust model-reaching law is designed to achieve estimated with sufficient accuracy during the first contact
the target model under the bounded uncertainties (7), thereby establishment process since the environment dynamics is not
realize the compromise between the motion tracking and force excited sufficiently, i.e., persistent excitation (PE) condition
tracking. is not always satisfied. Continuous contact and variations of
 the contact force should be exerted on the environment for
 accurate estimations.
 Robust Model-Reaching , ሶ
 Manipulator Environment
 Control 

 ( ) Replanning by
 
 ( ) ሷ − ሷ + ሶ − ሶ +
 B. Dynamic Trajectory Adaptation
 Dynamic Trajectory
 Adaptation − = − ( − )

 Contact Environment
 In actual applications, before the environment estimation,
 ෡ 
 ෝ , 
 ෡ Online Estimation
 the environment geometric and dynamic characteristics are
 Fig. 1. The overall control scheme of the proposed approach. often not sufficiently known, so that it is hard to give the
 desired motion trajectory xd (t) and force trajectory Fd (t)
A. Environment Online Estimation which meet the environment well. As such, the desired motion
 trajectory xd (t) and force trajectory Fd (t) are replanned
 The unknown environment dynamics in (8) is identified
 based on the environmental constraints estimated in previous
online via Recursive Least Square Estimation (RLSE), includ-
 subsection. The objectives of the dynamic trajectory adaptation
ing the environment equilibrium position xe , the stiffness Ke
 are mainly twofold: On the one hand, the online replanned
and damping De . Assuming that the environment dynamics is
 trajectories xr (t) and Fr (t) not only meet the environment
fully decoupled, and then the environment dynamics of each
 characteristics and manipulator kinematic constraints, but also
environment dimension can be written as the regressor form,
 converge to the desired trajectories as much as possible.
 Fe,i = ϕTi θi (10) On the other hand, the initial value of the online replanned
where •i , i = 1, 2, ...6 denote i th
 DOF, and ϕi and θi are reference trajectories xr (t) should be calibrated to the real-
defined as time measurements x(t) at the beginning of each replanning
 sampling period, i.e., the motion tracking error is small at
 ϕi = [xi − x̂e,i , ẋi ]T , θi = [Ke,i , De,i ]T (11)
 the initial instant of each replanning sampling period, so the
 The RLSE method is used to find an estimated θ̂i of each mismatches of the motion and velocity when switching the
DOF which can minimize the performance index function, control objectives are eliminated.
 Z t
 The objectives of dynamic trajectory adaptation can be
 J(t, θ̂i ) = kW (t, τ )[Fe,i (τ ) − ϕi (τ )T θ̂i ]k2 dτ (12)
 0 formulated as a constrained optimization problem via model
where W (t, τ ) represents the weighting matrix. predictive control in each sampling period,
 Z t0 +tp
 Then, the RLSE update equation can be derived as follows, 1
 min J = [ (xr − xd )T Q(xr − xd )
 ˙ xr ,Fr 2
 θ̂i (t) = Γi (t)ϕi (t)i (t), i (t) = Fe,i − ϕTi θi t0
 DΓ,i = µ1 (t)Γi (t) − µ2 (t)Γi (t)ϕi (t)ϕi (t)T Γi (t) 1
 (13) + (Fr − Fd )T R(Fr − Fd )]dt
 DΓ,i , if λmax (Γi (t)) < ρM 2
 Γ̇i (t) = 
 K̂ (xr − x̂e ) + D̂e ẋr = −Fr (15)
 0, otherwise 
  e
  xr (t0 ) = x(t0 )
 
 
where i (t) represents the prediction error; the covariance is
initialized to a value bounded as 0 < Γi (0) < ρM Ip ; s.t. ẋmin ≤ ẋr ≤ ẋmax
 ẍ ≤ ẍr ≤ ẍmax
 
 
µ1 (t) and µ2 (t) are the weighting which are chosen as  min
 
 
0 < µ1 (t) ≤ 1 and 0 < µ2 (t) ≤ 2 for stability. Fmin ≤ Fr ≤ Fmax
Theorem 1: The parameter estimation error i is uniformly where t0 and tp represent the initial time and prediction
bounded with the RLSE law (13), i.e., θ̃i (t) ∈ L∞ . horizon of each replanning sampling period, respectively; Q
Proof 1: Denote i (t) = −ϕTi θ̃i , and define the positive and R denote the weighting matrices of motion and force,
function Vθ,i = θ̃i (t)T Γi (t)−1 θ̃i (t), then differentiating Vθ,i respectively; ẋr and ẍr represent the velocity and acceleration
 dΓi (t) −1 of the replanned motion trajectory, respectively; ẋmin and
 ˙
V̇θ,i (t) = 2θ̃i (t)T Γi (t)−1 θ̃i (t) + θ̃i (t)T θ̃i (t) ẋmax are the minimum and maximum velocity constraints,
 dt
 respectively; ẍmin and ẍmax are the minimum and maximum
 = 2θ̃i (t)T ϕi (t)i (t) + θ̃i (t)T [−µ1 (t)Γi (t)−1 +
 acceleration constraints, respectively; also, Fmin and Fmax
 µ2 (t)ϕi (t)ϕi (t)T ]θ̃i (t) are the bound of replanned force.
 = −2i (t)2 − µ1 (t)θ̃i (t)T Γi (t)−1 θ̃i (t) + µ2 (t)i (t)2 The optimization problem in (15) is converted into a
 (14) quadratic programming (QP) problem and solved real-time

 195

with this form, Since the matrices A and F2 are diagonal matrices, the
 1 T T relationship F2 AF2−1 = A can be derived. Choosing the
 min fseq H(t0 )fseq + fseq g(t0 )
 2 matrices Kpz (t), Kdz (t) and Kf z (t) as
  (16)
 lbA(t0 ) ≤ Afseq ≤ ubA(t0 ) Kdz (t) = F2−1 Md (t)−1 Dd (t) − F1 +
A
 

 s.t.
 lb(t0 ) ≤ fseq ≤ ub(t0 ) Kpz (t) = F2−1 Md (t)−1 Kd (t) + AF1 (23)
where fseq is a sequence of Fr on time; H(t0 ) is the Hessian Kf z (t) = F2−1 Md (t)−1 Kf (t)
matrix related to Q, R and the environmental constraints in
(15); g(t0 ) is the gradient vector concerning with the x(t0 ), Then, the (24) can be derived by substituting (23) into (22)
Q and environmental constraints; lbA(t0 ) and ubA(t0 ) are Md (t)ëpr + Dd (t)ėpr + Kd (t)epr =
 (24)
derived from the kinematic constraints of xr ; lb(t0 ) and − Kf (t)ef r + Md (t)(ṡ − As)
ub(t0 ) are related to the bound of the replanned force. where the target model (17) can be achieved in the sliding
 With the replanned trajectories, noting the motion tracking mode {s → 0, ṡ → 0} [29].
error and force tracking error as epr = x(t) − xr (t) and
ef r = Fext (t) − Fr (t), respectively. The target model in (9) As such, for the manipulator described by (5), the manipu-
can be derived as lator can achieve the desired target model under the bounded
 uncertainties if the following control input is applied
 Md (t)ëpr + Dd (t)ėpr + Kd (t)epr = −Kf (t)ef r (17)
 T = M̂O ẍeq + ĈO ẋeq +ĜO
where the error epr is small at the initial instant of each s
sampling period. + F̂f + Fext − Ks s − d
 ksk
Remark 6: During the variation of target model, the passivity (25)
should be guaranteed under constraints with respect to the where
derivatives of these matrices. Define the energy function as
 ẋeq = ẋr (t)−F1 epr −F2 z, ẍeq = ẍr (t)−F1 ėpr −F2 ż (26)
Ve = 21 ėTpr Md (t)ėpr + 21 eTfr Kd (t)ef r , and its derivative is
 1 Ks is a defined s.p.d. matrix and d is a positive scalar
 V̇e = ėTpr (−Kf (t)ef r ) + ( ėTpr (Ṁd (t) − 2Dd (t))ėpr + satisfying
 2
 1 T d ≥ δMO kẍeq k + δCO kẋeq k + δGO + δ F̃ + ε (27)
 e K̇d (t)epr )
 2 pr where ε is a positive scalar.
 (18)
where the increase of inertia matrix and stiffness matrix may Theorem 2: With the designed control input (25), the ma-
destroy the passivity on the pair < ėpr , −Kf ef r >, refer to nipulator system described in (5) achieves the desired target
[28] for details. model (17) under the bounded uncertainties (7).
 Proof 2: Define a positive definite function as V = 12 sT MO s.
C. Robust Model-Reaching Control Law From Property 1, the following inequality can be derived
 The robust model-reaching strategy is designed to achieve 1 0 1 00
 kr ksk2 ≤ V ≤ kr ksk2 (28)
the target model with replanned trajectories (17) in this sub- 2 2
section. Differentiating V , using the Property 2, and substituting the
 Firstly, a dynamic compensator is formulated as control input T (25) into V̇ , we get
 ż = Az + Kpz (t)epr + Kdz (t)ėpr + Kf z (t)ef r (19) 1
 V̇ = sT MO ṡ + sT ṀO s = sT (MO ṡ + CO s)
 2
where z is the n-dimensional state vector of the compensator;
 = −sT Ks s − dksk + sT [−∆MO ẋeq − ∆CO ẋeq −
A ∈ Rn×n is a diagonal matrix, which is also negative semi-
defined; Kpz (t), Kdz (t) and Kf z (t) ∈ Rn×n are matrices ∆GO − ∆F − ∆F̃ ]
related to the target model. ≤ −sT Ks s − dksk + sT [k∆MO kkẋeq k+
 With the compensator state z, the switching function is k∆CO kkẋeq k + k∆GO k + k∆F k + k∆F̃ k]
forming as follows
 ≤ −λmin (Ks )ksk2 − εksk
 s(epr , ėpr , z) = ėpr + F1 epr + F2 z (20) s
 n×n 2λmin (Ks ) 2 √
where F1 , F2 ∈ R are any diagonal constant matrices, ≤− 00 V −ε V
and F2 should be chosen as non-singular matrix. kr kr00
 Differentiating (20) with respect to time as (29)

 ṡ = ëpr + F1 ėpr + F2 ż (21)
 The actual input in joint space can be derived from (6) and
 Substituting the dynamic compensator (19) into (21) and (25) as follows
noting (20), the z can be eliminated  
 T T s
 ëpr + F1 − F2 AF2−1 + F2 Kdz (t) ėpr + (F2 Kpz (t)− τ = M̂ q̈eq + Ĉ q̇eq + Ĝ + τ̂f + J Fext − J Ks s + d
 

 ksk
 F2 AF2−1 F1 )epr = −F2 Kf z (t)ef + ṡ − F2 AF2−1 s (30)
 (22) where q̇eq = J † ẋeq and q̈eq = J † (ẍeq − J˙ q̇eq ).

 196

cases with similar contact velocities and environment position
 uncertainties, i.e., xe − xd = 5mm to 10mm. Case I is a
 plastic surface, and its deformation is relatively large after
 Panda 7-DOF
 Manipulator being stressed, while Case II is a comparatively stiffer surface.
 Work Station PC
 Ethernet C1: The proposed approach. The target model parameters
 ATI Nano 25 F/T
 Sensor are chosen as Md = diag[1, 1, 1, 0.01,q 0.01, 0.01], Kd =
 Ethernet
 diag[300, 300, 300, 0.5, 0.5, 0.5], Dd = 2 Kd Md−1 Md and
 ATI Net Box
 Kf = 0 in motion tracking, respectively. The initial value of
 Emergency Button
 Ke and De in the dynamic trajectory adaptation are Ke =
 0, De = diag[0.001, ..., 0.001] in free-space, and changed
 Control Box
 by RLSE. Also, when the estimated environment stiffness
 Fig. 2. Overview of the experiment system. Ke [2] ≥ 10 (i.e., environment stiffness in z direction), the tar-
 get model matrices Kd and Kf smooth change to a more com-
 IV. E XPERIMENTAL VALIDATION pliant target model of Kd = diag[300, 300, 50, 0.5, 0.5, 0.5],
 Kf = diag[0, 0, −0.5, 0, 0, 0] as shown in the Fig. 3.
A. System Setup
 The overview of the hardware configuration is shown in
the Fig. 2, the Franka Emika Panda 7-DOF manipulator is 300
 Variations of Target Model
 300
 Variations of Target Model

equipped with a six-axis F/T sensor Nano 25 from ATI 200 200

 Kd
 Kd
 100 100

 0 0

Industrial Automation, Inc, which can be used to measure 2 3 4 5 6 7 8 9 10 11 12 2 4 6 8 10 12 14 16

the external force exerted on the end-effector. The Panda 40 40

 Dd
 Dd
 20 20

manipulator and ATI Nano 25 F/T sensor are connected to its 0

 2 3 4 5 6 7 8 9 10 11 12
 0

 2 4 6 8 10 12 14 16

Control Box and ATI Net Box, respectively. Then, both boxes 0.5

 0
 0.5

 0

communicate with the work station PC via Ethernet, which is

 Kf
 Kf

 -0.5 -0.5

 -1 -1

an Intel Core i9-9900k @ 3.60 GHz desktop computer running 2 3 4 5 6 7
 Time/s
 8 9 10 11 12 2 4 6 8
 Time/s
 10 12 14 16

with Ubuntu 16.04 LTS (real-time kernel, Linux 4.14.191- (a) Variation in C1 and C2. (b) Variation in Fig. 8.
rt90) and installed Robot Operating System (ROS) Kinetic. Fig. 3. Variations of target model.
The manipulator is controlled by the libfranka and franka-ros
 C2: The proposed approach without dynamic trajectory adap-
packages under the ROS Kinetic with the sampling period
 tation. The configuration is the same as the C1, while not use
Ts = 0.001s, the environment estimation is also running with
 the replanned trajectories xr (t) and Fr (t).
0.001s, and the dynamic trajectory adaptation loop is running
 C3: Impedance control. As in target model, the Kd and
with a sampling period Tsr = 0.005s.
 Kf are chosen as Kd = diag[300, 300, 50, 0.5, 0.5, 0.5],
 In the following experiments, the parameter configuration
 Kf = diag[0, 0, −0.5, 0, 0, 0] both in free-space motion and
of the proposed method is the same. The matrix A in the
 contact.
dynamic compensator (19) is chosen as diag[−0.1, ..., −0.1],
 C4: Admittance control. The achieved impedance model is
and F1 and F2 in the switching function (20) are chosen as
 the same as C3, while the inner loop is chosen as a motion
diag[0.1, ..., 0.1, ] and diag[10, ...10], respectively. Then, the
 tracking always, the motion tracking gain is the same as the
controller gain is chosen as Ks = diag[15, ..., 15]. As for
 C1 and C2 in free-space.
the optimization in dynamic trajectory adaptation, it is real-
time solved through qpOASES [30] quadratic programming C1: Contact with Environment Uncertainties C2: Contact with Environment Uncertainties
 20 20
library, which can program by C++ and conveniently call in Fd Fd
 Force Z/N

 Force Z/N

 Fr F ext
 10 10

the ROS programming. The weighting Q and R are chosen as 0
 F ext

 0
 80 80
diag[1, ..., 1] and diag[ 2000 2 , ..., 20002 ], respectively. Finally, 2 4 6 8 10 2 4 6 8 10
 0.075

the initial value of Γ is chosen as Γ(0) = diag[1000, 1000] in 0.075 xd
 0.07
 xd
 Positioin Z/m

 Positioin Z/m

 0.07 xr x
 0.15 0.15
 x 0.065 xe

each direction of environment estimation, and the forgetting 0.1
 0.065
 4 6 8 xe
 0.1
 5 6 7

factor is chosen as µ1 = µ2 = 0.9996. 0.05
 2 4 6 8 10
 0.05
 2 4 6 8 10

 10 -3 10 -3

B. Comparative Experiments: from Motion to Contact 10 10
 Error Z/m

 Error Z/m

 5 x - xd 5
 x - xd
 Comparative experiments were carried out with the follow- 0 x e - xd 0 x e - xd

ing algorithms to show the effectiveness of dynamic trajectory 2 4 6
 Time/s
 8 10 2 4 6
 Time/s
 8 10

adaptation during the transition process even in the case of Fig. 4. C1 and C2 under Case I.
arbitrary rigid contact. Four control algorithms (C1 - C4) were
implemented and compared. As shown in the Fig. 4, Fig. 5, Fig. 6 and Fig. 7, C1-C4
 The desired motion tracking trajectory is a point-to-point are commanded with the same desired trajectory and contact
curve, where the max acceleration and velocity are 2m/s2 and with two cases (Case I and II). It should be noted that the line
0.1m/s, respectively. All controllers are conducted under two x − xd represents the motion tracking error only in free-space,

 197

C3: Contact with Environment Uncertainties C4: Contact with Environment Uncertainties C3: Contact with Environment Uncertainties C4: Contact with Environment Uncertainties
 20 20 Fd
 30 30 Fd
 Fd Fd
 F ext

 Force/N

 Force/N
 F ext

 Force/N

 Force/N
 10 F ext 10 F ext 20 20
 10 10
 0 0 0 0

 2 4 6 8 10 2 4 6 8 10 2 4 6 8 10 2 4 6 8 10

 0.075 xd xd 0.11 xd
 0.075 0.105 xd
 x x 0.2 0.1 0.2 0.105 x
 x

 Positioin/m

 Positioin/m
 0.07 0.07
 Positioin/m

 Positioin/m
 0.15 xe 0.15 xe 0.095 xe
 xe 0.1
 0.065 0.065 0.15 0.15
 5 5.5 6 6.5 5 5.5
 0.1 5 6 0.1 5 6
 0.1 0.1
 0.05 0.05
 2 4 6 8 10 2 4 6 8 10
 2 4 6 8 10 2 4 6 8 10
 10 -3 x - xd 10 -3
 10 -3 10 -3 x e - xd
 10 10

 Error Z/m

 Error Z/m
 10 10
 Error Z/m

 Error Z/m
 5 5 x - xd
 5 5
 x - xd x - xd 0 0 x e - xd
 0 x e - xd 0 x e - xd
 2 4 6 8 10 2 4 6 8 10
 2 4 6 8 10 2 4 6 8 10 Time/s Time/s
 Time/s Time/s

 Fig. 5. C3 and C4 under Case I. Fig. 7. C3 and C4 under Case II.

while the “steady-state” error converges to the environment clined surface was used for contacting in this exper-
uncertainties xe − xd means that the contact force is small iment. The variation of the target model is shown
in these figures. From the comparative results, the C3 has in Fig. 3 from Kd = diag[300, 300, 300, 0.5, 0.5, 0.5],
the most compliant behavior both for Case I and II since the Kf = 0 to Kd = diag[300, 0, 0, 0.5, 0.5, 0.5], Kf =
contact force are small, while the motion tracking performance diag[0, −0.5, −0.5, 0, 0, 0], respectively.
of C3 in free-space is poor compare with other methods. As shown in the Fig. 8, the desired motion trajectory of each
Furthermore, a second contact will occur with C3 when in direction is point to point trajectory with maximum velocity
contact with Case II as shown in the magnified figure of Fig. 0.05m/s and maximum acceleration 1m/s2 . The desired force
7. Although C4 can improve the motion tracking performance trajectory of Z direction steps at t = 7s from 0N to 5N ,
through the inner motion tracking loop, the contact force is t = 15s from 5N to 10N , and the desired force trajectory of Y
larger, especially in Case II, so that a more serious second direction steps at t = 7s from 0N to 2.5N , t = 15s from 2.5N
contact will occur than C3. On the contrary, as shown in Fig. 4 to 5N . The results show that the dynamic trajectory adaptation
and Fig. 6, C1 and C2 have a more stable contact establishment method replans the desired motion and force trajectory with
than C4 as well as improved free-space motion tracking than respect to environment constraints well, where the replanned
C3. However, C2 suffers from a larger contact force than C1 motion trajectory tracks the desired motion trajectory well
when the contact occurs, especially in Case II. This is owing before contact and adapts to the environment after contact.
to although C1 and C2 both smoothly change the achieved After establishing a contact, the target model is smoothly
target model of the manipulator from stiff to compliant as varying to force tracking, so the tracking of the replanned
shown in the Fig. 3, while C1 replanned the desired motion position is not guaranteed. Moreover, with the variation of
trajectory which can eliminate the mismatch of position and the target model, the contact force can track the desired force
velocity when contacting occurs. dynamically as well. Meanwhile, the stiffness and damping of
 environment are estimated and converge, as shown in Fig. 9.
 C1: Contact with Environment Uncertainties
 Fd
 C2: Contact with Environment Uncertainties
 Fd
 To sum up, the manipulator tracks the replanned motion and
 30 30

 force trajectory well both in free-space and contact.
 Force Z/N

 Force Z/N

 Fr F ext
 20 20
 F ext
 10 10

 0 0

 2 3 4 5 6 7 8 9 2 3 4 5 6 7 8 9 xd
 Motion of Y Axis Motion of Z Axis
 xd 0.105 0.2 xr 0.2
 0.105 x xd 0.11
 d
 0.2 xr 0.2 0.1 xx
 x
 Positioin Z/m

 Positioin Z/m

 0.1 0.105
 x 0.095 x xe 0.15 xe 0.15
 0.1
 Positioin/m

 Positioin/m

 0.095 e
 0.15 xe 0.15
 5 6 7 5 5.5 6
 5 10
 0.1 0.1
 0.1 0.1
 0.105
 2 3 4 5 6 7 8 9 2 3 4 5 6 7 8 9 0.05 0.1 0.05

 10 -3 10 -3 0.095
 0 5 10 15 0
 10 10 5 10 15 20 5 10 15 20
 Error Z/m

 Error Z/m

 5 5
 x - xd
 Force of Y Axis Fd Force of Z Axis
 x - xd
 0 x e - xd 0 Fr 5.2
 x e - xd
 0 Fext 10
 5
 2 3 4 5 6 7 8 9 2 4 6 8 10
 4.8
 Force/m

 Force/m

 Time/s Time/s
 -5 -2.4 5 8 10 12
 -2.6
 Fig. 6. C1 and C2 under Case II. -2.8
 Fd
 Fr
 -10 0
 8 10 12 Fext

 To sum up, the aforementioned comparative experiments 5
 Time/s
 15 20 5 10
 Time/s
 15 20

show the promising results of the proposed approach. It allows Fig. 8. Continuous motion and force tracking results of Y and Z. The contact
stable and compliant behavior even in case of arbitrary rigid point is arbitrary chosen, and the actual force has noise within ±0.2N .
contact in the unknown environment.
C. Continuous Motion and Contact Force Tracking V. C ONCLUSION
 Continuous motion and force tracking performance were A two-loop approach has been developed to achieve the uni-
simultaneously verified in Y and Z directions, an in- fied motion/force/impedance control for manipulators in un-

 198

4000
 Eivironment Estimation of Y Axis
 4000
 Eivironment Estimation of Z Axis dancy,” International Journal of Intelligent Robotics and Applications,
 3000 3000
 vol. 3, no. 2, pp. 115–130, 2019.
 Ke,2/ N/m [8] Z. Chen, F. Huang, C. Yang, and B. Yao, “Adaptive fuzzy backstepping

 Ke,3/ N/m
 2000 2000
 control for stable nonlinear bilateral teleoperation manipulators with
 1000 1000
 enhanced transparency performance,” IEEE Transactions on Industrial
 0
 5 10 15 20
 0
 5 10 15 20
 Electronics, vol. 67, no. 1, pp. 746–756, 2020.
 [9] Z. Chen, F. Huang, W. Sun, J. Gu, and B. Yao, “Rbf-neural-network-
 150 150 based adaptive robust control for nonlinear bilateral teleoperation ma-
 nipulators with uncertainty and time delay,” IEEE/ASME Transactions
 D e,2/ N·s/m

 D e,3/ N·s/m
 100 100
 on Mechatronics, vol. 25, no. 2, pp. 906–918, 2020.
 50 50 [10] Y. Wang, F. Yan, J. Chen, F. Ju, and B. Chen, “A new adaptive time-
 0 0
 delay control scheme for cable-driven manipulators,” IEEE Transactions
 5 10 15 20 5 10 15 20 on Industrial Informatics, vol. 15, no. 6, pp. 3469–3481, 2019.
 Time/s Time/s
 [11] Y. Wang, B. Li, F. Yan, and B. Chen, “Practical adaptive fractional-
 order nonsingular terminal sliding mode control for a cable-driven
 Fig. 9. Environment estimations results. manipulator,” International Journal of Robust and Nonlinear Control,
 vol. 29, no. 5, pp. 1396–1417, 2019.
 [12] Y. Lin, Z. Chen, and B. Yao, “Decoupled torque control of series elastic
known contact environments by combining the robust model- actuator with adaptive robust compensation of time-varying load-side
 dynamics,” IEEE Transactions on Industrial Electronics, vol. 67, no. 7,
reaching control with a dynamics trajectory adaptation method. pp. 5604–5614, July 2020.
Specifically, the dynamic trajectory adaptation in the outer- [13] A. Calanca, R. Muradore, and P. Fiorini, “A review of algorithms for
loop not only can replan both the motion and force trajectories compliant control of stiff and fixed-compliance robots,” IEEE/ASME
 Trans. Mechatronics, vol. 21, no. 2, pp. 613–624, 2016.
to conform to the environmental constraints, but also can [14] N. Hogan, “Impedance control: An approach to manipulation: Part
eliminate the mismatches of the motion and velocity when i—theory,” 1985.
switching the control objectives. Meanwhile, the unknown [15] C. Ott, R. Mukherjee, and Y. Nakamura, “Unified impedance and
 admittance control,” pp. 554–561, 2010.
environment constraints are estimated online via RLSE law, [16] R. Kikuuwe, “Torque-bounded admittance control realized by a set-
including the stiffness, damping and equilibrium position. valued algebraic feedback,” IEEE Transactions on Robotics, vol. 35,
As for the inner-loop, the robust model-reaching control is no. 5, pp. 1136–1149, 2019.
 [17] J. Hu, C. Li, Z. Chen, and B. Yao, “Precision motion control of a
designed to directly realize the compromise between motion 6-dofs industrial robot with accurate payload estimation,” IEEE/ASME
and force tracking based on the replanned trajectories of outer- Transactions on Mechatronics, vol. 25, no. 4, pp. 1821–1829, 2020.
loop, stability are guaranteed theoretically. Comparative ex- [18] V. Ortenzi, R. Stolkin, J. Kuo, and M. Mistry, “Hybrid motion/force
 control: a review,” Advanced Robotics, vol. 31, no. 19-20, pp. 1102–
periments, continuous motion and force tracking experiments 1113, 2017.
were conducted to verify the motion and force tracking perfor- [19] F. Cavenago, L. Voli, and M. Massari, “Adaptive hybrid system frame-
mance together with the contact compliance during switching. work for unified impedance and admittance control,” Journal of Intelli-
 gent & Robotic Systems, vol. 91, no. 3, pp. 569–581, 2018.
Future works will mainly consist of extending the developed [20] C. Mei, J. Yuan, and R. Guan, “Adaptive unified impedance and
control scheme through reinforcement learning techniques so admittance control using online environment estimation,” in 2018 IEEE
that the target model can be automatically chosen. Also, International Conference on Robotics and Biomimetics (ROBIO). IEEE,
 2018, pp. 1864–1869.
further applications in contact-rich tasks of the developed [21] M. Matinfar and K. Hashtrudi-Zaad, “Optimization-based robot com-
approach will be considered, such as assembly and human- pliance control: Geometric and linear quadratic approaches,” The In-
robot interaction. ternational Journal of Robotics Research, vol. 24, no. 8, pp. 645–656,
 2005.
 [22] S. S. Ge, Y. Li, and C. Wang, “Impedance adaptation for opti-
 R EFERENCES mal robot–environment interaction,” International Journal of Control,
 vol. 87, no. 2, pp. 249–263, 2014.
 [1] W. Sun, Y. Liu, and H. Gao, “Constrained sampled-data arc for a class of [23] X. Liu, S. S. Ge, F. Zhao, and X. Mei, “Optimized impedance adaptation
 cascaded nonlinear systems with applications to motor-servo systems,” of robot manipulator interacting with unknown environment,” IEEE
 IEEE Transactions on Industrial Informatics, vol. 15, no. 2, pp. 766– Transactions on Control Systems Technology, vol. 29, no. 1, pp. 411–
 776, 2019. 419, 2021.
 [2] W. Sun, J. Zhang, and Z. Liu, “Two-time-scale redesign for antilock [24] B. Yao, S. Chan, and D. Wang, “Unified formulation of variable
 braking systems of ground vehicles,” IEEE Transactions on Industrial structure control schemes for robot manipulators,” IEEE Transactions
 Electronics, vol. 66, no. 6, pp. 4577–4586, 2019. on Automatic Control, vol. 39, no. 2, pp. 371–376, 1994.
 [3] L. Lyu, Z. Chen, and B. Yao, “Advanced valves and pump coordinated [25] Q. Xu, “Robust impedance control of a compliant microgripper for high-
 hydraulic control design to simultaneously achieve high accuracy and speed position/force regulation,” IEEE Trans. Ind. Electron., vol. 62,
 high efficiency,” IEEE Transactions on Control Systems Technology, no. 2, pp. 1201–1209, 2015.
 vol. 29, no. 1, pp. 236–248, 2021. [26] B. Siciliano and L. Villani, Robot Force Control. Boston, MA: Springer
 [4] B. Helian, Z. Chen, and B. Yao, “Precision motion control of a US, 1999.
 servomotor-pump direct-drive electrohydraulic system with a nonlinear [27] A. Haddadi and K. Hashtrudi-Zaad, “Online contact impedance identi-
 pump flow mapping,” IEEE Transactions on Industrial Electronics, fication for robotic systems,” 2008 IEEE/RSJ Int. Conf. Intell. Robot.
 vol. 67, no. 10, pp. 8638–8648, 2020. Syst. IROS, pp. 974–980, 2008.
 [5] J. Yao and W. Deng, “Active disturbance rejection adaptive control of [28] F. Ferraguti, C. Secchi, and C. Fantuzzi, “A tank-based approach to
 hydraulic servo systems,” IEEE Transactions on Industrial Electronics, impedance control with variable stiffness,” Proc. - IEEE Int. Conf. Robot.
 vol. 64, no. 10, pp. 8023–8032, 2017. Autom., pp. 4948–4953, 2013.
 [6] J. Yao, W. Deng, and Z. Jiao, “Rise-based adaptive control of hydraulic [29] H. K. KHALIL, Nonlinear Systems Third Edition, 2002.
 systems with asymptotic tracking,” IEEE Transactions on Automation [30] H. Ferreau, C. Kirches, A. Potschka, H. Bock, and M. Diehl, “qpOASES:
 Science and Engineering, vol. 14, no. 3, pp. 1524–1531, 2017. A parametric active-set algorithm for quadratic programming,” Mathe-
 [7] J. Liao, F. Huang, Z. Chen, and B. Yao, “Optimization-based motion matical Programming Computation, vol. 6, no. 4, pp. 327–363, 2014.
 planning of mobile manipulator with high degree of kinematic redun-

 199