编队队形保持-Leader-Follower法-matlab仿真

Leader-follower 方法是传统编队队形控制中最常用的方法之一。leader 跟踪一个预先给定的轨迹，follower 和 leader 轨迹保 持一定构型，并速度达到一致。

本文使用的应用编队控制情景就如上图所示，是一个小编队。编队中有一个leader，和两个follower。编写的代码是对Desai 团队发表的一篇leader-follower法开创性文献中实验的复现仿真，该团队对这种方法进行了大量的实验和研究，在这篇文章之后还发表了不少对该方法优化和拓展性的文章。

具体的仿真代码如下，代码中附带有详细注释。代码中关键部分在于对跟随者跟随线速度和角速度部分的代码，这二者的迭代计算更新公式参考自前面提到的Desai团队论文。

clc, clear,close all
% 参数设置
tfinal = 30; % 总时间
dt = 0.1; % 时间步长
time = 0:dt:tfinal; % 时间向量
d = 1; % 轴距
target_distance = 2;
target_angle1 = 3*pi / 4;
target_angle2 = -3*pi / 4;

% 领导者初始状态
leader_pos = [0, 0]; 
leader_speed = 1; % 领导者的速度
leader_theta = 0; % 领导者的角度
leader_angle_speed = pi/36;

% 跟随者初始状态
follower1_pos = [0, 5]; % 跟随者1初始位置
follower2_pos = [0, -5]; % 跟随者2初始位置
follower1_theta = 0;
follower2_theta = 0;
% follower_speed = 0.9; % 跟随者的速度

% 存储轨迹
leader_trajectory = zeros(length(time), 2);
follower1_trajectory = zeros(length(time), 2);
follower2_trajectory = zeros(length(time), 2);

figure;
hold on;


for i = 1:length(time)
%     % 队形变换设置
%     if i == 150
%         target_angle1 = pi / 6;
%         target_angle2 = -pi / 6;
%     end

    % 更新领导者位置
    leader_pos = leader_pos + leader_speed * [cos(leader_theta), sin(leader_theta)] * dt;
    leader_theta = leader_theta + leader_angle_speed * dt;
    leader_theta = wrapToPi(leader_theta);

    % 中间参数
    follower1_top = follower1_pos + [cos(follower1_theta), sin(follower1_theta)] * d;
    follower2_top = follower2_pos + [cos(follower2_theta), sin(follower2_theta)] * d;
    real_angle1 = atan2(follower1_top(2) - leader_pos(2), follower1_top(1) - leader_pos(1)) - leader_theta; % LF_angle
    real_angle2 = atan2(follower2_top(2) - leader_pos(2), follower2_top(1) - leader_pos(1)) - leader_theta;
    real_angle1 = wrapToPi(real_angle1);
    real_angle2 = wrapToPi(real_angle2);
    real_distance1 = norm(leader_pos - follower1_top); %LF_distance
    real_distance2 = norm(leader_pos - follower2_top);
    k1 = (target_distance - real_distance1 + leader_speed * cos(real_angle1))/cos(leader_theta + real_angle1 - follower1_theta);
    k2 = (target_distance - real_distance2 + leader_speed * cos(real_angle2))/cos(leader_theta + real_angle2 - follower2_theta);
    
    % 跟随者的角速度与角加速度的求取
    follower1_angle_speed = max(min((cos(leader_theta + real_angle1 - follower1_theta) * (real_distance1 * (target_angle1 - real_angle1) - leader_speed * sin(real_angle1) + real_distance1 * leader_angle_speed + k1 * sin(leader_theta + real_angle1 - follower1_theta)) / d), pi/2), -pi/2);
    follower2_angle_speed = max(min((cos(leader_theta + real_angle2 - follower2_theta) * (real_distance2 * (target_angle2 - real_angle2) - leader_speed * sin(real_angle2) + real_distance2 * leader_angle_speed + k2 * sin(leader_theta + real_angle2 - follower2_theta)) / d), pi/2), -pi/2);
    follower1_speed = max(min(k1 - d * follower1_angle_speed * tan(leader_theta + real_angle1 - follower1_theta), 2), 0);
    follower2_speed = max(min(k2 - d * follower2_angle_speed * tan(leader_theta + real_angle2 - follower2_theta), 2), 0);
    disp(follower1_angle_speed)

    
    % 更新跟随者位置，朝向领导者移动
    follower1_pos = follower1_pos + follower1_speed * [cos(follower1_theta), sin(follower1_theta)] * dt;
    follower2_pos = follower2_pos + follower2_speed * [cos(follower2_theta), sin(follower2_theta)] * dt;
    follower1_theta = follower1_theta + follower1_angle_speed * dt;
    follower2_theta = follower2_theta + follower2_angle_speed * dt;
    follower1_theta = wrapToPi(follower1_theta);
    follower2_theta = wrapToPi(follower2_theta);

    % 存储轨迹
    leader_trajectory(i, :) = leader_pos;
    follower1_trajectory(i, :) = follower1_pos;
    follower2_trajectory(i, :) = follower2_pos;

    % 绘制轨迹
    clf; % 清除当前图形
    plot(leader_trajectory(1:i, 1), leader_trajectory(1:i, 2), 'r', 'LineWidth', 2); % 领导者轨迹
    hold on;
    plot(follower1_trajectory(1:i, 1), follower1_trajectory(1:i, 2), 'b', 'LineWidth', 2); % 跟随者1轨迹
    plot(follower2_trajectory(1:i, 1), follower2_trajectory(1:i, 2), 'g', 'LineWidth', 2); % 跟随者2轨迹
    
    % 绘制当前角色位置
    plot(leader_pos(1), leader_pos(2), 'ro', 'MarkerSize', 10); % 领导者
    plot(follower1_pos(1), follower1_pos(2), 'bo', 'MarkerSize', 8); % 跟随者1
    plot(follower2_pos(1), follower2_pos(2), 'go', 'MarkerSize', 8); % 跟随者2
    
    xlim([-5, 30]);
    ylim([-6, 30]);
    title('Leader-Follower Model');
    legend('Leader','Follower 1','Follower 2');
    pause(0.1); % 添加延时
    hold off;
end

代码的仿真效果如下，可以看到跟随者可以平稳地以期望的角度与距离跟随领导者，这可以一定程度上说明该方法的有效性。

屏幕录制 2024-10-26 105835