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

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

1. 引言

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

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

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

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

2.1. 無人車基本組成

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

Figure 1. Unmanned vehicle basic structure

圖1. 無人車基本結(jié)構(gòu)

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

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

本文通過在車頭和車尾各安裝三個(gè)HC-SR04超聲波，來實(shí)現(xiàn)對無人車周圍障礙物的實(shí)時(shí)監(jiān)測。由于Pixhawk硬件接口限制，不能支持外接多個(gè)超聲波模塊，因此這里使用數(shù)據(jù)采集模塊對超聲波數(shù)據(jù)進(jìn)行預(yù)處理，將處理后的數(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ì)

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

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

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

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

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

Figure 3. Algorithm flowchart

圖3. 算法流程圖

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

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

3.1. 無人車建模

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

1) 無人車建模

這里我們參考自行車模型 [10] 對無人車的運(yùn)動(dòng)進(jìn)行簡化，主要有以下兩個(gè)假設(shè)：

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

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

Figure 4. Bicycle model

圖4. 自行車模型示意圖

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

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

3.2. 避障及路徑規(guī)劃設(shè)計(jì)

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

Figure 5. Detection area division

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

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

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

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

Figure 6. Obstacle avoidance process

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

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

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

Figure 7. Cascade PID block diagram

圖7. 串級PID原理框圖

3.3. 自定義主題添加

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

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

Figure 8. Data exchange process

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

1) 首先需要在Firmware/msg目錄下新建一個(gè)名為read_uart_sensor.msg文件，該文件定義Pixhawk串口接收到所有超聲波數(shù)據(jù)的結(jié)構(gòu)，然后在msg/CMakeLists.txt下添加該文件名。添加完成后重新編譯，會(huì)在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ā)布主題進(jìn)行定義，主要包括定義主題、公告主題和發(fā)布主題三個(gè)過程。程序中，orb_advertise()函數(shù)實(shí)現(xiàn)公告操作，在使用orb_publish()發(fā)布新數(shù)據(jù)前需要先執(zhí)行公告操作，公告成功后函數(shù)返回的處理值提供給orb_publish()函數(shù)，在主題發(fā)布時(shí)進(jìn)行調(diào)用。在對read_uart_sensor.c文件進(jìn)行定義后，需要對CMakeLists.txt文件進(jìn)行配置，主要是對編譯中的參數(shù)進(jìn)行定義。

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

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

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

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

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

4.1. 自動(dòng)巡航實(shí)驗(yàn)

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

Figure 9. Custom theme publishing process

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

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

Figure 10. Ground station flight plan operation interface

圖10. 地面站飛行計(jì)劃操作界面

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

Figure 11. Automatic cruise route map

圖11. 自動(dòng)巡航路線圖

4.2. 避障及路徑規(guī)劃實(shí)驗(yàn)

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

Figure 12. Obstacle avoidance system frame diagram

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

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

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

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

4.2.1. 正前方障礙物實(shí)驗(yàn)與分析

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

實(shí)驗(yàn)中設(shè)置自動(dòng)巡航行駛過程中速度為0.8 m/s。當(dāng)檢測到障礙物距離為3 m時(shí)，開始減速行駛，速度逐漸降低至避障速度0.5 m/s。經(jīng)過1.5 s左右無人車執(zhí)行變向避障操作，轉(zhuǎn)向過程中U2超聲波逐漸檢測不到障礙物。在此過程中以0.5 m/s左右的速度行駛，當(dāng)繞開障礙物后速度回到預(yù)設(shè)航線中逐漸回到設(shè)定值0.8 m/s。實(shí)驗(yàn)過程中速度的最大誤差為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. 兩側(cè)障礙物實(shí)驗(yàn)與分析

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

1) 障礙物在左前方

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

2) 障礙物在右前方

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

上述實(shí)驗(yàn)表明，地面無人車在自主巡航時(shí)兩側(cè)遇到障礙物，能夠順利地避開障礙物，并繼續(xù)回到預(yù)設(shè)航線中。在左右障礙物情況下，經(jīng)多次實(shí)驗(yàn)測試，從開始避障到回到預(yù)設(shè)航線上的時(shí)間在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. 總結(jié)

本文采用Pixhawk飛控作為主控器，在對無人車進(jìn)行建模的基礎(chǔ)上，結(jié)合串級PID控制算法實(shí)現(xiàn)無人車的避障及路徑規(guī)劃功能。本文提出的避障及路徑規(guī)劃方法，除了在無人車上應(yīng)用，還可以擴(kuò)展到多旋翼無人機(jī)的避障中，實(shí)現(xiàn)無人機(jī)飛行過程中的避障。

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

審核編輯：湯梓紅