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 end3 仿真结果
4 参考文献
[1]朱宏辉, 王嘉豪. 一种移动机器人路径规划新算法[J]. 计算机测量与控制, 2020, 28(11):6.
