matlab dubins路径,路径规划算法学习笔记（三）——局部路径规划（Dubins Curve）...
路径规划算法学习笔记(三)——局部路径规划(Dubins Curve)
路径规划算法学习笔记(三)——局部路径规划(Dubins Curve)
路径规划算法学习笔记(三)——局部路径规划(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*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
路径规划算法学习笔记(三)——局部路径规划(Dubins Curve)相关教程