智能農機GNSS/INS組合導航系統設計

鐘 銀，薛夢琦，袁洪良

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

0 引言

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

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

隨著北斗導航系統不斷完善，在船舶運輸、公路交通、鐵路運輸、環境監測、精準農業等領域廣泛應用[19]。在北斗系統背景下，本文設計了基于GNSS/INS的智能農機組合導航系統，以解決GNSS導航信號容易受到干擾、INS導航誤差累積的問題，并結合兩者的優勢，提高導航系統長時間運行的可靠性和精度。

1 系統設計

1.1 導航控制原理

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

1.2 GNSS/INS組合導航系統架構

GNSS/INS組合導航系統采用雙天線衛星系統和慣性導航系統的組合定位方式，結合電臺接收來自于差分基準站的差分改正數據，計算出精準的定位信息，該組合導航系統架構如圖2所示。

其中車載流動端由高精度定位接收機、GNSS雙天線、無線數傳電臺和接收天線、慣性導航系統、組合導航計算處理器等組成；差分基準站用于為車載流動端提供高精度差分改正數據。差分基準站由衛星測量天線、高精度定位接收機、差分基準站處理器、無線數傳電臺和發射天線組成。為了實現該系統的功能，研制了差分基準站和車載終端。

1.3 基于Kalman濾波的GNSS/INS組合導航算法

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

1.3.1 INS解算

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

比力方程和姿態矩陣計算方程為

通過以上的INS解算，得到農機在東-北-天坐標系下的位置、速度和姿態信息。

1.3.2 GNSS/INS組合導航

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

1）離散型卡爾曼濾波系統的一般模型如下：

式中Xk、Xk-1表示系統k時刻和k-1時刻的狀態；A表示系統狀態轉移矩陣；wk-1表示k-1時刻狀態轉移過程中的高斯噪聲；Yk表示系統k時刻的測量值；H表示測量系統的系統參數；vk表示k時刻測量系統的高斯白噪聲。

本系統的15維狀態向量X如公式（5）所示：

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

2）Kalman濾波方程：

1.4 系統軟件設計

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

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

2 田間試驗

2.1 東風DF1004-2試驗平臺

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

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

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

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

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

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

2.2 靜止狀態試驗

本試驗在農機靜止狀態下測試定位導航系統的性能，并對單獨GNSS定位和GNSS/INS組合定位的效果進行比較。試驗于2020年10月在江蘇省張家港市大新鎮長豐村的試驗田進行。試驗中，讓農機靜止，系統同時輸出單獨GNSS導航數據和GNSS/INS導航數據，數據輸出頻率為1 Hz。把位置和航向角數據與對應均值的偏差作為定位誤差用于衡量定位定向精度。單獨GNSS定位系統和GNSS/INS定位系統農機航向角和位置數據如圖7所示。以RTK基準站為坐標原點，地理位置東向為x軸，地理位置北向為y軸。單獨GNSS定位中，位置誤差在1 cm以內，x、y方向上均值分別是6 867.8、2 657.9 cm，方差分別是是0.032、0.043；航向角誤差在0.1°以內：平均值是1.59°，方差是0.001 7。GNSS/INS組合定位中，位置誤差在1 cm以內，x、y方向上均值分別是6 867.7、2 658.4 cm，方差分別是0.026、0.024；航向角誤差在0.1°以內：平均值1.62°，方差是0.002 1。

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

2.3 直線行駛狀態試驗

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

可見，在運動狀態下，相較于單獨GNSS導航定位， GNSS/INS組合導航得到的結果更加精準。

3 結 論

1）本文針對智能農機自動導航的問題，以東風DF1004-2為試驗平臺設計了基于GNSS/INS的智能農機導航控制系統。

2）為實現智能農機導航控制系統的功能，分別研制了基于UB4B0M高精度定位板卡的差分基準站、基于UB482高精度定位板卡與ADIS16460慣性測量單元組合的車載終端。

3）針對INS的確定性偏差以及隨機誤差，設計了基于Kalman濾波的GNSS/INS組合導航算法，融合GNSS數據以提高系統長期運行的精度。

4）試驗結果表明，農機在農田直線行駛時，GNSS/INS組合定位導航得到的航向角偏差在0.5°以內，位置誤差在3 cm以內，優于單獨GNSS系統產生的定位定向結果。本論文的系統設計可以滿足智能農機生產作業的需求。