智能農(nóng)機GNSS/INS組合導(dǎo)航系統(tǒng)設(shè)計

鐘 銀，薛夢琦，袁洪良

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

0 引言

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

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

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

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

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

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

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é)合電臺接收來自于差分基準(zhǔn)站的差分改正數(shù)據(jù)，計算出精準(zhǔn)的定位信息，該組合導(dǎo)航系統(tǒng)架構(gòu)如圖2所示。

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

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

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

1.3.1 INS解算

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

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

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

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

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

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

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

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

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

2）Kalman濾波方程：

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

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

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

2 田間試驗

2.1 東風(fēng)DF1004-2試驗平臺

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

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

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

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

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

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

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

本試驗在農(nóng)機靜止?fàn)顟B(tài)下測試定位導(dǎo)航系統(tǒng)的性能，并對單獨GNSS定位和GNSS/INS組合定位的效果進行比較。試驗于2020年10月在江蘇省張家港市大新鎮(zhèn)長豐村的試驗田進行。試驗中，讓農(nóng)機靜止，系統(tǒng)同時輸出單獨GNSS導(dǎo)航數(shù)據(jù)和GNSS/INS導(dǎo)航數(shù)據(jù)，數(shù)據(jù)輸出頻率為1 Hz。把位置和航向角數(shù)據(jù)與對應(yīng)均值的偏差作為定位誤差用于衡量定位定向精度。單獨GNSS定位系統(tǒng)和GNSS/INS定位系統(tǒng)農(nóng)機航向角和位置數(shù)據(jù)如圖7所示。以RTK基準(zhǔn)站為坐標(biāo)原點，地理位置東向為x軸，地理位置北向為y軸。單獨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組合定位和單獨GNSS定位數(shù)據(jù)的均值接近，但是組合導(dǎo)航的方差較小，意味著組合導(dǎo)航的定位數(shù)據(jù)較為集中，系統(tǒng)更加穩(wěn)定。在定向方面，兩者沒有明顯差別，雙天線配置的GNSS系統(tǒng)自身能夠在靜止條件下提供較高的定向精度。

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

試驗在農(nóng)機直線行駛狀態(tài)下測試定位導(dǎo)航系統(tǒng)的性能，并對單獨GNSS定位和GNSS/INS組合定位的效果進行比較。試驗中，固定農(nóng)機的方向盤，使農(nóng)機沿著預(yù)設(shè)直線以2 m/s的速度行駛，以1 Hz的頻率同時輸出單獨GNSS定位數(shù)據(jù)和GNSS/INS組合定位數(shù)據(jù)。單獨GNSS定位系統(tǒng)和GNSS/INS定位系統(tǒng)農(nóng)機航向角和路徑數(shù)據(jù)如圖8所示。以RTK基準(zhǔn)站為坐標(biāo)原點，地理位置東向為x軸，地理位置北向為y軸。單獨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。

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

3 結(jié) 論

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

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

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

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