基于PX4的地面無人車避障系統及路徑規劃研究

摘要: 地面無人車避障及路徑規劃是指，無人車在自動巡航過程中，遇到障礙物能夠自動避開，并對當前行進路線進行重新規劃，重新進入預設路線行駛。本文使用HPI公司的Savage Flux 2350差速驅動越野車作為實驗平臺，選用開源硬件Pixhawk飛控作為運動控制器，對PX4 Rover軟件系統進行二次開發。使用數據采集模塊獲得超聲波數據，并通過串口方式與飛控板進行通信，完成數據交換，從而實現對地面無人車周圍環境的實時檢測，完成巡航模式下的避障功能。

1. 引言

地面無人車輛(Unmanned ground vehicles, UGV)集環境感知、規劃決策與多等級輔助駕駛功能于一體，是涉及傳感器、信息融合、計算機、通訊、人工智能及自動控制技術等多學科、多領域的高新技術集合體 [1]。地面無人車又常被稱為無人車、智能車、智能機器人等。近些年來，它憑借自身優勢，體積小、機動靈活、易操作等特點被廣泛應用于各個行業 [2]。

地面無人車的研究在國外開始較早。隨著近些年計算機視覺、人工智能、電子電路等學科的快速發展，這些學科技術也被廣泛應用于無人車研究中 [3]。近年來，關于無人車、無人船的研究和使用開始火熱起來，涉及航空、航海、農業等各個領域，國內外許多研究機構也開始關注地面無人車自動控制領域 [4] [5]。目前研究人員重點關注避障技術的研究，主要包括障礙物識別、信息位置的獲取、車速和轉向控制等方面 [6]。地面無人車自主避障系統可以利用傳感器來獲取周圍環境信息，具備感知周圍環境和自適應的能力，在完成給定任務的同時，能夠在過程中實現自動避障及路徑規劃 [7]。

本文以開源飛控Pixhawk作為運動控制器，通過對無人車進行建模，結合串級PID控制算法，實現無人車的自主巡航功能。針對數據采集過程中出現錯誤數據的問題，提出一種新的數據處理算法。并對障礙物檢測區域進行劃分，使無人車在不同障礙物方位下采取不同的避障策略。

2. 地面無人車系統架構

2.1. 無人車基本組成

地面無人車的基本組成可以分為三部分，分別是機械系統、動力系統和控制系統 [8] [9]。本文使用HPI公司的Savage Flux 2350四驅差速越野車作為地面無人車的實驗平臺。動力系統中主要是由舵機、電調、電機和電池四部分構成，提供無人車的動力來源。控制系統主要由飛控板、一系列配套外設和地面站組成。本系統中使用的是Pixhawk控制器，外部設備包括數傳模塊、GPS、電源模塊、蜂鳴器、安全開關、遙控器接收機等。搭建完成的無人車基本結構如圖1所示。

Figure 1. Unmanned vehicle basic structure

圖1. 無人車基本結構

2.2. 數據采集模塊

2.2.1. 數據采集模塊硬件連接方式

本文通過在車頭和車尾各安裝三個HC-SR04超聲波，來實現對無人車周圍障礙物的實時監測。由于Pixhawk硬件接口限制，不能支持外接多個超聲波模塊，因此這里使用數據采集模塊對超聲波數據進行預處理，將處理后的數據通過串口發送給Pixhawk控制器，數據采集模塊與Pixhawk連接方式如圖2所示。

Figure 2. Data acquisition module connection diagram

圖2. 數據采集模塊連接圖

2.2.2. 數據采集模塊軟件設計

在對數據采集程序進行設計的時候，需要考慮多個超聲波傳感器共同工作時帶來的信號交叉問題。交叉問題是指使用多個超聲波傳感器時，由傳感器a發出的聲波，在經過反射后可能被傳感器b和c獲得，這時候b和c就會根據這個聲波信息來計算距離，造成測量數據的混亂。針對這個問題，我們對超聲波傳感器進行編碼，車頭左側的超聲波定義為U1，沿順時針方向分別定義為U1、U2、U3、U4、U5、U6，在測距程序中設置每個超聲波觸發信號的時間間隔為40 ms。在保證超聲波數據實時性的同時有效地解決了超聲波數據交叉影響的問題。

對超聲波數據處理過程中，最初使用常用的均值濾波算法，采集5次超聲波數據取平均值作為最終輸出，但是后來發現在實際的應用中傳感器的數值并不是很穩定，每10分鐘可能會出現一到兩次的數據錯誤，在多個超聲波上都出現了類似的錯誤，經過觀察總結發現錯誤數據有兩個特點：

1) 錯誤數據一般連續出現，個數通常不超過三個。

2) 錯誤數據離真實值偏離較大，并且連續的錯誤數據都比較相近。

無人車在運行過程中，需要獲得穩定的超聲波數據。根據錯誤數據的特點，本文對傳統的均值濾波算法進行改進，引入新的數據處理算法對超聲波數據進行處理。算法的主要思想是，利用少數遵循多數的原則，把采集到的一系列數據分成兩部分，將相鄰兩個數據值之差最大的數作為數據的分界點。然后取兩部分數據中個數最多的部分作為正確的測量值，另一部分為無效測量值。算法流程如下圖3所示，經過處理后的數據相比之前更加穩定，錯誤數據數量明顯減少。

Figure 3. Algorithm flowchart

圖3. 算法流程圖

最后經過處理的超聲波數據，通過數據采集模塊的串口發送到Pixhawk的TELEM2接口。首先對ATmega2560串口1進行初始化，設置串口通信波特率為9600;對每個超聲波設置啟動時間，獲得超聲波檢測數據，并將數據進行格式化，精確到個位數;在對超聲波數據進行處理后，將得到的新的數據列通過串口1進行發送。

3. 無人車避障及路徑規劃設計

3.1. 無人車建模

車輛運行路徑的動態調整是實現自動巡航功能的核心，在獲得車輛精準的位置信息后，如何使車輛能夠精確地沿著預設的路線進行巡航，是下一步需要研究的問題。要實現對無人車的控制分析，首先需要對控制對象進行建模，下面對本文中使用的無人車進行建模分析。

1) 無人車建模

這里我們參考自行車模型 [10] 對無人車的運動進行簡化，主要有以下兩個假設：

① 忽略車輛在垂直(z軸)方向的運動，可以理解為在高處以俯視的角度對車輛進行觀察。

② 假設車輛的前面兩個輪胎具有相同的角度和轉速，同樣后面兩個輪胎也是如此，這樣就可以把車輛的結構看作自行車，前后的輪胎就可以各用一個輪胎來描述。與自行車原理相同，假設車輛的轉向完全由前輪控制。這樣無人車模型就可以簡化為如圖4所示的自行車模型。

Figure 4. Bicycle model

圖4. 自行車模型示意圖

在該模型中，因為我們使用的車輛后輪不能旋轉，這里設置后輪的轉向角控制輸入 δfδf = 0，也就是說方向盤的控制輸入都反映到前輪的轉角上，因此整個系統的控制量可以簡化為(a, δfδf )，其中a代表車輛的加速度。加速度的正負分別代表無人車處于加速或減速階段， δfδf 代表當前輪胎的轉角，這樣就使用兩個變量描述車輛的控制輸入。使用四個狀態變量描述車輛當前的狀態，(x, y)表示車輛當前位置; ψψ 表示當前車輛的偏航角，逆時針方向為正;v表示當前的車速; ββ 表示車輛當前的質心側偏角，即小車質心的速度相對于無人車縱軸的角度。使用 lrlr 和 lflf 表示前輪和后輪到車輛重心的距離，根據動力學原理，無人車的各個狀態變量的更新公式如下所示：

基于該運動學自行車模型，在給定一個時刻的控制輸入后，可以通過計算求得在dt時間以后車輛的狀態信息(x, y, ψψ , v)，這樣就可以使用該模型結合串級PID控制算法實現對控制量(a, δfδf )的動態調整，從而實現對車輛狀態的實時調整，進而實現自動巡航功能。

3.2. 避障及路徑規劃設計

因為無人車不具備垂直方向的運動能力，這里我們僅考慮在二維空間中的避障策略。由前文超聲波的布局方式可知，超聲波的探測區域可以被簡化為一個扇形區域。超聲波的最大測量距離為4 m，但是實驗中發現在3 m到4 m范圍內的檢測數據容易出現波動，因此這里將超聲波的探測區域簡化為半徑r = 3 m、角度 θθ = 45?的扇形區域，如下圖5所示。

Figure 5. Detection area division

圖5. 檢測區域劃分示意

下面首先對檢測區域進行劃分，根據檢測到的距離將檢測區域劃分為F、M、N為三個扇形區域，分別表示距離無人車2米到3米之間的區域、1米到2米之間的區域、0到1米之間的區域。根據超聲波傳感器的檢測角度，可以將檢測區域劃分為九個檢測區域，分別表示為F1、F2、F3、M1、M2、M3和N1、N2、N3九個檢測區域，角標代表傳感器編號，1、2、3號傳感器代表在車身上安裝的U1、U2、U3超聲波傳感器，分別用來檢測無人車的左側、前方、右側區域。這里將F、M、N分別定義為減速區、避障區和停止區，在不同的距離范圍內無人車采取不同的避障操作。無人車的避障實現流程如圖6所示。當檢測到障礙物的時候，首先對障礙物所處區域進行判斷，然后判斷障礙物的方位，根據障礙物的方位執行相應的避障操作指令。

下面對障礙物位于M區域的情況進行簡要分析，當障礙物由1號超聲波檢測到的時候，表明障礙物在M1區域位于無人車的左側，此使應讓無人車執行小角度右轉操作;當障礙物由2號超聲波檢測到的時候，此時障礙物位于M2區域，障礙物為無人車的正前方，考慮到操場的實際情況，這里執行大角度左轉操作;同理，當障礙物被3號超聲波檢測到的時候，障礙物位于無人車右側，此時執行小角度左轉。

真實的場景下可能會遇到體積比較大的障礙物，同時會被多個超聲波檢測到。這里針對該種情況進行討論，當障礙物由1號和2號超聲波同時檢測到，此時障礙物跨越了M1和M2區域位于無人車的左前方，這時候僅執行小角度的右轉已經不能滿足避障的要求，需要使無人車執行大角度右轉來躲避障礙物。同樣，當障礙物被2號和3號傳感器同時檢測到的時候，執行大角度左轉來躲避障礙物。更極端的一種情況，障礙物同時被三個傳感器檢測到，這時僅執行轉向操作已經不能實現較好的避障，此時無人車執行和在剎車區域時相同的操作，首先停車，然后執行倒車指令，進入避障區域重新執行避障操作。

Figure 6. Obstacle avoidance process

圖6. 避障實現流程

在避障策略的研究中，主要通過減速及轉向來實現對障礙物的躲避。這就會產生另外一個問題，即躲避障礙物后，如何快速回到最初設定的航線上繼續執行巡航任務。

這里使用串級PID算法來對無人車的運動狀態進行動態調整。在串級PID中將角度控制PID算法和角速度控制PID算法進行串聯，經過串聯后的PID控制器可以控制多個變量，系統的抗干擾性得到增強，無人車的適應能力得到進一步的提升。在執行避障操作后，無人車的行進路線已經偏離了最初的航線，根據PID原理，此時的誤差值會突然變大，同時PID針對控制量 (a,δf)(a,δf) 不斷進行優化，優化過程中無人車逐漸回到最初航線上，無人車中使用的串級PID的原理框圖如圖7所示。

Figure 7. Cascade PID block diagram

圖7. 串級PID原理框圖

3.3. 自定義主題添加

在進行避障及路徑規劃實驗前，需要對數據采集模塊發送過來的超聲波數據進行處理。由于PX4采用uORB (微對象請求代理器)通信機制，因此通過串口獲得的數據并不能直接利用，需要以發布主題、訂閱主題的形式進行消息的傳遞。uORB是飛控程序中進行消息傳遞的異步消息機制。它在Pixhawk系統占有重要地位，負責整個系統的數據傳輸任務，飛控中所有的傳感器數據、GPS、PPM信號等數據都要通過uORB傳輸到系統中各個模塊進行計算處理。

Pixhawk的核心為NuttX實時ARM操作系統，uORB允許多個進程打開同一個文件，進程之間通過該文件節點進行數據的交互與共享。每個進程都可以訂閱或者發布主題，系統中可以存在多個發布者，一個進程也可以同時訂閱多個主題，但是在一條總線上始終只能有一條消息。飛控中所有的數據都通過這種主題的發布和訂閱方式來完成數據交換，如圖8所示。下面我們需要通過自定義主題的方式，將數據采集模塊發送過來的超聲波信息進行發布，供其他功能模塊訂閱來實現信息的交換。

Figure 8. Data exchange process

圖8. 數據交換流程

1) 首先需要在Firmware/msg目錄下新建一個名為read_uart_sensor.msg文件，該文件定義Pixhawk串口接收到所有超聲波數據的結構，然后在msg/CMakeLists.txt下添加該文件名。添加完成后重新編譯，會在Firmware/src/modules/uORB/topics目錄下生成read_uart_sensor.h頭文件。

2) 在Firmware/src/modules目錄下創建文件夾并命名為read_uart_sensor作為超聲波主題模塊的存放位置。在該文件夾下創建read_uart_sensor.c文件和CMakeLists.txt文件。

在read_uart_sensor.c文件中對發布主題進行定義，主要包括定義主題、公告主題和發布主題三個過程。程序中，orb_advertise()函數實現公告操作，在使用orb_publish()發布新數據前需要先執行公告操作，公告成功后函數返回的處理值提供給orb_publish()函數，在主題發布時進行調用。在對read_uart_sensor.c文件進行定義后，需要對CMakeLists.txt文件進行配置，主要是對編譯中的參數進行定義。

3) 完成前面的操作后需要在Firmware/cmake/configs/nuttx/nuttx_px4fmu-v2_default.cmake文件中注冊自定義的read_uart_sensor模塊。

4) 將超聲波主題添加到啟動腳本中。要實現上電后就可以在Pixhawk控制器中獲得超聲波數據，需要將該模塊添加到系統的啟動腳本文件中。添加指令read_uart_sensor start/dev/ttyS2。這樣就可以實現當數據采集模塊與TELEM2端口(/dev/ttyS2)相連的時候，自動啟動超聲波模塊。

至此已經完成了超聲波自定義主題的發布過程，整體的發布流程如圖9所示。

在完成主題的發布后，下一步需要訂閱該主題，獲得超聲波數據數據。orb_subscribe()函數用來訂閱數據，獲得的數據保存在int sensor_sub_fd結構體中;orb_set_interval()函數用來設置獲取數據的時間間隔;orb_check()函數用來檢查上次讀取主題后，主題是否更新過。最后對修改后的源碼重新編譯，并上傳編譯后的固件。經過以上操作，在nsh命令行中就可以通過輸入指令，查看當前數據的訂閱結果。

4. 地面無人車避障及路徑規劃實驗

4.1. 自動巡航實驗

避障及路徑規劃是在自動巡航的基礎上進行的，在實驗中首先對無人車的自動巡航功能進行驗證。在該模式下，無人車根據地面站上設定好的航線，自動巡航，GPS信號與慣性測量單元的精度，直接決定了巡航路線的精確度。

Figure 9. Custom theme publishing process

圖9. 自定義主題發布流程

實驗中，首先確保手動模式下的操作正常，打開地面站地圖界面，如圖10所示。在界面的右側可以實現，讀取航點、寫入航點，起始位置等操作，主界面下側設置各航點的具體參數(速度、停留時間、預定航點拍照等)，也可以刪除航點。除了設置路線之外，還可以設置H點，H代表家的位置，設置該位置可以實現無人車在巡航結束后能夠一鍵返航，自動返回H點。我們在操場中規劃簡單的路徑對自動巡航功能進行驗證，設置完航線之后點擊右側寫入航點即可將任務下載到飛行控制器中。

Figure 10. Ground station flight plan operation interface

圖10. 地面站飛行計劃操作界面

下載完成后進入飛行數據界面，檢查GPS信號強度，GPS衛星數量在十顆以上可以進行自動巡航實驗。使用遙控器將無人車從手動模式切換到自動模式，無人車將沿著預先設定的航線運行，使用數傳模塊可以將無人車的運動狀態在地面站上實時顯示，主要包括無人車的實時軌跡、位置及速度等信息。圖11為地面站中實時顯示的無人車運動軌跡，其中黃色曲線為初始設定航線，紫色曲線為無人車實際運行路線。從圖中可以看到實際航線和預定軌跡基本吻合，因此無人車的自動巡航功能滿足要求。

Figure 11. Automatic cruise route map

圖11. 自動巡航路線圖

4.2. 避障及路徑規劃實驗

本文的平臺搭建、自動巡航功能實現和自定義主題的添加都是為了實現最終的避障及路徑規劃實驗。在前文研究的基礎上，最終構建避障及路徑規劃的整體流程如下圖12所示。

Figure 12. Obstacle avoidance system frame diagram

圖12. 避障系統框架圖

首先通過數據采集模塊實現超聲波傳感器的數據采集功能，然后將采集到的數據通過串口發送給飛控板，接下來按照uORB消息機制添加自定義超聲波模塊，對超聲波數據主題進行發布和訂閱，最后在手動模式和自動模式下訂閱超聲波數據，并對獲得的數據進行處理實現無人車的避障及路徑規劃。

為了驗證文中提出方法的有效性，下面分別在手動模式和自動模式下對無人車進行避障測試。實驗中使用大小為20 cm * 30 cm * 50 cm的紙箱做障礙物，實驗前首先確保超聲波傳感器及地面無人車均可正常工作，分別在正前方障礙物和兩側障礙物的情況下對避障效果進行檢測。

由于目前業界并沒有統一的測試標準和評價指標來對避障效果進行量化考核，因此在測試的時候通常會從障礙物的個數、障礙物的運動狀態、運動軌跡是否平滑這幾個方面進行考慮。結合實際的應用場景，本文以安全避障和耗時最少作為評價指標，對無人車的避障及路徑規劃過程進行測試。下面對實驗現象和結果進行簡要分析。

4.2.1. 正前方障礙物實驗與分析

本小節對障礙物在正前方的情況進行實驗分析。首先在地面站設置飛行任務，進行航線規劃，并設置速度參數，上傳到飛行控制系統。地面無人車以0.8 m/s的速度巡航行駛，當距離障礙物3 m的時候，無人車檢測到障礙物，并開始降低巡航速度，進入避障準備階段，障礙物位置如圖13所示。當行駛至距離障礙物2 m的時候，無人車進入避障區域，執行避障操作向左轉彎45?，繞障礙物行駛如圖14所示。無人車在繞過障礙物之后返回規劃航線中，并沿規劃好的航線繼續行駛，如圖15所示。整體的避障及路徑規劃行駛路徑如圖16所示，圖中黃色航線為設定航線，紫色為無人車實際運行路線，其中障礙物位于航點1和航點2之間，從圖中紫色路徑可以看到，無人車在遇到障礙物時，通過向左繞行實現避障，并在繞過障礙物后快速回到預設航線中。對多次實驗的避障過程時間進行統計發現，從檢測到障礙物開始到回到預設航線中，耗時在9~11 s之間，符合實際應用中的要求。

實驗中設置自動巡航行駛過程中速度為0.8 m/s。當檢測到障礙物距離為3 m時，開始減速行駛，速度逐漸降低至避障速度0.5 m/s。經過1.5 s左右無人車執行變向避障操作，轉向過程中U2超聲波逐漸檢測不到障礙物。在此過程中以0.5 m/s左右的速度行駛，當繞開障礙物后速度回到預設航線中逐漸回到設定值0.8 m/s。實驗過程中速度的最大誤差為0.2 m/s，符合誤差要求。

Figure 13. Obstacle detected

圖13. 檢測到障礙物

Figure 14. Bypass obstacles

圖14. 繞開障礙物

Figure 15. Back to the route

圖15. 回到航線

Figure 16. Obstacle avoidance path display

圖16. 避障路徑顯示

4.2.2. 兩側障礙物實驗與分析

本小節對障礙物在左前方和右前方兩種情況分別進行實驗和分析。

1) 障礙物在左前方

當地面無人車按照航線前進過程中檢測到左前方有障礙物時，超聲波U1和U2均有數據輸出，障礙物位置如圖17所示。圖18為地面無人車在自動巡航模式下左前方遇到障礙物時通過地面站顯示的避障路徑圖，紫色曲線為無人車的實際運行路徑，黃色曲線為規劃路徑，障礙物位于航點1和航點2中間。在實驗中，地面無人車在檢測到障礙物之后開始減速，當行駛至距離障礙物2 m的時候，開始往右側45?轉彎進行繞障礙物行駛運動。在避過障礙物之后返回規劃航線，并沿規劃好的航線繼續行駛。

2) 障礙物在右前方

在同樣的路徑規劃任務下，只改變障礙物的位置，模擬在無人車行進過程中右前方出現障礙物的情況。此時超聲波U2和U3均有數據輸出，障礙物位置如圖19所示。圖20為地面無人車在自動巡航模式下，右前方遇到障礙物時地面站顯示的避障路徑圖，可以看出障礙物位于航點1和航點2中間。與左前方出現障礙物時的避障操作相似，執行一系列的減速動作后，通過向左側轉向的方式來實現避障。

上述實驗表明，地面無人車在自主巡航時兩側遇到障礙物，能夠順利地避開障礙物，并繼續回到預設航線中。在左右障礙物情況下，經多次實驗測試，從開始避障到回到預設航線上的時間在9~11 s之間。

Figure 17. U1, U2 ultrasonic detection of obstacles

圖17. U1、U2號超聲波檢測到障礙物

Figure 18. Left front obstacle avoidance path display

圖18. 左前方避障路徑顯示

Figure 19. U2, U3 ultrasonic detection of obstacles

圖19. U2、U3號超聲波檢測到障礙物

Figure 20. Right front obstacle avoidance path display

圖20. 右前方避障路徑顯示

5. 總結

本文采用Pixhawk飛控作為主控器，在對無人車進行建模的基礎上，結合串級PID控制算法實現無人車的避障及路徑規劃功能。本文提出的避障及路徑規劃方法，除了在無人車上應用，還可以擴展到多旋翼無人機的避障中，實現無人機飛行過程中的避障。

實驗表明，本文提出的避障及路徑規劃策略在室外場景下獲得較好的檢測效果。相比傳統的單片機實現方式，飛控具有強大的數據處理能力，能夠實現更穩定的避障及路徑規劃效果。與常用的激光雷達實現避障及路徑規劃的方案相比，本文提出的基于超聲波的避障方案成本更低、實現更加簡單，可以應用到校園巡邏、廠區安全檢查等精度要求不是很高的場景中。后續的研究，可以通過加入其他傳感器，與超聲波數據進行融合，來實現更復雜環境下的避障及路徑規劃。

審核編輯：湯梓紅