基于人工势场法的船舶自动避碰系统MATLAB实现之旅

基于人工势场法的船舶自动避碰系统，MATLAB代码实现，生成避障转角信息和gif动图。

在航海领域，船舶自动避碰系统至关重要，它关乎航行安全。人工势场法作为一种经典算法，在避碰系统中有广泛应用。今天咱就来聊聊基于人工势场法的船舶自动避碰系统，并用MATLAB实现它，还会生成避障转角信息和超酷的gif动图。

人工势场法原理简述

人工势场法将船舶周围环境视为一种势场。目标点产生引力势场，吸引船舶靠近；障碍物产生斥力势场，阻止船舶靠近。船舶就像在这个势场中受合力作用的质点，合力指引其运动方向，进而实现避碰。

MATLAB代码实现

初始化参数

matlab 复制代码

% 定义目标点位置
target_x = 100; 
target_y = 100; 
% 定义障碍物位置
obstacle_x = [50]; 
obstacle_y = [50]; 
% 船舶初始位置
ship_x = 0; 
ship_y = 0; 
% 引力系数
k_att = 0.1; 
% 斥力系数
k_rep = 100; 
% 安全距离
d0 = 10; 
% 步长
step_size = 1;

这里定义了系统运行所需的关键参数，像目标点、障碍物位置，船舶初始位置这些是系统运行的起始状态设定。引力系数 katt**和斥力系数 k rep 决定了引力和斥力的相对强度，安全距离 d0 界定了船舶与障碍物的安全范围，步长 step_size 则表示每次船舶移动的距离。

计算引力和斥力

matlab 复制代码

while true
    % 计算引力
    dist_to_target = sqrt((target_x - ship_x)^2 + (target_y - ship_y)^2);
    force_att_x = k_att * (target_x - ship_x) / dist_to_target;
    force_att_y = k_att * (target_y - ship_y) / dist_to_target;

    % 计算斥力
    force_rep_x = 0;
    force_rep_y = 0;
    for i = 1:length(obstacle_x)
        dist_to_obstacle = sqrt((obstacle_x(i) - ship_x)^2 + (obstacle_y(i) - ship_y)^2);
        if dist_to_obstacle < d0
            force_rep_x = force_rep_x + k_rep * (1/dist_to_obstacle - 1/d0) * (1/dist_to_obstacle^2) * (ship_x - obstacle_x(i));
            force_rep_y = force_rep_y + k_rep * (1/dist_to_obstacle - 1/d0) * (1/dist_to_obstacle^2) * (ship_y - obstacle_y(i));
        end
    end

    % 计算合力
    total_force_x = force_att_x + force_rep_x;
    total_force_y = force_att_y + force_rep_y;

在这个循环里，每次迭代都重新计算引力和斥力。先根据目标点与船舶当前位置计算引力，距离越远引力越大且方向指向目标点。再对每个障碍物计算斥力，只有当船舶与障碍物距离小于安全距离 d0 时斥力才起作用，距离越近斥力越大且方向远离障碍物。最后将引力和斥力在x和y方向上分别相加得到合力。

更新船舶位置与计算避障转角

matlab 复制代码

    % 更新船舶位置
    ship_x = ship_x + step_size * total_force_x / sqrt(total_force_x^2 + total_force_y^2);
    ship_y = ship_y + step_size * total_force_y / sqrt(total_force_x^2 + total_force_y^2);

    % 计算避障转角
    current_direction = atan2(ship_y, ship_x);
    new_direction = atan2(total_force_y, total_force_x);
    turn_angle = new_direction - current_direction;

    % 记录船舶位置和转角信息
    positions_x(end+1) = ship_x;
    positions_y(end+1) = ship_y;
    turn_angles(end+1) = turn_angle;

    % 判断是否到达目标点
    if dist_to_target < step_size
        break;
    end
end

利用合力来更新船舶位置，船舶沿着合力方向移动一个步长。通过当前船舶方向和合力方向计算避障转角 turn_angle，并记录船舶位置和转角信息。当船舶与目标点距离小于步长时，认为船舶到达目标点，结束循环。

生成gif动图

matlab 复制代码

% 生成gif动图
fig = figure;
axis([0 120 0 120]);
hold on;
plot(target_x, target_y, 'go', 'MarkerSize', 10);
for i = 1:length(obstacle_x)
    plot(obstacle_x(i), obstacle_y(i), 'ro', 'MarkerSize', 10);
end
for i = 1:length(positions_x)
    plot(positions_x(i), positions_y(i), 'bo', 'MarkerSize', 5);
    if i > 1
        line([positions_x(i-1) positions_x(i)], [positions_y(i-1) positions_y(i)], 'Color', 'b');
    end
    drawnow;
    frame = getframe(fig);
    im = frame2im(frame);
    [imind,cm] = rgb2ind(im,256);
    if i == 1
        imwrite(imind,cm,'ship_avoidance.gif','gif', 'LoopCount',inf,'DelayTime',0.1);
    else
        imwrite(imind,cm,'ship_avoidance.gif','gif','WriteMode','append','DelayTime',0.1);
    end
end
hold off;

这部分代码用于生成船舶航行过程的gif动图。先创建图形窗口并设置坐标轴范围，标记出目标点和障碍物。然后随着船舶位置更新，依次绘制船舶位置并连线，每更新一次位置就抓取一帧画面，转换为索引图像后写入gif文件，最终生成动图直观展示船舶避碰过程。

基于人工势场法的船舶自动避碰系统，MATLAB代码实现，生成避障转角信息和gif动图。

通过以上MATLAB代码，我们成功实现了基于人工势场法的船舶自动避碰系统，不仅能得到避障转角信息，还能用gif动图生动呈现避碰过程。希望这篇分享能给对船舶避碰或算法实现感兴趣的小伙伴们一些启发！