### 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}

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}

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}

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}

_{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}

_{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ˆ_{kh}_{s} =λUˆ_{(k−1)h}_{s} + (1−λ) c_{kh}_{s}
h_{(k−1)h}_{s}
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_{1}

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*_{1}
*f*_{4}

*f*_{3} *f*2

*y*_{n}*z**n*

*x**b*
*y**b*
*z*_{b}*x*_{n}

*Q*_{1}
*Q*_{4}

*Q*_{3} *Q*2

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^{2}h^{2}l1,1+2b0nhl1,2−b0nh^{2}l1,1−4−2b0hl1,2

ln,2= −2(nhl_{1,1}+2l1,2−hl1,1)

b0n^{2}h^{2}l1,1+2b0nhl1,2−b0nh^{2}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*_{1}
*f*_{4}

*f*_{3} *f*2

*y*_{n}*z**n*

*x**b*
*y**b*
*z*_{b}*x*_{n}

*Q*_{1}
*Q*_{4}

*Q*_{3} *Q*2

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 !