基于国际海上避碰规则（COLREGs）的两船自动避让实现

一、关键算法实现

1. 碰撞危险评估（CPA/TCPA计算）

function [DCPA, TCPA] = collision_risk(ship1, ship2)
    % 输入参数：
    % ship1: [x1,y1,u1,v1]船1状态（位置x,y，速度u,v）
    % ship2: [x2,y2,u2,v2]船2状态
    
    rel_pos = ship2(1:2) - ship1(1:2);
    rel_vel = ship2(3:4) - ship1(3:4);
    
    d = norm(rel_pos);
    if d == 0
        DCPA = 0;
        TCPA = Inf;
        return;
    end
    
    theta = atan2(rel_pos(2), rel_pos(1));
    gamma = atan2(rel_vel(2), rel_vel(1));
    
    DCPA = d * sin(gamma - theta);
    TCPA = -(d * cos(gamma - theta)) / norm(rel_vel);
end

2. COLREGs规则判断

function action = colreg_decision(ship1, ship2)
    % 输入：两船相对位置和航向
    % 输出：避让动作（左转/右转/直行）
    
    % 计算相对方位角
    dx = ship2(1) - ship1(1);
    dy = ship2(2) - ship1(2);
    bearing = atan2(dy, dx) * 180/pi;
    
    % 规则15：交叉相遇局面
    if (0 < bearing && bearing < 180) || (180 < bearing && bearing < 360)
        % 判断让路船
        if ship1(3) > ship2(3) % 船1速度更快
            action = '右转';
        else
            action = '左转';
        end
    else
        % 规则14：对遇局面
        action = '右转';
    end
end

3. 路径规划（人工势场法）

function [path, cmd] = potential_field(ship, obstacles, goal)
    % 参数：
    % ship: 当前船状态 [x,y,u,v](@ref)
    % obstacles: 障碍物列表 [x1,y1; x2,y2;...]
    % goal: 目标点 [xg,yg](@ref)
    
    k_att = 1.0; % 吸引力系数
    k_rep = 100; % 斥力系数
    d0 = 20;     % 斥力作用范围
    
    % 计算目标吸引力
    delta_goal = goal - ship(1:2);
    att_force = k_att * delta_goal / norm(delta_goal);
    
    % 计算障碍斥力
    rep_force = ;
    for i = 1:size(obstacles,1)
        delta_obs = obstacles(i,:) - ship(1:2);
        d = norm(delta_obs);
        if d < d0
            rep_force = rep_force + k_rep * (1/d - 1/d0) * (delta_obs/d^3);
        end
    end
    
    % 合成总力
    total_force = att_force + rep_force;
    
    % 生成控制指令
    cmd.thrust = norm(total_force);
    cmd.heading = atan2(total_force(2), total_force(1)) * 180/pi;
end

---

二、MATLAB仿真代码

%% 场景初始化
ship1 = ; % [x,y,u,v](@ref)
ship2 = ; % 相对船1的方位角30度
goal = ;

%% 主循环（仿真时间100秒）
dt = 1;
for t = 1:100
    % 状态更新
    ship1 = update_ship(ship1, dt);
    ship2 = update_ship(ship2, dt);
    
    % 碰撞检测
    [DCPA, TCPA] = collision_risk(ship1, ship2);
    if DCPA < 2 && TCPA > 0
        % 规则判断
        action = colreg_decision(ship1, ship2);
        
        % 路径规划
        obstacles = [ship2(1:2)];
        [path, cmd] = potential_field(ship1, obstacles, goal);
        
        % 执行控制
        ship1 = apply_control(ship1, cmd);
    end
    
    % 可视化
    plot(ship1(1), ship1(2),'ro', ship2(1), ship2(2),'bx');
    drawnow;
end

%% 辅助函数
function ship = update_ship(ship, dt)
    ship(1) = ship(1) + ship(3)*dt;
    ship(2) = ship(2) + ship(4)*dt;
    ship(3) = ship(3); % 假设匀速
    ship(4) = ship(4);
end

function ship = apply_control(ship, cmd)
    ship(3) = cmd.thrust * cosd(cmd.heading);
    ship(4) = cmd.thrust * sind(cmd.heading);
end

---

三、优化方向

1. 动态参数调整

   % 根据海况调整势场参数
   function [k_att, k_rep] = adaptive_params(wave_height)
       if wave_height > 3
           k_att = 0.8;
           k_rep = 120;
       else
           k_att = 1.0;
           k_rep = 100;
       end
   end
   

2. 多传感器融合

   % 融合雷达与AIS数据
   function fused_data = sensor_fusion(radar_data, ais_data)
       % 卡尔曼滤波融合
       fused_data = kalman_filter(radar_data, ais_data);
   end
   

3. 人机协同控制

   % 驾驶员风格学习
   function style = learn_driving_style(historical_data)
       % 使用LSTM网络学习操船模式
       net = train_lstm(historical_data);
       style = predict(net, current_state);
   end
   

参考代码 基于国际海上避碰规则实现两船自动避让 www.youwenfan.com/contentcnm/64928.html

四、注意事项

1. 规则合规性验证 需通过IMO MSC.1/Circ.1610标准测试 每季度更新规则库（如COLREGs 2025修订版）

2. 故障安全机制

   % 冗余传感器校验
   function valid = check_sensors(radar, ais)
       if norm(radar.pos - ais.pos) > 5
           valid = false;
       else
           valid = true;
       end
   end