【SPRESENSE】BMI160用のセンサーフュージョン・フィルターを検討する(2) [ロボット]
前回、SPRESENSEでセンサーフュージョン用のフィルターを検討しましたが、グラフでは効果がよく分からないので、今回は少し工夫をこらして Processing で表示しながら反応を見ることにしました。
Processingで作ったプログラムのグラフィック画面を次のようにしてみました。
Spresense のスケッチは次のようなものです。前回のものと出力形式以外は全く変わっていません。
Processing のスケッチは次のようになっています。
百聞は一見にしかず、ぜひ動画で見てみてください。どのフィルタがどのような性質を持っているかグラフィックで見てみるとひと目で分かります! 😀
(^^)/~
Processingで作ったプログラムのグラフィック画面を次のようにしてみました。
Spresense のスケッチは次のようなものです。前回のものと出力形式以外は全く変わっていません。
#include <Wire.h> #include <math.h> #include <BMI160Gen.h> #include <Kalman.h> #include <MadgwickAHRS.h> #define PRINT_PROCESSING #define BAUDRATE 115200 #define SENSE_RATE 100 #define GYRO_RANGE 250 #define ACCL_RANGE 2 #define deg_to_rad(a) (a/180*M_PI) #define rad_to_deg(a) (a/M_PI*180) Kalman kalmanRoll; Kalman kalmanPitch; Madgwick madgwick; float convertRawGyro(int gRaw) { // ex) if the range is +/-500 deg/s: +/-32768/500 = +/-65.536 LSB/(deg/s) float lsb_omega = float(0x7FFF) / GYRO_RANGE; return gRaw / lsb_omega; // deg/sec } float convertRawAccel(int aRaw) { // ex) if the range is +/-2g ; +/-32768/2 = +/-16384 LSB/g float lsb_g = float(0x7FFF) / ACCL_RANGE; return aRaw / lsb_g; } static float gyro_roll = 0, gyro_pitch = 0, gyro_yaw = 0; static float comp_roll = 0, comp_pitch = 0; static unsigned long last_mills = 0; void print_roll_pitch() { // read raw accl measurements from device int rawXAcc, rawYAcc, rawZAcc; // x, y, z BMI160.readAccelerometer(rawXAcc, rawYAcc, rawZAcc); float accX = convertRawAccel(rawXAcc); float accY = convertRawAccel(rawYAcc); float accZ = convertRawAccel(rawZAcc); float rad_a_roll = atan2(accY, accZ); float rad_a_pitch = atan2(-accX, sqrt(accY*accY + accZ*accZ)); float accl_roll = rad_to_deg(rad_a_roll); float accl_pitch = rad_to_deg(rad_a_pitch); // read raw gyro measurements from device int rawRoll, rawPitch, rawYaw; // roll, pitch, yaw BMI160.readGyro(rawRoll, rawPitch, rawYaw); float omega_roll = convertRawGyro(rawRoll); float omega_pitch = convertRawGyro(rawPitch); float omega_yaw = convertRawGyro(rawYaw); unsigned long cur_mills = micros(); unsigned long duration = cur_mills - last_mills; last_mills = cur_mills; double dt = duration / 1000000.0; // us->s if (dt > 0.1) return; // Gyro data gyro_roll += omega_roll * dt; // (ms->s) omega x time = degree gyro_pitch += omega_pitch * dt; gyro_yaw += omega_yaw * dt; // Complementary filter data comp_roll = 0.93 * (comp_roll + omega_roll * dt) + 0.07 * accl_roll; comp_pitch = 0.93 * (comp_pitch + omega_pitch * dt) + 0.07 * accl_pitch; // Kalman filter data float kalm_roll = kalmanRoll.getAngle(accl_roll, omega_roll, dt); float kalm_pitch = kalmanPitch.getAngle(accl_pitch, omega_pitch, dt); // Madgwick filter data madgwick.updateIMU2(omega_roll, omega_pitch, omega_yaw, accX, accY, accZ, dt); // madgwick.updateIMU(omega_roll, omega_pitch, omega_yaw, accX, accY, accZ); float madw_roll = madgwick.getRoll(); float madw_pitch = madgwick.getPitch(); float madw_yaw = madgwick.getYaw(); #ifdef PRINT_PROCESSING static int n = 0; if (n != 50) { ++n; return; } n = 0; Serial.print(accl_roll); Serial.print(","); Serial.print(accl_pitch); Serial.print(","); Serial.print(madw_yaw); Serial.print(","); Serial.print(gyro_roll); Serial.print(","); Serial.print(gyro_pitch); Serial.print(","); Serial.print(madw_yaw); Serial.print(","); Serial.print(comp_roll); Serial.print(","); Serial.print(comp_pitch); Serial.print(","); Serial.print(madw_yaw); Serial.print(","); Serial.print(kalm_roll); Serial.print(","); Serial.print(kalm_pitch); Serial.print(","); Serial.print(madw_yaw); Serial.print(","); Serial.print(madw_roll); Serial.print(","); Serial.print(madw_pitch); Serial.print(","); Serial.print(madw_yaw); Serial.print(","); #endif Serial.println(); } void setup() { Serial.begin(115200); BMI160.begin(BMI160GenClass::I2C_MODE); madgwick.begin(SENSE_RATE); BMI160.setGyroRate(SENSE_RATE); BMI160.setAccelerometerRate(SENSE_RATE); BMI160.setGyroRange(GYRO_RANGE); BMI160.setAccelerometerRange(ACCL_RANGE); BMI160.autoCalibrateGyroOffset(); BMI160.autoCalibrateAccelerometerOffset(X_AXIS, 0); BMI160.autoCalibrateAccelerometerOffset(Y_AXIS, 0); BMI160.autoCalibrateAccelerometerOffset(Z_AXIS, 1); delay(100); /* Set kalman and gyro starting angle */ int rawXAcc, rawYAcc, rawZAcc; BMI160.readAccelerometer(rawXAcc, rawYAcc, rawZAcc); float accX = convertRawAccel(rawXAcc); float accY = convertRawAccel(rawYAcc); float accZ = convertRawAccel(rawZAcc); float roll = rad_to_deg(atan(accY / accZ)); float pitch = rad_to_deg(atan(-accX / sqrt(accY * accY + accZ * accZ))); // Set starting angle kalmanRoll.setAngle(roll); kalmanPitch.setAngle(pitch); gyro_roll = comp_roll = roll; gyro_pitch = gyro_pitch = pitch; } void loop() { print_roll_pitch(); }
Processing のスケッチは次のようになっています。
import processing.serial.*; Serial myPort; // Create object from Serial class float [] Acc = new float[3]; float [] Gyr = new float[3]; float [] Cmp = new float[3]; float [] Klm = new float[3]; float [] Mdw = new float[3]; int lf = 10; // 10 is '\n' in ASCII byte[] inBuffer = new byte[1024]; PFont font; final int VIEW_SIZE_X = 1100, VIEW_SIZE_Y = 600; void settings() { size(VIEW_SIZE_X, VIEW_SIZE_Y, P3D); } void setup() { myPort = new Serial(this, "COM15", 115200); font = loadFont("CourierNew36.vlw"); } void readSensors() { if(myPort.available() > 0) { if (myPort.readBytesUntil(lf, inBuffer) > 0) { String inputString = new String(inBuffer); String [] inputStringArr = split(inputString, ','); Acc[0] = float(inputStringArr[0]); Acc[1] = float(inputStringArr[1]); Acc[2] = float(inputStringArr[2]); Gyr[0] = float(inputStringArr[3]); Gyr[1] = float(inputStringArr[4]); Gyr[2] = float(inputStringArr[5]); Cmp[0] = float(inputStringArr[6]); Cmp[1] = float(inputStringArr[7]); Cmp[2] = float(inputStringArr[8]); Klm[0] = float(inputStringArr[9]); Klm[1] = float(inputStringArr[10]); Klm[2] = float(inputStringArr[11]); Mdw[0] = float(inputStringArr[12]); Mdw[1] = float(inputStringArr[13]); Mdw[2] = float(inputStringArr[14]); } } } void normalize3DVec(float [] vector) { float R; R = sqrt(vector[0]*vector[0] + vector[1]*vector[1] + vector[2]*vector[2]); vector[0] /= R; vector[1] /= R; vector[2] /= R; } void buildBoxShape() { //box(60, 10, 40); noStroke(); beginShape(QUADS); //Z+ (to the drawing area) fill(#00ff00); vertex(-30, -5, 20); vertex(30, -5, 20); vertex(30, 5, 20); vertex(-30, 5, 20); //Z- fill(#0000ff); vertex(-30, -5, -20); vertex(30, -5, -20); vertex(30, 5, -20); vertex(-30, 5, -20); //X- fill(#ff0000); vertex(-30, -5, -20); vertex(-30, -5, 20); vertex(-30, 5, 20); vertex(-30, 5, -20); //X+ fill(#ffff00); vertex(30, -5, -20); vertex(30, -5, 20); vertex(30, 5, 20); vertex(30, 5, -20); //Y- fill(#ff00ff); vertex(-30, -5, -20); vertex(30, -5, -20); vertex(30, -5, 20); vertex(-30, -5, 20); //Y+ fill(#00ffff); vertex(-30, 5, -20); vertex(30, 5, -20); vertex(30, 5, 20); vertex(-30, 5, 20); endShape(); } void drawCube(int n, float vec[]) { pushMatrix(); translate(n, 450, 0); scale(3,3,3); rotateX(HALF_PI * vec[0]); rotateZ(HALF_PI * -vec[1]); buildBoxShape(); popMatrix(); } void draw() { readSensors(); normalize3DVec(Acc); normalize3DVec(Gyr); normalize3DVec(Cmp); normalize3DVec(Klm); normalize3DVec(Mdw); background(#000000); fill(#ffffff); textFont(font, 24); text("Sensor Fusion Test using Spresense with BMI160", 180, 100); textFont(font, 18); text("Accelerometer :\n" + Acc[0] + "\n" + Acc[1], 50, 180); drawCube(150, Acc); text("Gyroscope :\n" + Gyr[0] + "\n" + Gyr[1], 250, 180); drawCube(350, Gyr); text("Comp. Filter :\n" + Cmp[0] + "\n" + Cmp[1], 450, 180); drawCube(550, Cmp); text("Kalman Filter :\n" + Klm[0] + "\n" + Klm[1], 650, 180); drawCube(750, Klm); text("Madgwick Filter :\n" + Mdw[0] + "\n" + Mdw[1], 850, 180); drawCube(950, Mdw); }
百聞は一見にしかず、ぜひ動画で見てみてください。どのフィルタがどのような性質を持っているかグラフィックで見てみるとひと目で分かります! 😀
(^^)/~
SONY SPRESENSE メインボード CXD5602PWBMAIN1
- 出版社/メーカー: スプレッセンス(Spresense)
- メディア: Tools & Hardware
SODIAL 6軸レートジャイロ BMI160 6自由度重力加速度センサー モジュールIIC SPIブロード
- 出版社/メーカー: SODIAL
- メディア: その他