はじめに
独立ステアリング（Swerve Driveなど）を制御していると、ある角度の境界を跨いだ瞬間にステアリングが激しく振動する現象に遭遇しました。具体的には、目標角度と現在角度の差分を計算する際に、$-\pi$ から $\pi$ の境界付近で制御値が急激にジャンプし、モーターが暴れるというものです。
原因を調べていくと、オイラー角の不連続性と逆三角関数の出力範囲の制限に行き着きました。
本記事では、この問題をクォータニオンと std::atan2 で解決した方法と、なぜそれで直るのかを解説します。
解決策のコード
姿勢をクォータニオンで受け取り、そこからヨー角（Z軸周りの回転角）を算出する関数を実装しました。
double AxisMoveAction::yaw_from_quat(double x, double y, double z, double w) {
double siny_cosp = 2.0 * (w * z + x * y);
double cosy_cosp = 1.0 - 2.0 * (y * y + z * z);
return std::atan2(siny_cosp, cosy_cosp);
}
たったこれだけで、あの不安定な挙動がピタッと収まりました。
なぜこのコードで直るのか
理由1: std::atan2 が全象限を正しく判別してくれる
単純な std::atan(y / x) は出力範囲が $-\frac{\pi}{2}$ ～ $\frac{\pi}{2}$ に限定されます。つまり、第2象限と第4象限、第1象限と第3象限の区別ができません。
これだと何が困るかというと、ステアリングが $\pi$ 付近を通過した瞬間に、角度が $+\pi$ から $-\pi$ にジャンプ（あるいはその逆）し、制御器が「180度回転しなきゃ！」と誤認して暴れるわけです。
一方、std::atan2(y, x) は引数 y と x の符号を個別に評価します。
x |
y |
atan(y/x) |
atan2(y, x) |
|---|---|---|---|
| + | + | 第1象限 ✅ | 第1象限 ✅ |
| - | + | 第3象限 ❌ | 第2象限 ✅ |
| - | - | 第1象限 ❌ | 第3象限 ✅ |
| + | - | 第4象限 ✅ | 第4象限 ✅ |
atan2 は $-\pi$ ～ $\pi$ の全範囲で一意な角度を返すため、符号反転による値飛びが起きません。
下のグラフは、同じ入力角度 $\theta$ に対する atan と atan2 の出力を比較したものです。
赤線（atan）は ±90° で不連続にジャンプしていますが、青線（atan2）は -180°～+180° を滑らかにカバーしています。赤線のジャンプがまさにステアリングが暴れる原因です。
atan の反転を体感する
下のアニメーションは、ステアリング角を -180°～+180° で動かしたときの atan と atan2 の出力を針で可視化したものです。
±90° を超えたあたりで、左の atan の針が意図と逆方向を指すのがわかります。実際のステアリングでは、この瞬間にモーターが逆回転しようとして暴れます。右の atan2 は常に正しい方向を指しており、安定しています。
理由2: クォータニオンから回転行列要素を直接計算している
コード中の siny_cosp と cosy_cosp は、クォータニオン $q = (x, y, z, w)$ から $3 \times 3$ の回転行列 $R$ に変換したときの要素に対応しています。
$$R_{21} = 2(wz + xy)$$
$$R_{11} = 1 - 2(y^2 + z^2)$$
ヨー角 $\psi$ は、これらを使って以下のように求まります。
$$\psi = \text{atan2}(R_{21},\ R_{11})$$
ここで重要なのは、オイラー角ではなくクォータニオンで姿勢を管理しているという点です。オイラー角同士の加減算で姿勢計算をしていると、ジンバルロック（特定の角度で自由度が縮退する現象）や角度の不連続性が発生しやすくなります。
クォータニオンは特異点を持たないため、姿勢をクォータニオンで保持しておき、制御に必要なときだけ atan2 でヨー角に変換する設計にすることで、境界付近での数学的な破綻を避けられます。
制御に組み込むときの注意点
yaw_from_quat で安定したヨー角が得られるようになっても、PID制御などで偏差（目標値 - 現在値）を計算するときに油断すると同じ問題が再発します。
例えば、目標角度が $+170°$、現在角度が $-170°$ のとき、単純に引き算すると偏差は $340°$ になってしまいます。実際には $-20°$ 回ればいいのに、制御器は大回りしようとします。
これを防ぐために、偏差を $-\pi$ ～ $\pi$ に正規化するラップアラウンド処理を入れましょう。
double normalize_angle(double angle) {
while (angle > M_PI) angle -= 2.0 * M_PI;
while (angle < -M_PI) angle += 2.0 * M_PI;
return angle;
}
// 使い方
double error = normalize_angle(target_yaw - current_yaw);
これで偏差が常に最短経路になり、ステアリングが無駄に大回りすることを防げます。
おわりに
独立ステアリングの「ガチャガチャ問題」は、突き詰めるとオイラー角の不連続性に起因していました。クォータニオンで姿勢を管理し、必要なときだけ atan2 でヨー角に変換するという設計パターンは、ロボットの姿勢制御全般で使える考え方です。
ロボコンでステアリング制御に苦しんでいる方の参考になれば幸いです。
