Planificación de rutas RRT, RRTconnect y RRTstar código matlab simple

Prefacio

El algoritmo de planificación de ruta RRT de árbol aleatorio de rápida expansión en el algoritmo de planificación de ruta global es una técnica para generar trayectorias de bucle abierto para un sistema no lineal con restricciones de estado. Este método es probabilísticamente completo y no óptimo, pero se propone un método de planificación de ruta. La nueva idea puede manejar fácilmente obstáculos y restricciones diferenciales (incompletas y dinámicas) en comparación con otros algoritmos, y puede mejorarse sobre esta base para obtener mejores resultados.

1. Principio y pasos del RRT

1. Determine las coordenadas del borde del obstáculo y guárdelo en el sentido de las agujas del reloj en forma de matriz para determinar las coordenadas de los puntos inicial y final.

En este programa, la función de expansión se utiliza para realizar esta función, debido a que es un preprocesamiento, no se describirá temporalmente.

2. Tome el nuevo nodo en un bucle y juzgue si el nuevo nodo es deseable, y si es deseable, agréguelo al árbol de nodos (están involucrados varios pasos, divida y continúe)

2.1 Tome nuevos nodos en un bucle, use la función rand en matlab para generar direcciones aleatoriamente y seleccione nuevos nodos desde el punto de partida y crezca a lo largo de la dirección de generación

Este programa usa la función de muestra para generar la dirección de generación y usa la función cerca para determinar el nodo más cercano en el árbol de nodos y determinar el nodo recién generado qnew

function [M]=sample(t,sizemap)
if rand<0.5
    M=rand(1,2)*sizemap;
else
    M=t;
end

function [q,qnear,index]=near(qrand,q,stepsize)
for ii=1:size(q,1)
    d(ii)=sqrt((qrand(1)-q(ii,1))^2+(qrand(2)-q(ii,2))^2);
end
[mn,index]=min(d);
mn;
qnearest(1)=q(index,1);
qnearest(2)=q(index,2);
qvector=qrand-qnearest;
qvectornew(1)=qvector(1)*sqrt(stepsize)/sqrt(qvector(1)^2+qvector(2)^2);
qvectornew(2)=qvector(2)*sqrt(stepsize)/sqrt(qvector(1)^2+qvector(2)^2);
qnew(1)=qnearest(1)+qvectornew(1);
qnew(2)=qnearest(2)+qvectornew(2);
qnear=qnearest;
q=[q;qnew];

2.2 Determine si la línea entre el nuevo nodo y el nodo más cercano atraviesa el obstáculo y si la distancia desde el punto en el árbol de nodos es menor que el umbral (para evitar que el nuevo nodo esté demasiado cerca del punto en el árbol de nodos) y elimínelo si pasa a través o es menor que (debido a este artículo Los obstáculos del medio se almacenan en forma de vértices, por lo que se juzga si la conexión entre el nuevo nodo y el nodo más cercano tiene una intersección con el borde de cada obstáculo. Si hay una intersección, elimínela)

La función de juez se utiliza para juzgar en este programa. Debido a que hay demasiado código de subfunción, no se publica en el artículo.

2.3 Agregue el nuevo nodo al número de nodos y almacene la conexión entre el nuevo nodo y el nodo más cercano en la matriz de la cadena para facilitar la búsqueda inversa

3. Encuentra la ruta final desde el punto final hasta el punto de inicio en orden inverso

Este programa implementa esta función con el programa principal

En segundo lugar, la función principal completa

clc;
close all;
clear all;
syms qx qy
w=1/2;                         %车宽是1m
s=[1,1];                       %规定起点
t=[49,49];                     %规定终点
sizemap=50;                    %地图规模使50*50的
deltat=20000;                  %搜索次数上限
stepsize=2;                    %搜索步长
lentgh=stepsize/2;             %设定最终搜索阈值为步长的一半
XY=xlsread('coordinate.xlsx');
XYC=expand(XY,w);              %expand函数膨胀障碍物！
mm=size(XY,2)/2;               %提取障碍物数量
plot(s(1),s(2),'*r')           %标注起点
text(s(1),s(2),num2str('起点'))
hold on
plot(t(1),t(2),'*r')           %标注终点
text(t(1),t(2),num2str('终点'))
hold on
qj=[];
ij=1;
q=s;                           %将起点s放入节点树
step=deltat/100;               %进度条进度步数
hwait=waitbar(0,'请等待>>>>>>>>');%设置进度条
set(hwait,'Position',[425 120 300 100])%设置进度条位置
for i=1:deltat
    qrand=sample(t,sizemap);   %sample函数随机生成节点！
    [q,qnear,mn]=near(qrand,q,stepsize);  %near函数求出新节点，原节点树中节点最近的点为起点，搜索步长为长度！
    [q,ikk]=judge(q,XYC,qnear,mm,lentgh);      %judge函数判断新节点是否穿过障碍物并且是否与节点树上的点距离小于阈值，穿过或小于就去掉！
    if ikk==0                  %ikk为0说明节点未被删除，需要画出新路线
        ij=ij+1;
        qj=[qj;[mn,ij]];
        plot([qnear(1),q(end,1)],[qnear(2),q(end,2)],'g');
        hold on
    end
    if sqrt((q(end,1)-t(1))^2+(q(end,2)-t(2))^2)<=stepsize %如果新节点与终点的距离小于阈值lentgh结束
        plot([q(end,1),t(1)],[q(end,2),t(2)],'g');
        break
    else
    end
    if deltat-i<=1          %进度条设置
        waitbar(i/deltat,hwait,'即将完成');
        set(hwait,'Position',[425 120 300 100])
    else
        PerStr=fix(i/step);
        str=['正在运行中',num2str(PerStr),'%'];
        waitbar(i/deltat,hwait,str);
        set(hwait,'Position',[425 120 300 100])
    end
end
q=[q;t];                    %所有找到的节点
qj=[qj;[ij,ij+1]];          %qj是所有连接的节点的连线
for i=size(qj,1):-1:1       %倒推节点
    if i==size(qj,1)        %最后一对节点连线之间加入最终结果
        qp=qj(i,:);
    elseif qj(i,2)==qp(1)   %节点对首位相同即相邻，相邻的节点放入结果
        qp=[qj(i,1),qp];        %qp为最终路线的节点对
    end
end
for i=1:(size(qp,2)-1)      %将所有节点对依次连线画图
    plot([q(qp(i),1),q(qp(i+1),1)],[q(qp(i),2),q(qp(i+1),2)],'r')
    hold on
end
Q=[];                       %Q是最终路线坐标结果
for i=1:size(qp,2)
    Q=[Q;q(qp(i),:)];           %将坐标依次放入Q中
end
str=['已完成',num2str(100),'%'];%进度条设置
waitbar(deltat/deltat,hwait,str);
set(hwait,'Position',[425 120 300 100])
sprintf('搜索完成\n路线为:%s','Q')