Basic Toolkit › 信号処理コンポーネント

imu_orientation_estimator

ジャイロと加速度から、選べば磁気も使って、GNSS なしで姿勢のクォータニオンを推定します。

処理内容

• `gyro` のサンプルごとに、前と今のサンプルの平均で姿勢を動かし、最新の加速度でロールとピッチを補正します。
• `filter_mode` で補正の方式を選びます。Mahony（相補フィルタ。ジャイロのバイアスも推定）または Madgwick（勾配降下法）です。
• `use_magnetometer` を使うと、最新の磁場で yaw も補正し、yaw がドリフトしなくなります。
• 最初の `gyro` のサンプルで、加速度（と磁場）から最初の姿勢を作ります。磁気がなければ yaw は 0 から始まります。
• `gyro`、`acc`、`mag` のタイムスタンプはそろっていなくてかまいません。出力のタイムスタンプは `gyro` と同じです。

データ形式

• `gyro`、`acc`、`mag` は 3 要素で、ほかの要素数はエラーで止まります。軸ごとに出力があるセンサは、RTMaps の Vectorizer（Sampling mode = Synchronized）でまとめます。
• `acc` は比力で、静止して水平なとき +Z が正です。`acc` と `mag` は向きだけを使うので、単位は問いません。
• `quaternion` は `[w, x, y, z]`（機体 → ワールド）です。機体の軸が X 前、Y 左、Z 上なら、`imu_to_pose6D` に `wxyz`、`ENU` でつなぎます。

注意事項

• 磁気がなければ yaw は相対で、ドリフトします。磁気があれば yaw は磁北が基準で、`magnetic_declination_in_deg`（東偏が正）を入れると真北が基準になります。Topic「磁気偏角」を参照してください。
• 動きの加速度も重力として扱うので、大きな加減速の間はロールとピッチに誤差が出ます。
• 磁気センサはあらかじめ較正してください。モータ、鉄骨、バッテリーの近くでは乱れます。
• `acc`、`mag` が NaN・無限大、またはノルムが 1e-6 より小さいサンプルは無視し、最後の有効なサンプルを使います。`gyro` が NaN・無限大のサンプルは飛ばします。警告は 1 回出します。
• GNSS があるときは EKF を使ってください。このコンポーネントは位置を使いません。

参考文献

• [1] R. Mahony, T. Hamel and J.-M. Pflimlin, "Nonlinear Complementary Filters on the Special Orthogonal Group," IEEE Transactions on Automatic Control, vol. 53, no. 5, pp. 1203-1218, 2008. https://doi.org/10.1109/TAC.2008.923738 — 使っている機能：Mahony の更新。
• [2] S. O. H. Madgwick, A. J. L. Harrison and R. Vaidyanathan, "Estimation of IMU and MARG orientation using a gradient descent algorithm," 2011 IEEE International Conference on Rehabilitation Robotics, pp. 1-7, 2011. https://doi.org/10.1109/ICORR.2011.5975346 — 使っている機能：Madgwick の更新。
• [3] T. Foote and M. Purvis, REP 103: Standard Units of Measure and Coordinate Conventions, ROS Enhancement Proposals. https://www.ros.org/reps/rep-0103.html — 使っている機能：軸と単位。

プロパティ

出力

入力