waypointTrajectory
説明
waypointTrajectory
System object™ は、指定されたウェイポイントに基づいて軌跡を生成します。System object を作成すると、各ウェイポイントにおける到着時間、速度、または対地速度を指定することを選択できます。必要に応じて、各ウェイポイントにおける方向などの他のプロパティを指定できます。詳細については、アルゴリズムを参照してください。
ウェイポイントから軌跡を生成するには、以下のようにします。
waypointTrajectory
オブジェクトを作成して、そのプロパティを設定します。関数と同様に、オブジェクトを呼び出します。
System object の機能の詳細については、System object とはを参照してください。
作成
構文
説明
は、既定の静止ウェイポイントに基づいて軌跡を生成する System object である trajectory
= waypointTrajectorytrajectory
を返します。
は、生成された軌跡が各ウェイポイントを通過する到着時間を指定します。詳細については、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 でのシステム設計を参照してください。
SampleRate
— 軌跡のサンプル レート (Hz)
100
(既定値) | 正のスカラー
軌跡のサンプル レート (Hz)。正のスカラーとして指定します。
調整可能: Yes
データ型: double
SamplesPerFrame
— 出力フレームあたりのサンプル数
1
(既定値) | 正のスカラー整数
出力フレームあたりのサンプル数。正のスカラー整数として指定します。
データ型: double
Waypoints
— ナビゲーション座標系における位置 (m)
N 行 3 列の行列
ナビゲーション座標系における位置 (メートル単位)。N 行 3 列の行列として指定します。行列の列はそれぞれ、第 1 軸、第 2 軸、および第 3 軸に対応します。行列の N 番目の行は、個々のウェイポイントに対応します。
ヒント
軌跡を特定のウェイポイントで待機させるには、次の 2 つのオプションのいずれかを使用します。
入力引数
TimeOfArrival
を指定した場合は、連続する 2 行でウェイポイントの座標を繰り返します。入力引数
TimeOfArrival
を指定しなかった場合は、WaitTime
プロパティを使用して待機時間を指定します。
データ型: double
TimeOfArrival
— 各ウェイポイントにおける時間 (s)
増加する非負の数値の N 要素の列ベクトル
各ウェイポイントへの到着に対応する時間 (秒単位)。N 要素の列ベクトルとして指定します。サンプル数 N は、Waypoints
で定義されたサンプル (行) 数と同じでなければなりません。
依存関係
このプロパティを設定するには、次のプロパティを指定してはなりません。
JerkLimit
InitialTime
WaitTime
データ型: double
Velocities
— 各ウェイポイントにおけるナビゲーション座標系での速度 (m/s)
N 行 3 列の行列
各ウェイポイントにおけるナビゲーション座標系での速度 (メートル/秒単位)。N 行 3 列の行列として指定します。行列の列はそれぞれ、第 1 軸、第 2 軸、および第 3 軸に対応します。サンプル数 N は、Waypoints
で定義されたサンプル (行) 数と同じでなければなりません。
速度を非ゼロの値として指定した場合、オブジェクトは速度に基づいて軌跡のコースを自動的に計算します。速度をゼロに指定した場合、オブジェクトは隣接するウェイポイントから軌跡のコースを推定します。
データ型: double
Course
— 水平方向の進行方向 (度)
N 要素の実数ベクトル
水平方向の進行方向。度単位の N 要素の実数ベクトルとして指定します。サンプル数 N は、Waypoints
で定義されたサンプル (行) 数と同じでなければなりません。Velocities
も Course
も指定されていない場合、コースはウェイポイントから推定されます。
依存関係
このプロパティを設定するには、Velocities プロパティを指定してはなりません。
データ型: double
GroundSpeed
— 各ウェイポイントにおける対地速度 (m/s)
N 要素の実数ベクトル
各ウェイポイントにおける対地速度。m/s 単位の N 要素の実数ベクトルとして指定します。このプロパティが指定されていない場合は、ウェイポイントから推定されます。サンプル数 N は、Waypoints
で定義されたサンプル (行) 数と同じでなければなりません。
前進運動をレンダリングするには、正の対地速度値を指定します。
後退運動をレンダリングするには、負の対地速度値を指定します。
逆方向の運動をレンダリングするには、正の対地速度値と負の対地速度値をゼロの対地速度値で区切ります。
依存関係
このプロパティを設定するには、Velocities プロパティを指定してはなりません。
データ型: double
ClimbRate
— 各ウェイポイントにおける上昇率 (m/s)
N 要素の実数ベクトル
各ウェイポイントにおける上昇率 (メートル/秒単位)。N 要素の実数ベクトルとして指定します。サンプル数 N は、Waypoints
で定義されたサンプル (行) 数と同じでなければなりません。Velocities
も Course
も指定されていない場合、上昇率はウェイポイントから推定されます。
依存関係
このプロパティを設定するには、Velocities プロパティを指定してはなりません。
データ型: double
JerkLimit
— 縦加加速度の範囲 (m/s3)
Inf
(既定値) | 正のスカラー
縦加加速度の範囲。m/s3 単位の正のスカラーとして指定します。加加速度は加速度の時間微分です。このプロパティを指定すると、オブジェクトは加加速度の範囲に基づいて水平の台形加速度プロファイルを生成します。waypointTrajectory
オブジェクトが指定された JerkLimit
を達成できない場合、オブジェクトはエラーを出します。このプロパティは、オブジェクトの作成時にのみ設定できます。
依存関係
このプロパティを設定するには、TimeOfArrival プロパティを指定してはなりません。
データ型: double
InitialTime
— 軌跡が開始するまでの時間 (s)
0
(既定値) | 非負のスカラー
軌跡が開始するまでの時間。秒単位の非負のスカラーとして指定します。オブジェクトは、軌跡が開始するまで、位置や速度などの数量を NaN
として報告します。このプロパティは、オブジェクトの作成時にのみ設定できます。
依存関係
このプロパティを設定するには、TimeOfArrival プロパティを指定してはなりません。代わりに、オブジェクトの作成時に GroundSpeed
プロパティまたは Velocities
プロパティを指定する必要があります。
データ型: double
WaitTime
— 各ウェイポイントにおける待機時間 (s)
0
の N 要素ベクトル (既定値) | 非負のスカラーの N 要素ベクトル
各ウェイポイントにおける待機時間。非負のスカラーの N 要素ベクトルとして指定します。N は、Waypoints
で定義したサンプル (行) 数と同じでなければなりません。このプロパティは、オブジェクトの作成時にのみ設定できます。
依存関係
このプロパティを設定するには、TimeOfArrival プロパティを指定してはなりません。
TimeOfArrival プロパティを指定した場合、このプロパティを使用して待機時間を指定することはできません。代わりに、Waypoints プロパティの連続する 2 行でウェイポイントの座標を繰り返して待機時間を指定します。
データ型: double
Orientation
— 各ウェイポイントにおける方向
N 要素の四元数列ベクトル | 実数の 3×3×N の配列
各ウェイポイントにおける方向。N 要素の quaternion
列ベクトルまたは実数の 3×3×N の配列として指定します。各 quaternion
のノルムは 1
でなければなりません。3 行 3 列の各回転行列は正規直交行列でなければなりません。四元数または回転行列の数 N は、Waypoints
で定義されたサンプル (行) 数と同じでなければなりません。
Orientation
が四元数で指定された場合、基となるクラスは double
でなければなりません。
データ型: double
AutoPitch
— ピッチ角を運動方向に合わせる
false
(既定値) | true
ピッチ角を運動方向に合わせるかどうか。true
または false
として指定します。true
に指定すると、ピッチ角が運動方向に自動的に合わせられます。false
に指定すると、ピッチ角はゼロ (水平方向) に設定されます。
依存関係
このプロパティを設定するには、Orientation プロパティを指定してはなりません。
AutoBank
— ロール角を調整して向心力に抗する
false
(既定値) | true
ロール角を調整して向心力に抗するかどうか。true
または false
として指定します。true
に指定すると、ロール角が自動的に向心力に抗します。false
に指定すると、ロール角はゼロ (フラット方向) に設定されます。
依存関係
このプロパティを設定するには、Orientation プロパティを指定してはなりません。
ReferenceFrame
— 軌跡の基準座標系
'NED'
(既定値) | 'ENU'
軌跡の基準座標系。'NED'
(北-東-下) または 'ENU'
(東-北-上) として指定します。
使用法
説明
[
は、指定された作成引数とプロパティに基づいて軌跡データの座標系を出力します。position
,orientation
,velocity
,acceleration
,angularVelocity
] = trajectory()
出力引数
position
— ローカル ナビゲーション座標系における位置 (m)
M 行 3 列の行列
orientation
— ローカル ナビゲーション座標系における方向
M 要素の四元数列ベクトル | 3×3×M の実数配列
ローカル ナビゲーション座標系における方向。M 行 1 列の quaternion
列ベクトルまたは 3×3×M の実数配列として返されます。
各四元数または 3 行 3 列の回転行列は、ローカル ナビゲーション座標系から現在のボディ座標系への座標系の回転です。
M は SamplesPerFrame プロパティで指定します。
データ型: double
velocity
— ローカル ナビゲーション座標系の速度 (m/s)
M 行 3 列の行列
acceleration
— ローカル ナビゲーション座標系における加速度 (m/s2)
M 行 3 列の行列
ローカル ナビゲーション座標系における加速度 (メートル毎秒毎秒単位)。M 行 3 列の行列として返されます。
M は SamplesPerFrame プロパティで指定します。
データ型: double
angularVelocity
— ローカル ナビゲーション座標系における角速度 (rad/s)
M 行 3 列の行列
オブジェクト関数
オブジェクト関数を使用するには、System object を最初の入力引数として指定します。たとえば、obj
という名前の System object のシステム リソースを解放するには、以下の構文を使用します。
release(obj)
waypointTrajectory
に固有
waypointInfo | ウェイポイント情報の table を取得する |
lookupPose | 特定時間の姿勢情報を取得する |
perturbations | Perturbation defined on object |
perturb | 摂動をオブジェクトに適用する |
例
既定の waypointTrajectory
の作成
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"));
trajectory
で waypointInfo
を呼び出して、指定した制約の table を返します。作成プロパティ Waypoints
、TimeOfArrival
、および 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
生成された軌跡は空間的範囲内に収まるようになっている一方で、軌跡の加速度と角速度が多少不安定になっていることに注意してください。これはウェイポイントの指定過剰が原因です。
waypointTrajectory
を使用したレーストラックの軌跡の生成
次のようなレーストラックの軌跡を考えます。
軌跡の 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 と 10 です。加加速度の範囲を 0.5 に制限して、台形加速度プロファイルを有効にします。
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
プロパティの値によって制御されます。AutoBank
AutoPitch
説明 false
false
ビークルは常に水平です (ピッチとロールがゼロ)。これは通常、大型船舶で使用されます。 false
true
ビークルのピッチはパスに一致し、ビークルのロールは常にゼロです。これは通常、地上ビークルで使用されます。 true
false
ビークルのピッチとロールは、そのローカル z 軸が正味加速度 (重力を含む) と一致するように選択されます。これは通常、回転翼航空機で使用されます。 true
true
ビークルのロールは、そのローカル トランスバース面が正味加速度 (重力を含む) と一致するように選択されます。ビークルのピッチはパスに一致します。これは通常、二輪車や固定翼航空機で使用されます。
拡張機能
C/C++ コード生成
MATLAB® Coder™ を使用して C および C++ コードを生成します。
オブジェクト関数 waypointInfo
ではコード生成はサポートされていません。
使用に関するメモと制限:
MATLAB コード生成における System object (MATLAB Coder)を参照してください。
バージョン履歴
R2020b で導入R2023a: 対地速度または速度入力と新しいプロパティを使用した waypointTrajectory
の指定
waypointTrajectory
オブジェクトの作成時に、速度または対地速度の入力を指定した場合、到着時間の入力が不要になりました。到着時間の入力を指定しない場合は、次の新しいプロパティを使用できます。
JerkLimit
— 軌跡加加速度の縦の範囲。加加速度は並進加速度の微分です。加加速度範囲に有限の値を指定すると、waypointTrajectory
は、JerkLimit
に基づいて水平の台形加速度プロファイルを生成します。InitialTime
— 軌跡が開始するまでの時間。非ゼロに指定すると、waypointTrajectory
は初期時間だけ軌跡の開始を遅らせます。WaitTime
— 各ウェイポイントにおける待機時間。ウェイポイントに対して非ゼロに指定すると、waypointTrajectory
はウェイポイントで待機します。
R2022b: ウェイポイント軌跡における待機および逆方向の運動の指定
waypointTrajectory
System object を使用して、待機および逆方向の運動を指定できるようになりました。
軌跡を特定のウェイポイントで待機させるには、
Waypoints
プロパティを指定するときに、ウェイポイントの座標を連続する 2 行で繰り返すだけです。逆方向の運動をレンダリングするには、
GroundSpeed
プロパティの正 (前進) と負 (後退) の対地速度値をゼロ値で区切ります。
MATLAB コマンド
次の MATLAB コマンドに対応するリンクがクリックされました。
コマンドを MATLAB コマンド ウィンドウに入力して実行してください。Web ブラウザーは MATLAB コマンドをサポートしていません。
Select a Web Site
Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .
You can also select a web site from the following list:
How to Get Best Site Performance
Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)