CN104061931A - 一种基于fpga的小型便携式多传感器姿态检测系统 - Google Patents

Abstract

一种基于FPGA的小型便携式多传感器姿态检测系统。其包括FPGA芯片、两片SDRAM芯片、FLASH芯片、陀螺/加速度传感器、磁传感器、电平转换芯片、显示模块、通信接口、外部存储器和电源模块。本发明效果：使用一片FPGA芯片完成传感器数据采集、数据通讯及算法运算，使用一片FLASH芯片存储FPGA的配置数据和应用程序，具有较高系统集成度，提高了系统可靠性。在构建片上系统时，采用多种类型的总线连接处理器核和片上外围接口，并采用FPGA内部RAM存储应用程序的“数据段”和“堆栈段”，提高了处理器核执行效率。通过三维姿态角试验可看出，该系统从传感器数据采集到姿态解算的时间仅为360us。

Description

一种基于FPGA的小型便携式多传感器姿态检测系统

技术领域

本发明属于姿态检测技术领域，特别是涉及一种基于FPGA的小型便携式多传感器姿态检测系统。

背景技术

惯性导航系统具有自主性、高隐蔽性、抗干扰等特点，是航空航天和军事领域重要的导航系统，其特点也是其它导航系统无法比拟的。随着微机电系统(MEMS)的快速发展，出现了新一代微型陀螺仪和加速度计，降低了惯性传感器的制造成本，惯性导航系统发展前景一片大好。为了使惯性导航系统在民用领域迅速推广，有必要设计一种低功耗、便携式的小型硬件平台，实现惯导系统的功能。

发明内容

为了解决上述问题，本发明的目的在于提供一种基于FPGA的小型便携式多传感器姿态检测系统。

为了达到上述目的，本发明提供的基于FPGA的小型便携式多传感器姿态检测系统包括：FPGA芯片、两片SDRAM芯片、FLASH芯片、陀螺/加速度传感器、磁传感器、电平转换芯片、显示模块、通信接口、外部存储器和电源模块；其中：FPGA芯片为本系统的控制运算中心，其分别与两片SDRAM芯片、FLASH芯片、陀螺/加速度传感器、电平转换芯片、显示模块、通信接口、外部存储器相连接，电平转换芯片与磁传感器相连接，通信接口与上位机相连接，电源模块为供电电源，其与本系统中的各用电部件相连接。

所述的电源模块由锂电池和电平转换芯片组成；所述的通信接口为与外部设备进行数据交换的通信接口，采用MAX3222通信接口 芯片；所述的显示模块为液晶显示器，采用液晶屏LCD2004；外部存储器由SD卡组成。

所述的陀螺/加速度传感器为MEMS传感器，采用ADIS16385，磁传感器采用HMR3300，所述的电平转换芯片采用74LVC4245；所述的SDRAM芯片采用CY7C1021、FLASH芯片采用W25Q80；所述的FPGA芯片为现场可编程门阵列器件，采用Xilinx公司的XC6SLX9芯片。

所述的FPGA芯片的内部片上系统包括：处理器核、SPI_A接口、SPI_F接口、SPI_S接口、UART_H接口、UART_U接口、AXI_P总线、AXI_E总线、存储器控制器、LMB总线、BRAM_0控制器、BRAM_1控制器、BRAM_0存储器、BRAM_1存储器、FSL总线、显示控制器；

其中：处理器核通过AXI_P总线与SPI_A接口、UART_H接口、SPI_F接口、UART_U接口和SPI_S接口相连接，SPI_A接口与陀螺/加速度传感器相连接；UART_H接口通过电平转换芯片与磁传感器相连接；SPI_F接口与FLASH芯片相连接；UART_U接口通过通信接口与上位机相连接；SPI_S接口与外部存储器相连接。

所述的处理器核还通过AXI_E总线与存储器控制器相连接，存储器控制器通过32位总线与两片SDRAM芯片连接；

所述的处理器核还通过LMB总线分别与BRAM_0控制器和BRAM_1控制器相连接，BRAM_0控制器与BRAM_0存储器连接，BRAM_1控制器与BRAM_1存储器相连接；

处理器核还通过FSL总线与显示控制器相连接，显示控制器与显示模块连接。

所述的SPI_A接口是标准的SPI接口，所述的UART_H接口是标准的RS232接口，所述的SPI_F接口是标准的SPI接口，所述的UART_U接口是标准RS232接口，所述的SPI_S接口是标准的SPI接 口。

所述的BRAM_0存储器是FPGA芯片的内部RAM，所述的BRAM_1存储器是FPGA芯片的内部RAM。

本发明以导航系统中不可或缺的姿态检测系统为问题切入点，构建了以MEMS惯性器件、电子罗盘为传感器，FPGA为运算核心的导航计算平台，并研究了一种姿态检测系统的多传感器数据融合方法。

本发明提供的基于FPGA的小型便携式多传感器姿态检测系统的效果：该姿态检测系统使用一片FPGA芯片完成传感器数据采集、数据通讯及算法运算，并且使用一片FLASH芯片存储FPGA的配置数据和应用程序，具有较高的系统集成度，提高了系统可靠性。在构建片上系统时，考虑到处理器核的流水线工作方式，采用多种类型的总线连接处理器核和片上外围接口，并采用FPGA内部RAM存储应用程序的“数据段”和“堆栈段”，提高了处理器核的执行效率。通过三维姿态角试验可以看出，该姿态检测系统从传感器数据采集到姿态解算的时间仅为360us。

附图说明

图1为本发明提供的基于FPGA的小型便携式多传感器姿态检测系统的组成示意图。

图2为本发明提供的基于FPGA的小型便携式多传感器姿态检测系统中FPGA芯片的内部结构示意图。

具体实施方式

下面结合附图和具体实施例对本发明提供的基于FPGA的小型便携式多传感器姿态检测系统进行详细说明。

如图1所示，本发明提供的基于FPGA的小型便携式多传感器姿 态检测系统包括：FPGA芯片1、两片SDRAM芯片2、FLASH芯片3、陀螺/加速度传感器4、磁传感器5、电平转换芯片6、显示模块7、通信接口8、外部存储器9和电源模块10；其中：FPGA芯片1为本系统的控制运算中心，其分别与两片SDRAM芯片2、FLASH芯片3、陀螺/加速度传感器4、电平转换芯片6、显示模块7、通信接口8、外部存储器9相连接，电平转换芯片6与磁传感器5相连接，通信接口8与上位机相连接，电源模块10为供电电源，其与本系统中的各用电部件相连接。

所述的陀螺/加速度传感器4、磁传感器5和电平转换芯片6组成传感器模块，陀螺/加速度传感器4负责采集载体的转动角速率，磁传感器5负责采集地磁场强度，电平转换芯片6负责将磁传感器5的5v端口电平转换成3.3v，便于和FPGA芯片1进行数据通讯；

所述的FPGA芯片1、SDRAM芯片2、FLASH芯片3组成运算模块，FPGA芯片1负责数据采集、算法运算及数据通讯，SDRAM芯片2负责存储FPGA芯片1执行过程中的程序代码，它具有读写速度快、掉电数据易失的特点，系统中使用了两片SDRAM芯片2将数据总线扩展到32位，可以有效提高数据读写效率，FLASH芯片3负责存储FPGA芯片1的配置信息和FPGA芯片1执行的程序代码，它具有掉电数据不丢失的特点，当系统上电后，FPGA芯片1的配置信息和SDRAM芯片2中的程序代码都需要从该芯片中读取；

电源模块10由锂电池和电平转换芯片组成，锂电池提供的电压是7.4v，电平转换芯片负责将锂电池电压转换成5V、3.3V和1.2V，以供系统其他模块正常工作；

通信接口8为与外部设备进行数据交换的通信接口，采用MAX3222通信接口芯片，用于和上位机通信；

显示模块7为液晶显示器，采用液晶屏LCD2004，用于显示运算模块解算得到的姿态参数；

外部存储器9由SD卡组成，用于存储经过运算模块解算得到的姿态参数。

所述的陀螺/加速度传感器4为MEMS传感器，采用ADIS16385，磁传感器5采用HMR3300，所述的电平转换芯片6采用74LVC4245。

所述的SDRAM芯片2采用CY7C1021、FLASH芯片3采用W25Q80。

所述的FPGA芯片1为现场可编程门阵列器件，采用Xilinx公司的XC6SLX9芯片；为了使FPGA器件具有算法运算功能，需要构建片上系统，如图2所示；所述的FPGA芯片1的内部片上系统包括：处理器核101、SPI_A接口102、SPI_F接口104、SPI_S接口116、UART_H接口103、UART_U接口105、AXI_P总线106、AXI_E总线107、存储器控制器108、LMB总线109、BRAM_0控制器110、BRAM_1控制器112、BRAM_0存储器111、BRAM_1存储器113、FSL总线114、显示控制器115；

其中：处理器核101通过AXI_P总线106与SPI_A接口102、UART_H接口103、SPI_F接口104、UART_U接口105和SPI_S接口116相连接，SPI_A接口102与陀螺/加速度传感器4相连接；UART_H接口103通过电平转换芯片6与磁传感器5相连接；SPI_F接口104与FLASH芯片3相连接；UART_U接口105通过通信接口8与上位机相连接；SPI_S接口116与外部存储器9相连接；

处理器核101还通过AXI_E总线107与存储器控制器108相连接，存储器控制器108通过32位总线与两片SDRAM芯片2连接；

处理器核101还通过LMB总线109分别与BRAM_0控制器110和BRAM_1控制器112相连接，BRAM_0控制器110与BRAM_0存储器 111连接，BRAM_1控制器112与BRAM_1存储器113相连接；

处理器核101还通过FSL总线114与显示控制器115相连接，显示控制器115与显示模块7连接。

所述的处理器核101采用Xilinx公司的软处理器核MicroBlaze，负责程序指令的取指、解码及执行。

所述的SPI_A接口102是标准的SPI接口，用于处理器核101控制陀螺/加速度传感器4的工作模式，并采集它的数据。

所述的UART_H接口103是标准的RS232接口，用于处理器核101采集磁传感器5的数据。

所述的SPI_F接口104是标准的SPI接口，用于处理器核101读写FLASH芯片3中存储的应用程序。

所述的UART_U接口105是标准RS232接口，用于处理器核101将姿态参数上传给上位机。

所述的AXI_P总线106是一种高性能片上系统总线，用于处理器核101访问片上外设。

所述的AXI_E总线107和AXI_P总线106是同类型总线，是处理器核101访问存储器控制器108的专用通道。

所述的存储器控制器108等效于SDRAM芯片2的硬件驱动器。

所述的LMB总线109是一种本地存储器访问总线，可实现对BRAM_0存储器111和BRAM_1存储器113的高速访问。

所述的BRAM_0控制器110用于控制BRAM_0存储器111。

所述的BRAM_1控制器112用于控制BRAM_1存储器113。

所述的BRAM_0存储器111是FPGA芯片1的内部RAM，具有较高的访问速率，用于存储引导加载程序。

所述的BRAM_1存储器113是FPGA芯片1的内部RAM，具有较 高的访问速率，用于存储应用程序的“数据段”和“堆栈段”。

所述的FSL总线114是一种单向通道总线接口，用于处理器核101对显示控制器115的快速访问。

所述的显示控制器115是由VHDL语言编写的用户自定义IP核，用于将处理器核101对显示模块7的操作命令进行解码，从而得到显示模块7的操作时序。

所述的SPI_S接口116是标准的SPI接口，用于处理器核101对外部存储器9的数据读写。

针对上述系统的软件开发主要包含引导加载程序和应用程序，软件实现过程如下：系统上电后，FPGA芯片1读取FLASH芯片3中的配置数据，实现片上系统的构建，被同时读取到FPGA芯片1内的还有引导加载程序，片上系统构建完成后，引导加载程序开始运行，首先通过SPI_F接口104读取FLASH芯片3内的应用程序代码，将应用程序的“数据段”和“堆栈段”复制到BRAM_1存储器113中，将应用程序的“代码段”复制到SDRAM芯片2中，同时通过识别存储在FLASH芯片3中同步字标签获取应用程序的中断子程序存储在SDRAM芯片2中的地址量，并修改中断跳转指令，最后修改程序计数器将命令指针指向SDRAM芯片2的起始地址，执行应用程序，通过姿态解算算法得到姿态参数。

针对姿态解算算法使用互补滤波器实现，实现方法如下：

将采样得到的角速率利用欧拉角法算出姿态角：

$\left[\begin{array}{c}{\stackrel{.}{\mathrm{\θ}}}_1\\ {\stackrel{.}{\mathrm{\γ}}}_1\\ {\stackrel{.}{\mathrm{\ψ}}}_1\end{array}\right]=\left[\begin{array}{ccc}1& \sin \mathrm{\θ}\tan \mathrm{\γ}& \cos \mathrm{\θ}\tan \mathrm{\γ}\\ 0& \cos \mathrm{\θ}& -\sin \mathrm{\θ}\\ 0& \sin \mathrm{\θ}\sec \mathrm{\γ}& \cos \mathrm{\θ}\sec \mathrm{\γ}\end{array}\right]w^b$

根据重力加速度对加速度计的影响，将采样得到加速度量通过 如下方法得到姿态角中的俯仰角和横滚角：

${\mathrm{\θ}}_2=\arcsin (-\frac{f_y^b}{g}),$ ${\mathrm{\γ}}_2=\arcsin \left(\frac{f_s^b}{g\cos \mathrm{\θ}}\right)$

根据步骤2得到的俯仰角和横滚角，并结合磁传感器采样得到的磁场强度算出航向角：

$\left[\begin{array}{ccc}{\cos \mathrm{\ψ}}_2& {\sin \mathrm{\ψ}}_2& 0\\ {-\sin \mathrm{\ψ}}_2& {\cos \mathrm{\ψ}}_2& 0\\ 0& 0& 1\end{array}\right]m^n=\left[\begin{array}{ccc}{\cos \mathrm{\γ}}_2& 0& {\sin \mathrm{\γ}}_2\\ {\sin \mathrm{\θ}}_2{\sin \mathrm{\γ}}_2& {\cos \mathrm{\θ}}_2& {-\sin \mathrm{\θ}}_2{\cos \mathrm{\γ}}_2\\ {-\cos \mathrm{\θ}}_2{\sin \mathrm{\γ}}_2& {\sin \mathrm{\θ}}_2& {\cos \mathrm{\θ}}_2{\cos \mathrm{\γ}}_2\end{array}\right]m^b$

设计互补滤波器将步骤1得到姿态角和步骤2、3得到的姿态角数据融合得到姿态角的优化值，利用拉普拉斯变换将互补滤波模型方程表示为：

$s\widehat{\mathrm{\θ}}\left(s\right)={\mathrm{s\θ}}_1\left(s\right)-(K_{P1}+\frac{K_{I1}}{s})\·(\widehat{\mathrm{\θ}}\left(s\right)-{\mathrm{\θ}}_2\left(s\right))$

$s\widehat{\mathrm{\γ}}\left(s\right)={\mathrm{s\γ}}_1\left(s\right)-(K_{P2}+\frac{K_{I2}}{s})\·(\widehat{\mathrm{\γ}}\left(s\right)-{\mathrm{\γ}}_2\left(s\right))$

$s\widehat{\mathrm{\ψ}}\left(s\right)={\mathrm{s\ψ}}_1\left(s\right)-(K_{P3}+\frac{K_{I3}}{s})\·(\widehat{\mathrm{\ψ}}\left(s\right)-{\mathrm{\ψ}}_2\left(s\right))$

其中，θ表示载体的俯仰角，γ表示载体的横滚角，ψ表示载体的航向角，fb x、fb y、fb z分别表示三轴加速度计测量值，g表示重力加速度量，mn表示载体所处位置的地磁场在导航坐标系下的矢量，mb表示磁传感器测得的三轴磁场强度，KP1、KI1、KP2、KI2、KP3、KI3表示互补滤波器的参数，为了避免载体运动加速度给步骤2的计算结果带来较大误差，使用了阈值判定准则，当检测到载体存在运动加速度时，互补滤波器参数KP1、KI1、KP2、KI2、KP3、KI3置零，否则恢复设定值。

本发明提供的基于FPGA的小型便携式多传感器姿态检测系统是以MEMS惯性元件、电子罗盘为传感器构建的。在FPGA芯片1上 设计具有快速、高效率处理能力的片上系统，使FPGA芯片1能够实现导航算法运算，并增加SPI接口、RS232接口和显示控制器，用于传感器的数据采集、姿态角的上传和显示。该检测系统使用锂电池进行供电，系统通电后，FPGA芯片1读取FLASH芯片3中的配置数据，实现片上系统的构建，被同时读取到FPGA芯片1内的还有引导加载程序，片上系统构建完成后，引导加载程序开始运行，首先通过SPI_F接口104读取FLASH芯片3内的应用程序代码，将应用程序的“数据段”和“堆栈段”复制到BRAM_1存储器113中，将应用程序的“代码段”复制到SDRAM芯片2中，同时通过识别存储在FLASH芯片3中同步字标签获取应用程序的中断子程序存储在SDRAM芯片2中的地址量，并修改中断跳转指令，最后修改程序计数器将命令指针指向SDRAM芯片2的起始地址，执行应用程序，通过姿态解算算法得到姿态参数。并将姿态角发送给显示控制器115和UART接口105，用于显示和上传。

Claims (7)

1.一种基于FPGA的小型便携式多传感器姿态检测系统，其特征在于：其包括：FPGA芯片(1)、两片SDRAM芯片(2)、FLASH芯片(3)、陀螺/加速度传感器(4)、磁传感器(5)、电平转换芯片(6)、显示模块(7)、通信接口(8)、外部存储器(9)和电源模块(10)；其中：FPGA芯片(1)为本系统的控制运算中心，其分别与两片SDRAM芯片(2)、FLASH芯片(3)、陀螺/加速度传感器(4)、电平转换芯片(6)、显示模块(7)、通信接口(8)、外部存储器(9)相连接，电平转换芯片(6)与磁传感器(5)相连接，通信接口(8)与上位机相连接，电源模块(10)为供电电源，其与本系统中的各用电部件相连接。

2.根据权利要求1所述的基于FPGA的小型便携式多传感器姿态检测系统，其特征在于：所述的电源模块(10)由锂电池和电平转换芯片组成；所述的通信接口(8)为与外部设备进行数据交换的通信接口，采用MAX3222通信接口芯片；所述的显示模块(7)为液晶显示器，采用液晶屏LCD2004；外部存储器(9)由SD卡组成。

3.根据权利要求1所述的基于FPGA的小型便携式多传感器姿态检测系统，其特征在于：所述的陀螺/加速度传感器(4)为MEMS传感器，采用ADIS16385，磁传感器(5)采用HMR3300，所述的电平转换芯片(6)采用74LVC4245；所述的SDRAM芯片(2)采用CY7C1021、FLASH芯片(3)采用W25Q80；所述的FPGA芯片(1)为现场可编程门阵列器件，采用Xilinx公司的XC6SLX9芯片。

4.根据权利要求1所述的基于FPGA的小型便携式多传感器姿态检测系统，其特征在于：所述的FPGA芯片(1)的内部片上系统包括：处理器核(101)、SPI_A接口(102)、SPI_F接口(104)、SPI_S接口(116)、UART_H接口(103)、UART_U接口(105)、AXI_P总线(106)、AXI_E总线(107)、存储器控制器(108)、LMB总线(109)、BRAM_0控制器(110)、BRAM_1控制器(112)、BRAM_0存储器(111)、BRAM_1存储器(113)、FSL总线(114)、显示控制器(115)；

其中：处理器核(101)通过AXI_P总线(106)与SPI_A接口(102)、UART_H接口(103)、SPI_F接口(104)、UART_U接口(105)和SPI_S接口(116)相连接，SPI_A接口(102)与陀螺/加速度传感器(4)相连接；UART_H接口(103)通过电平转换芯片(6)与磁传感器(5)相连接；SPI_F接口(104)与FLASH芯片(3)相连接；UART_U接口(105)通过通信接口(8)与上位机相连接；SPI_S接口(116)与外部存储器(9)相连接。

5.根据权利要求4所述的基于FPGA的小型便携式多传感器姿态检测系统，其特征在于：所述的处理器核(101)还通过AXI_E总线(107)与存储器控制器(108)相连接，存储器控制器(108)通过32位总线与两片SDRAM芯片(2)连接；

所述的处理器核(101)还通过LMB总线(109)分别与BRAM_0控制器(110)和BRAM_1控制器(112)相连接，BRAM_0控制器(110)与BRAM_0存储器(111)连接，BRAM_1控制器(112)与BRAM_1存储器(113)相连接；

处理器核(101)还通过FSL总线(114)与显示控制器(115)相连接，显示控制器(115)与显示模块(7)连接。

6.根据权利要求4所述的基于FPGA的小型便携式多传感器姿态检测系统，其特征在于：所述的SPI_A接口(102)是标准的SPI接口，所述的UART_H接口(103)是标准的RS232接口，所述的SPI_F接口(104)是标准的SPI接口，所述的UART_U接口(105)是标准RS232接口，所述的SPI_S接口(116)是标准的SPI接口。

7.根据权利要求4所述的基于FPGA的小型便携式多传感器姿态检测系统，其特征在于：所述的BRAM_0存储器(111)是FPGA芯片(1)的内部RAM，所述的BRAM_1存储器(113)是FPGA芯片(1)的内部RAM。