CN108082422A - 水下机器人运动控制装置 - Google Patents

Abstract

本发明涉及一种水下机器人运动控制装置，包括：机身、位置控制器、姿态控制器、主推进器、垂推进器和侧推进器；主推进器、垂推进器和侧推进器均设置于机身上，主推进器的推进方向朝向机身的第一方向，侧推进器的推进方向朝向机身的第二方向，垂推进器的推进方向朝向机身的第三方向，第一方向、第二方向和第三方向两两相互垂直；位置控制器用于控制主推进器、垂推进器和侧推进器工作；姿态控制器用于控制垂推进器和侧推进器工作。主推进器、垂推进器和侧推进器可以分别驱动机身在三个方向上运动，通过位置控制器和姿态控制器控制垂推进器和侧推进器工作，以实现对机身的调整，使得水下机器人的运动控制更为精确，有效提高水下机器人的作业效率。

Description

水下机器人运动控制装置

技术领域

本发明涉及水下检测机器人控制技术领域，特别是涉及水下机器人运动控制装置。

背景技术

水下机器人也称无人遥控潜水器，是一种工作于水下的极限作业机器人。水下环境恶劣危险，人的潜水深度有限，所以水下机器人已成为进行水下作业的重要工具。

传统的水下机器人运动的方向控制较为不准确，造成水下机器人作业效率较低。

发明内容

基于此，有必要提供一种水下机器人运动控制装置。

一种水下机器人运动控制装置，包括：机身、位置控制器、姿态控制器、主推进器、垂推进器和侧推进器，所述位置控制器分别与所述主推进器、所述垂推进器和所述侧推进器连接，且所述姿态控制器分别与所述主推进器、所述垂推进器和所述侧推进器连接；

所述主推进器、所述垂推进器和所述侧推进器均设置于所述机身上，且所述主推进器的推进方向朝向所述机身的第一方向，所述侧推进器的推进方向朝向所述机身的第二方向，所述垂推进器的推进方向朝向所述机身的第三方向，其中，所述第一方向、所述第二方向和所述第三方向两两相互垂直；

所述位置控制器用于控制所述主推进器、所述垂推进器和所述侧推进器工作，以调整所述机身的位置；

所述姿态控制器用于控制所述垂推进器和所述侧推进器工作，以调整所述机身的姿态。

在其中一个实施例中，还包括角速度传感器和航向角控制器，所述角速度传感器与所述航向角控制器连接，所述航向角控制器与所述侧推进器连接，所述航向角控制器用于根据航向角度和所述角速度传感器测量的角速度数据控制所述侧推进器工作。

在其中一个实施例中，所述位置控制器包括第一方向控制器、第二方向控制器和第三方向控制器，所述第一方向控制器与所述主推进器连接，所述第二方向控制器与所述侧推进器连接，所述第三方向控制器与所述垂推进器连接。

在其中一个实施例中，还包括第一方向测距声呐，所述第一方向测距声呐与所述第一方向控制器连接，所述第一方向控制器用于根据第一预设距离和所述第一方向测距声呐测量的距离数据控制所述主推进器工作。

在其中一个实施例中，还包括左侧测距声呐和右侧测距声呐，所述左侧测距声呐和所述右侧测距声呐均与所述第二方向控制器连接，所述第二方向控制器用于根据左侧预设距离、右侧预设距离、所述左侧测距声呐测量的左侧距离数据和所述右侧测距声呐测量的右侧距离数据控制所述侧推进器工作。

在其中一个实施例中，还包括深度计，所述深度计和所述第三方向控制器连接，所述第三方向控制器用于根据预设深度数据和所述深度计测量的深度数据控制所述垂推进器工作。

在其中一个实施例中，还包括底部测距声呐，所述底部测距声呐和所述第三方向控制器连接，所述第三方向控制器用于根据预设高度和所述底部测距声呐测量的高度数据控制所述垂推进器工作。

在其中一个实施例中，所述主推进器包括第一推进器、第二推进器和第三推进器，所述第一推进器、所述第二推进器和所述第三推进器均设置于所述机身的一端，且所述第一推进器、所述第二推进器和所述第三推进器的推进方向朝向所述机身的第一方向。

在其中一个实施例中，所述侧推进器包括第四推进器和第五推进器，所述第四推进器设置于所述机身靠近所述机身的第一端的位置，所述第五推进器设置于所述机身靠近所述机身的第二端的位置。

在其中一个实施例中，所述垂推进器包括第六推进器和第七推进器，所述第六推进器设置于所述机身靠近所述机身的一侧的位置，所述第七推进器设置于所述机身靠近所述机身的另一侧的位置。

上述水下机器人运动控制装置，主推进器、垂推进器和侧推进器可以分别驱动机身在三个方向上运动，通过位置控制器控制主推进器、垂推进器和侧推进器工作，以实现对机身的位置的调整，通过姿态控制器控制垂推进器和侧推进器工作，以实现对机身的姿态的调整，使得水下机器人的运动控制更为精确，从而有效提高水下机器人的作业效率。

附图说明

图1为一个实施例的水下机器人运动控制装置的立体结构示意图；

图2为一个实施例的水下机器人运动控制装置的模块连接框图；

图3为一个实施例的水下机器人运动控制装置的各推进器的推进方向示意图；

图4为一个实施例的水下机器人反馈控制的总体控制框图；

图5为一个实施例的水下机器人运动控制装置的各控制模式的切换示意图；

图6为一个实施例的水下机器人运动控制装置的航向控制的原理示意图；

图7为一个实施例的水下机器人运动控制装置的X轴运动控制的原理示意图；

图8为一个实施例的水下机器人运动控制装置的Y轴运动控制的原理示意图；

图9为一个实施例的水下机器人运动控制装置的Z轴运动控制的原理示意图。

具体实施方式

为了便于理解本发明，下面将参照相关附图对本发明进行更全面的描述。附图中给出了本发明的较佳实施方式。但是，本发明可以以许多不同的形式来实现，并不限于本文所描述的实施方式。相反地，提供这些实施方式的目的是使对本发明的公开内容理解的更加透彻全面。

除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中所使用的术语只是为了描述具体的实施方式的目的，不是旨在于限制本发明。本文所使用的术语“及/或”包括一个或多个相关的所列项目的任意的和所有的组合。

例如，一种水下机器人运动控制装置，包括：机身、位置控制器、姿态控制器、主推进器、垂推进器和侧推进器，所述位置控制器分别与所述主推进器、所述垂推进器和所述侧推进器连接，且所述姿态控制器分别与所述主推进器、所述垂推进器和所述侧推进器连接；所述主推进器、所述垂推进器和所述侧推进器均设置于所述机身上，且所述主推进器的推进方向朝向所述机身的第一方向，所述侧推进器的推进方向朝向所述机身的第二方向，所述垂推进器的推进方向朝向所述机身的第三方向，其中，所述第一方向、所述第二方向和所述第三方向两两相互垂直；所述位置控制器用于控制所述主推进器、所述垂推进器和所述侧推进器工作，以调整所述机身的位置；所述姿态控制器用于控制所述垂推进器和所述侧推进器工作，以调整所述机身的姿态。

上述实施例中，主推进器、垂推进器和侧推进器可以分别驱动机身在三个方向上运动，通过位置控制器控制主推进器、垂推进器和侧推进器工作，以实现对机身的位置的调整，通过姿态控制器控制垂推进器和侧推进器工作，以实现对机身的姿态的调整，使得水下机器人的运动控制更为精确，从而有效提高水下机器人的作业效率。

在一个实施例中，如图1和图2所示，提供一种水下机器人运动控制装置10，包括：机身100、位置控制器140、姿态控制器150、主推进器110、垂推进器130和侧推进器120，所述位置控制器140分别与所述主推进器110、所述垂推进器130和所述侧推进器120连接，且所述姿态控制器150分别与所述主推进器110、所述垂推进器130和所述侧推进器120连接；所述主推进器110、所述垂推进器130和所述侧推进器120均设置于所述机身100上，且所述主推进器110的推进方向朝向所述机身100的第一方向，所述侧推进器120的推进方向朝向所述机身100的第二方向，所述垂推进器130的推进方向朝向所述机身100的第三方向，其中，所述第一方向、所述第二方向和所述第三方向两两相互垂直；所述位置控制器140用于控制所述主推进器110、所述垂推进器130和所述侧推进器120工作，以调整所述机身100的位置；所述姿态控制器150用于控制所述垂推进器130和所述侧推进器120工作，以调整所述机身100的姿态。

具体地，该机身为水下机器人的机身，本实施例中，位置控制器和姿态控制器均设置于机身内。所述位置控制器分别与所述主推进器、所述垂推进器和所述侧推进器电连接，且所述姿态控制器分别与所述主推进器、所述垂推进器和所述侧推进器电连接。该主推进器、垂推进器和侧推进器均用于驱动该机身运动，例如，主推进器用于驱动机身沿第一方向运动，垂推进器用于驱动机身沿第二方向运动，侧推进器用于驱动机身沿第三方向运动，例如，该第一方向为X轴方向，第二方向为Y轴方向，第三方向为Z轴方向。这样，通过主推进器、垂推进器和侧推进器的驱动，使得机身能够在三个维度上进行运动。可以理解，第一方向、第二方向与第三方向可以根据实际情况灵活地进行设计或者修改。

值得一提的是，主推进器、垂推进器和侧推进器均为涡轮推进器，也可以是其他现有技术可以实现对机身驱动的推进器。例如，主推进器、垂推进器和侧推进器均具有一涡轮口，该涡轮口用于在涡轮作用下喷射流体，以实现对机身的驱动。例如，所述主推进器的涡轮口的方向朝向所述机身的第一方向，所述侧推进器的涡轮口的方向朝向所述机身的第二方向，所述垂推进器的涡轮口的方向朝向所述机身的第三方向。

应该理解的是，第一方向、第二方向和第三方向均包括两个相反的方向，也就是说，第一方向的两个相反的方向为X轴的正方向和负方向，第二方向的两个相反的方向为Y轴的正方向和负方向，第三方向的两个相反的方向为Z轴的正方向和负方向，由于主推进器、垂推进器和侧推进器的涡轮分别可以工作在不同转速上，并且可以采用相反的转向进行工作，因此，在第一方向上，机身可以沿第一方向的两个相反的方向运动，在第二方向上，机身可以沿第二方向的两个相反的方向运动，在第三方向上，机身可以沿第三方向的两个相反的方向运动，这样，通过主推进器、垂推进器和侧推进器的驱动，使得机身可以实现六个自由度的运动。

此外，本实施例中，水下机器人运动控制装置对水下机器人的运动的控制为自动控制，也就是在水下机器人的与远程通信的通信光纤断开后，水下机器人运动控制装置对水下机器人进行自动控制。

本实施例中，对机身的运动分为外环控制回路和内环控制回路，外环控制回路由位置控制器执行，具体地，该位置控制器用于执行外环控制，例如，该位置控制器用于根据预设轨迹分别控制主推进器、垂推进器和侧推进器工作，以驱动机身分别在第一方向、第二方向和第三方向上运动，以实现机身的位置的调整，该预设轨迹也可称为期望轨迹。内环控制回路由姿态控制器执行，具体地，该姿态控制器用于执行内环控制，例如，该姿态控制器用于根据预设姿态控制所述垂推进器和所述侧推进器工作，以实现姿态的调整。

在一个实施例中，所述位置控制器包括第一方向控制器、第二方向控制器和第三方向控制器，所述第一方向控制器与所述主推进器连接，所述第二方向控制器与所述侧推进器连接，所述第三方向控制器与所述垂推进器连接。所述第一方向控制器用于控制所述主推进器工作，以驱动机身沿第一方向运动，所述第二方向控制器用于控制所述侧推进器工作，以驱动机身沿第二方向运动，所述第三方向控制器用于控制所述垂推进器工作，以驱动机身沿第三方向运动，例如，所述第一方向控制器用于根据预设轨迹控制所述主推进器工作，以驱动机身沿第一方向运动，所述第二方向控制器用于根据预设轨迹控制所述侧推进器工作，以驱动机身沿第二方向运动，所述第三方向控制器用于根据预设轨迹控制所述垂推进器工作，以驱动机身沿第三方向运动。

为了使得水下机器人能够沿第一方向运动，在一个实施例中，请结合图1和图3，所述主推进器110包括第一推进器T1、第二推进器T2和第三推进器T3，所述位置控制器分别与所述第一推进器T1、所述第二推进器T2和所述第三推进器T3连接，所述姿态控制器分别与所述第一推进器T1、所述第二推进器T2和所述第三推进器T3连接，所述第一推进器T1、所述第二推进器T2和所述第三推进器T3均设置于所述机身的一端，且所述第一推进器T1、所述第二推进器T2和所述第三推进器T3的推进方向朝向所述机身的第一方向。即该第一推进器T1、第二推进器T2和第三推进器T3用于驱动机身在第一方向上运动，例如，第一推进器T1、第二推进器T2和第三推进器T3的推进方向相同，例如，第一推进器T1、第二推进器T2和第三推进器T3用于驱动机身沿第一方向运动，第一推进器T1、第二推进器T2和第三推进器T3用于驱动机身沿X轴方向运动，对于机身而言，第一方向为机身的前端和机身的后端的连线的方向，本实施例中，将机身的前端朝向的方向定义为前方，将机身的后端朝向的方向定义为后方，例如，第一推进器T1、第二推进器T2和第三推进器T3均设置于所述机身的后端，例如，当第一推进器T1、第二推进器T2和第三推进器T3正向转动时，驱动机身朝向机身的前方运动，当第一推进器T1、第二推进器T2和第三推进器T3反向转动时，驱动机身朝向机身的后方运动。本实施例中，通过三个推进器对水下机器人进行驱动推进，使得水下机器人运动更为灵活。

例如，第一推进器、第二推进器和第三推进器呈品字形设置，例如，第一推进器、第二推进器和第三推进器的中心连线呈等边三角形设置，品字形设置的第一推进器、第二推进器和第三推进器使得机身在第一方向上运动时，受到更为均衡的驱动力，有利于使得该机身运动更为平衡，且运动方向更为精确。

为了使得该水下机器人运动更为灵活，例如，第一推进器的推进方向、第二推进器的推进方向和第三推进器的推进方向相互倾斜设置，例如，第一推进器的推进方向、第二推进器的推进方向和第三推进器的推进方向分别倾斜于第一方向，例如，第一推进器的推进方向、第二推进器的推进方向和第三推进器的推进方向分别倾斜于X轴，例如，第一推进器的推进方向、第二推进器的推进方向和第三推进器的推进方向由机身内侧朝向机身外侧倾斜，且第一推进器的推进方向、第二推进器的推进方向和第三推进器的推进方向与X轴之间的夹角均相等，由于第一推进器、第二推进器和第三推进器倾斜于X轴，这样，第一推进器、第二推进器和第三推进器的推进量可以在Y轴或者Z轴产生分力，这样，不仅可以驱动水下机器人沿X轴运动，还可以驱动水下机器人沿Y轴或者Z轴运动，此外，第一推进器、第二推进器和第三推进器的推进量在Y轴或者Z轴上产生的分力，能够使得机身产生倾斜，进而实现对机身的姿态的调整，从而使得对水下机器人的控制更为灵活。

为了使得水下机器人在第一方向上的运动更为精确，在一个实施例中，水下机器人运动控制装置还包括第一方向测距声呐，所述第一方向测距声呐与所述第一方向控制器连接，所述第一方向控制器用于根据第一预设距离和所述第一方向测距声呐测量的距离数据控制所述主推进器工作。例如，所述第一方向控制器用于根据第一预设距离和所述第一方向测距声呐测量的距离数据控制所述第一推进器、所述第二推进器和所述第三推进器工作。

例如，该第一方向测距声呐用于测量机身与第一方向上的物体之间的距离获得距离数据，例如，该第一方向测距声呐用于测量机身前方的物体与机身之间的距离获得距离数据，具体地，该第一方向测距声呐用于测量机身前方的障碍物的距离，从而获得距离数据。例如，该第一预设距离为控制指令携带的，例如，所述第一方向控制器用于根据控制指令中的第一预设距离和所述第一方向测距声呐测量的距离数据控制所述主推进器工作。具体地，该第一预设距离为预设的距离，即预设的与前方障碍物的距离。

具体地，第一方向控制器为前向定距PID(proportion、integral、derivative，比例、积分、导数)控制器，第一方向控制器在前向定距模式下，根据第一预设距离和第一方向测距声呐测量的距离数据控制所述第一推进器、所述第二推进器和所述第三推进器工作，例如，第一方向控制器控制所述第一推进器、所述第二推进器和所述第三推进器工作，驱动机身向前方运动，当第一方向测距声呐测量到前方的物体的距离数据达到第一预设距离时，第一方向控制器控制所述第一推进器、所述第二推进器和所述第三推进器停止工作，使得机身能够与前方的物体或者障碍物之间的距离保持在第一预设距离，从而实现前方的定距。通过该第一方向测距声呐的反馈，有效避免水下机器人撞击障碍物，使得水下机器人运动更为平稳安全，且依据前方的物体作为参考控制水下机器人运动，使得水下机器人在第一方向上的运动更为精确。

为了使得准确控制水下机器人的航向，在一个实施例中，水下机器人运动控制装置还包括角速度传感器和航向角控制器，所述角速度传感器与所述航向角控制器连接，所述航向角控制器与所述侧推进器连接，所述航向角控制器用于根据航向角度和所述角速度传感器测量的角速度数据控制所述侧推进器工作。

例如，该角速度传感器为光纤陀螺，该角速度传感器设置于机身上，该角速度传感器用于测量所述机身的角速度数据，该角速度数据即为机身转动的角速度的大小，例如，该角速度数据为机身在第一方向和第二方向所在平面上的转动的速度。航向角度由控制指令决定，例如，该控制指令携带航向角度，该控制指令携带Rotate(旋转)轴的控制量，该Rotate轴的控制量即为Rotate轴转动量，相当于航向改变的角度。

航向角控制器用于根据控制指令中携带的Rotate轴的控制量控制侧推进器工作，并根据角速度传感器测量的角速度数据调整侧推进器的工作。具体地，航向控制器根据Rotate轴的控制量计算获得期望角速度值，该期望角速度值预设的角速度，航向角控制器根据该期望角速度值控制侧推进器工作，使得机身产生转动，而角速度传感器实时测量机身的角速度数据，航向角控制器根据角速度传感器测量反馈的角速度数据对侧推进器的工作进行调整，使得机身的转动更为准确，减小误差。

具体地，该航向角控制器为航向角度PID控制器，航向角控制器的控制为闭环控制，该航向角度控制器根据期望角速度值与角速度传感器测量的角速度数据进行反馈控制，将Rotate轴的控制量转换为侧推进器的控制量，从而实现机身的旋转运动，例如，左旋或者右旋，其中，侧推进器的控制量的大小与期望角速度值以及实际角速度值(角速度传感器测量的角速度数据)的偏差正相关。应该理解的是，本实施例中，航向角控制器对侧推进器的工作的控制，不仅实现航向的调整，还实现了姿态的调整。

例如，所述姿态控制器包括航向角度控制器，即水下机器人运动控制装置包括多个姿态控制器，该航向角度控制器为姿态控制器中的其中一个控制器。

为了控制该水下机器人的转动，调整航向角度，并使得水下机器人能够在Y轴方向上运动，在一个实施例中，请参见图1和图3，所述侧推进器120包括第四推进器T4和第五推进器T5，所述位置控制器分别与所述第四推进器T4和所述第五推进器T5连接，所述姿态控制器分别与所述第四推进器T4和所述第五推进器T5连接，所述第四推进器T4设置于所述机身靠近所述机身的第一端的位置，所述第五推进器T5设置于所述机身靠近所述机身的第二端的位置。例如，所述第四推进器T4的推进方向和第五推进器T5的推进方向相同。例如，所述第四推进器T4的推进方向和第五推进器T5的推进方向分别沿第二方向设置，例如，所述第四推进器T4的推进方向和第五推进器T5的推进方向分别朝向机身的一侧。

具体地，第四推进器T4设置于靠近机身的前端的位置，第五推进器T5设置于靠近机身的后端的位置，且第四推进器T4和第五推进器T5的推进方向相同，比如，第四推进器T4和第五推进器T5的推进方向均朝向机身的左侧，例如，第四推进器T4和第五推进器T5的推进方向朝向Y轴方向，这样，当在位置控制器或者姿态控制器的控制下，第四推进器T4和第五推进器T5具有相同的推进量，由于两者的推进方向相同，使得机身沿Y轴方向运动，实现机身的横移，例如，当第四推进器T4和第五推进器T5朝向机身左侧具有相同的推进量时，机身向右侧运动，而当第四推进器T4和第五推进器T5同时反向推进，且具有相同的推进量时，机身向左侧运动。当在姿态控制器的控制下，第四推进器T4和第五推进器T5具有不同的推进量，比如，当第四推进器T4的推进量大于第五推进器T5的推进量时，或者第四推进器T4的推进方向与第五推进器T5的推进方向相反，且推进量不同时，机身将以自身的中心为轴旋转，这样，即可使得该机身的产生旋转，改变航向角度，从而实现了对水下机器人的转动的控制以及航向角度的控制。

值得一提的是，在本文中的各实施例中，推进器的推进量可以理解为涡轮的推进量或者涡轮的转速，涡轮的转速越大，推进量越大，涡轮的转速越小，则推进量越小。

为了进一步使得水下机器人运动更为精确，在一个实施例中，水下机器人运动控制装置还包括左侧测距声呐和右侧测距声呐，所述左侧测距声呐和所述右侧测距声呐均与所述第二方向控制器连接，所述第二方向控制器用于根据左侧预设距离、右侧预设距离、所述左侧测距声呐测量的左侧距离数据和所述右侧测距声呐测量的右侧距离数据控制所述侧推进器工作。例如，所述第二方向控制器用于根据左侧预设距离、右侧预设距离、所述左侧测距声呐测量的左侧距离数据和所述右侧测距声呐测量的右侧距离数据控制所述第四推进器和所述第五推进器工作。

例如，所述左侧测距声呐用于测量机身与左侧的障碍物之间的距离，获得左侧距离数据，例如，所述右侧测距声呐用于测量机身与右侧的障碍物之间的距离，获得右侧距离数据。左侧预设距离和右侧预设距离为控制指令携带的，左侧预设距离和右侧预设距离为预设的机身与两侧的距离，例如，左侧预设距离为机身与左侧的物体之间预设距离，例如，右侧预设距离为机身与右侧的物体之间预设距离。

例如，该第二方向控制器包括左侧定距PID控制器和右侧定距PID控制器，其中，左侧定距PID控制器用于根据左侧预设距离和所述左侧测距声呐测量的左侧距离数据控制所述第四推进器和所述第五推进器工作，右侧定距PID控制器用于根据右侧预设距离和所述右侧测距声呐测量的右侧距离数据控制所述第四推进器和所述第五推进器工作。这样，第二方向控制器通过左侧测距声呐和右侧测距声呐测量的距离数据，控制机身运动，使得机身与左侧以及右侧的物体之间的距离维持在预设距离，例如，在左侧定距模式下，左侧定距PID控制器根据左侧预设距离与所述左侧测距声呐测量的左侧距离数据控制第四推进器和第五推进器工作，比如，控制第四推进器和第五推进器工作以使得机身沿Y轴运动，并朝向机身的左侧运动，当左侧测距声呐测量到左侧的物体与机身之间的距离为左侧预设距离时，左侧定距PID控制器控制第四推进器和第五推进器工作，使得机身与左侧的物体保持左侧预设距离，进而避免机身撞击左侧的物体，使得水下机器人运动更为精确。

为了使得水下机器人能够在竖直方向上运动，也就是在Z轴上运动，在一个实施例中，请再次结合图1和图3，所述垂推进器130包括第六推进器T6和第七推进器T7，所述位置控制器分别与所述第六推进器T6和所述第七推进器T7连接，所述姿态控制器分别与所述第六推进器T6和所述第七推进器T7连接，所述第六推进器T6设置于所述机身靠近所述机身的一侧的位置，所述第七推进器T7设置于所述机身靠近所述机身的另一侧的位置。例如，所述第六推进器T6和所述第七推进器T7的推进方向朝向所述机身的第三方向，例如，所述第六推进器T6设置于所述机身靠近左侧的位置，所述第七推进器T7设置于所述机身靠近右侧的位置，例如，所述第六推进器T6和所述第七推进器T7的推进方向朝向机身的下方。也就是说，第六推进器T6和第七推进器T7分别设置于机身的两侧，且推进方向分别朝向机身的下方，这样，通过控制第六推进器T6和第七推进器T7工作，即可实现机身在竖直方向上的运动。此外，由于第六推进器T6和第七推进器T7分别设置于机身的两侧，这样，通过控制第六推进器T6和第七推进器T7的推进量，能够使得机身保持平衡，并且能够调整姿态。例如，当第六推进器T6和第七推进器T7的推进量相同时，能够使得机身两侧受到相同的驱动力，进而使得机身保持平衡，例如，当第六推进器T6和第七推进器T7的推进量不同时，能够使得机身的一侧向另一侧倾斜，实现机身的姿态的调整。

为了使得水下机器人在竖直方向上的运动更为精确，在一个实施例中，水下机器人运动控制装置还包括深度计，所述深度计和所述第三方向控制器连接，所述第三方向控制器用于根据预设深度数据和所述深度计测量的深度数据控制所述垂推进器工作。

例如，该深度计用于测量机身与水面之间的距离，获得深度数据，具体地，机身与水面之间的距离也就是该机身所处的深度，该预设深度数据为控制指令中携带的，例如，第三方向控制器包括定深控制PID控制器，该定深控制PID控制器用于根据控制指令携带的预设深度数据和所述深度计测量的深度数据控制所述垂推进器工作，例如，该定深控制PID控制器用于根据控制指令携带的预设深度数据和所述深度计测量的深度数据控制第六推进器T6和第七推进器T7工作，这样，在定深模式下，第三方向控制器控制第六推进器T6和第七推进器T7工作，使得机身上浮或者下潜，第三方向控制器根据该深度计测量的深度数据能够实时获取机身所处的深度，使得机身能够保持在预设深度数据，使得水下机器人在竖直方向上的位置更为准确，进而使得水下机器人在竖直方向上的运动更为精确。

为了使得水下机器人在竖直方向上的运动更为精确，在一个实施例中，水下机器人运动控制装置还包括底部测距声呐，所述底部测距声呐和所述第三方向控制器连接，所述第三方向控制器用于根据预设高度和所述底部测距声呐测量的高度数据控制所述垂推进器工作。

例如，该底部测距声呐用于测量机身与水底之间的距离，获得高度数据，该高度数据即为机身相对于水底的高度，具体地，第三方向控制器包括定高控制PID控制器，该预设高度为控制指令中携带的，例如，.该定高控制PID控制器用于根据控制指令携带的预设高度和所述底部测距声呐测量的高度数据控制所述垂推进器工作，例如，该定高控制PID控制器用于根据控制指令携带的预设高度和所述底部测距声呐测量的高度数据控制第六推进器T6和第七推进器T7工作，这样，在定高模式下，第三方向控制器控制第六推进器T6和第七推进器T7工作，使得机身上浮或者下潜，第三方向控制器根据该底部测距声呐测量的高度数据能够实时获取机身所处的高度，使得机身能够保持在预设高度，使得水下机器人在竖直方向上的位置更为准确，进而使得水下机器人在竖直方向上的运动更为精确。

为了更好地控制水下机器人的姿态，例如，所述第六推进器T6的推进方向倾斜于所述第三方向。例如，所述第七推进器T7的推进方向倾斜于所述第三方向。例如，所述第六推进器T6的推进方向朝向所述机身的一侧倾斜，所述第七推进器T7的推进方向朝向所述机身的另一侧倾斜。

具体地，该第六推进器T6的推进方向由机身内侧朝向机身外侧倾斜，该第七推进器T7的推进方向由机身内侧朝向机身外侧倾斜，由于第六推进器T6和第七推进器T7分别设置于机身的两侧，且第六推进器T6和第七推进器T7的推进方向分别朝向外侧倾斜，使得第六推进器T6和第七推进器T7在推进时，不仅在竖直方向上产生驱动力，还在水平方向上产生驱动力，这样，通过控制第六推进器T6和第七推进器T7的推进量，使得该水下机器人不仅能够在竖直方向上运动，还能产生相对于水平方向上倾斜，例如，该第六推进器T6和第七推进器T7的推进量在Y轴上的分力，能够使得机身产生倾斜，进而实现对机身的姿态的调整。

例如，所述第六推进器T6的推进方向与所述第三方向之间的夹角为10°～15°。例如，所述第六推进器T6的推进方向与所述第三方向之间的夹角为12°。例如，所述第七推进器T7的推进方向与所述第三方向之间的夹角为10°～15°。例如，所述第七推进器T7的推进方向与所述第三方向之间的夹角为12°。例如，所述第六推进器T6的推进方向与所述第七推进器T7的推进方向之间的夹角为20°～30°，例如，所述第六推进器T6的推进方向与所述第七推进器T7的推进方向之间的夹角为24°，也就是说，所述第六推进器T6的推进方向与所述Y轴方向之间的夹角为102°，所述第七推进器T7的推进方向与所述Y轴方向之间的夹角为102°。

值得一提的是，第六推进器T6和第七推进器T7的推进方向关于机身的中心轴线对称设置，第六推进器T6和第七推进器T7的推进方向朝外倾斜的角度相同，这样，能够使得机身能够在第六推进器T6和第七推进器T7的驱动下保持平衡。值得一提的是，第六推进器T6和第七推进器T7的推进方向的倾斜角度太大，将导致第六推进器T6和第七推进器T7在竖直方向上的推进分量较小，使得水下机器人在竖直方向上运动较为不灵活，第六推进器T6和第七推进器T7的推进方向的倾斜角度太小，将导致机身不容易产生倾斜，不利于机身的姿态调整，本实施例中，由于第六推进器T6和第七推进器T7的推进方向的倾斜角度为12°，使得第六推进器T6和第七推进器T7的在水平方向上的推进分量不会太大也不会太小，能够使得水下机器人在竖直方向上灵活运动，并且，使得机身的姿态调整更为灵活。

下面是一个具体地实施例：

本实施例中，控制器所对应的控制对象和7台推进器的布局如下图3所示：7台推进器可分成3组：主推、垂推和侧推，T1/T2/T3为主推，T4/T5为侧推，T6/T7为垂推，图中箭头为各推进器的推进方向。通过控制器7个推进器的转速来控制水下机器人的六自由度运动。

根据水电厂实际工作条件，对水下机器人进行合理化假设，实现动力学模型在水平和垂直两个平面内的解耦，并分别针对水平和垂直两个平面进行X/Y/Z轴运动控制器设计。运动控制器包括内外两个控制环，其中外环控制回路为位置控制环，根据机器人实际位置与期望位置间的偏差进行反馈控制，内回路为姿态增稳控制环，通过姿态偏差量进行负反馈控制，实现水下机器人姿态增稳控制，总体控制框图如图4所示。

各控制器间的各个控制模式之间的切换如图5所示，缆控状态包括Romate一个纯手动的运动控制，以及6个半自动控制模式：自定定向控制、自动定深控制、自定定高控制、前/左/右侧定距控制。各控制模式间的相互切换情况如图所示：

各组运动控制器的设计如下：

(1)航向控制器设计：主要是完成航向控制和自动定向控制两大功能，控制框图如图6所示，根据控制指令，在Remote控制模式下，根据控制指令中Rotate轴的控制量，转换为相应的期望角速度值，手动航向PID控制器根据角速度期望值与当前光纤陀螺返回的实际角速度数据进行偏差反馈控制，控制量转换为T4、T5推进器的控制量，控制ROV(RemoteOperated Vehicle，水下机器人)左旋、右旋运动，控制量的大小与期望角速度值和实际角速度值的偏差正相关。

在自动定向控制模式下，控制器输出量由角速度控制环和角度控制环两个控制环给出。角速度控制环期望的角速度值为零，即保持航向值的角速度稳定，控制量转换为T4、T5推进器的控制量，控制ROV稳定在定向控制点。角度控制环根据航向角的设定值与当前航向实际角度值数据进行偏差反馈控制，控制量转换为T4、T5推进器的控制量，控制ROV稳定在当前航向。

(2)X轴运动控制器设计：控制框图如图7所示，X轴控制器是实现ROV在前、后方向上的远程缆控运动控制和前向定距控制功能。在Remote控制模式下，根据控制指令中X轴的控制量，进行控制量转换后得到作用于T1T2T3推进器的控制量，从而进行前后运动控制，运动速度的快慢取决于X轴运动控制量的大小。在前向定距模式下，前向定距PID控制器根据前向定距设定值与前向测距声呐返回的实际距离数据进行偏差反馈控制，控制量转换为T1T2T3推进器的控制量，控制ROV稳定在前向定距控制点，控制量的大小与定距值和设定值偏差正相关。

(3)Y轴运动控制器设计：控制框图如图8所示，Y轴控制器是实现ROV在左移、右移方向上的远程缆控运动控制和左侧定距、右侧定距半自动控制。在Remote控制模式下，根据控制指令中Y轴的控制量，进行控制量转换后得到作用于T4T5推进器的控制量，从而进行左移、右移运动控制，运动速度的快慢取决于Y轴运动控制量的大小。在左侧定距模式下，左侧定距PID控制器根据左侧定距设定值与左侧测距声呐返回的实际距离数据进行偏差反馈控制，控制量转换为T4T5推进器的控制量，控制ROV稳定在左侧定距控制点，控制量的大小与定距值和设定值偏差正相关。在右侧定距模式下，右侧定距PID控制器根据右侧定距设定值与右侧测距声呐返回的实际距离数据进行偏差反馈控制，控制量转换为T4T5推进器的控制量，控制ROV稳定在右侧定距控制点，控制量的大小与定距值和设定值偏差正相关。

(3)Z轴运动控制器设计：控制框图如图9所示，实现ROV在上浮、下潜方向上的远程缆控运动控制和自动定深、自动定高控制。

在Remote控制模式下，根据控制指令中Z轴的控制量，进行控制量转换后得到作为T6、T7推进器的控制量，从而进行上浮、下潜运动控制，运动速度的快慢取决于Z轴运动控制量的大小。

在自动定深控制模式下，自动定深PID控制器根据深度设定值与深度计返回的实际深度数据进行偏差反馈控制，控制量转换为T6、T7推进器的控制量，控制ROV稳定在定深控制点，控制量的大小与深度数值和设定值偏差正相关。

在定高控制模式下，定高控制PID控制器根据定高设定值与底部测距声呐返回的实际距离数据进行偏差反馈控制，控制量转换为T6、T7推进器的控制量，控制ROV稳定在定高控制点，控制量的大小与定高值和设定值偏差正相关。

应该说明的是，上述系统实施例中，所包括的各个模块只是按照功能逻辑进行划分的，但并不局限于上述的划分，只要能够实现相应的功能即可；另外，各程序模块的具体名称也只是为了便于相互区分，并不用于限制本发明的保护范围。

以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

Claims (10)

1.一种水下机器人运动控制装置，其特征在于，包括：机身、位置控制器、姿态控制器、主推进器、垂推进器和侧推进器，所述位置控制器分别与所述主推进器、所述垂推进器和所述侧推进器连接，且所述姿态控制器分别与所述主推进器、所述垂推进器和所述侧推进器连接；

所述主推进器、所述垂推进器和所述侧推进器均设置于所述机身上，且所述主推进器的推进方向朝向所述机身的第一方向，所述侧推进器的推进方向朝向所述机身的第二方向，所述垂推进器的推进方向朝向所述机身的第三方向，其中，所述第一方向、所述第二方向和所述第三方向两两相互垂直；

所述位置控制器用于控制所述主推进器、所述垂推进器和所述侧推进器工作，以调整所述机身的位置；

所述姿态控制器用于控制所述垂推进器和所述侧推进器工作，以调整所述机身的姿态。

2.根据权利要求1所述的水下机器人运动控制装置，其特征在于，还包括角速度传感器和航向角控制器，所述角速度传感器与所述航向角控制器连接，所述航向角控制器与所述侧推进器连接，所述航向角控制器用于根据航向角度和所述角速度传感器测量的角速度数据控制所述侧推进器工作。

3.根据权利要求1所述的水下机器人运动控制装置，其特征在于，所述位置控制器包括第一方向控制器、第二方向控制器和第三方向控制器，所述第一方向控制器与所述主推进器连接，所述第二方向控制器与所述侧推进器连接，所述第三方向控制器与所述垂推进器连接。

4.根据权利要求3所述的水下机器人运动控制装置，其特征在于，还包括第一方向测距声呐，所述第一方向测距声呐与所述第一方向控制器连接，所述第一方向控制器用于根据第一预设距离和所述第一方向测距声呐测量的距离数据控制所述主推进器工作。

5.根据权利要求3所述的水下机器人运动控制装置，其特征在于，还包括左侧测距声呐和右侧测距声呐，所述左侧测距声呐和所述右侧测距声呐均与所述第二方向控制器连接，所述第二方向控制器用于根据左侧预设距离、右侧预设距离、所述左侧测距声呐测量的左侧距离数据和所述右侧测距声呐测量的右侧距离数据控制所述侧推进器工作。

6.根据权利要求3所述的水下机器人运动控制装置，其特征在于，还包括深度计，所述深度计和所述第三方向控制器连接，所述第三方向控制器用于根据预设深度数据和所述深度计测量的深度数据控制所述垂推进器工作。

7.根据权利要求6所述的水下机器人运动控制装置，其特征在于，还包括底部测距声呐，所述底部测距声呐和所述第三方向控制器连接，所述第三方向控制器用于根据预设高度和所述底部测距声呐测量的高度数据控制所述垂推进器工作。

8.根据权利要求1所述的水下机器人运动控制装置，其特征在于，所述主推进器包括第一推进器、第二推进器和第三推进器，所述第一推进器、所述第二推进器和所述第三推进器均设置于所述机身的一端，且所述第一推进器、所述第二推进器和所述第三推进器的推进方向朝向所述机身的第一方向。

9.根据权利要求1所述的水下机器人运动控制装置，其特征在于，所述侧推进器包括第四推进器和第五推进器，所述第四推进器设置于所述机身靠近所述机身的第一端的位置，所述第五推进器设置于所述机身靠近所述机身的第二端的位置。

10.根据权利要求1所述的水下机器人运动控制装置，其特征在于，所述垂推进器包括第六推进器和第七推进器，所述第六推进器设置于所述机身靠近所述机身的一侧的位置，所述第七推进器设置于所述机身靠近所述机身的另一侧的位置。