移動マニピュレーターの全身動作計画：台車とアームを同時に最適化する方法

Tue, 03 Nov 2026 15:28:56 +0900

台車とアームを別々に安全と判断しても、全身では別の問題が現れる点が気になります。

先に結論

移動マニピュレーターの全身動作計画では、台車とアームを別々に計画するのではなく、台車姿勢とアーム関節を結合した一つの構成空間として扱うのが基本です。平面移動なら台車状態をSE(2)、高さや姿勢も変化するならSE(3)として表し、全身状態を q=[q_base,q_arm] とします。

この状態と軌道に対して、エンドエフェクタの可到達性、台車・アーム・把持物を含む衝突、支持多角形に基づく安定性を同じ検証ループで評価します。衝突と安定性はハード制約、可操作性、経路長、台車移動量、クリアランスなどは目的関数またはソフト制約として分けると、設計意図を明確にできます。

MoveItのPlanning Sceneはロボット状態、環境中の衝突物、付着物を管理し、衝突判定に利用できます。一方、Nav2のフットプリントは主に台車外形を2Dコストマップ上で扱う仕組みです。これらだけでアームの掃引体積やペイロード依存の安定性まで自動的に統合されるわけではないため、共有状態モデルと追加の妥当性判定が必要です。

なぜ統合が必要なのか

台車だけを見れば障害物を避けていても、アームのリンクや把持物が接触することがあります。反対に、アームの逆運動学が解けても、その姿勢で重心が支持領域の外側へ移動すれば、転倒余裕が小さくなる可能性があります。台車姿勢が変われば、同じ関節値でもエンドエフェクタの位置、障害物との距離、可到達性が変化するためです。

Nav2では円形または多角形のフットプリントを設定し、台車の衝突・通行可能性をコストマップで評価します。しかし、アームのリンク形状、把持物、アームの動作による掃引体積までは自動的に表しません。MoveItのPlanning Sceneはロボットリンクと環境形状を含む3D衝突モデルに向いていますが、Nav2のフットプリントや安定性評価との同期は別途設計する必要があります。

用語と内部の仕組み

可到達性はアーム単体の性質ではありません。台車姿勢と、エンドエフェクタに許される位置・姿勢の組み合わせとして評価します。Task Space Regionは許容されるエンドエフェクタの位置・姿勢領域を表し、その範囲でのIK可解性を計画へ組み込むために利用できます。

候補状態 q について、まず台車姿勢とアーム関節から全リンクの姿勢を計算します。次に、エンドエフェクタが目標領域に入るか、IKが解けるかを確認します。そのうえで、台車、アーム、把持物と環境の衝突を判定し、支持多角形と重心投影、または設定した転倒余裕を評価します。アーム姿勢、ペイロード、把持物の位置が変わると重心も変わるため、安定性はアーム計画から切り離せません。

サンプリング型計画では、ウェイポイントだけでなく状態間のモーションも検証します。ウェイポイントが安全でも、補間区間でアームや把持物が障害物を横切る可能性があるためです。状態妥当性チェッカーとモーション妥当性チェッカーを分ける構成では、全身軌道の区間にも衝突・安定性検証を適用します。補間分解能、形状近似、連続衝突検出の有無によって検証精度が変わる点には注意が必要です。

計画問題は次のように分けると扱いやすくなります。

ハード制約：関節限界、全身衝突、支持条件、設定した安定余裕
ソフト目的：経路長、台車移動量、可操作性、クリアランス、速度・加速度
目標条件：エンドエフェクタの位置・姿勢、または許容姿勢領域

flowchart LR
  S["環境センサ・ロボット状態"] --> W["共有ワールドモデル\n台車・アーム・把持物・環境・予約済み空間"]
  W --> B["台車候補姿勢・粗い経路の生成"]
  W --> Q["全身状態 q=[q_base,q_arm] の生成"]
  B --> Q
  Q --> IK["全身IK・可到達性評価\nTask Space Region を確認"]
  IK -->|"可解"| C["全身衝突判定\n台車・アーム・把持物・環境"]
  IK -->|"不可解"| R["候補を除外"]
  C -->|"衝突なし"| ST["安定性評価\n支持多角形・重心投影・設定した安定余裕"]
  C -->|"衝突あり"| R
  ST -->|"条件を満たす"| O["軌道最適化\n経路長・台車移動量・可操作性・クリアランス"]
  ST -->|"条件を満たさない"| R
  O --> V["補間軌道全体の検証\n状態と軌道区間の衝突・安定性"]
  V -->|"妥当"| E["軌道実行"]
  V -->|"不妥当"| R
  E --> X["環境またはロボット状態の変化"]
  X --> S

台車、アーム、把持物、環境を共有状態モデルで扱い、可到達性・全身衝突・安定性を状態と軌道区間の両方で検証する計画ループ。

計画方式の比較

方式	特徴	向いている場面	主な制約
別計画	台車経路とアーム動作を独立に生成	単純な環境、動作範囲が明確な場合	全身衝突や安定性の不整合を見落としやすい
階層方式	台車候補や粗い経路を先に作り、全身IK・衝突・安定性で絞り込む	計算量を抑えたい場合	先に除外した台車姿勢を後段で使えない
全身同時最適化	台車とアームの変数を同時に探索・最適化	狭い環境、把持姿勢と台車位置の依存が強い場合	状態空間が大きく、計算量とモデル設計が難しい
ハイブリッド方式	候補生成、全身IK、最適化、軌道検証を組み合わせる	実装と性能のバランスを取りたい場合	各層の状態・環境同期が必要

実装構成と使いどころ