このページの内容は最新ではありません。最新版の英語を参照するには、ここをクリックします。

closestPoint

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

R2020b 以降

構文

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"])

パスの交差での最近傍点の取得

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

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

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

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.-"});

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

closestPts = closestPoint(refPath,pts);

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

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

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

buffWindow = [f0(1)-5 f1(1)+5];

plotInterval(buffWindow,100,{"Color",[.5 .5 .5],"LineWidth",5});

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

closestPtsInWindow = closestPoint(refPath,pts,buffWindow);

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

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

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

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"});

入力引数

refPath — 参照パス

referencePathFrenet オブジェクト

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

points — グローバル点

P 行 2 列の数値行列

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

searchWindow — 検索ウィンドウ

2 要素の行ベクトル

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

出力引数

pathPoints — 参照パス上の最近傍点

N 行 6 列の数値行列

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

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

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

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

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

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

inWindow — 検索ウィンドウ内に点があるかどうかを示すインジケーター

N 要素の logical 列ベクトル

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

拡張機能

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

バージョン履歴

R2020b で導入

参考

オブジェクト

• referencePathFrenet | trajectoryGeneratorFrenet | navPath

関数

• frenet2global | global2frenet | closestPointsToSequence | closestProjections | curvature | changeInCurvature | tangentAngle | interpolate | position | show

トピック

• Highway Trajectory Planning Using Frenet Reference Path