无人机项目跟踪记录四十四---姿态解算模块详解（1）

姿态解算模块中的函数，static void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt)，详解如下：

此函数作用是对采集的数据进行数据处理以及四元素的自适应的互补滤波。

    float recipNorm;                                                           //均方根值得倒数
    float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;                         //三轴误差值的一半

    // Make filter converge to initial solution faster
    // This function assumes you are in static position.
    // WARNING : in case air reboot, this can cause problem. But this is very unlikely happen.
    if(bFilterInit == 0) {
        NonlinearSO3AHRSinit(ax,ay,az,mx,my,mz);                              //初始化变量，这里没用磁力计，感觉没啥用只是给q0赋值1开始迭代
        bFilterInit = 1;
    }

上段函数的作用是初始化部分。

    //! If magnetometer measurement is available, use it.
    if(!((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))) {                       //利用磁力计来校准误差，此项目没有用到磁力计，因此不起作用
			float hx, hy, hz, bx, bz;                                                 //地理坐标系下的三轴磁力值以及 同一水平面上地磁线x方向与z方向值
			float halfwx, halfwy, halfwz;                                             //转换成机体坐标系的三轴磁力值

        // Normalise magnetometer measurement
        // Will sqrt work better? PX4 system is powerful enough?
        recipNorm = invSqrt(mx * mx + my * my + mz * mz);                         //求平方根的倒数
        mx *= recipNorm;                                                          //磁力计x轴方向进行归一化
        my *= recipNorm;                                                          //磁力计y轴方向进行归一化
        mz *= recipNorm;                                                          //磁力计z轴方向进行归一化

        // Reference direction of Earth's magnetic field
			  hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));       //将机体系测量出来的磁力计转换成地理系坐标中
        hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));       //将机体系测量出来的磁力计转换成地理系坐标中
        hz = 2.0f * mx * (q1q3 - q0q2) + 2.0f * my * (q2q3 + q0q1) + 2.0f * mz * (0.5f - q1q1 - q2q2);  //将机体系测量出来的磁力计转换成地理系坐标中
			  bx = sqrt(hx * hx + hy * hy);                                              //求将磁力计朝北时候x方向的值（此时y方向测得的值为零）
        bz = hz;                                                                   //求z方向的测量值，由于与xy平面平行水平放置，z轴的值相等

        // Estimated direction of magnetic field
			  halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);                   //又转化到机体坐标系的x方向值
        halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);                          //又转化到机体坐标系的y方向值
        halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);                   //又转化到机体坐标系的z方向值

        // Error is sum of cross product between estimated direction and measured direction of field vectors
        halfex += (my * halfwz - mz * halfwy);                                    //求两个向量的差值，一般情况又转换回机体坐标系的值应该与测量值相等，通过计算两个向量的叉乘来表示向量的误差值
        halfey += (mz * halfwx - mx * halfwz);
        halfez += (mx * halfwy - my * halfwx);
    }

以上代码是当无人机采用磁力计时才起作用，首先将磁力计采集上来的值进行归一化处理。由于磁力计采集的值是机体系的值，先将机体系的值转换到地理坐标系中。在地理坐标系中，当将磁力计朝北水平放置时，磁力计的读数是（bx,0,bz）。所以要先求出bx与bz，bx与地理系坐标满足勾股定理关系。当水平放置时，bz与hz重合相等。求出bx与bz后再将其转换到机体坐标系。也就是说将磁力计水平朝北放置读取的（bx，0，bz）值转换到机体坐标系。由于出厂和器件偏差等原因，陀螺仪总会出现偏差。我们将转换到机体坐标系后，磁力计的三轴向量值与磁力计在机体系读取的值，进行向量比较。就会求出向量之间的偏差。向量偏差怎么求得呢，一般的是利用向量之间的叉乘来求。因为两向相同，它们的叉乘是零。

    //增加一个条件：  加速度的模量与G相差不远时。 0.75*G < normAcc < 1.25*G
    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
    if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))                           //利用加速度的值来求误差值
    {
			  float halfvx, halfvy, halfvz;                                            //机体系三轴加速度的一半

        // Normalise accelerometer measurement
        //归一化，得到单位加速度
        recipNorm = invSqrt(ax * ax + ay * ay + az * az);                         //求平方根植的倒数

        ax *= recipNorm;                                                          //归一化x轴方向的值
        ay *= recipNorm;                                                          //归一化y轴方向的值
        az *= recipNorm;                                                          //归一化z轴方向的值

        // Estimated direction of gravity and magnetic field
			  halfvx = q1q3 - q0q2;                                                      //将地理坐标系归一化的重力转换成机体坐标系，取一半值x方向
        halfvy = q0q1 + q2q3;                                                      //将地理坐标系归一化的重力转换成机体坐标系，取一半值y方向
        halfvz = q0q0 - 0.5f + q3q3;                                               //将地理坐标系归一化的重力转换成机体坐标系，取一半值z方向

        // Error is sum of cross product between estimated direction and measured direction of field vectors
			  halfex += ay * halfvz - az * halfvy;           //通过叉乘来比较重力值转换成机体系与机体系测量的向量的误差值x轴方向
        halfey += az * halfvx - ax * halfvz;           //通过叉乘来比较重力值转换成机体系与机体系测量的向量的误差值y轴方向
        halfez += ax * halfvy - ay * halfvx;           //通过叉乘来比较重力值转换成机体系与机体系测量的向量的误差值z轴方向
    }

以上代码的意思是，当没有安装磁力计的时候，只能通过加速计来求偏差值了。首先将三轴加速度进行归一化。首先将无人机水平放置时三轴的加速度值是（0,0，g），将其归一化后是（0,0,1）.然后将此地理系归一化的加速度值通过四元素转换矩阵，转化到机体坐标系中，与机体坐标系测得的加速度值向量进行叉乘比较。四元素机体到地理系转化矩阵如下文章所示，从地理系到机体系的转换矩阵将其求转置即可。

https://blog.csdn.net/LSG_Down/article/details/80667889

    // Apply feedback only when valid data has been gathered from the accelerometer or magnetometer
    if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) {   //如果存在偏差
        // Compute and apply integral feedback if enabled
        if(twoKi > 0.0f) {
            gyro_bias[0] += twoKi * halfex * dt;	// integral error scaled by Ki   // x轴方向进行积分整定
            gyro_bias[1] += twoKi * halfey * dt;                                   // y轴方向进行积分整定                                 
            gyro_bias[2] += twoKi * halfez * dt;                                   // z轴方向进行积分整定   

            // apply integral feedback
            gx += gyro_bias[0];                                                     //积分修正
            gy += gyro_bias[1];
            gz += gyro_bias[2];
        }
        else {
            gyro_bias[0] = 0.0f;	// prevent integral windup
            gyro_bias[1] = 0.0f;
            gyro_bias[2] = 0.0f;
        }

        // Apply proportional feedback
        gx += twoKp * halfex;           //比例修正x轴方向
        gy += twoKp * halfey;           //比例修正y轴方向
        gz += twoKp * halfez;           //比例修正z轴方向
    }

以上代码是对偏差值进行pid整定，具体公式是ex = p * ex + i * ex dt + d * ex /dt。由于无人机系统延迟比较小，用不到制动的d算子，只用PI即可。

  // Time derivative of quaternion. q_dot = 0.5*q\otimes omega.
    //! q_k = q_{k-1} + dt*\dot{q}
    //! \dot{q} = 0.5*q \otimes P(\omega)
    dq0 = 0.5f*(-q1 * gx - q2 * gy - q3 * gz);    //四元素姿态解算公式
    dq1 = 0.5f*(q0 * gx + q2 * gz - q3 * gy);
    dq2 = 0.5f*(q0 * gy - q1 * gz + q3 * gx);
    dq3 = 0.5f*(q0 * gz + q1 * gy - q2 * gx);

    q0 += dt*dq0;  //四元素姿态解算公式，校准四元素
    q1 += dt*dq1;   
    q2 += dt*dq2;
    q3 += dt*dq3;

    // Normalise quaternion
    recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);  //均方根的倒数
    q0 *= recipNorm;   //对校准的值进行归一化操作
    q1 *= recipNorm;
    q2 *= recipNorm;
    q3 *= recipNorm;

    // Auxiliary variables to avoid repeated arithmetic
    q0q0 = q0 * q0;   //辅助计算
    q0q1 = q0 * q1;
    q0q2 = q0 * q2;
    q0q3 = q0 * q3;
    q1q1 = q1 * q1;
    q1q2 = q1 * q2;
    q1q3 = q1 * q3;
    q2q2 = q2 * q2;
    q2q3 = q2 * q3;
    q3q3 = q3 * q3;

解四元素微分方程的常规操作，具体解释请看文章：https://zhuanlan.zhihu.com/p/103623879

求q0,q1,q2,q3,q4进行迭代操作比如q0 = q0 + 1/2 dt (-wx*q1 - wy*q2 - wz* q3)，q1，q2,q3与其相同。就可求得上面的内容。dt是每周期调用采样的时间。