一种基于硬件FPU的姿态解算系统及其解算方法技术方案

本发明专利技术公开了一种基于硬件FPU的姿态解算系统，包括电源、IMU传感器模块、主控芯片、数据储存模块、串口模块和上位机。其中所述IMU传感器模块包括三轴加速度计和三轴陀螺仪。主控芯片包括定时器和硬件FPU。电源负责给IMU传感器模块、主控芯片、数据储存模块和串口模块供电，IMU传感器模块、数据储存模块和串口模块分别通过信号线与主控进行连接，上位机通过数据线与串口模块进行连接。本发明专利技术对IMU传感器模块测得的原始数据进行滑动平均滤波处理，并通过硬件FPU对互补滤波算法进行加速，使得姿态解算的速度更快，姿态估计的精度更高。

An Attitude Solution System Based on Hardware FPU and Its Solution Method

The invention discloses an attitude calculation system based on hardware FPU, which includes power supply, IMU sensor module, main control chip, data storage module, serial port module and upper computer. The IMU sensor module includes a three-axis accelerometer and a three-axis gyroscope. The main control chip includes timer and hardware FPU. The power supply is responsible for the power supply of IMU sensor module, main control chip, data storage module and serial module. The IMU sensor module, data storage module and serial module are connected with the main control through signal line, and the upper computer is connected with the serial module through data line. The method processes the original data measured by the IMU sensor module by sliding average filtering, and accelerates the complementary filtering algorithm through the hardware FPU, so that the speed of attitude calculation is faster and the accuracy of attitude estimation is higher.

【技术实现步骤摘要】
一种基于硬件FPU的姿态解算系统及其解算方法
本专利技术属于无人机领域，具体涉及一种基于硬件FPU的姿态解算系统及其解算方法。
技术介绍
姿态解算是四轴飞行器能够稳定飞行的基础，如何提高姿态解算的速度和姿态估计的精度一直以来都是国内外的研究热点。四旋翼和云台都离不开三轴加速度计和三轴陀螺仪。三轴加速度计提供实时的重力加速度方向，进而可以计算出机体姿态角度。三轴陀螺仪提供实时的旋转角度，通过积分也能换算成机体姿态角度。三轴加速度计对机体振动非常敏感，稍有扰动，加速度方向就会偏移(外力与重力的合力)，但是长期运行时，经过平滑滤波算法之后的三轴加速度计数据就比较可信。相比而言，三轴陀螺仪则对外部扰动不敏感，反而是长时间积分后会出现角度漂移。因此实际应用中，都是将二者采集的数据进行融合，相互取长补短。中国专利技术专利(专利号CN107543546A)公开了一种六轴运动传感器的姿态解算方法及装置，所述传感器包括三轴陀螺仪和三轴加速度计，在预设的周期内，包括：获取所述三轴陀螺仪的原始数据；获取所述三轴加速度计的原始数据；对所述三轴陀螺仪的原始数据进行积分计算得到实时角速度值；对所述的三轴加速度计本文档来自技高网...

【技术保护点】
1.一种基于硬件FPU的姿态解算系统，其特征在于：包括电源、IMU传感器模块、主控芯片、数据储存模块、串口模块和上位机；其中：电源分别与IMU传感器模块、主控芯片、数据储存模块和串口模块供电连接，为IMU传感器模块、主控芯片、数据储存模块和串口模块供电；IMU传感器模块、数据储存模块和串口模块分别通过信号线与主控芯片进行连接，上位机通过数据线与串口模块进行连接，当主控芯片计算得到飞行器的姿态信息后通过SPI通信将结果保存到数据储存模块并通过串口模块发送给上位机以供观察。

【技术特征摘要】
1.一种基于硬件FPU的姿态解算系统，其特征在于：包括电源、IMU传感器模块、主控芯片、数据储存模块、串口模块和上位机；其中：电源分别与IMU传感器模块、主控芯片、数据储存模块和串口模块供电连接，为IMU传感器模块、主控芯片、数据储存模块和串口模块供电；IMU传感器模块、数据储存模块和串口模块分别通过信号线与主控芯片进行连接，上位机通过数据线与串口模块进行连接，当主控芯片计算得到飞行器的姿态信息后通过SPI通信将结果保存到数据储存模块并通过串口模块发送给上位机以供观察。2.根据权利要求1所述的基于硬件FPU的姿态解算系统，其特征在于：IMU传感器模块包括三轴加速度计和三轴陀螺仪，IMU传感器模块通过信号线与主控芯片连接，在飞行过程中不断通过三轴加速度计测量来获得飞行器的加速度信息，通过三轴陀螺仪测量获得飞行器的角速度信息，并通过IIC通信发送给主控芯片。3.根据权利要求1-2任意一项所述的基于硬件FPU的姿态解算系统，其特征在于：主控芯片自带定时器和硬件FPU，并通过信号线与IMU传感器模块连接，主控芯片时时接收来自IMU传感器模块的加速度和角速度信息，经过相应算法运算处理后得到当前飞行器的姿态信息，在计算过程中利用硬件FPU对算法进行加速。4.根据权利要求1所述的基于硬件FPU的姿态解算系统，其特征在于：主控芯片采用STM32F407ZET6，其属于CortexM4F架构，自带32位单精度硬件FPU。5.一种基于上述权利要求1-4中任意一项所述的基于硬件FPU的姿态解算系统的解算方法，其特征在于：对IMU传感器模块测得的原始数据进行滑动平均滤波处理，并通过硬件FPU对互补滤波算法进行加速。6.根据权利要求5所述的基于硬件FPU的姿态解算系统的解算方法，其特征在于，具体步骤如下：步骤1、采集三轴加速度计的原始数据得到x、y、z轴的加速度，采集三轴陀螺仪的原始数据得到x、y、z轴的角速度；步骤2、对三轴加速度计获取的加速度原始数据进行滑动平均滤波处理，抑制环境对原始数据的周期性干扰，得到有效加速度采样值；对三轴陀螺仪获取的角速度数据进行去零漂处理以解决三轴陀螺仪存在零漂的情况，获得有效角速度采样值；步骤3、采用互补滤波算法对有效加速度采样值和有效角速度采样值进行融合，其中采用四元数法进行姿态更新，利用硬件FPU对互补滤波算法进行加速；步骤4、选用定时器TIM3将两次姿态更新之间的时间严格控制在10ms，在中断服务函数中利用LED0＝1作为一个标记，并进行实时扫描，当检测到标记时则进行一次姿态更新；步骤5、采用一阶龙格库塔法更新四元数，一阶龙格库塔法更新四元数的公式如下：q0+＝(-q1Last*gyrox-q2Last*gyroy-q3Last*gyroz)*halfT；q1+＝(q0Last*gyrox+q2Last*gyroz-q3Last*gyroy)*halfT；q2+＝(q0Last*gyroy-q1Last*gyroz+q3Last*gyrox)*...

【专利技术属性】
技术研发人员：李伟伟，于纪言，
申请(专利权)人：南京理工大学，
类型：发明
国别省市：江苏,32