用户
 找回密码
 立即注册

QQ登录

只需一步,快速开始

扫一扫,访问微社区

点击进入授权页面

只需一步,快速开始

  • QQ空间
  • 回复
  • 收藏

  • TA的每日心情
    奋斗
    2016-3-31 13:33
  • 签到天数: 58 天

    [LV.5]常住居民I

    参加活动: 0

    组织活动: 0

           继之前研究了一些飞行姿态理论方面的问题后,又找到了之前很流行的一段外国大神写的代码,来分析分析。
           第二篇文章的最后,讲到了文章中的算法在实际使用中有重大缺陷。
           大家都知道,分析算法理论的时候很多情况下我们没有考虑太多外界干扰的情况,原因是很多情况下,传感器的精度以及受到的干扰并不会特别大,而显著的影响到算法。但是在IMU系统中,有点不同。由于地磁场十分微弱,而我们生活中有大量使用电子设备,使得磁场非常的混乱,以至于地磁传感器非常容易受到干扰。
           由于以上算法把地磁传感器一同加入到姿态的测定中,并基本给予了地磁传感器与加速度传感器同样的加权,导致地磁传感器一旦被干扰,会对姿态产生地球重力突然被干扰一样的结果。。。对姿态的测量是毁灭性的。
           综上,考虑到磁场的不稳定性,必须对地磁传感器进行降权处理,使得他对姿态的影响变小。
           于是设计了以下的算法。
           将磁场传感器的数据在姿态角度中剔除,更新姿态的俯仰角(PITCH)以及横滚角(ROLL)的时候只使用加速度传感器以及陀螺仪(角速度传感器)。
           只在计算偏航角的(YAW)的时候使用磁场传感器。也就是只使用磁场传感器作为一个电子指南针,定位整个姿态在水平面旋转的角度。这样设计,让磁场传感器只影响姿态中的一个数值,减少了磁场的权重,即使磁场收到干扰,也不会导致姿态骤变,使得四轴坠机。
           在对YAW进行计算的时候使用了如下函数。
           eulerAngleRaw.yaw = 0.9 * (eulerAngleRaw.yaw - gzF*2*halfT) + 0.1 * angleMagYaw;
           此处的0.9和0.1是可以变动的但他们相加应该为1,此处为一个最简单的1阶低通滤波器,增加0.1则是增大截止频率。
           算法的流程图是这样的:
          
    新的姿态更新算法是这样的
    void        AHRSupdate(float gxf, float gyf, float gzf, float axf, float ayf, float azf, float mxf, float myf, float mzf)
    {
            double norm;
            float vx, vy, vz;
            float ex, ey, ez;
           float halfT;                                                                                                                                     //采样周期的一半
            
            // 辅助变量,以减少重复操作数
            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;

            // 测量归一化
            norm = invSqrt(axf*axf + ayf*ayf + azf*azf);      
            axf = axf * norm; //向量a 为传感器重力 飞行器分量
            ayf = ayf * norm;
            azf = azf * norm;
    //        norm = invSqrt(mxf*mxf + myf*myf + mzf*mzf);                              
    //      向量m 为传感器磁场 飞行器分量
    //        mxf = mxf * norm;
    //        myf = myf * norm;
    //        mzf = mzf * norm;
            
            // 计算参考磁通方向
    //        hx = 2*mxf*(0.5 - q2q2 - q3q3) + 2*myf*(q1q2 - q0q3) + 2*mzf*(q1q3 + q0q2);  //向量h 为磁场通过旋转以后 参考系分量
    //        hy = 2*mxf*(q1q2 + q0q3) + 2*myf*(0.5 - q1q1 - q3q3) + 2*mzf*(q2q3 - q0q1);
    //        hz = 2*mxf*(q1q3 - q0q2) + 2*myf*(q2q3 + q0q1) + 2*mzf*(0.5 - q1q1 - q2q2);
    //        bx = 1.0f/invSqrt((hx*hx) + (hy*hy));                                            
    //原则上应该只有X向的分量 ex的磁场传感器部分误差就是由此式产生。
    //        bz = hz;
            
            //估计方向的重力和磁通(V和W)                                            
           //反向使用 四元数 及把a用-a替换。
            vx = 2*(q1q3 - q0q2);                                                   
           //v为把重力反向旋转到飞行器参考系时重力的向量
            vy = 2*(q0q1 + q2q3);
            vz = q0q0 - q1q1 - q2q2 + q3q3;
            
    //        wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);                     
    //      w为把磁场反向旋转到飞行器参考系时磁场的向量
    //        wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
    //        wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);
            
            // 错误是跨产品的总和之间的参考方向的领域和方向测量传感器                         //建立误差函数    向量的外积
    //        ex = (ayf*vz - azf*vy) + (myf*wz - mzf*wy);
    //        ey = (azf*vx - axf*vz) + (mzf*wx - mxf*wz);
    //        ez = (axf*vy - ayf*vx) + (mxf*wy - myf*wx);
           ex = (ayf*vz - azf*vy);
           ey = (azf*vx - axf*vz);
           ez = (axf*vy - ayf*vx);
            
            halfT=getCurrentTime(timer4);
            if (halfT>0.05)halfT=0;
    //        printf("%f\n\r",halfT);
            if(ex != 0.0f && ey != 0.0f && ez != 0.0f)      
            {
                      // 积分误差比例积分增益                                                  
                      exInt = exInt + ex*Ki*(halfT);
                      eyInt = eyInt + ey*Ki*(halfT);
                      ezInt = ezInt + ez*Ki*(halfT);
                   
                      // 调整后的陀螺仪测量
                      gxf = gxf + Kp*ex + exInt;
                      gyf = gyf + Kp*ey + eyInt;
                      gzf = gzf + Kp*ez + ezInt;
            }
            
           // halfT=getCurrentTime();
           //  printf("%f \n\r", halfT);
            // 整合四元数率和归一化                                                      
           //龙格-库格法
            q0 = q0 + (-q1*gxf - q2*gyf - q3*gzf)*halfT;
            q1 = q1 + (q0*gxf + q2*gzf - q3*gyf)*halfT;
            q2 = q2 + (q0*gyf - q1*gzf + q3*gxf)*halfT;
            q3 = q3 + (q0*gzf + q1*gyf - q2*gxf)*halfT;
            
            // 归一化四元数
            norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);                                
            q0 = q0 * norm;
            q1 = q1 * norm;
            q2 = q2 * norm;
            q3 = q3 * norm;
    }
           与第二篇相比,算法中注释掉了所有与磁场有关的部分。
           姿态更新后,用如下语句与磁场传感器计算出来的YAW(偏航角)进行混合
           eulerAngleRaw.yaw = 0.9 * (eulerAngleRaw.yaw - gzF*2*halfT) + 0.1 * angleMagYaw;
           经过检验,该算法姿态稳定,准确,变化迅速,YAW的值也能长期保证稳定,能为四轴飞行器提供很好的姿态数据。
           如果喜欢观看类似科技新奇事物,以及了解创客圈最新资讯,或者您对Arduino有所耳闻,可以关注扫描以下二维码,一定会带给您最新的资讯,最实用的教程,以及创客最新的玩意。





    厉害 简洁 有大用 只是找不到笔记一
    笔记1 在Arduino区。。
    您好,不是很明白有了欧拉角之后怎么对四个电机进行控制,有资料推荐一下吗
    BIT.小彦 发表于 2016-8-21 17:25
    您好,不是很明白有了欧拉角之后怎么对四个电机进行控制,有资料推荐一下吗 ...

    电机控制用的PID算法
    发新帖
    发表评论
    高级模式  
    您需要登录后才可以回帖 登录 | 立即注册  
    关闭

    推荐主题 上一条 /4 下一条