朱亞坤 ，寇子明 ，李俊飛

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

1 引言

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

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

2 拖拉機(jī)轉(zhuǎn)彎策略及運(yùn)動(dòng)方程

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

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

2.2 運(yùn)動(dòng)方程的建立

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

圖1 運(yùn)動(dòng)學(xué)模型示意圖Fig.1 Schematic Diagram of Kinematics Model

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

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

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

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

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

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

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

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

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

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

算法流程包括搜尋、尋優(yōu)和軌跡生成，搜尋流程，如圖2所示。整個(gè)轉(zhuǎn)彎過程中每次前進(jìn)的前輪轉(zhuǎn)角φq不變，每次后退的前輪轉(zhuǎn)角φq不變，根據(jù)車輛參數(shù)和作業(yè)參數(shù)，在最大轉(zhuǎn)角范圍內(nèi)從1°開始搜尋，梯度為1°，以排列組合的方式進(jìn)行，經(jīng)過前進(jìn)后退計(jì)算車身掉頭成功后，判斷作業(yè)幅寬誤差是否在合理范圍，規(guī)定允許誤差為1cm，不在則返回前輪轉(zhuǎn)角并以梯度增加，在合理范圍就記錄前輪轉(zhuǎn)角、交替次數(shù)和作業(yè)幅寬誤差，直至達(dá)到前輪允許的最大轉(zhuǎn)角。在進(jìn)行前進(jìn)后退計(jì)算時(shí)把前進(jìn)和后退看成兩個(gè)獨(dú)立的階段，通過判斷轉(zhuǎn)彎空間的邊界來切換前進(jìn)、后退，每次交替時(shí)時(shí)間歸零。由于拖拉機(jī)平行掉頭后要沿作業(yè)方向前進(jìn)，所以在計(jì)算交替次數(shù)時(shí)，如果是在后退過程中達(dá)到了目的，交替次數(shù)要增加一次。在Matlab中，前進(jìn)、后退計(jì)算下標(biāo)量定義為h=0：50；j=2*h+1；i=2*j；e=i+1；s=i+2；g=i+3。前進(jìn)時(shí)掉頭成功，交替次數(shù) n=i/2-1，i=2、6、10、…，i表示前進(jìn)加速計(jì)算時(shí)的下標(biāo)量。后退時(shí)掉頭成功，交替次數(shù)n=s/2，s=4、8、12…，s表示后退加速計(jì)算時(shí)的下標(biāo)量。誤差z=y-w，y表示掉頭成功時(shí)后輪軸中心的y坐標(biāo)。誤差分正誤差和負(fù)誤差，比較誤差大小時(shí)，由拖拉機(jī)田間作業(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拖拉機(jī)為例，車輛參數(shù)：車長4.4m，車寬2.77m，前輪輪距1.92m，后輪輪距2.134m，前后輪軸軸距2.8m，轉(zhuǎn)彎速度1.5m/s，加速度1m/s2。在Matlab中給轉(zhuǎn)彎路徑算法輸入車輛參數(shù)進(jìn)行仿真。以x=0作為不可被輾軋的邊界，分別以x=5.9m和x=6.7m作為地頭障礙的邊界，留出0.2m的安全距離，軌跡起始點(diǎn)（0，0），每個(gè)轉(zhuǎn)彎空間進(jìn)行2m和3m兩次不同作業(yè)幅寬的仿真。前輪轉(zhuǎn)角最大45°，搜尋結(jié)果，如表1所示。在同一轉(zhuǎn)彎空間和作業(yè)幅寬下對(duì)比交替次數(shù)，從表1中可以看出，交替次數(shù)最少的可能不止一組，比如轉(zhuǎn)彎空間5.9m、作業(yè)幅寬2m時(shí)交替次數(shù)最少為6次，總共有三組，此時(shí)需要對(duì)誤差進(jìn)行對(duì)比，存在負(fù)誤差時(shí)取負(fù)誤差可以盡量減少作業(yè)遺漏區(qū)，正、負(fù)誤差都是選取誤差絕對(duì)值最小的。最終的尋優(yōu)結(jié)果，如表2所示。

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

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

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

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