四轴飞行器的姿态算法
四轴飞行器能够做垂直运动，俯仰运动，滚转运动，偏航运动。
四轴飞行器运用姿态解算计算出空间三轴欧拉角。结构框架如图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)。
3 高度计算
高度计算是通过气压传感器采集的大气压值计算出来，将气压传感器采集值进行校正后，在通过温度二阶补偿，得到准确的大气压值，最后经过气压转换为高度公式
式(6)中Altitude为计算出来的实际高度，CurrentPressure为当前气压值，StartPressure为起飞之前气压值。气压传感器选用MS5611芯片，其中集成了温度传感器和气压传感器，采用IIC总线与主控板通信。
举报
