基于Gazebo的水下機器人模型構(gòu)建與運動仿真

周善旻 馬佳杰

浙江大學(xué)舟山海洋研究中心 浙江 舟山 316000

針對傳統(tǒng)仿真平臺對水下機器人靜力學(xué)、動力學(xué)模擬困難的問題，提出基于Gazebo的水下機器人模型構(gòu)建與運動仿真方法。首先，采用Fossen方法構(gòu)建機器人水動力模型，通過CFD仿真實現(xiàn)主要參數(shù)辨識，建立推進器模型后得到推力分配矩陣；其次，在Gazebo平臺下聯(lián)合UUV仿真包及相關(guān)插件實現(xiàn)水下機器人傳感器、推進器及水動力仿真；最后，采用基于PID的艏向控制及定深控制程序?qū)崿F(xiàn)水下機器人在仿真環(huán)境下的運動控制，為后續(xù)復(fù)雜定位及控制算法仿真測試和優(yōu)化提供基礎(chǔ)。

引言

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

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

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

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

1 仿真平臺簡介

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

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

2 水下機器人模型構(gòu)建

2.1 機器人水動力模型建模

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

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

載體坐標系和慣性坐標系

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

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

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

2.2 推進器模型構(gòu)建及推力分配

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

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

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

水下機器人推進器分布

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

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

3 Gazebo與UUV聯(lián)合仿真

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

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

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

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

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

通過基于PID的艏向控制及定深控制程序驗證水下機器人運動控制仿真的有效性。控制程序接收來自UUV仿真包插件發(fā)布的里程計話題/Odom，跟蹤艏向誤差和深度誤差，最終發(fā)布控制量話題/thruster_input；Gazebo仿真平臺訂閱控制量話題/thruster_input并作用于機器人的base_link，施加具體的力和力，同時不斷更新當前載體坐標系下速度及方向，水動力插件同步更新機器人受到的水動力，最終實現(xiàn)水下機器人定艏向及定深度的運動控制仿真。在運動控制仿真過程中，可以通過基于ROS的RVIZ插件訂閱軌跡話題實時可視化運動過程，通過rqt工具實時監(jiān)控ROS下各話題內(nèi)容，通過rosbag工具保存控制輸出數(shù)據(jù)便于進一步優(yōu)化控制參數(shù)。

5 結(jié)語

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