[发明专利]一种无人机控制方法、控制装置及控制系统

说明书

本发明涉及无人机惯性导航领域，尤其涉及一种无人机控制方法、控制装置及控制系统；本发明根据XYZ轴旋转角速度数据得到无人机的初始姿态角；根据XYZ轴加速度数据和非重力加速度数据建立加速度修正模型，得到修正后的非重力加速度数值；修正后的非重力加速度数值结合初始姿态角对XYZ轴旋转角速度数据进行修正，得到修正姿态角；将步骤S3的初始姿态角替换成修正姿态角，并重复步骤S3；本发明提高了无人机在动态环境中姿态角的测量精度，同时也抑制了无人机由于机体震动对测量结果的影响。

技术领域

本发明涉及属于无人机惯性导航领域，尤其涉及一种无人机控制方法、控制装置及控制系统。

背景技术

稳定的姿态输出是无人机完成飞行任务的前提，考虑到无人机的成本和体积，目前低成本小型无人机通常采用微机电系统(micro-electromechanical system，MEMS)传感器构成的导航系统获取姿态信息。由于低成本传感器存在对非重力误差、机体震动误差的敏感性以及自身温漂等问题。因此如何利用低成本传感器达到高精度姿态输出成为了无人机导航领域的一个重要研究课题。为解决上述问题，通常应用互补滤波进行传感器信息的融合，提升系统的测量精度。但是，传统的互补滤波法不适用于存在非重力加速度的动态环境中，同时也没有考虑到机体自身震动对传感器产生的影响。即使有通过GPS/INS组合导航算法，但是获得的姿态角依然存在较大误差。

发明内容

本发明的目的是为了解决现有技术中存在的缺点，而提出的一种无人机控制方法、控制装置及控制系统。

为了实现上述目的，本发明采用了如下技术方案：

本发明提供了一种无人机控制方法，包括如下步骤：

S1、根据XYZ轴旋转角速度数据得到无人机的初始姿态角；

S2、根据XYZ轴加速度数据和非重力加速度数据建立加速度修正模型，得到修正后的重力加速度数值；

S3、所述修正后的非重力加速度数值结合初始姿态角对XYZ轴旋转角速度数据进行修正，得到修正姿态角；

S4、将步骤S3中所述的初始姿态角替换成修正姿态角，并重复步骤S3。

优选的，步骤S2包括：

通过四元数的微分方程求得初始地面到机体的旋转矩阵；

旋转矩阵由地面系旋转到机体坐标系，得到重力加速度经过旋转矩阵旋转到机体坐标系的重力矢量，进而得到修正后的加速度。

优选的，所述非重力加速度通过光流法获取。

优选的，所述步骤S2包括XYZ轴加速度数据经IIR数字滤波的步骤。

优选的，步骤S3包括：

根据四元数的微分方程和修正后的非重力加速度数值叉乘得到三轴误差角度；

经PI调节更新四元数；

通过龙格库塔法得修正姿态角。

优选的，步骤S1包括XYZ轴旋转角速度数据进行小波分解的步骤。

优选的，小波分析过程包括：

采集三轴旋转角数据；

对采集的所述三轴旋转角数据进行小波分解，将其分解为三层小波；

提取个尺度小波系数；

求各层的阈值；

根据选取的阈值进行去噪和重构后对去噪效果进行评估，输出去噪后的陀螺仪数据。

本发明提供了一种无人机控制装置，包括：