一种基于测距仪的水下清洗机器人自主竖立着落船舷控制方法与流程

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

背景技术：

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

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

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

综上所述，水下清洗机器人实现自主着落船舷存在三个难点问题：1、船舷倾角信息无法先验获取；2、如何选取恰当的传感器，并根据传感器量测值构造出合适的着落控制参考量；3、着落过程中水下清洗机器人会产生“漂移”问题。

技术实现要素：

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

本发明的技术方案为：

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

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

步骤2：对于安装在水下清洗机器人底面的推进器，采用了先“角度控制”后“角度和距离同时控制”的两阶段pid控制策略进行控制：

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

其中为安装在水下清洗机器人底面的推进器的推力向量，ndown为安装在水下清洗机器人底面的推进器数量，为分配矩阵c1的伪逆，分配矩阵c1由水下清洗机器人底面推进器的安装参数确定；

当|δθ|≤β1且|δφ|≤β2时为第二阶段，第二阶段的控制律为：

其中为分配矩阵c2的伪逆，kp1，ki1，kd1为pid距离控制中的待调参数，kp2，ki2，kd2为pid横滚角控制中的待调参数，kp4，ki4，kd4为pid俯仰角控制中的待调参数；

步骤3：根据测量的水下清洗机器人航向角ψ和航向角参考值ψd，对安装在水下清洗机器人侧面的推进器进行控制，航向角控制律为：

其中为安装在水下清洗机器人侧面的推进器的推力向量，nside为安装在水下清洗机器人侧面的推进器数量，为分配矩阵c3的伪逆，分配矩阵c3由水下清洗机器人侧面推进器的安装参数确定，kp3，ki3，kd3为pid航向角控制中的待调参数，δψ＝ψ-ψd。

进一步的优选方案，所述一种基于测距仪的水下清洗机器人自主竖立着落船舷控制方法，其特征在于：安装在水下清洗机器人底面的四个测距仪m1、m2、m3和m4，测距方向垂直于水下清洗机器人底面，测量的与船舷之间的距离分别为d1、d2、d3和d4；四个测距仪处于矩形的四角，其中测距仪m1与m4的连线垂直于水下清洗机器人俯仰轴zb；测距仪m1与m2的距离为l4，测距仪m1与m4的距离为l5；得到水下清洗机器人底面相对船舷的俯仰角θ、横滚角φ和距离d为：

。

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

进一步的优选方案，所述一种基于测距仪的水下清洗机器人自主竖立着落船舷控制方法，其特征在于：安装在水下清洗机器人底面有四个推进器t1、t2、t3、t4，四个推进器分布在矩形四个角上，且推力方向垂直于水下清洗机器人底面，推进器t1和t4的连线垂直于水下清洗机器人俯仰轴zb；推进器t1和t2的距离为l1，推进器t1和t4的距离为l2；

第一阶段的控制律为：

为分配矩阵c1的伪逆，

第二阶段的控制律为

为分配矩阵c2的伪逆，

。

进一步的优选方案，所述一种基于测距仪的水下清洗机器人自主竖立着落船舷控制方法，其特征在于：安装在水下清洗机器人侧面有两个推进器t5和t6；推进器t5和t6的推力方向平行于水下清洗机器人的横滚轴xb，推进器t5和t6的距离为l3，航向角控制律为

。

有益效果

本发明通过垂直安装于清洗机器人底部的测距仪对机器人与船舷之间的距离进行测量，并在此基础上，解算出机器人与船舷之间的俯仰角、横滚角和距离差，从而解决了自主着落控制中参考量难以获取以及船舷倾角信息无法先验获取的问题。

另外，本发明提出先“角度控制”后“角度和距离同时控制”的两阶段控制策略，可以解决在贴合过程中当角度差与距离差同时控制时导致的水下清洗机器人位置“漂移”问题。

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

附图说明

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

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

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

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

附图4为先“角度控制”后“角度和距离同时控制”的两阶段控制策略下水下清洗机器人的x-z与x-y平面的轨迹图。

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

附图6为水下清洗机器人x方向位置与三个相对地面坐标系的欧拉角变化曲线。

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

具体实施方式

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

本实施例中水下清洗机器人的初始位置和欧拉角分别为：即x0＝-1m，y0＝1m，z0＝-1m，初始速度和角速度均为零，dr＝3,仿真步长ts＝0.1s，l1＝0.574m，l2＝0.454m，l3＝0.454m，控制参数为：kp1＝-0.18，kp2＝-1，kp3＝-1，kp4＝-1，ki1＝0，ki2＝0，ki3＝0，ki4＝0，kd1＝0，kd2＝0，kd3＝0，kd4＝0。

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

图2给出了四个测距仪均可测距的工作示意图，其中，m1、m2、m3和m4分别为布置在水下清洗机器人底部的1号、2号、3号和4号超声波测距仪器；这四个测距仪在水下清洗机器人底部呈矩形分布，其中l4和l5分别为矩形的边长，具体地l4为m1与m2(m3与m4)之间的距离，l5为m1与m4(m2与m3)之间的距离。d1、d2、d3和d4分别为1号、2号、3号和4号超声波测距仪器沿yb方向到船舷的测量距离；n1、n2、n3和n4分别为四个测距仪在沿yb方向在船舷上的投影。

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

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

根据图3所示的六个推进器分布示意图，可知在推进器推力t1、t2、t3、t4、t5和t6作用下的水下清洗机器人受到的力fy和力矩mx、my、mz分别为：

其中，t1、t2、t3、t4、t5和t6分别为1至6号推进器的推力值；fy、mx、my和mz分别为在六个推进器作用下的y方向推力，x方向力矩，y方向力矩，z方向力矩；l1、l2、l3如图3所示。

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

对于安装在水下清洗机器人底面的推进器，采用了先“角度控制”后“角度和距离同时控制”的两阶段pid控制策略进行控制：

第一阶段，当|δθ|＞β1或|δφ|＞β2时，β1和β2均为大于零的小常数，其控制律设计为：

其中，为分配矩阵c1的伪逆；c1的定义为：

第二阶段，当|δθ|≤β1且|δφ|≤β2时，其控制律设计为：

其中，kp1，kp2，kp4，ki1，ki2，ki4，kd1，kd2和kd4为pid控制中的待调参数，为分配矩阵c2的伪逆；c2的定义为：

水下清洗机器人在着落过程中会受洋流等外界力矩的干扰，会造成机器人航向角不稳定的问题；因此，在着落船舷过程中需要对航向角进行实时控制，以防止因航向角不稳定对贴合效果造成的不利影响。水下清洗机器人的航向角控制律设计为：

其中，kp3，ki3，kd3为待调的pid参数，δψ＝ψ-ψd，ψ和ψd分别为水下清洗机器人的航向角和航向角参考值。

以上设计的自主着落控制方法可以使水下清洗机器人与船舷之间的俯仰角差、横滚角差和距离差趋于零，且能有效抑制贴合过程中的位置漂移。

图4给出了先“角度控制”后“角度和距离同时控制”的两阶段控制策略下机器人在x-z与x-y平面的轨迹图，通过对比图5中角度和距离同时控制算法下的平面轨迹图，可知本发明所提的控制策略能较好地抑制着落过程中的位置漂移。由图6和7可知，本节所提的控制律能使x→3，φ→0°，ψ→0°，θ→90°，即实现了以俯仰角90°方式着落至船舷。上述仿真结果验证了本发明所提自主着落船舷的控制策略的有效性，实现了以俯仰角90°的稳定贴合至船舷。

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