基于领航者人工势场法的队形变化避障控制matlab代码仿真，路径规划，改进人工势场法，拓扑结构，集群，变换队形，基于领航者与人工势场法相结合的编队控制算法，可随意变换队形增加机器人个数。-优快云博客

本文链接：https://blog.youkuaiyun.com/2501_90314346/article/details/146503363

基于领航者人工势场法的队形变化避障控制matlab代码仿真，路径规划，改进人工势场法，拓扑结构，集群，变换队形，基于领航者与人工势场法相结合的编队控制算法，可随意变换队形增加机器人个数。
在这里插入图片描述

文章目录

以下是一个基于领航者（Leader）与改进人工势场法相结合的编队控制 MATLAB 代码仿真示例。该代码实现了路径规划、避障控制、队形变换、拓扑结构和集群控制等功能，并支持动态增加机器人数量。

代码：基于领航者与人工势场法的编队控制

1. 主函数

% 基于领航者与人工势场法的编队控制
function LeaderFollowerFormationControl()
    % 参数初始化
    numUAVs = 6; % 初始无人机数量
    formationShape = [0, 0, 0; 5, 0, 0; -5, 0, 0; 0, 5, 0; 0, -5, 0; 5, 5, 0]; % 编队形状
    initialPositions = rand(numUAVs, 3) * 50; % 随机初始位置
    velocities = zeros(numUAVs, 3); % 初始速度
    dt = 0.1; % 时间步长
    totalTime = 20; % 总时间
    timeSteps = round(totalTime / dt);
    
    % 领航者目标点
    leaderTargetPosition = [80, 80, 30];
    
    % 改进人工势场法参数
    attractiveGain = 1.0; % 吸引力增益
    repulsiveGain = 50.0; % 排斥力增益
    obstacleRadius = 5; % 障碍物影响半径
    
    % 障碍物位置
    obstacles = [40, 40, 15; 60, 60, 10];
    
    % 动态仿真
    for t = 1:timeSteps
        % 更新领航者位置
        leaderPosition = updateLeaderPosition(initialPositions(1, :), leaderTargetPosition, dt);
        
        % 计算跟随者的控制输入
        controlInputs = computeFollowerControl(leaderPosition, initialPositions, formationShape, obstacles, ...
                                               attractiveGain, repulsiveGain, obstacleRadius);
        
        % 更新速度和位置
        velocities = velocities + controlInputs * dt;
        initialPositions = initialPositions + velocities * dt;
        
        % 动态增加无人机（每5秒增加1个）
        if mod(t, 50) == 0 && numUAVs < 10
            numUAVs = numUAVs + 1;
            initialPositions = [initialPositions; rand(1, 3) * 50];
            velocities = [velocities; zeros(1, 3)];
            formationShape = [formationShape; rand(1, 3) * 5 - 2.5]; % 新增随机形状
        end
        
        % 可视化
        visualizeFormation(initialPositions, formationShape, leaderPosition, leaderTargetPosition, obstacles);
        pause(0.05);
    end
end

2. 领航者位置更新

% 更新领航者位置
function leaderPosition = updateLeaderPosition(currentPosition, targetPosition, dt)
    velocity = (targetPosition - currentPosition) / norm(targetPosition - currentPosition) * 2; % 简单速度模型
    leaderPosition = currentPosition + velocity * dt;
end

3. 跟随者控制算法

% 计算跟随者的控制输入
function controlInputs = computeFollowerControl(leaderPosition, positions, formationShape, obstacles, ...
                                                 attractiveGain, repulsiveGain, obstacleRadius)
    numUAVs = size(positions, 1);
    controlInputs = zeros(numUAVs, 3);
    
    for i = 1:numUAVs
        % 计算吸引力（跟随意图）
        desiredPosition = leaderPosition + formationShape(i, :);
        attractiveForce = computeAttractiveForce(positions(i, :), desiredPosition, attractiveGain);
        
        % 计算排斥力（避障）
        repulsiveForce = computeRepulsiveForce(positions(i, :), obstacles, obstacleRadius, repulsiveGain);
        
        % 综合控制输入
        controlInputs(i, :) = attractiveForce + repulsiveForce;
    end
end

4. 改进人工势场法

% 计算吸引力
function force = computeAttractiveForce(position, targetPosition, gain)
    distance = targetPosition - position;
    force = gain * distance;
end

% 计算排斥力
function force = computeRepulsiveForce(position, obstacles, obstacleRadius, gain)
    force = [0, 0, 0];
    for i = 1:size(obstacles, 1)
        distance = norm(position - obstacles(i, :));
        if distance < obstacleRadius
            direction = (position - obstacles(i, :)) / distance;
            force = force + gain * (1 / distance - 1 / obstacleRadius)^2 * direction;
        end
    end
end

5. 变换队形

% 变换队形函数
function newFormationShape = transformFormation(formationShape, scale, rotationAngle)
    % 缩放
    newFormationShape = formationShape * scale;
    
    % 旋转（绕Z轴）
    theta = deg2rad(rotationAngle);
    rotationMatrix = [cos(theta), -sin(theta), 0;
                      sin(theta), cos(theta), 0;
                      0, 0, 1];
    newFormationShape = newFormationShape * rotationMatrix;
end

6. 可视化

% 可视化编队、目标点和障碍物
function visualizeFormation(positions, formationShape, leaderPosition, targetPosition, obstacles)
    clf;
    hold on;
    grid on;
    
    % 绘制无人机位置
    plot3(positions(:, 1), positions(:, 2), positions(:, 3), 'bo', 'MarkerSize', 10, 'LineWidth', 2);
    
    % 绘制领航者位置
    plot3(leaderPosition(1), leaderPosition(2), leaderPosition(3), 'mo', 'MarkerSize', 10, 'LineWidth', 2);
    
    % 绘制目标点
    plot3(targetPosition(1), targetPosition(2), targetPosition(3), 'go', 'MarkerSize', 10, 'LineWidth', 2);
    
    % 绘制障碍物
    if ~isempty(obstacles)
        plot3(obstacles(:, 1), obstacles(:, 2), obstacles(:, 3), 'rx', 'MarkerSize', 10, 'LineWidth', 2);
    end
    
    % 绘制编队形状
    plot3(positions(:, 1) + formationShape(:, 1), ...
          positions(:, 2) + formationShape(:, 2), ...
          positions(:, 3) + formationShape(:, 3), 'k--', 'LineWidth', 1);
    
    xlabel('X');
    ylabel('Y');
    zlabel('Z');
    xlim([0, 100]);
    ylim([0, 100]);
    zlim([0, 50]);
    title('Leader-Follower Formation Control with Potential Field');
    legend('UAVs', 'Leader', 'Target', 'Obstacles', 'Formation Shape');
    drawnow;
end