一种无人机控制方法、控制装置及控制系统

技术领域

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

背景技术

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

发明内容

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

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

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

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

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

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

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

优选的，步骤S2包括：

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

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

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

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

优选的，步骤S3包括：

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

经PI调节更新四元数；

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

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

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

采集三轴旋转角数据；

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

提取个尺度小波系数；

求各层的阈值；

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

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

初始模块，用以根据XYZ轴旋转角速度数据得到无人机的初始姿态角；

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

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

反馈模块，将姿态角修正模块中所述的初始姿态角替换成修正姿态角，并重复执行姿态角修正模块。

本发明提供了一种无人机控制系统，

包括存储器；

以及耦接至所述存储器的处理器，所述处理器被配置为基于存储在所述存储器中的指令，执行本发明所述的一种无人机控制方法。

优选的，所述控制系统还包括GPS、光流传感器、三轴加速度计、三轴陀螺仪和三轴磁强计。

与现有技术相比，本发明的有益效果是：

本发明提高了无人机在动态环境中姿态角的测量精度，同时也抑制了无人机由于机体震动对测量结果的影响。

附图说明

图1为本发明具体实施例的流程框图；

图2为采用传统互补滤波方法与本发明具体实施例的控制方法得到的横滚角偏差曲线；

图3为本实施例的坐标系模型。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施实例仅仅是本发明一部分实施实例，而不是全部的实施例。基于本发明中的实施实例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施实例，都属于本发明保护的范围。

请参照图1，本实施例为一种无人机控制方法，包括如下步骤：

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

具体包括如下步骤：

S11、建立机体坐标系和地面坐标系；

在无人机导航时,为正确计算导航必须使用合适的坐标系。无人机在飞行过程中，想要准确的求解姿态角、位置、速度等信息就需要建立所需要的坐标系。建立坐标系模型如图3，将无人机看作一个刚体，定义地面坐标系为Ox

S12、通过小波分解方法剔除XYZ轴旋转角速度的低频噪音和不稳定的信号，并且和高频分量重构，得到滤波后的三轴旋转角速度数据；

变换是将时间信号展开为小波数族的线性叠加小波，小波变换的核函数是小波函数，他在时间和频率域内都是局部化的。所以，通过小波变换可对信号同时在时-频域内进行联合分析。在去噪方面，小波分析能同时在时-频域中对信号进行分析，具有多层次分析功能，在不同的分解层上有效的区分信号的突变部分和噪声，从而实现信号的去噪。

本实施例的XYZ轴旋转角速度信号通过陀螺仪获取，陀螺仪的有用信号通常表现在高频信号，利用小波对含噪的原始信号分解后，含噪部分主要集中在低频小波函数中；并且包含有用信号的小波系数幅值较大，但是数目少；而噪声对应的小波系数幅值小，数目较多。基于上述特点，应用门阈值法对小波系数进行处理。

本实施例的小波分解的具体过程包括：

S121、采集三轴旋转角数据；

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

S123、提取个尺度小波系数；

S124、求各层的阈值；

本实施例采用小波函数为db3小波函数，应用硬阈值，当小波系数的绝对值大于等于给定阈值时，保持不变，而小于时，令其为0，如下式所示：

式中，ω为高频细节分量，λ为估计的阈值。

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

S13、使用四元数法获取旋转矩阵；

经过航向角ψ-俯仰角θ-横滚角γ旋转顺序，列出四元数的旋转矩阵为：

其中q

S14、通过四元数的微分方程求取四元数的实部和虚部，四元数的微分方程表示为：

其中，

其中，θ为俯仰角，γ为横滚角，ψ为航向角，q

S2、根据XYZ轴加速度数据和非重力加速度数据建立加速度修正模型，得到修正后的非重力加速度数值，用以减弱动态环境下非重力加速度对姿态解算的影响。

由于加速度计所测量机体的加速度，在动态环境中存在非重力加速度，导致姿态角的测量存在误差，本实施例通过建立加速度修正模型来实现加速度的修正，具体包括：

S21、对无人机的机体振动信号进行滤波，剔除由于无人机自身机体震动对加速度计产生的高频误差，具体包括如下步骤：

S211：无人机姿态保持不变，不断增加油门读取三轴加速度的数据；

S212：将得到的数据进行快速傅里叶变换，得到频域信息；

S213：通过频域信息和数据信号的对比，通过IIR数字滤波器滤除误差大的频域段，用以滤除无人机由于机体震动对加速度计产生的高频误差，得到检测到的三轴加速度信息g′

S22、通过光流法获取非重力加速度；

一个像素I(x,y,t)在第一帧的光强度，移动了(dx,dy)的距离到下一帧，用了dt时间，因为是同一个像素点，又依据假设的亮度恒定不变原则，得到光流法的基本约束方程为：

I(x,y,t)＝I(x+dx,y+dy,t+dt) (4)

将(4)进行泰勒展开得到：

其中ε代表二阶无穷小，可以忽略不计再将(5)带入(4)后同除dt可得到：

假设u,v分别为光流沿x轴和y轴的速度矢量，可得：

令

I

式子(8)中I

经归一化得：

其校正后的加速度更接近重力加速度。

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

S24、旋转矩阵由地面系旋转到机体坐标系，得到重力加速度经过旋转矩阵旋转到机体坐标系的重力矢量g

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

优选的，步骤S3包括：

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

e

对角度求导可以得到误差角速度。

经PID调节更新四元数；

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

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

本实施例的控制方法实现了无人机姿态角的动态调整，

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

初始模块，用以根据XYZ轴旋转角速度数据得到无人机的初始姿态角；

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

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

反馈模块，将姿态角修正模块中所述的初始姿态角替换成修正姿态角，并重复执行姿态角修正模块。

上述各模块的工作过程如上述实施例所述，此处不再赘述。

本发明提供了一种无人机控制系统，

包括存储器；

以及耦接至所述存储器的处理器，所述处理器被配置为基于存储在所述存储器中的指令，执行本发明所述的一种无人机控制方法。

还包括三轴加速度计模块、三轴陀螺仪模块、三轴磁强计模块和ATK-PMW3901光流模块、STM32F407开发板、SPL06气压计模块、GPS模块、福斯i6遥控器模块。人机交互和仿真模块：Linux下的QT上位机、Gazebo可视化仿真软件。其中：三轴加速度计模块获取无人机在XYZ轴上的加速度信息；三轴陀螺仪模块获取无人机绕XYZ轴旋转的角速度信息；三轴磁强计模块获取无人机航向角信息；ATK-PMW3901光流模块可以使无人机定点悬停并且提供无人机速度信息；STM32F407开发板为无人机主控板；SPL06气压计模块获取无人机的高度信息；QT上位机为自主开发的可视化上位机软件，可以规划无人机的航迹路线、读取无人机的姿态角信息并以图形的方式展现出来、读取传感器的原始数据，还可以连接Gazebo可视化仿真软件，实时提供仿真的数据信息；GPS模块来提供非重力加速度信息；在Gazebo可视化仿真软件可以调节无人机的飞行环境、飞行角度、飞行高度、飞行速度等参数。

为了验证本发明的实验效果要优于传统的姿态解算方法，本发明使用MATLAB进行数据验证，本发明通过横滚角进行实验效果说明。如图2所示：通过Gazebo仿真软件，设定风速为4m/s，打开四轴飞行器模型，其中曲线1代表改进后算法姿态角误差，曲线2表示原始算法姿态角误差，图中可以看出改进后的算法要优于传统的姿态解算算法。

以上所述，仅为本发明较佳的具体实施方式，但本实发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。