下面是写的一段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*sin(I(1,3)-pi/2),I(1,1)]; yiDir=[I(1,2)-ri*cos(I(1,3)-pi/2),I(1,2)]; xgDir=[G(1,1)+rg*sin(G(1,3)-pi/2),G(1,1)]; ygDir=[G(1,2)-rg*cos(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*sin(I(1,3)-pi/2),I(1,1)]; yiDir=[I(1,2)-ri*cos(I(1,3)-pi/2),I(1,2)]; xgDir=[G(1,1)+rg*sin(G(1,3)-pi/2),G(1,1)]; ygDir=[G(1,2)-rg*cos(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
