精华内容
下载资源
问答
  • 九轴姿态解算,四元数互补滤波法,磁力计数据单独,测试四轴可以起飞
  • 姿态解算的数据源可来自于陀螺仪或加速度计,他们分别都可以得到当前姿态,但实际上他们都有各自的传感器性能的优缺点,需要取长补短,进行数据融合才能得到真实的姿态,这种方法简称 互补滤波 算法 传感器特性 ...

    飞控算法-姿态解算之互补滤波

    姿态解算是通过读取各个传感器的数据进行融合最终得到当前飞行器的姿态,姿态通常用Euler角(Roll-Pitch-Yaw)来表示

    在这里插入图片描述

    姿态解算框架

    姿态解算的数据源可来自于陀螺仪或加速度计,他们分别都可以得到当前姿态,但实际上他们都有各自的传感器性能的优缺点,需要取长补短,进行数据融合才能得到真实的姿态,这种方法简称 互补滤波 算法

    传感器特性

    在这里插入图片描述

    互补滤波

    在这里插入图片描述

    姿态解算原理

    假设我们现在没有加速度计,只有陀螺仪,那么我们该如何求解姿态呢?

    陀螺仪

    1、陀螺仪传感器输出的是角速度,单位为x°/s,我们取ADI的一款高精度IMU来模拟
    在这里插入图片描述
    这款IMU默认输出为16位的LSB数据,最终得到的角速度为:LSB*Scaler
    假如:
    X_GYRO_OUT = 22887
    Scaler = 0.013108
    那么:
    g_X = X_GYRO_OUT * Scaler = +300°/sec
    也就是说在1秒内旋转了300度,正负根据设备定义的方向,右手定则来决定正负

    2、现在得到了角速度,我们可以直接对角速度对时间积分得到当前的角度,这是最简单,最直接的想法,但是通过积分后得到的角度漂移会吓死你,你可以把设备静止在桌面上,观察积分后的角度随直接变化,下面是我直接用角速度积分得到的姿态,用python来绘制的曲线:

    代码先送上来:

    /*
    * 陀螺仪积分求角度
    * param1: 原始3轴角速度,单位为x°/s
    * param2: 间隔时间,单位为ms
    */
    void gyro_int(Vector3f &g, float dt)
    {
        static Vector3f att(0,0,0);
        att.x += g.x * dt * 0.001;
        att.y += g.y * dt * 0.001;
        att.z += g.z * dt * 0.001;
        printf("att is %.3f %.3f %.3f\n", att.x, att.y, att.z);
    }
    
    // 主循环代码
    void stablize_loop()
    {
        static uint32_t last_time = 0;
        float dt = 0;
        if(last_time == 0)
            dt = 0;
        else
            dt = AP_HAL::millis() - last_time;
        last_time = AP_HAL::millis();
        
        gyro_int(gyro_data, dt);
    }
    

    目前我们只分析X轴的数据:

    3、表示姿态有很多种方式,主流的有欧拉角,旋转矩阵,四元数。但都有各自的优缺点,欧拉角最直接但是存在万象锁,旋转矩阵太麻烦耗费资源多,四元数是目前主流的姿态求解的方法。关于四元数的定义和基本概念我这里不做解释了,有兴趣可以自行推导

    • 如何得到四元数

    一阶龙格库塔法

    公式:
    在这里插入图片描述

    • q0(t+dt) = q0(t) + 1/2dt * (-Wxq1 - Wyq2 - Wz*q3)
    • q1(t+dt) = q1(t) + 1/2dt * ( Wxq0 - Wyq3 + Wz*q2)
    • q2(t+dt) = q2(t) + 1/2dt * ( Wxq3 + Wyq0 - Wz*q1)
    • q3(t+dt) = q3(t) + 1/2dt * (-Wxq2 + Wyq1 + Wz*q0)

    根据公式,我们需要W(x,y,z),这不就是角速率吗?我们可以直接从陀螺仪里读取到即可
    注意:W(x,y,z)为角速率,陀螺仪数据为角速度,需要把度转成弧度(Degree_2_Radian)

    上代码:

    /*
    * 一阶龙格库塔法求四元数
    * 四元数初始值: q0 = 1, q1 = q2 = q3 = 0
    * Wx Wy Wz为角速率单位为弧度
    * Wx = gx*(PI/180) WY = gy*(PI/180) Wz = gz*(PI/180)
    * q0(t+dt) = q0(t) + 1/2dt * (-Wx*q1 - Wy*q2 - Wz*q3)
    * q1(t+dt) = q1(t) + 1/2dt * ( Wx*q0 - Wy*q3 + Wz*q2)
    * q2(t+dt) = q2(t) + 1/2dt * ( Wx*q3 + Wy*q0 - Wz*q1)
    * q3(t+dt) = q3(t) + 1/2dt * (-Wx*q2 + Wy*q1 + Wz*q0)
    */
    void gyro_2_quaternion(Vector3f &gyro, float dt)
    {
        float half_dt = 0.5*dt*0.001; // 单位为秒
    
        float q0_t = 0, q1_t = 0, q2_t = 0, q3_t = 0;
    
        q0_t = half_dt * (-gyro.x * q1 - gyro.y*q2 - gyro.z*q3);
        q1_t = half_dt * ( gyro.x * q0 - gyro.y*q3 + gyro.z*q2);
        q2_t = half_dt * ( gyro.x * q3 + gyro.y*q0 - gyro.z*q1);
        q3_t = half_dt * (-gyro.x * q2 + gyro.y*q1 + gyro.z*q0);
    
        q0 += q0_t;
        q1 += q1_t;
        q2 += q2_t;
        q3 += q3_t;
    
        float normal = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
        q0 /= normal;
        q1 /= normal;
        q2 /= normal;
        q3 /= normal;
        //printf("Quaternion is %.3f %.3f %.3f %.3f\n", q0, q1, q2, q3);
    }
    
    • 如何得到欧拉角

    旋转矩阵

    前面说到,可以通过旋转矩阵来表示姿态,只不过计算量太大,而且存在奇异点,所以没用,但是他在四元数求欧拉角确实很有用的。

    首先上旋转矩阵:
    在这里插入图片描述
    旋转方式有很多种,不同的旋转方式得到的矩阵也不一样,要注意!
    这里采用东北天坐标系下的Z-X-Y旋转

    接着上四元数下的旋转矩阵:

    罗德里格旋转

    在这里插入图片描述
    接着根据矩阵相等,就可以反解出欧拉角
    在这里插入图片描述

    送上代码:

    /*
    * 四元数转欧拉角
    * 
    * 
    * 
    */
    Vector3f quaternion_2_euler_angle(float q0, float q1, float q2, float q3)
    {
    #if 0
        // NED北东地坐标系下的Z-Y-X旋转
        float roll = atan2f(2.0f*(q0*q1 + q2*q3), (q0*q0 - q1*q1 - q2*q2 + q3*q3));
        float pitch = safe_asin(2.0f*(q0*q2 - q1*q3));
        float yaw = atan2f(2.0f*(q0*q3 + q1*q2) , (q0*q0 + q1*q1 - q2*q2 - q3*q3));
    #else
        // ENU东北天坐标系下的Z-X-Y旋转
        float roll = -atan2f(2.0f*(q1*q3 - q0*q2), (q0*q0 - q1*q1 - q2*q2 + q3*q3));
        float pitch = safe_asin(2.0f*(q0*q1 + q2*q3));
        float yaw = atan2f(2.0f*(q1*q2 - q0*q3), (q0*q0 - q1*q1 + q2*q2 - q3*q3));
    #endif
        Vector3f euler_angle = {roll*RAD_TO_DEG, pitch*RAD_TO_DEG, yaw*RAD_TO_DEG};
        return euler_angle;
    }
    

    4、问题来了,为啥要用这么复杂的方式转来转去,目的就为了一个,更新四元数,来实现互补滤波


    现在我们可以通过四元数来求欧拉角了,但是还是存在一个积分漂移的情况,接下来我们就该我们的加速度计登场了

    加速度计

    我们可以通过加速度计得到3轴加速度把他和重力加速度求反余弦,就可以得到角度,但是这不是我们需要的!我们需要加速度计是为了获取他的静态性能来更新四元数,补偿陀螺仪

    1、得到实际加速度a)
    我们从加速度计读取得到的数据是实际的加速度值g,这里注意从传感器里读出的原始加速度值mg,这是一个力的单位,我们一般要把他转换为加速度单位m/s/s,只需要将mg * 9.80665f * 0.001,记得数据要归一化处理哦

        //归一化
        acc_data.normalize();
    

    2、得到理论加速度a’
    在这里插入图片描述

    我们的四元数又该出场了!

    /*
    * 四元数得出理论三轴加速度
    * ax = 2(q1*q3 - q0*q2)
    * ax = 2(q0*q1 + q2*q3)
    * az = q0*q0 - q1*q1 - q2*q2 + q3*q3
    */
    void quaternion_2_acc(Vector3f &acc, float q0, float q1, float q2, float q3)
    {
        acc.x = 2*(q1*q3 - q0*q2);
        acc.y = 2*(q0*q1 + q2*q3);
        acc.z = q0*q0 - q1*q1 - q2*q2 + q3*q3;
    }
    
    

    3、得到误差a(e)
    向量叉乘
    a(e) = a X a’ = |a| * |a’| * sinr = sinr
    小角近似
    a(e) = r
    得到的误差角为r

    上代码:

    /*
    * 向量叉乘得到误差e
    * ex = ay * vz - az * vy
    * ey = az * vx - ax * vz
    * ez = ax * vy - ay * vx
    */
    void vector_cross(Vector3f &a, Vector3f &v, Vector3f &e)
    {
        e.x = a.y * v.z - a.z * v.y;
        e.y = a.z * v.x - a.x * v.z;
        e.z = a.x * v.y - a.y * v.x;
    }
    

    4、误差得到了,现在该上我们的PI补偿控制器了
    这个控制器的作用,就是为了把计算出来的误差补偿到陀螺仪上,使之快速收敛到真实的数据上去,最后得到一个补偿后的陀螺仪数据
    上代码:

    /*
    * 互补滤波算法
    * PI补偿控制器u=kp*error+ ki*∫error
    *
    */
    void complementary_filter(Vector3f &_g, Vector3f &g, Vector3f &e)
    {
        float Kp = 0.8f;
        float Ki = 0.001f;
    
        //比例项
        Vector3f g_P(e.x * Kp, e.y * Kp, e.z * Kp);
    
        //积分项
        static Vector3f int_e(0, 0, 0);
        Vector3f g_I;
    
        g_I.x = int_e.x + e.x * Ki;
        g_I.y = int_e.y + e.y * Ki;
        g_I.z = int_e.z + e.z * Ki;
        
        _g = g + g_P + g_I;
    }
    

    5、现在陀螺仪的数据是补偿后的了,我们可以通过转四元数再反解欧拉角得到补偿后的姿态

    全部代码如下:

    static float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
    
    /*
    * 四元数转欧拉角
    * 
    * 
    * 
    */
    Vector3f quaternion_2_euler_angle(float q0, float q1, float q2, float q3)
    {
    #if 1
        // NED北东地坐标系下的Z-Y-X旋转
        float roll = atan2f(2.0f*(q0*q1 + q2*q3), (q0*q0 - q1*q1 - q2*q2 + q3*q3));
        float pitch = safe_asin(2.0f*(q0*q2 - q1*q3));
        float yaw = atan2f(2.0f*(q0*q3 + q1*q2) , (q0*q0 + q1*q1 - q2*q2 - q3*q3));
    #else
        // ENU东北天坐标系下的Z-X-Y旋转
        float roll = -atan2f(2.0f*(q1*q3 - q0*q2), (q0*q0 - q1*q1 - q2*q2 + q3*q3));
        float pitch = safe_asin(2.0f*(q0*q1 + q2*q3));
        float yaw = atan2f(2.0f*(q1*q2 - q0*q3), (q0*q0 - q1*q1 + q2*q2 - q3*q3));
    #endif
        Vector3f euler_angle = {roll*RAD_TO_DEG, pitch*RAD_TO_DEG, yaw*RAD_TO_DEG};
        return euler_angle;
    }
    
    /*
    * 一阶龙格库塔法求四元数
    * 四元数初始值: q0 = 1, q1 = q2 = q3 = 0
    * Wx Wy Wz为角速率单位为弧度
    * Wx = gx*(PI/180) WY = gy*(PI/180) Wz = gz*(PI/180)
    * q0(t+dt) = q0(t) + 1/2dt * (-Wx*q1 - Wy*q2 - Wz*q3)
    * q1(t+dt) = q1(t) + 1/2dt * ( Wx*q0 - Wy*q3 + Wz*q2)
    * q2(t+dt) = q2(t) + 1/2dt * ( Wx*q3 + Wy*q0 - Wz*q1)
    * q3(t+dt) = q3(t) + 1/2dt * (-Wx*q2 + Wy*q1 + Wz*q0)
    */
    void gyro_2_quaternion(Vector3f &gyro, float dt)
    {
        float half_dt = 0.5*dt*0.001; // 单位为秒
    
        float q0_t = 0, q1_t = 0, q2_t = 0, q3_t = 0;
    
        q0_t = half_dt * (-gyro.x * q1 - gyro.y*q2 - gyro.z*q3);
        q1_t = half_dt * ( gyro.x * q0 - gyro.y*q3 + gyro.z*q2);
        q2_t = half_dt * ( gyro.x * q3 + gyro.y*q0 - gyro.z*q1);
        q3_t = half_dt * (-gyro.x * q2 + gyro.y*q1 + gyro.z*q0);
    
        q0 += q0_t;
        q1 += q1_t;
        q2 += q2_t;
        q3 += q3_t;
    
        float normal = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
        q0 /= normal;
        q1 /= normal;
        q2 /= normal;
        q3 /= normal;
        //printf("Quaternion is %.3f %.3f %.3f %.3f\n", q0, q1, q2, q3);
    }
    
    /*
    * 四元数得出理论三轴加速度
    * ax = 2(q1*q3 - q0*q2)
    * ax = 2(q0*q1 + q2*q3)
    * az = q0*q0 - q1*q1 - q2*q2 + q3*q3
    */
    void quaternion_2_acc(Vector3f &acc, float q0, float q1, float q2, float q3)
    {
        acc.x = 2*(q1*q3 - q0*q2);
        acc.y = 2*(q0*q1 + q2*q3);
        acc.z = q0*q0 - q1*q1 - q2*q2 + q3*q3;
    }
    
    
    /*
    * 向量叉乘得到误差e
    * ex = ay * vz - az * vy
    * ey = az * vx - ax * vz
    * ez = ax * vy - ay * vx
    */
    void vector_cross(Vector3f &a, Vector3f &v, Vector3f &e)
    {
        e.x = a.y * v.z - a.z * v.y;
        e.y = a.z * v.x - a.x * v.z;
        e.z = a.x * v.y - a.y * v.x;
    }
    
    /*
    * 互补滤波算法
    * PI补偿控制器u=kp*error+ ki*∫error
    *
    */
    void complementary_filter(Vector3f &_g, Vector3f &g, Vector3f &e)
    {
        float Kp = 0.8f;
        float Ki = 0.001f;
    
        //比例项
        Vector3f g_P(e.x * Kp, e.y * Kp, e.z * Kp);
    
        //积分项
        static Vector3f int_e(0, 0, 0);
        Vector3f g_I;
    
        g_I.x = int_e.x + e.x * Ki;
        g_I.y = int_e.y + e.y * Ki;
        g_I.z = int_e.z + e.z * Ki;
        
        _g = g + g_P + g_I;
    }
    
    void get_gyro(Vector3f &g)
    {
        uint8_t i, sign;
        uint16_t gyro[3] = {0};
        float gryo_con[3];
    
        uint8_t reg[2] = {0};
        uint8_t res[2] = {0};
        uint8_t pTxFinal[2] = {0x0, 0x83};
        reg[0] = ADIS16XXX_REG_X_GYRO_OUT;
        _dev->transfer_fullduplex(reg, res, 2);
        gyro[0] = res[0]<<8|res[1];
    
        reg[0] = ADIS16XXX_REG_Y_GYRO_OUT;
        _dev->transfer_fullduplex(reg, res, 2);
        gyro[0] = res[0]<<8|res[1];
    
        reg[0] = ADIS16XXX_REG_Z_GYRO_OUT;
        _dev->transfer_fullduplex(reg, res, 2);
        gyro[1] = res[0]<<8|res[1];
    
        _dev->transfer_fullduplex(pTxFinal, res, 2);
        gyro[2] = res[0]<<8|res[1];
    
        for(i = 0; i < 3; i++) {
            sign = gyro[i]&0x8000;
            if(sign)
                gryo_con[i] = (-(~(short )gyro[i]+1)) * _adis_chip_info._gyro_scale * DEG_TO_RAD;
            else
                gryo_con[i] = ((short )gyro[i]) * _adis_chip_info._gyro_scale * DEG_TO_RAD;
    
            //printf("gryo_con[%d] [%d].\n", i, gryo_con[i]);
        }
    
        g.x = gryo_con[0];
        g.y = gryo_con[1];
        g.z = gryo_con[2];
    }
    
    void get_acc(Vector3f &a)
    {
        uint8_t i, sign;
        uint16_t accel[3] = {0};
        float accel_con[3];
    
        uint8_t reg[2] = {0};
        uint8_t res[2] = {0};
        uint8_t pTxFinal[2] = {0x0, 0x83};
        reg[0] = ADIS16XXX_REG_X_ACCEL_OUT;
        _dev->transfer_fullduplex(reg, res, 2);
        accel[0] = res[0]<<8|res[1];
    
        reg[0] = ADIS16XXX_REG_Y_ACCEL_OUT;
        _dev->transfer_fullduplex(reg, res, 2);
        accel[0] = res[0]<<8|res[1];
    
        reg[0] = ADIS16XXX_REG_Z_ACCEL_OUT;
        _dev->transfer_fullduplex(reg, res, 2);
        accel[1] = res[0]<<8|res[1];
    
        _dev->transfer_fullduplex(pTxFinal, res, 2);
        accel[2] = res[0]<<8|res[1];
    
        for(i = 0; i < 3; i++) {
            sign = accel[i]&0x8000;
            if(sign) {
                accel_con[i] = (-(~(short )accel[i]+1)) * _adis_chip_info._accel_scale *GRAVITY_MSS/1000.0f;
            } else {
                accel_con[i] = ((short )accel[i]) * _adis_chip_info._accel_scale *GRAVITY_MSS/1000.0f;
            }
            //printf("accel_con[%d] [%f].\n", i, accel_con[i]);
        }
    
        a.x = accel_con[0];
        a.y = accel_con[1];
        a.z = accel_con[2];
    }
    
    /*
    * 陀螺仪积分求角度
    * 
    */
    void gyro_int(Vector3f &g, float dt)
    {
        static Vector3f att(0,0,0);
        att.x += g.x * dt * 0.001;
        printf("roll is %.3f\n", att.x);
    }
    
    /*
    * 姿态矩阵更新
    *
    */
    void attitude_update()
    {
        Vector3f gyro_data;
        Vector3f acc_data;
    
        get_gyro(gyro_data);
        get_acc(acc_data);
        
        static uint32_t last_time = 0;
        float dt = 0;
        if(last_time == 0)
            dt = 0;
        else
            dt = AP_HAL::millis() - last_time;
        last_time = AP_HAL::millis();
            
    #if 0
        //直接求角度
        //gyro_int(gyro_data, dt);
    
        //四元数求角度
        //gyro_2_quaternion(gyro_data, dt);
        //Vector3f euler_angle = quaternion_2_euler_angle(q0, q1, q2, q3);
        //printf("%.3f,%.3f,%.3f\n", euler_angle.x, euler_angle.y, euler_angle.z);
    #else
        //实际三轴加速度
        //归一化
        acc_data.normalize();
        //printf("normalize acc x[%.3f] y[%.3f] z[%.3f]\n", acc_data.x, acc_data.y, acc_data.z);
    
        //理论三轴加速度
        Vector3f _acc;
        quaternion_2_acc(_acc, q0, q1, q2, q3);
        //printf("_acc x[%.3f] y[%.3f] z[%.3f]\n", _acc.x, _acc.y, _acc.z);
    
        //理论和实际三轴加速度的叉乘得到误差e
        Vector3f error_acc;
        vector_cross(acc_data, _acc, error_acc);
        //printf("error_acc x[%.3f] y[%.3f] z[%.3f]\n", error_acc.x, error_acc.y, error_acc.z);
    
        Vector3f _gyro;
        //PI比例积分补偿陀螺仪的数据
        complementary_filter(_gyro, gyro_data, error_acc);
    
        //得到新的四元数
        gyro_2_quaternion(_gyro, dt);
    
        //新四元数解算出补偿后的欧拉角
        Vector3f euler_angle = quaternion_2_euler_angle(q0, q1, q2, q3);
        printf("%.3f,%.3f,%.3f\n", euler_angle.x, euler_angle.y, euler_angle.z);
    #endif    
    }
    

    可以对比补偿前后补偿后的角度数据,发现补偿后没有出现积分漂移,而且在动态性能上也能快的收敛。我们可以调整PI控制器的参数K和i来控制更相信谁的数据!

    本次就不上传最后的测试数据了,不是没有测试,而是数据搞丢了,懒得重新搞!

    展开全文
  • 四旋翼姿态解算——互补滤波算法及理论推导

    万次阅读 多人点赞 2017-02-24 17:37:11
    上次我们讨论了姿态解算基础理论以及几个比较重要的公式的一些推导,...互补滤波算法: 顾名思义,是多组数据结合互补,并进行滤波处理稳定输出,得到姿态的算法。而我们使用的传感器就是加速度计和陀螺仪。加速度计

    转载请注明出处:http://blog.csdn.net/hongbin_xuhttp://hongbin96.com/
    文章链接:http://blog.csdn.net/hongbin_xu/article/details/56846490http://hongbin96.com/111

    上次我们讨论了姿态解算基础理论以及几个比较重要的公式的一些推导,如果有不熟悉的请点击这里打开链接。这次来介绍一些实际的姿态解算算法吧!
    一般在程序中,姿态解算的方式有两种:一种是欧拉角法,一种是四元数法。这里不介绍欧拉角法,只介绍四元数法,如有兴趣可以去查找相关资料。

    互补滤波算法:
    顾名思义,是多组数据结合互补,并进行滤波处理稳定输出,得到姿态的算法。而我们使用的传感器就是加速度计和陀螺仪。加速度计用于测量加速度,陀螺仪用于测量角速度。 加速度计的静态稳定性更好,而在运动时其数据相对不可靠;陀螺仪的动态稳定性更好,但是静止时数据相对不可靠。所以,我们可以通过加速度计的输出来修正陀螺仪的漂移误差,换句话说,通过加速度计来修正陀螺仪

    这个是我在网上找到的说明互补滤波法的框图:(原图下载:点击这里打开)

    首先,我们取定导航坐标系n中标准重力加速度g,定义为这里写图片描述,那么将导航坐标系n下的这里写图片描述 转换为载体坐标系b下的:这里写图片描述
    这里用到了这里写图片描述,前面在推导基础公式时导出了使用四元数表示的旋转矩阵这里写图片描述
    公式如下:
    这里写图片描述
    但是我们要用的是这里写图片描述,所以还要对这里写图片描述 做一个矩阵逆变换。由于它是正交矩阵,对于正交矩阵有这个性质:正交矩阵的逆矩阵等于其的转置。所以我们很容易得到:
    这里写图片描述
    将上式代入这里写图片描述 ,而且我们已知这里写图片描述,所以得到:
    这里写图片描述

    接着再定义载体坐标系b中加速度计输出为a,由于前面计算导航坐标系时我们采用的重力加速度是标准重力加速度,所以还需要对其进行归一化,才能继续运算。
    设加速度计三个轴的值分别是ax,ay,az。
    首先求模:这里写图片描述
    归一化:这里写图片描述

    这里写图片描述
    根据框图的说明再整理一下前面得到的结果:
    标准重力加速度这里写图片描述从n系转到b系中的矩阵表示:
    这里写图片描述
    b系下加速度计测量得到的加速度的矩阵表示:
    这里写图片描述 (备注:这是归一化之后的值)

    这里写图片描述
    这里写图片描述这里写图片描述 做向量叉乘,即可得到给陀螺仪的校正补偿值e。
    这里写图片描述
    表示成矩阵形式更为直观:
    这里写图片描述

    然后再使用PI控制器进行滤波,准确地说事消除漂移误差,只要存在误差控制器便会持续作用,直至误差为0。控制的效果取决于P和I参数,分别对应比例控制和积分控制的参数。
    这里写图片描述
    这里给出PI控制的公式:
    这里写图片描述
    这里写图片描述 是我们要负反馈给陀螺仪进行校正补偿的值,这里写图片描述 是比例控制项,这里写图片描述 是积分控制项,在程序中采用离散累加计算。关于PID控制理论的东西,这里不做赘述。

    这里写图片描述
    如框图中所写,接下来将前面得到的补偿值这里写图片描述加在陀螺仪输出的数据上进行校正。

    跟着框图往下走:
    这里写图片描述
    将前面的陀螺仪数据通过四元数微分方程转换为四元数输出。
    因为有几个地方我也没搞懂,所以就简单介绍一下四元数微分方程,详细步骤请查阅秦永元的惯性导航一书(第三篇 9.2.3节):
    由于载体的运动,四元数Q是变量,其参数可以表示成关于时间的函数。
    使用四元数的三角形式:这里写图片描述
    这里写图片描述 为刚体瞬时绕轴转过的角度,这里写图片描述 为归一化后的位置矢量。
    设角速度为:这里写图片描述
    对四元数的三角表示形式求导:
    这里写图片描述
    因为这里写图片描述 , 且这里写图片描述
    所以:
    这里写图片描述
    将上式表示成矩阵形式:
    这里写图片描述
    或者这里写图片描述
    上面的两个公式被称为四元数微分方程。利用陀螺仪的数据进行离散累积便可得到四元数的值,最后再转换成欧拉角形式输出即可。

    再附上代码:

    ////////////////////////////////////////////////////////////////////////////////
    #define Kp 10.0f                        // proportional gain governs rate of convergence to accelerometer/magnetometer
    #define Ki 0.008f                          // integral gain governs rate of convergence of gyroscope biases
    #define halfT 0.001f                   // half the sample period采样周期的一半
    
    float q0 = 1, q1 = 0, q2 = 0, q3 = 0;    // quaternion elements representing the estimated orientation
    float exInt = 0, eyInt = 0, ezInt = 0;    // scaled integral error
    void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
    {
      float norm;
    //  float hx, hy, hz, bx, bz;
      float vx, vy, vz;// wx, wy, wz;
      float ex, ey, ez;
    
      // 先把这些用得到的值算好
      float q0q0 = q0*q0;
      float q0q1 = q0*q1;
      float q0q2 = q0*q2;
    //  float q0q3 = q0*q3;
      float q1q1 = q1*q1;
    //  float q1q2 = q1*q2;
      float q1q3 = q1*q3;
      float q2q2 = q2*q2;
      float q2q3 = q2*q3;
      float q3q3 = q3*q3;
    
        if(ax*ay*az==0)
            return;
    
      norm = sqrt(ax*ax + ay*ay + az*az);       //acc数据归一化
      ax = ax /norm;
      ay = ay / norm;
      az = az / norm;
    
      // estimated direction of gravity and flux (v and w)              估计重力方向和流量/变迁
      vx = 2*(q1q3 - q0q2);                                             //四元素中xyz的表示
      vy = 2*(q0q1 + q2q3);
      vz = q0q0 - q1q1 - q2q2 + q3q3 ;
    
      // error is sum of cross product between reference direction of fields and direction measured by sensors
      ex = (ay*vz - az*vy) ;                                             //向量外积在相减得到差分就是误差
      ey = (az*vx - ax*vz) ;
      ez = (ax*vy - ay*vx) ;
    
      exInt = exInt + ex * Ki;                                //对误差进行积分
      eyInt = eyInt + ey * Ki;
      ezInt = ezInt + ez * Ki;
    
      // adjusted gyroscope measurements
      gx = gx + Kp*ex + exInt;                                              //将误差PI后补偿到陀螺仪,即补偿零点漂移
      gy = gy + Kp*ey + eyInt;
      gz = gz + Kp*ez + ezInt;                                          //这里的gz由于没有观测者进行矫正会产生漂移,表现出来的就是积分自增或自减
    
      // integrate quaternion rate and normalise                           //四元素的微分方程
      q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
      q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
      q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
      q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
    
      // normalise quaternion
      norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
      q0 = q0 / norm;
      q1 = q1 / norm;
      q2 = q2 / norm;
      q3 = q3 / norm;
    
      //Q_ANGLE.Yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yaw
      Q_ANGLE.Y  = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
      Q_ANGLE.X = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
    }

    这段代码网上很多地方都可以看见,匿名四轴(老版本的)的程序也是用的这段。
    上面也添加了一些注释,把互补滤波算法的理论部分过了一遍后再来对着代码读一遍,应该不会觉得有多难了吧。程序中就只是把前面推导的公式一个个用上去了而已,如果不懂原理直接使用也不会有太大问题。
    程序中的积分运算是采用离散累积的方法运算的。PID控制器只是起到了一个稳定数据消除漂移误差的作用。

    希望这些笔记能给更多的人提供帮助,欢迎交流指正。

    我的前一篇博文讲的是姿态解算的基础知识和一些相关公式的推导,有兴趣的可以去查看:http://blog.csdn.net/hongbin_xu/article/details/55667899

    最后再分享一些我在网上找到的资料,个人觉得对学习很有帮助。
    链接:
    http://blog.csdn.net/wkdwl/article/details/52119163
    http://blog.csdn.net/nemol1990/article/details/16924745
    http://blog.csdn.net/super_mice/article/details/45619945
    http://www.openedv.com/thread-42657-1-1.html

    展开全文
  • Pixhawk-姿态解算-互补滤波

    万次阅读 多人点赞 2016-09-13 08:49:03
    终于说到了正题,姿态解算这一部分很重要,主要的基础就是惯性导航和多传感器数据融合,很多公司都在招这方面的人才,如百度的无人驾驶在找传感器数据融合,网易的人机交互工程师也在找这方面的人,因为它是信息流的...

         根深方能叶茂 在等待的日子里,刻苦读书,谦卑做人,养得深根,日后才能枝叶茂盛。 --Better(根爷)

         

        终于说到了正题,姿态解算这一部分很重要,主要的基础就是惯性导航和多传感器数据融合,很多公司都在招这方面的人才,如百度的无人驾驶在招传感器数据融合,网易的人机交互工程师也在找这方面的人,因为它是信息流的源泉,准确的姿态信息需要靠他们解算出来才能进行后续的步骤。


        鉴于加速度计低频特性比较好,因为加速度的角度可以直接算出来,没有累积误差,所以长时间后也比较准。而陀螺仪长时间后由于积分误差的累加,会造成输出误差比较大,甚至无法使用。所以用互补滤波法根据他们的特性取长补短进行姿态解算,每过一段时间就让加速度计去校准一下陀螺仪。互补滤波就是在短时间内采用陀螺仪得到的角度做为最优值,定时对加速度采样来的加速度值进行取平均值来校正陀螺仪的得到的角度。短时间内用陀螺仪比较准确,以它为主;长时间用加速度计比较准确,这时候加大它的比重,这就是互补了,不过加速度计要滤掉高频信号,陀螺仪要滤掉低频信号,互补滤波器就是根据传感器特性不同,通过不同的滤波器(高通或低通,互补的),然后再相加得到整个频带的信号。互补是给他们不同的权重加权求和。


        当然这里面还有一些问题:如加速度无法区分惯性加速度和运动加速度,在固定翼上这个问题更为显著,再者磁力计准确的偏角怎么得到?


       下面融合的一些框架图,先建立一个整体的概念:


              

        下面这幅图才是准确的阐述了互补滤波的过程。正常情况下用陀螺仪的数据就可以进行姿态的更新,但是由于陀螺仪的积分误差,这里用acc和mag去校正,求出他们的误差用PI去弥补。注意看看pid的公式和作用,pid是作用于误差(实际个期望之间的差值),最终反复调节,让实际值=期望值。



       下面先说点基础内容,之后再贴源码:

       下面介绍三部分内容:

        1、姿态的表示方法,在源码之中姿态的表示方法有DCM、四元数,欧拉角。欧拉角法在求解姿态时存在奇点(万向节死锁),不能用于全姿态的解算;方向余弦可用于全姿态的解算但计算量大,不能满足实时性要求。四元数法,其计算量小,无奇点且可以满足飞行器运动过程中姿态的实时解算。

       2、阐述一下姿态解算的原理。

        姿态就是指飞行器的俯仰/横滚/航向情况。在咱们地球上,就是指飞行器在地球坐标系中的俯仰/横滚/航向情况。飞行器需要实时知道当前自己的姿态,才能够根据需要操控其接下来的动作,例如保持平稳,例如实现翻滚。
    姿态是用来描述一个刚体的固连坐标系和参考坐标系之间的角位置关系,有一些数学表示方法。很常见的就是欧拉角,四元数,矩阵,轴角。

    地球坐标系又叫做地理坐标系,是固定不变的。正北,正东,正向上构成了这个坐标系的X,Y,Z轴,我们用坐标系R表示。四轴飞行器上固定着一个坐标系,我们一般称之为机体坐标系,用坐标系r表示。那么我们就可以用欧拉角,四元数等来描述r和R的角位置关系。这就是四轴飞行器姿态解算的数学模型和基础。


        欧拉角的姿态表示方法最为直观,可以看做飞机绕固定轴的三次旋转达到现在的姿态。

    分解之后就是每次的旋转:


    方向余弦矩阵是一个3*3阶的矩阵,矩阵的列表示载体坐标系中的单位矢量在参考坐标系中的投影。



    这是一个总的旋转的表达,分解为三次旋转,可以理解为R=R3*R2*R1。


        四元数姿态表达式是一个四参数的表达式。它的基本思路是:一个坐标系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量 的单次转动来实现。四元数用符号q表示,它是一个具有4个元素的矢量,这些元素是该矢量方向和转动大小的函数。定义 的大小和方向是使参考系绕 转动一个角度 ,就能与载体坐标系重合。

        他们三者都可以表示姿态,求出一个就相当于知道其他的了。具体用什么形式表达,可以按照你的要求自己去换算,他们的之间的关系如下:





    2、下面对姿态解算的原理进行阐述。

       姿态解算常用的算法有欧拉角法、方向余弦法和四元数法。 欧拉角法在求解姿态时存在奇点(万向节死锁),不能用于全姿态的解算; 方向余弦可用于全姿态的解算但计算量大,不能满足实时性要求。 四元数法,其计算量小,无奇点且可以满足飞行器运动过程中姿态的实时解算。

       姿态解算的原理:对于一个确定的向量,用不同的坐标系表示时,他们所表示的大小和方向一定是相同的。但是由于这两个坐标系的旋转矩阵存在误差,那么当一个向量经过这么一个有误差存在的旋转矩阵后,在另一个坐标系中肯定和理论值是有偏差的,我们通过这个偏差来修正这个旋转矩阵。这个旋转矩阵的元素是四元数,我们修正的就是四元数,这样姿态就被修正了。

       陀螺仪动态响应特性良好,但计算姿态时会产生累积误差。 磁力计和加速度计测量姿态没有累积误差,但动态响应较差。因此他们在频域上特性互补,所以采用互补滤波器融合这三种传感器的数据,提高测量精度和系统的动态性能。



    3、四元数姿态解算的步骤:









    好了到这里姿态解算的四路已经比较清晰了,最后给大家一个“杀手锏”:



       看到这些之后再结合源码,想必思路会清晰很多!

       不用谢,请叫我根爷。大笑


    展开全文
  • 九轴传感器姿态解算方法(互补滤波和梯度下降法MATLAB)九轴传感器姿态解算方法(互补滤波和梯度下降法MATLAB)九轴传感器姿态解算方法(互补滤波和梯度下降法MATLAB)九轴传感器姿态解算方法(互补滤波和梯度下降法...
  • MPU6050姿态解算——Mahony互补滤波

    千次阅读 多人点赞 2020-07-18 11:37:43
    DMP与互补滤波数据对比 Long time no see,首先放一张图,黄色的是DMP获取的Pitch角数据,紫红色的是互补滤波解算的Pitch角数据(人为晃动陀螺仪增加了干扰)。 DMP与互补滤波的优缺点分析 DMP 互补滤波 不...

    引入

    DMP与互补滤波数据对比

    Long time no see,首先放一张图,黄色的是DMP获取的Pitch角数据,紫红色的是互补滤波解算的Pitch角数据(人为晃动陀螺仪增加了干扰)。
    DMP与Mahony数据的比较

    DMP与互补滤波的优缺点分析

    DMP 互补滤波
    不需要了解姿态解算知识,直接调用库函数,操作简单 需要掌握一定的姿态解算知识
    解算速度慢,最大只能到200Hz 速度快,而且随着单片机主频提高会更快
    真实性差 真实性较好

    从上面的曲线图结合我实际的干扰,在我大范围晃动陀螺仪的时候,DMP的角度变化还是很小,感觉是为了滤波而滤波,数据的真实性不可靠。而且在动态曲线观察的时候,互补滤波明显比DMP得到的的数据更快,时效性也🆗。
    当然这也不意味着DMP一无是处,在对数据要求没那么高的情况下还是适用的。
    接下来就分析一下互补滤波是怎么通过陀螺仪的原始数据解算出较为准确的姿态角儿~。

    Mahony互补滤波算法分析

    概述

    陀螺仪一般为六轴或者九轴构成,六轴就是三轴角速度计加上三轴加速度计,九轴就是在六轴的情况下再加上一个三轴的磁力计,而应用最广泛的MPU6050就是一个六轴陀螺仪。
    有一种现象很普遍也包括我自己,就是在没有用过陀螺仪之前,总以为陀螺仪可以直接得到角度数据,实际上并不然,陀螺仪(在下文中就代表MPU6050等六轴陀螺仪)只能得到角速度数据和角速度数据,从而间接得到角度。
    可能数理基础不错的小伙伴会想到,对角速度积分不就成了角度吗?没错,确实是这样,但是陀螺仪存在误差,假设陀螺仪有一个0.1°/s的误差,一分钟之后误差就到了6°,一个小时后就是360°,所以原始数据并不是可靠的。
    互补滤波就是对误差进行一个补偿,但是并不是用这种积分的方法,而是一个神奇的算法。

    欧拉角描述方向余弦矩阵

    在用欧拉角描述方向余弦矩阵之前先来了解一下坐标系的变换。注意是坐标系的变换而不是坐标。这篇文章就推导的十分详细

    空间中任意坐标的旋转,等效于依次绕三个坐标轴的定轴旋转(不用看图的旋转方向,单纯解释这句话)。
    在这里插入图片描述
    以下三个定轴旋转都是在右手坐标系下的正方向旋转:

    1. 绕Z轴旋转ψ角,有
    在这里插入图片描述
    2. 绕Y轴旋转θ角,有
    在这里插入图片描述

    3. 绕轴转动γ角,有
    在这里插入图片描述

    4. 整体坐标系转换
    在这里插入图片描述

    本文采用北东地坐标系为地理坐标系,并以地理坐标系作为导航坐标系,所以b系到t系的转换矩阵也是b系到n系的转换矩阵。b到n的转换矩阵被称为姿态矩阵或方向余弦矩阵
    在这里插入图片描述
    由姿态矩阵可以求解出载体的横滚角γ、俯仰角θ和航向角ψ的主值,即:
    在这里插入图片描述
    这样就解算出了欧拉角,但是用欧拉角描述的方向余弦矩阵伴随着大量的三角运算,对单片机的处理不友好,会影响解算速度,而且会出现一个欧拉角死锁问题点这里,故采用接下来的四元数做解算。

    四元数描述方向余弦矩阵

    顺口提一句,四元数的发明者就是互补滤波的发明者叫威廉姆·汉密顿,懂为啥要用四元素结算了吧。当然是开个玩笑,四元数也是一种表达姿态的形式,在姿态解算中它避免了大量的三角运算,处理速度快。
    在这里插入图片描述
    用四元数描述上面的用欧拉角描述的姿态矩阵(在阅读开源程序代码的时候一定要注意,可能这个矩阵的不同,那是因为旋转方向和顺序不一样,还要注意看清下标,是哪个系转换到哪个系):
    在这里插入图片描述
    由上面欧拉角的主值公式可以得到欧拉角与四元数的关系,这样就通过四元数来表示了方向余弦矩阵和欧拉角。
    在这里插入图片描述

    求解四元数

    把四元数表示的姿态矩阵对时间求微分(其中做左边一列q上面带个小点表示微分,ω就是各轴的角速度):
    在这里插入图片描述
    然后采用龙格库塔法解微分方程(我还不会推,直接用结论):
    在这里插入图片描述
    下标为t+▲t是当前四元数,右边下标t为上个周期四元数。

    传感器数据融合

    在概述中我们分析了陀螺仪中角速度测量可能存在一个误差,这个误差在短时间内可以忽略,但是随着时间的累积,误差会越来越大。陀螺仪中的加速度计对高频信号敏感,在振动环境中高频干扰大。

    陀螺仪得到的角速度数据短期内可信,长期由于积分误差不可信。加速度计得到的加速度数据,短期内由于高频抖动干扰不可信,长期可信。

    拿出我们的中学二年级知识,地球上一切物体的重力加速度都是9.8m/s^2,我们用地理坐标系的重力加速度g,乘以一个从地理坐标系n到机体坐标系b的姿态转换矩阵,可以推导出机体坐标系的理论重力加速度v。在这里插入图片描述

    误差补偿

    在这里插入图片描述

    由上图可以看出,陀螺仪测出角速度解算出四元数,再由四元素推导出理论重力加速度v,而加速度计测出实际重力加速度a又称gb(b系下的重力加速度),如果陀螺仪测出的角速度完全可靠,那么实际重力加速度gb应该是等于理论重力加速度vb的,但是很不幸,角速度数据存在误差,vb和gb就不会重合。

    灵魂来了
    用一个叉积来描述这个误差的大小

    几何意义表示:
    在这里插入图片描述

    在这里插入图片描述
    叉积公式(两种表达形式,第二种在程序里体现):

    1. aXb = |a|*|b|*sin θ
    2. 对于向量a(x1, y1), b(x2, y2),叉乘公式为x1 * y2 - x2 * y1

    小角近似(sin θ的值与θ的值在θ很小的时侯很相近):
    在这里插入图片描述
    在上面得到了理论重力加速度与实际重力加速度的误差erroor,这个error是一个实际的角度误差,我们用加速度计的值间接得出了误差变化的角度相关值,现在借助PID中的PI思想将这个误差补偿,这样就通过加速度计间接的得到了角度误差值(KP=1.5,KI=0.005,是哪篇论文研究过我忘记了,参数可调,按实际来):
    在这里插入图片描述

    偏差角度很小的情况下,我们可以将陀螺仪角速度误差和加速度计求得的角度差看做正比的关系:
    在这里插入图片描述
    得到准确的角速度后就可以用四元数反解出正确可靠的欧拉角了。

    STM32程序实例

    #include "imu.h"
    #include "math.h"
    #include "mpu6050.h"
    #include "stm32f10x.h"
    
    #define Acc_Gain 0.0001220f			//加速度转换单位(初始化加速度计量程+-4g,由于mpu6050的数据寄存器是16位的,LSBa = 2*4 / 65535.0)
    #define Gyro_Gain 0.0609756f		//角速度转换为角度(LSBg = 2000*2 / 65535)
    #define Gyro_Gr 0.0010641f			//角速度转换成弧度(3.1415 / 180 * LSBg)
    #define G 9.80665f					// m/s^2
    
    extern short ax,ay,az;
    extern short gx,gy,gz;
    
    
    static float invSqrt(float x) 		//快速计算 1/Sqrt(x)
    {
    	float halfx = 0.5f * x;
    	float y = x;
    	long i = *(long*)&y;
    	i = 0x5f3759df - (i>>1);
    	y = *(float*)&i;
    	y = y * (1.5f - (halfx * y * y));
    	return y;
    }
    
    
    void Prepare_Data(void)
    {
    	MPU_Get_Accelerometer(&ax,&ay,&az);			//获取加速度计原始值
    	MPU_Get_Gyroscope(&gx,&gy,&gz);				//获取角速度原始值
    	
    	//将加速度原始AD值转换为m/s^2
    	ax = (float)ax * Acc_Gain * G;
    	ay = (float)ay * Acc_Gain * G;
    	az = (float)az * Acc_Gain * G;
    	
    	//将陀螺仪AD值转换为 弧度/s
    	gx = (float)gx * Gyro_Gr;
    	gy = (float)gy * Gyro_Gr;
    	gz = (float)gz * Gyro_Gr;
    }
    
    
    
    #define Kp 1.50f
    #define Ki 0.005f
    #define halfT 0.0025f						//计算周期的一半,单位s
    
    extern float Yaw,Pitch,Roll;				//我要给其他文件用所以定义了extern,不用管
    float q0 = 1, q1 = 0, q2 = 0, q3 = 0;		//四元数
    float exInt = 0, eyInt = 0, ezInt = 0;		//叉积计算误差的累计积分
    
    
    void Imu_Update(float gx,float gy,float gz,float ax,float ay,float az)
    {
    	u8 i;
    	float vx,vy,vz;							//实际重力加速度
    	float ex,ey,ez;							//叉积计算的误差
    	float norm;
    	
     	float q0q0 = q0*q0;
     	float q0q1 = q0*q1;
    	float q0q2 = q0*q2;
    	float q0q3 = q0*q3;
    	float q1q1 = q1*q1;
     	float q1q2 = q1*q2;
     	float q1q3 = q1*q3;
    	float q2q2 = q2*q2;
    	float q2q3 = q2*q3;
    	float q3q3 = q3*q3;
    	
    	if(ax*ay*az == 0)
    		return;
    	
    	//加速度计测量的重力方向(机体坐标系)
    	norm = invSqrt(ax*ax + ay*ay + az*az);			//之前这里写成invSqrt(ax*ax + ay+ay + az*az)是错误的,现在修改过来了
    	ax = ax * norm;
    	ay = ay * norm;
    	az = az * norm;
    	
    	//四元数推出的实际重力方向(机体坐标系)
    	vx = 2*(q1q3 - q0q2);												
      	vy = 2*(q0q1 + q2q3);
      	vz = q0q0 - q1q1 - q2q2 + q3q3;
    	
    	//叉积误差
    	ex = (ay*vz - az*vy);
    	ey = (az*vx - ax*vz);
    	ez = (ax*vy - ay*vx);
    	
    	//叉积误差积分为角速度
    	exInt = exInt + ex * Ki;
    	eyInt = eyInt + ey * Ki;
    	ezInt = ezInt + ez * Ki;
    	
    	//角速度补偿
    	gx = gx + Kp*ex + exInt;
    	gy = gy + Kp*ey + eyInt;
    	gz = gz + Kp*ez + ezInt;
    	
    	//更新四元数
      	q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
      	q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
      	q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
      	q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;	
    	
    	//单位化四元数
      	norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
      	q0 = q0 * norm;
      	q1 = q1 * norm;
      	q2 = q2 * norm;  
      	q3 = q3 * norm;
    	
    	//四元数反解欧拉角
    	Yaw = atan2(2.f * (q1q2 + q0q3), q0q0 + q1q1 - q2q2 - q3q3)* 57.3f;
    	Pitch = -asin(2.f * (q1q3 - q0q2))* 57.3f;
    	Roll = atan2(2.f * q2q3 + 2.f * q0q1, q0q0 - q1q1 - q2q2 + q3q3)* 57.3f;
    }
    
    展开全文
  • 上一次的博文中(点我打开)介绍了互补滤波法的算法流程和程序实现,但是仅仅只是融合了三轴加速度计和三轴陀螺仪的数据解算姿态。 由于机体水平时,加速度计无法测量绕 Z 轴的旋转量,即偏航角yaw,并且磁力计也...
  • 1.2 Mahony算法原理 参考另一篇文章:基于Manony滤波算法的姿态解算 2,代码实现 1.1 MPU6050初始化及数据读取 该部分代码参考了正点原子的MPU6050例程;主要修改以下初始化代码 /* * MPU6050模块:绕x轴为roll,绕y...
  • 四旋翼姿态解算--互补滤波和拓展卡尔曼

    千次阅读 多人点赞 2018-03-17 21:46:08
    关于利用互补滤波进行姿态解算的文章随便一搜就有很多,就不列出来了。关于卡尔曼的理解和原理,觉得这篇挺通俗易懂的,(二)中还附有matlab代码:卡尔曼滤波 -- 从推导到应用:...
  • 姿态解算之显式互补滤波法(MahonyAHRS) #1 前言 本文以四旋翼为背景,说明一下姿态解算中经典的互补滤波算法,也称为Mahony算法。 文章说明:本文使用的机体系是“右-前-上”坐标系, 导航系是“东-北-天坐标系” ...
  • //移植只需改以下参数 /* #define IIC_SCL_Pin GPIO_Pin_6 #define IIC_SDA_Pin GPIO_Pin_7 #define IMU_IIC_GPIO GPIOB #define IMU_IIC_RCC RCC_APB2Periph_GPIOB #define IIC_SDA_In() {GPIOB->CRL&=0X0FFFFFFF...
  • 多旋翼姿态解算之Mahony互补滤波

    千次阅读 2019-06-05 20:10:26
    Mahony互补滤波器是多旋翼姿态解算中常用的一种简单又高效的方法。 在介绍之前,首先来看一些关于传感器的基础知识: 陀螺仪 用于测量角速度,具有高动态特性,它是一个间接测量角度的器件其中一个关键参数就是...
  • MPU6050姿态解算STM32代码(互补滤波、卡尔曼滤波),直接可用
  • 本人亲测非常好用的MPU6050姿态解算STM32源码(互补滤波)算法,希望帮助大家
  • 基于自适应显式互补滤波姿态解算方法.pdf
  • 关于MPU6050一阶互补滤波方法(从原理到代码实现) 1.写在前面 最近知道自己不用考研后便花了很多时间来准备机械创新设计大赛,在设计的多功能防台风窗中需要到MPU6050对窗户的姿态进行检测,用来反馈到步进电机控制...
  • MPU6050姿态解算STM32源码(互补滤波),MPU6050六轴传感器实验例子,学习MPU6050 六轴传感器(三轴加速度+三轴陀螺仪)的使用.
  • STM32互补滤波源码,MPU6050姿态解算。详细中文注释
  • 互补滤波姿态解算(加速度计、陀螺仪)
  • 前言:前段时间一直在备赛没时间做总结,其实无人机控制算法中主要是姿态解算系统和姿态控制系统,这里先总结下姿态解算系统的相关要点,研究一下关于姿态解算的过程和实现,本篇博文主要是以mahony的算法为基础理解...
  • https://blog.csdn.net/weixin_40599145/article/details/99937264?utm_medium=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-15.control&dist_request_id=35b7d4ac-c28b-4228-b2a1-...
  • 基于四元数的简单互补滤波姿态解算

    万次阅读 热门讨论 2016-11-11 17:02:54
    序言鉴于现在飞控的热度不减当年,越来越多“年轻人”加入飞控制作学习的阶段,也有许多新手上来就气势汹汹的进行姿态解算,往往使用的就是较为常用的Mahony的互补滤波姿态解算,笔者在多年前也一样,最开始接触的...

空空如也

空空如也

1 2 3 4 5 ... 7
收藏数 121
精华内容 48
关键字:

姿态解算互补滤波