基于光流和慣性導(dǎo)航的小型無人機(jī)定位方法*

宋 宇，翁新武，郭昕剛

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

0 引 言

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

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

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

1 光流傳感器工作原理與運(yùn)動(dòng)檢測

1.1 光流傳感器工作原理

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

1.2 光流測量

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

1.3 運(yùn)動(dòng)補(bǔ)償

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

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

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

光流傳感器實(shí)際測量值需要減去該誤差值才能得到真正的水平方向運(yùn)動(dòng)量。

1.4 地面速度和高度

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

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

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

圖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 時(shí)刻出現(xiàn)的概率，即為后驗(yàn)概率

由貝葉斯公式可以得到

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

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

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

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

在大多數(shù)應(yīng)用中直接使用上述方法是不可行的，這里需要假設(shè)條件概率滿足高斯分布，這時(shí)，可以有效利用EKF進(jìn)行狀態(tài)估計(jì)。圖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 時(shí)，必須建立系統(tǒng)的狀態(tài)模型，本文的狀態(tài)模型實(shí)際上是一個(gè)非線性慣性導(dǎo)航模型，這里取位置、速度和姿態(tài)作為系統(tǒng)的狀態(tài)量Xk=［P V Ψ］T，則有

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

2.3 觀測模型

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

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

以上各式已經(jīng)對橫滾和俯仰運(yùn)動(dòng)進(jìn)行了補(bǔ)償。

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

3 實(shí) 驗(yàn)

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

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

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

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

圖3 位置比較Fig 3 Position comparison

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

圖4 速度估計(jì)Fig 4 Velocity estimation

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

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

4 結(jié) 論

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

［1］ 張洪濤，張廣玉，李隆球，等.微型二維光流傳感器設(shè)計(jì)［J］.哈爾濱工程大學(xué)學(xué)報(bào)，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］ 于合龍，劉浩洋，蘇恒強(qiáng).基于光流追蹤技術(shù)的變形位移測量方法［J］.吉林大學(xué)學(xué)報(bào):理學(xué)版，2014，52(2):331－335.

［5］ 郭 力，昂海松，鄭祥明.基于單目視覺的微型飛行器移動(dòng)目標(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é)手冊編委會(huì).現(xiàn)代應(yīng)用數(shù)學(xué)手冊/概率統(tǒng)計(jì)與隨機(jī)過程卷［M］.北京:清華大學(xué)出版社，2000.

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