[发明专利]一种基于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)为供电电源，其与本系统中的各用电部件相连接；所述的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)相连接；

其特征在于：所述的BRAM_0存储器(111)是FPGA芯片(1)的内部RAM，所述的BRAM_1存储器(113)是FPGA芯片(1)的内部RAM；

针对上述系统的软件开发主要包含引导加载程序和应用程序，软件实现过程如下：系统上电后，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_x^b}{gcos\mathrm{\θ}}\right)$

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

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

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

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

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

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

其中，θ表示载体的俯仰角，γ表示载体的横滚角，ψ表示载体的航向角，分别表示三轴加速度计测量值，g表示重力加速度量，mⁿ表示载体所处位置的地磁场在导航坐标系下的矢量，m^b表示磁传感器测得的三轴磁场强度，K_P1、K_I1、K_P2、K_I2、K_P3、K_I3表示互补滤波器的参数，为了避免载体运动加速度给计算结果带来较大误差，使用了阈值判定准则，当检测到载体存在运动加速度时，互补滤波器参数K_P1、K_I1、K_P2、K_I2、K_P3、K_I3置零，否则恢复设定值。