河蟹養殖無人作業船路徑跟蹤算法研究

管凱 趙德安 洪劍青

摘要：河蟹養殖無人作業船能解決人工養殖成本高、效率低等問題，提高漁民收益。河蟹養殖無人作業船自動導航作業的關鍵是路徑跟蹤，傳統的純追蹤算法行駛路徑只包含直線作業與岸邊曲線轉彎作業，不能滿足作業船的自主循跡（自動導航）需求。為此針對預先規劃的路徑，研究了改進的純追蹤算法。通過試驗對該導航控制算法進行驗證，結果表明，無人作業船在以50cm/s的速度行駛并轉彎半徑為5m時，最大路徑跟蹤誤差可以控制在0.1m以內。

關鍵詞：無人作業船;自動導航;路徑跟蹤;純追蹤算法

DOI：10.11907/rjdk.182925開放科學（資源服務）標識碼（OSID）：

中圖分類號：TP312文獻標識碼：A 文章編號：1672-7800（2019）010-0078-04

0引言

隨著科技的發展，農業自動化成為研究熱點，自動導航技術成為各國促進農業發展的關鍵技術。自動導航路徑跟蹤誤差大小直接決定了導航控制效果。因此，研究導航控制中的路徑跟蹤算法對農業智能化有著重要意義。

導航控制算法主要研究有：日本學者Nagasaka等在設定轉彎處的轉彎半徑后，根據實際航向與目標航向的偏差控制插秧機的路徑跟蹤，該方法可實現轉彎，但會有很大的超調量;Kisem等采用三次函數控制實現最小轉彎半徑與最大八角速率轉向兩種轉彎方法，雖然仿真最大跟蹤誤差小于0.2m，但該方法實際控制難度較大。Asif等提出一種增加一個積分器的滑模控制算法，相比于傳統的滑模控制性能更好;羅錫文等模擬拖拉機的運動設計了二輪車運動模型，路徑跟蹤采用PID控制，PID參數通過時域階躍響應仿真分析和田間試驗結果綜合整定，但是只適合直線路徑跟蹤;李逃昌等設計的模糊自適應純追蹤算法雖然適用直線和曲線行駛，但是該控制規則由專家經驗設定，路徑跟蹤誤差較大且難以修正;韓科立等在直線路段與地頭轉彎的路徑跟蹤控制使用雙參數控制方法，研制了農業輪式自動作業機器人。

本文以河蟹養殖智能化為背景建立試驗平臺，以天寶LGR-BD9821接收機12V/450W直流有刷電機和ARM9控制版為主要硬件設備，設計基于GPS定位系統改進后的純追蹤控制算法，通過蟹塘試驗對該導航控制算法進行可行性驗證。

1坐標變換與蟹塘區域平面坐標系

1.2蟹塘平面坐標系

先通過GPS定位蟹塘4個角以及碼頭的經緯度，然后通過高斯正形投影轉換位，以碼頭為坐標原點，x軸指向正東方向，y軸指向正北方向，平面坐標系如圖1所示。

M₁、M₂、M₃、M₄真是蟹塘4個角的經緯度轉換后的平面坐標，O為碼頭。實際導航控制算法的路徑規劃以及路徑跟蹤基于該平面坐標系。

2作業船運動學模型

河蟹養殖船采用雙空心浮體作為船體，長3.2m，寬2.2m，使用直流無刷電機（12V/450W）驅動。該船可簡化為二輪小車運動學模型，圖1為平面坐標系下的運動學分析，圖2中α是小車前輪轉向角，以車的縱向中心軸為基準，左邊為負，右邊為正。θ為小車的航向角（小車縱向與真北即Y軸方向的夾角），取順時針方向夾角，L為小車前后車輪中心間距。

3導航控制方法設計

3.1計算前視目標點

前視目標點計算需用到前視距離，而前視距離是仿照人駕駛車輛時觀察車前路況時的距離，研究表明該距離大小與車速及轉彎路段彎曲程度有關。車速越大前視距離（l）越大，彎度越大前視距離越小，即：

3.2導航偏差設定

由圖3可知導航偏差有航向偏差Ω（當前航向與目標航向的角度差）與橫向偏差d（作業船當前位置與預設路徑的最小距離），若作業船位于預設路徑AB的左邊則d為負反之為正，若當前航向在目標航向的左邊則Ω為負反之為正。

3.3路徑跟蹤控制算法設計

河蟹養殖自主作業船由兩個帶有明輪的電機驅動行駛，本文路徑跟蹤控制算法基本原理是利用上述得到的航向偏差與橫向偏差作為驅動控制器的輸入量，將理論推導的電機驅動電壓及正反轉信號作為輸出量。

式（6）中，±0.56Ω前面的正負號表示驅動電機的正反轉信息，+表示正轉，-表示反轉，k_d是橫向偏差的比例系數，反映導航效果，當d：>0.2m時k_d=1.09，反之為0。該算法可實現直線路徑跟蹤也可實現曲線路徑跟蹤，適用性強。

4無人作業船水下試驗

4.1試驗與數據采集

本試驗是在江蘇大學西山操場旁邊的魚塘進行，數據通過無線串口和上位機SecureCRT通信采集，頻率為10Hz，船載GPS接收機單點定位RTK模式水平誤差為±2cm，航向測定誤差最大為0.09°，導航控制算法存儲在MINL2440開發板，試驗現場如圖4所示。

4.2數據分析

將自動導航作業時的橫向誤差d與實時位置P輸出到上位機，由此得到路徑跟蹤誤差曲線與實際行走路線。圖5上半部分為橫向誤差曲線（橫軸為時間，單位100ms，縱軸為距離，單位m），下半部分為實際行走路線（橫軸為經度，縱軸為緯度，系列1是實際導航行走路徑，系列2是預設路徑）。

對試驗數據分析可知，河蟹養殖船導航時的直線段路徑跟蹤平均誤差為0.066m，最大直線段跟蹤誤差為0.122m，轉彎部分跟蹤平均誤差為0.323m，最大轉彎跟蹤誤差為0.521m，誤差在可接受范圍內，表明本文的導航控制算法具有較好的導航效果。

5結語

通過動態調整前視距離1計算前視目標點的方法，能對河蟹養殖無人作業船進行路徑跟蹤時確定目標點，可在一定精度前提下實現準確導航作業。本文對傳統純追蹤算法的改進是針對直線路徑跟蹤，但試驗證明該方法也適用于轉彎路徑跟蹤，實時性強、魯棒性好。本文設計的路徑跟蹤算法雖是針對水面無人作業船，但若地面移動機器人在使用GPS定位時，也可采用坐標轉換實現路徑跟蹤。