張智剛 王桂民 羅錫文 何 杰 王 進 王 輝
(1.華南農業(yè)大學南方農業(yè)機械與裝備關鍵技術教育部重點實驗室, 廣州 510642;2.雷沃重工股份有限公司, 濰坊 261200)
智能化是農業(yè)機械(簡稱農機)的發(fā)展趨勢,農機自動駕駛是農機智能化領域的研究熱點[1-5]。輪角測量是實現農機自動駕駛的關鍵技術之一,轉向輪角測量的結果直接影響自動駕駛控制精度和作業(yè)效果[6]。目前,常見的農機轉向輪角測量方法包括位移式間接轉角測量法、角度傳感器直接測量法、四連桿式間接轉角測量法以及陀螺儀間接測量法等[7-9],可分為角度測量法和角速率測量法兩類[6-10]。兩類轉角測量方法各有利弊,角度測量法直接或者間接獲取農機轉向輪角的信息,檢測精度較高,但機械連接和傳感器安裝復雜;角速率測量法主要采用慣性傳感器如陀螺儀等,間接獲得轉向輪的角速率,進而獲得農機的轉向輪角,檢測精度受隨機漂移、累計誤差和外部干擾等影響較大,需進行優(yōu)化處理,但傳感器安裝簡單、工作壽命長、實際應用中不易損壞[11-12]。實際應用中,不論哪種測量方式都需要借助專用設備在轉向輪左右擺角極限范圍內進行標定,以期確定傳感器和農機轉向輪角之間的對應關系[13-14]。胡書鵬等[15]采用位移傳感器和四連桿角度傳感器獲取拖拉機前輪轉角,采用最小二乘擬合方法進行傳感器標定,獲得了傳感器與拖拉機轉向角的模型;繆存孝等[16]采用雙天線和單軸MEMS陀螺采集拖拉機輪角信息,通過卡爾曼濾波器對陀螺計算的角度實時校正,獲得了較好的應用效果。現有研究和工程實踐中,用以確定傳感器AD采樣值和物理輪角對應關系的標定試驗不明確,建立其線性或者非線性測量模型缺乏充分的標定數據。標定過程中,如待測農機輪角存在零度位置偏差(簡稱零位偏差),又沒有進行相應的誤差補償或者零位校正,則也會對測量模型的精度產生影響。
本文對傳感器的選取、安裝、輪角零位偏差補償等方面進行研究,提出一種輪式拖拉機轉向輪角檢測方法。利用帶有刻度的角度轉盤,通過對輪角傳感器在轉向輪左右擺角極限范圍內進行等間隔標定,建立輪角傳感器的測量模型;以RTK-GNSS定位定向系統為參照,基于阿克曼運動學模型和最小二乘方法,提出一種辨識轉向輪角零位偏差值的方法,使輪角傳感器能夠準確采集車輪轉角信息,為自動駕駛系統的轉向控制提供精確反饋。
本文采用的傳感器是北京通磁偉業(yè)傳感技術有限公司生產的WYH-3型無觸點角度傳感器,采用新型磁敏感元件,將機械轉動轉換為電信號變化輸出,可以無接觸地測量轉動的角度。該傳感器避免了工作環(huán)境和機械振動等對光電式角度傳感器、位移傳感器等的影響,適合農業(yè)機械轉向輪角測量。其主要的技術參數如表1所示。
表1 WYH-3型無接觸角度傳感器的技術參數Tab.1 Specifications of WYH-3 non-contact angle sensor
WYH-3型無觸點角度傳感器采用紅綠黃三線接口,紅、黃、綠線分別為電源正極、電源負極(接地端)、信號輸出。采用金屬屏蔽線,與外殼相通。信號輸出線與地線之間為輸出的模擬電壓信號。傳感器的實物圖如圖1所示。
圖1 WYH-3型無觸點角度傳感器Fig.1 WYH-3 type non-contact angle sensor
WYH-3型傳感器安裝于拖拉機左前輪轉向柱上。傳感器一部分安裝在相對機體靜止的部位,另一部分安裝在能隨車輪轉動而轉動的部位??梢院喕J為拖拉機兩前輪直接裝在同一前軸上,前軸中心與機體鉸鏈連接。傳感器的上端非旋轉部位與拖拉機的前軸緊固相連,傳感器的旋轉軸緊固拖拉機的轉動車輪部位,如圖2a所示,安裝位置如圖2b所示。
圖2 傳感器安裝圖Fig.2 Sensor installation diagrams1.WYH-3型角度傳感器 2.傳感器底座 3.聯軸器 4.連桿 5.連桿位置調整片 6.連桿支架 7.轉向軸 8.前輪轉向柱 9.前輪
依據1.2節(jié)方法在拖拉機上安裝傳感器后,先對車輪在轉盤上轉動的情況進行測量,再計算前輪轉向的零位偏差。
采用帶有標度的轉盤測量拖拉機的前輪轉向角度,以避免車輪轉動與地面接觸點產生平面移動帶來的測量誤差[17-18]。
圖3 角度傳感器標定現場圖Fig.3 Calibration scene of angle sensor
試驗前,兩個轉盤中心的距離應與兩個轉動前輪中心點之間的距離一致,并對轉盤進行校正調整,如圖3所示。固定好轉盤0°位置后,駕駛員依靠經驗將安裝好傳感器的拖拉機沿直線方向開至轉盤上,拖拉機的前輪接地點應落在轉盤的中心,并認為此時的前輪位置就是轉向的0°位置,記錄此時傳感器輸出的AD采樣值,然后依據轉盤顯示的角刻度,使左前輪向左依次以5°為間隔等角度轉動(左右極限約為30°),記錄每次轉動后傳感器輸出的采樣值和左右兩個轉盤對應的角刻度值。車輪向左轉動測試結束后,使轉盤刻度回到0°位置,采用上述同樣的方法,使右前輪向右以5°為間隔等角度進行轉動,并記錄下相應的數值。為確保測試精度,轉動角度的最大值應小于拖拉機的前輪最大轉向角。規(guī)定車輪向左偏轉為正方向,試驗結果如圖4、5所示??梢钥闯?,標定傳感器采樣值與車輪轉角之間的線性關系顯著,相關系數達到0.99以上。試驗結果表明,在采樣點間隔的區(qū)間里,角度的變化與傳感器的輸出值近似為線性,傳感器的安裝與使用過程未造成其測量上的較大誤差,沒有影響傳感器的靈敏性。
圖4 傳感器采樣值與轉盤測量的左輪角度關系曲線Fig.4 Relationship between sensor sample value and left wheel angle measured by scaled dial
圖5 傳感器采樣值與轉盤測量的右輪角度關系曲線Fig.5 Relationship between sensor sample value and right wheel angle measured by scaled dial
據此,可建立拖拉機轉向輪角的測量模型
δL=KLAD+bL
(1)
δR=KRAD+bR
(2)
(3)
式中δL、δR、δM——測量得到的拖拉機左前輪轉向角、右前輪轉向角、中位虛擬輪轉向角
KL、KR——拖拉機左前輪和右前輪轉向角標定公式的斜率
bL、bR——拖拉機左前輪和右前輪轉向角標定公式的截距
AD——WYH-3型角度傳感器的模擬輸出AD采樣值
其中,KL=-0.030 8,bL=58.145;KR=-0.030 9,bR=58.097。
利用角度轉盤獲得了拖拉機轉向輪角的測量模型,由于經過了多點采樣測量,其核心參數KL、KR比較精確,但bL、bR不太精確,因為駕駛員在將拖拉機左右前輪駛入角度轉盤時,左右轉向輪角很可能不是零,這一誤差存在于每一次角度測量值中,直接影響到bL、bR的線性回歸計算。為了提高2.1節(jié)中測量模型的精度,在轉盤標定的基礎上提出了一種拖拉機轉向輪角的零位偏差確定方法。
拖拉機行駛過程中,其行進方向角的遞推可根據阿克曼車輛運動學模型[19]得到
(4)
式中φk、φk+1——k、k+1時刻拖拉機車身運動方向角
vk——k時刻拖拉機行駛速度
δMk——k時刻拖拉機虛擬輪轉向角
ΔT——測試數據采樣時間間隔
B——拖拉機軸距
ε——拖拉機轉向輪角的零位偏差
在連續(xù)采樣獲取的N組測試數據的基礎上,定義零位偏差辨識的性能優(yōu)化指標函數為
(5)
假設測試數據來源于直線或者近似直線行駛,則全程拖拉機轉向角非常小,因此函數可進一步簡化為
(6)
(7)
獲得零位偏差ε后,可進一步對測量模型進行修正,提高輪角檢測精度,具體改進式為
δL=KLAD+bL-ε
(8)
δR=KRAD+bR-ε
(9)
(10)
利用式(8),可以方便地計算出轉向輪角零位時對應的角度傳感器AD采樣值
AD0=(ε-bL)/KL
(11)
利用轉盤標定結果可實現輪角傳感器的檢測和反饋,在此基礎上選擇平坦水泥路面進行直線路徑跟蹤控制測試,拖拉機軸距B=2.34 m,采樣間隔ΔT=0.1 s,所有測試數據都由輪角傳感器和GNSS定位系統獲得。將獲得的測試數據截取5段進行零位偏差識別算法的檢驗,得到結果如表2所示。
表2 拖拉機轉向輪角零位偏差的識別結果Tab.2 Recognition result of zero deviation of tractor steering wheel angle
分析表2中的數據可發(fā)現,5次識別結果中,第1,3,4次的識別結果最為接近,在3個最接近的結果中,選取中間結果作為最終零位偏差識別結果,即ε=0.615°、AD0=1 823。據此,可對轉盤標定結果進行零位偏差校正。
如果轉向輪角檢測存在系統偏差或者較大測量誤差,拖拉機直線路徑跟蹤控制的直線度會比較差、橫向位置偏差中也會有系統偏差,導航精度受很大影響。為了對標定好的拖拉機轉向輪角的測量模型進行驗證,進行了拖拉機直線路徑跟蹤控制試驗。
以雷沃M904-D型拖拉機作為試驗對象,并在其上面安裝自主研發(fā)的雙天線農機自動駕駛控制平臺。該平臺由監(jiān)控顯示器、自動駕駛控制器、轉向執(zhí)行裝置、輪角傳感器和位姿傳感器構成。采用司南528型雙天線板卡接收和處理GNSS信號,差分數傳通信電臺為華信420型,角度傳感器AD采樣和GNSS定位數據采集由基于Atmel9263核心板的工控板卡ECU(Electronic control unit)完成,各種算法已存入ECU電路板內的芯片中。
在平坦水泥路面上采集A、B兩點,將由其確定的直線作為拖拉機跟蹤的目標路徑。然后在路徑AB線附近啟動拖拉機和自動駕駛控制系統,進行自動路徑跟蹤控制試驗。當拖拉機接近路徑AB的另一端時,通過人機界面發(fā)送停止自動駕駛命令,結束一次直線路徑跟蹤試驗。為了驗證測量模型的精度,特別是零位AD值的準確性,路徑跟蹤控制算法設定為不含積分環(huán)節(jié)的純追蹤模型。在平整水泥路面的直線路徑跟蹤效果如圖6所示,試驗數據統計結果如表3所示。
圖6 水泥路面直線路徑跟蹤效果圖Fig.6 Effect photo of path tracking control for concrete road
對直線路徑跟蹤的橫向位置偏差均值、偏差絕對值的極值、偏差絕對值的均值和標準差進行了統計分析。偏差的均值反映了系統的穩(wěn)態(tài)誤差,偏差絕對值的極值和偏差絕對值的均值反映了駕駛控制的精度,標準差反映了駕駛控制的穩(wěn)定性。
表3 水泥路面環(huán)境下路徑跟蹤橫向位置偏差統計Tab.3 Statistical result of path tracking error for concrete road cm
由表3可知,在平整水泥路面上,其自動駕駛橫向位置偏差絕對值極值為2.74 cm,偏差絕對值的平均值為0.49 cm,標準差為0.58 cm。數據表明,本文提出的轉向輪角測量模型在水泥路面上應用表現出良好的應用效果。這主要是因為:角度傳感器在拖拉機轉向輪左右擺角極限范圍內進行了多點標定,模型參數精確;轉向輪角的零位偏差得到校正,路徑跟蹤不會產生系統偏差。
(1)采用WYH-3型無觸點角度傳感器和角度轉盤,可以較準確地標定出傳感器的輸出值與拖拉機轉向車輪實際轉角值之間的關系,標定結果顯示,傳感器的輸出AD采樣值與拖拉機轉向輪角值的線性關系極為顯著,相關系數在0.99以上。
(2)基于GNSS定位定向系統和阿克曼車輛運動學模型,提出了一種基于最小二乘法的拖拉機轉向角零位偏差識別方法。路徑跟蹤控制的橫向位置偏差絕對值的平均值為0.49 cm,證明轉向角零位偏差估計方法有效。
(3)提出了一種拖拉機轉向輪角的檢測方法,該方法既可保證輪角大角度檢測精度,也可通過在線辨識的方式完成零位偏差識別和校正,有利于提高拖拉機的路徑跟蹤控制精度,對拖拉機自動駕駛系統的前裝具有參考意義。