# n-th Order Sensor Output to Control k-DoF Serial Robot Arms

Authors: Sergio Alvarez-Rodríguez; Francisco Gerardo Peña Lecona
Journal: Journal of Sensors
 (2021)

---

## Abstract

Currently, zero-order sensors are commonly used as positioning feedback for the closed-loop control in robotics; thus, in order to expand robots’ control alternatives, other paths in sensing should be investigated more deeply. Conditions under which then-th order sensor output can be used to control k-DoF serial robot arms are formally studied in this work. In obtaining the mentioned control conditions, the Pickard-Lindeloff theorem has been used to prove the existence and uniqueness of the robot’s mathematical model solution with n order sensory systems included. To verify that the given conditions and claims guarantee controllability for both continuous-based and variable structure-based systems, two types of control strategies are used in obtaining simulation results: the conventional PID control and a second-order Sliding Mode control.


---

## Body

## 1. Introduction

Currently, sensors of order zero are the most extensively used positioning detectors in closed-loop control systems in industrial robotics. As a matter of fact, nowadays, robotic systems nearly exclusively use incremental and absolute encoders, resolvers, and high precision potentiometers. This is due to the fact that robotic controllers require delay-free information to achieve the desired control goals: accuracy, precision, repetitiveness, avoidance of tracking error, reduction of speed error, and so on.An extended paradigm considers that positioning sensors of higher order than zero are not reliable at all. This consideration demotivates engineers for designing sensors of ordern>0. Nevertheless, the authors of this work consider that the theme should be sufficiently investigated to eliminate restrictive considerations which could lead in the future, to improve sensor systems in robotics.In this work, the performance of ak-DoF serial robot arm with dynamical inclusion of linear n-order sensors is investigated, showing that robot’s properties with linear n-order sensors inclusion are invariant with respect to robot’s theoretical dynamics, provided that the solutions of the considered linear n-order sensors exist and are unique.Interest in studying the use of higher-order sensor devices in the robotic industry has increased due to the need to implement more efficient, faster, and optimal control systems in both types of sensor systems concentrated in a robotic arm or systems with distributed sensors connected in a network and which may also be subject to adverse environmental conditions. However, the use of higher-order sensor systems inevitably introduces time delays in robotic control systems, including those delays from the controller to the actuator and the delays from the sensor to the controller.On the other hand, some control methodologies have been proposed, ranging from sliding mode and adaptive techniques to manage uncertainties related to the knowledge of external dynamics and disturbances [1, 2]. However, these approaches always work well in lag-free environments.To try to compensate for these delays, low pass filter-based mechanisms have been implemented to obtain high-quality signals and be able to build control loops for control mechanisms [3].Mathematical scaffolding to ensure unique solutions in robotic device control algorithms including higher-order sensor systems is not widely studied.Even more, alternative solutions to deal with nonlinear systems with unmeasurable states have been proposed, in [4] where an adaptive fuzzy output-feedback control scheme is designed and in [5] where a fuzzy logic system-based switched observer is constructed to approximate the unmeasurable states.


### 1.1. Motivation

Sensors can be designed using a very wide variety of physical principles (e.g., inductive, capacitive); however, many phenomena must be modelled using the n-order differential equations, making them unreliable in the world of robotics.Motivated by the fact that currently sensory systems of ordern>0 are not included either in industrial or in other areas of robotics, the authors are interested in expanding the study of the control of serial mechanisms using sensors that may have the potential to improve in some way the current ones.As such, the first question is: under which conditions a serial robot mechanism can be controlled by positioning sensors of order different from zero?If this first question is satisfactorily solved, a second one comes to mind: how can we take advantage of the sensor output for control purposes?


### 1.2. Contribution of This Work

The main contribution of this work is to study conditions to make possible the control of serial robot arms, taking as the feedback for the closed-loop control the output of then-th order sensory systems. Also, a technique to obtain the mentioned feedback signal from the sensor’s output is proposed.As formal proofs for claims presented in the text are provided, the methodology of this work is rigorous.As a proof of concept, an application example is focused on the trajectory tracking control problem (for simplicity); however, using the first time derivative of the position, the robot’s joint speed control follows the same rules given herein.


### 1.3. Problem Statement

Figure1 shows a block control diagram with a negative feedback summing a reference signal (Ref) to get the tracking error (e) as the input for the control block which is the controller for the system Robot-Sensors. In Figure 1, τ is the control torque, q represents the actual robot’s position identified by q∗ (i.e., q∗→q), while q~ represents the sensor’s output. If sensors are of order zero, i.e., β¯q~=q, where matrix β¯−1 includes the sensor’s parameters of sensitiveness, the feedback of states can be taken directly from the sensor’s output. Nevertheless, sensors of order n can be designed and/or included to control robotic systems. In such a case, the problem consists of recovering q from the information q~ provided by sensors, using a processing method for this task. In the following section, this problem is addressed formally.Figure 1
Block diagram of the closed-loop control with sensors inclusion.The remainder of the document is organized as follows: in Section2, conditions and assumptions to control serial robots using the output of n order sensory systems are given, and formal proofs for the given claims are presented; in Section 3 an implementation example for a 3-DoF serial robot arm is performed, along with simulation results; and finally, concluding remarks are given in Section 4.


## 1.1. Motivation

Sensors can be designed using a very wide variety of physical principles (e.g., inductive, capacitive); however, many phenomena must be modelled using the n-order differential equations, making them unreliable in the world of robotics.Motivated by the fact that currently sensory systems of ordern>0 are not included either in industrial or in other areas of robotics, the authors are interested in expanding the study of the control of serial mechanisms using sensors that may have the potential to improve in some way the current ones.As such, the first question is: under which conditions a serial robot mechanism can be controlled by positioning sensors of order different from zero?If this first question is satisfactorily solved, a second one comes to mind: how can we take advantage of the sensor output for control purposes?


## 1.2. Contribution of This Work

The main contribution of this work is to study conditions to make possible the control of serial robot arms, taking as the feedback for the closed-loop control the output of then-th order sensory systems. Also, a technique to obtain the mentioned feedback signal from the sensor’s output is proposed.As formal proofs for claims presented in the text are provided, the methodology of this work is rigorous.As a proof of concept, an application example is focused on the trajectory tracking control problem (for simplicity); however, using the first time derivative of the position, the robot’s joint speed control follows the same rules given herein.


## 1.3. Problem Statement

Figure1 shows a block control diagram with a negative feedback summing a reference signal (Ref) to get the tracking error (e) as the input for the control block which is the controller for the system Robot-Sensors. In Figure 1, τ is the control torque, q represents the actual robot’s position identified by q∗ (i.e., q∗→q), while q~ represents the sensor’s output. If sensors are of order zero, i.e., β¯q~=q, where matrix β¯−1 includes the sensor’s parameters of sensitiveness, the feedback of states can be taken directly from the sensor’s output. Nevertheless, sensors of order n can be designed and/or included to control robotic systems. In such a case, the problem consists of recovering q from the information q~ provided by sensors, using a processing method for this task. In the following section, this problem is addressed formally.Figure 1
Block diagram of the closed-loop control with sensors inclusion.The remainder of the document is organized as follows: in Section2, conditions and assumptions to control serial robots using the output of n order sensory systems are given, and formal proofs for the given claims are presented; in Section 3 an implementation example for a 3-DoF serial robot arm is performed, along with simulation results; and finally, concluding remarks are given in Section 4.


## 2. Robot’s Model with Sensor System Inclusion

Let us consider the dynamic differential equation(1)β¯ndndtnq~+⋯+β¯2d2dt2q~+β¯1ddtq~+β¯0q~=q−Fx,y,z,t,where β¯i∈ℝk×k with i=0,1,⋯,n represents the functional diagonal matrices, i.e., each element of β¯ denoted as βline×column=0 for all line≠column; q~∈ℝk is the sensor’s response vector; q∈ℝk is the vector for the actual positions of robot joints; Fx,y,z,t∈ℝk represents a position and/or time dependent possible function; and k is the number of DoF of the robot.For the case of linear sensors, (1) becomes(2)∑i=0nβ¯ididtiq~=q.Now, let us consider the following nonlinear robot dynamics(3)ẋ=ft,x,withxt0=x0,where vector x represents the states of the electromechanical plant, i.e., x=χ1,χ2T; vector ft,x is a time and/or states-possible dependent function; and x0 is the initial condition value at t=t0.The inclusion of (2) in (3) is studied in this work, in order to obtain control conditions, according to the following claim:Claim 1.
It is possible the control of serial robots using positioning sensors of then-th order for the closed-loop feedback of the states if the mathematical structure of the solution q~ of the differential equations that model sensor’s behaviour (2) exists and is unique for all t≥t0, under the following assumptions:Assumption 1.
Sensors provide the dynamic solution of (2) for all t≥t0, i.e., it is assumed that the value for the signal q~ is available to be used at time t≥t0 by the closed-loop control system.Assumption 2.
It is assumed that the diagonal matricesβ¯i with i=1,⋯,n in (2) are known.


### 2.1. Existence and Uniqueness

Herein, the existence and uniqueness of the solution for the robot’s dynamics are verified for the case in which the solution of the sensor’s dynamics exists and is unique.Definition 1 (Lipschitz).
The functionf:D⊂ℝr+1↦ℝr is locally Lipschitz if it is continuous, and for all subset κ⊂D compact exists L∈ℝ such that if x,t and y,t∈κ, then,(4)fx,t−fy,t≤Lx−y,where L is called the Lipschitz constant. (See [6] among many others.)Theorem 1 (Picard-Lindeloff approach).
For functionf:D⊂ℝr+1↦ℝr locally Lipschitz, and also for the set(5)R=t,x:t0≤t≤t0+a,x−x0≤b⊂D,let H be a bound of ft,x in R, L the Lipschitz constant of ft,x in R, and α=mina,b/H,1/L. Then, problem (6) with initial conditions has a solution, and it is unique in the interval t0,t0+α (in the Picard-Lindeloff approach).(6)χ̇1=f1t,χ1,χ2,χ̇2=f2t,χ1,χ2,χ1t0=χ10,χ2t0=χ20,where f=f1,f2T, x=χ1,χ2T, and χ10 and χ20 are the initial values for position and velocity, respectively, at t=t0.Proof.
It is assumed thatq~ exists, then, also the set(7)F0=q~t∈Ct0,t0+α,ℝk:q~t0=x0,q~t−x0≤b0,exists, where Ct0,t0+α,ℝk is a Banach space with uniform norm(8)q~=supt0≤t≤t0+αq~t,and F0 is closed in C.According to (2) and control concept expressed in Figure 1, the identification process of q is(9)q∗=∑i=0nβ¯ididtiq~+ε,where q∗∈ℝk is the result of processing q~ and ε∈ℝk is an identification error given by the Euclidian norm ∣q∗−q∣. Observe that q∗t0=β¯0q~t0+ε, for simplicity β0=I and ε→0, then, q∗t0→x0, and the problem is reduced to the existence of the set(10)F=q∗t∈Ct0,t0+α,ℝk:q∗t0=x0,q∗t−x0≤b,as the norm q∗t−q∗t0 is bounded by b.Let us defineΓ:F→Ct0,t0+α,ℝk such that the image of the function q∗t is Γq∗t defined by(11)Γq∗t=x0+∫t0tfs,q∗sds,where we accomplished both(12)Γq∗t0=x0+∫t0tfs,q∗sds=x0,∫t0tfs,q∗sds≤Ht−t0≤Hα≤b.Then,Γ is a contraction defined in a closed subset of a complete metric space, which implies that there exists a unique q∗∈F such that Γq∗t=q∗t, according to the fixed-point theorem of Banach.Thus,q∗t exists and is a unique solution of (6) for all t0≤t≤t0+α.


### 2.2. Dynamics Invariance

The validity of Claim1 is conditioned to result in the transformation of the robot’s dynamics when the sensor’s dynamics are included. To solve this problem, the following theorem is presented:Theorem 2.
Dynamic properties of(13)χ̇1=χ2,χ̇2=−M−1Cq,q̇q̇+Gq+Ffq,q̇+M−1qτ,χ1t0=χ10,χ2t0=χ20χ1t0=χ10,χ2t0=χ20,which are the dynamic robot’s model with actual output q, remain invariant with respect to the transformed form(14)χ̇1=χ2,χ̇2=−M−1Cq~,q~̇ddt∑i=0nβ¯ididtiq~+Gq~+Ffq~,q~̇+M−1q~τ,χ1t0=χ10,χ2t0=χ20,which is the robot with the n-th order sensor inclusion, where τ∈ℝk is the torque vector, M∈ℝk×k is the inertia matrix, matrix C∈ℝk×k represents the Coriolis and centripetal enforces, G∈ℝk is the gravitational vector, and Ff1τ, ⋯Ffkτ can be obtained by any parametric identification method, and each of them is the sum of viscous, Coulomb, and static friction forces.The transformation method is given by the sensor inclusion in the robot’s joints, represented by(15)β¯i=f¯i⋯0⋮⋱⋮0⋯h¯i,χ1=q=∑i=0nf¯ididtiq~1,⋯,∑i=0nh¯ididtiq~kT,χ2=q̇=ddt∑i=0nf¯ididtiq~1,⋯,ddt∑i=0nh¯ididtiq~kT,which are the sensor’s gain matrices, the articular positions, and the articular velocities, respectively.Proof.
The proof starts withk=3, but the intermediate results are given to make evident it is the extension to k=2,1, and by induction conclude generalization to (13)–(15), i.e., generalization for all k≥1.
Let us consider a serial 3-DoF anthropomorphic robot arm as defined in [7] and as illustrated by Figure 2 with positioning sensors of order n.
As dynamics is a consequence of kinematics, let us consider the form of direct and inverse kinematics withn-th order sensor inclusion.Figure 2
Mechanical configuration of a 3-DoF anthropomorphic robot arm: shapes of articular displacements.Direct kinematics. The Denavit-Hartenberg (DH) convention (see [8, 9]) is a commonly used method for selecting frames of reference and the relationship between them to obtain the kinematics mathematical model for robotic plants. This method uses arrays as those given by (16) to represent the orientation and the positioning of the end effector (i.e., the tool of the robot) [10, 11].(16)H=O3×3X3×101,where O3×3 is a 3×3 matrix for the orientation, X3×1 is a 3×1 array for the position of the tool, and H is the so-called homogeneous transformation matrix (see [12]).The DH convention claims that it is possible to obtain both position and orientation, from the (j−1)-th to the j-th link, through two translation and two rotation movements, as is presented in the following homogeneous transformation matrix,(17)Hj−1i=Rz⌣j−1,θjTz⌣j−1,djTx⌣j,ljRx⌣j,αj=cosθj−sinθjcosαjsinθjsinαjαjcosθjsinθjcosθjcosαj−cosθjsinαjαjsinθj0sinαjcosαjdj0001,whereθ, d, a, and α are the DH parameters, and their values come from specific aspects of the geometric relationship between the coordinate frames for the mechanical configuration of the robot under study and the way to select each of the coordinate frames; x˘j, y˘j, and z˘j are the axes of the j-th coordinate frame; Rz˘j−1,θj represents the rotation matrix with respect to z˘j−1; Tz˘j−1,dj is a translation matrix with respect to z˘j−1; Tx˘j,aj is a translation matrix with respect to x˘j; and Rx˘j,αj is a rotation matrix with respect to x˘j (see [8–13] for a detailed procedure in obtaining (17)).To feed equation (17) with the values of θ, d, a and α, Table 1 was constructed with the DH parameters of the robot under consideration, taking into account Figure 3 which shows the way in which the author has selected each of the coordinate frames (see [13] for more details).Table 1
Denavit-Hartenberg parameters.
jDoFθjraddjmajmαjrad1q1l10π/22q2+π/20l203q30l30Figure 3
Cartesian coordinate frames and home position for the robot. Origins 1, 2, and 3 havez˘-axes entering the plane of drawing, and origin 0 has a y˘-axis leaving the plane of the drawing.Table1 shows the articular plant positions q1, q2, and q3, which represent angular displacements as is shown in Figure 2; l=l1,l2,l3T are the link lengths, which are illustrated in the right side of Figure 3.A 3-DoF robot arm hasj=1,2,3, as such, substituting DH parameters in (17), three homogeneous transformation matrices are obtained: H10, H21, and H32. To go from system 0 to system 3, we compute H30=H10×H21×H32, obtaining the final position and orientation of the tool(18)H03=ηaηbsinq1ηeηcηd−cosq1ηfcosq2+q3−sinq2+q30ηg0001,with(19)ηa=−cosq1sinq2+q3,ηb=−cosq1cosq2+q3,ηc=−sinq1sinq2+q3,ηd=−sinq1cosq2+q3,ηe=−cosq1l2sinq2+l3sinq2+q3,ηf=−sinq1l2sinq2+l3sinq2+q3,ηg=l1+l2cosq2+l3cosq2+q3.Thus, from the last column in (18) and including (2), direct kinematics is obtained for the robot with n-order sensor inclusion, as follows(20)Xqt=x˘y˘z˘T=−cos∑i=0nf¯ididtiq~1l2sin∑i=0ng¯ididtiq~2+l3sin∑i=0ng¯ididtiq~2+∑i=0nh¯ididtiq~3−sin∑i=0nf¯ididtiq~1l2sin∑i=0ng¯ididtiq~2+l3sin∑i=0ng¯ididtiq~2+∑i=0nh¯ididtiq~3l1+l2cos∑i=0ng¯ididtiq~2+l3cos∑i=0ng¯ididtiq~2+∑i=0nh¯ididtiq~3,where x˘, y˘, and z˘ are the spatial Cartesian coordinates of the end effector.Inverse kinematics. Solving equation (20) for q1, q2, and q3, inverse kinematics (see [14]) is obtained. This can be done involving the Moore-Penrose pseudoinverse Jacobian matrix JTJJT−1 and taking into account the chain rule Ẋ=Jq̇, for the first time derivative of q, given by q̇=JTJJT−1Ẋ, where J is the Jacobian matrix J=∂/∂qX (see [12]). Carrying out this procedure and involving (2), the inverse kinematics for the robot is given by(21)∑i=0nf¯ididtiq~1=tan−1y˘x˘,∑i=0ng¯ididtiq~2=tan−1l2+ρ/2l2z˘∗−l31−ρ/2l2l32x˘2+y˘2l2+ρ/2l2x˘2+y˘2+l31−ρ/2l2l32·z˘∗,∑i=0nh¯ididtiq~3=tan−12l2l3ρ1−ρ2l2l32,ρ=x˘2+y˘2+z˘∗2−l22−l32,z˘∗=z˘−l1,Attitude in the orientation of the end effector. Three elements of the submatrix O3×3 inside (16) are needed to obtain ϕ, γ, and ψ which represent the roll, pitch, and yaw movements of the end effector, respectively (see [13]):(22)O13=cosϕsinγcosψ+sinϕsinψ,O31=−sinγ,O33=cosγcosψ.Now, taking (18) as the master matrix and solving for ϕ, γ, and ψ, the orientation of the end effector of 3-DoF anthropomorphic robot arms with any order linear positioning sensors is obtained as(23)ϕγψ=∑i=0nf¯ididtiq~1∑i=0ng¯ididtiq~2+∑i=0nh¯ididtiq~3π/2.The same results presented in (20)–(23) are obtained including directly in (18) the DH parameters with sensor system inclusion given by Table 2, concluding that kinematics is invariant to sensor system inclusion.Table 2
Denavit-Hartenberg parameters with sensor system inclusion.
jDoFθjraddjmajmαjrad1∑i=0nf¯idi/dtiq~1l10π/22∑i=0ng¯idi/dtiq~2+π/20l203∑i=0nh¯idi/dtiq~30l30


#### 2.2.1. Robot’s Dynamics

Let us represent the total torque vector (τ) of the robot under consideration according to the Euler-Lagrange approach, as(24)τ=Mqq¨+Ṁqq̇−∂∂q12q̇TMqq̇+∂Uq∂q+Ffτq,q̇,where M∈ℝ3×3 is the inertia matrix, U∈ℝ3 is the potential energy vector, and Ffτ∈ℝ3 is the friction force vector.Using the methodology detailed in [15–17] to obtain the torque for the j-th link, (24) is transformed to the following form(25)τi=ddt∂∂q̇Lq,q̇−∂∂qLq,q̇+Ffτq,q̇,where L∈ℝ3 is the Lagrangian given by(26)Lq,q̇=Kq,q̇−Uq,with K and U as the kinetic and potential energies, respectively, which according to the referenced methodology are calculated for this work as(27)Kq,q̇=12I1+I2+I3+m2lc22sinq22+m3l22sinq22+m3l2lc3sinq2sinq2+q3+m3lc32sinq2+q32q̇12+12I2+I3+m2lc22+m3l22+m3l2lc3cosq3q̇22+12I3+m3lc32q̇32+I2+I3q̇1q̇2+I3q̇1q̇3+I3+m3l2lc3cosq3+m3lc32q̇2q̇3,Uq=m2glc21−cosπ−q2+m3gl2+lc3−l2cosπ−q2+lc3cosπ−q2−q3, where I=I1,I2,I3T, m=m1,m2,m3T, and lc=lc1,lc2,lc3T are the inertia, mass, and length vectors, respectively, for the j-th centroid, and g=9.81m/s2.Separating each element of vector (25) and calculating(28)τ1=ddt∂∂q̇1Lq,q̇−0+Ff1τ,τ2=ddt∂∂q̇2Lq,q̇−∂∂q2Lq,q̇+Ff2τ,τ3=ddt∂∂q̇3Lq,q̇−∂∂q3Lq,q̇+Ff3τ,where(29)ddt∂∂q̇1Lq,q̇=I1+I2+I3+m2lc22sinq22+m3l22sinq22+m3l2lc3sinq2sinq2+q3+m3lc32sinq2+q32q¨1+2m2lc22sinq2cosq2+2m3l22sinq2cosq2+m3l2lc3cosq2sinq2+q3+m3l2lc3sinq2cosq2+q3+2m3lc32sinq2+q3cosq2+q3q̇1q̇2+m3l2lc3sinq2cosq2+q3+2m3lc32sinq2+q3cosq2+q3q̇1q̇3+I2+I3q¨2+I3q¨3,ddt∂∂q̇2Lq,q̇=I2+I3q¨1+I2+I3+m2lc22+m3lc22+m3l2lc3cosq3q¨2+I3+m3lc32+m3l2lc3cosq3q¨3−m3l2lc3sinq3q̇2q̇3+q̇32,ddt∂∂q̇3Lq,q̇=I3q¨1+I3+m3lc32+m3l2lc3cosq3q¨2+I3+m3lc32q¨3−m3l2lc3sinq3q̇2q̇3,(30)∂∂q1Lq,q̇=0,∂∂q2Lq,q̇=m2lc22sinq2cosq2+m3l22sinq2cosq2+12m3l2lc3sinq2cosq2+q3+cosq2sinq2+q3+m3lc32sinq2+q3cosq2+q3q̇12+m2glc2sinπ−q2+m3gl2sinπ−q2+m3glc3sinπ−q2−q3,∂∂q3Lq,q̇=12m3l2lc3sinq2cosq2+q3+m3lc32sinq2+q3cosq2+q3q̇12−m3l2lc3sinq312q̇22+q̇2q̇3+m3glc3sinπ−q2−q3,Defining the Coriolis forces and gravitational vectors, respectively, as(31)Cq,q̇q̇=Ṁqq̇−∂∂q12q̇TMqq̇,Gq=∂Uq∂q,and including (2) in (24), the torque vector takes the form(32)τ=Mq~d2dt2∑i=0nβ¯ididtiq~+Cq~,q~̇ddt∑i=0nβ¯ididtiq~+Gq~+Ffτq~,q~̇.Rearranging results presented in (29) and (30) and also including (2), the elements of (32), (Mq~, Cq~,q~̇, and Gq~), are obtained and given in the following form(33)Mq~=M11I2+I3I3I2+I3M22M23I3M32I3+m3lc32,where(34)M11=I1+I2+I3+m2lc22+m3l22sin∑i=0ng¯ididtiq~22+m3l2lc3sin∑i=0ng¯ididtiq~2sin∑i=0ng¯ididtiq~2+∑i=0nh¯ididtiq~3+m3lc32sin∑i=0ng¯ididtiq~2+∑i=0nh¯ididtiq~32,M22=I2+I3+m2lc22+m3l2lc3cos∑i=0nh¯ididtiq~3,M23=M32=I3+m3lc32+m3l2lc3cos∑i=0nh¯ididtiq~3,(35)Cq~,q~̇=2Addt∑i=0nh¯ididtiq~202Bddt∑i=0nf¯ididtiq~1−Addt∑i=0nf¯ididtiq~1−2Dddt∑i=0nh¯ididtiq~3−2Dddt∑i=0nh¯ididtiq~3−Bddt∑i=0nf¯ididtiq~1Dddt∑i=0ng¯ididtiq~20,where(36)A=m2lc22+m3l22sin∑i=0ng¯ididtiq~2cos∑i=0ng¯ididtiq~2+m3l2lc32cos∑i=0ng¯ididtiq~2sin∑i=0ng¯ididtiq~2+∑i=0nh¯ididtiq~3+B,(37)B=12m3l2lc3sin∑i=0ng¯ididtiq~2cos∑i=0ng¯ididtiq~2+∑i=0nh¯ididtiq~3+m3lc32sin∑i=0ng¯ididtiq~2+∑i=0nh¯ididtiq~3cos∑i=0ng¯ididtiq~2+∑i=0nh¯ididtiq~3,(38)D=12m3l2lc3sin∑i=0nh¯ididtiq~3,(39)Gq~=−g0m2lc2+m3l2sinπ−∑i=0ng¯ididtiq~2+m3lc3sinπ−∑i=0ng¯ididtiq~2−∑i=0nh¯ididtiq~3m3lc3sinπ−∑i=0ng¯ididtiq~2−∑i=0nh¯ididtiq~3.Additionally, the friction vector symbolic form(40)Ffτq~,q~̇=Ff1τFf2τFf3τT,must be included in the dynamic model.Even more, the same results presented in (33)–(39) are obtained, when (2) is directly included in the Euler-Lagrange (24) and in (31), as follows(41)τ=Mq~d2dt2∑i=0nβ¯ididtiq~+Ṁq~ddt∑i=0nβ¯ididtiq~−∂∂q12ddt∑i=0nβ¯ididtiq~TMq~ddt∑i=0nβ¯ididtiq~+∂Uq~∂q+Ffτq~,q~̇,Cq~,q~̇d2dt2∑i=0nβ¯ididtiq~=Ṁq~ddt∑i=0nβ¯ididtiq~−∂∂q12ddt∑i=0nβ¯ididtiq~TMq~ddt∑i=0nβ¯ididtiq~Gq~=∂Uq~∂q,where(42)β¯i=f¯i000g¯i000h¯i,χ1=q=∑i=0nf¯ididtiq~1,∑i=0ng¯ididtiq~2,∑i=0nh¯ididtiq~3T,χ2=q̇=ddt∑i=0nf¯ididtiq~1,ddt∑i=0ng¯ididtiq~2,ddt∑i=0nh¯ididtiq~3T,which proves dynamics invariance to sensor inclusion.


#### 2.2.2. Extensive Considerations

Even more, for a serial robot with 2-DoF, from Tables1 to 2, line i=3 must be removed and (16) becomes(43)H=O2×2X2×101,with(44)β¯i=f¯i00g¯i,χ1=q=∑i=0nf¯ididtiq~1,∑i=0ng¯ididtiq~2T,χ2=q̇=ddt∑i=0nf¯ididtiq~1,ddt∑i=0ng¯ididtiq~2T.Also, when a 1-DoF is considered, from Tables1 to 2, line i=2,3 must be removed and (16) becomes(45)H=O1×1X1×101,with(46)β¯i=f¯i,χ1=q=∑i=0nf¯ididtiq~1,χ2=q̇=ddt∑i=0nf¯ididtiq~1.Since procedures (16)–(40) have intermediated outcomes, it is easy to compute results for a 1-DoF and for a 2-DoF robot, to obtain the same conclusions.Further, invoking the induction method, these results can be extended to serial robots withk-DoF for all k≥1.In order to deal with sensor inclusion mathematical complexity, the following remarks are useful for practical implementations:(47)dpdtp∑i=0nβ¯ididtiq~=∑i=0nβ¯idi+pdti+pq.~Remark 4.
For the case to consider matrixβi as a constant, the following identity should be used, in order to simplify the algebraic complications,And for the case to consider matrixβi=I,(48)dpdtp∑i=0nβ¯ididtiq~=∑i=pn+pdidtiq~.Remark 5.
For initial conditions at the originq~0=q~̇0=q~̇0=⋯=0, the Laplace transform for sensors of the k-th order with βi=I,(49)L∑i=0ndidtiq~=Q~s∑i=0nsi=Qs,such that(50)fq=L−1Qs∑i=0nsi.


### 2.3. Control Usingn-th Order Sensors’ Output

Now, in order to take advantage of Claim1, the following control conclusions are given:Assumption 3.
It is assumed that sensors are previously well characterized,i.e., both n (the order of every sensor) and sensor’s dynamics are known; this implies that ε→0.Claim 2.
Supported by Theorems2 and 3, and if q∗→q, it can be claimed that equation (2) becomes the method to identify q from q~ to control (14), which represents the robot with n-th order sensor inclusion. Thus,(51)e=Xref−∑i=0nβ¯ididtiq,~where e∈ℝk is the tracking error, and Xref∈ℝk represents the reference signal.
The method is depicted in Figure4, where the dashed squared block of the negative feedback represents (2).Note 1.
Solutionq~ of (2) cannot be directly used to control (6).Extension of Claim 2.
In [18], the Lasalle-invariance principle (generally used to show asymptotic stability in the Lyapunov approach) is extended to nonautonomous switching systems. This implies that Claim 2 is valid using both continuous-based and discontinuous-based controllers, e.g., Proportional Integral Derivative (PID) control and Sliding Mode (SM) controllers.
Even more, in [19], an extensive study to show the control of MIMO systems by the twisting-algorithm (a second-order SM control) is presented, showing asymptotic convergence of the system trajectories to the selected sliding manifold.
Further, in order to review the stability for biorder SM (twisting-algorithm included) control with variable PID gains, [20] can be consulted.Figure 4
Block diagram of the closed-loop control with sensor inclusion.


## 2.1. Existence and Uniqueness

Herein, the existence and uniqueness of the solution for the robot’s dynamics are verified for the case in which the solution of the sensor’s dynamics exists and is unique.Definition 1 (Lipschitz).
The functionf:D⊂ℝr+1↦ℝr is locally Lipschitz if it is continuous, and for all subset κ⊂D compact exists L∈ℝ such that if x,t and y,t∈κ, then,(4)fx,t−fy,t≤Lx−y,where L is called the Lipschitz constant. (See [6] among many others.)Theorem 1 (Picard-Lindeloff approach).
For functionf:D⊂ℝr+1↦ℝr locally Lipschitz, and also for the set(5)R=t,x:t0≤t≤t0+a,x−x0≤b⊂D,let H be a bound of ft,x in R, L the Lipschitz constant of ft,x in R, and α=mina,b/H,1/L. Then, problem (6) with initial conditions has a solution, and it is unique in the interval t0,t0+α (in the Picard-Lindeloff approach).(6)χ̇1=f1t,χ1,χ2,χ̇2=f2t,χ1,χ2,χ1t0=χ10,χ2t0=χ20,where f=f1,f2T, x=χ1,χ2T, and χ10 and χ20 are the initial values for position and velocity, respectively, at t=t0.Proof.
It is assumed thatq~ exists, then, also the set(7)F0=q~t∈Ct0,t0+α,ℝk:q~t0=x0,q~t−x0≤b0,exists, where Ct0,t0+α,ℝk is a Banach space with uniform norm(8)q~=supt0≤t≤t0+αq~t,and F0 is closed in C.According to (2) and control concept expressed in Figure 1, the identification process of q is(9)q∗=∑i=0nβ¯ididtiq~+ε,where q∗∈ℝk is the result of processing q~ and ε∈ℝk is an identification error given by the Euclidian norm ∣q∗−q∣. Observe that q∗t0=β¯0q~t0+ε, for simplicity β0=I and ε→0, then, q∗t0→x0, and the problem is reduced to the existence of the set(10)F=q∗t∈Ct0,t0+α,ℝk:q∗t0=x0,q∗t−x0≤b,as the norm q∗t−q∗t0 is bounded by b.Let us defineΓ:F→Ct0,t0+α,ℝk such that the image of the function q∗t is Γq∗t defined by(11)Γq∗t=x0+∫t0tfs,q∗sds,where we accomplished both(12)Γq∗t0=x0+∫t0tfs,q∗sds=x0,∫t0tfs,q∗sds≤Ht−t0≤Hα≤b.Then,Γ is a contraction defined in a closed subset of a complete metric space, which implies that there exists a unique q∗∈F such that Γq∗t=q∗t, according to the fixed-point theorem of Banach.Thus,q∗t exists and is a unique solution of (6) for all t0≤t≤t0+α.


## 2.2. Dynamics Invariance

The validity of Claim1 is conditioned to result in the transformation of the robot’s dynamics when the sensor’s dynamics are included. To solve this problem, the following theorem is presented:Theorem 2.
Dynamic properties of(13)χ̇1=χ2,χ̇2=−M−1Cq,q̇q̇+Gq+Ffq,q̇+M−1qτ,χ1t0=χ10,χ2t0=χ20χ1t0=χ10,χ2t0=χ20,which are the dynamic robot’s model with actual output q, remain invariant with respect to the transformed form(14)χ̇1=χ2,χ̇2=−M−1Cq~,q~̇ddt∑i=0nβ¯ididtiq~+Gq~+Ffq~,q~̇+M−1q~τ,χ1t0=χ10,χ2t0=χ20,which is the robot with the n-th order sensor inclusion, where τ∈ℝk is the torque vector, M∈ℝk×k is the inertia matrix, matrix C∈ℝk×k represents the Coriolis and centripetal enforces, G∈ℝk is the gravitational vector, and Ff1τ, ⋯Ffkτ can be obtained by any parametric identification method, and each of them is the sum of viscous, Coulomb, and static friction forces.The transformation method is given by the sensor inclusion in the robot’s joints, represented by(15)β¯i=f¯i⋯0⋮⋱⋮0⋯h¯i,χ1=q=∑i=0nf¯ididtiq~1,⋯,∑i=0nh¯ididtiq~kT,χ2=q̇=ddt∑i=0nf¯ididtiq~1,⋯,ddt∑i=0nh¯ididtiq~kT,which are the sensor’s gain matrices, the articular positions, and the articular velocities, respectively.Proof.
The proof starts withk=3, but the intermediate results are given to make evident it is the extension to k=2,1, and by induction conclude generalization to (13)–(15), i.e., generalization for all k≥1.
Let us consider a serial 3-DoF anthropomorphic robot arm as defined in [7] and as illustrated by Figure 2 with positioning sensors of order n.
As dynamics is a consequence of kinematics, let us consider the form of direct and inverse kinematics withn-th order sensor inclusion.Figure 2
Mechanical configuration of a 3-DoF anthropomorphic robot arm: shapes of articular displacements.Direct kinematics. The Denavit-Hartenberg (DH) convention (see [8, 9]) is a commonly used method for selecting frames of reference and the relationship between them to obtain the kinematics mathematical model for robotic plants. This method uses arrays as those given by (16) to represent the orientation and the positioning of the end effector (i.e., the tool of the robot) [10, 11].(16)H=O3×3X3×101,where O3×3 is a 3×3 matrix for the orientation, X3×1 is a 3×1 array for the position of the tool, and H is the so-called homogeneous transformation matrix (see [12]).The DH convention claims that it is possible to obtain both position and orientation, from the (j−1)-th to the j-th link, through two translation and two rotation movements, as is presented in the following homogeneous transformation matrix,(17)Hj−1i=Rz⌣j−1,θjTz⌣j−1,djTx⌣j,ljRx⌣j,αj=cosθj−sinθjcosαjsinθjsinαjαjcosθjsinθjcosθjcosαj−cosθjsinαjαjsinθj0sinαjcosαjdj0001,whereθ, d, a, and α are the DH parameters, and their values come from specific aspects of the geometric relationship between the coordinate frames for the mechanical configuration of the robot under study and the way to select each of the coordinate frames; x˘j, y˘j, and z˘j are the axes of the j-th coordinate frame; Rz˘j−1,θj represents the rotation matrix with respect to z˘j−1; Tz˘j−1,dj is a translation matrix with respect to z˘j−1; Tx˘j,aj is a translation matrix with respect to x˘j; and Rx˘j,αj is a rotation matrix with respect to x˘j (see [8–13] for a detailed procedure in obtaining (17)).To feed equation (17) with the values of θ, d, a and α, Table 1 was constructed with the DH parameters of the robot under consideration, taking into account Figure 3 which shows the way in which the author has selected each of the coordinate frames (see [13] for more details).Table 1
Denavit-Hartenberg parameters.
jDoFθjraddjmajmαjrad1q1l10π/22q2+π/20l203q30l30Figure 3
Cartesian coordinate frames and home position for the robot. Origins 1, 2, and 3 havez˘-axes entering the plane of drawing, and origin 0 has a y˘-axis leaving the plane of the drawing.Table1 shows the articular plant positions q1, q2, and q3, which represent angular displacements as is shown in Figure 2; l=l1,l2,l3T are the link lengths, which are illustrated in the right side of Figure 3.A 3-DoF robot arm hasj=1,2,3, as such, substituting DH parameters in (17), three homogeneous transformation matrices are obtained: H10, H21, and H32. To go from system 0 to system 3, we compute H30=H10×H21×H32, obtaining the final position and orientation of the tool(18)H03=ηaηbsinq1ηeηcηd−cosq1ηfcosq2+q3−sinq2+q30ηg0001,with(19)ηa=−cosq1sinq2+q3,ηb=−cosq1cosq2+q3,ηc=−sinq1sinq2+q3,ηd=−sinq1cosq2+q3,ηe=−cosq1l2sinq2+l3sinq2+q3,ηf=−sinq1l2sinq2+l3sinq2+q3,ηg=l1+l2cosq2+l3cosq2+q3.Thus, from the last column in (18) and including (2), direct kinematics is obtained for the robot with n-order sensor inclusion, as follows(20)Xqt=x˘y˘z˘T=−cos∑i=0nf¯ididtiq~1l2sin∑i=0ng¯ididtiq~2+l3sin∑i=0ng¯ididtiq~2+∑i=0nh¯ididtiq~3−sin∑i=0nf¯ididtiq~1l2sin∑i=0ng¯ididtiq~2+l3sin∑i=0ng¯ididtiq~2+∑i=0nh¯ididtiq~3l1+l2cos∑i=0ng¯ididtiq~2+l3cos∑i=0ng¯ididtiq~2+∑i=0nh¯ididtiq~3,where x˘, y˘, and z˘ are the spatial Cartesian coordinates of the end effector.Inverse kinematics. Solving equation (20) for q1, q2, and q3, inverse kinematics (see [14]) is obtained. This can be done involving the Moore-Penrose pseudoinverse Jacobian matrix JTJJT−1 and taking into account the chain rule Ẋ=Jq̇, for the first time derivative of q, given by q̇=JTJJT−1Ẋ, where J is the Jacobian matrix J=∂/∂qX (see [12]). Carrying out this procedure and involving (2), the inverse kinematics for the robot is given by(21)∑i=0nf¯ididtiq~1=tan−1y˘x˘,∑i=0ng¯ididtiq~2=tan−1l2+ρ/2l2z˘∗−l31−ρ/2l2l32x˘2+y˘2l2+ρ/2l2x˘2+y˘2+l31−ρ/2l2l32·z˘∗,∑i=0nh¯ididtiq~3=tan−12l2l3ρ1−ρ2l2l32,ρ=x˘2+y˘2+z˘∗2−l22−l32,z˘∗=z˘−l1,Attitude in the orientation of the end effector. Three elements of the submatrix O3×3 inside (16) are needed to obtain ϕ, γ, and ψ which represent the roll, pitch, and yaw movements of the end effector, respectively (see [13]):(22)O13=cosϕsinγcosψ+sinϕsinψ,O31=−sinγ,O33=cosγcosψ.Now, taking (18) as the master matrix and solving for ϕ, γ, and ψ, the orientation of the end effector of 3-DoF anthropomorphic robot arms with any order linear positioning sensors is obtained as(23)ϕγψ=∑i=0nf¯ididtiq~1∑i=0ng¯ididtiq~2+∑i=0nh¯ididtiq~3π/2.The same results presented in (20)–(23) are obtained including directly in (18) the DH parameters with sensor system inclusion given by Table 2, concluding that kinematics is invariant to sensor system inclusion.Table 2
Denavit-Hartenberg parameters with sensor system inclusion.
jDoFθjraddjmajmαjrad1∑i=0nf¯idi/dtiq~1l10π/22∑i=0ng¯idi/dtiq~2+π/20l203∑i=0nh¯idi/dtiq~30l30


### 2.2.1. Robot’s Dynamics

Let us represent the total torque vector (τ) of the robot under consideration according to the Euler-Lagrange approach, as(24)τ=Mqq¨+Ṁqq̇−∂∂q12q̇TMqq̇+∂Uq∂q+Ffτq,q̇,where M∈ℝ3×3 is the inertia matrix, U∈ℝ3 is the potential energy vector, and Ffτ∈ℝ3 is the friction force vector.Using the methodology detailed in [15–17] to obtain the torque for the j-th link, (24) is transformed to the following form(25)τi=ddt∂∂q̇Lq,q̇−∂∂qLq,q̇+Ffτq,q̇,where L∈ℝ3 is the Lagrangian given by(26)Lq,q̇=Kq,q̇−Uq,with K and U as the kinetic and potential energies, respectively, which according to the referenced methodology are calculated for this work as(27)Kq,q̇=12I1+I2+I3+m2lc22sinq22+m3l22sinq22+m3l2lc3sinq2sinq2+q3+m3lc32sinq2+q32q̇12+12I2+I3+m2lc22+m3l22+m3l2lc3cosq3q̇22+12I3+m3lc32q̇32+I2+I3q̇1q̇2+I3q̇1q̇3+I3+m3l2lc3cosq3+m3lc32q̇2q̇3,Uq=m2glc21−cosπ−q2+m3gl2+lc3−l2cosπ−q2+lc3cosπ−q2−q3, where I=I1,I2,I3T, m=m1,m2,m3T, and lc=lc1,lc2,lc3T are the inertia, mass, and length vectors, respectively, for the j-th centroid, and g=9.81m/s2.Separating each element of vector (25) and calculating(28)τ1=ddt∂∂q̇1Lq,q̇−0+Ff1τ,τ2=ddt∂∂q̇2Lq,q̇−∂∂q2Lq,q̇+Ff2τ,τ3=ddt∂∂q̇3Lq,q̇−∂∂q3Lq,q̇+Ff3τ,where(29)ddt∂∂q̇1Lq,q̇=I1+I2+I3+m2lc22sinq22+m3l22sinq22+m3l2lc3sinq2sinq2+q3+m3lc32sinq2+q32q¨1+2m2lc22sinq2cosq2+2m3l22sinq2cosq2+m3l2lc3cosq2sinq2+q3+m3l2lc3sinq2cosq2+q3+2m3lc32sinq2+q3cosq2+q3q̇1q̇2+m3l2lc3sinq2cosq2+q3+2m3lc32sinq2+q3cosq2+q3q̇1q̇3+I2+I3q¨2+I3q¨3,ddt∂∂q̇2Lq,q̇=I2+I3q¨1+I2+I3+m2lc22+m3lc22+m3l2lc3cosq3q¨2+I3+m3lc32+m3l2lc3cosq3q¨3−m3l2lc3sinq3q̇2q̇3+q̇32,ddt∂∂q̇3Lq,q̇=I3q¨1+I3+m3lc32+m3l2lc3cosq3q¨2+I3+m3lc32q¨3−m3l2lc3sinq3q̇2q̇3,(30)∂∂q1Lq,q̇=0,∂∂q2Lq,q̇=m2lc22sinq2cosq2+m3l22sinq2cosq2+12m3l2lc3sinq2cosq2+q3+cosq2sinq2+q3+m3lc32sinq2+q3cosq2+q3q̇12+m2glc2sinπ−q2+m3gl2sinπ−q2+m3glc3sinπ−q2−q3,∂∂q3Lq,q̇=12m3l2lc3sinq2cosq2+q3+m3lc32sinq2+q3cosq2+q3q̇12−m3l2lc3sinq312q̇22+q̇2q̇3+m3glc3sinπ−q2−q3,Defining the Coriolis forces and gravitational vectors, respectively, as(31)Cq,q̇q̇=Ṁqq̇−∂∂q12q̇TMqq̇,Gq=∂Uq∂q,and including (2) in (24), the torque vector takes the form(32)τ=Mq~d2dt2∑i=0nβ¯ididtiq~+Cq~,q~̇ddt∑i=0nβ¯ididtiq~+Gq~+Ffτq~,q~̇.Rearranging results presented in (29) and (30) and also including (2), the elements of (32), (Mq~, Cq~,q~̇, and Gq~), are obtained and given in the following form(33)Mq~=M11I2+I3I3I2+I3M22M23I3M32I3+m3lc32,where(34)M11=I1+I2+I3+m2lc22+m3l22sin∑i=0ng¯ididtiq~22+m3l2lc3sin∑i=0ng¯ididtiq~2sin∑i=0ng¯ididtiq~2+∑i=0nh¯ididtiq~3+m3lc32sin∑i=0ng¯ididtiq~2+∑i=0nh¯ididtiq~32,M22=I2+I3+m2lc22+m3l2lc3cos∑i=0nh¯ididtiq~3,M23=M32=I3+m3lc32+m3l2lc3cos∑i=0nh¯ididtiq~3,(35)Cq~,q~̇=2Addt∑i=0nh¯ididtiq~202Bddt∑i=0nf¯ididtiq~1−Addt∑i=0nf¯ididtiq~1−2Dddt∑i=0nh¯ididtiq~3−2Dddt∑i=0nh¯ididtiq~3−Bddt∑i=0nf¯ididtiq~1Dddt∑i=0ng¯ididtiq~20,where(36)A=m2lc22+m3l22sin∑i=0ng¯ididtiq~2cos∑i=0ng¯ididtiq~2+m3l2lc32cos∑i=0ng¯ididtiq~2sin∑i=0ng¯ididtiq~2+∑i=0nh¯ididtiq~3+B,(37)B=12m3l2lc3sin∑i=0ng¯ididtiq~2cos∑i=0ng¯ididtiq~2+∑i=0nh¯ididtiq~3+m3lc32sin∑i=0ng¯ididtiq~2+∑i=0nh¯ididtiq~3cos∑i=0ng¯ididtiq~2+∑i=0nh¯ididtiq~3,(38)D=12m3l2lc3sin∑i=0nh¯ididtiq~3,(39)Gq~=−g0m2lc2+m3l2sinπ−∑i=0ng¯ididtiq~2+m3lc3sinπ−∑i=0ng¯ididtiq~2−∑i=0nh¯ididtiq~3m3lc3sinπ−∑i=0ng¯ididtiq~2−∑i=0nh¯ididtiq~3.Additionally, the friction vector symbolic form(40)Ffτq~,q~̇=Ff1τFf2τFf3τT,must be included in the dynamic model.Even more, the same results presented in (33)–(39) are obtained, when (2) is directly included in the Euler-Lagrange (24) and in (31), as follows(41)τ=Mq~d2dt2∑i=0nβ¯ididtiq~+Ṁq~ddt∑i=0nβ¯ididtiq~−∂∂q12ddt∑i=0nβ¯ididtiq~TMq~ddt∑i=0nβ¯ididtiq~+∂Uq~∂q+Ffτq~,q~̇,Cq~,q~̇d2dt2∑i=0nβ¯ididtiq~=Ṁq~ddt∑i=0nβ¯ididtiq~−∂∂q12ddt∑i=0nβ¯ididtiq~TMq~ddt∑i=0nβ¯ididtiq~Gq~=∂Uq~∂q,where(42)β¯i=f¯i000g¯i000h¯i,χ1=q=∑i=0nf¯ididtiq~1,∑i=0ng¯ididtiq~2,∑i=0nh¯ididtiq~3T,χ2=q̇=ddt∑i=0nf¯ididtiq~1,ddt∑i=0ng¯ididtiq~2,ddt∑i=0nh¯ididtiq~3T,which proves dynamics invariance to sensor inclusion.


### 2.2.2. Extensive Considerations

Even more, for a serial robot with 2-DoF, from Tables1 to 2, line i=3 must be removed and (16) becomes(43)H=O2×2X2×101,with(44)β¯i=f¯i00g¯i,χ1=q=∑i=0nf¯ididtiq~1,∑i=0ng¯ididtiq~2T,χ2=q̇=ddt∑i=0nf¯ididtiq~1,ddt∑i=0ng¯ididtiq~2T.Also, when a 1-DoF is considered, from Tables1 to 2, line i=2,3 must be removed and (16) becomes(45)H=O1×1X1×101,with(46)β¯i=f¯i,χ1=q=∑i=0nf¯ididtiq~1,χ2=q̇=ddt∑i=0nf¯ididtiq~1.Since procedures (16)–(40) have intermediated outcomes, it is easy to compute results for a 1-DoF and for a 2-DoF robot, to obtain the same conclusions.Further, invoking the induction method, these results can be extended to serial robots withk-DoF for all k≥1.In order to deal with sensor inclusion mathematical complexity, the following remarks are useful for practical implementations:(47)dpdtp∑i=0nβ¯ididtiq~=∑i=0nβ¯idi+pdti+pq.~Remark 4.
For the case to consider matrixβi as a constant, the following identity should be used, in order to simplify the algebraic complications,And for the case to consider matrixβi=I,(48)dpdtp∑i=0nβ¯ididtiq~=∑i=pn+pdidtiq~.Remark 5.
For initial conditions at the originq~0=q~̇0=q~̇0=⋯=0, the Laplace transform for sensors of the k-th order with βi=I,(49)L∑i=0ndidtiq~=Q~s∑i=0nsi=Qs,such that(50)fq=L−1Qs∑i=0nsi.


## 2.2.1. Robot’s Dynamics

Let us represent the total torque vector (τ) of the robot under consideration according to the Euler-Lagrange approach, as(24)τ=Mqq¨+Ṁqq̇−∂∂q12q̇TMqq̇+∂Uq∂q+Ffτq,q̇,where M∈ℝ3×3 is the inertia matrix, U∈ℝ3 is the potential energy vector, and Ffτ∈ℝ3 is the friction force vector.Using the methodology detailed in [15–17] to obtain the torque for the j-th link, (24) is transformed to the following form(25)τi=ddt∂∂q̇Lq,q̇−∂∂qLq,q̇+Ffτq,q̇,where L∈ℝ3 is the Lagrangian given by(26)Lq,q̇=Kq,q̇−Uq,with K and U as the kinetic and potential energies, respectively, which according to the referenced methodology are calculated for this work as(27)Kq,q̇=12I1+I2+I3+m2lc22sinq22+m3l22sinq22+m3l2lc3sinq2sinq2+q3+m3lc32sinq2+q32q̇12+12I2+I3+m2lc22+m3l22+m3l2lc3cosq3q̇22+12I3+m3lc32q̇32+I2+I3q̇1q̇2+I3q̇1q̇3+I3+m3l2lc3cosq3+m3lc32q̇2q̇3,Uq=m2glc21−cosπ−q2+m3gl2+lc3−l2cosπ−q2+lc3cosπ−q2−q3, where I=I1,I2,I3T, m=m1,m2,m3T, and lc=lc1,lc2,lc3T are the inertia, mass, and length vectors, respectively, for the j-th centroid, and g=9.81m/s2.Separating each element of vector (25) and calculating(28)τ1=ddt∂∂q̇1Lq,q̇−0+Ff1τ,τ2=ddt∂∂q̇2Lq,q̇−∂∂q2Lq,q̇+Ff2τ,τ3=ddt∂∂q̇3Lq,q̇−∂∂q3Lq,q̇+Ff3τ,where(29)ddt∂∂q̇1Lq,q̇=I1+I2+I3+m2lc22sinq22+m3l22sinq22+m3l2lc3sinq2sinq2+q3+m3lc32sinq2+q32q¨1+2m2lc22sinq2cosq2+2m3l22sinq2cosq2+m3l2lc3cosq2sinq2+q3+m3l2lc3sinq2cosq2+q3+2m3lc32sinq2+q3cosq2+q3q̇1q̇2+m3l2lc3sinq2cosq2+q3+2m3lc32sinq2+q3cosq2+q3q̇1q̇3+I2+I3q¨2+I3q¨3,ddt∂∂q̇2Lq,q̇=I2+I3q¨1+I2+I3+m2lc22+m3lc22+m3l2lc3cosq3q¨2+I3+m3lc32+m3l2lc3cosq3q¨3−m3l2lc3sinq3q̇2q̇3+q̇32,ddt∂∂q̇3Lq,q̇=I3q¨1+I3+m3lc32+m3l2lc3cosq3q¨2+I3+m3lc32q¨3−m3l2lc3sinq3q̇2q̇3,(30)∂∂q1Lq,q̇=0,∂∂q2Lq,q̇=m2lc22sinq2cosq2+m3l22sinq2cosq2+12m3l2lc3sinq2cosq2+q3+cosq2sinq2+q3+m3lc32sinq2+q3cosq2+q3q̇12+m2glc2sinπ−q2+m3gl2sinπ−q2+m3glc3sinπ−q2−q3,∂∂q3Lq,q̇=12m3l2lc3sinq2cosq2+q3+m3lc32sinq2+q3cosq2+q3q̇12−m3l2lc3sinq312q̇22+q̇2q̇3+m3glc3sinπ−q2−q3,Defining the Coriolis forces and gravitational vectors, respectively, as(31)Cq,q̇q̇=Ṁqq̇−∂∂q12q̇TMqq̇,Gq=∂Uq∂q,and including (2) in (24), the torque vector takes the form(32)τ=Mq~d2dt2∑i=0nβ¯ididtiq~+Cq~,q~̇ddt∑i=0nβ¯ididtiq~+Gq~+Ffτq~,q~̇.Rearranging results presented in (29) and (30) and also including (2), the elements of (32), (Mq~, Cq~,q~̇, and Gq~), are obtained and given in the following form(33)Mq~=M11I2+I3I3I2+I3M22M23I3M32I3+m3lc32,where(34)M11=I1+I2+I3+m2lc22+m3l22sin∑i=0ng¯ididtiq~22+m3l2lc3sin∑i=0ng¯ididtiq~2sin∑i=0ng¯ididtiq~2+∑i=0nh¯ididtiq~3+m3lc32sin∑i=0ng¯ididtiq~2+∑i=0nh¯ididtiq~32,M22=I2+I3+m2lc22+m3l2lc3cos∑i=0nh¯ididtiq~3,M23=M32=I3+m3lc32+m3l2lc3cos∑i=0nh¯ididtiq~3,(35)Cq~,q~̇=2Addt∑i=0nh¯ididtiq~202Bddt∑i=0nf¯ididtiq~1−Addt∑i=0nf¯ididtiq~1−2Dddt∑i=0nh¯ididtiq~3−2Dddt∑i=0nh¯ididtiq~3−Bddt∑i=0nf¯ididtiq~1Dddt∑i=0ng¯ididtiq~20,where(36)A=m2lc22+m3l22sin∑i=0ng¯ididtiq~2cos∑i=0ng¯ididtiq~2+m3l2lc32cos∑i=0ng¯ididtiq~2sin∑i=0ng¯ididtiq~2+∑i=0nh¯ididtiq~3+B,(37)B=12m3l2lc3sin∑i=0ng¯ididtiq~2cos∑i=0ng¯ididtiq~2+∑i=0nh¯ididtiq~3+m3lc32sin∑i=0ng¯ididtiq~2+∑i=0nh¯ididtiq~3cos∑i=0ng¯ididtiq~2+∑i=0nh¯ididtiq~3,(38)D=12m3l2lc3sin∑i=0nh¯ididtiq~3,(39)Gq~=−g0m2lc2+m3l2sinπ−∑i=0ng¯ididtiq~2+m3lc3sinπ−∑i=0ng¯ididtiq~2−∑i=0nh¯ididtiq~3m3lc3sinπ−∑i=0ng¯ididtiq~2−∑i=0nh¯ididtiq~3.Additionally, the friction vector symbolic form(40)Ffτq~,q~̇=Ff1τFf2τFf3τT,must be included in the dynamic model.Even more, the same results presented in (33)–(39) are obtained, when (2) is directly included in the Euler-Lagrange (24) and in (31), as follows(41)τ=Mq~d2dt2∑i=0nβ¯ididtiq~+Ṁq~ddt∑i=0nβ¯ididtiq~−∂∂q12ddt∑i=0nβ¯ididtiq~TMq~ddt∑i=0nβ¯ididtiq~+∂Uq~∂q+Ffτq~,q~̇,Cq~,q~̇d2dt2∑i=0nβ¯ididtiq~=Ṁq~ddt∑i=0nβ¯ididtiq~−∂∂q12ddt∑i=0nβ¯ididtiq~TMq~ddt∑i=0nβ¯ididtiq~Gq~=∂Uq~∂q,where(42)β¯i=f¯i000g¯i000h¯i,χ1=q=∑i=0nf¯ididtiq~1,∑i=0ng¯ididtiq~2,∑i=0nh¯ididtiq~3T,χ2=q̇=ddt∑i=0nf¯ididtiq~1,ddt∑i=0ng¯ididtiq~2,ddt∑i=0nh¯ididtiq~3T,which proves dynamics invariance to sensor inclusion.


## 2.2.2. Extensive Considerations

Even more, for a serial robot with 2-DoF, from Tables1 to 2, line i=3 must be removed and (16) becomes(43)H=O2×2X2×101,with(44)β¯i=f¯i00g¯i,χ1=q=∑i=0nf¯ididtiq~1,∑i=0ng¯ididtiq~2T,χ2=q̇=ddt∑i=0nf¯ididtiq~1,ddt∑i=0ng¯ididtiq~2T.Also, when a 1-DoF is considered, from Tables1 to 2, line i=2,3 must be removed and (16) becomes(45)H=O1×1X1×101,with(46)β¯i=f¯i,χ1=q=∑i=0nf¯ididtiq~1,χ2=q̇=ddt∑i=0nf¯ididtiq~1.Since procedures (16)–(40) have intermediated outcomes, it is easy to compute results for a 1-DoF and for a 2-DoF robot, to obtain the same conclusions.Further, invoking the induction method, these results can be extended to serial robots withk-DoF for all k≥1.In order to deal with sensor inclusion mathematical complexity, the following remarks are useful for practical implementations:(47)dpdtp∑i=0nβ¯ididtiq~=∑i=0nβ¯idi+pdti+pq.~Remark 4.
For the case to consider matrixβi as a constant, the following identity should be used, in order to simplify the algebraic complications,And for the case to consider matrixβi=I,(48)dpdtp∑i=0nβ¯ididtiq~=∑i=pn+pdidtiq~.Remark 5.
For initial conditions at the originq~0=q~̇0=q~̇0=⋯=0, the Laplace transform for sensors of the k-th order with βi=I,(49)L∑i=0ndidtiq~=Q~s∑i=0nsi=Qs,such that(50)fq=L−1Qs∑i=0nsi.


## 2.3. Control Usingn-th Order Sensors’ Output

Now, in order to take advantage of Claim1, the following control conclusions are given:Assumption 3.
It is assumed that sensors are previously well characterized,i.e., both n (the order of every sensor) and sensor’s dynamics are known; this implies that ε→0.Claim 2.
Supported by Theorems2 and 3, and if q∗→q, it can be claimed that equation (2) becomes the method to identify q from q~ to control (14), which represents the robot with n-th order sensor inclusion. Thus,(51)e=Xref−∑i=0nβ¯ididtiq,~where e∈ℝk is the tracking error, and Xref∈ℝk represents the reference signal.
The method is depicted in Figure4, where the dashed squared block of the negative feedback represents (2).Note 1.
Solutionq~ of (2) cannot be directly used to control (6).Extension of Claim 2.
In [18], the Lasalle-invariance principle (generally used to show asymptotic stability in the Lyapunov approach) is extended to nonautonomous switching systems. This implies that Claim 2 is valid using both continuous-based and discontinuous-based controllers, e.g., Proportional Integral Derivative (PID) control and Sliding Mode (SM) controllers.
Even more, in [19], an extensive study to show the control of MIMO systems by the twisting-algorithm (a second-order SM control) is presented, showing asymptotic convergence of the system trajectories to the selected sliding manifold.
Further, in order to review the stability for biorder SM (twisting-algorithm included) control with variable PID gains, [20] can be consulted.Figure 4
Block diagram of the closed-loop control with sensor inclusion.


## 3. Implementation Example

To verify the validity of Claims1 and 2, this example is designed to solve the trajectory tracking control of the anthropomorphic robot with 3-DoF (see Figures 2, 3, and 5) when n-th order sensors are included on it and when conditions (including assumptions) are accomplished.Figure 5
Actual 3-DoF anthropomorphic robot arm parameter model.Selectingn=2, and(52)β¯0=10000.90001,β¯1=0.500000000.3,β¯2=0.100000000, linear sensors, with constant coefficients of order two, zero, and one, are included in joints one, two, and three, respectively, as(53)q1∗=q~1+12q~̇1+110q~¨1,q2∗=910q~2,q3∗=q~3+310q~̇3.Reference signal to track for the robot is selected as(54)X1ref=12if0≤t≤5,−12if5<t≤10,X2ref=25sin25πt,X3ref=250t+110.Thus, the ideal form of sensor systems is(55)q~1+12q~̇1+110q~¨1=±12,910q~2=25sin25πt,q~3+310q~̇3=250t+110.Solving forq~, for the open-loop system, the solution has the structure(56)q~1=12∓12exp−52tcos152t∓5215exp−52tsin152t,q~2=49sin25πt,q~3=11125+125t−11125exp−103t.As the second and third terms inq~1 are negative for X1ref=1/2 and they are positive for X1ref=−1/2, then, the form of solution q~ exists and is unique.Note 2.
Equations (55)–(56) are not included in the control method; they just show the ideal solution structure. For closed-loop control purposes, q~ are obtained dynamically from sensor’s output.Even more, for selecting parameters of Table3 and friction models, the actual robot shown in Figure 5 was used, but no other application was performed using the real robot.Table 3
Parameters of the actual 3-DoF anthropomorphic robot arm.
Link numberi (DoF)Total lengthl [m]Centroid lengthlc [m]Total massm [kg]Inertia averageI [kg·m2]10.2180.1622.80.8820.160.0581.20.9630.2550.120.521.08The following friction values are used:(57)f1=0.24q̇1kg·m2s+0.16sgnq̇1kg·m2s2,f2=0.21q̇2kg·m2s+0.18sgnq̇2kg·m2s2,f3=0.17q̇3kg·m2s+0.15sgnq̇3kg·m2s2,where it is possible to see that if any articular velocity of the robot becomes zero, then the corresponding friction equation is reduced to zero; the sign function (sgn) appears in the second terms. This vector includes friction forces produced by ball bearings, gear boxes, and drive belts. Thus, the approximated values can be obtained directly from the manufacturer’s mechanical components.Substituting the parameters of Table3 in (33)–(39) considering the friction model, and including sensors (52), numerical values for M, C, G, and Ffτ are given by(58)Mq~=M11∗2.041.082.04M22∗M23∗1.08M32∗1.0875,where(59)M11∗=2.92+17.3sin20.9q~2+9.984sin0.9q~2sin0.9q~2+q~3+0.3q~̇3+7.488sin20.9q~2+q~3+0.3q~̇3×10−3,M22∗=2.044+9.984cosq~3+0.3q~̇3×10−3,M23∗=M32∗=1.0875+9.984cosq~3+0.3q~̇3×10−3,Cq~,q~̇=1.8q~¨2A∗02q~̇1+q~¨1+0.2q~⃛1B∗−q~̇1+0.5q~¨1+0.1q~⃛1A∗−2q~̇3+0.6q~¨3D∗−2q~̇3+0.6q~¨3D∗−q~̇1+0.5q~¨1+0.1q~⃛1B∗q~¨2D∗0,with the following numerical values(60)A∗=17.3sin0.9q~2cos0.9q~2+4.992cos0.9q~2sin0.9q~2+q~3+0.3q~̇3×10−3+B∗,(61)B∗=4.992sin0.9q~2cos0.9q~2+q~3+0.3q~̇3+7.488sin0.9q~2+q~3+0.3q~̇3cos0.9q~2+q~3+0.3q~̇3×10−3,(62)D∗=4.992sinq~3+0.3q~̇3×10−3,(63)Gq~=00.1528sinπ−0.9q~2+0.0624sinπ−0.9q~2−q~3−0.3q~̇30.0624sinπ−0.9q~2−q~3−0.3q~̇3,(64)Ffτq~,q~̇=0.24q~̇1+0.12q~¨1+0.024q~⃛1+0.16sgnq~̇1+0.5q~¨1+0.1q~⃛10.189q~̇2+0.18sgnq~̇20.17q~̇3+0.051q~¨3+0.15sgnq~̇3+0.3q~¨3.


### 3.1. Controllers Used

Two techniques with different control properties are considered in this example to solve the trajectory tracking problem of the considered robot: the classical Proportional Integral Derivative (PID), and a Variable Structure System-based method.The PID technique produces a smooth control signal, given by,(65)upid=Kpe+Ki∫t0t1edt+Kddedt,where upid∈ℝ3 represents the value for the torque (control signal); Kp∈ℝ3, Ki∈ℝ3, and Kd∈ℝ3 are the proportional, integral, and derivative gains, respectively; and e=q−Xref∈ℝ3 represents the tracking error of the control system, with Xref as the reference signal. For this example, the PID controller is tuned with Kp=1000, Ki=500, and Kd=400 for every one of the robot joints.The other considered technique is the following Sliding Mode Control (SMC) of order two,(66)usmc=−r1sgne−r2sgnė,r1>r2≥0,where usmc∈ℝ3 represents the control signal generated by the SMC; r1,r2∈ℝ3 are the so-called twisting parameters; and sgn· is the well-known sign function. For this example, the twisting controller is tuned with r1=100 and r2=50, for the first joint; r1=150 and r2=75, for the second joint; and, r1=50 and r2=25, for the third joint.Thus, the whole control system for this implementation example is depicted in Figure6, where the reference signal (54), the control either (65) or (66), the robot (58)–(64), and the sensors (52)–(53), along with the closed-loop feedback given by Claim 2, are included.Figure 6
Closed-loop control diagram with sensor inclusion for this implementation example.


### 3.2. Simulation Results

It is worth to mention that this section is not intended to show the benefits of the two control techniques presented, but to show that the robot withn-th order sensor inclusion has controllability properties.Figures7, 8, 9 show the performance of the first, second, and third joints of the robot in tracking a square wave form, a sine wave, and an upward sloping function, respectively (in dashed red line), under the control of the PID (continuous black line) and under the SMC (continuous blue line).Figure 7
The first joint of the robot tracking a square wave as the reference signal.Figure 8
The control for the second joint of the robot in tracking a sine wave form.Figure 9
The third joint of the robot solving the trajectory tracking of an upward sloping.In these simulations, the characteristic behaviour of both controllers can be appreciated, which supports that Claim2 is correct. The PID is tuned with high gain values to obtain fast convergence and also to avoid large overshoots. Nevertheless, the PID comes out from convergence when an adjacent joint suddenly changes, i.e., the sudden change of a joint impacts (or even shocks) the convergence of the adjacent ones. Also, it can be appreciated that the SMC is a robust control technique, because it never comes out from convergence once it is reached (after the transient state).Even more, Figure10 shows an enlarged view of Figure 7 from 6 to 6.5 [s], where the so-called chattering effect produced by the SMC appears.Figure 10
Enlarged view (zoom) of Figure7 to show deeply the behaviour of the SMC.


## 3.1. Controllers Used

Two techniques with different control properties are considered in this example to solve the trajectory tracking problem of the considered robot: the classical Proportional Integral Derivative (PID), and a Variable Structure System-based method.The PID technique produces a smooth control signal, given by,(65)upid=Kpe+Ki∫t0t1edt+Kddedt,where upid∈ℝ3 represents the value for the torque (control signal); Kp∈ℝ3, Ki∈ℝ3, and Kd∈ℝ3 are the proportional, integral, and derivative gains, respectively; and e=q−Xref∈ℝ3 represents the tracking error of the control system, with Xref as the reference signal. For this example, the PID controller is tuned with Kp=1000, Ki=500, and Kd=400 for every one of the robot joints.The other considered technique is the following Sliding Mode Control (SMC) of order two,(66)usmc=−r1sgne−r2sgnė,r1>r2≥0,where usmc∈ℝ3 represents the control signal generated by the SMC; r1,r2∈ℝ3 are the so-called twisting parameters; and sgn· is the well-known sign function. For this example, the twisting controller is tuned with r1=100 and r2=50, for the first joint; r1=150 and r2=75, for the second joint; and, r1=50 and r2=25, for the third joint.Thus, the whole control system for this implementation example is depicted in Figure6, where the reference signal (54), the control either (65) or (66), the robot (58)–(64), and the sensors (52)–(53), along with the closed-loop feedback given by Claim 2, are included.Figure 6
Closed-loop control diagram with sensor inclusion for this implementation example.


## 3.2. Simulation Results

It is worth to mention that this section is not intended to show the benefits of the two control techniques presented, but to show that the robot withn-th order sensor inclusion has controllability properties.Figures7, 8, 9 show the performance of the first, second, and third joints of the robot in tracking a square wave form, a sine wave, and an upward sloping function, respectively (in dashed red line), under the control of the PID (continuous black line) and under the SMC (continuous blue line).Figure 7
The first joint of the robot tracking a square wave as the reference signal.Figure 8
The control for the second joint of the robot in tracking a sine wave form.Figure 9
The third joint of the robot solving the trajectory tracking of an upward sloping.In these simulations, the characteristic behaviour of both controllers can be appreciated, which supports that Claim2 is correct. The PID is tuned with high gain values to obtain fast convergence and also to avoid large overshoots. Nevertheless, the PID comes out from convergence when an adjacent joint suddenly changes, i.e., the sudden change of a joint impacts (or even shocks) the convergence of the adjacent ones. Also, it can be appreciated that the SMC is a robust control technique, because it never comes out from convergence once it is reached (after the transient state).Even more, Figure10 shows an enlarged view of Figure 7 from 6 to 6.5 [s], where the so-called chattering effect produced by the SMC appears.Figure 10
Enlarged view (zoom) of Figure7 to show deeply the behaviour of the SMC.


## 4. Conclusions

As a result of this work, it is possible to affirm that serial robots can be controlled by taking advantage of the output ofn-th order sensory systems, provided the following conditions and assumptions are fulfilled.Conditions:(1)
The solution of the differential equations that describe the dynamics of individual sensors must exist and be unique(2)
The identification value of the actual robot’s position should be negligible,ε→0(3)
The dynamics of the electromechanical plant withn-th order sensor inclusion should be invariant with respect to the dynamics of the plant’s output. In this work, it has been demonstrated that for the case of serial robot manipulators with k-DoF, this condition is always fulfilledAssumptions:(1)
The value of the plant’s outputq~ is always available to be used by the closed-loop control system(2)
Coefficients of terms that describe the sensor dynamics are known. In this work, these terms are represented by diagonal matricesβ¯i with i=1,⋯,n in (2).(3)
Plant’s dynamics is known,i.e., the differential equation that model plant’s dynamics is characterizedThen, serial robot manipulators are controllable using either continuous or variable structure-based controllers, and the differential equation that models sensor dynamics becomes the method to obtain the feedback for the closed-loop control.



---

*Source: 8884282-2021-01-11.xml*