Simulación MATLAB del algoritmo RRT en la planificación de trayectorias 3D

Planificación de trayectorias bidimensionales RRT

Acerca de la planificación de trayectorias bidimensionales RRT y el blog de referencia de implementación de matlab:
RRT

Análisis simple del algoritmo RRT

1. Genere el nodo raíz y proporcione el nodo final.
2. Encuentra aleatoriamente un punto rr en el espacio$r$ (50% de punto aleatorio, 50% como punto de destino), juezrr¿Qué nodo en el árbol de trayectoria tiene la distancia euclidiana más pequeña entre$r$ , y denotar este nodo como$nod{e}_{best}$。
3. De $nod{e}_{best}Para$$r$ Tamaño delpaso deextensión Tamaño del paso Tamaño del$stepSize$ unidades (implementadasporvector).
4. Determine el segmento de línea $nod{e}_{best}->r$ Si toca un obstáculo. Si es así, abandone la$r$ , de vuelta a 2.
5. Juez rrSi$r$ está cerca del punto de destino, si es así, salga del bucle, de lo contrario será$r\; se$ agrega al árbol de trayectorias, de nuevo a 2.

El algoritmo también puede introducir el número máximo de errores. Si un nuevo nodo hijo que no colisiona aún no se encuentra después de exceder el número máximo de errores en un proceso de iteración, el ciclo saldrá y un error "no puede encontrar una trayectoria razonable". .

Ventajas y desventajas de RRT

Al realizar la detección de colisiones en los puntos de muestreo en el espacio de estado, se evita el modelado del espacio y se puede resolver eficazmente el problema de planificación de la trayectoria del espacio de alta dimensión y las restricciones complejas. La característica de este método es que puede buscar en el espacio de alta dimensión de manera rápida y efectiva. A través de puntos de muestreo aleatorios en el espacio de estado, la búsqueda se dirige al área en blanco, para encontrar una ruta planificada desde el punto de partida hasta el objetivo. Punto Planificación de caminos en medio ambiente y ambiente dinámico. Similar al PRM, este método es probabilísticamente completo y no óptimo.

Simulación MATLAB de planificación de trayectorias 3D

Vea el blog dado arriba en dos dimensiones.
Configuramos el obstáculo tridimensional como una bola para facilitar la detección y el dibujo de la colisión.

%% 绘制障碍物(以球为例，主要是方便计算)
x0=100; y0=100; z0=100;%球心
circleCenter = [100,100,100;50,50,50;100,40,60;150,100,100;60,130,50];
r=[50;20;20;15;15];%半径
%下面开始画
figure(1);
[x,y,z]=sphere;
for i = 1:length(circleCenter(:,1))
    mesh(r(i)*x+circleCenter(i,1),r(i)*y+circleCenter(i,2),r(i)*z+circleCenter(i,3));hold on;
end
axis equal

Rellene el circleCenter y la matriz r con el centro de la esfera y el radio correspondiente.

Dados los parámetros de simulación: el
objetivo es el objetivo y la
fuente es el punto de partida.

%% 参数
source=[10 10 10];
goal=[150 150 150];
stepsize = 10;
threshold = 10;
maxFailedAttempts = 10000;
display = true;
searchSize = [250 250 250];      %探索空间六面体

El hexaedro del espacio de exploración aquí es el espacio donde se ubican los puntos aleatorios.

Dibuja el punto de inicio y el punto final:

%% 绘制起点和终点
hold on;
scatter3(source(1),source(2),source(3),"filled","g");
scatter3(goal(1),goal(2),goal(3),"filled","b");

Bucle principal:
failedAttempts es el número máximo de errores, pathFound es si se encuentra la ruta planificada y RRTree es el árbol de expansión.

tic;  % tic-toc: Functions for Elapsed Time
RRTree = double([source -1]);
failedAttempts = 0;
pathFound = false;

%% 循环
while failedAttempts <= maxFailedAttempts  % loop to grow RRTs
    %% chooses a random configuration
    if rand < 0.5
        sample = rand(1,3) .* searchSize;   % random sample
    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));  %单位化
    newPoint = closestNode + stepsize * movingVec;
    if ~checkPath3(closestNode, newPoint, circleCenter,r) % 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

Trayectoria de planificación de retroceso:

%% 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)
for i = 1:length(circleCenter(:,1))
    mesh(r(i)*x+circleCenter(i,1),r(i)*y+circleCenter(i,2),r(i)*z+circleCenter(i,3));hold on;
end
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','r');

Ejecutar (los códigos de las tres funciones de detección de colisiones se dan más adelante):

GIF:

línea de comando para imprimir el tiempo y la longitud de la ruta:

Código de función de detección de colisiones

%% checkPath3.m	
function feasible=checkPath3(n,newPos,circleCenter,r)
feasible=true;
movingVec = [newPos(1)-n(1),newPos(2)-n(2),newPos(3)-n(3)];
movingVec = movingVec/sqrt(sum(movingVec.^2)); %单位化
for R=0:0.5:sqrt(sum((n-newPos).^2))
    posCheck=n + R .* movingVec;
    if ~(feasiblePoint3(ceil(posCheck),circleCenter,r) && feasiblePoint3(floor(posCheck),circleCenter,r))
        feasible=false;break;
    end
end
if ~feasiblePoint3(newPos,circleCenter,r), feasible=false; end
end

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

%% feasiblePoint3.m
function feasible=feasiblePoint3(point,circleCenter,r)
feasible=true;
% check if collission-free spot and inside maps
for row = 1:length(circleCenter(:,1))
    if sqrt(sum((circleCenter(row,:)-point).^2)) <= r(row)
        feasible = false;break;
    end
end
end

Complemento de la planificación bidimensional

Utilice visio para dibujar varios mapas de obstáculos, binarizarlos y ejecutarlos con el código de blog recomendado:

Se puede ver que la trayectoria obtenida por el algoritmo RRT no es lo suficientemente suave y se puede mejorar. Se utiliza la técnica de ajuste de polinomios, y no voy a entrar en ella aquí. Los interesados ​​pueden discutir con el autor.