CN104898681A - 一种采用三阶近似毕卡四元数的四旋翼飞行器姿态获取方法 - Google Patents

Abstract

一种采用三阶近似毕卡四元数的四旋翼飞行器姿态获取方法，包括以下步骤：1)根据三阶近似毕卡四元数来进行姿态解算，避免了陀螺仪的积分累积误差，降低了噪声的微分放大；2)将解算得到的姿态角进行卡尔曼滤波，有效消除了四旋翼飞行器机体振动引起的传感器测量误差，得到更准确的姿态角。本发明所设计姿态解算方法能够对小型四旋翼飞行器进行精确的姿态估计。

Description

一种采用三阶近似毕卡四元数的四旋翼飞行器姿态获取方法

技术领域

本发明应用于四旋翼飞行器姿态控制领域，涉及到四旋翼飞行器姿态控制问题，尤其是如何避免陀螺仪的积分累积误差，消除四旋翼飞行器机体振动引起的传感器测量误差，实现一种有效的实时控制方法。

背景技术

四旋翼飞行器是四桨非共轴多旋翼飞行器的简称。作为非共轴多旋翼飞行器的代表，与固定翼无人机相比，具有结构简单、体积小、隐蔽性强、载荷高、可垂直起降、定点悬停和低速飞行等优势，能执行各种危险的特殊任务，比如航拍、灾害现场监视与求援、低空侦探与检测等，在民用领域和军事领域均体现出极高的研究和应用价值。四旋翼飞行器是无人机飞行器领域中的一个热点研究课题，随着纳米级低雷诺数空气动力学与MEMS技术的深入研究，四旋翼飞行器在近些年得到了快速的发展。

四旋翼飞行器虽然结构简单，但其是一个多变量、强耦合的非线性系统，且机体与外界和自身的干扰十分敏感，使得飞行器姿态估计和控制系统的设计变得十分困难。四旋翼飞行器4个旋桨高速运转时，相互作用产生的气流十分复杂，以至机体产生高频振颤，而这种高频干扰会引入到惯导系统的传感器输出中。

本发明主要考虑如何有效地进行四旋翼飞行器的姿态获取并避免惯导传感器输出中的高频噪声。通常姿态获取的算法常用的有欧拉角法、方向余弦法、四元数法等。欧拉角法解算过程中包含大量的三角运算，计算量很大，并且在俯仰角接近90时方程会出现退化现象，不适用于全姿态的姿态角解算。方向余弦法能进行全姿态获取，但计算量大，实时计算困难。四元数法计算量小、实用，但无法避免陀螺仪累积的积分误差问题。

发明内容

为了克服上述提到的现有姿态获取方法的局限性，本发明提出三阶近似毕卡四元数算法进行姿态获取。将三阶近似毕卡四元数与卡尔曼滤波相结合，避免了陀螺仪的积分累积误差，有效消除了四旋翼飞行器机体振动引起的传感器测量误差。

本发明解决其技术问题所采用的技术方案：

一种采用三阶近似毕卡四元数的四旋翼飞行器姿态获取方法，包括以下步骤：

步骤1)为了验证算法的可行性，首先建立一个飞行器实验平台，所述平台的硬件系统以3个STM32F103VET6控制芯片为核心划分为三大部分。

第一部分是主控制器和惯性导航系统的电路设计。主控芯片釆用的是STM32，进行姿态获取和姿态控制。惯性导航系统是采集飞行器机体的运动状态信息作为控制系统控制的重要信息依据。惯性导航平台的反馈精度直接影响到控制系统的控制精度。本项目的惯性导航系统由6轴运动处理组件MPU6050、电子罗盘HMC5883和气压计MS5611组成。

第二部分是以STM32控制芯片，用OV7620数字摄像头做图像信息采集和四旋翼飞行器飞行数据处理的信息处理部分，与第一部分的芯片用USART接口进行通信，与第三部分用蓝牙模块进行通信。

第三部分是用STM32设计UC/OSII嵌入式系统下位机，使用蓝牙与四旋翼飞行器进行通信，用作人机交互以及图像数据传输的中间站，能够完成飞行器各个状态之间的切换以及数据的接收显示。

步骤2)数据的预处理，具体方案如下：

在飞行器的姿态控制中，姿态数据通常由传感器获取，但是单个传感器获取的信息往往不精确。因此，在进行姿态估计之前，需要对传感器的原始数据进行预处理，以下主要简述3部分：加速度计校准及滤波、陀螺仪静态偏置的计算(陀螺仪零偏)以及磁力计的偏心矫正及滤波。

(1)加速度计校准及滤波

由于加速度传感器的测量值必然存在误差，因此，有必要在使用前对其进行校准，使用时对其进行滤波。这里使用均值滤波及滑动平均滤波的方式对加速度计的测量信息进行处理。

(2)陀螺仪零偏计算

陀螺仪存在静态偏置即零偏，因此在每一次系统初始化时需要对其零偏进行计算并在之后的计算中除去零偏。先将陀螺仪静置，连续读取200次陀螺仪的测量值进行均值滤波，并将其作为陀螺仪的零偏。

(3)磁力计偏心矫正及倾斜补偿

理论上，磁力计的三个轴的测量值范围是对称的，即在空间中呈球体分布。然而，由于传感器的误差以及外界磁场的干扰，本文所得到的磁力计测量值是一个中心不在原点的椭球体，因此需要对其测量值进行偏心校正。基本步骤为：

a.将磁力计水平旋转360度计算其X轴与Y轴最大值与最小值，由此计算修正增益X_S与Y_S，修正偏置X_b与Y_b

这里以X轴为基准，令X_S＝1。

$Y_s=\frac{(X_{\max }-X_{\min })}{(Y_{\max }-Y_{\min })}---\left(15\right)$

$\left\{\begin{array}{c}{X}_b=X_s[\frac{1}{2}(X_{\max }-X_{\min })-X_{\max }]\\ Y_b=Y_s[\frac{1}{2}(Y_{\max }-Y_{\min })-Y_{\max }]\end{array}\right.$

b.X_in、Y_in和Z_in为磁力计的测量输出值，通过修正增益与修正偏置计算磁力计的修正值X_out和Y_out：

$\left\{\begin{array}{c}{X}_{\mathrm{out}}=X_s\×X_{\mathrm{in}}+X_b\\ Y_{\mathrm{out}}=Y_s\×Y_{\mathrm{in}}+Y_b\end{array}\right.---\left(17\right)$

c.同样以X轴为基准，对Z轴进行矫正，最终得到三个轴的修正值X_out、Y_out和Z_out：

$\left\{\begin{array}{c}{X}_{\mathrm{out}}=X_s\×X_{\mathrm{in}}+X_b\\ Y_{\mathrm{out}}=Y_s\×Y_{\mathrm{in}}+Y_b\\ Z_{\mathrm{out}}=Z_s\×Z_{\mathrm{in}}+Z_b\end{array}\right.---\left(18\right)$

步骤3)设计基于毕卡四元数的四旋翼飞行器姿态获取，过程包括：

设由四旋翼的机体轴确定的坐标系为b，地面坐标系为n，则由b系到n系的坐标矩阵称为四旋翼的姿态矩阵，θ、γ、分别为飞机的俯仰、横滚、偏航角，加速度计输出的原始值为速度的增量即a_bx、a_by、a_bz，陀螺仪计输出为角增量即g_bx、g_by、g_bz，磁力计m_bx、m_by、m_bz。

(1)欧拉角初始化

(2)四元数初始值的确定及规范化处理

四旋翼飞行器的四元数的初始值Q(0)由捷联系统的初值状态对准确定。设初始对准的姿态阵为可由式(1)中求得的公式计算得出初始姿态角求得。

$C_{b0}^n=\left[\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{21}& c_{22}& c_{23}\\ c_{31}& c_{32}& c_{33}\end{array}\right]---\left(2\right)$

其中，c₁₁、c₁₂、c₁₃、c₂₁、c₂₂、c₂₃、c₃₁、c₃₂、c₃₃分别为

则四旋翼飞行器的四元数的初始值为：

$Q\left(0\right)=q_0+q_1\stackrel{\→}{i}+q_2\stackrel{\→}{j}+q_3\stackrel{\→}{k}---\left(4\right)$

其中，q₀、q₁、q₂、q₃的数值和符号可按下式(5)和(6)确定。

$\left\{\begin{array}{c}\left|q_1\right|=\frac{1}{2}\sqrt{1+c_{11}-c_{22}-c_{33}}\\ \left|q_2\right|=\frac{1}{2}\sqrt{1-c_{11}+c_{22}-c_{33}}\\ \left|q_3\right|=\frac{1}{2}\sqrt{1-c_{11}-c_{22}-c_{33}}\\ \left|q_0\right|=\frac{1}{2}\sqrt{1+c_{11}+c_{22}+c_{33}}\end{array}\right.---\left(5\right)$

$\left\{\begin{array}{c}\mathrm{sign}\left(q_1\right)=\mathrm{sign}\left(q_0\right)\left[\mathrm{sign}(c_{32}-c_{23})\right]\\ \mathrm{sign}\left(q_2\right)=\mathrm{sign}\left(q_0\right)\left[\mathrm{sign}(c_{13}-c_{31})\right]\\ \mathrm{sign}\left(q_3\right)=\mathrm{sign}\left(q_0\right)\left[\mathrm{sign}(c_{21}-c_{12})\right]\end{array}\right.---\left(6\right)$

上式中，sign(q₀)可任选，因为四元数表征刚体从初始位置到最终位置的一次性等效旋转，并不反应从初始位置到最终位置的中间过程。

(3)四元数和姿态角的换算

四旋翼飞行器的四元数矢量式为

$Q\left(t_k\right)=q_0+q_1\stackrel{\→}{i}+q_2\stackrel{\→}{j}+q_3\stackrel{\→}{k}---\left(7\right)$

四元数可确定出由四旋翼机体坐标_b系到地面坐标_n系的变换坐标矩阵

$C_b^n=\left[\begin{array}{ccc}1-2({q_2}^2+{q_3}^2)& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& 1-2({q_1}^2+{q_3}^2)& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& 1-2({q_1}^2+{q_2}^2)\end{array}\right]---\left(8\right)$

由变换坐标矩阵可得四旋翼飞行器的姿态角为

(4)更新四元数

在捷联式惯性导航系统中，陀螺仪的输出值一般是等时间采样周期内的角增量，为了避免积分的累积误差、降低噪声的微分放大，本文直接采用角增量来确定四元数，即毕卡四元数算法。

$\frac{\mathrm{dQ}\left(t_{k+1}\right)}{\mathrm{dt}}=\frac{1}{2}Q\left(t_k\right)\⊗\mathrm{\ω}\left(t_k\right)=\frac{1}{2}{M}^{\′}\left(\mathrm{\ω}\left(t_k\right)\right)Q\left(t_k\right)---\left(10\right)$

其中， $\mathrm{\ω}\left(t_k\right)=\left(\begin{array}{c}{\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right),$ 表示四元数乘法，其等效的换算矩阵为

$M^{\′}\left(\mathrm{\ω}\left(t_k\right)\right)=\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_x& -{\mathrm{\ω}}_y& -{\mathrm{\ω}}_z\\ {\mathrm{\ω}}_x& 0& {\mathrm{\ω}}_z& -{\mathrm{\ω}}_y\\ {\mathrm{\ω}}_y& -{\mathrm{\ω}}_z& 0& {\mathrm{\ω}}_x\\ {\mathrm{\ω}}_z& {\mathrm{\ω}}_y& -{\mathrm{\ω}}_x& 0\end{array}\right]$

求解式(10)的微分方程可得

$Q\left(t_{k+1}\right)=e^{\frac{1}{2}{\∫}_{t_k}^{t_{k+1}}{M}^{\′}\left(\mathrm{\ω}\left(t_k\right)\right)\mathrm{dt}}Q\left(t_k\right)---\left(11\right)$

进一步，

$Q\left(t_{k+1}\right)=e^{\frac{1}{2}\mathrm{\Δ\Θdt}}Q\left(t_k\right)---\left(12\right)$

其中， $\mathrm{\Δ\Θ}=\left[\begin{array}{cccc}0& -g_{\mathrm{bx}}& -g_{\mathrm{by}}& -g_{\mathrm{bz}}\\ g_{\mathrm{bx}}& 0& g_{\mathrm{bz}}& -g_{\mathrm{by}}\\ g_{\mathrm{by}}& -g_{\mathrm{bz}}& 0& g_{\mathrm{bx}}\\ g_{\mathrm{bz}}& g_{\mathrm{by}}& -g_{\mathrm{bx}}& 0\end{array}\right].$

对式(17)进行泰勒级数展开可得

$Q\left(t_{k+1}\right)=[I+\frac{\frac{1}{2}\mathrm{\Δ\Θ}}{1!}+\frac{{\left(\frac{1}{2}\mathrm{\Δ\Θ}\right)}^2}{2!}+...]Q\left(t_k\right)---\left(13\right)$

考虑到算法的精度和控制芯片的运算量，本文取三阶近似毕卡四元数算法，四元数求解如下：

$Q\left(t_{k+1}\right)==[I(1-\frac{{\mathrm{\Δ\θ}}^2}{8})+(\frac{1}{2}-\frac{{\mathrm{\Δ\θ}}^2}{48})\mathrm{\Δ\Θ}]Q\left(t_k\right)---\left(14\right)$

其中，Δθ²＝g_bx ²+g_by ²+g_bz ²。

步骤4)中还包括卡尔曼滤波，根据步骤3)中以上的姿态获取，可通过加速计和陀螺仪得到的姿态角，进而基于卡尔曼进行数据融合得到最终输出的姿态角。

作为优选的另一种方案：所述的步骤3)中还包括实时四旋翼飞行器监测控制台，姿态获取和数据融合的结果将在四旋翼飞行器监测控制台中显示。

与现有技术相比，本发明的优点在于：

(1)基于三阶近似毕卡四元数来进行姿态获取，避免了陀螺仪的积分累积误差，降低了噪声的微分放大。

(2)将解算得到的姿态角进行卡尔曼滤波，有效消除了四旋翼飞行器机体振动引起的传感器测量误差，得到更准确的姿态角。

附图说明

图1为本发明的四旋翼飞行器的硬件系统构成图。

图2为本发明的四旋翼飞行器姿态获取流程图

具体实施方式

为了使本发明的技术方案、设计思路能更加清晰，下面结合附图再进行详尽的描述。

一种采用三阶近似毕卡四元数的四旋翼飞行器姿态获取方法，包括以下步骤：

步骤1)为了验证算法的可行性，首先建立一个飞行器实验平台，所述平台的硬件系统以3个STM32F103VET6控制芯片为核心划分为三大部分。

第一部分是主控制器和惯性导航系统的电路设计。主控芯片釆用的是STM32，进行姿态获取和姿态控制。惯性导航系统是采集飞行器机体的运动状态信息作为控制系统控制的重要信息依据。惯性导航平台的反馈精度直接影响到控制系统的控制精度。本项目的惯性导航系统由6轴运动处理组件MPU6050、电子罗盘HMC5883和气压计MS5611组成。

第二部分是以STM32控制芯片，用OV7620数字摄像头做图像信息采集和四旋翼飞行器飞行数据处理的信息处理部分，与第一部分的芯片用USART接口进行通信，与第三部分用蓝牙模块进行通信。

第三部分是用STM32设计UC/OSII嵌入式系统下位机，使用蓝牙与四旋翼飞行器进行通信，用作人机交互以及图像数据传输的中间站，能够完成飞行器各个状态之间的切换以及数据的接收显示。

步骤2)数据的预处理，具体方案如下：

在飞行器的姿态控制中，姿态数据通常由传感器获取，但是单个传感器获取的信息往往不精确。因此，在进行姿态估计之前，需要对传感器的原始数据进行预处理，以下主要简述3部分：加速度计校准及滤波、陀螺仪静态偏置的计算(陀螺仪零偏)以及磁力计的偏心矫正及滤波。

(1)加速度计校准及滤波

由于加速度传感器的测量值必然存在误差，因此，有必要在使用前对其进行校准，使用时对其进行滤波。这里使用均值滤波及滑动平均滤波的方式对加速度计的测量信息进行处理。

(2)陀螺仪零偏计算

陀螺仪存在静态偏置即零偏，因此在每一次系统初始化时需要对其零偏进行计算并在之后的计算中除去零偏。先将陀螺仪静置，连续读取200次陀螺仪的测量值进行均值滤波，并将其作为陀螺仪的零偏。

(3)磁力计偏心矫正及倾斜补偿

理论上，磁力计的三个轴的测量值范围是对称的，即在空间中呈球体分布。然而，由于传感器的误差以及外界磁场的干扰，本文所得到的磁力计测量值是一个中心不在原点的椭球体，因此需要对其测量值进行偏心校正。基本步骤为：

a.将磁力计水平旋转360度计算其X轴与Y轴最大值与最小值，由此计算修正增益X_S与Y_S，修正偏置X_b与Y_b

这里以X轴为基准，令X_S＝1。

$Y_s=\frac{(X_{\max }-X_{\min })}{(Y_{\max }-Y_{\min })}---\left(15\right)$

$\left\{\begin{array}{c}{X}_b=X_s[\frac{1}{2}(X_{\max }-X_{\min })-X_{\max }]\\ Y_b=Y_s[\frac{1}{2}(Y_{\max }-Y_{\min })-Y_{\max }]\end{array}\right.$

b.X_in、Y_in和Z_in为磁力计的测量输出值，通过修正增益与修正偏置计算磁力计的修正值X_out和Y_out：

$\left\{\begin{array}{c}{X}_{\mathrm{out}}=X_s\×X_{\mathrm{in}}+X_b\\ Y_{\mathrm{out}}=Y_s\×Y_{\mathrm{in}}+Y_b\end{array}\right.---\left(17\right)$

c.同样以X轴为基准，对Z轴进行矫正，最终得到三个轴的修正值X_out、Y_out和Z_out：

$\left\{\begin{array}{c}{X}_{\mathrm{out}}=X_s\×X_{\mathrm{in}}+X_b\\ Y_{\mathrm{out}}=Y_s\×Y_{\mathrm{in}}+Y_b\\ Z_{\mathrm{out}}=Z_s\×Z_{\mathrm{in}}+Z_b\end{array}\right.---\left(18\right)$

步骤3)设计基于毕卡四元数的四旋翼飞行器姿态获取，过程包括：

设由四旋翼的机体轴确定的坐标系为b，地面坐标系为n，则由b系到n系的坐标矩阵称为四旋翼的姿态矩阵，θ、γ、分别为飞机的俯仰、横滚、偏航角，加速度计输出的原始值为速度的增量即a_bx、a_by、a_bz，陀螺仪计输出为角增量即g_bx、g_by、g_bz，磁力计m_bx、m_by、m_bz。

(1)欧拉角初始化

(2)四元数初始值的确定及规范化处理

四旋翼飞行器的四元数的初始值Q(0)由捷联系统的初值状态对准确定。设初始对准的姿态阵为可由式(1)中求得的公式计算得出初始姿态角求得。

$C_{b0}^n=\left[\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{21}& c_{22}& c_{23}\\ c_{31}& c_{32}& c_{33}\end{array}\right]---\left(2\right)$

其中，c₁₁、c₁₂、c₁₃、c₂₁、c₂₂、c₂₃、c₃₁、c₃₂、c₃₃分别为

则四旋翼飞行器的四元数的初始值为：

$Q\left(0\right)=q_0+q_1\stackrel{\→}{i}+q_2\stackrel{\→}{j}+q_3\stackrel{\→}{k}---\left(4\right)$

其中，q₀、q₁、q₂、q₃的数值和符号可按下式(5)和(6)确定。

$\left\{\begin{array}{c}\left|q_1\right|=\frac{1}{2}\sqrt{1+c_{11}-c_{22}-c_{33}}\\ \left|q_2\right|=\frac{1}{2}\sqrt{1-c_{11}+c_{22}-c_{33}}\\ \left|q_3\right|=\frac{1}{2}\sqrt{1-c_{11}-c_{22}-c_{33}}\\ \left|q_0\right|=\frac{1}{2}\sqrt{1+c_{11}+c_{22}+c_{33}}\end{array}\right.---\left(5\right)$

$\left\{\begin{array}{c}\mathrm{sign}\left(q_1\right)=\mathrm{sign}\left(q_0\right)\left[\mathrm{sign}(c_{32}-c_{23})\right]\\ \mathrm{sign}\left(q_2\right)=\mathrm{sign}\left(q_0\right)\left[\mathrm{sign}(c_{13}-c_{31})\right]\\ \mathrm{sign}\left(q_3\right)=\mathrm{sign}\left(q_0\right)\left[\mathrm{sign}(c_{21}-c_{12})\right]\end{array}\right.---\left(6\right)$

上式中，sign(q₀)可任选，因为四元数表征刚体从初始位置到最终位置的一次性等效旋转，并不反应从初始位置到最终位置的中间过程。

(3)四元数和姿态角的换算

四旋翼飞行器的四元数矢量式为

$Q\left(t_k\right)=q_0+q_1\stackrel{\→}{i}+q_2\stackrel{\→}{j}+q_3\stackrel{\→}{k}---\left(7\right)$

四元数可确定出由四旋翼机体坐标_b系到地面坐标n系的变换坐标矩阵

$C_b^n=\left[\begin{array}{ccc}1-2({q_2}^2+{q_3}^2)& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& 1-2({q_1}^2+{q_3}^2)& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& 1-2({q_1}^2+{q_2}^2)\end{array}\right]---\left(8\right)$

由变换坐标矩阵可得四旋翼飞行器的姿态角为

(4)更新四元数

在捷联式惯性导航系统中，陀螺仪的输出值一般是等时间采样周期内的角增量，为了避免积分的累积误差、降低噪声的微分放大，本文直接采用角增量来确定四元数，即毕卡四元数算法。

$\frac{\mathrm{dQ}\left(t_{k+1}\right)}{\mathrm{dt}}=\frac{1}{2}Q\left(t_k\right)\⊗\mathrm{\ω}\left(t_k\right)=\frac{1}{2}{M}^{\′}\left(\mathrm{\ω}\left(t_k\right)\right)Q\left(t_k\right)---\left(10\right)$

其中， $\mathrm{\ω}\left(t_k\right)=\left(\begin{array}{c}{\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right),$ 表示四元数乘法，其等效的换算矩阵为

$M^{\′}\left(\mathrm{\ω}\left(t_k\right)\right)=\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_x& -{\mathrm{\ω}}_y& -{\mathrm{\ω}}_z\\ {\mathrm{\ω}}_x& 0& {\mathrm{\ω}}_z& -{\mathrm{\ω}}_y\\ {\mathrm{\ω}}_y& -{\mathrm{\ω}}_z& 0& {\mathrm{\ω}}_x\\ {\mathrm{\ω}}_z& {\mathrm{\ω}}_y& -{\mathrm{\ω}}_x& 0\end{array}\right]$

求解式(10)的微分方程可得

$Q\left(t_{k+1}\right)=e^{\frac{1}{2}{\∫}_{t_k}^{t_{k+1}}{M}^{\′}\left(\mathrm{\ω}\left(t_k\right)\right)\mathrm{dt}}Q\left(t_k\right)---\left(11\right)$

进一步，

$Q\left(t_{k+1}\right)=e^{\frac{1}{2}\mathrm{\Δ\Θdt}}Q\left(t_k\right)---\left(12\right)$

其中， $\mathrm{\Δ\Θ}=\left[\begin{array}{cccc}0& -g_{\mathrm{bx}}& -g_{\mathrm{by}}& -g_{\mathrm{bz}}\\ g_{\mathrm{bx}}& 0& g_{\mathrm{bz}}& -g_{\mathrm{by}}\\ g_{\mathrm{by}}& -g_{\mathrm{bz}}& 0& g_{\mathrm{bx}}\\ g_{\mathrm{bz}}& g_{\mathrm{by}}& -g_{\mathrm{bx}}& 0\end{array}\right].$

对式(17)进行泰勒级数展开可得

$Q\left(t_{k+1}\right)=[I+\frac{\frac{1}{2}\mathrm{\Δ\Θ}}{1!}+\frac{{\left(\frac{1}{2}\mathrm{\Δ\Θ}\right)}^2}{2!}+...]Q\left(t_k\right)---\left(13\right)$

考虑到算法的精度和控制芯片的运算量，本文取三阶近似毕卡四元数算法，四元数求解如下：

$Q\left(t_{k+1}\right)==[I(1-\frac{{\mathrm{\Δ\θ}}^2}{8})+(\frac{1}{2}-\frac{{\mathrm{\Δ\θ}}^2}{48})\mathrm{\Δ\Θ}]Q\left(t_k\right)---\left(14\right)$

其中，Δθ²＝g_bx ²+g_by ²+g_bz ²。

步骤4)中还包括卡尔曼滤波，根据步骤3)中以上的姿态获取，可通过加速计和陀螺仪得到的姿态角，进而基于卡尔曼进行数据融合得到最终输出的姿态角。

参照图1和图2一种采用三阶近似毕卡四元数的四旋翼飞行器姿态获取方法，所述方法包括：

如图1所示为四旋翼飞行器的硬件系统构成图。本平台的硬件系统以3个STM32F103VET6控制芯片为核心划分为三大部分。第一部分是主控制器和惯性导航系统的电路设计。主控芯片釆用的是STM32，进行姿态获取和姿态控制。惯性导航系统是采集飞行器机体的运动状态信息作为控制系统控制的重要信息依据。惯性导航平台的反馈精度直接影响到控制系统的控制精度。本项目的惯性导航系统由6轴运动处理组件MPU6050、电子罗盘HMC5883和气压计MS5611组成。第二部分是以STM32控制芯片，用OV7620数字摄像头做图像信息采集和四旋翼飞行器飞行数据处理的信息处理部分，与第一部分的芯片用USART接口进行通信，与第三部分用蓝牙模块进行通信。第三部分是用STM32设计UC/OSII嵌入式系统下位机，使用蓝牙模块与四旋翼飞行器进行通信，用作人机交互以及图像数据传输的中间站，能够完成飞行器各个状态之间的切换以及数据的接收显示。

如图2为本发明的四旋翼飞行器姿态获取流程图。其中包括了传感器数据预处理、欧拉角初始化、四元数初始化、更新四元数、四元数规范化、姿态角更新以及卡尔曼滤波姿态估计，清晰地展示了算法的执行流程。

Claims (1)

1.一种采用三阶近似毕卡四元数的四旋翼飞行器姿态获取方法，包括以下步骤：

步骤1)为了验证算法的可行性，首先建立一个飞行器实验平台；将飞行器实验平台的硬件系统以3个STM32F103VET6控制芯片为核心划分为三大部分；

第一部分是主控制器和惯性导航系统的电路设计；主控芯片釆用的是STM32，进行姿态获取和姿态控制；惯性导航系统是采集飞行器机体的运动状态信息作为控制系统控制的重要信息依据；惯性导航平台的反馈精度直接影响到控制系统的控制精度；惯性导航系统由6轴运动处理组件MPU6050、电子罗盘HMC5883和气压计MS5611组成；

第二部分是以STM32控制芯片，用OV7620数字摄像头做图像信息采集和四旋翼飞行器飞行数据处理的信息处理部分，与第一部分的芯片用USART接口进行通信，与第三部分用蓝牙模块进行通信；

第三部分是用STM32设计UC/OSII嵌入式系统下位机，使用蓝牙模块与四旋翼飞行器进行通信，用作人机交互以及图像数据传输的中间站，能够完成飞行器各个状态之间的切换以及数据的接收显示；

步骤2)数据的预处理，具体方案如下：

在飞行器的姿态控制中，姿态数据通常由传感器获取，但是单个传感器获取的信息往往不精确；因此，在进行姿态估计之前，需要对传感器的原始数据进行预处理：加速度计校准及滤波、陀螺仪静态偏置的计算(陀螺仪零偏)以及磁力计的偏心矫正及滤波；

(21)加速度计校准及滤波

由于加速度传感器的测量值必然存在误差，因此，有必要在使用前对其进行校准，使用时对其进行滤波；这里使用均值滤波及滑动平均滤波的方式对加速度计的测量信息进行处理；

(22)陀螺仪零偏计算

陀螺仪存在静态偏置即零偏，因此在每一次系统初始化时需要对其零偏进行计算并在之后的计算中除去零偏；先将陀螺仪静置，连续读取200次陀螺仪的测量值进行均值滤波，并将其作为陀螺仪的零偏；

(23)磁力计偏心矫正及倾斜补偿

理论上，磁力计的三个轴的测量值范围是对称的，即在空间中呈球体分布；然而，由于传感器的误差以及外界磁场的干扰，本文所得到的磁力计测量值是一个中心不在原点的椭球体，因此需要对其测量值进行偏心校正；基本步骤为：

a.将磁力计水平旋转360度计算其X轴与Y轴最大值与最小值，由此计算修正增益X_S与Y_S，修正偏置X_b与Y_b

这里以X轴为基准，令X_S＝1；

$Y_s=\frac{(X_{\max }-X_{\min })}{(Y_{\max }-Y_{\min })}---\left(15\right)$

$\left\{\begin{array}{c}{X}_b=X_s[\frac{1}{2}(X_{\max }-X_{\min })-X_{\max }]\\ Y_b=Y_s[\frac{1}{2}(Y_{\max }-Y_{\min })-Y_{\max }]\end{array}\right.---\left(16\right)$

b.X_in、Y_in和Z_in为磁力计的测量输出值，通过修正增益与修正偏置计算磁力计的修正值X_out和Y_out：

$\left\{\begin{array}{c}{X}_{\mathrm{out}}=X_s\×X_{\mathrm{in}}+X_b\\ Y_{\mathrm{out}}=Y_s\×Y_{\mathrm{in}}+Y_b\end{array}\right.---\left(17\right)$

c.同样以X轴为基准，对Z轴进行矫正，最终得到三个轴的修正值X_out、Y_out和Z_out：

$\left\{\begin{array}{c}{X}_{\mathrm{out}}=X_s\×X_{\mathrm{in}}+X_b\\ Y_{\mathrm{out}}=Y_s\×Y_{\mathrm{in}}+Y_b\\ Z_{\mathrm{out}}=Z_s\×Z_{\mathrm{in}}+Z_b\end{array}\right.---\left(18\right)$

步骤3)设计基于毕卡四元数的四旋翼飞行器姿态获取，过程包括：

设由四旋翼的机体轴确定的坐标系为b，地面坐标系为n，则由b系到n系的坐标矩阵称为四旋翼的姿态矩阵，θ、γ、分别为飞机的俯仰、横滚、偏航角，加速度计输出的原始值为速度的增量即a_bx、a_by、a_bz，陀螺仪计输出的原始值为角增量即g_bx、g_by、g_bz，磁力计m_bx、m_by、m_bz；

(31)欧拉角初始化

(32)四元数初始值的确定及规范化处理

四旋翼飞行器的四元数的初始值Q(0)由捷联系统的初值状态对准确定；设初始对准的姿态阵为可由式(1)中求得的公式计算得出初始姿态角求得；

$C_{b0}^n=\left[\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{21}& c_{22}& c_{23}\\ c_{31}& c_{32}& c_{33}\end{array}\right]---\left(2\right)$

其中，c₁₁、c₁₂、c₁₃、c₂₁、c₂₂、c₂₃、c₃₁、c₃₂、c₃₃分别为

则四旋翼飞行器的四元数的初始值为：

$Q\left(0\right)=q_0+q_1\stackrel{\→}{i}+q_2\stackrel{\→}{j}+q_3\stackrel{\→}{k}---\left(4\right)$

其中，q₀、q₁、q₂、q₃的数值和符号可按下式(5)和(6)确定；

$\left\{\begin{array}{c}\left|q_1\right|=\frac{1}{2}\sqrt{1+c_{11}-c_{22}-c_{33}}\\ \left|q_2\right|=\frac{1}{2}\sqrt{1-c_{11}+c_{22}-c_{33}}\\ \left|q_3\right|=\frac{1}{2}\sqrt{1-c_{11}-c_{22}-c_{33}}\\ \left|q_0\right|=\frac{1}{2}\sqrt{1+c_{11}+c_{22}+c_{33}}\end{array}\right.---\left(5\right)$

$\left\{\begin{array}{c}\mathrm{sign}\left(q_1\right)=\mathrm{sign}\left(q_0\right)\left[\mathrm{sign}(c_{32}-c_{23})\right]\\ \mathrm{sign}\left(q_2\right)=\mathrm{sign}\left(q_0\right)\left[\mathrm{sign}(c_{13}-c_{31})\right]\\ \mathrm{sign}\left(q_3\right)=\mathrm{sign}\left(q_0\right)\left[\mathrm{sign}(c_{21}-c_{12})\right]\end{array}\right.---\left(6\right)$

上式中，sign(q₀)可任选，因为四元数表征刚体从初始位置到最终位置的一次性等效旋转，并不反应从初始位置到最终位置的中间过程；

(33)四元数和姿态角的换算

四旋翼飞行器的四元数矢量式为

$Q\left(t_k\right)=q_0+q_1\stackrel{\→}{i}+q_2\stackrel{\→}{j}+q_3\stackrel{\→}{k}---\left(7\right)$

四元数可确定出由四旋翼机体坐标_b系到地面坐标_n系的变换坐标矩阵

$C_b^n=\left[\begin{array}{ccc}1-2({q_2}^2+{q_3}^2)& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& 1-2({q_1}^2+{q_3}^2)& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& 1-2({q_1}^2+{q_2}^2)\end{array}\right]---\left(8\right)$

由变换坐标矩阵可得四旋翼飞行器的姿态角为

(34)更新四元数

在捷联式惯性导航系统中，陀螺仪的输出值一般是等时间采样周期内的角增量，为了避免积分的累积误差、降低噪声的微分放大，本文直接采用角增量来确定四元数；即毕卡四元数算法；

$\frac{\mathrm{dQ}\left(t_{k+1}\right)}{\mathrm{dt}}=\frac{1}{2}Q\left(t_k\right)\⊗\mathrm{\ω}\left(t_k\right)=\frac{1}{2}{M}^{\′}\left(\mathrm{\ω}\left(t_k\right)\right)Q\left(t_k\right)---\left(10\right)$

其中， $\mathrm{\ω}\left(t_k\right)=\left(\begin{array}{c}{\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right),$ 表示四元数乘法，其等效的换算矩阵为

$M^{\′}\left(\mathrm{\ω}\left(t_k\right)\right)=\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_x& -{\mathrm{\ω}}_y& -{\mathrm{\ω}}_z\\ {\mathrm{\ω}}_x& 0& {\mathrm{\ω}}_z& -{\mathrm{\ω}}_y\\ {\mathrm{\ω}}_y& -{\mathrm{\ω}}_z& 0& {\mathrm{\ω}}_x\\ {\mathrm{\ω}}_z& {\mathrm{\ω}}_y& -{\mathrm{\ω}}_x& 0\end{array}\right]$

求解式(10)的微分方程可得

$Q\left(t_{k+1}\right)=e^{\frac{1}{2}{\∫}_{t_k}^{t_k+1}{M}^{\′}\left(\mathrm{\ω}\left(t_k\right)\right)\mathrm{dt}}Q\left(t_k\right)---\left(11\right)$

进一步，

$Q\left(t_{k+1}\right)=e^{\frac{1}{2}\mathrm{\Δ\Θdt}}Q\left(t_k\right)---\left(12\right)$

其中， $\mathrm{\Δ\Θ}=\left[\begin{array}{cccc}0& -g_{\mathrm{bx}}& -g_{\mathrm{by}}& -g_{\mathrm{bz}}\\ g_{\mathrm{bx}}& 0& g_{\mathrm{bz}}& -g_{\mathrm{by}}\\ g_{\mathrm{by}}& -g_{\mathrm{bz}}& 0& g_{\mathrm{bx}}\\ g_{\mathrm{bz}}& g_{\mathrm{by}}& -g_{\mathrm{bx}}& 0\end{array}\right];$

对式(17)进行泰勒级数展开可得

$Q\left(t_{k+1}\right)=[I+\frac{\frac{1}{2}\mathrm{\Δ\Θ}}{1!}+\frac{{\left(\frac{1}{2}\mathrm{\Δ\Θ}\right)}^2}{2!}+...]Q\left(t_k\right)---\left(13\right)$

考虑到算法的精度和控制芯片的运算量，取三阶近似毕卡四元数算法，四元数求解如下：

$Q\left(t_{k+1}\right)==[I(1-\frac{{\mathrm{\Δ\θ}}^2}{8})+(\frac{1}{2}-\frac{{\mathrm{\Δ\θ}}^2}{48})\mathrm{\Δ\Θ}]Q\left(t_k\right)---\left(14\right)$

其中，Δθ²＝g_bx ²+g_by ²+g_bz ²；

(4)卡尔曼滤波，根据步骤3)中以上的姿态获取，可通过加速计和陀螺仪得到的姿态角，进而基于卡尔曼进行数据融合得到最终输出的姿态角。