一种采用三阶近似毕卡四元数的四旋翼飞行器姿态获取方法
一种采用三阶近似毕卡四元数的四旋翼飞行器姿态获取方法
【技术领域】
[0001] 本发明应用于四旋翼飞行器姿态控制领域，涉及到四旋翼飞行器姿态控制问题， 尤其是如何避免陀螺仪的积分累积误差，消除四旋翼飞行器机体振动引起的传感器测量误 差，实现一种有效的实时控制方法。
【背景技术】
[0002] 四旋翼飞行器是四桨非共轴多旋翼飞行器的简称。作为非共轴多旋翼飞行器的代 表，与固定翼无人机相比，具有结构简单、体积小、隐蔽性强、载荷高、可垂直起降、定点悬停 和低速飞行等优势，能执行各种危险的特殊任务，比如航拍、灾害现场监视与求援、低空侦 探与检测等，在民用领域和军事领域均体现出极高的研宄和应用价值。四旋翼飞行器是无 人机飞行器领域中的一个热点研宄课题，随着纳米级低雷诺数空气动力学与MEMS技术的 深入研宄，四旋翼飞行器在近些年得到了快速的发展。
[0003]四旋翼飞行器虽然结构简单，但其是一个多变量、强耦合的非线性系统，且机体与 外界和自身的干扰十分敏感，使得飞行器姿态估计和控制系统的设计变得十分困难。四旋 翼飞行器4个旋桨高速运转时，相互作用产生的气流十分复杂，以至机体产生高频振颤，而 这种高频干扰会引入到惯导系统的传感器输出中。
[0004] 本发明主要考虑如何有效地进行四旋翼飞行器的姿态获取并避免惯导传感器输 出中的高频噪声。通常姿态获取的算法常用的有欧拉角法、方向余弦法、四元数法等。欧拉 角法解算过程中包含大量的三角运算，计算量很大，并且在俯仰角接近90时方程会出现退 化现象，不适用于全姿态的姿态角解算。方向余弦法能进行全姿态获取，但计算量大，实时 计算困难。四元数法计算量小、实用，但无法避免陀螺仪累积的积分误差问题。
【发明内容】
[0005] 为了克服上述提到的现有姿态获取方法的局限性，本发明提出三阶近似毕卡四元 数算法进行姿态获取。将三阶近似毕卡四元数与卡尔曼滤波相结合，避免了陀螺仪的积分 累积误差，有效消除了四旋翼飞行器机体振动引起的传感器测量误差。
[0006] 本发明解决其技术问题所采用的技术方案：
[0007] -种采用三阶近似毕卡四元数的四旋翼飞行器姿态获取方法，包括以下步骤：
[0008] 步骤1)为了验证算法的可行性，首先建立一个飞行器实验平台，所述平台的硬件 系统以3个STM32F103VET6控制芯片为核心划分为三大部分。
[0009] 第一部分是主控制器和惯性导航系统的电路设计。主控芯片釆用的是STM32,进行 姿态获取和姿态控制。惯性导航系统是采集飞行器机体的运动状态信息作为控制系统控制 的重要信息依据。惯性导航平台的反馈精度直接影响到控制系统的控制精度。本项目的惯 性导航系统由6轴运动处理组件MPU6050、电子罗盘HMC5883和气压计MS5611组成。
[0010] 第二部分是以STM32控制芯片，用0V7620数字摄像头做图像信息采集和四旋翼飞 行器飞行数据处理的信息处理部分，与第一部分的芯片用USART接口进行通信，与第三部 分用蓝牙模块进行通信。
[0011] 第三部分是用STM32设计UC/0SII嵌入式系统下位机，使用蓝牙与四旋翼飞行器 进行通信，用作人机交互以及图像数据传输的中间站，能够完成飞行器各个状态之间的切 换以及数据的接收显示。
[0012] 步骤2)数据的预处理，具体方案如下：
[0013] 在飞行器的姿态控制中，姿态数据通常由传感器获取，但是单个传感器获取的信 息往往不精确。因此，在进行姿态估计之前，需要对传感器的原始数据进行预处理，以下主 要简述3部分：加速度计校准及滤波、陀螺仪静态偏置的计算（陀螺仪零偏）以及磁力计的 偏心矫正及滤波。
[0014] (1)加速度计校准及滤波
[0015] 由于加速度传感器的测量值必然存在误差，因此，有必要在使用前对其进行校准， 使用时对其进行滤波。这里使用均值滤波及滑动平均滤波的方式对加速度计的测量信息进 行处理。
[0016] (2)陀螺仪零偏计算
[0017] 陀螺仪存在静态偏置即零偏，因此在每一次系统初始化时需要对其零偏进行计算 并在之后的计算中除去零偏。先将陀螺仪静置，连续读取200次陀螺仪的测量值进行均值 滤波，并将其作为陀螺仪的零偏。
[0018] (3)磁力计偏心矫正及倾斜补偿
[0019] 理论上，磁力计的三个轴的测量值范围是对称的，即在空间中呈球体分布。然而， 由于传感器的误差以及外界磁场的干扰，本文所得到的磁力计测量值是一个中心不在原点 的椭球体，因此需要对其测量值进行偏心校正。基本步骤为：
[0020] a.将磁力计水平旋转360度计算其X轴与Y轴最大值与最小值，由此计算修正增 益&与Y s，修正偏置Xb与Yb
[0021] 这里以X轴为基准，令Xs= 1。
[0024] b.Xin、Yin和Zin为磁力计的测量输出值，通过修正增益与修正偏置计算磁力计的修 正值 UPYQUt:
[0026] c.同样以X轴为基准，对Z轴进行矫正，最终得到三个轴的修正值Z wt:
[0028] 步骤3)设计基于毕卡四元数的四旋翼飞行器姿态获取，过程包括：
[0029] 设由四旋翼的机体轴确定的坐标系为b，地面坐标系为n，则由b系到n系的坐标 矩阵C?称为四旋翼的姿态矩阵，0、Y、供分别为飞机的俯仰、横滚、偏航角，加速度计输出 的原始值为速度的增量即abx、aby、abz，陀螺仪计输出为角增量即gbx、gby、gbz，磁力计mbx、mby、 ^bz°
[0030] (1)欧拉角初始化
[0032] (2)四元数初始值的确定及规范化处理
[0033] 四旋翼飞行器的四元数的初始值Q(0)由捷联系统的初值状态对准确定。设初始 对准的姿态阵为〇可由式（1)中求得的公式计算得出初始姿态角求得。
[0035]其中，cn、c12、c13、c 21、c22、c23、c31、c32、c 33分别为
[0037] 则四旋翼飞行器的四元数的初始值为：
[0039] 其中，qp q2、。3的数值和符号可按下式（5)和（6)确定。
[0042] 上式中，sign(%)可任选，因为四元数表征刚体从初始位置到最终位置的一次性 等效旋转，并不反应从初始位置到最终位置的中间过程。
[0043] (3)四元数和姿态角的换算
[0044] 四旋翼飞行器的四元数矢量式为
[0046] 四元数可确定出由四旋翼机体坐标b系到地面坐标n系的变换坐标矩阵
[0048]由变换坐标矩阵G可得四旋翼飞行器的姿态角为
[0050] ⑷更新四元数
[0051] 在捷联式惯性导航系统中，陀螺仪的输出值一般是等时间采样周期内的角增量， 为了避免积分的累积误差、降低噪声的微分放大，本文直接采用角增量来确定四元数，即毕 卡四元数算法。
[0053] 其中，
，?表示四元数乘法，其等效的换算矩阵为
[0055]求解式（10)的微分方程可得
[0057]进一步，
[0060] 对式（17)进行泰勒级数展开可得
[0062] 考虑到算法的精度和控制芯片的运算量，本文取三阶近似毕卡四元数算法，四元 数求解如下：
[0064]其中，A0 2 =gbx2+gby2+gbz2。
[0065] 步骤4)中还包括卡尔曼滤波，根据步骤3)中以上的姿态获取，可通过加速计和陀 螺仪得到的姿态角，进而基于卡尔曼进行数据融合得到最终输出的姿态角。
[0066] 作为优选的另一种方案：所述的步骤3)中还包括实时四旋翼飞行器监测控制台， 姿态获取和数据融合的结果将在四旋翼飞行器监测控制台中显示。
[0067] 与现有技术相比，本发明的优点在于：
[0068] (1)基于三阶近似毕卡四元数来进行姿态获取，避免了陀螺仪的积分累积误差，降 低了噪声的微分放大。
[0069] (2)将解算得到的姿态角进行卡尔曼滤波，有效消除了四旋翼飞行器机体振动引 起的传感器测量误差，得到更准确的姿态角。
【附图说明】
[0070] 图1为本发明的四旋翼飞行器的硬件系统构成图。
[0071] 图2为本发明的四旋翼飞行器姿态获取流程图
【具体实施方式】
[0072] 为了使本发明的技术方案、设计思路能更加清晰，下面结合附图再进行详尽的描 述。
[0073] -种采用三阶近似毕卡四元数的四旋翼飞行器姿态获取方法，包括以下步骤：
[0074] 步骤1)为了验证算法的可行性，首先建立一个飞行器实验平台，所述平台的硬件 系统以3个STM32F103VET6控制芯片为核心划分为三大部分。
[0075] 第一部分是主控制器和惯性导航系统的电路设计。主控芯片釆用的是STM32,进行 姿态获取和姿态控制。惯性导航系统是采集飞行器机体的运动状态信息作为控制系统控制 的重要信息依据。惯性导航平台的反馈精度直接影响到控制系统的控制精度。本项目的惯 性导航系统由6轴运动处理组件MPU6050、电子罗盘HMC5883和气压计MS5611组成。
[0076] 第二部分是以STM32控制芯片，用0V7620数字摄像头做图像信息采集和四旋翼飞 行器飞行数据处理的信息处理部分，与第一部分的芯片用USART接口进行通信，与第三部 分用蓝牙模块进行通信。
[0077] 第三部分是用STM32设计UC/0SII嵌入式系统下位机，使用蓝牙与四旋翼飞行器 进行通信，用作人机交互以及图像数据传输的中间站，能够完成飞行器各个状态之间的切 换以及数据的接收显示。
[0078] 步骤2)数据的预处理，具体方案如下：
[0079] 在飞行器的姿态控制中，姿态数据通常由传感器获取，但是单个传感器获取的信 息往往不精确。因此，在进行姿态估计之前，需要对传感器的原始数据进行预处理，以下主 要简述3部分：加速度计校准及滤波、陀螺仪静态偏置的计算（陀螺仪零偏）以及磁力计的 偏心矫正及滤波。
[0080] (1)加速度计校准及滤波
[0081] 由于加速度传感器的测量值必然存在误差，因此，有必要在使用前对其进行校准， 使用时对其进行滤波。这里使用均值滤波及滑动平均滤波的方式对加速度计的测量信息进 行处理。
[0082] (2)陀螺仪零偏计算
[0083] 陀螺仪存在静态偏置 即零偏，因此在每一次系统初始化时需要对其零偏进行计算 并在之后的计算中除去零偏。先将陀螺仪静置，连续读取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日
【技术领域】
[0001] 本发明应用于四旋翼飞行器姿态控制领域，涉及到四旋翼飞行器姿态控制问题， 尤其是如何避免陀螺仪的积分累积误差，消除四旋翼飞行器机体振动引起的传感器测量误 差，实现一种有效的实时控制方法。
【背景技术】
[0002] 四旋翼飞行器是四桨非共轴多旋翼飞行器的简称。作为非共轴多旋翼飞行器的代 表，与固定翼无人机相比，具有结构简单、体积小、隐蔽性强、载荷高、可垂直起降、定点悬停 和低速飞行等优势，能执行各种危险的特殊任务，比如航拍、灾害现场监视与求援、低空侦 探与检测等，在民用领域和军事领域均体现出极高的研宄和应用价值。四旋翼飞行器是无 人机飞行器领域中的一个热点研宄课题，随着纳米级低雷诺数空气动力学与MEMS技术的 深入研宄，四旋翼飞行器在近些年得到了快速的发展。
[0003]四旋翼飞行器虽然结构简单，但其是一个多变量、强耦合的非线性系统，且机体与 外界和自身的干扰十分敏感，使得飞行器姿态估计和控制系统的设计变得十分困难。四旋 翼飞行器4个旋桨高速运转时，相互作用产生的气流十分复杂，以至机体产生高频振颤，而 这种高频干扰会引入到惯导系统的传感器输出中。
[0004] 本发明主要考虑如何有效地进行四旋翼飞行器的姿态获取并避免惯导传感器输 出中的高频噪声。通常姿态获取的算法常用的有欧拉角法、方向余弦法、四元数法等。欧拉 角法解算过程中包含大量的三角运算，计算量很大，并且在俯仰角接近90时方程会出现退 化现象，不适用于全姿态的姿态角解算。方向余弦法能进行全姿态获取，但计算量大，实时 计算困难。四元数法计算量小、实用，但无法避免陀螺仪累积的积分误差问题。
【发明内容】
[0005] 为了克服上述提到的现有姿态获取方法的局限性，本发明提出三阶近似毕卡四元 数算法进行姿态获取。将三阶近似毕卡四元数与卡尔曼滤波相结合，避免了陀螺仪的积分 累积误差，有效消除了四旋翼飞行器机体振动引起的传感器测量误差。
[0006] 本发明解决其技术问题所采用的技术方案：
[0007] -种采用三阶近似毕卡四元数的四旋翼飞行器姿态获取方法，包括以下步骤：
[0008] 步骤1)为了验证算法的可行性，首先建立一个飞行器实验平台，所述平台的硬件 系统以3个STM32F103VET6控制芯片为核心划分为三大部分。
[0009] 第一部分是主控制器和惯性导航系统的电路设计。主控芯片釆用的是STM32,进行 姿态获取和姿态控制。惯性导航系统是采集飞行器机体的运动状态信息作为控制系统控制 的重要信息依据。惯性导航平台的反馈精度直接影响到控制系统的控制精度。本项目的惯 性导航系统由6轴运动处理组件MPU6050、电子罗盘HMC5883和气压计MS5611组成。
[0010] 第二部分是以STM32控制芯片，用0V7620数字摄像头做图像信息采集和四旋翼飞 行器飞行数据处理的信息处理部分，与第一部分的芯片用USART接口进行通信，与第三部 分用蓝牙模块进行通信。
[0011] 第三部分是用STM32设计UC/0SII嵌入式系统下位机，使用蓝牙与四旋翼飞行器 进行通信，用作人机交互以及图像数据传输的中间站，能够完成飞行器各个状态之间的切 换以及数据的接收显示。
[0012] 步骤2)数据的预处理，具体方案如下：
[0013] 在飞行器的姿态控制中，姿态数据通常由传感器获取，但是单个传感器获取的信 息往往不精确。因此，在进行姿态估计之前，需要对传感器的原始数据进行预处理，以下主 要简述3部分：加速度计校准及滤波、陀螺仪静态偏置的计算（陀螺仪零偏）以及磁力计的 偏心矫正及滤波。
[0014] (1)加速度计校准及滤波
[0015] 由于加速度传感器的测量值必然存在误差，因此，有必要在使用前对其进行校准， 使用时对其进行滤波。这里使用均值滤波及滑动平均滤波的方式对加速度计的测量信息进 行处理。
[0016] (2)陀螺仪零偏计算
[0017] 陀螺仪存在静态偏置即零偏，因此在每一次系统初始化时需要对其零偏进行计算 并在之后的计算中除去零偏。先将陀螺仪静置，连续读取200次陀螺仪的测量值进行均值 滤波，并将其作为陀螺仪的零偏。
[0018] (3)磁力计偏心矫正及倾斜补偿
[0019] 理论上，磁力计的三个轴的测量值范围是对称的，即在空间中呈球体分布。然而， 由于传感器的误差以及外界磁场的干扰，本文所得到的磁力计测量值是一个中心不在原点 的椭球体，因此需要对其测量值进行偏心校正。基本步骤为：
[0020] a.将磁力计水平旋转360度计算其X轴与Y轴最大值与最小值，由此计算修正增 益&与Y s，修正偏置Xb与Yb
[0021] 这里以X轴为基准，令Xs= 1。
[0024] b.Xin、Yin和Zin为磁力计的测量输出值，通过修正增益与修正偏置计算磁力计的修 正值 UPYQUt:
[0026] c.同样以X轴为基准，对Z轴进行矫正，最终得到三个轴的修正值Z wt:
[0028] 步骤3)设计基于毕卡四元数的四旋翼飞行器姿态获取，过程包括：
[0029] 设由四旋翼的机体轴确定的坐标系为b，地面坐标系为n，则由b系到n系的坐标 矩阵C?称为四旋翼的姿态矩阵，0、Y、供分别为飞机的俯仰、横滚、偏航角，加速度计输出 的原始值为速度的增量即abx、aby、abz，陀螺仪计输出为角增量即gbx、gby、gbz，磁力计mbx、mby、 ^bz°
[0030] (1)欧拉角初始化
[0032] (2)四元数初始值的确定及规范化处理
[0033] 四旋翼飞行器的四元数的初始值Q(0)由捷联系统的初值状态对准确定。设初始 对准的姿态阵为〇可由式（1)中求得的公式计算得出初始姿态角求得。
[0035]其中，cn、c12、c13、c 21、c22、c23、c31、c32、c 33分别为
[0037] 则四旋翼飞行器的四元数的初始值为：
[0039] 其中，qp q2、。3的数值和符号可按下式（5)和（6)确定。
[0042] 上式中，sign(%)可任选，因为四元数表征刚体从初始位置到最终位置的一次性 等效旋转，并不反应从初始位置到最终位置的中间过程。
[0043] (3)四元数和姿态角的换算
[0044] 四旋翼飞行器的四元数矢量式为
[0046] 四元数可确定出由四旋翼机体坐标b系到地面坐标n系的变换坐标矩阵
[0048]由变换坐标矩阵G可得四旋翼飞行器的姿态角为
[0050] ⑷更新四元数
[0051] 在捷联式惯性导航系统中，陀螺仪的输出值一般是等时间采样周期内的角增量， 为了避免积分的累积误差、降低噪声的微分放大，本文直接采用角增量来确定四元数，即毕 卡四元数算法。
[0053] 其中，
，?表示四元数乘法，其等效的换算矩阵为
[0055]求解式（10)的微分方程可得
[0057]进一步，
[0060] 对式（17)进行泰勒级数展开可得
[0062] 考虑到算法的精度和控制芯片的运算量，本文取三阶近似毕卡四元数算法，四元 数求解如下：
[0064]其中，A0 2 =gbx2+gby2+gbz2。
[0065] 步骤4)中还包括卡尔曼滤波，根据步骤3)中以上的姿态获取，可通过加速计和陀 螺仪得到的姿态角，进而基于卡尔曼进行数据融合得到最终输出的姿态角。
[0066] 作为优选的另一种方案：所述的步骤3)中还包括实时四旋翼飞行器监测控制台， 姿态获取和数据融合的结果将在四旋翼飞行器监测控制台中显示。
[0067] 与现有技术相比，本发明的优点在于：
[0068] (1)基于三阶近似毕卡四元数来进行姿态获取，避免了陀螺仪的积分累积误差，降 低了噪声的微分放大。
[0069] (2)将解算得到的姿态角进行卡尔曼滤波，有效消除了四旋翼飞行器机体振动引 起的传感器测量误差，得到更准确的姿态角。
【附图说明】
[0070] 图1为本发明的四旋翼飞行器的硬件系统构成图。
[0071] 图2为本发明的四旋翼飞行器姿态获取流程图
【具体实施方式】
[0072] 为了使本发明的技术方案、设计思路能更加清晰，下面结合附图再进行详尽的描 述。
[0073] -种采用三阶近似毕卡四元数的四旋翼飞行器姿态获取方法，包括以下步骤：
[0074] 步骤1)为了验证算法的可行性，首先建立一个飞行器实验平台，所述平台的硬件 系统以3个STM32F103VET6控制芯片为核心划分为三大部分。
[0075] 第一部分是主控制器和惯性导航系统的电路设计。主控芯片釆用的是STM32,进行 姿态获取和姿态控制。惯性导航系统是采集飞行器机体的运动状态信息作为控制系统控制 的重要信息依据。惯性导航平台的反馈精度直接影响到控制系统的控制精度。本项目的惯 性导航系统由6轴运动处理组件MPU6050、电子罗盘HMC5883和气压计MS5611组成。
[0076] 第二部分是以STM32控制芯片，用0V7620数字摄像头做图像信息采集和四旋翼飞 行器飞行数据处理的信息处理部分，与第一部分的芯片用USART接口进行通信，与第三部 分用蓝牙模块进行通信。
[0077] 第三部分是用STM32设计UC/0SII嵌入式系统下位机，使用蓝牙与四旋翼飞行器 进行通信，用作人机交互以及图像数据传输的中间站，能够完成飞行器各个状态之间的切 换以及数据的接收显示。
[0078] 步骤2)数据的预处理，具体方案如下：
[0079] 在飞行器的姿态控制中，姿态数据通常由传感器获取，但是单个传感器获取的信 息往往不精确。因此，在进行姿态估计之前，需要对传感器的原始数据进行预处理，以下主 要简述3部分：加速度计校准及滤波、陀螺仪静态偏置的计算（陀螺仪零偏）以及磁力计的 偏心矫正及滤波。
[0080] (1)加速度计校准及滤波
[0081] 由于加速度传感器的测量值必然存在误差，因此，有必要在使用前对其进行校准， 使用时对其进行滤波。这里使用均值滤波及滑动平均滤波的方式对加速度计的测量信息进 行处理。
[0082] (2)陀螺仪零偏计算
[0083] 陀螺仪存在静态偏置 即零偏，因此在每一次系统初始化时需要对其零偏进行计算 并在之后的计算中除去零偏。先将陀螺仪静置，连续读取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日
最新回复(0)