Trajectory Tracking Control for Flexible-Joint Robot Based on Extended Kalman Filter and PD Control

The robot arm with flexible joint has good environmental adaptability and human robot interaction ability. However, the controller for such robot mostly relies on data acquisition of multiple sensors, which is greatly disturbed by external factors, resulting in a decrease in control precision. Aiming at the control problem of the robot arm with flexible joint under the condition of incomplete state feedback, this paper proposes a control method based on closed-loop PD (Proportional-Derivative) controller and EKF (Extended Kalman Filter) state observer. Firstly, the state equation of the control system is established according to the non-linear dynamic model of the robot system. Then, a state prediction observer based on EKF is designed. The state of the motor is used to estimate the output state, and this method reduces the number of sensors and external interference. The Lyapunov method is used to analyze the stability of the system. Finally, the proposed control algorithm is applied to the trajectory control of the flexible robot according to the stability conditions, and compared with the PD control algorithm based on sensor data acquisition under the same experimental conditions, and the PD controller based on sensor data acquisition under the same test conditions. The experimental data of comparison experiments show that the proposed control algorithm is effective and has excellent trajectory tracking performance.

Introduction

With the increasing use of robots in the fields of industry, the rehabilitation, aviation, and marine exploration, the demand for robots that can adapt to complex environments and enable human-robot interaction is increasing, which introduces flexible structure onto robotic joints (Schiavi et al., 2008; Grioli et al., 2015). The general flexible design is to adopt elastic elements and harmonic reducers to reduce the rigidity of robot joint (Zhu and Schutter, 1999). For higher usage requirements, the flexible cushioning is realized by a variable stiffness driver, and the stiffness performance can be adjusted in a wide range (Wolf and Hirzinger, 2008; Ham et al., 2009; Jafari et al., 2011; Torreaiba and Udelman, 2016). The reduction in the stiffness of the robot increases the safety, but at the same time leads to a reduction in the dynamic performance of the structure, which includes slow response, delayed control, and limited bandwidth. It makes the flexible robot based on variable stiffness more difficult to control (Hogan, 1985; Hurst et al., 2004; Erler et al., 2014).

In recent years, research on the control of flexible-joint robots has become more and more attractive. A spring-damping mode was first proposed to simplify the flexible-joint, and at the same time, the flexible-joint is divided into two subsystems for control by integral flow and perturbation theory (Spong, 1987), and the sliding mode controller was designed (Sira-ramirez and Spong, 1988). Then, based on this model, the robustness analysis of the feedback linearization method suitable for this simplified model was given (Grimm, 1990). A method for compensation system with parameter uncertainty was desidned (Zeman et al., 1997; Ge et al., 1998). To reduce the number of differentials required for motion equations and task equations the design method for task space tracking control and proposed an implicit numerical integration method was proposed which is effective (Ider and Ozgoren, 2000). The impedance model formed on the basis of the simplified spring- damping model is considered to be a typical compliant control strategy, that is, cross-compatibility is achieved through the interaction between the robot system and the external environment, allowing a certain degree of partial movement of the actual trajectory and a given trajectory (Jamwal et al., 2016; Losey et al., 2016). In order to control the robot trajectory more precisely, the effects of flexible properties on the dynamic performance of robots was systematically analyzed (Zaher and Megahed, 2015), and the control problems of flexible-joint robot position, torque, and impedance control based on passivity were studied (Albu- schäffer et al., 2007). However, these control models are somewhat stretched when it comes to external disturbances and non-linear systems.

In order to solve the problem of jitter and friction in flexible robot tracking control, the adaptive CFBC (command-filtered backstepping control) was proposed to improve tracking accuracy (Pan et al., 2018). For the purpose of solving the interference problem of control, the ADRC (Active Disturbance Rejection Control) was designed based on the modern control theory which relies on the accurate mathematical model, that can effectively control the system with uncertainty and external interference (Han, 2009). However, more parameters need to be calculated for non-linear systems. The disturbance observer proposed by Ohnishi in 1987 can be used for the disturbance that is difficult to measure in the system (Nakao et al., 1987). The external disturbance is estimated by the input amount and the feedback value of the inner loop, as the observation compensation amount, and it is added to the control to cancel the actual interference (Sariyildiz and Ohnishi, 2013; Sariyildiz et al., 2015). However, as the order for the filter increases, the large phase lag causes the system to be underdamped and even makes the system unstable.

Throughout the above control methods, the classic PD controller, with its “natural” anti-interference and model-independence, is widely used in the control of series elastic actuators by matching feedforward control (Zhu et al., 2012). It is worth noting that the PD controller relies on the data feedback of the system, therefore, its performance can be greatly affected by external disturbance introduced via sensory collection, and moreover, the use of multiple sensors increases the cost and structural design difficulty of the robot.

In order to solve the above problems, this paper first proposes the state estimation of non-linear stiffness-driven flexible robot with EKF, as it has good convergence and low computational complexity, and can handle system uncertainty and external disturbances in real time (Reif et al., 1998; Lightcap and Banks, 2010). This method can reduce the use of the sensor and introduce noise covariance into the observer design to reduce estimation error. Considering the external disturbance, a closed-loop PD controller based on EKF is designed to achieve precise position control that requires only joint motor side position and speed measurements.

This paper is organized as follows. The model, principle and dynamics analysis of the flexible-joint robot are introduced in section Robotic Prototype and its Dynamic Model. Subsequently, the design of the closed-loop PD controller and EKF state observer is introduced in section EKF Based Controller. The stability analysis based on Lyapunov method is presented in section Lyapunov Stability Analysis. The experimental results are presented in section Experimental Results. Finally, a conclusion is provided in section Conclusion.

Robotic Prototype and Dynamic Model of Robot

Robotic Prototype

In order to demonstrate our method, a 3-DOF robot with flexible joint is introduced in this section to verify the algorithm. As shown in Figure 1, the robot can be regarded as an open-chain series connected by two rotations and one moving joint. In the linkage system, each joint is driven by a non-linear actuator, and the third joint turns the rotation of the joint into translation through the slider and the guide rail.

FIGURE 1

Figure 1. Robotic prototype.

Since the control target of this paper is the trajectory output of the tip, based on the configuration of the robot, the variables of the three joints are respectively calculated by the inverse kinematics according to the desired trajectory output, and the three joints are respectively controlled according to the timing.

The structure of the three actuators is basically the same. In this paper, the first joint actuator is taken as an example to introduce the structure. As shown in Figure 2A, the structure of the non-linear stiffness actuator mainly includes the support frame, motor combination (DC brushless motor, reducer, encoder), pulley, outer cylinder, wire, inner cylinder, and elastic structure. The motor is the power source driving the pulley, which drives the outer cylinder through the wire. The inner wall of the outer cylinder is provided with a radially uniform roller shaft, and the rotation of the roller presses and fixes the bidirectional elastic structure fixed on the inner cylinder, thereby driving the inner cylinder to rotate.

FIGURE 2

Figure 2. Actuator structure (A) Main structure. (B) Elastic structure.

The non-linear elastic structure is shown in Figure 2B, the non-linear elastic structure is the core mechanical structure of the non-linear stiffness actuator and consists of a roller and an elastic component. The elastic component is composed of symmetrical elastic units, each of which consists of a cantilever beam portion and a contact portion. Although the two parts are made of the same material, when the roller presses the contact portion, the deformation of the contact portion can be regarded as a rigid structure, and the cantilever beam portion is deformed, and the deflection and the deflection angle of the elastic portion will cause the position of the contact point to change. There is a certain mapping relationship between the positional change of the contact point caused by the deflection perpendicular to the end of the cantilever beam portion and the pressing force.

In addition, the change of the deflection angle also affects the vertical component of the contact point position. Therefore, the relationship between the positional change in the vertical direction of the contact point and the pressing force is no longer a simple proportional relationship, but the combination of the deflection and the deflection angle. In short, the contour curve of the contact surface determines the relationship between the pressing force and displacement of the contact point, that is, the stiffness variation curve. The specific design scheme and the non-linear mechanism have been deeply studied by the researcher group (Lan and Song, 2016), and this paper will not go into details.

Dynamic Model

Non-linear stiffness actuator can be divided into power systems, transmission systems, elastic structures, and external loads. The power system is the motor combination, which mainly includes the motor rotor and the gear reducer. The equivalent moment of inertia of the motor combination can be obtained from the dynamics model of the motor combination. The dynamic equation of the rotor of the motor is:

$\begin{array}{ll}{J}_r{\ddot{\theta }}_r+b_r{\dot{\theta }}_r={\tau }_m-{\tau }_r& (1)\end{array}$

where J_r and b_r are the moment of inertia and damping of the rotor of the motor respectively; ${\dot{\theta }}_r$ and ${\ddot{\theta }}_r$ are the angular velocity and angular acceleration of the rotor of the motor respectively; τ_m is the torque generated by the rotor of the motor; τ_r is the torque output by the rotor of the motor.

The dynamic equation of the motor reducer is:

$\begin{array}{ll}{J}_g{\ddot{\theta }}_g+b_g{\dot{\theta }}_g=R_1{\tau }_r-{\tau }_g& (2)\end{array}$

where J_g and b_g are the moment of inertia and damping of the motor reducer respectively; ${\dot{\theta }}_g$ and ${\ddot{\theta }}_g$ are the angular velocity and angular acceleration of the motor reducer respectively; R₁ is the reduction ratio; τ_g is the torque output by the motor reducer.

Since the motor rotor and the gear reducer are rigidly connected, the following relationship is used:

$\begin{array}{ll}\frac{{\ddot{\theta }}_r}{{\ddot{\theta }}_g}=\frac{{\dot{\theta }}_r}{{\dot{\theta }}_g}=\frac{{\theta }_r}{{\theta }_g}=R_1& (3)\end{array}$

where θ_r and θ_g are the motor rotor angle and the gear reducer angle respectively. Combined with Equations (1–4) can be obtained:

$\begin{array}{ll}\left(J_r+\frac{J_g}{R_1^2}\right){\ddot{\theta }}_r+\left(b_r+\frac{b_g}{R_1^2}\right){\dot{\theta }}_r={\tau }_m-\frac{{\tau }_g}{R_1}& (4)\end{array}$

The schematic diagram of the actuator from the motor combination to the output is shown in Figure 3.

FIGURE 3

Figure 3. Schematic diagram of the actuator.

The dynamic equation of the outer cylinder section is:

$\begin{array}{ll}{J}_w{\ddot{\theta }}_w+b_w{\dot{\theta }}_w=R_2{\tau }_g-{\tau }_k& (5)\end{array}$

where: J_w and b_w are the moment of inertia and damping of the outer drum, respectively; ${\ddot{\theta }}_w$ and ${\dot{\theta }}_w$ are the angular velocity and angular acceleration of the output shaft of the outer drum of the non-linear stiffness drive, respectively; R₂ is the reduction ratio of the wire drive, and the relationship between the angular velocity and the angular velocity of the outer cylinder is:

$\begin{array}{ll}\frac{{\ddot{\theta }}_g}{{\ddot{\theta }}_w}=\frac{{\dot{\theta }}_g}{{\dot{\theta }}_w}=\frac{{\theta }_g}{{\theta }_w}=R_2& (6)\end{array}$

where θ_w is the angle of rotation of the outer cylinder for the non-linear stiffness drive, simultaneous Equations (4–6) can obtain:

$\begin{array}{l}\left(J_r+\frac{1}{{R_1}^2}{J}_g+\frac{J_w}{{R_1}^2{R_2}^2}\right){\ddot{\theta }}_r+\left(b_r+\frac{1}{{R_1}^2}{b}_g+\frac{b_w}{{R_1}^2{R_2}^2}\right){\dot{\theta }}_r\\ ={\tau }_m-\frac{{\tau }_k}{R_1{R}_2}& (7)\end{array}$

Then the equivalent dynamic equation of the motor assembly to the elastic part is:

$\begin{array}{ll}{J}_{eq}{\ddot{\theta }}_r+b_{eq}{\dot{\theta }}_r={\tau }_m-\frac{{\tau }_k}{R_1{R}_2}& (8)\end{array}$

where $J_{eq}=J_r+\frac{1}{{R_1}^2}{J}_g+\frac{J_w}{{R_1}^2{R_2}^2}$ is the actuator equivalent inertia and $b_{eq}=b_r+\frac{1}{{R_1}^2}{b}_g+\frac{b_w}{{R_1}^2{R_2}^2}$ is the actuator equivalent damping.

The dynamic equation of the outer cylinder part is:

$\begin{array}{ll}{J}_e{\ddot{\theta }}_e+b_e{\dot{\theta }}_e={\tau }_k-{\tau }_e& (9)\end{array}$

where: J_e and b_e are the moment of inertia and damping of the external load, respectively; τ_e is the output torque of the drive; ${\ddot{\theta }}_e$ and ${\dot{\theta }}_e$ are the angular velocity and angular acceleration of the external load, respectively.

Ekf Based Controller

Equation of State

It can be seen from the above formula that the dynamic equation from the motor to the output shaft without considering the external torque input is:

$\begin{array}{ll}{J}_e{\ddot{\theta }}_e+b_e{\dot{\theta }}_e=R_1{R}_2({\tau }_m-J_{eq}{\ddot{\theta }}_r-b_{eq}{\dot{\theta }}_r)& (10)\end{array}$

The EKF based PD controller is shown in Figure 4.

FIGURE 4

Figure 4. EKF based PD controller.

In the control system, τ_m can be considered to consist of two parts:

$\begin{array}{ll}{\tau }_m={\tau }_{dy}+{\tau }_d& (11)\end{array}$

where τ_dy is the part consumed by the equilibrium dynamics, and its expression is:

$\begin{array}{ll}{\tau }_{dy}-J_{eq}{\ddot{\theta }}_r-b_{eq}{\dot{\theta }}_r=0& (12)\end{array}$

τ_d is the torque required for the end output, which is output by the PD controller. The design expression is as follows:

$\begin{array}{ll}{\tau }_d=K_p({\theta }_e-{\theta }_{exp})+K_d({\dot{\theta }}_e-{\dot{\theta }}_{exp})& (13)\end{array}$

where K_p is the proportional stiffness coefficient and K_d is the differential damping coefficient, substituting (11–13) into (10), we can get:

$\begin{array}{l}{J}_e{\ddot{\theta }}_e+b_e{\dot{\theta }}_e=R_1{R}_2[J_{eq}{\ddot{\theta }}_r+b_{eq}{\dot{\theta }}_r+K_p({\theta }_e-{\theta }_{\exp })+\\ K_d({\dot{\theta }}_e-{\dot{\theta }}_{\exp })-J_{eq}{\ddot{\theta }}_r-b_{eq}{\dot{\theta }}_r]& (14)\end{array}$

It can be simplified to:

$\begin{array}{ll}{\ddot{\theta }}_e+a_1{\dot{\theta }}_e+a_2{\theta }_e=b_0+b_1{\dot{\theta }}_{exp}+b_2{\theta }_{exp}& (15)\end{array}$

where $a_1=\frac{b_e-R_1{R}_2{K}_d}{J_e}$; $a_2=\frac{R_1{R}_2{K}_p}{-J_e}$; b₀ = 0; $b_1=\frac{R_1{R}_2{K}_d}{-J_e}$; $b_2=\frac{R_1{R}_2{K}_p}{-J_e}$.

The formula is a typical input-output equation with a derivative term, so the state variables are chosen as follows:

$\begin{array}{l}{\theta }_1={\theta }_e-b_0{\theta }_{exp}\\ {\theta }_2={\dot{\theta }}_1-h_1{\theta }_{exp}& (16)\end{array}$

where h₁ = b₁ − a₁b₀, then the equation of state of the system is:

$\begin{array}{l}{\dot{\theta }}_1={\theta }_2+h_1{\theta }_{exp}\\ {\dot{\theta }}_2=-a_2{\theta }_1-a_1{\theta }_2+h_2{\theta }_{exp}& (17)\end{array}$

where h₂ = (b₂ − a₂b₀) − a₁h₁, rewritten into a matrix form:

$\begin{array}{ll}\left[\begin{array}{c}{\dot{\theta }}_1\\ {\dot{\theta }}_2\end{array}\right]=\left[\begin{array}{cc}0& 1\\ -a_2& -a_1\end{array}\right]\left[\begin{array}{c}{\theta }_1\\ {\theta }_2\end{array}\right]+\left[\begin{array}{c}{h}_1\\ h_2\end{array}\right]{\theta }_{exp}& (18)\end{array}$

Ekf State Observer Design

According to the control frame we designed in the previous section, we can see that the PD position controller based on EKF requires the output shaft angle and angular velocity to be the feedback amount. In order to solve the sensor's measurement interference, cost, and structural design issues, we use the EKF state observer to predict the angle of the output shaft and the angular velocity. The inputs are only the angle and angular velocity of the motor. The angular acceleration is obtained from the first derivative of the angular velocity and filtered by a low-pass filter to eliminate high-frequency interference.

According to [29], the relationship between the output torque of the elastic component and the rotor angle of the motor and the output angle of the shutdown section is as follows:

$\begin{array}{l}{\tau }_k=0.15{(\frac{{\theta }_r}{R_1{R}_2}-{\theta }_e)}^5-0.23{(\frac{{\theta }_r}{R_1{R}_2}-{\theta }_e)}^4+\\ \hfill 1.78{(\frac{{\theta }_r}{R_1{R}_2}-{\theta }_e)}^3+0.67(\frac{{\theta }_r}{R_1{R}_2}-{\theta }_e)& (19)\end{array}$

Then the overall dynamic equation can be written as:

$\begin{array}{l}{R}_1{R}_2({\tau }_m-J_{eq}{\ddot{\theta }}_r+b_{eq}{\dot{\theta }}_r)=0.15{(\frac{{\theta }_r}{R_1{R}_2}-{\theta }_e)}^5-\\ 0.23{(\frac{{\theta }_r}{R_1{R}_2}-{\theta }_e)}^4+1.78{(\frac{{\theta }_r}{R_1{R}_2}-{\theta }_e)}^3+0.67(\frac{{\theta }_r}{R_1{R}_2}-{\theta }_e)& (20)\end{array}$

$\begin{array}{l}{J}_e{\ddot{\theta }}_e+b_e{\dot{\theta }}_e=0.15{(\frac{{\theta }_r}{R_1{R}_2}-{\theta }_e)}^5-0.23{(\frac{{\theta }_r}{R_1{R}_2}-{\theta }_e)}^4+\\ 1.78{(\frac{{\theta }_r}{R_1{R}_2}-{\theta }_e)}^3+0.67(\frac{{\theta }_r}{R_1{R}_2}-{\theta }_e)& (21)\end{array}$

According to the formula, it can be seen that the experimental platform of this paper is a typical non-linear system. According to the EKF observation method in document [27], combined with the control objectives of this paper, the state variables are defined as:

$\begin{array}{ll}x={\left[\begin{array}{cccc}{x}_1& x_2& x_3& x_4\end{array}\right]}^T={\left[\begin{array}{cccc}{\theta }_e& {\dot{\theta }}_e& {\theta }_r& {\dot{\theta }}_r\end{array}\right]}^T& (22)\end{array}$

Deriving it to time t and substituting it into the dynamics equation, we get the state function f(x):

$\begin{array}{ll}f(x)=\frac{\partial x}{\partial t}=\left[\begin{array}{c}{\dot{\theta }}_e\\ M\\ {\dot{\theta }}_r\\ N\end{array}\right]& (23)\end{array}$

where $M=\frac{1}{J_e}({\tau }_k-b_e{\dot{\theta }}_e)$; $N=\frac{1}{J_{eq}}\left({\tau }_m-b_{eq}{\dot{\theta }}_r-\frac{{\tau }_k}{R_1{R}_2}\right)$ We can obtain state function by partial differentiation of equation (23):

$\begin{array}{ll}F(t)=\frac{\partial f(x)}{\partial x}=\left[\begin{array}{cccc}0& 1& 0& 0\\ F_1& F_2& F_3& 0\\ 0& 0& 0& 1\\ F_4& 0& F_5& F_6\end{array}\right]& (24)\end{array}$

where $F_1=\frac{1}{J_e}\blacksquare \frac{\partial {\tau }_k}{\partial {\theta }_e}$; $F_2=-\frac{b_e}{J_e}$; $F_3=\frac{1}{J_e}\blacksquare \frac{\partial {\tau }_k}{\partial {\theta }_r}$; $F_4=-\frac{1}{J_{eq}{R}_1{R}_2}\blacksquare \frac{\partial {\tau }_k}{\partial {\theta }_e}$; $F_5=-\frac{1}{J_{eq}{R}_1{R}_2}\blacksquare \frac{\partial {\tau }_k}{\partial {\theta }_r}$; $F_6=-\frac{b_{eq}}{J_{eq}}$

Define the observation vector as:

$\begin{array}{ll}h(x)=\left[\begin{array}{c}{x}_3\\ x_4\end{array}\right]=\left[\begin{array}{c}{\theta }_r\\ {\dot{\theta }}_r\end{array}\right]& (25)\end{array}$

Then the state observation matrix is:

$\begin{array}{ll}H(t)=\frac{\partial h(x)}{\partial x}=\left[\begin{array}{cccc}0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]& (26)\end{array}$

The EKF iteration formula is as follows:

$\begin{array}{ll}\dot{\widehat{x}}=f(\widehat{x},{\tau }_m)+G(t)(h(x)-h(\widehat{x}))& (27)\end{array}$

$\begin{array}{l}\dot{P}(t)=F(t)P(t)+P(t)F^T(t)+Q(t)-\\ P(t)H^T(t)R^{-1}(t)H(t)P(t)& (28)\end{array}$

$\begin{array}{ll}G(t)=P(t)H^T(t)R^{-1}(t)& (29)\end{array}$

where $\widehat{x}$ is the predicted estimate of x; Q(t) and R(t) are the process noise and measurement noise obeying the Gaussian distribution; G(t) is the extended Kalman gain, and P(t) is the predicted error covariance. Without external disturbance, we can accurately capture the information we need to know through the sensor, but in the presence of external noise and unknown factors, our prediction will be biased. After each prediction, the EKF state observer adds new uncertainty to establish a connection with external disturbances, that is, the measurement covariance R(t) and system covariance Q(t) obeying the Gaussian distribution. The end of the robot may be affected by other disturbances. By establishing different observation matrices, we can reasonably estimate and compensate for the disturbance force experienced by the robot joints.

Lyapunov Stability Analysis

According to the research of extended Kalman filter, the stability of the control system of the flexible joint robot proposed in this paper is that the overall PD control system is stable, and the EKF observer is stable. The system stability analysis in this paper is divided into the following two steps:

Step 1: Proof of PD controller stability.

The Lyapunov method is used to prove the stability of the controller. Therefore, the previous system state Equation (18) can be written as:

$\begin{array}{ll}\dot{\theta }=A\theta +B{\theta }_{exp}& (30)\end{array}$

where $\theta \text{=}{\left[\begin{array}{cc}{\theta }_{\text{1}}& {\theta }_{\text{2}}\end{array}\right]}^T;A\text{=}\left[\begin{array}{cc}0& 1\\ -a_2& -a_1\end{array}\right];B\text{=}\left[\begin{array}{c}{h}_1\\ h_2\end{array}\right]$

Define the Lyapunov equation as:

$\begin{array}{llll}V(\theta )& \text{=}& {\theta }^T\text{U}\theta & (31)\end{array}$

$\begin{array}{ll}{A}^{\tau }\text{U}+\text{U}A=-E& (32)\end{array}$

where E is unit matrix, then U can be contained:

$\begin{array}{ll}U=\left[\begin{array}{cc}\frac{a_1^2+a_2^2+a_2}{2a_1{a}_2}& \frac{1}{2a_2}\\ \frac{1}{2a_2}& \frac{1+a_2}{2a_1{a}_2}\end{array}\right]& (33)\end{array}$

It is a positive definite matrix. Then the derivative of the Lyapunov equation is:

$\begin{array}{l}\dot{V}(\theta )={\dot{\theta }}^{T}U\theta +{\theta }^{T}U\dot{\theta }=\frac{a_1^2+a_2^2+a_2}{a_1{a}_2}{\dot{\theta }}_1{\theta }_1\\ +\frac{1}{a_2}({\dot{\theta }}_2{\theta }_1+{\dot{\theta }}_1{\theta }_2)+\frac{1+a_2}{a_1{a}_2}{\dot{\theta }}_2{\theta }_2& (34)\end{array}$

Through the adjustment of the PD parameters, it can make $\dot{V}(\theta )<0$. The PD controller system is stable, and the intermediate calculation process will not be described in detail herein.

Step 2: Proof of EKF observer stability

Defining observation error: $\mu =x-\widehat{x}$.

Expand f(x) and h(x):

$\begin{array}{ll}f(x)-f(\widehat{x})=\text{F}(t)\mu +\alpha & (35)\end{array}$

$\begin{array}{ll}h(x)-\text{h}(\widehat{x})=\text{H}(t)\mu +\beta & (36)\end{array}$

where α and β are the higher order terms of μ, then:

$\begin{array}{ll}\dot{\mu }=\left[F(t)-G(t)H(t)\right]\mu +\alpha -G(t)\beta & (37)\end{array}$

Define the Lyapunov equation as:

$\begin{array}{ll}W={\mu }^T\Pi \mu & (38)\end{array}$

where Π = P⁻¹, then:

$\begin{array}{l}\dot{W}={\mu }^{\text{T}}\dot{\Pi }\mu +{\mu }^T(\text{F}-\text{GH})\Pi \mu +\\ {\mu }^T\Pi (\text{F}-\text{GH})\mu +2{\mu }^T\Pi (\alpha -\text{G}\beta )& (39)\end{array}$

Assumption: $\Vert \alpha \Vert \le k_{\alpha }{\Vert \mu \Vert }^2$, $\Vert \beta \Vert \le k_{\beta }{\Vert \mu \Vert }^2$,$\Vert \text{H}\Vert \le \bar{h}$, $\underset{-}{p}E\le P\le \bar{p}E$,$\underset{-}{q}E\le \text{Q}$,$\underset{-}{r}E\le R$, where k_α, k_β, $\bar{h}$, $\underset{-}{p}$, $\bar{p}$, $\underset{-}{q}$ and $\underset{-}{r}$ are positive constants.

Lemma: According to the assumptions, there are ε > 0 and κ > 0, then:

$\begin{array}{ll}{\mu }^{\tau }\Pi \alpha -{\mu }^{\tau }\Pi \text{G}\beta \le \kappa {\Vert \mu \Vert }^3& (40)\end{array}$

for any ‖μ‖ that satisfies ‖μ‖ ≤ ε

Proof: According to Π = P⁻¹ and assumption, it can be obtained that:

$\begin{array}{ll}\frac{1}{\bar{p}}{\Vert \mu \Vert }^2\le W\le \frac{1}{{\displaystyle \underset{-}{p}}}{\Vert \mu \Vert }^2& (41)\end{array}$

Using triangular inequalities, G = PH^TR⁻¹, Π = P⁻¹, and ‖μ‖ ≤ ε, it can be obtained that:

$\begin{array}{ll}\Vert {\mu }^{\tau }\Pi \alpha -{\mu }^{\tau }\Pi G\beta \Vert \le \Vert {\mu }^{\tau }\Pi \alpha \Vert +\Vert {\mu }^{\tau }{\text{H}}^{\tau }{R}^{-1}\beta \Vert & (42)\end{array}$

Inequality can be obtained as follows according to assumption:

$\begin{array}{llll}\Vert {\mu }^{\tau }\Pi \alpha -{\mu }^{\tau }\Pi \text{G}\beta \Vert \le \Vert \mu \Vert \frac{k_{\alpha }}{{\displaystyle \underset{-}{p}}}{\Vert \mu \Vert }^2& +& \Vert \mu \Vert \frac{\bar{h}{k}_{\beta }}{{\displaystyle \underset{-}{r}}}{\Vert \mu \Vert }^2& (43)\end{array}$

$\begin{array}{ll}\kappa =\frac{k_{\alpha }}{{\displaystyle \underset{-}{p}}}+\frac{\bar{h}{k}_{\beta }}{{\displaystyle \underset{-}{r}}}& (44)\end{array}$

According to the lemma and G = PH^τR⁻¹, we can obtain that:

$\begin{array}{ll}\dot{W}\le -2iW+\left(-\frac{{\displaystyle \underset{-}{q}}}{{\bar{p}}^2}+2\kappa \Vert \mu \Vert \right){\Vert \mu \Vert }^2& (45)\end{array}$

where i > 0, for any ‖μ‖ that satisfies $\Vert \mu \Vert \le {\epsilon }^{\prime }=min\left(\epsilon ,\frac{\underset{-}{q}}{4\kappa {\bar{p}}^2}\right)$, we can obtain that:

$\begin{array}{ll}\dot{W}(t)\le -\frac{{\displaystyle \underset{-}{q}}}{2{\bar{p}}^2}{\Vert \mu (t)\Vert }^2-2iW(t)=(-2i-\frac{{\displaystyle \underset{-}{q}}{\displaystyle \underset{-}{p}}}{2{\bar{p}}^2})W(t)& (46)\end{array}$

By using separation variable method, we can obtain that

$\begin{array}{ll}W(t)\le W(0)e^{(-2i(-2i-\frac{{\displaystyle \underset{-}{q}}{\displaystyle \underset{-}{p}}}{2{\bar{p}}^2})t}& (47)\end{array}$

so Ẇ(t) < 0. According to the inequality $\frac{1}{\bar{p}}{\Vert \mu \Vert }^2\le W\le \frac{1}{\underset{-}{p}}{\Vert \mu \Vert }^2$, we can get the solution:

$\begin{array}{ll}\Vert \mu (t)\Vert \le \sqrt{\frac{\bar{p}}{{\displaystyle \underset{-}{p}}}}\Vert \mu (0)\Vert e^{-\left(i+\frac{{\displaystyle \underset{-}{q}}{\displaystyle \underset{-}{p}}}{4{\bar{p}}^2}\right)t}& (48)\end{array}$

That is, the EKF state observer is exponentially stable.

In summary, step 1 and step 2 respectively prove that the PD control system is stable and the EKF state observer is stable. Therefore, the stability of the incomplete state feedback control system of the entire flexible joint robot is proved.

Experimental Results

Experimental Setup

In this part, the proposed control algorithm is applied to the prototype to prove the feasibility and stability of the control algorithm to compare with the sensor-based PD trajectory controller under the same experimental conditions.

A 64-bit-Windows-8.1-based host computer with an Intel Core i7 processor @2.40 GHz and 8-GB RAM is used to run the Kalman estimation and calculate the input torque to the motor. The control algorithm is able to operate on an execution rate of 1 kHz using Visual C++ 2010, which is enough for real-time applications. The DSP board is used to read, process, and calculate the signal from the motor encoder and transmit it to the computer, that is, obtain the real-time position information of the motor through the QEP module of the DSP chip. Then the results calculated by the host computer are sent to the DSP board through a RS232 serial port. The DSP board then converts the input torque command into a PWM wave signal to drive the motor, and the A-D electromagnetic tracking system (trakSTAR, produced by NDI) is used to measure the position of the robot end. Through the USB cable, the location data is sent to the host for comparative verification of the experimental results. The entire experimental platform is shown in Figure 5.

FIGURE 5

Figure 5. The entire experimental platform.

Experimental Data

For the fairness of the experiment and the validity of the comparison verification, the experiment was carried out in the same environment using the same machine, and the same PD controller parameters were used. The desired trajectory is a closed circular trajectory. The trajectory tracking results are shown in Figure 6.

FIGURE 6

Figure 6. Experimental results (A) EKF-based PD controller experimental result. (B) Sensor-based PD controller experimental result.

To further analyze the experimental data, define the error mean square error:

$\begin{array}{ll}RMSE=\sqrt{\left({\displaystyle \sum _{t=0}^T}{\Vert {\xi }_e(t)-{\xi }_{exp}(t)\Vert }_2\right)/T}& (49)\end{array}$

where ξ_e(t) and ξ_exp(t) represent the actual trajectory and the desired trajectory, respectively.

Through the experimental results shown in Figures 6–8, we can see that the mean square value of the trajectory tracking error of the PD controller based on EKF is smaller, and tracking error of each joint is also smaller, which indicates that under the same conditions, the control algorithm proposed in this paper has better control effect.

FIGURE 7

Figure 7. Trajectory tracking variance mean square. (A) EKF-based PD controller trajectory tracking variance mean square. (B) Sensor-based PD controller trajectory tracking variance mean square.

FIGURE 8

Figure 8. Comparison of tracking error of each joint. (A) First joint tracking error comparison. (B) Second joint tracking error comparison. (C) Third joint tracking error comparison.

The EKF observer in the control system can handle external disturbances in real time. In order to prove its ability to handle real-time interference, a force of sinusoidal variation along the direction of the guide rail is applied at the end of the robot. The force changes are shown in Figure 9. The experimental results are shown in Figures 10–12.

FIGURE 9

Figure 9. Disturbance Force.

FIGURE 10

Figure 10. Experimental results with disturbance force (A) EKF-based PD controller experimental result. (B) Sensor-based PD controller experimental result.

FIGURE 11

Figure 11. Trajectory tracking variance mean square with disturbance force. (A) EKF-based PD controller trajectory tracking variance mean square. (B) Sensor-based PD controller trajectory tracking variance mean square.

FIGURE 12

Figure 12. Comparison of tracking error with disturbance force of each joint. (A) First joint tracking error comparison. (B) Second joint tracking error comparison. (C) Third joint tracking error comparison.

From Figures 10–12, we can see that in the case of external disturbance, the control result based on EKF observer is more stable, and the position deviation of the robot end is almost unchanged, which means that the control based on EKF observer can effectively Handle external interference in real time.

Conclusion

In this paper, the trajectory tracking control problem of flexible joint robot is discussed. Aiming at the problem that sensor data acquisition is susceptible to interference, an EKF-based PD controller is proposed. The EKF state observer is designed for the control target to observe the output position, and only the position and speed feedback amount of the motor rotor is needed. And the stability analysis of the designed control system is given according to the Lyapunov method. Finally, the effectiveness and superiority of the proposed control algorithm are verified by experiments.

Data Availability

All datasets generated for this study are included in the manuscript and/or the supplementary files.

Author Contributions

TM: theoretical analysis. ZS: guide control plan. ZX: guide doing experiment. JD: guide writing paper.

Conflict of Interest Statement

The authors declare that the research was conducted in the absence of any commercial or financial relationships that could be construed as a potential conflict of interest.

Acknowledgments

This work was supported by Tianjin Municipal Science and Technology Department Program (Grant No. 17JCZDJC30300), the Natural Science Foundation of China (Project No. 51475322), and the Programme of Introducing Talents of Discipline to Universities (111 Program) under Grant No. B16034.

References

Albu- schäffer, A., Ott, C., and Hirzinger, G. (2007). A unified passivity-based control framework for position, torque and impedance control of flexible joint robots. Int. J. Robot. Res. 26, 23–39. doi: 10.1177/0278364907073776

CrossRef Full Text | Google Scholar

Erler, P., Beckerle, P., Strah, B., and Rinderknecht, S. (2014). Experimental comparison of nonlinear motion control methods for a variable stiffness actuator. Biomed. Robot. Biomech. 2014, 1045–1050. doi: 10.1109/BIOROB.2014.6913918

CrossRef Full Text | Google Scholar

Ge, S., Lee, T., and Tan, E. (1998). Adaptive neural networks control of flexible joint robots based on feedback linearization. Int. J. Syst. Sci. 29, 623–635

Google Scholar

Grimm, M. (1990). Robustness analysis of nonlinear decoupling for elastic-joint robots. IEEE Trans. Robot. Automat. 6, 373–377.

Google Scholar

Grioli, G., Wolf, S., Garabin, M., Catalano, M., Burdet, E., Caldwell, D., et al. (2015). Variable stiffness actuators: the user's point of view. Int. J. Robot. Res. 34, 727–743. doi: 10.1177/0278364914566515

CrossRef Full Text | Google Scholar

Ham, R., Sugar, T., Vanderborght, B., Hollander, K. W., and Lefeber, D. (2009). Compliant actuator design. IEEE Robot. Automat. Magaz. 16:8194. doi: 10.1109/MRA.2009.933629

CrossRef Full Text

Han, J. (2009). From PID to active disturbance rejection control. IEEE Trans. Indus. Electron. 56, 900–906.

Google Scholar

Hogan, N. (1985). Impedance control: an approach to manipulation: part I—theory. Trans. ASME J. Dyn. Syst. Meas. Control 107, 304–313.

Google Scholar

Hurst, W., Chestnutt, E., and Rizzi, A. (2004). “An actuator with physically variable stiffness for highly dynamic legged locomotion,” in IEEE International Conference on Robotics and Automation, ICRA 2004, April 26 - May 1, 2004, New Orleans, LA, 4662–4667.

Google Scholar

Ider, S., and Ozgoren, M. (2000). Trajectory tracking control of flexible-joint robots[J]. Comput. Struct. 76, 757–763. doi: 10.3389/fnbot.2019.00025

CrossRef Full Text | Google Scholar

Jafari, A., Tsagarakis, N. G., and Caldwell, G. (2011). “AwAS-II: a new actuator with adjustable stiffness based on the novel princi-ple of adaptable pivot point and variable lever ratio,” in IEEE International Conference on Robotics and Automation, Piscat-away, NJ: IEEE Press, 4638–4643.

Google Scholar

Jamwal, K., Hussain, S., Ghayesh, H., Rogozina, S. V., et al. (2016). Impedance control of an intrinsically compliant parallel ankle rehabilitation robot. IEEE Trans. Indus. Electron. 63, 3638–3647. doi: 10.1109/TIE.2016.2521600

CrossRef Full Text | Google Scholar

Lan, S., and Song, Z. (2016). Design of a new nonlinear stiffness compliant actuator and its error compensation method. J. Robot. 2016:7326905. doi: 10.1155/2016/7326905

CrossRef Full Text | Google Scholar

Lightcap, C. A., and Banks, S. A. (2010). An extended Kalman filter for real-time estimation and control of a rigid-link flexible-joint manipulator. IEEE Trans. Control Syst. Technol. 18, 91–103. doi: 10.1109/TCST.2009.2014959

CrossRef Full Text | Google Scholar

Losey, P., Erwin, A., Mcdonald, G., Sergi, F., and O'Malley, M. K. (2016). A time-domain approach to control of series elastic actuators: adaptive torque and passivity-based impedance control. IEEE/ASME Trans. Mech. 21, 2085–2096. doi: 10.1109/TMECH.2016.2557727

CrossRef Full Text | Google Scholar

Nakao, M., Ohnishi, K., and Miyachi, K. (1987). “A robust decentralized joint control based on interference estimation,” in IEEE International Conference on Robotics and Automation. Piscataway, NJ: IEEE Press, 326–331.

Google Scholar

Pan, Y., Wang, H., Li, X., and Yu, H. (2018). Adaptive command-filtered backstepping control of robot arms with compliant actuators. IEEE Trans. Control Syst. Technol. 26, 1149–1156. doi: 10.1109/TCST.2017.2695600

CrossRef Full Text | Google Scholar

Reif, K., Sonnemann, E., and Unbehauen, R. (1998). An EKF-based non-linear observer with a prescribed degree of stability. Automatica 34, 1119–1123.

Google Scholar

Sariyildiz, E., Chen, G., and Yu, H. (2015). “Robust position control of a novel series elastic actuator via disturbance observer,” in IEEE/RSJ International Conference on Intelligent Robots and Systems, Piscataway, NJ: IEEE Press, 54235428.

Sariyildiz, E., and Ohnishi, K. (2013). Analysis the robustness of control systems based on disturbance observer. Int. J. Control 86, 1733–1743. doi: 10.1109/AMC.2014.6823253

CrossRef Full Text | Google Scholar

Schiavi, R., Grioli, G., and Sen, S. (2008). “VSA-III: a novel proto-type of variable stiffness actuator for safe and performing robots interacting with humans,” in IEEE International Conference on Robotics and Automation, Piscataway, NJ: IEEE Press, 71–2176.

Sira-ramirez, H., and Spong, W. (1988). Variable structure control of flexible joint manipulator. Int. J. Robot. Automat. 3, 57–64.

Google Scholar

Spong, W. (1987). Modeling and control of elastic joint robots. ASME J. Dyn. Syst. Meas. Control 109, 310–319.

Google Scholar

Torreaiba, R., and Udelman, B. (2016). Design of cam shape for maximum stiffness variability on a novel compliant actuator using differential evolution. Mech. Mach. Theory 9 5, 114–124. doi: 10.1016/j.mechmachtheory.2015.09.002

CrossRef Full Text

Wolf, S., and Hirzinger, G. (2008). “A new variable stiffness design: Matching requirements of the next robot generation,” in IEEE International Conference on Robotics and Automation, Piscataway, NJ: IEEE Press, 17411–17746.

Zaher, H., and Megahed, M. (2015). Joints flexibility effect on the dynamic performance of robots. Robotica 33, 1424–1445. doi: 10.1017/S0263574714000848

CrossRef Full Text | Google Scholar

Zeman, V., Patel, R., and Khorasani, K. (1997). Control of flexible-joint robot using neural networks. IEEE Trans. Control Syst. Technol. 5, 453–462.

Google Scholar

Zhu, W., and Schutter, J. (1999). Adaptive control of mixed rigid/flexible joint robot manipulators based on virtual decomposition. IEEE Trans. Robot. Automat. 15, 310–317.

Google Scholar

Zhu, Z., He, Y., Qi, J., Han, J., et al. (2012). “A new disturbance observation based internal model controller,” in IEEE International Conference on Information and Automation. (Shenyang), 518–523.

Google Scholar