matlab-plan-robot-motion

マニピュレーターのモーションプランニング

マニピュレーターの衝突のない経路を計画し、速度と加速度の制限を満たす時間最適な軌道に変換します。

使用する場面

• 「軌道」「モーション」「経路」「プラン」のどの表現を使っているかにかかわらず、関節構成や直交座標系での姿勢間でロボットアームを移動させる場合
• ウェイポイント間の軌道（時間最適、一定速度、台形速度、最小ジャーク）を生成する場合
• 構成間の衝突のない経路を計画する場合
• 計画用の衝突環境として障害物をモデル化する場合
• モーションプランナー（RRT、CHOMP、dlCHOMP）を選択する場合
• 軌道に衝突がないことを検証する場合
• ロボットが空間内を移動し、作業空間にジオメトリが存在するあらゆるタスク。ユーザーが明示していなくても、衝突安全性は暗黙の要件です

使用しない場面

• ロボットモデルの構築、読み込み、インポート — /matlab-model-robot-kinematics を使用してください
• IK/FK の計算や解の検証 — /matlab-model-robot-kinematics を使用してください
• グリッパーの取り付けやエンドエフェクターの設定 — /matlab-model-robot-kinematics を使用してください
• 移動ロボットのナビゲーション（車輪付きプラットフォーム、地上車両）
• 動力学シミュレーション（inverseDynamics、forwardDynamics）
• プランナーのコード生成／MEX デプロイ

前提条件

計画を始める前に、以下を満たしていることを確認してください。

0. 必須：最初に /matlab-model-robot-kinematics を呼び出す — ロボットモデルが MATLAB ワークスペースに存在し、かつそのスキルでセットアップされるまで、以下の手順に進まないでください。モーションプランニングのワークフロー内で loadrobot、importrobot を呼び出したり、rigidBodyTree を直接構築したりしないでください。些細に見える場合でも例外はありません。このモデリングスキルでは、衝突メッシュの検証、グリッパーの選択、EE フレームの位置合わせ、既知の問題（例：関節式グリッパーがプランナーで自己衝突を起こすこと）に対処します。これを省くと、不要な試行を繰り返すことになります。

1. ロボットに衝突メッシュがある — 衝突ジオメトリのないロボットでは、実際には衝突する経路をプランナーが「衝突なし」と報告することがあります。次の方法で確認してください。

bodies = robot.Bodies;
hasCollision = any(cellfun(@(b) ~isempty(b.Collisions), bodies));
assert(hasCollision, "Robot has no collision meshes — planning results will be invalid.");

2. DataFormat="row" — すべての計画関数は行ベクトルの構成を想定しています。次の方法で確認してください。

assert(strcmp(robot.DataFormat, "row"), "Set DataFormat to 'row' before planning.");

ワークフロー

ステップ 0：衝突リスクを評価する

軌道ツールを選ぶ前に、作業空間にロボットが衝突する可能性のあるジオメトリがあるかを判断します。デフォルトの前提：ロボットの近くに物体（テーブル、ワーク、治具、ツール）がある場合は、衝突チェックが必須です — ユーザーが「軌道」や「補間」だけを依頼した場合でも同様です。

Does the workspace contain any geometry (workpiece, table, fixture, tool)?
├── YES → Model it as collision objects (Step 1) and either:
│         ├── Use a planner (Steps 2–3) if configs are far apart or path is non-trivial
│         └── Interpolate directly (Step 4) BUT verify collision-free (Step 5) — if collisions
│             are found, go back and use a planner
└── NO (free space, no objects) → Skip to Step 4 (trajectory generation)

関節の直線補間が安全だと決して決めつけないでください。 近接した構成間でも、中間姿勢がワークに入り込むことがあります。特に、溶接、機械加工、組み立てなど、表面近くで動作する 6-DOF アームでは注意が必要です。チェックのコストは低く、衝突のコストは高くなります。

ステップ 1：環境をモデル化する

利用できるデータと使用するプランナーに基づいて、表現方法を選択します。

表現	作成方法	対応プランナー	最適な用途
衝突オブジェクトのセル配列	collisionBox、collisionCylinder、collisionSphere、collisionMesh	manipulatorRRT	既知の幾何学的障害物（テーブル、壁、ビン）
occupancyMap3D	occupancyMap3D + insertPointCloud	manipulatorRRT（Map プロパティ経由）	点群またはセンサーデータ
meshtsdf	geom2struct の出力から meshtsdf	manipulatorCHOMP	滑らかなコストが必要なメッシュベースの環境
SphericalObstacles	N×4 行列 [x y z radius]	manipulatorCHOMP	単純な球近似

衝突オブジェクトのパターン：

% Table top surface at z = -0.01 (center at z = -0.02 for a 0.02 m thick box)
table = collisionBox(1.2, 0.8, 0.02);
table.Pose = trvec2tform([0.5, 0, -0.02]);

pillar = collisionCylinder(0.05, 0.6);
pillar.Pose = trvec2tform([0.4, 0.0, 0.3]);

env = {table, pillar};

occupancyMap3D のパターン：

omap = occupancyMap3D(20);
sensorPose = [0 0 0 1 0 0 0];
insertPointCloud(omap, sensorPose, pointCloudData, 5.0);

meshtsdf のパターン（CHOMP 用）：

meshStructs = geom2struct(env);
mTSDF = meshtsdf(meshStructs, Resolution=20, TruncationDistance=0.2);

TruncationDistance は、ロボットの衝突球の最大半径より大きくする必要があります。そうしないと、CHOMP が近接を正しく検出できません。

ステップ 2：プランナーを選択する

主な判断基準 — 環境の種類は何ですか？

What is the environment representation?
├── Simple primitives (boxes, cylinders, spheres) → manipulatorRRT
├── Dense mesh / CAD STLs → manipulatorCHOMP (meshtsdf is its native input)
├── Point cloud / sensor data → manipulatorRRT (via occupancyMap3D)
└── Just a few spheres → manipulatorCHOMP (SphericalObstacles, lightweight)

次の判断基準 — パスのトポロジー：

Does the path require going "around" obstacles (large joint-space excursion)?
├── YES → Favor manipulatorRRT (global search won't get stuck in local minima)
└── NO (small adjustment to avoid nearby obstacle) → Either works; CHOMP may be faster

デフォルト：manipulatorRRT — あらゆる種類の環境に対応し、堅牢な衝突判定を備えており、その出力を contopptraj にスムーズに渡せます。shorten は遠回りを減らし、contopptraj は速度／加速度の制限を守りますが、どちらもパス形状の急激な方向転換を滑らかにはしない点に注意してください。滑らかな関節空間のパスが重要な場合（ジャークや機械的摩耗の低減など）は、CHOMP が適しています。CHOMP は滑らかさのコストと衝突回避のコストを同時に最適化し、本質的に滑らかな軌道を生成します。

詳細なチューニングパラメーターとパフォーマンス特性については、references/planner-comparison.md を参照してください。

ステップ 3：パスを計画する

manipulatorRRT で計画する前に： 無駄な計画反復を避けるため、あらかじめ自己衝突のスキップ対象ペアを解決してください。

1. [isColl,sepDist,witPts] = checkCollision(robot, startConfig, {}, SkippedSelfCollisions="parent") — isColl(1)==0 の場合は、"parent" を使用します。
2. そうでない場合は、実測に基づいてスキップリストを作成します。references/self-collision-skip-procedure.md を参照してください。

manipulatorRRT：

rrt = manipulatorRRT(robot, env, SkippedSelfCollisions=skippedSelfCollisions);
rrt.MaxConnectionDistance = 0.3;
rrt.ValidationDistance = 0.05;
rrt.MaxIterations = 5000;
rrt.EnableConnectHeuristic = true;

rng(0, "twister");
path = plan(rrt, startConfig, goalConfig);
shortenedPath = shorten(rrt, path, 20);

plan の後には必ず shorten を呼び出してください。RRT のパスは本質的にぎざぎざしています。第 2 引数は短縮反復回数です（デフォルト値として 20 が適切です。複雑な環境では増やしてください）。

occupancyMap3D を使用した manipulatorRRT：

rrt = manipulatorRRT(robot, {}, Map=omap);
rrt.MaxConnectionDistance = 0.2;
rrt.ValidationDistance = 0.02;

マップのみを使用する場合、衝突オブジェクトには空のセル {} を渡してください。

manipulatorCHOMP：

chomp = manipulatorCHOMP(robot, MeshTSDF=mTSDF, ...
    SkippedSelfCollisions="parent");
chomp.SmoothnessOptions.Weight = 100;
chomp.CollisionOptions.CollisionCostWeight = 10;
chomp.SolverOptions.MaxIterations = 200;

trajectory = optimize(chomp, startConfig, goalConfig);

MeshTSDF は構築時に渡す必要があります。作成後は読み取り専用です。

自己衝突の処理： 構築時に SkippedSelfCollisions を渡してください。構築後に設定すると、型の不一致が発生することがあります（特にコード生成のコンテキスト）。

自己衝突の問題を回避するために、clearCollision を使用してはいけません。 衝突メッシュを削除すると、対象ボディの衝突判定が警告なしに無効になります。実際には衝突するパスでも、プランナーは「衝突なし」と報告します。

方針： まず SkippedSelfCollisions="parent" を試してください。有効な構成でもプランナーが自己衝突を報告する場合は、references/self-collision-skip-procedure.md の手順に従い、実測に基づいてスキップリストを作成してください。

checkCollision についての重要な事実（誤解されやすい点）：

• separationDist のボディ順序：行／列 1:m は順に robot.Bodies{1:m} です。インデックス m+1 は robot.BaseName です。ベースは最初ではなく最後です。
• NaN = ペアが衝突しています。Inf = ペアのスキップに成功しました。
• isColliding は、行列に NaN が 1 つでもあると true を返します。スキップリストで、すべての NaN エントリを解消する必要があります。リストに含めたペアに NaN が残っている場合、スキップは適用されていません（名前のスペルと char 型／string 型を確認してください）。
• 1×2 のセルに空白文字を入れた {" "," "} は、実際のペアをスキップせずに、デフォルトの親スキップ動作を抑制するダミーのスキップペアです。
• サンプル間で常に NaN になるペアを探す方法では、親子間の重なり（衝突率約 100%）を検出できますが、パラレルリンク式グリッパー（例：robotiq2F85）には、約 70～90% の頻度で発生する追加のペアがあり、これらもスキップする必要があります。最初の処理後にホーム構成を再確認し、まだ衝突している場合は、残りの NaN ペアを反復的に追加してください。

Step 4: 軌道を生成する（経路を時間パラメーター化する）

アプリケーションの要件に応じて選択します。

要件	ツール	主な特性
関節制限を守りながら最短時間で動作	contopptraj	TOPPRA アルゴリズム — 真の時間最適化
作業領域内で一定速度（溶接、切断、塗布）	trapveltraj	加速度ゼロの巡航フェーズを保証
ジャークを最小限に抑えた滑らかな動作（障害物なし）	minjerkpolytraj	∫jerk² コストを最小化

contopptraj（時間最適）：

velLimits = [-2*ones(numJoints,1), 2*ones(numJoints,1)];
accelLimits = [-5*ones(numJoints,1), 5*ones(numJoints,1)];
[q, qd, qdd, tSamples] = contopptraj(shortenedPath', velLimits, accelLimits);

trapveltraj（一定速度での巡航）：

[q, qd, qdd, tSamples] = trapveltraj(shortenedPath', 500, AccelTime=0.4);

AccelTime は加速時間を設定します。加速と減速の間はすべて一定速度です。明示的に巡航速度を設定するには、PeakVelocity を使用します。

minjerkpolytraj（滑らかな補間）：

tPoints = linspace(0, 3, size(shortenedPath', 2));
[q, qd, qdd, ~, tSamples] = minjerkpolytraj(shortenedPath', tPoints, 500);

重要な注意事項：

• 3 つとも numJoints×numWaypoints の形式を想定します。RRT の出力（shortenedPath'）を転置してください
• いずれも numJoints×numSamples の形式で返します
• 手動の台形補間コードや interp1 + gradient は使用しないでください。これらのツールが代わりになります
• contopptraj は一定速度を保証しません。trapveltraj は時間を最小化しません。適切なものを選んでください

Step 5: 軌道に衝突がないことを確認する

プランナーはウェイポイントを検証しますが、contopptraj はウェイポイント間を補間します。軌道全体を検証してください。

% q from contopptraj is numJoints×numSamples — transpose for checkCollision (expects row vectors)
qRows = q';
inCollision = false;
for i = 1:size(qRows, 1)
    result = checkCollision(robot, qRows(i,:), env, SkippedSelfCollisions="parent");
    % result is 1×2: [selfCollision, worldCollision] — check BOTH
    if any(result)
        warning("Collision at trajectory sample %d (t=%.3f s), self=%d world=%d", ...
            i, tSamples(i), result(1), result(2));
        inCollision = true;
        break;
    end
end
if ~inCollision
    fprintf("Trajectory is collision-free (%d samples verified).\n", size(qRows,1));
end

補間された軌道で衝突が見つかった場合は、プランナーの ValidationDistance を大きくし（値が小さいほど計画中のチェックが細かくなります）、再計画してください。

Step 6: 可視化する

figure;
show(robot, q(1,:), Frames="off");
hold on;
for k = 1:numel(env)
    show(env{k});
end
title("Motion Plan — Start Configuration");
axis auto;
view(45, 30);

軌道をアニメーション表示します。

figure;
ax = show(robot, q(1,:), Frames="off");
hold on;
for k = 1:numel(env)
    show(env{k});
end
axis auto;
view(45, 30);
title("Motion Plan Animation");

for i = 1:10:size(q,1)
    show(robot, q(i,:), Frames="off", PreservePlot=false, Parent=ax);
    drawnow;
end

主な関数

関数	目的	ツールボックス	利用可能なバージョン
manipulatorRRT	サンプリングベースの衝突回避プランナー	Robotics System Toolbox	R2021b
manipulatorCHOMP	最適化ベースの滑らかなプランナー	Robotics System Toolbox	R2023a
contopptraj	時間最適軌道（TOPPRA）	Robotics System Toolbox	R2023b
trapveltraj	台形速度プロファイル（一定速度での巡航）	Robotics System Toolbox	R2019a
minjerkpolytraj	最小ジャーク多項式軌道	Robotics System Toolbox	R2022a
checkCollision	ロボットと環境の衝突を確認	Robotics System Toolbox	R2020b
collisionBox	ボックス型衝突プリミティブ	Robotics System Toolbox	R2019b
collisionCylinder	円柱型衝突プリミティブ	Robotics System Toolbox	R2019b
collisionSphere	球型衝突プリミティブ	Robotics System Toolbox	R2019b
collisionMesh	メッシュ型衝突プリミティブ	Robotics System Toolbox	R2019b
occupancyMap3D	点群から 3D ボクセルマップを生成	Navigation Toolbox	R2019b
meshtsdf	メッシュから切り詰め符号付き距離場を生成	Robotics System Toolbox	R2023a
geom2struct	衝突ジオメトリをメッシュ構造体に変換	Robotics System Toolbox	R2023a
manipulatorStateSpace	プランナー用のカスタム状態空間	Robotics System Toolbox	R2021b
plannerBiRRT	双方向 RRT（状態空間を使用）	Navigation Toolbox	R2020b

よくある間違い

間違い	誤りである理由	正しい方法
時間最適化が必要なときに trapveltraj を使う	台形プロファイルは時間最適ではありません。時間を過剰に割り当てます	contopptraj（TOPPRA）を使います。一定速度が目的の場合は、trapveltraj が適切です
一定速度が必要なときに contopptraj を使う	TOPPRA は時間を最小化するために速度を連続的に変化させるため、一定速度で巡航する区間はありません	溶接、切断、塗布の用途では、AccelTime を指定して trapveltraj を使います
台形速度プロファイルの時間を手動で計算する	すでに解決されている問題を誤って再実装することになり、20 行以上のコードを 1 回の呼び出しに置き換えられます	必要に応じて contopptraj または trapveltraj を使います
軌道に interp1 + gradient を使う	数値微分はノイズが多く、制限も考慮しません	適切な軌道関数を使います。いずれも q、qd、qdd を解析的に返します
「単純な」補間の衝突チェックを省略する	近接した構成でも、中間姿勢がワークピースと衝突することがあります。特に表面付近で発生しやすくなります	すべての軌道サンプルで必ず checkCollision を使って検証します（ステップ 5）
自己衝突エラーの修正に clearCollision を使う	対象ボディの衝突チェックが完全に無効になり、プランナーが実際の衝突を黙って無視します	正しいスキップ対象のペアを実測で特定します（上記の自己衝突の処理を参照）
ロボットに衝突メッシュを設定せずに計画する	実際には衝突する経路を、プランナーが「衝突なし」と報告します	計画の前に、robot.Bodies に衝突ジオメトリがあることを確認します
構築後に SkippedSelfCollisions を設定する	型の不一致が発生する可能性があり、コード生成時にも不安定です	プランナーの構築時に名前と値のペアとして渡します
CHOMP の構築後に MeshTSDF を設定する	作成後は読み取り専用のプロパティです	構築時に MeshTSDF=mTSDF を渡します
RRT の plan の後に shorten を呼び出さない	RRT の経路はぎざぎざしており、不要な迂回が含まれます	必ず shorten(rrt, path, 20) を呼び出します
プランナーのウェイポイントだけで衝突を検証する	ウェイポイント間の補間された軌道点が、依然として衝突することがあります	contopptraj の出力サンプルすべてで checkCollision を使って確認します
RRT の経路を転置せずに contopptraj に渡す	manipulatorRRT/plan は N×numJoints（行＝ウェイポイント）を返しますが、contopptraj が期待するのは numJoints×N（列＝ウェイポイント）です	転置します：contopptraj(path', velLimits, accelLimits)
contopptraj の出力 q をそのまま checkCollision に使う	contopptraj は numJoints×numSamples を返しますが、checkCollision が期待するのは行ベクトルです	転置します：checkCollision(robot, q(:,i)', env, ...)。または q' を使います
プランナーで DataFormat="column" を使う	manipulatorRRT/plan は行ベクトルを期待します	計画の前に、ロボットの DataFormat="row" を設定します
checkCollision の出力をスカラーとして扱う	ワールドオブジェクトがある場合、checkCollision は 1×2 の [selfCollision, worldCollision] を返します。if checkCollision(...) として使うと、selfCollision（要素 1）しか検査されず、ワールドとの衝突が黙って無視されます	必ず結果を取得します：result = checkCollision(...) としてから、any(result) を確認するか、ワールドとの衝突のみを調べる場合は result(2) を確認します
meshtsdf の TruncationDistance が小さすぎる	切り詰め距離がロボットの球の半径より小さいと、CHOMP は近接を検出できません	TruncationDistance を最大の衝突球半径より大きく設定します（通常は 0.2 以上）
地面または床の衝突オブジェクトを、上面が z=0 になるように配置する	ロボットのベース衝突メッシュは z=0 より最大 5 mm 下まで伸びています（最悪の場合は universalUR5 の 5 mm、その他の大半は約 1 mm）。さらに、多くの loadrobot のホーム構成ではアームが水平に伸びるため、先端側のリンクが z=0 より下に下がることがあります。どちらも計画開始前にただちにワールド衝突を引き起こします。	地面の上面を z ≤ −0.01 に配置します（すべての loadrobot のベースメッシュを避けられます）。タスクで必要な場合にのみ、地面を追加します。plan を呼び出す前に、必ず checkCollision で開始構成と目標構成に衝突がないことを確認します。衝突する場合は、床を下げるのではなく、IK の初期推定を調整するか、目標位置を高くします。

---

Copyright 2026 The MathWorks, Inc.

---