Dubins Path Algoritması ve Çıktılarının Proj Kütüphanesi Kullanarak Koordinat Sistemine Dönüştürülmesi

Giriş

Dubins Path algoritması, iki nokta arasında en kısa mesafeyi izleyen sürekli bir yol çizen bir aracın hareketini modellemek için kullanılır. Bu algoritmanın çıktısı, bir dizi yönlendirme noktasıdır. Bu noktaların her biri, aracın hareket ettiği yönü belirler.

Ancak, bu noktaların koordinatları genellikle Dünya üzerindeki bir konum sistemine göre belirtilir. UTM (Universal Transverse Mercator) koordinat sistemi, bu noktaların Dünya üzerindeki konumlarını daha doğru bir şekilde ifade etmek için kullanılır.

UTM (Universal Transverse Mercator) Dönüşümleri

UTM dönüşümleri daha iyi anlamak için örnek olarak bu iki fonksiyon incelenebilir:

EPSG'den UTM'ye Dönüşüm

Node EPSG_to_utm(Node node)
{
    Node tmpnode;
    PJ_CONTEXT *C;
    PJ *P;
    PJ *norm;
    PJ_COORD a, b;

    C = proj_context_create();
    P = proj_create_crs_to_crs(C,
                               "EPSG:4326",
                               "+proj=utm +zone=35 +datum=WGS84",
                               NULL);

    if (0 == P)
    {
        fprintf(stderr, "Failed to create transformation object.\n");
        return node;
    }
    norm = proj_normalize_for_visualization(C, P);
    if (0 == norm)
    {
        fprintf(stderr, "Failed to normalize transformation object.\n");
        return node;
    }
    proj_destroy(P);
    P = norm;
    a = proj_coord(node.x, node.y, 0, 0);
    b = proj_trans(P, PJ_FWD, a);

    tmpnode.x = (double)b.enu.e;
    tmpnode.y = (double)b.enu.n;
    tmpnode.yaw = node.yaw;

    return tmpnode;
}

UTM'den EPSG'ye Dönüşüm

double *utm_to_EPSG(double q[3])
{
    static double tmp[3]; // static olarak tanımlandı, böylece fonksiyondan çıkıldığında da saklanır
    PJ_CONTEXT *C;
    PJ *P;
    PJ *norm;
    PJ_COORD a, b;

    C = proj_context_create();
    P = proj_create_crs_to_crs(C,
                               "EPSG:4326",
                               "+proj=utm +zone=35 +datum=WGS84",
                               NULL);
    if (0 == P)
    {
        fprintf(stderr, "Failed to create transformation object.\n");
        return NULL; 
    }
    norm = proj_normalize_for_visualization(C, P);
    if (0 == norm)
    {
        fprintf(stderr, "Failed to normalize transformation object.\n");
        return NULL; 
    }
    proj_destroy(P);
    P = norm;
    a = proj_coord(q[0], q[1], 0, 0);
    b = proj_trans(P, PJ_INV, a); 

    tmp[0] = (double)b.lp.lam ;
    tmp[1] = (double)b.lp.phi ;
    tmp[2] = q[2];

    proj_destroy(P);
    proj_context_destroy(C); 
    return tmp;
}

Dubins Path Algoritmasını Neden Tercih Ettik?

Verilen iki noktanın eğrilik kısıtı ile birbirine bağlayan en kısa yolunun, sonlu bir eğri kümesi içerisinden bulunabileceği Dubins Path algoritması tarafından ispatlanmıştır. Bu eğri kümesi, Dubins eğrileri olarak adlandırılan altı farklı eğriden oluşur:

RSR (Sağ-Düz-Sağ)
RSL (Sağ-Düz-Sol)
RLR (Sağ-Sol-Sağ)
LSL (Sol-Düz-Sol)
LSR (Sol-Düz-Sağ)
LRL (Sol-Sağ-Sol)

Bu nedenle, Dubins Path algoritması ile iki nokta arasındaki en kısa yol hesaplanırken, öncelikle bu altı eğri tipi arasından en uygun olanı seçilerek hesaplama yapılır ve en kısa yol bu seçilen eğri tipi üzerinde hesaplanır. Bu yöntem sayesinde, eğrilik kısıtı olan problemler için doğru ve optimize edilmiş çözümler elde edilebilir.

Dubins Eğrileri Ana Fonksiyonu

int dubins_shortest_path(DubinsPath *path, Node node1, Node node2, double rho)
{
    int i, errcode;
    DubinsIntermediateResults in;
    double params[3];
    double cost;
    double best_cost = INFINITY;
    int best_word = -1;

    node1 = EPSG_to_utm(node1);
    node2 = EPSG_to_utm(node2);

    errcode = dubins_intermediate_results(&in, node1, node2, rho);
    if (errcode != 0)
    {
        return errcode;
    }

    path->nodei.x = node1.x;
    path->nodei.y = node1.y;
    path->nodei.yaw = node1.yaw;
    path->rho = rho;

    for (i = 0; i < 4; i++)
    {
        DubinsPathType pathType = (DubinsPathType)i;
        errcode = dubins_word(&in, pathType, params);
        if (errcode == 0)
        {
            cost = params[0] + params[1] + params[2];
            if (cost < best_cost)
            {
                best_word = i;
                best_cost = cost;
                path->param[0] = params[0];
                path->param[1] = params[1];
                path->param[2] = params[2];
                path->type = pathType;
            }
        }
    }
    // errcode -1 dönersse yani dubins_word çalışmazsa
    if (best_word == -1)
    {
        return -1;
    }
    return 0;
}

Yukarıdaki fonksiyon iki düğümün konumları ve kıvrılma yarıçapı (rho) verilir. Kod, iki düğümü UTM koordinat sistemi kullanarak dönüştürür ve sonra Dubins kıvrımlı yol algoritmasının ara sonuçlarını hesaplar. Ardından, dört farklı Dubins yolu tipi için parametreleri hesaplar ve en kısa yolu seçer. En iyi yolu seçmek için maliyet hesaplanır ve en iyi maliyeti olan yolu seçmek için karşılaştırılır. Son olarak, hesaplanan parametreler Dubins Path yapısına kaydedilir.

Dubins Eğri Tipleri Fonksiyonları

LSL (Sol-Düz-Sol) Eğrisi

int dubins_LSL(DubinsIntermediateResults *in, double out[3])
{
    double tmp0, tmp1, p_sq;

    tmp0 = in->d + in->sa - in->sb;
    p_sq = 2 + in->d_sq - (2 * in->c_ab) + (2 * in->d * (in->sa - in->sb));

    if (p_sq >= 0)
    {
        tmp1 = atan2((in->cb - in->ca), tmp0);
        out[0] = mod2pi(tmp1 - in->alpha);
        out[1] = sqrt(p_sq);
        out[2] = mod2pi(in->beta - tmp1);
        return 0;
    }
    return -1;
}

RSR (Sağ-Düz-Sağ) Eğrisi

int dubins_RSR(DubinsIntermediateResults *in, double out[3])
{
    double tmp0 = in->d - in->sa + in->sb;
    double p_sq = 2 + in->d_sq - (2 * in->c_ab) + (2 * in->d * (in->sb - in->sa));
    if (p_sq >= 0)
    {
        double tmp1 = atan2((in->ca - in->cb), tmp0);
        out[0] = mod2pi(in->alpha - tmp1);
        out[1] = sqrt(p_sq);
        out[2] = mod2pi(tmp1 - in->beta);
        return 0;
    }
    return -1;
}

LSR (Sol-Düz-Sağ) Eğrisi

int dubins_LSR(DubinsIntermediateResults *in, double out[3])
{
    double p_sq = -2 + (in->d_sq) + (2 * in->c_ab) + (2 * in->d * (in->sa + in->sb));
    if (p_sq >= 0)
    {
        double p = sqrt(p_sq);
        double tmp0 = atan2((-in->ca - in->cb), (in->d + in->sa + in->sb)) - atan2(-2.0, p);
        out[0] = mod2pi(tmp0 - in->alpha);
        out[1] = p;
        out[2] = mod2pi(tmp0 - mod2pi(in->beta));
        return 0;
    }
    return -1;
}

RSL (Sağ-Düz-Sol) Eğrisi

int dubins_RSL(DubinsIntermediateResults *in, double out[3])
{
    double p_sq = -2 + in->d_sq + (2 * in->c_ab) - (2 * in->d * (in->sa + in->sb));
    if (p_sq >= 0)
    {
        double p = sqrt(p_sq);
        double tmp0 = atan2((in->ca + in->cb), (in->d - in->sa - in->sb)) - atan2(2.0, p);
        out[0] = mod2pi(in->alpha - tmp0);
        out[1] = p;
        out[2] = mod2pi(in->beta - tmp0);
        return 0;
    }
    return -1;
}

Yukarıdaki fonksiyonlar olası rotaların (RSR, RSL, LSL, LSR) maliyet hesaplarını yapar.

Program Çıktıları - Test Kodu

Algoritmanızı test etmek için aşağıdaki MATLAB kodunu kullanarak test edebilirsiniz:

clc; clear, close;
% fileID = fopen('Route.txt','r');
%
% formatSpec = '%s';
% sizeA = [2 Inf];
%
% A = fscanf(fileID,formatSpec,sizeA)
% fclose(fileID);
%
% A = A'

for c=1:300
    cx=string(c);
    deneme=["deneme" cx ".txt"];
    cx=join(deneme,"");
    B = convertStringsToChars(cx);
    
    data = importdata(B) ;
    x = data(:,1) ; y = data(:,2) ; head = data(:,3) ; step = data(:,3) ;
    headderece = rad2deg(head);
    plot(x,y,x(1),y(1),'o',x(size(x)),y(size(y)),'o') ;    % with the present data
    
    L=2;
    x2=x+(L*cos(head));
    y2=y+(L*sin(head));
    for i=1:size(y2,1)
        % plot(x(i),y(i),x2(i),y2(i));
        line([x(i),x2(i)],[y(i),y2(i)]);
    end
%     plot(x(1),y(1),'o')
    axis([0 45 0 25])
    [~,~,button]=ginput(1);
end

Sonuç

Bu yazıyı okuduğunuz için teşekkür ederim. Umarım yazdıklarım size faydalı olmuştur.