面向丘陵山地果樹(shù)植株的植保無(wú)人機(jī)軌跡跟蹤控制器設(shè)計(jì)*

陳再勵(lì) 李麗麗 鐘震宇

?

面向丘陵山地果樹(shù)植株的植保無(wú)人機(jī)軌跡跟蹤控制器設(shè)計(jì)*

陳再勵(lì)1,2李麗麗3鐘震宇1

（1.廣東省智能制造研究所 2.華南智能機(jī)器人創(chuàng)新研究院 3.順德職業(yè)技術(shù)學(xué)院）

針對(duì)小型四旋翼無(wú)人機(jī)在丘陵山地果樹(shù)植保作業(yè)場(chǎng)景下的自主飛行控制問(wèn)題，設(shè)計(jì)一種基于反步法的軌跡跟蹤控制器。根據(jù)丘陵山地果園植保特點(diǎn)，規(guī)劃無(wú)人機(jī)作業(yè)軌跡；基于小型四旋翼無(wú)人機(jī)非線性模型，采用反步法設(shè)計(jì)得到力和力矩控制量，并進(jìn)一步得到電機(jī)轉(zhuǎn)速控制量；通過(guò)李雅普諾夫穩(wěn)定性定理分析了系統(tǒng)的穩(wěn)定性；最后通過(guò)仿真，驗(yàn)證了控制器具有良好的軌跡跟蹤控制性能。

四旋翼無(wú)人機(jī)；軌跡跟蹤；反步法；植保無(wú)人機(jī)

0 引言

病蟲害防治是農(nóng)林生產(chǎn)不可或缺的一個(gè)重要環(huán)節(jié)，針對(duì)丘陵山地等復(fù)雜地形，以小型多旋翼無(wú)人機(jī)為載體，搭載農(nóng)藥噴施設(shè)備的植保無(wú)人機(jī)，具有結(jié)構(gòu)簡(jiǎn)單、使用門檻低、垂直起降、低空低速懸停、場(chǎng)地受限小、地形適應(yīng)性強(qiáng)、噴施效率高、霧化效果好和使用成本低等特點(diǎn)，可適應(yīng)丘陵山地等復(fù)雜地形并有效實(shí)現(xiàn)農(nóng)藥的減施增效[1]。因此，多旋翼無(wú)人機(jī)在農(nóng)林植保領(lǐng)域具有較高的應(yīng)用價(jià)值，近年來(lái)得到了國(guó)內(nèi)外研究者的重點(diǎn)關(guān)注。

當(dāng)前國(guó)內(nèi)外關(guān)于植保無(wú)人機(jī)開(kāi)展的研究工作主要集中在農(nóng)業(yè)低空遙感[2]、霧滴沉積漂移規(guī)律[3]、農(nóng)田信息監(jiān)測(cè)[4]和變量噴施控制[5]等方面，缺乏關(guān)于植保無(wú)人機(jī)航線規(guī)劃與軌跡跟蹤的自主飛行控制問(wèn)題的研究。目前植保無(wú)人機(jī)作業(yè)，尤其在丘陵山地環(huán)境作業(yè)，以操控人員地面遙控為主，對(duì)操控人員要求高、依賴大，在實(shí)際作業(yè)過(guò)程中存在作業(yè)強(qiáng)度大、遙控延時(shí)和視距誤差等問(wèn)題，導(dǎo)致飛行航線偏離、漏噴重噴，影響植保作業(yè)效率效果。

本文以丘陵山地果園環(huán)境中植保無(wú)人機(jī)自主飛行及高效作業(yè)噴施為目標(biāo)，基于小型四旋翼無(wú)人機(jī)非線性模型[6]，采用反步法[7]設(shè)計(jì)無(wú)人機(jī)軌跡跟蹤控制器。根據(jù)作業(yè)任務(wù)和場(chǎng)景特點(diǎn)設(shè)計(jì)針對(duì)果樹(shù)植株的環(huán)繞噴施軌跡，合理規(guī)劃噴施作業(yè)航線；滿足小型四旋翼無(wú)人機(jī)位置輸出調(diào)節(jié)快速響應(yīng)，保證無(wú)人機(jī)沿此航線精準(zhǔn)自主飛行，實(shí)現(xiàn)高精度軌跡跟蹤控制，保障丘陵山地植保作業(yè)的精度。

1 航跡規(guī)劃

傳統(tǒng)植保無(wú)人機(jī)航跡是根據(jù)作業(yè)區(qū)域范圍，配合飛行速度、噴施流量等參數(shù)進(jìn)行設(shè)計(jì)，一般以弓字型路線，往復(fù)飛行覆蓋工作區(qū)域，這種方法適合平原大田作業(yè)?？紤]丘陵山地坡度不平、作物種植田塊小、分散性大、不規(guī)則且果樹(shù)植株長(zhǎng)勢(shì)不一的特性，傳統(tǒng)植保作業(yè)航線難以滿足要求，需要對(duì)丘陵山地果樹(shù)植保無(wú)人機(jī)的作業(yè)航線進(jìn)行重新規(guī)劃。

圖1 環(huán)繞噴施航跡圖

得到期望的航跡方程表達(dá)式為

單株果樹(shù)的環(huán)繞噴施航跡規(guī)劃完成后，整個(gè)果園的作業(yè)任務(wù)航線可分解為一個(gè)個(gè)單獨(dú)的作業(yè)任務(wù)執(zhí)行。如，到達(dá)果樹(shù)A完成環(huán)繞噴施作業(yè)后，無(wú)人機(jī)平行移動(dòng)到果樹(shù)B再次進(jìn)行作業(yè)；完成一壟果樹(shù)的作業(yè)后移動(dòng)到下一壟，以此類推，組合得到整個(gè)果園果樹(shù)植保作業(yè)的航跡。不同果樹(shù)的環(huán)繞噴施參數(shù)可通過(guò)低空遙感采集數(shù)據(jù)得到，整個(gè)丘陵山地果園植保無(wú)人機(jī)作業(yè)復(fù)雜航跡的規(guī)劃不在此展開(kāi)討論。

2 四旋翼無(wú)人機(jī)系統(tǒng)模型

四旋翼飛行器機(jī)械結(jié)構(gòu)簡(jiǎn)單，通過(guò)4個(gè)電機(jī)轉(zhuǎn)速變化改變升力，通過(guò)電機(jī)間的差速產(chǎn)生力矩變化調(diào)整角速度和姿態(tài)，實(shí)現(xiàn)無(wú)人機(jī)的平動(dòng)和轉(zhuǎn)動(dòng)。四旋翼無(wú)人機(jī)系統(tǒng)簡(jiǎn)圖如圖2所示。

圖2 四旋翼無(wú)人機(jī)系統(tǒng)簡(jiǎn)圖

其中，1、3號(hào)電機(jī)順時(shí)針旋轉(zhuǎn)，2、4號(hào)電機(jī)逆時(shí)針旋轉(zhuǎn)，無(wú)人機(jī)參考坐標(biāo)系為固定于地球的慣性坐標(biāo)系。四旋翼無(wú)人機(jī)是典型的多入多出、強(qiáng)耦合的欠驅(qū)動(dòng)系統(tǒng)，其非線性模型可通過(guò)剛體動(dòng)力學(xué)模型、力和力矩表達(dá)式、電機(jī)動(dòng)力模型來(lái)表述。

小型四旋翼無(wú)人機(jī)可視為一個(gè)六自由度的剛體，在慣性坐標(biāo)系下滿足

小型四旋翼無(wú)人機(jī)的單翼升力與電機(jī)轉(zhuǎn)速滿足的動(dòng)力學(xué)關(guān)系為

在滿足四旋翼結(jié)構(gòu)對(duì)稱且剛性的條件下，單翼升力與作用在機(jī)體上的外力、外力矩滿足

3 控制器設(shè)計(jì)

控制器系統(tǒng)框圖如圖3所示。先基于小型無(wú)人直升機(jī)的動(dòng)力學(xué)方程推導(dǎo)位置控制律；再代入槳葉揮舞動(dòng)力學(xué)及電機(jī)動(dòng)力學(xué)方程求得最終的控制量表達(dá)式。

圖3 控制器系統(tǒng)框圖

其中是虛擬控制量。

根據(jù)位置輸出調(diào)節(jié)方程，得到

滿足正定條件，對(duì)其求導(dǎo)，得到

將式(6)和式(7)代入到式(5)中，可以驗(yàn)證

滿足正定條件，求導(dǎo)

將式(11)代入到式(9)中，得到

將式(11)代入到式(10)中，得到

至此，已經(jīng)根據(jù)反步法完成了無(wú)人機(jī)軌跡跟蹤控制器設(shè)計(jì)。下面利用李雅普諾夫穩(wěn)定性第二方法進(jìn)行系統(tǒng)穩(wěn)定性的分析驗(yàn)證。

系統(tǒng)的李雅普諾夫候選函數(shù)為

根據(jù)李雅普諾夫穩(wěn)定性定理，系統(tǒng)漸進(jìn)穩(wěn)定。在有限時(shí)間內(nèi)，小型四旋翼無(wú)人機(jī)的位置、速度和角速度都會(huì)收斂到期望的狀態(tài)，最終實(shí)現(xiàn)穩(wěn)定的軌跡跟蹤控制。

4 仿真與分析

根據(jù)本文提出的果樹(shù)環(huán)繞噴施航跡及軌跡控制器設(shè)計(jì)方法，利用MATLAB/SIMULINK仿真軟件搭建仿真實(shí)驗(yàn)平臺(tái)，對(duì)軌跡跟蹤控制器的性能進(jìn)行仿真驗(yàn)證。在仿真中，規(guī)劃航跡設(shè)置為

仿真使用的小型四旋翼植保無(wú)人機(jī)機(jī)型為GIA1707，具體參數(shù)如表1所示。

表1 GIA1707型植保四旋翼無(wú)人機(jī)參數(shù)

仿真過(guò)程：軌跡規(guī)劃器生成軌跡后，無(wú)人機(jī)在初始位置（5 m，0 m，0 m）處起飛；在8 s左右，飛行高度到達(dá)目標(biāo)高度2.1 m并全程穩(wěn)定；同時(shí)無(wú)人機(jī)在水平面內(nèi)沿期望軌跡實(shí)現(xiàn)環(huán)繞飛行，完成環(huán)繞噴施作業(yè)動(dòng)作。

圖4 無(wú)人機(jī)位置

圖5 無(wú)人機(jī)飛行軌跡位置誤差

圖6 無(wú)人機(jī)速度

圖7 無(wú)人機(jī)速度誤差

圖8 無(wú)人機(jī)環(huán)繞噴施三維軌跡

由圖4和圖6可以看出，無(wú)人機(jī)的位置、速度跟蹤效果較好；圖5和圖7的位置誤差、速度誤差數(shù)據(jù)反映出軌跡跟蹤控制器具有良好的穩(wěn)定性和響應(yīng)快速性，軌跡跟蹤控制器性能優(yōu)秀。

5 結(jié)論

本文設(shè)計(jì)了一種基于反步法的小型四旋翼植保無(wú)人機(jī)軌跡控制器，服務(wù)于丘陵山地果樹(shù)植株植保。針對(duì)丘陵山地的環(huán)境特殊性規(guī)劃無(wú)人機(jī)果樹(shù)環(huán)繞噴施的航線，能夠在一定程度上解決丘陵山地采用傳統(tǒng)目視操作作業(yè)航線準(zhǔn)確率低、重噴漏噴率高的問(wèn)題；基于四旋翼無(wú)人機(jī)非線性模型采用反步法設(shè)計(jì)得到控制律，通過(guò)理論分析和仿真實(shí)驗(yàn)驗(yàn)證了所設(shè)計(jì)的控制器具有漸進(jìn)穩(wěn)定性?；诜抡娼Y(jié)果表明，該植保無(wú)人機(jī)航跡跟蹤控制算法收斂性好、穩(wěn)態(tài)誤差小、響應(yīng)速率快，控制器性能理想、軌跡跟蹤特性強(qiáng)。后續(xù)將進(jìn)一步考慮無(wú)人機(jī)不確定性和陣風(fēng)等干擾因素，實(shí)現(xiàn)抗風(fēng)干擾控制，解決丘陵山地環(huán)境下陣風(fēng)干擾對(duì)飛行精度的影響，提升丘陵山地果樹(shù)植保無(wú)人機(jī)的飛行作業(yè)效率和植保作業(yè)效果。

[1] 周志艷,臧英,羅錫文,等.中國(guó)農(nóng)業(yè)航空植保產(chǎn)業(yè)技術(shù)創(chuàng)新發(fā)展戰(zhàn)略[J].農(nóng)業(yè)工程學(xué)報(bào),2013,29(24):1-10.

[2] 白由路,楊俐蘋,王磊,等.農(nóng)業(yè)低空遙感技術(shù)及其應(yīng)用前景[J].農(nóng)業(yè)網(wǎng)絡(luò)信息,2010(1):5-7.

[3] 陳盛德,蘭玉彬,李繼宇,等.小型無(wú)人直升機(jī)噴霧參數(shù)對(duì)雜交水稻冠層霧滴沉積分布的影響[J].農(nóng)業(yè)工程學(xué)報(bào),2016,32 (17):40-46.

[4] Dong Mianxiong, Ota Kaoru, Lin Man, et al. UAV-assisted data gathering in wireless sensor networks[J]. Journal of Super-computing, 2014,70(3):1142-1155.

[5] Wang L, Lan Y, Hoffmann W C, et al. Design of variable spraying system and influencing factors on droplets deposition of small UAV[J]. Transactions of the Chinese Society for Agricultural Machinery, 2016.

[6] 孟佳東,趙志剛.小型四旋翼無(wú)人機(jī)建模與控制仿真[J].蘭州交通大學(xué)學(xué)報(bào),2013,32(1):63-67.

[7] Pounds P, Mahony R, Corke P. Modelling and control of a large quadrotor robot[J]. Control Engineering Practice, 2010,18,(7): 691-699.

The Design of a Trajectory Tracking Controller for Plant Protection UAV of Fruit Trees in Hilly Area

Chen Zaili1,2Li Lili3Zhong Zhenyu1

(1.Guangdong Institute of Intelligent Manufacturing 2.South China Robotics Innovation Research Institute 3.Shunde Polytechnic)

Aiming at the problem of autonomous flight control of a small-scale unmanned autonomous agricultural drone in upland environment, a trajectory tracking controller based on back stepping method is proposed. According to the characteristics of plant protection in upland environment, a special elliptical flight route is designed. Based on the rigid body dynamics of the unmanned quadcopter nonlinear model, the force and torque control quantities are designed by the back stepping method, and the motor speed control variable is further obtained. The controller system proved to be asympotical stable by Lyapunov stability theorem. Finally, the simulation results proves that the controller has good trajectory tracking control performance.

Quadcopter UAV; Trajectory Tracking; Back Stepping; Plant Protection UAV

陳再勵(lì)，男，1989年生，碩士，工程師，主要研究方向：智能控制與系統(tǒng)、農(nóng)業(yè)航空、信息化等。E-mail: zl.chen@scri.ac.cn

李麗麗，女，1981年生，碩士，教師，研究方向：機(jī)器視覺(jué)與控制。

鐘震宇，男，1971年生，博士，研究員，主要研究方向：運(yùn)動(dòng)控制、機(jī)器視覺(jué)。

廣東省科技計(jì)劃項(xiàng)目（2015B010917001，2017B10117012）；佛山市科技創(chuàng)新專項(xiàng)資金（2013HK100113）。