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

smoothPathSpline

3 次スプライン内挿を使用した滑らかな車両パス

構文

[poses,directions] = smoothPathSpline(refPoses,refDirections,numSmoothPoses)

[poses,directions] = smoothPathSpline(refPoses,refDirections,numSmoothPoses,minSeparation)

[___,cumLengths,curvatures] = smoothPathSpline(___)

説明

[poses,directions] = smoothPathSpline(refPoses,refDirections,numSmoothPoses) は、入力参照パスの姿勢を 3 次スプラインに当てはめることで、numSmoothPoses 個の離散化された姿勢で構成された滑らかな車両パスを生成します。また、smoothPathSpline は入力参照パスの方向に基づき、各姿勢に対応する方向を返します。

この関数を使用して、C¹ 連続の車両パスを C² 連続のパスに変換します。C¹ 連続のパスには、pathPlannerRRT オブジェクトを使用して計画できる driving.DubinsPathSegment パスまたは driving.ReedsSheppPathSegment パスが含まれます。これらのパス タイプの詳細については、C1 連続のパスおよび C2 連続のパスを参照してください。

返された姿勢と方向を関数 lateralControllerStanley などの車両コントローラーで使用できます。

[poses,directions] = smoothPathSpline(refPoses,refDirections,numSmoothPoses,minSeparation) は、姿勢間の最小間隔のしきい値を指定します。2 つの姿勢間の距離が minSeparation より小さい場合、この関数はいずれか 1 つの姿勢のみを内挿に使用します。

また、[___,cumLengths,curvatures] = smoothPathSpline(___) は、前述のいずれかの構文を使用して、返される各姿勢における累積パス長および符号付きのパスの曲率を返します。これらの値を使用して、パスに沿った速度プロファイルを生成します。

例

計画されたパスの平滑化

RRT* パス プランナーで計画されたパスを平滑化します。

駐車場のコストマップを読み込んでプロットします。

data = load('parkingLotCostmap.mat');
costmap = data.parkingLotCostmap;
plot(costmap)

車両の開始姿勢とゴール姿勢を [x, y, Θ] ベクトルとして定義します。位置 (x,y) のワールド単位はメートルです。方向角度 Θ のワールド単位は度です。

startPose = [4,4,90]; % [meters, meters, degrees]
goalPose = [30,13,0];

pathPlannerRRTオブジェクトを使用して、開始姿勢からゴール姿勢までのパスを計画します。

planner = pathPlannerRRT(costmap);
refPath = plan(planner,startPose,goalPose);

計画されたパスをプロットしてズームインします。パスは、一連の Dubins 曲線で構成されます。これらの曲線には、乗客を伴う走行には適していない、曲率の急激な変化が含まれます。

hold on
plot(refPath,'Vehicle','off','DisplayName','Reference path')
xlim([3 31])
ylim([3 18])

パスの遷移する姿勢を内挿します。これらの姿勢を、滑らかなパスを内挿するための参照姿勢として使用します。また、各姿勢における運動方向を返します。

[refPoses,refDirections] = interpolate(refPath);

返される滑らかなパスの姿勢の数を指定します。パス全体の長さに沿って約 0.1 メートル間隔で姿勢を返します。

approxSeparation = 0.1; % meters
numSmoothPoses = round(refPath.Length / approxSeparation);

3 次スプラインを参照姿勢に当てはめて、滑らかなパスを生成します。smoothPathSpline は指定された数の、滑らかなパスに沿って離散化された姿勢を返します。

[poses,directions] = smoothPathSpline(refPoses,refDirections,numSmoothPoses);

滑らかなパスをプロットします。参照パスに存在していた曲率のさらに急激な変化が平滑化されています。

plot(poses(:,1),poses(:,2),'LineWidth',2,'DisplayName','Smooth path')
hold off

入力引数

refPoses — 参照姿勢

[x, y, Θ] のベクトルからなる M 行 3 列の行列

パスに沿った車両の参照姿勢。[x, y, Θ] のベクトルからなる M 行 3 列の行列として指定します。ここで、M は姿勢の数です。

x と y は、車両の位置をメートル単位で指定します。Θ は、車両の方向角度を度単位で指定します。

データ型: single | double

refDirections — 基準方向

1s (前進運動) と –1s (後退運動) からなる M 行 1 列の列ベクトル

パスに沿った車両の基準方向。1s (前進運動) と –1s (後退運動) からなる M 行 1 列の列ベクトルとして指定します。M は基準方向の数です。refDirections の各要素は refPoses 入力引数の姿勢に対応します。

データ型: single | double

numSmoothPoses — 返される滑らかな姿勢の数

正の整数

poses 出力引数で返される滑らかな姿勢の数。正の整数として指定します。返される姿勢の粒度を高めるには、numSmoothPoses を増やします。

minSeparation — 姿勢間の最小間隔

1e-3 (既定値) | 正の実数スカラー

姿勢間の最小間隔 (メートル単位)。正の実数スカラーとして指定します。2 つの姿勢間のユークリッド (x, y) 距離がこの値より小さい場合、この関数はこれらのいずれか 1 つの姿勢のみを内挿に使用します。

出力引数

poses — 平滑化されたパスの離散化された姿勢

[x, y, Θ] のベクトルからなる numSmoothPoses 行 3 列の行列

平滑化されたパスの離散化された姿勢。[x, y, Θ] のベクトルからなる numSmoothPoses 行 3 列の行列として返されます。

x と y は、車両の位置をメートル単位で指定します。Θ は、車両の方向角度を度単位で指定します。

poses の値は、refPoses 入力引数の値と同じデータ型です。

directions — 各出力姿勢における運動方向

1s (前進運動) と –1s (後退運動) からなる numSmoothPoses 行 1 列の列ベクトル

poses の各出力姿勢における運動方向。1s (前進運動) と –1s (後退運動) からなる numSmoothPoses 行 1 列の列ベクトルとして返されます。

directions の値は、refDirections 入力引数の値と同じデータ型です。

cumLengths — 累積パス長

numSmoothPoses 行 1 列の実数値列ベクトル

poses の各出力姿勢における累積パス長。numSmoothPoses 行 1 列の実数値列ベクトルとして返されます。単位はメートルです。

cumLengths 出力と curvatures 出力を使用して、滑らかなパスに沿った車両の速度プロファイルを生成できます。詳細については、Automated Parking Valetの例を参照してください。

curvatures — 符号付きのパスの曲率

numSmoothPoses 行 1 列の実数値列ベクトル

poses の各出力姿勢における符号付きのパスの曲率。numSmoothPoses 行 1 列の実数値列ベクトルとして返されます。単位はラジアン/mです。

curvatures 出力と cumLengths 出力を使用して、滑らかなパスに沿った車両の速度プロファイルを生成できます。詳細については、Automated Parking Valetの例を参照してください。

詳細

C¹ 連続のパスおよび C² 連続のパス

微分が存在し、連続しているパスは、C¹ 連続です。C¹ 連続のみのパスは、曲率に不連続性があります。たとえば、Dubins または Reeds-Shepp のパス セグメントで構成されたパスは、セグメントの結合点において曲率に不連続性があります。これらの不連続性があると、乗客を伴う走行のために十分に滑らかでない方向変更が行われます。

また、2 番目の微分が存在し、連続しているパスは、C² 連続です。C² 連続のパスは連続した曲率をもち、乗客を伴う走行のために十分に滑らかです。

ヒント

• 滑らかなパスに衝突がないことを確認するには、滑らかな姿勢を関数 checkPathValidity への入力として指定します。

アルゴリズム

• パス平滑化のアルゴリズムは、すべての入力参照姿勢点を通過するパラメトリック 3 次スプラインを内挿します。スプラインのパラメーターは、これらの点における累積コード長です。[1]

• 平滑化された出力パスの接線方向は、開始姿勢とゴール姿勢における車両の方向角度とほぼ一致します。

参照

[1] Floater, Michael S. "On the Deviation of a Parametric Cubic Spline Interpolant from Its Data Polygon." Computer Aided Geometric Design. Vol. 25, Number 3, 2008, pp. 148–156.

[2] Lepetic, Marko, Gregor Klancar, Igor Skrjanc, Drago Matko, and Bostjan Potocnik. "Time Optimal Path Planning Considering Acceleration Limits." Robotics and Autonomous Systems. Vol. 45, Numbers 3–4, 2003, pp. 199–210.

拡張機能

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

バージョン履歴

R2019a で導入

参考

関数

• lateralControllerStanley | pathPlannerRRT | driving.Path | checkPathValidity | spline | interpolate

ブロック

• Path Smoother Spline

トピック

• Automated Parking Valet