宋 宇，翁新武，郭昕剛

(長春工業(yè)大學(xué) 計算機科學(xué)與工程學(xué)院，吉林 長春130012)

0 引 言

光流的概念是由Gibson 于1950 年首先提出的，是指空間運動的物體在成像面上的像素運動的瞬時速度，它表征了二維圖像的灰度變化和場景中物體及其運動的關(guān)系［1］，它根據(jù)像素灰度的時域變化和相關(guān)性確定各個像素點的運動速度，因此，可被觀察者用來確定目標(biāo)的運動情況［2］。對光流的研究成為計算機視覺和有關(guān)研究領(lǐng)域中的一個重要部分。因為在計算機視覺中，光流扮演著重要角色，在目標(biāo)對象分割、識別、跟蹤、機器人導(dǎo)航以及形狀信息恢復(fù)等都有著非常重要的應(yīng)用［3］。

光流傳感器是一種把圖像采集系統(tǒng)(image acquisition system，IAS)和數(shù)字信號處理器(digital signal processor，DSP)整合到一個芯片上，并內(nèi)嵌光流算法的一體式視覺傳感器。光流傳感器能對視覺運動進行測量并輸出二維測量值，常用于計算機的光電鼠標(biāo)中，作為測量鼠標(biāo)運動的主要感應(yīng)元件。此外，光流傳感器也用于機器人進行視覺運動測量［4］和相對運動感知［5］。近年來，使用光流法進行飛行導(dǎo)航［6］和避障［7］也成為小型無人機研究領(lǐng)域的一個熱點問題。

雖然在機器人視覺中已經(jīng)大量使用光流法進行導(dǎo)航和避障，但是將二維光流信息與六自由度信息融合進行運動估計的方法還不是很成熟。本文將介紹如何把二維光流數(shù)據(jù)與慣性測量單元(IMU)通過擴展卡爾曼濾波器(EKF)進行數(shù)據(jù)融合，得到全向的六自由度飛行狀態(tài)。此外，該算法使用了捷聯(lián)式慣性導(dǎo)航系統(tǒng)(SINS)方程，而不是專用的平臺導(dǎo)航系統(tǒng)方程，本文提出的算法可以用于任何六自由度的平臺。

1 光流傳感器工作原理與運動檢測

1.1 光流傳感器工作原理

光流傳感器通過IAS 以一定速率連續(xù)采集物體表面圖像，再由DSP 對所產(chǎn)生的圖像數(shù)字矩陣進行分析。由于相鄰的兩幅圖像總會存在相同的特征，通過對比這些特征點的位置變化信息，便可以判斷出物體表面特征的平均運動，這個分析結(jié)果最終被轉(zhuǎn)換為二維的坐標(biāo)偏移量，并以像素數(shù)形式存儲在特定的寄存器中，實現(xiàn)對運動物體的檢測。

1.2 光流測量

假設(shè)光流傳感器的坐標(biāo)系與飛行器機體坐標(biāo)系對齊，光流傳感器的視場角為α(弧度)，視場寬度為d，視場寬度對應(yīng)的像素總數(shù)為Np，飛行器水平運動速度為v，飛行器距離地面高度為H，那么由光流傳感器測得的光流經(jīng)過采樣時間Δt 積累得到像素數(shù)(nx，ny)，通過光流傳感器的參數(shù)將像素點轉(zhuǎn)換為弧度形式的角增量(θx，θy)

1.3 運動補償

假設(shè)飛行器位置固定不變，飛行器做橫滾或俯仰運動時，關(guān)于繞機體坐標(biāo)系x 或y 軸的任何旋轉(zhuǎn)將表現(xiàn)為光流。因為光流傳感器的坐標(biāo)系與機體坐標(biāo)系對齊，所以，一個繞x 軸的旋轉(zhuǎn)將得到y(tǒng) 軸的光流變化;反之，亦然。而飛行器繞z 軸旋轉(zhuǎn)時，由于光流圍繞著圖像中心旋轉(zhuǎn)將被平均，導(dǎo)致光流為零，所以，繞z 軸旋轉(zhuǎn)不用進行補償。

旋轉(zhuǎn)運動可以通過旋轉(zhuǎn)矢量Φ=［φxφyφz］T及其微分方程［8］來描述

由式(1)和式(2)可以計算得到飛行器橫滾和俯仰時所引起的光流誤差

光流傳感器實際測量值需要減去該誤差值才能得到真正的水平方向運動量。

1.4 地面速度和高度

為了把傳感器輸出值轉(zhuǎn)換成實際運動的距離，需要考慮高度。如果兩個飛行器運動了同樣距離，但是一個低，一個高，低的一個會看到表面特征運動得更遠，所以，光流數(shù)值會更大。假設(shè)飛行器運動距離為一固定值，那么，光流的大小和高度呈反比，則光流角增量(θx，θy)也可以表示為

但是只使用一個光流傳感器測量時，只能得到速度和高度的比值，而不能分別計算出它們的具體大小。本文采用兩個型號一樣的光流傳感器，分別沿機體坐標(biāo)系z 軸一高一低安裝，兩者高度差為L，如圖1 所示，則兩個光流傳感器之間存在下列關(guān)系

且有H2=H1+L，經(jīng)代數(shù)運算后得

圖1 雙光流檢測幾何關(guān)系Fig 1 Geometric relationship of dual optical flow detection

2 光流組合導(dǎo)航

2.1 系統(tǒng)模型分析與建立

在概率導(dǎo)航中，關(guān)注的是在給定觀測值Zk條件下，飛行器狀態(tài)Xk在k 時刻出現(xiàn)的概率，即為后驗概率

由貝葉斯公式可以得到

式中 p(Xk)為Xk的先驗概率，p(Zk|Xk)為Xk的相似度函數(shù)，p(Zk)為Zk的先驗概率，也作標(biāo)準化常量。

根據(jù)查普曼—科爾莫戈羅夫(Chapman－Kolmogorov)方程［9］，可以得到該系統(tǒng)預(yù)測狀態(tài)的概率密度函數(shù)為

在已知觀測值條件下，根據(jù)貝葉斯公式，那么后驗概率就等于預(yù)測狀態(tài)的概率密度函數(shù)和觀測似然的乘積，然后再乘以比例系數(shù)，得

式中 c 為標(biāo)準化常量的倒數(shù)。

在大多數(shù)應(yīng)用中直接使用上述方法是不可行的，這里需要假設(shè)條件概率滿足高斯分布，這時，可以有效利用EKF進行狀態(tài)估計。圖2 為使用EKF 系統(tǒng)結(jié)構(gòu)示意圖，光流傳感器測量值首先和慣性測量、高度測量和航向測量的預(yù)測值相比較，然后再送入EKF 中，最后得到系統(tǒng)的位置、速度和姿態(tài)信息。

圖2 使用EKF 的系統(tǒng)結(jié)構(gòu)圖Fig 2 Structure of system using EKF

2.2 狀態(tài)模型

在應(yīng)用EKF 時，必須建立系統(tǒng)的狀態(tài)模型，本文的狀態(tài)模型實際上是一個非線性慣性導(dǎo)航模型，這里取位置、速度和姿態(tài)作為系統(tǒng)的狀態(tài)量Xk=［P V Ψ］T，則有

式中 f(·)為非線性狀態(tài)轉(zhuǎn)換函數(shù)，Wk為過程噪聲，然而f(·)不能直接應(yīng)用在協(xié)方差中，取而代之的是計算雅可比矩陣，這樣系統(tǒng)所傳播的不確定性其實就是使用雅可比矩陣和過程噪聲強度的近似。

2.3 觀測模型

取雙光流傳感器的測量值Zk=［nx1ny1nx2ny2］T作為系統(tǒng)的觀測模型，則有

式中 Vk作為觀測噪聲。通過式(1)、式(3)和式(4)結(jié)合可以得到非線性觀測模型為

以上各式已經(jīng)對橫滾和俯仰運動進行了補償。

由于篇幅有限，使用系統(tǒng)狀態(tài)模型和觀測模型進行KF的時間更新和狀態(tài)更新這里就不在詳細描述，具體更新過程可以在文獻［10］中找到。

3 實 驗

本文采用安捷倫ADNS—3080 光學(xué)鼠標(biāo)芯片作為光流傳感器，包含一個IAS、一個DSP 和一個四線串行接口(SPI)，其分辨率為1 600 像素/in(1in≈25.4 mm)，圖像采樣速率為2 000 幀/s，視場角α 為23°，視場寬度所對應(yīng)的像素總數(shù)NP為30。IMU 采用MPU—6050 模塊，包含三軸陀螺儀、加速計和磁場計及氣壓計，使用I2C 接口直接讀取飛行器三個軸的角速率、加速度和磁場大小，以及氣壓大小。MPU—6050 整合了三軸陀螺儀、三軸加速器，其中角速度測量范圍為:±250°/s，±500°/s，±1 000°/s，±2 000°/s，加速度測量范圍為:±2，±4，±8，±16 gn，磁場計HMC5583L 的磁場測量范圍為 ±(1.3～8)Gs。

本文采用小型四旋翼飛行器作為實驗平臺，采用兩組性能一樣的光流模塊作為光流傳感器，傳感器間隔L 為0.1 m，光流傳感器的坐標(biāo)系與飛行器機體坐標(biāo)系對齊。實驗設(shè)在室內(nèi)，為了確保實驗中光照強度足夠，在房間四周設(shè)置4 個500 W 的熒光燈，同時為了使光流傳感器采樣不失步，本文設(shè)置表面紋理分明且平坦地面作為檢測面。

實驗中，首先通過手動遙控使飛行器達到指定位置，并微調(diào)校準各通道使飛行器能夠保持當(dāng)前位置，然后發(fā)送指令進入光流傳感器輔助的自主懸停，懸停高度為2 m，懸停時間為60 s，主要測試和對比慣性導(dǎo)航和光流輔助導(dǎo)航的定位精度。使用無線數(shù)傳模塊采集各傳感器數(shù)據(jù)，然后通過Matlab 對數(shù)據(jù)進行處理和顯示，得出如下的數(shù)據(jù)曲線如圖3～圖5 所示。

從圖3～圖5 可以看出:當(dāng)單獨使用捷聯(lián)慣性導(dǎo)航算法時，由于慣性傳感器的參數(shù)性能較低，即使在開始時經(jīng)過初始校準，由于慣性傳感器誤差會隨時間而積累，其解算得到的位置、速度和姿態(tài)也會隨著時間呈現(xiàn)指數(shù)發(fā)散。而通過EKF 算法將光流傳感器與IMU 進行數(shù)據(jù)融合后，速度和位置誤差得到了很好的糾正，光流輔助懸停時x 和y 方向上的最大位置漂移量均小于30 cm，基本滿足室內(nèi)環(huán)境下的定位精度要求。

圖3 位置比較Fig 3 Position comparison

圖6 為在10 m 高度范圍內(nèi)以不同的采樣頻率得到的像素數(shù)之差。可以明顯的看出:以10 Hz 采樣時(圖中實線)需要高度大于3 m 才能分辨出雙光流傳感器像素數(shù)之差，即高度低于3 m 時，就不能得到當(dāng)前高度。如果想在各高度層都能正常工作，那么，采樣速率應(yīng)當(dāng)?shù)陀? Hz(圖中點劃線)。

圖4 速度估計Fig 4 Velocity estimation

圖5 姿態(tài)估計Fig 5 Attitude estimation

圖6 不同采樣頻率和高度時雙光流傳感器像素數(shù)之差Fig 6 Difference pixel numbers of dual optical-flow sensor with different sampling frequencies and height

4 結(jié) 論

本文提出了一種基于光流傳感器和IMU 相結(jié)合的定位方法，并使用EKF 進行數(shù)據(jù)融合，得到飛行器的實時位置、速度和姿態(tài)信息。實驗表明:該方法能夠有效提高小型無人機導(dǎo)航性能，減小導(dǎo)航時的位置、速度和姿態(tài)誤差，可以為小型無人機室內(nèi)導(dǎo)航提供一種有效的方案。在室外環(huán)境時可以通過與衛(wèi)星導(dǎo)航系統(tǒng)、地形地圖等相結(jié)合，為小型無人機低空飛行提供更準確的導(dǎo)航。

［1］ 張洪濤，張廣玉，李隆球，等.微型二維光流傳感器設(shè)計［J］.哈爾濱工程大學(xué)學(xué)報，2014，35(5):619－623.

［2］ Koenderink Jan J.Optic flow［J］.Vision Research，1986，26(1):161－180.

［3］ Sundareswaran V.Egomotion from global flow field data［C］Proceedings of the IEEE Workshop on Visual Motion，Los Alamitos，USA，1991:140－145.

［4］ 于合龍，劉浩洋，蘇恒強.基于光流追蹤技術(shù)的變形位移測量方法［J］.吉林大學(xué)學(xué)報:理學(xué)版，2014，52(2):331－335.

［5］ 郭 力，昂海松，鄭祥明.基于單目視覺的微型飛行器移動目標(biāo)定位方法［J］.系統(tǒng)工程與電子技術(shù)，2012，34(5):996－1000.

［6］ Zingg Simon，Scaramuzza Davide，Weiss Stephan，et al.MAV navigation through indoor corridors using optical flow［C］∥IEEE International Conference on Robotics and Automation，Anchorage，USA，2010:3361－3368.

［7］ Green W E，Oh P Y，Sevcik K，et al.Autonomous landing for indoor flying robots using optic flow［C］∥ASME International Mechanical Engineering Congress，2003.

［8］ Bortz J.A new mathematical formulation for strap down inertial navigation［J］.IEEE Transactions on Aerospace and Electronic Systems(AES)，1971，7(1):61－66.

［9］ 現(xiàn)代應(yīng)用數(shù)學(xué)手冊編委會.現(xiàn)代應(yīng)用數(shù)學(xué)手冊/概率統(tǒng)計與隨機過程卷［M］.北京:清華大學(xué)出版社，2000.

［10］Maybeck P.Stochastic models estimations and control［M］.New York:Academic Press，1982.