CN109724602A - 一种基于硬件fpu的姿态解算***及其解算方法 - Google Patents

Abstract

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

Description

一种基于硬件FPU的姿态解算***及其解算方法

技术领域

本发明属于无人机领域，具体涉及一种基于硬件FPU的姿态解算***及其解算方法。

背景技术

姿态解算是四轴飞行器能够稳定飞行的基础，如何提高姿态解算的速度和姿态估计的精度一直以来都是国内外的研究热点。

四旋翼和云台都离不开三轴加速度计和三轴陀螺仪。三轴加速度计提供实时的重力加速度方向，进而可以计算出机体姿态角度。三轴陀螺仪提供实时的旋转角度，通过积分也能换算成机体姿态角度。三轴加速度计对机体振动非常敏感，稍有扰动，加速度方向就会偏移(外力与重力的合力)，但是长期运行时，经过平滑滤波算法之后的三轴加速度计数据就比较可信。相比而言，三轴陀螺仪则对外部扰动不敏感，反而是长时间积分后会出现角度漂移。因此实际应用中，都是将二者采集的数据进行融合，相互取长补短。

中国发明专利(专利号CN107543546A)公开了一种六轴运动传感器的姿态解算方法及装置，所述传感器包括三轴陀螺仪和三轴加速度计，在预设的周期内，包括：获取所述三轴陀螺仪的原始数据；获取所述三轴加速度计的原始数据；对所述三轴陀螺仪的原始数据进行积分计算得到实时角速度值；对所述的三轴加速度计原始数据进行积分得到角度值；对所述角度值和所述实时角速度值进行卡尔曼滤波计算得到姿态倾角。使用上述方法和装置进行姿态检测能够充分发挥三轴陀螺仪动态特性优异以及三轴加速度计测量准确的特点。但是卡尔曼滤波要求***为线性，而四旋翼飞行器的噪声为非线性高斯白噪声，不能满足精度要求。

中国发明专利(专利号CN108363298A)公开了一种姿态融合解算方法及***，通过获取三轴加速度计、磁力计、三轴陀螺仪的测量数据；对所述三轴加速度计、磁力计、三轴陀螺仪的测量数据进行标定；利用梯度下降算法对所述三轴加速度计、所述磁力计采集的测量数据进行处理，以获得姿态更新四元数；利用四阶龙格库塔法，求解基于三轴陀螺仪输出的姿态四元数的微分方程，以获得姿态量测数据；对所述三轴加速度计和磁力计的姿态更新四元数和三轴陀螺仪输出的姿态暈测数据进行互补滤波，以进行融合姿态解算。本发明具有较高的姿态估算精度，快速性好，对***负载较低，具较高的工程实用价值。但是此方法利用梯度下降法配合互补滤波算法运算量较大，CPU负载高，不满足实时性要求。

发明内容

本发明的目的在于提供一种基于硬件FPU姿态解算***，对IMU传感器模块测得的原始数据进行滑动平均滤波处理，并通过硬件FPU对互补滤波算法进行加速。本方法使得姿态解算的速度更快，姿态估计的精度更高，满足小型四旋翼飞行器姿态控制的要求，增强了***的鲁棒性与稳定性。

实现本发明目的的技术解决方案为：一种基于硬件FPU的姿态解算***，包括电源、IMU传感器模块、主控芯片、数据储存模块、串口模块和上位机；其中：

电源分别与IMU传感器模块、主控芯片、数据储存模块和串口模块供电连接，为IMU传感器模块、主控芯片、数据储存模块和串口模块供电；IMU传感器模块、数据储存模块和串口模块分别通过信号线与主控芯片进行连接，上位机通过数据线与串口模块进行连接，当主控芯片计算得到飞行器的姿态信息后通过SPI通信将结果保存到数据储存模块并通过串口模块发送给上位机以供观察。

进一步地，IMU传感器模块包括三轴加速度计和三轴陀螺仪，IMU传感器模块通过信号线与主控芯片连接，在飞行过程中不断通过三轴加速度计测量来获得飞行器的加速度信息，通过三轴陀螺仪测量获得飞行器的角速度信息，并通过IIC通信发送给主控芯片。

进一步地，主控芯片自带定时器和硬件FPU，并通过信号线与IMU传感器模块连接，主控芯片时时接收来自IMU传感器模块的加速度和角速度信息，经过相应算法运算处理后得到当前飞行器的姿态信息，在计算过程中利用硬件FPU对算法进行加速。

进一步地，主控芯片采用STM32F407ZET6，其属于Cortex M4F架构，自带32位单精度硬件FPU。

一种基于硬件FPU的姿态解算***的解算方法，对IMU传感器模块测得的原始数据进行滑动平均滤波处理，并通过硬件FPU对互补滤波算法进行加速，具体步骤如下：

步骤1、采集三轴加速度计的原始数据得到x、y、z轴的加速度，采集三轴陀螺仪的原始数据得到x、y、z轴的角速度；

步骤2、对三轴加速度计获取的加速度原始数据进行滑动平均滤波处理，抑制环境对原始数据的周期性干扰，得到有效加速度采样值；

对三轴陀螺仪获取的角速度数据进行去零漂处理以解决三轴陀螺仪存在零漂的情况，获得有效角速度采样值；

步骤3、采用互补滤波算法对有效加速度采样值和有效角速度采样值进行融合，其中采用四元数法进行姿态更新，利用硬件FPU对互补滤波算法进行加速；

步骤4、选用定时器TIM3将两次姿态更新之间的时间严格控制在10ms，在中断服务函数中利用LED0＝1作为一个标记，并进行实时扫描，当检测到标记时则进行一次姿态更新；

步骤5、采用一阶龙格库塔法更新四元数，一阶龙格库塔法更新四元数的公式如下：

q₀+＝(-q₁Last*gyrox-q₂Last*gyroy-q₃Last*gyroz)*halfT；

q₁+＝(q₀Last*gyrox+q₂Last*gyroz-q₃Last*gyroy)*halfT；

q₂+＝(q₀Last*gyroy-q₁Last*gyroz+q₃Last*gyrox)*halfT；

q₃+＝(q₀Last*gyroz+q₁Last*gyroy-q₂Last*gyrox)*halfT；

其中，q₀、q₁、q₂、q₃为当前四元数变量，q₀Last、q₁Last、q₂Last、q₃Last为更新前的四元数变量，gyrox、gyrox、gyrox为当前周期的角速度，halfT为周期时间；

步骤6、将得到的四元数进行归一化处理，即将得到的四元数快速开平方求倒数，使其变为单位四元数；

步骤7、四元数转欧拉角，转化公式如下：

θ＝sin^-1(2(q₂q₃+q₀q₁)

其中，参数θ代表飞行器当前俯仰角，γ代表飞行器当前横滚角，ψ代表飞行器当前偏航角，q₀、q₁、q₂、q₃为四元数变量。

本发明与现有技术相比，其显著优点在于：(1)提高了姿态解算的精度。针对单纯的互补滤波算法会产生姿态角漂移的缺点，本方案对三轴加速度计和磁力计原始数据进行了滑动滤波处理。

(2)提高了姿态解算的速度。本发明使用DSP库的硬件FPU对互补滤波融合算法进行加速，大大加快其运算速度。

附图说明

图1是本发明实施例的姿态解算***总体结构示意图。

图2是本发明实施例的IMU传感器模块和飞行器各坐标关系图。

图3是本发明实施例的姿态解算***算法流程图。

图4是本发明实施例对比传统互补滤波算法和基于硬件FPU的改进型互补滤波算法的俯仰角数据图。

图5是本发明实施例对比传统互补滤波算法和基于硬件FPU的改进型互补滤波算法的横滚角数据图。

图6是本发明实施例对比传统互补滤波算法和基于硬件FPU的改进型互补滤波算法的偏航角数据图。

具体实施方式

下面结合附图对本发明作进一步详细描述。

结合图1，本发明提供了一种基于硬件FPU姿态解算***，包括电源、IMU传感器模块、主控芯片、数据储存模块、串口模块和上位机。其中：

所述电源分别与IMU传感器模块、主控芯片、数据储存模块和串口模块供电连接，为IMU传感器模块、主控芯片、数据储存模块和串口模块供电。IMU传感器模块、数据储存模块和串口模块分别通过信号线与主控芯片进行连接，上位机通过数据线与串口模块进行连接，当主控芯片计算得到飞行器的姿态信息后通过SPI通信将结果保存到数据储存模块并通过串口模块发送给上位机以供观察。

所述IMU传感器模块包括三轴加速度计和三轴陀螺仪，IMU传感器模块通过信号线与主控芯片连接，在飞行过程中不断通过三轴加速度计测量来获得飞行器的加速度信息，通过三轴陀螺仪测量获得飞行器的角度信息，并通过IIC通信发送给主控芯片。

所述主控芯片自带定时器和硬件FPU，并通过信号线与IMU传感器模块连接，主控芯片时时接收来自IMU传感器模块的加速度和角度信息，经过相应算法运算处理后得到当前飞行器的姿态信息，在计算过程中利用硬件FPU对算法进行加速。

所述主控芯片采用STM32F407ZET6，其属于Cortex M4F架构，自带32位单精度硬件FPU。

结合图2，本发明中IMU传感器模块和飞行器的各坐标关系为：

IMU传感器模块与飞行器固连，传感器z轴方向与飞行器竖直向上轴线保持重合，x轴方向与飞行器正前方向保持重合，y轴方向则设置同时与x、z轴保持垂直。其中坐标关系为：世界坐标系为Oxyz，飞行器坐标系为Ox₁y₁z₁。定义世界坐标系Oxyz为参考坐标系，则x₁y₁与xy平面的相交为交点线N，则定于飞行器偏航角φ为x轴与交点线N夹角，横滚角γ为z₁轴与z轴的夹角，俯仰角为x₁轴与交点线N的夹角。

世界坐标系Oxyz作为参考系，飞行器坐标系Ox₁y₁z₁在Oxyz中运动，产生俯仰、偏航与滚转的变化。由姿态传感器的三轴陀螺仪数据进行对时间积分，求得上述三个姿态角，同时引入三轴加速度计/磁力计的融合校正，最终求得飞行器相对与世界坐标系的三个姿态角：俯仰角偏航角φ和滚转角γ。

结合图3，本发明基于硬件FPU姿态解算***的解算方法，步骤如下：

步骤1、采集三轴加速度计的原始数据得到x、y、z轴的加速度，采集三轴陀螺仪的原始数据得到x、y、z轴的角速度。

步骤2、对三轴加速度计获取的加速度原始数据进行滑动平均滤波处理，抑制环境对原始数据的周期性干扰；即每回只采样一次，将一次采样值和过去的若干次采样值一起求平均，得到有效加速度采样值。取三轴加速度计的200个原始数据作为采样值求平均，从存储区中留出200个数据的暂存区并且采用环型队列结构存放数据，即每采集一个新的数据的同时去掉一个最老数据，从而保存的200个数据始终是最新更新的数据。

对三轴陀螺仪获取的角速度数据进行去零漂处理以解决三轴陀螺仪存在零漂的情况；即在IMU传感器模块初始化时，取三轴陀螺仪的若干个原始数据，求平均，从而得到三轴陀螺仪零漂平均值阈值，在进行姿态更新时，每次得到三轴陀螺仪原始数据都必须减去上述平均值阈值，获得有效角速度采样值。

步骤3、采用互补滤波算法对有效加速度采样值和有效角速度采样值进行融合，其中采用四元数法进行姿态更新，利用硬件FPU对互补滤波算法进行加速。

四元数和欧拉角都是常用的姿态表示方法，由于欧拉角法在姿态解算时存在奇点(万向节死锁)，不能用于全姿态解算，故在姿态解算过程中采用四元数法，算法最后再将四元数转化为欧拉角。

四旋翼和云台都离不开三轴加速度计和三轴陀螺仪，三轴加速度计提供实时的重力加速度方向，进而可以计算出机体姿态角度。三轴陀螺仪提供实时的旋转角度，通过积分也能换算成机体姿态角度。三轴加速度计对机体振动非常敏感，稍有扰动，加速度方向就会偏移(外力与重力的合力)，但是长期运行时，经过平滑滤波算法之后的三轴加速度计数据就比较可信。相比而言，三轴陀螺仪则对外部扰动不敏感，反而是长时间积分后会出现角度漂移。因此实际应用中，都是将二者采集的数据进行融合，相互取长补短。在短时间内以三轴陀螺仪为准，长时间以加速度为准。由于三轴加速度计、三轴陀螺仪自身的特性，使用低通滤波器将三轴加速度计的高频噪声滤除，用高通滤波器将三轴陀螺仪的低频噪声滤除，然后将两者滤波后的信号相加得到整个频带的信号，此信号在高频和低频都较好，兼具三轴加速度计和三轴陀螺仪的优点。

设三轴加速度计经姿态解算后得到的姿态角为φ_a，低通滤波器的函数f₁(t)＝d_t/(t₀+d_t)，三轴陀螺仪经过积分后得到的姿态角为φ_b，高通滤波函数f₂(t)＝t₀/(t₀+d_t)。互补滤波后得到的姿态角为φ，则根据互补滤波的原理可得：

其中，参数t₀为时间常数，1/t₀为滤波器的截止频率，d_t为滤波器采样时间。对于低通滤波器，频率高于1/t₀将被滤除，低于1/t₀的能顺利通过，高通滤波器则相反，具体融合步骤如下：

1)重力加速度归一化，单位化三轴加速度计测量值，快速开平方求倒数；三轴加速度计数据归一化，把三轴加速度计的三维向量转换为单位向量，因为是单位矢量到参考性的投影，所以要把三轴加速度计数据单位化，归一化只改变向量的长度，不改变向量的方向，同时与单位四元数对应。

2)提取四元数的等效余弦矩阵中的重力分量，即机体坐标系下Z方向的分量。将当前姿态的重力在三个轴上的分量分离出来，把四元数换算成方向余弦中的第三行的三个元素，根据余弦矩阵和欧拉角的定义，就是参考坐标系的Z轴的重力向量。

3)向量叉积得出姿态误差，即通过三轴加速度计测得的重力坐标系下的单位向量与上一次四元数转换成的单位向量进行叉乘得到的就是姿态误差；

4)对误差进行积分。将当前姿态分离出的重力分量与当前三轴加速度计测得的重力分量的差值进行积分消除误差。

5)互补滤波，姿态误差补偿到角速度上，修正角速度积分漂移。系数不停地被陀螺积分更新，也不停地被误差修正，它和公式所代表的姿态也在不断更新。将积分误差反馈到三轴陀螺仪上，修正三轴陀螺仪的值。将该误差与姿态更新周期中三轴陀螺仪测得的角速度相加，最终得到一个修正的角速度值，将其输入四元数微分方程，更新四元数。

本发明利用硬件FPU对互补滤波算法进行加速，浮点运算对于定点CPU(没有FPU的CPU)来说必须要按照IEEE-754标准的算法来完成运算，是相当耗费时间的。而对于有FPU的CPU来说，浮点运算则只是几条指令的事情，速度相当快。本发明所使用的STM32F407ZET6属于Cortex M4F架构，带有32位单精度硬件FPU，支持浮点指令集，相对于Cortex M0和CortexM3等，高出数十倍甚至上百倍的运算性能。故本发明使用DSP库的硬件FPU对互补滤波融合算法进行加速，大大加快其运算速度。

步骤4、选用定时器TIM3将两次姿态更新之间的时间严格控制在10ms，在中断服务函数中利用LED0＝1作为一个标记，并进行实时扫描，当检测到标记时则进行一次姿态更新；

步骤5、采用一阶龙格库塔法更新四元数，一阶龙格库塔法是一种在工程上应用广泛的高精度单步算法，可用于数值求解微分方程，本发明将利用一阶龙格库塔法对四元数进行实时更新，一阶龙格库塔法更新四元数的公式如下：

q₀+＝(-q₁Last*gyrox-q₂Last*gyroy-q₃Last*gyroz)*halfT；

q₁+＝(q₀Last*gyrox+q₂Last*gyroz-q₃Last*gyroy)*halfT；

q₂+＝(q₀Last*gyroy-q₁Last*gyroz+q₃Last*gyrox)*halfT；

q₃+＝(q₀Last*gyroz+q₁Last*gyroy-q₂Last*gyrox)*halfT；

其中，q₀、q₁、q₂、q₃为当前四元数变量，q₀Last、q₁Last、q₂Last、q₃Last为更新前的四元数变量，gyrox、gyrox、gyrox为当前周期的角速度，halfT为周期时间。

步骤6、将得到的四元数进行归一化处理，即将得到的四元数快速开平方求倒数，使其变为单位四元数，归一化只改变向量的长度，不改变向量的方向。

步骤7、四元数转欧拉角。转化公式如下：

θ＝sin^-1(2(q₂q₃+q₀q₁)

其中，参数θ代表飞行器当前俯仰角，γ代表飞行器当前横滚角，ψ代表飞行器当前偏航角，q₀、q₁、q₂、q₃为四元数变量。

结合图4-至图6，利用本发明的方法可以提高姿态解算的速度与姿态估计的精度。实验结果表明，此方法使得姿态解算的速度提高至800Hz，俯仰角的精度可达到±0.17°，横滚角的精度可达±0.26°，偏航角的精度可达±0.03°，满足了小型四旋翼飞行器姿态控制的要求。

Claims (8)

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，其属于Cortex M4F架构，自带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、采用一阶龙格库塔法更新四元数，一阶龙格库塔法更新四元数的公式如下：

q₀+＝(-q₁Last*gyrox-q₂Last*gyroy-q₃Last*gyroz)*halfT；

q₁+＝(q₀Last*gyrox+q₂Last*gyroz-q₃Last*gyroy)*halfT；

q₂+＝(q₀Last*gyroy-q₁Last*gyroz+q₃Last*gyrox)*halfT；

q₃+＝(q₀Last*gyroz+q₁Last*gyroy-q₂Last*gyrox)*halfT；

其中，q₀、q₁、q₂、q₃为当前四元数变量，q₀Last、q₁Last、q₂Last、q₃Last为更新前的四元数变量，gyrox、gyrox、gyrox为当前周期的角速度，halfT为周期时间；

步骤6、将得到的四元数进行归一化处理，即将得到的四元数快速开平方求倒数，使其变为单位四元数；

步骤7、四元数转欧拉角，转化公式如下：

θ＝sin^-1(2(q₂q₃+q₀q₁)

其中，参数θ代表飞行器当前俯仰角，γ代表飞行器当前横滚角，ψ代表飞行器当前偏航角，q₀、q₁、q₂、q₃为四元数变量。

7.根据权利要求6所述的基于硬件FPU的姿态解算***的解算方法，其特征在于，步骤2中，对三轴加速度计获取的加速度原始数据进行滑动平均滤波处理，抑制环境对原始数据的周期性干扰，得到有效加速度采样值，具体如下：每回只采样一次，将一次采样值和过去的若干次采样值一起求平均，得到有效加速度采样值；

对三轴陀螺仪获取的角速度数据进行去零漂处理以解决三轴陀螺仪存在零漂的情况，获得有效角速度采样值，具体如下：

在IMU传感器模块初始化时，取三轴陀螺仪的若干个原始数据，求平均，从而得到三轴陀螺仪零漂平均值阈值，在进行姿态更新时，每次得到三轴陀螺仪原始数据都必须减去上述平均值阈值，获得有效角速度采样值。

8.根据权利要求6所述的基于硬件FPU的姿态解算***的解算方法，其特征在于，步骤3中，具体方法如下：

设三轴加速度计经姿态解算后得到的姿态角为φ_a，低通滤波器的函数f₁(t)＝d_t/(t₀+d_t)，三轴陀螺仪经过积分后得到的姿态角为φ_b，高通滤波函数f₂(t)＝t₀/(t₀+d_t)。互补滤波后得到的姿态角为φ，则根据互补滤波的原理得：

其中，参数t₀为时间常数，1/t₀为滤波器的截止频率，d_t为滤波器采样时间。对于低通滤波器，频率高于1/t₀将被滤除，低于1/t₀的能顺利通过，高通滤波器则相反，具体融合步骤如下：

1)重力加速度归一化，单位化三轴加速度计测量值，快速开平方求倒数；三轴加速度计数据归一化，即把三轴加速度计的三维向量转换为单位向量；

2)提取四元数的等效余弦矩阵中的重力分量，即机体坐标系下Z方向的分量，将当前姿态的重力在三个轴上的分量分离出来，把四元数换算成方向余弦中的第三行的三个元素，根据余弦矩阵和欧拉角的定义，就是参考坐标系的Z轴的重力向量；

3)向量叉积得出姿态误差，即通过三轴加速度计测得的重力坐标系下的单位向量与上一次四元数转换成的单位向量进行叉乘得到的就是姿态误差；

4)对误差进行积分，将当前姿态分离出的重力分量与当前三轴加速度计测得的重力分量的差值进行积分消除误差；

5)互补滤波，姿态误差补偿到角速度上，修正角速度积分漂移；将积分误差反馈到三轴陀螺仪上，修正三轴陀螺仪的值；将该误差与姿态更新周期中三轴陀螺仪测得的角速度相加，最终得到一个修正的角速度值，将其输入四元数微分方程，更新四元数。