目标跟踪的matlab实现

本篇文章介绍了领导者与跟随者的全局目标均可知的环境下（可以理解为现实环境下可以接收领导者与跟随者GPS坐标信号的情况），实现跟随者对领导者的跟踪。

文章设置了一个领导者，领导者在x轴方向保持匀速，在y轴方向做正弦运动。还设置有两个跟随者，跟随者与领导者之间的距离和二者之间的夹角均已设定。

文章采用跟随领导者的编队队形控制策略，matlab仿真效果见下：

% 参数设置
tfinal = 20; % 模拟结束时间
dt = 0.1;    % 时间步长
time = 0:dt:tfinal; % 时间向量

% 初始状态
leader_pos = [0, 0]; % 领导者初始位置
follower1_pos = [0, 5]; % 跟随者1初始位置
follower2_pos = [0, -5]; % 跟随者2初始位置
leader_speed = 0.5; % 领导者速度
follow_distance = 2; % 跟随距离
angle_offset = 3*pi / 4; % 期望夹角（45度）
current_angle1 = atan2(0, 1); % 假设初始朝向为正x轴
current_angle2 = atan2(0, 1); % 假设初始朝向为正x轴

% 存储位置数据
leader_trajectory = zeros(length(time), 2);
follower1_trajectory = zeros(length(time), 2);
follower2_trajectory = zeros(length(time), 2);

% 图形窗口设置
figure;
hold on;
xlabel('X Position');
ylabel('Y Position');
title('Leader-Follower Model with Velocity and Angular Velocity');
axis equal;
xlim([-1, 10]);
ylim([-6, 6]);

% 仿真循环
for i = 1:length(time)
    % 更新领导者的位置
    if i > 1
        leader_velocity = [1, sin(0.1 * i)]; % 领导者以一定角度前进
        leader_velocity = leader_velocity / norm(leader_velocity); % 单位化
    else
        leader_velocity = [1, 0]; % 初始前进方向
    end
    
    leader_pos(1) = leader_pos(1) + leader_velocity(1) * leader_speed * dt; % x坐标
    leader_pos(2) = leader_pos(2) + leader_velocity(2) * leader_speed * dt; % y坐标
    
    % 计算跟随者目标位置
    follower1_target = leader_pos + [cos(angle_offset), sin(angle_offset)] * follow_distance;
    follower2_target = leader_pos + [cos(-angle_offset), sin(-angle_offset)] * follow_distance;

    % 跟随者1运动逻辑
    distance1 = norm(follower1_target - follower1_pos);
    
    % 计算角速度
    angle_to_target1 = atan2(follower1_target(2) - follower1_pos(2), follower1_target(1) - follower1_pos(1));
    
    angular_velocity1 = angle_to_target1 - current_angle1; % 相对角度
    angular_velocity1 = wrapToPi(angular_velocity1); % 确保角度在[-pi, pi]区间

    % 线速度与角速度的关系
    follower1_speed = min(1.0, distance1 / follow_distance); % 控制最大速度
    follower1_pos(1) = follower1_pos(1) + cos(current_angle1 + angular_velocity1) * follower1_speed * dt;
    follower1_pos(2) = follower1_pos(2) + sin(current_angle1 + angular_velocity1) * follower1_speed * dt;

    % 更新当前角度
    current_angle1 = current_angle1 + angular_velocity1 * dt; % 更新角度

    % 跟随者2运动逻辑
    distance2 = norm(follower2_target - follower2_pos);
    
    % 计算角速度
    angle_to_target2 = atan2(follower2_target(2) - follower2_pos(2), follower2_target(1) - follower2_pos(1));
    
    angular_velocity2 = angle_to_target2 - current_angle2; % 相对角度
    angular_velocity2 = wrapToPi(angular_velocity2); % 确保角度在[-pi, pi]区间

    % 线速度与角速度的关系
    follower2_speed = min(1.0, distance2 / follow_distance); % 控制最大速度
    follower2_pos(1) = follower2_pos(1) + cos(current_angle2 + angular_velocity2) * follower2_speed * dt;
    follower2_pos(2) = follower2_pos(2) + sin(current_angle2 + angular_velocity2) * follower2_speed * dt;

    % 更新当前角度
    current_angle2 = current_angle2 + angular_velocity2 * dt; % 更新角度

    % 存储轨迹
    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', 1.5);
    hold on
    plot(follower1_trajectory(1:i, 1), follower1_trajectory(1:i, 2), 'b-', 'LineWidth', 1.5); 
    plot(follower2_trajectory(1:i, 1), follower2_trajectory(1:i, 2), 'g-', 'LineWidth', 1.5); 

    % 绘制当前位置
    plot(leader_pos(1), leader_pos(2), 'ro', 'MarkerSize', 10, 'DisplayName', 'Leader');
    plot(follower1_pos(1), follower1_pos(2), 'bo', 'MarkerSize', 8, 'DisplayName', 'Follower 1');
    plot(follower2_pos(1), follower2_pos(2), 'go', 'MarkerSize', 8, 'DisplayName', 'Follower 2');
    %plot([leader_pos(1), follower1_pos(1)], [leader_pos(2), follower1_pos(2)], 'b--'); % 跟随者1连线
    %plot([leader_pos(1), follower2_pos(1)], [leader_pos(2), follower2_pos(2)], 'g--'); % 跟随者2连线
    axis equal; % 保持比例一致
    grid on; % 添加网格以便于观察
    legend('Leader','Follower 1','Follower 2');
    pause(0.1);
    hold off
end

% 绘制轨迹
% figure;
% hold on;
% plot(leader_trajectory(:, 1), leader_trajectory(:, 2), 'r-', 'LineWidth', 2, 'DisplayName', 'Leader Trajectory');
% plot(follower1_trajectory(:, 1), follower1_trajectory(:, 2), 'b--', 'LineWidth', 2, 'DisplayName', 'Follower 1 Trajectory');
% plot(follower2_trajectory(:, 1), follower2_trajectory(:, 2), 'g--', 'LineWidth', 2, 'DisplayName', 'Follower 2 Trajectory');
% xlabel('X Position');
% ylabel('Y Position');
% title('Leader and Followers Trajectories with Velocity and Angular Velocity');
% legend show;
% grid on;
% axis equal;

编队控制基于全局坐标（GPS）

存在问题：

代码中计算跟随者目标位置的方式限制该目标跟踪方法中编队的队形，运动中编队只能保持领导者在右，两个跟随者在右的队形。编队的队形灵活性和可控制性都受很大局限。