1 简介
移动机器人运动规划技术是自主移动机器人导航的核心技术之一,而路径规划技术是导航技术研究的一个关键课题.路径规划的任务是:依据一定的评价准则(如距离最短,时间最短,工作代价最小等等),在一个存在障碍物的工作环境内,寻求一条从初始点开始到目标点结束的较优的无碰撞路径.本文旨在结合实际环境基于快速扩展随机树(Rapidly-Exploring Random Tree, RRT)算法实现自主移动机器人的路径规划。
2 部分代码
%***************************************
%% ?????
clear all; close all;
x_I=1; y_I=1; % ?????
x_G=700; y_G=700; % ?????
Thr=50; % ???????
Delta= 30; % ??????
NearDelta= 60; % ??near?????
%% ?????
T.v(1).x = x_I; % T????????v??????????????T???
T.v(1).y = y_I;
T.v(1).xPrev = x_I; % ??????????????
T.v(1).yPrev = y_I;
T.v(1).dist=0; % ????????????????????
T.v(1).rootDist=0; % ????????????????????
T.v(1).indPrev = 0; %
T.v(1).p = []; % ?????????????
%% ?????棗????
figure(1);
ImpRgb=imread('newmap.png');
Imp=rgb2gray(ImpRgb);
imshow(Imp)
xL=size(Imp,1);
yL=size(Imp,2);
hold on
plot(x_I, y_I, 'ro', 'MarkerSize',10, 'MarkerFaceColor','r');
plot(x_G, y_G, 'go', 'MarkerSize',10, 'MarkerFaceColor','g');
count=1;
found = 0;
updateFlag = 0;
solutionEndIdx = -1;
solutionIdxSet = [];
solutionEllipse = inf;
for iter = 1:4000
%Step 1: ???????????x_rand,?????????????
x_rand=[xL, yL].*rand(1,2);
if norm(x_rand - [x_I,y_I])+norm(x_rand - [x_G,y_G])> solutionEllipse
continue;
end
%Step 2: ?????????????? x_phase1_near
x_phase1_near=[];
min_dis_toT = inf;
for i = 1:length(T.v)
t_node = T.v(i);
t_nodexy = [t_node.x, t_node.y];
dis_toT = norm(t_nodexy - x_rand);
if dis_toT < min_dis_toT
min_dis_toT = dis_toT;
x_phase1_near = t_nodexy;
end
end
%Step 3: ????x_new??
dxy = x_rand - x_phase1_near;
dxynorm = dxy/norm(dxy);
x_new = x_phase1_near + dxynorm*Delta;
if ~collisionChecking(x_new,[],Imp)
continue;
end
%Step 3: ??????x_new?????x_link,???near??
near_set = [];
min_dis_toT = inf;
x_link_idx = -1;
for i = 1:length(T.v)
t_node = T.v(i);
t_nodexy = [t_node.x, t_node.y];
dis_toT = norm(t_nodexy - x_new);
if dis_toT < NearDelta && collisionChecking(x_new,t_nodexy,Imp)
near_set = [near_set;t_nodexy, i];
curDist = dis_toT + T.v(i).rootDist;
if min_dis_toT > curDist
min_dis_toT = curDist;
x_link_idx = i;
end
end
end
if x_link_idx == -1
continue;
end
x_link = [T.v(x_link_idx).x,T.v(x_link_idx).y];
%Step 4: ?x_new???T
%??????x_new?????x_link
count=count+1;
T.v(count).x = x_new(1);
T.v(count).y = x_new(2);
T.v(count).xPrev = x_link(1);
T.v(count).yPrev = x_link(2);
T.v(count).dist=norm(x_new - x_link);
T.v(count).rootDist= T.v(x_link_idx).rootDist + norm(x_new - x_link);
T.v(count).indPrev = x_link_idx;
T.v(count).p = plot([x_link(1), x_new(1)],[x_link(2), x_new(2)], 'r', 'marker', '.');
drawnow;
%Step 5:??rewire
updateFlag = 0;
x_new_idx = count;
% disp(['cur is ', num2str(curNodeIdx), 'link to ', num2str(selected_nea_idx)])
for i = 1:size(near_set, 1)
node_xy = near_set(i,1:2);
node_idx = near_set(i,3);
% ???x_new???
if node_idx == x_new_idx
continue;
end
dist = norm(x_new - node_xy);
if T.v(node_idx).rootDist > T.v(x_new_idx).rootDist + dist
updateFlag = 1;
T.v(node_idx).rootDist = T.v(x_new_idx).rootDist + dist;
T.v(node_idx).xPrev = x_new(1);
T.v(node_idx).yPrev = x_new(2);
T.v(node_idx).dist = dist;
T.v(node_idx).indPrev = x_new_idx;
delete(T.v(node_idx).p)
T.v(node_idx).p = plot([x_new(1), node_xy(1)],[x_new(2), node_xy(2)], 'r', 'marker', '.');
drawnow;
end
end
%Step 6:?????????????
if norm(x_new-[x_G,y_G]) < Thr && collisionChecking(x_new,[x_G,y_G],Imp) && found == 0
% ??[x_G y_G]??
count=count+1;
solutionEndIdx = count;
T.v(count).x = x_G;
T.v(count).y = y_G;
T.v(count).xPrev = x_new(1);
T.v(count).yPrev = x_new(2);
T.v(count).dist=norm(x_new - [x_G,y_G]);
T.v(count).rootDist= T.v(x_new_idx).rootDist + norm(x_new - [x_G,y_G]);
T.v(count).indPrev = x_new_idx;
T.v(count).p = plot([x_new(1), x_G],[x_new(2), y_G], 'r', 'marker', '.');
found = 1;
% ??[x_I x_I]?x_new ???????????
pathIndex = solutionEndIdx;
solutionIdxSet = [pathIndex];
set(T.v(pathIndex).p, 'color', 'b','Linewidth', 3);
for k = 1:1000
pathIndex = T.v(pathIndex).indPrev;
if pathIndex == 1
break
end
solutionIdxSet = [solutionIdxSet, pathIndex];
set(T.v(pathIndex).p, 'color', 'b','Linewidth', 3);
end % ????????
drawnow;
end
%Step 7: ????????????????????????????????????
if found == 1 && updateFlag == 1
% ??????
solutionEllipse = -inf;
for i = 1:length(solutionIdxSet)
set(T.v(solutionIdxSet(i)).p, 'color', 'r','Linewidth', 1);
node_xy = [T.v(solutionIdxSet(i)).x,T.v(solutionIdxSet(i)).y];
if solutionEllipse < norm(node_xy - [x_I,y_I])+norm(node_xy - [x_G,y_G])
solutionEllipse = norm(node_xy - [x_I,y_I])+norm(node_xy - [x_G,y_G]);
end
end
% ???????????
pathIndex = solutionEndIdx;
solutionIdxSet = [pathIndex];
set(T.v(pathIndex).p, 'color', 'b','Linewidth', 3);
for k = 1:1000
pathIndex = T.v(pathIndex).indPrev;
if pathIndex == 1
break
end
solutionIdxSet = [solutionIdxSet, pathIndex];
set(T.v(pathIndex).p, 'color', 'b','Linewidth', 3);
end % ????????
drawnow;
end
end
3 仿真结果
4 参考文献
[1]朱宏辉, 王嘉豪. 一种移动机器人路径规划新算法[J]. 计算机测量与控制, 2020, 28(11):6.