全自動植保無人機彎道姿態控制算法

韓 賓， 吳開華， 王文杰

(杭州電子科技大學 生命信息與儀器工程學院，浙江 杭州 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.