DanielSimon Slackeningreal-timeconstraintsinrobotcontrol:designandimplementation

Slackening real-time constraints in robot control: design and implementation

Daniel Simon

DEMAR team

INRIA Sophia Antipolis - Méditerranée LIRMM, CNRS UMR 5506, Montpellier http://www.lirmm.fr/demar

Journée Architectures Logicielles pour la Robotique Autonome, les Systèmes Cyber-Physiques et les Systèmes Auto-Adaptables

Control and scheduling Feedback scheduling Implementation Summary

Control and scheduling

+

− e(k)

ZOH

h h

U(k)

Network Observer

CONTROLLER Xd(k)

X(k)

Y(k)

U(k) = K(e(k)) ^SAMP

PROCESS

Continuous time Discrete time

Goal : reaching a specified closed-loop performance under constraint of limited computing architecture

the loop must be ¨fast enough¨ compared with the process dynamics

fast sampling and low latency

Traditional solution :

Control design in continuous or discrete time single fixed rate, null or known and fixed delay; preemptive and fixed priority RTOS

known WCET, off line schedulability analysis neither optimal nor flexible w.r.t. the control objective

co-design approach

Control and scheduling Feedback scheduling Implementation Summary

Control and scheduling

+

− e(k)

ZOH

h h

U(k)

Network Observer

CONTROLLER Xd(k)

X(k)

Y(k)

U(k) = K(e(k)) ^SAMP

PROCESS

Continuous time Discrete time

Traditional solution :

Control design in continuous or discrete time single fixed rate, null or known and fixed delay;

preemptive and fixed priority RTOS

known WCET, off line schedulability analysis

Control and scheduling Feedback scheduling Implementation Summary

Control and scheduling

+

− e(k)

ZOH

h h

U(k)

Network Observer

CONTROLLER Xd(k)

X(k)

Y(k)

U(k) = K(e(k)) ^SAMP

PROCESS

Continuous time Discrete time

Traditional solution :

Control design in continuous or discrete time single fixed rate, null or known and fixed delay;

preemptive and fixed priority RTOS

known WCET, off line schedulability analysis neither optimal nor flexible w.r.t. the control objective

co-design approach

Control and scheduling Feedback scheduling Implementation Summary

Feedback

General benefits of closing the loop :

robustness w.r.t. process modeling uncertainties rejection of noise and disturbances

tolerance w.r.t. timing uncertainties

robustness improvement due to feedback action Trade off between control and computing

QoS formulation

relations between control performance and timing parameters?

Control and scheduling Feedback scheduling Implementation Summary

Feedback

General benefits of closing the loop :

robustness w.r.t. process modeling uncertainties rejection of noise and disturbances

tolerance w.r.t. timing uncertainties

Control tools in the scheduler implementation : on-line adaption to the real timing parameters robustness improvement due to feedback action

Trade off between control and computing QoS formulation

relations between control performance and timing parameters?

Feedback

General benefits of closing the loop :

robustness w.r.t. process modeling uncertainties rejection of noise and disturbances

tolerance w.r.t. timing uncertainties

Control tools in the scheduler implementation : on-line adaption to the real timing parameters robustness improvement due to feedback action Trade off between control and computing

QoS formulation

relations between control performance and timing parameters?

Control and scheduling Feedback scheduling Implementation Summary

Feedback scheduling

Hierarchical control structure :

− + Uk +−

Controller Scheduling

Scheduling Parameters

Process state estimates CPU/network state load/latency estimates

Y supervision

exceptions handling

RTOS Scheduler

(QoC) Process objectives Global objective

QoS

SAMP

Controller

Process ZOH Process

Inner loops : process controllers

independent control laws objectives robustness w.r.t. timing uncertainties adaptive w.r.t. scheduling parameters

Feedback scheduling

Hierarchical control structure :

− + Uk +−

Controller Scheduling

Scheduling Parameters

Process state estimates CPU/network state load/latency estimates

Y supervision

exceptions handling

RTOS Scheduler

(QoC) Process objectives Global objective

QoS

SAMP

Controller

Process ZOH Process

Outer loop : scheduling controller

global objective : trade off between control and scheduling managing concurrency between controllers

CPU load regulation, robustness, QoS optimization, . . .

Control and scheduling Feedback scheduling Implementation Summary

Dynamic model for tasks

U_r +

−

+ U_others Plant +

control tasks fi

Scheduling controller

Dynamical system between tasks frequencies and CPU usage

Dynamics = integrators (queues) and filters Other tasks act as external disturbances Actuators = scheduling parameters Sensors = probes for computing activity

actuators and sensors are made of (almost free) software

Dynamic model for tasks

U_r +

−

+ U_others Plant +

control tasks fi

Scheduling controller

Dynamical system between tasks frequencies and CPU usage

Dynamics = integrators (queues) and filters Other tasks act as external disturbances Actuators = scheduling parameters Sensors = probes for computing activity

actuators and sensors are made of (almost free) software

Control and scheduling Feedback scheduling Implementation Summary

Actuators

U = ^c_hⁱ

i =c_i∗f_i =⇒periods are effective commands CPU load = control variable, constraint, parameter...

Out of Control

Sampling Rate fast

slow Acceptable performance Unacceptable Performance

Performance

Control Networked Digital

Control

Continuous Control

continuous sampling variations event based control

(m,k)-firm selective dropping

control law variants with different execution costs PID vs. computed torque?

anytime predictive controllers (iterative refinements) CPU frequency scaling

priority :

no effect on the load

based on relative urgency criteria

Control and scheduling Feedback scheduling Implementation Summary

Actuators

U = ^c_hⁱ

i =c_i∗f_i =⇒periods are effective commands CPU load = control variable, constraint, parameter...

continuous sampling variations event based control

(m,k)-firm selective dropping

anytime predictive controllers (iterative refinements) CPU frequency scaling

priority :

no effect on the load

based on relative urgency criteria

Control and scheduling Feedback scheduling Implementation Summary

Actuators

U = ^c_hⁱ

i =c_i∗f_i =⇒periods are effective commands CPU load = control variable, constraint, parameter...

continuous sampling variations event based control

(m,k)-firm selective dropping

control law variants with different execution costs PID vs. computed torque?

anytime predictive controllers (iterative refinements)

CPU frequency scaling priority :

no effect on the load

based on relative urgency criteria

Control and scheduling Feedback scheduling Implementation Summary

Actuators

U = ^c_hⁱ

i =c_i∗f_i =⇒periods are effective commands CPU load = control variable, constraint, parameter...

continuous sampling variations event based control

(m,k)-firm selective dropping

control law variants with different execution costs PID vs. computed torque?

anytime predictive controllers (iterative refinements) CPU frequency scaling

based on relative urgency criteria

Control and scheduling Feedback scheduling Implementation Summary

Actuators

U = ^c_hⁱ

i =c_i∗f_i =⇒periods are effective commands CPU load = control variable, constraint, parameter...

continuous sampling variations event based control

(m,k)-firm selective dropping

control law variants with different execution costs PID vs. computed torque?

anytime predictive controllers (iterative refinements) CPU frequency scaling

priority :

no effect on the load

based on relative urgency criteria

Sensors

Estimation of the CPU load

Discrete filtering of the mean task load during one period of the feedback scheduler

Uˆ_khs =λUˆ_(k−1)hs + (1−λ) c_khs h_(k−1)hs Estimation of execution time in practice

direct measure ofc : not always available (e.g. OK with Xenomai, not with Linux)

using the time of response : over-estimates the load (but Posix compliant)

deadlines miss, laxity. . . (but coming too late)

Control and scheduling Feedback scheduling Implementation Summary

Control scheme for CPU resources

K(z)

1

c2 T ask2

−

+ T ask₁

H(z) Uothers

U_r

+ + + f1

f2 1 c1

Design of K using control methodology (e.g. H∞, LQG, . . . ) On line adaptation to execution time variations

Task frequency saturations

Feedback scheduling a robot controller

q

G PA10 Gc

Gi

Gg

Gt

End

Sing q

qd

j Mission

Sequencing Supervision Exceptions

manager

Linux/Xenomai

Sched_Ctrl

Mux

Torque

GeneTraj Gravite

Inertie Coriolis

Control and scheduling Feedback scheduling Implementation Summary

Robot arm control

Feedback scheduling a robot controller (Contd)

Dynamic CPU power assignment:

U_G=αGU, U_I= (1−αG)U, U_C =0.25U

α_G=αmin+ (αMax−αmin)× qd−qend

qini−qend

0 0.5 1 1.5 2 2.5

0 0.002 0.004 0.006 0.008 0.01 0.012 0.014 0.016 0.018 0.02

Time [s]

Periods [s]

Task periods Inertia Gravity Coriolis

0 0.5 1 1.5 2 2.5

0 0.1 0.2 0.3 0.4 0.5

Time [s]

Load

Task loads

Inertia Gravity Coriolis

0 0.5 1 1.5 2 2.5

0 0.2 0.4 0.6 0.8 1

Time [s]

Load

Processor Load Regulated Reference Integrator Grand total

0 0.5 1 1.5 2 2.5

−250

−200

−150

−100

−50 0 50 100 150

Time [s]

Joint torques [mN]

Commands

1 2 3 4 5 6 7

Intervals CPU load Torques

Cost of the FS < 1%

Control and scheduling Feedback scheduling Implementation Summary

Drone control

Discrete sampling control

f₁ f₄

f₃ f2

 



y_n zn

xb yb z_b x_n

Q₁ Q₄

Q₃ Q2

x_i+1 = Φ(hn)x_i+ Γ(hn)u_i yi = Cd(hn)xi+Dd(hn)ui

u_i = [τΦτΘτΨ]^T = [L_ΦL_ΘL_Ψ]x_i^T

(m,k)-firm:mmandatory executions everyksamples hn=ti+1−ti=n.h u_i=Lnx_i,Ln= [l_n,1,l_n,2]

Discrete-Time Switched System and Common Quadratic Lyapunov Function:

ln,1= −4l_1,1

b0n²h²l1,1+2b0nhl1,2−b0nh²l1,1−4−2b0hl1,2

ln,2= −2(nhl_1,1+2l1,2−hl1,1)

b0n²h²l1,1+2b0nhl1,2−b0nh²l1,1−4−2b0hl1,2

q3 q2 q1 q0

(m,k)-pattern

mΦ=mΘ=mΨ=5 mΦ=mΘ=1,mΨ=5

(m,k)-pattern q1 q0 q2 q3

Control and scheduling Feedback scheduling Implementation Summary

Drone control

Discrete sampling control

f₁ f₄

f₃ f2

 



y_n zn

xb yb z_b x_n

Q₁ Q₄

Q₃ Q2

x_i+1 = Φ(hn)x_i+ Γ(hn)u_i yi = Cd(hn)xi+Dd(hn)ui

u_i = [τΦτΘτΨ]^T = [L_ΦL_ΘL_Ψ]x_i^T

skipped execution=⇒do not release the clock

q3 q2 q1 q0

(m,k)-pattern

mΦ=mΘ=mΨ=5 mΦ=mΘ=1,mΨ=5

(m,k)-pattern q1 q0 q2 q3

Control and scheduling Feedback scheduling Implementation Summary

Summary

Computing/communication resources can be controlled Feedback control loops are firm real-time

No further need for WCET evaluation

=⇒sizing w.r.t. average values Robust w.r.t. timing disturbances

=⇒no need for perfect determinism Easily feasible, with off-the-shelf hard/soft

Control and scheduling Feedback scheduling Implementation Summary

Summary

Computing/communication resources can be controlled Feedback control loops are firm real-time

No further need for WCET evaluation

=⇒sizing w.r.t. average values Robust w.r.t. timing disturbances

=⇒no need for perfect determinism Easily feasible, with off-the-shelf hard/soft

BE FREE !