基于星基增強精密單點定位的農(nóng)機自動導航系統(tǒng)開發(fā)與測試

張智剛，王明昌，毛振強，王 輝，丁 凡，李洪開，張 天

(1 華南農(nóng)業(yè)大學 工程學院/南方農(nóng)業(yè)機械與裝備關鍵技術教育部重點實驗室，廣東 廣州 510642； 2 農(nóng)業(yè)農(nóng)村部農(nóng)業(yè)機械化技術開發(fā)推廣總站，北京 100021； 3 濰柴雷沃重工股份有限公司，山東 濰坊 261206)

自動導航是智能農(nóng)機裝備的核心技術之一。農(nóng)機自動導航作業(yè)技術可有效提高農(nóng)機的作業(yè)效率和質量，避免漏行疊行作業(yè)，以及降低駕駛員的勞動強度[1-2]。目前，自動導航農(nóng)機在我國新疆、黑龍江等地已實現(xiàn)產(chǎn)業(yè)化與規(guī)模化生產(chǎn)，但在實際應用中也存在一些問題：一是農(nóng)機自動導航系統(tǒng)需要依賴高精度GNSS定位技術，其復雜的地基增強差分通訊鏈路增加了系統(tǒng)復雜度和運營成本；二是導航控制算法對不同農(nóng)機作業(yè)速度的適應性不好，隨著速度的增加，導航精度會逐漸下降[3-4]。

星基增強精密單點定位技術可單機作業(yè)，靈活機動，作業(yè)不受通訊距離的限制[5-7]。它集成了標準單點定位和差分定位的優(yōu)點，克服了各自的缺點[8-11]，改變了只能使用雙差定位模式才能達到較高定位精度的現(xiàn)狀，較傳統(tǒng)的差分定位技術具有顯著的技術優(yōu)勢[12-16]。近年來，國內有些公司和科研機構開始提供星基增強精密單點定位技術的商業(yè)化服務，定位性能不斷提高，服務范圍不斷擴大。本研究嘗試采用星基增強精密單點定位技術開發(fā)農(nóng)機自動導航系統(tǒng)，以減少系統(tǒng)對地基增強差分通訊鏈路的依賴。與此同時，設計基于位速卡爾曼濾波的導航信息處理算法和預瞄跟隨PID路徑跟蹤控制算法，通過對不同農(nóng)機作業(yè)速度下的前視距離與PID參數(shù)進行試驗整定，獲得控制參數(shù)整定表，并在自動導航拖拉機上進行不同行駛速度下的性能驗證，以期為提高系統(tǒng)對不同速度的自適應能力提供參考。

1 系統(tǒng)總體結構

本研究以華南農(nóng)業(yè)大學研究的農(nóng)機自動導航系統(tǒng)為基礎，以配套高花輪胎的雷沃TX1204拖拉機為平臺[17-21]，基于精簡、通用、穩(wěn)定可靠的設計思想，融合導航信息獲取和路徑跟蹤控制算法，開發(fā)基于星基增強精密單點定位的拖拉機自動導航系統(tǒng)。導航系統(tǒng)由GNSS天線、合眾思壯V28星基增強定位板卡、導航控制器、顯示終端、輪角/壓力傳感器、電控液壓轉向系統(tǒng)等組成，如圖1所示。導航控制器是農(nóng)機導航系統(tǒng)的核心，主要功能是：1)根據(jù)顯示終端輸入的A1、A2點信息和作業(yè)幅寬規(guī)劃作業(yè)路徑；2)通過GNSS天線、星基增強定位板卡以及輪角/壓力傳感器獲取定位信息以及前輪轉向角度信息，之后對接收的各傳感器信息進行處理；3)利用路徑跟蹤控制算法決策期望輪角并輸出轉向控制信號。電控液壓轉向系統(tǒng)是農(nóng)機的轉向執(zhí)行機構，其作用是將轉向控制信號轉換成農(nóng)機轉向輪的轉向速度和角度，進而控制農(nóng)機按照預定作業(yè)路線精確跟蹤行走。顯示終端是導航系統(tǒng)的人機交互終端，通過CAN總線與導航控制器進行通信，實現(xiàn)導航系統(tǒng)的狀態(tài)顯示、系統(tǒng)調試、作業(yè)設置和系統(tǒng)操控等人機交互功能。

圖1 農(nóng)機自動導航系統(tǒng)的總體結構示意圖Fig.1 Schematic diagram of the overall structure of the agricultural machinery automatic navigation system

2 導航信息處理方法

2.1 GRS80大地坐標向大地導航坐標系的轉換

為了使GNSS星基增強定位板卡輸出的GRS80大地坐標定位數(shù)據(jù)能用于拖拉機的導航控制系統(tǒng)，需要采用高斯投影變換公式，將GRS80地心大地坐標轉換為對應于GRS80橢球的高斯投影平面坐標。

采用如下公式：

式中，c為極曲率半徑。

所用投影坐標系的主要參數(shù)為：投影方式：Gauss-Kruger；中央經(jīng)線：114.000000(3 度帶)；水平偏移量：500 km；地理坐標系：GCS_GRS_1980；大地參照系：D_GRS_198參考橢球體：GRS80；橢球長軸：6 378 137.000 000；橢球扁率：0.003 352 810 7。

2.2 GNSS天線位置點向導航控制點的轉換

本文所使用的大地導航坐標系由GRS80橢球對應的高斯-克呂格投影坐標平面加上GRS80地心坐標系中的大地高程構成，其中軸指向地理東向，軸 指向地理北向，軸指向天空方向，與軸構成右手握手坐標系，坐標原點設置在拖拉機作業(yè)區(qū)域的適當位置；所使用的車體坐標系原點位于拖拉機重心位置，三軸指向分別為：軸指向拖拉機機頭方向，軸指向拖拉機駕駛室頂棚方向。將導航控制點選擇在拖拉機2個后輪著地點的連線中點處，有利于建立更為嚴密的拖拉機航位推算遞推關系式；有利于GNSS天線定位點至導航控制點距離的量取；有利于拖拉機自動導航過程中后掛作業(yè)機具的位置控制；有利于GNSS天線傾斜誤差的校正。

假設導航系統(tǒng)測得的拖拉機三軸姿態(tài)角航向角度、橫滾角度和俯仰角度分別為、和，則拖拉機車體坐標系下的空間矢量向大地導航坐標系轉換的轉換矩陣為：

2.3 位速卡爾曼濾波器設計

為保證作業(yè)質量，拖拉機田間作業(yè)過程中大多采用往復直線作業(yè)的形式。針對這種情況，本文提出基于位置和速度的拖拉機運動學模型，來適配拖拉機田間直線行駛作業(yè)的實際過程。

星基增強精密單點定位可能存在異常跳點，會對導航系統(tǒng)產(chǎn)生干擾。為了提高導航系統(tǒng)的工作穩(wěn)定性，本文基于上述航位推算原理設計了位速卡爾曼濾波器來對星基增強精密單點定位進行濾波處理?？柭鼮V波器的設計基礎是狀態(tài)轉移方程和觀測方程?？紤]到拖拉機田間勻速行駛作業(yè)的實際特點，本文假設的穩(wěn)態(tài)是緩變的。將前述(6)、(7)式以卡爾曼濾波器狀態(tài)轉移方程的形式表示為：

根據(jù)前述分析是個常值矩陣；

以經(jīng)式(5)轉換得到的拖拉機導航控制點定位坐標作為觀測向量，得到卡爾曼濾波器的測量方程如下：

測量向量的噪聲方差矩陣為：

綜合上述推導，采用線性離散卡爾曼濾波器的遞歸差分方程進行狀態(tài)向量預測和測量向量校正：

預測方程組為：

校正方程組為：

3 預瞄跟隨路徑跟蹤控制算法設計

3.1 預瞄跟隨PID路徑跟蹤控制模型

本文設計的預瞄跟隨PID路徑跟蹤控制模型可用圖2描述：直線A1A2為作業(yè)規(guī)劃行直線；點為拖拉機的當前導航控制點，為拖拉機前進方向上的預瞄點，為預瞄跟隨的前視距離d；過點向A1A2做垂線，垂線的距離即為拖拉機在大地坐標系下的位置偏差； 預瞄點向 A1A2做垂線，垂線的距離為拖拉機的預瞄點位置偏差；設為拖拉機當前航向與目標航向的偏差角，稱為航向偏差。預瞄跟隨PID路徑跟蹤控制算法步驟如下：

圖2 預瞄追蹤模型幾何示意圖Fig.2 Geometric diagram of preview tracking model

2)計算拖拉機導航控制點到規(guī)劃路徑A1A2上位置偏差， 則拖拉機預瞄位置點處的位置偏差可計算為：

3.2 模型控制參數(shù)的試驗整定方法

由于算法簡單、魯棒性好和可靠性高，PID控制策略被廣泛應用于工業(yè)過程控制，尤其適用于可建立精確數(shù)學模型的確定性控制系統(tǒng)。比例控制P可快速、及時、按比例調節(jié)偏差，提高控制靈敏度[22]，在保持系統(tǒng)穩(wěn)定的前提下，P值越大，精度越高。積分控制I能消除偏差，提高控制精度、改善穩(wěn)態(tài)性能。微分控制D是一種超前控制，能調節(jié)系統(tǒng)速度、減小超調量、提高穩(wěn)定性。駕駛員在駕駛農(nóng)機作業(yè)時常常使用對中桿，對中桿與預定路線偏差大時，駕駛員對轉向輪角進行大角度控制，以快速校正預瞄航向偏差跟蹤預瞄路徑；當農(nóng)機方向和預瞄方向偏差較小時，駕駛員對轉向輪角進行小角度控制，以穩(wěn)定地跟蹤預瞄路徑[23-26]。在路徑跟蹤控制過程中，農(nóng)機的行駛速度越高，其需要設置的前視距離越大，也就是要瞄的更遠些。根據(jù)上述經(jīng)驗和規(guī)律，需合理選擇前視距離和PID控制器參數(shù)才能迅速、準確、平穩(wěn)地消除偏差，達到較好的控制效果。

本文制定的模型控制參數(shù)的試驗整定方法為：在某一恒定前進速度條件下，首先將P固定為一個經(jīng)驗適中值，前視距離由大向小調(步長1 m)，調到系統(tǒng)微微震蕩時停止，將前視距離+1作為此前進速度條件下前視距離的尋優(yōu)結果；然后，將前視距離固定為尋優(yōu)值，將P從那個適中值由小向大調(步長5)，調到系統(tǒng)微微震蕩時停止，當前的P值即為停止，當前的D值即為尋優(yōu)結果。對I的尋優(yōu)整定，遵循低速大些、高速小些的調節(jié)原則，保證在能夠快速糾偏的前提下不引起震蕩即可。整定后的參數(shù)如表1所示。

表1 不同速度條件下的路徑跟蹤控制參數(shù)Table 1 Navigation control parameters under different travelling speeds

4 試驗測試

4.1 測試方法

采用和芯星通UM482-BOX2 RTK定位終端輸出數(shù)據(jù)作為參考量，搭建拖拉機自動導航測試系統(tǒng)。自動導航系統(tǒng)測試的原理圖如圖3所示。

圖3 拖拉機自動導航測試系統(tǒng)結構Fig.3 Test system structure for satellite-based automatic pilot system of tractor

本文使用1拖2功分器將GNSS雙天線分別接入V28星基增強定位板卡和UM482-BOX2 RTK定位終端。導航控制器將拖拉機路徑跟蹤結果加上GNSS定位時間戳以10 Hz的輸出頻率從串口輸出到筆記本以供保存。導航控制器的輸出參數(shù)主要包括：星基增強定位的相關數(shù)據(jù)、A1和A2點、位置偏差、航向偏差、期望輪角、反饋輪角等。與此同時，UM482-BOX2 RTK定位終端通過內置4G網(wǎng)絡模塊接入千尋知寸厘米級差分服務數(shù)據(jù)，將帶有GNSS定位時間戳的NMEA GPGGA報文以10 Hz的輸出頻率從串口輸出到筆記本以供保存。筆記本同時接收2路數(shù)據(jù)輸入，以GNSS定位的時間戳保持數(shù)據(jù)同步，以UM482-BOX2 RTK定位終端的參考數(shù)據(jù)以及A1和A2點數(shù)據(jù)為依據(jù)，通過自主開發(fā)的Matlab程序和Python程序來測算導航控制精度指標。

具體測試過程為：在平坦地面，啟動拖拉機自動導航系統(tǒng)，按照速度為 3、5、7 km/h 3 個檔位開展系統(tǒng)性能測試[27]。每次測試執(zhí)行如下流程：首先是完成上線過程，再按規(guī)定的速度繼續(xù)沿著A1A2線一個方向自動導航，直線行駛測試段長度不小于80 m，然后掉頭，重新上線，并沿著相鄰行的相反方向自動導航至少80 m回到開始位置，最后依據(jù)和芯星通UM482 RTK接收機提供的記錄數(shù)據(jù)計算相關參數(shù)，進而測試系統(tǒng)的直線跟蹤誤差和鄰接行誤差，如圖4所示。

圖4 系統(tǒng)誤差測試行駛方式示意圖Fig.4 Schematic diagram of driving mode of system error test

4.2 試驗結果

用Python編程語言開發(fā)原始數(shù)據(jù)對照測試的程序。最后將測算的數(shù)據(jù)以文本文件的方式導入Matlab程序中，繪制出自動導航性能測試結果如圖5所示。

圖5 不同速度下自動駕駛系統(tǒng)路徑跟蹤誤差曲線Fig.5 Path tracking error curve of automatic autopilot system at different speed

4.3 測試結果分析

以和芯星通UM482 RTK接收機提供的記錄數(shù)據(jù)為參考，對速度 3、5、7 km/h 3 個檔位的路徑跟蹤數(shù)據(jù)進行統(tǒng)計分析，結果如表2所示。

由表2統(tǒng)計路徑跟蹤的誤差分析可知，農(nóng)機自動導航系統(tǒng)在直線跟蹤方面，3、5、7 km/h的平均誤差分別為-0.000 756 4、-0.000 917 0 和-0.001 157 5 m，直線跟蹤誤差大小隨著行駛速度的增加而略微增大，表明行駛速度對農(nóng)機自動導航系統(tǒng)的直線跟蹤性能的影響不明顯；農(nóng)機自動導航系統(tǒng)在鄰接行誤差方面，3、5、7 km/h 的平均誤差分別為 0.000 618 7、0.000 724 9 和 0.000 795 0 m，表明不同的行駛速度對農(nóng)機自動導航系統(tǒng)鄰接行跟蹤性能的影響不明顯。上述分析表明，預瞄跟隨PID參數(shù)的整定有利于提高系統(tǒng)對不同速度的適應性。

表2 農(nóng)機星基輔助自動導航系統(tǒng)性能統(tǒng)計表Table 2 Performance statistics of satellite-based auxiliary agricultural machinery automatic autopilot system

由不同速度下各個指標的平均值可知，在直線跟蹤誤差方面，平均誤差為-0.000 943 6 m，標準差為 0.024 52 m，最大誤差絕對值為 0.084 72 m；在鄰接行誤差方面，平均誤差為 0.000 712 8 m，標準差為 0.029 86 m，最大誤差絕對值為 0.154 44 m。結果表明，農(nóng)機自動導航作業(yè)直線平均誤差和鄰接行平均誤差都在1 mm左右，標準差小于3 cm，這一精度可滿足大部分農(nóng)機自動導航作業(yè)需求，星基增強精密單點定位技術可以用于農(nóng)機自動導航系統(tǒng)的產(chǎn)品研發(fā)和推廣應用。

5 結論

本文設計了基于星基增強精密單點定位的農(nóng)機自動導航系統(tǒng)，以和芯星通地基增強RTK接收機為參考，按 3、5、7 km/h 3 個速度檔位開展了系統(tǒng)測試工作。測試結果表明，國產(chǎn)星基增強定位技術已趨于成熟，將國產(chǎn)星基增強定位技術用于農(nóng)機自動導航是可行的，其路徑跟蹤控制精度可滿足大部分農(nóng)機作業(yè)需求。另外，本文設計的預瞄跟隨PID路徑跟蹤控制模型和提出的不同速度條件下PID參數(shù)與前視距離的整定方法，為提高系統(tǒng)對不同速度的自適應能力提供了基礎。

對于農(nóng)機自動導航系統(tǒng)來說，星基增強精密單點定位精度是影響其直線跟蹤精度和對行精度的重要因素，因而優(yōu)化精密單點定位的算法性能，提升其定位穩(wěn)定性，是提升星基增強自動導航系統(tǒng)精度的重要措施。