Kinodynamic Planning

• 別名: kinodynamic motion planning
• カテゴリ: Planning & manipulation › Motion planning
• タグ: motion-planning, dynamics, sampling-based-planning, trajectory-optimization, state-space

衝突などの幾何学的制約とシステムのダイナミクスの両方を考慮するモーションプランニング。衝突のない経路だけでなく、時間に沿った制御入力を求める。

定義

キノダイナミック計画（kinodynamic planning）は、障害物や関節可動範囲の回避といった運動学的制約と、加速度、力、トルクの上限といった動力学的制約を、解が同時に満たさなければならないモーションプランニングである。この用語は 1993 年に Donald、Xavier、Canny、Reif によって導入された。キノダイナミックプランナーは、コンフィギュレーション空間で幾何学的な経路を探索して後から時間を割り当てるのではなく、システムのダイナミクスを積分して得られる軌道が初期状態から始まり、自由空間内にとどまり、ゴール領域で終わるような、所要時間と制御関数を探索する。

そのため探索は、位置に加えて速度も含む状態空間で行われ、その次元はしばしばコンフィギュレーション空間の 2 倍になる。主な難しさは、近接する 2 つの状態を一般には直線で結べないことである。それらを結ぶには、ダイナミクスのもとで 2 点境界値問題を解く必要があり、これは二重積分器や Dubins カーのような単純なシステムでは容易だが、一般には難しい。LaValle と Kuffner が導入した kinodynamic RRT のようなサンプリングベースのプランナーは、木を前向きに成長させることでこれを回避する。ノードを 1 つ選び、サンプリングした制御または選んだ制御を短時間適用し、その結果得られる状態を保持するのである。SST のような後の手法は、ステアリング関数を必要とせずに漸近的な準最適性を加えている。

別の手法群は同じ問題を別のやり方で扱う。探索ベースのプランナーは、それぞれが動力学的に実行可能な、あらかじめ計算した動作プリミティブからなる格子を使う。直接コロケーション法やシューティング法を含む軌道最適化の手法は、制御と状態を決定変数として制約付き非線形計画問題を解き、局所的には速く収束するが局所解に陥ることがある。実用的なシステムはしばしば両者を組み合わせ、サンプリングや探索の段階で実行可能な軌道を見つけ、その後に最適化で洗練する。物体を押す場合のようにダイナミクスが不確かなときは、プランナーは学習したシステムモデルや残差モデルを使い、オープンループの制御で計画を立て、それをフィードバックで修正することがある。

重要な理由

速いロボットや重いロボット、自動車、ドローン、押される物体は、任意の幾何学的経路をたどれるわけではない。運動量やアクチュエータの制限を無視した経路は実行できないことがある。キノダイナミック計画は、システムが実際に追従できる軌道を生成し、ロボットが高速に動く場合や制御能力が限られている場合に特に重要となる。

主な用途

• 自動車などの非ホロノミックな車両の軌道計画
• ドローンや固定翼機のアグレッシブな飛行計画
• 一連の押し動作の計画などの非把持マニピュレーション
• トルクと速度の制限に近い高速なアーム動作
• 接触ダイナミクスを考慮した脚型ロボットやヒューマノイドのモーションプランニング
• 宇宙機や水中ビークルの機動

実際の使われ方

ロボットアームが、箱をつかまずにテーブルの上を押してゴール領域まで運び、障害物を避けなければならない。チームは、接触点、方向、継続時間といった押し動作のパラメータの関数として箱の動きをモデル化し、学習した残差で補正した解析的な押し動作モデルを使う。kinodynamic RRT は現在の箱の状態から押し動作をサンプリングし、それぞれをモデルで伝播させて、得られた箱の姿勢を木に加え、障害物に衝突する押し動作は破棄する。ある枝がゴール領域に到達すると一連の押し動作を実行し、押すたびに箱の姿勢を観測し直すことで、モデルが間違っていた場合には実際の状態から再計画できるようにする。

よくある誤解

• キノダイナミック計画とは、幾何学的な計画の後に時間パラメータ化を行うことである — 分離したパイプラインでは、動力学的に実行可能などの軌道でも追従できない経路が生じうるが、キノダイナミックプランナーは探索の中でダイナミクスを考慮する
• どんなサンプリングベースのプランナーも、距離尺度を入れ替えればキノダイナミックにできる — 2 つの状態を結ぶには、一般に直線を引くのではなく、境界値問題を解くか制御を前向きに伝播させる必要がある
• キノダイナミックな計画はオープンループで実行できる — モデル誤差と外乱が蓄積するため、計画は通常フィードバックコントローラで追従するか、再計画する

知っておくべき人

• 車両、ドローン、アームのモーションプランニングのエンジニア
• 押し動作などの動的なタスクに取り組むマニピュレーションの研究者
• 自動運転の計画のエンジニア
• コンフィギュレーション空間を超えたプランニングを学ぶロボティクスの学生

関連技術

• Kinodynamic RRT
• Stable Sparse RRT（SST）
• ステートラティスプランナー（State Lattice Planners）
• Open Motion Planning Library（OMPL）
• 軌道最適化（Trajectory Optimization）

関連用語

• Motion Planning
• Rapidly-exploring random tree
• Configuration Space
• Direct Collocation
• Double Integrator
• Nonprehensile Manipulation

関連プロジェクト

• Drake: MIT robotics toolbox for model-based dynamics, optimization, and control system design
• OMPL: Sampling-based motion planning library powering MoveIt and countless robotics applications
• MoveIt 2: Motion planning framework for ROS 2 robot arms, with kinematics and collision checking
• PyRoki: Modular Python toolkit for kinematic optimization across CPU, GPU, and TPU via JAX
• mink: Python inverse kinematics library built on MuJoCo supporting 20+ robot platforms
• Ruckig: Real-time jerk-constrained time-optimal trajectory generation for robots and machines

動画

• 6.8210 Spring 2024 Lecture 18: Sampling-based (kinodynamic) motion planning: underactuated