ESP32S3 6軸センサ ICM42670Pで姿勢を検出
2026.2.20 Coskx Lab
１ はじめに
6軸センサ ICM42670Pは自分自身のxyz3軸方向の加速度およびxyz3軸周りの角速度を測定するセンサです。
xyz3軸方向の加速度およびxyz3軸周りの角速度から，姿勢を表すオイラー角（pitch,roll,yaw）の変換すると，センサの姿勢を表現できます。
そして，倒立振り子台車やドローンなどに取り付けると，機器の姿勢を得ることができます。
3軸加速度・3軸角速度の値から，pitch,roll,yawに変換するフィルタはいろいろあるのですが，
演算量が少なくよく使われているmadgwickfilterを使用します。このフィルタはライブラリとして公開されていますので，
そのライブラリを取り込んで使います。
pitch,roll,yawの３つの角はいろいろな定義があるようですが，姿勢制御の場面では次のような定義が多く使われているようです。
飛行機での機首上げではpitchは正の値です。また左翼上げでrollは正の値になります。
しかし，ICM42670Pから3軸加速度・3軸角速度を受け取り，その受け取った値からmadgwickfilterライブラリで得られるpitch,roll,yawの３つの角は次のようになっています。
ICM42670Pのｘ軸方向を機器の前方と考えると，飛行機の機首上げでpitchの値が負になって符号が逆になっています。これはプログラム内で解決することにします。
なお，実際の機器にICM42670Pを取り付けたときに，ICM42670Pのy軸方向を機器の前方になってしまうこともあります。
その場合，pitchとrollが逆になりますが，これもプログラム内で解決することにします。
２ 使用環境
- Windows 11 64-bit
- Arduino IDE 2.3.7
- Xiao ESP32S3
- 6軸センサ ICM42670P
- library: ICM42670P by TDK/Invensense
- library: Madgwick by Arduino
３ 6軸センサ ICM42670PをxiaoESP32S3に取り付け
ICM42670PはI2C通信で測定値をマイコンに伝えるので，4本の線でつなぎます。
I2C通信のSDAとSCLの2つの端子はそれぞれ4.7kΩで3.3Vにプルアップしています。
このプルアップがないと，ICM42670Pの初期化が出来ないなどの不具合が生じます。
４ 6軸センサ ICM42670Pを用い，オイラー角を得るプログラム
ICM42670Pの操作はICM42670Pライブラリを使用するので，ArduinoIDEのライブラリマネージャを使って先にライブラリを取り込んでおきます。
「ICM42670P」で検索すると，「ICM42670P by TDK/Invensenset」が見つかるので取り込みます。
また，madgwickfilterもライブラリを使用するので，先にライブラリを取り込んでおきます。
「madgwick」で検索すると，「madgwick by Arduino」が見つかるので取り込みます。
上記2つの準備ができたら，次のプログラムを実行できます。
なお，プログラム中，ICM42670Pから直接得られるxyz軸方向加速度とxyz軸周り角速度は，ICM42670Pの内部表現です。
この内部表現は16itの符号付き整数になっていて，初期化ICM42670P.initialize()により次のように設定された値となっています。
加速度 2G(2x9.8m/s²)のとき32768
角速度 250deg/secのとき32768
この内部表現は，変換関数convertRawAcceleration()とconvertRawGyro()によってそれぞれm/s²，deg/sec（度/秒）に変換されています。
また，madgwickから得られるpitch,roll,yawの３つの角の単位はdeg（度）です。
6軸センサ ICM42670Pを用い，オイラー角を得るプログラム
//ICM42670P_test.ino
//library: ICM42670P by TDK/Invensense
//library: Madgwick by Arduino
#include "ICM42670P.h"
#include <MadgwickAHRS.h>
ICM42670 IMU_icm42670(Wire, false);
Madgwick madgwickfilter;
float acc_fsrange;
float gyro_fsrange;
unsigned long microsPerReading, microsPrevious;
int frequency = 100; //Hz
void setup() {
int ret;
Serial.begin(115200);
//while(!Serial) {}
delay(100);
//pinMode(SDA, INPUT_PULLUP);pinMode(SCL, INPUT_PULLUP);
// These won't work. The pull-up resistor value is too high.
//After adding 4.7k pull-up resistors to the SDA and SCL pins,
// startup issues completely disappeared.
// Initializing the ICM42670
Serial.println("ICM42670 initialization started");
ret = IMU_icm42670.begin();
if (ret != 0) {
Serial.print("ICM42670 initialization failed : ");
Serial.println(ret);
while(1);
}
Serial.println("ICM42670 initialization completed");
delay(100);
// Accel ODR = 100 Hz and Full Scale Range = 2G
IMU_icm42670.startAccel(100,2);
acc_fsrange = 2.0f;
// Gyro ODR = 100 Hz and Full Scale Range = 250 dps
IMU_icm42670.startGyro(100,250);
gyro_fsrange = 250.0f;
// Wait IMU_icm42670 to start
delay(100);
madgwickfilter.begin(frequency);
microsPerReading = 1000000 / frequency; //Period measured in microseconds
microsPrevious = micros();
Serial.print("\ntime[sec], acceleration[m/s2] x,y,z,");
Serial.print(" angular_velocity[deg/s] x,y,x,");
Serial.println(" pitch[deg],roll[deg],yaw[deg]");
}
void loop() {
inv_imu_sensor_event_t imu_event;
unsigned long microsNow;
static int cntr = 0;
microsNow = micros();
if (microsNow - microsPrevious >= microsPerReading) {
//Serial.println(microsNow - microsPrevious);
microsPrevious = microsPrevious + microsPerReading;
// Get last event
IMU_icm42670.getDataFromRegisters(imu_event);
float ax = convertRawAcceleration(imu_event.accel[0]); //acceleration_x [m/s2]
float ay = convertRawAcceleration(imu_event.accel[1]); //acceleration_y [m/s2]
float az = convertRawAcceleration(imu_event.accel[2]); //acceleration_z [m/s2]
float gx = convertRawGyro(imu_event.gyro[0]); //angular_velocity_x [deg/s]
float gy = convertRawGyro(imu_event.gyro[1]); //angular_velocity_y [deg/s]
float gz = convertRawGyro(imu_event.gyro[2]); //angular_velocity_z [deg/s]
madgwickfilter.updateIMU(gx, gy, gz, ax, ay, az);
// print the heading, pitch and roll
float roll = madgwickfilter.getRoll();
float pitch = madgwickfilter.getPitch();
float yaw = madgwickfilter.getYaw() - 180.0;
cntr++;
if (cntr != 100) return;
cntr = 0;
// Format data for Serial Plotter
Serial.print(microsNow/1000000);
Serial.print(", ");
Serial.print(ax);
Serial.print(", ");
Serial.print(ay);
Serial.print(", ");
Serial.print(az);
Serial.print(", ");
Serial.print(gx);
Serial.print(", ");
Serial.print(gy);
Serial.print(", ");
Serial.print(gz);
Serial.print(", ");
Serial.print(pitch);
Serial.print(", ");
Serial.print(roll);
Serial.print(", ");
Serial.println(yaw);
}
}
float convertRawAcceleration(int16_t aRaw) {
// since we are using acc_fsrange g range
// -2 g maps to a raw value of -32768
// +2 g maps to a raw value of 32767
float a = (aRaw * acc_fsrange) / 32768.0 * 9.8;
return a; // [m/s^2]
}
float convertRawGyro(int16_t gRaw) {
// since we are using gyro_fsrange degrees/seconds range
// -250 maps to a raw value of -32768
// +250 maps to a raw value of 32767
float g = (gRaw * gyro_fsrange) / 32768.0;
return g; // [deg/s]
}
ICM42670Pをほぼ水平に静止させ，得られた姿勢データは次のようになりました。
これは1秒間隔のデータです。
ICM42670 initialization started
ICM42670 initialization completed
time[sec], acceleration[m/s2] x,y,z, angular_velocity[deg/s] x,y,x, pitch[deg],roll[deg],yaw[deg]
1, -0.43, -0.18, 9.94, -0.38, 0.05, -0.61, 2.49, -1.10, -0.59
2, -0.44, -0.18, 9.94, -0.44, 0.05, -0.53, 2.55, -0.98, -1.14
3, -0.44, -0.17, 9.94, -0.46, 0.06, -0.60, 2.54, -0.99, -1.70
4, -0.43, -0.18, 9.96, -0.43, 0.08, -0.61, 2.53, -1.02, -2.26
5, -0.44, -0.18, 9.95, -0.38, 0.06, -0.52, 2.45, -1.15, -2.82
6, -0.43, -0.20, 9.95, -0.41, 0.12, -0.58, 2.49, -1.08, -3.38
7, -0.43, -0.19, 9.94, -0.40, 0.05, -0.69, 2.41, -1.13, -3.94
8, -0.44, -0.19, 9.95, -0.40, 0.09, -0.56, 2.53, -1.11, -4.50
9, -0.42, -0.19, 9.96, -0.44, -0.02, -0.52, 2.39, -1.01, -5.06
10, -0.43, -0.19, 9.95, -0.41, 0.06, -0.61, 2.43, -1.11, -5.62
11, -0.45, -0.19, 9.97, -0.46, 0.05, -0.63, 2.59, -1.09, -6.18
12, -0.43, -0.19, 9.95, -0.49, 0.15, -0.60, 2.46, -1.15, -6.75
13, -0.43, -0.19, 9.95, -0.46, 0.05, -0.58, 2.44, -1.10, -7.31
14, -0.45, -0.18, 9.94, -0.41, 0.09, -0.60, 2.55, -1.01, -7.87
15, -0.45, -0.18, 9.96, -0.41, 0.00, -0.64, 2.53, -1.13, -8.44
16, -0.44, -0.18, 9.96, -0.40, 0.06, -0.61, 2.54, -0.97, -9.00
17, -0.44, -0.19, 9.95, -0.41, 0.08, -0.60, 2.54, -1.03, -9.56
18, -0.42, -0.19, 9.97, -0.41, 0.06, -0.61, 2.40, -1.09, -10.11
19, -0.42, -0.19, 9.95, -0.46, 0.08, -0.52, 2.33, -1.11, -10.67
20, -0.43, -0.18, 9.96, -0.41, 0.06, -0.63, 2.46, -1.03, -11.23
21, -0.43, -0.18, 9.97, -0.43, 0.09, -0.56, 2.42, -1.01, -11.79
22, -0.44, -0.19, 9.95, -0.44, 0.05, -0.64, 2.52, -1.05, -12.35
23, -0.43, -0.19, 9.96, -0.38, 0.08, -0.56, 2.55, -1.06, -12.92
センサをほぼ水平の置いている状態で，Accelのz（鉛直方向の）加速度が9.9m/s2程度を示していますが，9.8m/s2であって欲しいです。
gainまたはoffsetに誤差があるようですが，これは放置します。
センサをほぼ水平の置いている状態で，pitchとrollは正しく測定出来ているかどうかは検証できませんが，0度近いので良いことにします。
yawは最初に0度ですが毎秒0.5度程度ずれていきます。
Gyro（x,y,z軸周りの角速度[deg/s]）データは，センサが静止しているため，0deg/sのはずですが，0.5deg/s程度のoffsetがあるようです。
pith,roll,yawの取得値をグラフにしてみました。
５ 6軸センサ ICM42670Pのキャリブレーション
ICM42670PのGyro（x,y,z軸周りの角速度[deg/s]）の出力はセンサ静止状態では0のはずなので，センサGyro出力に見えている値はoffset誤差と考えられます。
そこで。センサ静止状態で200個ほどGyro出力を取得し，その平均値をoffset誤差と考え，キャリブレーションしたことにして，本番の測定値でoffset誤差を引いて，測定にとすることにしました。
クラスICM42670Pにキャリブレーションとoffset誤差修正機能を持たせた，クラスICM42670Pextを使って，測定メソッドの呼び出しは変更不要としました。
また，キャリブレーション中と測定中とを区別するため，LEDの点滅速度で区別する機能も，クラスBuiltinLEDにして載せました。
クラスICM42670P，クラスBuiltinLEDに対応するファイルはzipファイルにてDLしてください。
zipファイルのダウンロード
6軸センサ ICM42670Pのキャリブレーション付のプログラム
//ICM42670P_test_withCal.ino
//library: ICM42670P by TDK/Invensense
//library: Madgwick by Arduino
#include <ICM42670Pext.h>
#include <MadgwickAHRS.h>
#include <BuiltinLED.h>
ICM42670Pext IMU_icm42670(Wire, false);
Madgwick madgwickfilter;
BuiltinLED builtinled;
float acc_fsrange;
float gyro_fsrange;
unsigned long microsPerReading, microsPrevious;
int frequency = 100; //Hz
void setup() {
int ret;
Serial.begin(115200);
//while(!Serial) {}
delay(100);
//pinMode(SDA, INPUT_PULLUP);pinMode(SCL, INPUT_PULLUP);
// These won't work. The pull-up resistor value is too high.
//After adding 4.7k pull-up resistors to the SDA and SCL pins,
// startup issues completely disappeared.
// Initializing the ICM42670
Serial.println("ICM42670 initialization started");
ret = IMU_icm42670.begin();
if (ret != 0) {
Serial.print("ICM42670 initialization failed: ");
Serial.println(ret);
while(1);
}
Serial.println("ICM42670 initialization completed");
delay(100);
// Accel ODR = 100 Hz and Full Scale Range = 2G
IMU_icm42670.startAccel(100,2);
acc_fsrange = 2.0f;
// Gyro ODR = 100 Hz and Full Scale Range = 250 dps
IMU_icm42670.startGyro(100,250);
gyro_fsrange = 250.0f;
// Wait IMU_icm42670 to start
delay(100);
//Calibration of the ICM42670 takes about 5 seconds.
//During this time, the LED will flash.
Serial.println("ICM42670 calibration started.");
Serial.println("It will take 5 seconds.");
builtinled.initialize();
builtinled.startBlinking();
IMU_icm42670.calibrateGyro();
builtinled.stopBlinking();
delay(100);
builtinled.startBlinking2();
Serial.println("ICM42670 calibration completed.");
madgwickfilter.begin(frequency);
microsPerReading = 1000000 / frequency; //Period measured in microseconds
microsPrevious = micros();
Serial.print("\ntime[sec], acceleration[m/s2] x,y,z,");
Serial.print(" angular_velocity[deg/s] x,y,x,");
Serial.println(" pitch[deg],roll[deg],yaw[deg]");
}
void loop() {
inv_imu_sensor_event_t imu_event;
unsigned long microsNow;
static int cntr = 0;
microsNow = micros();
if (microsNow - microsPrevious >= microsPerReading) {
//Serial.println(microsNow - microsPrevious);
microsPrevious = microsPrevious + microsPerReading;
// Get last event
IMU_icm42670.getDataFromRegisters(imu_event);
float ax = convertRawAcceleration(imu_event.accel[0]); //acceleration_x [m/s2]
float ay = convertRawAcceleration(imu_event.accel[1]); //acceleration_y [m/s2]
float az = convertRawAcceleration(imu_event.accel[2]); //acceleration_z [m/s2]
float gx = convertRawGyro(imu_event.gyro[0]); //angular_velocity_x [deg/s]
float gy = convertRawGyro(imu_event.gyro[1]); //angular_velocity_y [deg/s]
float gz = convertRawGyro(imu_event.gyro[2]); //angular_velocity_z [deg/s]
madgwickfilter.updateIMU(gx, gy, gz, ax, ay, az);
// print the heading, pitch and roll
float roll = madgwickfilter.getRoll();
float pitch = madgwickfilter.getPitch();
float yaw = madgwickfilter.getYaw() - 180.0;
cntr++;
if (cntr != 100) return;
cntr = 0;
// Format data for Serial Plotter
Serial.print(microsNow/1000000);
Serial.print(", ");
Serial.print(ax);
Serial.print(", ");
Serial.print(ay);
Serial.print(", ");
Serial.print(az);
Serial.print(", ");
Serial.print(gx);
Serial.print(", ");
Serial.print(gy);
Serial.print(", ");
Serial.print(gz);
Serial.print(", ");
Serial.print(pitch);
Serial.print(", ");
Serial.print(roll);
Serial.print(", ");
Serial.println(yaw);
}
}
float convertRawAcceleration(int16_t aRaw) {
// since we are using acc_fsrange g range
// -2 g maps to a raw value of -32768
// +2 g maps to a raw value of 32767
float a = (aRaw * acc_fsrange) / 32768.0 * 9.8;
return a; // [m/s^2]
}
float convertRawGyro(int16_t gRaw) {
// since we are using gyro_fsrange degrees/seconds range
// -250 maps to a raw value of -32768
// +250 maps to a raw value of 32767
float g = (gRaw * gyro_fsrange) / 32768.0;
return g; // [deg/s]
}
次のような実行結果が得られました。測定値は1秒間隔で表示しています。
ICM42670 initialization started
ICM42670 initialization completed
ICM42670 calibration started.
It will take 5 seconds.
ledc is ready.
calibrateGyro<....................>
accelc: -826 -335 18289
gyroc : -60 9 -83
ICM42670 calibration completed.
time[sec], acceleration[m/s2] x,y,z, angular_velocity[deg/s] x,y,x, pitch[deg],roll[deg],yaw[deg]
7, -0.45, -0.19, 9.95, 0.08, 0.01, 0.10, 2.60, -1.06, 0.03
8, -0.45, -0.18, 9.94, 0.02, -0.07, 0.04, 2.65, -0.96, 0.09
9, -0.44, -0.19, 9.95, 0.06, 0.02, 0.02, 2.49, -1.12, 0.14
10, -0.45, -0.18, 9.95, 0.03, 0.07, 0.07, 2.60, -1.06, 0.20
11, -0.45, -0.19, 9.95, 0.03, 0.02, 0.04, 2.65, -1.03, 0.25
12, -0.45, -0.18, 9.96, 0.05, -0.01, 0.01, 2.57, -1.03, 0.31
13, -0.45, -0.18, 9.96, 0.09, -0.01, 0.01, 2.60, -1.07, 0.37
14, -0.45, -0.18, 9.95, 0.02, -0.04, 0.05, 2.60, -1.03, 0.41
15, -0.45, -0.18, 9.96, 0.02, 0.02, 0.02, 2.52, -1.05, 0.47
16, -0.45, -0.18, 9.95, 0.02, 0.02, 0.11, 2.57, -1.02, 0.52
17, -0.43, -0.20, 9.95, 0.06, 0.01, 0.13, 2.50, -1.11, 0.57
18, -0.46, -0.19, 9.96, 0.06, -0.04, 0.13, 2.62, -1.09, 0.62
19, -0.46, -0.18, 9.96, 0.08, 0.01, 0.02, 2.59, -0.95, 0.69
20, -0.45, -0.18, 9.96, 0.11, -0.04, 0.02, 2.56, -1.06, 0.74
21, -0.45, -0.18, 9.96, 0.05, 0.04, 0.07, 2.57, -1.12, 0.79
22, -0.45, -0.18, 9.95, 0.03, -0.02, 0.07, 2.51, -1.01, 0.84
23, -0.44, -0.18, 9.95, 0.05, -0.05, 0.05, 2.51, -1.02, 0.90
24, -0.45, -0.18, 9.95, 0.03, -0.01, 0.04, 2.60, -1.02, 0.95
25, -0.45, -0.18, 9.95, 0.09, -0.01, 0.01, 2.59, -1.11, 1.00
26, -0.45, -0.17, 9.95, 0.05, 0.02, 0.10, 2.57, -1.01, 1.05
27, -0.45, -0.18, 9.96, 0.08, -0.05, 0.01, 2.59, -1.11, 1.10
28, -0.46, -0.19, 9.95, 0.09, -0.01, 0.14, 2.59, -1.07, 1.16
29, -0.46, -0.18, 9.95, -0.02, -0.02, 0.04, 2.62, -1.06, 1.20
30, -0.44, -0.18, 9.95, 0.05, -0.01, 0.10, 2.58, -1.11, 1.25
31, -0.44, -0.19, 9.97, 0.08, 0.01, 0.02, 2.46, -1.07, 1.29
32, -0.45, -0.19, 9.94, 0.06, 0.02, 0.02, 2.61, -1.10, 1.34
33, -0.46, -0.18, 9.95, 0.06, -0.04, 0.08, 2.65, -1.04, 1.39
Gyroに関するoffsetの値を修正して，yawの値に関しては30秒で１度程度の誤差となりました。
キャリブレーション前と比べるとずいぶん良くなっていると思います。
pith,roll,yawの取得値をグラフにしてみました。
６ まとめ
Xiao ESP32S3で，6軸センサICM42670Pおよびmadgwickfilterを用いて，機器の姿勢を得ました。
その際，ICM42670PのGyaroに関するoffset誤差をあらかじめ求めておくと，より安定したGyro値を得ることができることがわかりました。
ここでのプログラムはテストのため1sec周期の動作になっています。ICM42670Pは1msec間隔で動作できますので，各データに関しては
短い周期でデータ採取して時間軸平滑化フィルタを使うなどの工夫が可能と思われます。