基于向量場(chǎng)直方圖的無(wú)人機(jī)動(dòng)態(tài)避障算法的應(yīng)用研究

徐靜　苑征　梁寅博　甄澤安

關(guān)鍵詞：向量場(chǎng)直方圖；動(dòng)態(tài)避障；速度規(guī)劃；無(wú)人機(jī)；局部路徑規(guī)劃

中圖分類號(hào)：V249.1 文獻(xiàn)標(biāo)識(shí)碼：A

文章編號(hào)：1009-3044（2023）12-0095-05

0 引言

隨著人工智能技術(shù)的發(fā)展，無(wú)人機(jī)的自主路徑規(guī)劃能力也變得越來(lái)越高，其中避障問(wèn)題一直是無(wú)人機(jī)領(lǐng)域的熱點(diǎn)研究問(wèn)題。由于多旋翼無(wú)人機(jī)成本較為低廉，飛行震動(dòng)小，操作較為簡(jiǎn)單本文實(shí)驗(yàn)中采用的是以四旋翼無(wú)人機(jī)作為對(duì)象，進(jìn)行動(dòng)態(tài)避障算法的應(yīng)用研究。

路徑規(guī)劃算法是無(wú)人機(jī)在執(zhí)行任務(wù)時(shí)，首先需要解決的問(wèn)題。它需要考慮在三維地理空間以及自身動(dòng)力學(xué)約束的條件下，規(guī)劃出從起始點(diǎn)到目標(biāo)的安全線路，保證無(wú)人機(jī)不與三維地理空間中的障礙物進(jìn)行碰撞，同時(shí)還需要保證飛行姿態(tài)的平穩(wěn)，以及飛行軌跡的平滑。目前路徑規(guī)劃算法可以分為兩類即全局路徑規(guī)劃和局部路徑規(guī)劃，全局路徑規(guī)劃算法有Dijk?stra[1]，A*算法[2-3]，D*算法[4]，D* Lite算法[5]等，由于全路路徑規(guī)劃需要掌握所處三維地理空間的所有信息，或者在運(yùn)行時(shí)通過(guò)傳感器數(shù)據(jù)構(gòu)建一個(gè)全局三維地理空間地圖，該三維空間地圖是由數(shù)個(gè)大小一致的正方體組成，隨后對(duì)地圖進(jìn)行圖搜索找出可通行軌跡，它隨著空間維度的上升計(jì)算量也隨之陡增，全局路徑規(guī)劃比較適用于已知的三維地理空間；局部路徑規(guī)劃也有很多成熟的算法，如人工勢(shì)場(chǎng)法[6]、虛擬力場(chǎng)法[7]、VFH[8]，模糊混合勢(shì)場(chǎng)法[9]等，局部路徑規(guī)劃無(wú)須對(duì)三維空間進(jìn)行全局建圖，它是通過(guò)深度相機(jī)、激光雷達(dá)等傳感器實(shí)時(shí)感知無(wú)人機(jī)周圍的飛行環(huán)境進(jìn)行局部建圖，解析出障礙物位置距離等信息，找到一條當(dāng)前位置到下一目標(biāo)位置無(wú)碰撞的最優(yōu)軌跡[10]，局部路徑規(guī)劃更加適合于未知的三維地理空間。本文選用局部路徑規(guī)劃算法來(lái)進(jìn)行研究。

1 系統(tǒng)的總體設(shè)計(jì)

無(wú)人機(jī)自主避障系統(tǒng)總體設(shè)計(jì)如圖1所示，它是由四旋翼無(wú)人機(jī)飛行平臺(tái)、基于ROS導(dǎo)航與避障分系統(tǒng)、飛行控制分系統(tǒng)、數(shù)據(jù)傳輸分系統(tǒng)以及任務(wù)載荷分系統(tǒng)五部分組成。

1.1 動(dòng)態(tài)避障算法設(shè)計(jì)

動(dòng)態(tài)避障算法屬于導(dǎo)航與避障分系統(tǒng)，它是基于ROS機(jī)器人操作系統(tǒng)平臺(tái)。由于無(wú)人機(jī)在室內(nèi)或室外飛行面臨著動(dòng)態(tài)環(huán)境變化，本文選用局部路徑規(guī)劃算法，其中向量場(chǎng)直方圖法（VFH）在局部避障算法中表現(xiàn)出良好的性能[11]。VFH是Borenstein J.提出的理論，該算法的提出主要是解決人工勢(shì)場(chǎng)法在實(shí)際應(yīng)用中數(shù)據(jù)突變導(dǎo)致環(huán)境信息丟失的問(wèn)題[12]。若無(wú)人機(jī)具備自主避障能力，對(duì)于存在障礙物的飛行區(qū)域或者飛行過(guò)程中遇到突然出現(xiàn)障礙的情況，能夠保障無(wú)人機(jī)繼續(xù)執(zhí)行飛行任務(wù)，從而有效保障無(wú)人機(jī)的飛行安全和任務(wù)執(zhí)行能力，提高了無(wú)人機(jī)對(duì)飛行環(huán)境的適應(yīng)能力。

本文針對(duì)無(wú)人機(jī)在未知三維地理空間下局部路徑避障問(wèn)題，采用二維激光雷達(dá)建立局部直方圖地圖，并進(jìn)行二值化處理，通過(guò)代價(jià)函數(shù)求出代價(jià)最小的可通行區(qū)域，得出無(wú)人機(jī)飛行方向，為了在避障的同時(shí)解決過(guò)沖震蕩、姿態(tài)不穩(wěn)以及飛行效率等問(wèn)題，提出一種多場(chǎng)景自適應(yīng)的速度規(guī)劃控制器，通過(guò)條件判定是否遇到障礙物調(diào)用相應(yīng)的速度規(guī)劃控制器規(guī)劃出無(wú)人機(jī)飛行的速度，在板外控制的模式下，實(shí)時(shí)發(fā)送給無(wú)人機(jī)的飛行控制系統(tǒng)，實(shí)現(xiàn)對(duì)無(wú)人機(jī)實(shí)時(shí)水平速度控制以及垂直位置控制，保證無(wú)人機(jī)在避障同時(shí)提高飛行效率和飛行姿態(tài)的穩(wěn)定性。

1.2 激光雷達(dá)

單線激光雷達(dá)主要用于障礙物避障，多線激光雷達(dá)主要應(yīng)用于雷達(dá)成像，相對(duì)單線激光雷達(dá)多線激光雷達(dá)可以識(shí)別物體的高度信息，多線激光雷達(dá)需要由多個(gè)發(fā)射器和接收器組成，通過(guò)轉(zhuǎn)動(dòng)可以獲取多條線束，處理的數(shù)據(jù)量越大，硬件要求也更高。由于成本問(wèn)題在實(shí)機(jī)飛行實(shí)驗(yàn)的激光雷達(dá)采用的是上海思嵐科技公司RPLIDAR S1，它是單線的二維激光雷達(dá)，它的測(cè)距核心是順時(shí)針旋轉(zhuǎn)水平掃描范圍0°～360°，測(cè)量分辨率可達(dá)到3cm左右，最大測(cè)距白色物體40m，黑色物體10m，角度分辨率為0.5°，掃描一圈得到720 個(gè)點(diǎn)。它的基本原理是激光雷達(dá)傳感器首先通過(guò)發(fā)射器發(fā)出激光雷達(dá)信號(hào)，發(fā)出去的激光雷達(dá)遇到障礙物時(shí)就會(huì)被接收器檢測(cè)到信號(hào)，從而根據(jù)三角測(cè)距原理測(cè)出距離[13]。TOF 激光雷達(dá)測(cè)距示意圖如圖2 所示：

RPLIDAR S1通過(guò)USB接入板載計(jì)算單元作為通信接口，內(nèi)部處理器接收激光雷達(dá)信號(hào)，給出相應(yīng)測(cè)距數(shù)據(jù)[14]，板載計(jì)算單元需要安裝rplidar_ros功能包驅(qū)動(dòng)，獲取激光雷達(dá)掃描障礙物距離的實(shí)時(shí)數(shù)據(jù)，該數(shù)據(jù)可以用于建立無(wú)人機(jī)周圍環(huán)境的局部地圖。激光雷達(dá)安裝在無(wú)人機(jī)的正前方，使得無(wú)人機(jī)在飛行過(guò)程中實(shí)時(shí)探測(cè)障礙物并且進(jìn)行動(dòng)態(tài)避障。

2 VFH 算法

2.1 數(shù)據(jù)預(yù)處理

在無(wú)人機(jī)飛行過(guò)程中，利用激光雷達(dá)探測(cè)360度障礙物信息，本文按照每個(gè)扇區(qū)是30度，劃分為12個(gè)扇區(qū)，由于激光雷達(dá)的精度是0.5，每個(gè)扇區(qū)有60條激光雷達(dá)的測(cè)距信息，設(shè)定檢測(cè)距離范圍[dmin，dmax]，若測(cè)試距離di不在測(cè)距范圍內(nèi)，認(rèn)為該點(diǎn)未探測(cè)到障礙物，同時(shí)標(biāo)記距離di為零，若測(cè)試距離di在測(cè)距范圍內(nèi)，認(rèn)為該點(diǎn)探測(cè)到障礙物，同時(shí)標(biāo)記距離di為雷達(dá)探測(cè)最大距離dmax減去掃描距離di，依次累加計(jì)算每個(gè)扇區(qū)的距離總和作為該區(qū)的柱狀高度，每個(gè)扇區(qū)的值越小，代表越安全。

利用傳感器探測(cè)障礙物示意圖如圖3所示，無(wú)人機(jī)在飛往目標(biāo)點(diǎn)的過(guò)程中，探測(cè)到障礙1和障礙物2；二維直方圖示意圖如圖4所示其中橫坐標(biāo)表示是以無(wú)人機(jī)為中心感知到的周圍各個(gè)角度范圍（順時(shí)針?lè)较驗(yàn)檎?，縱坐標(biāo)表示在該方向存在障礙物的密度值：障礙物距離越近密度越大，障礙物距離越遠(yuǎn)密度越小。

2.2 二維直方圖二值化

上一小節(jié)中得到二維直方圖的數(shù)值較為雜亂，沒(méi)有一個(gè)確定的標(biāo)準(zhǔn)來(lái)判定直方圖中哪些方向是安全的，哪些方向是危險(xiǎn)的，不利于算法選擇可通行方向。因此需要對(duì)直方圖進(jìn)行二值化，從而簡(jiǎn)化信息，并最終區(qū)分出二維直方圖中的安全方向和不安全方向。之后依據(jù)無(wú)人機(jī)飛行的安全半徑等影響因素，從二維直方圖中找出連續(xù)的安全區(qū)域，并作為備選的可行方向。本算法通過(guò)設(shè)定兩個(gè)閾值T_high和T_low與二維直方圖中的每個(gè)扇區(qū)內(nèi)的密度值H_k進(jìn)行比較，若權(quán)重H_k<T_high，則H_k設(shè)為0，表示該方向安全；若H_k>T_high，則H_k設(shè)為2，表示該扇區(qū)危險(xiǎn)，不可通行;若權(quán)重滿足T_low≤H_k≤T_high ，則表示當(dāng)前扇區(qū)方向危險(xiǎn)還不確定[15]，H_k設(shè)為4。二值化每個(gè)扇區(qū)內(nèi)的密度值H_k表示為（1） ：

2.4 確定飛行速度

在確定可通行飛行方向后，需要確定無(wú)人機(jī)的飛行速度，避障時(shí)候通常采用較低的恒定速度Vg，將無(wú)人機(jī)當(dāng)前的飛行速度Vg進(jìn)行x和y軸的速度分解，賦值給MAVROS系統(tǒng)話題-/mavros/setpoint_raw/local發(fā)送給PX4飛行控制系統(tǒng)，此話題支持發(fā)送期望位置，期望速度，加速度，航向角以及航向角速率給飛控從而進(jìn)行控制，如果同時(shí)有位置，速度，加速度中兩項(xiàng)指令給出，那么后者是前饋?lái)?xiàng)。比如同時(shí)給出位置和加速度制指令，由于PX4是屬于串級(jí)PID控制，那么加速度是前饋?lái)?xiàng)，本文水平方向采用的是速度控制，垂直方向采用的位置控制方式。

2.5 多場(chǎng)景自適應(yīng)速度規(guī)劃控制器

當(dāng)采用較低的恒定速度，在沒(méi)有障礙物的時(shí)候飛行效率就非常低，為了解決在避障的同時(shí)提高飛行效率以及飛行速度過(guò)快容易引起過(guò)沖震蕩以及姿態(tài)不穩(wěn)等問(wèn)題，本文提出一種多場(chǎng)景自適應(yīng)速度規(guī)劃控制器，分為無(wú)人機(jī)檢測(cè)到障礙物和沒(méi)有檢測(cè)到障礙物兩種情形分別進(jìn)行速度規(guī)劃。

為了解決速度過(guò)快引起過(guò)沖震蕩和姿態(tài)不穩(wěn)等問(wèn)題，不再采用單一的速度控制，而是采用三段式的梯形速度規(guī)劃算法，設(shè)定加速和減速距離，（3） 和（4） 中的參數(shù)可以根據(jù)無(wú)人機(jī)本身的加速度，最大飛行速度等參數(shù)性能以及實(shí)際飛行場(chǎng)景進(jìn)行調(diào)整。此次實(shí)驗(yàn)中加速距離和減速距離分別設(shè)定20m，飛行最大速度設(shè)定Vg為5m/s，避障速度設(shè)置為0.5m/s，k值取0.3避免在剛開(kāi)始起飛的時(shí)候速度過(guò)小，a取值0.8為了使無(wú)人機(jī)減速到預(yù)設(shè)速度。

3 仿真實(shí)驗(yàn)

3.1 實(shí)驗(yàn)平臺(tái)搭建

為了能夠精準(zhǔn)而高效地模擬在復(fù)雜室內(nèi)和室外環(huán)境無(wú)人機(jī)飛行，Meyer J 等人使用ROS與Gazebo 環(huán)境進(jìn)行了四旋翼無(wú)人機(jī)綜合仿真[18]。本文實(shí)驗(yàn)平臺(tái)搭建需要在VMWARE虛擬機(jī)中安裝Ubuntu 18.04，并下載PX4的源碼然后運(yùn)行ubuntu.sh用于安裝所有的PX4所需要的工具鏈，工具鏈安裝完成后運(yùn)行仿真測(cè)試驗(yàn)證環(huán)境是否安裝成功，若運(yùn)行make px4_sitl_de?fault gazebo成功，則PX4和gazebo仿真搭建成功；繼續(xù)在Ubuntu 18.04下安裝ROS Melodic機(jī)器人操作系統(tǒng)以及MAVROS軟件功能包，安裝完可以進(jìn)行ROS以及MAVROS啟動(dòng)測(cè)試是否安裝成功，若運(yùn)行roslaunchposix_sitl.launch成功，表示ROS仿真環(huán)境安裝成功。最后下載QGC地面站安裝包進(jìn)行仿真測(cè)試驗(yàn)證，PX4 和gazebo 仿真環(huán)境運(yùn)行成功后，運(yùn)行地面站控制系統(tǒng)，地面站將自動(dòng)連接上飛控系統(tǒng)。

3.2 仿真平臺(tái)架構(gòu)

在Gazebo仿真平臺(tái)中，搭建了一套無(wú)人機(jī)飛行仿真環(huán)境，主要完成了仿真環(huán)境內(nèi)無(wú)人機(jī)和激光雷達(dá)的搭建和配置，為了使得仿真環(huán)境中無(wú)人機(jī)和激光雷達(dá)在對(duì)外接口、輸出數(shù)據(jù)等方面和現(xiàn)實(shí)的硬件基本一致，本實(shí)驗(yàn)采用PX4+Gazebo+ROS+MAVROS+QGC 的軟件在環(huán)（SITL）仿真測(cè)試，其中PX4+Gazebo+QGC 仿真架構(gòu)圖[19]如圖5。

由圖5軟件在環(huán)仿真架構(gòu)圖可以看出PX4飛控軟件由仿真器中獲取傳感器數(shù)據(jù)如IMU數(shù)據(jù)、磁羅盤(pán)、GPS、氣壓計(jì)以及激光雷達(dá)等，通過(guò)飛控軟件解算得到控制力矩給仿真器，仿真器根據(jù)無(wú)人機(jī)動(dòng)力模型再次解算出仿真數(shù)據(jù)反饋給飛控軟件，這樣使得PX4與仿真器形成了輸入與反饋的閉環(huán)回路。地面站軟件控制系統(tǒng)由PX4飛控軟件系統(tǒng)中獲取飛行信息、傳感器信息以及無(wú)人機(jī)的姿態(tài)信息，同時(shí)地面站軟件系統(tǒng)可以給飛控軟件系統(tǒng)下發(fā)無(wú)人機(jī)控制信息，包括起飛、解鎖、著陸、航跡規(guī)劃、飛行模式變換、飛行速度以及地理圍欄控制等指令。版外模式可以直接與飛控系統(tǒng)通過(guò)MAVLINK通信協(xié)議進(jìn)行通信，從而對(duì)無(wú)人機(jī)進(jìn)行實(shí)時(shí)控制以及信息監(jiān)控。

由于本文避障算法是基于ROS系統(tǒng)在板載計(jì)算單元進(jìn)行板外控制，在利用ROS開(kāi)發(fā)環(huán)境進(jìn)行編程時(shí)，由于各ROS節(jié)點(diǎn)（外部控制程序）之間通過(guò)ROS話題、服務(wù)進(jìn)行交互，還需要利用MAVROS功能包進(jìn)行MAVLINK消息與ROS話題、服務(wù)之間的轉(zhuǎn)換[20]。由于VFH算法是建立在ROS系統(tǒng)的一個(gè)包上，因此它可以通過(guò)發(fā)布/訂閱消息與其他節(jié)點(diǎn)通信。VFH節(jié)點(diǎn)所需的數(shù)據(jù)包括激光雷達(dá)傳感器，IMU（航向）、GPS （當(dāng)前位置）和制導(dǎo)控制，輸入通過(guò)訂閱MAROS話題和激光雷達(dá)?；赗OS避障系統(tǒng)發(fā)布與訂閱體系結(jié)構(gòu)如圖6所示。

VFH算法主要是通過(guò)/scan話題實(shí)時(shí)訂閱激光雷達(dá)傳感器信息，并判定是否遇到障礙物，再調(diào)用速度規(guī)劃控制器，對(duì)無(wú)人機(jī)的速度進(jìn)行規(guī)劃，通過(guò)發(fā)布話題/setpoint_raw/local話題對(duì)無(wú)人機(jī)進(jìn)行實(shí)時(shí)的速度和位置控制，此話題支持發(fā)送期望位置、期望速度、加速度、航向角以及航向角速率給飛控從而進(jìn)行控制，同時(shí)通過(guò)訂閱/compass_hdg 獲取航向信息，訂閱/lo?cal_position/pose獲取仿真實(shí)時(shí)的位置信息以及訂閱/mission/waypoints話題獲取通過(guò)地面站規(guī)劃的任務(wù)航點(diǎn)信息等。

3.3 仿真實(shí)驗(yàn)

本實(shí)驗(yàn)采用的四旋翼的無(wú)人機(jī)模型，其中以sdf 結(jié)尾為無(wú)人機(jī)模型文件，world結(jié)尾為環(huán)境模型文件，默認(rèn)的四旋翼無(wú)人機(jī)模型iris.sdf，沒(méi)有搭載激光雷達(dá)傳感器模型，需要打開(kāi)posix_sitl.launch，把iris模型修改為iris_rplidar 模型，運(yùn)行roslaunch posix_sitl.launch 啟動(dòng)之后gazebo將加載激光雷達(dá)傳感器模型，環(huán)境模型默認(rèn)為沒(méi)有任何障礙物的空環(huán)境，首先需要為無(wú)人機(jī)建立多個(gè)圓柱體類型的障礙物環(huán)境模型，打開(kāi)ga?zebo選擇insert，選擇圓柱體，調(diào)整圓柱體的大小、位置以及顏色等信息，然后另存為obstacles.world文件，修改posix_sitl.launch 中的empty.world 文件為obstacles.world，重新運(yùn)行啟動(dòng)roslaunch posix_sitl.launch，安裝有激光雷達(dá)的四旋翼無(wú)人機(jī)以及障礙物均已經(jīng)建立并運(yùn)行正常。

基于搭建好PX4+ROS+gazebo仿真環(huán)境之后并啟動(dòng)，之后啟動(dòng)地面站控制系統(tǒng)，地面站控制系統(tǒng)將自動(dòng)連接上仿真飛控系統(tǒng)，通過(guò)地面站設(shè)置航點(diǎn)1和航點(diǎn)2信息，并上傳至飛控系統(tǒng)。航點(diǎn)1到航點(diǎn)2的水平速度規(guī)劃如圖7所示，在遇到障礙物的過(guò)程保持避障低速運(yùn)行，在沒(méi)有障礙物的時(shí)候，由于限制了最大速度，主要分為三段式梯形速度規(guī)劃。設(shè)定值、估計(jì)值與真實(shí)俯仰角的示意圖如圖8所示，實(shí)際俯仰角的范圍在[-5，5]度之間，姿態(tài)角平穩(wěn)。

由圖9障礙物示意圖可以看到在gazebo仿真環(huán)境中分布著多個(gè)柱狀的障礙物，藍(lán)色的光束代表著無(wú)人機(jī)激光雷達(dá)掃描發(fā)射的光束，由于在Home點(diǎn)到航點(diǎn)1 的途中設(shè)置密集柱狀障礙物，由圖10避障軌跡示意圖中可以發(fā)現(xiàn)Home 點(diǎn)到航點(diǎn)1 的過(guò)程中有明顯的避障，最終順利穿過(guò)障礙物林，到達(dá)目標(biāo)點(diǎn)航點(diǎn)2，之后升高返航到Home點(diǎn)。

4 結(jié)束語(yǔ)

本文重點(diǎn)介紹了基于向量場(chǎng)直方圖的局部路徑規(guī)劃算法，根據(jù)收到的飛控的定位數(shù)據(jù)、激光雷達(dá)數(shù)據(jù)、航向信息以及目標(biāo)航點(diǎn)位置等，按照一定頻率向飛控模塊實(shí)時(shí)發(fā)布規(guī)劃后的無(wú)人機(jī)飛行方向和速度。在無(wú)人機(jī)執(zhí)行飛行任務(wù)過(guò)程中，實(shí)時(shí)檢測(cè)前方是否有障礙物，若遇到障礙物，根據(jù)代價(jià)函數(shù)找到能避開(kāi)障礙物的最近的可通行區(qū)域，從而確定無(wú)人機(jī)飛行航向，再根據(jù)速度規(guī)劃控制器規(guī)劃出無(wú)人機(jī)的飛行速度；若無(wú)障礙物則根據(jù)剩余距離進(jìn)行對(duì)無(wú)人機(jī)的速度進(jìn)行規(guī)劃，同時(shí)保持目標(biāo)航向，從而保證無(wú)人機(jī)姿態(tài)平衡并以較優(yōu)的路徑完成避障的同時(shí)快速到達(dá)目標(biāo)位置。通過(guò)gazebo仿真實(shí)驗(yàn)結(jié)果表明：該算法能快速穿越密集柱狀障礙物林的同時(shí)保持飛行姿態(tài)的平穩(wěn)，進(jìn)而達(dá)到目標(biāo)航點(diǎn)。然而對(duì)于還未確定的扇區(qū)，本文看作不可通行區(qū)域，后續(xù)研究將進(jìn)一步優(yōu)化算法查看未確定扇區(qū)周圍區(qū)域的可通行性進(jìn)一步判定，從而提高避障算法的抗干擾性能和健壯性。