Main Content

referencePathFrenet

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

説明

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

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

  • kappa — 曲率 (半径の逆数、メートル単位)

  • dkappa — 弧の長さに対する曲率の微分 (メートル毎秒単位)

  • 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(waypoints,'DiscretizationDistance',discretionDist) は、パス上の内挿点間の指定された距離を使用して、一連の連続した区分的曲線をウェイポイントに当てはめます。引数 discretionDistDiscretizationDistance プロパティを設定します。

プロパティ

すべて展開する

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

データ型: single | double

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

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

データ型: single | double

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

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

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

  • kappa — 曲率 (半径の逆数、メートル単位)

  • dkappa — 弧の長さに対する曲率の微分 (メートル毎秒単位)

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

データ型: single | double

名前と値の引数

オプションの引数のペアを Name1=Value1,...,NameN=ValueN として指定します。ここで、Name は引数名、Value は対応する値です。名前と値の引数は他の引数の後に表示されなければなりませんが、ペアの順序は重要ではありません。

R2021a より前は、名前と値をそれぞれコンマを使って区切り、Name引用符で囲みます。

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

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

パス上の内挿点間の弧の長さ (メートル単位)。正のスカラーとして指定します。オブジェクトは、パス上の内挿点を使用して、変換関数 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弧の長さに対する曲率を返す
changeInCurvatureReturn change-in-curvature at arclength
frenet2globalConvert Frenet states to global states
global2frenetConvert global states to Frenet states
interpolate参照パス上の指定された弧の長さを内挿
positionReturn xy-position at arclength
tangentAngleReturn tangent angle at arclength
showDisplay reference path in figure

すべて折りたたむ

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

一連のウェイポイントから参照パスを生成します。この参照パスから 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. 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. 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. 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.

拡張機能

バージョン履歴

R2020b で導入