本連載では、PX4 の PositionControl 全体像を整理し、位置・速度・加速度の setpoint を受け取り、最終的に加速度目標や推力目標へ変換していく流れを見ています。
- 第１回：PX4 PositionControl の全体像：位置目標が thrust / attitude setpoint になるまで
- 第２回：位置目標から速度目標 _vel_sp が作られるまで
- 第３回：PX4 PositionControl を読む：_velocityControl() 前半 — 速度PIDから加速度目標を作るまで
- 第４回：PX4 PositionControl を読む：_velocityControl() 後半 — 推力制限と anti-windup
対象とするコードは、PX4-Autopilot の以下の場所にあります。
PositionControl の主要な処理は、update() 関数を入口として実行されます。
bool PositionControl::update(const float dt)
{
bool valid = _inputValid();
if (valid) {
_positionControl();
_velocityControl(dt);
_yawspeed_sp = PX4_ISFINITE(_yawspeed_sp) ? _yawspeed_sp : 0.f;
_yaw_sp = PX4_ISFINITE(_yaw_sp) ? _yaw_sp : _yaw; // TODO: better way to disable yaw control
}
// There has to be a valid output acceleration and thrust setpoint otherwise something went wrong
return valid && _acc_sp.isAllFinite() && _thr_sp.isAllFinite();
}
そして、前回までで、_velocityControl() の後半を読み、推力制限と anti-windupを見ました。
今回読む範囲
void PositionControl::_accelerationControl()
{
// Assume standard acceleration due to gravity in vertical direction for attitude generation
float z_specific_force = -CONSTANTS_ONE_G;
if (!_decouple_horizontal_and_vertical_acceleration) {
// Include vertical acceleration setpoint for better horizontal acceleration tracking
z_specific_force += _acc_sp(2);
}
Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), -z_specific_force).normalized();
ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _lim_tilt);
// Convert to thrust assuming hover thrust produces standard gravity
const float thrust_ned_z = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust;
// Project thrust to planned body attitude
const float cos_ned_body = (Vector3f(0, 0, 1).dot(body_z));
const float collective_thrust = math::min(thrust_ned_z / cos_ned_body, -_lim_thr_min);
_thr_sp = body_z * collective_thrust;
}
今回は、これまで詳細には踏み込まなかった _accelerationControl() の中身を読みます。
_velocityControl() の前半では、速度PIDによって加速度目標 _acc_sp が作られました。
そして前回は、_accelerationControl() の後に生成された推力目標 _thr_sp に対して、推力制限と anti-windup が行われる流れを見ました。
つまり、今回読む _accelerationControl() は、その間にある処理です。
速度PID
↓
加速度目標 _acc_sp
↓
_accelerationControl()
↓
推力目標 _thr_sp
↓
推力制限・anti-windup
_accelerationControl() の全体像
_accelerationControl() の中でやっていることを先に整理すると、次の流れです。
加速度目標 _acc_sp
↓
ステップ1：欲しい加速度を出すための推力方向 body_z を求める
↓
ステップ2：tilt制限をかける
↓
ステップ3：Z方向に必要な推力成分 thrust_ned_z を求める
↓
ステップ4：傾いた機体に投影し直して、最終的な _thr_sp を作る
ここでひとつ重要な点があります。PX4 はいきなり roll / pitch を直接計算していません。
まず world / NED 座標上で「目標姿勢における機体Z軸の向き」を body_z として求め、そこから姿勢目標を生成します。これは、yaw が変わると world座標の加速度と機体の roll / pitch の関係が複雑になるためです。body_z をいったん world座標で作っておけば、yaw の影響を自然に吸収できます。
ステップ1：推力方向 body_z を求める
float z_specific_force = -CONSTANTS_ONE_G;
if (!_decouple_horizontal_and_vertical_acceleration) {
z_specific_force += _acc_sp(2);
}
Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), -z_specific_force).normalized();
z_specific_force とは
PX4のローカル座標はNEDです。Z軸は下向き正なので、上向きに働く推力は負方向になります。
ホバーするには重力に逆らう上向きの force が必要で、その初期値として -1G を置いています。
float z_specific_force = -CONSTANTS_ONE_G;
_decouple_horizontal_and_vertical_acceleration が false の場合は、Z方向の加速度目標 _acc_sp.z もここに加算します。
z_specific_force += _acc_sp(2);
ここで、_acc_sp.z が何者かというと、_velocityControl() の速度PIDが出力した Z方向の加速度目標です。具体的には、
Z速度誤差 × P ゲイン
+ 積分項 vel_int.z
− 現在加速度.z × D ゲイン
によって作られた「いま機体にZ方向へこれだけ加速してほしい」という要求値です。
_decouple_horizontal_and_vertical_acceleration の意味
このフラグが関係するのは、水平移動と上昇・下降を同時に行うケースです。
たとえば、前進しながら上昇するとき、機体には水平方向と垂直方向に同時に加速要求が来ます。このとき、推力方向（body_z）をどう決めるかに、2つの考え方があります。
decouple = false（結合）
水平・垂直の加速要求を一つの3D加速度ベクトルとして統合し、そこから推力方向を決めます。「前進しながら上昇したい」なら、その合力の向きに body_z を向けます。物理的には最も厳密な考え方です。
body_z の元 = (-acc_sp.x, -acc_sp.y, g - acc_sp.z) // 垂直加速要求も含む
decouple = true（分離）
body_z は水平加速要求と重力補償だけから決めます。垂直方向の加速要求は、後段の thrust_ned_z の計算で独立して処理されます。
body_z の元 = (-acc_sp.x, -acc_sp.y, g) // 垂直加速要求は含まない
TIPS：箱庭ドローンシミュレータの場合
第２回・第３回で見てきた構成を振り返ると、箱庭ドローンの Adapter では
Horizontal と Altitude を別インスタンスに分けているため、
Horizontal 側の _acc_sp.z には基本的に上昇・下降要求が入りません。
そのため、現行の設定 MPC_ACC_DECOUPLE = 1.0（分離）は、
その構成に自然に合致しています。
decouple = false の効果が本来の意味を持つのは、
x/y/z を一つの PositionControl インスタンスで扱う3軸一体構成のときです。
body_z の生成
Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), -z_specific_force).normalized();
body_z は、目標姿勢において推力が出る方向を、world / NED 座標で表した単位ベクトルです。言い換えると、欲しい加速度を出すために、目標姿勢の body Z 軸を world 座標上で表したものです。
図のように、水平加速度
acc_sp_xy（青実線）とZ方向のz_specific_force（赤実線）をそれぞれ反転した成分（点線）の合成ベクトルがbody_zの方向になります。最後に.normalized()で単位ベクトルにします。
ステップ2：tilt制限をかける
ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _lim_tilt);
body_z の傾きが最大 tilt 角 _lim_tilt を超えないように制限します。
傾きすぎると、推力の水平成分が大きくなりすぎて高度維持に必要な垂直成分が不足します。limitTilt は body_z の向きを鉛直軸からの角度で制限することで、これを防ぎます。
補足：ここでの tilt 角は roll/pitch そのものではなく、roll と pitch が合成された結果としての 総傾き角 です。body_z は world/NED 座標上の単位ベクトルなので、鉛直軸
(0,0,1)との角度が総傾き量を直接表しています。
ステップ3：Z方向に必要な推力成分を求める
ここで一度、body_z と thrust_ned_z の関係を整理します。
body_z → 推力の「向き」を表す単位ベクトル
thrust_ned_z → NED のZ方向に「どれだけの推力成分が必要か」を表すスカラー
collective_thrust → body_z 方向に沿って「実際に出す推力の大きさ」
最終的な thrust setpoint は、
_thr_sp = body_z × collective_thrust
として作られます。つまり、向きは body_z で決まり、大きさは collective_thrust で決まります。
ステップ3では、「Z方向に必要な推力成分がいくらか（thrust_ned_z）」を求めます。この値を使って、ステップ4で「body_z 方向に出すべき総推力（collective_thrust）」へ逆算します。
ちなみに、なぜこうしているかというと、機体が水平移動のために傾くと、推力の鉛直成分が
cos(tilt)倍(=1以下)に減るからです。そのまま放置すると、水平移動中に高度が下がります。NED Z方向に「必要な推力成分」を基準として逆算することで、傾いた分だけ総推力を増やし、高度低下を防ぎます。
では、thrust_ned_z の計算式を見ましょう。
const float thrust_ned_z = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust;
Z方向の加速度目標 _acc_sp.z を、PX4の normalized thrust に変換します。
PX4 は機体質量 m を直接使わず、_hover_thrust（MPC_THR_HOVER）を基準とします。
1G の加速度を支える normalized thrust = _hover_thrust
とみなすと、
thrust_ned_z = _hover_thrust * (_acc_sp.z / g - 1)
_acc_sp.z / g で「何G相当か」という無次元量にし、_hover_thrust を掛けてホバー推力基準の normalized thrust に変換しています。-1 は、加速度目標ゼロのとき（ホバー時）に thrust_ned_z = -_hover_thrust となるよう、ベースラインのホバー推力を引いている分です。
挙動を確認すると、
_acc_sp.z = 0 → thrust_ned_z = -_hover_thrust （ホバー）
_acc_sp.z < 0 → より負に大きくなる （上昇加速）
_acc_sp.z > 0 → 負方向が弱くなる （下降加速）
となり、NEDの符号と整合しています。
ステップ4：body_z 方向に投影し直して _thr_sp を作る
const float cos_ned_body = (Vector3f(0, 0, 1).dot(body_z));
const float collective_thrust = math::min(thrust_ned_z / cos_ned_body, -_lim_thr_min);
_thr_sp = body_z * collective_thrust;
先ほど述べたように、機体が傾くと body_z 方向に出した推力のZ方向成分が cos(tilt) 倍に減ります。ステップ4では、これを逆算で補正します。
cos_ned_body は、鉛直下向きZ軸と body_z のなす角の cos です。水平姿勢なら 1、傾くほど小さくなります。
_thr_sp.z = body_z.z * collective_thrust = cos_ned_body * collective_thrust であるため、
欲しいZ方向成分 thrust_ned_z を出すには、
collective_thrust = thrust_ned_z / cos_ned_body
と逆算できます。傾いた分だけ総推力を増やすことで、Z方向の推力成分を維持します。
math::min(..., -_lim_thr_min) は推力が弱くなりすぎないための下限制限です（NED符号で上向き推力は負なので min を使います）。
最後に、
_thr_sp = body_z * collective_thrust;
で、目標姿勢の body_z 方向に collective_thrust を掛けた world / NED 座標の3D thrust vector が完成します。
まとめ
_accelerationControl() 全体を一言で言うと、
「加速度目標から推力方向 body_z を作り、Z方向推力を tilt に応じて補正しながら、最終的な3D thrust vector を生成する処理」
です。
ここで生成された _thr_sp が、前回見た推力制限・anti-windup へと引き渡されます。
最後に：なぜ PositionControl を読むのか
ここまで、PX4 の PositionControl を数回に分けて読んできました。
最初は、位置目標がどのように速度目標へ変換されるのかを見ました。
次に、速度PIDによって加速度目標 _acc_sp が作られる流れを見ました。
さらに、その加速度目標から推力目標 _thr_sp が作られ、推力制限や anti-windup によって実際に出せる範囲へ整えられる流れを見ました。
今回読んだ _accelerationControl() は、その中でも特に重要な境界にあります。
ここでは、単に加速度を推力に換算しているのではありません。
マルチコプターが水平方向に加速するには、機体を傾けて推力ベクトルの向きを変える必要があります。
そのため PX4 は、加速度目標から body_z を作り、tilt 制限をかけ、Z方向推力を補正しながら、最終的な thrust vector を生成しています。
つまり PositionControl は、
位置目標
↓
速度目標
↓
加速度目標
↓
推力ベクトル
↓
姿勢・推力 setpoint
という流れの中で、上位の「どこへ動きたいか」という要求を、下位の「機体をどちらに傾け、どれだけ推力を出すか」という要求へ変換する層だと言えます。
PX4 を単なるブラックボックスとして使うだけなら、ここまで読む必要はないかもしれません。しかし、制御器の挙動を理解し、PIDチューニング、推力制限、故障注入、SimToReal などにつなげていくには、内部の計算を知ることが重要です。この連載を通じて、PX4 の位置制御は少なくとも完全なブラックボックスではなくなったと思います。