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 flexiblejoint 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 modelindependence, 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 closedloop 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
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.
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.
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 FIGURE 4 | EKF based PD controller.
Frontiers in Neurorobotics | www.frontiersin.org equation of the rotor of the motor is: where J r and b r are the moment of inertia and damping of the rotor of the motor respectively;θ r andθ 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: where J g and b g are the moment of inertia and damping of the motor reducer respectively;θ g andθ g are the angular velocity and angular acceleration of the motor reducer respectively; R 1 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: where θ r and θ g are the motor rotor angle and the gear reducer angle respectively. Combined with Equations (1-4) can be obtained: The schematic diagram of the actuator from the motor combination to the output is shown in Figure 3.
The dynamic equation of the outer cylinder section is: where: J w and b w are the moment of inertia and damping of the outer drum, respectively;θ w andθ w are the angular velocity and angular acceleration of the output shaft of the outer drum of the non-linear stiffness drive, respectively; R 2 is the reduction ratio of the wire drive, and the relationship between the angular velocity and the angular velocity of the outer cylinder is: where θ w is the angle of rotation of the outer cylinder for the nonlinear stiffness drive, simultaneous Equations (4-6) can obtain: Then the equivalent dynamic equation of the motor assembly to the elastic part is: where J eq = J r + 1 R 1 2 J g + J w R 1 2 R 2 2 is the actuator equivalent inertia and b eq = b r + 1 R 1 2 b g + b w R 1 2 R 2 2 is the actuator equivalent damping. The dynamic equation of the outer cylinder part is: 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; θ e andθ e are the angular velocity and angular acceleration of the external load, respectively. Frontiers in Neurorobotics | www.frontiersin.org

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: The EKF based PD controller is shown in Figure 4. In the control system, τ m can be considered to consist of two parts: whereτ dy is the part consumed by the equilibrium dynamics, and its expression is: τ d is the torque required for the end output, which is output by the PD controller. The design expression is as follows: where K p is the proportional stiffness coefficient and K d is the differential damping coefficient, substituting (11-13) into (10), we can get: (14) It can be simplified to: where The formula is a typical input-output equation with a derivative term, so the state variables are chosen as follows: where h 1 = b 1 − a 1 b 0 ,then the equation of state of the system is: where h 2 = (b 2 − a 2 b 0 ) − a 1 h 1 , rewritten into a matrix form:

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: FIGURE 9 | Disturbance Force.
Then the overall dynamic equation can be written as: 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: Deriving it to timetand substituting it into the dynamics equation, we get the state function f (x): where M = 1 J e τ k − b eθe ; N = 1 J eq τ m − b eqθr − τ k R 1 R 2 We can obtain state function by partial differentiation of equation (23): Define the observation vector as: Then the state observation matrix is: The EKF iteration formula is as follows: wherex 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.

P(t) = F(t)P(t) + P(t)F
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:θ where θ = θ 1 θ 2 T ; A= 0 1 −a 2 −a 1 ; B= h 1 h 2 Define the Lyapunov equation as: where E is unit matrix, thenUcan be contained: It is a positive definite matrix. Then the derivative of the Lyapunov equation is: Uθ + θ T Uθ = a 2 1 + a 2 2 + a 2 a 1 a 2θ 1 θ 1 + 1 a 2 (θ 2 θ 1 +θ 1 θ 2 ) + 1 + a 2 a 1 a 2θ 2 θ 2 Through the adjustment of the PD parameters, it can makė V(θ )<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: where α and β are the higher order terms of µ, then: Define the Lyapunov equation as: where = P −1 ,then: and r − are positive constants.
Lemma: According to the assumptions, there are ε > 0 and κ > 0, then: for any µ that satisfies µ ≤ ε Proof: According to = P −1 and assumption, it can be obtained that: Using triangular inequalities, G=PH T R −1 , Π = P -1 , and µ ≤ ε , it can be obtained that: Inequality can be obtained as follows according to assumption: According to the lemma and G = PH τ R −1 , we can obtain that: wherei > 0, for any µ that satisfies µ ≤ ε ′ = min ε, q − 4κp 2 , we can obtain that: By using separation variable method, we can obtain that soẆ(t) < 0. According to the inequality 1 we can get the solution: 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 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 realtime 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.

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.
To further analyze the experimental data, define the error mean square error: 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.
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.
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.