無溝鋪管機(jī)北斗導(dǎo)航控制系統(tǒng)設(shè)計(jì)與試驗(yàn)

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

（中國(guó)農(nóng)業(yè)機(jī)械化科學(xué)研究院，北京 100083）

0 引言

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

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

本研究基于載波相位差分技術(shù)的北斗定位系統(tǒng)（Real Time Kinematic-BeiDou Navigation Satellite System, RTK-BDS）實(shí)時(shí)獲取鋪管機(jī)定位和航向信息， 采用后向反饋（Back Propagation, BP）神經(jīng)網(wǎng)絡(luò)預(yù)測(cè)當(dāng)前行駛狀態(tài)，然后選擇器依據(jù)預(yù)測(cè)結(jié)果確定控制參數(shù)，并通過自適應(yīng)PID控制算法實(shí)現(xiàn)履帶式無溝鋪管機(jī)導(dǎo)航控制，以滿足鋪管直線性需求。

1 無溝鋪管機(jī)系統(tǒng)構(gòu)成及原理

1.1 整機(jī)結(jié)構(gòu)與原理

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

1.2 導(dǎo)航控制系統(tǒng)

該機(jī)采用雙泵雙回路全液壓驅(qū)動(dòng)的履帶行駛系統(tǒng)，其導(dǎo)航控制原理如圖2所示。機(jī)載控制器讀取RTK-BDS和各傳感器的工況信息，并依據(jù)暗管施工設(shè)置，通過脈沖寬度調(diào)制（Pulse Width Modulation, PWM）波控制比例閥調(diào)節(jié)左、右行走泵排量大小，進(jìn)而改變左、右行走馬達(dá)轉(zhuǎn)速，實(shí)現(xiàn)導(dǎo)航控制。

2 無溝鋪管機(jī)數(shù)學(xué)模型

2.1 履帶轉(zhuǎn)向運(yùn)動(dòng)學(xué)模型

在水平地面上，鋪管機(jī)穩(wěn)定轉(zhuǎn)向行駛[17-19]如圖3所示，點(diǎn)O為整機(jī)的轉(zhuǎn)向瞬心，從O點(diǎn)到履帶縱向?qū)ΨQ平面的距離R為轉(zhuǎn)向半徑。機(jī)器任意一點(diǎn)至點(diǎn)O的距離乘以角速度ω，即為該點(diǎn)的絕對(duì)速度，因此兩履帶的絕對(duì)速度分別為

式中B為履帶軌距，m；V1為外側(cè)履帶絕對(duì)速度，m/s；V2為內(nèi)側(cè)履帶絕對(duì)速度，m/s。

根據(jù)兩側(cè)履帶的絕對(duì)速度可求得轉(zhuǎn)向半徑R為

單位時(shí)間內(nèi)的偏航角度θ為

實(shí)際情況中，履帶會(huì)發(fā)生滑轉(zhuǎn)。引入滑轉(zhuǎn)速度后，求得內(nèi)、外側(cè)履帶的絕對(duì)速度為

式中1V'、2V'為考慮滑轉(zhuǎn)的外、內(nèi)側(cè)履帶絕對(duì)速度，m/s；ΔV1、ΔV2為外、內(nèi)側(cè)履帶滑轉(zhuǎn)速度，m/s。

此時(shí)，可求得實(shí)際轉(zhuǎn)向半徑R′不再等于R。

所以，受履帶滑轉(zhuǎn)的影響，回轉(zhuǎn)中心實(shí)際與點(diǎn)O并不重合。因此，針對(duì)不同的滑轉(zhuǎn)情況，應(yīng)采取不同的控制方式。

2.2 導(dǎo)航偏差模型

在控制鋪管機(jī)導(dǎo)航行駛時(shí)，首先判斷機(jī)器與預(yù)設(shè)鋪管參考線的相對(duì)位置，再采取不同的策略控制車輛行駛，圖4顯示了相對(duì)位置的9種情況。

預(yù)設(shè)用于確定參考線的兩點(diǎn)M和N坐標(biāo)分別為和，其中或。如圖5所示，可求得參考線方程為

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

其中

假設(shè)以正北方向?yàn)槠鹗挤较?，沿逆時(shí)針旋轉(zhuǎn)為正，由圖6可求得鋪管參考線方向角α(0°≤α＜360°)為

其中

由磁力計(jì)可實(shí)時(shí)獲取鋪管機(jī)的航向角β，則航向誤差Δθ為

3 基于BP神經(jīng)網(wǎng)絡(luò)的導(dǎo)航控制系統(tǒng)

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

3.1 BP神經(jīng)網(wǎng)絡(luò)設(shè)計(jì)

將鋪管機(jī)的可控性分為4種模態(tài)，每一模態(tài)對(duì)應(yīng)知識(shí)庫中不同控制參數(shù)。通過BP神經(jīng)網(wǎng)絡(luò)預(yù)測(cè)模態(tài)，以適應(yīng)復(fù)雜田間環(huán)境下的各種工況。BP神經(jīng)網(wǎng)絡(luò)架構(gòu)如圖8所示。

BP神經(jīng)網(wǎng)絡(luò)由輸入層、隱含層和輸出層構(gòu)成[20]。其中，輸入層設(shè)計(jì)有6個(gè)神經(jīng)元，分別對(duì)應(yīng)左行走馬達(dá)速度vL、右行走馬達(dá)速度vR、車輛實(shí)際平均車速v、發(fā)動(dòng)機(jī)功率P和左、右側(cè)行走泵壓力pL、pR的輸入。由于各輸入變量的單位不一致且數(shù)值范圍相差較大，提前依據(jù)最大最小原則[21]進(jìn)行歸一化，即

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

輸出層包含4個(gè)神經(jīng)元，分別對(duì)應(yīng)4個(gè)控制模態(tài)。輸出層中某神經(jīng)元輸出為1時(shí)，代表鋪管機(jī)的可控性處于該模態(tài)，輸出為0時(shí)代表可控性不處于該模態(tài)。

隱含層神經(jīng)元個(gè)數(shù)可由式（15）[22]確定為8個(gè)。

式中n為隱含層神經(jīng)元個(gè)數(shù)；n1為輸入層神經(jīng)元個(gè)數(shù)；n2為神經(jīng)元個(gè)數(shù)；a為可變系數(shù)。

輸入層和隱含層中，每個(gè)神經(jīng)元的激活函數(shù)均采用tanh函數(shù)

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

獲得該神經(jīng)網(wǎng)絡(luò)的輸出值后，使用概率最大閾值函數(shù)判斷系統(tǒng)狀態(tài)

式中max為閾值分割系數(shù)，取輸出層4個(gè)神經(jīng)元輸出值中的最大值。

1.制度完善。聯(lián)合體是由多個(gè)獨(dú)立經(jīng)營(yíng)的新型農(nóng)業(yè)經(jīng)營(yíng)主體，通過完備的制度體系組成的農(nóng)業(yè)產(chǎn)業(yè)化經(jīng)營(yíng)組織聯(lián)盟。制度體系包括四個(gè)層次：一是由聯(lián)合體成員共同制定的章程，含經(jīng)營(yíng)宗旨、業(yè)務(wù)范圍、成員義務(wù)和權(quán)利、代表大會(huì)等組織機(jī)構(gòu)及其職責(zé)、決策機(jī)制、退出機(jī)制、資產(chǎn)管理等內(nèi)容；二是主導(dǎo)產(chǎn)業(yè)的建設(shè)方案，含基本情況、經(jīng)營(yíng)目的、運(yùn)營(yíng)內(nèi)容、保障措施、辦公場(chǎng)所等；三是規(guī)范各成員生產(chǎn)經(jīng)營(yíng)行為的標(biāo)準(zhǔn)體系，含家庭農(nóng)場(chǎng)（或?qū)I(yè)大戶）和生產(chǎn)類合作社的技術(shù)規(guī)程、服務(wù)類合作社的行為準(zhǔn)則、倉(cāng)儲(chǔ)管理制度、財(cái)務(wù)管理制度、可追溯管理制度等；四是主體之間簽署的生產(chǎn)、服務(wù)等契約合同。

3.2 自適應(yīng)比較器設(shè)計(jì)

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

在獲取到航向偏差和橫向偏差后，自適應(yīng)比較器使用自適應(yīng)函數(shù)F(x)和H(x)，分別計(jì)算兩個(gè)偏差的比較值。當(dāng)航向偏差的比較值更大時(shí)，比較器命令控制器進(jìn)行橫向控制，否則進(jìn)行航向控制。經(jīng)過如此反復(fù)調(diào)整，可保證鋪管機(jī)的航向偏差和橫向偏差均逐漸減小，而不會(huì)出現(xiàn)瞬時(shí)轉(zhuǎn)向角度過大的現(xiàn)象。

3.3 模態(tài)選擇器設(shè)計(jì)

為了應(yīng)對(duì)神經(jīng)網(wǎng)絡(luò)的預(yù)測(cè)結(jié)果中可能出現(xiàn)的模態(tài)重疊現(xiàn)象，即同時(shí)存在兩個(gè)或兩個(gè)以上模態(tài)，設(shè)計(jì)了模態(tài)選擇器。

模態(tài)選擇器包含知識(shí)庫和選擇器。知識(shí)庫中存儲(chǔ)著預(yù)先通過試驗(yàn)得到的4種控制模態(tài)參數(shù)，每一種模態(tài)參數(shù)包含有比較器的自適應(yīng)函數(shù)和PID控制參數(shù)。

選擇器依據(jù)神經(jīng)網(wǎng)絡(luò)預(yù)測(cè)的控制模態(tài)做出判斷，再?gòu)闹R(shí)庫中調(diào)用相關(guān)模態(tài)的參數(shù)，并發(fā)送給控制器和比較器。選擇器的選擇規(guī)則為：若僅有1個(gè)模態(tài)預(yù)測(cè)結(jié)果，則選擇該模態(tài)為當(dāng)前模態(tài)；若有2個(gè)連續(xù)的預(yù)測(cè)結(jié)果，則選擇控制難度更高的模態(tài)為當(dāng)前模態(tài)；若有2種不連續(xù)的預(yù)測(cè)結(jié)果，則與上一時(shí)刻模態(tài)組合，選擇3個(gè)模態(tài)的中間模態(tài)作為當(dāng)前時(shí)刻的模態(tài)；如果有3個(gè)及以上預(yù)測(cè)結(jié)果，則保持前一時(shí)刻的模態(tài)。

4 田間試驗(yàn)

2020年12月，在山東省德州市慶云縣開展田間試驗(yàn)，采用5點(diǎn)取樣法測(cè)試土壤的含水率平均值為24.2%，堅(jiān)實(shí)度平均值為527.2 kPa。田間試驗(yàn)現(xiàn)場(chǎng)如圖9所示。

4.1 鋪管機(jī)模態(tài)控制試驗(yàn)

4.1.1 確定模態(tài)控制參數(shù)

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

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

4.1.2 模態(tài)控制試驗(yàn)

使用4種模態(tài)控制參數(shù)對(duì)所有模態(tài)分別進(jìn)行導(dǎo)航控制試驗(yàn)，橫向誤差結(jié)果如表1所示。

表1 模態(tài)控制橫向誤差 Table 1 Lateral error of modal control cm

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

4.2 BP神經(jīng)網(wǎng)絡(luò)樣本采集

在確定控制模態(tài)參數(shù)的試驗(yàn)中，通過傳感器采集，得到每個(gè)控制模態(tài)下鋪管機(jī)的狀態(tài)參數(shù)。在36組試驗(yàn)數(shù)據(jù)中，各隨機(jī)選取100條試驗(yàn)數(shù)據(jù)，作為BP神經(jīng)網(wǎng)絡(luò)的訓(xùn)練集。

4.3 上線控制試驗(yàn)

鋪管作業(yè)前，機(jī)器應(yīng)到達(dá)預(yù)定的參考線上，即首先進(jìn)行上線操作。上線過程中，犁體完全離開地面，即鋪管深度為0，符合第一控制模態(tài)的工況，因此直接使用相應(yīng)的控制參數(shù)實(shí)現(xiàn)導(dǎo)航，試驗(yàn)結(jié)果見圖10。

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

由圖11可知，在上線初期會(huì)不時(shí)地糾正鋪管機(jī)航向，最大航向誤差在±9°范圍內(nèi)。上線后，航向誤差在±4°范圍內(nèi)。由圖12可知，在控制初期，橫向誤差不斷減小且減小的速度不斷放緩，在出現(xiàn)4.58 cm的超調(diào)后，鋪管機(jī)與參考線間橫向誤差的波動(dòng)逐漸減小，最終穩(wěn)定在±2.6 cm之內(nèi)。

4.4 直線導(dǎo)航試驗(yàn)

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

圖13 和圖14表明，航向誤差在±5.5°之內(nèi)，橫向誤差在±3 cm之內(nèi)。航向和橫向誤差較大的情況雖偶有出現(xiàn)，但在96.2%的情況下，航向誤差在±3°之內(nèi)；在89.6%的情況下，橫向?qū)Ш秸`差在±1 cm之內(nèi)。因此，該導(dǎo)航控制能夠滿足鋪管機(jī)直線性作業(yè)要求。

4.5 工程應(yīng)用試驗(yàn)

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

表2 無溝鋪管機(jī)在不同鋪管深度工況下試驗(yàn)結(jié)果 Table 2 Test results of the pipelayer under different initial depths

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

5 結(jié) 論

1）基于載波相位差分技術(shù)的北斗定位系統(tǒng)（Real Time Kinematic-BeiDou Navigation Satellite System, RTK-BDS）設(shè)計(jì)了多模態(tài)自適應(yīng)比例-積分-微分（Proportion-Integral-Differential, PID）控制算法。算法使用后向反饋（Back Propagation, BP）神經(jīng)網(wǎng)絡(luò)判斷無溝鋪管機(jī)的控制模態(tài)，采用自適應(yīng)PID控制算法控制鋪管機(jī)導(dǎo)航，用于改善無溝鋪管機(jī)的適應(yīng)性。

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