CN113325865A - 一种无人机控制方法、控制装置及控制系统 - Google Patents
一种无人机控制方法、控制装置及控制系统 Download PDFInfo
- Publication number
- CN113325865A CN113325865A CN202110514735.6A CN202110514735A CN113325865A CN 113325865 A CN113325865 A CN 113325865A CN 202110514735 A CN202110514735 A CN 202110514735A CN 113325865 A CN113325865 A CN 113325865A
- Authority
- CN
- China
- Prior art keywords
- attitude angle
- corrected
- unmanned aerial
- data
- aerial vehicle
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
- G05D1/0808—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
- G05D1/106—Change initiated in response to external conditions, e.g. avoidance of elevated terrain or of no-fly zones
Abstract
本发明涉及无人机惯性导航领域,尤其涉及一种无人机控制方法、控制装置及控制系统;本发明根据XYZ轴旋转角速度数据得到无人机的初始姿态角;根据XYZ轴加速度数据和非重力加速度数据建立加速度修正模型,得到修正后的非重力加速度数值;修正后的非重力加速度数值结合初始姿态角对XYZ轴旋转角速度数据进行修正,得到修正姿态角;将步骤S3的初始姿态角替换成修正姿态角,并重复步骤S3;本发明提高了无人机在动态环境中姿态角的测量精度,同时也抑制了无人机由于机体震动对测量结果的影响。
Description
技术领域
本发明涉及属于无人机惯性导航领域,尤其涉及一种无人机控制方法、控制装置及控制系统。
背景技术
稳定的姿态输出是无人机完成飞行任务的前提,考虑到无人机的成本和体积,目前低成本小型无人机通常采用微机电系统(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,将无人机看作一个刚体,定义地面坐标系为Oxnynzn,机体坐标系为Oxbybzb,姿态角度由导航系到载体系的姿态求解过程如图3所示。导航坐标系为Oxnynzn经过由航向角-俯仰角-翻滚角的旋转顺序即可旋转到机体坐标系Oxbybzb,图中定义θ为俯仰角,γ为横滚角,ψ为航向角。
S12、通过小波分解方法剔除XYZ轴旋转角速度的低频噪音和不稳定的信号,并且和高频分量重构,得到滤波后的三轴旋转角速度数据;
变换是将时间信号展开为小波数族的线性叠加小波,小波变换的核函数是小波函数,他在时间和频率域内都是局部化的。所以,通过小波变换可对信号同时在时-频域内进行联合分析。在去噪方面,小波分析能同时在时-频域中对信号进行分析,具有多层次分析功能,在不同的分解层上有效的区分信号的突变部分和噪声,从而实现信号的去噪。
本实施例的XYZ轴旋转角速度信号通过陀螺仪获取,陀螺仪的有用信号通常表现在高频信号,利用小波对含噪的原始信号分解后,含噪部分主要集中在低频小波函数中;并且包含有用信号的小波系数幅值较大,但是数目少;而噪声对应的小波系数幅值小,数目较多。基于上述特点,应用门阈值法对小波系数进行处理。
本实施例的小波分解的具体过程包括:
S121、采集三轴旋转角数据;
S122、对采集的所述三轴旋转角数据进行小波分解,将其分解为三层小波;
S123、提取个尺度小波系数;
S124、求各层的阈值;
本实施例采用小波函数为db3小波函数,应用硬阈值,当小波系数的绝对值大于等于给定阈值时,保持不变,而小于时,令其为0,如下式所示:
式中,ω为高频细节分量,λ为估计的阈值。
根据选取的阈值进行去噪和重构后对去噪效果进行评估,输出去噪后的陀螺仪数据。
S13、使用四元数法获取旋转矩阵;
经过航向角ψ-俯仰角θ-横滚角γ旋转顺序,列出四元数的旋转矩阵为:
其中q0为实部q1、q2、q3为虚部的系数。
S14、通过四元数的微分方程求取四元数的实部和虚部,四元数的微分方程表示为:
其中,为四元数的微分,q为姿态角四元数,ω为无人机在旋转过程中的旋转角速度的四元数表示法,表示四元数的乘法。列出四元数的微分方程后,利用龙格库塔法可以求得四元数的系数q0、q1、q2、q3进而可以求得无人机在飞行时候的俯仰角、翻滚角以及航向角。
其中,θ为俯仰角,γ为横滚角,ψ为航向角,q0、q1、q2、q3为四元数的系数。
S2、根据XYZ轴加速度数据和非重力加速度数据建立加速度修正模型,得到修正后的非重力加速度数值,用以减弱动态环境下非重力加速度对姿态解算的影响。
由于加速度计所测量机体的加速度,在动态环境中存在非重力加速度,导致姿态角的测量存在误差,本实施例通过建立加速度修正模型来实现加速度的修正,具体包括:
S21、对无人机的机体振动信号进行滤波,剔除由于无人机自身机体震动对加速度计产生的高频误差,具体包括如下步骤:
S211:无人机姿态保持不变,不断增加油门读取三轴加速度的数据;
S212:将得到的数据进行快速傅里叶变换,得到频域信息;
S213:通过频域信息和数据信号的对比,通过IIR数字滤波器滤除误差大的频域段,用以滤除无人机由于机体震动对加速度计产生的高频误差,得到检测到的三轴加速度信息g′a;
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轴的速度矢量,可得:
Ixu+Iyv+It=0 (8)
式子(8)中Ix、Iy、It均可有图像数据求取,而u,v即为光流法所求得的速度,最后对u,v求取时间的导数,可以得到非重力加速度进而确定非重力加速度的矢量gf,将得到的gf和ga通过矢量减法得到校正后加速度gb1为:
经归一化得:
其校正后的加速度更接近重力加速度。
S24、旋转矩阵由地面系旋转到机体坐标系,得到重力加速度经过旋转矩阵旋转到机体坐标系的重力矢量gb为:
S3、所述修正后的非重力加速度数值结合初始姿态角对XYZ轴旋转角速度数据进行修正,得到修正姿态角;
优选的,步骤S3包括:
根据四元数的微分方程和修正后的非重力加速度数值叉乘得到三轴误差角度为:
eacc=gb×gb2 (13)
对角度求导可以得到误差角速度。
经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表示原始算法姿态角误差,图中可以看出改进后的算法要优于传统的姿态解算算法。
以上所述,仅为本发明较佳的具体实施方式,但本实发明的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本发明揭露的技术范围内,根据本发明的技术方案及其发明构思加以等同替换或改变,都应涵盖在本发明的保护范围之内。
Claims (10)
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、光流传感器、三轴加速度计、三轴陀螺仪和三轴磁强计。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110514735.6A CN113325865A (zh) | 2021-05-10 | 2021-05-10 | 一种无人机控制方法、控制装置及控制系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110514735.6A CN113325865A (zh) | 2021-05-10 | 2021-05-10 | 一种无人机控制方法、控制装置及控制系统 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN113325865A true CN113325865A (zh) | 2021-08-31 |
Family
ID=77415516
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110514735.6A Pending CN113325865A (zh) | 2021-05-10 | 2021-05-10 | 一种无人机控制方法、控制装置及控制系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113325865A (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117389322A (zh) * | 2023-12-08 | 2024-01-12 | 天津天羿科技有限公司 | 无人机控制方法 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP2730888A1 (en) * | 2012-11-07 | 2014-05-14 | Ecole Polytechnique Federale de Lausanne EPFL-SRI | Method to determine a direction and amplitude of a current velocity estimate of a moving device |
CN105094138A (zh) * | 2015-07-15 | 2015-11-25 | 东北农业大学 | 一种用于旋翼无人机的低空自主导航系统 |
CN106959110A (zh) * | 2017-04-06 | 2017-07-18 | 亿航智能设备(广州)有限公司 | 一种云台姿态检测方法及装置 |
CN109540126A (zh) * | 2018-12-03 | 2019-03-29 | 哈尔滨工业大学 | 一种基于光流法的惯性视觉组合导航方法 |
CN111982100A (zh) * | 2020-07-07 | 2020-11-24 | 广东工业大学 | 一种无人机的航向角解算算法 |
-
2021
- 2021-05-10 CN CN202110514735.6A patent/CN113325865A/zh active Pending
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP2730888A1 (en) * | 2012-11-07 | 2014-05-14 | Ecole Polytechnique Federale de Lausanne EPFL-SRI | Method to determine a direction and amplitude of a current velocity estimate of a moving device |
CN105094138A (zh) * | 2015-07-15 | 2015-11-25 | 东北农业大学 | 一种用于旋翼无人机的低空自主导航系统 |
CN106959110A (zh) * | 2017-04-06 | 2017-07-18 | 亿航智能设备(广州)有限公司 | 一种云台姿态检测方法及装置 |
CN109540126A (zh) * | 2018-12-03 | 2019-03-29 | 哈尔滨工业大学 | 一种基于光流法的惯性视觉组合导航方法 |
CN111982100A (zh) * | 2020-07-07 | 2020-11-24 | 广东工业大学 | 一种无人机的航向角解算算法 |
Non-Patent Citations (1)
Title |
---|
盛蔚 等: "飞行器导航系统优化设计仿真", 计算机仿真, vol. 34, no. 8, pages 80 - 84 * |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117389322A (zh) * | 2023-12-08 | 2024-01-12 | 天津天羿科技有限公司 | 无人机控制方法 |
CN117389322B (zh) * | 2023-12-08 | 2024-03-01 | 天津天羿科技有限公司 | 无人机控制方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108827299B (zh) | 一种基于改进四元数二阶互补滤波的飞行器姿态解算方法 | |
CN108168574B (zh) | 一种基于速度观测的8位置捷联惯导系统级标定方法 | |
CN111551174A (zh) | 基于多传感器惯性导航系统的高动态车辆姿态计算方法及系统 | |
Phuong et al. | A DCM based orientation estimation algorithm with an inertial measurement unit and a magnetic compass | |
Batista et al. | Accelerometer calibration and dynamic bias and gravity estimation: Analysis, design, and experimental evaluation | |
CN108731667B (zh) | 用于确定无人驾驶车辆的速度和位姿的方法和装置 | |
CN110017837B (zh) | 一种姿态抗磁干扰的组合导航方法 | |
Sun et al. | Mooring alignment for marine SINS using the digital filter | |
CN112066985B (zh) | 一种组合导航系统初始化方法、装置、介质及电子设备 | |
CN109425339B (zh) | 一种基于惯性技术的考虑杆臂效应的舰船升沉误差补偿方法 | |
CN105910606A (zh) | 一种基于角速度差值的方向修正方法 | |
CN110567492A (zh) | 低成本mems惯性传感器系统级标定方法 | |
CN106403952A (zh) | 一种动中通低成本组合姿态测量方法 | |
CN111121766A (zh) | 一种基于星光矢量的天文与惯性组合导航方法 | |
CN112665574B (zh) | 基于动量梯度下降法的水下机器人姿态采集方法 | |
CN110954102A (zh) | 用于机器人定位的磁力计辅助惯性导航系统及方法 | |
CN111189474A (zh) | 基于mems的marg传感器的自主校准方法 | |
CN111121820B (zh) | 基于卡尔曼滤波的mems惯性传感器阵列融合方法 | |
CN113218391A (zh) | 一种基于ewt算法的姿态解算方法 | |
CN109186630B (zh) | 基于改进阈值函数小波降噪的mems粗对准方法及系统 | |
Gao et al. | An integrated land vehicle navigation system based on context awareness | |
CN108426584B (zh) | 汽车多传感器的校准方法 | |
CN113325865A (zh) | 一种无人机控制方法、控制装置及控制系统 | |
CN108871319B (zh) | 一种基于地球重力场与地磁场序贯修正的姿态解算方法 | |
CN107389092B (zh) | 一种基于磁传感器辅助的陀螺标定方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination |