四轴飞行器飞行原理与双闭环PID控制

四轴飞行器 是微型飞行器的其中一种，相对于固定翼飞行器，它的方向控制灵活、抗干扰能力强、飞行稳定，能够携带一定的负载和有悬停功能，因此能够很好地进行空中拍摄、监视、侦查等功能，在军事和民用上具备广泛的运用前景。

四轴飞行器关键技术在于控制策略。由于智能控制算法在运行复杂的浮点型运算以及矩阵运算时，微处理器计算能力受限，难以达到飞行控制实时性的要求；而PID控制简单，易于实现，且技术成熟，因此目前主流的控制策略主要是围绕传统的PID控制展开。

1 四轴飞行器的结构与基本飞行原理

四轴飞行器结构主要由主控板和呈十字交叉结构的4个电子调速器、电机、旋浆组成，电机由电子调速器控制，主控板主要负责解算当前飞行姿态、控制电调等功能。

以十字飞行模式为例，l号旋翼为头，1、3号旋翼逆时针旋转，2、4号旋翼顺时针旋转，如图1所示。

图1 四轴飞行器结构图

参照飞行状态表1变化电机转速，由于四个电机转速不同，使其与水平面倾斜一定角度，如图l所示。四个电机产生的合力分解为向上的升力与前向分力。当重力与升力相等时，前向分力驱动四轴飞行器向倾斜角度的方向水平飞行。空间三轴角度欧拉角分为仰俯角、横滚角、航向角：倾斜角是仰俯角时，向前、向后飞行；倾斜角是横滚角时，向左、向右飞行；而倾斜航向角时，向左、右旋转运动，左(右)旋转是由于顺时针两电机产生的反扭矩之和与逆时针两电机产生的反扭矩之和不等，即不能相互抵消，机身便在反扭矩作用下绕z轴自旋转。

2 姿态解算

四轴飞行器运用姿态解算计算出空间三轴欧拉角。结构框架如图2所示，陀螺仪采样三轴角速度值，加速度传感器采样三轴加速度值，而磁力传感器采样得到三轴地磁场值，将陀螺仪、加速度传感器、磁力传感器采样后的数据进行标定、滤波、校正后得到三轴欧拉角度，其中陀螺仪和加速度传感器选用MPU6050芯片，磁力传感器选用HMC5883L芯片，采用IIC总线与主控板通信。

图2 姿态解算结构图

由于传感器存在器件误差，因此在使用前需要标定。陀螺仪在静止时，角速度为0；但实际情况由于器件误差并不为0，因此可在静止时采样500次数据，再求平均，得出偏移量，标定陀螺仪数据；加速度传感器可依据在静止时，三轴重力加速度平方和的开方为重力加速度的标定方程，利用最小二乘法求出标定偏移值和误差值进行标定。而磁力传感器校正，可将器件静置于桌面旋转一周找出最小值和最大值，通过电子罗盘校正计算公式计算出标定偏移值和误差值。

由于陀螺仪长时间采集角速度会有飘移，因此需要使用加速度传感器的值与磁力传感器的值进行校正。将加速度的测量矢量和磁场的测量矢量与参考矢量做叉积后相加

图2 姿态解算结构图

由于传感器存在器件误差，因此在使用前需要标定。陀螺仪在静止时，角速度为0；但实际情况由于器件误差并不为0，因此可在静止时采样500次数据，再求平均，得出偏移量，标定陀螺仪数据；加速度传感器可依据在静止时，三轴重力加速度平方和的开方为重力加速度的标定方程，利用最小二乘法求出标定偏移值和误差值进行标定。而磁力传感器校正，可将器件静置于桌面旋转一周找出最小值和最大值，通过电子罗盘校正计算公式计算出标定偏移值和误差值。

由于陀螺仪长时间采集角速度会有飘移，因此需要使用加速度传感器的值与磁力传感器的值进行校正。将加速度的测量矢量和磁场的测量矢量与参考矢量做叉积后相加

式(1)中：e_x、e_y、e_z为两叉积之和，a_x、a_y、a_z为加速度的测量矢量，m_x、m_y、m_z为磁场的测量矢量，a_xref、a_yref、a_zref为加速度的参考矢量，m_xref、m_yref、m_zref为磁场的参考矢量，参考矢量是通过实时四元数值与本次测量值计算出来。

图2 姿态解算结构图

由于传感器存在器件误差，因此在使用前需要标定。陀螺仪在静止时，角速度为0；但实际情况由于器件误差并不为0，因此可在静止时采样500次数据，再求平均，得出偏移量，标定陀螺仪数据；加速度传感器可依据在静止时，三轴重力加速度平方和的开方为重力加速度的标定方程，利用最小二乘法求出标定偏移值和误差值进行标定。而磁力传感器校正，可将器件静置于桌面旋转一周找出最小值和最大值，通过电子罗盘校正计算公式计算出标定偏移值和误差值。

由于陀螺仪长时间采集角速度会有飘移，因此需要使用加速度传感器的值与磁力传感器的值进行校正。将加速度的测量矢量和磁场的测量矢量与参考矢量做叉积后相加

式(1)中：e_x、e_y、e_z为两叉积之和，a_x、a_y、a_z为加速度的测量矢量，m_x、m_y、m_z为磁场的测量矢量，a_xref、a_yref、a_zref为加速度的参考矢量，m_xref、m_yref、m_zref为磁场的参考矢量，参考矢量是通过实时四元数值与本次测量值计算出来。

再将叉积修正角速度漂移值：

式(2)中ω_x(t)、ω_y(t)、ω_z(t)为角速度，k_pe_x(t)为比例项修正，

为积分修正项。

将校正后的角速度通过二阶毕卡算法转化为四元数，公式如式(3)。

图2 姿态解算结构图

由于传感器存在器件误差，因此在使用前需要标定。陀螺仪在静止时，角速度为0；但实际情况由于器件误差并不为0，因此可在静止时采样500次数据，再求平均，得出偏移量，标定陀螺仪数据；加速度传感器可依据在静止时，三轴重力加速度平方和的开方为重力加速度的标定方程，利用最小二乘法求出标定偏移值和误差值进行标定。而磁力传感器校正，可将器件静置于桌面旋转一周找出最小值和最大值，通过电子罗盘校正计算公式计算出标定偏移值和误差值。

由于陀螺仪长时间采集角速度会有飘移，因此需要使用加速度传感器的值与磁力传感器的值进行校正。将加速度的测量矢量和磁场的测量矢量与参考矢量做叉积后相加

式(1)中：e_x、e_y、e_z为两叉积之和，a_x、a_y、a_z为加速度的测量矢量，m_x、m_y、m_z为磁场的测量矢量，a_xref、a_yref、a_zref为加速度的参考矢量，m_xref、m_yref、m_zref为磁场的参考矢量，参考矢量是通过实时四元数值与本次测量值计算出来。

3 高度计算

高度计算是通过气压传感器采集的大气压值计算出来，将气压传感器采集值进行校正后，在通过温度二阶补偿，得到准确的大气压值，最后经过气压转换为高度公式

式(6)中Altitude为计算出来的实际高度，CurrentPressure为当前气压值，StartPressure为起飞之前气压值。气压传感器选用MS5611芯片，其中集成了温度传感器和气压传感器，采用IIC总线与主控板通信。

图2 姿态解算结构图

由于传感器存在器件误差，因此在使用前需要标定。陀螺仪在静止时，角速度为0；但实际情况由于器件误差并不为0，因此可在静止时采样500次数据，再求平均，得出偏移量，标定陀螺仪数据；加速度传感器可依据在静止时，三轴重力加速度平方和的开方为重力加速度的标定方程，利用最小二乘法求出标定偏移值和误差值进行标定。而磁力传感器校正，可将器件静置于桌面旋转一周找出最小值和最大值，通过电子罗盘校正计算公式计算出标定偏移值和误差值。

由于陀螺仪长时间采集角速度会有飘移，因此需要使用加速度传感器的值与磁力传感器的值进行校正。将加速度的测量矢量和磁场的测量矢量与参考矢量做叉积后相加

式(1)中：e_x、e_y、e_z为两叉积之和，a_x、a_y、a_z为加速度的测量矢量，m_x、m_y、m_z为磁场的测量矢量，a_xref、a_yref、a_zref为加速度的参考矢量，m_xref、m_yref、m_zref为磁场的参考矢量，参考矢量是通过实时四元数值与本次测量值计算出来。

4 PID控制

4．1 双闭环PID控制

当四轴飞行器正常飞行时，突遇外力(风等)或磁场干扰，使加速度传感器或磁力传感器采集数据失真，造成姿态解算出来的欧拉角错误，只用角度单环情况下，使系统很难稳定运行，因此可以加入角速度作为内环，角速度由陀螺仪采集数据输出，采集值一般不存在受外界影响情况，抗干扰能力强，并且角速度变化灵敏，当受外界干扰时，回复迅速；同理，高度环中气压传感器同样也会受到外界干扰，引入z轴加速度环可有效避免外界干扰造成的影响，增强了系统的鲁棒性。

四轴飞行器双闭环PID控制，如图3、图4所示。角度作为外环，角速度作为内环，进行姿态PID控制；当需要定高时，高度作为外环，z轴加速度作为内环，进行高度PID控制。其中，PID输出为油门值，油门给定电子调速器值，电子调速器控制电机使空间三轴欧拉角和高度变化

图2 姿态解算结构图

由于传感器存在器件误差，因此在使用前需要标定。陀螺仪在静止时，角速度为0；但实际情况由于器件误差并不为0，因此可在静止时采样500次数据，再求平均，得出偏移量，标定陀螺仪数据；加速度传感器可依据在静止时，三轴重力加速度平方和的开方为重力加速度的标定方程，利用最小二乘法求出标定偏移值和误差值进行标定。而磁力传感器校正，可将器件静置于桌面旋转一周找出最小值和最大值，通过电子罗盘校正计算公式计算出标定偏移值和误差值。

由于陀螺仪长时间采集角速度会有飘移，因此需要使用加速度传感器的值与磁力传感器的值进行校正。将加速度的测量矢量和磁场的测量矢量与参考矢量做叉积后相加

式(1)中：e_x、e_y、e_z为两叉积之和，a_x、a_y、a_z为加速度的测量矢量，m_x、m_y、m_z为磁场的测量矢量，a_xref、a_yref、a_zref为加速度的参考矢量，m_xref、m_yref、m_zref为磁场的参考矢量，参考矢量是通过实时四元数值与本次测量值计算出来。

图3姿态PID控制总体流程图

图4高度PID控制总体流程图

PID控制算法采用位置式数字PID控制：

图2 姿态解算结构图

由于传感器存在器件误差，因此在使用前需要标定。陀螺仪在静止时，角速度为0；但实际情况由于器件误差并不为0，因此可在静止时采样500次数据，再求平均，得出偏移量，标定陀螺仪数据；加速度传感器可依据在静止时，三轴重力加速度平方和的开方为重力加速度的标定方程，利用最小二乘法求出标定偏移值和误差值进行标定。而磁力传感器校正，可将器件静置于桌面旋转一周找出最小值和最大值，通过电子罗盘校正计算公式计算出标定偏移值和误差值。

由于陀螺仪长时间采集角速度会有飘移，因此需要使用加速度传感器的值与磁力传感器的值进行校正。将加速度的测量矢量和磁场的测量矢量与参考矢量做叉积后相加

式(1)中：e_x、e_y、e_z为两叉积之和，a_x、a_y、a_z为加速度的测量矢量，m_x、m_y、m_z为磁场的测量矢量，a_xref、a_yref、a_zref为加速度的参考矢量，m_xref、m_yref、m_zref为磁场的参考矢量，参考矢量是通过实时四元数值与本次测量值计算出来。

图3姿态PID控制总体流程图

图4高度PID控制总体流程图

PID控制算法采用位置式数字PID控制：

式(7)中u(t)为PID输出值，e(t)为期望值与实际值之差，为积分量，为微分量，k_p，、k_i、k_d。为比例、积分、微分系数。

在将积分量，微分量离散化得到PID计算公式

式(8)中T为更新时间。

基于公式(8)，姿态PID控制算法