基于嵌入式的農(nóng)業(yè)無(wú)人機(jī)航點(diǎn)規(guī)劃算法研究

洪向共，張心馳

(南昌大學(xué) 信息工程學(xué)院，南昌 330031)

0 引言

隨著傳感技術(shù)、無(wú)線網(wǎng)絡(luò)技術(shù)、電子集成技術(shù)和智能控制技術(shù)的飛速發(fā)展，無(wú)人機(jī)技術(shù)發(fā)生了巨大的變化，已成為科研人員研究的熱門(mén)領(lǐng)域之一。無(wú)人機(jī)技術(shù)將通過(guò)實(shí)時(shí)數(shù)據(jù)的采集和處理對(duì)農(nóng)業(yè)進(jìn)行高科技改造，已被廣泛應(yīng)用于農(nóng)田藥物噴灑、農(nóng)田信息檢測(cè)、災(zāi)害預(yù)報(bào)及農(nóng)業(yè)保險(xiǎn)勘察等場(chǎng)合。航點(diǎn)規(guī)劃算法是農(nóng)業(yè)無(wú)人機(jī)的核心，本文基于嵌入式技術(shù)，研發(fā)了一種農(nóng)業(yè)無(wú)人機(jī)航點(diǎn)規(guī)劃算法，實(shí)現(xiàn)了農(nóng)業(yè)無(wú)人機(jī)的航跡規(guī)劃。

1 農(nóng)業(yè)無(wú)人機(jī)航點(diǎn)計(jì)算

1.1 大地坐標(biāo)與地心地固ECEF的轉(zhuǎn)換

一般而言，經(jīng)緯度是對(duì)地面位置信息的一種直接描述，并不是指的實(shí)際距離和位置等數(shù)據(jù)，因此在進(jìn)行農(nóng)業(yè)無(wú)人機(jī)航點(diǎn)計(jì)算過(guò)程中，一般需要將大地坐標(biāo)(地球經(jīng)緯度)轉(zhuǎn)為地心地固直角坐標(biāo)系(ECEF)，轉(zhuǎn)換示意圖如圖1所示。在ECEF三維坐標(biāo)系中進(jìn)行航點(diǎn)的計(jì)算，即

Xe=(N+h)cosφcosλ

(1)

Ye=(N+h)cosφsinλ

(2)

Ze=[N(1-e2)+h]sinφ

(3)

圖1 大地坐標(biāo)與ECEF間的轉(zhuǎn)換

1.2 地心地固坐標(biāo)ECEF與北東地NED坐標(biāo)的轉(zhuǎn)換

地心地固坐標(biāo)ECEF下不能直接對(duì)坐標(biāo)點(diǎn)進(jìn)行相對(duì)地面的方向計(jì)算，需要轉(zhuǎn)換為北東地NED坐標(biāo)系，轉(zhuǎn)換示意圖如圖2所示。

ECEF與NED坐標(biāo)間轉(zhuǎn)換計(jì)算過(guò)程為

Pn=Rn/e(Pe-Pe,ref)

(4)

(5)

(6)

其中，Pe,ref為北東地NED坐標(biāo)系下的點(diǎn)；Pe,為ECEF中需要轉(zhuǎn)為北東地NED坐標(biāo)系下的點(diǎn)；Pn為已轉(zhuǎn)為北東地NED坐標(biāo)系下的點(diǎn)；Rn/e為ECEF與NED坐標(biāo)間的轉(zhuǎn)換矩陣。

圖2 ECEF與NED坐標(biāo)間的轉(zhuǎn)換

1.3 農(nóng)業(yè)無(wú)人機(jī)的航點(diǎn)計(jì)算方法

假設(shè)農(nóng)業(yè)無(wú)人機(jī)當(dāng)前的位置Pg(λ，φ，Z)已知，飛行的方向?yàn)棣危枰w行的空間距離為D，接下里應(yīng)該飛往的航點(diǎn)為Pg,n(λn，φn，Zn)，那么航點(diǎn)的計(jì)算步驟為：

1)將Pg(λ，φ，Ze)轉(zhuǎn)換為Pe(Xe，Ye，Ze)；

2)設(shè)定NED坐標(biāo)系中下一航點(diǎn)坐標(biāo)為Pn(Dcosξ，Dsinξ，-h)；

3)將Pn(Dcosξ，Dsinξ，-h)轉(zhuǎn)換為ECEF坐標(biāo)系下的點(diǎn)，Pe,n(Xe,n，Ye,n，Ze,n)；

這樣就可以精確地計(jì)算出任意知道距離和航向的農(nóng)業(yè)無(wú)人機(jī)接下來(lái)的航點(diǎn)坐標(biāo)。

2 農(nóng)業(yè)無(wú)人機(jī)航跡規(guī)劃數(shù)學(xué)建模

本文以農(nóng)業(yè)無(wú)人機(jī)為研究對(duì)象，采用數(shù)學(xué)建模的方法對(duì)構(gòu)建其航跡規(guī)劃，航跡規(guī)劃系統(tǒng)框架如圖3所示。系統(tǒng)核心由信息融合、環(huán)境建模、態(tài)勢(shì)評(píng)估、航點(diǎn)計(jì)算和航跡規(guī)劃等5個(gè)模塊組成。

1)信息融合模塊。多傳感器檢測(cè)技術(shù)是信息融合的前提，信息融合技術(shù)主要內(nèi)容是對(duì)多傳感器信息進(jìn)行協(xié)調(diào)分析。信息融合模塊的作用是利用各種傳感技術(shù)，依據(jù)一定的優(yōu)先級(jí)別，結(jié)合本地地形數(shù)據(jù)庫(kù)信息，對(duì)周邊環(huán)境信息進(jìn)行自動(dòng)分析與綜合考慮，給無(wú)人機(jī)飛控系統(tǒng)的事件檢測(cè)、態(tài)勢(shì)分析和飛行航跡的處理和規(guī)劃提供可靠依據(jù)，同時(shí)及時(shí)將信息傳回地面操作端。

2)態(tài)勢(shì)評(píng)估模塊。態(tài)勢(shì)評(píng)估模塊的功能是根據(jù)以往的知識(shí)和經(jīng)驗(yàn)，使系統(tǒng)自適應(yīng)對(duì)復(fù)雜多變的飛行環(huán)境實(shí)時(shí)監(jiān)控，并對(duì)環(huán)境進(jìn)行有效的評(píng)估，為航跡規(guī)劃提供可靠的環(huán)境評(píng)估報(bào)告。

3)環(huán)境建模模塊。利用農(nóng)業(yè)無(wú)人機(jī)系統(tǒng)搭載的高清相機(jī)、各類(lèi)傳感器設(shè)備對(duì)無(wú)人機(jī)周邊區(qū)域進(jìn)行實(shí)時(shí)的監(jiān)測(cè)，建立無(wú)人機(jī)周邊環(huán)境模型，并將圖像信息通過(guò)無(wú)人機(jī)鏈路傳回地面控制站。

4)航點(diǎn)計(jì)算模塊。航點(diǎn)計(jì)算模塊的作用是根據(jù)已知的農(nóng)業(yè)無(wú)人機(jī)當(dāng)前的位置Pg(λ，φ，Z)、飛行方向和距離等數(shù)據(jù)信息，計(jì)算下一個(gè)航點(diǎn)經(jīng)緯度Pg,n(λn，φn，Zn)。

5)航跡規(guī)劃模塊。農(nóng)業(yè)無(wú)人機(jī)的航跡規(guī)劃是整個(gè)控制系統(tǒng)的核心內(nèi)容，其需要根據(jù)GPS信號(hào)、地形信息、環(huán)境信息以及遠(yuǎn)程感知技術(shù)，獲得無(wú)人機(jī)飛行狀況，根據(jù)既定任務(wù)以及飛行過(guò)程中的重規(guī)劃，實(shí)現(xiàn)地形跟隨、地形回避和威脅回避等目標(biāo)的飛行，從而制定最優(yōu)參考路徑。

圖3 農(nóng)業(yè)無(wú)人機(jī)航跡規(guī)劃系統(tǒng)框架

3 嵌入式控制系統(tǒng)的總體方案設(shè)計(jì)

農(nóng)業(yè)無(wú)人機(jī)嵌入式控制系統(tǒng)分為地面和機(jī)載兩部分：地面部分是一個(gè)具有遠(yuǎn)程操控的遙控器；機(jī)載部分則由GPS、高清相機(jī)、電源模塊、存儲(chǔ)單元Exynos4412處理器，以及超聲波、MPU6050、高速、高度、航向等傳感器組成。農(nóng)業(yè)無(wú)人機(jī)嵌入式控制系統(tǒng)總體框架如圖4所示。

由圖4可以看出，農(nóng)業(yè)無(wú)人機(jī)嵌入式控制系統(tǒng)具有如下幾個(gè)特點(diǎn)：

1)Exynos4412處理器。Exynos4412是整個(gè)控制系統(tǒng)的核心，其處理多傳感器傳來(lái)的數(shù)據(jù)信息，根據(jù)多路分析結(jié)果，制定農(nóng)業(yè)無(wú)人機(jī)的避障、姿態(tài)控制與航跡規(guī)劃，同時(shí)實(shí)時(shí)將數(shù)據(jù)送回地面系統(tǒng)。

2)傳感器模塊。GPS、高清相機(jī)，以及MPU6050、高速、高度、航向等著組成的傳感器網(wǎng)絡(luò)，實(shí)時(shí)采集農(nóng)業(yè)無(wú)人機(jī)的周邊環(huán)境信息，獲取農(nóng)業(yè)無(wú)人機(jī)的經(jīng)緯度和高度、姿態(tài)及航向等信息，為Exynos4412處理器的決策提供依據(jù)。

3)電機(jī)驅(qū)動(dòng)模塊。Exynos4412處理器輸出PWM脈沖，由電機(jī)驅(qū)動(dòng)模塊控制無(wú)人機(jī)電機(jī)運(yùn)轉(zhuǎn)。

4)遙控器模塊。采用搖桿控制方式，將收據(jù)信息通過(guò)無(wú)線傳輸模塊發(fā)送給農(nóng)業(yè)無(wú)人機(jī)，供農(nóng)業(yè)無(wú)人機(jī)接收利用。

圖4 農(nóng)業(yè)無(wú)人機(jī)嵌入式控制系統(tǒng)總體框架圖

4 農(nóng)業(yè)無(wú)人機(jī)航跡規(guī)劃的實(shí)現(xiàn)

農(nóng)業(yè)無(wú)人機(jī)航跡規(guī)劃是以航點(diǎn)計(jì)算為基礎(chǔ)，根據(jù)給定位置信息，結(jié)合信息融合、環(huán)境建模、態(tài)勢(shì)評(píng)估等參數(shù)信息，在合理的區(qū)間給出一條農(nóng)業(yè)無(wú)人機(jī)的飛行航跡。即在避開(kāi)障礙物威脅和到達(dá)目的航點(diǎn)的基礎(chǔ)上，規(guī)劃出一條最優(yōu)的航跡。農(nóng)業(yè)無(wú)人機(jī)航跡規(guī)劃主要分為在無(wú)障礙威脅、單障礙威脅和多障礙威脅等3種情形下確定最優(yōu)的航跡。

1)農(nóng)業(yè)無(wú)人機(jī)當(dāng)前航點(diǎn)與下一航點(diǎn)無(wú)障礙威脅，如圖5所示。在這種情形下，無(wú)人機(jī)當(dāng)前航點(diǎn)與下一航點(diǎn)之間的直線距離就是最優(yōu)的航跡，無(wú)需規(guī)劃。

圖5 無(wú)障礙威脅的航跡規(guī)劃

圖6 一個(gè)障礙威脅的航跡規(guī)劃

3)農(nóng)業(yè)無(wú)人機(jī)當(dāng)前航點(diǎn)與下一航點(diǎn)有多個(gè)障礙威脅，如圖7所示。當(dāng)農(nóng)業(yè)無(wú)人機(jī)當(dāng)前航點(diǎn)與下一航點(diǎn)有多個(gè)障礙威脅時(shí)，看作是多個(gè)單一的障礙威脅的疊加，可以分別以當(dāng)前位置和障礙威脅作切線，之后將所有路線的長(zhǎng)度求出，最短的航跡則為所求。

這種情形下有4種航跡可供選擇：

通過(guò)對(duì)4種航跡的計(jì)算和比較，可以得知航跡2)路程最短，即為最優(yōu)路徑。

圖7 多個(gè)障礙威脅的航跡規(guī)劃

通過(guò)以上無(wú)障礙威脅、單障礙威脅和多障礙威脅等3種情形下航跡規(guī)劃，即可實(shí)現(xiàn)農(nóng)業(yè)無(wú)人機(jī)的航跡規(guī)劃。

5 試驗(yàn)結(jié)果與分析

為了驗(yàn)證基于嵌入式的農(nóng)業(yè)無(wú)人機(jī)航點(diǎn)規(guī)劃算法的可靠性和精準(zhǔn)性，將該算法程序固化到無(wú)人機(jī)控制器上，實(shí)現(xiàn)對(duì)無(wú)人機(jī)的航跡規(guī)劃，并進(jìn)行了實(shí)際的測(cè)試，試驗(yàn)內(nèi)容是農(nóng)業(yè)無(wú)人機(jī)是否能夠在路徑最優(yōu)情況下到達(dá)終點(diǎn)。試驗(yàn)中，在無(wú)人機(jī)起飛前，人為地在系統(tǒng)中設(shè)定了簡(jiǎn)單和復(fù)雜障礙威脅環(huán)境。障礙威脅環(huán)境圖和試驗(yàn)結(jié)果如圖8和圖9所示。

圖8 障礙物環(huán)境圖

圖9 試驗(yàn)結(jié)果圖

由圖9可以看出：針對(duì)不同情形的障礙威脅環(huán)境，農(nóng)業(yè)無(wú)人機(jī)都能規(guī)劃出一條合理的規(guī)避威脅的航跡，成功避開(kāi)障礙威脅，順利到達(dá)終點(diǎn)；從其移動(dòng)路徑長(zhǎng)度和拐點(diǎn)來(lái)看，航跡優(yōu)化效果非常明顯，說(shuō)明了無(wú)人機(jī)航點(diǎn)規(guī)劃算法能規(guī)劃出滿(mǎn)足要求的航跡，驗(yàn)證了該算法的可靠性和可行性。

6 結(jié)論

無(wú)人機(jī)技術(shù)將通過(guò)實(shí)時(shí)數(shù)據(jù)的采集和處理對(duì)農(nóng)業(yè)進(jìn)行高科技改造，目前已被廣泛應(yīng)用于農(nóng)田藥物噴灑、農(nóng)田信息檢測(cè)以及農(nóng)業(yè)保險(xiǎn)勘察等場(chǎng)合。本文首先研究了農(nóng)業(yè)無(wú)人機(jī)航點(diǎn)規(guī)劃算法，然后搭建了農(nóng)業(yè)無(wú)人機(jī)航跡規(guī)劃數(shù)學(xué)模型，并基于嵌入式控制技術(shù)，實(shí)現(xiàn)了農(nóng)業(yè)無(wú)人機(jī)航跡規(guī)劃。試驗(yàn)結(jié)果表明：針對(duì)不同情形的障礙威脅環(huán)境，農(nóng)業(yè)無(wú)人機(jī)都能規(guī)劃出一條合理的規(guī)避威脅的航跡，且移動(dòng)路徑長(zhǎng)度和拐點(diǎn)都最優(yōu)，驗(yàn)證了無(wú)人機(jī)航點(diǎn)規(guī)劃算法的可靠性和可行性。