RRT、RRTconnect和RRTstar路径规划简易matlab代码

前言

全局路径规划算法中的快速扩展随机树RRT路径规划算法是一种具有状态约束的非线性系统生成开环轨迹的技术，该方法是概率完备且不最优的，但是提出了一种路径规划的新思路，相比于其他算法可以轻松处理障碍物和差分约束（非完整和动力学）的问题，可以在此基础上进行改进，得到较优的结果。

一、RRT原理及步骤

1.确定障碍物边缘坐标以矩阵形式顺时针储存，确定起点和终点的坐标

本程序中以expand函数实现该功能，由于是前处理暂不叙述

2.循环取新节点并判断新节点是否可取，若可取加入节点树（涉及到多个步骤，拆分进行）

2.1循环取新节点，采用matlab中rand函数随机生成方向，从起点开始沿生成方向生长选择新节点

本程序采用sample函数生成生成方向，以near函数确定节点树中最近节点并确定新生成的节点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判断新节点与最近节点的连线是否穿过障碍物并且是否与节点树上的点距离小于阈值（防止新节点与节点树中的点距离过近），穿过或小于就去掉（由于本文中障碍物以顶点形式储存，因此判断新节点与最近节点的连线与各个障碍物的边是否有交点，有交点则去掉）

本程序中采用judge函数进行判断，由于子函数代码过多，不在文章中贴出来了

2.3将新节点添加到节点数中，并将新节点与最近节点的连线存入链矩阵中，方便逆序查找

3.倒序从终点查找至起点得到最终的路线

本程序以主程序实现该功能

二、完整主函数

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')