水稻穴直播機自主駕駛控制系統設計

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

(上海交通大學 機械與動力工程學院，上海 200240)

0 引言

自動導航與自主駕駛是智能農機裝備發展的一個關鍵技術組成要素，自動轉向控制和導航控制是智能農機自動導航系統研究的關鍵技術，直接影響自動導航控制系統的穩定性、響應的快速性和精度。

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

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

本文針對南方水田水稻穴直播機自動駕駛需求，以洋馬VP6型穴直播機為試驗平臺，自主設計了CAN總線化的穴直播機電動轉向控制、行進速度控制及擋位控制，分析了運動及控制模型，并開發了雙閉環軌跡跟蹤控制算法，實現了穴直播水田自主駕駛，水田直線跟蹤上線穩定精度可達8cm。

1 自主駕駛系統改造及控制系統組成

1.1 自主駕駛機電改造

自主駕駛系統的機電改造包括兩部分，即轉向機構的改造和變速機構的改造。其中，轉向機構的改造如圖1所示。

減速步進電機的扭矩通過同步齒形帶帶動同步帶輪傳遞到與方向盤的轉軸上，并通過轉動轉換機構傳遞到下面的轉向柱上；液壓轉向助力結構協助方向盤進行左右轉向，實現了穴直播機的轉向控制。

穴直播機的左右前輪和前橋上均不易安裝角度傳感器，且前輪轉角的變化可以由中間轉向軸通過平行四桿機構傳遞給安裝在后輪軸上的機構。因此，采用角度傳感器安裝在后輪軸上的方法對前輪轉角的變化進行檢測。由于前輪轉動時，前輪的左側偏角與右側偏角不對稱，故可以將前輪左右轉角變化的大小等效到中間轉向軸轉角的變化，即

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

其中，δm為中間轉向軸轉角；δl為左側前輪轉角；δr為右側前輪轉角。

中間轉向軸的轉角變化通過平行四桿傳遞到后輪軸上，經試驗確定，角度傳感器測量的角度可以等效于穴直播機的前輪轉角。

變速機構的改造包括主變速手柄的改造和變速踏板的改造，如圖2所示。主變速手柄控制穴直播機的前進、后退和停止，變速踏板的升降改變穴直播機的行駛速度。電動推桿的伸縮帶動連桿，使齒輪變速箱中的齒輪嚙合在前進與后退的齒輪上，主變速手柄位置被移動到相應的擋位，變速踏板的升降則是由電動推桿的伸縮通過拉線的拉伸來實現的，從而實現了穴直播機行駛速度的自動控制。

1.方向盤 2.轉動轉換機構 3.同步帶輪 4.減速步進電機 5.液壓轉向助力器 6.平行四桿機構 7.角度傳感器

圖2 變速機構改造

1.2 自主駕駛控制系統組成

自主駕駛控制系統的組成如圖3所示。

圖3 控制系統總體結構圖

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

2 自主駕駛系統控制算法設計

自主駕駛系統原理圖如圖4所示。自主駕駛控制系統是一個雙閉環的控制系統，由外環路徑跟蹤控制系統和內環轉向角跟蹤控制系統組成。其中，外環控制系統根據北斗定位系統接收到的的定位數據和位姿傳感器的姿態數據，與期望的規劃路徑進行比較，得出前輪的期望轉角δ；內環控制系統則根據期望轉角δ與實際轉角的差值通過轉向控制器驅動步進電機控制轉向機構，使穴直播機前輪達到期望的轉角。

圖4 自主駕駛系統原理圖

2.1 自主外環軌跡跟蹤控制器設計

根據農機的運動學模型[13]得

(1)

其中，d′為橫向偏差的導數；θe為航向偏差；L為軸距；v為行駛速度；δ為期望轉角。

由于穴直播機行駛速度很小，且直線跟蹤時速度和變化也很小，可以認為不變，因此得出

(2)

由非線性農機相對運動學模型[13]得

(3)

其中，ρ為農機即時轉彎曲率；c(s)為路徑曲率；s為曲線路徑弧長參數。

可以推導出

(4)

根據文獻[13]采用的反饋線性化理論和線性系統理論，將模型通過狀態變換和輸入變換得到最優控制律為

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為控制參數。

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

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

(6)

將式(6)代入式(4)，得穴直播機跟蹤直線時所需要的期望轉角為

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

(7)

2.2 自主內環轉向控制系統設計

圖4內環穴直播機轉向系統傳遞函數為

(8)

其中，δ為期望轉角；θm為實際反饋轉角；T為電機轉向系統時間常數；Km為電機轉向系統綜合放大系數；Kp為控制器比例系數。

由此可以看出：控制器比例系數越大，響應變快；但超調量變大，振蕩次數增加。經過試驗反復測試，內環轉向控制比例系數選為Kp=4.7。

在實際控制中，考慮步進電機頻率轉矩特性限制，采用了限幅比例控制，即控制器模型為

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

圖5為轉向跟蹤控制驗證曲線。以方波作為轉向控制器導向輸入信號，采樣頻率為10Hz，角度幅值為15°。

試驗結果表明：方波信號跟蹤上升時間為4s，超調量為不大于5%，自動轉向系統的跟蹤效果可以滿足要求。

圖5 轉向角跟蹤方波信號曲線

3 試驗測試及結果

以VP6型穴直播機為試驗平臺，如圖6所示。

圖6 VP6型穴直播機平臺

首先在松江農業推廣站試驗田附近的水泥地面上對穴直播機測試了穴直播機直線跟蹤的效果，而后進一步測試其在水田環境的直線跟蹤效果。

經過反復試驗確定了效果較為理想的控制器參數范圍為kp=(0.8,1.2)、kd=(1,3.5)，試驗時采用kp=1、kd=3.5。

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

圖7 水泥地面直線跟蹤的試驗結果

圖7中，上線距離約為5m，穩定后的最大橫向偏差不大于0.05m，標準方差為0.015m。轉向角跟蹤平均滯后時間不大于0.2s。這說明，自主駕駛控制系統能夠快速回到預定路徑，滿足穴直播機的作業要求。

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

圖8 水田直線跟蹤的試驗結果

圖8中，穴直播機上線距離約為6m，不發生側滑的情況下穩定后的最大橫向偏差不大于0.08m，標準差為0.027 4m，轉向角跟蹤平均延時不大于0.3s，自主駕駛控制系統能夠滿足水田的作業需求。

4 結論

1)為滿足水田水稻穴直播機自主作業和無人駕駛需求，根據洋馬VP6型穴直播機的結構特點，進行了穴直播機機電改造，完成了CAN總線化轉向、油門及擋位的控制系統設計，滿足自動駕駛功能要求。

2)針對穴直播機自動駕駛的高性能要求，進行了運動學及控制模型分析，分別建立了外環軌跡跟蹤和內環轉向角度跟蹤雙閉環控制算法策略，并經過試驗確定了合適參數，實現了水田干擾下自動駕駛±8cm跟蹤。

3)實現了穴直播機自動駕駛的完全自主化設計。