Main Content

referencePathFrenet

滑らかな参照パスのウェイポイントへの当てはめ

R2020b 以降

説明

referencePathFrenet オブジェクトは、連続した滑らかな区分的曲線を [x y] または [x y theta] として与えられる一連のウェイポイントに当てはめます。当てはめた後の曲線に沿ったパス上の点は [x y theta kappa dkappa s] として表されます。ここで、次のようになります。

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

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

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

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

Depiction of global Frenet coordinates

このオブジェクトを使用して、グローバル座標系とフレネ座標系の間で軌跡を変換し、弧の長さに基づいて状態をパスに沿って内挿し、グローバル状態からパス上の最近傍点をクエリします。

オブジェクトはフレネ状態を [S dS ddS L dL ddL] の形式のベクトルとして表します。ここで、S は弧の長さ、L は参照パスの方向からの垂直な偏差です。S の微分は時間を基準とします。L の微分は弧の長さ S を基準とします。

Depiction of Frenet states relative to a reference path

作成

説明

refPathObj = referencePathFrenet(waypoints) は、一連の連続した区分的曲線を指定されたウェイポイントに当てはめます。引数 waypointsWaypoints プロパティを設定します。

refPathObj = referencePathFrenet(___,Name,Value) は、前述の構文の入力引数に加え、1 つ以上の名前と値のペアの引数を使用してプロパティを設定します。DiscretizationDistance および MaxNumWaypoints の各プロパティを名前と値の引数として指定できます。

プロパティ

すべて展開する

パスに沿って事前にサンプリングした点。行が [x y] の形式である P 行 2 列の行列、または行が [x y theta] の形式である P 行 3 列の行列として指定します。x と y はメートル単位、theta はラジアン単位で指定します。P は事前にサンプリングされた点の数であり、2 以上でなければなりません。

データ型: single | double

この プロパティ は読み取り専用です。

パスに沿った弧の全長 (メートル単位)。

データ型: single | double

この プロパティ は読み取り専用です。

セグメントの始点のクロソイド パラメーター。N 行 6 列の行列として指定します。ここで、N はウェイポイントの合計数です。各行に、次に続くウェイポイントに接続するクロソイド セグメントのパラメーターが [x y theta kappa dkappa s] の形式で格納されます。ここで、次のようになります。

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

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

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

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

データ型: single | double

パス上の内挿点間の弧の長さ (メートル単位)。正のスカラーとして指定します。オブジェクトは、パス上の内挿点を使用して、変換関数 frenet2global および global2frenet のパフォーマンスを加速させます。離散距離が小さいほど精度は高まりますが、メモリや計算の効率が低下します。

例: refPath = referencePathFrenet(waypoints,'DiscretizationDistance',0.4)

データ型: single | double

パスで許可される最大ウェイポイント数。サイズ変更可能なパスの場合は Inf、静的な制限を適用する場合は正の整数として指定します。

MaxNumWaypointsInf に設定されている場合、コードを生成するには DynamicMemoryAllocation'on' に設定する必要があります。

例: refPath = referencePathFrenet(waypoints,'MaxNumWaypoints',10)

データ型: single | double

オブジェクト関数

closestPoint参照パス上のグローバル点への最近傍点の検索
closestPointsToSequenceProjects sequence of points onto path
closestProjectionsFind orthogonal projections between path tangent vector and query point
curvature弧の長さに対する曲率を返す
changeInCurvature弧の長さに対する曲率変化を返す
frenet2globalConvert Frenet states to global states
global2frenetConvert global states to Frenet states
interpolate参照パス上の指定された弧の長さを内挿
position弧の長さに対する xy 位置を返す
tangentAngle弧の長さに対する正接角度を返す
showDisplay reference path in figure
copyCopy reference path

すべて折りたたむ

フレネ座標を使用して参照パスの別の軌跡を生成します。軌跡に対して異なる初期状態と終了状態を指定します。生成された軌跡に基づいて状態を調整します。

一連のウェイポイントから参照パスを生成します。この参照パスから trajectoryGeneratorFrenet オブジェクトを作成します。

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

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

initState = [0 0 0 0 0 0];  % [S ds ddS L dL ddL]
termState = [30 0 0 0 0 0]; % [S ds ddS L dL ddL]
[~,trajGlobal] = connect(connector,initState,termState,5);

軌跡をグローバル座標で表示します。

show(refPath);
hold on
axis equal
plot(trajGlobal.Trajectory(:,1),trajGlobal.Trajectory(:,2),'b')
legend(["Waypoints","Reference Path","Trajectory to 30m"])

Figure contains an axes object. The axes object contains 3 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.

横方向の偏差を –3 m から 3 m までにして終了状態の行列を作成します。同じ弧の長さを 10 秒で進むが参照パスから横方向に逸脱する軌跡を生成します。新しい代替パスを表示します。

termStateDeviated = termState + ([-3:3]' * [0 0 0 1 0 0]);
[~,trajGlobal] = connect(connector,initState,termStateDeviated,10);

clf
show(refPath);
hold on
axis equal
for i = 1:length(trajGlobal)
    plot(trajGlobal(i).Trajectory(:,1),trajGlobal(i).Trajectory(:,2),'g')
end
legend(["Waypoints","Reference Path","Alternative Trajectories"])
hold off

Figure contains an axes object. The axes object contains 9 objects of type line. One or more of the lines displays its values using only markers These objects represent Waypoints, Reference Path, Alternative Trajectories.

新しい終了状態を指定して新しい軌跡を生成します。この軌跡は、縦方向の 10 m/s の速度を得るために後退運動を必要とするため望ましくありません。

newTermState = [5 10 0 5 0 0];
[~,newTrajGlobal] = connect(connector,initState,newTermState,3);

clf
show(refPath);
hold on
axis equal
plot(newTrajGlobal.Trajectory(:,1),newTrajGlobal.Trajectory(:,2),'b');
legend(["Waypoint","Reference Path","New Trajectory"])
hold off

Figure contains an axes object. The axes object contains 3 objects of type line. One or more of the lines displays its values using only markers These objects represent Waypoint, Reference Path, New Trajectory.

弧の長さとして NaN を指定して、縦方向の状態の制限を緩和します。軌跡を再度生成して表示します。参照パスから逸脱した適切な別の軌跡が新しい位置に表示されます。

relaxedTermState = [NaN 10 0 5 0 0];
[~,trajGlobalRelaxed] = connect(connector,initState,relaxedTermState,3);

clf
show(refPath);
hold on
axis equal
plot(trajGlobalRelaxed.Trajectory(:,1),trajGlobalRelaxed.Trajectory(:,2),'g');
hold off

Figure contains an axes object. The axes object contains 3 objects of type line. One or more of the lines displays its values using only markers

詳細

すべて展開する

参照

[1] Werling, Moritz, Julius Ziegler, Sören Kammel, and Sebastian Thrun. "Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame." 2010 IEEE International Conference on Robotics and Automation. 2010, pp. 987–993.

[2] Bertolazzi, Enrico, and Marco Frego. “Fast and Accurate Clothoid Fitting.” Mathematical Methods in the Applied Sciences 38, no. 5 (March 30, 2015): 881–97. https://doi.org/10.1002/mma.3114.

拡張機能

バージョン履歴

R2020b で導入