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

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

先に結論

移動マニピュレーターの全身動作計画では、台車とアームを別々に計画するのではなく、台車姿勢とアーム関節を結合した一つの構成空間として扱うのが基本です。平面移動なら台車状態を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と可到達性を評価し、目標を実現できる候補だけを残します。その後、Planning Sceneなどで台車・アーム・把持物・環境の衝突を判定し、安定性評価を加えた全身計画または軌道最適化を行います。最後に補間された軌道全体を検証し、環境やロボット状態が変化した場合は再計画します。

台車を大きく移動でき、作業空間にも余裕があるなら、階層方式から始めやすいでしょう。一方、棚の前などで台車位置とアーム姿勢を同時に調整する作業、アームや把持物の掃引体積が大きい作業では、全身方式またはハイブリッド方式が適しています。

制約と失敗しやすい点

第一に、Nav2のフットプリントだけで全身衝突を判定しないことです。アームや把持物を含めるには、3Dのロボット形状と環境形状を同じ衝突モデルで扱う必要があります。台車ナビゲーションとアーム操作計画では、ロボット状態、環境、必要に応じて予約済み空間を共有する設計が必要です。

第二に、アーム姿勢を決めた後で安定性を確認するだけにしないことです。全身軌道の各時刻で、支持多角形と重心投影、または設定した転倒余裕を検証します。ただし、急停止、旋回、加減速、把持物の揺動を含む動的安定性には、対象ロボットに応じた別のモデルが必要です。

第三に、MoveItの標準設定だけで、台車フットプリント、アームの掃引体積、ペイロード依存の転倒安定性、動的な接触力制約まで一括して保証できるとは考えないことです。カスタム妥当性チェッカー、共有ワールドモデル、または全身最適化層として必要な条件を明示的に組み込むべきです。台車のplanar jointやfloating jointをどこまで扱えるかも、使用するディストリビューション、プランナー、設定ごとに検証します。

採用判断の基準

次の条件が多いほど、全身構成空間での計画を優先します。

• 台車位置がエンドエフェクタの可到達性を大きく左右する
• アームや把持物が障害物の近くを通る
• アーム姿勢やペイロードによって重心が変化する
• 台車移動量、操作性、クリアランスを同時に調整したい
• 実行中に環境やロボット状態が変わり、再計画が必要になる

台車姿勢とアーム動作が十分に分離され、障害物が少なく、安定性も姿勢によらず一定とみなせる場合は、階層方式で始める判断もできます。ただし最終軌道では、台車だけでなくアームと把持物を含む衝突・安定性を検証します。

まとめ

移動マニピュレーターでは、台車姿勢とアーム関節を一つの状態として表し、可到達性、全身衝突、支持多角形や転倒余裕を同じ状態・軌道検証ループで評価することが基本です。Nav2のフットプリントとMoveItのPlanning Sceneは役割が異なるため、共有状態モデルで接続し、安定性と軌道区間の妥当性を追加します。衝突・安定性をハード制約、効率や操作性を最適化対象として分離すると、作業環境に応じた計画方式を選びやすくなります。

参考情報

• Planning Scene - MoveIt 2 Documentation
• Motion Planning with Constraints - MoveIt 2 Documentation
• Footprint - Nav2 Documentation
• MoveIt!: An Introduction
• The Open Motion Planning Library
• Task Space Regions: A Framework for Pose-Constrained Manipulation Planning
• A New Measure of Tipover Stability Margin for Mobile Manipulators

全身計画は、自由度を増やすことよりも、状態と制約を同じモデルで検証できるかが鍵になります。

AI利用に関する注記

本サイトの記事は、複数のAIエージェントによる調査・執筆・レビューを経て公開しています。正確性と品質の向上に努めていますが、内容については必ず一次情報をご確認ください。