基于慣性光流的多旋翼飛行器地形跟蹤

史添瑋，崔文華，任玲

（1.遼寧科技大學(xué)國際金融與銀行學(xué)院，遼寧鞍山114051；2.遼寧聚龍金融設(shè)備股份有限公司，遼寧鞍山114051）

基于慣性光流的多旋翼飛行器地形跟蹤

史添瑋1，崔文華1，任玲2

（1.遼寧科技大學(xué)國際金融與銀行學(xué)院，遼寧鞍山114051；2.遼寧聚龍金融設(shè)備股份有限公司，遼寧鞍山114051）

為解決目前多旋翼飛行器光流技術(shù)存在的定位精度低與多相機(jī)數(shù)據(jù)處理過程繁瑣等問題，本文應(yīng)用金字塔層數(shù)自適應(yīng)的Lucas-Kanade算法獲取平移慣性光流以實(shí)現(xiàn)多旋翼飛行器穩(wěn)定地形跟蹤。下視球面相機(jī)獲取多紋理的目標(biāo)平面圖像，同時(shí)將目標(biāo)平面特征點(diǎn)動(dòng)態(tài)映射至相機(jī)鏡頭表面；將比對兩幀連續(xù)時(shí)刻圖像后獲取的平移光流作為非線性地形跟蹤控制器的反饋，實(shí)現(xiàn)多旋翼飛行器地形跟蹤與避免碰撞。10次室內(nèi)實(shí)際飛行測試結(jié)果表明，不同地形變化區(qū)域的高度誤差分別為±3 cm與±6 cm，驗(yàn)證了該控制器是穩(wěn)定可行的。

多旋翼飛行器；平移慣性光流；地形跟蹤；非線性控制器

基于視覺的光流技術(shù)克服了多旋翼飛行器有效載荷小的限制，增加了其自主性。光流技術(shù)適應(yīng)性強(qiáng)，可借助于慣性導(dǎo)航系統(tǒng)提供精確的相對位置等信息及最大可能地為飛行器提供自主導(dǎo)航［1］。目前，國內(nèi)學(xué)者應(yīng)用光流技術(shù)實(shí)現(xiàn)了人體姿態(tài)識別［2］，移動(dòng)目標(biāo)跟蹤［3］與懸停控制［4］等。國外學(xué)者將光流技術(shù)成功應(yīng)用于異常視覺活動(dòng)檢測［5］與蜂鳥翅膀運(yùn)動(dòng)的自動(dòng)分析［6］與運(yùn)動(dòng)目標(biāo)檢測［7］等。

目前，應(yīng)用于多旋翼飛行器的光流技術(shù)存在定位精度低與多相機(jī)光流數(shù)據(jù)處理過程繁瑣等問題，為此，本文首先利用下視球面相機(jī)獲取目標(biāo)平面圖像，并結(jié)合捷聯(lián)慣性導(dǎo)航系統(tǒng)（SINS）獲取飛行器的實(shí)時(shí)速度、高度以及目標(biāo)平面特征點(diǎn)的平移慣性光流等信息。其次，采用金字塔層數(shù)自適應(yīng)的Lucas-Kanade算法估計(jì)并計(jì)算光流。然后，依據(jù)飛行器動(dòng)力學(xué)方程，設(shè)計(jì)了將平移慣性光流作為反饋信息的用于地形跟蹤的非線性控制器，并應(yīng)用Lyapunov分析證明了控制器的穩(wěn)定性。

1 目標(biāo)平面特征點(diǎn)動(dòng)態(tài)映射

假設(shè)下視球面相機(jī)放置在多旋翼飛行器的理論質(zhì)心處且焦距為1。圖1a與圖1b分別為平面與球面相機(jī)的目標(biāo)特征點(diǎn)動(dòng)態(tài)映射過程。其中，O與S2分別表示平面與球面相機(jī)的鏡頭表面；P=(X，Y，Z)∈R3為目標(biāo)平面中的可見特征點(diǎn)，其在平面與球面相機(jī)鏡頭表面的映射點(diǎn)分別為pO與pS。將兩種相機(jī)的映射過程對時(shí)間求導(dǎo)，并結(jié)合雅克比矩陣可得

圖1 平面與球面相機(jī)的目標(biāo)特征點(diǎn)動(dòng)態(tài)映射過程Fig.1 Dynamic mapping processes of planar camera and spherical camera

多旋翼飛行器的任意運(yùn)動(dòng)被認(rèn)為是質(zhì)心水平運(yùn)動(dòng)與環(huán)繞質(zhì)心轉(zhuǎn)動(dòng)的合成運(yùn)動(dòng)［8］。由于目標(biāo)平面中的特征點(diǎn)是穩(wěn)定的，因此，其位置的移動(dòng)僅取決于飛行器的移動(dòng)。

目標(biāo)特征點(diǎn)在球面相機(jī)鏡頭表面的映射點(diǎn)pS與相機(jī)鏡頭表面中心線間的動(dòng)態(tài)半徑可表示為［9］

式中：Ω為機(jī)體坐標(biāo)系相對于地面坐標(biāo)系的轉(zhuǎn)動(dòng)角速度在機(jī)體坐標(biāo)系各軸向的分量；πp為點(diǎn)p在球面相機(jī)鏡頭表面S2的切線空間映射；向量V=RTv，v=ξ 表示飛行器在地面坐標(biāo)系的飛行速度；ξ=[x,y,z]為飛行器在地面坐標(biāo)系的位置；R為坐標(biāo)系間的轉(zhuǎn)換關(guān)系矩陣。

定義d?d(t)為地面坐標(biāo)系中的目標(biāo)平面與機(jī)體坐標(biāo)系水平面之間的正交距離，則目標(biāo)平面的任意點(diǎn)P，滿足如下關(guān)系［9］

式中：k′=RTk，k為飛行器水平飛行時(shí)慣性方向；μ為慣性方向與垂直于目標(biāo)平面方向間的夾角。

2 金字塔層數(shù)自適應(yīng)的Lucas-Kanade算法

本文采用金字塔層數(shù)自適應(yīng)的Lucas-Kanade算法獲取目標(biāo)光流，但其存在層數(shù)確定問題：若層數(shù)選擇不足，則圖像尺度縮小程度不足，無法滿足Lucas-Kanade算法假設(shè)；若層數(shù)選擇過多，則頂層圖像分辨率過低，大量有效信息丟失。通常，構(gòu)建5層以上的金字塔就會(huì)造成信息丟失［10］。其優(yōu)化過程如下：

（1）假設(shè)兩幀連續(xù)時(shí)刻圖像的最大位移為像素t，并將原始圖像融入金字塔中。將第i幀圖像向上、向下、向左或向右平移t次，并將其與第i+1幀圖像比較。其結(jié)果可描述為

式中：T(k)為移動(dòng)k(k=1,2,…,t)次的結(jié)果矩陣；Ii(k)為第i幀圖像移動(dòng)像素k的圖像。

（2）對于第i幀圖像的每個(gè)像素點(diǎn)，將移動(dòng)過程中T(k)的最小值賦值給像素點(diǎn)，即：Mx,y=k。

（3）為金字塔各層設(shè)置閾值。若計(jì)算得到大于某預(yù)定閾值的最大移動(dòng)值，則將其作為第i幀圖像的最大移動(dòng)矢量，并為其賦值，即：MVmax=max(k)。其中，m與n分別為圖像的寬與高。

（4）依據(jù)MVmax值確定金字塔層數(shù)。

3 平移慣性光流計(jì)算

由于p為已知數(shù)據(jù)，Ω與k的估計(jì)值可以通過慣性測量單元獲得［11］，因此，球面相機(jī)鏡頭表面W2窗口的平均光流可表示為［9］

式中：γˉ是以k為中心的球面鏡頭視場角度；Q為隨時(shí)間變化的正定對稱矩陣；i為目標(biāo)平面中的特征點(diǎn)個(gè)數(shù)；∧為恒定對角矩陣；Rt為目標(biāo)平面與地面坐標(biāo)系間的方向矩陣。若目標(biāo)平面與地面坐標(biāo)系的水平面平行，則由式（5）可得平移慣性光流為

4 基于光流法的地形跟蹤原理

圖2 地形跟蹤過程中坐標(biāo)系之間的關(guān)系Fig.2 Relationship of coordinates during terrain-following

式中：kP與kD為正值；ωxt與ωzt分別為斜坡子坐標(biāo)系（t坐標(biāo)系）的光流分量。在機(jī)體坐標(biāo)系中，依據(jù)k′方向的動(dòng)力學(xué)方程可得

該式表明，只要d(t)＞0，則L＜L(0)。應(yīng)用La-Salle原則，可得出d ≡0。結(jié)合式（9），很容易得出d-d*漸進(jìn)趨于0，因此，d漸進(jìn)趨于d*。在保證d(t)為正值的條件下，飛行器可完成地形跟蹤，同時(shí)避免與地面碰撞。

5 實(shí)驗(yàn)結(jié)果及分析

5.1 實(shí)驗(yàn)參數(shù)

實(shí)驗(yàn)使用德國MK公司的Hexa系列六旋翼飛行器作為研究平臺(tái)。相機(jī)觀測角度為圓周70°，地形跟蹤控制器的參數(shù)為：kP=3，kD=1。設(shè)定ωd=(0.5，0，0)T，前向速度為1 m/s，飛行期望高度為2 m，光流采樣頻率為30 Hz。實(shí)驗(yàn)共進(jìn)行10組，多旋翼飛行器沿直線飛行的距離為40 m。

5.2 實(shí)驗(yàn)結(jié)果分析

在進(jìn)行地形跟蹤實(shí)驗(yàn)時(shí)，由于計(jì)算光流的方程多于未知參數(shù)個(gè)數(shù)，因此，對運(yùn)動(dòng)參數(shù)使用最小二乘法估計(jì)得到折中解。10組地形跟蹤實(shí)驗(yàn)結(jié)果為：k′方向誤差（2.4±1.1）°，X位置誤差（2.1±0.9）cm，Z位置誤差（-1.3±1.7）cm，前向光流（0.42± 0.11）lm，垂直光流（0.02±0.09）lm。其中，位置誤差與光流測量值采用平均值±標(biāo)準(zhǔn)差表示。

圖3為實(shí)驗(yàn)地形與實(shí)際跟蹤實(shí)驗(yàn)結(jié)果。圖3表明，地形相對平緩區(qū)域，高度方向的誤差為±3 cm；地形變化明顯區(qū)域，高度方向的誤差為±6 cm。圖4為地形跟蹤實(shí)驗(yàn)過程中，機(jī)體坐標(biāo)系中各軸向光流分量與k′方向的角度估計(jì)。由圖4可以看出，各軸向的光流分別相對平穩(wěn)并趨近于各自的期望光流；k′方向的角度估計(jì)與實(shí)驗(yàn)地形大致相同。證明了光流控制地形跟蹤的可行性與穩(wěn)定性。此外，在圖3與圖4的地形變化明顯區(qū)域，存在飛行器高度響應(yīng)及k′方向的角度估計(jì)滯后現(xiàn)象，滯后時(shí)間分別為（100±4）ms與（112±5）ms。其主要是由飛行器的飛行速度、慣性與光流采樣頻率較低等因素造成的。

圖3 實(shí)驗(yàn)地形與實(shí)際跟蹤結(jié)果Fig.3 Experimental terrain and actual terrain-following result

圖4 光流與k′方向估計(jì)Fig.4 Optical flow and estimation ofk′direction

為了進(jìn)一步驗(yàn)證用于實(shí)現(xiàn)光流控制方法的穩(wěn)定性，在完成地形跟蹤實(shí)驗(yàn)的同時(shí)，對飛行器的前向速度與前向光流的趨勢進(jìn)行測量，圖5為所測量的結(jié)果。圖5表明，前向速度與光流測量值均穩(wěn)定且趨近于期望值。實(shí)驗(yàn)過程中，盡管實(shí)驗(yàn)環(huán)境的光線變化與算法的光流估計(jì)誤差等因素導(dǎo)致觀測值中存在噪聲，但其并未對本文提出的慣性光流控制方法的穩(wěn)定性產(chǎn)生影響。

與劉小明等［12］提出的利用光流傳感器獲取光流信息，實(shí)現(xiàn)飛行器地形跟蹤控制的方法相比，本文提出的方法在地形跟蹤精度性能上大約提高±2 cm；在飛行器隨地形爬升與下降過程中的響應(yīng)滯后時(shí)間性能上有提升，時(shí)間減少約176～190 ms。雖然本文沒有使用光流傳感器直接獲取光流，但是前向速度與前向光流的穩(wěn)定性差異并不明顯。

圖5 前向速度與前向光流Fig.5 Forward velocity and forward optical flow

6 結(jié)語

本文應(yīng)用平移慣性光流方法實(shí)現(xiàn)了多旋翼飛行器的復(fù)雜地形跟蹤。實(shí)驗(yàn)結(jié)果表明：各軸向的光流、前向速度與前向光流可以控制在期望值附近，并趨近于期望值；地形相對平緩區(qū)域，高度誤差±3 cm；地形變化明顯區(qū)域，高度誤差±6 cm；k′方向的角度估計(jì)與測試地形大致相同。與類似方法相比，地形跟蹤精度性能大約提高±2 cm；在飛行器隨地形爬升與下降過程中的響應(yīng)滯后時(shí)間性能提升約176～190 ms。

［1］SRINIVASAN M V，ZHANG S W，CHAHL J S，et al. How honeybees make grazing landings on flat surfaces［J］.Biological Cybernetics，2000，83（3）：171-183.

［2］郭利，曹江濤，李平，等.累積方向-數(shù)量級光流梯度直方圖的人體動(dòng)作識別［J］.智能系統(tǒng)學(xué)報(bào)，2014，9（1）：104-108.

［3］吳垠，李良福，肖樟樹，等.基于尺度不變特征的光流法目標(biāo)跟蹤技術(shù)研究［J］.計(jì)算機(jī)工程與應(yīng)用，2013，49（15）：157-161.

［4］史添瑋，王宏.基于光流的六旋翼飛行器懸?？刂疲跩］.東北大學(xué)學(xué)報(bào)：自然科學(xué)版，2015，36（10）：1378-1382.

［5］WANG T，SNOUSSI H.Detection of Abnormal Visual Events via Global Optical Flow Orientation Histogram［J］.IEEE Transactions on Information Forensics and Security，2014，9（6）：988-998.

［6］MARTíNEZ F，MANZANERA A，ROMERO E.Automatic analysis and characterization of the hummingbird wings motion using dense optical flow features［J］.Bioinspiration and Biomimetics，2015，10（1）：016006-016006.

［7］PARRILLA E，ARMENGOT M，MATA M，et al.Primary ciliary dyskinesia assessment by means of optical flow analysis of phase-contrast microscopy images［J］.Computerized Medical Imaging and Graphics，2014，38（3）：163-170.

［8］宮勛，白越，趙常均，等.Hex-rotor無人飛行器及其飛行控制系統(tǒng)設(shè)計(jì)［J］.光學(xué)精密工程，2012，20（11）：2450-2458.

［9］HéRISSé B，HAMEL T，MAHONY R，et al.Landing a VTOL unmanned aerial vehicle on a moving platform using optical flow［J］.IEEE Transactions on Robotics，2012，28（1）：77-89.

［10］李亞楠，趙耀，林春雨，等.基于圖像分割的金字塔lucas-kanade光流法提取深度信息［J］.鐵道學(xué)報(bào)，2015，37（1）：63-68.

［11］METNI N，PFLIMLIN J M，HAMEL T，et al.Attitude and gyro bias estimation for a flying uav［C］//Edmonton，Canada.proceedings of the 2005 IEEE/RSJ International Conference on Intelligent Robots and Systems，2005：1114-1120.

［12］劉小明，陳萬春，邢曉嵐，等光流控制地形跟隨與自動(dòng)著陸［J］.北京航空航天大學(xué)學(xué)報(bào)，2012，38（1）：98-105.

Terrain-following for multi-rotor aircraft based on inertial optical flow

SHI Tianwei1，CUI Wenhua1，REN Ling2
（1.School of International Finance and Bank，University of Science and Technology of Liaoning，Anshan 114051，China；2.Liaoning Julong Financial Equipment Co Ltd，Anshan 114051，China）

To solve the optical flow technique problems of low positioning accuracy and cumbersome processing of multi-camera data in multi-rotor aircraft for the moment，the layer adaptive pyramid Lucas-Kanade algorithm is applied to get the translational inertial optical flow to achieve the stability terrain-following of multi rotor aircraft in this paper.The image of the textured target plane was captured by the bottom facing spherical camera.Simultaneously，the feature points in the target plane were dynamically mapped to the camera lens surface.Then the translational optical flow obtained from the comparison between the two time continuous images was used as feedback of the nonlinear terrain-following controller to achieve the terrain-following and collision avoidance for multi-rotor aircraft.The result of 10 indoor actual flight tests showed that the height accuracies in different changing terrains are±3 cm and±6 cm respectively.And they verified that the nonlinear controller proposed in this paper is stable and feasible.

multi-rotor aircraft；translational inertial optical flow；terrain-following；nonlinear controller

July 3，2016）

V249.1

A

1674-1048（2017）02-0155-06

10.13988/j.ustl.2017.02.014

2016-07-03。

國家自然科學(xué)基金（51405073，61071057）；遼寧省高等學(xué)校創(chuàng)新團(tuán)隊(duì)（LT2014006）。

史添瑋（1982—），男，遼寧鞍山人，講師。