硬件Kalman滤波器的航拍云台姿态如何获取-电子发烧友网

0 引言

航空摄影（Aerial Photography）又称航拍，是指在飞机或其他航空飞行器上利用航空摄影机摄取地面景物相片的技术。目前，航空摄影应用广泛，是获取地理信息的主要手段[1-2]。在航空摄影中，存在着一个难题需要解决：飞机在飞行摄影时会受到本机和气流等的影响，从而产生机体振动，相机的姿态角也会随着改变，相机姿态时刻改变就会给高精度拍摄带来困难。所以为提高航拍质量，必须对相机姿态进行校正[3]。

校正的前提是获取相机姿态，所以惯性测量元件必不可少，但是单一的惯性测量元件无法给出准确的姿态角，需要对多个惯性测量元件的数据进行数据融合。使用比较普遍的惯性测量元件的组合是加速度计和陀螺仪。它们输出的数据各有优缺点，可以使用Kalman滤波对其进行数据融合以得到准确的姿态角[4-5]。

现有的实现Kalman滤波的方式主要有两种：PC机和DSP，两者都存在着系统性能和成本不能兼顾的问题。而FPGA却可以很好地解决这个问题。本文主要讲述的便是用FPGA实现Kalman滤波来获取准确的相机姿态。

1 坐标模型

在进行航拍作业时，相机搭载在云台上，云台和飞机是一体的，当相机拍摄地面照片时，相机拍摄方向应与地面垂直，如图1所示。

为获取相机姿态，首先以相机为原点，建立坐标系，如图2所示。其中，XY平面与地面平行，Z轴垂直于地面，Y轴方向为飞机飞行方向。

将加速度计固定在相机上，当相机姿态发生改变时，加速度的3个敏感轴便可以输出在其相应方向产生的重力分量信号，如图2中的Gx，Gy，Gz所示。陀螺仪是一种角速度传感器，它可以输出对应轴上的瞬时角速度，如图2中的Wx，Wy，Wz所示。

根据以上坐标系，可以得到姿态角θx，θy，θz（分别代表着俯仰角、横滚角、偏航角），如式1所示，其中ΔT为采样时间，Δθ为采样时间内的姿态角变化。

可见，由加速度计或者陀螺仪都可以初步得到姿态角，但是无论是加速度计还是陀螺仪，它们输出的原始数据都存在着固有缺陷：当相机处于变速运动状态时，加速度计的输出并不是重力加速度，而是重力加速度和其自身加速度的矢量和。陀螺仪是通过对角速度积分得到姿态角，随着时间的推移，姿态角的误差会越来越大。所以单一的加速度计或者陀螺仪都无法获得正确的姿态角，必须对两者进行数据融合。

2 Kalman滤波模型的建立

Kalman滤波是在时域内以信号的一、二阶统计特性已知为前提、以均方误差极小为判据，能自动跟踪信号统计性质的非平稳变化，具有递归性质的一种算法[5]。Kalman滤波的状态方程和预测方程如式(2)所示：

其中，Xk+1为系统的状态向量，Φk+1，k为状态转移矩阵，Tk+1，k为控制系数矩阵，Uk为系统控制输入，Γk+1为噪声系数矩阵，Wk为系统的过程噪声序列，Zk+1为系统的观测序列，Hk+1为观测矩阵，Vk+1为观测噪声序列。

使用陀螺仪数据建立线性模型，如式(3)所示：

其中，θk为飞行姿态角，βk为陀螺仪输出的误差，Uk是陀螺仪输出的角速度数据，ΔT是采样时间间隔。因为陀螺仪输出的误差整体来看变化不大，可认为βk+1=βk。因Wk对系统影响甚小，这里规定Wk=0。

由以上建立状态矩阵方程，如式(4)所示：

陀螺仪和加速度计测量姿态角时各有优缺点，为获得比较可靠的姿态角数据，必须对两者的数据优势互补。以上算法用加速度计数据得到不同阶段的测量噪声协方差，并带入解算过程中，大大减少了姿态角的测量误差。

3 Kalman硬件实现

目前Kalman滤波器的实现方式主要有PC机和DSP两种方式，PC机可以同时满足计算精度和实时性的要求，但是PC机体积大、质量重、成本高；DSP实现的Kalman滤波器虽然体积小，质量轻，但是因其指令顺序执行的CPU架构，在系统复杂时无法满足系统的实时性要求[6-7]。

因此，Kalman滤波器在工程应用中的实现遇到了系统体积、重量、成本与系统精度、速度等性能不能兼顾的问题。FPGA是解决这一问题的有效途径，FPGA具有极高并行度的信号处理能力、高度的灵活性、现场可以编程、较短的开发周期和较低的成本等优势，在数字信号处理领域已经得到广泛的应用[8]。

本系统使用的FPGA芯片是Altera公司Cyclone II系列中的EP2C35F672C6，它共有33 216个逻辑单元、483 840个存储单元、70个乘法器单元（9 bit）。虽然Altera公司提供了很多功能强大的IP核，其中就包括加减乘除单精度浮点数运算，但是提供的IP核占用资源过多，所以使用自己编写的运算模块。下面以除法模块为例说明浮点数运算的硬件实现。

3.1 浮点数除法模块

根据IEEE754标准，单精度浮点数的格式如图2所示。