技術(shù)特征：

1.風力干擾環(huán)境下四旋翼飛行器姿態(tài)解算方法，其特征在于：包括如下步驟：

第一步：坐標系定義以及姿態(tài)矩陣，

為了描述飛行器的俯仰、偏航、橫滾的姿態(tài)信息，需要建立相應(yīng)的坐標系；本專利采用兩個不同的三維坐標系，分別為導(dǎo)航坐標系n,定義為東北天坐標系；載體坐標系b，其中x_b沿機體橫軸指向右，y_b沿機體縱軸指向前，z_b沿機體豎直指向上，滿足右手定則，原點皆為無人機重心；姿態(tài)解算在導(dǎo)航坐標系中完成，因而須將無人機上傳感器測得的姿態(tài)信息經(jīng)坐標變換矩陣映射至坐標系n；

從導(dǎo)航坐標系到載體坐標系的姿態(tài)矩陣可表示為：

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

式中，ψ，θ，φ分別表示無人機的航向角，俯仰角，翻滾角；比較姿態(tài)矩陣的四元數(shù)形式(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)$

至此，可得姿態(tài)角的四元數(shù)表示形式，基于此形式可對風力干擾條件下基于卡爾曼濾波的姿態(tài)計算問題進行深入分析；

第二步：卡爾曼濾波定姿方程

①卡爾曼濾波姿態(tài)解算的狀態(tài)方程

卡爾曼濾波姿態(tài)解算的預(yù)測方程表示為:

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

其中Φ_k,k-1為t_k-1時刻到t_k時刻的一步轉(zhuǎn)移矩陣，W_k為系統(tǒng)噪聲序列；

系統(tǒng)狀態(tài)方程可表示為：

$\stackrel{\·}{q}\left(t\right)=\frac{1}{2}{\mathrm{\Ω}}_{b}q\left(t\right)---\left(5\right)$

由于狀態(tài)估計量為四元數(shù)，則狀態(tài)方程用四元數(shù)微分方程可進一步表示為:

$\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為安裝在四旋翼飛行器上陀螺儀的角速度分量；

由于系統(tǒng)狀態(tài)方程是連續(xù)的，不易采用數(shù)字化方法對其進行求解；針對此問題，目前求解四元數(shù)微分方程主要有兩種方法:一種是龍格庫塔法，另一種是畢卡逼近法；本專利采用四階畢卡逼近法將其離散化，取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$

②卡爾曼濾波姿態(tài)解算的觀測方程

風力干擾環(huán)境下，觀測量可由以下三種測量值構(gòu)成：加速度計、磁力計和風力；其觀測方程為：

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

由以上所述，可得加速度計及磁力計量測值的四元數(shù)表示形式，下面針對風力進行深入分析；

第三步：風力觀測方程

①氣流坐標系

空氣流動用幅值為V_T的空速矢量V_T表示，其方向由相對機體的兩個角來定義，即攻角α和側(cè)滑角β，分別定義為：

$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)的旋轉(zhuǎn)矩陣可表述如下：

由于則：

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

導(dǎo)航坐標系下，擾動風表示為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)$

導(dǎo)航坐標系下，式(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)$

由系統(tǒng)觀測過程可知，四元數(shù)為關(guān)于加速度計磁力計的量測值及風力值的非線性函數(shù)；為求解四元數(shù)，須基于雅克比矩陣將其線性化；基于式(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)$

至此，得到了風力觀測方程及將其線性化的雅克比矩陣；基于此，即可利用擴展卡爾曼濾波對四元數(shù)進行解算；

第四步：基于擴展卡爾曼濾波流程的四元數(shù)姿態(tài)解算。

2.根據(jù)權(quán)利要求1所述的風力干擾環(huán)境下四旋翼飛行器姿態(tài)解算方法，其特征在于：包括如下步驟：

第一步：時間更新過程，

狀態(tài)一步預(yù)測：

${\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}---\left(21\right)$

均方誤差的一步預(yù)測：

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

協(xié)方差陣更新：

P_k＝(I-K_kH_k)P_k/k-1 (24)

狀態(tài)更新：

${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1}).---\left(25\right)$