專利名稱：平地機的水平控制系統(tǒng)及其控制方法
技術(shù)領(lǐng)域：
本發(fā)明涉及平地機的水平控制系統(tǒng)及其控制方法。
背景技術(shù)：
精細平整的水田可以保證農(nóng)田水層深淺一致，節(jié)省農(nóng)業(yè)用水；可以提高農(nóng) 藥、化肥和除草劑的使用效率，減少農(nóng)藥、化肥和除草劑的使用量，進而減少 生產(chǎn)成本和對環(huán)境的污染；可以增加農(nóng)作物產(chǎn)量，促進農(nóng)業(yè)可持續(xù)發(fā)展。世界
各國的經(jīng)驗證明，激光平地技術(shù)是農(nóng)業(yè)生產(chǎn)中一項重要的節(jié)本增效措施。 由于水田犁底層高低不平，水田激光機在水田平地作業(yè)中，不但要保證平
地妒高低可調(diào)，而且要保證平地4產(chǎn)水平可控，使平地伊在工作過程中始終保持
在與激光束平面平行的一個平面上。

發(fā)明內(nèi)容
針對現(xiàn)有技術(shù)的缺點，本發(fā)明的目的是提供一種能準(zhǔn)確計算出平地伊水平 傾角的平地機的水平控制系統(tǒng)及其控制方法。
為實現(xiàn)上述目的，本發(fā)明的技術(shù)方案為 一種平地機的水平控制系統(tǒng)，包 括平地伊、控制平地4產(chǎn)水平位置的水平調(diào)節(jié)液壓系統(tǒng)及控制水平調(diào)節(jié)液壓系統(tǒng) 工作的水平控制系統(tǒng)，平地鏟上設(shè)有感應(yīng)平地鏟水平傾角信號及傾角變化率信 號的傳感器系統(tǒng)，該傳感器系統(tǒng)將傾^H言息傳輸至水平控制器進4亍處理。
該傳感器系統(tǒng)包括用于感應(yīng)水平傾角信號的加速度傳感器及感應(yīng)傾角變化 率信號的角速度傳感器。
該加速度傳感器安裝于平地伊轉(zhuǎn)軸上。
該角速度傳感器為陀螺儀傳感器。
該水平控制系統(tǒng)包括處理感應(yīng)信號的微控制器及將微控制器輸出信號驅(qū)動 放大的驅(qū)動電路。該水平調(diào)節(jié)液壓系統(tǒng)包括連接至平地4產(chǎn)的雙作用油缸，雙作用油缸通過電 磁閥連接至水平控制系統(tǒng)。
另外本發(fā)明還提供了 一種平地機的水平控制方法，其包括以下步驟
a、 傳感器系統(tǒng)感應(yīng)平地伊的水平傾角信號及傾角變化率信號；
b、 水平控制系統(tǒng)采集感應(yīng)信號，并對感應(yīng)信號進行融合處理，發(fā)出脈沖寬 度調(diào)制信號至水平調(diào)節(jié)B系統(tǒng)；
c 、水平調(diào)節(jié)液壓系統(tǒng)根據(jù)發(fā)出的信號來控制平地伊的水平度。 步驟a中，傳感器系統(tǒng)包括加速度傳感器及角速度傳感器，加速度傳感器 用于感應(yīng)水平傾角信號，角速度傳感器用于感應(yīng)傾角變化率信號，步驟c中， 該水平調(diào)節(jié)液壓系統(tǒng)包括雙作用油缸，水平控制系統(tǒng)將信號發(fā)送至一電磁閥， 由電^茲閥來控制雙作用油缸的伸縮度，并通過雙作用油缸作用于平地4產(chǎn)來調(diào)節(jié) 平地鏟的水平度。
在步驟b中，對該水平傾角信號及傾角變化率信號的處理還包括以下步驟 (1)檢查水平傾角信號及傾角變化率信號是否改變，是則執(zhí)行步驟(2 )，否 則結(jié)束；
(2 )計算兩種傳感器測得的水平傾角信號COacc及傾角變化率信號(Ogyr。，并將 C0acx及(Ogyr。之差絕對值與設(shè)定閥值相比，若小于閥值，則按水平傾角信號CO acc 計算平地4產(chǎn)傾角，若大于閥值，則按設(shè)定傾角加上COgyr。xT，求出平地4產(chǎn)傾角，
其中T為樣吏控制器的工作周期；
(3) 根據(jù)平地l產(chǎn)的傾角變化刷新脈沖寬度調(diào)制信號的占空比；
(4) 驅(qū)動電,收脈沖寬度調(diào)制信號，進行放大并輸出至電磁閥。 在步驟(1)中，微控制器采集水平傾角信號及傾角變化率信號的周期為IOT。 與現(xiàn)有技術(shù)相比，本發(fā)明采用加速度傳感器及角速度傳感器來感應(yīng)平地鏟的
水平傾角信號，并采用微控制器對感應(yīng)信號進行融合計算得出傾角，最后通過 水平調(diào)節(jié)液壓系統(tǒng)調(diào)節(jié)平地伊，使其始終保持水平。由于采用了加速度傳感器
及角速度傳感器相結(jié)合的測量方法，能準(zhǔn)確地測出平地機在橫向擺動、轉(zhuǎn)彎、水田犁底層不平所造成的上下顛簸而產(chǎn)生的加速度分量，從而準(zhǔn)確地計算出平 地伊的傾角。
另夕卜，本發(fā)明的方法通過融合計算出兩種傳感器測得的水平傾角信號COacc及 傾角變化率信號 W gyro ， 將CO acc與CO gyro之差的纟色對值與i殳定閾 <直相比,判斷并
選擇傾角的計算方法，并計算出平地妒傾角的大小，再通過傾角的大小刷新PWM 信號的占空比，最后通過PWM來控制水平調(diào)節(jié)液壓系統(tǒng)，達到調(diào)節(jié)平地4產(chǎn)的目 的。
附固說明
圖l為本發(fā)明的平地才幾結(jié)構(gòu)示意圖； 圖2為加速度傳感器與角速度傳感器的安裝示意圖； 圖3為加速度傳感器測量平地伊水平傾角的原理圖； 圖4為水平控制系統(tǒng)的工作原理圖； 圖5為水平控制系統(tǒng)的結(jié)構(gòu)示意圖； 圖6為水平調(diào)節(jié)絲系統(tǒng)的電磁閥的控制邏輯原理圖； 圖7為水平控制系統(tǒng)的控制程序流程圖。
具體實施例方式
下面結(jié)合附圖對本發(fā)明作進一步的詳細說明。
參閱

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