路徑規劃算法學習筆記（三）——局部路徑規劃（Dubins Curve）

• 實際問題
• Dubins曲線基礎
• 問題關鍵
• CSC型
• CCC型
• Dubins曲線的一些讨論
• 參考文獻

實際問題

Dubins曲線基礎

問題關鍵

CSC型

CCC型

Dubins曲線的一些讨論

下面是寫的一段MATLAB代碼，隻是簡單實作，沒有考慮最優。

1、CSC

%Dubins Curve CSC型
%目标定義
    %定義起終點[x y dir]
    I=[1 1 7*pi/4];
    G=[6 8 3*pi/4];
    %定義或計算轉彎半徑
    ri=1;
    rg=2;
    %組合
    k=-1;    % 1:RSX,-1:LSX
    i=-1;   % 1:XSR,-1:XSL
    j=i*k;  % 1:RSR/LSL, -1:RSL/LSR
%計算首尾圓心坐标
    xi=I(1,1)-ri*k*cos(I(1,3));
    yi=I(1,2)+ri*k*sin(I(1,3));  
    xg=G(1,1)-rg*i*cos(G(1,3));
    yg=G(1,2)+rg*i*sin(G(1,3));
%計算法向量
    %首尾圓圓心之間的向量V1
    v1x=xg-xi;              %向量坐标x
    v1y=yg-yi;              %向量坐标y
    D=sqrt(v1x*v1x+v1y*v1y);%向量模長 
    v1x=v1x/D;              %機關化
    v1y=v1y/D;
    %計算法向量n
    c=(j*ri-rg)/D;            %定義中間量
    nx=v1x*c-i*v1y*sqrt(1-c*c);
    ny=v1y*c+i*v1x*sqrt(1-c*c);
%計算切點
    xit=xi+j*ri*nx;
    yit=yi+j*ri*ny;
    xgt=xg+rg*nx;
    ygt=yg+rg*ny;
%繪圖
    %初始方向
    xiDir=[I(1,1)-ri*cos(I(1,3)-pi/2),I(1,1)];
    yiDir=[I(1,2)+ri*sin(I(1,3)-pi/2),I(1,2)];
    xgDir=[G(1,1)-rg*cos(G(1,3)-pi/2),G(1,1)];
    ygDir=[G(1,2)+rg*sin(G(1,3)-pi/2),G(1,2)];
    %切線
    xxx=[xit xgt];
    yyy=[yit ygt];
    %首尾圓
    t=0:0.01:2*pi;
    %t=I(1,3)-pi/2:0.01:3*pi/2-atan(ny/nx);
    xxi=xi+ri*sin(t);
    yyi=yi+ri*cos(t);
    %t=-pi/2-atan(ny/nx):0.01:G(1,3)-pi/2;
    xxg=xg+rg*sin(t);
    yyg=yg+rg*cos(t);
    %法向量
    nxi=[xi xi+ri*nx];
    nyi=[yi yi+ri*ny];
    nxg=[xg xg+rg*nx];
    nyg=[yg yg+rg*ny];
    %繪圖
    plot(I(1,1),I(1,2),'go',G(1,1),G(1,2),'go',...      %初始位置
            xiDir,yiDir,'-g',xgDir,ygDir,'-g',...       %初始方向
            xi,yi,'k*',xg,yg,'k*',...                   %圓心
            xxi,yyi,'-r',xxg,yyg,'-r',...               %首尾圓
            nxi,nyi,'-b',nxg,nyg,'-b',...               %法向量
            xxx,yyy,'-ro')                              %切線
    axis([-3 15 -3 15])
           

2、CCC

%Dubins Curve CCC型

%目标定義
    %定義起終點[x y dir] 
    I=[1 1 7*pi/4];
    G=[4 5 3*pi/4];
    %定義或計算轉彎半徑
    ri=1;
    rg=1;
    rmid=4;
    %組合
    i=1;    % 1:RLR  -1:LRL
    
%三圓心相關量
    %計算首尾圓心坐标及其連線向量V12
    xi=I(1,1)-ri*i*cos(I(1,3));
    yi=I(1,2)+ri*i*sin(I(1,3));  
    xg=G(1,1)-rg*i*cos(G(1,3));
    yg=G(1,2)+rg*i*sin(G(1,3));
    
    V12=[xg-xi,yg-yi];
    angleV12=atan(V12(1,2)/V12(1,1));
    
    %計算中間圓坐标及三圓心連線向量V13、V32
    d12=sqrt((xg-xi)^2+(yg-yi)^2);
    rmid=max([rmid (d12-ri-rg)*0.5]);
    d13=ri+rmid;
    d32=rmid+rg;
    angleP213=acos((d12^2+d13^2-d32^2)/(2*d12*d13));
    
    xmid=xi+d13*cos(angleV12-angleP213);
    ymid=yi+d13*sin(angleV12-angleP213);
    
    V13=[xmid-xi,ymid-yi];
    V32=[xg-xmid,yg-ymid];
    
    V13=V13/d13;    %機關化
    V32=V32/d32;
    
%計算切點坐标
    xt1=xi+ri*V13(1,1);
    yt1=yi+ri*V13(1,2);
    xt2=xmid+rmid*V32(1,1);
    yt2=ymid+rmid*V32(1,2);
    
%繪圖
    %初始方向
    xiDir=[I(1,1)-ri*cos(I(1,3)-pi/2),I(1,1)];
    yiDir=[I(1,2)+ri*sin(I(1,3)-pi/2),I(1,2)];
    xgDir=[G(1,1)-rg*cos(G(1,3)-pi/2),G(1,1)];
    ygDir=[G(1,2)+rg*sin(G(1,3)-pi/2),G(1,2)];
    %首尾圓
    t=0:0.01:2*pi;
    xxi=xi+ri*sin(t);
    yyi=yi+ri*cos(t);
    xxg=xg+rg*sin(t);
    yyg=yg+rg*cos(t);
    xxmid=xmid+rmid*sin(t);
    yymid=ymid+rmid*cos(t);
    %繪圖
    plot(I(1,1),I(1,2),'go',G(1,1),G(1,2),'go',...          %初始位置
            xiDir,yiDir,'-g',xgDir,ygDir,'-g',...           %初始方向
            xi,yi,'k*',xg,yg,'k*',xmid,ymid,'k*',...        %圓心
            xt1,yt1,'bo',xt2,yt2,'bo',...                   %切點
            xxi,yyi,'-r',xxg,yyg,'-r',xxmid,yymid,'-r')     %三圓
    axis([-3 15 -3 15])
           

參考文獻

13、http://planning.cs.uiuc.edu/node821.html

14、http://www.banbeichadexiaojiubei.com/index.php/2020/03/15/自動駕駛運動規劃-dubins曲線

15、https://gieseanw.files.wordpress.com/2012/10/dubins.pdf