一直以来我想通过传感器让现实世界中的物体控制电脑中的物体姿态，最近入手了DFRobot的BMX160九轴加速度传感器模块【参考1】。

所谓9轴指的是三个加速度轴，三个地测轴和三个陀螺仪轴。简单的介绍一下需要了解的物理知识：首先，如果物体发生运动，那么在三个轴方向会出现加速度，那么我们可以知道它运动的指向，此外比较特别的是因为重力影响，所以一直会存在一个加速度G；接下来因为地磁的存在（指南针的基本原理），我们可以通过地磁轴得到相对于地磁的方向；最后陀螺因为惯性，它会一直保持指向，因此陀螺仪的轴可以用来反映当前时间和上一个时间姿态区别，得到诸如角速度这种方向。

很明显，如果单纯得到上面的9个参数，我们仍然难以用其中的一组描述物体当前的状态，因此，需要经过算法处理得到当前的状态。这个过程叫做“姿态解算”，同样也叫做姿态分析、姿态估计、姿态融合。姿态解算是根据IMU(惯性测量单元)数据（陀螺仪、加速度计、地磁计等）求解出飞行器的空中姿态，所以也叫做IMU数据融合。【参考2】

因为本人能力有限，无力应对复杂的数学，因此这里通过硬件和现成的库来解决。

DFRobot的BMX160引脚定义如下：

        

丝印

      

功能描述

  
   

VCC

  

电源正极

   

GND

  

电源负极

   

SCL

  

I2C时钟线

   

SDA

  

I2C数据线

   

CSB

  

BMX160协议选择引脚

   

INIT1

  

BMX160外部中断1

   

INIT2

  

BMX160外部中断2

   

ADDR

  

I2C地址选择

需要特别注意的是 CSB 是一个 SPI 和 I2C 选择引脚，这次我们使用I2C,因此只需要链接 VCC GND  SCL 和 SDA 引脚即可。我是用的是 ESP32 FireBeetle 进行测试:

连接好之后推荐使用 DFRobot 的BMX160库先进行测试【参考4】，确保串口能看到输出。

硬件部分非常简单，接下来要进行软件方面的编写。同样，还需要了解一些关于姿态描述的基本知识。描述姿态的主要方法有两种：欧拉角和四元数。下图是欧拉角的直观描述。三个参数分别是航向角、俯仰角和横滚角。

需要注意的是对于欧拉角有两种：动态和静态欧拉角。动态欧拉角指的是轴跟着物体姿态旋转，比如，下图中物体圆转之后蓝色XYZ坐标系发生了变化，变成了红色的XYZ，如果再次旋转，那么航向角、俯仰角和横滚角都是要相对于红色XYZ的。静态欧拉角的话，就是一直使用蓝色 XYZ。从资料上看，通常我们提到的欧拉角都是动态欧拉角。但是很多三维设计软件使用的是静态欧拉角。

欧拉角非常直观容易理解。但是在使用中，这种描述方法会存在“万向节死锁(gimballock)”的问题。就是说在一些特别的角度，两个轴重合了，然后再也不会分开，继续旋转时欧拉角只有2个参数会变。B站有很多视频讲述这个问题有兴趣的朋友可以看看。

为了解决这个问题，引入了四元数的概念。四元数没有欧拉角那么直观，也非常难以理解。

这次的目标是使用 Adafruit 提供的库和网站来直观的实现旋转一只兔子。

首先安装 BMX160 和 Adafruit_Sensor库；之后，烧录下面的程序：

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Mahony_BMX160.h>
#include <Madgwick_BMX160.h>
#include <DPEng_BMX160.h>


// Create sensor instance.
DPEng_BMX160 dpEng = DPEng_BMX160(0x160A, 0x160B, 0x160C);

// Mag calibration values are calculated via ahrs_calibration example sketch results.
// These values must be determined for each baord/environment.
// See the image in this sketch folder for the values used
// below.

// Offsets applied to raw x/y/z mag values
float mag_offsets[3]            = { 9.83F, 4.42F, -6.97F };

// Soft iron error compensation matrix
float mag_softiron_matrix[3][3] = { {  0.586,  0.006,  0.001 },
                                    {  0.006,  0.601, -0.002 },
                                    {  0.001,  -0.002,  2.835 } };

float mag_field_strength        = 56.33F;

// Offsets applied to compensate for gyro zero-drift error for x/y/z
float gyro_zero_offsets[3]      = { 0.0F, 0.0F, 0.0F };

// Mahony is lighter weight as a filter and should be used
// on slower systems
Mahony_BMX160 filter;
//Madgwick_BMX160 filter;

void setup()
{
  Serial.begin(115200);

  // Wait for the Serial Monitor to open (comment out to run without Serial Monitor)
  // while(!Serial);

  Serial.println(F("DPEng AHRS Fusion Example")); Serial.println("");

  // Initialize the sensors.
  if(!dpEng.begin(BMX160_ACCELRANGE_4G, GYRO_RANGE_250DPS))
  {
    /* There was a problem detecting the BMX160 ... check your connections */
    Serial.println("Ooops, no BMX160 detected ... Check your wiring!");
    while(1);
  }
  
  filter.begin();
}

void loop(void)
{
  sensors_event_t accel_event;
  sensors_event_t gyro_event;
  sensors_event_t mag_event;

  // Get new data samples
  dpEng.getEvent(&accel_event, &gyro_event, &mag_event);

  // Apply mag offset compensation (base values in uTesla)
  float x = mag_event.magnetic.x - mag_offsets[0];
  float y = mag_event.magnetic.y - mag_offsets[1];
  float z = mag_event.magnetic.z - mag_offsets[2];

  // Apply mag soft iron error compensation
  float mx = x * mag_softiron_matrix[0][0] + y * mag_softiron_matrix[0][1] + z * mag_softiron_matrix[0][2];
  float my = x * mag_softiron_matrix[1][0] + y * mag_softiron_matrix[1][1] + z * mag_softiron_matrix[1][2];
  float mz = x * mag_softiron_matrix[2][0] + y * mag_softiron_matrix[2][1] + z * mag_softiron_matrix[2][2];

  // Apply gyro zero-rate error compensation
  float gx = gyro_event.gyro.x + gyro_zero_offsets[0];
  float gy = gyro_event.gyro.y + gyro_zero_offsets[1];
  float gz = gyro_event.gyro.z + gyro_zero_offsets[2];

  // Update the filter
  filter.update(gx, gy, gz,
                accel_event.acceleration.x, accel_event.acceleration.y, accel_event.acceleration.z,
                mx, my, mz,
                                mag_event.timestamp);

  // Print the orientation filter output
  // Note: To avoid gimbal lock you should read quaternions not Euler
  // angles, but Euler angles are used here since they are easier to
  // understand looking at the raw values. See the ble fusion sketch for
  // and example of working with quaternion data.
  float roll = filter.getRoll();
  float pitch = filter.getPitch();
  float heading = filter.getYaw();
  //Serial.print(millis());
  Serial.print("Orientation: ");
  Serial.print(heading);
  Serial.print(",");
  Serial.print(pitch);
  Serial.print(",");
  Serial.println(roll);

  delay(100);
}
复制代码

在串口管理器中看到输出表明工作正常之后就可以进行下一步。

我们需要网页读取串口数据，在 Chrome 上地址栏输入 chrome://flags，打开 Experimental Web Platformfeatures：

之后，打开 https://adafruit.github.io/Adafruit_WebSerial_3DModelViewer/页面，设置波特率为 115200 , 同时选择“EulerAngles”：

接下来选择使用的串口（记得要关闭 Arduino 的串口，否则无法打开）

然后你就可以随心所欲的旋转，屏幕上的兔子会跟着你的板卡转动。

参考：

1.     https://www.dfrobot.com.cn/goods-2947.html

2.     https://www.cnblogs.com/codeit/p/15832606.html

3.     https://wiki.dfrobot.com.cn/_SKU_SEN0373_BMX160_9-axis_sensor_module

4.     https://github.com/DFRobot/DFRobot_BMX160

5.     https://learn.adafruit.com/adafruit-bno055-absolute-orientation-sensor/webserial-visualizer