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

权利要求书

1.一种无人机控制方法，其特征在于，包括如下步骤：

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

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

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

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

2.根据权利要求1所述一种无人机控制方法，其特征在于，步骤S2包括：

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

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

3.根据权利要求1所述一种无人机控制方法，其特征在于，所述非重力加速度通过光流法获取。

4.根据权利要求1所述一种无人机控制方法，其特征在于，所述步骤S2包括XYZ轴加速度数据经IIR数字滤波的步骤。

5.根据权利要求1所述一种无人机控制方法，其特征在于，步骤S3包括：

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

经PI调节更新四元数；

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

6.根据权利要求1所述一种无人机控制方法，其特征在于，步骤S1包括XYZ轴旋转角速度数据进行小波分解的步骤。

7.根据权利要求6所述一种无人机控制方法，其特征在于，小波分析过程包括：

采集三轴旋转角数据；

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

提取个尺度小波系数；

求各层的阈值；

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

8.一种无人机控制装置，其特征在于，包括：

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

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

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

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

9.一种无人机控制系统，其特征在于，

包括存储器；

以及耦接至所述存储器的处理器，所述处理器被配置为基于存储在所述存储器中的指令，执行如权利要求1至7中任一项所述的一种无人机控制方法。

10.根据权利要求9所述一种无人机控制系统，其特征在于，包括GPS、光流传感器、三轴加速度计、三轴陀螺仪和三轴磁强计。