matlab 土法求航海DCPA和TCPA，并根据DCPA正负判断目标船过本船船首or船尾

 呃....不会整跳转目录，手动添加目录，哈哈哈哈。

        一、前言

        二、算法思路

        三、自定义CPA计算函数参数说明

        四、DCPA和TCPA的计算函数

        五、函数调用及结果

一、前言

        由于研究所需，以简便的方法求取船舶最近会遇距离（DCPA）和最近会遇时间（TCPA）。

        但各文献中公式法求取DCPA和TCPA时存在bug（即相对运动方向θ=0°或θ=180°时，tan(θ)不存在，导致DCPA和TCPA无法求取）。如胥文的《基于模糊理论的船舶复合碰撞危险度计算》文献中所示

        虽然这种状况很少出现（只存在于两船相对运动矢量方向为0°或180°才会出现），但为了弥补这一缺陷，笔者根据DCPA和TCPA的计算原理，列出会遇态势所有情况，解决了这一bug。       

        除此之外，笔者外加判断目标船过本船船首or船尾功能。根据两船相对运动线及本船航向，若DCPA为正，则目标船过本船船首；反之则目标船过本船船尾。

        航海中航向圆周法与matlab默认的极坐标圆周法略有差异，在坐标统一的基础上根据DCPA和TCPA的计算公式进行计算，自行整理了以下代码形成自定义函数分享给大家，以便调用。（计算公式出自《基于模糊理论的船舶复合碰撞危险度计算》——胥文）

二、函数思路

        1、两船速度在XY轴分解，进行矢量相加，求得在XY轴的速度差后根据三角函数求相对运动线斜率及相对运动速度，外加考虑在X轴上速度差为0的情况，斜率K数值为“空”；

        2、以本船为原点，点斜式求相对运动线；

        3、用（本船）原点到直线距离公式求DCPA，同时求垂足P点（最近会遇点）的坐标。当DCPA为原点到与Y轴平行的相对运动线距离时，设斜率为空；

        4、求P点与（以本船为原点）目标船坐标的距离，再除以相对运动速度得TCPA；

        5、判断本船航向与相对运动线的相交情况，若交点在航向矢量前，则为目标船从本船船首过，反之从本船船尾过；

三、自定义CPA计算函数参数说明

        输入lon_o、lat_o为本船（object）经纬度，单位为°

        输入speed_o为本船（object）的航速，单位为kn

        输入course_o为本船（object）的真航向，单位为°

        输入lon_t、lat_t为目标船（target）经纬度，单位为°

        输入speed_t为目标船（target）的航速，单位为kn

        输入course_t为目标船（target）的真航向，单位为°

        输出DCPA单位为海里（nm）

        输出TCPA单位为分钟（min）

        （默认1nm=1.852km）

四、DCPA和TCPA的计算函数

function [DCPA,TCPA,k]=CPA(lon_o,lat_o,speed_o,course_o,lon_t,lat_t,speed_t,course_t)

%%   调整坐标，将航海真航向圆周法的角度转化为matlab默认的零刻度朝右的逆时针圆周法
    if course_o >=0 & course_o <=90
        course_o_=90-course_o;
    else
        course_o_=450-course_o;
    end

    if course_t >=0 & course_t <=90
        course_t_=90-course_t;
    else
        course_t_=450-course_t;
    end

%% 两船速度分解，以本船为原点，求目标船的相对运动方向（0刻度朝上的圆周法）  
    speed_Xo=speed_o.*cosd(course_o_);  %速度分解成xy轴
    speed_Yo=speed_o.*sind(course_o_);

    speed_Xt=speed_t.*cosd(course_t_);
    speed_Yt=speed_t.*sind(course_t_);

    diffspeed_x=speed_Xt-speed_Xo;   %x和y方向的速度差（kn）
    diffspeed_y=speed_Yt-speed_Yo;

    speed_r=sqrt((diffspeed_x)^2+(diffspeed_y)^2);  %求相对速度（kn）

    if diffspeed_x==0 & diffspeed_y>0     %在y正半轴
        k=[];
    elseif diffspeed_x==0 & diffspeed_y<0     %在y负半轴
        k=[];
    else
        k=diffspeed_y/diffspeed_x;
    end

    if isempty(k)==1
        if abs(lon_t-lon_o)==0
            DCPA=0;
            TCPA=60*abs(lat_t-lat_o)/speed_r*60;
            b=[];
        else 
            DCPA=abs(lon_t-lon_o)*60;
            TCPA=60*abs(lat_t-lat_o)/speed_r*60;
            b=[];
        end
    else 
        b=(lat_t-lat_o)-k*(lon_t-lon_o);
        DCPA=abs(b)/sqrt(1+k*k)*60;
        Y_p=b/(k*k+1);
        X_p=-b*k/(k*k+1);
        S=sqrt((Y_p+lat_o-lat_t)^2+(X_p+lon_o-lon_t)^2)*60;
        TCPA=60*S/speed_r;
    end
        
        
%% 判断DCPA是正or负（会遇船舶从船头过or船尾过） 
      % 会遇船在第1象限
    if isempty(b)==1
        DCPA=DCPA;
        TCPA=TCPA;
    else
        if (lon_t-lon_o)>0 & (lat_t-lat_o)>0  
            theta=atand(diffspeed_y/diffspeed_x);
            if b<=0
                if course_o_>theta & course_o_<(theta+180)
                    DCPA=-DCPA;
                end
            end
            if b>0
                if course_o_<theta | course_o_>(theta+180)
                    DCPA=-DCPA;
                end
            end
        end

            % 会遇船在第2象限
        if  (lon_t-lon_o)<0 & (lat_t-lat_o)>0  
            theta=180+atand(diffspeed_y/diffspeed_x);
            if b<=0
                if course_o_<theta | course_o_>theta+180
                    DCPA=-DCPA;
                end
            end
            if b>0
                if course_o_>theta & course_o_<theta+180
                    DCPA=-DCPA;
                end
            end
        end

             % 会遇船在第3象限
         if  (lon_t-lon_o)<0 & (lat_t-lat_o)<0
             theta=atand(diffspeed_y/diffspeed_x);
            if b<=0
                if course_o_>theta & course_o_<(theta+180)
                    DCPA=-DCPA;
                end
            end
            if b>0
                if course_o_<theta | course_o_>(theta+180)
                    DCPA=-DCPA;
                end
            end
         end

              % 会遇船在第4象限
          if  (lon_t-lon_o)>0 & (lat_t-lat_o)<0
              theta=180+atand(diffspeed_y/diffspeed_x);
              if b<0
                  if course_o_<theta | course_o_>theta+180
                      DCPA=-DCPA;
                  end
              end
              if b>0
                  if course_o_>theta & course_o_<theta+180
                      DCPA=-DCPA;
                  end
              end
          end
    end    
end
    

五、函数调用及结果

新建脚本，复制代码，保存文件至matlab搜索路径下。

例一：过船首的简单例子

计算结果：

 

例二：过船尾的简单例子

 

 计算结果：

        分享代码如有错漏之处，请大家留言批评指正！！

        注：若有需要地理（经纬度）坐标系转化为笛卡尔（直角坐标）坐标系代码，可在评论区留言。

        分享的第一篇文章，如果本篇文章对您有用的话，欢迎点赞收藏噢，谢谢谢谢，哈哈哈哈哈哈！！

主页还有更加丰富的内容噢 O(∩_∩)O ：
Matlab 地理(经纬度)坐标 转 笛卡尔(直角)坐标
改进埃尔米特(Hermite)分段三次插值——（在pchip函数中自定义导数值）