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

权利要求书

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

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

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

第二部分是由三个子模块组成，其中包括STM32F103VET6芯片，用OV7620数字摄像头做图像信息采集的模块和对四旋翼飞行器飞行数据进行处理的信息处理模块；第二部分的STM32F103VET6芯片与第一部分的STM32F103VET6芯片用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}_{out}=X_s\×X_{in}+X_b\\ Y_{out}=Y_s\×Y_{in}+Y_b\end{array}\right.---\left(17\right)$

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

$\left\{\begin{array}{c}{X}_{out}=X_s\×X_{in}+X_b\\ Y_{out}=Y_s\×Y_{in}+Y_b\\ Z_{out}=Z_s\×Z_{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)的计算结果代入式(3)，得出式(2)的初始姿态角；

$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}sign\left(q_1\right)=sign\left(q_0\right)\[sign(c_{32}-c_{23})\]\\ sign\left(q_2\right)=sign\left(q_0\right)\[sign(c_{13}-c_{31})\]\\ sign\left(q_3\right)=sign\left(q_0\right)\[sign(c_{21}-c_{12})\]\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{dQ\left(t_{k+1}\right)}{dt}=\frac{1}{2}Q\left(t_k\right)\⊗\mathrm{\ω}\left(t_k\right)=\frac{1}{2}{M}^{\′}\left(\mathrm{\ω}\right(t_k\left)\right)Q\left(t_k\right)---\left(10\right)$

其中，表示毕卡四元数乘法，其等效的换算矩阵为

$M^{\′}\left(\mathrm{\ω}\right(t_k\left)\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{\ω}\right(t_k\left)\right)dt}Q\left(t_k\right)---\left(11\right)$

进一步，

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

其中，

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

$Q\left(t_{k+1}\right)=\[I+\frac{\frac{1}{2}\mathrm{\Δ}\mathrm{\Θ}}{1!}+\frac{{\left(\frac{1}{2}\mathrm{\Δ}\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{\Δ}\mathrm{\Θ}\]Q\left(t_k\right)---\left(14\right)$

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

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