基于Gazebo的水下機器人模型構建與運動仿真

周善旻 馬佳杰

浙江大學舟山海洋研究中心 浙江 舟山 316000

針對傳統仿真平臺對水下機器人靜力學、動力學模擬困難的問題，提出基于Gazebo的水下機器人模型構建與運動仿真方法。首先，采用Fossen方法構建機器人水動力模型，通過CFD仿真實現主要參數辨識，建立推進器模型后得到推力分配矩陣；其次，在Gazebo平臺下聯合UUV仿真包及相關插件實現水下機器人傳感器、推進器及水動力仿真；最后，采用基于PID的艏向控制及定深控制程序實現水下機器人在仿真環境下的運動控制，為后續復雜定位及控制算法仿真測試和優化提供基礎。

引言

隨著我國加快海洋開發步伐，在十九大報告中明確提出要建設海洋強國，將該戰略納入國家大戰略，海洋權益上升到前所未有的高度。海洋工程裝備在中國制造2025規劃中是十大重要領域之一，水下機器人也逐步成為研究熱點。

對水下機器人來說，自主航行控制、水下定位、軌跡跟蹤等關鍵技術均需要不斷的下海試驗，存在成本高、機會少、風險大的問題，迫切需要合適的水下機器人運動仿真平臺實現相關算法測試及調優。

Matlab中Simulink為模型構建提供了圖形化界面，甚至也提供與ROS通信的包，但是缺乏3維實時可視化能力，且生成的代碼需要進一步移植才能使用在嵌入式系統中；Unity是世界領先的游戲開發實時引擎，可以做出逼真的模型，其圖形可視化也優于其他仿真平臺，但是其缺乏在工程應用的文檔支持，并很難在linux中使用。

本文采用通用機器人仿真平臺Gazebo構建水下機器人模型進行運動控制仿真，首先，其Gazebo基于ROS框架，應用統一通信規范，可以很好的應用在嵌入式系統中；其次，提供仿真測試實時3維可視化能力；最后，構建水動力模型后，可聯合UUV仿真包模擬水動力和靜水力，并以各插件形式實現水下機器人各傳感器和推進器仿真，提供系統化、規范化、可配置化的仿真子功能支持。

1 仿真平臺簡介

ROS是針對機器人開發的軟件編寫框架，提供標準化的用戶、計算機操作系統和計算機外部設備之間交互通信方式，已被廣泛應用于各種機器人平臺應用開發過程中。同時，ROS也提供了大量的工具和廣泛的庫文件，用以配置、自檢、調試機器人平臺，實現機器人操作控制及感知功能。

Gazebo作為通用機器人仿真平臺，一方面集成動力學仿真模塊、傳感器仿真模塊及環境擾動測試模塊，可以準確描述水下機器人系統；另一方面，統一ROS應用程序交互接口，便于算法測試、優化和部署，便于程序移植。

2 水下機器人模型構建

2.1 機器人水動力模型建模

建立正確的水動力模型是建立水下仿真平臺的基礎，具體可分為對運動學和動力學的研究。運動學主要研究運動過程中速度位姿之間的關系，而動力學是分析具體引起水下機器人運動的力，具體包括推進器推力、波浪力、粘性力及慣性力等。

本文采用輪機工程學會(SNAME)術語公報的體系，采用北－東－地坐標系(North－East－Down(NED)coordinate system)作為慣性坐標系，以機器人重心位置為載體坐標系原點。

載體坐標系和慣性坐標系

對比基于泰勒展開式展開的常見的水下航行器建模方法，采用Fossen水動力模型描述航行器水動力特性，并以剛體動力學方程的矢量形式表達。

其中η表示在NED慣性坐標系下機器人的位置，JΘ代表從載體坐標系到慣性坐標系之間的轉換關系，是水下機器人的運動學方程；ν為機器人在載體坐標系下的速度，M˙為慣性系數矩陣，C為科氏力系數矩陣，D為阻尼系數矩陣，g為恢復系數矩陣，τ為附加力，是水下機器人的動力學方程。

為得到較為精確的系數用以模型構建及仿真，利用ICEM設立計算域和邊界層，細分網格，進行壁面影響仿真；通過Fluent仿真水下航行器在直線運動和回轉運動的水動力，擬合仿真曲線得到水下機器人的的慣性系數、一階粘性系數、二階粘性系數及耦合系數。

2.2 推進器模型構建及推力分配

水下機器人本體要實現復雜的對接、航行等功能，需要有很強的運動控制能力，為了實現6個自由度的全驅動運動控制，需要簡單構建推進器模型，并采用矢量控制方法計算推力分配。

由于推進器本身具有慣性，不能立刻響應所需的控制輸入，因此必須構建較為精確的推進器模型。其中Ts為時間常數，通常由推進器技術手冊提供，同時擬合實際推力與轉速曲線得到推力轉換系數krotor，構建簡單的推進器模型。

水下機器人總共有6個推進器，如下圖所示，其中1號、2號和3號為垂推，提供升沉力；5號和6號為主推，提供進退力；4號為側推，同5號6號實現橫移功能。通過這6臺推進器實現6自由度全向運動控制。

水下機器人推進器分布

計算推進器各推力矢量時，取螺旋槳槳面中心為受力點。(按0.005m精度)，獲得推進器的單位推力矢量:

將每個推進器的單位推力轉化為相對于原點的力和力矩，可以獲得推進器到載體推力矢量變換關系，反解推進器推力分配矩陣Mprop:

3 Gazebo與UUV聯合仿真

現階段有較多開源仿真平臺適用于空中及地面機器人，相比而言適用于水下機器人的平臺較少，其主要原因是海洋環境復雜且機器人在水下水動力很難模擬。

SWARMS為Gazebo開發了水下仿真包，稱為UUV仿真包，擴展了通用機器人仿真平臺Gazebo在水下的仿真能力。UUV仿真包主要包括對典型的水下傳感器仿真功能、對推進器的動力學模型仿真功能，對機器人本體的水動力仿真。另外，還可以通過插件形式實現擴展機器人在水下的靜水力，傳感器噪聲設置等功能。在仿真過程中，海洋環境通過SDF進行文件初始化，需要直接通信的對象包括機器人、傳感器、推進器等以通用機器人描述語言URDF文件初始化，每個實例對象還需要提供STL幾何文件描述對象大小及碰撞體積。

在Gazebo中，水下機器人可以描述為固定在主體模型框架base_link下各link的集合，具體包括6個推進器、1個IMU、1個DLV和2個攝像機，他們都被添加到機器人配置文件中，以相對于的機器人主體模型框架base_link的方向和位置來描述并分別命名為thruster0_link、DVL_link、IMU_link和camera0_link。所有傳感器和執行機構都是默認為Gazebo和UUV仿真包里的插件，只要配置相關設備參數，與各自設備相對應。

4 水下機器人運動控制仿真

水下機器具備高度非線性、強耦合性和大慣性等特點，自主航行控制作為其核心關鍵技術，一直是備受關注的研究熱點，對于其位置和姿態的可靠控制是其能在水下平穩作業的前提。通過UUV仿真包配置基于Fossen水動力模型參數，以插件形式實現重力和浮力及各傳感器輸出及配置噪聲，以ROS節點形式訂閱控制程序直接下發的控制量，可以較好實現水下機器人運動控制仿真。

通過基于PID的艏向控制及定深控制程序驗證水下機器人運動控制仿真的有效性?？刂瞥绦蚪邮諄碜訳UV仿真包插件發布的里程計話題/Odom，跟蹤艏向誤差和深度誤差，最終發布控制量話題/thruster_input；Gazebo仿真平臺訂閱控制量話題/thruster_input并作用于機器人的base_link，施加具體的力和力，同時不斷更新當前載體坐標系下速度及方向，水動力插件同步更新機器人受到的水動力，最終實現水下機器人定艏向及定深度的運動控制仿真。在運動控制仿真過程中，可以通過基于ROS的RVIZ插件訂閱軌跡話題實時可視化運動過程，通過rqt工具實時監控ROS下各話題內容，通過rosbag工具保存控制輸出數據便于進一步優化控制參數。

5 結語

以水下機器人為研究對象，針對傳統仿真平臺對水下機器人靜力學、動力學模擬困難的問題，提出基于Gazebo的模型構建與運動仿真方法。仿真平臺主要基于ROS框架，通過配置UUV仿真包水動力插件中Fossen水動力模型參數，配置各傳感器插件于機器人本體坐標系base_link的相對位置及噪聲，配置推進器插件中的時間常數和推力轉換系數及推力分配矩陣實現水動力仿真、推進器仿真及傳感器仿真，并通過艏向控制 及定深控制程序測試運動控制仿真過程，便于進一步算法測試和優化。