基于领航跟随法的编队控制MATLAB实现

基于领航跟随法的编队控制MATLAB实现

一、核心代码

1. 参数初始化

%% 系统参数设置
num_robots = 5;       % 总机器人数量
dt = 0.1;             % 时间步长
max_time = 100;       % 仿真总时间
formation_types = {
   'circle', 'line', 'V-shape'};  % 可选队形类型
current_formation = 1; % 当前队形索引

%% 领航者参数
leader_pos = [0, 0];  % 初始位置
leader_vel = [0.2, 0.1]; % 初始速度
leader_traj = generate_spiral(100, 0.1); % 螺旋轨迹生成

%% 跟随者参数
followers = struct();
for i = 1:num_robots
    followers(i).pos = 2*rand(1,2); % 随机初始位置
    followers(i).vel = [0,0];
end

2. 队形控制算法

function target_pos = compute_formation(formation_type, leader_pos, num_robots)
    % 根据队形类型计算目标位置
    switch formation_type
        case 'circle'
            theta = linspace(0, 2*pi, num_robots+1);
            theta = theta(1:end-1);
            r = 2.0; % 圆形半径
            target_pos = r * [cos(theta); sin(theta)] + leader_pos;

        case 'line'
            x = linspace(-3, 3, num_robots);
            target_pos = [x; zeros(1,num_robots)] + leader_pos;

        case 'V-shape'
            angle = 60*pi/180; % V型夹角
            d = 3; % 基线距离
            for i = 1:num_robots
                theta = angle*(i-1)/(num_robots-1);
                target_pos(:,i) = d*[cos(theta); sin(theta)] + leader_pos;
            end
    end
end

3. 动态队形切换逻辑

%% 主循环
figure;
hold on;
plot(leader_pos(1), leader_pos(2), 'ro', 'MarkerSize', 10, 'LineWidth', 2);
for i = 1:num_robots
    plot(followers(i).pos(1), followers(i).pos(2), 'bo', 'MarkerSize', 6);
end
axis([-5 5 -5 5]);
grid on;

for t = 1:round(max_time/dt)
    % 更新领航者轨迹
    if t > 30 && t < 70
        current_formation = 2; % 中途切换为线性队形
    elseif t > 80
        current_formation = 3; % 后期切换为V型队形
    end

    % 计算目标队形位置
    target_positions = compute_formation(formation_types{
   current_formation}, leader_pos, num_robots);

    % 更新领航者位置
    leader_pos = leader_pos + leader_vel*dt;
    leader_traj = [leader_traj, leader_pos'];

    % 跟随者控制
    for i = 1:num_robots
        % 计算相对位置误差
        error = target_positions(:,i) - followers(i).pos;

        % PD控制律
        Kp = 1.2; Ki = 0.1; Kd = 0.5;
        followers(i).vel = followers(i).vel + Kp*error + Kd*(error - followers(i).vel)*dt;

        % 位置更新
        followers(i).pos = followers(i).pos + followers(i).vel*dt;
    end

    % 实时可视化
    cla;
    plot(leader_pos(1), leader_pos(2), 'ro', 'MarkerSize', 10);
    for i = 1:num_robots
        plot(followers(i).pos(1), followers(i).pos(2), 'bo');
    end
    plot(leader_traj(1,:), leader_traj(2,:), 'r--', 'LineWidth', 1.5);
    title(sprintf('Formation Control - Step %d', t));
    drawnow;
end

二、功能实现

1. 多模式队形变换

• 圆形队形：保持固定半径的环形编队
• 直线队形：沿领航者方向线性排列
• V型队形：对称展开的V型阵列

2. 自适应航迹规划

function traj = generate_spiral(turns, step)
    % 生成螺旋轨迹
    theta = linspace(0, 2*pi*turns, 1000);
    r = linspace(0, 5, 1000);
    traj = [r.*cos(theta); r.*sin(theta)];
end

3. 动态参数调整

% 根据队形类型调整控制参数
switch current_formation
    case 1 % 圆形
        Kp = 1.0; Kd = 0.3;
    case 2 % 直线
        Kp = 1.5; Kd = 0.5;
    case 3 % V型
        Kp = 1.2; Kd = 0.4;
end

三、可视化效果

1. 初始状态：随机分布的跟随者围绕红色领航者
2. 队形变换过程： 0-30步：默认圆形队形 30-70步：切换为直线队形 70-100步：转换为V型队形
3. 轨迹显示：红色虚线显示领航者路径

参考代码 领航跟随编队控制 www.youwenfan.com/contentale/63338.html

四、扩展功能实现

1. 障碍物避让（人工势场法）

function force = artificial_potential(pos, obstacles)
    % 人工势场力计算
    k_att = 1.0; k_rep = 50.0;
    force = [0,0];
    for i = 1:size(obstacles,1)
        d = norm(pos - obstacles(i,:));
        if d < 1.0
            force = force - k_rep*(1/d - 1/1.0)^2 * (pos - obstacles(i,:))/d^3;
        end
    end
end

2. 多智能体协同

% 添加通信延迟模拟
comm_delay = 2; % 2步延迟
follower_vel = followers(i).vel + comm_delay*prev_vel;

五、应用场景示例

1. 无人机表演：实现复杂队形变换
2. 仓储物流：多AGV协同作业
3. 灾害救援：搜索队形动态调整
4. 自动驾驶：车队协同跟驰