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

说明书

技术领域

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

背景技术

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

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

发明内容

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

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

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

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

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

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

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

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

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

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

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

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

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

系统状态方程可表示为：

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