Matlab求解基于RRT算法的自定义垛型的路径避障

目录

背景

1  RRT搜索算法

2  基于Matlab的RRT搜索算法

3  基于Matlab的自定义垛型绘制

4  基于RRT算法的自定义垛型的路径避障

---

背景

        在码垛机械臂路径规划过程中，需要根据现有箱子的码垛状态，给出下一个箱子的最佳码放无碰撞路径。RRT 快速搜索随机树算法是一种基于随机采样的增量式增长全局规划算法，也是在机械臂规划运动中最常使用的方法。

1  RRT搜索算法

        在空间环境中已知机器人路径初始点 和终止点 ，RRT 算法以初始点 作为根节点,在机器人的空间中产生随机点 ，遍历随机树找到最近的叶子节点，由节点 向 的连线方向扩展 step 步长,得到新节点并对其进行碰撞检测；若检测通过则将纳入随机树,否则放弃此次生长。 重复上述步骤,直到寻找到目标点为止,代表一次路径搜索完成,传统的 RRT 算法原理如下图 所示。

         RRT 算法与其他算法相比较更加适应于高维空间的规划,但是也有一些缺陷，由于随机点没有方向性，导致收敛速度较慢；在碰到的障碍物较多时，传统的 RRT 算法容易进入局部极小值的问题，针对传统RRT 算法的不足，目前常见的有目标偏置策略以加快收敛的改进算法。

        RRT的本质是基于空间采样的方法，虽然不是最优的，但是在概率上完备的。这类算法的核心在于随机采样，从父节点开始，随机在地图上生成子节点，连接父子节点并进行碰撞检测，若无碰撞，就扩展该子节点。

2  基于Matlab的RRT搜索算法

%% 确定障碍物空间(以长方体为例，在z方向上有共50的膨胀大小)
origin = [100,0,50];
rectsize=[200,30,150];

%下面开始绘制障碍物，[长 宽 高]=[200 30 100]的矩形
figure(1);
plotcube([200 30 100],[0  -15  -25],1,[1 0 0]);     % ([长 宽 高],[立方体初始顶点位置]，1，[颜色])
axis equal
%% 参数
source=[100 100 10];        % 初始点
goal=[100 -100 10];         % 目标点
stepsize = 5;
threshold = 5;
maxFailedAttempts = 10000;  % 最大搜索次数
display = true;
searchsize = [200 400 200];      %探索空间六面体
%% 绘制起点和终点
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;

%% 循环（开始RRT搜索）
while failedAttempts <= maxFailedAttempts  % loop to grow RRTs
    %% chooses a random configuration
    if rand < 0.5
        sample = (rand(1,3)-[0 0.75 0.2]);   % random sample
        sample = sample .* searchsize;
    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( distanceCost(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));  %单位化
    if rand < 0.5
        newPoint=closestNode + stepsize * movingVec;
    else
        newPoint=goal;
    end
    % checkPath3判断路径与障碍物是否发生碰撞的函数
    if ~checkPath3(closestNode, newPoint, origin,rectsize) % if extension of closest node in tree to the new point is feasible
        failedAttempts = failedAttempts + 1;
        continue;
    end

    if distanceCost(newPoint,goal) < threshold, pathFound = true; break; end % goal reached
    [A, I2] = min( distanceCost(RRTree(:,1:3),newPoint) ,[],1); % check if new node is not already pre-existing in the tree
    if distanceCost(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 + distanceCost(path(i,1:3),path(i+1,1:3)); end % calculate path length
fprintf('processing time=%d \nPath Length=%d \n\n', toc, pathLength); 
figure(2)
plotcube([200 30 100],[0  -15  -25],1,[1 0 0]);
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','y');

         ①碰撞检测函数checkPath3.m为：

%% checkPath3.m	
function feasible=checkPath3(n,newPos,origin, size)
feasible=true;
size=size/2;
% disp(size);
movingVec = [newPos(1)-n(1),newPos(2)-n(2),newPos(3)-n(3)];
movingVec = movingVec/sqrt(sum(movingVec.^2)); %单位化
% 判断是否与障碍物立方体（以立方体中心为基准的空间边界计算）相交
if  (n(1) < (origin(1)+size(1)) && n(1) > (origin(1)-size(1))) && (n(2) < (origin(2)+size(2)) && n(2) > (origin(2)-size(2))) && (n(3) < (origin(3)+size(3)) && n(3) > (origin(3)-size(3)))
    feasible=false;
    return;
elseif  (newPos(1) < (origin(1)+size(1)) && newPos(1) > (origin(1)-size(1))) && (newPos(2) < (origin(2)+size(2)) && newPos(2) > (origin(2)-size(2))) && (newPos(3) < (origin(3)+size(3)) && newPos(3) > (origin(3)-size(3)))
    feasible=false;
    return;
else
    t=(origin(1)+size(1)-n(1))/movingVec(1);
    y=n(2)+movingVec(2)*t;
    z=n(3)+movingVec(3)*t;
    if (y < (origin(2)+size(2)) && y > (origin(2)-size(2))) && (z < (origin(3)+size(3)) && z > (origin(3)-size(3)))
        feasible=false;
        return;
    end
    t=(origin(1)-size(1)-n(1))/movingVec(1);
    y=n(2)+movingVec(2)*t;
    z=n(3)+movingVec(3)*t;
    if (y < (origin(2)+size(2)) && y > (origin(2)-size(2))) && (z < (origin(3)+size(3)) && z > (origin(3)-size(3)))
        feasible=false;
        return;
    end
    t=(origin(2)+size(2)-n(2))/movingVec(2);
    x=n(1)+movingVec(1)*t;
    z=n(3)+movingVec(3)*t;
    if (x < (origin(1)+size(1)) && x > (origin(1)-size(1))) && (z < (origin(3)+size(3)) && z > (origin(3)-size(3)))
        feasible=false;
        return;
    end
    t=(origin(2)-size(2)-n(2))/movingVec(2);
    x=n(1)+movingVec(1)*t;
    z=n(3)+movingVec(3)*t;
    if (x < (origin(1)+size(1)) && x > (origin(1)-size(1))) && (z < (origin(3)+size(3)) && z > (origin(3)-size(3)))
        feasible=false;
        return;
    end
    t=(origin(3)+size(3)-n(3))/movingVec(3);
    x=n(1)+movingVec(1)*t;
    y=n(2)+movingVec(2)*t;
    if (x < (origin(1)+size(1)) && x > (origin(1)-size(1))) && (y < (origin(2)+size(2)) && y > (origin(2)-size(2)))
        feasible=false;
        return;
    end
    t=(origin(3)-size(3)-n(3))/movingVec(3);
    x=n(1)+movingVec(1)*t;
    y=n(2)+movingVec(2)*t;
    if (x < (origin(1)+size(1)) && x > (origin(1)-size(1))) && (y < (origin(2)+size(2)) && y > (origin(2)-size(2)))
        feasible=false;
        return;
    end
end
end

        ②两点间距离计算函数distanceCost.m：

function h=distanceCost(a,b)         %% distanceCost.m
	h = sqrt(sum((a-b).^2, 2));
end

        运行的结果可得：

         可以看到，左侧为在空间中进行采样搜索的过程，当采样点与终点的连线不会与碰撞空间【膨胀后的rectsize=[200,30,150]】发生交叉碰撞后，直线到达终点的运动是最快的搜索结果。

3  基于Matlab的自定义垛型绘制

        这里设定堆垛均是长方体形状，借助plotcube函数，完成绘制。【plotcube】在使用时需要准备长方体的顶点位置、长宽高及颜色设置，故通过表格数据的读入【readmatrix("XXX.xls")】初始化长方体的参数设置。

        例.垛型长方体表格数据如下：

        注意： Matlab中只读取上述图片中的数值数据，即从第二行开始（第一行是BoxSize_L等字符串描述，数据处理的时候不读入）

         第一行的参数依次表示：

                BoxSize_L,BoxSize_W,BoxSize_H：堆垛箱子的长、宽、高

                Position_X,Position_Y,Position_Z：箱子的顶点位置坐标

                ColorMode：颜色设置的编号

                       参考Matlab官方文档中的RGB值的设置，颜色设置的代码如下：

ColorMode = [[0.4940 0.1840 0.5560];            
        [0.4660 0.6740 0.1880];
        [0.3010 0.7450 0.9330];
        [0.6350 0.0780 0.1840]];

                StackNum：同一堆箱子有一样的编号（这里设置的4堆箱子）

        基于上述表格的垛型数据的绘制代码如下：

Boxes = readmatrix('PalletBoxData_CSDN.xls','Range','C2:J18');    %'C2:J18'对应的是上述图片中的数值数据（第一行是BoxSize_L等字符串描述，数据处理的时候不读入）
ColorMode = [[0.4940 0.1840 0.5560];            % 指定绘图颜色
        [0.4660 0.6740 0.1880];
        [0.3010 0.7450 0.9330];
        [0.6350 0.0780 0.1840]];
figure(1);
plot_Obstacle(Boxes,ColorMode)         % 绘制堆垛现状（障碍物）



function plot_Obstacle(Boxes,ColorMode)
    
    % 绘制码垛
    % figure(1);
    hold on;
    for i=1:length(Boxes(:,1))
        plotcube([Boxes(i,1) Boxes(i,2) Boxes(i,3)],[Boxes(i,4) Boxes(i,5) Boxes(i,6)],1,ColorMode(Boxes(i,7),:));
        hold on
    end
    axis equal
end

        得到的结果图如下：

4  基于RRT算法的自定义垛型的路径避障

        即将2、3章的内容结合起来，先读入自定义的垛型表格数据，以此垛型为空间障碍物进行避障的RRT路径规划。【物理意义：机械臂在码垛过程中，携带箱子不能与已经存在的现有垛型发生碰撞。】

         整体代码如下：

clear all
clc

%% 读取码垛箱子的位置（上图中的表格数据）
Boxes = readmatrix('PalletBoxData_CSDN.xls','Range','C2:J18');
ColorMode = [[0.4940 0.1840 0.5560];            % 指定绘图颜色，参考https://ww2.mathworks.cn/help/matlab/creating_plots/specify-plot-colors.html
        [0.4660 0.6740 0.1880];
        [0.3010 0.7450 0.9330];
        [0.6350 0.0780 0.1840]];
figure(1);
plot_Obstacle(Boxes,ColorMode)         % 绘制堆垛现状（障碍物）

%% 初始化参数

% 障碍物空间
StackNum = Boxes(:,8);      % 获取码垛编号
StackHeight = [];   % 每一堆垛箱子的总高度
h_dialate = 0;     % 障碍物在高度上膨胀一定距离

[origin,rectsize] = calc_obstacleSpace(Boxes,StackNum,h_dialate,StackHeight);   % 计算障碍物空间
searchsize = [300,200,600];      %探索空间六面体

source=[315 -200 0];        % 初始点
goal=[0 200 88];         % 目标点
stepsize = 20;
threshold = 5;
maxFailedAttempts = 10000;  % 最大搜索次数
display = true;
searchsize = [200 400 200];      %探索空间六面体
%% 绘制起点和终点
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;


%% 循环（开始RRT搜索）
while failedAttempts <= maxFailedAttempts  % loop to grow RRTs
    %% chooses a random configuration
    if rand < 0.5
        sample = (rand(1,3)-[0 0.75 0.2]);   % random sample
        sample = sample .* searchsize;
    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( distanceCost(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));  %单位化
    if rand < 0.5
        newPoint=closestNode + stepsize * movingVec;
    else
        newPoint=goal;
    end
    if ~checkPath3(closestNode, newPoint, origin,rectsize) % if extension of closest node in tree to the new point is feasible
        failedAttempts = failedAttempts + 1;
        continue;
    end

    t = [];
    for i = 1:4
       t(i) = ~checkPath3(closestNode, newPoint, origin(i,:),rectsize(i,:));
    end
            % 用  if判断及 或语句，保证不碰撞到每一个堆垛
    if t(1)||t(2)||t(3)||t(4)% if extension of closest node in tree to the new point is feasible
        failedAttempts = failedAttempts + 1;
        continue;                   % continue一旦被执行（if成立），就会终止当前循环，进行下一次循环。
    end

    if distanceCost(newPoint,goal) < threshold, pathFound = true; break; end % goal reached
    [A, I2] = min( distanceCost(RRTree(:,1:3),newPoint) ,[],1); % check if new node is not already pre-existing in the tree
    if distanceCost(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



%% 最终轨迹展示
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 + distanceCost(path(i,1:3),path(i+1,1:3)); end % calculate path length
fprintf('processing time=%d \nPath Length=%d \n\n', toc, pathLength); 
figure(2)
plot_Obstacle(Boxes,ColorMode)         % 绘制堆垛现状（障碍物）
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','y');


%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%自定义函数%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 绘制障碍物(以长方体为例，主要是方便计算)
function plot_Obstacle(Boxes,ColorMode)
    
    % 绘制码垛
    % figure(1);
    hold on;
    for i=1:length(Boxes(:,1))
        plotcube([Boxes(i,1) Boxes(i,2) Boxes(i,3)],[Boxes(i,4) Boxes(i,5) Boxes(i,6)],1,ColorMode(Boxes(i,7),:));
        hold on
    end
    axis equal
end
% 计算障碍物空间
% StackNum：堆垛的个数； h_dialate：堆垛障碍物的膨胀高度
% origin：膨胀后的障碍物空间的中心点； rectsize：膨胀后的障碍物空间
function [origin,rectsize] = calc_obstacleSpace(Boxes,StackNum,h_dialate,StackHeight)

    StackCount = hist(StackNum,unique(StackNum));       %获取每一堆有几个箱子
    origin = [];        % 膨胀后的障碍物空间的中心点
    rectsize=[];        % 膨胀后的障碍物空间
    for i=1:4          %%%%%%%%%%%%%%%%此时一共有4堆箱子
        h = Boxes(find(StackNum==i),3);         %分别求每一堆的箱子的高度（默认都是同一种箱子，高度相同）
        h_i = StackCount(i)*h(1);               %求每一堆的箱子总高度（目前累到的高度）
        StackHeight = [StackHeight,h_i];        %箱子总高度列到一个列表中
    
        x = Boxes(find(StackNum==i),4) + Boxes(find(StackNum==i),1)/2;         
        y = Boxes(find(StackNum==i),5) + Boxes(find(StackNum==i),2)/2;
        %分别求每一堆的箱子的中心点位置x,y,z坐标（默认都是同一种箱子，高度相同）
        x_center = x(1);
        y_center = y(1);
        z_center = h_i/2;
    
        obstacle_center = [x_center,y_center,z_center];
    
        %分别求每一堆的箱子的长、宽
        l = Boxes(find(StackNum==i),1);
        w = Boxes(find(StackNum==i),2);
        l_i = l(1);
        w_i = w(1);
    
        obstacle_volumn = [l_i,w_i,h_i+h_dialate];
    
        origin = cat(1,origin,obstacle_center);               % 膨胀后的障碍物空间的中心点
        rectsize = cat(1,rectsize,obstacle_volumn);
    end
end

        得到的运行结果为：

                ​​​​​​​

        多次实验发现，若步长（参数stepsize）较大，由于障碍物体积大且复杂，会导致搜索失败； 若步长较小，一直在空白搜索，不能收敛（在码垛旁边空白位置不断扩大搜索空间，没有效果）。