Adaptive Trajectory Control - uniroma1.it

Robotics 2

Prof. Alessandro De Luca

Adaptive Trajectory Control

Motivation and approach

n need of adaptation in robot motion control lawsn large uncertainty on the robot dynamic parametersn poor knowledge of the inertial payload

n characteristics of direct adaptive controln direct aim is to bring to zero the state trajectory error, i.e.,

position and velocity errorsn no need to estimate on line the true values of the dynamic

coefficients of the robot (as opposed to indirect adaptive control)n main tool and methodology

n linear parametrization of robot dynamicsn nonlinear control law of the dynamic type (the controller has its

own ‘states’)

Robotics 2 2

Summary of robot parametersn parameters assumed to be known

n kinematic description based, e.g., on Denavit-Hartenberg parameters ({𝛼", 𝑑", 𝑎", 𝑖 = 1, … , 𝑁} in case of all revolute joints), including link lengths (kinematic calibration)

n uncertain parameters that can be identified off linen masses 𝑚", positions 𝑟-" of CoMs, and inertia matrices 𝐼" of each link,

appearing in combinations (dynamic coefficients) ⇒ 𝑝 ≪ 10×𝑁n parameters that are (slowly) varying during operation

n viscous 𝐹5", dry 𝐹6", and stiction 𝐹7" friction at each joint ⇒ 1 ÷ 3×𝑁n unknown and abruptly changing parameters

n mass, CoM, inertia matrix of the payload w.r.t. the tool center point

when a payload is firmly attached to the robot E-E, only the 10 parameters of the last link are modified, influencing however most part of the robot dynamics

Robotics 2 3

Goal of adaptive control

n given a twice-differentiable desired joint trajectory 𝑞;(𝑡)n with known desired velocity ��;(𝑡) and acceleration ��;(𝑡)n possibly obtained by kinematic inversion + joint interpolation

n execute this trajectory under large dynamic uncertainties n with a trajectory tracking error vanishing asymptotically

n guaranteeing global stability, no matter how far are the initial estimates of the unknown/uncertain parameters from their true values and how large is the initial trajectory error

n identification is not of particular concern: in general, the estimates of dynamic coefficients will not to converge to the true ones!

n if this convergence is a specific extra requirement, then one should use (more complex) indirect adaptive schemes

Robotics 2 4

𝑒 = 𝑞; − 𝑞 ⟶ 0 �� = ��; − �� ⟶ 0

n vector 𝑎 contains only unknown or uncertain coefficientsn each component of 𝑎 is in general a combination of the

robot physical parameters (not necessarily all of them)n the model regression matrix 𝑌 depends linearly on ��,

quadratically on �� (for the terms related to kinetic energy), and nonlinearly (trigonometrically) on 𝑞

Linear parameterization

n there exists always a (𝑝-dimensional) vector 𝑎 of dynamic coefficients, so that the robot model takes the linear form

Robotics 2 5

𝑀 𝑞 �� + 𝑆 𝑞, �� �� + 𝑔 𝑞 + 𝐹5�� = 𝑢

𝑌 𝑞, ��, �� 𝑎 = 𝑢

Trajectory controllersbased on model estimates

n inverse dynamics feedforward (FFW) + PD (linear) control

n (nonlinear) control based on feedback linearization (FBL)

n approximate estimates of dynamic coefficients may lead to instability with FBL due to temporary ’non-positive’ PD gains (e.g., J𝑀 𝑞 𝐾L < 0!)

n not easy to turn these laws in adaptive schemes: inertia inversion/use of acceleration (FBL); bounds on PD gains (FFW)

Robotics 2 6

N𝑢;

𝑢 = J𝑀 𝑞; ��; + O𝑆 𝑞;, ��; ��; + N𝑔 𝑞; + P𝐹5��; + 𝐾L𝑒 + 𝐾6��

𝑢 = J𝑀 𝑞 (��; + 𝐾L𝑒 + 𝐾6��) + O𝑆 𝑞, �� �� + N𝑔 𝑞 + P𝐹5��

J𝑀, O𝑆, N𝑔, P𝐹5 ⟺ estimate N𝑎

A control law easily made ‘adaptive’n nonlinear trajectory tracking control (without cancellations)

having global asymptotic stabilization properties

n without extra assumptions, it can be shown only that joint velocities become eventually “clamped” to those of the desired trajectory (zero velocity error), but a permanent residual position error is left

𝑢 = J𝑀 𝑞 ��; + O𝑆 𝑞, �� ��; + N𝑔 𝑞 + P𝐹5��; + 𝐾L𝑒 + 𝐾6��n a natural adaptive version would require ...

designing a suitable update law(in continuous time) N𝑎 =

��; ⟶ ��R = ��; + Λ(𝑞; − 𝑞) Λ > 0

Robotics 2 7

n idea: on-line modification with a reference velocity

typically Λ = 𝐾6UV𝐾L (all matrices will be chosen diagonal)

Intuitive interpretation of ��Rn elementary case

n a mass ‘lagging behind’ its mobile reference (𝑒 > 0) on a linear rail

𝑞 𝑞;(𝑡)𝑒 > 0

𝑢

mobilereference

controlledmass

��

𝐾Ln a mass ‘leading in front’ of its mobile reference (𝑒 < 0)

in a symmetric way, a ‘reduced’ velocity error will appear (𝑠 < ��)

Robotics 2 8

��R = ��; + Λ 𝑒

��;

‘enhanced’ velocity error 𝑠 = ��R − �� > ��; − �� = ��

𝑢 = 𝐾6𝑠 = 𝐾6 ��R − �� = 𝐾6 ��; + Λ 𝑒 − �� = 𝐾6�� + 𝐾6Λ 𝑒

n substituting ��R = ��; + Λ𝑒, ��R = ��; + Λ�� in the previous nonlinear controller for trajectory tracking

n update law for the estimates of the dynamic coefficients (N𝑎 becomes the 𝑝-dimensional state of the dynamic controller)

dynamic parameterization ofthe control law using current estimates

(note here the 4 arguments in 𝑌(X) !)

PD stabilization(diagonal matrices, >0)

estimation gains(variation rate of estimates)

(diagonal)

Adaptive control law design

Robotics 2 9

𝑠‘modified’ velocity error

𝑢 = J𝑀 𝑞 ��R + O𝑆 𝑞, �� ��R + N𝑔 𝑞 + P𝐹5��R + 𝐾L𝑒 + 𝐾6��= 𝑌 𝑞, ��, ��R, ��R N𝑎 + 𝐾L𝑒 + 𝐾6��

Γ > 0N𝑎 = Γ𝑌Z(𝑞, ��, ��R, ��R)(��R − ��)

The introduced adaptive controller makes the tracking error along the desired trajectory globally asymptotically stable

Asymptotic stability of trajectory errorTheorem

Robotics 2 10

𝑒 = 𝑞; − 𝑞 ⟶ 0, �� = ��; − �� ⟶ 0

[𝑎 = 𝑎 − N𝑎

n a Lyapunov candidate for the closed-loop system (robot + dynamic controller) is given by

modified velocity error constant matrix(to be specified later)

Proof

error in parametricestimation

𝑉 =12𝑠Z𝑀 𝑞 𝑠 +

12𝑒Z𝑅𝑒 +

12[𝑎ZΓUV [𝑎 ≥ 0

𝑅 > 0𝑠 = ��R − �� (= �� + Λ𝑒)

𝑉 = 0 ⟺ N𝑎 = 𝑎, 𝑞 = 𝑞;, 𝑠 = 0 (⇒ �� = ��;)

n the time derivative of V is

since [𝑎 = − N𝑎 (�� = 0)

Proof (cont)

n the closed-loop dynamics is given by

subtracting the two sides from 𝑀 𝑞 ��R + 𝑆 𝑞, �� ��R + 𝑔 𝑞 + 𝐹5��R

Robotics 2 11

�� =12𝑠Z�� 𝑞 𝑠 + 𝑠Z𝑀 𝑞 �� + 𝑒Z𝑅�� − [𝑎ZΓUV N𝑎

𝑀 𝑞 �� + 𝑆 𝑞, �� �� + 𝑔 𝑞 + 𝐹5�� == J𝑀 𝑞 ��R + O𝑆 𝑞, �� ��R + N𝑔 𝑞 + P𝐹5��R + 𝐾L𝑒 + 𝐾6��

𝑀(𝑞)�� + 𝑆 𝑞, �� + 𝐹5 𝑠 == �� 𝑞 ��R + a𝑆 𝑞, �� ��R + [𝑔 𝑞 + b𝐹5��R − 𝐾L𝑒 − 𝐾6��

leads to

with �� = 𝑀 − J𝑀, a𝑆 = 𝑆 − O𝑆, [𝑔 = 𝑔 − N𝑔, b𝐹5 = 𝐹5 − P𝐹5

n from the property of linearity in the dynamic coefficients, it follows

Proof (cont)

n substituting in ��, together with N𝑎 = Γ𝑌Z𝑠, and using the skew-symmetry of matrix �� − 2𝑆 we obtain

n replacing 𝑠 = �� + Λ 𝑒 and being 𝐹5 = 𝐹5Z (diagonal)

Robotics 2 12

𝑀(𝑞)�� + 𝑆 𝑞, �� + 𝐹5 𝑠 = 𝑌 𝑞, ��, ��R, ��R [𝑎 − 𝐾L𝑒 − 𝐾6��

�� =12𝑠Z �� 𝑞 − 2𝑆 𝑞, �� 𝑠 − 𝑠Z𝐹5𝑠 + 𝑠Z𝑌 [𝑎

−𝑠Z(𝐾L𝑒 + 𝐾6��) + 𝑒Z𝑅�� − [𝑎Z𝑌Z𝑠= −𝑠Z𝐹5𝑠 − 𝑠Z(𝐾L𝑒 + 𝐾6��) + 𝑒Z𝑅��

�� = −𝑒Z(ΛZ𝐹5Λ + ΛZ𝐾L)𝑒−𝑒Z 2ΛZ𝐹5 + ΛZ𝐾6 + 𝐾L − 𝑅 �� − ��Z(𝐹5 + 𝐾6)��a complete

quadratic formin 𝑒, �� !

n defining now (all matrices are diagonal!)

Proof (end)

leads to

and thus

the thesis follows from Barbalat lemma + LaSalle theorem

the set of states of convergence has zero trajectory error and a constant value for N𝑎, not necessarily the true one ( [𝑎 ≠ 0)

Robotics 2 13

Λ = 𝐾6UV𝐾L > 0 𝑅 = 2𝐾L (𝐼 + 𝐾6UV𝐹5) > 0

�� = −𝑒ZΛZ(𝐹5 + 𝐾6)Λ𝑒 − ��Z(𝐹5 + 𝐾6)��= −𝑒Z𝐾L𝐾6UV(𝐹5 + 𝐾6)𝐾6UV𝐾L𝑒 − ��Z(𝐹5 + 𝐾6)�� ≤ 0

�� = 0 ⟺ 𝑒 = �� = 0

Remarksn if the desired trajectory 𝑞;(𝑡) is persistently exciting, then also the

estimates of the dynamic coefficients converge to their true valuesn condition of persistent excitation

n for linear systems: # of frequency components in the desired trajectory should be at least twice as large as # of unknown coefficients

n for nonlinear systems: the condition can be checked only a posteriori(a certain motion integral should be permanently lower bounded)

n in case of known absence of (viscous) friction (𝐹5 ≡ 0), the same proof applies (a bit easier in the final part)

n the adaptive controller does not require the inverse of the inertia matrix (true or estimated), nor the actual robot acceleration (only the desired acceleration), nor further lower bounds on 𝐾L > 0, 𝐾6 > 0

n adaptation can be also used only for a subset of dynamic coefficients, the remaining being known (𝑌𝑎 = 𝑌f;fgh N𝑎f;fgh + 𝑌ijklj𝑎ijklj)

n the non-adaptive version (using accurate estimates) is a static tracking controller based on the passivity property of robot dynamics

Robotics 2 14

Case study: Single-link under gravitymodellinear parameterization

adaptive controller

Robotics 2 15

(with friction)𝐼�� + 𝑚𝑔𝑑 sin 𝜃 + 𝑓5�� = 𝑢𝑢 𝑑

𝑚

𝜃

𝑌 𝜃, ��, �� 𝑎 = �� sin 𝜃 ��𝐼

𝑚𝑔𝑑𝑓5

= 𝑢

��R = ��; +𝑘L𝑘6

𝑒

𝑒 = 𝜃; − 𝜃

𝛾" > 0, 𝑖 = 1,2,3

𝑢 = J𝐼 ��R + t𝑚𝑔𝑑 sin 𝜃 + J𝑓5�� + 𝑘L𝑒 + 𝑘6��

N𝑎 =J𝐼t𝑚𝑔𝑑J𝑓5

=𝛾V��R𝛾u sin 𝜃𝛾v��R

(��R − ��)

Λ > 0

Simulation datan real dynamic coefficients

n initial estimates

n control parameters

n test trajectories (starting with 𝜃 0 = 0, �� 0 = 0) n first

n second

Robotics 2 16

Note: same test trajectoriesused also for robust control

𝐼 = 7.5, 𝑚𝑔𝑑 = 6, 𝑓5 = 1

J𝐼 = 5, t𝑚𝑔𝑑 = 5, J𝑓5 = 2

��; 𝑡 = (periodic) bang-bang acceleration profile with𝐴 = 1 rad/s2, 𝜔 = 1 rad/s

𝜃; 𝑡 = − sin 𝑡

𝑘L = 25, 𝑘6 = 10, 𝛾" = 5, 𝑖 = 1,2,3

Resultsfirst trajectory

position and velocity errors control torque

note the nonlinear system dynamics(no sinusoidal regime at steady state!)

Robotics 2 17

��

𝑒

𝜃; 𝑡 = − sin 𝑡

Resultssecond trajectory

note the torque discontinuities(due to those of the desired acceleration)

Robotics 2 18

position and velocity errors control torque

��; 𝑡 = (periodic) bang-bang acceleration profile

��

𝑒

Estimates of dynamic coefficients

second trajectoryfirst trajectoryerrors [𝑎 = 𝑎 − N𝑎

only the estimate of the viscousfriction coefficient converges

to the true value

all three estimates ofdynamic coefficients converge

to their true valuesRobotics 2 19

𝑚𝑔𝑑 − t𝑚𝑔𝑑

𝐼 − J𝐼

𝑓5 − J𝑓5

𝑚𝑔𝑑 − t𝑚𝑔𝑑

𝐼 − J𝐼

𝑓5 − J𝑓5

A special case: Adaptive regulationn adaptation in case 𝑞; is constantn no special simplifications for the presented adaptive control

law (designed for the general tracking case…)

since ��R = Λ(𝑞; − 𝑞) and ��R = −Λ�� do not vanish!n a different case would be the availability of an adaptive

version of the trajectory tracking controller

since, when 𝑞; collapses to a constant, only the adaptation of the gravity term would be left over (which is what one would naturally expect…)

Robotics 2 20

𝑢 = J𝑀 𝑞 ��R + O𝑆 𝑞, �� ��R + N𝑔 𝑞 + P𝐹}��R + 𝐾L𝑒 + 𝐾6��

N𝑎 = Γ𝑌Z(𝑞, ��, ��R, ��R)(��R − ��)

𝑢 = J𝑀 𝑞 ��; + O𝑆 𝑞, �� ��; + N𝑔 𝑞 + P𝐹}��; + 𝐾L𝑒 + 𝐾6��

An efficient adaptive regulatorn use a linear parameterization of the gravity term only

with a 𝑝𝑔-dimensional vector 𝑎𝑔n an adaptive regulator yielding global asymptotic stability

of the equilibrium state (𝑞𝑑, 0) is provided by

where 𝑒 = 𝑞𝑑 − 𝑞, 𝐾𝑃 > 0, 𝐾𝐷 > 0 (symmetric), and 𝛽 > 0 is chosen sufficiently large

Robotics 2 21(see paper by P. Tomei, IEEE TRA, 1991; available as extra material on the course web)

𝑔 𝑞 = 𝐺(𝑞)𝑎�

𝑢 = 𝐺 𝑞 N𝑎� + 𝐾L 𝑞; − 𝑞 − 𝐾6��

N𝑎� = 𝛾𝐺Z 𝑞2𝑒

1 + 2 𝑒 u − 𝛽�� , 𝛾 > 0

An adaptive regulatorSketch of asymptotic stability analysis

n use the function

n a sufficient condition for 𝑉 to be a Lyapunov candidate is that

n a sufficient condition which guarantees also that

is

Note: for any symmetric, positive definite matrix 𝐴

and thus, e.g., Vu ��Z𝑀 𝑞 �� ≥ V

u𝑀� �� u

Robotics 2 22

𝑉 =𝛽2 ��Z𝑀 𝑞 �� + 𝑒Z𝐾L𝑒 −

2��Z𝑀 𝑞 𝑒1 + 2 𝑒 u +

12 N𝑎� − 𝑎�

ZN𝑎� − 𝑎�

𝛽 >2𝑀�

𝑀�𝐾L,�

�� = ⋯ ≤ −𝑎 𝑒 u − 𝑏 �� u ≤ 0, 𝑎 > 0, 𝑏 > 0

𝛽 > max2𝑀�

𝑀�𝐾L,�,1

𝐾6,�𝐾6,�u

2𝐾L,�+ 4𝑀� +

𝛼72

𝑆(𝑞, ��) ≤ 𝛼7��

𝐴� = 𝜆��� 𝐴 = 𝜆��� 𝐴Z𝐴 = 𝐴𝐴� = 𝜆��� 𝐴