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

王家亮，李樹華，張海濤

（中國民航大學計算機科學與技術學院，天津 300300）

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

0 引言

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

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

1 動力學模型

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

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

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

2 基于貝葉斯估計的視頻圖像分析

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

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

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

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

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

圖3 貝葉斯估計過程Fig.3 Bayesian estimation process

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

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

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

如圖4 所示，基于貝葉斯估計的原理，當得到第一幀圖片的分析結果時，則默認后續(xù)的i-1幀（49幀）圖像的分析結果與第一幀一致，直到圖片幀處理至第i+1 幀時，再次進行障礙分析。

圖4 貝葉斯估計應用于圖像幀分析Fig.4 Bayesian estimation used in image frame analysis

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

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

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

充分利用四軸飛行器傳輸的視頻流數據，將四軸飛行器起飛后的第一幀圖像作為背景，下一幀圖像與此背景進行背景差分來識別障礙物。當四軸飛行器運動到新的坐標點后，將新坐標點處的第一幀圖像作為背景，下一幀圖像與此背景進行背景差分。通過不斷選取新坐標點處的背景，來提高障礙物識別的準確性，進而提高避障的精確度。

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

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

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

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

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

其中：

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

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

4 實驗結果與分析

4.1 軟硬件平臺

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

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

4.2 結果分析

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

圖10 飛行實驗場景Fig.10 Flight experimental scene

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

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

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

圖12 避障實驗結果Fig.12 Experimental result of obstacle avoidance

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

圖13 飛行軌跡Fig.13 Flight trajectories

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

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

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

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

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

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

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

5 結語

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