無溝鋪管機北斗導航控制系統設計與試驗

王吉中，趙 博，趙士猛，邢高勇，偉利國，胡小安

（中國農業機械化科學研究院，北京 100083）

0 引言

農田暗管排水系統利用人為埋入地下的孔管排出土壤中多余水分與鹽漬[1-2]，具有防澇、控鹽、節水節地、改善生態條件等多重功能[3]。其中，暗管鋪設直線性[4]關乎系統的工作性能、工程壽命和可維護性，乃至系統運行的經濟效益[5]。傳統方法通過人工瞄準標樁控制鋪管直線性[6-7]，不僅抬高了經驗和技術門檻、耗費工人精力，而且精度很難保證。近年來，通過鋪管機自動控制系統調節左右輪速實現直線性控制，但僅適用于土壤質地均勻、附著條件良好的平整地表[7]。因此，亟需開發自動化程度高、環境適應性好且符合鋪管直線性要求的鋪管機導航系統，以滿足實際工程需要。

目前，激光導航、視覺導航和衛星定位導航等技術已應用至農機領域[8-10]。其中，衛星定位導航使用便利、性能穩定且無需參照物，尤其適合農田暗管排水系統大面積施工場景。近年來，Han等[11]基于GNSS、RTK和運動傳感器開發出一套低成本的履帶車輛自動駕駛系統，并提出一種多傳感器數據融合算法。試驗表明，路徑跟隨均方根誤差小于9 cm，最大誤差小于30 cm。Takai等[12]基于RTK-GPS和慣性傳感器，采用自適應控制算法，實現履帶拖拉機自主導航。試驗表明，橫向誤差的均方根小于5 cm。偉利國等[13-14]通過融合GPS定位信息、姿態信息和行走速度檢測運行軌跡，采用模糊控制方法實現開溝鋪管機導航，試驗表明，直線跟蹤誤差不超過10 cm。宋彥等[15]提出一種基于啟發式動態規劃算法的路徑跟隨控制方法，并設計出一種適用于履帶式機器人的路徑跟隨控制系統。試驗表明，算法的直線跟蹤平均誤差絕對值為4 cm、均方根誤差為6 cm。丁幼春等[16]基于免疫比例-積分-微分（Proportion-Integral-Differential, PID）控制算法設計出一種適用于履帶車輛的導航控制器，田間試驗表明，作業速度為0.5 m/s時，直線跟蹤平均絕對偏差為5.8 cm，最大偏差為15.2 cm。上述研究表明，履帶式農機自動導航系統較少，且現有研究多以小型裝備為主，缺乏針對大型履帶農機裝備自動導航技術的研究。

本研究基于載波相位差分技術的北斗定位系統（Real Time Kinematic-BeiDou Navigation Satellite System, RTK-BDS）實時獲取鋪管機定位和航向信息， 采用后向反饋（Back Propagation, BP）神經網絡預測當前行駛狀態，然后選擇器依據預測結果確定控制參數，并通過自適應PID控制算法實現履帶式無溝鋪管機導航控制，以滿足鋪管直線性需求。

1 無溝鋪管機系統構成及原理

1.1 整機結構與原理

本研究基于中國農業機械化科學研究院研制的PV1509HT型V形犁式無溝鋪管機（如圖1）。作業時，使用V形犁體切開土壤，并掀起一個三角形土垡，柔性波紋管道經犁體內的導管進送至土垡底部，從而完成管道鋪設。為了保證V形犁正常作業，需要機器提供較大的牽引力。但由于田間土壤存在松軟差異和高低不平，勢必導致車輛行駛偏擺，進而影響管道鋪設的直線性。因此，迫切需要為機器設計導航控制系統，以保證鋪管作業質量。

1.2 導航控制系統

該機采用雙泵雙回路全液壓驅動的履帶行駛系統，其導航控制原理如圖2所示。機載控制器讀取RTK-BDS和各傳感器的工況信息，并依據暗管施工設置，通過脈沖寬度調制（Pulse Width Modulation, PWM）波控制比例閥調節左、右行走泵排量大小，進而改變左、右行走馬達轉速，實現導航控制。

2 無溝鋪管機數學模型

2.1 履帶轉向運動學模型

在水平地面上，鋪管機穩定轉向行駛[17-19]如圖3所示，點O為整機的轉向瞬心，從O點到履帶縱向對稱平面的距離R為轉向半徑。機器任意一點至點O的距離乘以角速度ω，即為該點的絕對速度，因此兩履帶的絕對速度分別為

式中B為履帶軌距，m；V1為外側履帶絕對速度，m/s；V2為內側履帶絕對速度，m/s。

根據兩側履帶的絕對速度可求得轉向半徑R為

單位時間內的偏航角度θ為

實際情況中，履帶會發生滑轉。引入滑轉速度后，求得內、外側履帶的絕對速度為

式中1V'、2V'為考慮滑轉的外、內側履帶絕對速度，m/s；ΔV1、ΔV2為外、內側履帶滑轉速度，m/s。

此時，可求得實際轉向半徑R′不再等于R。

所以，受履帶滑轉的影響，回轉中心實際與點O并不重合。因此，針對不同的滑轉情況，應采取不同的控制方式。

2.2 導航偏差模型

在控制鋪管機導航行駛時，首先判斷機器與預設鋪管參考線的相對位置，再采取不同的策略控制車輛行駛，圖4顯示了相對位置的9種情況。

預設用于確定參考線的兩點M和N坐標分別為和，其中或。如圖5所示，可求得參考線方程為

由RTK-BDS可獲得鋪管機當前位置為K(x0,y0)，則鋪管機與參考線的橫向誤差d為

其中

假設以正北方向為起始方向，沿逆時針旋轉為正，由圖6可求得鋪管參考線方向角α(0°≤α＜360°)為

其中

由磁力計可實時獲取鋪管機的航向角β，則航向誤差Δθ為

3 基于BP神經網絡的導航控制系統

導航控制流程如圖7，預先通過試驗獲取模態分類和相應控制參數，并存儲在知識庫內，同時采用試驗數據訓練BP神經網絡。導航控制時，首先采用霍爾傳感器檢測左、右行走馬達速度vL、vR，采用RTK-BDS獲取鋪管機絕對速度v，通過控制器局域網絡（Controller Area Network，CAN）通信讀取發動機功率P，采用壓力傳感器檢測左、右行走泵的壓力pL、pR，然后將上述參數值輸入至BP神經網絡，并預測鋪管機當前時刻的模態分類。根據神經網絡的預測結果，選擇器從知識庫中選擇自適應函數F(x)和H(x)，以及PID控制參數，分別發送給自適應比較器和控制器。自適應比較器依據橫向誤差和航向誤差，通過自適應函數選擇橫向或航向控制器，實現導航控制。

3.1 BP神經網絡設計

將鋪管機的可控性分為4種模態，每一模態對應知識庫中不同控制參數。通過BP神經網絡預測模態，以適應復雜田間環境下的各種工況。BP神經網絡架構如圖8所示。

BP神經網絡由輸入層、隱含層和輸出層構成[20]。其中，輸入層設計有6個神經元，分別對應左行走馬達速度vL、右行走馬達速度vR、車輛實際平均車速v、發動機功率P和左、右側行走泵壓力pL、pR的輸入。由于各輸入變量的單位不一致且數值范圍相差較大，提前依據最大最小原則[21]進行歸一化，即

式中 ,ijξ為輸入變量i的第j個值；ξ,mini為輸入變量i的最小值；ξ,maxi為輸入變量i的最大值。

輸出層包含4個神經元，分別對應4個控制模態。輸出層中某神經元輸出為1時，代表鋪管機的可控性處于該模態，輸出為0時代表可控性不處于該模態。

隱含層神經元個數可由式（15）[22]確定為8個。

式中n為隱含層神經元個數；n1為輸入層神經元個數；n2為神經元個數；a為可變系數。

輸入層和隱含層中，每個神經元的激活函數均采用tanh函數

由于輸出層的輸出值只有0和1兩種，則輸出層的激活函數采用sigmoid函數

獲得該神經網絡的輸出值后，使用概率最大閾值函數判斷系統狀態

式中max為閾值分割系數，取輸出層4個神經元輸出值中的最大值。

1.制度完善。聯合體是由多個獨立經營的新型農業經營主體，通過完備的制度體系組成的農業產業化經營組織聯盟。制度體系包括四個層次：一是由聯合體成員共同制定的章程，含經營宗旨、業務范圍、成員義務和權利、代表大會等組織機構及其職責、決策機制、退出機制、資產管理等內容；二是主導產業的建設方案，含基本情況、經營目的、運營內容、保障措施、辦公場所等；三是規范各成員生產經營行為的標準體系，含家庭農場（或專業大戶）和生產類合作社的技術規程、服務類合作社的行為準則、倉儲管理制度、財務管理制度、可追溯管理制度等；四是主體之間簽署的生產、服務等契約合同。

3.2 自適應比較器設計

作業時，鋪管機的犁體深入土壤，因此在導航控制時，機器轉向角度不能過大，不然輕則影響犁體切割土垡，重則導致暗管折斷。為保證在不影響犁體正常工作的同時，能夠快速減小導航誤差，設計了自適應比較器。

在獲取到航向偏差和橫向偏差后，自適應比較器使用自適應函數F(x)和H(x)，分別計算兩個偏差的比較值。當航向偏差的比較值更大時，比較器命令控制器進行橫向控制，否則進行航向控制。經過如此反復調整，可保證鋪管機的航向偏差和橫向偏差均逐漸減小，而不會出現瞬時轉向角度過大的現象。

3.3 模態選擇器設計

為了應對神經網絡的預測結果中可能出現的模態重疊現象，即同時存在兩個或兩個以上模態，設計了模態選擇器。

模態選擇器包含知識庫和選擇器。知識庫中存儲著預先通過試驗得到的4種控制模態參數，每一種模態參數包含有比較器的自適應函數和PID控制參數。

選擇器依據神經網絡預測的控制模態做出判斷，再從知識庫中調用相關模態的參數，并發送給控制器和比較器。選擇器的選擇規則為：若僅有1個模態預測結果，則選擇該模態為當前模態；若有2個連續的預測結果，則選擇控制難度更高的模態為當前模態；若有2種不連續的預測結果，則與上一時刻模態組合，選擇3個模態的中間模態作為當前時刻的模態；如果有3個及以上預測結果，則保持前一時刻的模態。

4 田間試驗

2020年12月，在山東省德州市慶云縣開展田間試驗，采用5點取樣法測試土壤的含水率平均值為24.2%，堅實度平均值為527.2 kPa。田間試驗現場如圖9所示。

4.1 鋪管機模態控制試驗

4.1.1 確定模態控制參數

為獲取不同負載下的鋪管機控制模態，在平整度偏差為±2 cm的地面上，鋪管深度從0開始，每次增加0.05 m。每組試驗中，鋪管機以0.2 m/s的速度行駛10 m。

自試驗開始起，一直采用滿足作業要求的同一套參數實現導航控制，直到第24組試驗（鋪管深度1.2 m）時，橫向誤差大于0.04 m，此時記0～1.2 m過程中的控制參數為第一模態控制參數。保持1.2 m鋪管深度，再次調整控制參數，直至滿足作業要求，一直沿用至第30組試驗（鋪管深度1.5 m）時，橫向誤差再次大于0.04 m，此時記1.2～1.5 m過程中的控制參數為第二模態控制參數。按上述步驟繼續試驗，直到第36次，鋪管深度為1.8 m時，鋪管機原地滑轉，地面行駛速度微乎其微，試驗終止。最后得到1.5～1.65 m過程中的控制參數為第三模態控制參數；1.65～1.8 m過程中的控制參數為第四模態控制參數。

4.1.2 模態控制試驗

使用4種模態控制參數對所有模態分別進行導航控制試驗，橫向誤差結果如表1所示。

表1 模態控制橫向誤差 Table 1 Lateral error of modal control cm

由表1可知，每種模態控制參數在各自模態下的橫向誤差均在±4 cm范圍內，即每種模態控制參數都能夠適應各自狀態的控制。但是，在其他的作業深度分組時，誤差超過±5 cm范圍，甚至不可控。

4.2 BP神經網絡樣本采集

在確定控制模態參數的試驗中，通過傳感器采集，得到每個控制模態下鋪管機的狀態參數。在36組試驗數據中，各隨機選取100條試驗數據，作為BP神經網絡的訓練集。

4.3 上線控制試驗

鋪管作業前，機器應到達預定的參考線上，即首先進行上線操作。上線過程中，犁體完全離開地面，即鋪管深度為0，符合第一控制模態的工況，因此直接使用相應的控制參數實現導航，試驗結果見圖10。

由圖10可知，在一定的初始偏差下，鋪管機逐漸靠近參考線，上線初期出現了一次超調，但之后與參考線的距離逐漸減小。上線期間的航向誤差如圖11所示，橫向距離誤差如圖12所示。

由圖11可知，在上線初期會不時地糾正鋪管機航向，最大航向誤差在±9°范圍內。上線后，航向誤差在±4°范圍內。由圖12可知，在控制初期，橫向誤差不斷減小且減小的速度不斷放緩，在出現4.58 cm的超調后，鋪管機與參考線間橫向誤差的波動逐漸減小，最終穩定在±2.6 cm之內。

4.4 直線導航試驗

在1.6 m的恒定鋪管深度下，以0.2 m/s的鋪管速度，沿預設鋪管參考線作業，以驗證導航控制的效果，航向誤差和橫向誤差如圖13和圖14所示。

圖13 和圖14表明，航向誤差在±5.5°之內，橫向誤差在±3 cm之內。航向和橫向誤差較大的情況雖偶有出現，但在96.2%的情況下，航向誤差在±3°之內；在89.6%的情況下，橫向導航誤差在±1 cm之內。因此，該導航控制能夠滿足鋪管機直線性作業要求。

4.5 工程應用試驗

試驗區域地面平整度偏差為±10 cm，選取施工作業中最為常見的幾種鋪管深度，設置坡降比例為3/10 000 （每100 m深度減小3 cm），進行導航直線性控制試驗，結果如表2所示。

表2 無溝鋪管機在不同鋪管深度工況下試驗結果 Table 2 Test results of the pipelayer under different initial depths

由表2可知，航向誤差控制在±7°范圍內，平均航向誤差在±3.5°范圍內，最大橫向誤差在±4 cm范圍內，平均橫向誤差在±1.5 cm范圍內，滿足鋪管機作業要求。

5 結 論

1）基于載波相位差分技術的北斗定位系統（Real Time Kinematic-BeiDou Navigation Satellite System, RTK-BDS）設計了多模態自適應比例-積分-微分（Proportion-Integral-Differential, PID）控制算法。算法使用后向反饋（Back Propagation, BP）神經網絡判斷無溝鋪管機的控制模態，采用自適應PID控制算法控制鋪管機導航，用于改善無溝鋪管機的適應性。

2）通過試驗獲取鋪管機模態分類及BP神經網絡訓練樣本數據后，開展一系列導航控制試驗。上線試驗中，橫向距離超調為4.58 cm。直線導航試驗中（鋪管深度1.6 m），航向誤差在±5.5°范圍內，最大橫向誤差在±3 cm范圍內。工程應用試驗中（坡降比例3/10 000），航向誤差控制在±7°范圍內，最大橫向誤差在±4 cm范圍內。上述結果表明，該導航控制系統能夠滿足V形犁式無溝鋪管機的作業要求。