Autonet2030: towards distributed

control of dynamic vehicle convoys

Iñaki Navarro

School of Architecture, Civil and Environmental Engineering

Distributed Intelligent Systems and Algorithms Laboratory

École Polytechnique Fédérale de Lausanne

CH-1015 Lausanne, Switzerland

http://disal.epfl.ch/

inaki.navarro@epfl.ch

Navigare 2015: Cooperative & Swarm Navigation, Thun, May 5, 2015

The DISAL Team

• professor Alcherio Martinoli

• 1 part-time system administrator

• 1 part-time secretary

• 3 R&D engineers

• 4 post-doctoral fellows

• 12 PhD candidates(6 co-supervised)

2

Distributed Mobile

Robotic Systems

Sensor and Actuator

NetworksDistributed Intelligent

Transportation Systems

3 Domains, 1 FrameworkDistributed Intelligent Systems

1. Focus: models, control,

algorithms

2. HW substrate: mechatronic

3. Publication venues:

robotics + more domain-

specific communities

3

3.5 m

C-Zero

DISAL Mobile Platforms

size

40 cm

14 cm

2 cm

Alice II

Khepera III & IV

e-puck

7 cm

• Different mobility

• Different environments

• Different standards

• Resources vs. size

• Modular vs. custom designRanger

Hummingbird

60 cm

MBot

Serafina

4

AutoNet2030

FP7 STREP project AutoNet2030 aims to research and validate

procedures and algorithms for interaction control among co-operative

vehicles, including both automated and manually driven vehicles.

5

AutoNet2030

• Standardized use of 5.9 GHz V2V communications in

service of automated driving

• A path for cost-optimized automated driving technology,

making it more widely deployable

• Demonstration of inherently safe cooperative maneuvering

control algorithms

6

(Robot) Convoy

7

Consensus

• In a network of connected agents (connected graph), the

goal is to make all agents agree on their state.

• Each node is given a state xi, the goal is to make x1 = x2

= … = xN as time tends to infinity.

• One possible solution is averaging:

8 [M.Mesbahi and M. Egerstedt, 2010]

Incidence Matrix

Graph Theory

Laplacian Matrix

Graph

Vertex Set:

Edge Set:

9 [M.Mesbahi and M. Egerstedt, 2010]

Consensus

• In a network of connected agents (connected graph), the

goal is to make all agents agree on their state.

• Each node is given a state xi, the goal is to make x1 = x2

= … = xN as time tends to infinity.

• One possible solution is averaging:

• Equivalent to Laplacian feedback control:

10 [M.Mesbahi and M. Egerstedt, 2010]

Consensus

• Control of holonomic robots in 2D space.

11 [M.Mesbahi and M. Egerstedt, 2010]

Consensus• The biased Laplacian feedback control to

achieve formations:

12 [M.Mesbahi and M. Egerstedt, 2010]

Car Model

• Let’s go back to our convoy problem.

• Simple car model:

13

From Coordinates to Control Variables

• Let’s assume we now have to control a single vehicle

with the holonomic control variables:

‣ We need to transform to .

• We create from this displacement vector a goal point and

goal line.

Assumption:

• Direction is known

• Speed is known

[Gowal et al., IROS 2010]14

Control Law

‣ Lateral controller:

‣ Longitudinal controller:

Reaches the goal line

Reaches the goal point

[Linderoth et al., 2008]

[Gowal et al., IROS 2010]15

Vehicular Formation

• If the biased Laplacian feedback control is used

we can create formations,

• Specifying only lateral and longitudinal

distances between vehicles.

[Gowal et al., IROS 2010]16

Handling Collisions

• Remember that

• Positive weights will attract vehicles together.

• Negative weights will create a repulsion mechanism.

+-

• If the body of a car enters the collision region of another

car, the edge linking these cars become repulsive

[Gowal et al., IROS 2010]17

Convoy no roads

[Gowal et al., IROS 2010]18

Webots & RO2IVSim project

• CTI project, collaboration with Cyberbotics Sarl and PSA

• Transporting high-fidelity simulation tools from robotics to

automotive engineering

• Multi-vehicle, submicroscopic simulator Webots

• Dedicated networking plug-in: OmNet++, NS-3 (within Autonet)

19 [Gowal et al., ITSC 2010]

Distributed convoys

• Cars only accounts for their close neighbours

• No global knowledge of the shape, of the number of cars, ...

• Flexibility, no long distance communication

• Globally the graph is connected (connection of local graphs)

[Marjovi et al., IV2015]20

Distributed convoys

• Cars know positions of neighboring cars

• Maintain local matrices: Laplacian & bias matrices

[Marjovi et al., IV2015]21

Going to the roads

• Car must know the orientation of the road and

their relative position to the expected trajectory.

– using computer vision

– static map of the road & GPS

• Convoy in a rotated reference system

– X axis parallel tangent to the road

[Marjovi et al., IV2015]22

Going to the roads

23

Going to the roads

• Car must know the orientation of the road and

their relative position to the expected trajectory.

– using computer vision

– Static map of the road & GPS

• Convoy in a rotated reference system

– X axis parallel tangent to the road

• Compose lateral controller with lane keeping

[Marjovi et al., IV2015]24

Going to the roads

25

Dynamical operations

• Distributed and local nature of the controller allows

for local modifications of the graph

• They propagate on a hop to hop communication basis

• Allow of join, leave, and lane change operations

• Implemented so far for 2 lane convoy

[Marjovi et al., IV2015]26

[Marjovi et al., IV2015]

Dynamical operations: leave

27

[Marjovi et al., IV2015]

Dynamical operations: join

28

Work in progress (& future)

• Applying Laplacian control in curvilinear coordinates of

the road.

• Dynamical operations with any number of lanes

• Dynamical operations robust to delays and package loss.

• Heterogeneous vehicles and general graph topology (non

rectangular restricted).

• Prove of stability.

29

Demonstration & validation

in real world in Autonet2030

• 2 automated Scania trucks + Fiat manual

using HMI

30

Other graph-based formations at DISAL

Distributed Odor Localization [Soares et al., ICRA 2015] MOnarCH FP7 [Wasik et al., IROS 2015, under review]

3D formation with quadrotors [Dias et al., IROS 2015, under review]31

References

• M.Mesbahi and M. Egerstedt, Graph theoretic methods in multiagent networks. Princeton

University Press, 2010.

• S. Gowal, R. Falconi, A. Martinoli , Local Graph-based Distributed Control for Safe Highway

Platooning, IROS 2010.

• M. Linderoth, K. Soltesz, R.M. Murray, Nonlinear lateral control strategy for nonholonomic

vehicles, American Control Conference, 3219-3224, 2008.

• S. Gowal, Y. Zhang, and A. Martinoli, A realistic simulator for the design and evaluation of

intelligent vehicles, in IEEE International Conference on Intelligent Transportation Systems,

2010, pp. 1039–1044.

• A. Marjovi, M. Vasic, J. Lemaitre, A. Martinoli, Distributed Graph-based Convoy Control for

Networked Intelligent Vehicles, IEEE Intelligent vehicle symposium, 2015 [to appear].

• J. M. Soares, A. P. Aguiar, A. M. Pascoal and A. Martinoli, A Distributed Formation-based

Odor Source Localization Algorithm – Design, Implementation, and Wind Tunnel Evaluation,

ICRA 2015 [to appear].

• A. Wasik, J. N. Pereira, R. Ventura, P. Lima and A. Martinoli, Graph Based Distributed

Control for Cooperative Patrolling in Human Populated Complex Environments, under review

IROS2015.

• Duarte Dias, R. Ventura, P. Lima, A. Martinoli, On-Board Vision-Based Relative Positioning

Sensing System and Formation Control for a Fleet of Quadrotors , under review IROS2015.

32

Thank you!

33

Thank you!

34