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

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

1. 引言

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

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

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

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

2.1. 無(wú)人車基本組成

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

Figure 1. Unmanned vehicle basic structure

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

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

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

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

Figure 2. Data acquisition module connection diagram

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

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

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

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

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

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

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

Figure 3. Algorithm flowchart

圖3. 算法流程圖

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

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

3.1. 無(wú)人車建模

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

1) 無(wú)人車建模

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

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

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

Figure 4. Bicycle model

圖4. 自行車模型示意圖

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

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

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

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

Figure 5. Detection area division

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

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

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

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

Figure 6. Obstacle avoidance process

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

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

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

Figure 7. Cascade PID block diagram

圖7. 串級(jí)PID原理框圖

3.3. 自定義主題添加

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

Pixhawk的核心為NuttX實(shí)時(shí)ARM操作系統(tǒng)，uORB允許多個(gè)進(jìn)程打開(kāi)同一個(gè)文件，進(jìn)程之間通過(guò)該文件節(jié)點(diǎn)進(jìn)行數(shù)據(jù)的交互與共享。每個(gè)進(jìn)程都可以訂閱或者發(fā)布主題，系統(tǒng)中可以存在多個(gè)發(fā)布者，一個(gè)進(jìn)程也可以同時(shí)訂閱多個(gè)主題，但是在一條總線上始終只能有一條消息。飛控中所有的數(shù)據(jù)都通過(guò)這種主題的發(fā)布和訂閱方式來(lái)完成數(shù)據(jù)交換，如圖8所示。下面我們需要通過(guò)自定義主題的方式，將數(shù)據(jù)采集模塊發(fā)送過(guò)來(lái)的超聲波信息進(jìn)行發(fā)布，供其他功能模塊訂閱來(lái)實(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文件中對(duì)發(fā)布主題進(jìn)行定義，主要包括定義主題、公告主題和發(fā)布主題三個(gè)過(guò)程。程序中，orb_advertise()函數(shù)實(shí)現(xiàn)公告操作，在使用orb_publish()發(fā)布新數(shù)據(jù)前需要先執(zhí)行公告操作，公告成功后函數(shù)返回的處理值提供給orb_publish()函數(shù)，在主題發(fā)布時(shí)進(jìn)行調(diào)用。在對(duì)read_uart_sensor.c文件進(jìn)行定義后，需要對(duì)CMakeLists.txt文件進(jìn)行配置，主要是對(duì)編譯中的參數(shù)進(jìn)行定義。

3) 完成前面的操作后需要在Firmware/cmake/configs/nuttx/nuttx_px4fmu-v2_default.cmake文件中注冊(cè)自定義的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ā)布過(guò)程，整體的發(fā)布流程如圖9所示。

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

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

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

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

Figure 9. Custom theme publishing process

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

實(shí)驗(yàn)中，首先確保手動(dòng)模式下的操作正常，打開(kāi)地面站地圖界面，如圖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)無(wú)人車在巡航結(jié)束后能夠一鍵返航，自動(dòng)返回H點(diǎn)。我們?cè)诓賵?chǎng)中規(guī)劃簡(jiǎn)單的路徑對(duì)自動(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信號(hào)強(qiáng)度，GPS衛(wèi)星數(shù)量在十顆以上可以進(jìn)行自動(dòng)巡航實(shí)驗(yàn)。使用遙控器將無(wú)人車從手動(dòng)模式切換到自動(dòng)模式，無(wú)人車將沿著預(yù)先設(shè)定的航線運(yùn)行，使用數(shù)傳模塊可以將無(wú)人車的運(yùn)動(dòng)狀態(tài)在地面站上實(shí)時(shí)顯示，主要包括無(wú)人車的實(shí)時(shí)軌跡、位置及速度等信息。圖11為地面站中實(shí)時(shí)顯示的無(wú)人車運(yùn)動(dòng)軌跡，其中黃色曲線為初始設(shè)定航線，紫色曲線為無(wú)人車實(shí)際運(yùn)行路線。從圖中可以看到實(shí)際航線和預(yù)定軌跡基本吻合，因此無(wú)人車的自動(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)框架圖

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

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

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

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

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

實(shí)驗(yàn)中設(shè)置自動(dòng)巡航行駛過(guò)程中速度為0.8 m/s。當(dāng)檢測(cè)到障礙物距離為3 m時(shí)，開(kāi)始減速行駛，速度逐漸降低至避障速度0.5 m/s。經(jīng)過(guò)1.5 s左右無(wú)人車執(zhí)行變向避障操作，轉(zhuǎn)向過(guò)程中U2超聲波逐漸檢測(cè)不到障礙物。在此過(guò)程中以0.5 m/s左右的速度行駛，當(dāng)繞開(kāi)障礙物后速度回到預(yù)設(shè)航線中逐漸回到設(shè)定值0.8 m/s。實(shí)驗(yàn)過(guò)程中速度的最大誤差為0.2 m/s，符合誤差要求。

Figure 13. Obstacle detected

圖13. 檢測(cè)到障礙物

Figure 14. Bypass obstacles

圖14. 繞開(kāi)障礙物

Figure 15. Back to the route

圖15. 回到航線

Figure 16. Obstacle avoidance path display

圖16. 避障路徑顯示

4.2.2. 兩側(cè)障礙物實(shí)驗(yàn)與分析

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

1) 障礙物在左前方

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

2) 障礙物在右前方

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

上述實(shí)驗(yàn)表明，地面無(wú)人車在自主巡航時(shí)兩側(cè)遇到障礙物，能夠順利地避開(kāi)障礙物，并繼續(xù)回到預(yù)設(shè)航線中。在左右障礙物情況下，經(jīng)多次實(shí)驗(yàn)測(cè)試，從開(kāi)始避障到回到預(yù)設(shè)航線上的時(shí)間在9~11 s之間。

Figure 17. U1, U2 ultrasonic detection of obstacles

圖17. U1、U2號(hào)超聲波檢測(cè)到障礙物

Figure 18. Left front obstacle avoidance path display

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

Figure 19. U2, U3 ultrasonic detection of obstacles

圖19. U2、U3號(hào)超聲波檢測(cè)到障礙物

Figure 20. Right front obstacle avoidance path display

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

5. 總結(jié)

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

實(shí)驗(yàn)表明，本文提出的避障及路徑規(guī)劃策略在室外場(chǎng)景下獲得較好的檢測(cè)效果。相比傳統(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)更加簡(jiǎn)單，可以應(yīng)用到校園巡邏、廠區(qū)安全檢查等精度要求不是很高的場(chǎng)景中。后續(xù)的研究，可以通過(guò)加入其他傳感器，與超聲波數(shù)據(jù)進(jìn)行融合，來(lái)實(shí)現(xiàn)更復(fù)雜環(huán)境下的避障及路徑規(guī)劃。

審核編輯：湯梓紅