路径规划之实现全局路径（改进的RRT算法）加局部重规划（DWA）进行实时避障
在上面两篇博客中，我们分别实现了改进的三维RRT算法和三维DWA算法，很多同学问我怎么实现全局轨迹加局部局部实时轨迹，下面就是实现的思路。
1、首先，我们的代码主体还是DWA三维的代码；
2、我们生成一条全局的参考代码（也可以是三维RRT算法计算得到的轨迹）；
3、给机器人一个感知范围，当感知到全局路径上有障碍物时，则计算出可以避开障碍物的切入点和切出点，这两个分别是全局路径上的路径点；（切出点就是从全局路径点出来的点，切入点就是回到全局路径上的点）；
下面是运行过程图：（最下面的圆为动态障碍物，最上面的两个圆为静态障碍物，全局路径使用了最简单的对角线为全局路径。）
运行过程图，此时计算出了切入点和切出点
运行结果图
下面是实现函数，仅供参考
主函数：
clear;
clc;
%句柄函数，求距离需要使用
calcuDis = @(x,y) sqrt((x(1)-y(1))^2+(x(2)-y(2))^2+(x(3)-y(3))^2);
%%定义无人机初始状态
startPoint = [0 0 0];
endPoint=[100 100 100];
v0=[0 0 0];
% v0=[3 4 3];
%定义初始状态
x = [startPoint v0];
allPath.x=[];
allPath.x=[allPath.x,x];
% result.x=[];
% result.x=[result.x;x];
%定义障碍物
%静态障碍物
sphereInfo.exist = 0;
% sphereInfo = creatSphereObject(sphereInfo);
%动态障碍物
sphereInfo = creatSphereObjectDynamic(sphereInfo);
%边界限制，防止无人机调整时跑出边界
axisStart = [0 0 0];
axisLWH = [105 105 105];
% global dt;
dt = 0.1;%时间，每条路径由多个值构成，dt为每个点之间的时间间隔
paraT=3;%前向模拟的时间
%%无人机运动学参数
vMax = [5,5,5];%各轴最大速度
acc = [5,5,5];%各轴加速度
VResolution = 0.1;%速度分辨率，用于轨迹推算
model = [vMax acc VResolution];
%评价函数参数[heading,dist,velocity,predictDT]
%航向得分比重，距离得分比重，速度得分比重，向前模拟轨迹的时间
% evalParam = [0.045,0.1,0.1,3.0];
% evalParam = [0.1,0.15,0.1,3.0];%静态障碍物效果较好
evalParam = [0.1,0.05,0.1,3.0]; %动态障碍物效果较好
% evalParam = [0.1,0.1,0.08,3.0];
%模拟区域的范围=边界限制
% area = [0 0 0 105 105 105];
% 根据起点终点生成参考路径path
path=startPoint;
step=6;
k4 =0;
deltaX = -startPoint(1) + endPoint(1);
deltaY = -startPoint(2) + endPoint(2);
deltaZ = -startPoint(3) + endPoint(3);
r = sqrt(deltaX^2+deltaY^2+deltaZ^2);
fai = atan2(deltaY,deltaX);
theta = acos(deltaZ/r);
while k4 < 100
k4=k4+1;
nx = k4*step*sin(theta)*cos(fai);
ny = k4*step*sin(theta)*sin(fai);
nz = k4*step*cos(theta);
if nx >=endPoint(1) || ny >=endPoint(2)||nz >=endPoint(3)
path =[path;endPoint];
break;
end
path =[path;[nx ny nz]];
end
%定义感知域R（应该用制动距离）
% R =paraT*calcuDis(vMax,[0 0 0]);
xmax=[0 0 0 vMax];
R=calcBreakingDist(xmax,model,dt);
% R=5;
%定义参考路径点速度
v2=[2 2 2];
%定义寻径的时间
N=10000;
j1=1;
j2=0;
%基于参考轨迹跟踪
ok = false;
while ~ok
% j2=j2+dt;
j2=dt;
DistTemp=[];
%无人机状态更新
xTemp=[allPath.x(end,1)+j2*2 allPath.x(end,2)+j2*2 allPath.x(end,3)+j2*2 2 2 2];
%圆心位置更新
%更新障碍物信息
for n2 = 1:size(sphereInfo.centerX,2)
%X
if sphereInfo.centerX(n2) >= sphereInfo.originalcenterX(n2)+sphereInfo.limtX ||...
sphereInfo.centerX(n2) <= sphereInfo.originalcenterX(n2)-sphereInfo.limtX
sphereInfo.vX(n2)=-sphereInfo.vX(n2);
end
%sphereInfo.centerX(n1)=sphereInfo.centerX(n1)+sphereInfo.vX(n1)*t1;
%Y
if sphereInfo.centerY(n2) >= sphereInfo.originalcenterY(n2)+sphereInfo.limtY ||...
sphereInfo.centerY(n2) <= sphereInfo.originalcenterY(n2)-sphereInfo.limtY
sphereInfo.vY(n2)=-sphereInfo.vY(n2);
end
%sphereInfo.centerY(n1)=sphereInfo.centerY(n1)+sphereInfo.vY(n1)*t1;
%z
if sphereInfo.centerZ(n2) >= sphereInfo.originalcenterZ(n2)+sphereInfo.limtZ ||...
sphereInfo.centerZ(n2) <= sphereInfo.originalcenterZ(n2)-sphereInfo.limtZ
sphereInfo.vZ(n2)=-sphereInfo.vZ(n2);
end
sphereInfo.centerX(n2)=sphereInfo.centerX(n2)+sphereInfo.vX(n2)*dt;
sphereInfo.centerY(n2)=sphereInfo.centerY(n2)+sphereInfo.vY(n2)*dt;
sphereInfo.centerZ(n2)=sphereInfo.centerZ(n2)+sphereInfo.vZ(n2)*dt;
distTemp=calcuDis(xTemp(:,1:3),[sphereInfo.centerX(n2) sphereInfo.centerY(n2) sphereInfo.centerZ(n2)]);
DistTemp=[DistTemp,distTemp];
% distMin=100;
% if distTemp < distMin
% distMin = distTemp
% indx=n2;
% % R+sphereInfo.radius(indx)
% end
end
[distMin,indx]=min(DistTemp);
disp(indx);
if distMin <R+sphereInfo.radius(indx)
%可能相撞，则切入DWA，避碰
%确定起点和终点
sp=xTemp(:,1:3)
while j1<=size(path,1)-1
distSwitch =calcuDis(path(j1,:),[sphereInfo.centerX(indx) sphereInfo.centerY(indx) sphereInfo.centerZ(indx)])
%满足条件的第一个点直接退出
%判断该点与所有障碍物的距离
if (xTemp(1)< path(end,1))&&(xTemp(2)< path(end,2))&&(xTemp(3)< path(end,3))&&...
(path(j1,1)> sphereInfo.centerX(indx))&&(path(j1,2)> sphereInfo.centerY(indx))&&(path(j1,3)> sphereInfo.centerZ(indx))&&...
distSwitch >R+sphereInfo.radius(indx)
ep=path(j1,:)
%判断该节点是否满足无碰撞和距离条件
[flag1,flag2] = isCollision([ep,2,2,2],sphereInfo,R);
if flag1==1||flag2==1
j1=j1+1;
else
break;
end
elseif(xTemp(1)>= path(end,1))&&(xTemp(2)>= path(end,2))&&(xTemp(3)>= path(end,3))
ep=path(end,1:3);
break;
else
j1=j1+1;
end
% ep=path(end,1:3);
end
%没有满足条件的点，则直接取终点
if j1==size(path,1)
ep=path(end,1:3);
end
[result] = DWA_3D(sp,ep,xTemp,sphereInfo,axisStart,axisLWH,dt,paraT,model,evalParam,R);
allPath.x=[allPath.x;result.x];
%切换回原来的轨迹之后，时间归零,j1复位
% j2=0;
j1=1;
% %把该终点插入到结果中
% result.x=[result.x;[ep 2 2 2]];
else
%如果感知域范围内没有障碍物，则继续前进
allPath.x=[allPath.x;xTemp];
end
% distToGoal=calcuDis(result.x(end,1:3),path(end,1:3))
distToGoal=calcuDis((xTemp(:,1:3)),path(end,1:3));
if distToGoal <3
disp('==========Arrive Goal!!==========');
ok=true;
break;
end
% plot3([allPath.x(end-1,1) allPath.x(end,1)],[allPath.x(end-1,2) allPath.x(end,2)],[allPath.x(end-1,3) allPath.x(end,3)],'--r',LineWidth=3);
% % hold on;
% pause(0.1);
% figure(1);
% for k6=1:size(allPath.x,1)-1
% plot3([allPath.x(k6,1) allPath.x(k6+1,1)],[allPath.x(k6,2) allPath.x(k6+1,2)],[allPath.x(k6,3) allPath.x(k6+1,3)],'--g',LineWidth=3);hold on;
% end
end
for k6=1:size(allPath.x,1)-1
plot3([allPath.x(k6,1) allPath.x(k6+1,1)],[allPath.x(k6,2) allPath.x(k6+1,2)],[allPath.x(k6,3) allPath.x(k6+1,3)],'-g',LineWidth=3);hold on;
end
里面的函数在DWA的代码中有，这里就不放上来了。
完整代码下载地址。
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
