本實(shí)用新型涉及拖拉機(jī)自動(dòng)導(dǎo)航領(lǐng)域，具體地講，是一種多傳感器融合的拖拉機(jī)導(dǎo)航控制系統(tǒng)。

背景技術(shù)：

目前的農(nóng)業(yè)作業(yè)中，通常由駕駛員獨(dú)自操作，勞動(dòng)強(qiáng)度大且重復(fù)單調(diào)，容易使人疲勞并產(chǎn)生誤操作。農(nóng)機(jī)自動(dòng)導(dǎo)航技術(shù)是精細(xì)農(nóng)業(yè)的一項(xiàng)重要技術(shù)，該技術(shù)可以自動(dòng)獲得導(dǎo)航路徑并控制農(nóng)業(yè)機(jī)械沿著目標(biāo)路徑行駛，輔助駕駛員操作農(nóng)業(yè)機(jī)械進(jìn)行作業(yè)。拖拉機(jī)自動(dòng)導(dǎo)航是農(nóng)業(yè)現(xiàn)代化的重要基礎(chǔ)，實(shí)現(xiàn)拖拉機(jī)自動(dòng)導(dǎo)航可以讓農(nóng)業(yè)作業(yè)者降低工作強(qiáng)度，并且能顯著地提高農(nóng)機(jī)的作業(yè)精度，提高農(nóng)田的土地利用率，降低生產(chǎn)成本，提高作物產(chǎn)量與經(jīng)濟(jì)效益，將勞動(dòng)者從繁重的農(nóng)田作業(yè)中解放出來。

拖拉機(jī)導(dǎo)航技術(shù)在北美、日本和歐洲一些國(guó)家中研究起步較早，并取得了較多研究成果，國(guó)內(nèi)的專家學(xué)者也做了廣泛的研究，取得了一定的成果。如申請(qǐng)?zhí)枮?014101751440的發(fā)明專利公開了一種對(duì)射式激光拖拉機(jī)輔助導(dǎo)航控制系統(tǒng)，授權(quán)號(hào)101833334A的發(fā)明專利公開了一種拖拉機(jī)自動(dòng)導(dǎo)航控制系統(tǒng)及其方法，申請(qǐng)?zhí)枮?04656647A的發(fā)明專利公開了一種低矮作物田間自走拖拉機(jī)導(dǎo)航控制系統(tǒng)等。可以看出，采用的都是單一導(dǎo)航定位技術(shù)，比如GPS導(dǎo)航、激光發(fā)射導(dǎo)航技術(shù)等，每種導(dǎo)航方式各有優(yōu)點(diǎn)，但是因單一導(dǎo)航技術(shù)受條件限制都存在一定的使用條件，很難能夠提供連續(xù)、穩(wěn)定高質(zhì)量的定位信息；另外，目前拖拉機(jī)路徑跟蹤控制算法中，大多都僅僅將拖拉機(jī)航向角和偏移量做為被控量或參考量，但沒有考慮到選取了拖拉機(jī)預(yù)估路徑曲度和當(dāng)前行駛速度；導(dǎo)致現(xiàn)有的技術(shù)方案往往只能保證直線行駛時(shí)的控制效果，而很難保證曲線行駛或拐彎時(shí)的路徑跟蹤效果。

技術(shù)實(shí)現(xiàn)要素：

本實(shí)用新型要解決的技術(shù)問題是提供一種多傳感器融合的拖拉機(jī)導(dǎo)航控制系統(tǒng)，具有精確的車輛定位和路徑跟蹤性能。

本實(shí)用新型采用如下技術(shù)方案實(shí)現(xiàn)實(shí)用新型目的：

一種多傳感器融合的拖拉機(jī)導(dǎo)航控制系統(tǒng)，包括上位機(jī)和下位機(jī)，所述的上位機(jī)和下位機(jī)通過串口通信模塊相互連接，實(shí)現(xiàn)拖拉機(jī)位置的精準(zhǔn)定位和設(shè)定路徑的精確跟蹤；所述的上位機(jī)連接觸摸屏、北斗定位接收模塊、圖像采集模塊、位姿檢測(cè)模塊、串口通信模塊；所述的下位機(jī)連接數(shù)據(jù)存儲(chǔ)模塊、壓力檢測(cè)模塊、CAN通信模塊、換向閥控制模塊、電壓檢測(cè)模塊、車速檢測(cè)模塊、車輪轉(zhuǎn)角檢測(cè)模塊、電源模塊、報(bào)警及緊急處理模塊和串口通信模塊。

(1)本技術(shù)方案中導(dǎo)航控制系統(tǒng)通過采用北斗定位和機(jī)器視覺定位相結(jié)合的方式，能夠?qū)崿F(xiàn)兩種導(dǎo)航技術(shù)的優(yōu)勢(shì)互補(bǔ)，可以避免使用單一定位造成的車輛位置誤差；組合導(dǎo)航功能既可以用在有作物的農(nóng)田作業(yè)中也可以用在無(wú)作物的農(nóng)田中，組合導(dǎo)航定位技術(shù)在可靠性、準(zhǔn)確性等方面都較單獨(dú)導(dǎo)航情況要好。(2)采用雙天線的北斗信號(hào)接收裝置，并且采用位姿檢測(cè)模塊的數(shù)據(jù)對(duì)北斗定位信息進(jìn)行修正，得到的拖拉機(jī)位置數(shù)據(jù)更精確；(3)不僅選取了拖拉機(jī)當(dāng)前航線偏移量、當(dāng)前航向角偏移量，而且選取了拖拉機(jī)預(yù)估路徑曲度和參考車速進(jìn)行處理，使導(dǎo)航控制系統(tǒng)具有良好的路徑控制效果。

拖拉機(jī)當(dāng)前航線偏移量、當(dāng)前航向角偏移量、拖拉機(jī)預(yù)估路徑曲度值由上位機(jī)通過北斗導(dǎo)航數(shù)據(jù)或圖像數(shù)據(jù)計(jì)算輸出；拖拉機(jī)參考車速V_U(s)由上位機(jī)發(fā)送的車速V_R(s)和車速采集V_M(s)決定，按照如下公式確定：

因拖拉機(jī)在低速行駛時(shí)，通過北斗導(dǎo)航獲得的行駛速度其準(zhǔn)確度低于車速檢測(cè)模塊獲得的車速，所以在確定車速時(shí)，如果車速檢測(cè)模塊采集的車速V_M(s)小于等于2公里/秒或接收的上位機(jī)的車速V_R(s)等于0時(shí)，則確定的參考車速為V_M(s)，這樣可以避免因北斗導(dǎo)航在低速或故障時(shí)造成的速度誤差；當(dāng)車速檢測(cè)模塊采集的車速V_M(s)大于2公里/秒或車速檢測(cè)模塊出現(xiàn)故障時(shí)，確定參考車速為上位機(jī)發(fā)送車速V_R(s)。

作為對(duì)本技術(shù)方案的進(jìn)一步限定：

換向閥控制模塊分別與電壓檢測(cè)模塊和液壓轉(zhuǎn)向系統(tǒng)電連接。

壓力檢測(cè)模塊還與液壓轉(zhuǎn)向系統(tǒng)電連接。

上位機(jī)采用EPCS-8980型工控機(jī)。

觸摸屏選用四線電阻式觸摸屏SV0804S-03。

北斗定位接收模塊采用UM220模塊。

圖像采集模塊由OKAC1310CCD攝像頭和信號(hào)調(diào)理電路組成。

位姿檢測(cè)模塊選用AHRS-3000小型航姿測(cè)量模塊。

下位機(jī)采用16位飛思卡爾單片機(jī)MC9S12XS128MAL。

數(shù)據(jù)存儲(chǔ)模塊采用兩片32MB的SDRAM MT48LC4M32B2數(shù)據(jù)存儲(chǔ)芯片。

壓力檢測(cè)模塊由壓力傳感器MBS1250和信號(hào)調(diào)理電路組成。

CAN通信模塊由TJA1043T芯片與其外圍電路組成，通過該模塊，下位機(jī)能夠?qū)⒏鶕?jù)預(yù)估行駛距離和預(yù)估路徑曲度確定的拖拉機(jī)期望速度發(fā)送給拖拉機(jī)控制器，確保在拖拉機(jī)行駛至終點(diǎn)或拐彎時(shí)的控制效果，特別在當(dāng)前航線偏移量O_R(s)超過可允許最大航線偏移量O_M時(shí)，通過該模塊給拖拉機(jī)控制器發(fā)送停車信號(hào)。

換向閥控制模塊由D/A轉(zhuǎn)換芯片DAC0832、運(yùn)算放大器LM358N及外圍電路組成。

電壓檢測(cè)模塊由運(yùn)算放大器AD741和外圍電路組成。

通過設(shè)計(jì)電壓檢測(cè)模塊，能夠?qū)Q向閥控制模塊的輸出電壓實(shí)施閉環(huán)控制，通過實(shí)時(shí)檢測(cè)輸出電壓，避免輸出電壓失控現(xiàn)象發(fā)生。

車速檢測(cè)模塊由測(cè)速傳感器VB-Z9400及信號(hào)調(diào)理電路組成。

車輪轉(zhuǎn)角檢測(cè)模塊由線位移傳感器HPS-M1和信號(hào)調(diào)理電路組成。

電源模塊由產(chǎn)生24V電壓芯片LT4356IS、產(chǎn)生5V電壓芯片LM7805、產(chǎn)生正負(fù)15電壓芯片MD20-12D15、產(chǎn)生10V電壓芯片AD581及外圍電路組成。

串口通信模塊由串口通信芯片MAX232芯片及其外圍電路組成。

報(bào)警及緊急處理模塊由喇叭、發(fā)光燈、光耦和繼電器等組成。報(bào)警和緊急處理模塊能夠在液壓閥運(yùn)行至最大或當(dāng)曲線路徑彎度過大時(shí)發(fā)出報(bào)警信息并啟動(dòng)相應(yīng)的緊急響應(yīng)動(dòng)作。

上位機(jī)通過串口通信模塊發(fā)送給下位機(jī)用于路徑跟蹤的數(shù)據(jù)包括：當(dāng)前航線偏移量、當(dāng)前航向角偏移量、預(yù)估行駛距離、當(dāng)前行駛速度、預(yù)估路徑曲度。

多傳感器融合的拖拉機(jī)導(dǎo)航控制系統(tǒng)的定位方法，通過上位機(jī)進(jìn)行拖拉機(jī)定位，包括如下步驟：

(1)通過觸摸屏設(shè)定拖拉機(jī)工作田間模式：無(wú)農(nóng)作物田地，有農(nóng)作物田地；

(2)采用雙天線的北斗接收裝置，用北斗定位接收模塊獲取拖拉機(jī)的絕對(duì)位置、行駛速度和航向角信息；

(3)采用位姿檢測(cè)模塊獲取拖拉機(jī)的航向角、橫滾角、俯仰角數(shù)據(jù)，并依據(jù)這些數(shù)據(jù)對(duì)北斗定位信息進(jìn)行修正；

(4)如果田間模式設(shè)定為無(wú)農(nóng)作物田地，則關(guān)閉圖像采集模塊，不采用機(jī)器視覺定位方式；如果田間模式設(shè)定為有農(nóng)作物田地，則開啟圖采集模塊，對(duì)采集圖像數(shù)據(jù)進(jìn)行處理，并生成導(dǎo)航基準(zhǔn)線、獲取表征農(nóng)作物特征的基本點(diǎn)。

(5)如果田間模式設(shè)定為無(wú)農(nóng)作物田地，則完全采用經(jīng)過位姿信息修正的北斗定位和路徑數(shù)據(jù)；如果田間模式設(shè)定為有農(nóng)作物田地，則采用模糊自適應(yīng)擴(kuò)展卡爾曼濾波器對(duì)北斗定位數(shù)據(jù)和機(jī)器視覺數(shù)據(jù)進(jìn)行融合，其中北斗定位提供拖拉機(jī)的絕對(duì)位置坐標(biāo)、航向角度和行駛速度，機(jī)器視覺提供導(dǎo)航路徑中已知點(diǎn)的相對(duì)位置坐標(biāo)，北斗定位系統(tǒng)和機(jī)器視覺系統(tǒng)坐標(biāo)統(tǒng)一后，由濾波器對(duì)北斗提供的位置數(shù)據(jù)和機(jī)器視覺提供的位置數(shù)據(jù)進(jìn)行濾波，得到精確的位置數(shù)據(jù)和路徑數(shù)據(jù)。

與目前多傳感器融合技術(shù)中常用的擴(kuò)展卡爾曼濾波器和無(wú)跡卡爾曼濾波器相比，模糊自適應(yīng)擴(kuò)展卡爾曼濾波算法能夠通過實(shí)時(shí)得到的測(cè)量新息方差和理論方差的比值，并利用模糊自適應(yīng)控制器不斷調(diào)整量測(cè)噪聲協(xié)方差陣的加權(quán)系數(shù)，減少校正位置誤差，提高拖拉機(jī)定位數(shù)據(jù)的準(zhǔn)確性和穩(wěn)定性。

多傳感器融合的拖拉機(jī)導(dǎo)航控制系統(tǒng)的控制方法，控制方法為路徑跟蹤控制，包括如下步驟：

(1)路徑跟蹤控制系統(tǒng)采用串級(jí)控制結(jié)構(gòu)進(jìn)行設(shè)計(jì)，其中主回路采取前饋加反饋控制方法，副回路采用單位負(fù)反饋方法，整個(gè)系統(tǒng)結(jié)構(gòu)由偏移量設(shè)定值O_S(s)、當(dāng)前航線偏移量O_R(s)、偏移量控制器C₀(s)、轉(zhuǎn)角控制器C_S(s)、換向閥控制器C_V(s)、電壓采集V_M(s)、轉(zhuǎn)向系統(tǒng)G(s)、預(yù)估路徑曲度PC_R(s)、當(dāng)前航向角偏移量A_R(s)、航向角控制器C_A(s)、車輪轉(zhuǎn)角反饋A(s)、上位機(jī)發(fā)送的車速V_R(s)、車速采集V_M(s)、參考車速V_U(s)組成；

(2)主回路中的反饋控制器為偏移量控制器C₀(s)，其設(shè)定值輸入為偏移量設(shè)定值O_S(s)和當(dāng)前航線偏移量O_R(s)的差值，偏移量控制器C₀(s)的輸出為轉(zhuǎn)向角度θ_FB，通過如下公式計(jì)算：

其中ΔO＝O_S(s)-O_R(s)，K_FB1為偏移量控制器比例系數(shù)，K_FB2為偏移量控制器微分系數(shù)，K_FB3為偏移量控制器二階微分系數(shù)。因拖拉機(jī)路徑跟蹤的直接控制量是航線偏移量，本實(shí)用新型通過對(duì)偏移量差值實(shí)施比例、微分和二階微分的控制，可減少控制過程中的超調(diào)，克服振蕩現(xiàn)象發(fā)生；通過設(shè)定可允許最大航線偏移量O_M，可使拖拉機(jī)在航線偏差超過可允許范圍時(shí)不再繼續(xù)執(zhí)行轉(zhuǎn)向，并且控制偏移量控制器輸出為0，避免損壞液壓閥或安全事故發(fā)生；

(3)主回路中的前饋控制器為航向角控制器C_A(s)，其參考輸入為預(yù)估路徑曲度PC_R(s)、當(dāng)前航向角偏移量A_R(s)和當(dāng)前航線偏移量O_R(s)，航向角控制器C_A(s)的輸出為轉(zhuǎn)向角度θ_FF，通過如下公式計(jì)算：

其中K_FF1為航向角控制器路徑曲度控制比例系數(shù)，K_FF2為航向角控制器比例系數(shù)，K_FF3為航向角控制器微分比例系數(shù)。在航線偏移量允許值O_P范圍內(nèi)，通過將航向角作為參考量，可實(shí)施預(yù)先控制，減少路徑跟蹤過程的波動(dòng)；將預(yù)估路徑曲度作為參考量，可實(shí)現(xiàn)曲線行駛或拐彎時(shí)的預(yù)先控制，提高非直線路徑的跟蹤效果。

(4)副回路控制器為轉(zhuǎn)角控制器C_S(s)，其設(shè)定值為θ_FB與θ_FF之和與車輪轉(zhuǎn)角反饋A(s)的差值，參考輸入量為車輪轉(zhuǎn)角反饋A(s)和參考車速V_U(s)，其中參考車速V_U(s)由上位機(jī)發(fā)送的車速V_R(s)和車速采集V_M(s)決定，按照如下公式確定參考車速：

轉(zhuǎn)角控制器C_S(s)的輸出為車輪轉(zhuǎn)角設(shè)定值θ，通過如下公式計(jì)算：其中K_θ為轉(zhuǎn)角控制器C_S(s)的比例系數(shù)。因拖拉機(jī)在低速行駛時(shí)，通過北斗導(dǎo)航獲得的行駛速度其準(zhǔn)確度低于車速檢測(cè)模塊獲得的車速，所以在確定車速時(shí)，如果車速檢測(cè)模塊采集的車速V_M(s)小于等于2公里/秒或接收的上位機(jī)的車速V_R(s)等于0時(shí)，則確定的參考車速為V_M(s)，這樣可以避免因北斗導(dǎo)航在低速或故障時(shí)造成的速度誤差；當(dāng)車速檢測(cè)模塊采集的車速V_M(s)大于2公里/秒或車速檢測(cè)模塊出現(xiàn)故障時(shí)，確定參考車速為上位機(jī)發(fā)送車速V_R(s)。將車速信號(hào)作為確定車輪轉(zhuǎn)角設(shè)定值的參數(shù)，可減少系統(tǒng)調(diào)整時(shí)間；

(5)換向閥控制器C_V(s)的設(shè)定值為車輪轉(zhuǎn)角設(shè)定值θ，其通過換向閥控制模塊輸出負(fù)10V至正10V范圍的電壓控制液壓轉(zhuǎn)向系統(tǒng)實(shí)施轉(zhuǎn)向。

與現(xiàn)有技術(shù)相比，本實(shí)用新型在控制量和參考量設(shè)定時(shí)，不僅選取了拖拉機(jī)當(dāng)前航線偏移量、當(dāng)前航向角偏移量，而且選取了拖拉機(jī)預(yù)估路徑曲度和當(dāng)前行駛速度，提出的拖拉機(jī)路徑跟蹤串級(jí)控制結(jié)構(gòu)，主回路采取前饋加反饋控制方法，副回路采取單位負(fù)反饋方法，該控制方法不僅保證直線行駛時(shí)的路徑跟蹤效果，而且在拐彎時(shí)也具有很高的控制精度。

附圖說明

圖1為本實(shí)用新型的原理框圖。

圖2為本實(shí)用新型所采用的控制方法結(jié)構(gòu)示意圖。

圖3為本實(shí)用新型的換向閥控制模塊的電路原理圖。

圖4為本實(shí)用新型的電壓檢測(cè)模塊的電路原理圖。

圖5為本實(shí)用新型的車速檢測(cè)模塊的電路原理圖。

圖6為本實(shí)用新型的轉(zhuǎn)角檢測(cè)模塊的電路原理圖。

附圖標(biāo)記說明：1－觸摸屏；2－數(shù)據(jù)存儲(chǔ)模塊；3－壓力檢測(cè)模塊；4－北斗定位接收模塊；5－上位機(jī)；6－報(bào)警及緊急處理模塊；7－下位機(jī)；8－CAN通信模塊；9－圖像采集模塊；10－串口通信模塊；11－換向閥控制模塊；12－液壓轉(zhuǎn)向系統(tǒng)；13－拖拉機(jī)；14－位姿檢測(cè)模塊；15－電源模塊；16－車速檢測(cè)模塊；17－車輪轉(zhuǎn)角檢測(cè)模塊；18－電壓檢測(cè)模塊。

具體實(shí)施方式

為了使本實(shí)用新型的目的、技術(shù)方案及優(yōu)點(diǎn)更加清楚明白，以下結(jié)合附圖及實(shí)施例，對(duì)本實(shí)用新型進(jìn)行進(jìn)一步詳細(xì)說明。此處所描述的具體實(shí)施例僅用以解釋本實(shí)用新型，并不用于限定本實(shí)用新型。本實(shí)用新型涵蓋任何由權(quán)利要求定義的在本實(shí)用新型的精髓和范圍上做的替代、修改、等效方法及方案。為了使公眾對(duì)本實(shí)用新型有更好的了解，在具體實(shí)施方式中對(duì)本實(shí)用新型的細(xì)節(jié)描述中，詳盡描述了一些特定的細(xì)節(jié)部分，對(duì)于未描述的部分，均為本領(lǐng)域技術(shù)的常規(guī)技術(shù)。

結(jié)合圖1、圖2可知，本實(shí)用新型實(shí)施例提供了一種多傳感器融合的拖拉機(jī)導(dǎo)航控制系統(tǒng)，由上位機(jī)5和下位機(jī)7組成，上位機(jī)5和下位機(jī)7通過串口通信模塊10相互連接，實(shí)現(xiàn)拖拉機(jī)13位置的精準(zhǔn)定位和設(shè)定路徑的精確跟蹤；所述的上位機(jī)5連接觸摸屏1、北斗定位接收模塊4、圖像采集模塊9、位姿檢測(cè)模塊14、串口通信模塊10；所述的下位機(jī)7連接數(shù)據(jù)存儲(chǔ)模塊2、壓力檢測(cè)模塊3、CAN通信模塊8、換向閥控制模塊11、電壓檢測(cè)模塊、車速檢測(cè)模塊16、車輪轉(zhuǎn)角檢測(cè)模塊17、電源模塊15、報(bào)警及緊急處理模塊6和串口通信模塊10；換向閥控制模塊11分別與電壓檢測(cè)模塊18和液壓轉(zhuǎn)向系統(tǒng)12電連接，壓力檢測(cè)模塊3還與液壓轉(zhuǎn)向系統(tǒng)12電連接。

本實(shí)用新型中上位機(jī)5選用EPCS-8980型工控機(jī)，它是整個(gè)導(dǎo)航系統(tǒng)中進(jìn)行車輛位置信息處理的核心部件以及人機(jī)對(duì)話的中心，與觸摸屏1結(jié)合完成田間模式設(shè)定、導(dǎo)航參數(shù)配置、路徑下載、導(dǎo)航任務(wù)設(shè)定、路徑規(guī)劃、車輛運(yùn)行軌跡實(shí)時(shí)顯示等功能。

觸摸屏1與上位機(jī)5通過功能引腳相互連接，作為拖拉機(jī)13作業(yè)時(shí)的人機(jī)交互界面，可實(shí)現(xiàn)導(dǎo)航模式選擇、導(dǎo)航參數(shù)輸入和車輛運(yùn)行軌跡顯示等功能，方便操作人員對(duì)拖拉機(jī)13導(dǎo)航系統(tǒng)工作狀態(tài)進(jìn)行判斷，本實(shí)用新型選用四線電阻式觸摸屏1SV0804S-03。

北斗定位接收模塊4與上位機(jī)5通過串行通信引腳相互連接，本實(shí)用新型采用UM220模塊接收北斗衛(wèi)星信號(hào)用來確定車輛位置，因采用的是北斗常規(guī)接收技術(shù)，在此不再贅述。

圖像采集模塊9與上位機(jī)5通過串行通信引腳相互連接，由OKAC1310CCD攝像頭采集農(nóng)作物圖像，經(jīng)過信號(hào)調(diào)理電路后經(jīng)串口送至上位機(jī)5，經(jīng)灰度變換、圖像分割、圖像去噪后提取出導(dǎo)航基準(zhǔn)線，確定導(dǎo)航路徑特征值。

位姿檢測(cè)模塊14與上位機(jī)5通過串行通信引腳相互連接，選用AHRS-3000小型航姿測(cè)量模塊將拖拉機(jī)13動(dòng)靜態(tài)環(huán)境下的橫滾角、俯仰角和航向角通過串口發(fā)送至上位機(jī)5，上位機(jī)5利用這些參數(shù)對(duì)北斗定位數(shù)據(jù)進(jìn)行補(bǔ)償，可減少或避免因受地面起伏影響車體產(chǎn)生傾斜而導(dǎo)航的定位誤差。

下位機(jī)7采用16位飛思卡爾單片機(jī)MC9S12XS128MAL，是拖拉機(jī)13路徑跟蹤控制的核心，通過串口通信模塊10接收上位機(jī)5發(fā)送的拖拉機(jī)13位置信息并經(jīng)過控制運(yùn)算后，由換向閥控制模塊11輸出電壓信號(hào)實(shí)現(xiàn)對(duì)液壓轉(zhuǎn)向系統(tǒng)12的控制。

數(shù)據(jù)存儲(chǔ)模塊2，采用兩片32MB的SDRAM MT48LC4M32B2，分別與飛思卡爾單片機(jī)通過功能引腳相互連接，用于存儲(chǔ)導(dǎo)航狀態(tài)信息和控制輸出信息，方便后期對(duì)導(dǎo)航系統(tǒng)工作效果進(jìn)行評(píng)估和分析。

壓力檢測(cè)模塊3與下位機(jī)7和液壓轉(zhuǎn)向系統(tǒng)12電連接，用于判斷拖拉機(jī)13的駕駛模式，由安裝在導(dǎo)航閥組上的壓力傳感器MBS1250檢測(cè)閥組壓力并轉(zhuǎn)換成電壓信號(hào)送至下位機(jī)7的數(shù)字量輸入口，傳感器輸出低壓信號(hào)表示拖拉機(jī)13處于自動(dòng)導(dǎo)航模式，傳感器輸出高壓信號(hào)表示拖拉機(jī)13處于人工駕駛模式。

CAN通信模塊8由TJA1043T芯片與其外圍電路組成，通過該模塊，下位機(jī)7能夠?qū)⒏鶕?jù)預(yù)估行駛距離和預(yù)估路徑曲度確定的拖拉機(jī)13期望速度發(fā)送給拖拉機(jī)控制器，確保在拖拉機(jī)13行駛至終點(diǎn)或拐彎時(shí)的控制效果，特別在當(dāng)前航線偏移量O_R(s)超過可允許最大航線偏移量O_M時(shí)，通過該模塊給拖拉機(jī)控制器發(fā)送停車信號(hào)。

結(jié)合圖3可知，換向閥控制模塊11分別與電壓檢測(cè)模塊18和液壓轉(zhuǎn)向系統(tǒng)12電連接，選用D/A轉(zhuǎn)換芯片DAC0832輸出0-255的數(shù)字量，該數(shù)字量經(jīng)過放大器U18-U21產(chǎn)生正10V至負(fù)10V范圍內(nèi)的電壓用于控制換向電磁閥，實(shí)現(xiàn)對(duì)液壓轉(zhuǎn)向系統(tǒng)12的控制。本實(shí)用新型選用4WRA6E20-10B/24Z4型號(hào)的比例方向閥。

結(jié)合圖4可知，電壓檢測(cè)模塊18由運(yùn)算放大器和外圍電路組成，通過設(shè)計(jì)電壓檢測(cè)模塊，能夠?qū)Q向閥控制模塊11的輸出電壓實(shí)施閉環(huán)控制，通過實(shí)時(shí)檢測(cè)輸出電壓，避免輸出電壓失控現(xiàn)象發(fā)生。

結(jié)合圖5可知，車速檢測(cè)模塊16與下位機(jī)7的中斷輸入口相連接，該模塊由測(cè)速傳感器和調(diào)理電路組成，測(cè)速傳感器選用VB-Z9400霍爾轉(zhuǎn)速傳感器，它通過感應(yīng)導(dǎo)磁體上凸起的齒或是凹下的槽，輸出方波信號(hào)，該方波信號(hào)經(jīng)過四分之一個(gè)LM339和電阻R38,R39,R40構(gòu)成的滯回比較電路去除噪聲干擾，送至下位機(jī)7的外部中斷引腳，下位機(jī)7通過下降沿觸發(fā)的外部中斷方式對(duì)車速信號(hào)進(jìn)行捕捉。

結(jié)合圖6可知，車輪轉(zhuǎn)角檢測(cè)模塊17與下位機(jī)7電連接，該模塊用于完成對(duì)車輪轉(zhuǎn)角的實(shí)時(shí)檢測(cè)，選用HPS-M1型線位移傳感器將車輪轉(zhuǎn)角信號(hào)轉(zhuǎn)換成電壓信號(hào)后經(jīng)過電阻R13和電容C11濾波后進(jìn)入放大器U12的反向輸入端，放大器U12的正向輸入端經(jīng)電阻R15接地，U12的輸出端經(jīng)電阻R14后接入其反向輸入端，同時(shí)U12的輸出端經(jīng)電阻R16后接入放大器U13的反向輸入端，放大器U13的同向輸入端經(jīng)電阻R18后接地，U13的輸出端經(jīng)電阻R17后接入其反向輸入端，同時(shí)與下位機(jī)7的模擬量輸入口PA5相接。

電源模塊15與下位機(jī)7電連接，由車載蓄電池供電，分別經(jīng)過LT4356IS電路和LM7805電路產(chǎn)生24V防浪涌電壓和5V電壓，經(jīng)過MD20-12D15模塊產(chǎn)生正負(fù)15的電壓，經(jīng)過AD581產(chǎn)生10V的基準(zhǔn)電壓，為保證下位機(jī)7系統(tǒng)的供電電壓不至于過高或過低，設(shè)計(jì)了電源監(jiān)控保護(hù)電路。

串口通信模塊10分別與上位機(jī)5和下位機(jī)7相連接，該模塊由串口通信芯片MAX232芯片及其外圍電路組成，通過串口通信模塊10可完成下位機(jī)7和上位機(jī)5的數(shù)據(jù)交換。

報(bào)警及緊急處理模塊6與下位機(jī)7電連接，用于在液壓閥運(yùn)行至最大或當(dāng)曲線路徑彎度過大時(shí)，下位機(jī)7可以控制繼電器打開，使與蜂鳴器和發(fā)光燈相連的引腳為高電平，發(fā)出報(bào)警信息，并且控制輸出電壓為零使換向閥回歸中心位置，同時(shí)置位手動(dòng)模式。

上位機(jī)5通過串口通信模塊10發(fā)送給下位機(jī)7用于路徑跟蹤的數(shù)據(jù)包括：當(dāng)前航線偏移量、當(dāng)前航向角偏移量、預(yù)估行駛距離、當(dāng)前行駛速度、預(yù)估路徑曲度。

多傳感器融合的拖拉機(jī)導(dǎo)航控制系統(tǒng)的定位方法，通過上位機(jī)5對(duì)拖拉機(jī)13進(jìn)行定位，包括如下步驟：

(1)通過觸摸屏1設(shè)定拖拉機(jī)13工作田間模式：無(wú)農(nóng)作物田地，有農(nóng)作物田地；

(2)采用雙天線的北斗接收裝置，用北斗定位接收模塊4獲取拖拉機(jī)13的絕對(duì)位置、行駛速度和航向角信息；

(3)采用位姿檢測(cè)模塊14獲取拖拉機(jī)13的航向角、橫滾角、俯仰角數(shù)據(jù)，并依據(jù)這些數(shù)據(jù)對(duì)北斗定位信息進(jìn)行修正；

(4)如果田間模式設(shè)定為無(wú)農(nóng)作物田地，則關(guān)閉圖像采集模塊9，不采用機(jī)器視覺定位方式；如果田間模式設(shè)定為有農(nóng)作物田地，則開啟圖像采集模塊9，對(duì)采集圖像數(shù)據(jù)進(jìn)行處理，并生成導(dǎo)航基準(zhǔn)線、獲取表征農(nóng)作物特征的基本點(diǎn)。

(5)如果田間模式設(shè)定為無(wú)農(nóng)作物田地，則完全采用經(jīng)過位姿信息修正的北斗定位和路徑數(shù)據(jù)；如果田間模式設(shè)定為有農(nóng)作物田地，則采用模糊自適應(yīng)擴(kuò)展卡爾曼濾波器對(duì)北斗定位數(shù)據(jù)和機(jī)器視覺數(shù)據(jù)進(jìn)行融合，其中北斗定位提供拖拉機(jī)13的絕對(duì)位置坐標(biāo)、航向角度和行駛速度，機(jī)器視覺提供導(dǎo)航路徑中已知點(diǎn)的相對(duì)位置坐標(biāo)，北斗定位系統(tǒng)和機(jī)器視覺系統(tǒng)坐標(biāo)統(tǒng)一后，由濾波器對(duì)北斗提供的位置數(shù)據(jù)和機(jī)器視覺提供的位置數(shù)據(jù)進(jìn)行濾波，得到精確的位置數(shù)據(jù)和路徑數(shù)據(jù)。

與目前多傳感器融合技術(shù)中常用的擴(kuò)展卡爾曼濾波器和無(wú)跡卡爾曼濾波器相比，模糊自適應(yīng)擴(kuò)展卡爾曼濾波算法能夠通過實(shí)時(shí)得到的測(cè)量新息方差和理論方差的比值，并利用模糊自適應(yīng)控制器不斷調(diào)整量測(cè)噪聲協(xié)方差陣的加權(quán)系數(shù)，減少校正位置誤差，提高拖拉機(jī)13定位數(shù)據(jù)的準(zhǔn)確性和穩(wěn)定性。

一種多傳感器融合的拖拉機(jī)導(dǎo)航控制系統(tǒng)的控制方法，控制方法為路徑跟蹤控制，包括如下步驟：

(1)路徑跟蹤控制系統(tǒng)采用串級(jí)控制結(jié)構(gòu)進(jìn)行設(shè)計(jì)，其中主回路采取前饋加反饋控制方法，副回路采用單位負(fù)反饋方法，整個(gè)系統(tǒng)結(jié)構(gòu)由偏移量設(shè)定值O_S(s)、當(dāng)前航線偏移量O_R(s)、偏移量控制器C₀(s)、轉(zhuǎn)角控制器C_S(s)、換向閥控制器C_V(s)、電壓采集V_M(s)、轉(zhuǎn)向系統(tǒng)G(s)、預(yù)估路徑曲度PC_R(s)、當(dāng)前航向角偏移量A_R(s)、航向角控制器C_A(s)、車輪轉(zhuǎn)角反饋A(s)、上位機(jī)5發(fā)送的車速V_R(s)、車速采集V_M(s)、參考車速V_U(s)組成；

(2)主回路中的反饋控制器為偏移量控制器C₀(s)，其設(shè)定值輸入為偏移量設(shè)定值O_S(s)和當(dāng)前航線偏移量O_R(s)的差值，偏移量控制器C₀(s)的輸出為轉(zhuǎn)向角度θ_FB，通過如下公式計(jì)算：

其中ΔO＝O_S(s)-O_R(s)，K_FB1為偏移量控制器比例系數(shù)，K_FB2為偏移量控制器微分系數(shù)，K_FB3為偏移量控制器二階微分系數(shù)。因拖拉機(jī)13路徑跟蹤的直接控制量是航線偏移量，本實(shí)用新型通過對(duì)偏移量差值實(shí)施比例、微分和二階微分的控制，可減少控制過程中的超調(diào)，克服振蕩現(xiàn)象發(fā)生；通過設(shè)定可允許最大航線偏移量O_M，可使拖拉機(jī)13在航線偏差超過可允許范圍時(shí)不再繼續(xù)執(zhí)行轉(zhuǎn)向，并且控制偏移量控制器輸出為0，避免損壞液壓閥或安全事故發(fā)生；

(3)主回路中的前饋控制器為航向角控制器C_A(s)，其參考輸入為預(yù)估路徑曲度PC_R(s)、當(dāng)前航向角偏移量A_R(s)和當(dāng)前航線偏移量O_R(s)，航向角控制器C_A(s)的輸出為轉(zhuǎn)向角度θ_FF，通過如下公式計(jì)算：

其中K_FF1為航向角控制器路徑曲度控制比例系數(shù)，K_FF2為航向角控制器比例系數(shù)，K_FF3為航向角控制器微分比例系數(shù)。在航線偏移量允許值O_P范圍內(nèi)，通過將航向角作為參考量，可實(shí)施預(yù)先控制，減少路徑跟蹤過程的波動(dòng)；將預(yù)估路徑曲度作為參考量，可實(shí)現(xiàn)曲線行駛或拐彎時(shí)的預(yù)先控制，提高非直線路徑的跟蹤效果。

(4)副回路控制器為轉(zhuǎn)角控制器C_S(s)，其設(shè)定值為θ_FB與θ_FF之和與車輪轉(zhuǎn)角反饋A(s)的差值，參考輸入量為車輪轉(zhuǎn)角反饋A(s)和參考車速V_U(s)，其中參考車速V_U(s)由上位機(jī)5發(fā)送的車速V_R(s)和車速采集V_M(s)決定，按照如下公式確定參考車速：

轉(zhuǎn)角控制器C_S(s)的輸出為車輪轉(zhuǎn)角設(shè)定值θ，通過如下公式計(jì)算：其中K_θ為轉(zhuǎn)角控制器C_S(s)的比例系數(shù)。因拖拉機(jī)13在低速行駛時(shí)，通過北斗導(dǎo)航獲得的行駛速度其準(zhǔn)確度低于車速檢測(cè)模塊16獲得的車速，所以在確定車速時(shí)，如果車速檢測(cè)模塊16采集的車速V_M(s)小于等于2公里/秒或接收的上位機(jī)5的車速V_R(s)等于0時(shí)，則確定的參考車速為V_M(s)，這樣可以避免因北斗導(dǎo)航在低速或故障時(shí)造成的速度誤差；當(dāng)車速檢測(cè)模塊16采集的車速V_M(s)大于2公里/秒或車速檢測(cè)模塊16出現(xiàn)故障時(shí)，確定參考車速為上位機(jī)5發(fā)送車速V_R(s)。將車速信號(hào)作為確定車輪轉(zhuǎn)角設(shè)定值的參數(shù)，可減少系統(tǒng)調(diào)整時(shí)間。

(5)換向閥控制器C_V(s)的設(shè)定值為車輪轉(zhuǎn)角設(shè)定值θ，其通過換向閥控制模塊11輸出負(fù)10V至正10V范圍的電壓控制液壓轉(zhuǎn)向系統(tǒng)12實(shí)施轉(zhuǎn)向。

與現(xiàn)有技術(shù)相比，本實(shí)用新型在控制量和參考量設(shè)定時(shí)，不僅選取了拖拉機(jī)13當(dāng)前航線偏移量、當(dāng)前航向角偏移量，而且選取了拖拉機(jī)13預(yù)估路徑曲度和當(dāng)前行駛速度。提出的拖拉機(jī)13路徑跟蹤串級(jí)控制結(jié)構(gòu)，主回路采取前饋加反饋控制方法，副回路采取單位負(fù)反饋方法，該控制方法不僅保證直線行駛時(shí)的路徑跟蹤效果，而且在拐彎時(shí)也具有很高的控制精度。

最后應(yīng)說明的是：以上所述僅為本實(shí)用新型的優(yōu)選實(shí)施例而已，并不用于限制本實(shí)用新型，盡管參照前述實(shí)施例對(duì)本實(shí)用新型進(jìn)行了詳細(xì)的說明，對(duì)于本領(lǐng)域的技術(shù)人員來說，其依然可以對(duì)前述各實(shí)施例所記載的技術(shù)方案進(jìn)行修改，或者對(duì)其中部分技術(shù)特征進(jìn)行等同替換。凡在本實(shí)用新型的精神和原則之內(nèi)，所作的任何修改、等同替換、改進(jìn)等，均應(yīng)包含在本實(shí)用新型的保護(hù)范圍之內(nèi)。