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

船舶重工集團(tuán)公司723所 周秀珍 吳連慧

本文針對GPS信號無效且無已知地面合作目標(biāo)的情況，開展了一種基于三層嵌入式卡爾曼濾波器的導(dǎo)航算法研究，提出了一種光流理論在基于視覺的微型飛行器定位系統(tǒng)中的應(yīng)用方案。文章首先通過三維視覺實(shí)時算法，并結(jié)合傳感器測量數(shù)據(jù)，構(gòu)造三層嵌入式卡爾曼濾波器，然后分別對高度、姿態(tài)角及水平面上的速度進(jìn)行估計。試驗結(jié)果表明，基于該導(dǎo)航算法的視覺系統(tǒng)能夠成功地實(shí)現(xiàn)微型飛行器的自主飛行。

引言

近年來，低成本高效率的慣性傳感器以及高精度的導(dǎo)航系統(tǒng)GPS已經(jīng)在無人機(jī)系統(tǒng)中占據(jù)了重要地位，在復(fù)雜的環(huán)境條件下，GPS信號很可能獲取不到，限制了傳統(tǒng)傳感器的使用。視覺傳感器重量輕，并且可以提供微型飛行器的運(yùn)動姿態(tài)以及周圍環(huán)境的實(shí)時觀測結(jié)果，因此依賴于計算機(jī)視覺實(shí)現(xiàn)微型飛行器的自主導(dǎo)航控制越來越受到關(guān)注。

本文開展了一種基于三層嵌入式卡爾曼濾波器的導(dǎo)航算法研究，提出了一種光流理論在基于視覺的微型飛行器定位系統(tǒng)中的應(yīng)用方案[1]。該方案采用三層嵌入式卡爾曼濾波結(jié)構(gòu)，其中每個卡爾曼濾波器都與光流計算，數(shù)據(jù)融合，運(yùn)動結(jié)構(gòu)估計相結(jié)合。采用三層嵌入式卡爾曼濾波結(jié)構(gòu)進(jìn)行微型飛行器自主飛行的數(shù)據(jù)處理具有快速性，準(zhǔn)確性及魯棒性[2]。

1.自主導(dǎo)航控制方案

應(yīng)用光流理論對微型飛行器進(jìn)行導(dǎo)航和控制是包含圖像處理，計算機(jī)視覺，機(jī)器人控制的多科目工程，本文研究考慮的是整個感知-運(yùn)動環(huán)路，即視覺系統(tǒng)考慮控制器。該視覺架構(gòu)從功能上可分為三部分：光流計算、視覺與姿態(tài)角速率融合、運(yùn)動與結(jié)構(gòu)恢復(fù)。

由三層嵌入式卡爾曼濾波器的自適應(yīng)控制器采用文獻(xiàn)[3]的設(shè)計思想。本文采用的光流算法：首先，其將圖像序列及姿態(tài)角速率作為輸入量。其次，采用運(yùn)動結(jié)構(gòu)預(yù)測子系統(tǒng)提供光流計算的有效性及魯棒性，估計的光流作為第二個卡爾曼濾波器的測量值，該測量值用來融合視覺信息與慣性測量單元信息從而消除光流的旋轉(zhuǎn)部分信息。最后，基于廣義卡爾曼濾波器的運(yùn)動結(jié)構(gòu)算法來估計光流的旋轉(zhuǎn)部分信息，從而恢復(fù)旋轉(zhuǎn)速度以及場景結(jié)構(gòu)。本文的算法與其它運(yùn)動結(jié)構(gòu)算法的不同之處主要在于：采用三層嵌入式卡爾曼濾波器結(jié)構(gòu)實(shí)時進(jìn)行光流計算及三維轉(zhuǎn)換。

2.基于三層嵌入式卡爾曼濾波器的導(dǎo)航算法

我們用于遞推估計無人機(jī)運(yùn)動和結(jié)構(gòu)的計算框架采用擴(kuò)展卡爾曼濾波器（EKF），由于擁有完整的系統(tǒng)動力學(xué)，且通過一種將慣性測量數(shù)據(jù)與視覺融合的有效方案，從而克服了模糊性。

2.1 圖像模型

將相機(jī)模型Pi的投影映射到焦平面可以通過以下的幾何變換：

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

2.2 基于廣義卡爾曼濾波器的運(yùn)動結(jié)構(gòu)估計算法

通常假定相機(jī)運(yùn)動可用動力學(xué)模型來描述：

其中，γk代表從慣性測量單元獲得相機(jī)或者微型飛行器的加速度。

場景可認(rèn)為是由剛體構(gòu)成并且保持靜止?fàn)顟B(tài)，精確的三維場景重構(gòu)可以通過景深來對場景進(jìn)行參數(shù)化，其中N代表計算的光流的N個像素。結(jié)構(gòu)參數(shù)景深Zi(t)的模型計算公式如下：

其中，模型的誤差用模型噪聲wk來表示，并且wk被假定為具有零均值和協(xié)方差矩陣的高斯分布。

前一個卡爾曼濾波器的輸出被當(dāng)作廣義卡爾曼濾波器的測量值，將慣性數(shù)據(jù)與光流相融合以后，便獲得了與微型飛行器旋轉(zhuǎn)速度V相關(guān)的光流轉(zhuǎn)換(OFtrans)，通過方程式（2）可獲得觀測的離散模型：

其中，Y表示包含光流轉(zhuǎn)換估計值的測量向量，非線性函數(shù)g為：

3.試驗結(jié)果與分析

為了驗證應(yīng)用于閉環(huán)控制視覺系統(tǒng)的魯棒性及穩(wěn)定性，針對室內(nèi)環(huán)境進(jìn)行基于光流視覺的微型飛行器自主飛行試驗。由于室內(nèi)地板是均勻平滑無紋理的，試驗中在地板上放置一些物品，以提供光流計算所需的紋理。在微型飛行器盤旋過程中，將其切換到手動遙控飛行幾秒鐘，這樣的目的在于驗證微型飛行器本身是不穩(wěn)定地，而基于視覺的控制系統(tǒng)對于穩(wěn)定微型飛行器起著至關(guān)重要的作用。試驗結(jié)果表明，基于該導(dǎo)航算法的視覺系統(tǒng)能夠成功地實(shí)現(xiàn)微型飛行器的自主飛行，試驗中GPS數(shù)據(jù)用來進(jìn)行數(shù)據(jù)對照。試驗結(jié)果如圖1所示。

圖1 室內(nèi)環(huán)境下微型飛行器高度及位置跟蹤軌跡圖

4.結(jié)論

本文針對GPS信號無效且無已知地面合作目標(biāo)的情況，開展了一種基于三層嵌入式卡爾曼濾波器的導(dǎo)航算法研究，提出了一種光流理論在基于視覺的微型飛行器定位系統(tǒng)中的應(yīng)用方案。通過多種試驗，視覺系統(tǒng)對室內(nèi)和室外飛行的效率和魯棒性得到了充分的證實(shí)。試驗結(jié)果表明，基于該導(dǎo)航算法的視覺系統(tǒng)能夠成功地實(shí)現(xiàn)微型飛行器的自主飛行，未來可繼續(xù)基于該方法進(jìn)行環(huán)境映射以及躲避障礙物研究。