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

洪向共，張心馳

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

0 引言

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

1 農(nóng)業(yè)無人機(jī)航點計算

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

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

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下不能直接對坐標(biāo)點進(jìn)行相對地面的方向計算，需要轉(zhuǎn)換為北東地NED坐標(biāo)系，轉(zhuǎn)換示意圖如圖2所示。

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

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

(4)

(5)

(6)

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

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

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

假設(shè)農(nóng)業(yè)無人機(jī)當(dāng)前的位置Pg(λ，φ，Z)已知，飛行的方向為ξ，需要飛行的空間距離為D，接下里應(yīng)該飛往的航點為Pg,n(λn，φn，Zn)，那么航點的計算步驟為：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖9 試驗結(jié)果圖

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

6 結(jié)論

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