基于光流傳感器的旋翼無人機實時避障系統(tǒng)

余超凡 孫建輝

(浙江工業(yè)大學(xué)機械工程學(xué)院 浙江 杭州 310000)

0 引 言

近年來，在民用領(lǐng)域中，無人機發(fā)揮著越來越重要的作用。無人機因其便于使用與功能強大等特點，已從昔日的玩具升格為多用途的科技產(chǎn)品。但是，無人機卻總會存在一系列的安全性問題。因此，無人機想要持續(xù)發(fā)展，除了其續(xù)航與功能的不斷完善外，還需考慮其安全性能。"避障系統(tǒng)"目前是提升無人機安全性能必不可少的一個環(huán)節(jié),也是無人機智能化的一個顯著標(biāo)志。目前可以使用聲吶、紅外、激光測距與計算機視等方法檢測障礙物。

(1) 基于超聲波傳感器的避障策略 超聲波傳感器廣泛的應(yīng)用于許多場,如測距儀、倒車?yán)走_等。但是，超聲波為機械波，若在多旋翼飛行器上使用，極易受到干擾，從而導(dǎo)致測量精度不高，甚至產(chǎn)生大量野值點的問題。

(2) 基于激光雷達的避障策略 該方法采用激光雷達傳感器獲取飛行器周邊的環(huán)境信息，可以獲得飛行器與障礙物的相對距離、角度等信息。其優(yōu)點是精度高，但是價格較高且極易受到強光干擾。

(3) 基于視覺傳感器的避障策略 該方法可以通過兩個光流傳感器獲取豐富的圖像信息，且精度高，測量束角大，但是當(dāng)障礙物位于兩個光流傳感器的正中間時，飛行器檢測到的兩側(cè)的光流值便會相等，該方法便會失效。

因此，本文采用一個光流傳感器實時獲取飛行器當(dāng)前的環(huán)境信息后，通過改進后的人工勢場法來實現(xiàn)飛行器自主避障的實時路徑規(guī)劃。再與飛行器的飛行控制系統(tǒng)綜合完成旋翼飛行器的自主避障飛行。

1 光流法

1.1 光流算法

本文在兩幅連續(xù)幀之間采用SAD(sum of absolute differences)算法計算光流。將兩幅圖像的SAD值后與給定的SAD值進行比較，提取出最匹配的圖像，從而得出兩幅圖像之間的光流值。在兩幅最匹配的圖像之間采用提出的雙線性插值法I2A(interpolation algorithm)在亞像素精度上計算最終的光流值。

I2A算法在一個較小的時間差Δt內(nèi)計算兩幅連續(xù)的圖像的位移s。比率s/Δt(pixel/s)與實際的光流(°/s)成正比。

(1)

(2)

令：

(3)

(4)

在本文中,將k設(shè)為半幀的大小，從而改善光流法的精度。

1.2 傳感器數(shù)據(jù)融合

本質(zhì)上,通過添加角速度信息,SFM(structure from motion)算法的性能會得到改善。在文獻[3]中,提出IMU信息可以有效地提高運動物體追蹤的抗干擾能力。他們同樣展示了當(dāng)獲得IMU數(shù)據(jù)時,只要少量的特征點信息便可很好的恢復(fù)相機的運動情況。

飛行器的平動和轉(zhuǎn)動都會產(chǎn)生光流向量，根據(jù)文獻[4]可知,深度信息只與光流的平動部分有關(guān)。因此，本文將使用KF融合光流數(shù)據(jù)與IMU數(shù)據(jù)的策略來預(yù)測實際因飛行器平動而產(chǎn)生的光流值和飛行器的角速度信息,從而提高飛行器的避障性能和魯棒性。

本文中卡爾曼濾波器的狀態(tài)向量為X=(dx,dy)T∈R2,其動力學(xué)模型可以描述為：

Xk+1=AXk+αk

(5)

Yk=dmCXk+βk

(6)

此處,αk是狀態(tài)噪聲向量,預(yù)測的誤差協(xié)方差矩陣為Qα。Yk∈R2是測量向量,QB是測量誤差的協(xié)方差矩陣。A∈R2,C∈R2,兩個都是單位矩陣。

圖1 點Q=(X,Y,Z)由通過投影中心的光線投影到圖像平面上,相應(yīng)的圖像點為q=(x,y,z)

如圖1所示，點Q(X,Y)由通過投影中心的光線投影到圖像平面上，相應(yīng)的圖像點為q(x,y)。幾何轉(zhuǎn)換方程為：

(7)

通過對式(7)求導(dǎo)操作，我們可以通過圖像坐標(biāo)系(xi,yi),機體速度、角速度(Vx,Vy,Vz,Ωx,Ωy,Ωz)與深度信息Zi來表達光流的大小[5]：

(8)

式(8)為卡爾曼濾波器的動力學(xué)模型，通過式(8),得出測量方程：

(9)

使用該融合方法對px4flow光流傳感器進行二次開發(fā)，最后，圖像傳感器得到的數(shù)據(jù)通過串口發(fā)送給上位機進行顯示，分別如圖2-圖4所示。

圖2 融合處理后因旋轉(zhuǎn)產(chǎn)生的光流值

圖3 圖2中去掉旋轉(zhuǎn)部分

圖4 融合處理后因平動產(chǎn)生的光流

其中，圖2中的波動較大的曲線的為角速度，單位為rad/s，圖2波動小的曲線、圖3的曲線和圖4中的曲線為光流值，單位為m/s,三幅圖的橫坐標(biāo)都為當(dāng)?shù)貢r間。

由于飛行器pitch和roll方向最大轉(zhuǎn)動角度限制在±35°，圖2為將光流傳感器來回旋轉(zhuǎn)一定角度(±30°)產(chǎn)生的角速度與光流值信息。圖3表明因旋轉(zhuǎn)產(chǎn)生的光流值，該值較小。圖4為光流傳感器平動產(chǎn)生的光流值，該值較大。因此，圖2-圖4表明使用KF將IMU數(shù)據(jù)與光流值進行融合，可以有效過濾掉部分因旋轉(zhuǎn)產(chǎn)生的光流值，從而能夠預(yù)測實際因飛行器平動而產(chǎn)生的光流值，因此可以作為障礙物的檢測方法。

2 基于光流的改進的人工勢場法及控制算法

2.1 雙目光流避障策略

當(dāng)無人機進入避障模式后,可以將無人機從左右兩側(cè)的圖像傳感器獲得的光流值取絕對值,再將這兩個絕對值相減做差,從而決定無人機的為躲避障礙物機身需傾斜的方向：

光流絕對值差=|左側(cè)光流|-|右側(cè)光流|

假設(shè)當(dāng)障礙物靠近飛行器左側(cè)時為正，反之為負(fù)。因此，當(dāng)無人機兩側(cè)的光流值的絕對值的差為正時,表明障礙物靠近飛行器左側(cè),無人機機身需向右傾斜一定角度來躲避障礙物。反之較靠近右側(cè),無人機機身需向左傾斜一定角度來躲避障礙物。

但是，雖然該避障策略對于飛行器避障有較好的效果，但其不能同時考慮目標(biāo)的位置。這樣沒有目的性的導(dǎo)航會使飛行器在避障后的航線與初始航向產(chǎn)生較大的偏離。

2.2 人工勢場法的原理

目標(biāo)點對飛行器有引力作用，距離越遠，引力越大：障礙物對飛行器有斥力作用，距離越近，斥力越大。因此，可得飛行器的引力、斥力勢[6]：

(10)

(11)

其中，katt、katt——引力、斥力場比例系數(shù)；

X——飛行器當(dāng)前位置；

Xtar——目標(biāo)位置；

d——飛行器與障礙物之間的距離；

d0——障礙物的作用半徑。

由式(10)可得引力函數(shù)：

Fatt(d)=-▽Uatt(d)=-katt|X-Xtar|

(12)

斥力函數(shù)為：

Frep(d)= -▽Urep(d)=

(13)

飛行器收到的合力F=Fatt+Frep決定了飛行器的運動方向。

2.3 改進后的人工勢場法

由于傳統(tǒng)人工勢場有局部極小點問題，即在某些特殊位置的障礙物會使飛行器所受合力為0，此時飛行器將不能到達指定目的地而在中途停止；需要獲取較詳細的障礙物信息[7]。因此需要對傳統(tǒng)的勢場法進行改進。

飛行器人工勢場法的避障是通過調(diào)整無人機的姿態(tài)角度來實現(xiàn)的，但四旋翼飛行器pitch、roll和yaw存在耦合性，其橫滾角、俯仰角和偏航角的關(guān)系[8]：

(14)

其中，q,r為機體角速度ω在機體坐標(biāo)系oy,oz上的分量。φ，θ，ψ分別為橫滾角、俯仰角、偏航角。

由于飛行器在實際避障過程中只調(diào)整橫滾角φ，而不調(diào)整俯仰角θ，因此可以令θ=0，則式(14)可改寫為：

(15)

由光流值引起的橫滾角φ的變化：

φ=αVor

(16)

因此改進后的人工引力、斥力場分別如式(17)、式(18)所示：

(17)

Urep(ψ)=krep(-qcosφ+rsinφ)=

krep(-qcos(αVor)+rsin(αVor))

(18)

其中，ψtar為飛行器的初始偏航角，Vor為光流值的大小，α為光流系數(shù)，由式(17)、式(18)可得引力、斥力函數(shù)：

Fatt(ψ)=-▽Uatt(ψ)=-katt|ψ-ψtar|

(19)

Frep(V)=-▽Urep(V)=krep(qsin(αVor)+rcos(αVor))

(20)

3 飛行控制算法設(shè)計

飛行器控制系統(tǒng)采用內(nèi)外環(huán)的控制結(jié)構(gòu)。外環(huán)為位置控制，內(nèi)環(huán)為姿態(tài)控制。本文主要介紹內(nèi)環(huán)(姿態(tài))控制算法。其中，四旋翼飛行器的飛行姿態(tài)通過慣性測量組合元件(IMU)來獲取, 主要包括三個MEMS陀螺儀和三軸加速度傳感器[10]。由于四旋翼飛行器的斥力可以作為其姿態(tài)環(huán)的參考指令，所以可以將其輸入內(nèi)環(huán)進行姿態(tài)環(huán)的閉環(huán)控制。通過串級PID控制器控制飛行器的角度環(huán)和角速度環(huán)。本文中姿態(tài)環(huán)的控制器參數(shù)如表1所示，其中姿態(tài)環(huán)控制模型設(shè)計如圖6所示。

表1 PID控制器參數(shù)

圖6 無人機姿態(tài)環(huán)控制模型

4 飛行實驗結(jié)果及分析

本文實驗平臺為450 cm軸距的四旋翼飛行器，最大飛行速度10 m/s，姿態(tài)測試的實驗環(huán)境為室外，懸停高度2 m，通過地面站mission planer進行實時顯示。姿態(tài)曲線如圖7所示。

圖7 飛行姿態(tài)曲線

其中各曲線分別為：pitch方向的期望姿態(tài)；pitch方向的實際姿態(tài)；roll方向的期望姿態(tài)；roll方向的實際姿態(tài)曲線?？v坐標(biāo)為角度，橫坐標(biāo)為時間。

如圖7所示，飛行器的姿態(tài)控制使用串級PID控制器，再通過整定合適的PID參數(shù)，其在室外環(huán)境下的pitch、roll方向都能獲得理想的姿態(tài)響應(yīng)曲線，可以作為避障的基礎(chǔ)，并且可以用于植保作業(yè)或者航拍等需要較好姿態(tài)響應(yīng)的場合。

在滿足飛行器pitch和roll方向姿態(tài)的要求后，需對其避障性能進行實驗。將光流傳感器裝于飛行器x方向的正中間，飛行器以一定速度向前直線飛行，在前方設(shè)置一個障礙物，如果與障礙物產(chǎn)生的光流值的絕對值≥0.5 m/s，飛行器開始執(zhí)行避障任務(wù)；在將光流值與改進的人工勢場法相結(jié)合時，需要確定出人工勢場的引力、斥力場比例系數(shù)。經(jīng)過大量的實際測試，本文中的引力、斥力場比例系數(shù)分別為1.8與4.0。飛行器執(zhí)行避障任務(wù)時的測試曲線如圖8、圖9所示。

圖8 飛行器執(zhí)行避障任務(wù)時的測試曲線圖

圖9 圖8中光流值的放大圖

其中，避障測試實驗中使用匿名地面站作為上位機對飛行器的期望、實際偏航角與光流值進行實時顯示；圖8中的曲線分別為飛行器避障過程中的期望偏航角與實際偏航角，單位都為°；圖9中的為飛行器避障過程中產(chǎn)生的光流值，單位為m/s；橫坐標(biāo)為時間，單位為ms。

如圖8所示，使用結(jié)合光流的人工勢場法時，飛行器在避開障礙物之后可以繼續(xù)以期望偏航角方向保持直線飛行，從而達到局部路徑規(guī)劃的目的。

5 結(jié) 語

本文采用光流傳感器和改進的勢場法設(shè)計了基于光流傳感器的旋翼無人機實時避障系統(tǒng)。光流傳感器獲取當(dāng)前飛行環(huán)境中與障礙物相對運動產(chǎn)生的光流值。改進的勢場法則可以實時地對飛行器最優(yōu)飛行路徑進行規(guī)劃，這樣飛行器既可以躲避障礙物又不會迷失方向。該實時避障系統(tǒng)與旋翼飛行器的飛行控制系統(tǒng)結(jié)合成功地實現(xiàn)了旋翼飛行器的自主避障飛行。本文的避障策略在軸距450 cm的四旋翼飛行器上得到了很好的驗證。結(jié)果表明在速度不太高的場合下，表現(xiàn)性能良好，可以有效地對移動的障礙物進行規(guī)避，并能在避障后保持初始的期望偏航角，使飛行器能夠更順利地完成路徑規(guī)劃的任務(wù)。

[1] 趙海,陳星池,王家亮,等.基于四軸飛行器的單目視覺避障算法[J].光學(xué)精密工程,2014,22(8):2232-2241.

[2] 蔡卓凡.基于多超聲波傳感器避障機器人小車的設(shè)計[J].自動化技術(shù)與應(yīng)用,2014,33(5):85-89.

[3] 宋宇,翁新武,郭昕剛.基于光流和慣性導(dǎo)航的小型無人機定位方法[J].傳感器與微系統(tǒng),2015,34(1):13-16.

[4] Qian G,Chellappa R,Zheng Q.Robust structure from motion estimation using inertial data[J].Journal of the Optical Society of America A Optics Image Science & Vision,2001,18(12):2982-2997.

[5] Motai Y,Jha S K,Kruse D.Human tracking from a mobile agent: Optical flow and Kalman filter arbitration[J].Signal Processing:Image Communication,2012,27(1):83-95.

[6] 姬偉,程風(fēng)儀,趙德安,等.基于改進人工勢場的蘋果采摘機器人機械手避障方法[J].農(nóng)業(yè)機械學(xué)報,2013,44(11):253-259.

[7] 鄭來芳,孫煒,歐陽明華,等.結(jié)合光流和人工勢場的風(fēng)管機器人避障方法[J].計算機工程與應(yīng)用,2016,52(9):243-247.

[8] 李杰,齊曉慧,韓帥濤.基于自抗擾技術(shù)的四旋翼姿態(tài)解耦控制方法[J].電光與控制,2013,20(3):44-48.

[9] Konolige K,Augenbraun J,Donaldson N,et al.A low-cost laser distance sensor[C]//IEEE International Conference on Robotics and Automation.IEEE,2008:3002-3008.

[10] Souhila K,Karim A.Optical Flow based Robot Obstacle Avoidance[J].International Journal of Advanced Robotic Systems,2008,4(1):13-16.

[11] 肖雪,秦貴和,陳筠翰.基于光流的自主移動機器人避障系統(tǒng)[J].計算機工程,2013,39(10):305-308.

[12] 王銘,王瑞,李曉娟,等.非確定性環(huán)境中移動機器人實時避障的概率模型檢測[J].小型微型計算機系統(tǒng),2014,35(9):2104-2109.