PX4 PositionControl の全体像：位置目標が thrust / attitude setpoint になるまで

はじめに

PX4 は、ArduPilot と並ぶ代表的なオープンソースのフライトコントローラです。
ETH Zürichの研究者たちによって開発が始まり、現在は世界中の研究者・エンジニアがDronecode Foundationのもとで開発・メンテナンスに参加しています。

PX4の制御パイプラインは、大きく次のように構成されています。

この図の左側、慣性系(Inertial Frame) で動く50Hz(20ms周期)の Position Control、Velocity Control、および Acceleration and Yaw to Attitude までの流れが、PX4コード上の PositionControl.cpp が担う範囲に対応します。

注意点として、図中の "Position Control" ブロックは、狭義には位置P制御、つまり位置目標から速度目標 V_sp を生成する部分を指しています。一方、本記事で扱う PositionControl は、PX4実装上の PositionControl クラスを指しており、位置制御だけでなく、速度制御、加速度目標の生成、推力ベクトル・姿勢setpoint生成までを含みます。

この PositionControl.cpp では、おおまかに次のようなカスケード構造になっています。

位置目標
  ↓
速度目標
  ↓
加速度目標
  ↓
推力ベクトル
  ↓
姿勢目標

筆者は、PX4 の制御器を外部SITLとしてブラックボックス的に使うだけでなく、自作のシミュレーション基盤に接続して評価するために、PositionControl の内部処理を読み解いています。

その過程で分かったのは、PX4 の PositionControl は単なる位置制御器ではなく、位置・速度・加速度の setpoint を統合し、最終的に thrust vector と attitude setpoint を生成するための中核的な変換器だということです。

この記事では、PX4 の PositionControl をコードベースで読む前段として、まず全体像を整理します。細かいコードの各行の意味は次回以降に分け、本記事では全体のデータフローと各ブロックの役割を把握することを目的にします。

続きの記事

• PositionControl のコードを具体的に読みたい方
  • PX4 PositionControl を読む：位置目標から速度目標 _vel_sp が作られるまで
  • PX4 PositionControl を読む：_velocityControl() 前半 — 速度PIDから加速度目標を作るまで
  • PX4 PositionControl を読む：_velocityControl() 後半 — 推力制限と anti-windup
  • PX4 PositionControl を読む：_accelerationControl() — 加速度目標が thrust vector になるまで
  • PX4 PositionControl のパラメータを読む：単位・役割・コード上の効き方まとめ

全体像

まず、PositionControl の大まかな構造を図で見ると、次のようになります。

PX4 : PositionControl の全体構成図

この図で見るべきポイントは、PX4 の PositionControl が大きく次の3段に分かれていることです。

1. 位置制御系
   Position setpoint と State position から _vel_sp を作る

2. 速度制御系
   _vel_sp と State velocity から _acc_sp を作る

3. 推力 / 姿勢ベクトル生成系
   _acc_sp から _thr_sp と姿勢目標を作る

ここで重要なのは、位置制御の出力が thrust ではなく _vel_sp であり、速度制御の出力も thrust ではなく _acc_sp である、という点です。

PX4 は、位置誤差から直接推力を出しているわけではありません。

まず「どの速度で目標位置へ向かうべきか」を作り、次に「その速度へ近づくにはどの加速度が必要か」を作り、最後に「その加速度を出すには、どの向きに推力を出すべきか」を計算します。

入力：setpoint と state

PositionControl には、大きく分けて次の入力があります。

Setpoint:
  - position
  - velocity
  - acceleration
  - yaw / yaw speed など

State:
  - position
  - velocity
  - acceleration

ここでの setpoint は、上位の FlightTask や外部入力から与えられる目標値です。

たとえば、位置を目標として与える場合は、position setpoint が主になります。
速度を目標として与える場合は、position setpoint ではなく velocity setpoint が主になります。
さらに acceleration setpoint が与えられる場合は、それを acceleration feed-forward として使うこともできます。

つまり、PositionControl は常に position / velocity / acceleration のすべてを同じように使うわけではありません。
利用者や上位制御がどの setpoint を指定するかによって、内部の使われ方が変わります。

位置目標から速度目標を作る
速度目標を直接使う
加速度目標を feed-forward として足す

PX4 では、指定されていない setpoint 成分に NaN を使います。

これはかなり重要です。

NaN は単なる異常値ではなく、PX4 の制御入力においては「この軸の目標値は未指定である」という意味を持ちます。
そのため、位置目標が指定されていない軸では位置制御由来の速度目標を作らず、速度目標が指定されている軸では velocity feed-forward として扱う、といった処理ができます。

では、順番に各ブロックの概要を説明します。

第1段：位置制御系

最初のブロックは、位置制御です。

図では次の部分に相当します。

Position setpoint
State position
  ↓
位置制御（P制御）
  ↓
_vel_sp

ここで行われることは、目標位置と現在位置の差分から、位置制御由来の速度目標を生成することです。

考え方としては、次のようになります。

位置誤差 = 目標位置 - 現在位置
速度目標 = 位置誤差 × 位置Pゲイン

PX4 のパラメータでいうと、主に次が関係します。

MPC_XY_P
MPC_Z_P

MPC_XY_P は水平位置制御のPゲイン、MPC_Z_P は高度方向の位置制御Pゲインです。

ここで大事なのは、位置制御の出力は「推力」ではないということです。

位置制御の出力は、あくまで _vel_sp です。

つまり、PX4 は位置誤差を見て、

目標位置へ向かうために、どの速度で動くべきか

を計算しています。

velocity feed-forward との合成

_vel_sp は、位置制御だけで決まるわけではありません。

外部から velocity setpoint が与えられている場合、それは feed-forward として位置制御由来の速度目標に加算されます。

概念的には、次のような処理です。

_vel_sp = velocity feed-forward + 位置制御由来の速度目標

たとえば、移動軌道を追従するときに、位置目標だけでなく「この方向へこの速度で動いてほしい」という速度目標が与えられていれば、PX4 はそれを加味します。

一方、position mode のように velocity setpoint が与えられていない場合、その成分は NaN になり、未指定として扱われます。

その場合は、位置制御由来の速度目標だけが使われます。

速度制限

位置制御によって生成された _vel_sp は、そのまま使われるわけではありません。

速度上限によって制限されます。

関係する主なパラメータは次です。

MPC_XY_VEL_MAX
MPC_Z_VEL_MAX_UP
MPC_Z_VEL_MAX_DN

水平速度については、MPC_XY_VEL_MAX によって制限されます。

Z方向については、上昇と下降で別々の速度上限があります。

MPC_Z_VEL_MAX_UP : 上昇速度上限
MPC_Z_VEL_MAX_DN : 下降速度上限

ここで PX4 の座標系に注意が必要です。

PX4 のローカル座標は NED です。

NED:
  X : North
  Y : East
  Z : Down

つまり、Z軸は下向きが正です。

そのため、上昇方向の速度は負、下降方向の速度は正になります。

Z方向速度目標は、概念的には次の範囲に制限されます。

-_lim_vel_up <= _vel_sp.z <= _lim_vel_down

上昇側が負になるのは、NED座標では上向きがZ負方向だからです。

第2段：速度制御系

次のブロックは速度制御です。

図では次の部分に相当します。

_vel_sp
State velocity
State acceleration
  ↓
速度制御（PID制御）
  ↓
_acc_sp

速度制御では、目標速度 _vel_sp と現在速度 _vel の差分を使って、加速度目標 _acc_sp を生成します。

考え方としては、次のようになります。

速度誤差 = 目標速度 - 現在速度
加速度目標 = P項 + I項 + D項

PX4 のパラメータでいうと、主に次が関係します。

MPC_XY_VEL_P_ACC
MPC_XY_VEL_I_ACC
MPC_XY_VEL_D_ACC

MPC_Z_VEL_P_ACC
MPC_Z_VEL_I_ACC
MPC_Z_VEL_D_ACC

ここでも重要なのは、速度制御の出力が推力ではないことです。

速度制御の出力は _acc_sp、つまり加速度目標です。

PX4 はここで、

目標速度へ近づくためには、どの加速度を出すべきか

を計算しています。

D項は「速度誤差の微分」ではなく現在加速度を見る

PX4 の速度制御で面白い点のひとつは、D項です。

一般的なPIDの説明では、D項は「誤差の微分」と説明されることが多いです。

しかし PX4 の PositionControl では、速度制御のD項として、現在速度の微分、つまり現在加速度に相当する _vel_dot を使います。

概念的には、次のような意味になります。

P項 : 目標速度へ近づくための加速度を出す
I項 : 定常的に足りない加速度を補正する
D項 : いま加速しすぎているなら抑える

つまりD項は、目標速度の変化を直接追いかけるというより、機体が実際にどれだけ加速しているかを見て、過剰な加速を減衰させる役割を持ちます。

このあたりは、コードを読むとかなり理解が深まる部分です。

acceleration feed-forward との合成

_acc_sp も、速度制御だけで決まるわけではありません。

外部から acceleration setpoint が与えられている場合、それは feed-forward として速度制御由来の加速度目標に加算されます。

概念的には、次のような処理です。

_acc_sp = acceleration feed-forward + 速度制御由来の加速度目標

これにより、軌道追従などで事前に分かっている加速度要求を、制御器に直接渡すことができます。

ただし、未指定成分は NaN として扱われるため、指定されていない軸には影響しません。

第3段：推力 / 姿勢ベクトル生成系

最後の大きなブロックが、推力 / 姿勢ベクトル生成です。

図では次の部分に相当します。

_acc_sp
  ↓
推力 / 姿勢ベクトル生成
  ↓
_thr_sp
  ↓
目標推力 / 目標姿勢角

ここで PX4 は、加速度目標 _acc_sp から、推力ベクトル _thr_sp と姿勢生成に必要な情報を作ります。

この段階で関係する主なパラメータは次です。

MPC_THR_HOVER
MPC_THR_MIN
MPC_THR_MAX
MPC_THR_XY_MARG
MPC_TILTMAX_AIR
MPC_ACC_DECOUPLE

ここで特に重要なのは、PX4 がいきなり roll / pitch を直接計算しているわけではない、という点です。

PX4 はまず、欲しい加速度を出すための推力方向を考えます。

その推力方向を表す重要なベクトルが body_z です。

body_z とは何か

body_z は、現在の機体Z軸そのものではありません。

また、roll角やpitch角そのものでもありません。

body_z は、

目標姿勢における機体Z軸の向き

を、world / NED 座標上の単位ベクトルとして表したものです。

言い換えると、

欲しい加速度を出すために、機体のZ軸をどちらへ向けるべきか

を表すベクトルです。

PX4 は、

欲しい加速度
  ↓
目標姿勢の body_z
  ↓
yaw 目標と組み合わせる
  ↓
roll / pitch / yaw の姿勢目標

という順番で姿勢目標を作ります。

この構造にしておくと、yaw が変わっても、world座標上の加速度要求から自然に姿勢目標を作ることができます。

なぜ roll / pitch を直接作らないのか

直感的には、次のように考えたくなります。

前に進みたい
  ↓
pitch を前傾させる

しかし、この考え方は yaw が 0 のときには分かりやすいものの、機体の向きが進行方向に対してずれている場合には少し複雑になります。

world座標での +X 方向と、機体の前方方向が一致するとは限らないからです。

そのため、PX4 は world / NED 座標上でまず「どちら向きに推力を出すべきか」を body_z として計算します。

その後、body_z と yaw 目標から最終的な姿勢目標を作ります。

この方が、world座標系での加速度要求を、yaw を考慮した姿勢目標へ変換しやすくなります。

ホバー推力と normalized thrust

PX4 の PositionControl では、推力は物理単位のニュートンではなく、normalized thrust として扱われます。

ここで重要になるのが MPC_THR_HOVER です。

MPC_THR_HOVER は、ホバーに必要な正規化推力を表します。

本来、物理的にはホバーに必要な推力は次のようになります。

F_hover = m × g

しかし PX4 の PositionControl は、この段階で機体質量 m を直接使うわけではありません。

代わりに、質量、モータ特性、プロペラ特性などをまとめて、

ホバーするには normalized thrust がどれくらい必要か

という MPC_THR_HOVER に畳み込んで扱います。

そのため、Z方向加速度目標は、次のような考え方で thrust に変換されます。

加速度目標
  ↓
何G相当か
  ↓
hover thrust 基準の normalized thrust

この設計により、PositionControl は機体固有の物理推力を直接扱うのではなく、正規化された推力指令を生成します。

機体を傾けると、なぜ推力を増やす必要があるのか

マルチコプターが水平移動するには、機体を傾ける必要があります。

機体を傾けると、推力ベクトルも傾きます。

すると、同じ総推力を出していても、鉛直方向に効く成分は小さくなります。

水平姿勢:
  推力のほぼ全てが鉛直方向に効く

傾いた姿勢:
  推力の一部が水平方向に使われる
  鉛直方向成分は減る

この「機体の推力方向に沿って出す推力の大きさ」が、コード上では collective_thrust として扱われます。

概念的には、次のような考え方です。

欲しいZ方向推力成分 = thrust_ned_z

実際のZ方向成分
  = collective_thrust × cos(tilt)

したがって、
  collective_thrust = thrust_ned_z / cos(tilt)

これにより、水平加速度を出すために機体が傾いても、Z方向の推力成分が不足しないように補正されます。

このあたりが、PositionControl の中でも非常に重要な部分です。

tilt（機体の傾き）制限

加速度要求が大きくなると、必要な機体の傾きも大きくなります。

しかし、マルチコプターには最大姿勢角があります。

PX4 では、MPC_TILTMAX_AIR によって、空中で許容される最大tilt角が制限されます。

ここで注意したいのは、PX4 が roll と pitch を個別に単純制限しているわけではないことです。

PX4 は body_z が鉛直方向からどれだけ傾いているか、つまり総tilt角を制限します。

そのため、roll と pitch が同時に入る場合でも、合成された傾きが MPC_TILTMAX_AIR を超えないように制限されます。

body_z から roll / pitch が生まれる

PX4 は、欲しい水平加速度から直接 roll / pitch を決めるわけではありません。

まず、目標姿勢における機体Z軸の向きである body_z を計算します。

図では、world / NED 座標上で水平加速度 acc_sp.x / acc_sp.y を出したい場合を示しています。

水平加速度を出すには、推力ベクトルに水平成分を持たせる必要があります。

そのため PX4 は、欲しい水平加速度の方向へ直接 roll / pitch を決めるのではなく、まず body_z を傾けます。

このとき、body_z の水平成分は、欲しい水平加速度とは反対向きになります。

これは、コード上では次のような関係に対応します。

Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), -z_specific_force).normalized();

PositionControl を一言でまとめる

PX4 の PositionControl を一言でまとめると、次のようになります。

位置・速度・加速度の目標値を統合し、
制約をかけながら、
最終的に thrust vector と attitude setpoint を生成する制御器

もう少し処理の流れに沿って書くと、次のようになります。

位置誤差から速度目標を作る
  ↓
速度誤差から加速度目標を作る
  ↓
加速度目標から推力方向 body_z を作る
  ↓
推力制限・tilt制限をかける
  ↓
thrust setpoint と attitude setpoint を作る

ここでの重要ポイントは次です。

- 位置制御の出力は thrust ではなく _vel_sp
- 速度制御の出力は thrust ではなく _acc_sp
- _acc_sp から body_z と _thr_sp が作られる
- body_z は roll / pitch そのものではなく、目標姿勢の機体Z軸方向
- thrust は normalized thrust として扱われる
- hover thrust は MPC_THR_HOVER に畳み込まれている
- tilt 制限は roll / pitch 個別ではなく body_z の総tilt角に対して効く

箱庭ラボが PX4 PositionControl を読む意味

今回 PositionControl を整理した理由は、PX4 の制御構造を理解するためだけではありません。

箱庭ドローンシミュレータでは、PX4 の制御コードをライブラリとして組み込み、必要に応じて静的リンクも含めた形で、シミュレータ側の物理計算・センサ値・機体状態と接続することを目指しています。

そのためには、PX4 が内部で何を入力として受け取り、どの段階でどの setpoint を生成し、どのパラメータがどこに効くのかを理解する必要があります。

PositionControl の全体像を押さえることで、たとえば次のような見通しが立ちます。

MPC_XY_P を変えると、位置誤差から作られる速度目標が変わる

MPC_XY_VEL_P_ACC を変えると、速度誤差から作られる加速度目標が変わる

MPC_THR_HOVER を変えると、加速度目標から normalized thrust への変換が変わる

MPC_TILTMAX_AIR を変えると、水平加速度要求に対する最大傾きが変わる

つまり、パラメータ調整やシミュレーション評価をするときに、

どのパラメータが、制御パイプラインのどの段に効いているのか

を見通せるようになります。

これは、PX4 を単に動かすだけでなく、PX4 の制御応答を理解し、評価し、必要に応じて接続・拡張していくうえで重要です。

おわりに

この記事では、PX4 の PositionControl の全体像を整理しました。

ポイントは、PositionControl が位置目標から直接 thrust や roll / pitch を作るのではなく、

Position
  ↓
Velocity
  ↓
Acceleration
  ↓
Thrust / Attitude

という段階的な変換を行っていることです。

次回は、このうち最初のブロックである _positionControl() を詳しく見ていきます。

特に、

位置誤差から _vel_sp がどう作られるのか
velocity feed-forward はどう合成されるのか
NaN はなぜ「未指定」として扱われるのか
MPC_XY_VEL_MAX による速度制限はどう効くのか

を、コードベースで整理していきます。