一種基于三層嵌入式卡爾曼濾波器的導航算法研究

船舶重工集團公司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信號無效且無已知地面合作目標的情況，開展了一種基于三層嵌入式卡爾曼濾波器的導航算法研究，提出了一種光流理論在基于視覺的微型飛行器定位系統中的應用方案。通過多種試驗，視覺系統對室內和室外飛行的效率和魯棒性得到了充分的證實。試驗結果表明，基于該導航算法的視覺系統能夠成功地實現微型飛行器的自主飛行，未來可繼續基于該方法進行環境映射以及躲避障礙物研究。