Distributed Model Predictive Control for Multi-Vehicle formation control

​

By,

Eleonora Chiarantano - 1708602

Leonardo Brizi - 1703210

Giulia Ciabatti - 1532244

Akshay Dhonthi Ramesh Babu - 1887502

​

Academic Year 2020 − 2021

​

​

​

​

​

​

Introduction

• Various approaches and strategies for multi-agent systems have been studied:
• virtual structure
• behavior based
• leader following
• We focus on two leader following method that use distributed MPC, as each robot solves its own optimization problem and each one has to know:
• first approach: leader position and relative desired positions
• second approach: leader predicted trajectory and relative desired positions
• Feedback linearization is used by both approach
• Collision avoidance is managed in different ways

​

2/43

Collision Avoidance as MPC constraint

External Collision Avoidance

(Extention)

Linear MPC via Feedback Linearization

1

2

Problem Formulation

​

​

“The problem considered in these papers is to drive a team

of differentially driven WMRs in a desired formation motion

while avoiding any obstacles located in the surrounding environment”

3/43

Approaches Summary

4/43

Linearized model

Linearized model

Linearized model

Differential Drive Robots

Nonlinear kinematics

of each robot

Feedback Linearization

MPC [1]

Formation Control

Differential Drive Robots

Nonlinear kinematics

of each robot

Feedback Linearization

MPC with collision avoidance as a constraint [2]

Differential Drive Robots

Nonlinear kinematics

of each robot

Feedback Linearization

MPC

Collision Avoidance using Artificial Potential Fields [3]

Approach 1

Approach 2

Approach 1 (Ext)

Formation Control

Formation Control

Approach 1 - Single Robot Trajectory Tracking

5/43

Reference Trajectory

Linear MPC

Dynamic Compensator

Non linear Kinematic Model

Feedforward

control

Approach 1 - Feedforward

6/43

Reference Trajectory

Linear MPC

Dynamic Compensator

Non linear Kinematic Model

Feedforward

control

• The sign depends on the robot direction, i.e + for the forward motion, - for the reverse motion
• comes from the derivation of :

​

​

k defines the driving direction (0 for forward)

Approach 1 - I/O Feedback Linearization

7/43

• Select an output vector and derive it until all inputs appear

​

​

​

​

​

​

​

from kinematic model of unicycle

The input appears but the decoupling matrix is not invertible

• Put v in the compensator state and continue differentiating

• Assuming that the matrix is nonsingular

Approach 1 - I/O Feedback Linearization

8/43

Reference Trajectory

Linear MPC

Non linear Kinematic Model

Feedforward

control

Approach 1 - Linear MPC

9/43

Reference Trajectory

Linear MPC

Linear Model

​

​

Feedforward

control

• The linearized system can be written in first order differential equations

Approach 1 - Linear MPC

10/43

• Start discretizing the system using Euler forward:

where , and is the integration step

• The optimization problem is:

where:

• is the state error to be minimized
• p and c denote prediction and control horizons

​

• The control goal is to find which drives the system toward the equilibrium in which

Approach 1 - Linear MPC

11/43

• The objective function can be written in quadratic form

​

​

where:

• can be rewritten in function of first error and the input sequence

​

where:

Approach 1 - Linear MPC

12/43

• Finally substituting into

we obtain the standard quadratic form as:

​

​

where:

​

​

​

​

• Now the objective function is function of the first state error and the control sequence

Approach 1 - Stability Analysis

13/43

Theorem: Suppose the following assumptions hold for a nominal controller L(z_e), a terminal state

weight Ω(z_e) and a terminal state domani Z

​

​

​

​

​

​

Then, assuming the feasibility at the initial state, the optimization problem will be

updated to guarantee the stability as follows:

New cost term used for the stability analysis

Approach 1 - Stability Analysis

14/43

Proof: using as a Lyapunov function, always positive since it is quadratic plus a term always positive,

the optimal cost at time k is obtained with the control sequence:

​

​

at time k+1:

we want to write it in function of

Approach 1 - Stability Analysis

15/43

using the assumption 3)

Since

Decaying sequence

At the equilibrium (z_e=0) and applying u=0 the value of the Lyapunov function is zero

Approach 1 - Formation Control

16/43

Leader Trajectory

Presented Controller

Desired Formation of follower 1

Desired Formation of follower n

Presented Controller

Presented Controller

Leader

Followers

• Distributed controller
• Each follower has to know:
• leader position
• relative position to leader
• Desired follower position is:

​

​

where:

Approach 1(Ext) - APF for Collision Avoidance

17/43

Reference Trajectory

Linear MPC

Dynamic Compensator

Non linear Kinematic Model

Feedforward

control

Artificial Potential Fields

Robot

Laser scan

“Decentralized Leader-Follower Formation Control with Obstacle Avoidance of Multiple Unicycle Mobile Robots”, Kamel, Zhang

Approach 1(Ext) - APF for Collision Avoidance

18/43

• Assumption 1. Each robot in the formation has an onboard sensing system.
• Assumption 2. The maximum range of sensors is greater than the distance d_max

​

• The linear and angular velocities of the robot will be changed according to this repulsive force

​

​

​

where:

• d is the measured distance between the closest obstacle
• s is a positive integer
• c is choosen as:
• d_min is the minimum acceptable robot-obstacle distance

• In case of many obstacles, each robot generates two fictitious forces F_R and

F_L that associate to the closest right and left obstacles to itself

​

where Z represent the mechanical impedance of the environment

Approach 2 - Feedback Linearization

19/43

• Substituting with the kinematics of the unicycle, we finally point out the control inputs

• As output is chosen the point in the i-th unicycle frame. Expressing it in function of q:

• Now deriving, we obtain:

• The final feedback linearized system is:

where:

G is always invertible :D

“Distributed Model Predictive Control for Multi-Vehicle Formation with Collision Avoidance Constraints”, Fukushima et. al.

Approach 2 - Formation Control

20/43

• The reference position of the vehicle i is given as a constant vector (r_i , l_i ) in local frame of the leader. Expressing it wrt global frame:

​

​

​

​

((・)_r is referred to leader)

​

• Differentiating it wrt time we have:

​

where:

Approach 2 - Formation Control

21/43

• Let’s define the tracking error as:

The derivative can be written as:

• By applying the control input:

• We have the tracking error dynamic as follows:

​

where λ is a design parameter and the new

control input

• We assume that the vehicles can communicate necessary information about the future predicted trajectory needed for the collision avoidance and the desired trajectory.

Approach 2 -Distributed MPC Algorithm

22/43

• The follower i solves the optimal control problem at every update interval δ
• The predicted value of at t = kδ is defined as

for follower i

if

if

Linear MPC

Update optimal trajectories

for

Receive from vehicle p = k (mod n)

else

Apply

control input

for

1

2

3

4

• where
• (・)_j is referred to other follower j with j≠i
• δ is the update interval
• τ the continuous time variable
• t the discrete time variable
• T is the prediction horizon, with T≥nδ (for collision avoidance)

Transmit

Leader Trajectory

Approach 2 - Optimal Control Problem

23/43

Linear MPC

symmetric positive definite matrix

Control Goal

Determine

Prediction model for

Collision avoidance constraint

Constraint the control input

Constraint to guarantee feasibility

where:

​

η is a design parameter η>0

Approach 2 - Problem Discretization

24/43

Linear MPC

Discretized as

Discretized as

1

2

Approach 2 - Problem Discretization

25/43

Thus, each row can be written in the form

Given T=3 and two followers:

where

• K_ijp are binary variables
• Ψ is a positive number much larger than the possible values of z_i

​

For the discretization of the uniform norm we �consider that

Collision avoidance with leader is not consider since in the application it is virtual!

where

• k_i are binary variables
• x and y are the components of the considered row

​

Approach 2 - Problem Discretization

26/43

Linear MPC

Discretized as

3

Approach 2 - Problem Discretization

27/43

Given T=3:

For the discretization of the uniform norm we consider that given a generic vector v

​

Thus, each row can be written in the form

Approach 2 - Problem Discretization

28/43

Discretized as

4

Linear MPC

Given T=3:

As for the previous constraint, this can be rewritten as

Approach 2 - Quadratic Form

29/43

Linear MPC

Approach 2 - Feasibility and Stability

30/43

Assumption 1: Each (i = 1, … ,n) in the last constraint satisfies the following conditions:

Lemma 1: If and for and satisfying Assumption 1, then it is� satisfied that:

​

​

​

for and any

Theorem 1: Assume the optimization is feasible at the initial update time t =(i − 1)δ of each follower i (i =1, ..., n), for which satisfies Assumption 1. Then, the optimization is feasible at each update time t ≥ nδ

Proof: Proved using mathematical induction

​

Theorem 2: If assumptions in theorem 1 are satisfied, all followers converge to the reference positions.

Implementation

31/43

• Implementation of all the approaches are on MATLAB.

​

• A CoppeliaSim Scene is developed as an extension of the first approach for simulation of formation control on the Robot, 3D-X Pioneer mobile base.

​

• MPC is computed using the MATLAB toolbox ‘quadprog’ for the first approach, and ‘Hybrid Toolbox’ developed by Ford Research Laboratory for the second.

Experiments

Approach 1 - Single Robot Trajectory

33/43

• For all the simulations of the first approach the sampling time and control horizon chose are: T_s=0.05 sec and p = 5.
• The robot initial position is (1, 0.7, 45°)
• The reference trajectory is:

Single Robot Trajectory Tracking

Actual and reference positions

Approach 1 - WMRs Performing a Triangular Formation

34/43

• In this simulation three WMRs performing a triangular formulation.
• The leader’s initial position is (1.1, -0.1,100°)
• The leader tracks a reference trajectory defined as:

​

​

• The initial position of the followers are (-0.5, 0.8, 100°) and (-0.4, 1.5, 100°)

Desired trajectory

Actual trajectory

Approach 1 - WMRs Performing a Triangular Formation

35/43

• The desired formation with respect to the leader is 0.5 m and 135° for the first follower, and 225° for the second one.

Approach 1 - WMRs Performing a Triangular Formation

36/43

• In Vrep a pid at low level is used in order to command the robot at kinematic level
• The velocities of the wheels are computed using linear and angular velocities

Approach 1(Ext) - WMRs Performing a Triangular Formation

37/43

• Controller parameters

​

​

​

​

• A team of three WMRs performing a triangular

formation should navigate in a partially structured environment containing ten obstacles, each one is 1m×1m.

x-y plots of triangular formation with obstacles

Approach 1(Ext) - WMRs Performing a Triangular Formation

38/43

Errors in desired formation of the followers

Linear and Angular velocities of the Robots

Approach 1(Ext) - Additional Experiments

39/43

Elliptical trajectory with same size obstacles

Elliptical trajectory with different size obstacles

Approach 2 - Formation Control

40/43

x-y plots of followers

x-y plots of followers

without input constraints

Approach 2 - Formation Control

41/43

Minimum Distance between followers

w_i of the follower 2

Conclusion

• We presented and discussed the theory and replicated the experiments proposed in the three papers.
1. Linear MPC via Feedback Linearization for Formation Control of Multiple Wheeled Mobile Robots
2. Decentralized Leader-Follower Formation Control with Obstacle Avoidance of Multiple Unicycle Mobile Robots
3. Distributed Model Predictive Control for Multi-Vehicle Formation with Collision Avoidance Constraints

​

• We managed to replicate all the experimental results in the papers and a couple of additional experiments.

​

• We extended the experimental part in (a) for static-obstacle avoidance and implemented it also in the case of elliptical trajectory with same-sized multi-obstacle and in the case of different-sized multiple obstacles (fine tuning parameters using grid-search).

​

• We also developed V-Rep scene for simulating paper (a) on three mobile robots for triangular formation control task.

Future Work:

• Apply the feedback linearization of the second approach to the first one
• Develop a simulation environment in Vrep also for the second approach

42/43

Thank you for your attention