关注公众号，点击公众号主页右上角“ ··· ”，设置星标，实时关注智能汽车电子与软件最新资讯
%% 生成障碍物ob1 = [0,-10,10,5]; % 三个矩形障碍物ob2 = [-5,5,5,10];ob3 = [-5,-2,5,4];ob_limit_1 = [-15,-15,0,31]; % 边界障碍物ob_limit_2 = [-15,-15,30,0];ob_limit_3 = [15,-15,0,31];ob_limit_4 = [-15,16,30,0];ob = [ob1;ob2;ob3;ob_limit_1;ob_limit_2;ob_limit_3;ob_limit_4]; % 放到一个数组中统一管理x_left_limit = -16; % 地图的边界x_right_limit = 15;y_left_limit = -16;y_right_limit = 16;
我在这随便选择生成三个矩形的障碍物，并统一放在ob数组中管理，同时定义地图的边界。
%% 初始化参数设置extend_area = 0.2; % 膨胀范围resolution = 1; % 分辨率robot_radius = 0.2; % 机器人半径goal = [-10, -10]; % 目标点x_start = [13, 10]; % 起点grow_distance = 1; % 生长距离goal_radius = 1.5; % 在目标点为圆心，1.5m内就停止搜索
%% 初始化随机树tree.child = []; % 定义树结构体，保存新节点及其父节点tree.parent = [];tree.child = x_start; % 起点作为第一个节点flag = 1; % 标志位new_node_x = x_start(1,1); % 将起点作为第一个生成点new_node_y = x_start(1,2);new_node = [new_node_x, new_node_y];
rd_x = 30 * rand() - 15; % 生成随机点rd_y = 30 * rand() - 15;if (rd_x >= x_right_limit || rd_x <= x_left_limit ||... % 判断随机点是否在地图边界范围内rd_y >= y_right_limit || rd_y <= y_left_limit)flag = 0;end
[angle, min_idx] = cal_distance(rd_x, rd_y, tree); % 返回tree中最短距离节点索引及对应的和x正向夹角function [angle, min_idx] = cal_distance(rd_x, rd_y, tree)distance = [];i = 1;while i<=size(tree.child,1)dx = rd_x - tree.child(i,1);dy = rd_y - tree.child(i,2);d = sqrt(dx^2 + dy^2);distance(i) = d;i = i+1;end[~, min_idx] = min(distance);angle = atan2(rd_y - tree.child(min_idx,2),rd_x - tree.child(min_idx,1));end
new_node_x = tree.child(min_idx,1)+grow_distance*cos(angle);% 生成新的节点new_node_y = tree.child(min_idx,2)+grow_distance*sin(angle);new_node = [new_node_x, new_node_y];
for k=1:1:size(ob,1)for i=min(tree.child(min_idx,1),new_node_x):0.01:max(tree.child(min_idx,1),new_node_x) % 判断生长之后路径与障碍物有无交叉部分j = (tree.child(min_idx,2) - new_node_y)/(tree.child(min_idx,1) - new_node_x) *(i - new_node_x) + new_node_y;if(i >=ob(k,1)-resolution && i <= ob(k,1)+ob(k,3) && j >= ob(k,2)-resolution && j <= ob(k,2)+ob(k,4))flag = 0;breakendendend
在这我采用的方法是写出新节点和父节点连线的直线方程，然后将x变化范围限制在min(tree.child(min_idx,1),new_node_x)max(tree.child(min_idx,1),new_node_x)内，0.01即坐标变换的步长，步长越小判断的越精确，但同时会增加计算量；
if (flag == true) % 若标志位为1，则可以将该新节点加入tree中tree.child(end+1,:) = new_node;tree.parent(end+1,:) = [tree.child(min_idx,1), tree.child(min_idx,2)];plot(rd_x, rd_y, '.r');hold onplot(new_node_x, new_node_y,'.g');hold onplot([tree.child(min_idx,1),new_node_x], [tree.child(min_idx,2),new_node_y],'-b');endflag = 1; % 标志位归位
%% 显示for i=1:1:size(ob,1) % 绘制障碍物fill([ob(i,1)-resolution, ob(i,1)+ob(i,3),ob(i,1)+ob(i,3),ob(i,1)-resolution],...[ob(i,2)-resolution,ob(i,2)-resolution,ob(i,2)+ob(i,4),ob(i,2)+ob(i,4)],'k');endhold onplot(x_start(1,1)-0.5*resolution, x_start(1,2)-0.5*resolution,'b^','MarkerFaceColor','b','MarkerSize',4*resolution); % 起点plot(goal(1,1)-0.5*resolution, goal(1,2)-0.5*resolution,'m^','MarkerFaceColor','m','MarkerSize',4*resolution); % 终点set(gca,'XLim',[x_left_limit x_right_limit]); % X轴的数据显示范围set(gca,'XTick',[x_left_limit:resolution:x_right_limit]); % 设置要显示坐标刻度set(gca,'YLim',[y_left_limit y_right_limit]); % Y轴的数据显示范围set(gca,'YTick',[y_left_limit:resolution:y_right_limit]); % 设置要显示坐标刻度grid ontitle('D-RRT');xlabel('横坐标 x');ylabel('纵坐标 y');pause(0.05);if (sqrt((new_node_x - goal(1,1))^2 + (new_node_y- goal(1,2))^2) <= goal_radius) % 若新节点到目标点距离小于阈值，则停止搜索，并将目标点加入到node中tree.child(end+1,:) = goal; % 把终点加入到树中tree.parent(end+1,:) = new_node;disp('find goal!');breakend
%% 绘制最优路径temp = tree.parent(end,:);trajectory = [tree.child(end,1)-0.5*resolution, tree.child(end,2)-0.5*resolution];for i=size(tree.child,1):-1:2if(size(tree.child(i,:),2) ~= 0 & tree.child(i,:) == temp)temp = tree.parent(i-1,:);trajectory(end+1,:) = tree.child(i,:);if(temp == x_start)trajectory(end+1,:) = [temp(1,1) - 0.5*resolution, temp(1,2) - 0.5*resolution];endendendplot(trajectory(:,1), trajectory(:,2), '-r','LineWidth',2);pause(2);
关注公众号，点击公众号主页右上角“ ··· ”，设置星标，实时关注智能汽车电子与软件最新资讯
