即零偏，因此在每一次系统初始化时需要对其零偏进行计算 并在之后的计算中除去零偏。先将陀螺仪静置，连续读取200次陀螺仪的测量值进行均值 滤波，并将其作为陀螺仪的零偏。
[0084] (3)磁力计偏心矫正及倾斜补偿
[0085] 理论上，磁力计的三个轴的测量值范围是对称的，即在空间中呈球体分布。然而， 由于传感器的误差以及外界磁场的干扰，本文所得到的磁力计测量值是一个中心不在原点 的椭球体，因此需要对其测量值进行偏心校正。基本步骤为：
[0086] a.将磁力计水平旋转360度计算其X轴与Y轴最大值与最小值，由此计算修正增 益&与Ys，修正偏置Xb与Y b
[0087] 这里以X轴为基准，令Xs= 1。
[0090] b.Xin、Yin和Zin为磁力计的测量输出值，通过修正增益与修正偏置计算磁力计的修 正值 UPYQUt:
[0092] c.同样以X轴为基准，对Z轴进行矫正，最终得到三个轴的修正值Z wt:
[0094] 步骤3)设计基于毕卡四元数的四旋翼飞行器姿态获取，过程包括：
[0095] 设由四旋翼的机体轴确定的坐标系为b，地面坐标系为n，则由b系到n系的坐标 矩阵G称为四旋翼的姿态矩阵，0、Y、p分别为飞机的俯仰、横滚、偏航角，加速度计输出 的原始值为速度的增量即abx、aby、abz，陀螺仪计输出为角增量即g bx、gby、gbz，磁力计mbx、mby、 ^bz°
[0096] (1)欧拉角初始化
[0098] (2)四元数初始值的确定及规范化处理
[0099]四旋翼飞行器的四元数的初始值Q(0)由捷联系统的初值状态对准确定。设初始 对准的姿态阵为〇可由式（1)中求得的公式计算得出初始姿态角求得。
[0101] 其中，cn、c12、c13、c21、c 22、c23、c31、c32、c33分别为
[0102]
[0103] 则四旋翼飞行器的四元数的初始值为：
[0105] 其中，qp q2、的数值和符号可按下式（5)和（6)确定。
[0108] 上式中，sign(qQ)可任选，因为四元数表征刚体从初始位置到最终位置的一次性 等效旋转，并不反应从初始位置到最终位置的中间过程。
[0109] (3)四元数和姿态角的换算
[0110] 四旋翼飞行器的四元数矢量式为
[0112] 四元数可确定出由四旋翼机体坐标b系到地面坐标n系的变换坐标矩阵
[0114] 由变换坐标矩阵可得四旋翼飞行器的姿态角为
[0115]
[0116] (4)更新四元数
[0117] 在捷联式惯性导航系统中，陀螺仪的输出值一般是等时间采样周期内的角增量， 为了避免积分的累积误差、降低噪声的微分放大，本文直接采用角增量来确定四元数，即毕 卡四元数算法。
[0119] 其中，
，0表示四元数乘法，其等效的换算矩阵为
[0121] 求解式（10)的微分方程可得
[0123]进一步，
[0126] 对式（17)进行泰勒级数展开可得
[0128] 考虑到算法的精度和控制芯片的运算量，本文取三阶近似毕卡四元数算法，四元 数求解如下：
[0130]其中，A02=gbx2+gby2+gbz2。
[0131] 步骤4)中还包括卡尔曼滤波，根据步骤3)中以上的姿态获取，可通过加速计和陀 螺仪得到的姿态角，进而基于卡尔曼进行数据融合得到最终输出的姿态角。
[0132] 参照图1和图2 -种采用三阶近似毕卡四元数的四旋翼飞行器姿态获取方法，所 述方法包括：
[0133] 如图1所示为四旋翼飞行器的硬件系统构成图。本平台的硬件系统以3个 STM32F103VET6控制芯片为核心划分为三大部分。第一部分是主控制器和惯性导航系统的 电路设计。主控芯片釆用的是STM32,进行姿态获取和姿态控制。惯性导航系统是采集飞 行器机体的运动状态信息作为控制系统控制的重要信息依据。惯性导航平台的反馈精度直 接影响到控制系统的控制精度。本项目的惯性导航系统由6轴运动处理组件MPU6050、电 子罗盘HMC5883和气压计MS5611组成。第二部分是以STM32控制芯片，用0V7620数字摄 像头做图像信息采集和四旋翼飞行器飞行数据处理的信息处理部分，与第一部分的芯片用 USART接口进行通信，与第三部分用蓝牙模块进行通信。第三部分是用STM32设计UC/0SII 嵌入式系统下位机，使用蓝牙模块与四旋翼飞行器进行通信，用作人机交互以及图像数据 传输的中间站，能够完成飞行器各个状态之间的切换以及数据的接收显示。
[0134] 如图2为本发明的四旋翼飞行器姿态获取流程图。其中包括了传感器数据预处 理、欧拉角初始化、四元数初始化、更新四元数、四元数规范化、姿态角更新以及卡尔曼滤波 姿态估计，清晰地展示了算法的执行流程。
【主权项】
1. 一种采用三阶近似毕卡四元数的四旋翼飞行器姿态获取方法，包括以下步骤： 步骤1)为了验证算法的可行性，首先建立一个飞行器实验平台；将飞行器实验平台的 硬件系统以3个STM32F103VET6控制芯片为核心划分为三大部分； 第一部分是主控制器和惯性导航系统的电路设计；主控芯片釆用的是STM32,进行姿 态获取和姿态控制；惯性导航系统是采集飞行器机体的运动状态信息作为控制系统控制的 重要信息依据；惯性导航平台的反馈精度直接影响到控制系统的控制精度；惯性导航系统 由6轴运动处理组件MPU6050、电子罗盘HMC5883和气压计MS5611组成； 第二部分是以STM32控制芯片，用0V7620数字摄像头做图像信息采集和四旋翼飞行器 飞行数据处理的信息处理部分，与第一部分的芯片用USART接口进行通信，与第三部分用 蓝牙模块进行通信； 第三部分是用STM32设计UC/OSII嵌入式系统下位机，使用蓝牙模块与四旋翼飞行器 进行通信，用作人机交互以及图像数据传输的中间站，能够完成飞行器各个状态之间的切 换以及数据的接收显示； 步骤2)数据的预处理，具体方案如下： 在飞行器的姿态控制中，姿态数据通常由传感器获取，但是单个传感器获取的信息往 往不精确；因此，在进行姿态估计之前，需要对传感器的原始数据进行预处理：加速度计校 准及滤波、陀螺仪静态偏置的计算（陀螺仪零偏）以及磁力计的偏心矫正及滤波； (21) 加速度计校准及滤波 由于加速度传感器的测量值必然存在误差，因此，有必要在使用前对其进行校准，使用 时对其进行滤波；这里使用均值滤波及滑动平均滤波的方式对加速度计的测量信息进行处 理； (22) 陀螺仪零偏计算 陀螺仪存在静态偏置即零偏，因此在每一次系统初始化时需要对其零偏进行计算并在 之后的计算中除去零偏；先将陀螺仪静置，连续读取200次陀螺仪的测量值进行均值滤波， 并将其作为陀螺仪的零偏； (23) 磁力计偏心矫正及倾斜补偿 理论上，磁力计的三个轴的测量值范围是对称的，即在空间中呈球体分布；然而，由于 传感器的误差以及外界磁场的干扰，本文所得到的磁力计测量值是一个中心不在原点的椭 球体，因此需要对其测量值进行偏心校正；基本步骤为： a.将磁力计水平旋转360度计算其X轴与Y轴最大值与最小值，由此计算修正增益Xs与Ys，修正偏置Xb与Y b 这里以X轴为基准，令Xs= 1 ;(15) (16) b. Xin、Yin和Z in为磁力计的测量输出值，通过修正增益与修正偏置计算磁力计的修正值 X-和 Yout:(17) C.同样以X轴为基准，对Z轴进行矫正，最终得到三个轴的修正值Xwt、￥_和Z(18) 步骤3)设计基于毕卡四元数的四旋翼飞行器姿态获取，过程包括： 设由四旋翼的机体轴确定的坐标系为b，地面坐标系为n，则由b系到η系的坐标矩阵 称为四旋翼的姿态矩阵，θ、γ、供分别为飞机的俯仰、横滚、偏航角，加速度计输出的原 始值为速度的增量即abx、aby、abz，陀螺仪计输出的原始值为角增量即g bx、gby、gbz，磁力计mbx、 Hlby、 (31) 欧拉角初始化(1) (32) 四元数初始值的确定及规范化处理 四旋翼飞行器的四元数的初始值Q(O)由捷联系统的初值状态对准确定；设初始对准 的姿态阵为G。，可由式（1)中求得的公式计算得出初始姿态角求得；(2) (3) 则四旋翼飞行器的四元数的初始值为： Q(O) = cj() + i/, / + q2 j + c], k (4) 其中，qd、qp q2、。3的数值和符号可按下式（5)和（6)确定；(5) (6) 上式中，Sign(Cici)可任选，因为四元数表征刚体从初始位置到最终位置的一次性等效 旋转，并不反应从初始位置到最终位置的中间过程； (33) 四元数和姿态角的换算 四旋翼飞行器的四元数矢量式为 = +^1^ + ^7 + ?^ (7) 四元数可确定出由四旋翼机体坐标b系到地面坐标n系的变换坐标矩阵(8) 由变换坐标矩阵Qn可得四旋翼飞行器的姿态角为(9) (34) 更新四元数 在捷联式惯性导航系统中，陀螺仪的输出值一般是等时间采样周期内的角增量，为了 避免积分的累积误差、降低噪声的微分放大，本文直接采用角增量来确定四元数；即毕卡四 元数算法；(10) 其中i表示四元数乘法，其等效的换算矩阵为求解式（10)的微分方程可得对式（17)进行泰勒级数展开可得(13) 考虑到算法的精度和控制芯片的运算量，取三阶近似毕卡四元数算法，四元数求解如 下：(14) 其中，Λ 02=gbx2+gby2+gbz2; (4)卡尔曼滤波，根据步骤3)中以上的姿态获取，可通过加速计和陀螺仪得到的姿态 角，进而基于卡尔曼进行数据融合得到最终输出的姿态角。
【专利摘要】一种采用三阶近似毕卡四元数的四旋翼飞行器姿态获取方法，包括以下步骤：1)根据三阶近似毕卡四元数来进行姿态解算，避免了陀螺仪的积分累积误差，降低了噪声的微分放大；2)将解算得到的姿态角进行卡尔曼滤波，有效消除了四旋翼飞行器机体振动引起的传感器测量误差，得到更准确的姿态角。本发明所设计姿态解算方法能够对小型四旋翼飞行器进行精确的姿态估计。
【IPC分类】G01C21/20, G01C21/16, G05D1/08
【公开号】CN104898681
【申请号】CN201510221435
【发明人】张丹, 叶孝璐, 肖紫阳
【申请人】浙江工业大学
【公开日】2015年9月9日
【申请日】2015年5月4日