鐘 銀，薛夢(mèng)琦，袁洪良

（同濟(jì)大學(xué)電子與信息工程學(xué)院，上海 201804）

0 引言

農(nóng)業(yè)是國(guó)民經(jīng)濟(jì)的基礎(chǔ)[1]，也是經(jīng)濟(jì)發(fā)展、社會(huì)安定、國(guó)家自立的基礎(chǔ)。在全面建成小康社會(huì)的決勝階段，國(guó)家明確提出要加大農(nóng)業(yè)科技投入，發(fā)展現(xiàn)代化農(nóng)業(yè)，建設(shè)以農(nóng)業(yè)物聯(lián)網(wǎng)和農(nóng)業(yè)智能化裝備為重點(diǎn)的農(nóng)業(yè)全程信息化和機(jī)械化技術(shù)體系[2]。目前農(nóng)業(yè)拖拉機(jī)的田間作業(yè)完全依靠駕駛員的經(jīng)驗(yàn)，在一些作業(yè)面積大的地區(qū)，工作時(shí)間長(zhǎng)，農(nóng)機(jī)操作人員任務(wù)繁重，農(nóng)機(jī)作業(yè)質(zhì)量難以保證，所以研究農(nóng)機(jī)自動(dòng)導(dǎo)航系統(tǒng)具有重要意義[3]。農(nóng)機(jī)自動(dòng)導(dǎo)航有助于降低成本并提高盈利能力[4]，是支撐現(xiàn)代化農(nóng)業(yè)發(fā)展的關(guān)鍵技術(shù)之一[5-6]，是農(nóng)業(yè)機(jī)械智能化的一個(gè)重要組成部分，對(duì)于增強(qiáng)農(nóng)業(yè)生產(chǎn)力有重大作用。

國(guó)內(nèi)外對(duì)無人農(nóng)機(jī)定位導(dǎo)航使用的系統(tǒng)包括全球?qū)Ш叫l(wèi)星系統(tǒng)（Global Navigation Satellite System, GNSS）、慣性導(dǎo)航系統(tǒng)（Inertial Navigation System, INS）、激光雷達(dá)以及機(jī)器視覺導(dǎo)航系統(tǒng)等[7-10]，目前GNSS和Real-time kinematic（RTK）載波相位差分技術(shù)結(jié)合的組合導(dǎo)航在農(nóng)機(jī)上應(yīng)用廣泛[7]，但是存在GNSS容易受環(huán)境干擾信號(hào)丟失的問題，INS利用相對(duì)定位的特點(diǎn)，不受環(huán)境干擾，得到廣泛應(yīng)用，但是其系統(tǒng)誤差隨時(shí)間不斷累積[11]。國(guó)外發(fā)達(dá)國(guó)家由于地廣人稀，勞動(dòng)力成本高以及科技進(jìn)步等特點(diǎn)，對(duì)無人農(nóng)機(jī)自動(dòng)駕駛技術(shù)的研究開展較早[12]，尤其以美國(guó)對(duì)農(nóng)機(jī)自動(dòng)導(dǎo)航技術(shù)研究更為深入，應(yīng)用更為廣泛。Noguchi等[13]開發(fā)了基于機(jī)器視覺、差分GPS等定位導(dǎo)航單元，利用擴(kuò)展卡爾曼濾波和多維密度函數(shù)算法將傳感器信息進(jìn)行了融合，設(shè)計(jì)了農(nóng)機(jī)的自動(dòng)導(dǎo)航定位系統(tǒng)，在比較平坦的路面，最大偏差小于10 cm。日本也是研究農(nóng)機(jī)自動(dòng)導(dǎo)航技術(shù)比較深入的國(guó)家之一。Keiich等[14]以旱田四驅(qū)拖拉機(jī)為平臺(tái)，對(duì)農(nóng)機(jī)油門、離合器等進(jìn)行控制，采用差分GPS和光纖陀螺組合的方式實(shí)現(xiàn)導(dǎo)航，并基于農(nóng)機(jī)運(yùn)動(dòng)學(xué)建模，通過卡爾曼濾波估計(jì)農(nóng)機(jī)的側(cè)滑，在車速1.5 m/s的情況下，跟蹤誤差在0.1 m以內(nèi)。Nagasaka等[15]使用Trimble公司2 cm精度的GPS與日本航空電子生產(chǎn)的IMUJCS-7401A，對(duì)改造后的水稻收割機(jī)進(jìn)行自動(dòng)導(dǎo)航控制，實(shí)現(xiàn)了插秧精度平均橫向偏差小于3 cm，最大橫向偏差不大于4 cm。歐盟在自動(dòng)導(dǎo)航農(nóng)機(jī)方面研究也相對(duì)更早，Zuydam[16]將農(nóng)田電子地圖和RTK-GPS相結(jié)合的方法，完成了農(nóng)機(jī)自動(dòng)導(dǎo)航，試驗(yàn)結(jié)果表明該系統(tǒng)在農(nóng)田間的橫向跟蹤誤差最大達(dá)到12 cm，在水泥地上的橫向跟蹤誤差最大為2 cm。中國(guó)無人農(nóng)機(jī)自動(dòng)導(dǎo)航技術(shù)發(fā)展相對(duì)比較晚，但是國(guó)內(nèi)各大高校和企業(yè)也逐漸在農(nóng)機(jī)自動(dòng)駕駛導(dǎo)航技術(shù)上投入大量的精力。李云伍等[17]以鐵牛654拖拉機(jī)和雷沃TG1254拖拉機(jī)為試驗(yàn)平臺(tái)，通過GPS、陀螺儀、電子羅盤等傳感器并利用卡爾曼濾波將信息融合，得到精準(zhǔn)的導(dǎo)航數(shù)據(jù)。景云鵬等[18]通過X-804搭建開發(fā)平臺(tái)設(shè)計(jì)了基于實(shí)時(shí)動(dòng)態(tài)定位技術(shù)結(jié)合差分GPS的自動(dòng)導(dǎo)航系統(tǒng)，但是存在GPS信號(hào)遮擋，出現(xiàn)信號(hào)不連續(xù)的問題。

隨著北斗導(dǎo)航系統(tǒng)不斷完善，在船舶運(yùn)輸、公路交通、鐵路運(yùn)輸、環(huán)境監(jiān)測(cè)、精準(zhǔn)農(nóng)業(yè)等領(lǐng)域廣泛應(yīng)用[19]。在北斗系統(tǒng)背景下，本文設(shè)計(jì)了基于GNSS/INS的智能農(nóng)機(jī)組合導(dǎo)航系統(tǒng)，以解決GNSS導(dǎo)航信號(hào)容易受到干擾、INS導(dǎo)航誤差累積的問題，并結(jié)合兩者的優(yōu)勢(shì)，提高導(dǎo)航系統(tǒng)長(zhǎng)時(shí)間運(yùn)行的可靠性和精度。

1 系統(tǒng)設(shè)計(jì)

1.1 導(dǎo)航控制原理

本文的研究?jī)?nèi)容是設(shè)計(jì)基于GNSS/INS的智能農(nóng)機(jī)組合導(dǎo)航系統(tǒng)，在農(nóng)機(jī)行駛過程中，計(jì)算出農(nóng)機(jī)精確的位置、速度和姿態(tài)信息。圖1為導(dǎo)航控制原理圖，采用內(nèi)外雙閉環(huán)反饋控制結(jié)構(gòu)，外環(huán)是農(nóng)機(jī)位置偏差控制，內(nèi)環(huán)是農(nóng)機(jī)航向角控制（也即是偏差速率）的控制。農(nóng)機(jī)通過GNSS高精度定位接收機(jī)、INS航位推算給出車輛的實(shí)時(shí)位置信息，與期望軌跡相比較得到偏差值，由控制算法得到期望的轉(zhuǎn)角，結(jié)合轉(zhuǎn)角傳感器調(diào)整農(nóng)機(jī)姿態(tài)完成自動(dòng)轉(zhuǎn)向，從而達(dá)到追蹤期望軌跡目的。

1.2 GNSS/INS組合導(dǎo)航系統(tǒng)架構(gòu)

GNSS/INS組合導(dǎo)航系統(tǒng)采用雙天線衛(wèi)星系統(tǒng)和慣性導(dǎo)航系統(tǒng)的組合定位方式，結(jié)合電臺(tái)接收來自于差分基準(zhǔn)站的差分改正數(shù)據(jù)，計(jì)算出精準(zhǔn)的定位信息，該組合導(dǎo)航系統(tǒng)架構(gòu)如圖2所示。

其中車載流動(dòng)端由高精度定位接收機(jī)、GNSS雙天線、無線數(shù)傳電臺(tái)和接收天線、慣性導(dǎo)航系統(tǒng)、組合導(dǎo)航計(jì)算處理器等組成；差分基準(zhǔn)站用于為車載流動(dòng)端提供高精度差分改正數(shù)據(jù)。差分基準(zhǔn)站由衛(wèi)星測(cè)量天線、高精度定位接收機(jī)、差分基準(zhǔn)站處理器、無線數(shù)傳電臺(tái)和發(fā)射天線組成。為了實(shí)現(xiàn)該系統(tǒng)的功能，研制了差分基準(zhǔn)站和車載終端。

1.3 基于Kalman濾波的GNSS/INS組合導(dǎo)航算法

GNSS/INS組合導(dǎo)航可以實(shí)現(xiàn)GNSS絕對(duì)定位和INS相對(duì)定位的優(yōu)勢(shì)互補(bǔ)。INS具有短期精度高、輸出頻率高、受外界干擾小、實(shí)時(shí)性強(qiáng)的優(yōu)點(diǎn)，但器件誤差將會(huì)隨著時(shí)間累積[20-21]；GNSS長(zhǎng)期精度高，但是容易受到外界干擾、數(shù)據(jù)輸出頻率低[22-25]；GNSS/INS組合能夠長(zhǎng)時(shí)間提供精度高、實(shí)時(shí)性高的位置和姿態(tài)信息[24-26]。根據(jù)本文選擇的獨(dú)立測(cè)量器件，松耦合模式組合導(dǎo)航不僅能夠提供精確的位置、速度和姿態(tài)角信息，而且具有較好的魯棒性[26-30]。

1.3.1 INS解算

本文INS的解算在東-北-天導(dǎo)航坐標(biāo)系下進(jìn)行，用n系表示，農(nóng)機(jī)坐標(biāo)系用b系表示，地心地固坐標(biāo)系用e系表示，地心慣性坐標(biāo)系用i系表示，具體解算流程如圖3所示，導(dǎo)航微分方程如公式（1）。

比力方程和姿態(tài)矩陣計(jì)算方程為

通過以上的INS解算，得到農(nóng)機(jī)在東-北-天坐標(biāo)系下的位置、速度和姿態(tài)信息。

1.3.2 GNSS/INS組合導(dǎo)航

由于INS解算存在累積誤差，所以本文采用GNSS信息進(jìn)行組合導(dǎo)航以提高系統(tǒng)長(zhǎng)期運(yùn)行的精度。主要使用Kalman濾波算法估計(jì)INS的誤差狀態(tài)，反饋校正INS，算法流程如圖4所示。

1）離散型卡爾曼濾波系統(tǒng)的一般模型如下：

式中Xk、Xk-1表示系統(tǒng)k時(shí)刻和k-1時(shí)刻的狀態(tài)；A表示系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣；wk-1表示k-1時(shí)刻狀態(tài)轉(zhuǎn)移過程中的高斯噪聲；Yk表示系統(tǒng)k時(shí)刻的測(cè)量值；H表示測(cè)量系統(tǒng)的系統(tǒng)參數(shù)；vk表示k時(shí)刻測(cè)量系統(tǒng)的高斯白噪聲。

本系統(tǒng)的15維狀態(tài)向量X如公式（5）所示：

式中ΔL表示緯度誤差，(°)；Δλ表示經(jīng)度誤差，(°)；Δh示高度位置誤差，m；δvE、δvN、δvU表示農(nóng)機(jī)3軸速度誤差，m/s；φE、φN、φU表示農(nóng)機(jī)3軸姿態(tài)角誤差，rad；εx、εy、εz表示農(nóng)機(jī)3軸陀螺儀常值誤差，rad/s；表示農(nóng)機(jī)3軸加速度計(jì)常值誤差，m/s2。

2）Kalman濾波方程：

1.4 系統(tǒng)軟件設(shè)計(jì)

差分基準(zhǔn)站初始化UB4B0M、華信電臺(tái)、處理器等模塊，來自于高精度定位板卡UB4B0M的差分?jǐn)?shù)據(jù)通過華信電臺(tái)（發(fā)送端）和車載終端的華信電臺(tái)（接收端）透?jìng)髦淋囕d終端的高精度板卡UB482，UB482根據(jù)實(shí)時(shí)差分?jǐn)?shù)據(jù)解算出厘米級(jí)的定位數(shù)據(jù)。

車載終端上電初始化UB482、華信電臺(tái)、組合導(dǎo)航處理器STM32F7VIT6、慣性測(cè)量單元ADIS16460等模塊：1）處理器初始化系統(tǒng)時(shí)鐘計(jì)時(shí)器，配置中斷，初始化串口、SPI等外設(shè)；2）配置華信電臺(tái)、配置UB482和ADIS16460；3）配置組合導(dǎo)航系統(tǒng)參數(shù)，包括IMU的初始零偏和安裝角誤差、GNSS天線和IMU之間的桿臂、系統(tǒng)Q矩陣等；4）系統(tǒng)進(jìn)入輪詢，農(nóng)機(jī)在大于0.5 m/s的速度下行駛進(jìn)行動(dòng)態(tài)對(duì)準(zhǔn)；5）等待IMU數(shù)據(jù)更新、等待GNSS數(shù)據(jù)更新；6）數(shù)據(jù)更新完成，進(jìn)行組合導(dǎo)航解算得出農(nóng)機(jī)實(shí)時(shí)的姿態(tài)角、速度、位置信息；7）輪詢等待下一次數(shù)據(jù)更新。圖5為基準(zhǔn)站和車載終端程序流程圖。

2 田間試驗(yàn)

2.1 東風(fēng)DF1004-2試驗(yàn)平臺(tái)

為檢驗(yàn)本系統(tǒng)在實(shí)際農(nóng)機(jī)、農(nóng)田中的工作狀態(tài)，包括系統(tǒng)的穩(wěn)定性和精準(zhǔn)性，本文以東風(fēng)DF1004-2為載體，搭建測(cè)試試驗(yàn)平臺(tái)，差分基準(zhǔn)站放置在沒有遮擋物的空曠區(qū)域；GNSS雙天線橫置在車頂上，基線長(zhǎng)度為1.6 m，數(shù)傳電臺(tái)天線固定安裝在農(nóng)機(jī)后端；包括INS和高精度定位板卡在內(nèi)的車載終端以及平板計(jì)算機(jī)安裝在農(nóng)機(jī)駕駛艙，圖6為試驗(yàn)平臺(tái)示組成和試驗(yàn)現(xiàn)場(chǎng)圖。

INS使用慣性導(dǎo)航單元ADIS16460和STM32F767的組合。ADIS16460是一款緊湊型精密6軸MEMS慣性傳感器，內(nèi)置1個(gè)三軸MEMS陀螺儀和1個(gè)三軸MEMS加速度計(jì)。該傳感器體積小，啟動(dòng)時(shí)間快，方便集成到硬件電路，工作溫度范圍?25～+85℃，能夠適應(yīng)比較惡劣的環(huán)境。ADIS16460的測(cè)量范圍最小為±100°/s，運(yùn)動(dòng)中偏置穩(wěn)定度為8°/h，x軸角向隨機(jī)游動(dòng)為0.12°/(30 min)，三軸數(shù)字加速度計(jì)動(dòng)態(tài)范圍為±5 g，抗沖擊能力為2 000 g。同時(shí)該傳感器支持串行外設(shè)接口（SPI）數(shù)據(jù)通信，SPI和寄存器結(jié)構(gòu)針對(duì)數(shù)據(jù)收集和配置控制提供了簡(jiǎn)單的接口，便于電路設(shè)計(jì)。STM32F767具有足夠多的通用I/O口，支持串口、SPI、CAN等通信外設(shè)，處理器的工作頻率可達(dá)到216 MHz，可以滿足自動(dòng)導(dǎo)航中對(duì)計(jì)算的需求。

高精度定位板卡選擇的是和芯星通科技有限公司的產(chǎn)品，差分基準(zhǔn)站和車載終端中使用的分別是UB4B0M和UB482。UB4B0M采用低功耗設(shè)計(jì)，提供毫米級(jí)載波相位觀測(cè)值和厘米級(jí)RTK定位精度，支持芯片級(jí)多路徑抑制，領(lǐng)先的瞬時(shí)RTK技術(shù)，自適應(yīng)抗干擾，支持BDS、GPS、Galileo、GLONASS的多頻信號(hào)，同時(shí)支持3個(gè)串口，方便和其他模塊進(jìn)行通信；UB482是全系統(tǒng)多頻高精度定向板卡，支持BDS、GPS、Galileo、GLONASS的多頻信號(hào)，支持雙天線信號(hào)輸入，單板卡定位定向，20 Hz以上的數(shù)據(jù)輸出，1 s以內(nèi)的RTK重捕獲時(shí)間；支持熱啟動(dòng)、支持網(wǎng)絡(luò)功能、支持多個(gè)串口、SPI、PPS等多種接口。

無線數(shù)傳電臺(tái)型號(hào)是HX-DU5002D。這是一款內(nèi)置式高功率數(shù)傳電臺(tái)，具有體積小、功耗低等特點(diǎn)；該電臺(tái)具備高低功率切換、空中波特率切換、串口波特率切換、配置參數(shù)更改等功能，便于處理器控制；該電臺(tái)支持工作頻率為410～470 MHz，工作溫度-30～60℃，傳輸距離可達(dá)5 km，滿足農(nóng)機(jī)工作的環(huán)境需求；數(shù)傳電臺(tái)模塊配備有1.27 mm間距的數(shù)據(jù)接口，方便于集成到差分基準(zhǔn)站。

GNSS天線型號(hào)GPS500，該天線是一款涵蓋BDS和GLONASS以及GNSS的三系統(tǒng)外置測(cè)量天線；天線部分采用多饋點(diǎn)設(shè)計(jì)方案，實(shí)現(xiàn)相位中心與幾何中心的重合，將天線對(duì)測(cè)量誤差的影響降低到最??；天線單元增益高，確保低仰角信號(hào)的接收效果，在一些遮擋較嚴(yán)重的場(chǎng)合仍能正常收星；該天線防水、具有防紫外線外罩，能夠?yàn)樘炀€長(zhǎng)期在野外工作提供保障。

本文分別在農(nóng)機(jī)靜止、固定直線行駛狀態(tài)下，比較基于GNSS導(dǎo)航定位和GNSS/INS導(dǎo)航定位2種導(dǎo)航系統(tǒng)之下農(nóng)機(jī)的位置、姿態(tài)角度誤差，驗(yàn)證系統(tǒng)性能是否能夠達(dá)到智能農(nóng)機(jī)作業(yè)的要求。

2.2 靜止?fàn)顟B(tài)試驗(yàn)

本試驗(yàn)在農(nóng)機(jī)靜止?fàn)顟B(tài)下測(cè)試定位導(dǎo)航系統(tǒng)的性能，并對(duì)單獨(dú)GNSS定位和GNSS/INS組合定位的效果進(jìn)行比較。試驗(yàn)于2020年10月在江蘇省張家港市大新鎮(zhèn)長(zhǎng)豐村的試驗(yàn)田進(jìn)行。試驗(yàn)中，讓農(nóng)機(jī)靜止，系統(tǒng)同時(shí)輸出單獨(dú)GNSS導(dǎo)航數(shù)據(jù)和GNSS/INS導(dǎo)航數(shù)據(jù)，數(shù)據(jù)輸出頻率為1 Hz。把位置和航向角數(shù)據(jù)與對(duì)應(yīng)均值的偏差作為定位誤差用于衡量定位定向精度。單獨(dú)GNSS定位系統(tǒng)和GNSS/INS定位系統(tǒng)農(nóng)機(jī)航向角和位置數(shù)據(jù)如圖7所示。以RTK基準(zhǔn)站為坐標(biāo)原點(diǎn)，地理位置東向?yàn)閤軸，地理位置北向?yàn)閥軸。單獨(dú)GNSS定位中，位置誤差在1 cm以內(nèi)，x、y方向上均值分別是6 867.8、2 657.9 cm，方差分別是是0.032、0.043；航向角誤差在0.1°以內(nèi)：平均值是1.59°，方差是0.001 7。GNSS/INS組合定位中，位置誤差在1 cm以內(nèi)，x、y方向上均值分別是6 867.7、2 658.4 cm，方差分別是0.026、0.024；航向角誤差在0.1°以內(nèi)：平均值1.62°，方差是0.002 1。

由此可見，在靜止條件下，GNSS/INS組合定位和單獨(dú)GNSS定位數(shù)據(jù)的均值接近，但是組合導(dǎo)航的方差較小，意味著組合導(dǎo)航的定位數(shù)據(jù)較為集中，系統(tǒng)更加穩(wěn)定。在定向方面，兩者沒有明顯差別，雙天線配置的GNSS系統(tǒng)自身能夠在靜止條件下提供較高的定向精度。

2.3 直線行駛狀態(tài)試驗(yàn)

試驗(yàn)在農(nóng)機(jī)直線行駛狀態(tài)下測(cè)試定位導(dǎo)航系統(tǒng)的性能，并對(duì)單獨(dú)GNSS定位和GNSS/INS組合定位的效果進(jìn)行比較。試驗(yàn)中，固定農(nóng)機(jī)的方向盤，使農(nóng)機(jī)沿著預(yù)設(shè)直線以2 m/s的速度行駛，以1 Hz的頻率同時(shí)輸出單獨(dú)GNSS定位數(shù)據(jù)和GNSS/INS組合定位數(shù)據(jù)。單獨(dú)GNSS定位系統(tǒng)和GNSS/INS定位系統(tǒng)農(nóng)機(jī)航向角和路徑數(shù)據(jù)如圖8所示。以RTK基準(zhǔn)站為坐標(biāo)原點(diǎn)，地理位置東向?yàn)閤軸，地理位置北向?yàn)閥軸。單獨(dú)GNSS定位中，位置誤差在6 cm以內(nèi)，x、y方向上的誤差均值分別是-5.5、1.2 cm，方差值分別是0.063、0.004；航向角誤差在1°以內(nèi)：平均值是-13.90°，方差是0.162。GNSS/INS組合定位中，位置誤差在3 cm以內(nèi)，x、y方向上的誤差均值分別是2.5、1.0 cm，方差值分別是0.059、0.003 8；航向角誤差在0.5°以內(nèi)：平均值是-12.85°，方差是0.104。

可見，在運(yùn)動(dòng)狀態(tài)下，相較于單獨(dú)GNSS導(dǎo)航定位， GNSS/INS組合導(dǎo)航得到的結(jié)果更加精準(zhǔn)。

3 結(jié) 論

1）本文針對(duì)智能農(nóng)機(jī)自動(dòng)導(dǎo)航的問題，以東風(fēng)DF1004-2為試驗(yàn)平臺(tái)設(shè)計(jì)了基于GNSS/INS的智能農(nóng)機(jī)導(dǎo)航控制系統(tǒng)。

2）為實(shí)現(xiàn)智能農(nóng)機(jī)導(dǎo)航控制系統(tǒng)的功能，分別研制了基于UB4B0M高精度定位板卡的差分基準(zhǔn)站、基于UB482高精度定位板卡與ADIS16460慣性測(cè)量單元組合的車載終端。

3）針對(duì)INS的確定性偏差以及隨機(jī)誤差，設(shè)計(jì)了基于Kalman濾波的GNSS/INS組合導(dǎo)航算法，融合GNSS數(shù)據(jù)以提高系統(tǒng)長(zhǎng)期運(yùn)行的精度。

4）試驗(yàn)結(jié)果表明，農(nóng)機(jī)在農(nóng)田直線行駛時(shí)，GNSS/INS組合定位導(dǎo)航得到的航向角偏差在0.5°以內(nèi)，位置誤差在3 cm以內(nèi)，優(yōu)于單獨(dú)GNSS系統(tǒng)產(chǎn)生的定位定向結(jié)果。本論文的系統(tǒng)設(shè)計(jì)可以滿足智能農(nóng)機(jī)生產(chǎn)作業(yè)的需求。