序文
グローバルパスプランニングアルゴリズムの高速拡張ランダムツリーRRTパスプランニングアルゴリズムは、状態制約のある非線形システムの開ループ軌道を生成するための手法です。この方法は確率論的に完全で最適ではありませんが、パスプランニング方法が提案されています。新しいアイデアは、他のアルゴリズムと比較して、障害や微分制約（不完全で動的）の問題を簡単に処理でき、これに基づいて改善してより良い結果を得ることができます。
1. RRTの原則と手順
1.障害物のエッジ座標を決定し、それを行列の形で時計回りに保存して、開始点と終了点の座標を決定します
このプログラムでは、この機能を実現するために拡張機能が使用されていますが、これは前処理であるため、一時的には説明しません。
2.新しいノードをループで取得し、新しいノードが望ましいかどうかを判断し、望ましい場合はノードツリーに追加します（複数のステップが含まれ、分割して続行します）。
2.1ループ内の新しいノードを取得し、MATLABでrand関数を使用してランダムに方向を生成し、開始点から新しいノードを選択して生成の方向に沿って拡大します
このプログラムは、サンプル関数を使用して生成の方向を生成し、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新しいノードと最も近いノードの間の線が障害物を通過しているかどうか、ノードツリー上のポイントからの距離がしきい値よりも小さいかどうか（新しいノードがノードツリー内のポイントに近づきすぎないようにするため）を確認し、通過しているか、またはこのノードよりも短い場合は削除します（この記事による）中央の障害物は頂点の形で格納されているため、新しいノードと最も近いノードの間の接続に各障害物のエッジとの交差があるかどうかが判断され、交差がある場合は削除されます）
このプログラムではジャッジ機能を使用して判定していますが、サブ機能コードが多すぎるため記事に掲載されていません
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')
第三に、最終結果
4、RRT、RRTconnect、RRTstarの結果
RRT
RRTconnect
RRTstar
特定のビデオ：https : //www.bilibili.com/video/BV1oA411Y7p9/
完全なRRTコードについては、ダウンロードを参照してください：https ://download.csdn.net/download/weixin_41971010/12698239
RRTconnectコードはダウンロードを参照してください：https ://download.csdn.net/download/weixin_41971010/12698242
RRTstarコードはダウンロードを参照してください：https ://download.csdn.net/download/weixin_41971010/12706890
expand.mのダウンロードを参照してください：https ://download.csdn.net/download/weixin_41971010/12681068