一种基于三层嵌入式卡尔曼滤波器的导航算法研究

船舶重工集团公司723所 周秀珍 吴连慧

本文针对GPS信号无效且无已知地面合作目标的情况，开展了一种基于三层嵌入式卡尔曼滤波器的导航算法研究，提出了一种光流理论在基于视觉的微型飞行器定位系统中的应用方案。文章首先通过三维视觉实时算法，并结合传感器测量数据，构造三层嵌入式卡尔曼滤波器，然后分别对高度、姿态角及水平面上的速度进行估计。试验结果表明，基于该导航算法的视觉系统能够成功地实现微型飞行器的自主飞行。

引言

近年来，低成本高效率的惯性传感器以及高精度的导航系统GPS已经在无人机系统中占据了重要地位，在复杂的环境条件下，GPS信号很可能获取不到，限制了传统传感器的使用。视觉传感器重量轻，并且可以提供微型飞行器的运动姿态以及周围环境的实时观测结果，因此依赖于计算机视觉实现微型飞行器的自主导航控制越来越受到关注。

本文开展了一种基于三层嵌入式卡尔曼滤波器的导航算法研究，提出了一种光流理论在基于视觉的微型飞行器定位系统中的应用方案[1]。该方案采用三层嵌入式卡尔曼滤波结构，其中每个卡尔曼滤波器都与光流计算，数据融合，运动结构估计相结合。采用三层嵌入式卡尔曼滤波结构进行微型飞行器自主飞行的数据处理具有快速性，准确性及鲁棒性[2]。

1.自主导航控制方案

应用光流理论对微型飞行器进行导航和控制是包含图像处理，计算机视觉，机器人控制的多科目工程，本文研究考虑的是整个感知-运动环路，即视觉系统考虑控制器。该视觉架构从功能上可分为三部分：光流计算、视觉与姿态角速率融合、运动与结构恢复。

由三层嵌入式卡尔曼滤波器的自适应控制器采用文献[3]的设计思想。本文采用的光流算法：首先，其将图像序列及姿态角速率作为输入量。其次，采用运动结构预测子系统提供光流计算的有效性及鲁棒性，估计的光流作为第二个卡尔曼滤波器的测量值，该测量值用来融合视觉信息与惯性测量单元信息从而消除光流的旋转部分信息。最后，基于广义卡尔曼滤波器的运动结构算法来估计光流的旋转部分信息，从而恢复旋转速度以及场景结构。本文的算法与其它运动结构算法的不同之处主要在于：采用三层嵌入式卡尔曼滤波器结构实时进行光流计算及三维转换。

2.基于三层嵌入式卡尔曼滤波器的导航算法

我们用于递推估计无人机运动和结构的计算框架采用扩展卡尔曼滤波器（EKF），由于拥有完整的系统动力学，且通过一种将惯性测量数据与视觉融合的有效方案，从而克服了模糊性。

2.1 图像模型

将相机模型Pi的投影映射到焦平面可以通过以下的几何变换：

其中，(xi,yi)是Pi映射到焦平面的投影，方程式（1）是中心投影模型，β是逆焦距。

2.2 基于广义卡尔曼滤波器的运动结构估计算法

通常假定相机运动可用动力学模型来描述：

其中，γk代表从惯性测量单元获得相机或者微型飞行器的加速度。

场景可认为是由刚体构成并且保持静止状态，精确的三维场景重构可以通过景深来对场景进行参数化，其中N代表计算的光流的N个像素。结构参数景深Zi(t)的模型计算公式如下：

其中，模型的误差用模型噪声wk来表示，并且wk被假定为具有零均值和协方差矩阵的高斯分布。

前一个卡尔曼滤波器的输出被当作广义卡尔曼滤波器的测量值，将惯性数据与光流相融合以后，便获得了与微型飞行器旋转速度V相关的光流转换(OFtrans)，通过方程式（2）可获得观测的离散模型：

其中，Y表示包含光流转换估计值的测量向量，非线性函数g为：

3.试验结果与分析

为了验证应用于闭环控制视觉系统的鲁棒性及稳定性，针对室内环境进行基于光流视觉的微型飞行器自主飞行试验。由于室内地板是均匀平滑无纹理的，试验中在地板上放置一些物品，以提供光流计算所需的纹理。在微型飞行器盘旋过程中，将其切换到手动遥控飞行几秒钟，这样的目的在于验证微型飞行器本身是不稳定地，而基于视觉的控制系统对于稳定微型飞行器起着至关重要的作用。试验结果表明，基于该导航算法的视觉系统能够成功地实现微型飞行器的自主飞行，试验中GPS数据用来进行数据对照。试验结果如图1所示。

图1 室内环境下微型飞行器高度及位置跟踪轨迹图

4.结论

本文针对GPS信号无效且无已知地面合作目标的情况，开展了一种基于三层嵌入式卡尔曼滤波器的导航算法研究，提出了一种光流理论在基于视觉的微型飞行器定位系统中的应用方案。通过多种试验，视觉系统对室内和室外飞行的效率和鲁棒性得到了充分的证实。试验结果表明，基于该导航算法的视觉系统能够成功地实现微型飞行器的自主飞行，未来可继续基于该方法进行环境映射以及躲避障碍物研究。