Matlab resolve o caminho para evitar obstáculos do tipo de empilhamento personalizado com base no algoritmo RRT

Language 2023-09-30 03:59:21 views: null

Índice

fundo

1 algoritmo de pesquisa RRT

2 Algoritmo de pesquisa RRT baseado em Matlab

3 Desenho de empilhamento personalizado baseado em Matlab

4 Evitação de obstáculos no caminho de empilhamento personalizado com base no algoritmo RRT

fundo

Durante o processo de planejamento do caminho         do braço do robô de paletização , é necessário fornecer o caminho ideal sem colisões para a próxima caixa com base no status de paletização das caixas existentes. O algoritmo de árvore aleatória de busca rápida RRT é um algoritmo de planejamento global de crescimento incremental baseado em amostragem aleatória e também é o método mais comumente usado no movimento de planejamento de braço de robô.

1 algoritmo de pesquisa RRT

q_{inicialização}O ponto inicial e o ponto final         do caminho do robô são conhecidos no ambiente espacial q_{meta}. O algoritmo RRT usa o ponto inicial q_{inicialização}como nó raiz, gera pontos aleatórios no espaço do robô q_{rand}, atravessa a árvore aleatória para encontrar o nó folha mais próximo e expande passo paranóq_{perto}do , pegue o novo nó e execute a detecção de colisão nele ; se a detecção for aprovada, ele será incluído na árvore aleatória, caso contrário o crescimento será abandonado. Repita as etapas acima até que o ponto alvo seja encontrado , o que significa que a busca do caminho foi concluída. O princípio do algoritmo RRT tradicional é mostrado na figura abaixo.q_{perto}q_{rand}q_{novo}

         Comparado com outros algoritmos, o algoritmo RRT é mais adequado para planejamento espacial de alta dimensão , mas também apresenta algumas falhas. Como os pontos aleatórios não têm direcionalidade, a velocidade de convergência é lenta ; ao encontrar muitos obstáculos, o algoritmo RRT tradicional é fácil de entrar em áreas locais.Para o problema do valor mínimo, tendo em conta as deficiências do algoritmo RRT tradicional, atualmente um algoritmo comum melhorado tem uma estratégia de viés alvo para acelerar a convergência.

        A essência do RRT é baseada no método de amostragem espacial, embora não seja ideal, mas é probabilisticamente completo . O núcleo deste tipo de algoritmo está na amostragem aleatória, começando no nó pai, gerando aleatoriamente nós filhos no mapa, conectando os nós pai e filho e realizando a detecção de colisão. Se não houver colisão, o nó filho é expandido.

2 Algoritmo de pesquisa RRT baseado em Matlab

%% 确定障碍物空间(以长方体为例，在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');

         ①A função de detecção de colisão 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

        ②A função de cálculo de distância distanceCost.m entre dois pontos:

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

Os resultados         da operação são:

         Pode-se ver que o lado esquerdo mostra o processo de busca de amostragem no espaço.Quando a linha que conecta o ponto de amostragem e o ponto final não colide cruzadamente com o espaço de colisão [expanded rectsize=[200,30,150]], a reta linha atinge o ponto final. Esportes é o resultado de pesquisa mais rápido.

3 Desenho de empilhamento personalizado baseado em Matlab

        Aqui, as pilhas são definidas no formato de um cubóide e o desenho é concluído com a ajuda da função plotcube. [ plotcube ] precisa preparar a posição do vértice, comprimento, largura, altura e configurações de cor do cubóide ao usá-lo, para que as configurações dos parâmetros do cubóide sejam inicializadas lendo os dados da tabela [ readmatrix ("XXX.xls")].

        Por exemplo , os dados da tabela de paralelepípedos retangulares empilhados são os seguintes:

        Nota : Matlab lê apenas os dados numéricos da imagem acima , ou seja, a partir da segunda linha (a primeira linha é uma descrição de string como BoxSize_L, que não é lida durante o processamento de dados)

         Os parâmetros na primeira linha representam:

                BoxSize_L, BoxSize_W, BoxSize_H: comprimento, largura e altura das caixas empilhadas

                Position_X,Position_Y,Position_Z: As coordenadas da posição do vértice da caixa

                ColorMode: o número da configuração de cor

                       Consulte as configurações de valor RGB na documentação oficial do Matlab . O código de configuração de cores é o seguinte:

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: A mesma pilha de caixas tem o mesmo número (as 4 pilhas de caixas definidas aqui)

        O código para desenhar dados do tipo pilha com base na tabela acima é o seguinte:

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

        O gráfico resultante é o seguinte:

4 Evitação de obstáculos no caminho de empilhamento personalizado com base no algoritmo RRT

        Para combinar o conteúdo dos Capítulos 2 e 3, primeiro leia os dados da tabela de formato de pilha personalizada e use esse formato de pilha como um obstáculo espacial para planejar o caminho RRT para evitar obstáculos. [Significado físico: Durante o processo de paletização do braço robótico, as caixas transportadas não podem colidir com o tipo de paletização existente. 】

         O código geral é o seguinte:

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

        O resultado da operação é:

                ​​​​​​​

        Muitas experiências descobriram que se o tamanho do passo (parâmetro stepsize) for grande, a pesquisa falhará devido a obstáculos grandes e complexos; se o tamanho do passo for pequeno, a pesquisa estará sempre em branco e não poderá convergir (a próxima posição em branco para o palete continuará a se expandir) espaço de pesquisa, não tem efeito).

Acho que você gosta

Origin blog.csdn.net/m0_46427461/article/details/131867880
Recomendado
Clasificación
Diario
Más
2024-06-11(0)
2024-06-10(0)
2024-06-09(0)
2024-06-08(0)
2024-06-07(0)
2024-06-06(0)
2024-06-05(0)
2024-06-04(0)
2024-06-03(0)
2024-06-02(0)

Code World

© 2018 codetd.com