2 基于双闭环PID控制的四轴飞行器_姿态解算

基于双闭环PID控制的四轴飞行器

2 姿态解算

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

图2 姿态解算结构图

由于传感器存在器件误差，因此在使用前需要标定。陀螺仪在静止时，角速度为0；但实际情况由于器件误差并不为0，因此可在静止时采样500次数据，

再求平均，得出偏移量，标定陀螺仪数据；加速度传感器可依据在静止时，三轴重力加速度平方和的开方为重力加速度的标定方程，利用最小二乘法求出标定偏移值和误差值进行标定。而磁力传感器校正，可将器件静置于桌面旋转一周找出最小值和最大值，通过电子罗盘校正计算公式计算出标定偏移值和误差值。

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

式(1)中：ex、ey、ez为两叉积之和，ax、ay、az为加速度的测量矢量，mx、my、mz为磁场的测量矢量，axref、ayref、azref为加速度的参考矢量，mxref、myref、mzref为磁场的参考矢量，参考矢量是通过实时四元数值与本次测量值计算出来。

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

式(2)中ωx(t)、ωy(t)、ωz(t)为角速度，kpex(t)为比例项修正，

为积分修正项。

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