李志騰，李彥明，唐小濤，劉成良

(上海交通大學(xué) 機(jī)械與動(dòng)力工程學(xué)院，上海 200240)

0 引言

自動(dòng)導(dǎo)航與自主駕駛是智能農(nóng)機(jī)裝備發(fā)展的一個(gè)關(guān)鍵技術(shù)組成要素，自動(dòng)轉(zhuǎn)向控制和導(dǎo)航控制是智能農(nóng)機(jī)自動(dòng)導(dǎo)航系統(tǒng)研究的關(guān)鍵技術(shù)，直接影響自動(dòng)導(dǎo)航控制系統(tǒng)的穩(wěn)定性、響應(yīng)的快速性和精度。

自動(dòng)轉(zhuǎn)向控制有電液控制和電機(jī)控制兩種方式。對(duì)于大型農(nóng)機(jī)，轉(zhuǎn)向直接由油缸驅(qū)動(dòng)，可采用電液轉(zhuǎn)向控制方法。伊利諾伊大學(xué)厄巴納-香檳分校的H. Qiu[1]，中國(guó)農(nóng)業(yè)大學(xué)的陳文良[2-3]、何卿[4]，華南農(nóng)業(yè)大學(xué)的羅錫文[5]、吳曉鵬[6]等，都對(duì)液壓轉(zhuǎn)向農(nóng)機(jī)自動(dòng)駕駛技術(shù)進(jìn)行了研究，表明液壓自動(dòng)轉(zhuǎn)向控制系統(tǒng)具有輸出扭矩大、響應(yīng)速度快及節(jié)省能量等優(yōu)點(diǎn)。目前，液壓轉(zhuǎn)向自動(dòng)駕駛在新疆、黑龍江等地被廣泛運(yùn)用在大型農(nóng)業(yè)機(jī)械上。

插秧機(jī)、穴直播機(jī)等小型農(nóng)業(yè)機(jī)械因其油路封閉而不易實(shí)現(xiàn)電液控制，必須采用電機(jī)轉(zhuǎn)向控制方法。拓普康及Autofarm公司均推出了電動(dòng)轉(zhuǎn)向控制產(chǎn)品，但由于其價(jià)格、系統(tǒng)不開(kāi)放或結(jié)構(gòu)尺寸固定等原因，不適合國(guó)內(nèi)穴直播機(jī)使用。伊利諾伊大學(xué)厄巴納-香檳分校的Yoshisada Nagasaka[7]等，華南農(nóng)業(yè)大學(xué)張智剛、胡煉、羅錫文[8-10]等，中國(guó)農(nóng)業(yè)機(jī)械化科學(xué)院偉利國(guó)[11-12]等，沈陽(yáng)自動(dòng)化研究所的李逃昌[13]等分別以插秧機(jī)為實(shí)驗(yàn)平臺(tái)，進(jìn)行了電控農(nóng)機(jī)的轉(zhuǎn)向控制系統(tǒng)及導(dǎo)航控制系統(tǒng)的相關(guān)研究。相對(duì)于液壓轉(zhuǎn)向控制、電動(dòng)轉(zhuǎn)向控制傳動(dòng)鏈長(zhǎng)，存在傳動(dòng)間隙等非線性因素，其控制較液壓轉(zhuǎn)向更為復(fù)雜，尤其是水田惡劣復(fù)雜環(huán)境下的高精度控制仍是現(xiàn)在研究的重點(diǎn)。

本文針對(duì)南方水田水稻穴直播機(jī)自動(dòng)駕駛需求，以洋馬VP6型穴直播機(jī)為試驗(yàn)平臺(tái)，自主設(shè)計(jì)了CAN總線化的穴直播機(jī)電動(dòng)轉(zhuǎn)向控制、行進(jìn)速度控制及擋位控制，分析了運(yùn)動(dòng)及控制模型，并開(kāi)發(fā)了雙閉環(huán)軌跡跟蹤控制算法，實(shí)現(xiàn)了穴直播水田自主駕駛，水田直線跟蹤上線穩(wěn)定精度可達(dá)8cm。

1 自主駕駛系統(tǒng)改造及控制系統(tǒng)組成

1.1 自主駕駛機(jī)電改造

自主駕駛系統(tǒng)的機(jī)電改造包括兩部分，即轉(zhuǎn)向機(jī)構(gòu)的改造和變速機(jī)構(gòu)的改造。其中，轉(zhuǎn)向機(jī)構(gòu)的改造如圖1所示。

減速步進(jìn)電機(jī)的扭矩通過(guò)同步齒形帶帶動(dòng)同步帶輪傳遞到與方向盤(pán)的轉(zhuǎn)軸上，并通過(guò)轉(zhuǎn)動(dòng)轉(zhuǎn)換機(jī)構(gòu)傳遞到下面的轉(zhuǎn)向柱上；液壓轉(zhuǎn)向助力結(jié)構(gòu)協(xié)助方向盤(pán)進(jìn)行左右轉(zhuǎn)向，實(shí)現(xiàn)了穴直播機(jī)的轉(zhuǎn)向控制。

穴直播機(jī)的左右前輪和前橋上均不易安裝角度傳感器，且前輪轉(zhuǎn)角的變化可以由中間轉(zhuǎn)向軸通過(guò)平行四桿機(jī)構(gòu)傳遞給安裝在后輪軸上的機(jī)構(gòu)。因此，采用角度傳感器安裝在后輪軸上的方法對(duì)前輪轉(zhuǎn)角的變化進(jìn)行檢測(cè)。由于前輪轉(zhuǎn)動(dòng)時(shí)，前輪的左側(cè)偏角與右側(cè)偏角不對(duì)稱(chēng)，故可以將前輪左右轉(zhuǎn)角變化的大小等效到中間轉(zhuǎn)向軸轉(zhuǎn)角的變化，即

δm=(δl+δr)/2

其中，δm為中間轉(zhuǎn)向軸轉(zhuǎn)角；δl為左側(cè)前輪轉(zhuǎn)角；δr為右側(cè)前輪轉(zhuǎn)角。

中間轉(zhuǎn)向軸的轉(zhuǎn)角變化通過(guò)平行四桿傳遞到后輪軸上，經(jīng)試驗(yàn)確定，角度傳感器測(cè)量的角度可以等效于穴直播機(jī)的前輪轉(zhuǎn)角。

變速機(jī)構(gòu)的改造包括主變速手柄的改造和變速踏板的改造，如圖2所示。主變速手柄控制穴直播機(jī)的前進(jìn)、后退和停止，變速踏板的升降改變穴直播機(jī)的行駛速度。電動(dòng)推桿的伸縮帶動(dòng)連桿，使齒輪變速箱中的齒輪嚙合在前進(jìn)與后退的齒輪上，主變速手柄位置被移動(dòng)到相應(yīng)的擋位，變速踏板的升降則是由電動(dòng)推桿的伸縮通過(guò)拉線的拉伸來(lái)實(shí)現(xiàn)的，從而實(shí)現(xiàn)了穴直播機(jī)行駛速度的自動(dòng)控制。

1.方向盤(pán) 2.轉(zhuǎn)動(dòng)轉(zhuǎn)換機(jī)構(gòu) 3.同步帶輪 4.減速步進(jìn)電機(jī) 5.液壓轉(zhuǎn)向助力器 6.平行四桿機(jī)構(gòu) 7.角度傳感器

圖2 變速機(jī)構(gòu)改造

1.2 自主駕駛控制系統(tǒng)組成

自主駕駛控制系統(tǒng)的組成如圖3所示。

圖3 控制系統(tǒng)總體結(jié)構(gòu)圖

基于安卓系統(tǒng)的小米Pad規(guī)劃管理系統(tǒng)通過(guò)藍(lán)牙將設(shè)定路徑傳輸給自主駕駛控制器；自主駕駛控制器接收到北斗定位系統(tǒng)定位的穴直播機(jī)的位置信息和航向信息，并根據(jù)讀取姿態(tài)傳感器的數(shù)據(jù)，與期望路徑相比較，得出期望轉(zhuǎn)角與行進(jìn)速度，通過(guò)CAN總線對(duì)基于STM8的變速踏板控制器、主變速手柄控制器及驅(qū)動(dòng)步進(jìn)電機(jī)的轉(zhuǎn)向系統(tǒng)控制器進(jìn)行控制。變速踏板控制器和主變速手柄控制器都是通過(guò)CAN總線控制電動(dòng)推桿，實(shí)現(xiàn)穴直播機(jī)的變速踏板的升降控制和擋位位置控制。姿態(tài)傳感器將穴直播機(jī)的姿態(tài)信息以20Hz的輸出頻率傳輸給自主駕駛控制器。

2 自主駕駛系統(tǒng)控制算法設(shè)計(jì)

自主駕駛系統(tǒng)原理圖如圖4所示。自主駕駛控制系統(tǒng)是一個(gè)雙閉環(huán)的控制系統(tǒng)，由外環(huán)路徑跟蹤控制系統(tǒng)和內(nèi)環(huán)轉(zhuǎn)向角跟蹤控制系統(tǒng)組成。其中，外環(huán)控制系統(tǒng)根據(jù)北斗定位系統(tǒng)接收到的的定位數(shù)據(jù)和位姿傳感器的姿態(tài)數(shù)據(jù)，與期望的規(guī)劃路徑進(jìn)行比較，得出前輪的期望轉(zhuǎn)角δ；內(nèi)環(huán)控制系統(tǒng)則根據(jù)期望轉(zhuǎn)角δ與實(shí)際轉(zhuǎn)角的差值通過(guò)轉(zhuǎn)向控制器驅(qū)動(dòng)步進(jìn)電機(jī)控制轉(zhuǎn)向機(jī)構(gòu)，使穴直播機(jī)前輪達(dá)到期望的轉(zhuǎn)角。

圖4 自主駕駛系統(tǒng)原理圖

2.1 自主外環(huán)軌跡跟蹤控制器設(shè)計(jì)

根據(jù)農(nóng)機(jī)的運(yùn)動(dòng)學(xué)模型[13]得

(1)

其中，d′為橫向偏差的導(dǎo)數(shù)；θe為航向偏差；L為軸距；v為行駛速度；δ為期望轉(zhuǎn)角。

由于穴直播機(jī)行駛速度很小，且直線跟蹤時(shí)速度和變化也很小，可以認(rèn)為不變，因此得出

(2)

由非線性農(nóng)機(jī)相對(duì)運(yùn)動(dòng)學(xué)模型[13]得

(3)

其中，ρ為農(nóng)機(jī)即時(shí)轉(zhuǎn)彎曲率；c(s)為路徑曲率；s為曲線路徑弧長(zhǎng)參數(shù)。

可以推導(dǎo)出

(4)

根據(jù)文獻(xiàn)[13]采用的反饋線性化理論和線性系統(tǒng)理論，將模型通過(guò)狀態(tài)變換和輸入變換得到最優(yōu)控制律為

sec3θ3[1-dc(s)]2ρ+[dc(s)-1]×

c(s)(tan2θe+sec2θe)

=-kpd-kd[1-dc(s)]tanθe

(5)

其中，kp、kd為控制參數(shù)。

穴直播機(jī)跟蹤直線時(shí)，可以合理地假設(shè)θe非常小，c(s)恒為0，則可以得到

ρ=cos3θe(-kdtanθe-kpd)

(6)

將式(6)代入式(4)，得穴直播機(jī)跟蹤直線時(shí)所需要的期望轉(zhuǎn)角為

δ=arctan[Lcos3θe(-kdtanθe-kpd)]

(7)

2.2 自主內(nèi)環(huán)轉(zhuǎn)向控制系統(tǒng)設(shè)計(jì)

圖4內(nèi)環(huán)穴直播機(jī)轉(zhuǎn)向系統(tǒng)傳遞函數(shù)為

(8)

其中，δ為期望轉(zhuǎn)角；θm為實(shí)際反饋轉(zhuǎn)角；T為電機(jī)轉(zhuǎn)向系統(tǒng)時(shí)間常數(shù)；Km為電機(jī)轉(zhuǎn)向系統(tǒng)綜合放大系數(shù)；Kp為控制器比例系數(shù)。

由此可以看出：控制器比例系數(shù)越大，響應(yīng)變快；但超調(diào)量變大，振蕩次數(shù)增加。經(jīng)過(guò)試驗(yàn)反復(fù)測(cè)試，內(nèi)環(huán)轉(zhuǎn)向控制比例系數(shù)選為Kp=4.7。

在實(shí)際控制中，考慮步進(jìn)電機(jī)頻率轉(zhuǎn)矩特性限制，采用了限幅比例控制，即控制器模型為

其中，u為控制器輸出頻率；±um分別為控制器輸出頻率正負(fù)限值；±ε分別為轉(zhuǎn)角偏差閾值，取ε=35°。

圖5為轉(zhuǎn)向跟蹤控制驗(yàn)證曲線。以方波作為轉(zhuǎn)向控制器導(dǎo)向輸入信號(hào)，采樣頻率為10Hz，角度幅值為15°。

試驗(yàn)結(jié)果表明：方波信號(hào)跟蹤上升時(shí)間為4s，超調(diào)量為不大于5%，自動(dòng)轉(zhuǎn)向系統(tǒng)的跟蹤效果可以滿(mǎn)足要求。

圖5 轉(zhuǎn)向角跟蹤方波信號(hào)曲線

3 試驗(yàn)測(cè)試及結(jié)果

以VP6型穴直播機(jī)為試驗(yàn)平臺(tái)，如圖6所示。

圖6 VP6型穴直播機(jī)平臺(tái)

首先在松江農(nóng)業(yè)推廣站試驗(yàn)田附近的水泥地面上對(duì)穴直播機(jī)測(cè)試了穴直播機(jī)直線跟蹤的效果，而后進(jìn)一步測(cè)試其在水田環(huán)境的直線跟蹤效果。

經(jīng)過(guò)反復(fù)試驗(yàn)確定了效果較為理想的控制器參數(shù)范圍為kp=(0.8,1.2)、kd=(1,3.5)，試驗(yàn)時(shí)采用kp=1、kd=3.5。

在水泥地面試驗(yàn)時(shí)，穴直播機(jī)掛載播種器。首先，設(shè)定跟蹤的AB直線，并設(shè)定穴直播機(jī)所在直線離AB直線的初始橫向偏差為0.55m，主變速手柄在前進(jìn)擋位，由變速踏板控制器設(shè)定行走速度恒為0.8m/s。試驗(yàn)結(jié)果如圖7所示。

圖7 水泥地面直線跟蹤的試驗(yàn)結(jié)果

圖7中，上線距離約為5m，穩(wěn)定后的最大橫向偏差不大于0.05m，標(biāo)準(zhǔn)方差為0.015m。轉(zhuǎn)向角跟蹤平均滯后時(shí)間不大于0.2s。這說(shuō)明，自主駕駛控制系統(tǒng)能夠快速回到預(yù)定路徑，滿(mǎn)足穴直播機(jī)的作業(yè)要求。

水田環(huán)境試驗(yàn)時(shí)，穴直播機(jī)也掛載播種器。首先，設(shè)定AB直線，并設(shè)定穴直播機(jī)距離AB直線的初始距離為0.5m，主變速手柄在前進(jìn)擋位，由變速踏板控制器設(shè)定行走速度恒為0.8m/s。試驗(yàn)結(jié)果如圖8所示。

圖8 水田直線跟蹤的試驗(yàn)結(jié)果

圖8中，穴直播機(jī)上線距離約為6m，不發(fā)生側(cè)滑的情況下穩(wěn)定后的最大橫向偏差不大于0.08m，標(biāo)準(zhǔn)差為0.027 4m，轉(zhuǎn)向角跟蹤平均延時(shí)不大于0.3s，自主駕駛控制系統(tǒng)能夠滿(mǎn)足水田的作業(yè)需求。

4 結(jié)論

1)為滿(mǎn)足水田水稻穴直播機(jī)自主作業(yè)和無(wú)人駕駛需求，根據(jù)洋馬VP6型穴直播機(jī)的結(jié)構(gòu)特點(diǎn)，進(jìn)行了穴直播機(jī)機(jī)電改造，完成了CAN總線化轉(zhuǎn)向、油門(mén)及擋位的控制系統(tǒng)設(shè)計(jì)，滿(mǎn)足自動(dòng)駕駛功能要求。

2)針對(duì)穴直播機(jī)自動(dòng)駕駛的高性能要求，進(jìn)行了運(yùn)動(dòng)學(xué)及控制模型分析，分別建立了外環(huán)軌跡跟蹤和內(nèi)環(huán)轉(zhuǎn)向角度跟蹤雙閉環(huán)控制算法策略，并經(jīng)過(guò)試驗(yàn)確定了合適參數(shù)，實(shí)現(xiàn)了水田干擾下自動(dòng)駕駛±8cm跟蹤。

3)實(shí)現(xiàn)了穴直播機(jī)自動(dòng)駕駛的完全自主化設(shè)計(jì)。