Robust Trajectory Tracking Control for Variable Stiffness Actuators With Model Perturbations

Variable Stiffness Actuators (VSAs) have been introduced to develop new-generation compliant robots. However, the control of VSAs is still challenging because of model perturbations such as parametric uncertainties and external disturbances. This paper proposed a non-linear disturbance observer (NDOB)-based composite control approach to control both stiffness and position of VSAs under model perturbations. Compared with existing non-linear control approaches for VSAs, the distinctive features of the proposed approach include: (1) A novel modeling method is applied to analysis the VSA dynamics under complex perturbations produced by parameter uncertainties, external disturbances, and flexible deflection; (2) A novel composite controller integrated feedback linearization with NDOB is developed to increase tracking accuracy and robustness against uncertainties. Both simulations and experiments have verified the effectiveness of the proposed method on VSAs.


INTRODUCTION
Recently, compliant robots have attracted increasing attention in the robotics community. Variable stiffness actuators (VSAs), a kind of compliant actuators, have been introduced to develop newgeneration robots because of its abilities to increase safety in human-robot interaction, to satisfy dynamic requirements, and to provide adaptability in unknown environments (Vanderborght et al., 2013;Grioli et al., 2015;Guo et al., 2015;Wolf et al., 2016;Pan et al., 2017). VSAs are usually multi-input multi-output (MIMO) non-linear systems, where the stiffness and position of the VSAs can be adjusted simultaneously by decoupling control methods (Kim and Song, 2012). However, in these actuators, the stiffness variation brings physical modifications, which requires the controllers to transit among different working conditions quickly. The physical coupling between stiffness and position mechanisms also introduces undesired complexity to control systems (Jafari, 2014). Furthermore, the performances of these actuators are severely affected by parametric uncertainties and external load perturbations, especially during interacting with environments. Therefore, it is essential to develop advanced control strategies for VSAs used in robotic systems.
Different control approaches have been proposed to for VSAs. The PD-based control is a simple and easy method to regulate position and stiffness of VSAs simultaneously. However, PD parameters should be tuned manually to obtain good tracking accuracy in different stiffness condition. Recently, a feedback linearization technique was also exploited for the control of VSAs in Palli et al. (2008) and Buondonno and De Luca (2016). This technique requires significant efforts in system modeling as well as the identification of the system parameters. In addition, a control strategy with fixed gains can cause limited performance in the dynamic variations of the VSAs (Buondonno and De Luca, 2016). To improve the control performance, other advanced control approaches, such as backstepping control (Petit et al., 2015), gainscheduling control (Sardellitti et al., 2013), non-linear model predictive control (Zhakatayev et al., 2015), adaptive neural network control , and prescribed performance control (Psomopoulou et al., 2015), have been proposed for VSAs. Although these control approaches have been proved to be effective to improve tracking performances of VSAs, they have a significant limitation that the performances heavily depend on exact models of VSAs (Palli and Melchiorri, 2011;Petit and Albu-Schaffer, 2011). In addition, the disturbance rejection ability of these controllers is achieved by sacrificing the nominal control performance. A novel approach has been proposed to the control of VSA actuated robots, aiming to preserve their dynamic behavior which has been obtained because of the elastic element in the robot structure (Della Santina et al., 2017;Keppler et al., 2018). Furthermore, a decentralized, iteratively learned feedforward approach, combined with a locally optimal feedback control has been introduced in (Angelini et al., 2018). The effectiveness of the method is experimentally verified on several robotic structures and working conditions. Disturbance observer (DOB)-based control is promising to reject disturbances and to improve robustness against modeling uncertainties (Roozing et al., 2016). This approach has been adopted in the control of serial elastic actuators (SEAs). For instance, a linear DOB-based control method was used for the prismatic SEA to achieve high precision force control in Park et al. (2017). However, this method cannot be directly applied to control VSAs because of non-linearities and model uncertainties. This paper introduces a non-linear disturbance observer (NDOB)-based composite controller to improve the control performance and reject load disturbances for a new type of serial VSA (SVSA), in which stiffness and position can be separately controlled by two motors with a series configuration (Sun et al., , 2018a. In the proposed control framework, a NDOB is applied to estimate disturbances so as to enhance the disturbance rejection ability. Based on feedback linearization, a composite control law is developed to stabilize the nonlinear dynamics. It is proven that the proposed controller can eliminate external disturbances by a proper selection of the compensation gain. The major contributions of this study include: (i) Different from exising VSA models, the SVSA model in this study considers the composite disturbances produced by system uncertainties, flexible effects, and external disturbances; (ii) A novel disturbance compensation method is developed to attenuate model perturbations for the control of SVSAs; (iii) Experimental studies have been carried out to demonstarete effectivencess and robustness of the proposed controller for SVSAs. In our previous work (Guo et al., 2018), we introduced a NDOB-based control (NDOBC) method for SVSAs, and conducted basic experiments related to position and theoretical stiffness tracking. The current work extends our previous work in terms of dynamic modeling and real-time control of SVSAs. We conduct both simulations and experiments comparing our approach with a feedback linearization-based controller.
The remainder of this paper is organized as follows. Section Actuator Dynamics and Problem Formulation introduces the SVSA dynamics and formulates the control problem. Section Non-Linear Disturbance Observer-Based Control describes the proposed NDOBC design and the control system stability issue. Section Simulation Results shows simulation and experimental results of the proposed controller. Section Experimental Results draws the conclusion of this study.

ACTUATOR DYNAMICS AND PROBLEM FORMULATION
In this section, the SVSA model is presented firstly. Subsequently, by considering parametric variations and external disturbances acted on the actuator, the control problem is formulated.

Actuator Dynamics
A novel SVSA based on an Archimedean spiral relocation mechanism (ASRM) was developed in Sun et al. (2017). As illustrated in Figure 1, this SVSA consists of a variable stiffness mechanism (VSM), a principal motor and a secondary motor, where the principal motor drives the output link motion through the spring transmission, and the secondary motor adjusts the actuator theoretical stiffness by the ASRM. Figure 1 shows the CAD model, prototype, and schematic model of the SVSA.
By considering gravity and external loads, the SVSA dynamics can be represented as follows: where q is a position of the output link, θ i with i = 1, 2 is the angle position of each motor, ϕ : =q − θ 1 is a deflection angle of the elastic transmission, M is an inertia of the output link, B i is a reflected inertia of each motor, D is a reflected damping of the link, D i is a reflected damping of each motor, τ g (q) is a gravity torque, τ r is a coupling reaction torque, τ e is an elastic torque of the spring transmission, u i is a control input of each motor, and τ ext is an external torque. The general specifications are shown in Table 1.
The elastic torque across the transmission is given by where K s is a spring stiffness, R is a radius of the output link, and µ is a lever length ratio. The stiffness of this SVSA is the first order of elastic torque The level length ratio µ can be written by the position of the secondary motor as follows: where µ 0 is an initial level length ratio. The coupled resistance torque, demonstrating the transmission deformation reacts on the stiffness motor, is given by where β= arctan(−θ 2 /γ ) is a tangent angle of the Archimedean Spiral gear, γ is a reduction gear ratio of the secondary motor, and a = µR = Rθ 2 /2π is a distance from the pivot point to the joint center.

Problem Formulation
Considering the parametric variations and modeling uncertainties in (1), we define the differences between the nominal and real variables as where M n is an equivalent inertia of the output link, B in (i = 1, 2) is an equivalent reflected inertia of each motor, D n is an equivalent damping of the link, D in (i = 1, 2) is an equivalent damping of each motor, τ en and τ rn are nominal elastic torque and resistance torque.
Substituting these variations into (1), we wet a nominal model The model uncertainties, gravity, and external disturbances are regarded as equivalent disturbances of the system: Frontiers in Neurorobotics | www.frontiersin.org Substituting Equation (7) into (6), the dynamic equations can be obtained as follows: The above dynamics can be rewritten in standard form where x = [q,q, θ 1 ,θ 1 , θ 2 ,θ 2 ] T ∈ R 6 a states vector, u = [u 1 , u 2 ] T is the control input for each motor, y = [q, σ ] T is the output position and stiffness of the actuator, and The equilibrium point of the system (9) be an equivalent disturbance. This paper aims to design a NDOB-based composite control law to compensate for unknown disturbances, without knowing the exact SVSA model. The control inputs of the SVSA are from two motors, while the control outputs are the position and stiffness of the actuator.

Non-linear Disturbance Observer Design
A NDOB as follows is applied to compensate for the unknown disturbance in the non-linear system (9) (Chen et al., 2000;Yang et al., 2012): where z w is internal state of the NDOB, andŵ = [ŵ 1 , ...,ŵ n ] T is the estimated vector of the unknown disturbance, λ(x)is an intermediate variable for the observer gain l(x), which is defined as , l 4 (x), l 5 (x), l 6 (x)]. (11) We define the disturbance error e w = w −ŵ. The estimated disturbance error of (10) is given bẏ Assumption 1:The first time derivative of the disturbanceė w is bounded, and satisfy lim t→∞ẇ (t) = 0. If the observer gain satisfies the differential equatioṅ The estimated disturbance error (12) is locally input-to-state stable (ISS). In order to make sure the observer error converges to 0, the observer gain is defined as We define the intermediate variable λ(x) as Thus, the state equation of the disturbance observer is given by

Composite Control Law Design
Substituting the disturbance w in system (22), a NDOB based composite control law is developed as whereŵ is the estimated disturbance by (10), and where parameters c i k (i = 1, 2; k = 0, 1, · · · , r i − 1) are selected such that the polynomials p 1 0 (s) = c 1 0 + c 1 1 s + · · · + c 1 3 s 3 +s 4 , p 2 0 (s) = c 2 0 + c 2 1 s+s 2 are Hurwitz stable. The schematic diagram of the proposed NDOB-based control design can be expressed in Figure 2. In order to prove that the control law is effective on disturbance, the disturbance estimation should be replaced by real disturbance.
Substituting (26) into (23), we can geṫ Combining (27) with (22), the error dynamic equation can be rewritten as where Equation (28) can be written as and We can see that the disturbance has been compensated according to (31) and lim t→∞ e i 1 (t) = 0.

System Stability Analysis
Theorem 1. If the following conditions are satisfied, the system (9) is locally ISS around x 0 : (i) The parameters c i in the NDOBC law (24) are chosen such that the polynomials (25) are Hurwitz stable; (ii) The disturbance gain is chosen to keep the function g(x)A −1 (x)Ŵ(x) + p(x) continuously differentiable at x 0 ; (iii) The observer gain is chosen such that the system (13) is asymptotically stable.
Proof: Substituting the NDOBC law (24) into the dynamic system (9), we can get the closed-loop system: Based on the new coordinate transformation [e 1 1 , e 1 2 , e 1 3 , e 1 4 , e 2 1 , e 2 2 ], the closed-loop system (26) includes the systemẋ = f (x) + g(x)u and the control law where It can be concluded that the system (34) is asymptotically stable at equilibrium x = 0.  Let X = [x T , e w T ] T , the system (34) is given bẏ Based on the theorem of the asymptotic stability (Khalil, 2002), the systemẊ = G − (x) is locally asymptotically stable at X = 0, according to the condition (ii), the system (34) is locally ISS.

SIMULATION RESULTS
To demonstrate the proposed NDOBC approach and point out its performance properties, a comparative simulation study with the control law has been conducted for the SVSA under external load disturbances as presented in Figure 3. The SVSA is first considered to verify and clarify the operation of the developed controller. The specifications of the SVSA given in Table 1 is used for simulation. We set the parameters for nominal model M n = 0.0153kg m 2 , B 1n = 0.0284kg m 2 ,B 2n = 0.019kg m 2 ,D n = 0.007N m s, D 1n = 0.007N m s, D 2n = 0.003N m s, and the initial variables are set as x(0)=[0 0 0 0 0 0]. To make a comparison, a feedback linearization-based (FL) controller is selected as a baseline controller, which is given by The unknown external disturbances are given by    w 1 (t)=w 3 (t)=w 5 (t) = 0 w 2 (t) = w 4 (t) = w 6 (t) = 0, t < 5 2, t ≥ 5.
The results of the comparison between the baseline controller and the NDOB controller are illustrated.

Tracking Under Fixed Stiffness
Sinusoidal trajectory of the actuator position with frequency of 0.2 Hz and amplitude of 60 • was taken. A 3 kg load disturbance is introduced at 5 s. The purpose of the simulation is to test the performance of the controller to track the trajectory at two different stiffness conditions, which is low stiffness (15 Nm/rad) and high stiffness (60 Nm/rad), respectively. Figure 4 shows that the proposed NDOBC approach exhibits promising disturbance attenuation and reference tracking performance. It is also observed that the tracking trajectory under the NDOBC is overlapped with the baseline control method during the first 5 s when there is no disturbance acted on the system, but poor tracking performance after loading, which proves that the property of the NDOBC method. In addition, the stiffness has little effect on the tracking performance under constant stiffness condition.

Tracking With Variable Stiffness
Sinusoidal trajectory was taken under variable stiffness condition, where σ (t) = 35 + sin(2πft + 1.5π)with the frequency of 0.2 Hz. In Figure 5, it can be seen that both the position and stiffness tracking errors are small without external load for two controllers. After loading 3 kg at 5 s, position and stiffness tracking errors increase with FL control, but the NDOBC performance is better than the baseline. The tracking error is small, which means the disturbance can be compensated.

EXPERIMENTAL RESULTS
To further verify the robustness of the controller, an experimental procedure was carried out on the SVSA platform. Two DC motors (RE50, 60W and RE25, 20W, Maxon motor) were selected as the driving modules. Two motor drivers (RMDS-102, ShenZhen RoboModule Technology Co., China) were used to control the motors. Encoders with 500 pluses per revolution were installed to measure the motor position. An Omron encoder (E6B2-CWZ1X) was utilized to measure the deflection angle of the SVSA. A Simulink real-time control system was built based on MATLAB/RTW in xPC target environment using CAN-AC2-PCI board (as shown in Figure 3). The angles of the encoders were collected via a data acquisition card (PCI-6259, National Instruments Corp., TX) to MATLAB/RTW control system. The communication between the real-time system and the Plant is through CAN Bus.

Tracking With Fixed Stiffness
Sinusoidal tracking experiments with frequency of 0.2 Hz and amplitude of 60 • at two different conditions, low stiffness (15 Nm/rad) and high stiffness (60 Nm/rad), were conducted. Figure 6 shows the position tracking and output errors for both controllers in the presence of external load disturbance at 5 s. The robustness of the NDOB controller is obvious because the error continues to reduce despite the external load. The disturbance is also clearly shown in the output error. It shows that the NDOB control can achieve better position tracking results within the first 5 s. The baseline control performance is deteriorated when adding the 3 kg load. In addition, compared with the low stiffness condition, we can find that the tracking error is reduced in high stiffness, which means external disturbances have less impact on the position tracking error at high stiffness. This can be explained that the deflection angle in low stiffness is larger than that of the high stiffness condition. However, compared with the simulation results, the experimental data exhibit small oscillations during the tracking.

Tracking With Variable Stiffness
Secondly, the controller performance has been tested while tracking a sine wave reference on continuous position and stiffness. Three kilograms load is applied at 5 s. The stiffness σ (t) = 35 + sin(2πft + 1.5π) has been adjusted with the frequency of 0.2 Hz. Figure 7 shows the position and stiffness tracking results with and without external load disturbance for two controllers. The NDOB control achieved better results than the FL controller. The position tracking error suddenly increases due to the external disturbances at 5 s. In stiffness tracking, there is no obvious change under the disturbance compensation algorithm while the error increases for FL control.

CONCLUSION
This paper proposed a NDOBC to attenuate the model uncertainties and external disturbances for a class of SVSA. Simulation and experimental results verify the ability of the proposed approach to cope with load disturbance by showing remarkable control performances for both position and stiffness tracking. The stability of the composite controller has been proved by the tracking results. Future work will focus on other non-linear composite adaptive control designs for the SVSA to solve the input saturation and unmodeled dynamics (Pan and Yu, 2016;Sun N. et al., 2018) and the application of this actuator to the design of variable stiffness robots in realworld applications.

DATA AVAILABILITY
The raw data supporting the conclusions of this manuscript will be made available by the authors, without undue reservation, to any qualified researcher.

AUTHOR CONTRIBUTIONS
ZG: theoretical analysis and writing paper. JS: VSA design. JL: guide doing experiment. YP: guide control plan. XX: guide writing paper.