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

摘要

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

主权项

风力干扰环境下四旋翼飞行器姿态解算方法，其特征在于：包括如下步骤：第一步：坐标系定义以及姿态矩阵，为了描述飞行器的俯仰、偏航、横滚的姿态信息，需要建立相应的坐标系；本专利采用两个不同的三维坐标系，分别为导航坐标系n,定义为东北天坐标系；载体坐标系b，其中xb沿机体横轴指向右，yb沿机体纵轴指向前，zb沿机体竖直指向上，满足右手定则，原点皆为无人机重心；姿态解算在导航坐标系中完成，因而须将无人机上传感器测得的姿态信息经坐标变换矩阵映射至坐标系n；从导航坐标系到载体坐标系的姿态矩阵可表示为：Cnb=q02+q12-q22-q322(q1q2-q0q3)2(q1q3+q0q2)2(q1q2+q0q3)q02-q12+q22-q322(q2q3-q0q1)2(q1q3-q0q2)2(q2q3+q0q1)q02-q12+q22-q32---(1)]]> 其方向余弦形式可表述如下：Cnb=cosθcosψcosψsinθsinφ-cosφsinψsinφsinψ+cosφsinθscosψcosθsinψcosφcosψ+sinφsinθsinψcosφsinθsinψ-sinφcosψ-sinθsinφcosθcosφcosθ---(2)]]> 式中，ψ，θ，φ分别表示无人机的航向角，俯仰角，翻滚角；比较姿态矩阵的四元数形式(1)及欧拉角形式(2)，可知：ψ=arctan(2(q1q2+q0q3)q12+q02-q22-q32)θ=-arcsin(2(q1q3-q0q2))φ=arctan(2(q2q3+q0q1)-q12+q02-q22+q32)---(3)]]> 至此，可得姿态角的四元数表示形式，基于此形式可对风力干扰条件下基于卡尔曼滤波的姿态计算问题进行深入分析；第二步：卡尔曼滤波定姿方程①卡尔曼滤波姿态解算的状态方程卡尔曼滤波姿态解算的预测方程表示为:Xk＝Φk,k‑1Xk‑1+Wk‑1                    (4)其中Φk,k‑1为tk‑1时刻到tk时刻的一步转移矩阵，Wk为系统噪声序列；系统状态方程可表示为：q·(t)=12Ωbq(t)---(5)]]>由于状态估计量为四元数，则状态方程用四元数微分方程可进一步表示为:q·0(t)q·1(t)q·2(t)q·3(t)=120-ωx-ωy-ωzωx0ωz-ωyωy-ωx0ωxωzωy-ωx0q0(t)q1(t)q2(t)q3(t)---(6)]]>其中，ωx,ωy,ωz为安装在四旋翼飞行器上陀螺仪的角速度分量；由于系统状态方程是连续的，不易采用数字化方法对其进行求解；针对此问题，目前求解四元数微分方程主要有两种方法:一种是龙格库塔法，另一种是毕卡逼近法；本专利采用四阶毕卡逼近法将其离散化，取q(t)＝[q0(t) q1(t) q2(t) q3(t)]，将(6)式离散化可得：q(k+1)＝Φk,k‑1q(k)          (7)其中：Φk,k-1=1-Δθ028+Δθ04384-(12-Δθ0248)Δθx-(12-Δθ0248)Δθy-(12-Δθ0248)Δθz(12-Δθ0248)Δθx1-Δθ028+Δθ04384(12-Δθ0248)Δθz-(12-Δθ0248)Δθy(12-Δθ0248)Δθy-(12-Δθ0248)Δθz1-Δθ028+Δθ04384(12-Δθ0248)Δθx(12-Δθ0248)Δθz(12-Δθ0248)Δθy-(12-Δθ0248)Δθx1-Δθ028+Δθ04384]]>Δθx=∫kk+1ωxdt,Δθy=∫kk+1ωydt,Δθz=∫kk+1ωzdt,Δθ02=Δθx2+Δθy2+Δθz2]]>②卡尔曼滤波姿态解算的观测方程风力干扰环境下，观测量可由以下三种测量值构成：加速度计、磁力计和风力；其观测方程为：Z(t)＝HX(t)+V(t)                      (8)其中V(t)是白噪声，下面对测量值进行深入分析；首先针对观测量加速度计和磁强计进行分析：参考坐标系下重力向量定义为G＝[0 0 1]T，地磁场向量h＝[hx hy hz]T；其矩阵形式可分别表示为：gxgygz=Cnb001---(9)]]>mxmymz=Cnbhxhyhz---(10)]]>其中，g，m分别是载体坐标系下加速度计及磁力计的量测值；由式(9)及(10)可得：gx=2(q1q3-q0q2)gy=2(q2q3+q0q1)gz=q02-q12-q22+q32---(11)]]> mx=(q02+q02-q02-q02)hx+2(q1q1+q1q1)hy+2(q1q3-q0q2)hzmy=2(q1q2-q0q3)hx+(q02+q02-q02-q02)hy+2(q0q1+q2q3)hzmz=2(q1q3+q0q2)hx+2(q2q3-q0q1)hy+(q02-q02-q02+q02)hz---(12)]]>由以上所述，可得加速度计及磁力计量测值的四元数表示形式，下面针对风力进行深入分析；第三步：风力观测方程①气流坐标系空气流动用幅值为VT的空速矢量VT表示，其方向由相对机体的两个角来定义，即攻角α和侧滑角β，分别定义为：VT=uT2+vT2+wT2,α=arctan(wTuT),β=arcsin(vTVT),]]>机体坐标系(b)到气流坐标系(w)的旋转矩阵可表述如下：由于则：Aw=CbwAb---(13)]]>其中A为矢量，由上式可得：其中，Cbw=cosβsinβ0-sinβcosβ0001cosα0sinα010-sinα0cosα=cosαcosβsinβsinαcosβ-sinβcosαcosβ-sinαsinβ-sinα0cosα]]>基于上述分析，空速矢量可表示为：VTb=CwbVTw---(14)]]>在机体坐标系中，空速矢量可改写为：uTvTwT=CwbVT00---(15)]]>至此，气流坐标系已建立，下面将在此坐标系下对风力干扰进行分析；②风力干扰飞机惯性速度v为空速VT及风速W之和，可表示为：v＝VT+W           (16)导航坐标系下，扰动风表示为Wn，机体坐标系飞机速度可表示为：vb=VTb+CnbWn---(17)]]>上式可改写为：CnbWn=vb-VTb---(18)]]>导航坐标系下，式(18)的矩阵形式可表示为：uTvTwT=Cnbuvw-CnbWNWEWD---(19)]]>由系统观测过程可知，四元数为关于加速度计磁力计的量测值及风力值的非线性函数；为求解四元数，须基于雅克比矩阵将其线性化；基于式(12)及(13)，雅克比矩阵H可表示为：H=-2q22q3-2q02q12q12q02q32q22q0-2q1-2q22q32(q0hx+q3hy-q2hz)2(q1hx+q2hy+q3hz)2(-q2hx+q1hy-q0hz)2(-q3hx+q0hy+q1hz)2(-q3hx+q0hy+q1hz)2(q2hx-q1hy+q0hz)2(q1hx+q2hy+q3hz)2(-q0hx-q3hy+q2hz)2(q2hx-q1hy+q0hz)2(q3hx-q0hy-q1hz)2(q0hx+q3hy-q2hz)2(q1hx+q2hy+q3hz)2q3-2q2-2q12q0-2q02q1-2q22q3-2q1-2q0-2q2-2q3---(20)]]>至此，得到了风力观测方程及将其线性化的雅克比矩阵；基于此，即可利用扩展卡尔曼滤波对四元数进行解算；第四步：基于扩展卡尔曼滤波流程的四元数姿态解算。