基于向量場直方圖的無人機動態避障算法的應用研究

徐靜　苑征　梁寅博　甄澤安

關鍵詞：向量場直方圖；動態避障；速度規劃；無人機；局部路徑規劃

中圖分類號：V249.1 文獻標識碼：A

文章編號：1009-3044（2023）12-0095-05

0 引言

隨著人工智能技術的發展，無人機的自主路徑規劃能力也變得越來越高，其中避障問題一直是無人機領域的熱點研究問題。由于多旋翼無人機成本較為低廉，飛行震動小，操作較為簡單本文實驗中采用的是以四旋翼無人機作為對象，進行動態避障算法的應用研究。

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

1 系統的總體設計

無人機自主避障系統總體設計如圖1所示，它是由四旋翼無人機飛行平臺、基于ROS導航與避障分系統、飛行控制分系統、數據傳輸分系統以及任務載荷分系統五部分組成。

1.1 動態避障算法設計

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

本文針對無人機在未知三維地理空間下局部路徑避障問題，采用二維激光雷達建立局部直方圖地圖，并進行二值化處理，通過代價函數求出代價最小的可通行區域，得出無人機飛行方向，為了在避障的同時解決過沖震蕩、姿態不穩以及飛行效率等問題，提出一種多場景自適應的速度規劃控制器，通過條件判定是否遇到障礙物調用相應的速度規劃控制器規劃出無人機飛行的速度，在板外控制的模式下，實時發送給無人機的飛行控制系統，實現對無人機實時水平速度控制以及垂直位置控制，保證無人機在避障同時提高飛行效率和飛行姿態的穩定性。

1.2 激光雷達

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

RPLIDAR S1通過USB接入板載計算單元作為通信接口，內部處理器接收激光雷達信號，給出相應測距數據[14]，板載計算單元需要安裝rplidar_ros功能包驅動，獲取激光雷達掃描障礙物距離的實時數據，該數據可以用于建立無人機周圍環境的局部地圖。激光雷達安裝在無人機的正前方，使得無人機在飛行過程中實時探測障礙物并且進行動態避障。

2 VFH 算法

2.1 數據預處理

在無人機飛行過程中，利用激光雷達探測360度障礙物信息，本文按照每個扇區是30度，劃分為12個扇區，由于激光雷達的精度是0.5，每個扇區有60條激光雷達的測距信息，設定檢測距離范圍[dmin，dmax]，若測試距離di不在測距范圍內，認為該點未探測到障礙物，同時標記距離di為零，若測試距離di在測距范圍內，認為該點探測到障礙物，同時標記距離di為雷達探測最大距離dmax減去掃描距離di，依次累加計算每個扇區的距離總和作為該區的柱狀高度，每個扇區的值越小，代表越安全。

利用傳感器探測障礙物示意圖如圖3所示，無人機在飛往目標點的過程中，探測到障礙1和障礙物2；二維直方圖示意圖如圖4所示其中橫坐標表示是以無人機為中心感知到的周圍各個角度范圍（順時針方向為正），縱坐標表示在該方向存在障礙物的密度值：障礙物距離越近密度越大，障礙物距離越遠密度越小。

2.2 二維直方圖二值化

上一小節中得到二維直方圖的數值較為雜亂，沒有一個確定的標準來判定直方圖中哪些方向是安全的，哪些方向是危險的，不利于算法選擇可通行方向。因此需要對直方圖進行二值化，從而簡化信息，并最終區分出二維直方圖中的安全方向和不安全方向。之后依據無人機飛行的安全半徑等影響因素，從二維直方圖中找出連續的安全區域，并作為備選的可行方向。本算法通過設定兩個閾值T_high和T_low與二維直方圖中的每個扇區內的密度值H_k進行比較，若權重H_k<T_high，則H_k設為0，表示該方向安全；若H_k>T_high，則H_k設為2，表示該扇區危險，不可通行;若權重滿足T_low≤H_k≤T_high ，則表示當前扇區方向危險還不確定[15]，H_k設為4。二值化每個扇區內的密度值H_k表示為（1） ：

2.4 確定飛行速度

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

2.5 多場景自適應速度規劃控制器

當采用較低的恒定速度，在沒有障礙物的時候飛行效率就非常低，為了解決在避障的同時提高飛行效率以及飛行速度過快容易引起過沖震蕩以及姿態不穩等問題，本文提出一種多場景自適應速度規劃控制器，分為無人機檢測到障礙物和沒有檢測到障礙物兩種情形分別進行速度規劃。

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

3 仿真實驗

3.1 實驗平臺搭建

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

3.2 仿真平臺架構

在Gazebo仿真平臺中，搭建了一套無人機飛行仿真環境，主要完成了仿真環境內無人機和激光雷達的搭建和配置，為了使得仿真環境中無人機和激光雷達在對外接口、輸出數據等方面和現實的硬件基本一致，本實驗采用PX4+Gazebo+ROS+MAVROS+QGC 的軟件在環（SITL）仿真測試，其中PX4+Gazebo+QGC 仿真架構圖[19]如圖5。

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

由于本文避障算法是基于ROS系統在板載計算單元進行板外控制，在利用ROS開發環境進行編程時，由于各ROS節點（外部控制程序）之間通過ROS話題、服務進行交互，還需要利用MAVROS功能包進行MAVLINK消息與ROS話題、服務之間的轉換[20]。由于VFH算法是建立在ROS系統的一個包上，因此它可以通過發布/訂閱消息與其他節點通信。VFH節點所需的數據包括激光雷達傳感器，IMU（航向）、GPS （當前位置）和制導控制，輸入通過訂閱MAROS話題和激光雷達。基于ROS避障系統發布與訂閱體系結構如圖6所示。

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

3.3 仿真實驗

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

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

由圖9障礙物示意圖可以看到在gazebo仿真環境中分布著多個柱狀的障礙物，藍色的光束代表著無人機激光雷達掃描發射的光束，由于在Home點到航點1 的途中設置密集柱狀障礙物，由圖10避障軌跡示意圖中可以發現Home 點到航點1 的過程中有明顯的避障，最終順利穿過障礙物林，到達目標點航點2，之后升高返航到Home點。

4 結束語

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