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)相关教程