基于GPS/INS导航的农用无人机航点规划算法研究及应用

Maloba Nyjot Tshikala，黄李炳，袁发业，江先志

(浙江理工大学 机械与自动控制学院，浙江 杭州 310018)

基于GPS/INS导航的农用无人机航点规划算法研究及应用

Maloba Nyjot Tshikala，黄李炳，袁发业，江先志

(浙江理工大学 机械与自动控制学院，浙江 杭州 310018)

讨论大地坐标与ECEF坐标的转换、ECEF坐标与NED坐标的转换.提出了4个经纬度点构成的两条线段交点经纬度的算法，以及已知航向与距离的航点经纬度的算法.将算法应用于农用四轴飞行器，规划了栅栏式的航线.

GPS/INS；坐标转换；农用四轴飞行器；航线规划

在农作物生长过程中，病虫害的有效防控是保证粮食安全生产的重要环节，直接影响粮食的产量与质量[1].无人机低空施药作为一种新型的防治病虫害手段，正在成为农机行业新的亮点和热点[2].同时，随着传感器技术、微机电技术的发展，四轴飞行器的研发成为了国内外研究的热点[3].目前大部分四轴飞行器基于GPS/INS导航[4]，通常事先设置一系列的经纬度航点，起飞前将其导入四轴飞行器，起飞后四轴飞行器将按设定好的经纬度路径飞行.因此，精确地计算经纬度航点至关重要.GPS的经纬度点在空间中位于一个椭球面(地球)上，计算它们之间的距离、方向和位置较为困难.如果忽略地表曲面的影响直接推算无人机飞行过程中的关键航点，实际的飞行试验中就会发现存在较大误差.本文的研究重点在于通过坐标变换、空间几何计算等精确地规划四轴无人机的GPS航点，并将该航点算法应用于农业植保无人机(图1).

图1 应用航点算法的无人机

1 航点算法

1.1 大地坐标与ECEF坐标的转换

由于经纬度是对地球表面地理位置的描述，并不涉及实际的距离和位置计算，因此这里需要将经纬度及高度转换成地心地固(ECEF)坐标，在ECEF坐标系下规划航点(图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坐标系下的坐标点；Pg=(λ,φ,h)为对应点的经纬度及高度；a为地球长轴的半径，它的值是6 378 137m；b为地球短轴半径，它的值是6 356 752.314 2m；e为偏心率.

图2 大地坐标与ECEF坐标转换示意图

1.2 ECEF坐标与NED坐标的转换

ECEF坐标系下无法做相对地面的方向计算，需要转换成NED(North East Down)坐标系，如图3所示.

《麦克白》是莎士比亚创作后期的一部文学作品，创作于1606年。莎士比亚早期的作品着重于对人文精神和个性解放的宣扬。可是，由于资本主义社会的发展，激烈的社会矛盾暴露无遗。对权力的尊崇、对物质的渴望以及极端的个人主义使得整个社会风气日下。所以，莎士比亚后期的作品则着重表现这个社会对物质权利的膨胀主义以及极端的个人主义。例如，莎士比亚的作品《奥瑟罗》和《李尔王》，对恶棍伊阿古和埃特加以及贡纳梨等人物形象的塑造，都体现了作者对极端个人主义的讽刺。《麦克白》也在这样的历史背景下应运而生，这部作品也寄托了作者对个人欲望的膨胀必然导致自我毁灭的思想并将作者对人性的思考推向巅峰。[1]

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

(7)

(8)

(9)

图3 ECEF坐标与NED坐标转换示意图

1.3 交点求解

本文借助地心点(即在ECEF坐标系下的原点)，与两个经纬度点组成一个平面，求解4个经纬度点组成的线段在椭球面上弧线的交点，将问题转换成求两个面的交线，然后通过式(1)-式(6)反解出交点的经纬度[6-7].

根据3点确定一个平面，可以求出两个平面的方程，即：

A1X+B1Y+C1Z=0

(10)

(11)

最后，通过求两平面的交线来求交点的经纬度，即：

(12)

(13)

值得注意的是，交线上的经纬度是不变的，只需要知道经纬度而不需要知道高度.因此，知道交线便可以求出经纬度.

1.4 航点计算

当已知GPS点Pg=(λ,φ,h)，航向ξ，需要飞行距离D时，为了计算下一个GPS航点Pg,n=(λn,φn,hn)，首先需要把Pg=(λ,φ,h)转换成ECEF坐标Pe,ref=(Xe,ref,Ye,ref,Ze,ref).航向是NED坐标系下的表述，在NED坐标系下一个航点的坐标为Pn=(Dcosξ,Dsinξ,-h).将它转换成ECEF坐标系下的一个点，利用式(8)、式(9)便可求出ECEF坐标系下的下一个航点坐标Pe,n=(Xe,n,Ye,n,Ze,n)，最后利用下式：

(14)

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

利用这种算法便可以精确地解出任意知道距离与航向的下一航点经纬度.

2 农用无人机的航线规划应用

目前，农用无人机主要用于田间作物的植保和监测.其中，植保无人机多用于代替人工喷洒农药.本文使用的无人机飞行控制器为ardupolit(APM)，它是一款社区维护的开源飞控.该飞控的自动导航系统基于GPS数据，并由惯导补偿得到.使用Ublox Neo-M8N的GPS和MPU6000的惯导可实现较高的定位精度.由于APM根据经纬度及高度组成的航点序列自动导航，因此需要事先生成一个航点文件，下载到APM中.该航点文件是由一系列的经纬度、高度按特定协议构成的.本文的目的是计算无人机需要飞经航点的经纬度信息，并规划其航线.

用于植保的农用无人机需要将农药均匀地喷洒到农田上面，一般采用栅栏方式的飞行轨迹(图4).绝大多数的农田为四边形，且地形都相对平整，植保无人机的作业空域也几乎不存在障碍物.因此，可不考虑高空避障的情况，只要使用手持GPS仪读取农田4个顶点的经纬度，并设定航向，在这个四边形中规划栅栏式的航点轨迹即可.首先，在4个定点中选定初始航点，然后按照设定的航向与四点边界依次计算下一个航点，再按无人机的尺寸确定栅栏间隔，依次计算出各边界上的航点.

图4 栅栏方式的飞行轨迹

3 结束语

本文列出了大地坐标与ECEF坐标转换、ECEF坐标与NED坐标转换的公式.提出了一种已知距离与航向，计算下一个GPS航点经纬度的方法.在实际的植保无人机应用中，该算法精度能够满足应用要求，具有普遍性，可用于无人机的航线规划.

[1] 薛新宇,梁 建，傅锡敏.我国航空植保技术的发展前景[J].中国农机化，2008，4(5):72-74.

[2] 刘剑君，贾世通，杜新武，等.无人机低空施药技术发展现状与趋势[J].农业工程，2014(5)：11-14.

[3] 徐大远,王英健,陈冠军,等.四轴飞行器的动力学建模和位置控制研究[J].电子科技,2015，28(1)：69-72.

[4] 屈耀红.小型无人机航迹规划及组合导航技术研究[D].西安：西北工业大学，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

浙江理工大学校企合作资助项目(14020089-J)

Maloba Nyjot Tshikla(1990-)，男，刚果民主共和国人，硕士研究生，研究方向为机器人和无人机.

1006-3269(2017)02-0009-03

U8

A

10.3969/j.issn.1006-3269.2017.02.003