全自動(dòng)植保無人機(jī)彎道姿態(tài)控制算法

韓 賓， 吳開華， 王文杰

(杭州電子科技大學(xué) 生命信息與儀器工程學(xué)院，浙江 杭州 310018)

0 引 言

多旋翼無人機(jī)[1]具有多變量、強(qiáng)耦合、適應(yīng)能力強(qiáng)等特點(diǎn)。在農(nóng)業(yè)領(lǐng)域中，無人機(jī)多用于噴灑農(nóng)藥、授粉等,特別是多旋翼植保無人機(jī)(下文簡(jiǎn)稱植保機(jī)[2])的噴灑農(nóng)藥應(yīng)用大幅度提高了農(nóng)藥的利用率。

現(xiàn)有植保機(jī)航路規(guī)劃算法[3,4]規(guī)劃的航路多為“幾”字型航路，規(guī)劃算法既簡(jiǎn)單又能實(shí)現(xiàn)農(nóng)田全覆蓋。但其相鄰兩條航路間的連接呈一定角度，不夠平滑，當(dāng)植保機(jī)經(jīng)過轉(zhuǎn)彎點(diǎn)時(shí)，必然要經(jīng)歷先減速到達(dá)轉(zhuǎn)彎點(diǎn)再加速離開的過程，不僅耗費(fèi)大量時(shí)間，而且降低植保作業(yè)效率。因此，需對(duì)彎道處航路和彎道姿態(tài)控制算法進(jìn)行重新規(guī)劃設(shè)計(jì)。

本文提出了彎道處航路的重規(guī)劃算法和彎道姿態(tài)控制算法，設(shè)彎道處噴灑自動(dòng)控制的實(shí)現(xiàn)方法。在保證彎道處噴灑作業(yè)全覆蓋前提下，實(shí)現(xiàn)植保機(jī)高效快速地完成轉(zhuǎn)彎動(dòng)作。

1 算法原理

1.1 多旋翼無人機(jī)的工作原理

根據(jù)飛行姿態(tài)[5]，多旋翼無人機(jī)具有以下 6 種運(yùn)動(dòng)狀態(tài)：俯仰運(yùn)動(dòng)、垂直運(yùn)動(dòng)、翻滾運(yùn)動(dòng)、偏航運(yùn)動(dòng)、前后運(yùn)動(dòng)以及側(cè)向運(yùn)動(dòng)[6]?？梢酝ㄟ^不同運(yùn)動(dòng)狀態(tài)的結(jié)合，使無人機(jī)產(chǎn)生特定的飛行軌跡；再結(jié)合彎道處的航路設(shè)計(jì)，即可在不降速的前提下，實(shí)現(xiàn)多旋翼無人機(jī)的快速轉(zhuǎn)彎。

1.2 彎道航路重規(guī)劃

圖1為重規(guī)劃后的航路及噴灑軌跡。H1，H2，H3為噴灑邊界線；G1，G2點(diǎn)為噴灑邊界交點(diǎn)；L1，L3為規(guī)劃航路直線部分；L2為重規(guī)劃彎道處曲線航路；O點(diǎn)為曲線航路L2圓心；R為L(zhǎng)2半徑，也是植保機(jī)噴灑作業(yè)半徑；A點(diǎn)為彎道姿態(tài)控制起始點(diǎn)、B點(diǎn)為彎道曲線的頂點(diǎn)、C點(diǎn)為彎道姿態(tài)控制結(jié)束點(diǎn)；E1，E2為噴灑覆蓋范圍最大處位置點(diǎn)；D為植保機(jī)實(shí)際位置；d為轉(zhuǎn)彎過程中植保機(jī)位置到噴灑邊界距離；J點(diǎn)為過O，D兩點(diǎn)與噴灑邊界的交點(diǎn)；ω為偏航角。

圖1 彎道航路重規(guī)劃

彎道航路重規(guī)劃的基本原理：以邊界H1，H2，H3為切線，做內(nèi)切圓可得圓心O；以O(shè)點(diǎn)為圓心，R為噴灑作業(yè)半徑，做圓，即可得切點(diǎn)A，C以及曲線L2。曲線L2即為重規(guī)劃后的彎道航路。

1.3 彎道飛行姿態(tài)控制算法

由于植保機(jī)的控制系統(tǒng)屬于欠驅(qū)動(dòng)控制系統(tǒng)，可將植保機(jī)的前進(jìn)方向和前進(jìn)速度作為2個(gè)單獨(dú)變量分開控制，即利用前后運(yùn)動(dòng)控制植保機(jī)的前進(jìn)速度，偏航運(yùn)動(dòng)控制植保機(jī)的前進(jìn)方向。因此，可以在保證前進(jìn)速度不變的前提下，僅通過調(diào)節(jié)偏航速度改變其前進(jìn)方向，使植保機(jī)能夠按照彎道航路L2飛行完成轉(zhuǎn)彎動(dòng)作

ω=vyaw×t

(1)

L=v×t

(2)

L=(π×R×ω)/180

(3)

式(1)表示在時(shí)間t內(nèi)植保機(jī)以偏航速度vyaw飛行所轉(zhuǎn)過的角度為ω；式(2)表示在時(shí)間t內(nèi)，植保機(jī)以飛行速度v前進(jìn)的距離為L(zhǎng)；式(3)為時(shí)間t內(nèi)，角度ω所對(duì)應(yīng)的圓弧長(zhǎng)度為L(zhǎng)。由式(1)～式(3)可推導(dǎo)出彎道航路半徑R與飛行速度v、偏航速度vyaw之間的關(guān)系

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

(4)

即彎道姿態(tài)控制算法中的偏航速度vyaw計(jì)算公式。

植保機(jī)在經(jīng)過彎道航路時(shí)，其飛行姿態(tài)算法控制采用雙線程，主線程循環(huán)檢測(cè)植保機(jī)當(dāng)前位置是否到達(dá)A，B，C3點(diǎn)。當(dāng)植保機(jī)到達(dá)A點(diǎn)時(shí)，調(diào)用彎道姿態(tài)控制線程，進(jìn)入彎道姿態(tài)控制模式；到達(dá)C點(diǎn)時(shí)，退出彎道姿態(tài)控制模式，進(jìn)入直線航路L3繼續(xù)執(zhí)行噴灑作業(yè)任務(wù)。

圖2為彎道姿態(tài)控制。當(dāng)植保機(jī)進(jìn)入彎道姿態(tài)控制模式后，彎道姿態(tài)控制模塊綜合偏航速度、前進(jìn)速度、以及植保機(jī)相對(duì)位置信息，利用式(4)計(jì)算得出植保機(jī)在當(dāng)前位置所需的偏航速度vyaw，并將偏航速度vyaw發(fā)送給脈寬調(diào)制(pulse width modulation,PWM)模塊轉(zhuǎn)換成對(duì)應(yīng)的PWM值輸出調(diào)整旋翼電機(jī)轉(zhuǎn)速，最終植保機(jī)按照彎道航路L2順利完成轉(zhuǎn)彎動(dòng)作。

圖2 彎道姿態(tài)控制

1.4 彎道處噴灑調(diào)控

當(dāng)植保機(jī)進(jìn)入彎道姿態(tài)控制模式后，噴灑半徑需要根據(jù)植保機(jī)的位置進(jìn)行調(diào)整[7]，以便在噴灑邊界的拐角(G1，G2)處也可以順利完成噴灑作業(yè)，避免漏噴現(xiàn)象。

如圖1所示，植保機(jī)到達(dá)A點(diǎn)后，開始計(jì)算植保機(jī)位置到噴灑邊界的距離d值。在A-E1段，d值為植保機(jī)位置到噴灑邊界H1的距離；在E1-E2段，d值為植保機(jī)位置到噴灑邊界H2的距離；E2-C段，d值為植保機(jī)位置到噴灑邊界H3的距離。

設(shè)單個(gè)噴頭的初始噴灑半徑為r；植保機(jī)在過彎道過程中，靠近噴灑邊界一方噴頭的噴灑半徑為a；b表示兩噴頭間交叉作業(yè)部分間距，則a，b，r，d之間關(guān)系為

a=2×r+d-R-b

(5)

植保無人機(jī)在轉(zhuǎn)彎過程中，利用式(5)對(duì)噴頭噴灑半徑進(jìn)行調(diào)整，保證彎道拐角處亦可順利完成噴灑作業(yè)。

2 仿真與田間飛行測(cè)試

2.1 彎道姿態(tài)控制算法仿真測(cè)試

首先，使用Mission Planner地面站規(guī)劃飛行航路并上傳至SITL(software in the loop)模擬器。啟動(dòng)SITL模擬器，模擬無人機(jī)按照規(guī)劃航路飛行。圖3為模擬無人機(jī)按規(guī)劃航路飛行。圖4曲線為植保機(jī)在轉(zhuǎn)彎過程中其實(shí)際位置D到O的距離與規(guī)劃曲線航路半徑R的差值|Y|，其橫坐標(biāo)表示植保機(jī)在過彎道時(shí)的位置，A1位置為彎道開始點(diǎn)A點(diǎn)，A10位置為彎道結(jié)束點(diǎn)C點(diǎn)，A2～A9位置將彎道航路10等分；縱坐標(biāo)為差值|Y|。由圖4可以看出，差值|Y|小于20 cm，符合所設(shè)定的位置誤差要求(不大于50 cm)。

圖3 無人機(jī)模擬飛行航路

圖4 植保機(jī)偏離預(yù)設(shè)航線差值

2.2 噴灑半徑實(shí)驗(yàn)

選用的離心噴頭[8]轉(zhuǎn)速范圍在3 000～9 000 r/min，霧化盤直徑為60 mm。采用傳統(tǒng)方式對(duì)離心噴頭噴幅的大小進(jìn)行測(cè)驗(yàn)，即通過測(cè)量某一高度下噴頭噴霧的覆蓋直徑來獲得噴頭噴幅。噴頭測(cè)驗(yàn)高度設(shè)為120 cm，則噴頭電機(jī)轉(zhuǎn)速與噴幅間關(guān)系如表1所示。

表1 轉(zhuǎn)速與噴幅對(duì)照表

實(shí)驗(yàn)機(jī)型的噴灑裝置采用單排4個(gè)離心噴頭，噴頭噴灑半徑r=1 m，彎道航路半徑R=1.5 m。由式(5)求出彎道處各位置對(duì)應(yīng)的噴頭噴灑半徑大小，結(jié)合表1得出噴頭噴幅與轉(zhuǎn)速間的關(guān)系，擬合出的噴頭電機(jī)轉(zhuǎn)速與植保機(jī)位置關(guān)系，如圖5。轉(zhuǎn)速—位置擬合曲線即為植保機(jī)轉(zhuǎn)彎過程中噴頭轉(zhuǎn)速的調(diào)節(jié)依據(jù)。

圖5 噴幅—位置擬合曲線

2.3 飛行實(shí)驗(yàn)

為了驗(yàn)證所設(shè)計(jì)的彎道飛行姿態(tài)控制算法的實(shí)際應(yīng)用效果，設(shè)計(jì)A，B 2組飛行實(shí)驗(yàn)進(jìn)行對(duì)比分析。A組作為對(duì)照組，其轉(zhuǎn)彎方式采用普通彎道算法，即植保機(jī)先減速靠近轉(zhuǎn)彎點(diǎn)，轉(zhuǎn)彎動(dòng)作完成后加速離開；B組轉(zhuǎn)彎方式采用重規(guī)劃彎道航路再配合對(duì)應(yīng)的彎道姿態(tài)控制算法進(jìn)行轉(zhuǎn)彎。2組實(shí)驗(yàn)除彎道控制算法不同外，具有相同外部條件，如表2。

表2 飛行試驗(yàn)基本參數(shù)

根據(jù)場(chǎng)地特點(diǎn)，設(shè)計(jì)單程航路為100 m ，實(shí)驗(yàn)場(chǎng)地內(nèi)規(guī)劃往返共17道航路。如圖6所示，圖6(a)為采用普通彎道算法的飛行航路；圖6(b)為采用彎道姿態(tài)控制算法的飛行航路。表3為A，B兩組植保機(jī)在相同時(shí)間、速度情況下噴灑作業(yè)的面積。

圖6 實(shí)際飛行航路

組別10次飛行中的作業(yè)面積/m212345678910平均A組29403000293030103020299029702970296030302982B組36303530361036003620357035503600355036103587

從表3中可以看出:在共計(jì)10次飛行中， A組完成的作業(yè)面積平均為2 982 m2，B組完成的作業(yè)面積平均為3 587 m2?？芍?，在相同條件下，相比采用普通轉(zhuǎn)彎方式采用彎道飛行姿態(tài)控制算法轉(zhuǎn)彎植保機(jī)作業(yè)效率提高了20 %。

3 結(jié) 論

針對(duì)傳統(tǒng)多旋翼無人植保機(jī)轉(zhuǎn)彎效率低的問題，重點(diǎn)研究并完成了彎道航路的重新規(guī)劃和轉(zhuǎn)彎過程中的姿態(tài)控制算法設(shè)計(jì)，并設(shè)計(jì)完成了彎道姿態(tài)控制算法的SITL飛行仿真、噴幅—位置擬合曲線以及綜合實(shí)際飛行試驗(yàn)。實(shí)驗(yàn)結(jié)果表明:利用重新規(guī)劃的彎道航路與轉(zhuǎn)彎飛行姿態(tài)控制算法相配合，可以在不降速的情況下實(shí)現(xiàn)轉(zhuǎn)彎動(dòng)作。相比“幾”字型彎道控制，作業(yè)效率提高了20 %，為植保機(jī)田間實(shí)際噴灑作業(yè)提供了理論和實(shí)現(xiàn)基礎(chǔ)。

參考文獻(xiàn):

[1] 于雅莉,孫 楓,王元昔.基于多傳感器的四旋翼飛行器硬件電路設(shè)計(jì)[J].傳感器與微系統(tǒng),2011,30(8):113-115,123.

[2] 溫 源,張向東,沈建文,等.中國(guó)植保無人機(jī)發(fā)展技術(shù)路線及行業(yè)趨勢(shì)[J].農(nóng)業(yè)技術(shù)與裝備,2014(281):37-38.

[3] 萬順飛,祖家奎,劉世隆.農(nóng)用植保無人機(jī)直升機(jī)飛行航路的自動(dòng)規(guī)劃算法與實(shí)現(xiàn)[J].電子測(cè)量技術(shù),2016,39(4):36-41.

[4] 陳雯雯.小型四旋翼無人機(jī)軌跡規(guī)劃算法研究[D].青島：青島理工大學(xué),2015.

[5] 李 輝,蘆利斌,金國(guó)棟.基于Kinect的四旋翼無人機(jī)體感控制[J].傳感器與微系統(tǒng),2015,34(8):99-102.

[6] 周立新,薛新宇,孫 竹,等.無人機(jī)直升機(jī)農(nóng)藥噴灑系統(tǒng)的設(shè)計(jì)及應(yīng)用[J].江蘇農(nóng)業(yè)科學(xué),2013,41(10):396-368.

[7] 范慶妮.小型無人直升機(jī)農(nóng)藥霧化系統(tǒng)的研究[D].南京：南京林業(yè)大學(xué),2011.

[8] 張吉利.農(nóng)用航空噴霧系統(tǒng)的試驗(yàn)與研究[J].農(nóng)業(yè)技術(shù)與裝備,2013,41(10):396-368.