[发明专利]风力干扰环境下四旋翼飞行器姿态解算方法

权利要求书

1.风力干扰环境下四旋翼飞行器姿态解算方法，其特征在于：包括如下步骤：

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

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

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

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

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

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

X_k＝Φ_k,k-1X_k-1+W_k-1(4)

其中Φ_k,k-1为t_k-1时刻到t_k时刻的一步转移矩阵，W_k为系统噪声序列；

系统状态方程可表示为：

$\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)＝[q₀(t) q₁(t) q₂(t) q₃(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＝[h_x h_y h_z]^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)$

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

第三步：风力观测方程

①气流坐标系

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

$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为空速V_T及风速W之和，可表示为：

v＝V_T+W (16)

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

$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)$

协方差阵更新：

P_k＝(I-K_kH_k)P_k/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)$