本連載では、PX4 の PositionControl 全体像を整理し、位置・速度・加速度の setpoint を受け取り、最終的に加速度目標や推力目標へ変換していく流れを見ています。
- 第１回：PX4 PositionControl の全体像：位置目標が thrust / attitude setpoint になるまで
- 第２回：位置目標から速度目標 _vel_sp が作られるまで
- 第３回：PX4 PositionControl を読む：_velocityControl() 前半 — 速度PIDから加速度目標を作るまで
対象とするコードは、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() の前半を読み、速度PIDから加速度目標 _acc_sp が作られる流れを見ました。
今回読む範囲
今回は、同じ _velocityControl() の後半を読みます。
具体的には、_accelerationControl() の呼び出し後から、関数の最後までです。
void PositionControl::_velocityControl(const float dt)
{
// Constrain vertical velocity integral
_vel_int(2) = math::constrain(_vel_int(2), -CONSTANTS_ONE_G, CONSTANTS_ONE_G);
// PID velocity control
Vector3f vel_error = _vel_sp - _vel;
Vector3f acc_sp_velocity = vel_error.emult(_gain_vel_p) + _vel_int - _vel_dot.emult(_gain_vel_d);
// No control input from setpoints or corresponding states which are NAN
ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity);
_accelerationControl();
// Integrator anti-windup in vertical direction
if ((_thr_sp(2) >= -_lim_thr_min && vel_error(2) >= 0.f) ||
(_thr_sp(2) <= -_lim_thr_max && vel_error(2) <= 0.f)) {
vel_error(2) = 0.f;
}
// Prioritize vertical control while keeping a horizontal margin
const Vector2f thrust_sp_xy(_thr_sp);
const float thrust_sp_xy_norm = thrust_sp_xy.norm();
const float thrust_max_squared = math::sq(_lim_thr_max);
// Determine how much vertical thrust is left keeping horizontal margin
const float allocated_horizontal_thrust = math::min(thrust_sp_xy_norm, _lim_thr_xy_margin);
const float thrust_z_max_squared = thrust_max_squared - math::sq(allocated_horizontal_thrust);
// Saturate maximal vertical thrust
_thr_sp(2) = math::max(_thr_sp(2), -sqrtf(thrust_z_max_squared));
// Determine how much horizontal thrust is left after prioritizing vertical control
const float thrust_max_xy_squared = thrust_max_squared - math::sq(_thr_sp(2));
float thrust_max_xy = 0.f;
if (thrust_max_xy_squared > 0.f) {
thrust_max_xy = sqrtf(thrust_max_xy_squared);
}
// Saturate thrust in horizontal direction
if (thrust_sp_xy_norm > thrust_max_xy) {
_thr_sp.xy() = thrust_sp_xy / thrust_sp_xy_norm * thrust_max_xy;
}
// Use tracking Anti-Windup for horizontal direction: during saturation, the integrator is used to unsaturate the output
// see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990
const Vector2f acc_sp_xy_produced = Vector2f(_thr_sp) * (CONSTANTS_ONE_G / _hover_thrust);
// The produced acceleration can be greater or smaller than the desired acceleration due to the saturations and the actual vertical thrust (computed independently).
// The ARW loop needs to run if the signal is saturated only.
if (_acc_sp.xy().norm_squared() > acc_sp_xy_produced.norm_squared()) {
const float arw_gain = 2.f / _gain_vel_p(0);
const Vector2f acc_sp_xy = _acc_sp.xy();
vel_error.xy() = Vector2f(vel_error) - arw_gain * (acc_sp_xy - acc_sp_xy_produced);
}
// Make sure integral doesn't get NAN
ControlMath::setZeroIfNanVector3f(vel_error);
// Update integral part of velocity control
_vel_int += vel_error.emult(_gain_vel_i) * dt;
}
_accelerationControl() で得られるもの
_accelerationControl() を呼び出すと、加速度目標 _acc_sp は推力目標 _thr_sp に変換されます。
_acc_sp は [m/s²] の加速度目標ですが、_thr_sp は物理的な推力 [N] そのものではなく、PX4内部で扱う正規化された thrust setpoint です。
今回読む後半部分では、この _thr_sp に対して推力制限や anti-windup を行います。
なお、_accelerationControl() の中身については、PositionControl 編の最終回で改めて扱います。
では、読み進めていきましょう。
Z方向の anti-windup
まず出てくるのは、Z方向の anti-windup 処理です。
// Integrator anti-windup in vertical direction
if ((_thr_sp(2) >= -_lim_thr_min && vel_error(2) >= 0.f) ||
(_thr_sp(2) <= -_lim_thr_max && vel_error(2) <= 0.f)) {
vel_error(2) = 0.f;
}
ここで見ているのは、Z方向の推力目標 _thr_sp.z が、すでに推力の上下限に達しているかどうかです。
もし、推力がすでに限界に達している状態で、さらに同じ方向へ推力を要求する速度誤差を積分すると、積分項だけが過剰に蓄積されます。
これが windup です。
前回の記事では、Z方向の積分項 _vel_int.z 自体に、次のような制限がかかっていることを見ました。
// Constrain vertical velocity integral
_vel_int(2) = math::constrain(_vel_int(2), -CONSTANTS_ONE_G, CONSTANTS_ONE_G);
これは、すでに蓄積された _vel_int.z の大きさを ±1G の範囲に抑える処理でした。
一方、今回の処理では、この周期の速度誤差 vel_error.z を、積分器に入れてよいかどうかを判断しています。
つまり、前回の制限が「積分項の上限を決める処理」だとすると、今回の処理は「飽和を悪化させる速度誤差を積分しないための処理」です。
ここも、Z方向速度制御における windup 対策です。
ここから先の読み進め方について
ここまで、一行ずつコードを読み解いてきましたが、この後のコードブロックを同じ進め方で読むと、たぶん迷子になります。
理由は、このブロックが単純な逐次処理ではなく、複数の目的が混ざっているからです。
少しだけ要約すると、ここでは次のようなことを行っています。
1. 総推力上限の中で、垂直方向の推力を優先する
2. ただし、水平制御のための最低限の推力余裕も残す
3. 垂直推力を優先した後、残った推力枠で水平推力を制限する
4. 水平推力が制限された場合、実際に出せた水平加速度を見積もる
5. 出したかった加速度と、実際に出せた加速度の差を使って、
水平方向の tracking anti-windup を行う
6. 最後に、補正後の速度誤差で積分項 `_vel_int` を更新する
そのため、ここから先は一行ずつではなく、処理の目的ごとにコードを分けて見ていきます。
垂直方向の推力を優先する
まず、PX4 は総推力上限 _lim_thr_max の範囲内で、Z方向にどれだけ推力を使えるかを計算します。
対象となるコードは次の部分です。
// Prioritize vertical control while keeping a horizontal margin
const Vector2f thrust_sp_xy(_thr_sp);
const float thrust_sp_xy_norm = thrust_sp_xy.norm();
const float thrust_max_squared = math::sq(_lim_thr_max);
// Determine how much vertical thrust is left keeping horizontal margin
const float allocated_horizontal_thrust = math::min(thrust_sp_xy_norm, _lim_thr_xy_margin);
const float thrust_z_max_squared = thrust_max_squared - math::sq(allocated_horizontal_thrust);
// Saturate maximal vertical thrust
_thr_sp(2) = math::max(_thr_sp(2), -sqrtf(thrust_z_max_squared));
ここでやっていることは、ざっくり言うと、
総推力上限 _lim_thr_max の中で、
水平推力用の最低限の余裕を残しつつ、
Z方向に使える最大推力を決める
です。
ここでのポイントは、PX4 が thrust vector 全体を単純に縮小しているわけではない、ということです。
PX4 はまず、水平推力成分 _thr_sp.xy() の大きさを見ます。
const Vector2f thrust_sp_xy(_thr_sp);
const float thrust_sp_xy_norm = thrust_sp_xy.norm();
norm() は、ベクトルの大きさを求める関数です。
ここでは、_thr_sp の X/Y 成分だけを取り出しているので、thrust_sp_xy_norm は水平推力成分の大きさになります。
次に、総推力上限 _lim_thr_max の2乗を計算します。
const float thrust_max_squared = math::sq(_lim_thr_max);
TIPS：なぜ2乗で扱っているか
これは、推力ベクトル全体の大きさは、水平成分とZ成分の2乗和で表せるからです。
|thrust|² = thrust_xy² + thrust_z²
この形にしておくと、総推力上限の中で、ある水平推力を確保したときに、Z方向に使える最大推力を次のように求められます。
thrust_z² = thrust_max² - thrust_xy²
つまり、水平成分として確保する分を決めれば、残りからZ方向に使える推力上限を計算できます。
水平推力用に見込む枠を決める
次に PX4 は、Z方向推力の上限を計算するために、水平推力用としてどれだけ見込むかを決めます。
const float allocated_horizontal_thrust = math::min(thrust_sp_xy_norm, _lim_thr_xy_margin);
ここで注意したいのは、allocated_horizontal_thrust が、実際に出す水平推力そのものではないという点です。
これは、Z方向に使える推力上限を計算するために、水平制御用として先に見込んでおく推力枠です。
_lim_thr_xy_margin は、Z方向推力を制限するときに、水平制御用として最大どこまで見込むかを表す値です。
ただし、常に _lim_thr_xy_margin 全部を予約するわけではありません。
実際に要求されている水平推力 thrust_sp_xy_norm がそれより小さい場合は、その要求分だけを水平用の枠として見積もります。
つまり、
水平推力要求が margin より小さい
→ 要求分だけ見込めばよい
水平推力要求が margin より大きい
→ margin 分までを水平用として見込む
ということです。
そのうえで、総推力上限から、この水平推力分を差し引いて、Z方向に使える推力の2乗を計算します。
const float thrust_z_max_squared =
thrust_max_squared - math::sq(allocated_horizontal_thrust);
これは、先ほど見た次の関係から来ています。
thrust_z² = thrust_max² - thrust_xy²
つまり、水平制御用として見込む分を先に決めることで、その残りからZ方向に使える最大推力を計算しています。
最後に、Z方向の推力を制限する
最後に、その範囲を超えないように _thr_sp.z を制限します。
_thr_sp(2) = math::max(_thr_sp(2), -sqrtf(thrust_z_max_squared));
ここで少し符号に注意が必要です。
PX4 のローカル座標は NED なので、上向き推力は _thr_sp.z の負方向として表されます。
そのため、より強い上向き推力ほど _thr_sp.z は小さい値になります。
_thr_sp.z = -0.3 弱めの上向き推力
_thr_sp.z = -0.8 強めの上向き推力
-sqrtf(thrust_z_max_squared) は、Z方向に許される最大上向き推力を表します。
したがって、
math::max(_thr_sp(2), -sqrtf(thrust_z_max_squared))
によって、_thr_sp.z がこれ以上負方向に大きくならないように制限しています。
NEDでは上向き推力が負方向なので、ここで max() を使う点が少し直感に反します。
現在の _thr_sp.z が -0.8
許される最大上向き推力が -0.6
math::max(-0.8, -0.6) = -0.6
つまり、強すぎる上向き推力を、許される範囲まで弱めています。
この処理により、PX4 は総推力上限 _lim_thr_max の中で、水平推力用として見込む分を考慮しながら、Z方向に使える最大上向き推力を制限しています。
残った推力枠で水平推力を制限する
次に PX4 は、制限後の _thr_sp.z をもとに、水平推力に使える残りの大きさを計算します。
// Determine how much horizontal thrust is left after prioritizing vertical control
const float thrust_max_xy_squared = thrust_max_squared - math::sq(_thr_sp(2));
float thrust_max_xy = 0.f;
if (thrust_max_xy_squared > 0.f) {
thrust_max_xy = sqrtf(thrust_max_xy_squared);
}
推力ベクトル全体の大きさは、先ほど見たように次の関係で表せます。
|thrust|² = thrust_xy² + thrust_z²
したがって、総推力上限 _lim_thr_max と、すでに決まったZ方向推力 _thr_sp.z が分かれば、水平推力に使える残りは次のように求められます。
thrust_xy² = thrust_max² - thrust_z²
コードでは、これを次のように計算しています。
const float thrust_max_xy_squared = thrust_max_squared - math::sq(_thr_sp(2));
ここで得られる thrust_max_xy_squared は、水平推力として使える最大値の2乗です。
値が正の場合だけ平方根を取り、水平推力の上限 thrust_max_xy を求めます。
if (thrust_max_xy_squared > 0.f) {
thrust_max_xy = sqrtf(thrust_max_xy_squared);
}
もし thrust_max_xy_squared が 0 以下なら、水平推力に使える余裕はないので、thrust_max_xy は 0 のままです。
そして、現在の水平推力要求 thrust_sp_xy_norm が、この上限 thrust_max_xy を超えている場合は、水平成分 _thr_sp.xy() を縮小します。
// Saturate thrust in horizontal direction
if (thrust_sp_xy_norm > thrust_max_xy) {
_thr_sp.xy() = thrust_sp_xy / thrust_sp_xy_norm * thrust_max_xy;
}
補足すると、thrust_sp_xy と thrust_sp_xy_norm は、_accelerationControl() によって生成された _thr_sp の水平成分を、Z方向推力を制限する前に取り出しておいたものです。
const Vector2f thrust_sp_xy(_thr_sp);
const float thrust_sp_xy_norm = thrust_sp_xy.norm();
なので、この式は、
_thr_sp.xy() = thrust_sp_xy / thrust_sp_xy_norm * thrust_max_xy;
次のように読めます。
水平方向の推力ベクトル
=
水平推力方向の単位ベクトル
×
水平方向に使える最大推力
ここで、
thrust_sp_xy / thrust_sp_xy_norm
は、もともと要求されていた水平推力の向きだけを取り出した単位ベクトルです。
そこに thrust_max_xy を掛けることで、向きは維持したまま、大きさだけを水平推力上限まで縮小しています。
つまり、この処理は、総推力上限を超えない範囲で、もともと要求されていた水平推力の向きを維持しながら、大きさだけを制限する saturation です。
ちょっと休憩
ここまでで、PX4 は _thr_sp を総推力上限 _lim_thr_max の範囲に収めました。
つまり、推力目標そのものの制限処理は、ここで一段落しています。
しかし、ここで別の問題が出てきます。
水平推力が制限された場合、もともと要求していた水平加速度 _acc_sp.xy() をそのまま実現できているとは限りません。
この先でやることを整理すると、次の3ステップです。
ステップ1：推力制限によって「実際に出せた水平加速度」を加速度次元で見積もる
ステップ2：要求加速度と、実際に出せた加速度のずれ（飽和量）を計算する
ステップ3：そのずれを使って積分器への入力を引き戻し、windup を防ぐ
これが、この後の水平方向 tracking anti-windup の全体像です。
なお、「推力をもう一度作り直す」のではない点に注意してください。推力制限はすでに終わっています。ここでやるのは、積分器が「出せない加速度を追いかけ続ける」状態に陥らないようにする補正です。
水平方向の tracking anti-windup
対象となるコードは次の部分です。
// Use tracking Anti-Windup for horizontal direction: during saturation, the integrator is used to unsaturate the output
// see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990
const Vector2f acc_sp_xy_produced = Vector2f(_thr_sp) * (CONSTANTS_ONE_G / _hover_thrust);
// The produced acceleration can be greater or smaller than the desired acceleration due to the saturations and the actual vertical thrust (computed independently).
// The ARW loop needs to run if the signal is saturated only.
if (_acc_sp.xy().norm_squared() > acc_sp_xy_produced.norm_squared()) {
const float arw_gain = 2.f / _gain_vel_p(0);
const Vector2f acc_sp_xy = _acc_sp.xy();
vel_error.xy() = Vector2f(vel_error) - arw_gain * (acc_sp_xy - acc_sp_xy_produced);
}
なぜ Anti-Windup が必要か
PIDの積分項は「誤差をずっと足し続ける」ものです。
たとえばドローンが「もっと前に進め」という指令を受け続けているとき、積分器は誤差を足し続けます。しかし推力には上限があります。いくら積分器が大きな値を出しても、それ以上の推力は出せない。この状態が飽和です。
飽和中も積分器は溜まり続けるため、飽和が解けた瞬間に「溜まりすぎた積分値」がドカッと出力されます。これが windup（巻き上がり） であり、オーバーシュートや振動の原因になります。
ARW（Anti-Reset Windup）の解決策はシンプルです。
「飽和しているときは、飽和量に応じて積分器への入力を引き戻す」
ステップ1：推力制限によって「実際に出せた水平加速度」を加速度次元で見積もる
まず、制限後の _thr_sp.xy() を加速度次元に換算します。
const Vector2f acc_sp_xy_produced =
Vector2f(_thr_sp) * (CONSTANTS_ONE_G / _hover_thrust);
ここでやっているのは、推力をもう一度作り直すことではありません。
すでに制限された水平 thrust setpoint _thr_sp.xy() が、加速度としてはどれくらいに相当するのかを見積もっています。
ここで少し直感に反するのは、_hover_thrust はホバー、つまりZ方向の推力に関係する値なのに、なぜ水平加速度の換算にも使えるのか、という点です。
ポイントは、_hover_thrust が「Z方向専用の係数」ではなく、PX4 の normalized thrust を加速度スケールに変換するための基準値だということです。
ホバー時には、normalized thrust _hover_thrust が重力 1G を支えます。
_hover_thrust ↔ 1G
つまり、_hover_thrust は、
1G 相当の力を出すために必要な normalized thrust
だと考えられます。
そのため、任意の normalized thrust 成分 T は、次の比によって「何G相当か」に換算できます。
T / _hover_thrust
補足：T = _hover_thrust なら、1G相当です。
これに重力加速度 g を掛ければ、加速度 [m/s²] になります。
a = ( T / _hover_thrust ) * g
ここで確認したいのは、制限後の水平 thrust setpoint _thr_sp.xy() から、なぜ水平加速度を見積もれるのか、という点です。
水平方向だけを見ると、重力は直接は出てきません。
重力はZ方向に働くため、水平加速度は水平推力成分によって決まります。
m * a_xy = F_xy
つまり、水平推力 F_xy が分かれば、水平加速度 a_xy を見積もることができます。
ただし、PX4 が持っている _thr_sp.xy() は物理推力 [N] ではなく、normalized thrust です。
そこで _hover_thrust を使って、normalized thrust を加速度スケールに換算します。
コードでは、T が制限後の _thr_sp.xy()、g が CONSTANTS_ONE_G に対応します。
Vector2f(_thr_sp) * (CONSTANTS_ONE_G / _hover_thrust)
（この換算の詳しい導出は、末尾の補足情報で整理しています。）
したがって、acc_sp_xy_produced は、
制限後の水平 thrust setpoint を加速度次元に換算した値
です。
「実際に機体が出した加速度」ではなく、あくまで制限後セットポイントから逆算した推定値である点に注意してください。
コード自体のコメントも、実際の加速度とは一致しない場合があると明示しています。
ステップ2：飽和しているかを判定する
if (_acc_sp.xy().norm_squared() > acc_sp_xy_produced.norm_squared())
要求加速度のノルムが、制限後セットポイントから逆算した加速度のノルムより大きいとき、つまり推力が飽和して要求通りの加速度が出せていないときにARW補正を走らせます。
ノルムの比較、すなわち大きさだけを見ているため、ここでは主に「要求した水平加速度の大きさが、制限後に出せる大きさを超えているか」を見ています。
この条件を、水平推力が飽和しているかどうかを判定するための簡潔なチェックだと考えると読みやすいです。
ステップ3：積分器への入力を引き戻す（ARW補正）
飽和が検出された場合、速度誤差を次のように修正します。
const float arw_gain = 2.f / _gain_vel_p(0);
vel_error.xy() = Vector2f(vel_error) - arw_gain * (acc_sp_xy - acc_sp_xy_produced);
acc_sp_xy - acc_sp_xy_produced が「要求加速度と制限後加速度のずれ」、つまり飽和量です。このずれに比例して積分器への入力（速度誤差）を引き戻すことで、飽和が大きいほど積分器の入力を強く抑え、windup を防ぎます。
arw_gain は比例ゲインの逆数スケールで設計されており、Rundqwist (1990) の手法に基づいています。
まとめ
このブロック全体でやっていることを一言で言うと、
「推力が頭打ちになっているとき、その飽和量を加速度次元で測り、積分器が暴走しないよう入力を引き戻す」
です。
積分項の更新
最後に、補正済みの速度誤差で積分項を更新します。
// Make sure integral doesn't get NAN
ControlMath::setZeroIfNanVector3f(vel_error);
// Update integral part of velocity control
_vel_int += vel_error.emult(_gain_vel_i) * dt;
NaN ガード
ControlMath::setZeroIfNanVector3f(vel_error);
積分器を更新する前に、vel_error に NaN が含まれていないかを確認します。
NaN は「無効な値」であり、一度積分項に混入すると以降の演算がすべて NaN に汚染されます。ここで NaN の成分を 0 に置き換えることで、積分項が壊れるのを防いでいます。
積分項の更新
_vel_int += vel_error.emult(_gain_vel_i) * dt;
emult は要素ごとの積（element-wise multiply）です。つまり、
_vel_int += [vel_error.x * gain_i.x,
vel_error.y * gain_i.y,
vel_error.z * gain_i.z] * dt
という計算をしています。X・Y・Z それぞれに独立した積分ゲイン _gain_vel_i を掛け、時間刻み dt を乗じて積分項を更新します。
ここで使われる vel_error は、この関数内で段階的に補正されてきた値です。
元の速度誤差
↓ Z方向 anti-windup（飽和時は vel_error.z = 0）
↓ 水平方向 tracking ARW（飽和量に応じて vel_error.xy を引き戻し）
↓ NaN ガード
↓ ここで積分
つまり、この一行は _velocityControl() の締めくくりであり、上流のすべての補正を反映したうえで積分項を更新しています。
次回扱う内容
ここまでで、_velocityControl() の後半を読み、PX4 が推力制限と anti-windup をどのように扱っているかを見てきました。
_velocityControl() は、速度PIDから加速度目標 _acc_sp を作り、_accelerationControl() を呼び出した後、生成された推力目標 _thr_sp を制限し、最後に補正済みの速度誤差で積分項 _vel_int を更新します。
次回は、ここまで詳細には踏み込まなかった _accelerationControl() の中身を読みます。
加速度目標 _acc_sp から、どのように body_z と collective thrust を作り、最終的に推力ベクトル _thr_sp が生成されるのかを見ていきます。
補足情報
水平方向だけを見れば、運動方程式は次のようになります。
m * a_xy = F_xy
ここには重力 g は出てきません。
ただし、PX4 が持っている _thr_sp.xy() は物理推力 F_xy [N] ではなく、normalized thrust です。仮に、水平 thrust setpoint を T_xy、正規化に使う基準推力を F_ref とすると、
T_xy = F_xy / F_ref
なので、
F_xy = T_xy * F_ref
です。
これを水平方向の運動方程式に代入すると、
a_xy = F_xy / m
= T_xy * F_ref / m
となります。
ここで必要なのは、F_ref / m という換算係数です。
PX4 はこれを、ホバー推力 _hover_thrust から求めます。
ホバー時に必要な物理推力は、
F_hover = m * g
です。
一方、PX4 ではこのホバー推力を normalized thrust として _hover_thrust で表します。
_hover_thrust = F_hover / F_ref
= m * g / F_ref
この式を変形すると、
F_ref / m = g / _hover_thrust
になります。
したがって、
a_xy = T_xy * F_ref / m
= T_xy * g / _hover_thrust
となります。
つまり、この式に出てくる g は、水平方向に重力が働くという意味ではなく、normalized thrust を加速度 [m/s²] に換算するための基準として使われています。