四旋翼无人机的控制原理与实现

材料

无人机部分

飞控开发板：STM32F411CEU6 31.9元

无刷电机：2204-2300KV 正、反齿各两个 24.8 × 4元

电调：新西达30A（无校准） 30 * 4元

机架及尺寸：XL7碳纤维，臂长7英寸 129元

螺旋桨：APC 6*4 正反桨 8 × 2 + 9

陀螺仪：MPU6050 7元

通信模块：NRF24L01天线1100米 10.4元

电压模块：INA226 18元

气压计：GY-63 30元

3S电池：2200mAh + 2800mAh 62+54.9元

控制部分

控制芯片：STM32F103C8T6 17元

通信模块：NRF24L01天线1100米 10.4元

串口模块：USB—TTL

选型上的几个参数顺便说两句：2204是电机的定子尺寸，2300KV表示每伏电压对应2300转的空载转速，配3S电池（标称11.1V）算下来空载转速很高，配6寸桨正合适。电调的30A是持续电流上限，四个电机一起抽流，选型的时候得留点余量。F411CEU6主频100M还带FPU，跑姿态解算绰绰有余；遥控端用F103C8T6就足够了，反正只负责发指令、收状态。NRF24L01是2.4G的通信模块，INA226负责测电压电流，顺便做低电压报警，电池掉到阈值以下就该准备降落了；GY-63板上就是MS5611气压计，走IIC读气压和温度。螺旋桨标的6*4指的是直径6英寸、螺距4英寸，螺距越大每圈“切”进的空气越多，拉力和电流也跟着涨。

关键技术

关键技术主要是控制无人机的稳定飞行，按数据流来说就三块：先做姿态解算，把传感器数据变成飞机当前的姿态角；再用PID比较“现在是什么姿态”和“想要什么姿态”，算出四个电机该怎么出力；最后把控制量变成PWM丢给电调。至于PWM驱动电调这一层就不做探讨了，协议本身比较简单，会配寄存器就行。

姿态解算

姿态解算要做的事很简单：拿MPU6050出来的三轴加速度和角速度，算出机体相对于地面的姿态。难点在于两个传感器都不完美——加速度计静态下算出来的角度是准的，但飞机一抖就全是噪声；陀螺仪短时间很顺滑，但零偏积分起来角度会越漂越大。所以思路就是把两者融合，常用的手段有互补滤波、卡尔曼滤波，再就是下面要说的四元数表示。四元数本身不是滤波器，它解决的是姿态怎么表示、角速度怎么更新姿态的问题。

先说说两个传感器各自的本事：加速度计靠重力分量算角度，静止时横滚角就是$atan2(A_y,A_z)$，俯仰角是$atan2(-A_x,\sqrt{A_y^2+A_z^2})$，反三角函数一算就出来，代价是一有振动数据就全乱；陀螺仪把角速度乘时间积起来得到角度，短时间内非常稳，但零偏会随着时间攒出一份越来越大的误差。互补滤波就是把两者按频率互补一下，比如 angle = 0.98 * (angle + Gx * dt) + 0.02 * accAngle，高频段信陀螺、低频段信加速度计，写起来就几行代码，效果还不错。这里说的姿态，指的是机体坐标系相对地理坐标系的旋转，机头朝向、机身倾角都是相对地面来说的。

四元数法

欧拉角与四元数

表达姿态一般用欧拉角，分为俯仰角(Pitch)、横滚角(Roll)、偏航角(Yaw)，分别对应绕机体Y轴、X轴、Z轴转过的角度，直观好理解，遥控器上几个摇杆量最后对应的就是它们。旋转某一个角可以用一个旋转矩阵表示，也可以用e的矩阵次幂做泰勒展开来逼近，两种写法其实是一回事。

他们对应的三个旋转矩阵为：

我们把三个矩阵乘到一起，就可以用一个矩阵表达一个连续的变化了。这里要注意，因为矩阵乘法没有交换律，所以先后顺序对最终结果的影响很大，换一个旋转顺序得到的姿态就不一样。航空上比较常用的是先偏航、再俯仰、最后横滚的顺序，我们后面的解算也要保持这个约定，不然代码和文档对不上。

做无人机时经常会听到一个词汇叫“万向锁”，它的起因是当俯仰角接近±90°的时候，也就是机头快朝天或者朝地的时候，横滚和偏航的旋转轴在空间里重合了，丢了一个自由度，姿态就表征不出来了。对四旋翼来说虽然不用天天翻跟头，但计算过程中一旦靠近这个奇异点，数值就容易失控，所以内部解算并不适合直接用欧拉角。聪明的人们想到了用复数表达姿态，这里面用到了超复数，三个虚数单位分别为i、j、k，它们有这样的关系：

$$i^2=j^2=k^2=-1$$

以及交换变号的性质：

$$ij=k → ji=-k$$ $$jk=i→kj=-i$$ $$ki=j→ik=-j$$

最后我们在每一个复数单位前增加一个系数，最后增加一个常数，就得到了四元数：

$$Q=q_0+q_1i+q_2j+q_3k$$

其中它的二范数只计算系数，也就是$\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}$，用来做旋转的四元数必须是单位四元数，模长要等于1。关于计算的性质可以阅读下面链接的内容。同时我们的四元数还可以表达一次90度的旋转，这样就有了用四元数表达旋转的方法：绕某个轴转θ角，对应的四元数就是$\cos(\frac{\theta}{2})$加上该轴方向乘$\sin(\frac{\theta}{2})$，出现半角是因为旋转在矩阵层面要作用两次，这也是代码里那几个$\cos(\frac{angle}{2})$的来历。当然，既然是复数就可以用欧拉公式变成三角值。

https://www.zhihu.com/tardis/bd/art/97186723

至此，我们得到了用四元数表达角度的方法，不过实际用的时候还有一个坑：四元数经过多次积分之后模长会慢慢偏离1，所以每更新若干次就要归一化一次，不然算出来的姿态会越走越歪，这个后面代码里说。接下来我们看看如何求解角速度：

龙格库塔更新四元数

这个过程我们主要是求解$\frac{dQ}{dt}$，所以我们需要表达一下：

$$\frac{dQ}{dt}=-\frac{1}{2}sin(\frac{\theta}{2})\frac{d\theta}{dt}+\frac{1}{2}\vec C cos(\frac{\theta}{2})\frac{d\theta}{dt} + \frac{d\vec C}{dt}sin(\frac{\theta}{2})$$

其中的C为三个复数组成的向量，导数等于0，平方等于-1。$\theta$为四元数利用欧拉公式计算的角度，我们将其转化为角速度就得到：

$$\frac{dQ}{dt}=\frac{1}{2}\vec C \omega(cos(\frac{\theta}{2}) + \vec C sin(\frac{\theta}{2}))=\frac{1}{2}\omega Q$$

最后化简得到：

当然可以简单写作：

$$\frac{dQ}{dt}=\Omega_bQ$$

其中：

更新四元数的方法为：

$$Q_{t+\Delta t}=Q_t+\frac{\Delta t}{2}\Omega_bQ_t$$

这个式子做的事就是拿角速度对四元数做一阶积分，Δt是解算周期，代码里就是每个周期把上式右边算出来累加到四元数上。步长越小越准，解算频率上到几百赫兹时误差可以接受；如果频率比较低，可以换成四阶龙格库塔，多取几个点估斜率，精度会好不少。

代码

// 四元数
typedef struct {
    double q0;
    double q1;
    double q2;
    double q3;
} Quaternion_t;

// 姿态传感器数据
typedef struct {
	// 角度
    int16_t Accel_X_RAW;
    int16_t Accel_Y_RAW;
    int16_t Accel_Z_RAW;
    double Ax;
    double Ay;
    double Az;

    // 角速度
    int16_t Gyro_X_RAW;
    int16_t Gyro_Y_RAW;
    int16_t Gyro_Z_RAW;
    double Gx;
    double Gy;
    double Gz;

    // 温度
    float Temperature;

    //卡尔曼滤波后的角度
    double KalmanAngleX;
    double KalmanAngleY;
    double KalmanAngleZ;

    // 原始角度
    double NormalAngleX;
    double NormalAngleY;
    double NormalAngleZ;
} MPU6050_t;

void Quaternion_fromAxisAngle(Quaternion_t *Quat, MPU6050_t *data){
    double cosRoll = cos(data->KalmanAngleX / 2.0);
    double sinRoll = sin(data->KalmanAngleX / 2.0);
    double cosPitch = cos(data->KalmanAngleY / 2.0);
    double sinPitch = sin(data->KalmanAngleY / 2.0);
    double cosYaw = cos(data->NormalAngleZ / 2.0);
    double sinYaw = sin(data->NormalAngleZ / 2.0);
    Quat->q0 = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
    Quat->q1 = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
    Quat->q2 = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
    Quat->q3 = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
}

// 龙格库塔更新
void Quaternion_fromAngularVelocity(Quaternion_t *Quat, MPU6050_t *data, double dt){
    double p = data->Gx;
    double q = data->Gy;
    double r = data->Gz;
    Quat->q0 += dt * (0.5 * (-q * Quat->q1 - r * Quat->q2) - 0.5 * (Quat->q0 * p + Quat->q1 * q + Quat->q2 * r));
    Quat->q1 += dt * (0.5 * (p * Quat->q0 + r * Quat->q3) - 0.5 * (q * Quat->q0 + Quat->q2 * p + Quat->q3 * r));
    Quat->q2 += dt * (0.5 * (q * Quat->q3 - r * Quat->q0) - 0.5 * (Quat->q0 * r + Quat->q1 * p + Quat->q3 * p));
    Quat->q3 += dt * (0.5 * (q * Quat->q2 + r * Quat->q1) - 0.5 * (Quat->q0 * q + Quat->q1 * r + Quat->q2 * p));
}

代码里第一个函数是用当前的角度拼四元数：横滚和俯仰取的是卡尔曼滤波后的角度，偏航取的是没滤波的原始角度，yaw没有磁力计校准；第二个函数就是拿角速度积分更新四元数，式子看着长，其实就是把$\frac{dQ}{dt}=\Omega_bQ$逐项展开了。

每轮积分完记得归一化，就是上面说的模长发散的坑，补一个函数就行：

// 四元数归一化
void Quaternion_normalize(Quaternion_t *Quat){
    double norm = sqrt(Quat->q0 * Quat->q0 + Quat->q1 * Quat->q1 + Quat->q2 * Quat->q2 + Quat->q3 * Quat->q3);
    Quat->q0 /= norm;
    Quat->q1 /= norm;
    Quat->q2 /= norm;
    Quat->q3 /= norm;
}

另外MPU6050读出来的原始数据要先处理：减去静止时的零偏，再按量程换算成°/s和g，这个在读取函数里做，代码贴出来的部分默认拿到的已经是物理量了。

卡尔曼法

在我的代码中使用到的就是卡尔曼滤波法。这里的用法比通用卡尔曼简单，状态只有两维：角度和陀螺零偏。加速度计解算出的角度当观测，陀螺角速度减掉零偏之后积分当预测，卡尔曼增益会自动在两者之间分配权重，顺便把零偏也估计出来，不需要像互补滤波那样手工定系数。此法难度不大，但是过程较长，缺点是因为没有磁力计无法直接获取yaw值——加速度计只能感知重力方向，绕重力轴的旋转它看不出来，所以yaw只能靠陀螺积分硬撑，飞一会儿就慢慢漂走了，想要绝对航向得加磁力计做九轴融合。再补充一下两维卡尔曼为什么能顺便估零偏：把零偏也当成一个状态，让它每个周期随机地漂一点，观测角度里带着零偏造成的误差，修正角度的时候零偏的估计也就一起被更新了，所以陀螺的静态漂移会被慢慢吃掉。如果对卡尔曼滤波公式不了解的可以阅读我的往期博客：卡尔曼滤波https://blog.minloha.cn/2023/02/16/%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2/

---

公式：

$$1):\hat x_i^-=F\hat x_{i-1}+Bu_{i-1}$$ $$2):P_i^-=FP_{i-1}F^T+Q$$ $$3):K_i=P_i^-H^T(HP_i^-H^T+R)^{-1}$$ $$4):\hat x_i=\hat x_i^-+K_i(y_i-H\hat x_i^-)$$ $$5):P_i=(E-K_iH)P_i^-$$

公式转换为代码可以阅读下面的代码：

typedef struct {
    double Q_angle; // 噪声协方差
    double Q_bias; // 噪声偏差
    double R_measure; // 随机噪声
    double angle;
    double bias;
    double P[2][2];
} Kalman_t;


double Kalman_getAngle(Kalman_t *Kalman, double newAngle, double newRate, double dt) {
    // 公式1
    double rate = newRate - Kalman->bias;
    Kalman->angle += dt * rate;

    // 公式2
    Kalman->P[0][0] += dt * (dt * Kalman->P[1][1] - Kalman->P[0][1] - Kalman->P[1][0] + Kalman->Q_angle);
    Kalman->P[0][1] -= dt * Kalman->P[1][1];
    Kalman->P[1][0] -= dt * Kalman->P[1][1];
    Kalman->P[1][1] += Kalman->Q_bias * dt;

    // 公式3
    double S = Kalman->P[0][0] + Kalman->R_measure;
    double K[2];
    K[0] = Kalman->P[0][0] / S;
    K[1] = Kalman->P[1][0] / S;

    // 公式4
    double y = newAngle - Kalman->angle;
    Kalman->angle += K[0] * y;
    Kalman->bias += K[1] * y;

    // 公式5
    double P00_temp = Kalman->P[0][0];
    double P01_temp = Kalman->P[0][1];

    Kalman->P[0][0] -= K[0] * P00_temp;
    Kalman->P[0][1] -= K[0] * P01_temp;
    Kalman->P[1][0] -= K[1] * P00_temp;
    Kalman->P[1][1] -= K[1] * P01_temp;
	// 结束，返回角度
    return Kalman->angle;
}

代码和公式是对应的：公式1对应 angle += dt * rate 那一行，公式2的P矩阵递推因为状态只有两维，展开写就是公式2注释下面那四行。调用Kalman_getAngle的时候，newAngle传加速度计算出来的角度，newRate传陀螺的角速度，dt就是调用周期，三个轴各跑一个滤波器就行。对于卡尔曼调参，这里给出一段python算法，只需要通过串口获取角度，保存到文件即可，然后使用下面的代码，让蓝色曲线更加平滑即可。调参主要盯三个量：Q_angle越大越信观测、曲线跟红线跟得紧；R_measure越大越信预测、曲线更平但会有滞后；Q_bias是留给陀螺零偏漂移的余量，一般给个小值。

import matplotlib.pyplot as plt
normal_degree = []
acc = []

with open('test.txt', 'r') as file:
    lines = file.readlines()
    for st in lines:
        sp = st.split(",")
        normal_degree.append(float(sp[0]))
        acc.append(float(sp[1]))


Q_angle = 0.2
Q_bias = 0.4
R_measure = 0.4
dt = 0.001


global angle
global bias
global P
global S
global K

def Kalman(newAngle, newRate, dt):
    global Q_angle
    global Q_bias
    global R_measure
    global angle
    global bias
    global P
    global S
    global K
    rate = newRate - bias
    angle += dt * rate
    P[0][0] += dt * (dt * P[1][1] - P[0][1] - P[1][0] + Q_angle)
    P[0][1] -= dt * P[1][1]
    P[1][0] -= dt * P[1][1]
    P[1][1] += Q_bias * dt
    S = P[0][0] + R_measure
    K[0] = P[0][0] / S
    K[1] = P[1][0] / S
    y = newAngle - angle
    angle += K[0] * y
    bias += K[1] * y
    P[0][0] -= K[0] * P[0][0]
    P[0][1] -= K[0] * P[0][1]
    P[1][0] -= K[1] * P[0][0]
    P[1][1] -= K[1] * P[0][1]
    return angle


if __name__ == "__main__":
    for i in range(len(normal_degree)):
        normal_degree[i] = float(normal_degree[i])
        pass
    angle = 0
    bias = 0
    P = [[0, 0], [0, 0]]
    S = 0
    K = [0, 0]
    list_ex = []
    list_x = []
    y = []
    lim = 20
    for i in range(lim):
        list_ex.append(Kalman(normal_degree[i], acc[i], dt))
        list_x.append(normal_degree[i])
        pass
    
    x = [i for i in range(lim)]
    y = [0 for i in range(lim)]
    plt.plot(x, list_x, color="red")
    plt.plot(x, list_ex, color="blue")
    plt.plot(x, y, color="black")
    plt.legend(['Real', 'Excepted','Zero'])
    plt.show()
    pass

效果展示图：

PID算法

提及控制算法，就不得不提到PID算法，这是一种闭环控制算法，输入参数只需要实际值和期望值即可，它共有两种格式：增量式和位置式。三个环节用大白话说：P是当前差多少就补多少，I是把历史上的欠账攒起来慢慢补，D是看误差变化的趋势提前收手，防止冲过头。

$$位置式: o(k)=K_pe(k)+K_i\sum_{i=0}e(i)+K_d[e(k)-e(k-1)]$$

其中e为实际值与期望值的差，Kp，Ki，Kd为手工调节的参数。

$$增量式：\Delta o(k)=K_p[e(k)-e(k-1)]+K_ie(k)+K_d[e(k)-2e(k-1)+e(k-2)]$$

一般都是使用位置式，输出直接就是执行器的量，四旋翼上每个控制环的输出最后都落到PWM或者期望角速度上，用位置式更直观。相关代码如下：

typedef struct{
    double P;
    double I;
    double D;
    double I_threshold;

    double Pout;
    double Iout;
    double Dout;

    double finnalOUT;
    double Error;
    double lastError;
    double integral;
} PID_t;

#define limit(value,max) if(value > max) value = max;

double PID_Run(PID_t *PID,double target,double measure){
    PID->Error = target - measure;
    PID->integral += PID->Error;

    PID->Pout = PID->P * PID->Error;
    PID->integral += PID->Error;
    PID->Iout = -PID->I * PID->integral;
    PID->Dout = PID->D * (PID->Error - PID->lastError);
    limit(PID->integral,PID->I_threshold);
    PID->lastError = PID->Error;
    PID->finnalOUT = PID->Pout + PID->Iout + PID->Dout;
    return PID->finnalOUT;
}

几个容易踩的坑说一下：积分项是一直累加的，误差大或者执行器到不了目标的时候积分会越滚越大，所以要拿I_threshold限幅，这就是所谓的抗积分饱和；另外这里积分项前面带了个负号，输出方向和期望值、测量值的定义方向有关，上电前一定要确认正方向，不然错误方向的反馈叠加起来就是正反馈，飞机直接就掀了。还有就是PID_Run要放在定时器中断里按固定周期调用，周期飘了，调好的参数就全白搭。误差很小时PWM会在零附近来回抖，可以给误差设个死区，小误差直接当零；输出也别忘了限幅，别让PID算出一个超出PWM范围的数。

相关的流程图如下：

定高

我将定高放在PID后面是为了综合上面所有内容。这里我选择的获取高度的模块是气压计MS5611，它是IIC总线读取压强和温度的模块，通过气压换算就能得到当前的高度，公式是$h=44330(1-(\frac{P}{P_0})^{1/5.255})$，P是当前气压，P0一般取海平面标准大气压101325Pa。气压换算出来的高度受气流和温度影响很大，尤其是飞机自己螺旋桨吹出来的风，所以实际用的时候高度数据要先滤波，最好再配合加速度计融合一下。定高肯定需要有期望高度和实际高度，我们就可用PID控制输出的PWM——高度差进PID，输出叠加到基础油门上，飞机自己就会往上或者往下找补。定高环一般用P和D就够，I放在高度上容易积出麻烦；如果飞机总是在目标高度上下点头，多半是P给大了，压一点、补一点D，曲线就圆滑了。

对于姿态稳定，我们可以把卡尔曼滤波得到的姿态角作为外环、角速度作为内环分别做PID，然后输出PWM，也就是串级PID。串级的好处是内环响应快，专门对付突发扰动，外环负责慢慢把姿态修到目标值，两级各司其职，参数也分开调，不会互相打架。

展示一下PID调参后的输出值。调参的顺序一般是从内环开始：先把角速度环的P加到能快速响应又不震荡，再补一点D压住超调，然后才是外环角度环，最后才动积分项；定高环放最后调，因为它最容易受气流干扰，参数也最保守。

总结

本次做一个四旋翼单纯只是玩一玩，展示一下图片后，本期博客就结束了。关键技术点和代码都在上面，想玩的话照着采购一套物资，就能攒出一架自己的无人机。