基于貝葉斯估計(jì)與區(qū)域劃分遍歷的四軸飛行器避障路徑規(guī)劃算法

王家亮，李樹華，張海濤

（中國民航大學(xué)計(jì)算機(jī)科學(xué)與技術(shù)學(xué)院，天津 300300）

（*通信作者電子郵箱lsh3438526746@163.com）

0 引言

隨著科技的發(fā)展，小型無人機(jī)與人們的生活聯(lián)系得越來越緊密，且具有造價低、體積小、使用方便、穩(wěn)定、靈活等特點(diǎn)。小型無人機(jī)中最為常用的是四軸飛行器，具有機(jī)動靈活且能垂直起降的特點(diǎn)，可用來執(zhí)行比較危險(xiǎn)的任務(wù)［1-3］。在抗擊新冠病毒肺炎疫情期間，新浪網(wǎng)2020 年2 月27 日報(bào)道：上海臨港新片區(qū)無人機(jī)出發(fā)戰(zhàn)“疫”，執(zhí)行任務(wù)120 余次、飛行總里程達(dá)1 000 km、提醒佩戴口罩28 人次、發(fā)現(xiàn)人群聚集現(xiàn)象9 次、亂設(shè)攤7 次——這是2 月以來上海自貿(mào)區(qū)臨港新片區(qū)7 架無人機(jī)完成的“戰(zhàn)績”［4］。小型無人機(jī)由于其獨(dú)特的優(yōu)勢，應(yīng)用非常廣泛，如：物流、農(nóng)業(yè)植保、巡檢、安防、救援、測繪、直播、編隊(duì)飛行、未來云端人工智能（Artificial Intelligent，AI）自主飛行等［5］。

國內(nèi)外許多高校以及研究機(jī)構(gòu)均已經(jīng)開展了四軸飛行器的研究工作，如路徑規(guī)劃［6-7］、穩(wěn)態(tài)飛行控制［8］、安全［9］、飛行姿態(tài)調(diào)整［10］、定位［11］、避障［12］、導(dǎo)航［13］、多機(jī)協(xié)同飛行控制［14］等，并取得了顯著的技術(shù)成果。四軸飛行器避障算法的研究主要有兩個方面［15-16］：一是基于A*算法、蟻群算法、人工勢場法等路徑規(guī)劃算法進(jìn)行避障；二是依據(jù)四軸飛行器與障礙物之間的位置關(guān)系，并與飛行速度、姿態(tài)角度（俯仰角、偏航角、滾動角）等信息進(jìn)行融合計(jì)算從而實(shí)現(xiàn)避障。四軸飛行器的避障直接決定了它的自主飛行的能力，對完成飛行任務(wù)至關(guān)重要。區(qū)別于目前現(xiàn)有的利用多傳感器（激光測距儀、雙目攝像頭、雷達(dá)等）進(jìn)行研究，本文只對單一傳感器（單目攝像頭）采集的視頻圖像信息進(jìn)行研究，基于貝葉斯估計(jì)與區(qū)域劃分遍歷，旨在實(shí)現(xiàn)四軸飛行器高實(shí)時性避障路徑規(guī)劃算法。

1 動力學(xué)模型

四軸飛行器是一種能夠垂直起降的非共軸的多旋翼飛行器，同一對角線上的一組旋翼采用逆時針旋轉(zhuǎn)，另一組旋翼采用順時針旋轉(zhuǎn)，相互抵消了旋翼旋轉(zhuǎn)帶來的反扭矩力。通過調(diào)節(jié)對稱分布的四個旋翼轉(zhuǎn)速，可以改變四軸飛行器的位置，從而實(shí)現(xiàn)對飛行姿態(tài)的控制，比如懸停、俯仰、偏航和滾動等。根據(jù)四軸運(yùn)動方式的不同，可將四軸飛行器分為“+”型（如圖1（a）所示）和“×”型（如圖1（b）所示），本文采用的“×”型結(jié)構(gòu)。

圖1 四軸飛行器動力學(xué)模型Fig.1 Dynamic model of quad-rotor helicopter

四軸飛行器的三軸角度反映了它相對于地面的運(yùn)動狀態(tài)，其中：繞X軸旋轉(zhuǎn)的Ф角為俯仰角；繞Y軸旋轉(zhuǎn)的θ角為偏航角；繞Z軸旋轉(zhuǎn)的Ψ角為滾動角。對障礙物的避開路徑規(guī)劃，主要是通過改變四軸飛行器的俯仰角、偏航角、滾動角，使四軸飛行器產(chǎn)生不同的飛行姿態(tài)從而實(shí)現(xiàn)自主避障飛行。

2 基于貝葉斯估計(jì)的視頻圖像分析

對四軸飛行器所處三維環(huán)境進(jìn)行物理建模如圖2 所示，以四軸飛行器所在的坐標(biāo)系（xoy）為基準(zhǔn)將空間分為無限個離散點(diǎn)。按照圖中ABCD方向進(jìn)行圖片幀的劃分，并依據(jù)后續(xù)的圖像分析將這些離散點(diǎn)區(qū)分為障礙點(diǎn)和非障礙點(diǎn)。

根據(jù)四軸飛行器攝像頭沿著飛行方向的掃描區(qū)域可以看出acdb所在區(qū)域的內(nèi)部均為障礙點(diǎn)，而掃描區(qū)域內(nèi)的其他點(diǎn)則為非障礙點(diǎn)；同時，非掃描區(qū)域的障礙物eghf區(qū)域內(nèi)部也是障礙點(diǎn)。

圖2 四軸飛行器飛行環(huán)境模型Fig.2 Flight environment model of quad-rotor helicopter

很多情況下，在進(jìn)行障礙物檢測時，需要對每一幀圖像均進(jìn)行障礙物的檢測，從而使四軸飛行器可以更加精準(zhǔn)地進(jìn)行避障，因此四軸飛行器在飛行過程中處理數(shù)據(jù)的時效性是一個很重要的指標(biāo)。貝葉斯估計(jì)提供了一種計(jì)算假設(shè)概率的方法，是基于假設(shè)的先驗(yàn)概率（將待估計(jì)的參數(shù)看作符合某種先驗(yàn)概率分布的隨機(jī)變量）、給定假設(shè)下觀察到不同數(shù)據(jù)的概率以及觀察到的數(shù)據(jù)本身，因此考慮將貝葉斯估計(jì)運(yùn)用到障礙物的檢測中。

貝葉斯估計(jì)的過程如圖3 所示：首先將待估計(jì)參數(shù)的概率默認(rèn)為一個假設(shè)概率，即先驗(yàn)概率p(θ)；再求出該待估計(jì)參數(shù)依據(jù)樣本集D的分布函數(shù)；之后利用貝葉斯公式計(jì)算出待估計(jì)參數(shù)的后驗(yàn)分布，進(jìn)而計(jì)算出貝葉斯估計(jì)值。

圖3 貝葉斯估計(jì)過程Fig.3 Bayesian estimation process

貝葉斯估計(jì)的本質(zhì)是通過貝葉斯決策得到參數(shù)θ的最優(yōu)估計(jì)，并使總期望風(fēng)險(xiǎn)最小。定義在樣本X下的條件風(fēng)險(xiǎn)為：

利用四軸飛行器傳輸?shù)囊曨l流進(jìn)行障礙檢測，對于障礙物的分析采用背景差法，需要對四軸飛行器傳輸?shù)膱D像進(jìn)行逐幀分析做差分，進(jìn)而識別出障礙物。由于視頻流傳輸速度較快，每秒傳輸30 個圖像幀，若逐幀對所有圖片都進(jìn)行分析會耗費(fèi)大量的時間進(jìn)而降低四軸飛行器避障的實(shí)時性。根據(jù)以上公式中對貝葉斯估計(jì)得出的最優(yōu)估計(jì)，可有效進(jìn)行四軸飛行器采集圖像后的視頻預(yù)處理。

應(yīng)用貝葉斯估計(jì)的思想，首先對一幀圖像進(jìn)行障礙物分析。得出分析結(jié)果后，由貝葉斯估計(jì)可以得知，在后續(xù)的圖片幀中，有一定數(shù)目的圖片幀的處理結(jié)果和該圖片大概率是一致的，所以跳過這些圖片，減少識別圖片的數(shù)目，進(jìn)而減少識別時間。雖然在四軸飛行器運(yùn)行過程中有小概率會有障礙物突然出現(xiàn)，而出現(xiàn)的時間恰好處在檢測障礙物圖片幀中間，導(dǎo)致該障礙物未被識別出，但是根據(jù)實(shí)際情況，突然出現(xiàn)的障礙物基本會存在兩種可能：一是障礙物一閃即逝，并不會影響到避障進(jìn)行；另一種則是障礙物出現(xiàn)時間超過1 s，則在后續(xù)的識別中也能被檢測出來，并不會影響避障進(jìn)程。

如圖4 所示，基于貝葉斯估計(jì)的原理，當(dāng)?shù)玫降谝粠瑘D片的分析結(jié)果時，則默認(rèn)后續(xù)的i-1幀（49幀）圖像的分析結(jié)果與第一幀一致，直到圖片幀處理至第i+1 幀時，再次進(jìn)行障礙分析。

圖4 貝葉斯估計(jì)應(yīng)用于圖像幀分析Fig.4 Bayesian estimation used in image frame analysis

3 基于區(qū)域劃分的像素點(diǎn)遍歷算法

障礙物識別流程如圖5 所示：在接收四軸飛行器傳輸?shù)囊曨l流后，對每一幀圖像進(jìn)行灰度化以及高斯濾波的處理；然后利用背景差法檢測出障礙物，并將障礙物用填充色標(biāo)記出來，得到識別出障礙物的圖像信息。

圖5 障礙物識別流程Fig.5 Obstacle identification process

充分利用四軸飛行器傳輸?shù)囊曨l流數(shù)據(jù)，將四軸飛行器起飛后的第一幀圖像作為背景，下一幀圖像與此背景進(jìn)行背景差分來識別障礙物。當(dāng)四軸飛行器運(yùn)動到新的坐標(biāo)點(diǎn)后，將新坐標(biāo)點(diǎn)處的第一幀圖像作為背景，下一幀圖像與此背景進(jìn)行背景差分。通過不斷選取新坐標(biāo)點(diǎn)處的背景，來提高障礙物識別的準(zhǔn)確性，進(jìn)而提高避障的精確度。

將分析出障礙物的圖像（像素點(diǎn)個數(shù)：480×640）劃分為50 個區(qū)域（5×10），如圖6 所示。四軸飛行器當(dāng)前的起飛位置位于圖中的Φ區(qū)域，對該區(qū)域的像素點(diǎn)進(jìn)行遍歷，當(dāng)發(fā)現(xiàn)存在標(biāo)記的像素點(diǎn)時（在障礙物檢測階段，對檢測出障礙物的像素點(diǎn)進(jìn)行標(biāo)記），默認(rèn)該區(qū)域四軸飛行器不可通過。之后按照圖中1→49 的區(qū)域編號順序依次進(jìn)行像素點(diǎn)的遍歷，直到發(fā)現(xiàn)某一區(qū)域無標(biāo)記的像素點(diǎn)，表明該區(qū)域可通過，四軸飛行器下一次運(yùn)動的目標(biāo)點(diǎn)坐標(biāo)即為該區(qū)域的中心點(diǎn)。例如，四軸飛行器檢測到當(dāng)前區(qū)域（Ф）存在障礙點(diǎn)無法通過時，會掃描1號區(qū)域，若1 號區(qū)域仍舊存在障礙點(diǎn)，繼續(xù)掃描2 號區(qū)域。若2號區(qū)域只存在非障礙點(diǎn)，則將該區(qū)域的中心點(diǎn)坐標(biāo)轉(zhuǎn)換后發(fā)送給四軸飛行器，使其進(jìn)行下一步動作，若2 號區(qū)域仍舊不可通過，則繼續(xù)向下掃描直至發(fā)現(xiàn)可通過區(qū)域。當(dāng)掃描到49 區(qū)域仍為不可通過時，提示“NoWays！”，四軸飛行器降落。算法的執(zhí)行過程如圖7所示。

圖6 圖像幀劃分為50個區(qū)域Fig.6 Image frame divided into 50 regions

圖7 基于區(qū)域劃分的像素點(diǎn)遍歷算法流程Fig.7 Flowchart of pixel traversal algorithm based on region division

由于四軸飛行器所處坐標(biāo)系和圖像像素點(diǎn)處理的坐標(biāo)系不同，所以在獲得可通過區(qū)域的中心點(diǎn)坐標(biāo)后，需要將該坐標(biāo)轉(zhuǎn)化為四軸飛行器所處坐標(biāo)系的坐標(biāo)。四軸飛行器根據(jù)轉(zhuǎn)化后的坐標(biāo)進(jìn)行相應(yīng)的移動，圖像所處坐標(biāo)系（xoy）以及四軸飛行器所處坐標(biāo)系（XYZ）的關(guān)系如圖8所示。其中：圖片像素個數(shù)為480 × 640；height為所選區(qū)域上邊界距離圖片上邊界的距離；width為所選區(qū)域左邊界距離圖片左邊界的距離。

將可通過區(qū)域的中心坐標(biāo)轉(zhuǎn)化為四軸飛行器所在坐標(biāo)系的坐標(biāo)，轉(zhuǎn)化后的坐標(biāo)為：

其中：

若遍歷所有區(qū)域后均未發(fā)現(xiàn)可通過區(qū)域，即“無路可飛”，提示“Noways！”，四軸飛行器自動降落。

圖8 圖像坐標(biāo)系以及四軸飛行器坐標(biāo)系Fig.8 Coordinate system of quad-rotor helicopter and coordinate system of image

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

4.1 軟硬件平臺

軟件平臺為python2.7開發(fā)環(huán)境，硬件平臺為小型四軸飛行器Tello，如圖9 所示。Tello 飛行器是睿熾科技自主研發(fā)的第一款無人機(jī)產(chǎn)品，其飛行控制系統(tǒng)由深圳市大疆創(chuàng)新公司授權(quán)使用，同時還搭載了英特爾高性能處理器。Tello 支持拋飛、全向翻滾等功能，具備紅外定高、氣壓計(jì)定高、下視視覺等功能，攜帶500萬像素?cái)z像頭，并可以進(jìn)行720P高清圖傳。

圖9 小型四軸飛行器TelloFig.9 Small quad-rotor helicopter Tello

4.2 結(jié)果分析

為了驗(yàn)證所提出的基于貝葉斯估計(jì)的圖像預(yù)處理，以及基于區(qū)域劃分的像素點(diǎn)遍歷算法的有效性，在室內(nèi)環(huán)境布置如圖10所示的測試環(huán)境。

圖10 飛行實(shí)驗(yàn)場景Fig.10 Flight experimental scene

四軸飛行器起飛后進(jìn)入避障階段，其飛行場景以及圖像處理結(jié)果如圖11 所示。其中：圖11（a）為四軸飛行器起飛時所在的實(shí)際場景；圖11（b）為四軸飛行器在執(zhí)行飛行實(shí)驗(yàn)時其攝像頭采集到的視頻幀；圖11（c）為系統(tǒng)終端顯示的障礙物的識別結(jié)果，其中多個實(shí)心矩形框覆蓋區(qū)域?yàn)樽R別出的障礙物區(qū)域。

當(dāng)四軸飛行器避開第一組障礙物時，實(shí)驗(yàn)結(jié)果如圖12 所示。其中：圖12（a）為四軸飛行器避開第一組障礙物時的實(shí)景；圖12（b）為四軸飛行器在執(zhí)行飛行實(shí)驗(yàn)時其攝像頭采集到的視頻幀；圖12（c）為系統(tǒng)終端顯示的障礙物檢測結(jié)果，其中所有的實(shí)心矩形框覆蓋區(qū)域?yàn)樽R別出的障礙物區(qū)域。

圖11 四軸飛行器實(shí)時飛行場景以及圖像處理結(jié)果Fig.11 Real-time flight scene and image processing result of quad-rotor helicopter

圖12 避障實(shí)驗(yàn)結(jié)果Fig.12 Experimental result of obstacle avoidance

四軸飛行器識別出障礙物之后，依據(jù)區(qū)域劃分的像素點(diǎn)遍歷算法將圖片幀劃分為50 個區(qū)域，按照圖6 所示的順序?qū)γ恳粋€區(qū)域進(jìn)行像素點(diǎn)的掃描。當(dāng)遇到某一區(qū)域無填充像素時，終止掃描，將該區(qū)域的中心坐標(biāo)點(diǎn)作為四軸飛行器下次運(yùn)動的目的坐標(biāo)；當(dāng)四軸飛行器接收到降落命令或者“NoWays！”指令時，將四軸飛行器運(yùn)行過程中的坐標(biāo)點(diǎn)繪制在三維坐標(biāo)圖中，如圖13所示。其中圖13（a）和（b）為四軸飛行器在不同起飛位置進(jìn)行實(shí)驗(yàn)，所實(shí)現(xiàn)的四軸飛行器避障過程的軌跡呈現(xiàn)。

圖13 飛行軌跡Fig.13 Flight trajectories

圖14 中實(shí)線路徑為四軸飛行器實(shí)際飛行軌跡，虛線路徑為理想的飛行軌跡，理想軌跡為綜合了當(dāng)時環(huán)境中存在的所有障礙情況及四軸飛行器的所在位置、相關(guān)距離等信息而生成的軌跡。兩條軌跡中存在兩處偏移：第一處偏移為理想軌跡判定該位置距離第二組障礙物的距離較近，應(yīng)先向左上方避開，再直行，而實(shí)際運(yùn)行軌跡中，四軸飛行器的攝像頭未探測機(jī)體右側(cè)信息，選擇直行；第二處偏移為理想軌跡認(rèn)為四軸飛行器正前方的床架可以直接通過，而實(shí)際運(yùn)行軌跡中將床架檢測為障礙物，選擇向右上方偏移以避開此處。實(shí)際軌跡與理想軌跡產(chǎn)生偏差，一方面的原因是障礙物的識別精度問題，另一方面是由于四軸飛行器只依靠攝像頭采集到的圖像進(jìn)行避障，無法有效采集到環(huán)境的深度信息，所以會將遠(yuǎn)方障礙物（無須避讓）仍然視為避障目標(biāo)，從而導(dǎo)致軌跡發(fā)生偏移。

分析四軸飛行器實(shí)驗(yàn)中的兩組數(shù)據(jù)（理想軌跡與實(shí)際飛行軌跡坐標(biāo)點(diǎn)間的距離，以像素點(diǎn)為單位），理想軌跡與實(shí)際飛行軌跡這兩條軌跡的最大距離為25.6 cm，最小距離為0.2 cm。為了計(jì)算兩條軌跡的擬合度，計(jì)算得到的方差值為194.61個像素點(diǎn)。由于圖像中的點(diǎn)每偏移10個像素點(diǎn)，實(shí)際飛行器會偏移1 cm，所以實(shí)際飛行軌跡與理想軌跡之間的距離方差對應(yīng)為19.461 cm。實(shí)驗(yàn)結(jié)果表明，本文提出的算法具有較好的效果。

圖14 飛行軌跡與理想軌跡對比圖Fig.14 Comparison of flight trajectory and ideal trajectory

在實(shí)驗(yàn)過程中，四軸飛行器飛行了24 s，每秒鐘傳輸30幀圖片，則總共傳輸圖片720 幀，利用提出的基于貝葉斯估計(jì)的四軸飛行器避障算法進(jìn)行取樣，該實(shí)驗(yàn)過程中，總計(jì)處理圖片14幀，處理時間為0.229 3 s。若未使用貝葉斯估計(jì)原理，則處理圖片720 幀，需要處理時間11.464 3 s。處理不同數(shù)目的圖片幀，圖像預(yù)處理的時間差別非常大。圖15 為處理每一幀圖片所需時間；圖16 為基于貝葉斯估計(jì)的圖像預(yù)處理以及原算法（對所有視頻幀進(jìn)行處理）在處理不同數(shù)目的圖片幀時的圖片預(yù)處理時間對比。

由圖15、16 可以看出，基于貝葉斯估計(jì)的圖像預(yù)處理可有效節(jié)省圖像幀處理時間，從而提升四軸飛行器的避障實(shí)時性，為四軸飛行器的實(shí)時自主避障飛行提供了保障。

圖15 圖片幀預(yù)處理執(zhí)行時間測試Fig.15 Preprocessing time test of image frame

圖16 兩種算法處理不同數(shù)目的圖片幀的效率對比Fig.16 Efficiency comparison of two algorithms for processing different numbers of image frames

5 結(jié)語

本文充分利用四軸飛行器所攜帶的攝像頭，將其采集到的圖像信息作為避障的依據(jù)，將視頻流分解為一幀幀的圖片后，對圖片幀進(jìn)行灰度化以及高斯濾波的處理，進(jìn)而利用背景差分的方法來識別障礙物。背景差分中背景圖的選取方式為四軸飛行器運(yùn)動到新坐標(biāo)點(diǎn)后的第一幀圖像作為新的背景圖，從而提高四軸飛行器識別障礙物的準(zhǔn)確性。其間充分利用貝葉斯估計(jì)的原理簡化圖像預(yù)處理的過程，從而提高四軸飛行器避障的實(shí)時性。同時，將識別出障礙物的圖片進(jìn)行區(qū)域劃分，利用像素點(diǎn)遍歷的方式來判斷某一區(qū)域是否存在障礙物：若不存在障礙物則將該區(qū)域的中心點(diǎn)坐標(biāo)轉(zhuǎn)化后發(fā)送給四軸飛行器；若存在障礙物則繼續(xù)掃描下一個區(qū)域。掃描完50個區(qū)域仍未發(fā)現(xiàn)可通過區(qū)域時，則提示“NoWays！”，四軸飛行器降落，此次避障結(jié)束。四軸飛行器完成避障過程后，將四軸飛行器運(yùn)動過程中的坐標(biāo)點(diǎn)繪制在三維坐標(biāo)系中，進(jìn)行四軸飛行器避障軌跡的三維呈現(xiàn)。下一步考慮綜合利用激光測距儀、雙目攝像頭、雷達(dá)等額外的傳感器進(jìn)行四軸飛行器的避障及路徑規(guī)劃。