本連載では、PX4 の PositionControl 全体像を整理し、位置・速度・加速度の setpoint を受け取り、最終的に加速度目標や推力目標へ変換していく流れを見ています。
対象とするコードは、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();
}
そして、前回までで、_positionControl() を読み、位置目標 _pos_sp から速度目標 _vel_sp が作られる流れを見ました。
今回読む範囲
今回は、その次に呼ばれる _velocityControl() を読みます。
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;
}
ご覧の通り、_velocityControl() はかなり密度の高い関数です。
でも、この関数の中では、大まかに言うと、
速度目標 `_vel_sp`
↓
速度誤差 `vel_error`
↓
速度PID
↓
加速度目標 `_acc_sp`
↓
_accelerationControl()
↓
推力目標 `_thr_sp`
↓
推力制限
↓
anti-windup
↓
積分項更新
という処理をしているだけなのです。
ただ、これらをコードベースで説明しようとすると、１回だけでは紙面が足りないので、今回は、前半部分に絞ります。
具体的には次の範囲です。
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();
// 説明の都合上、今回扱わない後半部分は省略しています。
...
}
_velocityControl() 前半では、_vel_sp から _acc_sp を作ります。
では、コードを見ていきましょう。
Z方向の積分項を制限する
最初に出てくるのは、Z方向の積分項を制限する処理です。
_vel_int(2) = math::constrain(_vel_int(2), -CONSTANTS_ONE_G, CONSTANTS_ONE_G);
_vel_int は、速度制御の I 項です。
後で見るように、この _vel_int は速度PIDの結果として、加速度目標に加算されます。
Vector3f acc_sp_velocity = vel_error.emult(_gain_vel_p) + _vel_int - _vel_dot.emult(_gain_vel_d);
つまり、_vel_int の単位は速度ではなく、加速度です。
_vel_int : [m/s²]
ここでは、そのうち Z方向成分だけを、-CONSTANTS_ONE_G から +CONSTANTS_ONE_G の範囲に制限しています。
_vel_int.z = [-1G, +1G] に制限
CONSTANTS_ONE_G は重力加速度（9.80665 m/s²）です。
つまりこの処理は、Z方向の積分補正を ±1G 相当の加速度範囲に制限しているだけです。
補足：geo.h
static constexpr float CONSTANTS_ONE_G = 9.80665f; // m/s^2
TIPS：なぜZ方向の積分項を制限するのか
ホバー推力の推定がずれている場合や外乱がある場合、P項だけでは定常的な速度誤差が残ることがあります。I項はその不足分を少しずつ補正するために必要です。
一方で、このI項は積分であり、それが蓄積され続けると、飽和が解除された後に大きなオーバーシュートを生み出す場合があります。
具体的なシナリオで説明します。
たとえば、強い下降気流にドローンが押されているとします。
- 機体が下に押されるので、Z方向に速度誤差が出続ける
- I項がその誤差を積分し、どんどん蓄積される
- 推力は上限に張り付いて（飽和して）、それ以上出せない
- それでも誤差が残るので、I項はさらに蓄積され続ける
この状態で突然、下降気流がやむと、
- 推力の飽和が解除される
- しかしI項にはすでに巨大な値が蓄積されている
- その分だけ過剰な上昇推力が出て、機体が目標高度を大きく超えてしまう
これがwindup による オーバーシュートです。
そのため、PX4 はZ方向の積分項 _vel_int.z を ±1G の範囲に制限しています。これは「積分器を止める処理」ではなく、すでに蓄積されている値の大きさを抑える処理です。
なお、推力飽和時に積分を進めないための anti-windup 処理は _velocityControl() 後半にあります。次回扱います。
速度誤差を計算する
次に、速度誤差を計算します。
Vector3f vel_error = _vel_sp - _vel;
これは非常に素直な式です。
速度誤差 = 速度目標 - 現在速度
各変数の意味は次の通りです。
_vel_sp : 速度目標 [m/s]
_vel : 現在速度 [m/s]
vel_error : 速度誤差 [m/s]
速度PIDで加速度目標を作る
次が、今回の中心です。
Vector3f acc_sp_velocity = vel_error.emult(_gain_vel_p) + _vel_int - _vel_dot.emult(_gain_vel_d);
分解すると、次のようになります。
acc_sp_velocity
= P項 + I項 + D項
それぞれを見ると、
P項 : vel_error.emult(_gain_vel_p)
I項 : _vel_int
D項 : - _vel_dot.emult(_gain_vel_d)
です。
ここで emult() は element-wise multiplication、つまり成分ごとの掛け算です。
通常の行列積ではありません。
vel_error.emult(_gain_vel_p)
= (
vel_error.x * gain_vel_p.x,
vel_error.y * gain_vel_p.y,
vel_error.z * gain_vel_p.z
)
PX4 では、X/Y/Z 方向ごとに異なるゲインを持てるため、このように成分ごとの掛け算になっています。
P項：速度誤差から加速度を作る
P項は次の部分です。
vel_error.emult(_gain_vel_p)
意味としては、
速度誤差 [m/s] × 速度Pゲイン [1/s]
= 加速度目標 [m/s²]
です。
単位を見ると分かりやすいです。
vel_error : [m/s]
_gain_vel_p : [1/s]
vel_error * _gain_vel_p : [m/s²]
つまり、速度Pゲインは、
速度誤差をどれくらいの加速度要求に変換するか
を決める係数です。
速度目標に対して現在速度が足りない場合、P項はその差を埋める方向の加速度を要求します。
I項：積分器の状態を加速度目標に足す
I項は次の部分です。
_vel_int
ここで少し用語を整理しておきます。
_vel_int は、速度制御の積分器の状態です。
一方、_gain_vel_i は、その積分器を更新するときに使われる I ゲインです。
_vel_int : 積分器の状態 [m/s²]
_gain_vel_i : 積分器更新に使う I ゲイン
今回読んでいる _velocityControl() 前半では、_vel_int はすでに蓄積済みの補正量として使われます。
なお、_vel_int の更新処理は _velocityControl() 後半にあります。次回扱います。
D項：現在加速度を使って減衰を与える
D項は次の部分です。
- _vel_dot.emult(_gain_vel_d)
ここで重要なのは、PX4 が「速度誤差の微分」を直接使っているわけではないという点です。
使っているのは _vel_dot です。
_vel_dot は、現在速度 _vel の時間微分、つまり現在加速度に相当する量です。
_vel_dot : [m/s²]
通常、PID制御のD項というと、誤差の微分を使うイメージがあります。
速度制御であれば、
速度誤差 = 速度目標 - 現在速度
なので、速度誤差の微分は、
速度目標の微分 - 現在速度の微分
になります。
しかし、速度目標 _vel_sp は setpoint です。
setpoint は外部から急に変わることがあります。
もし setpoint 側の微分をそのままD項に入れると、速度目標が急変した瞬間にD項が大きく跳ねる可能性があります。
そこで PX4 では、setpoint 側の微分ではなく、現在速度の微分、つまり measurement 側の微分である _vel_dot を使って減衰を与えています。
式にはマイナスが付いています。
- _vel_dot.emult(_gain_vel_d)
つまり、現在すでに加速している場合、その加速を抑える方向の補正が入ります。
直感的には、
P項：
速度目標に近づくために加速しろ
D項：
でも、すでに加速しているなら少し抑えろ
という関係です。
これにより、setpoint の急変に対するD項のスパイクを避けつつ、実際の機体運動に対して減衰を与えることができます。
単位で見る速度PID
ここまでの式を、単位で整理します。
Vector3f acc_sp_velocity = vel_error.emult(_gain_vel_p) + _vel_int - _vel_dot.emult(_gain_vel_d);
各項の単位は次の通りです。
_vel_sp : [m/s]
_vel : [m/s]
vel_error : [m/s]
_gain_vel_p : [1/s]
P項 : [m/s²]
_vel_int : [m/s²]
I項 : [m/s²]
_vel_dot : [m/s²]
_gain_vel_d : [-]
D項 : [m/s²]
acc_sp_velocity : [m/s²]
最終的に acc_sp_velocity は加速度目標になります。
ここが重要です。
速度PIDの出力は thrust ではない
速度PIDの出力は加速度目標である
この設計により、PX4 は PositionControl の内部で、
位置目標
↓
速度目標
↓
加速度目標
↓
推力目標
という段階的な構造を持っています。
関係するPX4パラメータ
速度制御のゲインは、PX4 パラメータとしてはおおむね次のように対応します。
_gain_vel_p.x/y : MPC_XY_VEL_P_ACC
_gain_vel_i.x/y : MPC_XY_VEL_I_ACC
_gain_vel_d.x/y : MPC_XY_VEL_D_ACC
_gain_vel_p.z : MPC_Z_VEL_P_ACC
_gain_vel_i.z : MPC_Z_VEL_I_ACC
_gain_vel_d.z : MPC_Z_VEL_D_ACC
XY方向とZ方向で、別々の速度制御ゲインを持っている点が重要です。
マルチコプターでは、水平移動と高度制御で力の出し方が異なります。
水平移動では、機体を傾けることで推力に水平成分を作ります。
一方、Z方向では、主に総推力の増減によって上昇・下降を制御します。
そのため、XY方向とZ方向の制御ゲインは分けて調整されます。
加速度setpointに合成する
速度PIDで作った acc_sp_velocity は、そのまま _acc_sp に加算されます。
ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity);
ここで _acc_sp は、加速度 setpoint です。
_acc_sp : 加速度目標 [m/s²]
この処理は、単純な代入ではなく、加算です。
_acc_sp に acc_sp_velocity を加える
なぜ加算なのかというと、PX4 の PositionControl では、外部から acceleration setpoint / feed-forward が与えられる場合があるからです。
つまり _acc_sp には、すでに外部から与えられた加速度 feed-forward が入っている可能性があります。
そこに、速度PID由来の加速度目標 acc_sp_velocity を加算します。
外部 acceleration setpoint / feed-forward
+
速度制御由来の加速度目標
=
最終的な加速度目標 `_acc_sp`
ここで足し算には、addIfNotNanVector3f()が使われています。これは、前回で説明しています。
_accelerationControl() に渡す
加速度目標 _acc_sp が作られた後、次に呼ばれるのが _accelerationControl() です。
_accelerationControl();
ここで、ようやく加速度目標が推力目標へ変換されます。
今回の記事では _accelerationControl() の中身には深く入りませんが、役割を一言でいうと、
加速度目標 `_acc_sp` から、推力ベクトル `_thr_sp` を生成する処理
です。
より具体的には、PX4 は _acc_sp から、
欲しい加速度
↓
その加速度を出すための推力方向
↓
body_z
↓
collective thrust
↓
3D thrust vector `_thr_sp`
という流れで thrust setpoint を作ります。
ここでも重要なのは、PX4 がいきなり roll / pitch や thrust を直接作っているわけではないという点です。
まず加速度目標を作り、その加速度を実現するための推力方向と推力大きさを計算します。
ここまでの流れ
今回読んだ _velocityControl() 前半の流れをまとめると、次のようになります。
1. Z方向の積分項 `_vel_int.z` を ±1G に制限する
2. 速度目標 `_vel_sp` と現在速度 `_vel` から速度誤差を計算する
3. 速度誤差に対して PID 制御を行い、
速度制御由来の加速度目標 `acc_sp_velocity` を作る
4. `acc_sp_velocity` を `_acc_sp` に加算する
5. `_accelerationControl()` を呼び出し、
加速度目標から推力目標 `_thr_sp` を生成する
コードとの対応は次の通りです。
// 1. Z方向積分項を制限
_vel_int(2) = math::constrain(_vel_int(2), -CONSTANTS_ONE_G, CONSTANTS_ONE_G);
// 2. 速度誤差
Vector3f vel_error = _vel_sp - _vel;
// 3. 速度PIDで加速度目標を作る
Vector3f acc_sp_velocity =
vel_error.emult(_gain_vel_p)
+ _vel_int
- _vel_dot.emult(_gain_vel_d);
// 4. 加速度setpointに合成する
ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity);
// 5. 加速度目標から推力目標を作る
_accelerationControl();
次回扱う内容
_accelerationControl() によって _thr_sp が生成された後、さらに次のような処理が続きます。
Z方向の anti-windup
垂直制御を優先した推力制限
水平推力の余裕確保
総推力上限による saturation
水平 tracking anti-windup
速度積分項の更新
次回は、この _velocityControl() 後半を読み、PX4 が推力制限と anti-windup をどのように扱っているかを見ていきます。
なお、_accelerationControl() の中身については、PositionControl 編の最終回で改めて扱います。