Designing Adaptive Robust Extended Kalman Filter Based on Lyapunov-Based Controller for Robotics Manipulators
Total Page:16
File Type:pdf, Size:1020Kb
This is a preprint version of the paper. To get the final version and for citation purposes please refer to the link on first page. Designing Adaptive Robust Extended Kalman Filter Based on Lyapunov-Based Controller for Robotics Manipulators A.R. Ghiasi*, A.A. Ghavifekr**, Y. Shabbouei Hagh***,H. SeyedGholami**** * Department of Electrical and Computer Engineering, University of Tabriz, Tabriz, Iran Email: [email protected] ** Department of Electrical and Computer Engineering, University of Tabriz, Tabriz, Iran Email: [email protected] *** Department of Electrical and Computer Engineering, University of Tabriz, Tabriz, Iran Email: [email protected] **** Department of Electrical and Computer Engineering, University of Tabriz, Tabriz, Iran Email: [email protected] Abstract: In this paper, a position and velocity estimation points[4].Neural networks and fuzzy controllers have method for robotic manipulators which are affected by constant bounded disturbances is considered. The tracking control been used recently to reduce tracking errors of problem is formulated as a disturbance rejection problem, with manipulators[5, 6]. Sliding Mode control approach based all the unknown parameters and dynamic uncertainties lumped on neural network is considered in [7] where the model into disturbance. Using adaptive robust extended Kalman parameters are used in the equivalent control law. The filter(AREKF) the movement and velocity of each joint is predicted to use in discontinuous Lyapunov-based controller inertia matrix, the parameters of the matrix of the structure. The parameters of the error dynamics have been Coriolis/centripetal terms and the gravity vector are validated off-line by real data. estimated in this method. Computer simulation results given for a two degree of freedom There are several inherent difficulties associated with manipulator demonstrate the efficacy of the improved Kalman Filter by comparing the performance of EKF and improved internal and external disturbances. In [8] an adaptive AREKF. Although it is shown that accurate trajectory tracking control of a robotic manipulator and a parameter can be achieved by using the proposed controller. estimation method are discussed. It considers a five-bar linkage manipulator with unknown external disturbance. A novel entropy based indirect iterative learning control Keywords: Tracking Problem, Adaptive robust extended methodology for robotic manipulator with random Kalman filter, Kalman filter, Robotic Manipulators disturbance is sated in [9] by combining the minimum error entropy and the optimal strategy, which is used to 1. Introduction update local controller parameters between any two adjacent batches. In [10] disturbance observers (DOB) The tracking control of robotic manipulators has been are used to reject both external disturbances as well as extensively studied in recent years. The design of high- inherent internal uncertainties. As manipulator have to be performance robotic control systems, involving nonlinear robust against internal and external variations in the work control algorithms for robotic mechanisms is of much place the disturbance observer has to estimate unknown interest[1]. Various Controllers are designed for states and rejecting decoupled terms between links[11]. manipulators that are subjected to disturbances and In this paper discontinuous Lyapunov-based method is parametric uncertainties [2]. In [3] a novel robust evaluated to reject bounded and time variant passive control approach is introduced for a robot disturbances. All system uncertainties are lumped into the manipulator with model uncertainties to interact with its disturbance term. dynamic environment by considering the robot's Kalman filters have been used in robotics literature to mechanic energy. An adaptive position/force controller estimate the joint angles[12, 13]. The main idea behind has been used for robotic manipulator to compensate the EKF is to linearize the model before implementing parametric uncertainties while only requiring the the Kalman filter. For linearization purpose, it is measurements of the position and velocity of robot arms, common to use Taylor expansions. The system can be but not the measurements of forces at contact extended to any orders, but to save the computing This is a preprint version of the paper. To get the final version and for citation purposes please refer to the link on first page. resources and also to make the calculation as simple as generally a nonlinear mapping between joint space and possible, usually only first (Jacobian) order of Taylor task space expansion is used. The kalman filter formulation Consider that d is the constant bounded vector completely depends on a priori knowledge of the system describing the load torque, the motion equation of the dynamics and it is also assumed that the process and robot can be written as measurement noise covariance matrices are pure white, zero-mean, completely uncorrelated and completely D()qq Cqqq (,) Gq () u d (4) known. If any of these assumptions were violated in any way, as they always were in real implementations, the 0; sup()dt (5) 2 filter performance will go bad and in some cases may 0t even go divergent. So it can be expected that an adaptive robust extended kalman filter will prevent filter 3. Designing Kalman Filters divergence. The rest of this paper is organized as follows: In section 2 the preliminaries of the problem are established. In 3.1 Extended Kalman Filter section 3 designing kalman filter is formulated into a The most vital requirement of the Kalman filter is the proper disturbance rejection problem. Discontinous linearity of the system. However, in real world, finding Lyapunov based controller is proposed in section 4. As an linear systems are quite hard and most of the systems that application example, a two degree of freedom are being dialed with are highly nonlinear, so the Kalman manipulator is discussed in 5 and simulation results are filter should be outstretched that it could handle nonlinear presented, and finally the conclusion is given in section 6. systems too. One of the methods that are being used to 2. Preliminaries overcome this problem is through a linearization Energy-based methods and the methods based on procedure. The resulting filter is referred as the extended Newton formulation are two general dynamics modeling Kalman filter (EKF). of robots. Denavit Hartenberg[14] parameters can be used A non-linear system can be typically formulated as to represent the position of end-effector in Cartesian xfxu( , ) w (6) coordinate relative to ground . k kk11 k 1 Equation. 1 shows the dynamic model of the manipulator: ygxrkkk( ) (7) n m Where xk R is state, yRk is measurement, D ()qq Cqqq (,) gq () u (1) wNQ∼ (0, ) is Gaussian noise process, rNR∼ (0, ) kk kk where Dq() Rnn is the inertia matrix, is measurement noise, f is non-linear dynamic model C(q,q)q R n is the centripetal and Coriolis matrix, function and g is the non-linear measurement model g(q) R n is the gravitational force and u is the exerted function. joint input. qR n is the joint angle vector. Even if the The extended Kalman filter can be summarized into 4 equations of motion of the robot are complex and highly main steps. nonlinear, there are still some basic properties in Eq. (1) that are convenient for controller design. These properties 1. The state-space model of system are given as are as follows. follows: xfxu( , ) w (8) Property 1. The inertia matrix D(q) is uniformly positive k kk11 k definite, and there exist positive constants 1 , 2 such that: ygxrkkk () 12IDq() I (2) 2. Initialize the filter as follows: Property 2. The manipulator dynamics (1) is linear in a set of physical parameters ( , ,..., )T . ddddp12 xˆ Ex[] 00 PExExxExˆ [( [ ])( [ ])T ] (9) Dqq() Cqqq (,) gq () Y (,,) qqq d (3) 00000 np Where Yqqq( , , ) R is usually called the dynamic 3. For k 1,2,... , perform the following: regressor matrix. n 3.1. Calculate the Jacobian matrices of f and h : The generalized end-effector’s position x R can be nn expressed as x hq()where hRR(.) is f k 1 Fk 1 |ˆ (10) x xk 1 g k H k | ˆ x xk This is a preprint version of the paper. To get the final version and for citation purposes please refer to the link on first page. 3.2. Perform the time update of the sate estimation The tuning parameter γ is found by searching over and error covariance matrix γ 0 and λt is another tuning parameter which should T be large enough that Ξλtt|1 tP tt |1 is fulfilled. PFPFQkkkkk11 1 1 (11) 3. Perform the measurement update: xfxuˆˆkkkk 111(,) xxˆˆ Kygx(()) ˆ tttttttt|1|1 K Ξ GPT 1 4. Perform the measurement update: ttttyt|1 , T T 1 PGyt,|1 tΞ tt GR t t (16) KPHHPHRkkkkkkk() xxKyhxˆˆkk kkkk( ( ˆ )) (12) 111T PGRGtttttt()Ξ |1 PIKHPkk()kk 4. Manipulator Control Law Formulation 3.2 Adaptive Robust Extended Kalman Filter The proposed discontinuous Lyapunov-based method can eliminate time-varying and bounded disturbances. The The AREKF algorithm can be summarized as follows: proposed controller is: 1. The nonlinear discrete-time system is uDqCqqGqK() (,) () (17) represented by D 2 xfxutttt(11 , ) w 1 (13) where K D kIdnn is the positive diagonal matrix and q . By applying the above control law, ygxvttt() closed loop form of equation can be written as following: wNQ∼ (0, ) is Gaussian noise process, vNR∼ (0, ) tt tt is measurement noise. D ()qCqqK (,) d (18) D 2. For k 1,2,... , perform the following: 2 After applying controller closed-loop dynamic will be 2.1 Calculate the Jacobian matrices of F and G : globally asymptotically stable, for which the proof is given in theorem.1. It should be mentioned that all states f t estimated by proposed Kalman filters. Ft |ˆ (14) x xt1 gt Theorem 1. Choose proposed discontinuous Gt |ˆ x xt Lyapunov based control law as (17).