风力干扰环境下四旋翼飞行器姿态解算方法.pdf

本发明属于无人机控制领域，针对风力会影响无人机姿态解算，进而干扰无人机飞行控制问题，基于扩展卡尔曼滤波理论，本文提出一种风力干扰环境下无人机姿态解算方法以增强姿态解算的稳健性从而提高无人机控制性能。所提方法通过四元数微分方程和陀螺仪噪声误差建立扩展卡尔曼滤波状态方程，并基于加速度计、磁强计、风力相关数据，得到四元数解算结果。基于实测数据测试结果表明，所提算法可以有效抑制姿态角发散，提高姿态解算精度，改善无人机控制性能。

1.风力干扰环境下四旋翼飞行器姿态解算方法，其特征在于：包括如下步骤：
第一步：坐标系定义以及姿态矩阵，
为了描述飞行器的俯仰、偏航、横滚的姿态信息，需要建立相应的坐标系；本专利采用
两个不同的三维坐标系，分别为导航坐标系n,定义为东北天坐标系；载体坐标系b，其中xb
沿机体横轴指向右，yb沿机体纵轴指向前，zb沿机体竖直指向上，满足右手定则，原点皆为无
人机重心；姿态解算在导航坐标系中完成，因而须将无人机上传感器测得的姿态信息经坐
标变换矩阵映射至坐标系n；
从导航坐标系到载体坐标系的姿态矩阵可表示为：
$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(1\right)$
其方向余弦形式可表述如下：
$C_n^b=\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}-\cos \mathrm{\φ}\sin \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\ψ}+\cos \mathrm{\φ}\sin \mathrm{\θ}s\cos \mathrm{\ψ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\φ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}-\sin \mathrm{\φ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\θ}& \sin \mathrm{\φ}\cos \mathrm{\θ}& \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]---\left(2\right)$
式中，ψ，θ，φ分别表示无人机的航向角，俯仰角，翻滚角；比较姿态矩阵的四元数形式
(1)及欧拉角形式(2)，可知：
$\begin{array}{c}\mathrm{\ψ}=\arctan \left(\frac{2(q_1{q}_2+q_0{q}_3)}{q_1^2+q_0^2-q_2^2-q_3^2}\right)\\ \mathrm{\θ}=-\arcsin \left(2\right(q_1{q}_3-q_0{q}_2\left)\right)\\ \mathrm{\φ}=\arctan \left(\frac{2(q_2{q}_3+q_0{q}_1)}{-q_1^2+q_0^2-q_2^2+q_3^2}\right)\end{array}---\left(3\right)$
至此，可得姿态角的四元数表示形式，基于此形式可对风力干扰条件下基于卡尔曼滤
波的姿态计算问题进行深入分析；
第二步：卡尔曼滤波定姿方程
①卡尔曼滤波姿态解算的状态方程
卡尔曼滤波姿态解算的预测方程表示为:
Xk＝Φk,k-1Xk-1+Wk-1 (4)
其中Φk,k-1为tk-1时刻到tk时刻的一步转移矩阵，Wk为系统噪声序列；
系统状态方程可表示为：
$\stackrel{\·}{q}\left(t\right)=\frac{1}{2}{\mathrm{\Ω}}_{b}q\left(t\right)---\left(5\right)$
由于状态估计量为四元数，则状态方程用四元数微分方程可进一步表示为:
$\left[\begin{array}{c}{\stackrel{\·}{q}}_0\left(t\right)\\ {\stackrel{\·}{q}}_1\left(t\right)\\ {\stackrel{\·}{q}}_2\left(t\right)\\ {\stackrel{\·}{q}}_3\left(t\right)\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_x& -{\mathrm{\ω}}_y& -{\mathrm{\ω}}_z\\ {\mathrm{\ω}}_x& 0& {\mathrm{\ω}}_z& -{\mathrm{\ω}}_y\\ {\mathrm{\ω}}_y& -{\mathrm{\ω}}_x& 0& {\mathrm{\ω}}_x\\ {\mathrm{\ω}}_z& {\mathrm{\ω}}_y& -{\mathrm{\ω}}_x& 0\end{array}\right]\left[\begin{array}{c}{q}_0\left(t\right)\\ q_1\left(t\right)\\ q_2\left(t\right)\\ q_3\left(t\right)\end{array}\right]---\left(6\right)$
其中，ωx,ωy,ωz为安装在四旋翼飞行器上陀螺仪的角速度分量；
由于系统状态方程是连续的，不易采用数字化方法对其进行求解；针对此问题，目前求
解四元数微分方程主要有两种方法:一种是龙格库塔法，另一种是毕卡逼近法；本专利采用
四阶毕卡逼近法将其离散化，取q(t)＝[q0(t) q1(t) q2(t) q3(t)]，将(6)式离散化可得：
q(k+1)＝Φk,k-1q(k) (7)
其中：
${\mathrm{\Φ}}_{k,k-1}=\left[\begin{array}{cccc}1-\frac{{\mathrm{\Δ\θ}}_0^2}{8}+\frac{{\mathrm{\Δ\θ}}_0^4}{384}& -(\frac{1}{2}-\frac{{\mathrm{\Δ\θ}}_0^2}{48}){\mathrm{\Δ\θ}}_x& -(\frac{1}{2}-\frac{{\mathrm{\Δ\θ}}_0^2}{48}){\mathrm{\Δ\θ}}_y& -(\frac{1}{2}-\frac{{\mathrm{\Δ\θ}}_0^2}{48}){\mathrm{\Δ\θ}}_z\\ (\frac{1}{2}-\frac{{\mathrm{\Δ\θ}}_0^2}{48}){\mathrm{\Δ\θ}}_x& 1-\frac{{\mathrm{\Δ\θ}}_0^2}{8}+\frac{{\mathrm{\Δ\θ}}_0^4}{384}& (\frac{1}{2}-\frac{{\mathrm{\Δ\θ}}_0^2}{48}){\mathrm{\Δ\θ}}_z& -(\frac{1}{2}-\frac{{\mathrm{\Δ\θ}}_0^2}{48}){\mathrm{\Δ\θ}}_y\\ (\frac{1}{2}-\frac{{\mathrm{\Δ\θ}}_0^2}{48}){\mathrm{\Δ\θ}}_y& -(\frac{1}{2}-\frac{{\mathrm{\Δ\θ}}_0^2}{48}){\mathrm{\Δ\θ}}_z& 1-\frac{{\mathrm{\Δ\θ}}_0^2}{8}+\frac{{\mathrm{\Δ\θ}}_0^4}{384}& (\frac{1}{2}-\frac{{\mathrm{\Δ\θ}}_0^2}{48}){\mathrm{\Δ\θ}}_x\\ (\frac{1}{2}-\frac{{\mathrm{\Δ\θ}}_0^2}{48}){\mathrm{\Δ\θ}}_z& (\frac{1}{2}-\frac{{\mathrm{\Δ\θ}}_0^2}{48}){\mathrm{\Δ\θ}}_y& -(\frac{1}{2}-\frac{{\mathrm{\Δ\θ}}_0^2}{48}){\mathrm{\Δ\θ}}_x& 1-\frac{{\mathrm{\Δ\θ}}_0^2}{8}+\frac{{\mathrm{\Δ\θ}}_0^4}{384}\end{array}\right]$
${\mathrm{\Δ\θ}}_x={\∫}_k^{k+1}{\mathrm{\ω}}_{x}dt,{\mathrm{\Δ\θ}}_y={\∫}_k^{k+1}{\mathrm{\ω}}_{y}dt,{\mathrm{\Δ\θ}}_z={\∫}_k^{k+1}{\mathrm{\ω}}_{z}dt,{\mathrm{\Δ\θ}}_0^2={\mathrm{\Δ\θ}}_x^2+{\mathrm{\Δ\θ}}_y^2+{\mathrm{\Δ\θ}}_z^2$
②卡尔曼滤波姿态解算的观测方程
风力干扰环境下，观测量可由以下三种测量值构成：加速度计、磁力计和风力；其观测
方程为：
Z(t)＝HX(t)+V(t) (8)
其中V(t)是白噪声，下面对测量值进行深入分析；
首先针对观测量加速度计和磁强计进行分析：
参考坐标系下重力向量定义为G＝[0 0 1]T，地磁场向量h＝[hx hy hz]T；其矩阵形式可
分别表示为：
$\left[\begin{array}{c}{g}_x\\ g_y\\ g_z\end{array}\right]=C_n^b\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]---\left(9\right)$
$\left[\begin{array}{c}{m}_x\\ m_y\\ m_z\end{array}\right]=C_n^b\left[\begin{array}{c}{h}_x\\ h_y\\ h_z\end{array}\right]---\left(10\right)$
其中，g，m分别是载体坐标系下加速度计及磁力计的量测值；由式(9)及(10)可得：
$\left\{\begin{array}{c}{g}_x=2(q_1{q}_3-q_0{q}_2)\\ g_y=2(q_2{q}_3+q_0{q}_1)\\ g_z=q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right.---\left(11\right)$
$\left\{\begin{array}{c}{m}_x=(q_0^2+q_0^2-q_0^2-q_0^2)h_x+2(q_1{q}_1+q_1{q}_1)h_y+2(q_1{q}_3-q_0{q}_2)h_z\\ m_y=2(q_1{q}_2-q_0{q}_3)h_x+(q_0^2+q_0^2-q_0^2-q_0^2)h_y+2(q_0{q}_1+q_2{q}_3)h_z\\ m_z=2(q_1{q}_3+q_0{q}_2)h_x+2(q_2{q}_3-q_0{q}_1)h_y+(q_0^2-q_0^2-q_0^2+q_0^2)h_z\end{array}\right.---\left(12\right)$
由以上所述，可得加速度计及磁力计量测值的四元数表示形式，下面针对风力进行深
入分析；
第三步：风力观测方程
①气流坐标系
空气流动用幅值为VT的空速矢量VT表示，其方向由相对机体的两个角来定义，即攻角α
和侧滑角β，分别定义为：
$V_T=\sqrt{u_T^2+v_T^2+w_T^2},\mathrm{\α}=\arctan \left(\frac{w_T}{u_T}\right),\mathrm{\β}=\arcsin \left(\frac{v_T}{V_T}\right),$
机体坐标系(b)到气流坐标系(w)的旋转矩阵可表述如下：
由于则：
$A^w=C_b^w{A}^b---\left(13\right)$
其中A为矢量，由上式可得：
其中，
$C_b^w=\left[\begin{array}{ccc}\cos \mathrm{\β}& \sin \mathrm{\β}& 0\\ -\sin \mathrm{\β}& \cos \mathrm{\β}& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}\cos \mathrm{\α}& 0& \sin \mathrm{\α}\\ 0& 1& 0\\ -\sin \mathrm{\α}& 0& \cos \mathrm{\α}\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\α}\cos \mathrm{\β}& \sin \mathrm{\β}& \sin \mathrm{\α}\cos \mathrm{\β}\\ -\sin \mathrm{\β}\cos \mathrm{\α}& \cos \mathrm{\β}& -\sin \mathrm{\α}\sin \mathrm{\β}\\ -\sin \mathrm{\α}& 0& \cos \mathrm{\α}\end{array}\right]$
基于上述分析，空速矢量可表示为：
$V_T^b=C_w^b{V}_T^w---\left(14\right)$
在机体坐标系中，空速矢量可改写为：
$\left[\begin{array}{c}{u}_T\\ v_T\\ w_T\end{array}\right]=C_w^b\left[\begin{array}{c}{V}_T\\ 0\\ 0\end{array}\right]---\left(15\right)$
至此，气流坐标系已建立，下面将在此坐标系下对风力干扰进行分析；
②风力干扰
飞机惯性速度v为空速VT及风速W之和，可表示为：
v＝VT+W (16)
导航坐标系下，扰动风表示为Wn，机体坐标系飞机速度可表示为：
$v^b=V_T^b+C_n^b{W}^n---\left(17\right)$
上式可改写为：
$C_n^b{W}^n=v^b-V_T^b---\left(18\right)$
导航坐标系下，式(18)的矩阵形式可表示为：
$\left[\begin{array}{c}{u}_T\\ v_T\\ w_T\end{array}\right]=C_n^b\left[\begin{array}{c}u\\ v\\ w\end{array}\right]-C_n^b\left[\begin{array}{c}{W}_N\\ W_E\\ W_D\end{array}\right]---\left(19\right)$
由系统观测过程可知，四元数为关于加速度计磁力计的量测值及风力值的非线性函
数；为求解四元数，须基于雅克比矩阵将其线性化；基于式(12)及(13)，雅克比矩阵H可表示
为：
$H=\left[\begin{array}{cccc}-2q_2& 2q_3& -2q_0& 2q_1\\ 2q_1& 2q_0& 2q_3& 2q_2\\ 2q_0& -2q_1& -2q_2& 2q_3\\ 2(q_0{h}_x+q_3{h}_y-q_2{h}_z)& 2(q_1{h}_x+q_2{h}_y+q_3{h}_z)& 2(-q_2{h}_x+q_1{h}_y-q_0{h}_z)& 2(-q_3{h}_x+q_0{h}_y+q_1{h}_z)\\ 2(-q_3{h}_x+q_0{h}_y+q_1{h}_z)& 2(q_2{h}_x-q_1{h}_y+q_0{h}_z)& 2(q_1{h}_x+q_2{h}_y+q_3{h}_z)& 2(-q_0{h}_x-q_3{h}_y+q_2{h}_z)\\ 2(q_2{h}_x-q_1{h}_y+q_0{h}_z)& 2(q_3{h}_x-q_0{h}_y-q_1{h}_z)& 2(q_0{h}_x+q_3{h}_y-q_2{h}_z)& 2(q_1{h}_x+q_2{h}_y+q_3{h}_z)\\ 2q_3& -2q_2& -2q_1& 2q_0\\ -2q_0& 2q_1& -2q_2& 2q_3\\ -2q_1& -2q_0& -2q_2& -2q_3\end{array}\right]---\left(20\right)$
至此，得到了风力观测方程及将其线性化的雅克比矩阵；基于此，即可利用扩展卡尔曼
滤波对四元数进行解算；
第四步：基于扩展卡尔曼滤波流程的四元数姿态解算。
2.根据权利要求1所述的风力干扰环境下四旋翼飞行器姿态解算方法，其特征在于：包
括如下步骤：
第一步：时间更新过程，
状态一步预测：
${\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}---\left(21\right)$
均方误差的一步预测：
$P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k+1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(22\right)$
第二步：量测更新过程，
卡尔曼滤波增益：
$K_k=P_{k/k-1}{H}_k^T(H_k{P}_{k/k-1}+H_k^T)---\left(23\right)$
协方差阵更新：
Pk＝(I-KkHk)Pk/k-1 (24)
状态更新：
${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1}).---\left(25\right)$

风力干扰环境下四旋翼飞行器姿态解算方法

技术领域

本发明涉及风力干扰环境下四旋翼飞行器姿态解算方法，属于无人机飞行控制领
域。

背景技术

近年来，随着微电子技术，微机电技术，高聚合物新能源技术，先进控制技术的发
展，四旋翼飞行器已成为人们研究的热点。无人飞行器的种类繁多，其中四旋翼无人飞行器
是一个重要的研究方向。与其他无人飞行器相比，四旋翼飞行器可以实现垂直起降，定点悬
停等功能，同时具有体积小，机构简单，成本低，机动性能好，有效载荷能力好，易于穿行狭
小空间等优势，因此在环境监测、灾区营救、输电线路巡查和空中航拍等领域得到了广泛的
应用。

飞行器的位姿估计是实现无人飞行器(UAV)自主能力飞行的基础。互补滤波最先
在无人机姿态解算中得到很好的应用，然而姿态解算的精度还是达不到要求。在过去的几
十年里，众多学者致力于研究卡尔曼滤波算法，如：扩展卡尔曼滤波(EKF)，无迹卡尔曼滤波
(UKF)和粒子滤波。并成功将EKF和UKF运用到无人机姿态解算中去，得到了更高精度的姿态
角。万晓凤，康利平，余运俊等基于互补滤波算法进行姿态解算，并取得了较高精度的姿态
角。李萧然，陈谋等人基于EKF进行无人机姿态解算，得到了很高精度的姿态角。朱岩，付巍
等人基于UKF进行无人机姿态解算，得到了更高精度的姿态角。以上学者皆没有考虑风力估
算，针对此问题，A Cho等人提出用一个单天线GPS和流速计对风进行估计。然而，该学者仅
仅考虑了风力的估计，而没有深入研究风力对无人机控制的影响。在实际环境下，飞行器飞
行时易受风力影响，从而导致飞行器姿态解算误差较大，进而影响到对无人机的有效控制。
针对此问题，本发明提出一种风力干扰环境下无人机姿态解算算法。本算法将风力作为观
测量，基于四元数微分方程和陀螺仪噪声误差建立卡尔曼滤波状态方程，并利用来自加速
度计，磁力计，GPS，空速计等传感器补偿陀螺仪引起的误差，最后利用加速度计，磁力计和
风力相关数据进行四元数解算。基于实测数据的测试表明，所提算法可以有效抑制姿态角
发散，提高姿态解算精度，改善无人机控制性能。

发明内容

针对风力会影响无人机姿态解算，进而干扰无人机飞行控制问题，基于扩展卡尔
曼滤波理论，本发明提出一种风力干扰环境下无人机姿态解算方法以增强姿态解算的稳健
性从而提高无人机控制性能。所提方法通过四元数微分方程和陀螺仪噪声误差建立扩展卡
尔曼滤波状态方程，并基于加速度计、磁强计、风力相关数据，得到四元数解算结果。基于实
测数据测试结果表明，所提算法可以有效抑制姿态角发散，提高姿态解算精度，改善无人机
控制性能。实现本发明的基本思路是，首先建立无人机姿态模型，建立扩展卡尔曼滤波状态
方程和观测方程，引入风力作为观测量，对状态方程进行线性化，求解最优姿态角。

本发明的具体步骤包含如下：

第一步：坐标系定义以及姿态矩阵

为了描述飞行器的俯仰、偏航、横滚的姿态信息，需要建立相应的坐标系。本专利
采用两个不同的三维坐标系，分别为导航坐标系n,定义为东北天坐标系；载体坐标系b，其
中xb沿机体横轴指向右，yb沿机体纵轴指向前，zb沿机体竖直指向上，满足右手定则，原点皆
为无人机重心。姿态解算在导航坐标系中完成，因而须将无人机上传感器测得的姿态信息
经坐标变换矩阵映射至坐标系n。

从导航坐标系到载体坐标系的姿态矩阵可表示为：

其方向余弦形式可表述如下：

式中，ψ，θ，φ分别表示无人机的航向角，俯仰角，翻滚角。比较姿态矩阵的四元数
形式(1)及欧拉角形式(2)，可知：

至此，可得姿态角的四元数表示形式，基于此形式可对风力干扰条件下基于卡尔
曼滤波的姿态计算问题进行深入分析。

第二步：卡尔曼滤波定姿方程，

1.卡尔曼滤波姿态解算的状态方程，

卡尔曼滤波姿态解算的预测方程表示为：

Xk＝Φk,k-1Xk-1+Wk-1 (4)

其中Φk,k-1为tk-1时刻到tk时刻的一步转移矩阵，Wk为系统噪声序列。

系统状态方程可表示为：

由于状态估计量为四元数，则状态方程用四元数微分方程可进一步表示为:

其中，ωx,ωy,ωz为安装在四旋翼飞行器上陀螺仪的角速度分量。

由于系统状态方程是连续的，不易采用数字化方法对其进行求解。针对此问题，目
前求解四元数微分方程主要有两种方法:一种是龙格库塔法，另一种是毕卡逼近法。本专利
采用四阶毕卡逼近法将其离散化，取q(t)＝[q0(t)q1(t)q2(t)q3(t)]，将(6)式离散化可得：

q(k+1)＝Φk,k-1q(k) (7)

其中：

2.卡尔曼滤波姿态解算的观测方程

风力干扰环境下，观测量可由以下三种测量值构成：加速度计、磁力计和风力。其
观测方程为：

Z(t)＝HX(t)+V(t) (8)

其中V(t)是白噪声，下面对测量值进行深入分析。

首先针对观测量加速度计和磁强计进行分析：

参考坐标系下重力向量定义为G＝[0 0 1]T，地磁场向量h＝[hx hy hz]T。其矩阵形
式可分别表示为：

其中，g，m分别是载体坐标系下加速度计及磁力计的量测值。由式(9)及(10)可得：

由以上所述，可得加速度计及磁力计量测值的四元数表示形式，下面针对风力进
行深入分析。

第三步：风力观测方程，

1.气流坐标系

空气流动用幅值为VT的空速矢量VT表示，其方向由相对机体的两个角来定义，即攻
角α和侧滑角β，分别定义为：

机体坐标系(b)到气流坐标系(w)的旋转矩阵可表述如下：

由于则：

其中A为矢量，由上式可得：

其中，

基于上述分析，空速矢量可表示为：

在机体坐标系中，空速矢量可改写为：

至此，气流坐标系已建立，下面将在此坐标系下对风力干扰进行分析。

2.风力干扰

飞机惯性速度v为空速VT及风速W之和，可表示为：

v＝VT+W (16)

导航坐标系下，扰动风表示为Wn，机体坐标系飞机速度可表示为：

上式可改写为：

导航坐标系下，式(18)的矩阵形式可表示为：

由系统观测过程可知，四元数为关于加速度计磁力计的量测值及风力值的非线性
函数。为求解四元数，须基于雅克比矩阵将其线性化。基于式(12)及(14)，雅克比矩阵H可表
示为：

至此，得到了风力观测方程及将其线性化的雅克比矩阵。基于此，即可利用扩展卡
尔曼滤波对四元数进行解算。

第四步：基于扩展卡尔曼滤波流程的四元数姿态解算

基于以上讨论可知，基于风力的无人机姿态解算可利用扩展卡尔曼滤波实现。在
一个滤波周期内，卡尔曼滤波具有两个明显的更新过程：时间更新过程和量测更新过程。因
此，基于以上所述，基于风力的时间更新过程及量测更新过程可表述如下：

1.时间更新过程

状态一步预测：

均方误差的一步预测：

2.量测更新过程

卡尔曼滤波增益：

协方差阵更新：

Pk＝(I-KkHk)Pk/k-1 (24)

状态更新：

本发明有益效果：

本发明与现有技术相比具有以下优点：

本发明基于扩展卡尔曼滤波，将风力作为观测变量，在风力干扰下可以有效抑制
姿态角发散，提高姿态解算精度，改善无人机控制性能。针对风力会影响无人机姿态解算，
进而干扰无人机飞行控制问题，基于扩展卡尔曼滤波理论，本发明提出一种风力干扰环境
下无人机姿态解算方法以增强姿态解算的稳健性从而提高无人机控制性能。所提方法通过
四元数微分方程和陀螺仪噪声误差建立扩展卡尔曼滤波状态方程，并基于加速度计、磁强
计、风力相关数据，得到四元数解算结果。基于实测数据测试结果表明，所提算法可以有效
抑制姿态角发散，提高姿态解算精度，改善无人机控制性能

附图说明

图1为本发明实现的流程图；

图2为本发明分别在无风，有风时考虑风和有风时不考虑风的翻滚角偏差对比图；

图3为本发明分别在无风，有风时考虑风和有风时不考虑风的俯仰角偏差对比图；

图4位本发明分别在无风，有风时考虑风和有风时不考虑风的偏航角偏差对比图。

具体实施方式

本发明的效果可通过以下仿真进一步说明：

下面结合附图1对本发明的实现步骤做进一步详细描述：

第一步：坐标系定义以及姿态矩阵

为了描述飞行器的俯仰、偏航、横滚的姿态信息，需要建立相应的坐标系。本专利
采用两个不同的三维坐标系，分别为导航坐标系n,定义为东北天坐标系；载体坐标系b，其
中xb沿机体横轴指向右，yb沿机体纵轴指向前，zb沿机体竖直指向上，满足右手定则，原点皆
为无人机重心。姿态解算在导航坐标系中完成，因而须将无人机上传感器测得的姿态信息
经坐标变换矩阵映射至坐标系n。

从导航坐标系到载体坐标系的姿态矩阵可表示为：

其方向余弦形式可表述如下：

式中，ψ，θ，φ分别表示无人机的航向角，俯仰角，翻滚角。比较姿态矩阵的四元数
形式(1)及欧拉角形式(2)，可知：

至此，可得姿态角的四元数表示形式，基于此形式可对风力干扰条件下基于卡尔
曼滤波的姿态计算问题进行深入分析。

第二步：卡尔曼滤波定姿方程

1.卡尔曼滤波姿态解算的状态方程

卡尔曼滤波姿态解算的预测方程表示为:

Xk＝Φk,k-1Xk-1+Wk-1 (4)

其中Φk,k-1为tk-1时刻到tk时刻的一步转移矩阵，Wk为系统噪声序列。

系统状态方程可表示为：

由于状态估计量为四元数，则状态方程用四元数微分方程可进一步表示为:

其中，ωx,ωy,ωz为安装在四旋翼飞行器上陀螺仪的角速度分量。

由于系统状态方程是连续的，不易采用数字化方法对其进行求解。针对此问题，目
前求解四元数微分方程主要有两种方法:一种是龙格库塔法，另一种是毕卡逼近法。本专利
采用四阶毕卡逼近法将其离散化，取q(t)＝[q0(t)q1(t)q2(t)q3(t)]，将(6)式离散化可得：

q(k+1)＝Φk,k-1q(k) (7)

其中：

2.卡尔曼滤波姿态解算的观测方程

风力干扰环境下，观测量可由以下三种测量值构成：加速度计、磁力计和风力。其
观测方程为：

Z(t)＝HX(t)+V(t) (8)

其中V(t)是白噪声，下面对测量值进行深入分析。

首先针对观测量加速度计和磁强计进行分析：

参考坐标系下重力向量定义为G＝[0 0 1]T，地磁场向量h＝[hx hy hz]T。其矩阵形
式可分别表示为：

其中，g，m分别是载体坐标系下加速度计及磁力计的量测值。由式(9)及(10)可得：

由以上所述，可得加速度计及磁力计量测值的四元数表示形式，下面针对风力进
行深入分析。

第三步：风力观测方程

1.气流坐标系

空气流动用幅值为VT的空速矢量VT表示，其方向由相对机体的两个角来定义，即攻
角α和侧滑角β，分别定义为：

机体坐标系(b)到气流坐标系(w)的旋转矩阵可表述如下：

由于则：

其中A为矢量，由上式可得：

其中，

基于上述分析，空速矢量可表示为：

在机体坐标系中，空速矢量可改写为：

至此，气流坐标系已建立，下面将在此坐标系下对风力干扰进行分析。

2.风力干扰

飞机惯性速度v为空速VT及风速W之和，可表示为：

v＝VT+W (16)

导航坐标系下，扰动风表示为Wn，机体坐标系飞机速度可表示为：

上式可改写为：

导航坐标系下，式(18)的矩阵形式可表示为：

由系统观测过程可知，四元数为关于加速度计磁力计的量测值及风力值的非线性
函数。为求解四元数，须基于雅克比矩阵将其线性化。基于式(12)及(13)，雅克比矩阵H可表
示为：

至此，得到了风力观测方程及将其线性化的雅克比矩阵。基于此，即可利用扩展卡
尔曼滤波对四元数进行解算。

第四步：基于扩展卡尔曼滤波流程的四元数姿态解算

基于以上讨论可知，基于风力的无人机姿态解算可利用扩展卡尔曼滤波实现。在
一个滤波周期内，卡尔曼滤波具有两个明显的更新过程：时间更新过程和量测更新过程。因
此，基于以上所述，基于风力的时间更新过程及量测更新过程可表述如下：

1.时间更新过程

状态一步预测：

均方误差的一步预测：

2.量测更新过程

卡尔曼滤波增益：

协方差阵更新：

Pk＝(I-KkHk)Pk/k-1 (24)

状态更新：

卡尔曼滤波是一种递推算法，该算法充分利用陀螺输出的角速率信号，磁强计输
出的地磁信号和风力数据，得到状态最优估计，且状态每一次更新估计都是由前一次估计
和新的输入数据计算得到，因此只需存储前一次估计，即可以实现实时处理。

本发明的效果可通过以下仿真进一步说明：

仿真条件

本节基于实测数据测试所提出方法的有效性，所得数据由如下传感器输出：陀螺
仪，磁强计，GPS和空速管。仿真数据基于以下场景得到：采样频率是100HZ，采样个数是250，
取大连为实验地，垂直分量为3.12×10-5，水平分量是3.26×10-5，h＝10-5×[0.256 3.26
3.12]T。卡尔曼滤波初值为q0＝[1 0 0 0]T，其对应的均方误差阵为：p0＝diag[0.2846
0.2846 0.2846 0.2846]T。其中α＝0，β＝0，VT＝1，u＝0，v＝1，w＝0。

仿真内容

从图2中可以看出，无风时翻滚角测量值更接近真实值，和有风时相比收敛的更
快，有风时考虑风测得的翻滚角与有风时不考虑风测得的翻滚角更准确，同时可以看出有
风时考虑风测得的翻滚角波动更大。从图3中可以看出，无风时俯仰角测量值更接近真实
值，有风时考虑风测得的俯仰角与有风时不考虑风测得的俯仰角更准确。从图4中可以看
出，无风时偏航角测量值更接近真实值，有风时考虑风测得的偏航角与有风时不考虑风测
得的偏航角更准确，同时可以看出有风时考虑风测得的偏航角波动更大。综上所述，风力的
存在会显著影响姿态解算解结果，进而影响无人机的有效控制，因此将风作为观测量，通过
扩展卡尔曼滤波算法进行姿态解算，从而提高无人机姿态解算精度，进而改善无人机控制
性能。

结论

针对风力降低无人机姿态解算精度，进而影响无人机飞行有效控制的问题，基于
扩展卡尔曼滤波理论，本专利提出一种风力干扰环境下无人机姿态解算方法以增强姿态解
算的稳健性，进而提高无人机控制性能。所提方法通过四元数微分方程和陀螺仪噪声误差
建立扩展卡尔曼滤波状态方程，并基于加速度计、磁强计、风力相关数据，得到四元数解算
结果。基于实测数据的仿真结果表明：与无风场景下的姿态解算方法及有风场景下不考虑
风力的姿态解算方法相比，所提方法可有效提高姿态解算的稳健性，从而改善无人机控制
性能。