本發(fā)明涉及拖拉機(jī)自動導(dǎo)航領(lǐng)域，具體地講，是多傳感器融合的拖拉機(jī)導(dǎo)航控制系統(tǒng)及其定位、控制方法。

背景技術(shù)：

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

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

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

本發(fā)明要解決的技術(shù)問題是提供多傳感器融合的拖拉機(jī)導(dǎo)航控制系統(tǒng)及其定位、控制方法，具有精確的車輛定位和路徑跟蹤性能。

本發(fā)明采用如下技術(shù)方案實現(xiàn)發(fā)明目的：

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

(1)本技術(shù)方案中導(dǎo)航控制系統(tǒng)通過采用北斗定位和機(jī)器視覺定位相結(jié)合的方式，能夠?qū)崿F(xiàn)兩種導(dǎo)航技術(shù)的優(yōu)勢互補(bǔ)，可以避免使用單一定位造成的車輛位置誤差；組合導(dǎo)航功能既可以用在有作物的農(nóng)田作業(yè)中也可以用在無作物的農(nóng)田中，組合導(dǎo)航定位技術(shù)在可靠性、準(zhǔn)確性等方面都較單獨導(dǎo)航情況要好。(2)采用雙天線的北斗信號接收裝置，并且采用位姿檢測模塊的數(shù)據(jù)對北斗定位信息進(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ī)參考車速V_U(s)由上位機(jī)發(fā)送的車速V_R(s)和車速采集V_M(s)決定，按照如下公式確定：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

電壓檢測模塊由運算放大器AD741和外圍電路組成。

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

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

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

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

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

報警及緊急處理模塊由喇叭、發(fā)光燈、光耦和繼電器等組成。報警和緊急處理模塊能夠在液壓閥運行至最大或當(dāng)曲線路徑彎度過大時發(fā)出報警信息并啟動相應(yīng)的緊急響應(yī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ī)工作田間模式：無農(nóng)作物田地，有農(nóng)作物田地；

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

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

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

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

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

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

(1)路徑跟蹤控制系統(tǒng)采用串級控制結(jié)構(gòu)進(jìn)行設(shè)計，其中主回路采取前饋加反饋控制方法，副回路采用單位負(fù)反饋方法，整個系統(tǒng)結(jié)構(gòu)由偏移量設(shè)定值O_S(s)、當(dāng)前航線偏移量O_R(s)、偏移量控制器C_O(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_O(s)，其設(shè)定值輸入為偏移量設(shè)定值O_S(s)和當(dāng)前航線偏移量O_R(s)的差值，偏移量控制器C_O(s)的輸出為轉(zhuǎn)向角度θ_FB，通過如下公式計算：

其中ΔO＝O_S(s)-O_R(s)，K_FB1為偏移量控制器比例系數(shù)，K_FB2為偏移量控制器微分系數(shù)，K_FB3為偏移量控制器二階微分系數(shù)。因拖拉機(jī)路徑跟蹤的直接控制量是航線偏移量，本發(fā)明通過對偏移量差值實施比例、微分和二階微分的控制，可減少控制過程中的超調(diào)，克服振蕩現(xiàn)象發(fā)生；通過設(shè)定可允許最大航線偏移量O_M，可使拖拉機(jī)在航線偏差超過可允許范圍時不再繼續(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，通過如下公式計算：

其中K_FF1為航向角控制器路徑曲度控制比例系數(shù)，K_FF2為航向角控制器比例系數(shù)，K_FF3為航向角控制器微分比例系數(shù)。在航線偏移量允許值O_P范圍內(nèi)，通過將航向角作為參考量，可實施預(yù)先控制，減少路徑跟蹤過程的波動；將預(yù)估路徑曲度作為參考量，可實現(xiàn)曲線行駛或拐彎時的預(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è)定值θ，通過如下公式計算：

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

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

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

附圖說明

圖1為本發(fā)明的原理框圖。

圖2為本發(fā)明所采用的控制方法結(jié)構(gòu)示意圖。

圖3為本發(fā)明的換向閥控制模塊的電路原理圖。

圖4為本發(fā)明的電壓檢測模塊的電路原理圖。

圖5為本發(fā)明的車速檢測模塊的電路原理圖。

圖6為本發(fā)明的轉(zhuǎn)角檢測模塊的電路原理圖。

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

具體實施方式

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

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

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

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

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

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

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

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

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

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

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

結(jié)合圖3可知，換向閥控制模塊11分別與電壓檢測模塊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)的電壓用于控制換向電磁閥，實現(xiàn)對液壓轉(zhuǎn)向系統(tǒng)12的控制。本發(fā)明選用4WRA6E20-10B/24Z4型號的比例方向閥。

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

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

結(jié)合圖6可知，車輪轉(zhuǎn)角檢測模塊17與下位機(jī)7電連接，該模塊用于完成對車輪轉(zhuǎn)角的實時檢測，選用HPS-M1型線位移傳感器將車輪轉(zhuǎn)角信號轉(zhuǎn)換成電壓信號后經(jīng)過電阻R13和電容C11濾波后進(jìn)入放大器U12的反向輸入端，放大器U12的正向輸入端經(jīng)電阻R15接地，U12的輸出端經(jīng)電阻R14后接入其反向輸入端，同時U12的輸出端經(jīng)電阻R16后接入放大器U13的反向輸入端，放大器U13的同向輸入端經(jīng)電阻R18后接地，U13的輸出端經(jīng)電阻R17后接入其反向輸入端，同時與下位機(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è)計了電源監(jiān)控保護(hù)電路。

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

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

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

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

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

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

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

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

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

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

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

(1)路徑跟蹤控制系統(tǒng)采用串級控制結(jié)構(gòu)進(jìn)行設(shè)計，其中主回路采取前饋加反饋控制方法，副回路采用單位負(fù)反饋方法，整個系統(tǒng)結(jié)構(gòu)由偏移量設(shè)定值O_S(s)、當(dāng)前航線偏移量O_R(s)、偏移量控制器C_O(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_O(s)，其設(shè)定值輸入為偏移量設(shè)定值O_S(s)和當(dāng)前航線偏移量O_R(s)的差值，偏移量控制器C_O(s)的輸出為轉(zhuǎn)向角度θ_FB，通過如下公式計算：

其中ΔO＝O_S(s)-O_R(s)，K_FB1為偏移量控制器比例系數(shù)，K_FB2為偏移量控制器微分系數(shù)，K_FB3為偏移量控制器二階微分系數(shù)。因拖拉機(jī)13路徑跟蹤的直接控制量是航線偏移量，本發(fā)明通過對偏移量差值實施比例、微分和二階微分的控制，可減少控制過程中的超調(diào)，克服振蕩現(xiàn)象發(fā)生；通過設(shè)定可允許最大航線偏移量O_M，可使拖拉機(jī)13在航線偏差超過可允許范圍時不再繼續(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，通過如下公式計算：

其中K_FF1為航向角控制器路徑曲度控制比例系數(shù)，K_FF2為航向角控制器比例系數(shù)，K_FF3為航向角控制器微分比例系數(shù)。在航線偏移量允許值O_P范圍內(nèi)，通過將航向角作為參考量，可實施預(yù)先控制，減少路徑跟蹤過程的波動；將預(yù)估路徑曲度作為參考量，可實現(xiàn)曲線行駛或拐彎時的預(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è)定值θ，通過如下公式計算：

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

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

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

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