Leader follower approach formation control problem
23 views (last 30 days)
Show older comments
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
0 Comments
Answers (1)
Sam Chak
on 12 Jul 2024
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)
%% 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')
5 Comments
Sam Chak
on 17 Jul 2024
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
on 17 Jul 2024
Hi @Enrica
You can also test and evaluate the following control problem for a simple all-famous Double Integrator system:
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1735412/image.png)
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?
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1735417/image.png)
where
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1735422/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1735427/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1735432/image.png)
The full lengthy control equation looks like this:
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1735437/image.png)
See Also
Categories
Find more on Gain Scheduling in Help Center and File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!