关于实现四轴无人机的姿态解算

1.无人机起飞基础所需

我这个四轴无人机是基于STM32所设计的，陀螺仪使用的MPU6050,远程控制通信用的NRF24L01。想要让自己的无人机能起飞，还要再空中保持稳定，所需要有什么呢。
我这边总结了一下：

1.1硬件设计：

整体架构，飞控、电池、电机、螺旋桨等选择。

1.2软件设计：

陀螺仪姿态解算，能随时掌握无人机的状态，辅助电机操控保持无人机平稳。
PID计算控制电机，调整电机输出让无人机按照自己的指令飞行。
此篇也主要是讲解无人机飞行中的软件设计部分，这边也只会讲解STM32联动MPU6050如何控制电机。

2.软件设计

该部分中关于头文件、引用这些都没有写进去，只写了主体部分，记得自己加一下。有计算的部分也记得“include一下math.h和string.h文件。

2.1姿态解算

对于陀螺仪的姿态解算，计算量小、代码简单、实时性强的二阶互补滤波再将加速度分量转化为弧度/秒后采用的如下公式：

但二阶互补滤波是属于单轴独立运算，欧拉角存在万向锁（俯仰接近±90°失效） ，二阶结构抑制漂移并不是非常稳定。所以对于无人机的姿态解算我这边就采用了精度更高、更稳定的滤波。卡尔曼滤波、Madgwick滤波还有Mahony滤波就在我的考虑范围了。这边我采用的是Mahony四元互补滤波，主要是讲软件设计，所以对于该滤波方式只会大概讲一下。主要是通过四元数在三维空间的向量计算方式，来掌握无人机的姿态，简单理解，就是四元函数基于复数的形式在三维空间把它的运动向量的结果计算出来。我的理解是先定义一个轴，然后将物体在三维空间三个旋转的自由度进行计算。如果想要详细了解，可以去小破站搜索一下别人的讲解视频。那么也不多说，直接上软件代码。
首先先看一下MPU6050的配置部分：

void MPU6050_Init(void)
{
    MyI2C_Init();
    MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x80);  // 软复位
    Delay_ms(100);
    MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x00);  // 唤醒
    Delay_ms(100);
    
    MPU6050_WriteReg(MPU6050_PWR_MGMT_2, 0x00);
    MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x07);
    MPU6050_WriteReg(MPU6050_CONFIG, 0x00); //调整陀螺仪自带的滤波器，关闭低通滤波降低延迟
    MPU6050_WriteReg(MPU6050_GYRO_CONFIG, 0x18);//±2000°/S
    MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0x18);//±16g
}

MPU6050_WriteReg(MPU6050_CONFIG, 0x00); //调整陀螺仪自带的滤波器，关闭低通滤波降低延迟我这里是关闭了陀螺仪自带的低通滤波的，但是关闭之后陀螺仪会因为数据毛刺过多，姿态解算误差频繁跳动，导致PID输出抖动，从而影响飞行。那么，关于陀螺仪本身的滤波怎么选择呢，这里有三档选择：
DLPF_CFG=0  0x00  8000 Hz 0.64 ms 关闭滤波，无延迟、噪声最大 
DLPF_CFG=1  0x01  188 Hz 2.0 ms 高速机动、特技飞行，延迟很小 
DLPF_CFG=2  0x02  98 Hz 3.0 ms 通用均衡档（首推），降噪+响应兼顾 
DLPF_CFG=3  0x03  42 Hz 4.9 ms 悬停、航拍、震动大的机架，降噪强
这边我就修改为（0X02）了。
然后还有MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x07);这个寄存器配置，先看一下手册，我这是翻译后的部分：

消费级小四轴（8520空心杯、室内穿越机）：
 陀螺仪推荐采样：200~500Hz，PID控制周期尽量和陀螺仪采样同步，做到闭环同步控制。
 常规航拍无人机、中型机架：
 主流配置：500~1000Hz，是兼顾算力与稳定性的黄金区间。
 专业竞速穿越机：
 最高常用1kHz、2kHz，搭配高性能主控、硬件滤波，依靠高采样实现极致操控跟手性。
因此我选择500HZ的采样率，改为MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x01);
还有呢MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0x18);//±16g。这个位置，我修改为0x00对应±2g。因为使用0x18的±16g分辨率太小了，2g精度更高，所以就改为2g（0x00）。
因此修改后总体为：

void MPU6050_Init(void)
{
    MyI2C_Init();
    MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x80);  // 软复位
    Delay_ms(100);
    MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x00);  // 唤醒
    Delay_ms(100);
    
    MPU6050_WriteReg(MPU6050_PWR_MGMT_2, 0x00);
    MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x01);//在使能陀螺仪滤波器时，陀螺仪输出频率为1KHZ.最后采样率：1000/（1+1）=500hz
    MPU6050_WriteReg(MPU6050_CONFIG, 0x02); //调整陀螺仪自带的滤波器，42HZ，输出延迟4.8ms
    MPU6050_WriteReg(MPU6050_GYRO_CONFIG, 0x18);//±2000°/S
    MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0x00);//±2g
}

配置完MPU6050之后，再编写姿态解算部分，Mahony姿态解算的公式网上讲解的很多，也不需要太过于深入理解，直接套公式拿过来用就行，我这边是先进行了一个MPU6050的校准，所以在程序一开始记得调用一下，然后姿态解算可以在中断调用就行：

//先宏定义需要的参数
#define Gyro_Gr 0.001065f   // rad/LSB 姿态解算使用    2000/32768*Π/180
#define Gyro_G  0.061035f   // °/LSB 角速度、Yaw积分使用  2000/32768
#define RAD_TO_ANG   57.2958f    // 弧度转角度,180/Π
#define LOOP_DT      0.002f      // 匹配2ms定时中断,与陀螺仪采样率对应
//定义一下姿态解算所需要的结构体成员
typedef struct {
    float x;
    float y;
    float z;
}Vector3f;

typedef volatile struct {
    float q0;
    float q1;
    float q2;
    float q3;
} Quaternion;

static Quaternion NumQ = {1,0,0,0};
static Vector3f GyroIntegError={0};
// Mahony 姿态融合参数
static float KpDef = 0.45f;  //值越大响应越快
static float KiDef = 0.00012f;

// ==================== MPU6050零偏校准 ====================
void MPU_Calibrate_Gyro(void)
{
    Delay_ms(1500);  //静置无人机
    int32_t gx_sum=0,gy_sum=0,gz_sum=0;
	int32_t ax_sum=0,ay_sum=0,az_sum=0;
    for(uint16_t i=0;i<400;i++)      /*次数和下面求平均数要一致，可以自行掂量选择，数值越大，校准时间越长*/
    {
        MPU6050_GetData(&AX,&AY,&AZ,&GX,&GY,&GZ);
        gx_sum += GX; gy_sum += GY; gz_sum += GZ;
		ax_sum += AX; ay_sum += AY; az_sum += AZ;
    }
    gx_off = gx_sum / 400;
    gy_off = gy_sum / 400;
    gz_off = gz_sum / 400;

	ax_off = ax_sum / 400;
    ay_off = ay_sum / 400;
    az_off = az_sum / 400 - 16384; 
 /*扣除1g重力，对应MPU里面的ACCEL_CONFIG的值。
0x00 ±2g为16384 LSB/g
0x08 ±4g为8192 LSB/g
0x10 ±8g为4096 LSB/g
0x18 ±18g为2048 LSB/g
*/
}


// ==================== 姿态解算 Mahony 算法 ====================
void GetAngle(int16_t accX, int16_t accY, int16_t accZ, int16_t gyroX,int16_t gyroY, int16_t gyroZ, float dt)
{
	accX -=ax_off;accY -=ay_off;accZ -=az_off;
    Vector3f Gravity,Acc,Gyro,AccGravity;
    float q0_t,q1_t,q2_t,q3_t;
    float NormQuat;
    float HalfTime = dt * 0.5f;

    // 从四元数提取重力向量
    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);

    // 加速度归一化
    NormQuat = Q_rsqrt(squa((float)accX)+squa((float)accY)+squa((float)accZ));
    Acc.x = (float)accX * NormQuat;
    Acc.y = (float)accY * NormQuat;
    Acc.z = (float)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 = (float)gyroX * Gyro_Gr + KpDef*AccGravity.x + GyroIntegError.x;
    Gyro.y = (float)gyroY * Gyro_Gr + KpDef*AccGravity.y + GyroIntegError.y;
    Gyro.z = (float)gyroZ * Gyro_Gr + KpDef*AccGravity.z + GyroIntegError.z;

    // 一阶龙格库塔更新四元数
    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;

    // 四元数转欧拉角
    float vecxZ = 2*NumQ.q0*NumQ.q2 - 2*NumQ.q1*NumQ.q3;
    float vecyZ = 2*NumQ.q2*NumQ.q3 + 2*NumQ.q0*NumQ.q1;
    float veczZ = 1 - 2*NumQ.q1*NumQ.q1 - 2*NumQ.q2*NumQ.q2;

    // Yaw航向积分 ，限制Yaw的零飘+角度限幅，防止卡死/溢出
    float yaw_G = (float)gyroZ * Gyro_G;
    if(yaw_G > 1.2f || yaw_G < -1.2f)
    {
        Yaw += yaw_G * dt;
    }
    if(Yaw > 180.0f)  Yaw -= 360.0f;
    if(Yaw < -180.0f) Yaw += 360.0f;

    Pitch = asin(vecxZ) * RAD_TO_ANG;
    Roll  = atan2f(vecyZ,veczZ) * RAD_TO_ANG;

}

2.2PID计算

比较难的算法部分解决之后，就开始编写PID部分：
 

//还是一样，先定义一下需要的结构体
typedef struct
{
    float Kp;
    float Ki;
    float Kd;
    float err;
    float err_last;
    float integral;
}PID_TypeDef;

PID_TypeDef PID_Roll;
PID_TypeDef PID_Pitch;
PID_TypeDef PID_Yaw;

float PID_Calc(PID_TypeDef *pid,float target,float now)
{
    pid->err = target - now;
    float p = pid->Kp * pid->err;
    pid->integral += pid->err;
    if(pid->integral > 25)  pid->integral = 25; //输出限幅，自行调整
    if(pid->integral < -25) pid->integral = -25;
    float i = pid->Ki * pid->integral;
    float d = pid->Kd * (pid->err - pid->err_last);
    pid->err_last = pid->err;
    return p + i + d;
}

// ==================== PID参数初始化 ====================
/*此PID参数不用作参考，我是通过结构体的形式将各个参数通过NRF把数据包发给无人机进行远程调参的，所以这些数据连参考的价值都没有*/
void PID_Init(void)
{
    PID_Roll.Kp=3.5f;   PID_Roll.Ki=0.022f; PID_Roll.Kd=0.12f;
    PID_Pitch.Kp=3.2f;  PID_Pitch.Ki=0.022f; PID_Pitch.Kd=0.12f;
    PID_Yaw.Kp=0.5f;    PID_Yaw.Ki=0.0007f; PID_Yaw.Kd=0.012f;
}

PID部分编写好之后就只需要在电机控制的函数里面直接调用
Out_Roll  = PID_Calc(&PID_Roll,  0, Roll);
Out_Pitch = PID_Calc(&PID_Pitch, 0, Pitch);
Out_Yaw   = PID_Calc(&PID_Yaw,   0, Yaw);
然后再通过自己的混控公式计算各个电机的输出值就行了。
这样看来，通过STM32来控制无人机的软件程序设计还是挺简单的是吧，但是这样可是还没有结束的，最重要的PID调参也是需要时间的，更别说如果要加入其他模块比如摄像头来航拍什么的，这些也都能给软件留有很大的提升空间的,再比如还可以将其他的算法也加入到软件里面，想用哪个算法就调用哪个算法的值，这样设计出新的无人机就能直接用对吧。

3.总结

以上就是我对于无人机姿态解算部分的一些理解和代码编写思路，谢谢大家的观看。