CN110096063A - 一种基于测距仪的水下清洗机器人自主倒立着落船底控制方法 - Google Patents

Abstract

本发明提出了一种基于测距仪的水下清洗机器人自主倒立着落船底的控制方法。针对着落控制中参考量难以推导、船底倾角信息无法先验获取以及存在重浮心不重合的问题，本发明利用垂直安装于清洗机器人底面的测距仪实时测量机器人与船底之间的距离，得到机器人相对船底的俯仰角、横滚角和距离。在着落过程中，为解决角度与距离同时控制时导致的机器人位置“漂移”问题，以及重浮心不重合导致的难以实现倒立问题，本发明提出了能补偿重浮心不重合，且先进行“角度和定深控制”，后进行“角度和距离控制”的两阶段PID控制策略，使机器人相对船底坐标系的俯仰角、横滚角先趋于零，再使距离之差趋于零，实现无“漂移”自主倒立着落船底的目标，并通过仿真验证了有效性。

Description

一种基于测距仪的水下清洗机器人自主倒立着落船底控制 方法

技术领域

本发明涉及的是用于水下清洗机器人自主控制技术领域，具体为一种基于测距仪的水下清洗机器人自主倒立着落船底的控制方法。

背景技术

随着远洋运输事业的飞速发展，船舶作为海上交通运输的主要工具发挥着日益重要的作用。船舶在航行过程中，海洋生物、贝类以及锈皮等会附着在船壁上，其会增加船舶阻力，减小船舶航行速度，降低船舶使用寿命。因此，为了延长船舶使用寿命，提高航行效率，保证航行安全，需要对船舶表面附着物进行定期清洗。

由于传统水下手工清洗方式存在效率低、安全性差和成本高等问题，因此，需要研究和寻求机器人和自动化技术在水下清洗作业中的应用。水下清洗机器人作为能提高水下清洗作业自动化水平的有效工具之一，其能大大节省劳动力，提高清洗效率，具有广阔的应用前景和发展空间。

水下清洗机器人的工作模式通常有三种组成，即巡游模式、着落船体模式、船体爬行模式，其中，着落船体模式包括着落船舷和着落船底两类，是介于水下巡游和船体爬行模式之间的过渡阶段。由于船底的倾角信息无法先验获取且不同船的船底倾角不同，因此，如何选取合适的传感器和控制策略实现自主稳定地倒立着落船底是一项极具挑战性的研究课题。同时，在实际着落过程中，当角度和距离同时控制时，水下清洗机器人会产生“漂移”现象，如何设计合适控制方法抑制“漂移”也是着落船底控制中的一个重要研究方向。此外，在倒立着落控制时，清洗机器人的重浮心不重合会导致较大的跟踪误差，不利于稳定地自主着落船底。

综上所述，水下清洗机器人实现自主着落船底存在三个难点问题：1、船体底面倾角信息无法先验获取；2、如何选取恰当的传感器，并根据传感器量测值构造出合适的着落控制参考量；3、如何克服在自主着落船底过程由于重浮心不重合对贴合效果造成的不利影响。

发明内容

针对着落控制中参考量难以获取以及船底倾角信息无法先验获取的问题，本发明利用多个垂直安装于清洗机器人底部的测距仪实时测量机器人与船底之间的距离，分别解算水下清洗机器人相对船底坐标系的俯仰角、横滚角，以及相对于船底的距离。本发明的控制目标是使机器人与船底坐标系之间的俯仰角、横滚角和距离误差趋于零，从而实现自主着落贴合船壁的目标。另外，在着落过程中，为了解决角度差与距离差同时控制时导致的水下清洗机器人位置“漂移”问题，以及水下清洗机器人重浮心不重合问题，本发明提出了先进行“角度和定深控制”，后进行“角度和距离控制”的两阶段控制策略。

本发明的技术方案为：

所述一种基于测距仪的水下清洗机器人自主倒立着落船底控制方法，其特征在于：包括以下步骤：

步骤1：利用安装于水下清洗机器人底面的测距仪实时测量各个测距仪与船底之间的距离，并根据测距仪的安装参数以及实时测量得到的各个测距仪与船底之间的距离，解算出水下清洗机器人底面相对船底的俯仰角θ、横滚角φ和距离d；根据使机器人自主着落至船底的指令参数，得到待控制的水下清洗机器人底面相对船底的俯仰角差值Δθ＝θ-θ_r、横滚角差值Δφ＝φ-φ_r和距离差值Δd＝d-d_r；

步骤2：考虑水下清洗机器人重浮心不重合而在体坐标系下产生的附加力矩，得到用于补偿附加力矩的推进器推力为：

其中T_b为用于补偿附加力矩的推力向量，G为机器人的重力，和分别为传感器测得的机器人在地面坐标系下的俯仰角和横滚角，(x_c,y_c,z_c)为机器人的重心相对浮心在体坐标系下的偏移量，所述体坐标系以浮心为原点；为分配矩阵C₃的伪逆，分配矩阵C₃由推进器的安装参数确定；

步骤3：对于角度和距离控制，还采用先“角度和定深控制”后“角度和距离控制”两阶段PID控制策略：

当|Δθ|＞β₁或|Δφ|＞β₂时为第一阶段，其中β₁和β₂均为设定的大于零的阈值常数；第一阶段的控制律为：

其中T₀为用于角度和距离控制的推力向量，和分别为水下清洗机器人在地面坐标系下的偏航角和偏航角参考值，Δh＝h-h_d，h为机器人深度传感器的测量值，h_d为期望深度；为分配矩阵C₁的伪逆，分配矩阵C₁由推进器的安装参数确定；

当|Δθ|≤β₁且|Δφ|≤β₂时为第二阶段，第二阶段的控制律为：

其中K_p1，K_i1，K_d1为PID距离控制中的待调参数，K_p2，K_i2，K_d2为PID横滚角控制中的待调参数，K_p3，K_i3，K_d3为PID航向角控制中的待调参数，K_p4，K_i4，K_d4为PID俯仰角控制中的待调参数，K_p5，K_i5，K_d5为PID深度控制中的待调参数；为分配矩阵C₂的伪逆，分配矩阵C₂由推进器的安装参数确定；

步骤4：综合步骤2和步骤3，得到水下清洗机器人控制律T＝T₀-T_b。

进一步的优选方案，所述一种基于测距仪的水下清洗机器人自主倒立着落船底控制方法，其特征在于：安装在水下清洗机器人底面的四个测距仪M₁、M₂、M₃和M₄，测距方向垂直于水下清洗机器人底面，测量的与船底之间的距离分别为d₁、d₂、d₃和d₄；四个测距仪处于矩形的四角，其中测距仪M₁与M₄的连线垂直于水下清洗机器人俯仰轴Z_b；测距仪M₁与M₂的距离为l₄，测距仪M₁与M₄的距离为l₅；得到水下清洗机器人底面相对船底的俯仰角θ、横滚角φ和距离d为：

进一步的优选方案，所述一种基于测距仪的水下清洗机器人自主倒立着落船底控制方法，其特征在于：θ_r＝0，φ_r＝0，d_r为当水下清洗机器人处于贴合船底状态时四个测距仪中心点到船底的距离。

进一步的优选方案，所述一种基于测距仪的水下清洗机器人自主倒立着落船底控制方法，其特征在于：水下清洗机器人共采用六个推进器；其中安装在水下清洗机器人底面有四个推进器T₁、T₂、T₃、T₄，四个推进器分布在矩形四个角上，且推力方向垂直于水下清洗机器人底面，推进器T₁和T₄的连线垂直于水下清洗机器人俯仰轴Z_b；推进器T₁和T₂的距离为l₁，推进器T₁和T₄的距离为l₂；安装在水下清洗机器人侧面有两个推进器T₅和T₆，推进器T₅和T₆的推力方向平行于水下清洗机器人的横滚轴X_b，推进器T₅和T₆的距离为l₃；

分配矩阵C₃为：

分配矩阵C₁为：

分配矩阵C₂为：

有益效果

本发明通过多个垂直安装于清洗机器人底部的测距仪对机器人与船底之间的距离进行测量，并在此基础上，解算出机器人与船底之间的俯仰角、横滚角和距离差，从而解决了自主着落控制中参考量难以获取以及船底倾角信息无法先验获取的问题。同时，本发明提出先进行“角度和定深控制”，后进行“角度和距离控制”的两阶段控制策略，可以解决在着落过程中当角度差与距离差同时控制时导致的水下清洗机器人位置“漂移”问题。另外，本发明提出了补偿重浮心不重合方法，解决了由于重浮心不重合导致的倒立控制误差大的问题。

本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

附图1为基于测距仪的水下清洗机器人自主着落控制原理框图。

附图2为四个测距仪均可探测到船底的工作示意图。

附图3为水下清洗机器人六个推进器分布示意图。

附图4为水下清洗机器人先进行“角度和定深控制”，再进行“角度和距离控制”的两阶段控制策略下的X-Z与X-Y平面的轨迹图。

附图5为水下清洗机器人角度和距离同时控制策略下的X-Z与X-Y平面的轨迹图。

附图6(a)和(b)分别为无重浮心补偿与有重浮心补偿自主着落控制策略的三个欧拉角变化曲线。

附图7为水下清洗机器人与船底坐标系之间的距离和三个欧拉角跟踪误差曲线。

具体实施方式

下面详细描述本发明的实施例，所述实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。

本实施例中水下清洗机器人的初始位置和欧拉角分别为：即X₀＝-1m，Y₀＝-4m，Z₀＝-1m，初始速度和角速度均为零，仿真步长T_s＝0.1s，l₁＝0.574m，l₂＝0.454m，l₃＝0.454m，x_c＝z_c＝0m，y_c＝0.024m。控制参数为：K_p1＝50，K_p2＝-22，K_p3＝-13，K_p4＝23，K_p5＝-2.9，K_i1＝0，K_i2＝0，K_i3＝0，K_i4＝0，K_i5＝0，K_d1＝0，K_d2＝0，K_d3＝0，K_d4＝0，K_d5＝0。

利用安装于水下清洗机器人底面的测距仪实时测量各个测距仪与船底之间的距离，并根据测距仪的安装参数以及实时测量得到的各个测距仪与船底之间的距离，解算出水下清洗机器人底面相对船底的俯仰角θ、横滚角φ和距离d。

图2给出了四个测距仪均可测距的工作示意图，其中，M₁、M₂、M₃和M₄分别为布置在水下清洗机器人底部的1号、2号、3号和4号超声波测距仪器；这四个测距仪在水下清洗机器人底部呈矩形分布，其中l₄和l₅分别为矩形的边长，具体地l₄为M₁与M₂(M₃与M₄)之间的距离，l₅为M₁与M₄(M₂与M₃)之间的距离。d₁、d₂、d₃和d₄分别为1号、2号、3号和4号超声波测距仪器沿Y_b方向到船底的测量距离；N₁、N₂、N₃和N₄分别为四个测距仪在沿Y_b方向在船底上的投影。

利用四个垂直安装于水下清洗机器人底面的测距仪实时测量机器人与船底之间的距离，分别解算出水下清洗机器人相对船壁坐标系的俯仰角、横滚角和距离，其表达式为：

其中，d为水下清洗机器人的四个测距仪中心点到船壁的距离；θ和φ分别为水下清洗机器人相对船底的俯仰和横滚角。此外，定义Δθ＝θ-θ_r，Δφ＝φ-φ_r，Δd＝d-d_r为待控制的角度和距离差；为使机器人自主着落至船底，定义θ_r＝0，φ_r＝0，d_r为当水下清洗机器人处于贴合船壁状态时四个测距仪中心点到船壁的距离。

根据图3所示的六个推进器分布示意图，可知在推进器推力T₁、T₂、T₃、T₄、T₅和T₆作用下的水下清洗机器人受到的力F_y和力矩M_x、M_y、M_z分别为：

其中，T₁、T₂、T₃、T₄、T₅和T₆分别为1至6号推进器的推力值；F_x、F_y、M_x、M_y和M_z分别为在六个推进器作用下的机器人的体坐标系下x方向推力，y方向推力，x方向力矩，y方向力矩，z方向力矩；F_yn和M_ψ分别为在六个推进器作用下的地坐标系y方向推力和y方向力矩；l₁、l₂、l₃如图3所示。

在推导出的距离测量值与俯仰、横滚角之差的对应关系的基础上，给出了跟踪误差的比例-微分-积分与F_y、M_x、M_ψ、M_z和F_yn之间的关系如下所示：

通过计算重浮心不重合而在体坐标系下产生的力矩，可推导出用于补偿重浮心不重合的六个推进器推力为：

其中，T_b＝[T_1b,T_2b,T_3b,T_4b,T_5b,T_6b]^T，G为清洗机器人的重力，和分别清洗机器人在地面坐标系下的俯仰角和横滚角，(x_c,y_c,z_c)为机器人的重心相对浮心在体坐标系下的偏移量，所述体坐标系以浮心为原点；为分配矩阵C₃的伪逆，C₃的定义为：

在步骤1所述的映射关系基础上，以及图3给出的水下清洗机器人的六个推进器分配示意图，设计了考虑重浮心不重合的先“角度和定深控制”后“角度和距离控制”两阶段PID控制策略。

当|Δθ|＞β₁或|Δφ|＞β₂时为第一阶段，其中β₁和β₂均为设定的大于零的阈值常数；第一阶段的控制律为：

其中T₀＝[T₁₀,T₂₀,T₃₀,T₄₀,T₅₀,T₆₀]^T，在着落船底过程中需要对偏航角进行实时控制，以防止因该角度不稳定对贴合效果造成的不利影响，即和分别为水下清洗机器人在地面坐标系下的偏航角和偏航角参考值，Δh＝h-h_d，h为机器人深度传感器的测量值，h_d为期望深度；为分配矩阵C₁的伪逆，C₁的定义为：

当|Δθ|≤β₁且|Δφ|≤β₂时为第二阶段，第二阶段的控制律为：

其中K_p1，K_i1，K_d1为PID距离控制中的待调参数，K_p2，K_i2，K_d2为PID横滚角控制中的待调参数，K_p3，K_i3，K_d3为PID航向角控制中的待调参数，K_p4，K_i4，K_d4为PID俯仰角控制中的待调参数，K_p5，K_i5，K_d5为PID深度控制中的待调参数；为分配矩阵C₂的伪逆，C₂的定义为：

在重浮力不重合的补偿基础上，联合第一阶段控制律和第二阶段控制律，可推导出水下清洗机器人控制律T＝T₀-T_b。以上设计的自主着落控制方法可以使水下清洗机器人与船底之间的俯仰角差、横滚角差和距离差趋于零，且能有效抑制贴合过程中的位置漂移。

图4给出了先进行“角度和定深控制”，后进行“角度和距离控制”的两阶段控制策略下机器人在X-Z与X-Y平面的轨迹图，通过对比图5中角度和距离同时控制算法下的平面轨迹图，可知本发明所提的控制策略能较好地抑制着落过程中的位置漂移。图6(a)和图6(b)分别给出了无重浮心不重合补偿器和有重浮心不重合补偿器下清洗机器人倒立控制的三个相对地面坐标系欧拉角变化曲线。由图6(b)和图7可知，本节所提的控制律能使Δd→0，Δφ→0°，Δψ→0°，Δθ→90°，即实现了以横滚角180°方式稳定着落至船体底面。上述仿真结果验证了本发明所提自主着落船底的控制策略的有效性，实现了以横滚角180°的稳定着落至船体底面。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (4)

1.一种基于测距仪的水下清洗机器人自主倒立着落船底控制方法，其特征在于：包括以下步骤：

步骤1：利用安装于水下清洗机器人底面的测距仪实时测量各个测距仪与船底之间的距离，并根据测距仪的安装参数以及实时测量得到的各个测距仪与船底之间的距离，解算出水下清洗机器人底面相对船底的俯仰角θ、横滚角φ和距离d；根据使机器人自主着落至船底的指令参数，得到待控制的水下清洗机器人底面相对船底的俯仰角差值Δθ＝θ-θ_r、横滚角差值Δφ＝φ-φ_r和距离差值Δd＝d-d_r；

步骤2：考虑水下清洗机器人重浮心不重合而在体坐标系下产生的附加力矩，得到用于补偿附加力矩的推进器推力为：

其中T_b为用于补偿附加力矩的推力向量，G为机器人的重力，和分别为传感器测得的机器人在地面坐标系下的俯仰角和横滚角，(x_c,y_c,z_c)为机器人的重心相对浮心在体坐标系下的偏移量，所述体坐标系以浮心为原点；为分配矩阵C₃的伪逆，分配矩阵C₃由推进器的安装参数确定；

步骤3：对于角度和距离控制，还采用先“角度和定深控制”后“角度和距离控制”两阶段PID控制策略：

当|Δθ|＞β₁或|Δφ|＞β₂时为第一阶段，其中β₁和β₂均为设定的大于零的阈值常数；第一阶段的控制律为：

其中T₀为用于角度和距离控制的推力向量，和分别为水下清洗机器人在地面坐标系下的偏航角和偏航角参考值，Δh＝h-h_d，h为机器人深度传感器的测量值，h_d为期望深度；为分配矩阵C₁的伪逆，分配矩阵C₁由推进器的安装参数确定；

当|Δθ|≤β₁且|Δφ|≤β₂时为第二阶段，第二阶段的控制律为：

其中K_p1，K_i1，K_d1为PID距离控制中的待调参数，K_p2，K_i2，K_d2为PID横滚角控制中的待调参数，K_p3，K_i3，K_d3为PID航向角控制中的待调参数，K_p4，K_i4，K_d4为PID俯仰角控制中的待调参数，K_p5，K_i5，K_d5为PID深度控制中的待调参数；为分配矩阵C₂的伪逆，分配矩阵C₂由推进器的安装参数确定；

步骤4：综合步骤2和步骤3，得到水下清洗机器人控制律T＝T₀-T_b。

2.根据权利要求1所述一种基于测距仪的水下清洗机器人自主倒立着落船底控制方法，其特征在于：安装在水下清洗机器人底面的四个测距仪M₁、M₂、M₃和M₄，测距方向垂直于水下清洗机器人底面，测量的与船底之间的距离分别为d₁、d₂、d₃和d₄；四个测距仪处于矩形的四角，其中测距仪M₁与M₄的连线垂直于水下清洗机器人俯仰轴Z_b；测距仪M₁与M₂的距离为l₄，测距仪M₁与M₄的距离为l₅；得到水下清洗机器人底面相对船底的俯仰角θ、横滚角φ和距离d为：

3.根据权利要求1所述一种基于测距仪的水下清洗机器人自主倒立着落船底控制方法，其特征在于：θ_r＝0，φ_r＝0，d_r为当水下清洗机器人处于贴合船底状态时四个测距仪中心点到船底的距离。

4.根据权利要求1所述一种基于测距仪的水下清洗机器人自主倒立着落船底控制方法，其特征在于：水下清洗机器人共采用六个推进器；其中安装在水下清洗机器人底面有四个推进器T₁、T₂、T₃、T₄，四个推进器分布在矩形四个角上，且推力方向垂直于水下清洗机器人底面，推进器T₁和T₄的连线垂直于水下清洗机器人俯仰轴Z_b；推进器T₁和T₂的距离为l₁，推进器T₁和T₄的距离为l₂；安装在水下清洗机器人侧面有两个推进器T₅和T₆，推进器T₅和T₆的推力方向平行于水下清洗机器人的横滚轴X_b，推进器T₅和T₆的距离为l₃；

分配矩阵C₃为：

分配矩阵C₁为：

分配矩阵C₂为：