Leader follower approach formation control problem

Hello everyone, I'm Enrica. For a university project I have a task consisting on modeling on simulink three robots of which one leader and two followers. In the first scenario the robot has to follow a linear trajectory while in the second one the trajectory to follow is circular. In both scenarios the robots must start from different positions. For what concern the initial position of the robots, through the delay present in the loops, I tried to set the initial conditions inside the delay block but there is something that doesn't work. The robots are described through a space-state block on both x and y axes and they are controlled by two different discrete PID, the internal one controls the velocity (that must be mantained constant) while the external one regulates the position. I tried to model the trajectories using the matlab function block and I tuned the PID with the "tune" button. Plotting the robots on the XY graph they don't follow each other and I'm not able to identify where is the problem, if it's related to the structure and connections of the simulink scheme or if it's related to the space-state block or PID settinggs.

For a better understanding, attached you can find simulink and matlab codes (matlab versione R2023A).

Thank you to those who can help me

Answer 1

[Sam Chak]

Hi @Enrica

Regarding the task of designing a Leader-Follower formation, the first step is to ensure that the leader robot tracks the reference trajectory properly. Once this is achieved, you can design a similar approach for Follower #1, with a slight modification to the reference trajectory. Ideally, the controller for the Follower should also include a "collision avoidance component".

Since this is a mini project, and the mathematical aspects may be considered somewhat advanced, I will refrain from delving into those details here.

However, I can provide a demonstration of the leader robot tracking a circular path, which may serve as a starting point for your implementation.

%% state-space
Ts  = 0.05;
M   = 1;
A   = [Ts/M 0 
       1 Ts/M];
B   = [Ts/M; 
       0];
C   = [0 1];
D   = 0*C*B;
sys = ss(A, B, C, D);

%% control gain
K  = lqr(sys, 1e2*eye(2), 1)

K = 1x2

24.4166 11.1834

<mw-icon class=""></mw-icon>

<mw-icon class=""></mw-icon>

%% closed-loop system
dummy   = ss(A - B*K, B, C, D);   % as a test dummy
sso     = dcgain(dummy);          % steady-state output (dummy)
cls = ss(A - B*K, B/sso, C, D);   % real closed-loop

%% Reference circular trajectory
t   = linspace(0, 20*pi, 3601);   % intentionally not closing the trajectory loop
r   = 1;
w   = 0.1;
xr  = r*cos(w*t);
dxr = gradient(xr)./gradient(t);
yr  = r*sin(w*t);
dyr = gradient(yr)./gradient(t);

%% simulate the closed-loop system to generate trajectories
inputx  = xr + dxr/((Ts/M)/sso);
inputy  = yr + dyr/((Ts/M)/sso);
xout    = lsim(cls, inputx, t);     % trajectory in the x-axis
yout    = lsim(cls, inputy, t);     % trajectory in the y-axis

%% plot results in 2D plane
plot(xr, yr, xout, yout), hold on
plot(0, 0, "^", 'linewidth', 2, 'markersize', 10), 
plot(xout(end), yout(end), "v", 'linewidth', 2, 'markersize', 10, 'color', "#77AC30"), hold off
grid on, axis square, axis([-2 2 -2 2])
xlabel('x-axis'), ylabel('y-axis')
legend('Reference circular path', 'Leader''s trajectory', 'Departure', 'Arrival')
title('Circular Trajectory Tracking for the Leader Robot')

[Enrica]

Hi Sam, thank you a lot for your answer. The answer is valid, but I attached the files because the project has already begun and I don't know how to achieve the task according to what it has been done until now. In other words I would need to understang how to adjust or how to go on with what has been already implemented.

Thank you for the availability

[Sam Chak]

Hi @Enrica

Regarding the implementation of the control scheme for the Leader-Follower system, I understand your desire to adjust the control gains and use the existing PID controllers in your Simulink model. However, with the double PID control architecture (involving 6 design parameters) per axis (for a 2nd-order system) for each system (Leader, Follower 1, and Follower 2), the task may prove to be quite challenging.

Is the concept of a double PID control strategy a novel approach in the context of the Leader-Follower control problem? Typically, such systems are addressed using more conventional control techniques, such as state-feedback control. The introduction of a double PID architecture may introduce additional complexity and design considerations that should be carefully evaluated.

[Enrica]

According with the professor I inserted two PIDs becasue one is for the external loop and controls the position according with the difference between the reference position and the measured position while the second PID control the velocity according with the difference between the reference velocity and the velocity measured

[Sam Chak]

Hi @Enrica

Regarding the implementation of your control scheme, I have not previously encountered a configuration involving a double PID control architecture like that. The control equation should adhere to the fundamental principle of "balancing" the difference between the reference and measured values for both position and velocity.

I would recommend that you carefully examine the full control equation, based on the exact signal flows depicted in the Simulink model, to ensure that it aligns with this fundamental principle. Once you have thoroughly reviewed the control equation and block configuration, present your findings to your professor. It is possible that there may have been a misunderstanding in the interpretation or implementation of the control scheme.

Additionally, it is advisable to maintain a continuous-time design framework, as your state-space representation is also in continuous-time. Ensuring consistency across the modeling and control approaches is crucial for the overall system's stability and performance.

[Sam Chak]

Hi @Enrica

You can also test and evaluate the following control problem for a simple all-famous Double Integrator system:

How exactly would you implement the double PID control architecture to stabilize both the position and velocity loops in the embeded manner as seen in your Simulink model?

where

The full lengthy control equation looks like this: