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

余超凡 孫建輝

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

0 引 言

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

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

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

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

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

1 光流法

1.1 光流算法

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

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

(1)

(2)

令：

(3)

(4)

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

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

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

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

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

Xk+1=AXk+αk

(5)

Yk=dmCXk+βk

(6)

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

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

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

(7)

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

(8)

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

(9)

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

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

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

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

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

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

2 基于光流的改進(jìn)的人工勢(shì)場(chǎng)法及控制算法

2.1 雙目光流避障策略

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

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

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

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

2.2 人工勢(shì)場(chǎng)法的原理

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

(10)

(11)

其中，katt、katt——引力、斥力場(chǎng)比例系數(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決定了飛行器的運(yùn)動(dòng)方向。

2.3 改進(jìn)后的人工勢(shì)場(chǎng)法

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

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

(14)

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

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

(15)

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

φ=αVor

(16)

因此改進(jìn)后的人工引力、斥力場(chǎng)分別如式(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è)計(jì)

飛行器控制系統(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)通過(guò)慣性測(cè)量組合元件(IMU)來(lái)獲取, 主要包括三個(gè)MEMS陀螺儀和三軸加速度傳感器[10]。由于四旋翼飛行器的斥力可以作為其姿態(tài)環(huán)的參考指令，所以可以將其輸入內(nèi)環(huán)進(jìn)行姿態(tài)環(huán)的閉環(huán)控制。通過(guò)串級(jí)PID控制器控制飛行器的角度環(huán)和角速度環(huán)。本文中姿態(tài)環(huán)的控制器參數(shù)如表1所示，其中姿態(tài)環(huán)控制模型設(shè)計(jì)如圖6所示。

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

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

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

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

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

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

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

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

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

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

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

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

5 結(jié) 語(yǔ)

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

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

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

[3] 宋宇,翁新武,郭昕剛.基于光流和慣性導(dǎo)航的小型無(wú)人機(jī)定位方法[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)人工勢(shì)場(chǎng)的蘋果采摘機(jī)器人機(jī)械手避障方法[J].農(nóng)業(yè)機(jī)械學(xué)報(bào),2013,44(11):253-259.

[7] 鄭來(lái)芳,孫煒,歐陽(yáng)明華,等.結(jié)合光流和人工勢(shì)場(chǎng)的風(fēng)管機(jī)器人避障方法[J].計(jì)算機(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] 肖雪,秦貴和,陳筠翰.基于光流的自主移動(dòng)機(jī)器人避障系統(tǒng)[J].計(jì)算機(jī)工程,2013,39(10):305-308.

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