拖拉機方向平行作業(yè)轉(zhuǎn)彎路徑規(guī)劃研究

朱亞坤 ，寇子明 ，李俊飛

（1.太原理工大學(xué)機械工程學(xué)院，山西 太原 030024；2.山西省礦山流體控制工程實驗室，山西 太原 030024；3.礦山流體控制國家地方聯(lián)合工程實驗室，山西 太原 030024）

1 引言

隨著信息科技的發(fā)展，農(nóng)業(yè)正在進入以知識高度密集為主要特點的精確高效智能作業(yè)方式即“精細(xì)農(nóng)業(yè)”，農(nóng)業(yè)機械向著信息化、智能化的方向發(fā)展[1]。拖拉機作為重要的農(nóng)業(yè)機械之一，是實現(xiàn)各種各樣機械化作業(yè)的動力[2]，無人駕駛拖拉機應(yīng)勢而生。農(nóng)用車輛自動行走研究作為基礎(chǔ)研究受到了人們的重視。車輛自動行走設(shè)計時，主要由路徑規(guī)劃和跟蹤控制設(shè)計兩方面組成[3]。作業(yè)路徑的離線規(guī)劃是自動駕駛系統(tǒng)中的必要組成部分。工農(nóng)業(yè)生產(chǎn)中，類似需要覆蓋作業(yè)區(qū)域規(guī)劃作業(yè)軌跡的例子還有很多，移動機器人、割草機除草等。有所不同的是，農(nóng)業(yè)機械轉(zhuǎn)彎直徑往往大于作業(yè)幅寬，因此不能照搬上述領(lǐng)域中路徑規(guī)劃的理論和算法，轉(zhuǎn)彎方式和自動生成算法成為農(nóng)機作業(yè)路徑離線規(guī)劃中的一項關(guān)鍵問題。

文獻[4]對農(nóng)機地頭轉(zhuǎn)彎方式進行了研究，提出了基于作業(yè)間距與車輛最小轉(zhuǎn)彎半徑關(guān)系的轉(zhuǎn)彎策略。文獻[5]建立拖拉機運動方程，利用二次變分法求解兩點邊值問題，設(shè)計出拖拉機地頭轉(zhuǎn)彎路徑，但在有限的轉(zhuǎn)彎空間內(nèi)不易實現(xiàn)。在實際生產(chǎn)中，由于轉(zhuǎn)彎空間的限制，農(nóng)用車輛不一定能按照所設(shè)計的最小轉(zhuǎn)彎半徑掉頭成功，需要經(jīng)過多次的前進、后退完成掉頭。這里主要對拖拉機在田頭多次前進、后退的轉(zhuǎn)彎路徑進行研究。

2 拖拉機轉(zhuǎn)彎策略及運動方程

2.1 拖拉機轉(zhuǎn)向原理及轉(zhuǎn)彎策略

農(nóng)用車輛轉(zhuǎn)向方式分為，差速轉(zhuǎn)向、阿克曼轉(zhuǎn)向和鉸接轉(zhuǎn)向等方式，通常履帶式車輛按差速方式轉(zhuǎn)向，輪式車輛按阿克曼方式轉(zhuǎn)向。這里研究對象為輪式拖拉機，轉(zhuǎn)向方式為阿克曼轉(zhuǎn)向。依據(jù)阿克曼原理設(shè)計的車輛，轉(zhuǎn)彎時使四個車輪路徑的圓心大致在后輪軸的延長線上交會，即瞬時轉(zhuǎn)向中心，轉(zhuǎn)彎順暢。轉(zhuǎn)向時，車輪作無側(cè)偏純滾動[8]。田間作業(yè)時，拖拉機有輪廓平行路徑和方向平行路徑兩種典型的作業(yè)路徑[4-6]。前者是沿田地邊界輪廓向內(nèi)作螺旋型移動，后者是沿一固定方位作平行交替移動，方向平行路徑轉(zhuǎn)彎方式大致可分為T型、π型和Ω型，詳細(xì)內(nèi)容見參考文獻[4]。實際作業(yè)時，由于地頭可能會有高地或溝壑，所以用于轉(zhuǎn)彎的空間是有限的。轉(zhuǎn)彎策略是拖拉機到達(dá)地頭后，在不發(fā)生碰撞和盡量不影響已作業(yè)區(qū)域或未作業(yè)區(qū)域的前提下[7]，經(jīng)過多次的前進、倒退來完成掉頭，不一定以最小轉(zhuǎn)彎半徑進行轉(zhuǎn)彎。

2.2 運動方程的建立

由于地頭轉(zhuǎn)彎是低速行駛過程，此時可以不考慮側(cè)向力，認(rèn)為車輪滾動時無側(cè)向滑動。拖拉機運動學(xué)模型示意圖，如圖1所示。將拖拉機簡化成剛體，圖1中A、B、C、D—拖拉機車身的四個點；a、b、c、d—拖拉機四個車輪與地面的接觸點；f—前輪軸中心點；r—后輪軸中心點；l—前后輪軸軸距；φ—拖拉機內(nèi)轉(zhuǎn)向輪相對車身的偏角即前輪轉(zhuǎn)角；θ—車身與x軸正方向的夾角即車身角度；φ和θ—逆時針為正、順時針為負(fù)。

圖1 運動學(xué)模型示意圖Fig.1 Schematic Diagram of Kinematics Model

設(shè)拖拉機后軸中點坐標(biāo)為（x，y），由幾何關(guān)系可得到前輪軸中心點、A、B、C、D、a、b、c、d 的坐標(biāo)。根據(jù)以上所述可導(dǎo)出前輪軸中點速度與后輪軸中點速度的關(guān)系：

由于轉(zhuǎn)彎過程車速緩慢，后輪軸的軸向速度可近似為0，再根據(jù)前輪軸中心點速度分量與車速之間的關(guān)系可導(dǎo)出基于后輪軸中點的勻速運動學(xué)方程[9-10]：

給定初始條件為 x（t0）=m，y（t0）=n，θ（t0）=p，假設(shè)每次轉(zhuǎn)向過程中前輪轉(zhuǎn)角保持不變，對式（2）求解得：

式（3）只是對一個點的求解描述，沒有考慮車身尺寸。根據(jù)上述的阿克曼轉(zhuǎn)向原理還需在式（3）基礎(chǔ)上加上車身結(jié)構(gòu)的影響，得如下方程：

式中：lf—拖拉機前輪輪距，m；v—車輛速度，前進取+，后退取-；±—前輪逆時針轉(zhuǎn)取+，順時針轉(zhuǎn)取-。

加速階段與勻速階段求法相同，這里規(guī)定每次停車的時候時間歸零，初始條件為 x（0）=e，y（0）=f，θ（0）=g，考慮車身尺寸的影響，得加速階段運動方程為：

式中：a—拖拉機加速度，m/s2，前進取+，后退取-；±—前輪逆時針轉(zhuǎn)取+，順時針轉(zhuǎn)取-。

式（4）和式（5）就是基于后輪軸中點的運動方程?？傮w來說，轉(zhuǎn)彎分四個階段，前進加速、前進勻速、后退加速、后退勻速，由于轉(zhuǎn)彎過程速度低，配合剎車停車可以看成無位移停車，四個階段不一定是順序進行的。

3 轉(zhuǎn)彎路徑算法

由于拖拉機在農(nóng)田作業(yè)方式的特性，拖拉機在地頭轉(zhuǎn)彎時會受到環(huán)境的約束，比如車輪與地面的接觸點盡量不軋苗，車身不與地頭發(fā)生碰撞，車身掉頭至指定作業(yè)幅寬處。拖拉機轉(zhuǎn)彎路徑的算法要綜合考慮轉(zhuǎn)彎半徑r、作業(yè)幅寬w和轉(zhuǎn)彎空間K。根據(jù)建立的運動方程，考慮拖拉機地頭轉(zhuǎn)彎的限制條件，基于Matlab編寫了轉(zhuǎn)彎路徑算法。在給定車輛參數(shù)和作業(yè)參數(shù)的前提下，轉(zhuǎn)彎路徑算法要實現(xiàn)在允許誤差范圍內(nèi)，以盡量少的前進、后退交替次數(shù)完成指定作業(yè)幅寬的掉頭。算法中先以拖拉機參數(shù)、作業(yè)幅寬和地頭空間作為識別量，尋找前輪轉(zhuǎn)角，之后以交替次數(shù)和誤差作為判斷依據(jù)進行尋優(yōu)，經(jīng)過尋優(yōu)，再把前輪轉(zhuǎn)角加入識別量中，最后生成軌跡。

算法流程包括搜尋、尋優(yōu)和軌跡生成，搜尋流程，如圖2所示。整個轉(zhuǎn)彎過程中每次前進的前輪轉(zhuǎn)角φq不變，每次后退的前輪轉(zhuǎn)角φq不變，根據(jù)車輛參數(shù)和作業(yè)參數(shù)，在最大轉(zhuǎn)角范圍內(nèi)從1°開始搜尋，梯度為1°，以排列組合的方式進行，經(jīng)過前進后退計算車身掉頭成功后，判斷作業(yè)幅寬誤差是否在合理范圍，規(guī)定允許誤差為1cm，不在則返回前輪轉(zhuǎn)角并以梯度增加，在合理范圍就記錄前輪轉(zhuǎn)角、交替次數(shù)和作業(yè)幅寬誤差，直至達(dá)到前輪允許的最大轉(zhuǎn)角。在進行前進后退計算時把前進和后退看成兩個獨立的階段，通過判斷轉(zhuǎn)彎空間的邊界來切換前進、后退，每次交替時時間歸零。由于拖拉機平行掉頭后要沿作業(yè)方向前進，所以在計算交替次數(shù)時，如果是在后退過程中達(dá)到了目的，交替次數(shù)要增加一次。在Matlab中，前進、后退計算下標(biāo)量定義為h=0：50；j=2*h+1；i=2*j；e=i+1；s=i+2；g=i+3。前進時掉頭成功，交替次數(shù) n=i/2-1，i=2、6、10、…，i表示前進加速計算時的下標(biāo)量。后退時掉頭成功，交替次數(shù)n=s/2，s=4、8、12…，s表示后退加速計算時的下標(biāo)量。誤差z=y-w，y表示掉頭成功時后輪軸中心的y坐標(biāo)。誤差分正誤差和負(fù)誤差，比較誤差大小時，由拖拉機田間作業(yè)特性決定了優(yōu)先選取負(fù)誤差。尋優(yōu)流程，如圖3所示。

圖2 前輪轉(zhuǎn)角搜尋流程圖Fig.2 The Front Wheel Corner Search Flowchart

圖3 尋優(yōu)流程圖Fig.3 Optimization Flowchart

4 路徑仿真

以洛陽市某公司的博馬-X1604拖拉機為例，車輛參數(shù)：車長4.4m，車寬2.77m，前輪輪距1.92m，后輪輪距2.134m，前后輪軸軸距2.8m，轉(zhuǎn)彎速度1.5m/s，加速度1m/s2。在Matlab中給轉(zhuǎn)彎路徑算法輸入車輛參數(shù)進行仿真。以x=0作為不可被輾軋的邊界，分別以x=5.9m和x=6.7m作為地頭障礙的邊界，留出0.2m的安全距離，軌跡起始點（0，0），每個轉(zhuǎn)彎空間進行2m和3m兩次不同作業(yè)幅寬的仿真。前輪轉(zhuǎn)角最大45°，搜尋結(jié)果，如表1所示。在同一轉(zhuǎn)彎空間和作業(yè)幅寬下對比交替次數(shù)，從表1中可以看出，交替次數(shù)最少的可能不止一組，比如轉(zhuǎn)彎空間5.9m、作業(yè)幅寬2m時交替次數(shù)最少為6次，總共有三組，此時需要對誤差進行對比，存在負(fù)誤差時取負(fù)誤差可以盡量減少作業(yè)遺漏區(qū)，正、負(fù)誤差都是選取誤差絕對值最小的。最終的尋優(yōu)結(jié)果，如表2所示。

表1 搜尋結(jié)果Tab.1 Search Results

表2 尋優(yōu)結(jié)果Tab.2 Optimization Results

以轉(zhuǎn)彎空間6.7m為例，生成的軌跡圖，如圖4、圖5所示。以圖中的方向為準(zhǔn)，軌跡起始點（0，0），車輛初始行駛方向向右與x軸正方向成0°夾角，目標(biāo)位置分別為y=2m、y=3m。經(jīng)過一定次數(shù)的前進、后退變換，車輛行駛方向方向向左與x軸正方向成180°夾角。作業(yè)幅寬3m時，各交替點處的車身角度，如表3所示。整個過程的變化，如圖6所示。車身角度連續(xù)增加，其變化規(guī)律符合式（2）。

表3 交替點處車身角度Tab.3 Alternate Point Body Angle

圖4 作業(yè)幅寬2m軌跡Fig.4 The Track of the Work Width is 2m

圖5 作業(yè)幅寬3m軌跡Fig.5 The Track of the Work Width is 3m

圖6 車身角度變化Fig.6 Body Angle Variation

5 結(jié)論

（1）以后輪驅(qū)動前輪導(dǎo)向的拖拉機為研究對象，建立了考慮車輛外形尺寸和實際運動情況的前進、后退運動方程。（2）根據(jù)所建立的運動方程，考慮地頭轉(zhuǎn)彎空間和作業(yè)幅寬，編寫了轉(zhuǎn)彎路徑算法并進行仿真，作業(yè)幅寬誤差可以保證在0.01m以內(nèi)。仿真結(jié)果表明，所探討的轉(zhuǎn)彎策略、運動方程及路徑算法可以為無人駕駛拖拉機地頭自動轉(zhuǎn)彎路徑規(guī)劃提供設(shè)計基礎(chǔ)。