一昨年からBNO085を使っていて、精度的に5度程度の誤差であきらめていたのですが、GPTと相談しながら
IMUのジャイロ角精度向上活動を再開しました。

●概要
「ICM42688をエンコーダー軸上で回転させて、ピーク補正を行った結果、±1度未満で補正できることが確認できた」
　下記グラフはN=5回測定したデータで90度往復回転での
エンコーダー角とIMUジャイロ積分角をピーク補正した場合
の誤差のプロットですが、ベストで±０．１度、
ワーストで±０．５度で収まってます。
＝＞マイコン側のログプログラムとPC側のPythonプログラムにノウハウ詰まってます、
全部ChtGPTに作らせてますが、仕様は私が指示したものです。
速度は4KHｚ=360*4000/4800=300dps

●IMU 　TDK ICM42688
https://strawberry-linux.com/catalog/items?code=42688
https://strawberry-linux.com/pub/ICM-42688-P_DS_Rev1.2.pdf
千円台クラスでは上出来のスペックをもったIMUです。
ノイズがMU6050の半分です。
　30秒間放置してもノイズ影響での誤差は0.015度です。

• ICM-42688（N=0.0028°/s/√Hz）→ 0.015°（1σ）

• MPU-6050（N=0.005）→ 0.027°（1σ）

●回転治具

エンコーダー軸上にIMU中心を固定してモーターで回転させる。
　エンコーダー：600ppr
   モーター：200pprステップモーター
　タイミングベルト：1/3減速
　分解能：1パルス8マイクロステップで駆動
 　　　　　モーター1回転＝1600ppr
                    エンコーダー（ 減速比３）=4800ppr
　　　　　90度回転＝1200パルス

●モータードライバ
TMC2209　V4をスタンドアローンモードで使用で使用

●マイコン配線

TMC２０９９のRXTXは使わない。

 

●Teensyプログラム
OK_MOTOR4KHz_05_SStop_gZInterp_IMU500Hz_Enc_DRDY_05_delay0msec.ino
GISTにあります。
https://gist.github.com/dj1711572002/9ef133ba335c54ea2c6395e9ca473937

特徴１：データログは、SDカードとかUSBシリアル通信では数十μsecのばらつきが発生するので
IMUとエンコーダーの時刻がばらついて、IMUの積分誤差の原因になるので、マイコンの配列
を大きくしてマイコンのメモリーに保持しておいて、測定終了後にUSBシリアルに送信する。

特徴２：IMUとエンコーダの時刻は独立してログする。PC　Pythonで時間を補間同期する。
割り込みタイミングの時刻をセンシング時刻としてログ

特徴３：ステップモーターの駆動処理がIMUとエンコーダーで遅延するので、PWMでパルスを発生させて
プログラムではモータードライブのスタートとストップしか手間をかけてません。

特徴４：モーターのSTOP関数内でなだらかに静止させる処理をいれてある。これがないと
停止と開始時のノイズとスパイクの発生が大きくて誤差の原因となる

特徴５：IMUデータのエラーをカウントしてある。

特徴６：電源オンから回転まで30秒ほどまってから回転して30秒分回って停止する。
モーターの回転指示は全て時間単位で指定する、PWMではステップ数が指定できないため。

 

●Pythonプログラム
　OK_Plot_imu_enc_Flatten_16_PeakReset.py
https://gist.github.com/dj1711572002/2ceff8ff12542f04fb8ff0ac3f7b039a

特徴１：エンコーダーのピーク値をアンカーとしてIMUのピーク値を修正することで誤差をゼロからスタートして、次のピークまでIMUのジャイロ積分角で補間する。

特徴２：関数　 _diff_ranged_peakwin(df)でピークの検索
IMUとENCのピーク位置がずれているが、強制的に合わせる。
特徴３：関数　_flatten_steps(df)でIMU　ｇｚ値のノイズ、欠落部を検出して平滑化

特徴４：PLOTして誤差の出方を観察して保存。

 

●感想
①測定
速度条件を変えながらN=5回測定しましたが、誤差の波形が随分違ってきます。
1回目が一番良くて、±０．１度になりますが、4回、5回で±０，５度くらいになります。
速度は、１KHｚ(75dps)、２KHz(150dps)、３KHｚ(225dps)、４KHｚ（300dps）では、1KHzは非常にノイズとばらつきが少なくて、良いデータがでます。
２，３，４KHzは同じ傾向がでます。

②IMUは振動に敏感に影響を受ける
　　モーターのドライブで急加速急減速があるとIMUの値に大きく影響します。
　　きれいな補正処理をしたい場合は、なめらかな回転にする必要があります。
③GPTからアドバイス

実験フロー（最短手戻り用）

1. 条件セット定義：周波数, S字窓, デューティ上限, 負荷, 温度

2. ログ取得：MOTOR01_SmoothStop_IMU_Enc_DRDY_05_Sep10_30sec を条件ごとに1本

3. 同期＆回転インデックス：imu_enc_sync_13_TurnIndexReset.py（rotNo 単位で分割）

4. 指標計算 → 失敗判定で rotNo 除外 → 条件別に集計

5. 可視化（箱ひげ＋散布）

指標の計算メモ（rotNo 単位）

• range(diff_ranged)

  • rotNo 区間の max(diff_ranged) - min(diff_ranged)

• RMSE

  • 区間内 sqrt(mean(diff_ranged**2))

• p95−p5

  • 区間内パーセンタイル 95 − 5

• 区間長（点数）

  • rotNo 区間のサンプル数

• slope(diff_ranged)

  • 区間内で t に対する一次回帰の傾き（符号＝残留ドリフト方向）

• 段差検出数（±100 ms 基準・可変窓対応）

  • まず速度依存窓：周期 T に対し window_us ≈ clip(0.05*T, 0.15*T)

  • diff_ranged を移動平均（窓 = 上記）で平滑 → 隣接点差の絶対値がしきい値超えを step とカウント

  • しきい値は「安定判定の閾値（下記）」と同一に固定

プロファイル整合

• 正弦 / 等加速度 / S字（tanh / 3-2-1）

• 角速度ピークが同一になるようパラメータを事前合わせ（ピーク±基準窓で比較）

速度依存の窓

• 基準：ピーク±100 ms

• 周期 T の 10% を目安に可変（下限 5%, 上限 15% 相当）

• 実装は window_us = int( clip(0.05*T, 0.15*T) )

安定判定（閾値の固定・共通化）

• 区間内 |Δ(I−E)| = abs(diff_ranged)

• 閾値 = median(|Δ|) + 4*MAD(|Δ|)

• 条件間で同一値を使う（最初の条件で算出→固定）

失敗検知（除外ロジック）

• 区間長 < 10 点

• RMSE が直近条件比で急増（例：中央値×2 以上）

• 段差多発（> 2 / 区間）
  → 上記いずれかでその rotNo を除外し、条件集計から外す

物理確認

• 反転直後の電流波形と diff_ranged の時系列を重ね、位相一致ならドライブ起因の可能性高

可視化（最短セット）

• 箱ひげ：条件ごとに range(diff_ranged) と p95−p5

• 散布：x=速度（または加速度）、y=slope(diff_ranged)、条件別にマーカー

準備OK。次ターンで「集計列名」を教えてください（既存列名のまま計算に差し込みます）。