このページの内容は最新ではありません。最新版の英語を参照するには、ここをクリックします。
waypointTrajectory
ウェイポイント軌跡ジェネレーター
説明
waypointTrajectory System object™ は、指定されたウェイポイントに基づいて軌跡を生成します。System object を作成すると、各ウェイポイントにおける到着時間、速度、または対地速度を指定することを選択できます。必要に応じて、各ウェイポイントにおける方向などの他のプロパティを指定できます。詳細については、アルゴリズムを参照してください。
ウェイポイントから軌跡を生成するには、以下のようにします。
waypointTrajectoryオブジェクトを作成して、そのプロパティを設定します。関数と同様に、オブジェクトを呼び出します。
System object の機能の詳細については、System object とはを参照してください。
作成
構文
説明
trajectory = waypointTrajectorytrajectory を返します。
trajectory = waypointTrajectory(Waypoints,TimeOfArrival)TimeOfArrival プロパティを参照してください。
ヒント
TimeOfArrival 引数を指定した場合は、次のプロパティを指定してはなりません。
JerkLimitInitialTimeWaitTime
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 を作成します。
プロパティ
使用法
説明
[ は、指定された作成引数とプロパティに基づいて軌跡データの座標系を出力します。position,orientation,velocity,acceleration,angularVelocity] = trajectory()
出力引数
オブジェクト関数
オブジェクト関数を使用するには、System object を最初の入力引数として指定します。たとえば、obj という名前の System object のシステム リソースを解放するには、以下の構文を使用します。
release(obj)
例
trajectory = waypointTrajectory
trajectory =
waypointTrajectory with properties:
SampleRate: 100
SamplesPerFrame: 1
Waypoints: [2×3 double]
TimeOfArrival: [2×1 double]
Velocities: [2×3 double]
Course: [2×1 double]
GroundSpeed: [2×1 double]
ClimbRate: [2×1 double]
Orientation: [2×1 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*") axis([-1,2,-1,2]) axis square xlabel("X") ylabel("Y") title("Position") 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=4×3 table
TimeOfArrival Waypoints Orientation
_____________ ____________________ ________________
0 20 20 0 {1×1 quaternion}
3 50 20 0 {1×1 quaternion}
4 58 15.5 0 {1×1 quaternion}
5.5 59.5 0 0 {1×1 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 と 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プロパティの値によって制御されます。AutoBankAutoPitch説明 falsefalseビークルは常に水平です (ピッチとロールがゼロ)。これは通常、大型船舶で使用されます。 falsetrueビークルのピッチはパスに一致し、ビークルのロールは常にゼロです。これは通常、地上ビークルで使用されます。 truefalseビークルのピッチとロールは、そのローカル z 軸が正味加速度 (重力を含む) と一致するように選択されます。これは通常、回転翼航空機で使用されます。 truetrueビークルのロールは、そのローカル トランスバース面が正味加速度 (重力を含む) と一致するように選択されます。ビークルのピッチはパスに一致します。これは通常、二輪車や固定翼航空機で使用されます。
