Main Content

waypointTrajectory

ウェイポイント軌跡ジェネレーター

R2020b 以降

説明

waypointTrajectory System object™ は、指定されたウェイポイントに基づいて軌跡を生成します。System object を作成すると、各ウェイポイントにおける到着時間、速度、または対地速度を指定することを選択できます。必要に応じて、各ウェイポイントにおける方向などの他のプロパティを指定できます。詳細については、アルゴリズムを参照してください。

ウェイポイントから軌跡を生成するには、以下のようにします。

  1. waypointTrajectory オブジェクトを作成して、そのプロパティを設定します。

  2. 関数と同様に、オブジェクトを呼び出します。

System object の機能の詳細については、System object とはを参照してください。

作成

説明

trajectory = waypointTrajectory は、既定の静止ウェイポイントに基づいて軌跡を生成する System object である trajectory を返します。

trajectory = waypointTrajectory(Waypoints,TimeOfArrival) は、生成された軌跡が各ウェイポイントを通過する到着時間を指定します。詳細については、TimeOfArrival プロパティを参照してください。

ヒント

TimeOfArrival 引数を指定した場合は、次のプロパティを指定してはなりません。

  • JerkLimit

  • InitialTime

  • WaitTime

trajectory = waypointTrajectory(Waypoints,GroundSpeed=groundSpeed) は、生成された軌跡が各ウェイポイントを通過する際の対地速度を指定します。詳細については、GroundSpeed プロパティを参照してください。

trajectory = waypointTrajectory(Waypoints,Velocities=velocities) は、生成された軌跡が各ウェイポイントを通過する際の速度を指定します。詳細については、Velocities プロパティを参照してください。

trajectory = waypointTrajectory(___,Name=Value) は、名前と値の引数を使用して各プロパティを設定します。指定されていないプロパティには既定値または推定値が設定されます。前述の任意の構文でこの構文を使用できます。

例: trajectory = waypointTrajectory([10,10,0;20,20,0;20,20,10],[0,0.5,10]) は、ウェイポイント [10,10,0] から開始し、0.5 秒後に [20,20,0] を通過し、10 秒後に [20,20,10] を通過するウェイポイント軌跡 System object の trajectory を作成します。

プロパティ

すべて展開する

特に指定がない限り、プロパティは "調整不可能" です。つまり、オブジェクトを呼び出した後にプロパティ値を変更することはできません。オブジェクトは呼び出し時にロックされ、関数 release でロック解除されます。

プロパティが "調整可能" な場合は、いつでも値を変更できます。

プロパティ値の変更の詳細については、System object を使用した MATLAB でのシステム設計を参照してください。

軌跡のサンプル レート (Hz)。正のスカラーとして指定します。

調整可能: Yes

データ型: double

出力フレームあたりのサンプル数。正のスカラー整数として指定します。

データ型: double

ナビゲーション座標系における位置 (メートル単位)。N 行 3 列の行列として指定します。行列の列はそれぞれ、第 1 軸、第 2 軸、および第 3 軸に対応します。行列の N 番目の行は、個々のウェイポイントに対応します。

ヒント

軌跡を特定のウェイポイントで待機させるには、次の 2 つのオプションのいずれかを使用します。

  • 入力引数 TimeOfArrival を指定した場合は、連続する 2 行でウェイポイントの座標を繰り返します。

  • 入力引数 TimeOfArrival を指定しなかった場合は、WaitTime プロパティを使用して待機時間を指定します。

データ型: double

各ウェイポイントへの到着に対応する時間 (秒単位)。N 要素の列ベクトルとして指定します。サンプル数 N は、Waypoints で定義されたサンプル (行) 数と同じでなければなりません。

依存関係

このプロパティを設定するには、次のプロパティを指定してはなりません。

  • JerkLimit

  • InitialTime

  • WaitTime

データ型: double

各ウェイポイントにおけるナビゲーション座標系での速度 (メートル/秒単位)。N 行 3 列の行列として指定します。行列の列はそれぞれ、第 1 軸、第 2 軸、および第 3 軸に対応します。サンプル数 N は、Waypoints で定義されたサンプル (行) 数と同じでなければなりません。

速度を非ゼロの値として指定した場合、オブジェクトは速度に基づいて軌跡のコースを自動的に計算します。速度をゼロに指定した場合、オブジェクトは隣接するウェイポイントから軌跡のコースを推定します。

データ型: double

水平方向の進行方向。度単位の N 要素の実数ベクトルとして指定します。サンプル数 N は、Waypoints で定義されたサンプル (行) 数と同じでなければなりません。VelocitiesCourse も指定されていない場合、コースはウェイポイントから推定されます。

依存関係

このプロパティを設定するには、Velocities プロパティを指定してはなりません。

データ型: double

各ウェイポイントにおける対地速度。m/s 単位の N 要素の実数ベクトルとして指定します。このプロパティが指定されていない場合は、ウェイポイントから推定されます。サンプル数 N は、Waypoints で定義されたサンプル (行) 数と同じでなければなりません。

  • 前進運動をレンダリングするには、正の対地速度値を指定します。

  • 後退運動をレンダリングするには、負の対地速度値を指定します。

  • 逆方向の運動をレンダリングするには、正の対地速度値と負の対地速度値をゼロの対地速度値で区切ります。

依存関係

このプロパティを設定するには、Velocities プロパティを指定してはなりません。

データ型: double

各ウェイポイントにおける上昇率 (メートル/秒単位)。N 要素の実数ベクトルとして指定します。サンプル数 N は、Waypoints で定義されたサンプル (行) 数と同じでなければなりません。VelocitiesCourse も指定されていない場合、上昇率はウェイポイントから推定されます。

依存関係

このプロパティを設定するには、Velocities プロパティを指定してはなりません。

データ型: double

縦加加速度の範囲。m/s3 単位の正のスカラーとして指定します。加加速度は加速度の時間微分です。このプロパティを指定すると、オブジェクトは加加速度の範囲に基づいて水平の台形加速度プロファイルを生成します。waypointTrajectory オブジェクトが指定された JerkLimit を達成できない場合、オブジェクトはエラーを出します。このプロパティは、オブジェクトの作成時にのみ設定できます。

依存関係

このプロパティを設定するには、TimeOfArrival プロパティを指定してはなりません。

データ型: double

軌跡が開始するまでの時間。秒単位の非負のスカラーとして指定します。オブジェクトは、軌跡が開始するまで、位置や速度などの数量を NaN として報告します。このプロパティは、オブジェクトの作成時にのみ設定できます。

依存関係

このプロパティを設定するには、TimeOfArrival プロパティを指定してはなりません。代わりに、オブジェクトの作成時に GroundSpeed プロパティまたは Velocities プロパティを指定する必要があります。

データ型: double

各ウェイポイントにおける待機時間。非負のスカラーの N 要素ベクトルとして指定します。N は、Waypoints で定義したサンプル (行) 数と同じでなければなりません。このプロパティは、オブジェクトの作成時にのみ設定できます。

依存関係

このプロパティを設定するには、TimeOfArrival プロパティを指定してはなりません。

TimeOfArrival プロパティを指定した場合、このプロパティを使用して待機時間を指定することはできません。代わりに、Waypoints プロパティの連続する 2 行でウェイポイントの座標を繰り返して待機時間を指定します。

データ型: double

各ウェイポイントにおける方向。N 要素の quaternion 列ベクトルまたは実数の 3×3×N の配列として指定します。各 quaternion のノルムは 1 でなければなりません。3 行 3 列の各回転行列は正規直交行列でなければなりません。四元数または回転行列の数 N は、Waypoints で定義されたサンプル (行) 数と同じでなければなりません。

Orientation が四元数で指定された場合、基となるクラスは double でなければなりません。

データ型: double

ピッチ角を運動方向に合わせるかどうか。true または false として指定します。true に指定すると、ピッチ角が運動方向に自動的に合わせられます。false に指定すると、ピッチ角はゼロ (水平方向) に設定されます。

依存関係

このプロパティを設定するには、Orientation プロパティを指定してはなりません。

ロール角を調整して向心力に抗するかどうか。true または false として指定します。true に指定すると、ロール角が自動的に向心力に抗します。false に指定すると、ロール角はゼロ (フラット方向) に設定されます。

依存関係

このプロパティを設定するには、Orientation プロパティを指定してはなりません。

軌跡の基準座標系。'NED' (北-東-下) または 'ENU' (東-北-上) として指定します。

使用法

説明

[position,orientation,velocity,acceleration,angularVelocity] = trajectory() は、指定された作成引数とプロパティに基づいて軌跡データの座標系を出力します。

出力引数

すべて展開する

ローカル ナビゲーション座標系における位置 (メートル単位)。M 行 3 列の行列として返されます。

M は SamplesPerFrame プロパティで指定します。

データ型: double

ローカル ナビゲーション座標系における方向。M 行 1 列の quaternion 列ベクトルまたは 3×3×M の実数配列として返されます。

各四元数または 3 行 3 列の回転行列は、ローカル ナビゲーション座標系から現在のボディ座標系への座標系の回転です。

M は SamplesPerFrame プロパティで指定します。

データ型: double

ローカル ナビゲーション座標系における速度 (メートル/秒単位)。M 行 3 列の行列として返されます。

M は SamplesPerFrame プロパティで指定します。

データ型: double

ローカル ナビゲーション座標系における加速度 (メートル毎秒毎秒単位)。M 行 3 列の行列として返されます。

M は SamplesPerFrame プロパティで指定します。

データ型: double

ローカル ナビゲーション座標系における角速度 (ラジアン/秒単位)。M 行 3 列の行列として返されます。

M は SamplesPerFrame プロパティで指定します。

データ型: double

オブジェクト関数

オブジェクト関数を使用するには、System object を最初の入力引数として指定します。たとえば、obj という名前の System object のシステム リソースを解放するには、以下の構文を使用します。

release(obj)

すべて展開する

waypointInfoウェイポイント情報の table を取得する
lookupPose特定時間の姿勢情報を取得する
perturbationsPerturbation defined on object
perturb摂動をオブジェクトに適用する
cloneSystem object の複製
stepSystem object のアルゴリズムの実行
releaseリソースを解放し、System object のプロパティ値と入力特性の変更を可能にします。
resetSystem object の内部状態のリセット
isDoneデータ終了ステータス

すべて折りたたむ

trajectory = waypointTrajectory
trajectory = 
  waypointTrajectory with properties:

         SampleRate: 100
    SamplesPerFrame: 1
          Waypoints: [2x3 double]
      TimeOfArrival: [2x1 double]
         Velocities: [2x3 double]
             Course: [2x1 double]
        GroundSpeed: [2x1 double]
          ClimbRate: [2x1 double]
        Orientation: [2x1 quaternion]
          AutoPitch: 0
           AutoBank: 0
     ReferenceFrame: 'NED'

waypointInfo を呼び出して、既定のウェイポイントと到着時間を検査します。既定では、ウェイポイントは 1 秒間の静止位置を示します。

waypointInfo(trajectory)
ans=2×2 table
    TimeOfArrival     Waypoints 
    _____________    ___________

          0          0    0    0
          1          0    0    0

正方形の軌跡を作成し、ウェイポイント制約、サンプル レート、生成された軌跡の間の関係を調べます。

正方形の頂点を定義して、正方形の軌跡を作成します。運動方向を指すように各ウェイポイントにおける方向を定義します。1 Hz のサンプル レートを指定し、既定の SamplesPerFrame の 1 を使用します。

waypoints = [0,0,0; ... % Initial position
             0,1,0; ...
             1,1,0; ...
             1,0,0; ...
             0,0,0];    % Final position

toa = 0:4; % time of arrival

orientation = quaternion([0,0,0; ...
                          45,0,0; ...
                          135,0,0; ...
                          225,0,0; ...
                          0,0,0], ...
                          "eulerd","ZYX","frame");

trajectory = waypointTrajectory(waypoints, ...
    TimeOfArrival=toa, ...
    Orientation=orientation, ...
    SampleRate=1);

Figure を作成し、プラットフォームの初期位置をプロットします。

figure(1)
plot(waypoints(1,1),waypoints(1,2),"b*")
title("Position")
axis([-1,2,-1,2])
axis square
xlabel("X")
ylabel("Y")
grid on
hold on

ループで、軌跡をステップ実行して、現在の位置と現在の方向を出力します。現在の位置をプロットし、方向をログ記録します。pause を使用してリアルタイム処理を模倣します。

orientationLog = zeros(toa(end)*trajectory.SampleRate,1,"quaternion");
count = 1;
while ~isDone(trajectory)
   [currentPosition,orientationLog(count)] = trajectory();

   plot(currentPosition(1),currentPosition(2),"bo")

   pause(trajectory.SamplesPerFrame/trajectory.SampleRate)
   count = count + 1;
end
hold off

解釈を容易にするために方向の四元数をオイラー角に変換し、経時的に方向をプロットします。

figure(2)
eulerAngles = eulerd([orientation(1);orientationLog],"ZYX","frame");
plot(toa,eulerAngles(:,1),"ko", ...
     toa,eulerAngles(:,2),"bd", ...
     toa,eulerAngles(:,3),"r.");
title("Orientation Over Time")
legend("Rotation around Z-axis","Rotation around Y-axis","Rotation around X-axis")
xlabel("Time (seconds)")
ylabel("Rotation (degrees)")
grid on

これまでのところ、軌跡オブジェクトは作成時に指定されたウェイポイントのみを出力しています。ウェイポイント間に内挿するために、サンプル レートをウェイポイントの到着時間よりも速いレートまで増加させます。trajectory のサンプル レートを 100 Hz に設定し、reset を呼び出します。

trajectory.SampleRate = 100;
reset(trajectory)

Figure を作成し、プラットフォームの初期位置をプロットします。ループで、軌跡をステップ実行して、現在の位置と現在の方向を出力します。現在の位置をプロットし、方向をログ記録します。pause を使用してリアルタイム処理を模倣します。

figure(1)
plot(waypoints(1,1),waypoints(1,2),"b*")
title("Position")
axis([-1,2,-1,2])
axis square
xlabel("X")
ylabel("Y")
grid on
hold on

orientationLog = zeros(toa(end)*trajectory.SampleRate,1,"quaternion");
count = 1;
while ~isDone(trajectory)
   [currentPosition,orientationLog(count)] = trajectory();

   plot(currentPosition(1),currentPosition(2),"bo")

   pause(trajectory.SamplesPerFrame/trajectory.SampleRate)
   count = count + 1;
end
hold off

軌跡の出力が円形になりました。これは、waypointTrajectory System object™ が内挿時に加速度と角速度を最小限に抑えるため、ほとんどのシナリオでより滑らかで現実的な運動が実現されるためです。

解釈を容易にするために方向の四元数をオイラー角に変換し、経時的に方向をプロットします。方向も内挿されます。

figure(2)
eulerAngles = eulerd([orientation(1);orientationLog],"ZYX","frame");
t = 0:1/trajectory.SampleRate:4;
plot(t,eulerAngles(:,1),"ko", ...
     t,eulerAngles(:,2),"bd", ...
     t,eulerAngles(:,3),"r.");
title("Orientation Over Time")
legend("Rotation around Z-axis","Rotation around Y-axis","Rotation around X-axis")
xlabel("Time (seconds)")
ylabel("Rotation (degrees)")
grid on

waypointTrajectory アルゴリズムはウェイポイントを内挿して滑らかな軌跡を作成します。正方形の軌跡に戻すには、特に急激な変化がある場所の付近にウェイポイントを追加します。対応する時間、ウェイポイント、および方向を追跡するには、すべての軌跡情報を 1 つの行列で指定します。

               % Time, Waypoint, Orientation
trajectoryInfo = [0,   0,0,0,    0,0,0; ... % Initial position
                  0.1, 0,0.1,0,  0,0,0; ...

                  0.9, 0,0.9,0,  0,0,0; ...
                  1,   0,1,0,    45,0,0; ...
                  1.1, 0.1,1,0,  90,0,0; ...

                  1.9, 0.9,1,0,  90,0,0; ...
                  2,   1,1,0,    135,0,0; ...
                  2.1, 1,0.9,0,  180,0,0; ...

                  2.9, 1,0.1,0,  180,0,0; ...
                  3,   1,0,0,    225,0,0; ...
                  3.1, 0.9,0,0,  270,0,0; ...

                  3.9, 0.1,0,0,  270,0,0; ...
                  4,   0,0,0,    270,0,0];    % Final position

trajectory = waypointTrajectory(trajectoryInfo(:,2:4), ...
    TimeOfArrival=trajectoryInfo(:,1), ...
    Orientation=quaternion(trajectoryInfo(:,5:end),"eulerd","ZYX","frame"), ...
    SampleRate=100);

Figure を作成し、プラットフォームの初期位置をプロットします。ループで、軌跡をステップ実行して、現在の位置と現在の方向を出力します。現在の位置をプロットし、方向をログ記録します。pause を使用してリアルタイム処理を模倣します。

figure(1)
plot(waypoints(1,1),waypoints(1,2),"b*")
title("Position")
axis([-1,2,-1,2])
axis square
xlabel("X")
ylabel("Y")
grid on
hold on

orientationLog = zeros(toa(end)*trajectory.SampleRate,1,"quaternion");
count = 1;
while ~isDone(trajectory)
   [currentPosition,orientationLog(count)] = trajectory();

   plot(currentPosition(1),currentPosition(2),"bo")

   pause(trajectory.SamplesPerFrame/trajectory.SampleRate)
   count = count+1;
end
hold off

軌跡の出力が、特にウェイポイントのある頂点の付近で、より正方形に近くなりました。

解釈を容易にするために方向の四元数をオイラー角に変換し、経時的に方向をプロットします。

figure(2)
eulerAngles = eulerd([orientation(1);orientationLog],"ZYX","frame");
t = 0:1/trajectory.SampleRate:4;
eulerAngles = plot(t,eulerAngles(:,1),"ko", ...
                   t,eulerAngles(:,2),"bd", ...
                   t,eulerAngles(:,3),"r.");
title("Orientation Over Time")
legend("Rotation around Z-axis", ...
       "Rotation around Y-axis", ...
       "Rotation around X-axis", ...
       "Location", "SouthWest")
xlabel("Time (seconds)")
ylabel("Rotation (degrees)")
grid on

この例では、waypointTrajectory System object™ を使用して弧の軌跡を作成する方法を示します。waypointTrajectory は、加速度と角速度を最小限に抑えるように、指定されたウェイポイントを通過するパスを作成します。弧の軌跡を作成した後、軌跡が事前設定した範囲内に収まるように制限します。

弧の軌跡の作成

弧の軌跡のウェイポイント、到着時間、および方向で構成された制約行列を定義します。生成された軌跡は、指定された時間に指定された方向に向かってウェイポイントを通過します。waypointTrajectory System object では、四元数または回転行列を使用して方向を指定する必要があります。Orientation プロパティを指定する場合は、制約行列に保存されているオイラー角を四元数に変換します。

          % Arrival, Waypoints, Orientation
constraints = [0,    20,20,0,    90,0,0;
               3,    50,20,0,    90,0,0;
               4,    58,15.5,0,  162,0,0;
               5.5,  59.5,0,0    180,0,0];

trajectory = waypointTrajectory(constraints(:,2:4), ...
    TimeOfArrival=constraints(:,1), ...
    Orientation=quaternion(constraints(:,5:7),"eulerd","ZYX","frame"));

trajectorywaypointInfo を呼び出して、指定した制約の table を返します。作成プロパティ WaypointsTimeOfArrival、および Orientation は table の変数です。table は、プロット中にインデックスを付けるのに便利です。

tInfo = waypointInfo(trajectory)
tInfo =

  4x3 table

    TimeOfArrival         Waypoints            Orientation   
    _____________    ____________________    ________________

           0           20      20       0    {1x1 quaternion}
           3           50      20       0    {1x1 quaternion}
           4           58    15.5       0    {1x1 quaternion}
         5.5         59.5       0       0    {1x1 quaternion}

軌跡オブジェクトは、呼び出しごとに現在位置、速度、加速度、および角速度を出力します。ループ内で trajectory を呼び出し、経時的に位置をプロットします。他の出力をキャッシュします。

figure(1)
plot(tInfo.Waypoints(1,1),tInfo.Waypoints(1,2),"b*")
title("Position")
axis([20,65,0,25])
xlabel("North")
ylabel("East")
grid on
daspect([1 1 1])
hold on

orient = zeros(tInfo.TimeOfArrival(end)*trajectory.SampleRate,1,"quaternion");
vel = zeros(tInfo.TimeOfArrival(end)*trajectory.SampleRate,3);
acc = vel;
angVel = vel;

count = 1;
while ~isDone(trajectory)
   [pos,orient(count),vel(count,:),acc(count,:),angVel(count,:)] = trajectory();

   plot(pos(1),pos(2),"bo")

   pause(trajectory.SamplesPerFrame/trajectory.SampleRate)
   count = count + 1;
end

方向、速度、加速度、および角速度を経時的に検査します。waypointTrajectory System object™ は、加速度と角速度を最小限に抑えるように、指定された制約を使用してパスを作成します。

figure(2)
timeVector = 0:(1/trajectory.SampleRate):tInfo.TimeOfArrival(end);
eulerAngles = eulerd([tInfo.Orientation{1};orient],"ZYX","frame");
plot(timeVector,eulerAngles(:,1), ...
     timeVector,eulerAngles(:,2), ...
     timeVector,eulerAngles(:,3));
title("Orientation Over Time")
legend("Rotation around Z-axis", ...
       "Rotation around Y-axis", ...
       "Rotation around X-axis", ...
       "Location","southwest")
xlabel("Time (seconds)")
ylabel("Rotation (degrees)")
grid on

figure(3)
plot(timeVector(2:end),vel(:,1), ...
     timeVector(2:end),vel(:,2), ...
     timeVector(2:end),vel(:,3));
title("Velocity Over Time")
legend("North","East","Down")
xlabel("Time (seconds)")
ylabel("Velocity (m/s)")
grid on

figure(4)
plot(timeVector(2:end),acc(:,1), ...
     timeVector(2:end),acc(:,2), ...
     timeVector(2:end),acc(:,3));
title("Acceleration Over Time")
legend("North","East","Down","Location","southwest")
xlabel("Time (seconds)")
ylabel("Acceleration (m/s^2)")
grid on

figure(5)
plot(timeVector(2:end),angVel(:,1), ...
     timeVector(2:end),angVel(:,2), ...
     timeVector(2:end),angVel(:,3));
title("Angular Velocity Over Time")
legend("North","East","Down")
xlabel("Time (seconds)")
ylabel("Angular Velocity (rad/s)")
grid on

弧の軌跡を事前設定された範囲内に制限

追加のウェイポイントを指定して、指定した範囲内で軌跡を作成できます。弧の軌跡の上限と下限を作成します。

figure(1)
xUpperBound = [(20:50)';50+10*sin(0:0.1:pi/2)';60*ones(11,1)];
yUpperBound = [20.5.*ones(31,1);10.5+10*cos(0:0.1:pi/2)';(10:-1:0)'];

xLowerBound = [(20:49)';50+9*sin(0:0.1:pi/2)';59*ones(11,1)];
yLowerBound = [19.5.*ones(30,1);10.5+9*cos(0:0.1:pi/2)';(10:-1:0)'];

plot(xUpperBound,yUpperBound,"r","LineWidth",2);
plot(xLowerBound,yLowerBound,"r","LineWidth",2)

範囲内に軌跡を作成するために、さらにウェイポイントを追加します。新しい waypointTrajectory System object™ を作成し、それをループ内で呼び出して、生成された軌跡をプロットします。trajectory オブジェクトから出力された方向、速度、加速度、および角速度をキャッシュします。

            % Time,  Waypoint,     Orientation
constraints = [0,    20,20,0,      90,0,0;
               1.5,  35,20,0,      90,0,0;
               2.5   45,20,0,      90,0,0;
               3,    50,20,0,      90,0,0;
               3.3,  53,19.5,0,    108,0,0;
               3.6,  55.5,18.25,0, 126,0,0;
               3.9,  57.5,16,0,    144,0,0;
               4.2,  59,14,0,      162,0,0;
               4.5,  59.5,10,0     180,0,0;
               5,    59.5,5,0      180,0,0;
               5.5,  59.5,0,0      180,0,0];

trajectory = waypointTrajectory(constraints(:,2:4), ...
    TimeOfArrival=constraints(:,1), ...
    Orientation=quaternion(constraints(:,5:7),"eulerd","ZYX","frame"));
tInfo = waypointInfo(trajectory);

figure(1)
plot(tInfo.Waypoints(1,1),tInfo.Waypoints(1,2),"b*")

count = 1;
while ~isDone(trajectory)
   [pos,orient(count),vel(count,:),acc(count,:),angVel(count,:)] = trajectory();

   plot(pos(1),pos(2),"gd")

   pause(trajectory.SamplesPerFrame/trajectory.SampleRate)
   count = count + 1;
end

生成された軌跡が、指定した範囲内に収まるようになりました。生成された軌跡の方向、速度、加速度、および角速度を可視化します。

figure(2)
timeVector = 0:(1/trajectory.SampleRate):tInfo.TimeOfArrival(end);
eulerAngles = eulerd(orient,"ZYX","frame");
plot(timeVector(2:end),eulerAngles(:,1), ...
     timeVector(2:end),eulerAngles(:,2), ...
     timeVector(2:end),eulerAngles(:,3));
title("Orientation Over Time")
legend("Rotation around Z-axis", ...
       "Rotation around Y-axis", ...
       "Rotation around X-axis", ...
       "Location","southwest")
xlabel("Time (seconds)")
ylabel("Rotation (degrees)")
grid on

figure(3)
plot(timeVector(2:end),vel(:,1), ...
     timeVector(2:end),vel(:,2), ...
     timeVector(2:end),vel(:,3));
title("Velocity Over Time")
legend("North","East","Down")
xlabel("Time (seconds)")
ylabel("Velocity (m/s)")
grid on

figure(4)
plot(timeVector(2:end),acc(:,1), ...
     timeVector(2:end),acc(:,2), ...
     timeVector(2:end),acc(:,3));
title("Acceleration Over Time")
legend("North","East","Down")
xlabel("Time (seconds)")
ylabel("Acceleration (m/s^2)")
grid on

figure(5)
plot(timeVector(2:end),angVel(:,1), ...
     timeVector(2:end),angVel(:,2), ...
     timeVector(2:end),angVel(:,3));
title("Angular Velocity Over Time")
legend("North","East","Down")
xlabel("Time (seconds)")
ylabel("Angular Velocity (rad/s)")
grid on

生成された軌跡は空間的範囲内に収まるようになっている一方で、軌跡の加速度と角速度が多少不安定になっていることに注意してください。これはウェイポイントの指定過剰が原因です。

次のようなレーストラックの軌跡を考えます。

軌跡の 4 隅の点は、それぞれメートル単位で (0,0,0)、(20,0,0)、(20,5,0)、および (0,5,0) です。したがって、ループ状のウェイポイントを次のように指定します。

wps = [0 0 0;
      20 0 0;
      20 5 0;
      0  5 0;
      0  0 0];

軌跡の速度が一定の 2 m/s であると仮定すると、5 つのウェイポイントにおける速度は次のようになります。

vels = [2 0 0;
        2 0 0;
       -2 0 0;
       -2 0 0;
        2 0 0];

5 つのウェイポイントへの到着時間は次のとおりです。

t = cumsum([0 20/2 5*pi/2/2 20/2 5*pi/2/2]');

5 つのウェイポイントにおける軌跡の方向は次のとおりです。

eulerAngs = [0 0 0;
             0 0 0;
           180 0 0;
           180 0 0;
             0 0 0]; % Angles in degrees.
% Convert Euler angles to quaternions.
quats = quaternion(eulerAngs,"eulerd","ZYX","frame");

軌跡の線を滑らかにするためにサンプル レートを 100 に指定します。

fs = 100;

waypointTrajectory を作成します。

traj = waypointTrajectory(wps,SampleRate=fs, ...
        Velocities=vels,...
        TimeOfArrival=t,...
        Orientation=quats);

軌跡をサンプリングしてプロットします。

[pos, orient, vel, acc, angvel] = traj();
i = 1;

spf = traj.SamplesPerFrame;
while ~isDone(traj)
    idx = (i+1):(i+spf);
    [pos(idx,:), orient(idx,:), ...
        vel(idx,:), acc(idx,:), angvel(idx,:)] = traj();
    i = i+spf;
end

軌跡と指定したウェイポイントをプロットします。

plot(pos(:,1),pos(:,2), wps(:,1),wps(:,2), "--o")
xlabel("X (m)")
ylabel("Y (m)")
zlabel("Z (m)")
legend({"Trajectory", "Waypoints"})
axis equal

2 つのウェイポイントを接続する waypointTrajectory オブジェクトを作成します。2 つのウェイポイントにおける軌跡の速度は、それぞれ 0 m/s と 10 m/s です。加加速度の範囲を 0.5 m/s3 に制限して、台形加速度プロファイルを有効にします。

waypoints = [0  0  0;
            10 50 10];
speeds = [0 10];
jerkLimit = 0.5;
trajectory = waypointTrajectory(waypoints,GroundSpeed=speeds,JerkLimit=jerkLimit);

TimeOfArrival プロパティをクエリして、軌跡の初期時間と最終時間を取得します。タイム スタンプを作成して軌跡をサンプリングします。

t0 = trajectory.TimeOfArrival(1);
tf = trajectory.TimeOfArrival(end);
sampleTimes = linspace(t0,tf,100);

オブジェクト関数 lookupPose を使用して、これらのサンプリングされたタイム スタンプにおける位置、速度、および加速度の情報を取得します。

[position,~,velocity,acceleration,~] = lookupPose(trajectory,sampleTimes);

軌跡をプロットします。

figure()
plot3(position(:,1),position(:,2),position(:,3))
xlabel("x (m)")
ylabel("y (m)")
zlabel("z (m)")
title("Trajectory")

速度プロファイルをプロットします。

figure()
subplot(3,1,1)
plot(sampleTimes,velocity(:,1));
ylabel("v_x (m/s)")
title("Velocity Profile")
subplot(3,1,2)
plot(sampleTimes,velocity(:,2));
ylabel("v_y (m/s)")
subplot(3,1,3)
plot(sampleTimes,velocity(:,3));
ylabel("v_z (m/s)")
xlabel("Time (sec)")

加速度プロファイルをプロットします。結果を見ると、平面運動の加速度プロファイルは台形になっています。

figure()
subplot(3,1,1)
plot(sampleTimes,acceleration(:,1));
axis padded
ylabel("a_x (m/s^2)")
title("Acceleration Profile")
subplot(3,1,2)
plot(sampleTimes,acceleration(:,2));
ylabel("a_y (m/s^2)")
axis padded
subplot(3,1,3)
plot(sampleTimes,acceleration(:,3));
ylabel("a_z (m/s^2)")
xlabel("Time (sec)")

アルゴリズム

waypointTrajectory System object は、ウェイポイントを滑らかに通過する軌跡を定義します。軌跡は、軌跡基準座標系で表される重力方向が一定であると仮定した内挿によってウェイポイントを接続します。一般的に、waypointTrajectory を使用して、数百キロメートルの距離範囲内のプラットフォームやビークルの軌跡をモデル化できます。

軌跡の平面パス (x-y 平面投影) は区分的なクロソイド曲線で構成されます。2 つの連続するウェイポイント間の曲線の曲率は、ウェイポイント間の曲線の長さに応じて線形に変化します。コースが Course プロパティによって明示的に指定されるか、または Velocities プロパティによって暗黙的に指定されない限り、各ウェイポイントにおけるパスの接線方向は、曲率の不連続性を最小限に抑えるように選択されます。パスが確立されると、オブジェクトは 3 次エルミート内挿を使用して、パス全体にわたるビークルの位置を時間と平面移動距離の関数として計算します。JerkLimit プロパティが指定されている場合、オブジェクトは 2 つのウェイポイントの間にある任意のセグメントに対して水平の台形加速度プロファイルを生成します。台形加速度プロファイルは次の 3 つのサブセグメントで構成されます。

  • 一定の大きさの加加速度サブセグメント

  • 一定の大きさの加速度サブセグメント

  • 一定の大きさの加加速度サブセグメント

上昇率が ClimbRate プロパティまたは Velocities プロパティの 3 番目の列によって明示的に指定されていない限り、軌跡の法線成分 (z 成分) は、その後、形状保持区分的スプライン (PCHIP) を満たすように選択されます。選択した ReferenceFrame に基づいて上昇率の符号を選択します。

  • 'ENU' 基準座標系が選択されている場合、正の上昇率を指定すると、z の値が増加します。

  • 'NED' 基準座標系が選択されている場合、正の上昇率を指定すると、z の値が減少します。

パスを通るビークルの方向は、主に次の 2 つの方法で定義できます。

  • Orientation プロパティが指定されている場合、オブジェクトは区分的な 3 次の四元数スプラインを使用して、パスに沿った方向を時間の関数として計算します。

  • Orientation プロパティが指定されていない場合、ビークルのヨーは常にパスに合わせられます。ロールとピッチは、それぞれ AutoBank プロパティと AutoPitch プロパティの値によって制御されます。

    AutoBankAutoPitch説明
    falsefalseビークルは常に水平です (ピッチとロールがゼロ)。これは通常、大型船舶で使用されます。
    falsetrueビークルのピッチはパスに一致し、ビークルのロールは常にゼロです。これは通常、地上ビークルで使用されます。
    truefalseビークルのピッチとロールは、そのローカル z 軸が正味加速度 (重力を含む) と一致するように選択されます。これは通常、回転翼航空機で使用されます。
    truetrueビークルのロールは、そのローカル トランスバース面が正味加速度 (重力を含む) と一致するように選択されます。ビークルのピッチはパスに一致します。これは通常、二輪車や固定翼航空機で使用されます。

拡張機能

バージョン履歴

R2020b で導入

すべて展開する

参考

オブジェクト