平地機的水平控制系統及其控制方法
2023-08-01 08:30:36 1
專利名稱：平地機的水平控制系統及其控制方法
技術領域：
本發明涉及平地機的水平控制系統及其控制方法。
背景技術：
精細平整的水田可以保證農田水層深淺一致，節省農業用水；可以提高農 藥、化肥和除草劑的使用效率，減少農藥、化肥和除草劑的使用量，進而減少 生產成本和對環境的汙染；可以增加農作物產量，促進農業可持續發展。世界
各國的經驗證明，雷射平地技術是農業生產中一項重要的節本增效措施。 由於水田犁底層高低不平，水田雷射機在水田平地作業中，不但要保證平
地妒高低可調，而且要保證平地4產水平可控，使平地伊在工作過程中始終保持
在與雷射束平面平行的一個平面上。
發明內容
針對現有技術的缺點，本發明的目的是提供一種能準確計算出平地伊水平 傾角的平地機的水平控制系統及其控制方法。
為實現上述目的，本發明的技術方案為 一種平地機的水平控制系統，包 括平地伊、控制平地4產水平位置的水平調節液壓系統及控制水平調節液壓系統 工作的水平控制系統，平地鏟上設有感應平地鏟水平傾角信號及傾角變化率信 號的傳感器系統，該傳感器系統將傾^H言息傳輸至水平控制器進4亍處理。
該傳感器系統包括用於感應水平傾角信號的加速度傳感器及感應傾角變化 率信號的角速度傳感器。
該加速度傳感器安裝於平地伊轉軸上。
該角速度傳感器為陀螺儀傳感器。
該水平控制系統包括處理感應信號的微控制器及將微控制器輸出信號驅動 放大的驅動電路。該水平調節液壓系統包括連接至平地4產的雙作用油缸，雙作用油缸通過電 磁閥連接至水平控制系統。
另外本發明還提供了 一種平地機的水平控制方法，其包括以下步驟
a、 傳感器系統感應平地伊的水平傾角信號及傾角變化率信號；
b、 水平控制系統採集感應信號，並對感應信號進行融合處理，發出脈沖寬 度調製信號至水平調節B系統；
c 、水平調節液壓系統根據發出的信號來控制平地伊的水平度。 步驟a中，傳感器系統包括加速度傳感器及角速度傳感器，加速度傳感器 用於感應水平傾角信號，角速度傳感器用於感應傾角變化率信號，步驟c中， 該水平調節液壓系統包括雙作用油缸，水平控制系統將信號發送至一電磁閥， 由電^茲閥來控制雙作用油缸的伸縮度，並通過雙作用油缸作用於平地4產來調節 平地鏟的水平度。
在步驟b中，對該水平傾角信號及傾角變化率信號的處理還包括以下步驟 (1)檢查水平傾角信號及傾角變化率信號是否改變，是則執行步驟(2 )，否 則結束；
(2 )計算兩種傳感器測得的水平傾角信號COacc及傾角變化率信號(Ogyr。，並將 C0acx及(Ogyr。之差絕對值與設定閥值相比，若小於閥值，則按水平傾角信號CO acc 計算平地4產傾角，若大於閥值，則按設定傾角加上COgyr。xT，求出平地4產傾角，
其中T為樣吏控制器的工作周期；
(3) 根據平地l產的傾角變化刷新脈沖寬度調製信號的佔空比；
(4) 驅動電,收脈沖寬度調製信號，進行放大並輸出至電磁閥。 在步驟(1)中，微控制器採集水平傾角信號及傾角變化率信號的周期為IOT。 與現有技術相比，本發明採用加速度傳感器及角速度傳感器來感應平地鏟的
水平傾角信號，並採用微控制器對感應信號進行融合計算得出傾角，最後通過 水平調節液壓系統調節平地伊，使其始終保持水平。由於採用了加速度傳感器
及角速度傳感器相結合的測量方法，能準確地測出平地機在橫向擺動、轉彎、水田犁底層不平所造成的上下顛簸而產生的加速度分量，從而準確地計算出平 地伊的傾角。
另夕卜，本發明的方法通過融合計算出兩種傳感器測得的水平傾角信號COacc及 傾角變化率信號 W gyro ， 將CO acc與CO gyro之差的糹色對值與i殳定閾 <直相比,判斷並
選擇傾角的計算方法，並計算出平地妒傾角的大小，再通過傾角的大小刷新PWM 信號的佔空比，最後通過PWM來控制水平調節液壓系統，達到調節平地4產的目 的。
附固說明
圖l為本發明的平地才幾結構示意圖； 圖2為加速度傳感器與角速度傳感器的安裝示意圖； 圖3為加速度傳感器測量平地伊水平傾角的原理圖； 圖4為水平控制系統的工作原理圖； 圖5為水平控制系統的結構示意圖； 圖6為水平調節絲系統的電磁閥的控制邏輯原理圖； 圖7為水平控制系統的控制程序流程圖。
具體實施例方式
下面結合附圖對本發明作進一步的詳細說明。
參閱
圖1， 一種平地機的水平控制系統，包括平地伊1及控制平地4產1水平 位置的水平調節液壓系統及用於控制水平調節液壓系統工作的水平控制系統2 ， 平地鏟上設有感應平地4產水平傾角信號及傾角變化率信號的感應系統，該感應 系統將感應信號傳輸至水平控制系統2進行處理。
該感應系統包括用於感應水平傾角信號的加速度傳感器4及感應傾角變化 率信號的角速度傳感器5。如圖2所示，該加速度傳感器4安裝於平地伊1轉軸 上，才艮據平地4產1轉軸所在的平面定出加速度傳感器4的測量軸x軸和y軸(與
傳感器固連)，其x軸和y軸的加速度分別為ax與ay，可用下式計算平地4產1的水平傾角oc ，其原理可用圖3來說明。重力加速度g在測量軸的分量ax與ay (由傳感器測出)，其大小取決於測量軸水平傾斜角a:
ax = g x s in a
ay = g x cos oc
才艮據上述二式，通過傳感器輸出可以計算出平地鏟1水平傾角a:
a=arctg (ax/ay)
式中ax是傳感器測出的加速度值。上式成立的前提條件是a,完全由g引起, 即平地4產1的運動不在x軸產生加速度，例如平地4產1靜止、勻速直線運動等 時即屬於這種情況，否則當平地機引起的橫向擺動及平地機在轉彎時引起的甩 尾在加速度傳感器4的測量軸上產生的加速度分量，或者由於水田犁底層不平 引起平地機上下顛簸而在加速度傳感器4的測量軸上產生的加速度分量等時， 單純使用加速度傳感器4和上述公式並不能準確測量平地伊1水平傾角，為此 必須引入角速度傳感器5對平地伊1的水平傾角進行測定。本實施例中，該角 速度傳感器5為一陀螺儀傳感器。利用角速度傳感器5測得的傾角變化率co對 時間T的積分，即在上次的傾角勤出上每次加上co x T得到當前的平地伊1的實 際傾角。
如圖4及圖5所示，該水平控制系統2包括處理感應信號的孩t控制器及將 微控制器輸出信號驅動放大的驅動電路。其中，該微控制器為ARM7內核的32 位微處理器，其內集成快速存儲器，分別具有256KB的Flash ROM和64KB的 SRAM，無需擴展存儲器，且該微控制器還內置8通道10位A/D轉換器，為提高 控制精度,用軟體方式提高轉換精度至13位。此外它內有PWM信號發生器，及 RS232、 SPI、 UART等多種通信接口。該水平控制系統2還包括提供3. 3V、 5V、 12V等各種不同電壓的電源電路，該電源電路連接至外部的蓄電池或其他電源設 備。
該水平調節液壓系統包括連接至平地伊l的雙作用油缸3，雙作用油釭3通過電磁閥連接至水平控制系統2。本發明中，由上述兩個傳感器分別測出平地伊 1的水平傾角及水平傾角變化率，再才艮據融合控制算法計算輸出脈衝寬度調製信 號，即PWM控制信號的佔空比，利用電石茲閥，控制雙作用油缸3動作調整平地 伊l，直至平地伊l調整至水平狀態。電磁閥使用PWM脈衝控制，可有效地起到 流量控制效果，實現雙作用油缸3行程的平滑模擬控制，從而使控制過程平穩。
水平控制系統2還設有兩個用於調節平地妒1與感應系統安裝時的水平誤 差的拾睫與兩個用於指示升降電磁閥動作情況的LED指示燈。由於感應系統安 裝時產生的誤差，感應系統測定的平地4產1水平傾角與平地4產1實際水平傾角 難免存在一個固定偏差值，M該值調整好後寫入系統的存貯器中。而LED指 示燈則在相應電磁閥動作時點亮。
另外，本發明還提供了一種平地才幾的水平控制方法，其包括以下步驟
a、 加速度傳感器感應平地伊的水平傾角信號，角速度傳感器感應傾角變化 率信號；
b、 微控制器採集感應信號，並對感應信號進行融合處理，發出脈衝寬度調 制信號至驅動電路；
c、 電磁閥根據驅動電路發出的信號來控制雙作用油缸的伸縮度；
d、 通過雙作用油缸作用於平地伊來調節平地伊的水平度。
在步驟b中，對該水平傾角信號及傾角變化率信號的處理還包括以下步驟
(1) 檢查水平傾角信號及傾角變化率信號是否改變，是則執行步驟(2)， 否則結束；
(2) 計算兩種傳感器測得的水平傾角信號C0acc及傾角變化率信號COgyr。，為
判斷平地4產是否受到幹擾，分別求出兩個傳感器測得的水平傾角信號COacc及傾
角變化率信號CO gyr。，求出二者之差的絕對值，由該值決定平地伊受到幹擾與否， 若小於設定的閾值則認為未受千擾，否則出現千擾，由此可決定使用加速度傳
感器還是角速度傳感器來測量平地伊傾角，若平地4產未受幹擾，則按水平傾角信號COacc計算平地鏟傾角，若平地鏟受到幹擾，則按上一次平地鏟的傾角加上 COgyr。xT,求出平地鏟傾角，其中T為微控制器的工作周期；
(3 )根據平地伊的傾角變化刷新脈衝寬度調製信號的佔空比； (4)驅動電5^#收脈沖寬度調製信號，進4亍;汶大並輸出至電》茲閥。 步驟(1 )中，微控制器採集水平傾角信號及傾角變化率信號的周期為IOT。 孩i:控制器的工作周期T由定時器設定為10ms,採集感應信號的周期即為100ms， 這樣設定可實現對信號採樣及對採集的信號進行滑動濾波，同時較大刷新周期 內才計算平地4產傾角變化率co，可保證結果比較準確，因為計算時間太短的話 則變化量太小。
微控制器的控制程序主要是基於定時器中斷的中斷服務程序，如圖7所示， 其流程如下
微控制器先採集加速度傳感器與角速度傳感器的輸出，通過超採樣提高分 辨率並進行軟體濾波，得到加速度傳感器與角速度傳感器的當前值；
PWM佔空比是否刷新，若是，則執行下一步驟，若否，則定時器中斷服務程
序結束；
分別計算兩種傳感器測得的平地妒的水平傾角及傾角變化率C0acc、 COgyr。； 00 acx與(0 gyr。之差絕對值是否小於設定閥值，若是則按水平傾角信號(0 acc
計算平地伊傾角，若否則按上一次平地4產的傾角加上cogyr。 x T,求出平地伊傾角；
檢測平地4產的傾角是否在增加，若是則選擇上升死區，若否則選擇下 降死區，定義兩個電i茲閥均不動作的平地4產傾角範圍為死區，則滑動死區 保證平地4產接近水平位置時根據傾角變化的方向有不同的死區，進一步提 高控制過程的穩定性；
按控制算法、死區設置要求計算並刷新輸出佔空比；
定時器中斷服務程序結束。
圖6是水平調節絲系統的電磁閥的控制邏輯原理圖。為了補償安裝中加速度傳感器的X軸與平地伊之間平行度誤差，可以通過鍵盤微調設置要 求的平地妒傾角00)(實為加速度傳感器本身傾角，該角對應平地妒水平位
置)。設定值a。一般來說不為0，除非安裝平行度誤差為0,即x軸嚴格平
行於平地4產。圖6中4黃坐標為平地4產傾角(測量得到)，下部分則是電》茲 岡工作區間分析圖，上部分是相應區間電^f茲閥PWM驅動信號佔空比大小。
以a。為分界點，左邊傾角誤差為負，為雙作用油缸上升電》茲閥工作區，而
CO)右側區間傾角誤差為正，為下降電i茲閥工作區。上升與下降電i茲閥動作 互鎖，即不能同時動作。為提高控制穩定性，減少平地伊處於水平位置附 近時電/茲閥頻繁動作，設計上採取如下措施， 一是在a。兩側附近(亦即平 地伊水平位置附近)設置滑動死區，死區內兩電磁閥不工作(對應佔空比 為0)， 二是在水平位置(co))附近時根據誤差大小減小PWM佔空比。死區 設置上，每個電磁閥均加有施密特特性即動作滯後區間H。以對上升電f茲 閥為例，傾角由左邊大的負誤差區上升向ao靠近時，電磁閥一直工作直到 ca處停止，此後僅當誤差又變負(傾角由ao向右變化)且傾角達到a" a4<a0， 二者之差為H)時，電磁閥才開始動作。同理圖中m與co定義了下降電磁
閥動作滯後區間H。 ca、 a2、 a3、與cu形成了兩個不重疊的死區DZO與DZl, 它們對應平地鏟傾角不同的變化趨勢，本設計稱為滑動死區，其具體位置 與co)有關。見圖6, DZO為基本死區，它以ao為中心對稱設置，上升死區 DZO與下降死區DZ1分別為DZO與右側下降電磁閥滯後H或左側上升電磁 閥滯後H合成的結果。平地妒工作時，如果傾角是由左到右變化(上升)， 則死區為上升死區DZO，具體說來就是，傾角誤差較負(靠左)時上升油
缸動作直到傾角變為oa,此時ii^上升死區DZO, DZO —直維持到％才結束，此時誤差已偏正，下降油缸開始動作。同理傾角由右向左變化(下降)
則死區為下降死區DZ1。具體程序上可用一個全局變量平地伊傾角變化趨 勢，由它決定使用那個死區。提高控制穩定性採取的另一措施是對水平控 制電磁閥使用PWM控制，見圖6上部分所示。圖中表示出平地伊不同水平 傾角下PWM佔空比變化，誤差小時(接進水平位置)使用小佔空比以減小 衝擊，本設計中採用簡單線性規律，即佔空比隨誤差(可經控制算法校正 後的誤差，圖中直接應用誤差本身，相當於比例控制算法)大小線性變化， 死區參數決定後，由佔空比上下限Dmax與Dmin即可決定任意傾角下輸出 的佔空比D,其中Dmax與Dmin由試驗決定，本設計中分別為75 %與30%。
權利要求
1. 一種平地機的水平控制系統，包括平地鏟、控制平地鏟水平位置的水平調節液壓系統及控制水平調節液壓系統工作的水平控制系統，其特徵在於平地鏟上設有感應平地鏟水平傾角信號及傾角變化率信號的傳感器系統，該傳感器系統將信號傳輸至水平控制器進行處理。
2、 根據權利要求l所述的平地機的水平控制系統，其特徵在於該傳感器 系統包括用於感應水平傾角信號的加速度傳感器及感應傾角變化率信號的角速 度傳感器。
3、 根據權利要求2所述的平地機的水平控制系統，其特徵在於該加速度 傳感器安裝於平地伊轉軸上。
4、 根據權利要求2所述的平地機的水平控制系統，其特徵在於該角速度 傳感器為一陀螺儀傳感器。
5、 根據權利要求1所述的平地機的水平控制系統，其特徵在於該水平控 制系統包括處理感應信號的微控制器及將微控制器輸出信號驅動放大的驅動電 路。
6、 根據權利要求1至5任一項所述的平地機的水平控制系統，其特徵在於 該7K平調節液壓系統包括連接至平地妒的雙作用油缸，雙作用油缸通過電磁閥連接至水平控制系統。
7、 根據權利要求1所述平地機的水平控制系統的控制方法，其包括以下步驟a、 傳感器系統感應平地伊的水平傾角信號及傾角變化率信號；b、 水平控制系統採集感應信號，並對感應信號進行融合處理，發出脈衝寬度調製信號至水平調節g系統；c 、水平調節液壓系統才艮據發出的信號來控制平地4產的水平度。
8、 根據權利要求7所述的平地機的水平控制系統的控制方法，其特徵在於 步驟a中，傳感器系統包括加速度傳感器及角速度傳感器，加速度傳感器用於感應水平傾角信號，角速度傳感器用於感應傾角變化率信號，步驟C中，該水 平調節液壓系統包括雙作用油缸，水平控制系統將信號發送至一電i茲閥，由電 f茲閥來控制雙作用油缸的伸縮度，並通過雙作用油缸作用於平地妒來調節平地 鏟的水平度。
9、根據權利要求8所述的平地機的水平控制系統的控制方法，其特徵在於 對該水平傾角信號及傾角變化率信號的處理還包括以下步驟(1)採樣並檢查水平傾角信號及傾角變化率信號是否改變，是則執行步驟 (2)，否則結束；(2 )計算兩種傳感器測得的水平傾角信號00 acc及傾角變化率信號co gyr。，並將 C0,及COgyr。之差絕對值與設定閥值相比，若小於閥值，則按水平傾角信號COacc 計算平地鏟傾角，若大於闊值，則按設定傾角加上(Ogyr。xT，求出平地4產傾角，其中T為微控制器的工作周期；(3 )根據平地鏟的傾角變化刷新脈衝寬度調製信號的佔空比；(4)驅動電,收脈沖寬度調製信號，進4亍;故大並輸出至電i茲閥。
10、根據權利要求9所述的平地機的水平控制系統的控制方法，其特徵在於步驟(1)中，孩史控制器採集水平傾角信號及傾角變化率信號的周期為IOT。
全文摘要
本發明公開了一種平地機的水平控制系統，包括平地鏟、控制平地鏟水平位置的液壓系統及控制該液壓系統工作的水平控制系統。平地鏟上設有感應平地鏟水平傾角及傾角變化率信號的傳感器系統，將信號傳輸至水平控制系統進行處理；傳感器系統包括用於感應水平傾角信號的加速度傳感器及感應傾角變化率信號的角速度傳感器。另外，本發明還公開了一種平地機的水平控制方法。本發明能準確地計算出平地鏟的水平傾角。
文檔編號E02F3/76GK101280570SQ200810028328
公開日2008年10月8日 申請日期2008年5月27日 優先權日2008年5月27日
發明者慶 李, 李就好, 王在滿, 羅錫文, 趙祚喜 申請人:華南農業大學