果園巡檢機器人長臂抖動抑制方法

姜海勇，姜文光，邢雅周，李 娜，楊 欣

（1. 燕山大學河北省輕質結構裝備設計與制備工藝技術創新中心，秦皇島，066004；2. 燕山大學機械工程學院，秦皇島，066004；3. 河北農業大學機電工程學院，保定，071001）

0 引 言

現代果樹培育生產體系中，巡檢機器人是重要的組成部分之一，主要完成果樹圖像自動采集和相關環境信息提取與上報等功能[1-2]。矮砧密植蘋果園冠層高度約為2.5～4 m，采用桁架結構機器人進行圖像采集時，受行間距限制觀測范圍小且成本高；采用無人機則難以在風雨條件下開展圖像采集工作。為了研究諸如落花落果原因及防治措施，特別是在風雨等氣候條件下進行監測，迫切需要一款機動性強、成本低廉而穩定性較高的果園巡檢機器人。

長臂結構具有輕量化優勢，但剛度低帶來的不穩定問題顯著，采用增加用料提高剛度或者增加阻尼機構進行被動減振的方式，不適應輕量化發展的需求，以合理的控制方法改善臂體的穩定性是重要的發展方向。結構振動抑制方法主要有開環和閉環兩類，張美艷等[3]采用輸入整形法，通過輸入量的規劃使二自由度機械臂在運動過程中保持穩定。Li等[4]針對高空作業平臺在旋轉運動過程中的橫向抖動問題，將臂體等效為擺動的單梁，基于拉格朗日動力學模型，在變位啟停階段采用傅里葉級數規劃軌跡。反饋控制在不同結構的振動抑制過程中形式多樣，如李君等[5]針對果園鏈索的抖動問題，采用Hamilton方法建立了鏈鎖行進與橫向抖動的動力學耦合模型，基于圖像反饋鏈索的抖動，采用步進電機驅動機構，實現了在5個周期內快速抑制鏈鎖抖動的效果。薛濤等[6]針對長臂噴藥機臂體抖動，制約設備作業效率和安全性能的問題，采用超聲波傳感器監測長臂末端到地面的距離，使用PID控制器進行主動抑制，有效抑制了臂體抖動。李揚等[7]通過加速度傳感器積分得到振幅的方式反饋控制高空剪枝剪的末端抖動，采用粒子群優化PID控制器參數，取得了良好的穩定效果。Qiu等[8]針對懸臂水平面內抖動問題，采用光學成像方式監測末端抖動，并構建了遞歸神經網絡控制器，以基頻抖動抑制為目標取得了抑振的效果。Fareh等[9]采用系統辨識技術為一段長0.3 m的等截面水平擺動懸臂梁建立了動力學模型，采用壓電陶瓷作為運動檢測傳感器和執行器進行振動抑制，該方法在微機電設備中取得了優越的穩定性效果。Zuyev等[10]和Weldegiorgis等[11]采用臂體根部應力反饋的方式進行抖動抑制。以上研究均采用連續體偏微分方程建模，在結構復雜及擾動不確定情況下，此類方法在應用過程中由于建模困難而不便應用。

針對變形體建模復雜的難題，Howell等[12-14]于1992年提出了偽剛體模型（Pseudo Rigid Body Model， PRBM）法，在柔性結構設計與控制領域取得了良好效果。Yu等[15]以末端集中載荷下的變形量相等為約束條件，采用滑動副與旋轉副串聯的機構形式，建立了精確描述柔性梁在同時存在軸向伸長和側向撓曲變形情況下的偽剛體模型，而后以基頻相等條件確定了各剛性組件的質量，這可以理解為獲得了一階固有頻率等效的動力學模型[16]。近年來微分平坦理論在欠驅動裝備控制中廣泛采用[17-19]，其基本思想是：非線性系統只要有一組輸出量，使得狀態變量和輸入變量能由這組輸出量及其微分表示，該系統即為平坦系統。該理論應用表現為對原系統的降維處理，也可以理解為一系統在某一坐標系下的欠驅動特征，在微分平坦空間下卻可以表現為全驅動系統。Sira-Ramírez等[18]采用微分平坦方法解決了多個欠驅動系統的控制問題。但同時他也認為微分平坦輸出量的尋找經常是非常復雜且巧妙，而無定式可循的一項工作[19]。姜海勇等[20]針對變長度臂體的抖動問題，將懸臂段等效為一個剛性桿，采用加速度傳感器檢測末端振動，在臂體基頻約為3 Hz的情況下取得了較好的效果，此方法在長臂柔性顯著情況下，基頻低響應復雜而難以勝任。

自抗擾控制（Active Disturbance Rejection Control,ADRC）技術的核心思想是將模型的誤差以及外界的擾動作為擴張狀態量，進行實時觀測與補償。大量研究表明線性或非線性系統在合適的反饋作用下都能夠被轉化為積分串聯標準型[21-22]。簡言之，只要擾動和誤差在觀測器的帶寬范圍之內即可被觀測器捕獲，進而由跟隨的控制器補償，在控制器校正能力充足的條件下即可消除誤差[23]，這使得控制系統在無模型信息或模型信息不充分的條件下，仍具有優異的魯棒性[24]。

綜上所述，針對長柔性臂體低頻抖動抑制問題的研究中，采用多點反饋進行控制的方法未見報道。本文研究目標是基于二階模態振型等效方法建立柔性臂的等效多剛體動力學模型；基于3個傾角傳感器的仰角信號反饋，按照微分平坦思想獲取合成輸出量；基于自抗擾思想將高階模型低階化處理[25-26]，擬建立巡檢機器人臂體的抖動抑制系統，實現臂體快速穩定進而達到高效影像采集的目的。

1 柔性長臂的等效動力學模型

為了對果樹冠層進行檢測，需采用長臂將攝像頭抬升至冠層高度，本研究設計了如圖1所示果園巡檢機器人，其長臂總長為4.8 m，為了適應果園的不確定地形設計長臂仰角調節范圍為25°～70°，在長臂末端安裝無線傳輸型攝像頭。工作過程中抑制臂體低頻抖動，維持攝像頭與水平面之間的角度穩定在某一設定值，以保證高質量圖像信息的采集工作。本文將整個臂體等效為三桿兩扭簧平面機構，如圖2所示，實施過程中以前二階振型等效為依據，將懸臂段等效為兩個鉸接的剛性桿和兩個扭簧。在傳感器選擇方面，傾角傳感器和加速度傳感器同屬微機電系統技術產品。在運動信息采集時，基頻越低的條件下需要加速度傳感器的靈敏度越高；而采樣頻率相似的傾角傳感器檢測仰角，抖動頻率低時測量精度更高。這便導致基頻越低時，加速度傳感器的成本比傾角傳感器更高。因此采用三個傾角傳感器進行臂體運動信息采集是更經濟可行的方案。進一步根據傳感器輸出值的物理意義建立等效模型，再憑借平面二桿二扭簧機構的靜平衡條件與二階振型等效條件，獲得各等效桿及扭簧的參數。電推桿變幅推力等效為三桿機構固定鉸接點O處的扭矩。在等效模型與真實臂體總長相等的條件下，兩個支撐鉸鏈點O與O′之間的臂體看作一段剛性桿。外伸的懸臂段等效為兩段剛性桿，傳感器2和3的安裝位置以檢測最大截面轉角為目標。

等效模型建立過程如下：首先，基于二階模態振型及其導數曲線確定外伸段等效二桿桿長及臂體上傳感器的安裝位置；然后，根據靜力平衡條件下的仰角變形等效建立靜平衡約束；將固有頻率代入到等效剛性桿的仰角振型中，確定各桿等效質量及扭簧等效剛度，構建三桿二扭簧機構與長臂整體等效模型。

1.1 二桿二扭簧機構動力學模型

按照拉格朗日動力學原理，參照圖2可得二桿二扭簧機構的拉格朗日函數為

設θi0為臂體保持某一平衡狀態時第i桿的仰角，考慮各等效桿的仰角θi均在其平衡位置附近的小范圍內變化，基于小變形近似，式（1）可簡化為

考慮長臂體低階模態為主的低頻振動特性，結合靜平衡約束條件，與模態等效約束條件，可確定兩個桿長與兩個扭簧剛度系數。設簡諧振動頻率為ω，則式（2）所表示的無阻尼振動模型的特征方程B可表示為

臂體在重力作用下發生彎曲變形并保持在某一穩定姿態，臂體根部的變幅力矩維持整個臂體的平衡。式（4）是根據圖2建立的等效靜平衡方程。

采用有限元法計算臂體的前兩節模態，將前兩階模態的固有角頻率i0ω（i=1，2）分別代入到B的行列式中，獲得兩個模態等效約束方程為

聯立式（4）～（5）可以求解得到懸臂段所等效的兩剛性桿的質量（m2，m3）和兩個等效扭簧的剛度系數（k1，k2）。

1.2 三桿二扭簧機構動力學模型

臂體根部段看作一剛性桿，其桿長及質量等物理量直接測取，則臂體等效為所示的平面三桿二扭簧模型。各仰角增量均為小量，表示為θδi=θi-θi0，在靜平衡條件下懸臂段的根部所受到的支撐力矩與臂體自重產生力矩平衡，而根部力矩在等效兩桿機構中將由扭簧1提供，為此存在如下關系：

按照臂體靜止狀態時長臂根部力矩與臂體自重產生的力矩對應關系，可獲得靜平衡力矩為

臂體所需力矩τ可表達為力矩增量τδ與靜平衡力矩之和，記為τ=τδ+τ0，三桿二扭簧平面機構的拉格朗日動力學模型可寫為增量形式如下：

其中

2 基于微分平坦輸出的控制系統

由于系統輸入只有變幅驅動，從式（8）可以看出，該系統是一個單輸入三輸出的系統，所以模型將表現為單輸入多輸出的欠驅動系統結構[27]。3個仰角在不同平衡姿態時變形量的不一致導致控制難度較大。為了得到單入單出結構以簡化抖動抑制系統，按照微分平坦原理為臂體的等效平面三桿二扭簧機構尋找平坦量。

2.1 微分平坦輸出量

動力學模型式（8）整理為

控制目標在于給出一個角度目標時，臂體穩定時將回復到一個特定平衡姿態，為此設定如下最簡線性組合形式，將臂體3個部位的仰角增量組合為一個系統輸出量：

式（9）兩側同時左乘(βα1)，結果為

其中

再對式（10）兩側同時取二階導數可得：

微分平坦思想的核心在于，平坦量及其多階導數能夠作為一個向量基代數表示系統原有的m個狀態量和n個輸入量[19]，即所尋找的平坦量及其前（m+n-1）階導數線性無關。由于式（9）中有1個輸入和3個仰角及其二階導數項，這就需要平坦量y及其前6階導數線性無關。此外，τδ是唯一輸入，所有狀態量在其作用下應共同響應。據此θδi應該由y及其前4階導數線性表達，否則某一仰角的二階狀態量將必須由平坦量及其6階以上導數表達。當式（12）中僅包含y與各仰角增量的二階導數，此時系統輸入顯然無法被平坦量解耦，因此增加約束條件如下：

在此條件下式（11）化為

其中

由此待定微分平坦量的兩個變量可以由p表示為將式（16）代回到（11）式可得：

其中

式（17）的兩側同時取二階導數得：

與式（13）的處理方式相同，令該系數為0可得：

將該結果代入到式（16）得到平坦輸出F的顯式表達，再代入到式（18）可得：

其中

再對該式兩端同時取二階導數得：

將式（10）、式（12）、式（17）～（18）、式（22）～（23）聯立求解可得：

將此結果中各表達式代入到式（8）得：

其中

至此，三桿二扭簧等效平面機構模型中三個仰角及其二階導數項能夠被平坦輸出量y及其前6階導數項線性表達，表明在此設定下系統是平坦的。

2.2 基于微分平坦輸出量的自抗擾控制系統

將式（25）中平坦輸出量的4階和6階導數項、與線性化過程中產生的誤差及未建模部分、以及外部擾動一并看作綜合擾動f，據此公式（25）可改寫為

參考線性自抗擾控制理論 ，微分平坦量與輸入力矩一同送入擴張狀態觀測器。將f看作新的擴張狀態量，且認為擴張狀態量是可微的，即定義擴張狀態觀測器的狀態量為采用式（27）所示的Luenberger觀測器對系統狀態進行觀測。

式中

將觀測器的極點布置在oω-處[27-28]，取基于擴張狀態觀測器并進行如式（28）所示的極點配置，系統將是漸近穩定的，意味著觀測器捕獲擾動量，并將其送入反饋控制環節中；在控制率合適的條件下，系統將會簡化為標準積分串聯型，如式（29）所示。

將反饋控制器的特征值設置在cω-處[29]，控制率為

3 抖動抑制試驗

為了適應蘋果、梨、櫻桃等果樹的表型信息采集需求，巡檢機器人臂體總長為4.9 m，臂體采用鋁合金方管，電推桿調節仰角，懸臂部分長為4.2 m。在臂體末端安裝無線傳輸型攝像頭其重量為311 g。3處傾角的檢測均采用CTS系列傾角傳感器，其分辨率為0.01°，動態精度為0.1°，輸出頻率為100 Hz。以Arm32芯片構建控制器主體，通過三路數字輸入通道讀取傳感器信號，一路輸出用于向脈寬調制（Pulse Width Modulation, PWM）直流電機驅動模塊輸送占空比指令。

3.1 等效模型參數

長臂中電推桿作用點之上的懸臂部分是一段等截面均質梁，當末端安裝具有顯著質量攝像頭時，其質量不可忽略，采用有限元模型進行模態分析。本文采用Ansys軟件中的BEAM189單元和Mass21單元，為長臂的懸臂段及末端質量建立300個單元的有限元模型，進行模態分析，其前兩階模態的固有頻率為

提取第二階模態下各節點處截面轉角θsec的振型如圖3所示。對數據進行擬合得到二階模態轉角振型曲線方程為

距固定端2.39 m處轉角為0，以該點作為兩個等效剛性桿的鉸接點。即確定第二桿長度為2.39 m，第三桿長度為1.81 m；再對振型曲線方程求導，距離固定端1.07和4.2 m的兩點處導數為0，該點是二階振型轉角最大點，該兩點分別為第2、3傾角傳感器的安裝位置，即確定lk1=1.07 m，將第3傾角傳感器安裝在臂體末端。

在臂體仰角工作范圍內的隨機平衡狀態下讀取3個傾角傳感器的讀數（rad）為

將靜變形量和前兩階固有頻率代入式（4）（5），以及等效前后質量相等條件聯立所構成的方程組求解，便可獲得兩桿質量和兩扭簧剛度系數共4個待定量。長臂根部段處于最底端的鉸鏈與電推桿鉸鏈點之間，根部段按剛性桿等效，其質量質心位置及長度在實際模型中直接測取。這樣便確定了長柔性臂等效為3個剛性桿的質量、長度和兩個扭簧的剛度，如表1所示。

表1 等效機構參數Table 1 Parameters of the equivalent mechanism

3.2 計算微分平坦輸出量

將表1中數據代入到公式（21）中得到p=0.014 95，再代入式（16）中得到微分平坦量因子α=3.41，β=1.13。臂體處于不同姿態并保持平衡時，三傾角傳感器所在位置的仰角受具體剛度和質量分布影響，其各自平衡仰角是變化的，為此不易直接給出規劃。在小變形振動的假設下，以θ0作為臂體仰角的參考目標，三個測點的統一角度參考便于操作，則微分平坦輸出量記為

臂體抖動抑制的目標是通過控制力矩，使y趨近于零。試驗過程中通過在臂體中下部用手或橡皮錘給以錘擊，記錄傳感器的輸出數據。首先記錄無控制條件下，臂體在錘擊作用下的振動情況如圖4所示。在沖擊開始時刻臂體末端的擺幅達到10°，由于長臂自身阻尼微弱，經過約30 s時間后末端擺幅降至4°，而后還要再經歷大約40 s時間末端擺幅降至2°以內，緩慢恢復穩定狀態。

3.3 PID抖動抑制試驗

PID控制器作為應用最為廣泛的反饋控制方法適應性強，本文將臂體3處的仰角輸出值按照微分平坦輸出計算方法得到系統單一輸出y，采用PID控制器對y進行反饋控制的系統框圖如圖5所示。

經調諧得到比例系數為10.21，積分系數為0.31，微分系數為3.55。取得的控制效果如圖6所示，從圖6a中可見與圖4相似的錘擊作用致使臂體末端在第6 s產生擺幅約為12°的初始振動，在控制作用下，約7 s后擺幅下降至2°以內恢復基本穩定狀態。從圖6b中可見平坦輸出量在振動抑制過程中一直存在小幅抖動情況，系統輸入PWM占空比一直處于高速的換向狀態，且抑振過程中輸出PWM占空比出現了4次飽和現象。

3.4 ADRC抖動抑制試驗

在自抗擾控制器中引入模型信息能夠有效的降低觀測器的負擔提高跟蹤效率。等效三桿兩簧機構中扭簧剛度、桿長、桿質量共8個常值，代入式（26）中得到：

調諧觀測器帶寬為1.26，控制器帶寬為0.93，仍然在臂體的中部與前述實驗相同位置施加沖擊載荷，試驗結果如圖7所示。圖7a中可看出，初始沖擊在第5 s時帶來的末端抖動幅值達到11°，在ADRC控制器的作用下，經過7 s時間，末端振幅降至3°以內，沖擊作用發生9s后，末端抖動幅值降至2°以內。從圖7b中可以看出，采用該方法時，振動抑制過程中輸出有兩次飽和，抖動抑制過程與PID控制器相比較為平滑。從圖7c中可以看出，平坦輸出量與綜合擾動量觀測量z3基本保持一致，觀測器對合成輸出跟蹤較為準確，說明擾動量能夠被準確捕獲。在大幅沖擊發生時由于驅動器的飽和，觀測值出現小幅超過被跟蹤量的情況，但仍能快速恢復對平坦量的跟蹤。此外，對比圖6和圖7可以看出，ADRC控制方法在小幅振動過程中動作較為柔和，有效抑制了高頻的小幅振動。

4 結 論

本文采用有限元方法為帶有攝像頭的果園巡檢機器人長柔性臂提取模態數據，結合靜平衡等效與前兩階模態等效的方法，構建了長臂的等效動力學模型。采用線性無關法得到了系統的微分平坦輸出量，構建了單入單出的反饋控制系統。在沖擊擾動下，通過PID與ADRC控制器試驗驗證了該方法的有效性。以平坦輸出為反饋量的控制作用下，臂體均能在9 s以內迅速抑制大幅抖動，采用PID控制器時，末端擺動幅度達到10°的抖動在3個控制周期內便減弱到2°以內，但后期存在小幅較高頻率抖動難以消除的問題；采用ADRC控制器時系統輸出較為柔和，力矩輸出飽和2次，明顯少于PID方法，雖然大幅抖動需要5個周期才能有效抑制，但是后期不易發生高頻抖動。農業工程中涉及長臂機構存在沖擊擾動，或行進條件下存在臂體抖動的情況，需要進行主動抖動抑制時均可試用該方法。