全身制御とヒューマノイド

なぜ「全身制御」という専用の枠組みが要るのか

ヒューマノイドは、両脚・両腕・胴・首を合わせると30個を超える関節を持つ高冗長な機構です。ここで「右手を目標位置へ動かす」という作業を考えると、手先位置は6自由度しか拘束しないのに使える関節ははるかに多く、余った自由度（冗長自由度）をどう使うかが未定のまま残ります。同時にロボットは、脚で床を踏んで転倒しないという別種の要求も満たさなければなりません。腕の作業と脚のバランスを別々の制御器で担当させると、腕が動いた反作用で重心が動いてバランス制御と干渉し、互いに指令を打ち消し合います。

横にスクロール

全身制御（Whole-Body Control, WBC）は、これらの要求を独立したタスクとして一つの最適化問題にまとめ、全関節トルクを一括で決める枠組みです。鍵は、ヤコビ行列と操作性で扱ったヌル空間射影を多段に積み上げ、優先度の高いタスク（転倒回避・接触維持）を低いタスク（手先作業）より確実に守る「階層タスク制御」にあります。

冗長自由度と階層タスク：ヌル空間の再利用

あるタスク（たとえば手先速度 ẋ1 = J1 q̇）を実現する関節速度の一般解は、最小ノルム解に、そのタスクを一切変化させない成分を加えた形で書けます。この「変化させない成分」がヤコビアンのヌル空間で、N1 = I - J1⁺ J1 という射影行列で取り出せます。

1次タスクの解:      q̇ = J1⁺ ẋ1 + N1 z     （z は任意ベクトル、N1 = I - J1⁺ J1）
2次タスクを重ねる:  ẋ2 = J2 q̇ を、J1 タスクを崩さない z の範囲で最大限満たす

  → q̇ = J1⁺ ẋ1 + (J2 N1)⁺ ( ẋ2 - J2 J1⁺ ẋ1 )

  一般に k 段まで:  低優先タスクは常に上位すべてのヌル空間の中でだけ解かれる

ここで本質的なのは、2次タスクが1次タスクのヌル空間 N1 の内側でしか解かれない、という非対称性です。上位タスクは下位タスクを気にせず自分の目標を満たし、下位タスクは「上位を邪魔しない残りの自由度」だけを使います。優先度が明確な階層（ハードな優先度）を作れるため、「絶対に守るべき制約」と「できれば達成したい作業」を安全に両立できます。

全身の運動方程式：浮遊ベースと接触力

マニピュレータと決定的に違うのは、ヒューマノイドが床に固定されていない「浮遊ベース」系である点です。ベースの位置・姿勢6自由度を含めた一般化座標で、全身の運動方程式は接触力の項を明示して次のように書けます（詳しい導出の土台はロボットの動力学を参照）。

全身動力学（浮遊ベース + 接触）:

  M(q) q̈ + h(q, q̇) = Sᵀ τ + Σ_c Jcᵀ fc

  M   : 質量（慣性）行列
  h   : コリオリ・遠心力 + 重力をまとめた項
  S   : アクチュエータ選択行列（ベース6自由度は駆動できない=劣駆動）
  τ   : 関節トルク
  Jc  : 接触点 c のヤコビ行列
  fc  : 接触点 c に働く接触反力

ここで S の存在が重要です。ベースの6自由度には直接トルクをかけられない（劣駆動）ため、重心を動かす唯一の手段は脚が床から受ける接触力 fc です。上半身をどう振っても、外から働く力は重力と接触力だけであり、重心の並進加速度は接触力の合力によってしか作れません。全身制御が接触力の配分を中心課題にするのはこのためです。

接触拘束：等式と不等式の二重の制約

足裏が床に張り付いている間、その接触点は動いてはいけません。これは接触点の速度・加速度がゼロという等式拘束として課されます。

接触の等式拘束（接触点は静止）:

  Jc q̇ = 0        （速度レベル）
  Jc q̈ + J̇c q̇ = 0  （加速度レベル、制御ではこちらを使う）

しかし等式だけでは足りません。床は押す力しか返せず引く力は返せない（単方向拘束）ため接触力の法線成分は非負に限られ、滑らないためには接線力が摩擦係数の範囲に収まる必要があります。これが摩擦錐（friction cone）の不等式拘束です。

接触力の不等式拘束:

  fz ≥ 0                    （法線力は非負＝床は引っ張れない）
  √(fx² + fy²) ≤ μ fz       （摩擦錐：滑らない条件、μ は摩擦係数）

実装では円錐そのままだと最適化が扱いにくいため、多面体の錐（線形不等式の集合）で近似し、線形制約として二次計画に組み込みます。面で接触する足裏では、さらに接触面まわりのモーメントが足裏の広がりで決まる範囲に収まる条件も加わり、これらを一括した記述が接触レンチ錐（contact wrench cone）です。詳しい接触力そのものの扱いは力制御とコンプライアント制御が背景になります。

重心とZMP：接触力配分への帰着

転倒しないための古典的な指標が、脚型ロボットの歩行とバランス制御で扱ったZMP（ゼロモーメントポイント）です。全身制御の文脈では、ZMPは全接触力の合力（合成レンチ）から決まる床反力の作用点であり、支持多角形の内側にある限り、その接触力配分が物理的に実現可能であることを保証します。

より一般的には、全身の質量分布を重心一点に集約したセントロイダル動力学が土台になります。重心まわりでは、接触力の合力が重心の並進運動を、接触力による重心まわりのモーメントの総和が全身の角運動量の変化を支配します。

セントロイダル動力学（重心まわりの集約）:

  m c̈ = Σ fc - m g          （重心の並進：接触力の合力で決まる）
  L̇   = Σ (pc - c) × fc     （重心まわり角運動量の変化）

  m  : 全質量,  c : 重心位置,  L : 重心まわり角運動量
  pc : 接触点位置,  g : 重力加速度ベクトル

  ZMP は Σ fc が作る床反力の作用点として求まり、
  この点が支持多角形の内側 ⇔ その接触力配分が実現可能

この式が示すのは、ZMP制約が結局「接触力 fc をどう配分すればよいか」という問いに帰着することです。全身制御では、目標の重心加速度と目標の角運動量を実現する接触力を、摩擦錐とZMP（支持多角形内）の制約を満たしながら求め、その接触力と両立する関節トルクを運動方程式から逆算します。上体や腕を振って角運動量 L を能動的に使えば、脚の接触状態を変えずに重心の並進を補償でき、これは外乱回復の角運動量戦略そのものです。

タスクの束を一度に解く：QPベース全身制御

以上の要素——階層タスク、接触の等式・不等式拘束、ZMP・摩擦錐制約——を毎制御周期で同時に満たす現代的な実装が、二次計画（Quadratic Programming, QP）による全身制御です。決定変数に関節加速度・接触力・関節トルクをまとめて取り、運動方程式と接触拘束を等式・不等式制約に、タスク誤差の最小化を目的関数に据えます。

単一の重み付きQPは、すべてのタスクを一つの目的関数に重み付き二乗和として詰め込む最も軽い方式で、不等式制約（摩擦錐・関節トルク上限・関節可動域）を自然に扱える一方、優先度は重みでしか表現できずハードな階層にはなりません。厳密なハード優先度と不等式制約の両立が要る場合は、優先度レベルごとにQPを順に解き、上位レベルで達成した最適値を下位QPの等式制約として固定していく階層QP（Hierarchical QP）を使います。実運用では、接触維持と関節上限を絶対制約、重心・姿勢を高優先、手先作業を低優先とした2〜4レベル程度の階層をミリ秒オーダーの周期で解き続けるのが典型です。

まとめ

全身制御は、高冗長なヒューマノイドの腕・脚・重心・姿勢という互いに干渉する要求を、独立に制御器を割り当てるのではなく一つの最適化問題として一括で解く枠組みです。中核は三つあります。第一に、冗長自由度をヤコビアンのヌル空間射影で階層化し、上位タスク（転倒回避・接触維持）を崩さない範囲で下位タスク（手先作業）を解く階層タスク制御。ここでは動的な干渉を避けるため慣性重み付きの擬似逆行列が使われます。第二に、浮遊ベース系ゆえ重心を動かせるのは接触力だけという劣駆動の事実で、これが接触の等式拘束（接触点静止）と不等式拘束（法線力非負・摩擦錐）を制御の中心に押し上げます。第三に、ZMPと重心・角運動量を集約したセントロイダル動力学が、転倒しない接触力配分の条件を与えます。これらを毎制御周期で二次計画として同時に解く動力学WBCが現代の標準であり、単なる運動学の延長ではなく、接触・動力学・優先度を一貫して扱う統合的な制御設計としてヒューマノイドの全身運動を成立させています。