# Discrete-Time Sliding Mode Control Coupled with Asynchronous Sensor Fusion for Rigid-Link Flexible-Joint Manipulators

Authors: Guangyue Xue; Xuemei Ren; Kexin Xing; Qiang Chen
Journal: Complexity
 (2021)

---

## Abstract

This paper proposes a novel discrete-time terminal sliding mode controller (DTSMC) coupled with an asynchronous multirate sensor fusion estimator for rigid-link flexible-joint (RLFJ) manipulator tracking control. A camera is employed as external sensors to observe the RLFJ manipulator’s state which cannot be directly obtained from the encoders since gear mechanisms or flexible joints exist. The extended Kalman filter- (EKF-) based asynchronous multirate sensor fusion method deals with the slow sampling rate and the latency of camera by using motor encoders to cover the missing information between two visual samples. In the proposed control scheme, a novel sliding mode surface is presented by taking advantage of both the estimation error and tracking error. It is proved that the proposed controller achieves convergence results for tracking control in the theoretical derivation. Simulation and experimental studies are included to validate the effectiveness of the proposed approach.


---

## Body

## 1. Introduction

In many high-performance robotic manipulator applications, positioning of the end effector (or/and links) is critical as the ultimate goal is to track the desired trajectory. However, the challenge of achieving high accuracy and dynamic performance is increased due to the nonlinear flexibilities found in rigid-link flexible-joint (RLFJ) manipulators. Some researchers have designed observers to estimate states in the robot model, since link positions/velocities are typically not measured in the industrial robot system. Nicosia and Tomei [1, 2] designed a controller combined with the observer which estimates motor positions or/and motor velocities for RLFJ manipulators. Dixon et al. [3] developed a globally adaptive output-feedback tracking controller for the RLFJ dynamics, which is based on the link velocity filter. Globally, output-feedback methods are not easily implemented in the real system because of the requirement of link position measurements which are not frequently measured in real systems. A controller was designed in [4, 5] by using a neural network (NN) observer to estimate link and motor positions/velocities and dynamic parameters. But NN observer-based controllers do not take advantage of motor positions. Kalman filter (KF) and extended Kalman filter (EKF) are utilized to estimate positions of joints or/and end effectors, driving torque, and dynamic parameters for manipulators [6]. Lightcap and Banks [7] designed an EKF-RLFJ controller by using EKF to estimate link and motor positions/velocities. Garcła et al. [8] proposed compliant robot motion controllers by using EKF to fuse wrist force sensors, accelerometers, and joint position sensors. EKF-based sensor fusion was presented by Jassemi-Zargani and Necsulescu [9] to estimate the acceleration for operational space control of a robotic manipulator. However, these reported EKF-based control methods do not discuss the case of asynchronous measurements from multirate sensors for RLFJ manipulators.Observer-based sliding mode control (SMC) is one of the most important approaches to handle systems with uncertainties and nonlinearities [10]. For the RLFJ manipulator system, observer-based SMC of robot manipulators has been widely studied since the state of manipulators (e.g., acceleration velocity of joints) need not always be measured directly [11]. The terminal SMC (TSMC) is used in rigid manipulator control (e.g., robust TSMC and finite-time control) since it has some superior properties compared with SMC such as better tracking precision and fast error convergence [12–14]. In particular, the singularity problem of the TSMC was addressed in [15, 16]. However, most of these papers use the continuous-time dynamic model of the manipulator. Indeed, discrete-time models exist widely in the real digital control system. It has been proved that the discrete-time SMC has technological advances in digital electronics, computer control, and robotic system. Corradinia and Orlando [17] presented a robust DSMC coupled with an uncertainty estimator designed for planar robotic manipulators. However, the flexibilities of joints are not involved in those controller designs.To remedy such limitations, the paper proposes a novel controller, AMSFE-DTSMC, which is implemented based on DTSMC coupled with an asynchronous multirate sensor fusion estimator. The robotic multirate sensor unit contains vision and non-vision-based sensors whose sampling rate and processing time are different. In the proposed scheme, the delayed slow sampling vision measurement is treated as a kind of periodic “out-of-sequence” measurement (OOSM) [18] which is used to update the non-vision-based state estimation in an EKF-based asynchronous multirate sensor fusion algorithm. Using the position and velocity estimation from sensor fusion estimator, the DTSMC is designed by using a novel sliding surface which is implemented by considering estimation error and tracking error. The main contributions of this work are summarized as follows.(i)
We propose a novel tracking control scheme, AMSFE-DTSMC, which is based on the DTSMC coupled with the sensor fusion estimator for a RLFJ manipulator. The sliding surface of AMSFE-DTSMC is designed by utilzing estimation error and tracking control error.(ii)
We construct an asynchronous multirate measurement model for robotic sensors and design a sensor fusion to fuse such asynchronous multirate data for robotic state estimation.This paper is organized as follows. Section2 gives the problem formulation. In Section 3, the multirate sensor data fusion algorithm is presented. Section 4 designs a novel DTSMC for tracking control. The simulation and experimental studies are implemented in Section 5. The paper ends with conclusion about the proposed approach.


## 2. Problem Formulation

In this paper, a robotic manipulator system is given with the sensor unit including joint motor encoders and cameras fixed in the workspace. The tracking control scheme for RLFJ manipulators can be developed by using the robotic state estimate via multisensor fusion. The state of the robot is estimated by these sensors directly and indirectly; however, there are some limitations for a single sensor to obtain precise information. To fuse asynchronous multirate data from different sensors, the dynamic and sensor models are formulated in this section.


### 2.1. Discrete Rigid-Link Flexible-Joint Robot Model

The discrete dynamic model of ann−link RLFJ manipulator can be obtained by the minimization of the action functional suggested by Nicosia [2] as follows:(1a)qk+1=qk+Tq˙k,(1b)q˙k+1−q˙k=M−1q¯,θTKsδk−FLq¯,q,q˙,θT,(1c)Mmq˙mk+1−q˙mk+Fmq˙mkT=Tuk−KsδkT,(1d)δk=qmk−qk,where qk,q˙k and qmk,q˙mk denote the position, velocity of the link and motor angles at k time, respectively, q¯k≅qk+Tq˙k, T is the sampling interval, M⋅ is the invertible inertia matrix which satisfies ξmin≤M≤ξmax, FLq¯,q,q˙,θ=−M−1q¯,θMq¯,θq˙−Mq,θq˙−Tfq,q˙,θ, f⋅ represents centrifugal, Coriolis and gravitational forces; Ks, Mm and Fm are constant, diagonal, positive-definite matrices representing joint stiffness, motor inertia, motor viscous friction, respectively; the joint deflection δk is defined as the difference between the motor and link position and uk denotes the motor torque; the unknown or varying dynamic parameter in the robotic model is defined as θk which satisfy θk+1−θk=Twθk, the dynamics uncertainties of the links and motors are modeled with random variables wlk and wmk.Define a state vector(2)xk=qkTq˙kTqmkTq˙mkTθkTT.The dynamics in equations (1a)–(1d) can be transformed to a state space representation:(3)xk+1=Fk+1,kxk,uk+Γkωk+1,k,where(4)Fk+1,kxk,uk=qk+Tq˙kq˙k−FLkT+M−1q¯KsδkTqmk+Tq˙mkMm−1q˙mk−Fmq˙mkT−KsδkT+Tukθk,Γk=000Mq¯,θ−1000000Mm−1000IT,ωk+1,k=wlkT,wmkT,wθkTT.


### 2.2. Measurement Model

The observation vector is given by(5)hxk=qmkTq˙mkTykTT,where qmk,q˙mkT are the position and velocity of motor angles and yk represents the position of the end effector in image space observed by a fixed camera.By using the standard pinhole camera model, the mapping from the Cartesian space to image space is given by(6)yk=Imrk,where rk denotes the position of the end effector in the Cartesian space and Im⋅ is a transformation function from task space to image space. From the forward kinematics, the position relationship between robotic joints and end effector is described by a transform relationship as follows:(7)rk=Fkqk,where rk is the position of the end effector at kth time and Fk⋅ is a transformation function from the joint space to task space.From the equations (6) and (7), The joint position can be observed by a fixed camera:(8)yk=Ikqk,where Ik⋅=ImFk⋅ is the mapping from the joint space to image space. With the random noise νk=v1Tv2Tkv3Tkk, the measurement equation is given by(9)zik=hkixk+νik,i=c,m,where zik is the measurement of the state from different sensor, i=c denotes the camera measurement, and i=m represents the measurement from motors. We assume that the process noise ωk and measurement noise νk are sampled from the independent and identically distributed white Gaussian noise which satisfies following equations at time tk:(10)Eωk+1,k=0,Eνk=0,EνjωTk+1,k=0,Eωj+1,jωNTk+1,k=Qkδjk,EνjνTk=Rkδjk.Since the measurement of the vision sensor at timetk is obtained from the visual image taken at time step tκ=tk−td, td denotes the delay time. The relation of different sensors’ sampling rates is given by(11)sm=lsc,where sm and sc denote the sampling rate of motor encoders and visual sensors, respectively, and l is a positive integer.We show the sampling rate difference between vision and non-vision measurements in Figure1, where l step lags of vision measurements are described. According to the characteristics of visual sensor, we treat the vision measurements as the periodic l-step lag out-of-sequence measurements.Figure 1
The schematic diagram of the relation between vision and non-vision measurements.Remark 1.
The velocity of joints (or/and end effector) also can be observed by vision sensors,y˙k=Jikqkq˙k, with the Jacobian matrix Jikq=JirJkq mapping from the joint space to the image space. Jir is the image Jacobian matrix, and Jkq denotes the Jacobian matrix from joint space to task space. However, the measurement of velocity is always impaired by the noisy image data with slow sampling rate and the dynamic uncertainties in the RLFJ manipulator system. Therefore, the velocity measurement in the image space is not utilized in the measurement model.Remark 2.
Assume that the camera can cover the entire workspace of the robot. With the prior knowledge of motion planning of the robot, it is basically assumed that there is an one-to-one mapping from the image space to joint space in the real robotic system.


## 2.1. Discrete Rigid-Link Flexible-Joint Robot Model

The discrete dynamic model of ann−link RLFJ manipulator can be obtained by the minimization of the action functional suggested by Nicosia [2] as follows:(1a)qk+1=qk+Tq˙k,(1b)q˙k+1−q˙k=M−1q¯,θTKsδk−FLq¯,q,q˙,θT,(1c)Mmq˙mk+1−q˙mk+Fmq˙mkT=Tuk−KsδkT,(1d)δk=qmk−qk,where qk,q˙k and qmk,q˙mk denote the position, velocity of the link and motor angles at k time, respectively, q¯k≅qk+Tq˙k, T is the sampling interval, M⋅ is the invertible inertia matrix which satisfies ξmin≤M≤ξmax, FLq¯,q,q˙,θ=−M−1q¯,θMq¯,θq˙−Mq,θq˙−Tfq,q˙,θ, f⋅ represents centrifugal, Coriolis and gravitational forces; Ks, Mm and Fm are constant, diagonal, positive-definite matrices representing joint stiffness, motor inertia, motor viscous friction, respectively; the joint deflection δk is defined as the difference between the motor and link position and uk denotes the motor torque; the unknown or varying dynamic parameter in the robotic model is defined as θk which satisfy θk+1−θk=Twθk, the dynamics uncertainties of the links and motors are modeled with random variables wlk and wmk.Define a state vector(2)xk=qkTq˙kTqmkTq˙mkTθkTT.The dynamics in equations (1a)–(1d) can be transformed to a state space representation:(3)xk+1=Fk+1,kxk,uk+Γkωk+1,k,where(4)Fk+1,kxk,uk=qk+Tq˙kq˙k−FLkT+M−1q¯KsδkTqmk+Tq˙mkMm−1q˙mk−Fmq˙mkT−KsδkT+Tukθk,Γk=000Mq¯,θ−1000000Mm−1000IT,ωk+1,k=wlkT,wmkT,wθkTT.


## 2.2. Measurement Model

The observation vector is given by(5)hxk=qmkTq˙mkTykTT,where qmk,q˙mkT are the position and velocity of motor angles and yk represents the position of the end effector in image space observed by a fixed camera.By using the standard pinhole camera model, the mapping from the Cartesian space to image space is given by(6)yk=Imrk,where rk denotes the position of the end effector in the Cartesian space and Im⋅ is a transformation function from task space to image space. From the forward kinematics, the position relationship between robotic joints and end effector is described by a transform relationship as follows:(7)rk=Fkqk,where rk is the position of the end effector at kth time and Fk⋅ is a transformation function from the joint space to task space.From the equations (6) and (7), The joint position can be observed by a fixed camera:(8)yk=Ikqk,where Ik⋅=ImFk⋅ is the mapping from the joint space to image space. With the random noise νk=v1Tv2Tkv3Tkk, the measurement equation is given by(9)zik=hkixk+νik,i=c,m,where zik is the measurement of the state from different sensor, i=c denotes the camera measurement, and i=m represents the measurement from motors. We assume that the process noise ωk and measurement noise νk are sampled from the independent and identically distributed white Gaussian noise which satisfies following equations at time tk:(10)Eωk+1,k=0,Eνk=0,EνjωTk+1,k=0,Eωj+1,jωNTk+1,k=Qkδjk,EνjνTk=Rkδjk.Since the measurement of the vision sensor at timetk is obtained from the visual image taken at time step tκ=tk−td, td denotes the delay time. The relation of different sensors’ sampling rates is given by(11)sm=lsc,where sm and sc denote the sampling rate of motor encoders and visual sensors, respectively, and l is a positive integer.We show the sampling rate difference between vision and non-vision measurements in Figure1, where l step lags of vision measurements are described. According to the characteristics of visual sensor, we treat the vision measurements as the periodic l-step lag out-of-sequence measurements.Figure 1
The schematic diagram of the relation between vision and non-vision measurements.Remark 1.
The velocity of joints (or/and end effector) also can be observed by vision sensors,y˙k=Jikqkq˙k, with the Jacobian matrix Jikq=JirJkq mapping from the joint space to the image space. Jir is the image Jacobian matrix, and Jkq denotes the Jacobian matrix from joint space to task space. However, the measurement of velocity is always impaired by the noisy image data with slow sampling rate and the dynamic uncertainties in the RLFJ manipulator system. Therefore, the velocity measurement in the image space is not utilized in the measurement model.Remark 2.
Assume that the camera can cover the entire workspace of the robot. With the prior knowledge of motion planning of the robot, it is basically assumed that there is an one-to-one mapping from the image space to joint space in the real robotic system.


## 3. Asynchronous Multirate Sensor Fusion Estimation

According to the system model described inSection 2, the robotic link position can be estimated by using the multisensor system. For asynchronous multirate sensors, the sensor fusion method is designed to use the late measurements for updating the current estimated state to get a more accurate estimation in two steps:Step 1: when the vision measurement is unavailable, the robotic state is estimated using the non-vision-based sensors which keep the real-time estimate by recovering the missing information between two vision samples.Step 2: as the delayed vision measurement arrives at every vision sample at timet=tlk,l=1,2,…, the state should be re-estimated to cope with the limitations of other sensors in the absolute position measurement.


### 3.1. Estimation by Using Non-Vision Sensor Measurements

From Figure1, before the k−1th vision frame is available, we have the estimation of the state at time tlk using the non-vision sensor measurements:(12)x^lklk=ExlkZmlk,Plklk=covxlkZmlk,where Zlkm=zm1,zm2,…,zmlk represents all encoder motor measurements up to time tlk. Using the extended Kalman filter (EKF), the estimation at time tlk via the motor encoder measurement is given as follows:(13)x^−lklk=Flk,lk−1x^lk−1lk−1,ulk,(14)Plklk−1=AlkPlk−1lk−1AlkT+WlkQlk−1WlkT,(15)Klk=Plklk−1HlkTHlk×Plklk−1HlkT+Rlk−1,(16)x^lklk=x^−lklk+Klkzmlk−hlkmx^−lklk,0,(17)Plklk=I−KlkHlkPlklk−1,where Alk,Wlk,Hlk, and Vlk are Jacobian matrices and Klk is the correction gain vector. According to above equations, the state and covariance estimates are updated with the measurement with a fast sampling rate.


### 3.2. Re-Estimation via Vision Sensor Measurements

Suppose that at the timetlk the vision measurement at time tκ=tkl−l is obtained. A new estimation x^lkκ+ is calculated using the information about the delayed k−1th vision measurement zcκ. x^lkκ+ is defined with ℤlk=Zlkm,zcκ:(18)x^lkκ+=Exlkℤlk,Plkκ+=covxlkℤlk.The delayed measurementzcκ observed by vision measurement will be used to correct the accumulated estimation errors which are caused by sensors with fast sampling rate. The equations for updating the estimation x^lklk with the delayed vision measurement zcκ are given by(19)x^lkκ+=x^lklk+Pxzlk,κlk×Pzzκlk−1zκ−z^κlk,(20)Plkκ+=Plklk−Pxzlk,κlk×Pzzκlk−1Pxzlk,κlkT,where Pxzlk,κlk represents cross covariance between x^lklk and z^κlk, Pzzκlk is the covariance of z^κlk which denotes the estimation of measurement at time tκ, and(21)Pxzlk,κlk=Ex˜lkz˜cκT,Pxzlk,κlk=Ez˜cκz˜cκT,z^κlk=EzκZlk,with x˜lk=xlk−x^lklk,z˜cκ=zcκ−z^κlkT.Using the EKF,Pxzlk,κlk,Pxzlk,κlk, and z^κlk are obtained by assuming that the function Fk,κ⋅ is invertible. Define Flk,κ−1⋅=Fκ,lk⋅ which denotes the backward transition function to estimate the state back from tlk to tκ. Since the previous state xκ is not affected by present input signal ulk, we give the state relationship between tκ and tlk by(22)xκ=Fκ,lkxlk−ωlk,κ,where the process noise ωlk,κ and covariance Qkl,κ are calculated by(23)ωlk,κ≈Γlkωlk,lk−1+Alk,lk−1Γlk−1ωlk−1,lk−2+⋅⋅⋅+Alk,lk−1Alk−1,lk−2×⋯×Alk−l+2,lk−l+1Γlk−l+1ωlk−l+1,lk−l,Qkl,κ=Eω˜lk,κω˜lk,κT,ω˜lk,κ=ωlk,κ−ω^lk,κ.The estimation in equation (19) can be determined by(24)z^κlk=Hlkmκx^κlk,x^κlk=Fκ,lkx^lklk−ω^lk,κlk.To estimate the process noise as in equations (13)–(17), we have(25)ω^lk,κlk=Qlk,κHlkTPzzlklk−1−1×zlk−zlk−1.Then,Pxzlk,κlk and Pzzκlk are obtained as follows:(26)Pxzlk,κlk=Plk,κlk−Pxωlk,κlkAκ,lkTHκT,Pzzκlk=HκPκlkHκT+Rκ,where the covariances Pκlk and Pxωlk,κlk are derived as follows:(27)Pκlk=Aκ,lkPlklk+Pωωκlk−Pxωlk,κlkTAκ,lkT−Pxωlk,κlk,Pωωκlk=Qlk,κ−Qlk,κHlkT×Pzzlklk−1−1HlkQlk,κT,Pxωlk,κlk=Qlk,κ−Plklk−1HlkT×Pzzlklk−1−1HlkQlk,κT.


### 3.3. Summary of the Fusion Estimate Method

The state of a RLFJ manipulator is estimated using the indirect measurements from asynchronous multirate sensors. The sensor fusion estimate can be implemented in the practical application using a switching mechanism in accordance with sampling time. As shown in Figure1, the update equations are chosen at the different sampling times. The state estimation can be given by(28)x^Fkk=Fk,k−1x^−kk+Kkzmk−hkmx^−kk,0,t≠tlk,x^kk+Pxzk,k−lkPzz−1k−lkzck−l−z^k−lk,t=tlk.Remark 3.
The exponential convergence of the sensor fusion estimate can be proved in the similar way which is presented in [19], which has more detailed conclusion on the stability.


## 3.1. Estimation by Using Non-Vision Sensor Measurements

From Figure1, before the k−1th vision frame is available, we have the estimation of the state at time tlk using the non-vision sensor measurements:(12)x^lklk=ExlkZmlk,Plklk=covxlkZmlk,where Zlkm=zm1,zm2,…,zmlk represents all encoder motor measurements up to time tlk. Using the extended Kalman filter (EKF), the estimation at time tlk via the motor encoder measurement is given as follows:(13)x^−lklk=Flk,lk−1x^lk−1lk−1,ulk,(14)Plklk−1=AlkPlk−1lk−1AlkT+WlkQlk−1WlkT,(15)Klk=Plklk−1HlkTHlk×Plklk−1HlkT+Rlk−1,(16)x^lklk=x^−lklk+Klkzmlk−hlkmx^−lklk,0,(17)Plklk=I−KlkHlkPlklk−1,where Alk,Wlk,Hlk, and Vlk are Jacobian matrices and Klk is the correction gain vector. According to above equations, the state and covariance estimates are updated with the measurement with a fast sampling rate.


## 3.2. Re-Estimation via Vision Sensor Measurements

Suppose that at the timetlk the vision measurement at time tκ=tkl−l is obtained. A new estimation x^lkκ+ is calculated using the information about the delayed k−1th vision measurement zcκ. x^lkκ+ is defined with ℤlk=Zlkm,zcκ:(18)x^lkκ+=Exlkℤlk,Plkκ+=covxlkℤlk.The delayed measurementzcκ observed by vision measurement will be used to correct the accumulated estimation errors which are caused by sensors with fast sampling rate. The equations for updating the estimation x^lklk with the delayed vision measurement zcκ are given by(19)x^lkκ+=x^lklk+Pxzlk,κlk×Pzzκlk−1zκ−z^κlk,(20)Plkκ+=Plklk−Pxzlk,κlk×Pzzκlk−1Pxzlk,κlkT,where Pxzlk,κlk represents cross covariance between x^lklk and z^κlk, Pzzκlk is the covariance of z^κlk which denotes the estimation of measurement at time tκ, and(21)Pxzlk,κlk=Ex˜lkz˜cκT,Pxzlk,κlk=Ez˜cκz˜cκT,z^κlk=EzκZlk,with x˜lk=xlk−x^lklk,z˜cκ=zcκ−z^κlkT.Using the EKF,Pxzlk,κlk,Pxzlk,κlk, and z^κlk are obtained by assuming that the function Fk,κ⋅ is invertible. Define Flk,κ−1⋅=Fκ,lk⋅ which denotes the backward transition function to estimate the state back from tlk to tκ. Since the previous state xκ is not affected by present input signal ulk, we give the state relationship between tκ and tlk by(22)xκ=Fκ,lkxlk−ωlk,κ,where the process noise ωlk,κ and covariance Qkl,κ are calculated by(23)ωlk,κ≈Γlkωlk,lk−1+Alk,lk−1Γlk−1ωlk−1,lk−2+⋅⋅⋅+Alk,lk−1Alk−1,lk−2×⋯×Alk−l+2,lk−l+1Γlk−l+1ωlk−l+1,lk−l,Qkl,κ=Eω˜lk,κω˜lk,κT,ω˜lk,κ=ωlk,κ−ω^lk,κ.The estimation in equation (19) can be determined by(24)z^κlk=Hlkmκx^κlk,x^κlk=Fκ,lkx^lklk−ω^lk,κlk.To estimate the process noise as in equations (13)–(17), we have(25)ω^lk,κlk=Qlk,κHlkTPzzlklk−1−1×zlk−zlk−1.Then,Pxzlk,κlk and Pzzκlk are obtained as follows:(26)Pxzlk,κlk=Plk,κlk−Pxωlk,κlkAκ,lkTHκT,Pzzκlk=HκPκlkHκT+Rκ,where the covariances Pκlk and Pxωlk,κlk are derived as follows:(27)Pκlk=Aκ,lkPlklk+Pωωκlk−Pxωlk,κlkTAκ,lkT−Pxωlk,κlk,Pωωκlk=Qlk,κ−Qlk,κHlkT×Pzzlklk−1−1HlkQlk,κT,Pxωlk,κlk=Qlk,κ−Plklk−1HlkT×Pzzlklk−1−1HlkQlk,κT.


## 3.3. Summary of the Fusion Estimate Method

The state of a RLFJ manipulator is estimated using the indirect measurements from asynchronous multirate sensors. The sensor fusion estimate can be implemented in the practical application using a switching mechanism in accordance with sampling time. As shown in Figure1, the update equations are chosen at the different sampling times. The state estimation can be given by(28)x^Fkk=Fk,k−1x^−kk+Kkzmk−hkmx^−kk,0,t≠tlk,x^kk+Pxzk,k−lkPzz−1k−lkzck−l−z^k−lk,t=tlk.Remark 3.
The exponential convergence of the sensor fusion estimate can be proved in the similar way which is presented in [19], which has more detailed conclusion on the stability.


## 4. Rigid-Link Flexible-Joint Tracking Control

In this section, the discrete-time terminal sliding mode tracking control-based fusion estimation (AMSFE-DTSMC) is presented for rigid-link flexible manipulators whose state is estimated by the sensor fusion method described in the previous section. The controller is designed by using both position and velocity of the link from the sensor fusion estimate. To design the novel controller, the model in (1a)–(1d) can written as(29a)qk+1=qk+Tq˙k,(29b)q˙k+1=q˙k+M−1q¯,θTuk−Fq¯,q,q¯˙m,q˙m,T,θ,where Fq¯,q,q¯˙m,q˙m,T,θ=FLq¯,q,q˙,θT+M−1q¯,θMmq˙mk+1−q˙mk+Fmq˙mk denotes the variable which includes dynamic parameters of links and motors.In order to formulate the tracking control, define the tracking errorq˜tk and estimation error q˜ek at time tk as(30)q˜tk=qk−qdk,q˜ek=qk−q^k,where qdk is the desired position and q^k denotes the estimation position.Define the reference velocity for tracking and estimation:(31)q˙tk=q˙dk+Λtqdk−q^k=q˙dk−Λtq˜tk−q˜ek,q˙ek=q^˙k−Λeqk−q^k=q^˙k−Λeq˜ek,where q^˙k denotes the estimation of q˙k and Λt,Λe are constants and diagonal matrices.Define the filtered variables including the estimation error:(32)q˙˜tk=q˙k−q˙tk=q˜˙tk+Λtq˜tk−q˜ek.Consider the discrete terminal sliding surface as follows:(33)sk=q˙˜tk+λq˜tkp,where λ is a positive constant diagonal parameter matrix and p=p1/p2, in which p1 and p2 are positive odd integers satisfying p2>p1. Motivated by the reaching law presented by Weibing Gao et al. in [20], we use the reaching law for exponential discrete sliding mode control as follows:(34)sk+1=I−Thsk−Tεsgnsk,where sgn⋅ is the signum function, ε>0, and h>0.Since system states cannot be measured directly, parameters which contain variablesqk,q˙k, and θ need to be estimated in the controller design. Assume that the estimate error and uncertainties are bounded, and we have(35)Mq¯,θ=M^q¯,θ+ΔMq¯,θ,Fq¯,q,q¯˙m,q˙m,T,θ=M^Fq¯,q,q¯˙m,q˙m,T,θ+ΔFq¯,q,q¯˙m,q˙m,T,θ,where q¯˙m denotes q˙mk+1; M^q¯,θ=Mq¯^,θ^ and F^q¯,q,q¯˙m,q˙m,T,θ=Fq¯^,q^,q¯˙^m,q˙^m,T,θ^ represent the estimation of dynamic parameters; and ΔMq¯,θ and ΔFq¯,q,q¯˙m,q˙m,T,θ are bounded variables including the estimation error and system uncertainties, which satisfy(36)ΔMq¯,θ≤δM,ΔFq¯,q,q¯˙m,q˙m,T,θ≤δF,where δM and δF are constants.Theorem 1.
Consider the rigid-link flexible-joint manipulator system described by equations (29a) and (29b) and the discrete sliding manifold described by equation (33); by using the reaching law in equation (34), stable control law is designed as(37)uk=M^q¯,θTF^q¯,q,q¯˙m,q˙m,T,θ+q˙tk−q˙tk−1+λq˜tpk−λTq˙˜tk−Λtq˜tk+Λtq˜ek+q˜tkp−Thsk+εksgnsk,where ε and h are positive diagonal matrices which satisfy the following inequations:(38)εk>1TξMΔq˙tk−λTq˙˜tk−Λtq˜tk+Λtq˜ek+q˜tkp−λq˜tpk+1TξF+hsk,(39)T<2sk−ΔFhsk+εk.Proof.
Substituting control law (37) in the rigid-link flexible-joint system equations (29a) and (29b), the error dynamics are obtained:(40)q˜tk+1=q˜tk+Tq˙˜tk−Λtq˜tk+Λtq˜ek,(41)q˙˜tk+1=q˙˜tk+λq˜tkp−λTq˙˜tk−Λtq˜tk+Λtq˜ek+q˜tkp+M−1M^−IF^+Δq˙tk−λTq˙˜tk−Λtq˜tk+Λtq˜ek+q˜tkp−λq˜tpk+M−1M^ΔF−Thsk+εksgnsk.
For simplicity, defineM=M−1M^ which is invertible and M−1=M^−1M. Substituting (40) and (41) into (34) yields(42)sk+1=sk+M−Thsk−εksgnsk+MF^−F+M−IΔq˙tk−λq˜tpk−λTq˙˜tk−Λtq˜tk+Λtq˜ek+q˜tkp.
Stability conditions for the discrete sliding mode control are given by Sarpturk [21]:(43a)sTksk+1−sk<0,(43b)sTksk+1+sk>0.
Combining (42) and (43a), we have(44)sT−MThsk+εksgnsk+MF^−F+M−IΔq˙tk−λTq˙˜tk−TΛtq˜tk+TΛtq˜ek+q˜tkp−λq˜tpk<0.
Ifsk>0, sgnsk=1, and sk+1−sk<0, the sliding gain lower bound can be given as(45)εk>1TI−M−1−λTq˙˜tk−Λtq˜tk+Λtq˜ek+q˜tkp−λq˜tpk+1TI−M−1Δq˙tk+1TF^−M−1F−hsk,Employ ξM and ξF which satisfy I−M−1<ξM, F^−M−1F<ξF, we can obtain εk as follows:(46)εk>1TξMΔq˙tk−λTq˙˜tk−Λtq˜tk+Λtq˜ek+q˜tkp−λq˜tpk+1TξF+hsk.
Ifsk<0, then sgnsk=−1, and sk+1−sk can be calculated by(47)sk+1=sk+MTεk−hsk+MF^−F+M−IΔq˙tk−λq˜tpk−λTq˙˜tk−Λtq˜tk+Λtq˜ek+q˜tkp.
Employεk given by equation (46), and we have(48)sk+1−sk>MThsk−sk+MξF−MF^+F+MξMΔq˙tk−λTq˙˜tk−TΛtq˜tk+TΛtq˜ek+q˜tkp−λq˜tpk−M−IΔq˙tk−λTq˙˜tk−TΛtq˜tk+TΛtq˜ek+q˜tkp−λq˜tpk,where sk+1−sk>0 is obtained by adopting εk according to equation (46). Therefore, equation (46) satisfied the Sarpturk stability (43a).
AssumingM=I, Sarpturk condition (3) is formulated by combining equations (42) and (43b):(49)sk+1+sk=2sk−ΔF−Thsk+εksgnsk.
Ifsk<0, sk+1+sk<0, we have(50)2sk−Thsk−εk−ΔF<0.
Else ifsk>0, sk+1+sk>0, we have(51)2sk−Thsk+εk−ΔF>0.
The sampling timeT which satisfies the following equation can guarantee the condition in (43b):(52)0<T<2sk−ΔFhsk+εk.
From (52), we know that the stability conditions of DTSMC given by (43a) and (43b) are guaranteed by chosen parameters ε and h defined in (38) and (39). Theorem 1 is proved.


## 5. Simulation and Experimental Studies

### 5.1. Simulation Study

The results obtained from the simulation of proposed control scheme on a two-link RLFJ manipulators shown in Figure2 are presented in this section. In the simulation, the aim is make the RLFJ manipulator track desired trajectories q1,d=sinπt and q2,d=cosπt from the initial position q1iq2i=0.5,0.5. The robotic dynamic parameters are given by(53)Mq=M11M12M21m2l22,gq=g1−m2gl2sinq1+q2,C=−m2l1l2sinq22q˙1q˙2+q˙22−m2l1l2sinq2q˙1q˙2,,M11=m1+m2l12+m2l22+2m2l1l2cosq2,M12=M21=m2l22+m2l1l2cosq2,g1=−m1+m2gl1sinq1−m2gl2sinq1+q2,where mi and li are given in Table 1. For i=1,2, mi and li denote mass and length of the ith link. km=diag10,10Num/rad,Mm=diag0.5,0.5kg⋅m2,Fm=diag4,0.5Nm⋅sec/rad.Figure 2
The schematic diagram of robotic manipulator with kinematics parameters.Table 1
The manipulator parameters.
Linkimikglicmqi12.535q10∘∼180∘2320q2−160∘∼160∘In order to demonstrate the influence of estimation for tracking performance, a DTSMC controller without estimation is also simulated. The DTSMC control law is given by(54)uk=M^TF^+q˙dk−q˙dk−1+λq˜tpk−λTq˜˙tk+q˜tkp−hsk−εksgnsk,with a sliding surface sk=q˜˙tk+λq˜tkp which is different from proposal sliding mode surface.Parameters of the proposed controller and DTSMC are selected asT=0.001s, h=diag2,2, λ=diag1.5,1.5,Λt=diag5,5, and ε=diag25,25. θk=0.15 is chosen as a constant model parameter whose initial value is θk=0, p=3, q=5, ξM=diag3,3, and ξF=diag1,1. In the simulation, as shown in Figure 3, we assume that the end effector is observed by a fixed camera whose delayed measurements are used directly to calculate the joint position. The delayed time of slow measurement is 0.125s. The process and measurement noise are chosen as Q=0.01I and R=0.1I. The initial position value of joints (joint1, joint2) for tracking control is given by 0.5rad,0.5rad, and for fusion estimator, it is given by 0.9rad,0.9rad. The initial velocity value for tracking control is given by 0,0, and for estimation, it is given by 0.25rad/s,0.Figure 3
Asynchronous multirate sensor measurements.The estimate errors of position and velocity are plotted in Figure4. The estimate errors of parameter θ are shown in Figure 5. The position tracking of the proposed method for two links is shown in Figures 6 and 7 where the comparative result of SMC without fusion estimator is also plotted. In conclusion, the simulation results clearly indicate that the proposed approach guarantees the convergence of tracking errors and has better tracking accuracy.Figure 4
The estimation errors ofq1,q˙1,q2,q˙2.Figure 5
The estimation error ofθ.Figure 6
Comparative control performances of joint1 between the proposed method and SMC.Figure 7
Comparative control performances of joint2 between the proposed method and SMC.


### 5.2. Experimental Study

To validate the applicability of the proposed control schemes, a single-link flexible-joint manipulator with a fixed camera is employed as the experimental plant which is shown in Figure8. The aim is to make the end effector move along the desired trajectory xd=l1cosqd, yd=l1sinqd with qd=0.4−t/75, and the initial position of joint is qi=0.35rad. Parameters of the single-link robot are measured offline: l1=0.6m and m1=10kg; the measurement errors are Δl1=0.05m and Δm1=0.1kg.Figure 8
The experiment setup.To observe the state of end effector, the calibrated camera is fixed perpendicular to the robot notion plane. The coordinate relation of Cartesian space and image space is shown in Figure9. Camera measurements are obtained from the image sequences shown in Figure 10. We define the position in the image coordinate yk=xmk,ymk given in Figure 11, and the position in Cartesian coordinate is rk=xck,yck. By using the pinhole camera model, we calculate the mapping from the joint space to image space Ik⋅ in equation (14) as follows:(55)fHc−fxmkymk=xckyck=l1cosqkl2sinqk,where Hc is the perpendicular distance between the camera and the robot motion plane and f denotes the focal length of the camera. In the experiment, camera parameters are f=0.035m, 640×512 pixels, and Hc=0.8m. According to the mapping Ik⋅, the joint position can be obtained by calculating the inverse circular trigonometric functions arccosxmkf/l1Hc−f or arcsinymkf/l1Hc−f. The delayed vision measurements xmk,ymk are converted to joint measurements arccosxmkf/l1Hc−f which are shown in Figure 12, where the encoder measurements are also plotted. The position estimation of the joint is calculated by using the proposed fusion estimate method shown as red solid line in Figure 12. To show the performance detail of the fusion estimate method, Figure 13 shows the error of joint position estimation.Figure 9
The coordinate relation of Cartesian space and image space.Figure 10
The image sequences of 10 frames.Figure 11
The trajectories of end effector in image space.Figure 12
The encoder and visual measurements.Figure 13
The position estimation error of the joint.To validate the performance of the fusion estimate-based DTSMC, comparative experiments are implemented in this paper. The proposed controller and a DTSMC without estimator are employed in this test. Parameters are selected asT=0.01s, h=3, λ=2, Λt=4 and ε=20, θk=0.05; p=3, q=5, ξM=0.2 and ξF=2. According to the comparative position tracking performances given in Figure 14, it is obvious that the proposed controller provides a superior behavior.Figure 14
Comparative position tracking errors of the end effector.


## 5.1. Simulation Study

The results obtained from the simulation of proposed control scheme on a two-link RLFJ manipulators shown in Figure2 are presented in this section. In the simulation, the aim is make the RLFJ manipulator track desired trajectories q1,d=sinπt and q2,d=cosπt from the initial position q1iq2i=0.5,0.5. The robotic dynamic parameters are given by(53)Mq=M11M12M21m2l22,gq=g1−m2gl2sinq1+q2,C=−m2l1l2sinq22q˙1q˙2+q˙22−m2l1l2sinq2q˙1q˙2,,M11=m1+m2l12+m2l22+2m2l1l2cosq2,M12=M21=m2l22+m2l1l2cosq2,g1=−m1+m2gl1sinq1−m2gl2sinq1+q2,where mi and li are given in Table 1. For i=1,2, mi and li denote mass and length of the ith link. km=diag10,10Num/rad,Mm=diag0.5,0.5kg⋅m2,Fm=diag4,0.5Nm⋅sec/rad.Figure 2
The schematic diagram of robotic manipulator with kinematics parameters.Table 1
The manipulator parameters.
Linkimikglicmqi12.535q10∘∼180∘2320q2−160∘∼160∘In order to demonstrate the influence of estimation for tracking performance, a DTSMC controller without estimation is also simulated. The DTSMC control law is given by(54)uk=M^TF^+q˙dk−q˙dk−1+λq˜tpk−λTq˜˙tk+q˜tkp−hsk−εksgnsk,with a sliding surface sk=q˜˙tk+λq˜tkp which is different from proposal sliding mode surface.Parameters of the proposed controller and DTSMC are selected asT=0.001s, h=diag2,2, λ=diag1.5,1.5,Λt=diag5,5, and ε=diag25,25. θk=0.15 is chosen as a constant model parameter whose initial value is θk=0, p=3, q=5, ξM=diag3,3, and ξF=diag1,1. In the simulation, as shown in Figure 3, we assume that the end effector is observed by a fixed camera whose delayed measurements are used directly to calculate the joint position. The delayed time of slow measurement is 0.125s. The process and measurement noise are chosen as Q=0.01I and R=0.1I. The initial position value of joints (joint1, joint2) for tracking control is given by 0.5rad,0.5rad, and for fusion estimator, it is given by 0.9rad,0.9rad. The initial velocity value for tracking control is given by 0,0, and for estimation, it is given by 0.25rad/s,0.Figure 3
Asynchronous multirate sensor measurements.The estimate errors of position and velocity are plotted in Figure4. The estimate errors of parameter θ are shown in Figure 5. The position tracking of the proposed method for two links is shown in Figures 6 and 7 where the comparative result of SMC without fusion estimator is also plotted. In conclusion, the simulation results clearly indicate that the proposed approach guarantees the convergence of tracking errors and has better tracking accuracy.Figure 4
The estimation errors ofq1,q˙1,q2,q˙2.Figure 5
The estimation error ofθ.Figure 6
Comparative control performances of joint1 between the proposed method and SMC.Figure 7
Comparative control performances of joint2 between the proposed method and SMC.


## 5.2. Experimental Study

To validate the applicability of the proposed control schemes, a single-link flexible-joint manipulator with a fixed camera is employed as the experimental plant which is shown in Figure8. The aim is to make the end effector move along the desired trajectory xd=l1cosqd, yd=l1sinqd with qd=0.4−t/75, and the initial position of joint is qi=0.35rad. Parameters of the single-link robot are measured offline: l1=0.6m and m1=10kg; the measurement errors are Δl1=0.05m and Δm1=0.1kg.Figure 8
The experiment setup.To observe the state of end effector, the calibrated camera is fixed perpendicular to the robot notion plane. The coordinate relation of Cartesian space and image space is shown in Figure9. Camera measurements are obtained from the image sequences shown in Figure 10. We define the position in the image coordinate yk=xmk,ymk given in Figure 11, and the position in Cartesian coordinate is rk=xck,yck. By using the pinhole camera model, we calculate the mapping from the joint space to image space Ik⋅ in equation (14) as follows:(55)fHc−fxmkymk=xckyck=l1cosqkl2sinqk,where Hc is the perpendicular distance between the camera and the robot motion plane and f denotes the focal length of the camera. In the experiment, camera parameters are f=0.035m, 640×512 pixels, and Hc=0.8m. According to the mapping Ik⋅, the joint position can be obtained by calculating the inverse circular trigonometric functions arccosxmkf/l1Hc−f or arcsinymkf/l1Hc−f. The delayed vision measurements xmk,ymk are converted to joint measurements arccosxmkf/l1Hc−f which are shown in Figure 12, where the encoder measurements are also plotted. The position estimation of the joint is calculated by using the proposed fusion estimate method shown as red solid line in Figure 12. To show the performance detail of the fusion estimate method, Figure 13 shows the error of joint position estimation.Figure 9
The coordinate relation of Cartesian space and image space.Figure 10
The image sequences of 10 frames.Figure 11
The trajectories of end effector in image space.Figure 12
The encoder and visual measurements.Figure 13
The position estimation error of the joint.To validate the performance of the fusion estimate-based DTSMC, comparative experiments are implemented in this paper. The proposed controller and a DTSMC without estimator are employed in this test. Parameters are selected asT=0.01s, h=3, λ=2, Λt=4 and ε=20, θk=0.05; p=3, q=5, ξM=0.2 and ξF=2. According to the comparative position tracking performances given in Figure 14, it is obvious that the proposed controller provides a superior behavior.Figure 14
Comparative position tracking errors of the end effector.


## 6. Conclusion

A novel RLFJ manipulator tracking controller, AMSFE-DTSMC, is proposed in this paper based on DTSMC coupled with asynchronous multirate sensor fusion. States of the manipulator are estimated by EKF-based sensor fusion algorithm which combines asynchronous multirate measurements from visual and non-vision-based sensors. Compared with the non-vision measurements, visual measurements are considered as periodic out-of-sequence measurements, which are used to re-estimate the state. With the state estimation, DTSMC is designed by using a novel sliding surface that includes tracking error and estimation error. By using the Sarpturk inequalities, boundedness of the controlled variables is proved. The effectiveness of the proposed approach is shown in simulation and experimental studies.



---

*Source: 9927850-2021-07-27.xml*