实际问题

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