惯性人体姿态解算——动捕设备的实现 Part1

1.设计思路&硬件选择

我希望动捕设备最好能无线连接PC，所以esp32就成了首选，不用再多加一个透传模块，而且价格也便宜，另外就是IMU的选型，这个的选择项就非常多了，一些精度高的IMU价格非常夸张，所以这里就使用最常见的六轴IMU MPU6050，物美价廉，漂移和噪声问题确定很大，但是可以放在软件算法里去尽量做优化，不过调过姿态控制的都知道，六轴传感器的偏航角几乎是不可用的，这是六轴传感器工作机制导致的，只能用角速度去积分获取偏航角，然后一定程度上通过横滚角和俯仰角去矫正，理想的状态下，静态响应还是凑合的，但是动态响应几乎就是灾难，在动捕这个领域，这显然是不合适的，因此为了获得更为准确的yaw角度，我选择添加一个磁力计，HMC5883L，虽然磁力计也同样会有被环境干扰问题，比如周围的磁场，但是它的yaw角很大程度上至少是可信的，环境干扰也可以放在软件算法里面去尽量矫正。
所以最后确定的下位机方案就是使用esp32+mpu6050+HMC5883L,成本大概30块钱左右，然后关于上位机的选择，因为这次涉及到3D模型，当然是请出老朋友unity，下位机采集解算出来的欧拉角直接通过wifi与PC走UDP（延时小）通信传输数据，然后再在PC端根据欧拉角还原人体姿态，大致思路就是这个样子。

2.硬件设计

硬件方面IMU直接用的是模块，因为裸片买不起（悲），然后尺寸尽可能小巧，没有画成两面板是因为就算把尺寸再画小一点，电池容量也可能不太够了，索性定在了45mm * 45mm的尺寸上了能一面画完，能放一个800mah的锂电池，项目工程都再立创开源了，以下是链接：
立创开源广场项目链接

PCB Layout & 3D渲染图片

实物图

3.软件设计

3.1 设备软件

软件上项目源代码我会放在github仓库上，需要参考的可直接点击–>

下面主要贴一些比较关键的代码:

#include <Solution_algorithm.h>
#include <init.h>

/*计算偏移量*/
static float i;                                    //计算偏移量时的循环次数
static float ax_offset = 0, ay_offset = 0;         //x,y轴的加速度偏移量
static float gx_offset = 0, gy_offset = 0;         //x,y轴的角速度偏移量
static float yaw_offset = 0;
static float mag_offset_x = 0.0, mag_offset_y = 0.0, mag_offset_z = 0.0; //x,y,z轴的地磁偏移量

/*参数*/
static float rad2deg = 57.29578;                   //弧度到角度的换算系数
static float roll_v = 0, pitch_v = 0, yaw_v = 0;              //换算到x,y,z轴上的角速度
static float gravity = 9.8;
static float alpha = 0.5;  // 互补滤波系数
static float beta = 0.1;   // 倾斜补偿系数

/*定义微分时间*/
static float now = 0, lasttime = 0, dt = 0;        //定义微分时间

/*三个状态，先验状态，观测状态，最优估计状态*/
static float gyro_roll = 0, gyro_pitch = 0, gyro_yaw = 0;        //陀螺仪积分计算出的角度，先验状态
static float acc_roll = 0, acc_pitch = 0, acc_yaw = 0;          //加速度计观测出的角度，观测状态
static float k_roll = 0, k_pitch = 0, t_yaw = 0;              //卡尔曼滤波后估计出最优角度，最优估计状态
static float tilt_yaw = 0;  //倾斜补偿后的yaw角数据
static float yaw_buf[WINDOW_SIZE];

/*误差协方差矩阵P*/
static float e_P[2][2] ={{1,0},{0,1}};             //误差协方差矩阵，这里的e_P既是先验估计的P，也是最后更新的P

/*卡尔曼增益K*/
static float k_k[2][2] ={{0,0},{0,0}};             //这里的卡尔曼增益矩阵K是一个2X2的方阵

static float sliding_window_filter(float *buf, float new_data) 
{
    int i;
    float sum = 0.0;

    for(i = (WINDOW_SIZE - 1); i>0; i--)
    {
        buf[i - 1] = buf[i];
    }

    // 将新数据存入缓存区
    buf[WINDOW_SIZE - 1] = new_data;

    // 计算缓存区中所有数据的和
    for (i = 0; i < WINDOW_SIZE; i++)
    {
        sum += buf[i];
    }

    // 返回平均值
    return sum / WINDOW_SIZE;
}

void Caculate_RollPitch_by_Kalman()
{
    /*计算微分时间*/
    now = millis();                           //当前时间(ms)
    dt = (now - lasttime) / 1000.0;           //微分时间(s)
    lasttime = now;                           //上一次采样时间(ms)

    /*获取角速度和加速度 */
    sensors_event_t a, g, temp;
    mpu.getEvent(&a, &g, &temp);//获取加速度、角速度、温度

    /*step1:计算先验状态*/
    /*计算x,y轴上的角速度*/
    roll_v = (g.gyro.x-gx_offset) + ((sin(k_pitch)*sin(k_roll))/cos(k_pitch))*(g.gyro.y-gy_offset) + ((sin(k_pitch)*cos(k_roll))/cos(k_pitch))*g.gyro.z;//roll轴的角速度
    pitch_v = cos(k_roll)*(g.gyro.y-gy_offset) - sin(k_roll)*g.gyro.z;//pitch轴的角速度
    gyro_roll = k_roll + dt*roll_v;//先验roll角度
    gyro_pitch = k_pitch + dt*pitch_v;//先验pitch角度

    /*step2:计算先验误差协方差矩阵P*/
    e_P[0][0] = e_P[0][0] + 0.0025;//这里的Q矩阵是一个对角阵且对角元均为0.0025
    e_P[0][1] = e_P[0][1] + 0;
    e_P[1][0] = e_P[1][0] + 0;
    e_P[1][1] = e_P[1][1] + 0.0025;

    /*step3:更新卡尔曼增益K*/
    k_k[0][0] = e_P[0][0]/(e_P[0][0]+0.3);
    k_k[0][1] = 0;
    k_k[1][0] = 0;
    k_k[1][1] = e_P[1][1]/(e_P[1][1]+0.3);

    /*step4:计算最优估计状态*/
    /*观测状态*/
    //roll角度
    acc_roll = atan((a.acceleration.y - ay_offset) / (a.acceleration.z)) * rad2deg;
    //pitch角度
    acc_pitch = -1*atan((a.acceleration.x - ax_offset) / sqrt(sq(a.acceleration.y - ay_offset) + sq(a.acceleration.z))) * rad2deg;

    /*最优估计状态*/
    k_roll = gyro_roll + k_k[0][0]*(acc_roll - gyro_roll);
    k_pitch = gyro_pitch + k_k[1][1]*(acc_pitch - gyro_pitch);

    /*step5:更新协方差矩阵P*/
    e_P[0][0] = (1 - k_k[0][0])*e_P[0][0];
    e_P[0][1] = 0;
    e_P[1][0] = 0;
    e_P[1][1] = (1 - k_k[1][1])*e_P[1][1];
}

float caculate_Yaw(float roll,float pitch,bool init_flag)
{
    float rollRad = 0, pitchRad = 0;
    float xh = 0,yh = 0;
    sensors_event_t event; 
    mag.getEvent(&event);

    if(init_flag)
    {
        /*获取角速度和加速度 */
        sensors_event_t a, g, temp;
        mpu.getEvent(&a, &g, &temp);

        rollRad = asin(-(a.acceleration.x/gravity));
        pitchRad = atan(a.acceleration.y/a.acceleration.z);
    }
    else
    {
        rollRad = roll/rad2deg;
        pitchRad = pitch/rad2deg;
    }

    xh = event.magnetic.x * cos(pitchRad) + event.magnetic.y * sin(pitchRad) * sin(rollRad) + event.magnetic.z * sin(pitchRad) * cos(rollRad);
    yh = event.magnetic.y * cos(rollRad) - event.magnetic.z * sin(rollRad);

    // Calculate tilt compensated heading
    float tiltCompensatedHeading = atan2(yh, xh);

    // Correct for when signs are reversed.
    if(tiltCompensatedHeading < 0)
        tiltCompensatedHeading += 2*PI;
        
    // Check for wrap due to addition of declination.
    if(tiltCompensatedHeading > 2*PI)
        tiltCompensatedHeading -= 2*PI;
    
    // Convert radians to degrees for readability.
    float headingDegrees = tiltCompensatedHeading * 180/M_PI;

    return headingDegrees;
}

void offset_init()
{
    for (i = 1; i <= 2000; i++) {
        sensors_event_t a, g, m, temp;
        sensors_event_t event; 
        mpu.getEvent(&a, &g, &temp);//获取加速度、角速度、温度
        mag.getEvent(&m);
        ax_offset = ax_offset + a.acceleration.x;
        ay_offset = ay_offset + a.acceleration.y;
        gx_offset = gx_offset + g.gyro.x;
        gy_offset = gy_offset + g.gyro.y;

        mag_offset_x += m.magnetic.x;
        mag_offset_y += m.magnetic.y;
        mag_offset_z += m.magnetic.z;
    }
    ax_offset = ax_offset / 2000;
    ay_offset = ay_offset / 2000;
    gx_offset = gx_offset / 2000;
    gy_offset = gy_offset / 2000;

    mag_offset_x /= 2000;
    mag_offset_y /= 2000;
    mag_offset_z /= 2000;

    for(i = 0; i < 100; i++)
    {
        yaw_offset += caculate_Yaw(k_roll, k_pitch, true);
    }

    yaw_offset /= 100;
}

void get_Euler_angle(float* roll, float* pitch, float* yaw)
{
    Caculate_RollPitch_by_Kalman();
    tilt_yaw = caculate_Yaw(k_roll, k_pitch, false);
    tilt_yaw = tilt_yaw >= yaw_offset ? (tilt_yaw - yaw_offset) : ((360.0 - yaw_offset) + tilt_yaw);

    if(tilt_yaw > 180.0)
    {
        tilt_yaw -= 360.0;
    }
    t_yaw = sliding_window_filter(yaw_buf, tilt_yaw);

    *roll = k_roll;
    *pitch = k_pitch;
    *yaw = t_yaw;
}

由于IMU和磁力计采集到的数据都是有很大噪声的，而且消费级的IMU的漂移问题很严重，所以他们的数据直接拿来其实是不能用的，上面那段代码其实是一个卡尔曼滤波的处理，也算是姿态解算中很经典的处理算法了，具体的原理就不在这里展开了，关于卡尔曼滤波的教程和讲解网上有太多太多，我就不在这里班门弄斧了，卡尔曼滤波主要针对IMU，可以得到一个可用的Roll角和Pitch角，而Yaw角则需要用磁力计来解决，磁力计在外部坏境相对稳定的情况下，指北的表现还是比较好的，所以只对其数据进行简单的均值滤波其实在平面上就已可用，其问题在于倾斜情况下需要对磁偏角进行倾斜校正，不然会得到一个完全错误的数值，原因跟磁力计的工作方式有关，在此不多赘述，感兴趣可以看这篇文章了解其原理基于加速度计与磁力计的姿态解算方法(加计补偿偏航)。最后将获得的Roll、Pitch、Yaw角传出，后面的工作就是组报文 + UDP通信了，将数据传给PC上位机后，再由PC去根据欧拉角还原物体姿态。

3.2 上位机软件

如上面所说，上位机我用的是Unity，这里其实没有太多可说的，上位机全部的工作就是建立一个UDP的服务器，接收来自设备的包含欧拉角信息的消息并对其进行解析，然后使用角度信息对场景物体进行对应旋转，真要说的话，本文只是Part1，所以只做了一个Cube去做测试，具体人体姿态的解算和还原会放在Part2中去详细说明。上位机的Unity工程我同样会放在上文（3.1）提到的Github链接中。（Part2已更新–>惯性人体姿态解算——动捕上位机的实现 Part2）

4.效果演示

这里放一张动图用来展示实际的效果：

5.总结

这篇主要是实现一个基础验证，显示完成数据采集的工作，具体根据采集的数据解算出来的欧拉角去还原姿态的部分我打算放在上位机去做，这个就要等到Part2再说了。