多无人船人工势场编队避开障碍matlab代码

以下是一个简单的无人船人工势场编队避开障碍的 Matlab 代码示例：

% 设置初始状态
x1 = [0; 0; pi/2]; % 领航者起始位置和朝向
x2 = [-1; 0; pi/2]; % 从属者1起始位置和朝向
x3 = [1; 0; pi/2]; % 从属者2起始位置和朝向

% 设置目标位置
goal = [5; 5];

% 设置障碍物位置
obstacle = [2; 3];

% 设置控制参数
Kv = 1; % 速度增益
Kh = 1; % 转向增益
d = 1; % 安全半径
rho0 = 10; % 引力范围
eta0 = 1; % 斥力幅度

% 开始仿真
for i = 1:1000
    % 计算领航者到目标的距离和朝向
    rho = norm(x1(1:2) - goal);
    alpha = atan2(goal(2)-x1(2), goal(1)-x1(1)) - x1(3);
    
    % 计算障碍物斥力
    eta = eta0 / (norm(x1(1:2) - obstacle) - d)^2;
    
    % 计算总控制力
    F = Kv*rho*[cos(alpha); sin(alpha)] - Kh*x1(3) + eta*[cos(alpha+pi); sin(alpha+pi)];
    
    % 更新领航者状态
    x1 = x1 + F;
    
    % 计算从属者1和从属者2到领航者的距离和朝向
    rho2 = norm(x2(1:2) - x1(1:2));
    alpha2 = atan2(x1(2)-x2(2), x1(1)-x2(1)) - x2(3);
    
    rho3 = norm(x3(1:2) - x1(1:2));
    alpha3 = atan2(x1(2)-x3(2), x1(1)-x3(1)) - x3(3);
    
    % 计算从属者1和从属者2的控制力
    F2 = Kv*rho2*[cos(alpha2); sin(alpha2)] - Kh*x2(3);
    F3 = Kv*rho3*[cos(alpha3); sin(alpha3)] - Kh*x3(3);
    
    % 更新从属者1和从属者2的状态
    x2 = x2 + F2;
    x3 = x3 + F3;
    
    % 绘制场景
    plot(x1(1), x1(2), 'ro'); % 领航者
    hold on;
    plot(x2(1), x2(2), 'bo'); % 从属者1
    plot(x3(1), x3(2), 'go'); % 从属者2
    plot(goal(1), goal(2), 'rx'); % 目标位置
    plot(obstacle(1), obstacle(2), 'kx'); % 障碍物位置
    axis([-10 10 -10 10]);
    hold off;
    
    % 判断是否到达目标位置
    if rho < 0.5
        break;
    end
    
    pause(0.1);
end

此代码演示了一个简单的无人船编队避开障碍的场景，其中三艘船分别作为领航者和两个从属者。领航者通过人工势场方法计算出到目标位置的控制力，从属者则通过跟随领航者控制。同时，领航者还会受到障碍物斥力的影响，避免碰撞障碍。你可以根据自己的需要进行修改和扩展。