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

基于双闭环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 。