这是我之前做过的两轮自平衡车最终版控制程序，你参考一下吧~

使用四元数算法进行计算，下面有代码看一下

使用mpu6050读取的参数，代入到下面的函数，就可以了。

void mix_gyrAcc_crossMethod(quaternion * attitude,const float gyr,const float acc,float interval)

{

    const static float FACTOR = 0.001;//取接近0的数

    //

    float w_q = attitude->w;

    float x_q = attitude->x;

    float y_q = attitude->y;

    float z_q = attitude->z;

    float x_q_2 = x_q * 2;

    float y_q_2 = y_q * 2;

    float z_q_2 = z_q * 2;

    //

    // 加速度计的读数，单位化。

    float a_rsqrt = math_rsqrt(acc*acc+acc*acc+acc*acc);

    float x_aa = acc * a_rsqrt;

    float y_aa = acc * a_rsqrt;

    float z_aa = acc * a_rsqrt;   //加速度计测量出的加速度向量(载体坐标系下)

    //

    // 载体坐标下的重力加速度向量，单位化。

    float x_ac = x_q*z_q_2 - w_q*y_q_2;

    float y_ac = y_q*z_q_2 + w_q*x_q_2; //通过四元数旋转矩阵与地理坐标系下的重力加速度向量叉乘得到载体坐标系下的重力加速度向量

    float z_ac = 1 - x_q*x_q_2 - y_q*y_q_2;//（主要）角速度计测出的四元数表示的载体坐标系下的重力加速度向量（这里已转换成载体坐标系下）

    //

    // 测量值与常量的叉积。

    float x_ca = y_aa * z_ac - z_aa * y_ac;

    float y_ca = z_aa * x_ac - x_aa * z_ac;

    float z_ca = x_aa * y_ac - y_aa * x_ac;//角速度计测出的角度误差，叠加的FACTOR大小可以实验试凑

    //

    // 构造增量旋转。

    float delta_x = gyr * interval / 2 + x_ca * FACTOR;

    float delta_y = gyr * interval / 2 + y_ca * FACTOR;

    float delta_z = gyr * interval / 2 + z_ca * FACTOR;

    //

    // 融合，四元数乘法。

    attitude->w = w_q         - x_q*delta_x - y_q*delta_y - z_q*delta_z;

    attitude->x = w_q*delta_x + x_q         + y_q*delta_z - z_q*delta_y;

    attitude->y = w_q*delta_y - x_q*delta_z + y_q         + z_q*delta_x;

    attitude->z = w_q*delta_z + x_q*delta_y - y_q*delta_x + z_q;

    quaternion_normalize(attitude);//归一化

}

谢谢楼主啊，这些资料真棒

谢了