全自动植保无人机弯道姿态控制算法

韩 宾， 吴开华， 王文杰

(杭州电子科技大学 生命信息与仪器工程学院，浙江 杭州 310018)

0 引 言

多旋翼无人机[1]具有多变量、强耦合、适应能力强等特点。在农业领域中，无人机多用于喷洒农药、授粉等,特别是多旋翼植保无人机(下文简称植保机[2])的喷洒农药应用大幅度提高了农药的利用率。

现有植保机航路规划算法[3,4]规划的航路多为“几”字型航路，规划算法既简单又能实现农田全覆盖。但其相邻两条航路间的连接呈一定角度，不够平滑，当植保机经过转弯点时，必然要经历先减速到达转弯点再加速离开的过程，不仅耗费大量时间，而且降低植保作业效率。因此，需对弯道处航路和弯道姿态控制算法进行重新规划设计。

本文提出了弯道处航路的重规划算法和弯道姿态控制算法，设弯道处喷洒自动控制的实现方法。在保证弯道处喷洒作业全覆盖前提下，实现植保机高效快速地完成转弯动作。

1 算法原理

1.1 多旋翼无人机的工作原理

根据飞行姿态[5]，多旋翼无人机具有以下 6 种运动状态：俯仰运动、垂直运动、翻滚运动、偏航运动、前后运动以及侧向运动[6]。可以通过不同运动状态的结合，使无人机产生特定的飞行轨迹；再结合弯道处的航路设计，即可在不降速的前提下，实现多旋翼无人机的快速转弯。

1.2 弯道航路重规划

图1为重规划后的航路及喷洒轨迹。H1，H2，H3为喷洒边界线；G1，G2点为喷洒边界交点；L1，L3为规划航路直线部分；L2为重规划弯道处曲线航路；O点为曲线航路L2圆心；R为L2半径，也是植保机喷洒作业半径；A点为弯道姿态控制起始点、B点为弯道曲线的顶点、C点为弯道姿态控制结束点；E1，E2为喷洒覆盖范围最大处位置点；D为植保机实际位置；d为转弯过程中植保机位置到喷洒边界距离；J点为过O，D两点与喷洒边界的交点；ω为偏航角。

图1 弯道航路重规划

弯道航路重规划的基本原理：以边界H1，H2，H3为切线，做内切圆可得圆心O；以O点为圆心，R为喷洒作业半径，做圆，即可得切点A，C以及曲线L2。曲线L2即为重规划后的弯道航路。

1.3 弯道飞行姿态控制算法

由于植保机的控制系统属于欠驱动控制系统，可将植保机的前进方向和前进速度作为2个单独变量分开控制，即利用前后运动控制植保机的前进速度，偏航运动控制植保机的前进方向。因此，可以在保证前进速度不变的前提下，仅通过调节偏航速度改变其前进方向，使植保机能够按照弯道航路L2飞行完成转弯动作

ω=vyaw×t

(1)

L=v×t

(2)

L=(π×R×ω)/180

(3)

式(1)表示在时间t内植保机以偏航速度vyaw飞行所转过的角度为ω；式(2)表示在时间t内，植保机以飞行速度v前进的距离为L；式(3)为时间t内，角度ω所对应的圆弧长度为L。由式(1)～式(3)可推导出弯道航路半径R与飞行速度v、偏航速度vyaw之间的关系

vyaw=v×180/(π×R×t)

(4)

即弯道姿态控制算法中的偏航速度vyaw计算公式。

植保机在经过弯道航路时，其飞行姿态算法控制采用双线程，主线程循环检测植保机当前位置是否到达A，B，C3点。当植保机到达A点时，调用弯道姿态控制线程，进入弯道姿态控制模式；到达C点时，退出弯道姿态控制模式，进入直线航路L3继续执行喷洒作业任务。

图2为弯道姿态控制。当植保机进入弯道姿态控制模式后，弯道姿态控制模块综合偏航速度、前进速度、以及植保机相对位置信息，利用式(4)计算得出植保机在当前位置所需的偏航速度vyaw，并将偏航速度vyaw发送给脉宽调制(pulse width modulation,PWM)模块转换成对应的PWM值输出调整旋翼电机转速，最终植保机按照弯道航路L2顺利完成转弯动作。

图2 弯道姿态控制

1.4 弯道处喷洒调控

当植保机进入弯道姿态控制模式后，喷洒半径需要根据植保机的位置进行调整[7]，以便在喷洒边界的拐角(G1，G2)处也可以顺利完成喷洒作业，避免漏喷现象。

如图1所示，植保机到达A点后，开始计算植保机位置到喷洒边界的距离d值。在A-E1段，d值为植保机位置到喷洒边界H1的距离；在E1-E2段，d值为植保机位置到喷洒边界H2的距离；E2-C段，d值为植保机位置到喷洒边界H3的距离。

设单个喷头的初始喷洒半径为r；植保机在过弯道过程中，靠近喷洒边界一方喷头的喷洒半径为a；b表示两喷头间交叉作业部分间距，则a，b，r，d之间关系为

a=2×r+d-R-b

(5)

植保无人机在转弯过程中，利用式(5)对喷头喷洒半径进行调整，保证弯道拐角处亦可顺利完成喷洒作业。

2 仿真与田间飞行测试

2.1 弯道姿态控制算法仿真测试

首先，使用Mission Planner地面站规划飞行航路并上传至SITL(software in the loop)模拟器。启动SITL模拟器，模拟无人机按照规划航路飞行。图3为模拟无人机按规划航路飞行。图4曲线为植保机在转弯过程中其实际位置D到O的距离与规划曲线航路半径R的差值|Y|，其横坐标表示植保机在过弯道时的位置，A1位置为弯道开始点A点，A10位置为弯道结束点C点，A2～A9位置将弯道航路10等分；纵坐标为差值|Y|。由图4可以看出，差值|Y|小于20 cm，符合所设定的位置误差要求(不大于50 cm)。

图3 无人机模拟飞行航路

图4 植保机偏离预设航线差值

2.2 喷洒半径实验

选用的离心喷头[8]转速范围在3 000～9 000 r/min，雾化盘直径为60 mm。采用传统方式对离心喷头喷幅的大小进行测验，即通过测量某一高度下喷头喷雾的覆盖直径来获得喷头喷幅。喷头测验高度设为120 cm，则喷头电机转速与喷幅间关系如表1所示。

表1 转速与喷幅对照表

实验机型的喷洒装置采用单排4个离心喷头，喷头喷洒半径r=1 m，弯道航路半径R=1.5 m。由式(5)求出弯道处各位置对应的喷头喷洒半径大小，结合表1得出喷头喷幅与转速间的关系，拟合出的喷头电机转速与植保机位置关系，如图5。转速—位置拟合曲线即为植保机转弯过程中喷头转速的调节依据。

图5 喷幅—位置拟合曲线

2.3 飞行实验

为了验证所设计的弯道飞行姿态控制算法的实际应用效果，设计A，B 2组飞行实验进行对比分析。A组作为对照组，其转弯方式采用普通弯道算法，即植保机先减速靠近转弯点，转弯动作完成后加速离开；B组转弯方式采用重规划弯道航路再配合对应的弯道姿态控制算法进行转弯。2组实验除弯道控制算法不同外，具有相同外部条件，如表2。

表2 飞行试验基本参数

根据场地特点，设计单程航路为100 m ，实验场地内规划往返共17道航路。如图6所示，图6(a)为采用普通弯道算法的飞行航路；图6(b)为采用弯道姿态控制算法的飞行航路。表3为A，B两组植保机在相同时间、速度情况下喷洒作业的面积。

图6 实际飞行航路

组别10次飞行中的作业面积/m212345678910平均A组29403000293030103020299029702970296030302982B组36303530361036003620357035503600355036103587

从表3中可以看出:在共计10次飞行中， A组完成的作业面积平均为2 982 m2，B组完成的作业面积平均为3 587 m2。可知，在相同条件下，相比采用普通转弯方式采用弯道飞行姿态控制算法转弯植保机作业效率提高了20 %。

3 结 论

针对传统多旋翼无人植保机转弯效率低的问题，重点研究并完成了弯道航路的重新规划和转弯过程中的姿态控制算法设计，并设计完成了弯道姿态控制算法的SITL飞行仿真、喷幅—位置拟合曲线以及综合实际飞行试验。实验结果表明:利用重新规划的弯道航路与转弯飞行姿态控制算法相配合，可以在不降速的情况下实现转弯动作。相比“几”字型弯道控制，作业效率提高了20 %，为植保机田间实际喷洒作业提供了理论和实现基础。

参考文献:

[1] 于雅莉,孙 枫,王元昔.基于多传感器的四旋翼飞行器硬件电路设计[J].传感器与微系统,2011,30(8):113-115,123.

[2] 温 源,张向东,沈建文,等.中国植保无人机发展技术路线及行业趋势[J].农业技术与装备,2014(281):37-38.

[3] 万顺飞,祖家奎,刘世隆.农用植保无人机直升机飞行航路的自动规划算法与实现[J].电子测量技术,2016,39(4):36-41.

[4] 陈雯雯.小型四旋翼无人机轨迹规划算法研究[D].青岛：青岛理工大学,2015.

[5] 李 辉,芦利斌,金国栋.基于Kinect的四旋翼无人机体感控制[J].传感器与微系统,2015,34(8):99-102.

[6] 周立新,薛新宇,孙 竹,等.无人机直升机农药喷洒系统的设计及应用[J].江苏农业科学,2013,41(10):396-368.

[7] 范庆妮.小型无人直升机农药雾化系统的研究[D].南京：南京林业大学,2011.

[8] 张吉利.农用航空喷雾系统的试验与研究[J].农业技术与装备,2013,41(10):396-368.