基于PX4的地面無人車避障系統(tǒng)及路徑規(guī)劃研究

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

1. 引言

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

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

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

2. 地面無人車系統(tǒng)架構

2.1. 無人車基本組成

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

Figure 1. Unmanned vehicle basic structure

圖1. 無人車基本結構

2.2. 數(shù)據(jù)采集模塊

2.2.1. 數(shù)據(jù)采集模塊硬件連接方式

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

Figure 2. Data acquisition module connection diagram

圖2. 數(shù)據(jù)采集模塊連接圖

2.2.2. 數(shù)據(jù)采集模塊軟件設計

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

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

1) 錯誤數(shù)據(jù)一般連續(xù)出現(xiàn)，個數(shù)通常不超過三個。

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

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

Figure 3. Algorithm flowchart

圖3. 算法流程圖

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

3. 無人車避障及路徑規(guī)劃設計

3.1. 無人車建模

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

1) 無人車建模

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

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

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

Figure 4. Bicycle model

圖4. 自行車模型示意圖

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

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

3.2. 避障及路徑規(guī)劃設計

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

Figure 5. Detection area division

圖5. 檢測區(qū)域劃分示意

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

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

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

Figure 6. Obstacle avoidance process

圖6. 避障實現(xiàn)流程

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

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

Figure 7. Cascade PID block diagram

圖7. 串級PID原理框圖

3.3. 自定義主題添加

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

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

Figure 8. Data exchange process

圖8. 數(shù)據(jù)交換流程

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

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

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

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

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

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

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

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

4.1. 自動巡航實驗

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

Figure 9. Custom theme publishing process

圖9. 自定義主題發(fā)布流程

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

Figure 10. Ground station flight plan operation interface

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

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

Figure 11. Automatic cruise route map

圖11. 自動巡航路線圖

4.2. 避障及路徑規(guī)劃實驗

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

Figure 12. Obstacle avoidance system frame diagram

圖12. 避障系統(tǒng)框架圖

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

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

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

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

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

實驗中設置自動巡航行駛過程中速度為0.8 m/s。當檢測到障礙物距離為3 m時，開始減速行駛，速度逐漸降低至避障速度0.5 m/s。經(jīng)過1.5 s左右無人車執(zhí)行變向避障操作，轉向過程中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. 兩側障礙物實驗與分析

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

1) 障礙物在左前方

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

2) 障礙物在右前方

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

上述實驗表明，地面無人車在自主巡航時兩側遇到障礙物，能夠順利地避開障礙物，并繼續(xù)回到預設航線中。在左右障礙物情況下，經(jīng)多次實驗測試，從開始避障到回到預設航線上的時間在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控制算法實現(xiàn)無人車的避障及路徑規(guī)劃功能。本文提出的避障及路徑規(guī)劃方法，除了在無人車上應用，還可以擴展到多旋翼無人機的避障中，實現(xiàn)無人機飛行過程中的避障。

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

審核編輯：湯梓紅