周善旻 馬佳杰

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

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

引言

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

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

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

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

1 仿真平臺(tái)簡(jiǎn)介

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

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

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

2.1 機(jī)器人水動(dòng)力模型建模

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

本文采用輪機(jī)工程學(xué)會(huì)(SNAME)術(shù)語(yǔ)公報(bào)的體系，采用北－東－地坐標(biāo)系(North－East－Down(NED)coordinate system)作為慣性坐標(biāo)系，以機(jī)器人重心位置為載體坐標(biāo)系原點(diǎn)。

載體坐標(biāo)系和慣性坐標(biāo)系

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

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

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

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

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

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

水下機(jī)器人總共有6個(gè)推進(jìn)器，如下圖所示，其中1號(hào)、2號(hào)和3號(hào)為垂推，提供升沉力；5號(hào)和6號(hào)為主推，提供進(jìn)退力；4號(hào)為側(cè)推，同5號(hào)6號(hào)實(shí)現(xiàn)橫移功能。通過(guò)這6臺(tái)推進(jìn)器實(shí)現(xiàn)6自由度全向運(yùn)動(dòng)控制。

水下機(jī)器人推進(jìn)器分布

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

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

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

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

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

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

4 水下機(jī)器人運(yùn)動(dòng)控制仿真

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

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

5 結(jié)語(yǔ)

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