Tabla de contenido
2 algoritmo de búsqueda RRT basado en Matlab
3 Dibujo de apilamiento personalizado basado en Matlab
4 Evitación de obstáculos en la ruta de apilamiento personalizada basada en el algoritmo RRT
fondo
Durante el proceso de planificación de la ruta del brazo robótico de paletización , es necesario proporcionar la ruta óptima sin colisiones para la siguiente caja en función del estado de paletización de las cajas existentes. El algoritmo de árbol aleatorio de búsqueda rápida RRT es un algoritmo de planificación global de crecimiento incremental basado en muestreo aleatorio y también es el método más utilizado en la planificación del movimiento del brazo robótico.
1 algoritmo de búsqueda RRT
El punto inicial y el punto final de la ruta del robot se conocen en el entorno espacial
. El algoritmo RRT utiliza el punto inicial
como nodo raíz, genera puntos aleatorios en el espacio del robot
, atraviesa el árbol aleatorio para encontrar el nodo hoja más cercano y se expande . paso anodo
del , obtenga el nuevo nodo y realice la detección de colisiones en él ; si la detección pasa, se incluirá en el árbol aleatorio; de lo contrario, se abandonará el crecimiento. Repita los pasos anteriores hasta encontrar el punto objetivo , lo que significa que se completa la búsqueda de ruta. El principio del algoritmo RRT tradicional se muestra en la siguiente figura.
En comparación con otros algoritmos, el algoritmo RRT es más adecuado para la planificación espacial de alta dimensión , pero también tiene algunos defectos. Dado que los puntos aleatorios no tienen direccionalidad, la velocidad de convergencia es lenta ; cuando se encuentran muchos obstáculos, el algoritmo RRT tradicional es fácil de entrar en áreas locales Para el problema del valor mínimo , en vista de las deficiencias del algoritmo RRT tradicional, actualmente un algoritmo mejorado común tiene una estrategia de sesgo objetivo para acelerar la convergencia.
La esencia de RRT se basa en el método de muestreo espacial, que aunque no es óptimo, es probabilísticamente completo . El núcleo de este tipo de algoritmo radica en el muestreo aleatorio, comenzando desde el nodo principal, generando nodos secundarios aleatoriamente en el mapa, conectando los nodos principal e secundario y realizando la detección de colisiones. Si no hay colisión, el nodo secundario se expande.
2 algoritmo de búsqueda RRT basado en 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');①La función de detección de colisiones checkPath3.m es:
%% 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②La función de cálculo de distancia distanciaCosto.m entre dos puntos:
function h=distanceCost(a,b) %% distanceCost.m
h = sqrt(sum((a-b).^2, 2));
endLos resultados de la operación son:
Se puede ver que el lado izquierdo muestra el proceso de búsqueda de muestreo en el espacio. Cuando la línea que conecta el punto de muestreo y el punto final no choca con el espacio de colisión [rectsize expandido = [200,30,150]], la línea recta La línea llega al punto final. Deportes es el resultado de búsqueda más rápido.
3 Dibujo de apilamiento personalizado basado en Matlab
Aquí, las pilas están configuradas para que tengan la forma de un cuboide y el dibujo se completa con la ayuda de la función plotcube. [ plotcube ] necesita preparar la posición del vértice, la longitud, el ancho, la altura y el color del cuboide cuando se usa, por lo que la configuración de los parámetros del cuboide se inicializa leyendo los datos de la tabla [ readmatrix ("XXX.xls")].
Por ejemplo , los datos de la tabla del paralelepípedo rectangular apilado son los siguientes:
Nota : Matlab solo lee los datos numéricos en la imagen de arriba , es decir, comenzando desde la segunda línea (la primera línea es una descripción de cadena como BoxSize_L, que no se lee durante el procesamiento de datos)
Los parámetros en la primera línea representan:
BoxSize_L, BoxSize_W, BoxSize_H: largo, ancho y alto de cajas apiladas
Position_X, Position_Y, Position_Z: las coordenadas de posición del vértice del cuadro
ColorMode: el número de la configuración de color
Consulte la configuración del valor RGB en la documentación oficial de Matlab.El código de configuración de color es el siguiente:
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: La misma pila de cajas tiene el mismo número (las 4 pilas de cajas configuradas aquí)
El código para dibujar datos de tipo de pila según la tabla anterior es el siguiente:
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
endEl gráfico resultante es el siguiente:
4 Evitación de obstáculos en la ruta de apilamiento personalizada basada en el algoritmo RRT
Para combinar el contenido de los Capítulos 2 y 3, primero lea los datos de la tabla de formas de pila personalizadas y utilice esta forma de pila como obstáculo espacial para planificar la ruta RRT para evitar obstáculos. [Significado físico: Durante el proceso de paletizado del brazo robótico, las cajas transportadas no pueden chocar con el tipo de paletizado existente. 】
El código general es el siguiente:
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
El resultado de la operación es:
Muchos experimentos han encontrado que si el tamaño del paso (parámetro tamaño del paso) es grande, la búsqueda fallará debido a obstáculos grandes y complejos; si el tamaño del paso es pequeño, la búsqueda siempre estará en el espacio en blanco y no podrá converger (la posición en blanco siguiente a la paleta seguirá expandiéndose) espacio de búsqueda, no tiene ningún efecto).