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

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

Attitude calculation method of four rotor aircraft in wind jamming environment

The invention belongs to the field of UAV control, the wind will affect the UAV attitude solution, and jamming UAV flight control problem, the extended Calman filter based on the theory, this paper proposes a wind interference environment of UAV attitude calculation method to enhance the robustness of the attitude calculation so as to improve the control performance of uav. In the proposed method, the extended Calman filtering equation of state is established by means of four element differential equation and gyro noise error. Based on the accelerometer, magnetometer and wind related data, the results of four variables calculation are obtained. Based on the measured data, the test results show that the proposed algorithm can effectively reduce the divergence of attitude angle, improve the accuracy of attitude calculation, and improve the UAV control performance.

【技术实现步骤摘要】
风力干扰环境下四旋翼飞行器姿态解算方法
本专利技术涉及风力干扰环境下四旋翼飞行器姿态解算方法，属于无人机飞行控制领域。
技术介绍
近年来，随着微电子技术，微机电技术，高聚合物新能源技术，先进控制技术的发展，四旋翼飞行器已成为人们研究的热点。无人飞行器的种类繁多，其中四旋翼无人飞行器是一个重要的研究方向。与其他无人飞行器相比，四旋翼飞行器可以实现垂直起降，定点悬停等功能，同时具有体积小，机构简单，成本低，机动性能好，有效载荷能力好，易于穿行狭小空间等优势，因此在环境监测、灾区营救、输电线路巡查和空中航拍等领域得到了广泛的应用。飞行器的位姿估计是实现无人飞行器(UAV)自主能力飞行的基础。互补滤波最先在无人机姿态解算中得到很好的应用，然而姿态解算的精度还是达不到要求。在过去的几十年里，众多学者致力于研究卡尔曼滤波算法，如：扩展卡尔曼滤波(EKF)，无迹卡尔曼滤波(UKF)和粒子滤波。并成功将EKF和UKF运用到无人机姿态解算中去，得到了更高精度的姿态角。万晓凤，康利平，余运俊等基于互补滤波算法进行姿态解算，并取得了较高精度的姿态角。李萧然，陈谋等人基于EKF进行无人机姿态解算，得到了很高精度的姿态角。朱岩，付巍等人基于UKF进行无人机姿态解算，得到了更高精度的姿态角。以上学者皆没有考虑风力估算，针对此问题，ACho等人提出用一个单天线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＝[001]T，地磁场向量h＝[hxhyhz]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为本专利技术分别在无风，有风本文档来自技高网...

【技术保护点】
风力干扰环境下四旋翼飞行器姿态解算方法，其特征在于：包括如下步骤：第一步：坐标系定义以及姿态矩阵，为了描述飞行器的俯仰、偏航、横滚的姿态信息，需要建立相应的坐标系；本专利采用两个不同的三维坐标系，分别为导航坐标系n,定义为东北天坐标系；载体坐标系b，其中x

【技术特征摘要】
1.风力干扰环境下四旋翼飞行器姿态解算方法，其特征在于：包括如下步骤：第一步：坐标系定义以及姿态矩阵，为了描述飞行器的俯仰、偏航、横滚的姿态信息，需要建立相应的坐标系；本专利采用两个不同的三维坐标系，分别为导航坐标系n,定义为东北天坐标系；载体坐标系b，其中xb沿机体横轴指向右，yb沿机体纵轴指向前，zb沿机体竖直指向上，满足右手定则，原点皆为无人机重心；姿态解算在导航坐标系中完成，因而须将无人机上传感器测得的姿态信息经坐标变换矩阵映射至坐标系n；从导航坐标系到载体坐标系的姿态矩阵可表示为：其方向余弦形式可表述如下：式中，ψ，θ，φ分别表示无人机的航向角，俯仰角，翻滚角；比较姿态矩阵的四元数形式(1)及欧拉角形式(2)，可知：至此，可得姿态角的四元数表示形式，基于此形式可对风力干扰条件下基于卡尔曼滤波的姿态计算问题进行深入分析；第二步：卡尔曼滤波定姿方程①卡尔曼滤波姿态解算的状态方程卡尔曼滤波姿态解算的预测方程表示为: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)其中：

【专利技术属性】
技术研发人员：王洪雁，裴炳南，郑佳，房云飞，季科，乔惠娇，
申请(专利权)人：大连大学，
类型：发明
国别省市：辽宁,21