RRTの2次元軌道計画

RRTの2次元軌道計画とMATLAB実装リファレンスブログについて：
RRT

RRTアルゴリズムの簡単な分析

ルートノードを生成し、エンドノードを指定します。
空間内の点ランダムに見つけます $r$ （50％ランダムポイント、50％をターゲットポイント）、判断します軌道ツリー内のどのノードが $r$ 間のユークリッド距離が最小であり、このノードを $node_ {best}$ として示します $n o d e_{B E S T}$ 。
$node_に{最高}$ $r$ 拡張ステップサイズステップサイズ $s t e p S i z e$ units（vectorによって実装されます）。
線分 $node_ {best}-> rを決定します$ 障害物に触れるかどうか。はいの場合、あきらめます $r$ 、2に戻ります。
裁判官 $r$ がターゲットポイントに近いかどうか、近い場合はループを終了し、そうでない場合はになります $rが$ 軌道ツリーに追加され、2に戻ります。

アルゴリズムはエラーの最大数を導入することもできます。反復プロセスでエラーの最大数を超えても衝突しない新しい子ノードが見つからない場合、ループは終了し、エラーは「適切な軌道を見つけることができません」。

RRTの長所と短所

状態空間内のサンプリングポイントで衝突検出を実行することにより、空間のモデリングが回避され、高次元空間と複雑な制約の経路計画問題を効果的に解決できます。この方法の特徴は、高次元空間を迅速かつ効果的に検索できることです。状態空間内のランダムなサンプリングポイントを介して、検索は空白領域に向けられ、開始点からターゲットまでの計画されたパスが検索されます。ポイント。環境および動的環境でのパスプランニング。PRMと同様に、この方法は確率的に完全であり、最適ではありません。

3D軌道計画のMATLABシミュレーション

上記のブログを2次元で参照してください。
衝突の検出と描画を容易にするために、3次元の障害物をボールとして設定しました。

%% 绘制障碍物(以球为例，主要是方便计算)
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

circleCenterとr行列を球の中心と対応する半径で埋めます。

与えられたシミュレーションパラメータ：
目標は目標であり、
ソースは開始点です

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

ここでの探査空間六面体は、ランダムな点が配置されている空間です。

開始点と終了点を描画します。

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

ここに画像の説明を挿入
メインループ：
failedAttemptsはエラーの最大数、pathFoundは計画されたパスが見つかったかどうか、RRTreeはスパニングツリーです。

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

計画軌道のバックトラック：

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

実行（3つの衝突検出機能のコードは後で説明します）：
ここに画像の説明を挿入
GIF：

時間とパスの長さを出力するコマンドライン：

衝突検出機能コード

%% 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

二次元計画の補足

visioを使用して、いくつかの障害物マップを描画し、それらを2値化し、推奨されるブログコードで実行します。
ここに画像の説明を挿入

ここに画像の説明を挿入
RRTアルゴリズムによって得られた軌道は十分に滑らかではなく、改善できることがわかります。多項式フィッティング手法が使用されているため、ここでは説明しません。興味のある方は、著者と話し合うことができます。

3D軌道計画におけるRRTアルゴリズムのMATLABシミュレーション