海參捕撈機器人運動控制系統的仿真研究

葛安亮，陳 浩，邵緒新，李相坤

（1.中國海洋大學工程訓練中心，山東 青島 266100；2.中國海洋大學工程學院，山東 青島 266100）

0 引 言

隨著水下機器人技術的發展，海參捕撈機器人正逐步取代費時費力且危險性極高的人工捕撈作業[1]。采用水下機器人進行海參捕撈，不僅解放了人力，還擴大了捕撈范圍，提高了海參的捕獲量。為了保證海參捕撈機器人的作業時間，同時保證機器人通信的穩定性，一般將其設計為有纜水下機器人（ROV）[2-3]。但是由于海底的作業環境惡劣多變和機器人的結構功能復雜，同時所捕撈的海參也會對機器人的運動造成干擾，導致機器人的運動控制難度增大，一旦控制精度降低，將會影響海參的捕撈效率，嚴重時還會導致機器人發生側翻。由此可見，海參捕撈機器人的運動控制系統設計是保證機器人捕撈效率和運動穩定性的關鍵。

由于捕撈目標的復雜性，在設計過程中通常會忽略掉其對機器人所造成的干擾[4-8]。但在實際情況下，隨著捕撈時間的增加，捕撈目標的質量會越來越大，而且捕撈目標在慣性和流體作用下還會發生移動，從而導致機器人的質量、質心等參數發生變化。如果系統的魯棒性不強，機器人的運動控制精度將會大大下降。針對捕撈目標對機器人的干擾問題，最常用的解決方法主要有：設計魯棒性更強的控制系統、通過質量塊或浮體移動補償裝置進行補償、抽送高密度液體進行補償、通過推進器冗余推力進行補償等[9]。關于水下機器人運動的干擾研究，方磊等人利用自抗擾控制技術建立了AUV 的路徑跟蹤誤差方程[10]；田烈余等人通過結合PI 控制和PID控制，對水下機器人的控制器進行了抗干擾測試[11]；陳恩志等人和華長春等人均是利用干擾觀測器對水下機器人的運動干擾補償，從而提高了控制系統的魯棒性[12-13]。G. V. Lakhekar 等人基于干擾觀測器的模糊自適應S 面控制器對水下機器人的空間軌跡跟蹤進行了控制分析[14]；R. Fernandez 等人基于非線性狀態反饋線性化原理和有限水平線性二次調節器，對球形水下機器人進行了控制系統設計[15]；V. L. Oliveira 等人同樣運用干擾觀測器對水下機器人的運動控制系統進行了模型預測控制[16]；J. Bak 等人基于冗余驅動模型的分解補償法對傾斜推進器水下機器人的懸停進行了控制研究[17]。

綜上所述，水下捕撈機器人運動控制的設計需要綜合考慮各方面的影響，捕撈目標的影響更是不容忽視。目前，針對海參捕撈機器人運動控制系統的研究也相對較少，因此，本文綜合考慮海參的干擾，對機器人的運動控制系統進行更全面的研究分析，對于提升機器人的運動穩定性和抗干擾能力，實現捕撈目標的精準定位具有重要的實際意義，也將推動海參捕撈機器人的市場化進程。

1 海參捕撈機器人的動力學模型

針對淺海海底的海參捕撈作業需求，設計一種海底行進式的有纜水下機器人（長×寬×高為1 600 mm×800 mm×450 mm），如圖1 所示。該機器人主要由控制艙、收集艙、履帶、吸捕結構、推進器、浮體材料和臍帶纜等幾部分組成，機器人的總重力約為1 500 N，機器人可以通過推進器實現在水中的運動，通過履帶牽引實現在水底的行進。

圖1 海參捕撈機器人結構圖

1.1 動力學模型

本文參考文獻[18]中等提出的水下機器人建模方法，綜合考慮機器人在水中作業時會受到自身重力、浮力、推進器推力、恢復力、流體阻力及海參干擾，建立一種海參捕撈機器人在水中運動時的動力學模型。

1）剛體動力學

機器人的運動主要是平動和轉動，將機器人假設為理想的剛體，根據歐拉定理和牛頓第二定律，建立機器人的剛體動力學模型[19-20]，公式如下：

式中：MRB為機器人的慣性質量矩陣；CRB為科氏力矩陣[21]。

2）流體動力學

機器人在水下運動時，會受到流體的作用力，其合外力（矩）τH可表示為：

式中：τT為所有推進器的合外力（矩）；MA為附加質量；CA為科氏力；D(υ)為阻尼力；g(η)為恢復力。

3）恢復力

為保證機器人在水中不會發生傾斜和側翻，通常將機器人的浮心設計在重心的正上方，則浮力P與重力W就組成了一組恢復力矩[22]。當機器人的俯仰角φ和橫滾角θ改變時，恢復力矩始終保持在豎直方向上，恢復力所產生的分力和力矩會促使機器人發生位移和恢復到初始的平穩姿態。恢復力的矩陣表示如下：

4）推進器推力

機器人在水中的運動是依靠推進器的槳葉正反轉所產生的向前和向后的推力。為方便建模，將推進器的推力T與槳葉的轉速n簡化為二階非線性關系[23]，表達式如下：

式中：D為槳葉直徑；KT為推力因數；J0為進速比。

所有推進器的總推力τT可表示為：

式中：H為推力分配矩陣，其與機器人的自由度和推進器的布置方式、位置相關。

5）海參干擾

由于海參質量的增加是不規律的，而且在慣性和流體的作用下，收集艙內的海參還會發生移動，所以海參所造成的干擾也是不確定的。將這部分干擾定義為τd，τd可表示為各個量的變化量之和，公式為：

綜上，得到海參捕撈機器人的動力學模型，公式為：

1.2 模型參數求解

1.2.1 轉動慣量

在Solidworks 中對機器人進行三維建模，并賦予各零部件材料屬性，利用質量參數功能，求解得到機器人空載時的轉動慣量矩陣，如下：

1.1.2 推力分配矩陣

為實現機器人的六自由度運動，本機器人布置了8個推進器，推進器布置如圖2 所示。

圖2 推進器布置

與水平方向呈45°角布置的4 個推進器距離X軸和Y軸的距離分別是0.38 m 和0.6 m，豎直布置的4 個推進器距離X軸和Y軸的距離分別是0.33 m 和0.38 m。因此，推力分配矩陣H公式如下：

1.2.3 阻尼系數

由于一階和二階阻尼系數分別是阻力與速度多項式中的一次項和二次項系數，所以借助于Fluent 軟件模擬機器人在各運動自由度下的不同速度工況，得到機器人在各運動自由度下不同流速工況的阻力。然后對阻力和速度進行多項式擬合，提取擬合得到的多項式中的一次項和二次項系數，即為一階和二階阻尼系數。

在進行運動模擬之前，需要先對模型進行適當簡化，主要包括忽略模型上角度較小或突出的較小零部件，將推進器、履帶總成、控制艙均簡化為實體，忽略復雜的局部曲面、倒角、定位孔和連接結構。簡化后的機器人模型如圖3 所示。

圖3 簡化后的機器人模型

選取RANS 方程作為計算流場的控制方程，以標準k-ε模型作為湍流模型，機器人直線運動的流體域設置為長方形（長30 m、寬14 m、高14 m），旋轉運動的流體域設置為圓柱體（半徑7 m、長30 m），模型均位于流體域的正中央。為保證計算精度，流體域的網格最大尺寸為200 mm，對機器人的網格進行局部細化，網格最大尺寸為10 mm。劃分好網格的模型局部圖如圖4 所示。

圖4 局部網格劃分

機器人在6 個自由度的阻尼系數如表1 所示。

表1 阻尼系數

1.2.4 附加質量

由于海參捕撈機器人的結構復雜，所以其附加質量通過AQWA 軟件來求解。計算模型與求解阻尼系數的模型相同，流體域設置為長方體（長30 m、寬30 m、高50 m），設置吃水深度為10 m，使模型完全浸沒在水下。在模型的重心位置增加質量點，添加6 個自由度的附加質量求解項。最終得到機器人的附加質量，公式如下：

2 水動力學性能分析

以動力學模型為基礎，運用模型預測控制（MPC）理論，創建海參捕撈機器人的預測控制模型。由于海參干擾具有不確定性，因此采用非線性干擾觀測器（NDO）來預估該部分不規則干擾，然后進行反饋補償。整個控制系統的結構如圖5 所示。

圖5 控制系統結構圖

2.1 預測模型

為了實現對機器人位置和姿態的優化控制，在機器人動力學模型的基礎上，增加機器人在6 個自由度上的位移和角度作為新的狀態量，增廣后的狀態方程為：

為了實現預測模型對未來狀態量的迭代預測，還需要對式（11）進行線性化處理，本文采用工作點處近似線性化的方法，將其轉化為線性時變的狀態方程。則在任意時刻t，機器人經過線性化后的狀態方程為：

線性化后的狀態方程是連續的，還需將其離散化才能進行后續的迭代預測。離散化后的狀態方程矩陣公式為：

2.2 目標函數

目標函數可以使系統對目標進行跟蹤，并且預測時域內各時刻的控制增量。以機器人的位置、姿態和控制量作為優化目標，設計系統的目標函數為：

為實現對控制增量的精確控制，將式（14）轉化為以控制增量為變量的目標優化函數，表示為：

式中：第一項是對系統的跟蹤性能優化；第二項是要求系統控制量平穩變化[24]；Q和R為權重矩陣。

2.3 約束條件

在系統運動過程中，各狀態量的變化會受到系統本身性能的限制，比如：推進器輸出的力不是無限的，推進器無刷電機的轉速和推力的關系等，因此需要對控制量施加約束條件。將目標函數的未知量設為預測時域內的一系列控制增量Δu，其他變量轉化為用控制增量表達的形式[25]，則控制量與增量滿足如下關系：

因此，控制量的約束轉換為：

根據處理后的約束條件，可以將原來的目標函數優化問題轉化為解決以下方程的二次規劃問題：

2.4 滾動優化

在每一個時刻模型預測控制器對目標函數進行求解之后，得到該時刻控制時域Nc內的控制增量為：

在這基礎上，結合上一時刻的控制量，即為當前時刻的控制量。機器人運行到下一個時刻時，繼續重復求解過程，就可以實現軌跡跟蹤。滾動優化流程見圖6。

圖6 滾動優化流程

2.5 非線性干擾觀測器

非線性干擾觀測器是將所有的外界干擾和模型參數變化引起的內部干擾全部等效為一個整體，并反饋到系統輸入端，通過調節推進器的冗余推力對所有干擾進行補償。本文主要考慮機器人的質量和質心參數變化所引起的干擾，設計的非線性干擾觀測器公式如下：

3 運動控制系統仿真研究

根據所建立的機器人動力學模型，在Simulink 中搭建海參捕撈機器人的運動控制仿真模型，并將運動過程中產生的狀態量輸入到Matlab 的工作空間，求出增廣后的機器人動力學模型分別關于增廣狀態量和控制量的雅可比矩陣，并將其離散化后代入到模型預測控制程序中。運動控制系統的Simulink 仿真模型如圖7 所示。

圖7 控制系統Simulink 仿真模型

以機器人未開始捕撈作業時作為初始穩定狀態，將模型的軌跡追蹤權重系數Q設置為5 000，控制增量約束權重系數R設置為0.01，非線性干擾觀測器收斂系數C設置為1，預測時域NP 長度設置為15 步，控制時域NC 長度設置為1 步。

3.1 定深控制仿真

海參捕撈機器人通常是在海底進行捕撈作業，但也有極個別的目標作業深度較淺，這時就需要通過推進器控制機器人，實現定深下潛。以水下5 m 深度為目標量，對機器人進行定深控制仿真，仿真結果如圖8 所示，其中無深度控制的機器人下沉曲線如圖8a）所示，進行深度控制后的機器人下沉運動曲線如圖8b）所示。

圖8 定深控制仿真結果

從仿真結果可以看出：當沒有進行深度控制時，在下沉的開始階段，機器人在重力作用下加速下沉；隨著下沉速度的增大，水阻力也在增大，當水阻力與重力相互平衡之后，機器人就會轉變成勻速下沉，在圖8a）中的曲線斜率也是呈現先增大后保持不變的變化規律。當對機器人進行定深控制時，機器人會下沉至目標深度附近，通過推進器克服重力作用并進行深度調整，直至調整至目標深度。在定深過程中，上升時間為23.8 s，最大超調量約為13.6%，穩態誤差為0.01 m。機器人到達目標深度的時間會受推進器推力大小的影響，推進器推力越大，到達穩定的時間越短，但會對超調量有所影響。

3.2 姿態保持控制仿真

機器人在水中的姿態運動主要表現為偏航運動和小角度的俯仰運動。根據右手定則，設置目標仰角為0.3 rad，右偏轉角度為0.5 rad，分別進行姿態保持控制仿真，仿真結果如圖9 所示。

圖9 姿態保持仿真結果

從圖9 中可以看出，當給定目標角度后，機器人經過一段時間的調整，最終均可達到目標角度的姿態。其中，俯仰姿態的上升時間為20 s，穩態誤差為0.002 rad；偏航姿態的上升時間為9.6 s，最大超調量為10.2%，穩態誤差為0.001 rad。俯仰運動的上升時間和穩態誤差均大于偏航運動，其原因是在俯仰運動過程中，由于俯仰角的存在，會導致重力和浮力組成的恢復力在X軸方向和繞Y軸方向產生阻力（矩），該力（矩）會促使機器人向前運動和恢復到原始姿態。為了抵消恢復力矩，需要通過推進器進行調節，使機器人保持姿態穩定，這也導致機器人會產生一定的波動。在偏航角度控制時，由于只有水阻力，因此在控制過程中會產生一定的超調量，但是經過反饋矯正后最終會達到目標角度，且姿態保持穩定。

3.3 路徑跟蹤控制仿真

機器人在水中的運動是連續的，因此控制系統需要對動態軌跡具備良好的連續追蹤能力。此次仿真選取的是半徑為10 m 的圓形軌跡，圓心偏離X軸正方向15 m。設定參考點的速度為0.5 m/s，軌跡起點坐標為（5 m，0 m），追蹤軌跡如圖10 所示，機器人在X軸和Y軸的動態追蹤誤差如圖11 所示。

圖10 圓形追蹤軌跡

圖11 X 軸和Y 軸動態追蹤誤差

從仿真結果可以看出，初始時刻機器人的機體坐標系與大地坐標系相互重合，X軸的正方向為機器人頭部的指向。隨著運動的開始，機器人在X軸方向相對初始目標點有一定的偏移，X方向的偏差使機器人開始向軌跡點運動，在追蹤軌跡的過程中，X軸誤差逐漸減小。同時，軌跡點在Y方向運動，使得機器人Y軸誤差逐漸增大，機器人開始在Y方向追蹤軌跡點。經過一段時間的運行后，誤差逐漸收斂，實現對圓形參考軌跡的追蹤控制。最終，X軸的穩定誤差為0.02 m，Y軸的穩定誤差為0.01 m，機器人穩定后的速度達到0.5 m/s。

3.4 海參干擾仿真

隨著收集艙內海參的增多和移動，機器人的質量和質心等參數就會改變，恢復力矩就會產生分力（矩），對機器人的運動產生干擾。在干擾模塊中，設置收集艙內的海參質量為20 kg，并設置海參在X軸發生-0.3 m 的位移，在Z軸產生-0.1 m 的位移，運動過程設置為從水底上升0.5 m，則運動過程中機器人在有無非線性干擾觀測器情況下俯仰角的仿真結果如圖12 所示。

圖12 海參干擾仿真結果

從仿真結果可以看出，無非線性干擾觀測器時，機器人產生了俯仰角，在原有控制系統的控制下，經過一段時間后會調整到穩定狀態，但由于魯棒性有限，而且機器人的數學模型發生了改變，所以無法使機器人完全恢復到零俯仰角的狀態，俯仰角最終穩定在0.26 rad。采用非線性干擾觀測器后，機器人狀態量的變化會被及時評估并反饋補償，機器人最終會達到零俯仰角狀態，調整時間為7.9 s。由此可見，當不考慮所捕撈海參所造成的干擾時，原有的控制系統只可在一定程度下保證機器人的穩定運動；但是考慮干擾之后，機器人參數的變化會被推進器的冗余推力抵消，避免機器人發生側翻，提高了運動的可控性和穩定性。

4 結 論

海參捕撈機器人的運動控制系統設計是保證機器人運動穩定性和提高捕撈效率的關鍵。本文在綜合考慮機器人所有載荷的前提下，又考慮到海參對機器人造成的干擾，創建了更加全面的動力學模型；運用模型預測控制理論，基于機器人的動力學模型，創建了運動控制模型，采用非線性干擾觀測器模擬海參所造成的干擾。在Simulink 環境下對機器人進行定深控制仿真、姿態保持控制仿真和路徑跟蹤控制仿真。通過分析仿真結果發現：該運動控制模型具有較高的控制精度。同時，還通過設定某一運動過程，比較了是否考慮海參干擾情況下機器人運動姿態的變化，通過比較發現：隨著海參的累積，機器人的自身參數會發生變化，由于原控制系統的魯棒性有限，機器人的運動狀態會被海參所干擾；當在控制系統中引入海參干擾之后，機器人會通過調節推進器的冗余推力來抵消干擾，實現穩定的運動狀態。通過本文的研究工作，形成了一套系統完善的海參捕撈機器人運動控制系統，對于改進海參捕撈機器人的運動穩定性和抗干擾能力具有重要的意義。

注：本文通訊作者為李相坤。