CN106342284B - 一种飞行载体姿态确定方法 - Google Patents

Abstract

本发明公开了一种飞行载体姿态确定方法，属姿态测量领域。主要步骤包括：建立飞行载体的坐标系及姿态矩阵；采集传感器信号；利用陀螺测量值进行捷联姿态解算，得到飞行载体的姿态四元数及姿态角；利用磁强计测量值解算飞行载体航向角；利用速度信息对加速度测量值补偿，求解俯仰角和横滚角；求解由磁强计及加速度计信息确定的姿态四元数；设计卡尔曼滤波器，估计状态向量；校正姿态四元数及提取姿态角。本方法将陀螺、加速度计、磁强计、速度传感器组合，满足飞行载体实时性姿态控制的要求；将陀螺漂移引入到***状态向量中，对其实时估计校正，同时采用磁强计测量值直接计算航向角，避免地磁场矢量在地理坐标系下的计算，提高了姿态确定的精度。

Description

一种飞行载体姿态确定方法

技术领域

本发明涉及一种飞行载体姿态确定方法，属于姿态测量领域。

背景技术

姿态确定是姿态控制的前提，对于飞机、导弹、卫星姿态控制具有重要意义。姿态确定是将传感器测量信息，经过数据处理，得到飞行载体相对于空间某坐标系的姿态。利用陀螺和加速度计测量信息进行捷联姿态确定，具有自主性、抗干扰能力强等特点，但精度主要取决于惯性器件的精度，且陀螺漂移将导致姿态确定误差随时间积累，不能满足长时间高精度姿态确定的应用要求。为了解决这一问题，人们将陀螺、加速度计和磁强计组合，利用重力和地磁场矢量作为参考，设计组合卡尔曼滤波器，校正由陀螺测量信息解算的姿态，抑制了陀螺漂移导致的姿态确定误差的积累，然而，当飞行载体处于大机动环境下，加速度计测量的比力包含了重力矢量和飞行载体本身的加速度及干扰加速度，单纯依靠加速度计很难从比力中将所需的重力矢量分离出来，即惯性力与引力不可区分原理，这将导致姿态确定误差迅速增大。为此，人们根据飞行载体不同运动状态设定不同算法或在线调整卡尔曼滤波噪声方差阵，增大在大机动环境下对利用陀螺测量解算的姿态信息的依赖度，减小对量测信息的可信度，这对陀螺的性能提出了较高的要求。申请号为200510011763.7的专利“微小型动态载体姿态测量装置及其测量方法”提出了一种微小型载体姿态测量装置及方法，该测量方法的关键是改进了算法设计，在姿态确定算法设计中状态向量取为姿态矩阵元素，基于姿态矩阵微分方程建立了卡尔曼滤波状态方程，卡尔曼滤波量测值为磁强计和加速度计的测量值。由于引入了速度传感器信息，可消除运动载体本身的加速度对重力矢量测量的影响，提高了运动载体在大机动环境下姿态确定的精度。此方法虽然能够解决运动载体加速度对重力矢量测量的影响问题，但也存在一些明显的不足：其测量算法未将陀螺漂移作为***状态进行估计校正，降低了陀螺测量精度及姿态确定精度；地磁场矢量在惯性坐标系下采用简化的固定模型，导致其测量值和估计值之间的偏差较大，从而降低了姿态确定精度。

发明内容

为了克服现有技术的上述缺陷，本发明提出了一种飞行载体姿态确定方法，实现飞行载体姿态确定及陀螺漂移实时估计校正，提高姿态确定精度，实现该方法的具体步骤为：

步骤一：建立飞行载体的坐标系及姿态矩阵。参阅图1，机体坐标系b系中X_b，Y_b，Z_b三个正交轴分别沿飞行载体的横轴向右、纵轴向前、竖轴向上。地理坐标系n系，X_n，Y_n，Z_n三个正交轴分别指向东、北、天。姿态角定义为：航向角ψ为(-180°，180°)，俯仰角θ为(-90°，90°)，横滚角γ为(-180°，180°)。按照航向-俯仰-横滚的旋转顺序，得到飞行载体的姿态角形式的姿态矩阵为：

$C_n^b=\left[\begin{array}{ccc}c\mathrm{\γ}c\mathrm{\ψ}+s\mathrm{\γ}s\mathrm{\θ}s\mathrm{\ψ}& -c\mathrm{\γ}s\mathrm{\ψ}+s\mathrm{\γ}s\mathrm{\θ}c\mathrm{\ψ}& -s\mathrm{\γ}c\mathrm{\θ}\\ c\mathrm{\θ}s\mathrm{\ψ}& c\mathrm{\θ}c\mathrm{\ψ}& s\mathrm{\θ}\\ s\mathrm{\γ}c\mathrm{\ψ}-c\mathrm{\γ}s\mathrm{\θ}s\mathrm{\ψ}& -s\mathrm{\γ}s\mathrm{\ψ}-c\mathrm{\γ}s\mathrm{\θ}c\mathrm{\ψ}& c\mathrm{\γ}c\mathrm{\θ}\end{array}\right]---\left(1\right)$

其中s和c分别为函数sin和cos的简写，定义飞行载体姿态四元数Q＝[q₀ q₁ q₂ q₃]^T，得到四元数形式的姿态矩阵为：

$C_n^b=\left[\begin{array}{ccc}{q}_0^2+q_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)& q_0^2-q_1^2+q_2^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)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]---\left(2\right)$

步骤二：采集传感器信号。利用陀螺、加速度计、单轴速度传感器分别测量飞行载体的角速度、加速度及速度，经过A/D转换得到角速度ω_b＝[ω_bx ω_by ω_bz]^T和加速度f_b＝[f_bx f_by f_bz]^T及速度v_y，利用磁强计得到地磁场矢量m_b＝[m_bx m_by m_bz]^T。

步骤三：利用角速度ω_b，采用捷联姿态求解算法，求取利用角速度ω_b解算的飞行载体姿态四元数Q_I和俯仰角θ_I及横滚角γ_I。

步骤四：利用地磁场矢量m_b求解飞行载体的航向角ψ_S。利用步骤三获得的俯仰角θ_I和横滚角γ_I建立机体坐标系b系和水平坐标系h系之间的转换矩阵，将地磁场矢量m_b转换到水平坐标系h系中，解算出飞行载体的磁航向角，经过磁偏角修正得到航向角ψ_S。

步骤五：利用速度v_y对加速度f_b补偿，得到重力矢量在机体坐标系b系中的投影值，利用重力矢量在地理坐标系n系和机体坐标系b系之间的转换关系，解算出飞行载体的俯仰角θ_S和横滚角γ_S。

当飞行载体处于静止或加速度很小的状态下，忽略加速度的影响，则加速度f_b近似认为是重力矢量在机体坐标系b系中的投影值，当飞行载体处于大机动运动环境下，如变速度飞行及转弯状态下，加速度f_b将不再反映真实的重力矢量，因此，对加速度f_b利用速度v_y补偿如下：

$\left\{\begin{array}{c}{f}_{bx}^{\′}=f_{bx}+{\widehat{\mathrm{\ω}}}_{bz}\·v_y\\ f_{by}^{\′}=f_{by}-{\stackrel{\·}{v}}_y\end{array}\right.---\left(3\right)$

其中f′_bx和f′_by分别为经过由飞行载体的变速运动及转动引起的线加速度和向心加速度补偿后的加速度值，为沿Z_b轴的陀螺经过陀螺漂移补偿后的角速度，v_y为沿飞行载体机体坐标系Y_b轴放置的单轴速度传感器测量值，则求取横滚角γ_S和俯仰角θ_S为：

$\left\{\begin{array}{c}{\mathrm{\γ}}_S=a\tan (-f_{bx}^{\′}/f_{bz})\\ {\mathrm{\θ}}_S=a\tan \left(\right(-f_{by}^{\′}/f_{bz}\left){\mathrm{sin\γ}}_S\right)\end{array}\right.---\left(4\right)$

步骤六：对于航向角ψ_S、俯仰角θ_S及横滚角γ_S，利用姿态四元数与姿态角之间的关系，求取由地磁场矢量m_b和加速度f_b解算的飞行载体姿态四元数Q_S。

步骤七：采用卡尔曼滤波技术，设计卡尔曼滤波器，对利用角速度ω_b解算的飞行载体姿态四元数Q_I、利用地磁场矢量m_b和加速度f_b解算的飞行载体姿态四元数Q_S、陀螺测量值ω_b，进行卡尔曼滤波的数据处理，估计出利用角速度ω_b解算的飞行载体姿态四元数Q_I对应的姿态误差四元数和陀螺漂移误差包括如下子步骤：

①建立***卡尔曼滤波状态方程

陀螺测量模型建立为：ω_b＝ω+b+w_g，其中ω_b为陀螺输出值，ω为真实角速率，w_g为陀螺漂移白噪声，b为陀螺漂移，满足方程w_b为陀螺漂移随机游走白噪声

定义利用角速度ω_b解算的飞行载体姿态四元数Q_I对应的姿态误差四元数为其中真实的飞行载体姿态四元数为Q，则Q可以表示为：

$Q=Q_I\⊗Q_{Ie}---\left(5\right)$

对(5)式两边求微分，得：

$\stackrel{\·}{Q}={\stackrel{\·}{Q}}_I\⊗Q_{Ie}+Q_I\⊗{\stackrel{\·}{Q}}_{Ie}---\left(6\right)$

${\stackrel{\·}{Q}}_{Ie}=\frac{1}{2}{Q}_{Ie}\⊗{\mathrm{\ω}}_b-\frac{1}{2}{\widehat{\mathrm{\ω}}}_b\⊗Q_{Ie}---\left(7\right)$

则：

${\stackrel{\·}{\stackrel{\→}{q}}}_e=-\[{\widehat{\mathrm{\ω}}}_b\×\]\·{\stackrel{\→}{q}}_e-\frac{1}{2}\mathrm{\Δ}b-\frac{1}{2}{w}_g---\left(8\right)$

其中为陀螺漂移估计误差，b为陀螺真实漂移，为陀螺漂移估计值，为的斜对称矩阵，为角速度ω_b经过陀螺漂移补偿后的值，建立***卡尔曼滤波状态方程为：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)\·X\left(t\right)+G\left(t\right)\·W\left(t\right)---\left(9\right)$

其中X(t)为t时刻的***状态向量，F(t)和G(t)为t时刻***状态转移矩阵及噪声矩阵，W(t)为***噪声向量。

***状态向量：X(t)＝[q_e1 q_e2 q_e3 Δb_x Δb_y Δb_z]^T

***噪声向量：W(t)＝[w_gx w_gy w_gz w_bx w_by w_bz]^T

其中q_e1、q_e2、q_e3为姿态误差四元数Q_Ie的三个分量，Δb_x、Δb_y、Δb_z为陀螺漂移估计误差，w_gx、w_gy、w_gz为陀螺漂移白噪声，w_bx、w_by、w_bz为陀螺漂移随机游走白噪声，***状态转移矩阵F(t)和噪声矩阵G(t)为：

$F\left(t\right)=\left[\begin{array}{cc}-\[{\widehat{\mathrm{\ω}}}_b\×\]& -\frac{1}{2}{I}_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\end{array}\right],G\left(t\right)=\left[\begin{array}{cc}-\frac{1}{2}{I}_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& I_{3\×3}\end{array}\right]$

式中，I表示单位矩阵，0表示全零矩阵，下标表示矩阵的维数。

②建立***卡尔曼滤波量测方程

定义利用地磁场矢量m_b和加速度f_b解算的飞行载体姿态四元数Q_S对应的姿态误差四元数为Q_Se＝[1 q_se1 q_se2 q_se3]^T，则：

$\left\{\begin{array}{c}Q=Q_I\⊗Q_{Ie}\\ Q=Q_S\⊗Q_{Se}\end{array}\right.---\left(10\right)$

定义向量由式(10)得：

$Y=Q_S^{-1}\⊗Q_I=Q_{Se}\⊗Q_{Ie}^{-1}=\left[\begin{array}{cccc}1& -q_{se1}& -q_{se2}& -q_{se3}\\ q_{se1}& 1& -q_{se3}& q_{se2}\\ q_{se2}& q_{se3}& 1& -q_{se1}\\ q_{se3}& -q_{se2}& q_{se1}& 1\end{array}\right]\·\left[\begin{array}{c}1\\ -q_{e1}\\ -q_{e2}\\ -q_{e3}\end{array}\right]---\left(11\right)$

忽略二阶项，得到***卡尔曼滤波量测方程为：

$Z\left(t\right)=\left[\begin{array}{c}Y\left(2\right)\\ Y\left(3\right)\\ Y\left(4\right)\end{array}\right]=\left[\begin{array}{c}{q}_{se1}-q_{e1}\\ q_{se2}-q_{e2}\\ q_{se3}-q_{e3}\end{array}\right]=H\left(t\right)X\left(t\right)+V\left(t\right)---\left(12\right)$

其中量测矩阵H(t)＝[-I_3×3 0_3×3]，V(t)为量测噪声，近似为白噪声。

③***卡尔曼滤波方程离散化及状态估计

对①和②建立的***卡尔曼滤波方程进行离散化，得到离散化卡尔曼滤波状态方程和量测方程为：

$\left\{\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+{\mathrm{\Γ}}_{k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right.---\left(13\right)$

其中一步转移阵***噪声驱动阵T为离散化周期，***噪声方差阵Q_k和量测噪声方差阵R_k为：

$Q_k=diag\left[\begin{array}{cccccc}{\mathrm{\σ}}_{gx}^2& {\mathrm{\σ}}_{gy}^2& {\mathrm{\σ}}_{gz}^2& {\mathrm{\σ}}_{bx}^2& {\mathrm{\σ}}_{bx}^2& {\mathrm{\σ}}_{bx}^2\end{array}\right]---\left(14\right)$

R_k＝diag[δq_e1 ² δq_e2 ² δq_e3 ²] (15)

其中σ_gx，σ_gy，σ_gz为陀螺漂移白噪声均方根，σ_bx，σ_by，σ_bz为陀螺漂移随机游走白噪声均方根，δq_e1，δq_e2，δq_e3为飞行载体姿态四元数估计误差，对利用角速度ω_b解算的飞行载体姿态四元数Q_I和地磁场矢量m_b及加速度f_b解算的飞行载体姿态四元数Q_S，通过(11)式计算卡尔曼滤波量测值Z_k，采用离散型卡尔曼滤波基本方程进行滤波估计，得到利用角速度ω_b解算的飞行载体姿态四元数Q_I对应的姿态误差四元数Q_Ie的估计值及陀螺漂移估计误差

步骤八：校正利用角速度ω_b解算的飞行载体姿态四元数Q_I及提取飞行载体姿态角，即航向角ψ、俯仰角θ及横滚角γ。利用步骤七中估计出的姿态误差四元数校正利用角速度ω_b解算的飞行载体姿态四元数Q_I，得到当前时刻飞行载体姿态四元数最优估计值

$\hat{Q}=Q_I\⊗{\hat{Q}}_{Ie}---\left(16\right)$

利用获得的得到对应的姿态矩阵，利用姿态矩阵提取飞行载体姿态角，即航向角ψ、俯仰角θ及横滚角γ。对经过卡尔曼滤波得到的陀螺漂移估计误差获得陀螺漂移估计值采用校正陀螺测量值，从而提高陀螺测量精度。将作为下一时刻姿态解算四元数的初始值，返回步骤二采集传感器信号，进行下一时刻的姿态解算，循环计算。

本发明的有益效果是：1)将陀螺、加速度计、微磁强计、速度传感器组合，从而满足了飞行载体实时性姿态控制的要求。2)将陀螺漂移引入到***状态向量中，对其进行实时估计校正，从而提高了陀螺测量精度和姿态确定精度。3)采用磁强计测量值直接计算航向角，避免了地磁场矢量在地理坐标系下的计算，提高了姿态确定的精度。

附图说明

图1是飞行载体的机体坐标系b系和地理坐标系n系

图2是本发明的飞行载体姿态确定原理图

图3是本发明的飞行载体姿态确定方法实现流程图

具体实施方式

本实施例采用基于微机电技术的三轴速率陀螺、三轴加速度计、三轴磁强计、一个单轴速度传感器作为***传感器，采集传感器信号，进行飞行载体姿态确定，参照图3飞行载体姿态确定方法实现流程，具体完成当前时刻的飞行载体的姿态解算需要完成以下步骤：

步骤一：建立飞行载体的坐标系及姿态矩阵。参阅图1，机体坐标系b系中X_b，Y_b，Z_b三个正交轴分别沿飞行载体的横轴向右、纵轴向前、竖轴向上。地理坐标系n系中X_n，Y_n，Z_n三个正交轴分别指向东、北、天。三轴速率陀螺、三轴加速度计、三轴磁强计分别沿飞行载体机体坐标系三轴放置，单轴速度传感器沿机体坐标系Y_b轴放置，姿态角定义为：航向角ψ为(-180°，180°)，俯仰角θ为(-90°，90°)，横滚角γ为(-180°，180°)，则姿态角形式的姿态矩阵为：

$C_n^b=\left[\begin{array}{ccc}c\mathrm{\γ}c\mathrm{\ψ}+s\mathrm{\γ}s\mathrm{\θ}s\mathrm{\ψ}& -c\mathrm{\γ}s\mathrm{\ψ}+s\mathrm{\γ}s\mathrm{\θ}c\mathrm{\ψ}& -s\mathrm{\γ}c\mathrm{\θ}\\ c\mathrm{\θ}s\mathrm{\ψ}& c\mathrm{\θ}c\mathrm{\ψ}& s\mathrm{\θ}\\ s\mathrm{\γ}c\mathrm{\ψ}-c\mathrm{\γ}s\mathrm{\θ}s\mathrm{\ψ}& -s\mathrm{\γ}s\mathrm{\ψ}-c\mathrm{\γ}s\mathrm{\θ}c\mathrm{\ψ}& c\mathrm{\γ}c\mathrm{\θ}\end{array}\right]---\left(1\right)$

定义飞行载体姿态四元数Q＝[q₀ q₁ q₂ q₃]^T，则四元数形式的姿态矩阵为：

$C_n^b=\left[\begin{array}{ccc}{q}_0^2+q_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)& q_0^2-q_1^2+q_2^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)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]---\left(2\right)$

步骤二：采集传感器信号。设定传感器采样率为0.05s，利用三轴速率陀螺、三轴加速度计、单轴速度传感器分别测量飞行载体的角速度、加速度及速度信息，经过A/D转换得到当前时刻角速度ω_bx＝0.01°/s、ω_by＝0.02°/s、ω_bz＝0.03°/s，加速度f_bx＝0.08m/s²、f_by＝-0.08m/s²、f_bz＝-9.76m/s²，速度v_y＝50m/s，利用三轴磁强计得到地磁场矢量m_bx＝-80、m_by＝3500、m_bz＝-4500，其中转换系数为15000/Gs。

步骤三：利用三轴速率陀螺测量的角速度ω_bx，ω_by，ω_bz，采用捷联姿态求解算法，求取利用角速度ω_b解算的飞行载体姿态四元数Q_I和俯仰角θ_I及横滚角γ_I。设初始时刻的姿态角均为0.5°，三轴速率陀螺漂移初始估计值均为0.001°/s，则角速度ω_bx，ω_by，ω_bz经过陀螺漂移补偿后为：

其中为经过陀螺漂移补偿后的角速度，利用通过捷联姿态求解得到飞行载体由角速度ω_bx，ω_by，ω_bz解算的飞行载体姿态四元数Q_I和俯仰角θ_I及横滚角γ_I为：

θ_I＝0.50046°，γ_I＝0.50094°

步骤四：利用地磁场矢量求解飞行载体的航向角ψ_S。利用步骤三获得的俯仰角θ_I和横滚角γ_I建立机体坐标系b系和水平坐标系h系之间的转换矩阵，将地磁场矢量m_bx，m_by转换到水平坐标系h系中：

$\{\begin{array}{c}{m}_{hx}={\mathrm{cos\γ}}_I\·m_{bx}+{\mathrm{sin\γ}}_I\·m_{bz}=-119\\ m_{hy}={\mathrm{sin\γ}}_I\·{\mathrm{sin\θ}}_I\·m_{bx}+{\mathrm{cos\θ}}_I\·m_{by}-{\mathrm{cos\γ}}_I\·{\mathrm{sin\θ}}_I\·m_{bz}=3539\end{array}---\left(4\right)$

其中m_bx，m_by为地磁场矢量m_bx，m_by转换到水平坐标系h系中的值，则求取磁航向角ψ_m为：

${\mathrm{\&psi;}}_m=\left\{\begin{array}{cc}\frac{\mathrm{\&pi;}}{2},& m_{hy}=0,m_{hx}<0\\ -\frac{\mathrm{\&pi;}}{2},& m_{hy}=0,m_{hx}>0\\ -atan(m_{hx}/m_{hy}),& m_{hy}>0\\ \mathrm{\&pi;}-a\tan (m_{hx}/m_{hy}),& m_{hy}<0,m_{hx}<0\\ -\mathrm{\&pi;}-a\tan (m_{hx}/m_{hy}),& m_{hy}<0,m_{hx}>0\end{array}\right.---\left(5\right)$

经过磁偏角修正得到利用地磁场矢量求解的飞行载体航向角ψ_S＝-0.552°

步骤五：利用单轴速度传感器获得的速度v_y对加速度f_bx，f_by进行补偿，解算飞行载体的俯仰角θ_S和横滚角γ_S。假定当前时刻速度相对于上一时刻速度无变化，即则对加速度f_bx和f_by利用速度v_y补偿如下：

$\left\{\begin{array}{c}{f}_{bx}^{\&prime;}=f_{bx}+{\widehat{\mathrm{\&omega;}}}_{bz}\&CenterDot;v_y=0.1053m/s^2\\ f_{by}^{\&prime;}=f_{by}-{\stackrel{\&CenterDot;}{v}}_y=-0.08m/s^2\end{array}\right.---\left(6\right)$

其中f′_bx和f′_by分别为经过由飞行载体的变速运动及转动引起的线加速度和向心加速度补偿后的加速度值，利用重力矢量在地理坐标系n系和机体坐标系b系之间的转换关系，求取横滚角γ_s和俯仰角θ_s为：

步骤六：对于航向角ψ_S、俯仰角θ_S及横滚角γ_S，利用姿态四元数与姿态角之间的关系，求取由地磁场矢量m_bx，m_by，m_bz和加速度f_bx，f_by，f_bz解算的飞行载体姿态四元数Q_S为：

步骤七：采用卡尔曼滤波技术，设计卡尔曼滤波器，对利用角速度ω_bx，ω_by，ω_bz解算的飞行载体姿态四元数Q_I、利用地磁场矢量m_bx，m_by，m_bz和加速度f_bx，f_by，f_bz解算的飞行载体姿态四元数Q_S、经过陀螺漂移补偿后的角速度进行卡尔曼滤波的数据处理，估计出利用角速度ω_bx，ω_by，ω_bz解算的飞行载体姿态四元数Q_I对应的姿态误差四元数和三轴速率陀螺漂移误差如下所示：

①建立***卡尔曼滤波状态方程

定义利用角速度ω_bx，ω_by，ω_bz解算的飞行载体姿态四元数Q_I对应的姿态误差四元数为其中建立***卡尔曼滤波状态方程为：

$\stackrel{\&CenterDot;}{X}\left(t\right)=F\left(t\right)\&CenterDot;X\left(t\right)+G\left(t\right)\&CenterDot;W\left(t\right)---\left(8\right)$

其中X(t)为t时刻的***状态向量，F(t)和G(t)为t时刻***状态转移矩阵及噪声矩阵，W(t)为***噪声向量。

***状态向量：X(t)＝[q_e1 q_e2 q_e3 Δb_x Δb_y Δb_z]^T

***噪声向量：W(t)＝[w_gx w_gy w_gz w_bx w_by w_bz]^T

其中Δb_x、Δb_y、Δb_z为三轴速率陀螺漂移估计误差，w_gx、w_gy、w_gz为陀螺漂移白噪声，w_bx、w_by、w_bz为陀螺漂移随机游走白噪声，***状态转移矩阵F(t)和噪声矩阵G(t)为：

$F\left(t\right)=\left[\begin{array}{cc}-\&lsqb;{\widehat{\mathrm{\&omega;}}}_b\&times;\&rsqb;& -\frac{1}{2}{I}_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}\end{array}\right],G\left(t\right)=\left[\begin{array}{cc}-\frac{1}{2}{I}_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& I_{3\&times;3}\end{array}\right]$

②建立***卡尔曼滤波量测方程

建立***卡尔曼滤波量测方程为：

Z(t)＝H(t)X(t)+V(t) (9)

其中量测矩阵H(t)＝[-I_3×3 0_3×3]，V(t)为量测噪声，近似为白噪声。

定义向量：

$Y=Q_S^{-1}\&CircleTimes;Q_I---\left(10\right)$

则***量测值为：

$Z\left(t\right)=\left[\begin{array}{c}Y\left(2\right)\\ Y\left(3\right)\\ Y\left(4\right)\end{array}\right]---\left(11\right)$

③***卡尔曼滤波方程离散化及状态估计

对①和②建立的***卡尔曼滤波方程进行离散化，得到离散化卡尔曼滤波状态方程和量测方程：

$\left\{\begin{array}{c}{X}_k={\mathrm{\&Phi;}}_{k,k-1}{X}_{k-1}+{\mathrm{\&Gamma;}}_{k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right.---\left(12\right)$

其中一步转移阵：Ф_k，k-1＝I_6×6+T·F(t_k)，***噪声驱动阵Г_k-1＝[I_6×6+T/2·F(t_k)]·G(t_k)·T，离散化周期T＝0.05s，***噪声方差阵Q_k和量测噪声方差阵R_k为：

Q_k＝diag[(0.05°/s)² (0.05°/s)² (0.05°/s)² (0.003°/s)² (0.003°/s)² (0.003°/s)²]

R_k＝diag[0.0001² 0.0001² 0.0001²]

对利用角速度ω_bx，ω_by，ω_bz解算的飞行载体姿态四元数Q_I和地磁场矢量m_bx，m_by，m_bz及加速度f_bx，f_by，f_bz解算的飞行载体姿态四元数Q_S，通过(10)式得卡尔曼滤波量测值采用如下离散型卡尔曼滤波基本方程进行滤波估计：

$P_{k/k-1}={\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T+{\mathrm{\&Gamma;}}_{k-1}{Q}_k{\mathrm{\&Gamma;}}_{k-1}^T---\left(13\right)$

$K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}---\left(14\right)$

${\hat{X}}_k=K_k{Z}_k---\left(15\right)$

$P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T---\left(16\right)$

经过滤波估计得到利用角速度ω_bx，ω_by，ω_bz解算的飞行载体姿态四元数Q_I对应的姿态误差四元数Q_Ie的估计值和三轴速率陀螺漂移估计误差为：

步骤八：校正利用角速度ω_bx，ω_by，ω_bz解算的飞行载体姿态四元数Q_I及提取飞行载体姿态角，即航向角ψ、俯仰角θ及横滚角γ。利用步骤七中估计出的姿态误差四元数校正利用角速度ω_bx，ω_by，ω_bz解算的飞行载体姿态四元数Q_I，得到当前时刻飞行载体姿态四元数最优估计值为：利用获得的根据四元数形式的姿态矩阵，得到当前时刻的最优姿态矩阵记：则提取航向角ψ、俯仰角θ及横滚角γ如下：

航向角：

$\mathrm{\&psi;}=\left\{\begin{array}{cc}a\tan (T_{12}/T_{22}),& T_{22}>0\\ \mathrm{\&pi;}+a\tan (T_{12}/T_{22}),& T_{22}<0,T_{12}>0\\ -\mathrm{\&pi;}+a\tan (T_{12}/T_{22}),& T_{22}<0,T_{12}<0\end{array}\right.---\left(17\right)$

横滚角：

$\mathrm{\&gamma;}=\left\{\begin{array}{cc}\arctan (-T_{31}/T_{33}),& T_{33}>0\\ \mathrm{\&pi;}+\arctan (-T_{31}/T_{33}),& T_{31}<0,T_{33}<0\\ -\mathrm{\&pi;}+\arctan (-T_{31}/T_{33}),& T_{31}>0,T_{33}<0\end{array}\right.---\left(18\right)$

俯仰角：

θ＝arcsin(T₃₂) (19)

求取姿态角为：

ψ＝-0.0389°，θ＝0.4844°，γ＝0.5609°

利用步骤七获得的三轴速率陀螺漂移估计误差得到当前时刻三轴速率陀螺漂移估计值采用校正三轴速率陀螺测量值，从而提高陀螺测量精度。将作为下一时刻的飞行载体姿态四元数初始值，返回步骤二采集传感器信号，进行下一时刻的姿态解算，循环计算，得到飞行载体实时姿态信息。

本实施例中采用微机电技术的陀螺、加速度计、磁强计和速度传感器组合，能够克服单个传感器的不足，有效提高姿态确定的精度，尤其对于微小型飞行载体的姿态测量具有十分重要的意义。

Claims (1)

1.一种飞行载体姿态确定方法，其特征在于包括下述步骤：

步骤一：建立飞行载体的坐标系及姿态矩阵：机体坐标系b系中X_b，Y_b，Z_b三个正交轴分别沿飞行载体的横轴向右、纵轴向前、竖轴向上；地理坐标系n系，X_n，Y_n，Z_n三个正交轴分别指向东、北、天；姿态角定义为：航向角ψ为(-180°，180°)，俯仰角θ为(-90°，90°)，横滚角γ为(-180°，180°)；按照航向-俯仰-横滚的旋转顺序，得到飞行载体的姿态角形式的姿态矩阵为：

$C_n^b=\left[\begin{array}{ccc}c\mathrm{\&gamma;}c\mathrm{\&psi;}+s\mathrm{\&gamma;}s\mathrm{\&theta;}s\mathrm{\&psi;}& -c\mathrm{\&gamma;}s\mathrm{\&psi;}+s\mathrm{\&gamma;}s\mathrm{\&theta;}c\mathrm{\&psi;}& -s\mathrm{\&gamma;}c\mathrm{\&theta;}\\ c\mathrm{\&theta;}s\mathrm{\&psi;}& c\mathrm{\&theta;}c\mathrm{\&psi;}& s\mathrm{\&theta;}\\ s\mathrm{\&gamma;}c\mathrm{\&psi;}-c\mathrm{\&gamma;}s\mathrm{\&theta;}s\mathrm{\&psi;}& -s\mathrm{\&gamma;}s\mathrm{\&psi;}-c\mathrm{\&gamma;}s\mathrm{\&theta;}c\mathrm{\&psi;}& c\mathrm{\&gamma;}c\mathrm{\&theta;}\end{array}\right]---\left(1\right)$

其中s和c分别为函数sin和cos的简写，定义飞行载体姿态四元数Q＝[q₀ q₁ q₂ q₃]^T，得到四元数形式的姿态矩阵为：

$C_n^b=\left[\begin{array}{ccc}{q}_0^2+q_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)& q_0^2-q_1^2+q_2^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)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]---\left(2\right)$

步骤二：采集传感器信号：利用陀螺、加速度计、单轴速度传感器分别测量飞行载体的角速度、加速度及速度，经过A/D转换得到角速度ω_b＝[ω_bx ω_by ω_bz]^T和加速度f_b＝[f_bx f_by f_bz]^T及速度v_y，利用磁强计得到地磁场矢量m_b＝[m_bx m_by m_bz]^T；

步骤三：利用角速度ω_b，采用捷联姿态求解算法，求取利用角速度ω_b解算的飞行载体姿态四元数Q_I和俯仰角θ_I及横滚角γ_I；

步骤四：利用地磁场矢量m_b求解飞行载体的航向角ψ_S：利用步骤三获得的俯仰角θ_I和横滚角γ_I建立机体坐标系b系和水平坐标系h系之间的转换矩阵，将地磁场矢量m_b转换到水平坐标系h系中，解算出飞行载体的磁航向角，经过磁偏角修正得到航向角ψ_S；

步骤五：利用速度v_y对加速度f_b补偿，得到重力矢量在机体坐标系b系中的投影值，利用重力矢量在地理坐标系n系和机体坐标系b系之间的转换关系，解算出飞行载体的俯仰角θ_S和横滚角γ_S；

对加速度f_b利用速度v_y补偿如下：

$\left\{\begin{array}{c}{f}_{bx}^{\&prime;}=f_{bx}+{\widehat{\mathrm{\&omega;}}}_{bz}\&CenterDot;v_y\\ f_{by}^{\&prime;}=f_{by}-{\stackrel{\&CenterDot;}{v}}_y\end{array}\right.---\left(3\right)$

其中f′_bx和f′_by分别为经过由飞行载体的变速运动及转动引起的线加速度和向心加速度补偿后的加速度值，为沿Z_b轴的陀螺经过陀螺漂移补偿后的角速度，v_y为沿飞行载体机体坐标系Y_b轴放置的单轴速度传感器测量值，则求取横滚角γ_S和俯仰角θ_S为：

$\left\{\begin{array}{c}{\mathrm{\&gamma;}}_S=a\tan (-f_{bx}^{\&prime;}/f_{bz})\\ {\mathrm{\&theta;}}_S=a\tan \left(\right(-f_{by}^{\&prime;}/f_{bz}\left){\mathrm{sin\&gamma;}}_S\right)\end{array}\right.---\left(4\right)$

步骤六：对于航向角ψ_S、俯仰角θ_S及横滚角γ_S，利用姿态四元数与姿态角之间的关系，求取由地磁场矢量m_b和加速度f_b解算的飞行载体姿态四元数Q_S；

步骤七：采用卡尔曼滤波技术，设计卡尔曼滤波器，对利用角速度ω_b解算的飞行载体姿态四元数Q_I、利用地磁场矢量m_b和加速度f_b解算的飞行载体姿态四元数Q_S、陀螺测量值ω_b，进行卡尔曼滤波的数据处理，估计出利用角速度ω_b解算的飞行载体姿态四元数Q_I对应的姿态误差四元数和陀螺漂移误差包括如下子步骤：

①建立***卡尔曼滤波状态方程

陀螺测量模型建立为：ω_b＝ω+b+w_g，其中ω_b为陀螺输出值，ω为真实角速率，w_g为陀螺漂移白噪声，b为陀螺漂移，满足方程为陀螺漂移随机游走白噪声；

定义利用角速度ω_b解算的飞行载体姿态四元数Q_I对应的姿态误差四元数为其中真实的飞行载体姿态四元数为Q，则Q可以表示为：

$Q=Q_I\&CircleTimes;Q_{Ie}---\left(5\right)$

对(5)式两边求微分，得：

$\stackrel{\&CenterDot;}{Q}={\stackrel{\&CenterDot;}{Q}}_I\&CircleTimes;Q_{Ie}+Q_I\&CircleTimes;{\stackrel{\&CenterDot;}{Q}}_{Ie}---\left(6\right)$

${\stackrel{\&CenterDot;}{Q}}_{Ie}=\frac{1}{2}{Q}_{Ie}\&CircleTimes;{\mathrm{\&omega;}}_b-\frac{1}{2}{\widehat{\mathrm{\&omega;}}}_b\&CircleTimes;Q_{Ie}---\left(7\right)$

则：

${\stackrel{\&CenterDot;}{\stackrel{\&RightArrow;}{q}}}_e=-\&lsqb;{\widehat{\mathrm{\&omega;}}}_b\&times;\&rsqb;\&CenterDot;{\stackrel{\&RightArrow;}{q}}_e-\frac{1}{2}\mathrm{\&Delta;}b-\frac{1}{2}{w}_g---\left(8\right)$

其中为陀螺漂移估计误差，b为陀螺真实漂移，为陀螺漂移估计值，为的斜对称矩阵，为角速度ω_b经过陀螺漂移补偿后的值，建立***卡尔曼滤波状态方程为：

$\stackrel{\&CenterDot;}{X}\left(t\right)=F\left(t\right)\&CenterDot;\left(t\right)+G\left(t\right)\&CenterDot;W\left(t\right)---\left(9\right)$

其中X(t)为t时刻的***状态向量，F(t)和G(t)为t时刻***状态转移矩阵及噪声矩阵，W(t)为***噪声向量；

***状态向量：X(t)＝[q_e1 q_e2 q_e3 Δb_x Δb_y Δb_z]^T

***噪声向量：W(t)＝[w_gx w_gy w_gz w_bx w_by w_bz]^T

其中q_e1、q_e2、q_e3为姿态误差四元数Q_Ie的三个分量，Δb_x、Δb_y、Δb_z为陀螺漂移估计误差，w_gx、w_gy、w_gz为陀螺漂移白噪声，w_bx、w_by、w_bz为陀螺漂移随机游走白噪声，***状态转移矩阵F(t)和噪声矩阵G(t)为：

$F\left(t\right)=\left[\begin{array}{cc}-\&lsqb;{\widehat{\mathrm{\&omega;}}}_b\&times;\&rsqb;& -\frac{1}{2}{I}_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}\end{array}\right],G\left(t\right)=\left[\begin{array}{cc}-\frac{1}{2}{I}_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& I_{3\&times;3}\end{array}\right]$

②建立***卡尔曼滤波量测方程

定义利用地磁场矢量m_b和加速度f_b解算的飞行载体姿态四元数Q_S对应的姿态误差四元数为Q_Se＝[1 q_se1 q_se2 q_se3]^T，则：

$\left\{\begin{array}{c}Q=Q_I\&CircleTimes;Q_{Ie}\\ Q=Q_S\&CircleTimes;Q_{Se}\end{array}\right.---\left(10\right)$

定义向量由式(10)得：

$Y=Q_S^{-1}\&CircleTimes;Q_I=Q_{Se}\&CircleTimes;Q_{Ie}^{-1}=\left[\begin{array}{cccc}1& -q_{se1}& -q_{se2}& -q_{se3}\\ q_{se1}& 1& -q_{se3}& q_{se2}\\ q_{se2}& q_{se3}& 1& -q_{se1}\\ q_{se3}& -q_{se2}& q_{se1}& 1\end{array}\right]\&CenterDot;\left[\begin{array}{c}1\\ -q_{e1}\\ -q_{e2}\\ -q_{e3}\end{array}\right]---\left(11\right)$

忽略二阶项，得到***卡尔曼滤波量测方程为：

$Z\left(t\right)=\left[\begin{array}{c}Y\left(2\right)\\ Y\left(3\right)\\ Y\left(4\right)\end{array}\right]=\left[\begin{array}{c}{q}_{se1}-q_{e1}\\ q_{se2}-q_{e2}\\ q_{se3}-q_{e3}\end{array}\right]=H\left(t\right)X\left(t\right)+V\left(t\right)---\left(12\right)$

其中量测矩阵H(t)＝[-I_3×3 0_3×3]，V(t)为量测噪声，近似为白噪声；

③***卡尔曼滤波方程离散化及状态估计

对①和②建立的***卡尔曼滤波方程进行离散化，得到离散化卡尔曼滤波状态方程和量测方程为：

$\left\{\begin{array}{c}{X}_k={\mathrm{\&Phi;}}_{k,k-1}{X}_{k-1}+{\mathrm{\&Gamma;}}_{k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right.---\left(13\right)$

其中一步转移阵***噪声驱动阵T为离散化周期，***噪声方差阵Q_k和量测噪声方差阵R_k为：

$\begin{array}{c}{Q}_k=diag\left[\begin{array}{cccccc}{\mathrm{\&sigma;}}_{gx}^2& {\mathrm{\&sigma;}}_{gy}^2& {\mathrm{\&sigma;}}_{gz}^2& {\mathrm{\&sigma;}}_{bx}^2& {\mathrm{\&sigma;}}_{bx}^2& {\mathrm{\&sigma;}}_{bx}^2\end{array}\right]---\left(14\right)\\ R_k=diag\left[\begin{array}{ccc}{{\mathrm{\&delta;q}}_{e1}}^2& {{\mathrm{\&delta;q}}_{e2}}^2& {{\mathrm{\&delta;q}}_{e3}}^2\end{array}\right]---\left(15\right)\end{array}$

其中σ_gx，σ_gy，σ_gz为陀螺漂移白噪声均方根，σ_bx，σ_by，σ_bz为陀螺漂移随机游走白噪声均方根，δq_e1，δq_e2，δq_e3为飞行载体姿态四元数估计误差，对利用角速度ω_b解算的飞行载体姿态四元数Q_I和地磁场矢量m_b及加速度f_b解算的飞行载体姿态四元数Q_S，通过(11)式计算卡尔曼滤波量测值Z_k，采用离散型卡尔曼滤波基本方程进行滤波估计，得到利用角速度ω_b解算的飞行载体姿态四元数Q_I对应的姿态误差四元数Q_Ie的估计值及陀螺漂移估计误差

步骤八：校正利用角速度ω_b解算的飞行载体姿态四元数Q_I及提取飞行载体姿态角，即航向角ψ、俯仰角θ及横滚角γ；利用步骤七中估计出的姿态误差四元数校正利用角速度ω_b解算的飞行载体姿态四元数Q_I，得到当前时刻飞行载体姿态四元数最优估计值

利用获得的得到对应的姿态矩阵，利用姿态矩阵提取飞行载体姿态角，即航向角ψ、俯仰角θ及横滚角γ；对经过卡尔曼滤波得到的陀螺漂移估计误差获得陀螺漂移估计值采用校正陀螺测量值，从而提高陀螺测量精度；将作为下一时刻姿态解算四元数的初始值，返回步骤二采集传感器信号，进行下一时刻的姿态解算，循环计算。