四周无人机的姿态解算（2）

接上一篇

4、尽量直观地分析一下程序

直接把小飞机的程序考过来了。。。
说明：

---

 函数名：姿态更新
 输入：mpu6050结构体，也就是测得数据+初始偏置 
 输出：三个姿态角数据

 函数变量的结构体定义： typedef struct{
    int16_t accX;
    int16_t accY;
    int16_t accZ;
    int16_t gyroX;
    int16_t gyroY;
    int16_t gyroZ;
    
    int16_t Offset[6];
    bool Check;
     }MPU6050Manager_t;

typedef struct{
    float roll;
    float pitch;
    float yaw; }Attitude_t;

---

接下来就是代码了

void ATT_Update(const MPU6050Manager_t *pMpu,Attitude_t *pAngE, float dt) 

{    
Vector_t Gravity,Acc,Gyro,AccGravity;

vector_t结构体的定义如下，实际上就是三维向量：
typedef struct
{
float x;
float y;
float z;
}Vector_t;
看名字应该知道定义的是哪些向量。

static Vector_t GyroIntegError = {0};
static float KpDef = 0.8f ; //四元数收勉值
static float KiDef = 0.0003f; 

以上数据是在静态存储区，下面两个是参数，第一个GyroIntegError可以理解是陀螺仪数据的积分的error，这里的error之前总是认为是误差的意思，实际上这样理解非常不准确，应该是期望值与实际值的偏差。为啥要定义这个就要往后看。

float q0_t,q1_t,q2_t,q3_t;

这四个带_t的数是四元数的增量，实际上看到字母q表示的量还是四个一起出现，那肯定是四元数Quaternions了。

float NormQuat; 

加速度计读取出来的向量的模，norm就是模的意思。

float HalfTime = dt * 0.5f;

dt单位时间

Gravity.x = 2 * (NumQ.q1 * NumQ.q3 - NumQ.q0 * NumQ.q2);                
Gravity.y = 2 * (NumQ.q0 * NumQ.q1 + NumQ.q2 * NumQ.q3);              
Gravity.z = 1 - 2 * (NumQ.q1 * NumQ.q1 + NumQ.q2 * NumQ.q2);  

这里的NumQ.qx,是在函数外部定义的静态结构体，结构体里有四个变量。实际上，这就是表示系统姿态的四元数，最终积分也是积在这上面。
static Quaternion_t NumQ = {1, 0, 0, 0};
那么上面这三行是什么意思？
实际上是利用四元数求取当前重力加速度在物坐标系的三个分量，因为四元数是可以表示物体姿态的，所以也就可以求出这个向量。
当然，如果物理含义理解的比较好还会发现这三个式子就是用四元数表示的姿态变换矩阵的第三行元素。

NormQuat = Q_rsqrt(squa(g_MPUManager.accX)+ squa(g_MPUManager.accY) +squa(g_MPUManager.accZ));

Acc.x = pMpu->accX * NormQuat; //归一后可化为单位向量下方向分量
Acc.y = pMpu->accY * NormQuat;  
Acc.z = pMpu->accZ * NormQuat;  

上面这里就是对加速度计测出的向量归一化

//向量叉乘得出的值，叉乘后可以得到旋转矩阵的重力分量在新的加速度分量上的偏差
AccGravity.x = (Acc.y * Gravity.z - Acc.z * Gravity.y);
AccGravity.y = (Acc.z * Gravity.x - Acc.x * Gravity.z);
AccGravity.z = (Acc.x * Gravity.y - Acc.y * Gravity.x);

这里明显是一个叉积运算，两个参与运算的向量分别为：加速度计测量出的向量和当前四元数推导出来的向量的叉积。他们都表示重力加速度在物坐标系的向量，都是单位向量。

GyroIntegError.x += AccGravity.x * KiDef;
GyroIntegError.y += AccGravity.y * KiDef;
GyroIntegError.z += AccGravity.z * KiDef;

至于为什么求叉积，因为叉积的大小，或者说叉积各个分量的大小可以表现出两个向量的偏差。在都是单位向量的情况下，显然偏差越大，即角度越大，得到的叉积越大。如果两个向量重合，夹角为零了，那叉积也是零了。

将通过叉积运算得到这个偏差乘以一个系数加到积分偏差中，作为姿态变化的一部分。

Gyro.x = pMpu->gyroX * Gyro_Gr + KpDef * AccGravity.x  +  GyroIntegError.x;//弧度制，此处补偿的是角速度的漂移
Gyro.y = pMpu->gyroY * Gyro_Gr + KpDef * AccGravity.y  +  GyroIntegError.y;
Gyro.z = pMpu->gyroZ * Gyro_Gr + KpDef * AccGravity.z  +  GyroIntegError.z;  
//角速度融合加速度比例补偿值，与上面三句共同形成了PI补偿，得到矫正后的角速度值  

这里就很精髓，还用了pid。。。
实质上，Gyro.xyz是角速度，是相对于物坐标系，它等于后面三项相加，其中后面第一项是从陀螺仪中读到的角速度值，第二项是偏差值，第三项是偏差值的积分，实际上第二三项是PI控制。
由此就得到了当前的角速度（物坐标系）

// 一阶龙格库塔法, 更新四元数
//矫正后的角速度值积分，得到两次姿态解算中四元数一个实部Q0，三个虚部Q1~3的值的变化
q0_t = (-NumQ.q1 * Gyro.x - NumQ.q2 * Gyro.y - NumQ.q3 * Gyro.z) * HalfTime;
q1_t = ( NumQ.q0 * Gyro.x - NumQ.q3 * Gyro.y + NumQ.q2 * Gyro.z) * HalfTime;
q2_t = ( NumQ.q3 * Gyro.x + NumQ.q0 * Gyro.y - NumQ.q1 * Gyro.z) * HalfTime;
q3_t = (-NumQ.q2 * Gyro.x + NumQ.q1 * Gyro.y + NumQ.q0 * Gyro.z) * HalfTime;

NumQ.q0 += q0_t; //积分后的值累加到上次的四元数中，即新的四元数
NumQ.q1 += q1_t;
NumQ.q2 += q2_t;
NumQ.q3 += q3_t;

利用角速度更新四元数。
什么意思呢，就是角速度（物坐标系）是角度（物坐标系）的微分，因此加到角度上可以得到新的角度，因为是离散情况。
而角度和四元数又存在对应，因此根据角度的增量是可以求出四元数的增量的，加到原来四元数上面，就有了新的四元数，代表了当前的姿态信息。

// 重新四元数归一化，得到单位向量下
NormQuat = Q_rsqrt(squa(NumQ.q0) + squa(NumQ.q1) + squa(NumQ.q2) + squa(NumQ.q3)); //得到四元数的模长
NumQ.q0 *= NormQuat; //模长更新四元数值
NumQ.q1 *= NormQuat;
NumQ.q2 *= NormQuat;
NumQ.q3 *= NormQuat;

加完之后可能不满足归一化条件，重新归一化

/*机体坐标系下的Z方向向量*/
float NormAccz = -pMpu->accX * vecxZ+ pMpu->accY * vecyZ + pMpu->accZ * veczZ;  /*Z轴垂直方向上的加速度，此值涵盖了倾斜时在Z轴角速度的向量和，不是单纯重力感应得出的值*/        
wz_acc_tmp[0] = (NormAccz - 8192) * 0.1196f;// cm/ss //0.1196f;厘米每二次方秒
wz_acc_tmp[1] += 0.1f *(wz_acc_tmp[0] - wz_acc_tmp[1]);//LPF
HeightInfo.Z_Acc = wz_acc_tmp[1];

最后这几句是跟Z方向的加速度有关的，虽然前面说过不考虑除重力加速度之外的加速度，但这个跟高度有关，所以可能用得到。。。虽然我还没看到那里。。。

}

然后下面这个GetAngle就很容易理解了：

void GetAngle(Attitude_t *pAngE)
{
    vecxZ = 2 * NumQ.q0 *NumQ.q2 - 2 * NumQ.q1 * NumQ.q3 ;/*矩阵(3,1)项*///地理坐标系下的X轴的重力分量
    vecyZ = 2 * NumQ.q2 *NumQ.q3 + 2 * NumQ.q0 * NumQ.q1;/*矩阵(3,2)项*///地理坐标系下的Y轴的重力分量
    veczZ = NumQ.q0 * NumQ.q0 - NumQ.q1 * NumQ.q1 - NumQ.q2 * NumQ.q2 + NumQ.q3 * NumQ.q3;  /*矩阵(3,3)项*///地理坐标系下的Z轴的重力分量 
    
    pAngE->yaw = atan2f((2 * NumQ.q1 *NumQ.q2 + 2 * NumQ.q0 * NumQ.q3),(1 - 2 * (NumQ.q2 * NumQ.q2 + NumQ.q3 * NumQ.q3))) * RtA;
    pAngE->pitch = asin(vecxZ)* RtA;   //俯仰角          
    pAngE->roll  = atan2f(vecyZ,veczZ) * RtA;  //横滚角
}

四元数换算成欧拉角。

至于根据欧拉角如何控制输出信号，还没看懂。。。下次写吧。估计要看pid和遥控器才能搞清楚。。。

然后还剩下高度控制，然后应该就没啥了。。。把代码看完再做个总结，一个比较典型的工程就算解析完成了，接下来还有一个匿名科创的无人机代码，复杂一些而且还没注释（orz），估计大同小异，花不了太多时间（但愿）。。。