【三维路径规划】基于快速扩展随机树（RRT）实现无人机三维路径规划算法附matlab代码
【三维路径规划】基于快速扩展随机树（RRT）实现无人机三维路径规划算法附matlab代码
TT_Matlab
博主简介：擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真，完整matlab代码或者程序定制加qq1575304183。
1 简介
移动机器人运动规划技术是自主移动机器人导航的核心技术之一,而路径规划技术是导航技术研究的一个关键课题.路径规划的任务是:依据一定的评价准则(如距离最短,时间最短,工作代价最小等等),在一个存在障碍物的工作环境内,寻求一条从初始点开始到目标点结束的较优的无碰撞路径.本文旨在结合实际环境基于快速扩展随机树(Rapidly-Exploring Random Tree, RRT)算法实现自主移动机器人的路径规划。
2 部分代码
%% 绘制障碍物(以球为例，主要是方便计算)
x0=
100
; y0=
100
; z0=
100
;%球心
circleCenter = [
100
,
100
,
100
;
50
,
50
,
50
;
100
,
40
,
60
;
150
,
100
,
100
;
60
,
130
,
50
];
r=[
50
;
20
;
20
;
15
;
15
];%半径
%下面开始画
figure(
1
);
[
x,y,z
]=sphere;
for
i =
1
:length(circleCenter(:,
1
))
mesh(r(i)*x+circleCenter(i,
1
),r(i)*y+circleCenter(i,
2
),r(i)*z+circleCenter(i,
3
));hold
on
;
end
axis equal
%% 参数
source=[
10
10
10
];
goal=[
150
150
150
];
stepsize =
10
;
threshold =
10
;
maxFailedAttempts =
10000
;
display =
true
;
searchSize = [
250
250
250
]; %探索空间六面体
%% 绘制起点和终点
hold
on
;
scatter3(source(
1
),source(
2
),source(
3
),
’filled’
,
’g’
);
scatter3(goal(
1
),goal(
2
),goal(
3
),
’filled’
,
’b’
);
tic; % tic-toc: Functions
for
Elapsed Time
RRTree =
double
([source
-1
]);
failedAttempts =
0
;
pathFound =
false
;
%% 循环
while
failedAttempts <= maxFailedAttempts % loop to grow RRTs
%% chooses a random configuration
if
rand <
0.5
sample = rand(
1
,
3
) .* searchSize; % random sample
else
sample = goal; % sample taken
as
goal to bias tree generation to goal
end
%% selects the node
in
the RRT tree that
is
closest to qrand
[
A, I
] = min( distanceCost3(RRTree(:,
1
:
3
),sample) ,[],
1
); % find the minimum
value
of each column
closestNode = RRTree(I(
1
),
1
:
3
);
%% moving
from
qnearest an incremental distance
in
the direction of qrand
movingVec = [sample(
1
)-closestNode(
1
),sample(
2
)-closestNode(
2
),sample(
3
)-closestNode(
3
)];
movingVec = movingVec/sqrt(sum(movingVec.^
2
)); %单位化
newPoint = closestNode + stepsize * movingVec;
if
~checkPath3(closestNode, newPoint, circleCenter,r) %
if
extension of closest node
in
tree to the
new
point
is
feasible
failedAttempts = failedAttempts +
1
;
continue
;
end
if
distanceCost3
(
newPoint,goal
) < threshold, pathFound
=
true
;
break
; end % goal reached
[
A, I2
] = min( distanceCost3(RRTree(:,
1
:
3
),newPoint) ,[],
1
); % check
if
new
node
is
not already pre-
existing
in
the tree
if
distanceCost3
(
newPoint,RRTree(I2(
1
),1:3)) < threshold, failedAttempts
= failedAttempts +
1
;
continue
; end
RRTree = [RRTree;
newPoint
I
(
1
)]
; %
add
node
failedAttempts =
0
;
if
display, plot3([closestNode(
1
);newPoint(
1
)],[closestNode(
2
);newPoint(
2
)],[closestNode(
3
);newPoint(
3
)],
’LineWidth’
,
1
);
end
pause
(
0.05
)
;
end
if
display && pathFound, plot3([closestNode(
1
);goal(
1
)],[closestNode(
2
);goal(
2
)],[closestNode(
3
);goal(
3
)]); end
if
display, disp(
’click/press any key’
); waitforbuttonpress; end
if
~pathFound, error(
’no path found. maximum attempts reached’
); end
%% retrieve path
from
parent information
path = goal;
prev = I(
1
);
while
prev >
0
path = [RRTree(prev,
1
:
3
); path];
prev = RRTree(prev,
4
);
end
pathLength =
0
;
for
i=
1
:length(path(:,
1
))
-1
, pathLength = pathLength + distanceCost3(path(i,
1
:
3
),path(i+
1
,
1
:
3
)); end %
calculate path length
fprintf
(
’processing time=%d
Path Length=%d
’
, toc, pathLength
)
;
figure(
2
)
for
i =
1
:length(circleCenter(:,
1
))
mesh(r(i)*x+circleCenter(i,
1
),r(i)*y+circleCenter(i,
2
),r(i)*z+circleCenter(i,
3
));hold
on
;
end
axis equal
hold
on
;
scatter3(source(
1
),source(
2
),source(
3
),
’filled’
,
’g’
);
scatter3(goal(
1
),goal(
2
),goal(
3
),
’filled’
,
’b’
);
plot3(path(:,
1
),path(:,
2
),path(:,
3
),
’LineWidth’
,
2
,
’color’
,
’r’
);
3 仿真结果
4 参考文献
[1]马蓉. 基于改进RRT算法的无人机航路规划与跟踪方法研究[J]. 导航定位与授时, 2020, 7(1):6.