基于GNSS的農(nóng)機(jī)自動(dòng)導(dǎo)航控制系統(tǒng)設(shè)計(jì)

陳 康 周志峰 王永泉 陳 寧

(1.上海工程技術(shù)大學(xué)機(jī)械工程學(xué)院，上海 201600；2.上海司南衛(wèi)星導(dǎo)航技術(shù)有限公司，上海 201100)

在國外,農(nóng)機(jī)自動(dòng)導(dǎo)航控制系統(tǒng)研究較早，如Qiu H和Zhang Q設(shè)計(jì)了由液壓泵、比例換向閥及轉(zhuǎn)向控制器等組成的拖拉機(jī)電液操控系統(tǒng)[1]；美國天寶公司的Autopilot用GPS和GLONASS雙衛(wèi)星定位，控制農(nóng)機(jī)的轉(zhuǎn)向液壓系統(tǒng)，驅(qū)使農(nóng)機(jī)自動(dòng)行駛；日本拓普康公司的System350使用GPS、GLONASS和Galileo多星一體化接收機(jī)并帶慣導(dǎo)傳感器。在國內(nèi),謝斌等基于ISO11783提出了一種CAN總線和GPS的分布式控制系統(tǒng)[2]；何卿等采用帶非線性補(bǔ)償?shù)腜ID控制器設(shè)計(jì)了雙層控制器接觸式導(dǎo)航控制系統(tǒng)[3]；白曉鴿等設(shè)計(jì)了一種基于神經(jīng)網(wǎng)絡(luò)的拖拉機(jī)自動(dòng)導(dǎo)航系統(tǒng)[4]；紀(jì)朝鳳等開發(fā)了基于CAN總線的農(nóng)業(yè)車輛自動(dòng)導(dǎo)航控制系統(tǒng)[5]；胡煉等設(shè)計(jì)了一種基于CAN總線的分布式控制系統(tǒng)[6]；張智剛等在久保田插秧機(jī)上開發(fā)了基于DGPS和電子羅盤的導(dǎo)航控制系統(tǒng)[7]。由于目前國內(nèi)外所有的自動(dòng)導(dǎo)航控制系統(tǒng)均不支持北斗導(dǎo)航衛(wèi)星系統(tǒng)，因此筆者設(shè)計(jì)了一款支持北斗導(dǎo)航衛(wèi)星的農(nóng)機(jī)自動(dòng)導(dǎo)航控制系統(tǒng)。

1 控制系統(tǒng)硬件設(shè)計(jì)①

筆者設(shè)計(jì)的農(nóng)機(jī)自動(dòng)導(dǎo)航控制系統(tǒng)結(jié)構(gòu)如圖1所示。全球?qū)Ш叫l(wèi)星系統(tǒng)(GNSS)基準(zhǔn)站調(diào)制并發(fā)射無線電信號(hào)，安裝在農(nóng)機(jī)頂部的GNSS接收機(jī)接收GPS、BDS和GLONASS衛(wèi)星信號(hào)，駕駛室壁上的電臺(tái)解調(diào)無線電信號(hào)，工控機(jī)處理接收機(jī)和電臺(tái)接收的信號(hào)并發(fā)送轉(zhuǎn)向控制信號(hào)給轉(zhuǎn)向執(zhí)行機(jī)構(gòu)，該工控機(jī)固定于座位底部，使用觸摸屏設(shè)置農(nóng)機(jī)參數(shù)和行進(jìn)路線，在農(nóng)機(jī)原油路系統(tǒng)的基礎(chǔ)上改造并增加電控比例液壓閥，在左前輪軸上安裝角度傳感器，實(shí)時(shí)采集前輪動(dòng)向回饋給工控機(jī)。

圖1 農(nóng)機(jī)自動(dòng)導(dǎo)航控制系統(tǒng)結(jié)構(gòu)框圖

1.1 基準(zhǔn)站和接收機(jī)

三星定位，即接收機(jī)能接收到GPS、BDS和GLONASS的衛(wèi)星信號(hào)，保證在每一接收點(diǎn)至少可接收到9～12顆衛(wèi)星的信號(hào),通過復(fù)雜的運(yùn)算得到高精度的位置坐標(biāo)。基準(zhǔn)站指用作固定參考站的儀器，作業(yè)時(shí)基準(zhǔn)站將已知的精密坐標(biāo)和接收到的衛(wèi)星信息直接或經(jīng)過處理后實(shí)時(shí)發(fā)送給流動(dòng)站接收機(jī)，即農(nóng)機(jī)頂部的接收機(jī)，流動(dòng)站接收機(jī)在進(jìn)行GNSS觀測(cè)的同時(shí)，也接收到基準(zhǔn)站的信息，通過對(duì)結(jié)果進(jìn)行改正來提高定位精度。

接收機(jī)的OEM主板采用上海司南衛(wèi)星導(dǎo)航技術(shù)有限公司的支持BDS(北斗導(dǎo)航衛(wèi)星)、GPS和GLONASS的三星八頻低功耗K508 GNSS OEM主板，該主板采用北斗三頻、GPS三頻及GLONASS雙頻進(jìn)行聯(lián)合定位，具有超遠(yuǎn)距離、超快速的高動(dòng)態(tài)RTK解算引擎，且可獲得分米級(jí)定位精度，作業(yè)距離可達(dá)300km，可直接輸出PJK平面坐標(biāo)、高精簡報(bào)文。

1.2 工控機(jī)

設(shè)計(jì)中采用速率達(dá)720MHz的AM3359處理器、運(yùn)行全面的Linux Angstrom操作系統(tǒng)和接口豐富的Beagle Bone作為工控機(jī)，保證了穩(wěn)定性和處理大量數(shù)據(jù)的速度。工控機(jī)通過RS232串口接收GNSS接收機(jī)和電臺(tái)的實(shí)時(shí)信息，通過CAN總線和下位機(jī)保持通信，通過RS232串口和LCD觸摸屏連接。工控機(jī)根據(jù)衛(wèi)星定位的坐標(biāo)和車輪的轉(zhuǎn)動(dòng)情況，用雙通道PD算法通過判斷偏差信號(hào)的正負(fù)得出轉(zhuǎn)動(dòng)方向，實(shí)時(shí)向液壓控制閥發(fā)送指令，通過控制液壓系統(tǒng)油量的流量和流向，控制農(nóng)機(jī)的行駛，確保農(nóng)機(jī)自動(dòng)導(dǎo)航。工控機(jī)組成框圖如圖2所示。

圖2 工控機(jī)組成框圖

1.3 轉(zhuǎn)向控制

工控機(jī)通過處理GNSS接收機(jī)、無線電臺(tái)和前輪轉(zhuǎn)向角傳感器實(shí)時(shí)信息實(shí)現(xiàn)智能行為決策,產(chǎn)生轉(zhuǎn)向信息指令，轉(zhuǎn)向控制ECU驅(qū)動(dòng)電控比例液壓閥動(dòng)作，實(shí)現(xiàn)轉(zhuǎn)向輪偏轉(zhuǎn),進(jìn)而實(shí)現(xiàn)轉(zhuǎn)向動(dòng)作。設(shè)計(jì)采用模糊自適應(yīng)PID控制方法[8]，首先計(jì)算實(shí)際航向與目標(biāo)航向的偏差,然后減去實(shí)際的前輪轉(zhuǎn)角,將其作為轉(zhuǎn)向控制ECU的輸入,轉(zhuǎn)向控制ECU輸出車輪的目標(biāo)轉(zhuǎn)角。該目標(biāo)轉(zhuǎn)角和實(shí)際轉(zhuǎn)角作為電控比例液壓閥的輸入,電控比例液壓閥控制油缸執(zhí)行動(dòng)作,消除轉(zhuǎn)角偏差,從而達(dá)到自動(dòng)導(dǎo)航的目的(圖3)。

圖3 轉(zhuǎn)向控制框圖

1.4 切換方向盤

農(nóng)機(jī)遇到突發(fā)情況時(shí)，需迅速切換到人工駕駛，為此設(shè)計(jì)一個(gè)方向盤，方向盤套(圖4)內(nèi)的壓力傳感器帶有長形皮管，人用在壓力管上的壓力最大為30N，管的橫截面積為300mm2，則最大壓力量程為100kPa。筆者選擇飛思卡爾傳感器(型號(hào)MPX5100AP，精度±2.5%，量程100kPa)。當(dāng)人手握住方向盤時(shí)，壓力傳感器的壓力管受到擠壓，壓力變化,傳感器輸出信號(hào)給工控機(jī)，從而實(shí)現(xiàn)無人和人工駕駛的迅速切換。

圖4 方向盤套子示意圖

2 導(dǎo)航算法

本設(shè)計(jì)采用帶定位誤差校正的粒子濾波(PF)方法進(jìn)行數(shù)據(jù)融合[9]，可削弱GNSS跳變引起的誤差，且可獲得準(zhǔn)確的導(dǎo)航定位信息。粒子退化問題在粒子濾波過程中不可避免，因此引入無跡卡爾曼濾波方法(UKF)，采用不同重采樣方法，且對(duì)重要密度函數(shù)進(jìn)行改進(jìn)，從而有效抑制了粒子退化，又增加(馬爾科夫鏈蒙特卡羅)MCMC步驟，減少了樣本枯竭現(xiàn)象。

2.1 定位誤差校正

農(nóng)機(jī)在田間作業(yè)時(shí),因地面凹凸不平,不可避免地產(chǎn)生俯仰角和橫滾角，GNSS天線端與車體中心不重合，使測(cè)得的GNSS數(shù)據(jù)存在誤差、定位精度下降，影響了初始狀態(tài)，導(dǎo)致濾波發(fā)散，因此必須加以校正[10]。

首先計(jì)算車體在橫軸和縱軸方向產(chǎn)生的定位誤差：

(1)

(2)

式中H——GNSS天線的距地高度；

θ——俯仰角度，車體縱向中軸線與地平面的夾角；

?——橫滾角度，車體橫向軸線與地平面的夾角。

(3)

(4)

式中φ——航向角度，車體縱向前進(jìn)方向與高斯平面坐標(biāo)系橫軸之間的夾角。

使用該定位誤差即可對(duì)GPS原始定位數(shù)據(jù)進(jìn)行傾斜校正。

2.2 PF-UKF-MCMC方法

(5)

狀態(tài)更新的表達(dá)式為：

(9)

(10)

式中pk——協(xié)方差；

3 試驗(yàn)測(cè)試

為檢測(cè)所設(shè)計(jì)的農(nóng)機(jī)自動(dòng)導(dǎo)航控制系統(tǒng)的精度，在江蘇省鹽城市大豐縣四岔河農(nóng)場(chǎng)，將設(shè)計(jì)的系統(tǒng)安裝到New Holland T1554農(nóng)機(jī)上，然后預(yù)定好行走的直線和曲線路線，設(shè)置農(nóng)機(jī)速度為1m/s進(jìn)行試驗(yàn)。GNSS接收機(jī)記錄農(nóng)機(jī)行走實(shí)時(shí)位置的經(jīng)度、緯度數(shù)據(jù)后，對(duì)數(shù)據(jù)進(jìn)行高斯坐標(biāo)轉(zhuǎn)換處理，繪制實(shí)際跟蹤軌跡。進(jìn)行多次試驗(yàn)，對(duì)導(dǎo)航系統(tǒng)進(jìn)行評(píng)價(jià)和分析。

直線行走時(shí)跟蹤結(jié)果如圖5所示，最大橫向偏差16cm，平均橫向偏差小于8cm。

圖5 一次直線行走的跟蹤結(jié)果

曲線行走時(shí)跟蹤結(jié)果如圖6所示，最大橫向偏差31cm，平均橫向偏差小于14cm。

圖6 一次曲線行走的跟蹤結(jié)果

試驗(yàn)結(jié)果表明，安裝該自動(dòng)導(dǎo)航控制系統(tǒng)的農(nóng)機(jī)能夠滿足農(nóng)田作業(yè)要求。

4 結(jié)論

4.1使用上海司南衛(wèi)星導(dǎo)航技術(shù)有限公司的三星八頻GNSS接收機(jī)，可以接收GPS、GLONASS和北斗衛(wèi)星，優(yōu)化以前接收機(jī)收星少、精準(zhǔn)度不高及易受障礙物干擾等缺點(diǎn)。

4.2采用帶傳感器的特制方向盤，有效解決了以前自動(dòng)導(dǎo)航系統(tǒng)在遇到突發(fā)情況時(shí)，切回人工駕駛模式有延遲的問題。

4.3采用帶定位誤差校正的粒子無跡卡爾曼濾波方法，削弱了因田地復(fù)雜環(huán)境引起的GNSS跳變誤差，使導(dǎo)航定位更加準(zhǔn)確。

4.4試驗(yàn)表明，農(nóng)機(jī)行進(jìn)速度1m/s時(shí)，該自動(dòng)導(dǎo)航系統(tǒng)跟蹤最大誤差小于16cm，且具有良好的控制精度和穩(wěn)定性。