Maloba Nyjot Tshikala，黃李炳，袁發(fā)業(yè)，江先志

(浙江理工大學(xué) 機(jī)械與自動控制學(xué)院，浙江 杭州 310018)

基于GPS/INS導(dǎo)航的農(nóng)用無人機(jī)航點(diǎn)規(guī)劃算法研究及應(yīng)用

Maloba Nyjot Tshikala，黃李炳，袁發(fā)業(yè)，江先志

(浙江理工大學(xué) 機(jī)械與自動控制學(xué)院，浙江 杭州 310018)

討論大地坐標(biāo)與ECEF坐標(biāo)的轉(zhuǎn)換、ECEF坐標(biāo)與NED坐標(biāo)的轉(zhuǎn)換.提出了4個經(jīng)緯度點(diǎn)構(gòu)成的兩條線段交點(diǎn)經(jīng)緯度的算法，以及已知航向與距離的航點(diǎn)經(jīng)緯度的算法.將算法應(yīng)用于農(nóng)用四軸飛行器，規(guī)劃了柵欄式的航線.

GPS/INS；坐標(biāo)轉(zhuǎn)換；農(nóng)用四軸飛行器；航線規(guī)劃

在農(nóng)作物生長過程中，病蟲害的有效防控是保證糧食安全生產(chǎn)的重要環(huán)節(jié)，直接影響糧食的產(chǎn)量與質(zhì)量[1].無人機(jī)低空施藥作為一種新型的防治病蟲害手段，正在成為農(nóng)機(jī)行業(yè)新的亮點(diǎn)和熱點(diǎn)[2].同時，隨著傳感器技術(shù)、微機(jī)電技術(shù)的發(fā)展，四軸飛行器的研發(fā)成為了國內(nèi)外研究的熱點(diǎn)[3].目前大部分四軸飛行器基于GPS/INS導(dǎo)航[4]，通常事先設(shè)置一系列的經(jīng)緯度航點(diǎn)，起飛前將其導(dǎo)入四軸飛行器，起飛后四軸飛行器將按設(shè)定好的經(jīng)緯度路徑飛行.因此，精確地計算經(jīng)緯度航點(diǎn)至關(guān)重要.GPS的經(jīng)緯度點(diǎn)在空間中位于一個橢球面(地球)上，計算它們之間的距離、方向和位置較為困難.如果忽略地表曲面的影響直接推算無人機(jī)飛行過程中的關(guān)鍵航點(diǎn)，實(shí)際的飛行試驗(yàn)中就會發(fā)現(xiàn)存在較大誤差.本文的研究重點(diǎn)在于通過坐標(biāo)變換、空間幾何計算等精確地規(guī)劃四軸無人機(jī)的GPS航點(diǎn)，并將該航點(diǎn)算法應(yīng)用于農(nóng)業(yè)植保無人機(jī)(圖1).

圖1 應(yīng)用航點(diǎn)算法的無人機(jī)

1 航點(diǎn)算法

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

由于經(jīng)緯度是對地球表面地理位置的描述，并不涉及實(shí)際的距離和位置計算，因此這里需要將經(jīng)緯度及高度轉(zhuǎn)換成地心地固(ECEF)坐標(biāo)，在ECEF坐標(biāo)系下規(guī)劃航點(diǎn)(圖2)[5].

Xe=(N+h)cosφcosλ

(1)

Ye=(N+h)cosφsinλ

(2)

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

(3)

(4)

f=(a-b)/a

(5)

e2=2f-f2

(6)

式中：Pe=(Xe,Ye,Ze)為ECEF坐標(biāo)系下的坐標(biāo)點(diǎn)；Pg=(λ,φ,h)為對應(yīng)點(diǎn)的經(jīng)緯度及高度；a為地球長軸的半徑，它的值是6 378 137m；b為地球短軸半徑，它的值是6 356 752.314 2m；e為偏心率.

圖2 大地坐標(biāo)與ECEF坐標(biāo)轉(zhuǎn)換示意圖

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

ECEF坐標(biāo)系下無法做相對地面的方向計算，需要轉(zhuǎn)換成NED(North East Down)坐標(biāo)系，如圖3所示.

《麥克白》是莎士比亞創(chuàng)作后期的一部文學(xué)作品，創(chuàng)作于1606年。莎士比亞早期的作品著重于對人文精神和個性解放的宣揚(yáng)?？墒牵捎谫Y本主義社會的發(fā)展，激烈的社會矛盾暴露無遺。對權(quán)力的尊崇、對物質(zhì)的渴望以及極端的個人主義使得整個社會風(fēng)氣日下。所以，莎士比亞后期的作品則著重表現(xiàn)這個社會對物質(zhì)權(quán)利的膨脹主義以及極端的個人主義。例如，莎士比亞的作品《奧瑟羅》和《李爾王》，對惡棍伊阿古和埃特加以及貢納梨等人物形象的塑造，都體現(xiàn)了作者對極端個人主義的諷刺。《麥克白》也在這樣的歷史背景下應(yīng)運(yùn)而生，這部作品也寄托了作者對個人欲望的膨脹必然導(dǎo)致自我毀滅的思想并將作者對人性的思考推向巔峰。[1]

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

(7)

(8)

(9)

圖3 ECEF坐標(biāo)與NED坐標(biāo)轉(zhuǎn)換示意圖

1.3 交點(diǎn)求解

本文借助地心點(diǎn)(即在ECEF坐標(biāo)系下的原點(diǎn))，與兩個經(jīng)緯度點(diǎn)組成一個平面，求解4個經(jīng)緯度點(diǎn)組成的線段在橢球面上弧線的交點(diǎn)，將問題轉(zhuǎn)換成求兩個面的交線，然后通過式(1)-式(6)反解出交點(diǎn)的經(jīng)緯度[6-7].

根據(jù)3點(diǎn)確定一個平面，可以求出兩個平面的方程，即：

A1X+B1Y+C1Z=0

(10)

(11)

最后，通過求兩平面的交線來求交點(diǎn)的經(jīng)緯度，即：

(12)

(13)

值得注意的是，交線上的經(jīng)緯度是不變的，只需要知道經(jīng)緯度而不需要知道高度.因此，知道交線便可以求出經(jīng)緯度.

1.4 航點(diǎn)計算

當(dāng)已知GPS點(diǎn)Pg=(λ,φ,h)，航向ξ，需要飛行距離D時，為了計算下一個GPS航點(diǎn)Pg,n=(λn,φn,hn)，首先需要把Pg=(λ,φ,h)轉(zhuǎn)換成ECEF坐標(biāo)Pe,ref=(Xe,ref,Ye,ref,Ze,ref).航向是NED坐標(biāo)系下的表述，在NED坐標(biāo)系下一個航點(diǎn)的坐標(biāo)為Pn=(Dcosξ,Dsinξ,-h).將它轉(zhuǎn)換成ECEF坐標(biāo)系下的一個點(diǎn)，利用式(8)、式(9)便可求出ECEF坐標(biāo)系下的下一個航點(diǎn)坐標(biāo)Pe,n=(Xe,n,Ye,n,Ze,n)，最后利用下式：

(14)

可求得Pg,n=(λn,φn,hn).

利用這種算法便可以精確地解出任意知道距離與航向的下一航點(diǎn)經(jīng)緯度.

2 農(nóng)用無人機(jī)的航線規(guī)劃應(yīng)用

目前，農(nóng)用無人機(jī)主要用于田間作物的植保和監(jiān)測.其中，植保無人機(jī)多用于代替人工噴灑農(nóng)藥.本文使用的無人機(jī)飛行控制器為ardupolit(APM)，它是一款社區(qū)維護(hù)的開源飛控.該飛控的自動導(dǎo)航系統(tǒng)基于GPS數(shù)據(jù)，并由慣導(dǎo)補(bǔ)償?shù)玫?使用Ublox Neo-M8N的GPS和MPU6000的慣導(dǎo)可實(shí)現(xiàn)較高的定位精度.由于APM根據(jù)經(jīng)緯度及高度組成的航點(diǎn)序列自動導(dǎo)航，因此需要事先生成一個航點(diǎn)文件，下載到APM中.該航點(diǎn)文件是由一系列的經(jīng)緯度、高度按特定協(xié)議構(gòu)成的.本文的目的是計算無人機(jī)需要飛經(jīng)航點(diǎn)的經(jīng)緯度信息，并規(guī)劃其航線.

用于植保的農(nóng)用無人機(jī)需要將農(nóng)藥均勻地噴灑到農(nóng)田上面，一般采用柵欄方式的飛行軌跡(圖4).絕大多數(shù)的農(nóng)田為四邊形，且地形都相對平整，植保無人機(jī)的作業(yè)空域也幾乎不存在障礙物.因此，可不考慮高空避障的情況，只要使用手持GPS儀讀取農(nóng)田4個頂點(diǎn)的經(jīng)緯度，并設(shè)定航向，在這個四邊形中規(guī)劃柵欄式的航點(diǎn)軌跡即可.首先，在4個定點(diǎn)中選定初始航點(diǎn)，然后按照設(shè)定的航向與四點(diǎn)邊界依次計算下一個航點(diǎn)，再按無人機(jī)的尺寸確定柵欄間隔，依次計算出各邊界上的航點(diǎn).

圖4 柵欄方式的飛行軌跡

3 結(jié)束語

本文列出了大地坐標(biāo)與ECEF坐標(biāo)轉(zhuǎn)換、ECEF坐標(biāo)與NED坐標(biāo)轉(zhuǎn)換的公式.提出了一種已知距離與航向，計算下一個GPS航點(diǎn)經(jīng)緯度的方法.在實(shí)際的植保無人機(jī)應(yīng)用中，該算法精度能夠滿足應(yīng)用要求，具有普遍性，可用于無人機(jī)的航線規(guī)劃.

[1] 薛新宇,梁 建，傅錫敏.我國航空植保技術(shù)的發(fā)展前景[J].中國農(nóng)機(jī)化，2008，4(5):72-74.

[2] 劉劍君，賈世通，杜新武，等.無人機(jī)低空施藥技術(shù)發(fā)展現(xiàn)狀與趨勢[J].農(nóng)業(yè)工程，2014(5)：11-14.

[3] 徐大遠(yuǎn),王英健,陳冠軍,等.四軸飛行器的動力學(xué)建模和位置控制研究[J].電子科技,2015，28(1)：69-72.

[4] 屈耀紅.小型無人機(jī)航跡規(guī)劃及組合導(dǎo)航技術(shù)研究[D].西安：西北工業(yè)大學(xué)，2006.

[5] Farrell J A,Barth M. The global positioning system & inertial navigation[M].Maidenhead:McGraw-Hill Professional,1998.

[6] Kwon J H,Jekeli C.A new approach for airborne vector gravimetry using GPS/INS[J].Journal of Geodesy,2001,74(10):690-700.

[7] Phuong N H Q, Kang H J, Suh Y S, et al. A DCM based orientation estimation algorithm with an inertial measurement unit and a magnetic compass[J]. Journal of Universal Computer Science,2009,15(4):859-876.

Research and Application of Agricultural Unmanned Aerial Vehicle Waypoint Planning Algorithm Based on GPS / INS Navigation

Maloba Nyjot Tshikala, HUANG Li-bing,YUAN Fa-ye, JIANG Xian-zhi

(College of Mechanical Engineering and Automation, Zhejiang Sci-Tech University,Hangzhou 310018,China)

This paper shows the coordinate transformation of geodetic coordinate, Earth-Centered Earth-Fixed (ECEF) coordinate and North East Down (NED) coordinate conversion formulas puts forward a method to calculate the intersection of the arc with constitute off our latitude and longitude points. Another approach is proposed to calculate the GPS waypoint when knows the distance and heading via coordinate transformation. The approach could be used for planning the fence route of agricultural unmanned aerial vehicle (UAV).

GPS/INS; coordinate transformation; agricultural unmanned aerial vehicle; route planning

2016-11-29

浙江理工大學(xué)校企合作資助項(xiàng)目(14020089-J)

Maloba Nyjot Tshikla(1990-)，男，剛果民主共和國人，碩士研究生，研究方向?yàn)闄C(jī)器人和無人機(jī).

1006-3269(2017)02-0009-03

U8

A

10.3969/j.issn.1006-3269.2017.02.003