基于5G通讯的双路IMU单元无人机控制器及控制方法与流程

基于5g通讯的双路imu单元无人机控制器及控制方法
技术领域
1.本发明涉及无人机控制技术领域，特别涉及一种基于5g通讯的双路imu单元无人机控制器及控制方法。


背景技术：

2.随着无人机技术的发展，其在航拍、农业、植保、微型自拍、快递运输、灾难救援、观察野生动物、监控传染病、测绘、新闻报道、电力巡检、救灾、影视拍摄、制造浪漫等等领域具有广泛的应用，已逐渐进入人们的生活，imu(惯性测量单元)为无人机的重要组成部分，主要用于测量测量物体三轴姿态角或角速率以及加速度，一般集成在无人机飞行控制器中。
3.然而目前无人机飞行控制器仅能通过usb、usart、i2c等有线连接的方式进行数据通讯与交互，无法进行有效的自主控制；且飞控中的用于姿态检测的imu单元为单组imu单元，这使得无人机在飞行过程中易受到干扰；另一方面，随着时间的变化，由于imu单元产生的温度漂移也会导致无人机对自身姿态预估的不准确。


技术实现要素：

4.为解决上述问题，本发明提供了一种基于5g通讯的双路imu单元无人机控制器及控制方法，控制器采用5g模组通过无线的通信的方式实现姿态的控制，同时设有两路imu单元，通过记录两路imu单元的参数变化，进行算法处理，基于两组imu单元进行互补来修正由于温度产生的误差，有效的减弱由于温度变化产生的误差对无人机飞行控制器的影响，提高无人机的稳定性。
5.本发明提供了一种基于5g通讯的双路imu单元无人机控制器，具体技术方案如下：
6.包括嵌入式处理单元、5g通讯模块，电源稳压模块、gps模块、地磁传感器模块以及两组imu传感器模块，两组所述imu传感器模块分别与所述嵌入式处理单元连接，且所述嵌入式处理单元与所述5g通讯模块连接，两组所述imu传感器模块呈z轴方向相反，x、y轴方向相同的镜像对称设置。
7.进一步的，还设有can通讯模块、串口通讯模块、i2c通讯模块和spi通讯模块与嵌入式处理单元连接，并设有对应的硬件接口，所述can通讯模块设有两个。
8.本发明还提供了一种基于5g通讯的双路imu单元无人机控制方法，基于上述无人机控制器，具体包括如下步骤：
9.s1：检测imu传感器模块是否异常，若正常则通过双路imu检测数据进行互补滤波处理调整飞行姿态，若出现异常则执行返航操作；
10.s2：获取地磁传感器模块的检测数据进行校准并记录保存；
11.s3：获取两组imu传感器模块的反馈信号，并进行滤波处理，得到加速度数据和角速度数据，所述加速度数据和所述角速度数据为所述imu传感器模块在不同轴向的加速度以及角速度；
12.s4：对滤波处理后得到的加速度数据和角速度数据分别进行融合处理和积分处
理；
13.s5：将融合处理和积分处理后的数据以及校准后地磁传感器模块的检测数据作为输入，通过扩展卡尔曼滤波算法计算无人机的姿态角并输出。
14.进一步的，步骤s1中，imu传感器模块检测，具体过程如下：
15.获取两组imu传感器模块的检测数据，根据检测数据判断imu传感器模块是否异常，若存在异常，则通过usb通讯模块和can通讯模块上传故障信息，并基于正常工作的imu传感器模块和地磁传感器的数据通过扩展卡尔曼滤波算法计算无人机的姿态角，判断无人机航向，执行返航操作；
16.计算得到两组imu传感器模块检测数据的绝对值差值，并与预设值进行比较，当超出预设值时，则通过5g模块连接服务器端推送故障信息，并通过gps模块进行返航。
17.进一步的，步骤s3中，加速度数据和角速度数据的获取具体为：
18.s301：根据采集频率，获取设定数量的相邻时间采集节点采集的加速度和角速度，并按照不同轴向构成加速度数据集和角速度数据集；
19.s302：对所述加速度数据集和所述角速度数据集中的数据进行均值处理，分别获取所述加速度数据集和所述角速度数据集对应的各imu传感器不同轴向的加速度数据和角速度数据。
20.进一步的，在步骤s302中，所述均值处理之前，还包括：
21.对所述加速度数据集和所述角速度数据集中的数据分别进行桶排序处理，并去除至少一个最大加速度、角速度数据和至少一个最小加速度、角速度数据。
22.进一步的，所述滤波处理采用陷波滤波器进行滤波处理，具体如下：
[0023][0024]
其中，s为输入信号的频率，ωn为陷波频率，δ1、δ2为陷波系数。
[0025]
进一步的，所述陷波系数通过陷波频率计算得到，所述陷波频率为无人机的电机换向频率，具体计算如下：
[0026][0027][0028]
其中，depth为陷波深度、δf为陷波频率范围的差值。
[0029]
本发明的有益效果如下：
[0030]
1、设有两路imu，在无人机控制器电路板的两面呈镜像对称设置，通过两路imu采集的数据进行互补滤波处理，减弱由于温度变化产生的误差对控制器的影响，提高了控制器的稳定性，同时保证在外部因素干扰的情况下，能够准确的预估无人机的飞行姿态。
[0031]
2、控制器采用5g通讯模组实现数据的通讯与传输，便于引入外部远程控制，提高
了无人机飞行过程中通讯的稳定性。
附图说明
[0032]
图1是本发明的控制器结构示意图；
[0033]
图2是本发明的方法流程示意图；
[0034]
图3是本发明的两组imu相对位置结构示意图。
具体实施方式
[0035]
在下面的描述中对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0036]
实施例1
[0037]
本发明的实施例1公开了一种基于5g通讯的双路imu单元无人机控制器，如图1所示，
[0038]
包括嵌入式处理单元、5g通讯模块，电源稳压模块、gps模块、地磁传感器模块以及两组imu传感器模块，所述imu传感器模块分别为imu传感器模块1和imu传感器模块2，且分别与所述嵌入式处理单元连接，且所述嵌入式处理单元与所述5g通讯模块连接，两组所述imu传感器模块呈y、z轴方向相反，x轴方向相同的镜像对称设置；
[0039]
如图3所示，即所述imu传感器模块，一路安装在无人机控制器电路板的正面，朝上安装，一路imu单元安装在无人机控制器的背面，朝下安装，保持x轴正方向一致，y、z轴正方向相反，x和y轴均为无人机水平面的轴向，z轴为无人机纵面的轴向，x、y、z轴相互垂直。
[0040]
本实施例中，控制器还设有can通讯模块、串口通讯模块、i2c通讯模块和spi通讯模块与嵌入式处理单元连接，并设有对应的硬件接口，所述can通讯模块设有两个，所述串口通讯模块采用usb3.0模块电路；
[0041]
控制器还设有隔离电路和电平转换电路，所述嵌入式处理单元通过电平转换电路和隔离电路分别与各模块连接，所述电源稳压模块设有两个分别为第一电源稳压模块和第二电源稳压模块，且分别与嵌入式处理单元和各模块连接，提供稳定电压输入。
[0042]
本实施例中，飞行控制器设有金属外壳，即控制硬件电路放置在密闭的金属腔体中，有效减少外部的电磁干扰和自身电磁辐射。
[0043]
所述嵌入式处理单元，采用gd32芯片作为处理器，用于实时采集无人机姿态等信息，实现无人机自主控制。
[0044]
所述电平转换电路，由于各个外部芯片存在不同的电平协议，用于通过电平转换芯片将各模块不同的电平协议转换成对于嵌入式处理单元有效的电平进行输入。
[0045]
所述隔离电路，用于保证嵌入式处理单元的稳定，使得在外部元器件被意外击穿时保证嵌入式处理单元和其他器件不受影响，最大程度的保护电路中的各路元器件。
[0046]
所述i2c通讯模块，主要用于与两组imu传感器模块的通讯连接以及外围传感器接入
[0047]
imu传感器模块，用为对无人机的姿态进行检测，实时监控无人机的姿态并将数据
传入嵌入式处理器单元。
[0048]
5g通讯模块，用于无人机与外部实现通信，实现远程控制和实时监控的模块，有助于实现无人机的管控。
[0049]
串口通讯模块，用于无人机控制器的初步调试和gps模块的输入，以及can通讯协议的转化。
[0050]
can口通讯模块，用于无人机控制器外扩其他传感器。
[0051]
spi通讯模块，用于嵌入式处理单元外扩存储的接口。
[0052]
实施例2
[0053]
本发明的实施例2基于上述实施例1公开了一种基于5g通讯的双路imu单元无人机控制方法，基于上述实施例1所述的无人机控制器；在此记两组imu传感器模块分别为imu1和imu2，俯视无人机时，imu1在上，imu2在下；
[0054]
由于imu1和imu2测量的x轴方向的加速度和角速度正负性一致，数值变化方向相同，通过两者之间和的差值即可大致得出飞行控制器在x轴方向上的加速度值，ax＝(a1x a2x)/2，ax表示无人机在x轴方向的加速度值，a1x和a2x分别表示imu1和imu2检测的x轴方向的加速度值；
[0055]
imu1和imu2测量的y轴方向上的加速度和角速度的参数相反，可记为，当a1y》0且a2y《0时，此时俯视无人机，无人机受到向左的力，即ay》0；反之a1y《0且a2y》0时，此时俯视无人机，无人机受到向右的力，即ay《0；其中，ay表示无人机在y轴方向的加速度值，a1y、a2y分别表示imu1和imu2在y轴方向上的加速度值；无人机在y轴上的加速度记为为ay＝(a1y-a2y)/2；
[0056]
imu1和imu2在z轴方向上的加速度记为az＝(a1z a2z)/2，a1z和a2z分别表示imu1和imu2在z轴方向上的加速度值，由于无人机在z轴方向上两个加速度增长方向相同，但由于z轴的加速度计本质是测算压力，对于imu1正面朝上，压力在底面，静止时测出的加速度为g，对于imu2正面朝下，静止时测出的加速度为-g，即本实施例中，在飞控静止时测算z轴加速度值为0。
[0057]
基于上述，如图2所示，本实施例中，控制方法具体步骤流程如下：
[0058]
s1：嵌入式处理单元新建imu检测线程，检测imu1和imu2反馈的参数；
[0059]
根据检测数据判断imu传感器模块是否异常，若存在异常，则通过usb通讯模块和can通讯模块上传故障信息，并基于正常工作的imu传感器模块和地磁传感器的数据通过扩展卡尔曼滤波算法计算无人机的姿态角，判断无人机航向，执行返航操作；
[0060]
计算得到两组imu传感器模块检测数据的绝对值差值，并与预设值进行比较，当超出预设值时，则通过5g模块连接服务器端推送故障信息，并通过gps模块进行返航。
[0061]
s2：获取地磁传感器模块的检测数据进行校准并记录保存；
[0062]
s3：获取两组imu传感器模块的加速度数据和角速度数据，并进行滤波处理，所述加速度和角速度数据为所述imu传感器模块在不同轴向的加速度以及角速度；
[0063]
嵌入式处理单元新建一个线程用于对imu数据的采集，本实施例中，采集频率设定为100hz；
[0064]
由于采用的无刷电机通常与无人机的机臂是紧耦合连接的，在无人机在飞行过程中，无刷电机高频换向会产生磁场干扰和一定频率的电机震动，因而通过检测无人机的转
速进行陷波处理来滤除这部分夹杂的干扰；
[0065]
将采集的数据信号输入到陷波滤波器中进行滤波处理，过程如下：
[0066][0067]
其中，s为输入信号的频率，ωn为陷波频率，δ1、δ2为陷波系数。
[0068]
所述陷波系数通过陷波频率计算得到，所述陷波频率为无人机的电机换向频率，具体计算如下：
[0069][0070][0071]
其中，depth为陷波深度、δf为陷波频率范围的差值；
[0072]
所述电机换向频率通过电机转速计算得到，具体计算公式为：f＝n*p/60，其中，n为电机转速(转/分)、p为磁极数；
[0073]
对采集的数据信号进行滤波后得到加速度数据和角速度数据，嵌入式处理单元开辟12个1*2byte的链表空间分别记为a1x、a1y、a1z、ω1x、ω1y、ω1z、a2x、a2y、a2z、ω2x、ω2y、ω2z，分别保存imu1的x、y、z轴的加速度，imu1的x、y、z轴的角速度，imu2的x、y、z轴的加速度，imu2的x、y、z轴的角速度。
[0074]
s3：对得到的加速度数据和角速度数据分别进行融合处理和积分处理；
[0075]
所述融合处理即：
[0076][0077][0078][0079]
s4：将融合处理和积分处理后的数据以及校准后地磁传感器模块的检测数据作为输入，通过扩展卡尔曼滤波算法计算无人机的姿态角并输出，实现无人机的姿态控制。
[0080]
实施例3
[0081]
本发明的实施例3公开了一种基于5g通讯的双路imu单元无人机控制方法，基于上述实施例2，在步骤s3中，通过获取多组加速度和角速度数据进行均值处理，得到进行姿态处理输入的加速度数据和角速度数据，具体过程如下：
[0082]
s301：根据采集频率，获取10个的相邻时间采集节点采集的加速度和角速度，并按照不同轴向构成加速度数据集和角速度数据集，即获取imu1、imu2采集的连续10组加速度和角速度数据，构成相应的数据集；
[0083]
嵌入式处理单元开辟12个20*2byte的链表空间存储得到的加速度数据集和角速度数据集的加速度和角速度；
[0084]
将12组采集到得数据，对每组中的10个数据进行桶排序处理并保存到原链表中，并去除每组中两个最大的和两个最小的值。
[0085]
s302：对每组桶排序处理和最大最小值去除后得到的六个数据分别进行均值处理，求取每组的平均值，并开辟12个1*2byte的链表空间进行存储。
[0086]
本发明并不局限于前述的具体实施方式。本发明扩展到任何在本说明书中披露的新特征或任何新的组合，以及披露的任一新的方法或过程的步骤或任何新的组合。