一種實(shí)現(xiàn)山地機(jī)械等間距直線行駛的方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明設(shè)及山地機(jī)械行駛技術(shù)領(lǐng)域，尤其設(shè)及一種實(shí)現(xiàn)山地機(jī)械等間距直線行駛 的方法。
【背景技術(shù)】
[0002] 在進(jìn)行機(jī)械化田間作業(yè)時(shí)，通常需要山地機(jī)械按照預(yù)先規(guī)劃好的直線行駛作業(yè)， W便合理利用空間，避免浪費(fèi)。尤其是在大田且實(shí)行套種的地區(qū)，山地機(jī)械播種時(shí)，如果沒 有按照規(guī)劃好的直線作業(yè)，將給后續(xù)的農(nóng)作物收割、套播作業(yè)帶來很大的困難，所W實(shí)現(xiàn)山 地機(jī)械直線行駛，對(duì)提高農(nóng)業(yè)作業(yè)效率和質(zhì)量有至關(guān)重要的意義。同時(shí)，山地機(jī)械在大田作 業(yè)時(shí)，單靠駕駛員的判斷很容易出現(xiàn)偏差，并且運(yùn)種偏差不容易被察覺，所W山地機(jī)械直線 行駛實(shí)現(xiàn)方法能提高作業(yè)精度，提高生產(chǎn)效率；使駕駛員從單調(diào)重復(fù)的勞動(dòng)中解放出來，降 低駕駛員的勞動(dòng)強(qiáng)度；避免重復(fù)作業(yè)，減少浪費(fèi)，有效地降低生產(chǎn)成本。
[0003] 目前實(shí)現(xiàn)山地機(jī)械的直線行駛控制系統(tǒng)多數(shù)WGI^為主要的技術(shù)，采用RTK-DGPS 與各種傳感器技術(shù)相結(jié)合的方式；激光Ξ點(diǎn)動(dòng)態(tài)定位方法等等。WGI^為主的導(dǎo)航技術(shù)其 成本較高，從而影響了基于GI^導(dǎo)航技術(shù)在我國農(nóng)業(yè)生產(chǎn)中的推廣；激光Ξ點(diǎn)動(dòng)態(tài)定位方 法定位實(shí)時(shí)性好，但固定基站與主站之間要求嚴(yán)格同步，便于基站的激光發(fā)射器實(shí)時(shí)追蹤 主站的接收器，若激光接收器接收不到激光，容易造成定位精度波動(dòng)大，從而影響農(nóng)用機(jī)械 直線行駛的穩(wěn)定性。

【發(fā)明內(nèi)容】

[0004] 本發(fā)明的目的在于克服現(xiàn)有技術(shù)中存在的缺點(diǎn)，提供一種分段式、高精度、穩(wěn)定性 好的實(shí)現(xiàn)山地機(jī)械等間距直線行駛的方法。 陽0化]實(shí)現(xiàn)本發(fā)明目的的技術(shù)方案是：
[0006] 一種實(shí)現(xiàn)山地機(jī)械等間距直線行駛的方法，包括下述步驟：
[0007] (1)將山地機(jī)械所行駛的基準(zhǔn)直線分為等長的X段，設(shè)各段長度為L;
[0008] (2)在平行于山地機(jī)械所行駛的基準(zhǔn)直線外的一條直線上設(shè)置兩個(gè)硬件結(jié)構(gòu)完全 相同的固定節(jié)點(diǎn)，分別固定在高于農(nóng)作物高度的支架上，在山地機(jī)械的正左邊靠近山地機(jī) 械起始點(diǎn)的位置設(shè)置第一固定節(jié)點(diǎn)，在第一固定節(jié)點(diǎn)正前方設(shè)置第二固定節(jié)點(diǎn)，在第一固 定節(jié)點(diǎn)上設(shè)置第一激光發(fā)射器，在第二固定節(jié)點(diǎn)上設(shè)置第二激光發(fā)射器，所述的兩個(gè)激光 發(fā)射器，其初始發(fā)射的兩束激光線在俯視圖上的交點(diǎn)剛好落在山地機(jī)械的起始點(diǎn)上；
[0009] (3)第一激光發(fā)射器和第二激光發(fā)射器由其各自固定節(jié)點(diǎn)上的電機(jī)控制轉(zhuǎn)動(dòng)，所 述的電機(jī)每次轉(zhuǎn)動(dòng)的角度與山地機(jī)械所行駛直線的等分線段長度L采用類正比控制方法 匹配，其角速度的大小與山地機(jī)械的行駛速度采用類正比控制方法匹配，控制第一激光發(fā) 射器和第二激光發(fā)射器對(duì)行駛的山地機(jī)械進(jìn)行追蹤，并始終保持發(fā)射的兩束激光線在俯視 圖上的交點(diǎn)剛好落在山地機(jī)械的所走的基準(zhǔn)直線上的下一個(gè)等分點(diǎn)上；
[0010] (4)在山地機(jī)械上安裝一個(gè)移動(dòng)節(jié)點(diǎn)，在移動(dòng)節(jié)點(diǎn)上設(shè)置慣性傳感器、激光測距傳 感器、第一激光接收器和第二激光接收器，所述的兩個(gè)激光接收器在豎直方向上共線，安裝 高度分別與第一激光發(fā)射器和第二激光發(fā)射器相同；慣性傳感器為巧螺儀和加速度計(jì)，且 巧螺儀和加速度計(jì)分別對(duì)山地機(jī)械的角速度和加速度進(jìn)行測量，經(jīng)過計(jì)算后得到山地機(jī)械 的水平航向角（左、右偏轉(zhuǎn)角度正負(fù)顯示）和垂直方向上的側(cè)傾角測量值，各自精確度小于 0.5°，垂直方向上的最大可傾斜的角度設(shè)置成固定數(shù)值，當(dāng)側(cè)傾角大于固定值時(shí)，則整個(gè) 系統(tǒng)停止運(yùn)行，預(yù)防出現(xiàn)交通事故，系統(tǒng)一旦停止運(yùn)行，則需要重新啟動(dòng)方能再次運(yùn)行；
[0011] (5)山地機(jī)械行駛到等分點(diǎn)處，根據(jù)左右偏向檢測法判斷是否向左、右偏，再由航 向角計(jì)算法推算出航向角的大小，航向角用α。表示，再根據(jù)航向角的大小通過PID控制算 法推算出前輪轉(zhuǎn)向角的大小自動(dòng)糾正偏差，實(shí)現(xiàn)直線行駛； 陽〇1引 （6)山地機(jī)械行駛到基準(zhǔn)直線末端，由人工操作掉頭，并根據(jù)激光測距傳感器測量 山地機(jī)械與固定節(jié)點(diǎn)的距離大小，快速地把山地機(jī)械開到兩個(gè)激光接收器能同時(shí)接收到激 光的位置，確定山地機(jī)械即將行駛直線的起點(diǎn)，實(shí)現(xiàn)山地機(jī)械即將行駛直線與剛行駛直線 的間距保持一定值。
[0013] 所述的基準(zhǔn)直線，其確定方法：將山地機(jī)械開到作業(yè)場地起始點(diǎn)，此時(shí)巧螺儀的測 量值初始化為0,山地機(jī)械所在的方向即為基準(zhǔn)直線所在的方向。將基準(zhǔn)直線分為等長的X 段，可W根據(jù)實(shí)際的情況改變X的取值，X的取值越大，等長線段的數(shù)量越多，山地機(jī)械的檢 偏、糾偏的實(shí)時(shí)性和準(zhǔn)確性越高。
[0014] 所述的兩個(gè)固定節(jié)點(diǎn)包括：中央處理器，激光發(fā)射器，電機(jī)，無線數(shù)據(jù)收發(fā)模塊和 電源模塊；所述的激光發(fā)射器，電機(jī)，無線數(shù)據(jù)收發(fā)模塊，電源模塊分別通過導(dǎo)線連接到中 央處理器相應(yīng)的I/O口。
[0015] 所述的移動(dòng)節(jié)點(diǎn)包括：中央處理器，第一激光接收器，第二激光接收器，無線數(shù)據(jù) 收發(fā)模塊，激光測距傳感器，電機(jī)，慣性傳感器，光電測速模塊和電源模塊；所述的兩個(gè)激光 接收器，無線數(shù)據(jù)收發(fā)模塊，激光測距傳感器，電路，慣性傳感器，光電測速模塊，電源模塊 分別通過導(dǎo)線連接到中央處理器相應(yīng)的I/O口。
[0016] 所述固定節(jié)點(diǎn)和移動(dòng)節(jié)點(diǎn)的電機(jī)為自帶驅(qū)動(dòng)電路的步進(jìn)電機(jī)。
[0017] 所述的左右偏向檢測法是在等分點(diǎn)處，當(dāng)兩個(gè)激光接收器同時(shí)接收到激光，說明 山地機(jī)械沒有出現(xiàn)左、右偏轉(zhuǎn)；當(dāng)?shù)谝患す饨邮掌飨冉邮盏降谝患す獍l(fā)射器發(fā)射的激光，說 明山地機(jī)械向左偏；當(dāng)?shù)诙す饨邮掌飨冉邮盏降诙す獍l(fā)射器發(fā)射的激光說明山地機(jī)械 向右偏。
[0018] 所述的固定節(jié)點(diǎn)和移動(dòng)節(jié)點(diǎn)之間的通信方式由無線數(shù)據(jù)收發(fā)模塊實(shí)現(xiàn)。
[0019] 步驟（3)所述的類正比控制方法是，固定節(jié)點(diǎn)上的電機(jī)，其每次轉(zhuǎn)動(dòng)的角度與線 段的長度L匹配，其角速度的大小與山地機(jī)械的行駛速度匹配，計(jì)算兩個(gè)固定節(jié)點(diǎn)上的電 機(jī)每次轉(zhuǎn)動(dòng)的角度與角速度的表達(dá)式為：
[0020] (1)第一固定節(jié)點(diǎn)的電機(jī)每次轉(zhuǎn)動(dòng)角度AΘ。和角速度最小值Δω計(jì)算表達(dá) 式：
[0021]
U)
[0022]
[0023] 其中，η表示的是等分點(diǎn)的序號(hào)，取值為：1，2, . ..，X;m的取值為山地機(jī)械所走的 基準(zhǔn)直線的序號(hào)；AL表示基準(zhǔn)直線之間的間距；d為第一固定節(jié)點(diǎn)與山地機(jī)械起始點(diǎn)的距 離；V為山地機(jī)械行駛的速度；
[0024] 似第二固定節(jié)點(diǎn)的電機(jī)每次轉(zhuǎn)動(dòng)角度ΔΘ。'和角速度最小值Δ 計(jì)算表 達(dá)式：
[0027] 表達(dá)式中各參數(shù)的含義與表達(dá)式（2)相同，X表示的是等分點(diǎn)的總數(shù)。
[00測步驟妨所述的航向角α。的計(jì)算法，其步驟如下，設(shè)山地機(jī)械在兩個(gè)相鄰的等分 點(diǎn)之間實(shí)際行駛的路程為S。;由巧螺儀數(shù)據(jù)獲得山地機(jī)械在兩個(gè)相鄰的等分點(diǎn)之間的航向 角測量值為代=
[0029] (1)當(dāng)?shù)谝患す饨邮掌飨冉邮盏降谝患す獍l(fā)射器發(fā)射的激光時(shí)，則可判定向左偏 離，且
α。的計(jì)算表達(dá)式：
[0030]
[0031] 表達(dá)式中各參數(shù)的含義與表達(dá)式（1)相同。 陽0巧同理，
αη的計(jì)算表達(dá)式：
[0033]
[0034] 表達(dá)式中各參數(shù)的含義與表達(dá)式（1)相同；
[0035] (2)當(dāng)?shù)诙す饨邮掌飨冉邮盏降诙す獍l(fā)射器發(fā)射的激光時(shí)，則可判定向右偏 離，且
α。的計(jì)算表達(dá)式：
[0036]
[0037] 表達(dá)式中各參數(shù)的含義與表達(dá)式（1