Main Content

interpolate

計画された車両パスに沿った姿勢の内挿

説明

poses = interpolate(refPath) は、参照パスの長さに沿って内挿を行い、遷移する姿勢を返します。詳細については、遷移する姿勢を参照してください。

poses = interpolate(refPath,lengths) は、パスの長さに沿って指定された点に姿勢を内挿します。poses には、指定された長さに対応する姿勢だけでなく、遷移する姿勢も含まれます。

[poses,directions] = interpolate(___) は、前述の任意の構文の入力を使用して、各姿勢における車両の移動方向も返します。

すべて折りたたむ

最適な RRT* (Rapidly Exploring Random Tree) アルゴリズムを使用して駐車場の車両パスを計画します。パスが有効であることを確認してから、パスに沿って遷移する姿勢をプロットします。

駐車場のコストマップを読み込みます。コストマップをプロットし、駐車場、および車両が回避するインフレート エリアを確認します。

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

Figure contains an axes object. The axes object with xlabel X, ylabel Y contains 2 objects of type image, patch. This object represents Inflated Areas.

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

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

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

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

パスが有効か確認します。

isPathValid = checkPathValidity(refPath,costmap)
isPathValid = logical
   1

パスに沿って遷移する姿勢を内挿します。

transitionPoses = interpolate(refPath);

コストマップ上に計画されたパスと遷移する姿勢をプロットします。

hold on
plot(refPath,'DisplayName','Planned Path')
scatter(transitionPoses(:,1),transitionPoses(:,2),[],'filled', ...
    'DisplayName','Transition Poses')
hold off

Figure contains an axes object. The axes object with xlabel X, ylabel Y contains 13 objects of type image, patch, scatter, line, polygon. These objects represent Inflated Areas, Planned Path, Transition Poses.

Rapidly Exploring Random Tree (RRT*) アルゴリズムを使用して、駐車場の車両パスを計画します。パスに沿った複数の点に車両の姿勢を内挿します。

駐車場のコストマップを読み込みます。コストマップをプロットし、駐車場、および車両が回避するインフレート エリアを確認します。

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

Figure contains an axes object. The axes object with xlabel X, ylabel Y contains 2 objects of type image, patch. This object represents Inflated Areas.

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

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

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

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

パス全体に沿って 1 m ごとに車両姿勢を内挿します。

lengths = 0 : 1 : refPath.Length;
poses = interpolate(refPath,lengths);

内挿した姿勢をコストマップ上にプロットします。

plot(costmap)
hold on
scatter(poses(:,1),poses(:,2),'DisplayName','Interpolated Poses')
hold off

Figure contains an axes object. The axes object with xlabel X, ylabel Y contains 3 objects of type image, patch, scatter. These objects represent Inflated Areas, Interpolated Poses.

入力引数

すべて折りたたむ

計画した車両パス。driving.Path オブジェクトとして指定します。

パスの長さに沿った点。実数値ベクトルとして指定します。値は 0 からパスの長さ (refPathLength プロパティで決定) までの範囲でなければなりません。関数 interpolate は、これらの指定された点に姿勢を内挿します。lengths はメートルなどのワールド単位です。

例: poses = interpolate(refPath,0:0.1:refPath.Length) は、パス全体の長さに沿って 0.1 メートルごとに姿勢を内挿します。

出力引数

すべて折りたたむ

パスに沿った車両姿勢。[x, y, Θ] のベクトルからなる m 行 3 列の行列として返されます。m は返される姿勢の数です。

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

poses には、パスに沿って指定された点にのみ内挿する場合でも、遷移する姿勢が常に含まれます。入力引数 lengths を指定しない場合、poses には遷移する姿勢のみが含まれます。

車両姿勢の運動方向。1s (前進運動) と –1s (後退運動) からなる m 行 1 列のベクトルとして返されます。m は返される姿勢の数です。directions の各要素は poses の行に対応します。

詳細

すべて折りたたむ

遷移する姿勢

パスは、運動 (左折、直進、右折など) の組み合わせからなる複数のセグメントで構成されます。"遷移する姿勢" とは、ある運動の終了と別の運動の開始に対応する車両姿勢です。これらは、車両の方向または向きの変化に対応するパスに沿った点を表します。関数 interpolate は、パスに沿って指定された点にのみ姿勢を内挿する場合でも、遷移する姿勢を常に返します。

遷移する姿勢間のパスの長さは、パス セグメントの MotionLengths プロパティで指定されます。たとえば、次のパスを考えます。これは単一の Dubins パス セグメントで構成される driving.Path オブジェクトです。このセグメントは、セグメントの MotionLengths プロパティと MotionTypes プロパティで記述されるように、3 つの運動で構成されています。

Sample MATLAB output of Path object and one of its DubinsPathSegment objects

関数 interpolate は、次の遷移する姿勢を次の順序で内挿します。

  1. 車両の初期姿勢 StartPose

  2. 車両が最大ステアリング角度で 4.39 メートル左折 ("L") した後の姿勢。

  3. 車両が 6.32 メートル直進 ("S") した後の姿勢。

  4. 車両が最大ステアリング角度で 4.39 メートル右折 ("R") した後の姿勢。これはパス全体の最後の姿勢であるため、この姿勢はゴール姿勢でもあります。

プロットは、これらの遷移する姿勢を示しており、それらは [x, y, Θ] のベクトルです。x と y は、車両の位置をメートルなどのワールド単位で指定します。Θ は車両の向きの角度を度単位で指定します。

Transition poses of vehicle

拡張機能

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

バージョン履歴

R2018b で導入