[ROS项目]uuv_simulator概述

一. 介绍

​ 水下机器人的应用与算法都是需要一个理想的仿真环境进行测试，不管是AUV还是ROV都是需要一个理想的水下环境。相对于地面机器人和空中机器人来说，水下机器人的仿真环境的建模较为困难。要考虑到水下的流体力学以及作用在机器人本身上的一些复杂力。而且，由于水下机器人模型自身的一个原因，昂贵的硬件成本导致仿真的费用变得非常大。因此，需要选择合适的仿真平台，在复杂的水下环境中完成机器人原型建模以及仿真。

​ 为了选择更好的仿真环境适用于多用户开发和开源的环境，首先选择分析的是在TRIDENT项目UWSim中开发基于ROS的模拟器。该模拟器提供了非常庞大的插件，以及拥有OpenSceneGraph渲染的逼真随机水下环境，且已经应用于多家学术期刊的发表。其中物理引擎用于计算接触力和机器人动力学的应用实现，包括在一个ROS结点下运行的推进器的动力仿真，也可以提供连接其他像Matlab一样更加模块化结构的接口。但是，每次仿真一个新的环境时，就需要一些在XML格式下繁琐的修改，这将使得整个操作变得更加冗杂。

​ 为了将上述物理引擎拓展至更加高效的方式，freefloating-gazebo被作为连接Gazebo和UWSim的桥梁而被提出。Gazebo作为常用的物理仿真环境，具有4种不同的物理仿真引擎，而且freefloating-gazebo软件包包含了多种用于转换数据信息的插件，可以将两者连接的非常完美。但是，该应用出于稳定性的考虑并没有附加矩阵力的计算。而且，对于UWSim可视化的初始化仍然需要在XML格式下完成初步修改。

​ 还有，一个与Gazebo环境相似的仿真环境是将其拓展至Robot Construction Kit(ROCK)的Rock-Gazebo。当物理仿真在Gazebo中运行的时候，由OpenSceneGraph拓展而来的ROCK可视化工具可以完成水下环境的随机工作。但是在这个仿真环境中，无法完成多个机器人编队的任务。

​ 由于上述几个仿真环境无法满足任务需求，所以在Gazebo下开发新的仿真环境来应对新的环境下新机器人的设置工作和其他任务。Gazebo仿真环境为机器人可视化提供了高鲁棒性的仿真框架，允许通过仿真插件完成对新的动力学模型、传感器以及世界环境的扩展。总之，Gazebo作为主流的仿真环境可以在此基础上完成开发任务。

二. 软件说明

​ SWARMs项目中的仿真环境有一个需求是与ROS兼容，只有这样才可以对机器人完成精密测试。然而，为了在未来的机器人开源环境中，其他人也可以用其他中间体使用该仿真环境，所以将整个仿真软件包分为两个部分。机器人、传感器以及环境模型是通过插件来完成实现的，且Gazebo及其自身的API接口通过该插件完成了功能拓展。这些模型并没有嵌入整合在ROS系统本身当中，所以可以用不同的中间体完成对插件的应用。通过ROS bridege完成仿真环境和ROS之间的通讯，像是CV Bridge一样，就是将ROS系统和OpenCV库完成连接，可以在ROS中调用OpenCV的数据，那么ROS bridge一样可以允许Gazebo环境中的插件订阅ROS节点消息，也允许发布至ROS话题中完成数据的交互。下图将展示该软件包中的一个整体框架，主要是插件和控制器。

​ 从图中可以看出Gazebo中的插件有多个传感器插件、推进器插件以及世界环境模型插件等等，在ROS就是通过ROS Bridges和Gazebo的插件联系起来，并且通过多种控制器完成控制任务。

三. Gazebo仿真软件包

​ 在Gazebo仿真环境中允许用户使用多种不同的插件完成任务，正常每个机器人都需要编写一个XML格式的URDF文件来对机器人完成定义，像质量、惯量矩阵、力矩、摩擦等等，在此基础上添加的传感器插件就可以在Gazebo仿真环境下进行仿真，可以说Gazebo为用户留下了一个非常友好的插件API接口。

​ Gazebo因为集成的四个物理引擎，已经能够很好的完成刚体动力学仿真的任务，只需要在世界模型描述下对刚体完成定义和初始化。在水下环境中，流体动力和流体静力及力矩和推进器等动力学因素，所以将该软件包使用的外部插件分为以下类别：

• 执行器模型（例如：推进器）
• 水下传感器
• 流体动力，流体静力和力矩
• 水下环境和环境负载

A. 执行器模型

​ 推进器动力在现实环境中是一个高度非线性化的系统，这通常是水下机器人位置姿态控制性能低下的一个原因，尤其在低俗情况下该弊端更加明显。该模型通常利用角速度作为状态变量来描述螺旋桨的动态行为，且通过角速度函数和输出推力之间的稳定状态关系来将角速度转换为推力。

​ 为了涵盖所有的不同类型的推进器模型并将其应用于仿真的水下机器人，如下图所示，将动力学模型和稳定状态下的转换方程函数作为两个主要框图节点来描述推进器模型。当前可用的动力学模型包括零阶模型，一阶模型，Yoerger螺旋桨动力模型和Bessa扩展模型。推进力转换的稳定曲线函数可以用常数增益建模，或具有死区的非线性增益，或通过输入输出对之间的插值进行建模。

​ 如下图所示，在水下机器人推进器连杆中创建的推进器插件一般都会以x轴作为推进力的方向。每一个推进力$\tau_i$τi​，其中$i$i代表的是推进器的索引，将会单独作用于推进器坐标系。也就是说，所有推进器和机器人本体是分开来计算的，所以每个推进器都可以拥有不同的推进力和方向。

B. 水下传感器

​ 为了完成水下任务，创建并应用了一系列的水下传感器，这是因为在Gazebo环境中的插件都是适合于地面或空中机器人的传感器插件，为了匹配水下环境，不得不重新创建一些适合水下作业的传感器。接下来介绍一些水下传感器。

​ 在Gazebo中已经集成了惯性测量单位传感器（IMU），但是只适用于陆地和空中，在水下其传感器忽略了漂移误差带来的重要影响。所以，在此软件包中的惯性测量单位传感器中是通过机器人仿真器模型来实现的，该模型包括了零均值高斯噪声和领军值白高斯噪声作为速率的随机跟随游动的传感器装置。

​ 磁力计可以用于测量水下机器人航行的方向，该插件是通过指南针插件来实现的。

​ 多普勒速度测井（DVL）的四个声纳传感器已经在Gazebo中完成建模，并且为用户提供可视化效果。

​ 多波束反馈测深仪对于模拟测深度图或避障也很重要，方便理解，可以把这个插件当作Gazebo中陆地传感器中的激光雷达传感器，可以完成2D距离传感，而这个可以用来完成3D的深度传感器以及避障等等。下图显示的是多波束反馈测深仪所展现的图像。

​ 最后，Gazebo中的相机插件不能完美的体现水下环境，所以，在这里创建了一个特殊的水下相继模块，集成了一些图像预处理来完成，该预处理对相机获取的图像添加了像素强度的指数衰减：
$i_{c,m}=i_ce^{-ra_c}+(1-e^{-ra_c})b_c,\forall c\in \{R,G,B\}$ic,m​=ic​e−rac​+(1−e−rac​)bc​,∀c∈{R,G,B}
​ 下图中左边的是预处理之前的图，右边是预处理之后的图。

C. 流体动力和流体静力

​ 对于完全理想化刚体的仿真，Gazebo仿真环境已经提供了对机器人刚体进行描述的方式来完成仿真的方法，包括该刚体的质量、惯量矩阵、关节参数等等。对于计算水下机器人这个复杂的几何体和水下环境之间的力是非常不容易的，也是计算量非常庞大导致无法实现的。所以做好的选择方法是利用已知的模型来对水下机器人运动方程进行仿真描述，就是利用Fossen运动方程：
$(M_{RB}+M_A)\dot{v}_\tau+(C_{RB}(v_\tau)+C_{A}(v_{\tau}))v_\tau+D(v_\tau)v_{\tau}+g_0+g(\eta)=\tau$(MRB​+MA​)v˙τ​+(CRB​(vτ​)+CA​(vτ​))vτ​+D(vτ​)vτ​+g0​+g(η)=τ

对于如何描述水下机器人的运动变换方程，首先需要对水下机器人的坐标系进行标定。接着水下机器人的运动变换方程就可以基于标定过的坐标系以向量形式描述为
$M\dot{v}+K(v)+H(v)+g(\eta)=u\tag{1}$Mv˙+K(v)+H(v)+g(η)=u(1)
其中，$v=[v_x\space v_y \space v_z \space \omega_x\space \omega_y\space \omega_z]^{T}$v=[vx​ vy​ vz​ ωx​ ωy​ ωz​]T表示的是基于水下机器人坐标系的线速度和角速度，$\eta=[x\space y \space z \space \alpha \space \beta \space \gamma]^{T}$η=[x y z α β γ]T代表的是相对于初始状态时候的位置和姿态，$M\in R^{6*6}$M∈R6∗6表示的是惯量矩阵，$K(v)\in R^{6*6}$K(v)∈R6∗6表示的是广义科氏向量和离心力，$H(v)\in R^{6*6}$H(v)∈R6∗6表示的是水波或者海浪的动力效应，也可以是广义的摩擦。$g(\eta)\in R^{6*6}$g(η)∈R6∗6表示的是广义恢复力（重力和浮力）的向量，$u$u表示的是控制力的向量。

机器人的速度和相对坐标系的转换关系可以被描述为
$\dot{\eta} = J(\eta)v\tag{2}$η˙​=J(η)v(2)
其中$J(\eta)$J(η)是雅可比矩阵且其中的$\eta=[\eta_1\space\eta_2]^{T}$η=[η1​ η2​]T，第一个向量$\eta_1=[x\space y \space z]^{T}$η1​=[x y z]T代表的是机器人的位置，第二个向量$\eta_2=[\alpha\space \beta\space \gamma]^{T}$η2​=[α β γ]T表示的是机器人的姿态的欧拉角。

假设惯量矩阵$M$M是对称的、正定的而且其每一个元素都是已知的，那么就可以对其进行可逆分解。但是，一般来说，惯量矩阵是由几何物体、流体速度和其他不确定性的函数组成的，此外，附加的质量系数一般都是通过实验验证和经验数据来估算得到的，导致结果是不太准确。因此，应该明确矩阵$M$M若是看起来不太对陈，则无法将其分解成对角线形式，并且剔除的方法将无效。

假设该惯量矩阵$M$M可以被分解成三个矩阵的形式，如$M=L^{T}NL$M=LTNL，其中$N$N是一个对角矩阵，那么上述的运动变换方程则可以描述成
KaTeX parse error: No such environment: align at position 8: \begin{̲a̲l̲i̲g̲n̲}̲N\xi+K_\xi(\xi)…
其中的矩阵以及向量可以由如下表示
KaTeX parse error: No such environment: align at position 8: \begin{̲a̲l̲i̲g̲n̲}̲N& = \Gamma^{T}…
且$\Gamma=L^{-1}$Γ=L−1，矩阵$N$N是一个在对角线上都是常数的对角矩阵，$\dot{v}=\Gamma\dot{\xi}$v˙=Γξ˙​，因为矩阵上包含常数元素所以$\dot{\Gamma}=0$Γ˙=0。

动力学变换方程的优点

1. 从上述的公式(1)推导可以知道，一般来说虽然至少有一些加速度是独立于其他加速度的，但是对应于惯性力的每个加速度都不可能是完全独立的。而相对来说，从公式(3)可以知道惯性力是独立于每个加速度的，因为其准加速度都是可以单独获取的。矩阵$N$N 是一个对角矩阵，$N=diag\{N_{11}，N_{22}，...，N_{66}\}$N=diag{N11​，N22​，...，N66​}，所以第k个惯性力$(k=1,2,3,...,6)$(k=1,2,3,...,6)可以被描述成$N_{kk}\dot{\xi}_k$Nkk​ξ˙​k​。总的来说，我们可以将惯性力和每个加速度变量联系在一起，用来考虑变量之间的一个相互作用。

2. 每个准速度$\xi_k$ξk​都与其他准速度无关，是独立的变量，且可以通过$\xi_k$ξk​获得动力学能量，如$K(\xi)=\frac{1}{2}v^{T}Mv = \frac{1}{2}\xi^{T}N\xi=\frac{1}{2}\sum_{k=1}^{6}{N_{kk}\xi^{2}_k}$K(ξ)=21​vTMv=21​ξTNξ=21​∑k=16​Nkk​ξk2​。所以这种独立的准速度可以是单独获取并且以用于解耦控制器。

3. 如果比率之间的耦合程度过大，这就意味着非对角线元素的值与对角线元素之间的差距相当或者台大，那么动力学变换方程将相比于耦合程度较小的情况更适用于欠驱动的水下机器人。一般来说，非对角线元素的值由机器人周围的环境或流体的相互作用动力学以及流速来进行确定，机器人的几何形状和流体特性将导致耦合本质上的非线性，如果这些非对角线元素特别大，说明耦合很强，如果机器人是不对称的，则也会导致耦合非常大，在惯性矩阵中获得类似的效果。

   假设，惯量矩阵是
   $M=\begin{bmatrix}m_{11}&m_{12}&\cdots &m_{16}\\m_{21}&m_{22}&\cdots &m_{26}\\\vdots & \vdots &\ddots & \vdots\\m_{61}& m_{62}&\cdots & m_{66}\end{bmatrix}$M=⎣⎢⎢⎢⎡​m11​m21​⋮m61​​m12​m22​⋮m62​​⋯⋯⋱⋯​m16​m26​⋮m66​​⎦⎥⎥⎥⎤​

   如果$m_{ki}\gg m_{kk}$mki​≫mkk​ 或者$m_{ki}\approx m_{kk}(k=1,\cdots,6,i=k+1,\cdots,6)$mki​≈mkk​(k=1,⋯,6,i=k+1,⋯,6)，则将比率之间的耦合称之为很“强”，而如果$m_{ki}\ll m_{kk}$mki​≪mkk​，则比率之间的耦合称之为很“弱”。

四. ROS模块

​ 在ROS中的一些主要模块可以分为远程控制通信模块，机械手控制模块，水下机器人控制模块和对Gazebo中运行插件调用的模块。具体模块的实现都是在软件包中的程序封装好的，有PID控制模式也有其他控制算法。具体控制算法是如何设计或者算法是什么还没有具体细深入，但是整体框架就是ROS中的节点获取插件数据，之后通过推进器分配算法或者推进器控制算法来完成控制机器人姿态和方向的任务。

​ 上图中展现了该rexrov机器人所有的插件，左边是8个推进器，下面是多波束测深仪和IMU姿态测量模块，右边是相机，rpt和压力计，上面是磁力计和dvl插件。