李小斌,吳宏岐,陳渭紅,張 力
(寶雞文理學(xué)院 電子電氣工程學(xué)院,陜西 寶雞 721016)
中國目前已成為世界上最大的蘋果生產(chǎn)國,在每年的蘋果采摘時節(jié),其收獲采摘作業(yè)量約占整個生產(chǎn)作業(yè)量的50%~70%[1],其采摘工作主要依靠人工完成,由于果樹植株較高、果實成熟期較短,使得蘋果采摘勞動強度大、勞動力需求大、危險性高。然而,城鎮(zhèn)化進程的推進和中國人口的老齡化,使得農(nóng)村勞動力大量減少,嚴重制約著蘋果產(chǎn)業(yè)的發(fā)展。因此,研制商業(yè)化蘋果采摘機器人實現(xiàn)其收獲采摘作業(yè)自動化刻不容緩。
應(yīng)用機器人技術(shù)進行果蔬采摘工作的思想是由美國學(xué)者Schertz 和 Brown 于1968年提出的[2-3],隨后世界各發(fā)達國家也都投入了大量的研究,中國近些年來在各大高校和科研院所也掀起了研究熱潮,目前所研制的蘋果采摘機器人大多采用多關(guān)節(jié)型機器人,其系統(tǒng)復(fù)雜、外部環(huán)境未知,存在靈活避障性差、結(jié)構(gòu)復(fù)雜、成本高昂等問題,且只能采摘樹冠外開闊視野的果實,因而難以商業(yè)化。造成這一問題的一個重要因素就是關(guān)節(jié)驅(qū)動系統(tǒng),由于大都采用電動機加減速器裝置,其機構(gòu)復(fù)雜,采摘效率低,環(huán)境適應(yīng)能力受限。這里提出一種采用直流力矩電動機加諧波減速器的蘋果采摘機器人轉(zhuǎn)動關(guān)節(jié)驅(qū)動系統(tǒng)實現(xiàn)方案,可以減小體積、簡化關(guān)節(jié)驅(qū)動機構(gòu),提高控制性能。仿真結(jié)果表明該系統(tǒng)能夠取得令人滿意的控制效果[4-5]。
蘋果作為水果的一種,其嬌嫩性、易脆性以及生長環(huán)境的復(fù)雜性、收獲采摘所要求的時限性等,對其采摘機器人關(guān)節(jié)驅(qū)動裝置有如下要求:
1)傳動裝置的功率質(zhì)量比要盡可能高,即其質(zhì)量要盡可能輕,單位質(zhì)量的輸出功率大,傳動效率也要盡可能的高;
2)為保證采摘效率,要求響應(yīng)速度要快,即要求力矩、轉(zhuǎn)動質(zhì)量比要大;
3)功率放大器及控制系統(tǒng)都必須可逆,動作平滑,且不會產(chǎn)生機械沖擊;
4)有足夠?qū)挼恼{(diào)速范圍,控制精度高,能夠?qū)崿F(xiàn)準確地定位且無超調(diào);
5)穩(wěn)定性好,安全可靠;
6)使用方便,維護簡單;
7)經(jīng)濟合理,尤其是盡量減少占地面積。
要使蘋果采摘機器人順利實現(xiàn)蘋果采摘,必須使其末端執(zhí)行器準確定位到目標果實,因此必須保證各個關(guān)節(jié)都有足夠的定位精度,才能保證系統(tǒng)整體定位精度。傳統(tǒng)的關(guān)節(jié)驅(qū)動都采用電動機加齒輪減速器的模式,其齒輪間隙會降低系統(tǒng)的剛度,引起系統(tǒng)在小范圍內(nèi)振蕩,對提高系統(tǒng)控制性能有害。這里考慮選擇直流力矩電動機加諧波減速器來實現(xiàn)各個關(guān)節(jié)的驅(qū)動[6-7]。
直流力矩電動機是一種適合低轉(zhuǎn)速、大轉(zhuǎn)矩負載的電動機,它能夠在長期堵轉(zhuǎn)低速運行時產(chǎn)生足夠大的轉(zhuǎn)矩,反應(yīng)速度快、易于控制,轉(zhuǎn)矩和轉(zhuǎn)速波動小,機械特性和調(diào)節(jié)特性線性度好,特別適合于在位置系統(tǒng)中作為執(zhí)行元件,一般情況可以不經(jīng)過齒輪減速而直接驅(qū)動負載,為進一步減小關(guān)節(jié)驅(qū)動環(huán)節(jié)體積,保證質(zhì)量體積比,考慮采用諧波減速器。
諧波減速器能滿足機器人關(guān)節(jié)體積小、重量輕、機電時間常數(shù)小、節(jié)能省電等要求,主要因為它具有結(jié)構(gòu)非常簡單、體積較小、重量較輕,傳動過程零件數(shù)量少,集成度高,傳動效率高且精度更高的優(yōu)點,同時也具有承載能力大、平穩(wěn)性更強、低噪音、大速比等特性。
多關(guān)節(jié)蘋果采摘機器人一般采用D-H坐標系建立機械臂的運動模型,當(dāng)給定各機械臂桿長和各個關(guān)節(jié)的轉(zhuǎn)角時,可以確定末端執(zhí)行器的位姿;反之,如果知道末端執(zhí)行器的位姿,可以求得各個關(guān)節(jié)的轉(zhuǎn)角。各個關(guān)節(jié)采用直流力矩電機獨立驅(qū)動時,其控制系統(tǒng)設(shè)計方法類似,控制系統(tǒng)結(jié)構(gòu)框圖如圖1所示。為保證系統(tǒng)響應(yīng)速度和跟蹤精度,設(shè)置3個調(diào)節(jié)器,分別調(diào)節(jié)位置、轉(zhuǎn)速和電流。關(guān)節(jié)控制系統(tǒng)輸入信號為角位移信號(通過運動學(xué)正逆解求得),其中位置調(diào)節(jié)器的參數(shù)決定關(guān)節(jié)定位精度和跟隨性能,轉(zhuǎn)速調(diào)節(jié)器的輸出限幅值決定著電機允許的最大電流,電流調(diào)節(jié)器的輸出限幅值決定著驅(qū)動裝置的最大輸出電壓。當(dāng)機器人每個關(guān)節(jié)的運動位移確定后,整個機械臂的運動軌跡就確定了;反之,當(dāng)確定了機器人末端執(zhí)行器位置時,可以通過逆運動求解,得到每個關(guān)節(jié)的輸入角位移信號[9]。
圖1 單關(guān)節(jié)伺服控制系統(tǒng)結(jié)構(gòu)框圖
直流力矩電動機本質(zhì)是直流電動機,與其等效電路相同,如圖2所示。
圖2 直流力矩電動機的等效電路
假定氣隙磁通恒定,則直流力矩電動機的狀態(tài)方程為:
(1)
式中,Te為電磁轉(zhuǎn)矩(N·m);TL為負載轉(zhuǎn)矩(N·m);J為機械轉(zhuǎn)動慣量(kg·m2);ω為直流力矩的電機角速度(rad/s);Ud0為驅(qū)動器理想空載電壓(V);Id為力矩電動機電樞電流(A);R為電樞回路總電阻(Ω);L為電樞回路總等效電感(H);E為反電動勢(V)。
感應(yīng)電動勢和電磁轉(zhuǎn)矩分別為:
E=Keω=Cen
(2)
Te=CmId
(3)
式中,Ce為電機常數(shù)(V·min/r);Ke為反電勢系數(shù)(N·m/A);電動機轉(zhuǎn)速常用n(r/min)表示,它和角速度的換算關(guān)系為ω=2 /60(rad/s)。Cm為轉(zhuǎn)矩系數(shù)(N·m/A)。
機械傳動機構(gòu)的狀態(tài)方程為:
(4)
式中,θm為關(guān)節(jié)輸出轉(zhuǎn)角;j為機械傳動機構(gòu)傳動比。
(5)
式中,TS為驅(qū)動裝置的等效慣性時間常數(shù);Ks為驅(qū)動裝置的放大系數(shù);uc為驅(qū)動裝置的控制信號。
綜合式(1)~(5)可得控制對象的數(shù)學(xué)模型為式(6)。
(6)
式中,Tl為電樞回路電磁時間常數(shù),Tl=L/R;
式(6)中的第二個式子又可以寫成:
(7)
式中,GD2為電力拖動系統(tǒng)折算到電動機軸上的飛輪慣量(N·m2),電力拖動系統(tǒng)機電時間常數(shù)Tm(s),Tm=GD2R/(375CeCm),且Te=CmId,TL=CmIdL,E=Keω=Cen,于是有:
(8)
式中,IdL為負載電流(A),為便于計算,取傳動比j=1,于是得控制對象動態(tài)結(jié)構(gòu)如圖3所示,輸入為驅(qū)動裝置控制電壓uc,輸出為關(guān)節(jié)轉(zhuǎn)角θm(單位為“°”)。轉(zhuǎn)速n(r/min)換算成角速度(°/s)的變換系數(shù)為360/60,即Kθ=6,然后積分可得輸出角度(“°”)。
圖3 直流力矩電動機動態(tài)結(jié)構(gòu)框圖
轉(zhuǎn)速、電流反饋環(huán)節(jié)的傳遞函數(shù)可以認為就是它們的放大系數(shù),即:
測速反饋環(huán)節(jié)傳遞函數(shù):
(9)
電流反饋環(huán)節(jié)傳遞函數(shù):
(10)
式中,α是轉(zhuǎn)速反饋系數(shù)(V·min/r),β是電流反饋系數(shù)(V/A)。為了方便計算,取位置反饋環(huán)節(jié)傳遞函數(shù)為1。
將各環(huán)節(jié)的傳遞函數(shù)關(guān)系組合后,就可以畫出三環(huán)關(guān)節(jié)控制系統(tǒng)的動態(tài)結(jié)構(gòu)框圖,如圖4所示。
圖4 關(guān)節(jié)控制系統(tǒng)動態(tài)結(jié)構(gòu)框圖
圖中,APR為位置調(diào)節(jié)器,ASR為轉(zhuǎn)速調(diào)節(jié)器,ACR為電流調(diào)節(jié)器。為了抑制反饋環(huán)節(jié)中的干擾信號,在電流反饋和轉(zhuǎn)速反饋環(huán)節(jié)增加低通濾波(可用一階慣性環(huán)節(jié)來表示),其作用結(jié)果會帶來信號延遲。于是,在轉(zhuǎn)速給定和電流給定環(huán)節(jié)也需加入一個同等時間常數(shù)的慣性環(huán)節(jié)以平衡反饋環(huán)節(jié)的信號延遲作用。
單關(guān)節(jié)控制系統(tǒng)設(shè)計采用工程設(shè)計方法,各調(diào)節(jié)器的設(shè)計采用由內(nèi)到外的原則,即先設(shè)電流調(diào)節(jié)器,隨后將電流環(huán)等效成轉(zhuǎn)速環(huán)中一個環(huán)節(jié),再設(shè)計轉(zhuǎn)速調(diào)節(jié)器,接著把轉(zhuǎn)速環(huán)節(jié)等效為位置環(huán)中的一個環(huán)節(jié),就可以設(shè)計出位置調(diào)節(jié)器。
按照圖4所構(gòu)成的系統(tǒng),選用90LY54型力矩電動機,系統(tǒng)各環(huán)節(jié)參數(shù)如表1所示。
表1 單關(guān)節(jié)控制系統(tǒng)各環(huán)節(jié)參數(shù)
在圖4中,單獨拿出電流環(huán)。由于反電動勢相對于電流環(huán)是一個變化較慢的擾動,因此設(shè)計時忽略反電動勢的影響不會對設(shè)計結(jié)果造成影響,于是去掉反電動勢作用,再把給定信號濾波和反饋環(huán)節(jié)濾波等效地移到控制環(huán)內(nèi),將小慣性環(huán)節(jié)合并處理,得到化簡后的電流環(huán)動態(tài)結(jié)構(gòu)圖,如圖5所示。
圖5 簡化的電流環(huán)動態(tài)結(jié)構(gòu)框圖
小慣性環(huán)節(jié)合并處理后時間常數(shù)為:
T∑i=Ts+Toi=0.002 1s
(11)
系統(tǒng)要求電流超調(diào)量小且以跟隨性能為主,故將電流環(huán)校正成典型Ⅰ型系統(tǒng),調(diào)節(jié)器選擇PI型,則ACR傳遞函數(shù)為:
(12)
這里需要設(shè)計Ki和τi兩個參數(shù),選取τi=Tl=3 ms=0.003 s,對消掉控制對象中的大慣性環(huán)節(jié),得電流環(huán)開環(huán)傳遞函數(shù)如式(13),為典型Ⅰ型系統(tǒng)標準形式。
(13)
其中:
(14)
按“二階最佳系統(tǒng)”設(shè)計,取KIT∑i=0.5,則:
(15)
至此,電流調(diào)節(jié)器兩個參數(shù)就都設(shè)計出來了。
由圖5,電流環(huán)開環(huán)傳遞函數(shù)設(shè)計出來后,可得電流環(huán)的閉環(huán)傳遞函數(shù),采用高階系統(tǒng)的降階近似處理方法進行降階處理,等效為一階慣性環(huán)節(jié),如式(16)所示。
(16)
將等效的一階慣性環(huán)節(jié)電流環(huán)作為轉(zhuǎn)速環(huán)的一個小慣性環(huán)節(jié)處理,于是轉(zhuǎn)速環(huán)的動態(tài)結(jié)構(gòu)簡化為圖6(a),將其進行等效變換,合并等效電流環(huán)小慣性環(huán)節(jié)與轉(zhuǎn)速濾波小慣性環(huán)節(jié)后得到圖6(b)。
圖6 等效后的轉(zhuǎn)速環(huán)動態(tài)結(jié)構(gòu)框圖
合并以后總的時間常數(shù)是:
(17)
按照控制要求,轉(zhuǎn)速環(huán)校正為典型Ⅱ型系統(tǒng),調(diào)節(jié)器選擇PI型,ASR傳遞函數(shù)是:
(18)
不考慮擾動作用,令I(lǐng)dL(S)=0,則速度環(huán)的開環(huán)傳遞函數(shù)為式(19),符合標準典型Ⅱ型系統(tǒng)形式。
(19)
按照典型Ⅱ型系統(tǒng)跟隨性能和抗擾性能都較好的原則,選取h=5,另外選取轉(zhuǎn)速反饋系數(shù)α=0.01 V·min/r,Ton=0.001 s,T∑n=0.005 2 s,則:
τn=hT∑n=0.026s
(20)
(21)
這樣,轉(zhuǎn)速調(diào)節(jié)器的兩個參數(shù)Kn和τn就設(shè)計好了。令轉(zhuǎn)速環(huán)開環(huán)增益為:
(22)
于是所設(shè)計的速度環(huán)開環(huán)傳遞函數(shù)為:
(23)
將轉(zhuǎn)速環(huán)等效成位置環(huán)的環(huán)節(jié)1/(1+TMs)[11],得位置環(huán)動態(tài)結(jié)構(gòu)圖,如圖7所示。蘋果采摘機器人要求各個關(guān)節(jié)定位準確且響應(yīng)快,因此考慮位置環(huán)校正成典型Ⅰ型系統(tǒng),調(diào)節(jié)器選擇比例型,其電壓放大系數(shù)Kp采用穩(wěn)定邊界法[12]整定可得。
圖7 位置環(huán)簡化動態(tài)結(jié)構(gòu)圖
按以上各步所設(shè)計的各調(diào)節(jié)器參數(shù)如表2所示。
表2 系統(tǒng)各調(diào)節(jié)器參數(shù)
首先,在win10系統(tǒng),啟動MATLAB(R2015a版),直接鍵入simulink命令或點擊工具欄中的Simulink Library圖標,打開Simulink瀏覽器窗口,單擊Simulink工具欄中的新模型圖標或選擇HOME→New→Simulink Model菜單項,進入模型編輯窗口。
然后,將各相關(guān)模塊從各模塊庫中拖入模型編輯窗口,調(diào)整模塊位置,并按照設(shè)計結(jié)果修改相應(yīng)參數(shù)。系統(tǒng)所需模塊及其參數(shù)如表3所示。
表3 系統(tǒng)各主要模塊及其參數(shù)
接著,按照控制原理將各模塊連接起來,如圖8所示。
圖8 關(guān)節(jié)伺服系統(tǒng)仿真結(jié)構(gòu)圖
最后,將階躍輸入設(shè)為60°,仿真時間設(shè)置為2 s,單擊仿真工具條的按鈕 或者選擇Simulation→Run菜單項,則可啟動仿真過程,再雙擊示波器P模塊,就可以看到圖9(a)所示結(jié)果;將階躍輸入設(shè)為0.5°,重復(fù)上述步驟則可看到圖9(b)所示結(jié)果;雙擊手動開關(guān)S模塊,切換到正弦輸入,將仿真時間設(shè)為10 s,運行后可觀察到9(c)所示結(jié)果。
當(dāng)給定信號分別為60°和0.5°時,其階躍響應(yīng)曲線分別如圖9(a)和9(b),可以看到兩種給定的穩(wěn)態(tài)誤差都為0,超調(diào)量也都為0;給定信號為60°時調(diào)節(jié)時間為0.92 s,給定信號為0.5°時調(diào)節(jié)時間為0.75 s,說明系統(tǒng)具有很高的定位精度、快速性和相對穩(wěn)定性。
當(dāng)輸入信號為頻率3.14 rad/s、幅值為5°的正弦信號時,輸出完全跟隨輸入,如圖9(c),說明系統(tǒng)具有很高的跟蹤性能。
圖9 關(guān)節(jié)定位及跟隨性能仿真曲線
本文將基于直流力矩電動機和諧波減速器的三閉環(huán)位置伺服系統(tǒng)應(yīng)用到了蘋果采摘機器人關(guān)節(jié)驅(qū)動系統(tǒng)中,給出了完整的設(shè)計方法,并通過Matlab仿真驗證了所設(shè)計的控制系統(tǒng),結(jié)果表明單關(guān)節(jié)系統(tǒng)能夠獲得令人滿意的控制效果。
此方案簡單易行、控制精度高,如果采用輕質(zhì)高強度材料自行研制輕巧型采摘機械臂,則可以簡化掉諧波減速器,實現(xiàn)機械臂直接驅(qū)動,從而實現(xiàn)更靈巧的控制和更復(fù)雜的采摘路徑規(guī)劃,有望實現(xiàn)樹冠中果實的順利采摘[14]。