Una breve introducción al algoritmo RRT.
1. Definición del algoritmo RRT
El algoritmo RRT (árbol aleatorio de exploración rápida) es un algoritmo de planificación de rutas basado en muestreo, que a menudo se utiliza en la planificación de rutas de robots móviles y es adecuado para resolver problemas de planificación de rutas en espacios de alta dimensión y restricciones complejas. La idea básica es buscar el punto objetivo a través de un tamaño de paso generando puntos aleatorios, evitando obstáculos de manera efectiva, evitando que el camino caiga en un mínimo local y una convergencia rápida. En este artículo, matlab implementa el algoritmo RRT para resolver el problema de planificación de ruta del plano bidimensional.
2. Pasos básicos del algoritmo RRT
1) Determinar el punto de partida y el punto final;
2) Generar aleatoriamente nuevos puntos r en el espacio (50% son puntos aleatorios, 50% son puntos objetivo, el propósito es mejorar la orientación de RRT para la generación de puntos objetivo);
3) Determine qué nodo tiene la distancia euclidiana más pequeña entre el punto r y el árbol de trayectoria, y registre este punto como el nodo padre más cercano closeNode;
4) Genere un nuevo nodo secundario newNode de acuerdo con el tamaño del paso stepSize a lo largo de la dirección del vector de crecimiento (el nodo principal más cercano closeNode apunta a la dirección del punto aleatorio r);
5) Detección de colisiones: detecta si la conexión de closeNode a newNode chocará con obstáculos. En caso afirmativo, regrese al paso 2 para regenerar puntos aleatorios; en caso contrario, agregue newNode al árbol de trayectoria.
6) Detectar si está cerca de la portería. En caso afirmativo, finalice la búsqueda; en caso contrario, regrese al paso 2 para continuar la búsqueda.
Efecto de simulación de Matlab de trayectoria tridimensional:
Obstáculos Sólo colocamos obstáculos esféricos, pero también se pueden colocar en forma de cubos, cilindros, etc.
Efecto de implementación 1:
Izquierda: árbol aleatorio RRT Derecha: árbol aleatorio optimizado por algoritmo codicioso
Antes de la optimización, el árbol RRT tenía 80 nodos y el camino no era fluido. Después de la optimización, el árbol aleatorio tiene solo tres puntos y la ruta consta de solo dos líneas rectas. Por supuesto, puede continuar allanando el camino y este artículo no continúa realizando estas tareas.
Efecto de implementación 2:
El siguiente es el código de implementación de Matlab del algoritmo RRT:
función principal:
%created by MW
%date: 2022/5/25
%func: RRT avoiding obstacle
clc;
clear;
%% 创建并绘制障碍物
circleCenter = [100 200 100;
200 700 100;
200 500 500;
700 700 300;
900 200 100];
radius = [100;100;200;200;300];
%绘制球形障碍物
figure(1);
[x, y, z] = sphere; %创建一个坐标在原点，半径为1的标准圆，用于绘制自定义的圆
for i = 1: length(radius)
mesh(radius(i)*x + circleCenter(i,1), radius(i)*y + circleCenter(i,2), radius(i)*z + circleCenter(i,3));
hold on;
end
axis equal;
%% 创建初始位置和目标位置并绘制
start = [0 0 0];
goal = [700 800 1000];
hold on;
scatter3(start(1),start(2),start(3),'filled','g');
scatter3(goal(1),goal(2),goal(3),'filled','b');
%% 图片标注
text(start(1),start(2),start(3),'起点');
text(goal(1),goal(2),goal(3),'终点');
view(3);
grid on;
axis equal;
axis([0 1000 0 1000 0 1000]);
xlabel('x');
ylabel('y');
zlabel('z');
%% RRT方法生成避障轨迹
path = RRT(start,goal,radius,circleCenter);
%% 贪心算法优化RRT轨迹
newPath = GreedyOptimize(path,radius,circleCenter);
figure(2);
%绘制球形障碍物
[x, y, z] = sphere; %创建一个坐标在原点，半径为1的标准圆，用于绘制自定义的圆
for i = 1: length(radius)
mesh(radius(i)*x + circleCenter(i,1), radius(i)*y + circleCenter(i,2), radius(i)*z + circleCenter(i,3));
hold on;
end
axis equal;
%绘制原RRT树
plot3(path(:,1), path(:,2), path(:,3),'LineWidth',1,'Color','r');
%绘制优化后的RRT树
for k1 =1: length(newPath)
point = newPath(k1,:);
scatter3(point(1),point(2),point(3),'filled','k');
end
plot3(newPath(:,1), newPath(:,2), newPath(:,3),'LineWidth',2,'Color','y');
%图片标注
text(start(1),start(2),start(3),'起点');
text(goal(1),goal(2),goal(3),'终点');
view(3);
grid on;
axis equal;
axis([0 1000 0 1000 0 1000]);
xlabel('x');
ylabel('y');
zlabel('z');
Algoritmo RRT:
function path = RRT(start,goal,radius,circleCenter)
%% 定义RRT参数
stepSize = 20; %步长
maxIterTimes = 5000; %最大迭代步数
iterTime = 0; %当前迭代次数
threshold = 20; %阈值
searchSize = [1000 1000 1000]; %空间尺寸
RRTree = double([start -1]); %创建RRT树,共4列。前3列为节点坐标，第4列为当前节点的父节点的索引。初始点的索引为-1
%多个点到某一点欧式距离计算方法
calcDis = @(a,b) sqrt((b(:,1)-a(1,1)).^2 + (b(:,2)-a(1,2)).^2 + (b(:,3)-a(1,3)).^2);
%% 寻找RRT路径
tic % tic-toc函数，用于计时，记录完成整个RRT树的运行时间
pathFound = false; %标志物，记录是否正确找到避障路径
while iterTime <= maxIterTimes
iterTime = iterTime +1;
%step1 - 生成随机点
%为了提高RRT扩展的导向性，以50%的概率在空间中随机生成生成新的随机点，50%的概率以目标点为新的随机点
if rand < 0.5
sample = rand(1,3) .* searchSize + start;
else
sample = goal;
end
%step2 - 寻找树上最近父节点
[val,nearIndex] = min(calcDis(sample, RRTree(:,1:3)),[],1); %计算树上每个节点到随机点的欧氏距离，并返回最短距离的值和index
closestNode = RRTree(nearIndex,1:3);
%step3 - 沿生长向量方向按照步长生成新的子节点
growVec = sample - closestNode;
growVec = growVec/sqrt(sum(growVec.^2));
newPoint = closestNode + growVec*stepSize;
%step4 - 碰撞检测
feasible = collisionDetec(newPoint,closestNode,radius,circleCenter);
if ~feasible
continue; %如果发生碰撞，则重新寻找随机点
end
%为树添加新节点
RRTree = [RRTree;
newPoint nearIndex];
plot3([closestNode(1) newPoint(1)],[closestNode(2) newPoint(2)],[closestNode(3) newPoint(3)],'LineWidth',1,'Color','b');
pause(0.01);
%检测是否到达goal附近
if sqrt(sum((newPoint - goal).^2)) <= threshold
pathFound = true;
break; %如果节点已到达goal附近，则结束搜寻
end
end
%搜索结束后如果搜索失败，显示错误信息
if ~pathFound
disp('no path found. maximum attempts reached');
end
toc
%% 绘制回溯路径
path = goal;
lastNode = nearIndex; %父节点索引，这里的nearIndex是goal的父节点索引
while lastNode >= 0
path =[RRTree(lastNode,1:3); path]; %将当前节点的上一个节点添加进回溯路径中
lastNode = RRTree(lastNode, 4); %更新父节点索引
end
plot3(path(:,1), path(:,2), path(:,3),'LineWidth',1,'Color','r');
end
Algoritmo de detección de colisiones:
Detección de segmento de línea:
%新的生长向量是否发生碰撞
function feasible = collisionDetec(newPoint,closestNode,radius,circleCenter)
feasible = true;
checkVec = newPoint - closestNode; %检测向量
%将检测向量以0.5为步长等比均分为无数个检测点，检测每个检测点是否与球发生碰撞
for i = 0:0.5:sqrt(sum(checkVec.^2))
checkPoint = closestNode + i.*(checkVec/sqrt(sum(checkVec.^2))); %生成检测点
checkPointFeasible = pointCollisionCheck(checkPoint,radius,circleCenter);
if ~checkPointFeasible
feasible = false;
break;
end
end
end
Detección de puntos:
%监测点是否发生碰撞
function pointFeasible = pointCollisionCheck(checkPoint,radius,circleCenter)
pointFeasible = true;
for s = 1:length(radius)
if sqrt(sum((checkPoint - circleCenter(s,:)).^2)) <= radius(s)
pointFeasible = false;
break;
end
end
end
Una breve introducción al algoritmo codicioso.
1. Definición del algoritmo codicioso:
Un algoritmo codicioso significa que al resolver un problema, siempre toma la mejor decisión en el momento. Es decir, sin considerar la optimización general, lo que hace es solo una solución óptima local en cierto sentido.
El algoritmo codicioso no tiene un marco algorítmico fijo y la clave para el diseño del algoritmo es la elección de la estrategia codiciosa. Cabe señalar que el algoritmo codicioso no puede obtener la solución óptima general para todos los problemas, y la estrategia codiciosa seleccionada no debe tener efectos secundarios, es decir, el proceso después de un determinado estado no afectará el estado anterior, sino que solo estará relacionado con el actual. estado.
2. Pasos básicos del algoritmo codicioso:
Paso 1: Partiendo de una solución inicial;
Paso 2: Usando un proceso iterativo, cuando sea posible avanzar hacia la meta, obtener una parte de la solución de acuerdo con la estrategia óptima local para reducir la escala del problema; Paso 3 : Combina
todas las soluciones.
El algoritmo codicioso bajo este problema:
1) Conecte el punto objetivo y el punto inicial.
2) Detectar si se ha producido una colisión.
3) En caso afirmativo, significa que no se puede omitir el punto medio desde el punto inicial hasta este punto, vuelva a rastrear el punto objetivo hasta el nodo principal del nivel anterior (el nodo principal del nivel anterior se actualiza al nuevo punto objetivo ), y regrese al paso 1; de lo contrario, indica que todos los nodos desde el punto inicial hasta este punto se pueden omitir, agregue este punto a un nuevo árbol aleatorio y actualice este punto a un nuevo punto inicial, restablezca el punto objetivo a el último nodo y regrese al paso 1.
4) Verifique hasta que se actualice el inicio hasta el final de la meta.
function newPath = GreedyOptimize(path,radius,circleCenter)
startIndex = 1;
goalIndex = length(path(:,1));
detectTimes = length(path(:,1)) - 1; %检测次数
newPath = [path(startIndex,:)]; %添加初始位置
while detectTimes >0;
detectTimes = detectTimes-1;
start = path(startIndex,:);
goal = path(goalIndex,:);
%碰撞检测
feasible = collisionDetec(goal,start,radius,circleCenter);
if ~feasible
goalIndex = goalIndex - 1; %若碰撞，index减1，继续向上探索父节点
continue;
else
newPath = [newPath; goal]; %若未发生碰撞，表示找到一个最优节点，则从该节点往上的所有父节点均可省略，将该节点添加至路径中
detectTimes = length(path(:,1)) - goalIndex; %检测次数更位
startIndex = goalIndex; %将当前节点索引更新为新的树的起点
goalIndex = length(path(:,1)); %将终点索引复位
end
end
newPath = [newPath; path(end,:)]; %添加目标位置
end