closestPoint

参照パス上のグローバル点への最近傍点の検索

ページ内をすべて折りたたむ

構文

pathPoints = closestPoint(refPath,points)
[pathPoints,inWindow] = closestPoint(refPath,points)
[_] = closestPoint(refPath,points,searchWindow)

説明

pathPoints = closestPoint(refPath,points) は、指定した各位置 (x,y)、points への参照パス上の最近傍点を検索します。

例

[pathPoints,inWindow] = closestPoint(refPath,points) は、points 内の対応する xy 座標の各点が検索ウィンドウ内に投影されているかどうかを示す logical ベクトル inWindow をオプションで返します。

[_] = closestPoint(refPath,points,searchWindow) は、最近傍点の検索に使用するパスの区間を定義する非減少行ベクトル searchWindow をオプションで受け入れます。

例

例

すべて折りたたむ

ライブ スクリプトを開く

一連のウェイポイントから参照パスを生成します。

waypoints = [0 0; 50 20; 100 0; 150 10];
refPath = referencePathFrenet(waypoints);

この参照パスから trajectoryGeneratorFrenet オブジェクトを作成します。

connector = trajectoryGeneratorFrenet(refPath);

パスの原点と、パスに沿って 30 メートルのポイントとの間の 5 秒の軌跡を、フレネ状態として生成します。

initCartState = refPath.SegmentParameters(1,:);
initFrenetState = global2frenet(refPath,initCartState);
termFrenetState = initFrenetState + [30 zeros(1,5)];
frenetTraj = connect(connector,initFrenetState,termFrenetState,5);

この軌跡をグローバル状態に変換します。

globalTraj = frenet2global(refPath,frenetTraj.Trajectory);

この参照パスと軌跡を表示します。

show(refPath);
axis equal
hold on
plot(globalTraj(:,1),globalTraj(:,2),'b')

グローバル点を指定し、参照パス上の最近傍点を求めます。

globalPoints = waypoints(2:end,:) + [20 -50];
nearestPathPoint = closestPoint(refPath,globalPoints);

グローバル点と、参照パス上の最近傍点を表示します。

plot(globalPoints(:,1),globalPoints(:,2),'r*','MarkerSize',10)
plot(nearestPathPoint(:,1),nearestPathPoint(:,2),'b*','MarkerSize',10)

参照パスに沿った最初の 2 つの最近傍点の間の弧の長さを内挿します。

arclengths = linspace(nearestPathPoint(1,6),nearestPathPoint(2,6),10);
pathStates = interpolate(refPath,arclengths);

内挿されたパス上の点を表示します。

plot(pathStates(:,1),pathStates(:,2),'g')
legend(["Waypoints","Reference Path","Trajectory to 30m",...
        "Global Points","Closest Points","Interpolated Path Points"])

Figure contains an axes object. The axes object contains 6 objects of type line. One or more of the lines displays its values using only markers These objects represent Waypoints, Reference Path, Trajectory to 30m, Global Points, Closest Points, Interpolated Path Points.

ライブ スクリプトを開く

自己交差する参照パスを作成します。

refPath = referencePathFrenet([0 100 -pi/4; ...
                               50 50 -pi/4;...
                               75 50  pi/2; ...
                               50 50 -3*pi/4; ...
                               0  0  -3*pi/4]);

参照パスを表示します。

figure
show(refPath); 
title("Closest Points Around Intersection")
xlim([0 125])
ylim([0 125])
hold on

Figure contains an axes object. The axes object with title Closest Points Around Intersection contains 2 objects of type line. One or more of the lines displays its values using only markers

交差が発生する弧の長さを求めます。

sIntersection = refPath.SegmentParameters(2,end);

交差の直前と直後にあるフレネ状態を生成します。

f0 = [sIntersection-20 5 0 10 0 0]; % [S dS ddS L Lp Lpp]
f1 = [sIntersection+20 5 0 -5 0 0]; % [S dS ddS L Lp Lpp]

一定の縦方向速度で移動する時間を計算します。

T = (f1(1)-f0(1))/f1(2);

この参照パスを使用して軌跡ジェネレーターを作成します。

generator = trajectoryGeneratorFrenet(refPath);

点間の軌跡を生成します。

[fTraj,gTraj] = connect(generator,f0,f1,T);
pts = gTraj.Trajectory;

プロット補助関数を定義します。

mergeFcn = @(v1,v2)reshape([v1 v2 nan(size(v1,1),size(v2,2))]',[],1);
plotFcn = @(L1,L2,linespec)plot(mergeFcn(L1(:,1),L2(:,1:min(1,size(L2,2)))),mergeFcn(L1(:,2),L2(:,2:min(2,size(L2,2)))),linespec{:});
plotInterval = @(bounds,nPt,linespec)plotFcn(interpolate(refPath,linspace(bounds(1),bounds(2),nPt)'),[],linespec);

軌跡をプロットします。

plotFcn(pts,[],{"k.-"});

Figure contains an axes object. The axes object with title Closest Points Around Intersection contains 3 objects of type line. One or more of the lines displays its values using only markers

各グローバル状態へのパス上の最近傍点を計算します。

closestPts = closestPoint(refPath,pts);

最近傍点ベクトルをプロットします。

plotFcn(pts,closestPts,{"b"});

Figure contains an axes object. The axes object with title Closest Points Around Intersection contains 4 objects of type line. One or more of the lines displays its values using only markers

最近傍点を検索するウィンドウを定義します。

buffWindow = [f0(1)-5 f1(1)+5];
plotInterval(buffWindow,100,{"Color",[.5 .5 .5],"LineWidth",5});

Figure contains an axes object. The axes object with title Closest Points Around Intersection contains 5 objects of type line. One or more of the lines displays its values using only markers

ウィンドウ内の最近傍点を求めます。

closestPtsInWindow = closestPoint(refPath,pts,buffWindow);

ウィンドウを適用した結果を表示します。

plotFcn(pts,closestPtsInWindow,{"g","LineWidth",3});

Figure contains an axes object. The axes object with title Closest Points Around Intersection contains 6 objects of type line. One or more of the lines displays its values using only markers

小さすぎるウィンドウを使用して最近傍点を求めます。

smallWindow = [f0(1)+5 f1(1)-5];
[closestPtsSmall,inWindow] = closestPoint(refPath,pts,smallWindow);

小さいウィンドウの結果を重ね合わせます。

plotInterval(smallWindow,100,{"m","LineWidth",3});
plotFcn(pts(inWindow,:),closestPtsSmall(inWindow,:),{"Color",[.5 1 .5]});
plotFcn(pts(~inWindow,:),closestPtsSmall(~inWindow,:),{"r"});
legend({"Waypoints","ReferencePath","Trajectory","ClosestPoints",...
    "BuffWindow","ClosestInsideBuffWindow","SmallWindow",...
    "ClosestInsideSmallWindow","ClosestOutsideSmallWindow"});

Figure contains an axes object. The axes object with title Closest Points Around Intersection contains 9 objects of type line. One or more of the lines displays its values using only markers These objects represent Waypoints, ReferencePath, Trajectory, ClosestPoints, BuffWindow, ClosestInsideBuffWindow, SmallWindow, ClosestInsideSmallWindow, ClosestOutsideSmallWindow.

入力引数

すべて折りたたむ

参照パス。referencePathFrenet オブジェクトとして指定します。

グローバル点。行が [x y] の形式である P 行 2 列の数値行列として指定します。P は点の数です。位置はメートル単位です。

最近傍点を決定するパス上の検索ウィンドウ。弧の長さの 2 要素の行ベクトルとして指定します。

出力引数

すべて折りたたむ

参照パス上の最近傍点。行の形式が [x y theta kappa dkappa s] である N 行 6 列の数値行列として返されます。ここで次のようになっています。

  • x y および theta — グローバル座標で表された SE(2) の状態 (x と y はメートル単位、theta はラジアン単位)

  • kappa — 曲率 (半径の逆数、m-1 単位)

  • dkappa — 弧の長さに対する曲率の微分 (m-2 単位)

  • s — 弧の長さ (パスの原点からパスに沿った距離、メートル単位)

N は参照パスに沿ってサンプリングされた点の数です。

points 内の対応する xy 座標に最も近い各点が検索ウィンドウ内に投影されているかどうかを示します。N 要素の logical 列ベクトルとして返されます。N は points 内の点の数です。検索ウィンドウ内に点が投影されている場合は true、ウィンドウの最後にある場合は false です。

拡張機能

すべて展開する

C/C++ コード生成
MATLAB® Coder™ を使用して C および C++ コードを生成します。

バージョン履歴

R2020b で導入

参考

オブジェクト

関数

トピック