前回の記事では、PX4 の PositionControl 全体像を整理し、位置・速度・加速度の setpoint を受け取り、最終的に加速度目標や推力目標へ変換していく流れを見ました。
今回は、その PositionControl の実装を実際に読み進めながら、制御内容を具体的に読み解いていきたいと思います。
対象とするコードは、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()
まず、今回読む _positionControl() の実装を見てみます。
void PositionControl::_positionControl()
{
// P-position controller
Vector3f vel_sp_position = (_pos_sp - _pos).emult(_gain_pos_p);
// Position and feed-forward velocity setpoints or position states being NAN results in them not having an influence
ControlMath::addIfNotNanVector3f(_vel_sp, vel_sp_position);
// make sure there are no NAN elements for further reference while constraining
ControlMath::setZeroIfNanVector3f(vel_sp_position);
// Constrain horizontal velocity by prioritizing the velocity component along the
// the desired position setpoint over the feed-forward term.
_vel_sp.xy() = ControlMath::constrainXY(
vel_sp_position.xy(),
(_vel_sp - vel_sp_position).xy(),
_lim_vel_horizontal);
// Constrain velocity in z-direction.
_vel_sp(2) = math::constrain(_vel_sp(2), -_lim_vel_up, _lim_vel_down);
}
この関数の中で中心になるのは、_vel_sp です。
_vel_sp は PositionControl クラスのメンバ変数で、velocity setpoint、つまり速度目標を表します。
PX4 のコードでは、メンバ変数に _ prefix を付けるスタイルが使われています。そのため、_pos_sp、_pos、_gain_pos_p、_vel_sp などは、いずれも PositionControl の内部状態として扱われる変数です。
ここで重要なのは、_positionControl() という名前であっても、この関数が直接 thrust setpoint を作っているわけではない、という点です。
この関数が作っているのは、次段の _velocityControl(dt) に渡すための速度目標 _vel_sp です。
つまり、_positionControl() は、
位置目標 _pos_sp
↓
現在位置 _pos との差分
↓
位置P制御
↓
速度目標 _vel_sp
という変換を行う関数です。
位置誤差から速度目標を作る
Vector3f vel_sp_position = (_pos_sp - _pos).emult(_gain_pos_p);
ここでは、位置目標 _pos_sp と現在位置 _pos の差分を取り、その位置誤差に位置Pゲイン _gain_pos_p を掛けています。
式として見ると、次のようになります。
vel_sp_position = (_pos_sp - _pos) * _gain_pos_p
ただし、ここで使われている .emult() は、行列積ではありません。
emult は element-wise multiplication、つまり成分ごとの掛け算です。
そのため、x, y, z の各軸について、
vel_sp_position.x = (_pos_sp.x - _pos.x) * _gain_pos_p.x
vel_sp_position.y = (_pos_sp.y - _pos.y) * _gain_pos_p.y
vel_sp_position.z = (_pos_sp.z - _pos.z) * _gain_pos_p.z
という計算になります。
単位で見ると、さらに意味が分かりやすくなります。
_pos_sp - _pos : [m]
_gain_pos_p : [1/s]
vel_sp_position: [m/s]
つまり、位置誤差 [m] に位置Pゲイン [1/s] を掛けることで、速度目標 [m/s] を作っています。
ここで重要なのは、位置制御の出力が直接 thrust ではないことです。
PX4 の _positionControl() では、位置誤差からまず「目標位置へ向かうための速度」を作ります。
位置誤差が大きい
↓
大きな速度目標を出す
位置誤差が小さい
↓
小さな速度目標にする
という、非常に素直なP制御になっています。
velocity feed-forward と合成する
ControlMath::addIfNotNanVector3f(_vel_sp, vel_sp_position);
次に、位置制御由来の速度目標 vel_sp_position を、_vel_sp に加算しています。
ここでの _vel_sp は、外部から与えられた velocity setpoint、または velocity feed-forward として扱われます。
つまり、この行では、
速度目標 _vel_sp
=
外部から与えられた速度目標 / feed-forward
+
位置制御由来の速度目標 vel_sp_position
という合成を行っています。
ただし、関数名に IfNotNan とあるように、単純にすべての成分を加算しているわけではありません。
PX4 では、NaN が「未指定」を表す値として使われます。
そのため、たとえば position setpoint が指定されていない軸では、その軸の位置制御由来速度は有効な値になりません。
この関数は、NaN の成分を無視し、有効な成分だけを _vel_sp に加算します。
つまり、PX4 の setpoint では、
値が finite
→ 指定されている
値が NaN
→ 未指定
という扱いになっています。
この仕組みによって、position control と velocity feed-forward を柔軟に組み合わせることができます。
たとえば、位置目標を与えつつ、同時に速度 feed-forward を与えると、
目標位置へ向かう速度
+
あらかじめ進ませたい速度
を合成した速度目標を作ることができます。
NaN を 0 に正規化する
ControlMath::setZeroIfNanVector3f(vel_sp_position);
次の行では、vel_sp_position の NaN 成分を 0 に置き換えています。
一見すると、直前の addIfNotNanVector3f() で NaN を無視しているので、ここでさらに 0 にする必要があるのか疑問に思うかもしれません。
しかし、この後の処理では vel_sp_position を速度制限の計算に使います。
_vel_sp.xy() = ControlMath::constrainXY(
vel_sp_position.xy(),
(_vel_sp - vel_sp_position).xy(),
_lim_vel_horizontal);
ここで vel_sp_position に NaN が残っていると、速度制限の計算が壊れてしまいます。
そのため、この段階で NaN を 0 に置き換えています。
意味としては、
未指定の軸
↓
その軸の位置制御由来速度は 0 として扱う
ということです。
ここで注意したいのは、NaN を 0 にする対象が _vel_sp ではなく、vel_sp_position である点です。
vel_sp_position は、あくまで位置制御由来の速度成分です。
位置制御として未指定だった軸は、その後の速度制限処理では「位置制御由来の寄与はない」として扱うため、0 に正規化しています。
水平速度を制限する
_vel_sp.xy() = ControlMath::constrainXY(
vel_sp_position.xy(),
(_vel_sp - vel_sp_position).xy(),
_lim_vel_horizontal);
次に、水平速度目標 _vel_sp.xy() を制限しています。
ここで扱っているのは x/y 成分なので、Z方向ではなく、水平方向の速度制限です。
constrainXY() の引数を分解すると、次のようになります。
第1引数: vel_sp_position.xy()
これは、位置制御由来の水平速度目標です。
第2引数: (_vel_sp - vel_sp_position).xy()
これは、velocity feed-forward 由来の水平速度成分です。
少し分かりづらいですが、この時点の _vel_sp には、すでに
velocity feed-forward
+
位置制御由来の速度目標
が合成されています。
そのため、そこから vel_sp_position を引くことで、位置制御由来の成分を取り除き、feed-forward 成分だけを取り出しています。
第3引数: _lim_vel_horizontal
これは、水平速度の上限です。
PX4 のパラメータとしては、主に MPC_XY_VEL_MAX に対応します。
ここで重要なのは、constrainXY() が単純に _vel_sp.xy() 全体を正規化しているわけではない、という点です。
PX4 のコメントにも、次のように書かれています。
// Constrain horizontal velocity by prioritizing the velocity component along the
// the desired position setpoint over the feed-forward term.
つまり、この関数は、
位置制御由来の速度を優先し、
そのうえで feed-forward 速度を足せるだけ足す
という制限を行います。
constrainXY(v0, v1, max) の考え方は、次のように見ると分かりやすいです。
v0 = 位置制御由来の速度
v1 = feed-forward 由来の速度
max = 水平速度上限
このとき、constrainXY(v0, v1, max) は、
v0 をできるだけ維持する
v1 は max を超えない範囲で足す
最終的な速度ベクトルの大きさを max 以下に収める
という処理を行います。
単純に v0 + v1 を作って、それが上限を超えたら全体を縮小する、という処理ではありません。
もし単純に全体を縮小してしまうと、位置目標へ向かうための速度成分 v0 まで削られてしまいます。
そこで PX4 では、まず位置制御由来の速度 v0 を優先します。
そして、速度上限に余裕がある範囲で、feed-forward 速度 v1 を加えます。
たとえば、位置目標へ向かうためにすでに速度上限近くの速度が必要な場合、feed-forward 成分はあまり足されません。
逆に、位置制御由来の速度が小さい場合は、そのぶん feed-forward 成分を足す余地があります。
つまり、この処理は、
位置目標へ向かう制御を優先しながら、
水平速度上限の範囲内で velocity feed-forward も反映する
ための速度制限です。
constrainXY() の幾何的な見方
ここで、少しだけ constrainXY() の内部処理を補足しておきます。
この関数は、|v0 + v1| <= max の場合、そのまま v0 + v1 を返します。
つまり、constrainXY() は常に速度を最大値 max に張り付ける関数ではありません。
しかし、|v0 + v1| > max の場合は、水平速度上限を超えないように速度ベクトルを制限します。
このとき、単純に v0 + v1 全体を縮小するのではなく、v0 を優先して残し、v1 方向の成分を調整します。
一般の場合、constrainXY() は最終的な速度ベクトル vf を次のように考えます。
vf = v0 + s * unit(v1)
ここで、unit(v1) は v1 の向きだけを取り出した単位ベクトルです。
s は、その方向にどれだけ足せるかを表す係数です。
|v0 + v1| > max の場合に求めたいのは、最終的な速度ベクトル vf の大きさが、ちょうど水平速度上限 max になるような s です。
|vf| = max
つまり、
|v0 + s * unit(v1)| = max
を満たす s を求めています。
幾何的には、
v0 の先端から v1 方向へ直線を伸ばし、
半径 max の円と交わる点を探している
と見ることができます。
この交点までが、速度上限の範囲内で feed-forward 成分を足せる最大量です。
そのため、v0 を優先して残しながら、v1 を速度上限内で最大限反映できます。
コード上では、この s を二次方程式として解いています。
Vector2f u1 = v1.normalized();
float m = u1.dot(v0);
float c = v0.dot(v0) - max * max;
float s = -m + sqrtf(m * m - c);
return v0 + u1 * s;
ここでやっていることは、数式としては少し複雑に見えますが、意味としてはシンプルです。
位置制御由来の速度 v0 は優先して残す
feed-forward 由来の速度 v1 は、水平速度上限を超えない範囲で足す
このため、constrainXY() は、位置制御と velocity feed-forward を合成するうえで重要な役割を持っています。
Z方向速度を制限する
_vel_sp(2) = math::constrain(_vel_sp(2), -_lim_vel_up, _lim_vel_down);
最後に、Z方向の速度目標を制限しています。
ここで _vel_sp(2) は、Z方向の速度目標です。
PX4 のローカル座標系は NED です。
NED:
x : North
y : East
z : Down
つまり、Z軸は下向きが正です。
そのため、上昇方向の速度は負、下降方向の速度は正になります。
上昇速度 : 負
下降速度 : 正
このため、Z方向速度の制限は次のようになります。
math::constrain(_vel_sp(2), -_lim_vel_up, _lim_vel_down);
上昇速度の上限は _lim_vel_up ですが、NEDでは上昇が負方向なので、
-_lim_vel_up
が下限になります。
一方、下降速度は NED の正方向なので、
_lim_vel_down
が上限になります。
つまり、この行は、
_vel_sp.z を [-上昇速度上限, 下降速度上限] に制限する
という処理です。
関係する PX4 パラメータとしては、次のものがあります。
_lim_vel_up : MPC_Z_VEL_MAX_UP
_lim_vel_down : MPC_Z_VEL_MAX_DN
ここで重要なのは、高度方向の追従性が、位置Pゲインだけで決まるわけではないという点です。
たとえば、高度誤差が大きくても、_vel_sp.z はこの行で制限されます。
そのため、Z方向の挙動は、
MPC_Z_P
MPC_Z_VEL_MAX_UP
MPC_Z_VEL_MAX_DN
の影響を受けます。
位置Pゲインで速度目標を作り、その後で上昇・下降速度上限に収める、という流れです。
_vel_sp は周期をまたいで加算され続けるわけではない
ここで少し注意が必要です。
ControlMath::addIfNotNanVector3f(_vel_sp, vel_sp_position);
というコードを見ると、_vel_sp に毎回 vel_sp_position が加算されていき、制御周期をまたいで値が蓄積していくように見えるかもしれません。
しかし、そういう意味ではありません。
_vel_sp はメンバ変数ですが、各 update の入口で入力 setpoint の velocity から設定されます。
そのため、この加算は、
前回周期の _vel_sp にさらに加算する
という意味ではなく、
今回周期で与えられた velocity setpoint / feed-forward に、
今回周期で計算した位置制御由来速度を加算する
という意味です。
つまり、これは制御周期をまたいだ累積ではありません。
あくまで、当該周期における
velocity feed-forward
+
position control output
の合成です。
_positionControl() のまとめ
_positionControl() の処理をまとめると、次のようになります。
1. 位置目標 _pos_sp と現在位置 _pos の差分を取る
2. 位置誤差に位置Pゲイン _gain_pos_p を掛ける
3. 位置制御由来の速度目標 vel_sp_position を作る
4. velocity setpoint / feed-forward と合成する
5. NaN を未指定として扱う
6. 水平速度を _lim_vel_horizontal で制限する
7. Z方向速度を _lim_vel_up / _lim_vel_down で制限する
8. 制約済み速度目標 _vel_sp を得る
一言でいうと、_positionControl() は、
位置目標から、制約済みの速度目標 _vel_sp を作る関数
です。
ここで作られた _vel_sp は、次の _velocityControl(dt) に渡されます。
つまり、PX4 の PositionControl は、位置誤差からいきなり推力を作るのではなく、
位置目標
↓
_positionControl()
↓
速度目標 _vel_sp
↓
_velocityControl(dt)
↓
加速度目標 _acc_sp
↓
推力目標 _thr_sp
という段階的な構造になっています。
この構造を押さえておくと、次に読む _velocityControl(dt) の意味がかなり分かりやすくなります。
_velocityControl(dt) では、今回作った速度目標 _vel_sp と現在速度 _vel の差分から、速度PID制御によって加速度目標 _acc_sp を作ります。
次回は、この _velocityControl(dt) の中を読み、速度目標がどのように加速度目標や推力目標へ変換されていくのかを見ていきます。