CN111846166A - 水下机器人运动控制系统及水下机器人 - Google Patents

Abstract

本发明提供一种水下机器人运动控制系统及水下机器人，以解决现有技术中的水下机器人的推进器利用率不高的问题，其中，水下机器人运动控制系统中的第一推进器、第二推进器、第三推进器均配置成输出朝向水下机器人的机身的第一方向的推力，第四推进器和第五推进器均配置成输出朝向所述机身的第三方向的推力，第六推进器配置成输出朝向所述机身的第二方向的推力，其中，第一方向、第二方向和第四方向均两两相互垂直，并且第三方向与第四方向之间具有预设角度；该运动控制系统还包括运动控制装置，用于控制第一推进器、第二推进器、第三推进器、第四推进器、第五推进器和第六推进器各自的推力的输出。

Description

水下机器人运动控制系统及水下机器人

技术领域

本发明属于水下机器人运动控制技术领域，尤其涉及一种水下机器人运动控制系统及水下机器人。

背景技术

在传统水下机器人的推进器布局设计中，由于开放式框架结构、充足的空间以及没有严格的自重限制，推进器通常采用带矢量角度布置。在矢量角度布置上，推进器相对于轴线的倾斜角度较大，一般在30°～45°间。深水水下机器人的推进器推力较大，采用矢量角度布置的方式，能够满足不同轴线方向上的推力输出要求，并符合多自由度方向上的运动控制要求。

在传统水下机器人的控制系统设计中，通常采用闭环反馈控制中经典的PID控制，其为基于反馈概念以减少不确定性的控制方法与策略。目前在工程实际中应用最为广泛的是PID控制器，PID控制器(即比例-积分-微分控制器)由比例单元P、积分单元I和微分单元D组成，其中，PID控制的基础是比例控制；积分控制可消除稳态误差，但可能增加超调；微分控制可加快大惯性系统响应速度以及减弱超调趋势。在水下机器人根据控制指令完成不同的运动时，需要不同的闭环反馈控制以保证其姿态、深度、航向等状态参数的稳定和平衡，包括定深PID控制器，负责稳定保持在特定深度上的闭环控制策略；定向PID控制器，负责水下机器人保持特定航向航行的闭环控制策略；姿态稳定PID控制器则负责维持水下机器人机器姿态平稳的闭环控制。

在传统水下机器人中，为实现全自由度运动控制，多采取开放式框架搭载数量超过全自由度数目的推进器，一般在8个推进器及以上，对于结构开放空间要求较高，并且由于矢量布置的推进器倾斜角度较大(在30～45°之间)，造成在主要自由度轴线方向，比如水平和垂向上的推进器利用效率不高，实现自动平衡控制将导致较高的推进器效率损失的产生。同时，传统水下机器人中采用的定深、定向和姿态稳定PID控制器各自为独立模块，单独工作，由于存在冗余推进器，单独工作的PID控制器所控制的并不是所有的推进器，导致推进器使用效率不高，运动控制相应存在一定延迟。

发明内容

有鉴于此，本发明实施例提供一种水下机器人运动控制系统及水下机器人，以解决现有技术中的水下机器人的推进器利用率不高的问题。

本发明实施例的第一方面提供了一种水下机器人运动控制系统，所述系统包括：第一推进器、第二推进器、第三推进器、第四推进器、第五推进器和第六推进器，所述第一推进器、第二推进器、第三推进器、第四推进器、第五推进器和第六推进器均设置于水下机器人的机身上，其中，所述第一推进器、第二推进器、第三推进器均配置成输出朝向所述机身的第一方向(Z)的推力，所述第四推进器和第五推进器均配置成输出朝向所述机身的第三方向(W)的推力，所述第六推进器配置成输出朝向所述机身的第二方向(Y)的推力，其中，第四方向(X)和所述第一方向(Z)、所述第二方向(Y)均两两相互垂直，并且所述第三方向(W)与所述第四方向(X)之间具有预设角度；所述系统还包括：运动控制装置，用于控制所述第一推进器、第二推进器、第三推进器、第四推进器、第五推进器和第六推进器各自的推力的输出。

可选地，所述第一推进器、第二推进器和第三推进器配置成产生使所述机身沿所述第一方向(Z)确定的轴线运动的第一运动力(Fz)、绕所述第二方向(Y)确定的轴线旋转运动的第二运动力(Ny)以及绕所述第三方向(W)确定的轴线旋转运动的第三运动力(Nx)；所述第四推进器和第五推进器配置成产生使所述机身沿所述第三方向(W)确定的轴线运动的第四运动力(Fx)、绕所述第一方向(Z)确定的轴线旋转运动的第五运动力(Nz)；所述第六推进器配置成与所述第四推进器、第五推进器配合而产生使所述机身沿所述第二方向(Y)确定的轴线运动的第六运动力(Fy)。

可选地，所述第一推进器、第二推进器、第三推进器、第四推进器、第五推进器、第六推进器各自的推力与所述第一运动力、第二运动力、第三运动力、第四运动力、第五运动力、第六运动力满足以下关系：

Fx＝F4*cos(α)+F5*cos(α)

Fz＝F1+F2+F3

Fy＝F6+F4*sin(α)+F5*sin(α)

Ny＝F1*S2+F2*S2+(-F3*S3)

Nx＝F1*S1+(-F2*S1)

Nz＝(-F4*sin(α)*S6)+F5*sin(α)*S6+(-F4*cos(α)*S5)+F5*cos(α)*S5+(-F6*S4)

其中，F1为所述第一推进器的推力，F2为所述第二推进器的推力，F3为所述第三推进器的推力，F4为所述第四推进器的推力，F5为所述第五推进器的推力，F6为所述第六推进器的推力，Fz为所述第一运动力、Ny为所述第二运动力、Nx为所述第三运动力、Fx为所述第四运动力、Nz为所述第五运动力、Fy为所述第六运动力，α为所述预设角度，S1为所述第一推进器、所述第二推进器两者之一与所述第四方向(X)确定的轴线的距离，S2为所述第一推进器、所述第二推进器两者之一与所述第二方向(Y)确定的轴线的距离，S3为所述第三推进器与所述第二方向(Y)确定的轴线的距离，S4为所述第六推进器与所述第二方向(Y)确定的轴线的距离，S5为所述第四推进器、所述第五推进器两者之一与所述第六推进器的距离，S6为所述第四推进器、所述第五推进器两者之一与所述第二方向(Y)确定的轴线的距离。

可选地，所述运动控制装置包括：定深控制器，配置成根据获得的深度控制所述第一运动力(Fz)的大小；定向控制器，配置成根据获得的航向角控制所述第五运动力(Nz)的大小；横摇稳定控制器，配置成根据获得的横摇角控制第三运动力(Nx)的大小；纵倾稳定控制器，配置成根据获得的纵倾角控制第二运动力(Ny)的大小。

可选地，所述系统还包括：深度传感器，所述深度传感器与所述定深控制器信号连接，用于检测深度并将检测的深度信号发送至所述定深控制器；航向角测量仪，所述航向角测量仪与所述定向控制器信号连接，用于检测航向角并将检测的航向角信号发送至所述定向控制器；摇摆角测量仪，所述摇摆角测量仪与所述横摇稳定控制器信号连接，用于检测摇摆角并将检测的摇摆角信号发送至所述横摇稳定控制器；纵倾角测量仪，所述纵倾角测量仪与所述纵倾稳定控制器信号连接，用于检测纵倾角并将检测的纵倾角信号发送至所述纵倾稳定控制器。

本发明实施例的第二方面提供了一种水下机器人，包括：机身；第一动力装置、第二动力装置和第三动力装置，所述第一动力装置、第二动力装置和第三动力装置均设置于水下机器人的机身上，所述第一动力装置配置成输出朝向所述机身的第一方向(Z)的推力，所述第二动力装置配置成输出朝向所述机身的第二方向(Y)的推力，所述第三动力装置配置成输出朝向所述机身的第三方向(W)的推力，其中，所述第一方向(Z)、所述第二方向(Y)和第四方向(X)均两两相互垂直，并且所述第三方向(W)与所述第四方向(X)之间具有预设角度；运动控制装置，用于控制所述第一动力装置、所述第二动力装置和所述第三动力装置各自推力的输出。

可选地，所述第一动力装置包括第一推进器、第二推进器和第三推进器，第一推进器、第二推进器和第三推进器配置成产生使所述机身沿所述第一方向(Z)确定的轴线运动的第一运动力(Fz)、绕所述第二方向(Y)确定的轴线旋转运动的第二运动力(Ny)以及绕所述第三方向(W)确定的轴线旋转运动的第三运动力(Nx)；所述第三动力装置包括第四推进器和第五推进器，所述第四推进器和第五推进器配置成产生使所述机身沿所述第三方向(W)确定的轴线运动的第四运动力(Fx)、绕所述第一方向(Z)确定的轴线旋转运动的第五运动力(Nz)；所述第二动力装置包括第六推进器，所述第六推进器配置成与第四推进器、第五推进器配合而产生使所述机身沿所述第二方向(Y)确定的轴线运动的第六运动力(Fy)。

可选地，所述第一推进器、所述第二推进器各自与所述第四方向(X)确定的轴线之间具有相同的第一距离(S1)；所述第一推进器、所述第二推进器各自与所述第二方向(Y)确定的轴线之间具有相同的第二距离(S2)；所述第三推进器与所述第二方向(Y)确定的轴线之间具有第三距离(S3)；所述第六推进器与所述第二方向(Y)确定的轴线之间具有第三距离(S4)；所述第四推进器、所述第五推进器各自与所述第六推进器之间具有相同的第五距离(S5)；所述第四推进器、所述第五推进器各自与所述第二方向(Y)确定的轴线之间具有相同的第六距离(S6)。

可选地，所述运动控制装置包括：定深控制器，配置成根据获得的深度控制所述第一运动力(Fz)的大小；定向控制器，配置成根据获得的航向角控制所述第五运动力(Nz)的大小；横摇稳定控制器，配置成根据获得的横摇角控制第三运动力(Nx)的大小；纵倾稳定控制器，配置成根据获得的纵倾角控制第二运动力(Ny)的大小。

可选地，所述机器人还包括：深度传感器，所述深度传感器与所述定深控制器信号连接，用于检测深度并将检测的深度信号发送至所述定深控制器；航向角测量仪，所述航向角测量仪与所述定向控制器信号连接，用于检测航向角并将检测的航向角信号发送至所述定向控制器；摇摆角测量仪，所述摇摆角测量仪与所述横摇稳定控制器信号连接，用于检测摇摆角并将检测的摇摆角信号发送至所述横摇稳定控制器；纵倾角测量仪，所述纵倾角测量仪与所述纵倾稳定控制器信号连接，用于检测纵倾角并将检测的纵倾角信号发送至所述纵倾稳定控制器。

本发明实施例与现有技术相比存在的有益效果是：

本发明实施例的水下机器人运动控制系统及水下机器人，其中，采用的6个推进器中，2个推进器(第四推进器14和第五推进器15)设计为带矢量倾斜角度α，其他4个推进器(第一推进器11、第二推进器12、第三推进器13和第六推进器16)设计为不带矢量角度的布置，通过这种矢量布局方式，可以用最少的推进器数量，实现水下机器人的全自由度运动控制，使该水下机器人在水平、侧向、垂向、俯仰、横摇以及横滚的所有自由度上均可以实现精确运动控制，推进器利用率高。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1是本发明实施例的水下机器人运动控制系统的示意图；

图2是本发明实施例的水下机器人运动控制系统的又一示意图；

图3是本发明实施例的运动控制装置的示意图。

附图标记说明：

11第一推进器、12第二推进器、13第三推进器、14第四推进器、15第五推进器、16第六推进器、Z第一方向、Y第二方向、W第三方向、X第四方向、α预设角度、Fz第一运动力、Ny第二运动力、Nx第三运动力、Fx第四运动力、Nz第五运动力、Fy第六运动力、21定深控制器、22定向控制器、23横摇稳定控制器、24纵倾稳定控制器、31深度传感器、32航向角测量仪、33摇摆角测量仪、34纵倾角测量仪

具体实施方式

以下描述中，为了说明而不是为了限定，提出了诸如特定系统结构、技术之类的具体细节，以便透彻理解本发明实施例。然而，本领域的技术人员应当清楚，在没有这些具体细节的其它实施例中也可以实现本发明。在其它情况中，省略对众所周知的系统、装置、电路以及方法的详细说明，以免不必要的细节妨碍本发明的描述。

本发明的技术方案应用于全自由度水下机器人产品中，采用最少数量的推进器及小矢量角度的布置，并结合带矢量角分配矩阵的自平衡闭环运动控制，实现全自由度的高效自平衡运动控制。

为了说明本发明所述的技术方案，下面通过具体实施例来进行说明。

图1示出本发明实施例的一种水下机器人运动控制系统。如图1所示，该水下机器人运动控制系统包括：

第一推进器11、第二推进器12、第三推进器13、第四推进器14、第五推进器15和第六推进器16，所述第一推进器11、第二推进器12、第三推进器13、第四推进器14、第五推进器15和第六推进器16均设置于水下机器人的机身(图中未示出)上，所述第一推进器11、第二推进器12、第三推进器13、第四推进器14、第五推进器15和第六推进器16均可采用为螺旋桨推进器，其中：

所述第一推进器11、第二推进器12、第三推进器13均配置成输出朝向所述机身的第一方向Z的推力，所述第四推进器14和第五推进器15均配置成输出朝向所述机身的第三方向W的推力，所述第六推进器16配置成输出朝向所述机身的第二方向Y的推力，其中，所述第一方向Z、所述第二方向Y和第四方向X均两两相互垂直，并且所述第三方向W与所述第四方向X之间具有预设角度α，其中，所述预设角度α可以是15～20°中的任意值，所述预设角度也可称为带矢量倾斜角度。

所述水下机器人运动控制系统还包括：

运动控制装置，用于控制所述第一推进器11、第二推进器12、第三推进器13、第四推进器14、第五推进器15和第六推进器16各自的推力的输出。

本发明实施例的水下机器人运动控制系统，采用的6个推进器中，2个推进器(第四推进器14和第五推进器15)设计为带矢量倾斜角度α，其他4个推进器(第一推进器11、第二推进器12、第三推进器13和第六推进器16)设计为不带矢量角度的布置，通过这种矢量布局方式，可以用最少的推进器数量，实现水下机器人的全自由度运动控制，使该水下机器人在水平、侧向、垂向、俯仰、横摇以及横滚的所有自由度上均可以实现精确运动控制。

在一个具体实施方式中，如图2所示，所述第一推进器11、第二推进器12和第三推进器13配置成产生使所述机身沿所述第一方向Z确定的轴线运动的第一运动力Fz、绕所述第二方向Y确定的轴线旋转运动的第二运动力Ny以及绕所述第三方向W确定的轴线旋转运动的第三运动力Nx，具体地说，所述第一运动力Fz使所述机身升沉，所述第二运动力Ny使所述机身纵倾，所述第三运力Nx使所述机身横摇。

所述第四推进器14和第五推进器15配置成产生使所述机身沿所述第三方向W确定的轴线运动的第四运动力Fx、绕所述第一方向Z确定的轴线旋转运动的第五运动力Nz。其中，所述第四运动力Fx使所述机身前后移动，所述第五运动力Nz使所述机身转艏。

所述第六推进器16配置成与所述第四推进器14、第五推进器15配合而产生使所述机身沿所述第二方向Y确定的轴线运动的第六运动力Fy。其中，所述第六运动力Fy使所述机身横移。

在一种具体实施方式中，如图1所示，所述第一推进器11、所述第二推进器12各自与所述第四方向X确定的轴线之间具有相同的第一距离S1。所述第一推进器11、所述第二推进器12各自与所述第二方向Y确定的轴线之间具有相同的第二距离S2。所述第三推进器13与所述第二方向Y确定的轴线之间具有第三距离S3。所述第六推进器16与所述第二方向Y确定的轴线之间具有第三距离S4。所述第四推进器14、所述第五推进器15各自与所述第六推进器16之间具有相同的第五距离S5。所述第四推进器14、所述第五推进器15各自与所述第二方向Y确定的轴线之间具有相同的第六距离S6。

进一步地，所述第一推进器11、第二推进器12、第三推进器13、第四推进器14、第五推进器15、第六推进器16各自的推力与所述第一运动力Fz、第二运动力Ny、第三运动力Nx、第四运动力Fx、第五运动力Nz、第六运动力Fy可以满足以下关系：

Fx＝F4*cosα+F5*cosα

Fz＝F1+F2+F3

Fy＝F6+F4*sinα+F5*sinα

Ny＝F1*S2+F2*S2+-F3*S3

Nx＝F1*S1+-F2*S1

Nz＝-F4*sinα*S6+F5*sinα*S6+-F4*cosα*S5+F5*cosα*S5+-F6*S4

其中，参照图1所示，F1为所述第一推进器11的推力，F2为所述第二推进器12的推力，F3为所述第三推进器13的推力，F4为所述第四推进器14的推力，F5为所述第五推进器15的推力，F6为所述第六推进器16的推力，Fz为所述第一运动力、Ny为所述第二运动力、Nx为所述第三运动力、Fx为所述第四运动力、Nz为所述第五运动力、Fy为所述第六运动力，α为所述预设角度α，S1为所述第一推进器11、所述第二推进器12两者之一与所述第四方向X确定的轴线的距离，S2为所述第一推进器11、所述第二推进器12两者之一与所述第二方向Y确定的轴线的距离，S3为所述第三推进器13与所述第二方向Y确定的轴线的距离，S4为所述第六推进器16与所述第二方向Y确定的轴线的距离，S5为所述第四推进器14、所述第五推进器15两者之一与所述第六推进器16的距离，S6为所述第四推进器14、所述第五推进器15两者之一与所述第二方向Y确定的轴线的距离。可以看出，所述第四运动力Fx，即前后移动合力Fx，由水平推力F4和F5合力组成；所述第一运动力Fz，即升沉合力Fz，由垂直推力F1、F2、F3组成；所述第六运动力Fy，即侧向横移合力Fy，由水平推力F4、F5和侧向推力F6共同组成；所述第二运动力Ny，即纵倾合力矩Ny，由垂直推力F1、F2、F3在X方向上不同力臂下的合力矩组成；所述第三运动力Nx，即横摇合力矩Nx，由垂直推力F1、F2在Y方向上不同力臂下和合力矩组成；所述第五运动力Nz，即转艏合力矩Nz，由水平推力F4、F5和侧向推力F6的合力矩组成。

通过以上转换关系，将刚体力学模型通过力学简化转换为运动控制模型，运动学控制模型中每一个自由度都对应单独的合力或合力矩，由此可实现全自由度的闭环运动反馈控制。

根据以上力学简化过程，可知6个推进器的推力和运动控制模型中的6个自由度上的合力(合力矩)存在固定的转换关系。将以上力学简化过程通过矩阵方式表达，即控制转换矩阵B将刚体力学中的6个推进器的推力转换为控制模型中的6个自由度方向的合力(合力矩)：

在运动控制模型中，6个运动自由度即逐个对应的可以控制的6个运动控制量及实际控制对象，包括：前后移动合力Fx-前后位移、升沉合力Fz-深度、侧向横移合力Fy-横向位移、纵倾合力矩Ny-纵倾角、横摇合力矩Nx-横摇角和转艏合力矩Nz-航向角，每个运动控制量参数分别与每个运动自由度单独相关，因此，这样的全自由度运动控制模型非常便于PID控制器设计。

在一个具体实施方式中，如图3所示，本发明实施例的水下机器人运动控制系统中的所述运动控制装置可以包括以下控制器：

定深控制器21，配置成根据获得的深度控制所述第一运动力Fz的大小。具体地，是根据深度变化量ΔH反馈控制所述第一运动力Fz。

定向控制器22，配置成根据获得的航向角控制所述第五运动力Nz的大小。具体地，是根据航向角变化量Δα反馈控制所述第五运动力Nz。

横摇稳定控制器23，配置成根据获得的横摇角控制第三运动力Nx的大小。具体地，是根据横摇角度变化量Δβ反馈控制所述第三运动力Nx。

纵倾稳定控制器24，配置成根据获得的纵倾角控制第二运动力Ny的大小。具体地，是根据纵倾角度变化量Δγ反馈控制所述第二运动力Ny。

其中，所述定深控制器21、定向控制器22、横摇稳定控制器23、纵倾稳定控制器24均可以采用PID控制器实现，由此，定深控制器21可称为定深PIDZ，定向控制器22可称为定向PIDNz，横摇稳定控制器23可称为横摇稳定PIDNx，纵倾稳定控制器24可称为纵倾稳定PIDNy。

本发明实施例的水下机器人运动控制系统，利用经典PID控制技术和增量式PID算法，基于水下机器人机体姿态反馈(深度、航向角、横摇角和纵倾角)实时调整并补充机体状态，实现全自由度下实时姿态自平衡闭环运动控制。

进一步地，如图3所示，本发明实施例的水下机器人运动控制系统还包括：

深度传感器31，所述深度传感器31与所述定深控制器21信号连接，用于检测深度并将检测的深度信号发送至所述定深控制器21。

航向角测量仪32，所述航向角测量仪32与所述定向控制器22信号连接，用于检测航向角并将检测的航向角信号发送至所述定向控制器22。其中，所述航向角测量仪32可以采用磁罗经实现。

摇摆角测量仪33，所述摇摆角测量仪33与所述横摇稳定控制器23信号连接，用于检测摇摆角并将检测的摇摆角信号发送至所述横摇稳定控制器23。其中，所述摇摆角测量仪33可以采用九轴传感器实现。

纵倾角测量仪34，所述纵倾角测量仪34与所述纵倾稳定控制器24信号连接，用于检测纵倾角并将检测的纵倾角信号发送至所述纵倾稳定控制器24。其中，所述纵倾角测量仪34也可以采用九轴传感器实现。

根据水下机器人工程需求和可实施性原则，PID闭环自动反馈控制设计遵循模块化和实用性原则，所有单独的PID控制器整合成所述运动控制装置。其中，PID控制器的设计参数包括比例参数K_p，积分参数K_i和微分参数K_d。针对本实施例的水下机器人的运动控制模型，反馈信号为水下机器人的深度变化、航向角变化、纵倾角度和横摇角度等变化量，因此，起关键作用的为直接与参数变化量有关系的比例参数K_p和积分参数K_i，PID控制器的结构设计主要以比例参数K_p，积分参数K_i为主，微分参数K_d起到的作用有限。控制器的设计可以考虑增量式PID或普通PID结合积分分离概念方法对PID控制器进行设计，即当被控量与设定值的偏差较大时，取消积分作用，减少较大的静差带来过大的反馈控制；当被控量接近给定值时，引入积分控制，以消除静差，提高控制精度。建立完成针对各个自由度的PID控制器后，需要建立整体PID控制系统，提供PID控制器调用逻辑和闭环结构。

在本发明的所述运动控制装置中，主要设计定深PIDZ、定向PIDNz、横摇稳定PIDNx和纵倾稳定PIDNy这4个PID控制器用于闭环反馈控制。前移控制器(可称为前移PIDX)和前移控制器(可称为横移PIDY)由于反馈参数的无法准确获取，暂时不进入整体控制策略中。在水下机器人的自平衡模式下，以上4个PID控制器默认共同启动和作用，运动控制装置在控制机体进行自动平衡过程中，实时反馈输出的合力Fz,Nz,Nx,Ny，始终要与操控终端指令下达的力F_x,F_z,F_y,N_z,N_x,N_y合并，成为最终的泰坦刚体运动所需合力：

FX＝F_x

FZ＝Fz+F_z

FY＝F_y

NZ＝Nz+N_z

NX＝Nx+N_x

NY＝Ny+N_y

6个推进器的推力由合并后的力通过对公式(1)反向求解得到推力分配矩阵C，从而分别求解得到最终每个推进器的推力要求F1、F2、F3、F4、F5、F6。

可通过公式(2)中的推力分配矩阵C计算得到6个推进器分别需要的推力，因此，本发明的运动控制装置是耦合和连续的。最后每个推力要做饱和限制(输出限制，不可超过极限)。

本发明实施例的水下机器人运动控制系统易于编程实现并方便调试修改，可通过水面控制终端的软件中实现该控制系统的应用，且对水下机器人的硬件系统不造成过多的负担和过高的要求。

在本发明的实施例中，还提供一种水下机器人，其还可称为水下无人机、潜水器等。本发明实施例的水下机器人包括：

机身；

第一动力装置、第二动力装置和第三动力装置，所述第一动力装置、第二动力装置和第三动力装置均设置于水下机器人的机身上。所述第一动力装置配置成输出朝向所述机身的第一方向Z的推力，所述第二动力装置配置成输出朝向所述机身的第二方向Y的推力，所述第三动力装置配置成输出朝向所述机身的第三方向W的推力，其中，所述第一方向Z、所述第二方向Y和第四方向X均两两相互垂直，并且所述第三方向W与所述第四方向X之间具有预设角度α，其中，所述预设角度α可以是15～20°中的任意值，所述预设角度也可称为带矢量倾斜角度。

运动控制装置，用于控制所述第一动力装置、所述第二动力装置和所述第三动力装置各自推力的输出。

在一个具体实施方式中，如图2所示，所述第一动力装置包括第一推进器11、第二推进器12和第三推进器13，第一推进器11、第二推进器12和第三推进器13配置成产生使所述机身沿所述第一方向Z确定的轴线运动的第一运动力Fz、绕所述第二方向Y确定的轴线旋转运动的第二运动力Ny以及绕所述第三方向W确定的轴线旋转运动的第三运动力Nx。所述第三动力装置包括第四推进器14和第五推进器15，所述第四推进器14和第五推进器15配置成产生使所述机身沿所述第三方向W确定的轴线运动的第四运动力Fx、绕所述第一方向Z确定的轴线旋转运动的第五运动力Nz。所述第二动力装置包括第六推进器16，所述第六推进器16配置成与第四推进器14、第五推进器15配合而产生使所述机身沿所述第二方向Y确定的轴线运动的第六运动力Fy。

进一步地，如图1所示，所述第一推进器11、所述第二推进器12各自与所述第四方向X确定的轴线之间具有相同的第一距离S1。所述第一推进器11、所述第二推进器12各自与所述第二方向Y确定的轴线之间具有相同的第二距离S2。所述第三推进器13与所述第二方向Y确定的轴线之间具有第三距离S3。所述第六推进器16与所述第二方向Y确定的轴线之间具有第三距离S4。所述第四推进器14、所述第五推进器15各自与所述第六推进器16之间具有相同的第五距离S5。所述第四推进器14、所述第五推进器15各自与所述第二方向Y确定的轴线之间具有相同的第六距离S6。

进一步地，所述第一推进器11、第二推进器12、第三推进器13、第四推进器14、第五推进器15、第六推进器16各自的推力与所述第一运动力Fz、第二运动力Ny、第三运动力Nx、第四运动力Fx、第五运动力Nz、第六运动力Fy可以满足以下关系：

Fx＝F4*cosα+F5*cosα

Fz＝F1+F2+F3

Fy＝F6+F4*sinα+F5*sinα

Ny＝F1*S2+F2*S2+-F3*S3

Nx＝F1*S1+-F2*S1

Nz＝-F4*sinα*S6+F5*sinα*S6+-F4*cosα*S5+F5*cosα*S5+-F6*S4

其中，参照图1所示，F1为所述第一推进器11的推力，F2为所述第二推进器12的推力，F3为所述第三推进器13的推力，F4为所述第四推进器14的推力，F5为所述第五推进器15的推力，F6为所述第六推进器16的推力，Fz为所述第一运动力、Ny为所述第二运动力、Nx为所述第三运动力、Fx为所述第四运动力、Nz为所述第五运动力、Fy为所述第六运动力，α为所述预设角度α，S1为所述第一推进器11、所述第二推进器12两者之一与所述第四方向X确定的轴线的距离，S2为所述第一推进器11、所述第二推进器12两者之一与所述第二方向Y确定的轴线的距离，S3为所述第三推进器13与所述第二方向Y确定的轴线的距离，S4为所述第六推进器16与所述第二方向Y确定的轴线的距离，S5为所述第四推进器14、所述第五推进器15两者之一与所述第六推进器16的距离，S6为所述第四推进器14、所述第五推进器15两者之一与所述第二方向Y确定的轴线的距离。可以看出，所述第四运动力Fx，即前后移动合力Fx，由水平推力F4和F5合力组成；所述第一运动力Fz，即升沉合力Fz，由垂直推力F1、F2、F3组成；所述第六运动力Fy，即侧向横移合力Fy，由水平推力F4、F5和侧向推力F6共同组成；所述第二运动力Ny，即纵倾合力矩Ny，由垂直推力F1、F2、F3在X方向上不同力臂下的合力矩组成；所述第三运动力Nx，即横摇合力矩Nx，由垂直推力F1、F2在Y方向上不同力臂下和合力矩组成；所述第五运动力Nz，即转艏合力矩Nz，由水平推力F4、F5和侧向推力F6的合力矩组成。

在一个具体实施方式中，如图3所示，本发明实施例的水下机器人中的所述运动控制装置可以包括以下控制器：

定深控制器21，配置成根据获得的深度控制所述第一运动力Fz的大小。具体地，是根据深度变化量ΔH反馈控制所述第一运动力Fz。

定向控制器22，配置成根据获得的航向角控制所述第五运动力Nz的大小。具体地，是根据航向角变化量Δα反馈控制所述第五运动力Nz。

横摇稳定控制器23，配置成根据获得的横摇角控制第三运动力Nx的大小。具体地，是根据横摇角度变化量Δβ反馈控制所述第三运动力Nx。

纵倾稳定控制器24，配置成根据获得的纵倾角控制第二运动力Ny的大小。具体地，是根据纵倾角度变化量Δγ反馈控制所述第二运动力Ny。

其中，所述定深控制器21、定向控制器22、横摇稳定控制器23、纵倾稳定控制器24均可以采用PID控制器实现，由此，定深控制器21可称为定深PIDZ，定向控制器22可称为定向PIDNz，横摇稳定控制器23可称为横摇稳定PIDNx，纵倾稳定控制器24可称为纵倾稳定PIDNy。

本发明实施例的水下机器人，利用经典PID控制技术和增量式PID算法，基于水下机器人机体姿态反馈(深度、航向角、横摇角和纵倾角)实时调整并补充机体状态，实现全自由度下实时姿态自平衡闭环运动控制。

进一步地，如图3所示，本发明实施例的水下机器人还可以包括：

深度传感器31，所述深度传感器31与所述定深控制器21信号连接，用于检测深度并将检测的深度信号发送至所述定深控制器21。

航向角测量仪32，所述航向角测量仪32与所述定向控制器22信号连接，用于检测航向角并将检测的航向角信号发送至所述定向控制器22。其中，所述航向角测量仪32可以采用磁罗经实现。

摇摆角测量仪33，所述摇摆角测量仪33与所述横摇稳定控制器23信号连接，用于检测摇摆角并将检测的摇摆角信号发送至所述横摇稳定控制器23。其中，所述摇摆角测量仪33可以采用九轴传感器实现。

纵倾角测量仪34，所述纵倾角测量仪34与所述纵倾稳定控制器24信号连接，用于检测纵倾角并将检测的纵倾角信号发送至所述纵倾稳定控制器24。其中，所述纵倾角测量仪34也可以采用九轴传感器实现。

在本发明实施例的水下机器人中，运动控制装置能够给出耦合并连续的6个推进器分别要输出的推力，可以非常平稳和快速的完成在全自由度6个自由度上的闭环运动控制，控制系统耦合度高、控制算法易于实现、控制策略简单高效。同时，通过部分推进器矢量布局方式，只需6个推进器，即可实现水下机器人的全自由度运动控制，使该水下机器人在水平、侧向、垂向、俯仰、横摇以及横滚的所有自由度上均可以实现精确的运动控制及实时闭环姿态自平衡。

以上所述实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围，均应包含在本发明的保护范围之内。

Claims (10)

1.一种水下机器人运动控制系统，其特征在于，所述系统包括：

第一推进器、第二推进器、第三推进器、第四推进器、第五推进器和第六推进器，所述第一推进器、第二推进器、第三推进器、第四推进器、第五推进器和第六推进器均设置于水下机器人的机身上，其中，

所述第一推进器、第二推进器、第三推进器均配置成输出朝向所述机身的第一方向的推力，所述第四推进器和第五推进器均配置成输出朝向所述机身的第三方向的推力，所述第六推进器配置成输出朝向所述机身的第二方向的推力，其中，第四方向和所述第一方向、所述第二方向均两两相互垂直，并且所述第三方向与所述第四方向之间具有预设角度；

所述系统还包括：

运动控制装置，用于控制所述第一推进器、第二推进器、第三推进器、第四推进器、第五推进器和第六推进器各自的推力的输出。

2.根据权利要求1所述的水下机器人运动控制系统，其特征在于，

所述第一推进器、第二推进器和第三推进器配置成产生使所述机身沿所述第一方向确定的轴线运动的第一运动力、绕所述第二方向确定的轴线旋转运动的第二运动力以及绕所述第三方向确定的轴线旋转运动的第三运动力；

所述第四推进器和第五推进器配置成产生使所述机身沿所述第三方向确定的轴线运动的第四运动力、绕所述第一方向确定的轴线旋转运动的第五运动力；

所述第六推进器配置成与所述第四推进器、第五推进器配合而产生使所述机身沿所述第二方向确定的轴线运动的第六运动力。

3.根据权利要求2所述的水下机器人运动控制系统，其特征在于：

所述第一推进器、第二推进器、第三推进器、第四推进器、第五推进器、第六推进器各自的推力与所述第一运动力、第二运动力、第三运动力、第四运动力、第五运动力、第六运动力满足以下关系：

Fx＝F4*cos(α)+F5*cos(α)

Fz＝F1+F2+F3

Fy＝F6+F4*sin(α)+F5*sin(α)

Ny＝F1*S2+F2*S2+(-F3*S3)

Nx＝F1*S1+(-F2*S1)

Nz＝(-F4*sin(α)*S6)+F5*sin(α)*S6+(-F4*cos(α)*S5)+F5*cos(α)*S5+(-F6*S4)；

其中，F1为所述第一推进器的推力，F2为所述第二推进器的推力，F3为所述第三推进器的推力，F4为所述第四推进器的推力，F5为所述第五推进器的推力，F6为所述第六推进器的推力，Fz为所述第一运动力、Ny为所述第二运动力、Nx为所述第三运动力、Fx为所述第四运动力、Nz为所述第五运动力、Fy为所述第六运动力，α为所述预设角度，S1为所述第一推进器、所述第二推进器两者之一与所述第四方向确定的轴线的距离，S2为所述第一推进器、所述第二推进器两者之一与所述第二方向确定的轴线的距离，S3为所述第三推进器与所述第二方向确定的轴线的距离，S4为所述第六推进器与所述第二方向确定的轴线的距离，S5为所述第四推进器、所述第五推进器两者之一与所述第六推进器的距离，S6为所述第四推进器、所述第五推进器两者之一与所述第二方向确定的轴线的距离。

4.根据权利要求3所述的水下机器人运动控制系统，其特征在于，所述运动控制装置包括：

定深控制器，配置成根据获得的深度控制所述第一运动力的大小；

定向控制器，配置成根据获得的航向角控制所述第五运动力的大小；

横摇稳定控制器，配置成根据获得的横摇角控制第三运动力的大小；

纵倾稳定控制器，配置成根据获得的纵倾角控制第二运动力的大小。

5.根据权利要求4所述的水下机器人运动控制系统，其特征在于，所述系统还包括：

深度传感器，所述深度传感器与所述定深控制器信号连接，用于检测深度并将检测的深度信号发送至所述定深控制器；

航向角测量仪，所述航向角测量仪与所述定向控制器信号连接，用于检测航向角并将检测的航向角信号发送至所述定向控制器；

摇摆角测量仪，所述摇摆角测量仪与所述横摇稳定控制器信号连接，用于检测摇摆角并将检测的摇摆角信号发送至所述横摇稳定控制器；

纵倾角测量仪，所述纵倾角测量仪与所述纵倾稳定控制器信号连接，用于检测纵倾角并将检测的纵倾角信号发送至所述纵倾稳定控制器。

6.一种水下机器人，其特征在于，包括：

机身；

第一动力装置、第二动力装置和第三动力装置，所述第一动力装置、第二动力装置和第三动力装置均设置于水下机器人的机身上，所述第一动力装置配置成输出朝向所述机身的第一方向的推力，所述第二动力装置配置成输出朝向所述机身的第二方向的推力，所述第三动力装置配置成输出朝向所述机身的第三方向的推力，其中，所述第一方向、所述第二方向和第四方向均两两相互垂直，并且所述第三方向与所述第四方向之间具有预设角度；

运动控制装置，用于控制所述第一动力装置、所述第二动力装置和所述第三动力装置各自推力的输出。

7.根据权利要求6所述的水下机器人，其特征在于，

所述第一动力装置包括第一推进器、第二推进器和第三推进器，第一推进器、第二推进器和第三推进器配置成产生使所述机身沿所述第一方向确定的轴线运动的第一运动力、绕所述第二方向确定的轴线旋转运动的第二运动力以及绕所述第三方向确定的轴线旋转运动的第三运动力；

所述第三动力装置包括第四推进器和第五推进器，所述第四推进器和第五推进器配置成产生使所述机身沿所述第三方向确定的轴线运动的第四运动力、绕所述第一方向确定的轴线旋转运动的第五运动力；

所述第二动力装置包括第六推进器，所述第六推进器配置成与第四推进器、第五推进器配合而产生使所述机身沿所述第二方向确定的轴线运动的第六运动力。

8.根据权利要求7所述的水下机器人，其特征在于，

所述第一推进器、所述第二推进器各自与所述第四方向确定的轴线之间具有相同的第一距离；

所述第一推进器、所述第二推进器各自与所述第二方向(Y)确定的轴线之间具有相同的第二距离；

所述第三推进器与所述第二方向确定的轴线之间具有第三距离；

所述第六推进器与所述第二方向确定的轴线之间具有第三距离；

所述第四推进器、所述第五推进器各自与所述第六推进器之间具有相同的第五距离；

所述第四推进器、所述第五推进器各自与所述第二方向确定的轴线之间具有相同的第六距离。

9.根据权利要求8所述的水下机器人，其特征在于，所述运动控制装置包括：

定深控制器，配置成根据获得的深度控制所述第一运动力的大小；

定向控制器，配置成根据获得的航向角控制所述第五运动力的大小；

横摇稳定控制器，配置成根据获得的横摇角控制第三运动力的大小；

纵倾稳定控制器，配置成根据获得的纵倾角控制第二运动力的大小。

10.根据权利要求9所述的水下机器人，其特征在于，所述机器人还包括：

深度传感器，所述深度传感器与所述定深控制器信号连接，用于检测深度并将检测的深度信号发送至所述定深控制器；

航向角测量仪，所述航向角测量仪与所述定向控制器信号连接，用于检测航向角并将检测的航向角信号发送至所述定向控制器；

摇摆角测量仪，所述摇摆角测量仪与所述横摇稳定控制器信号连接，用于检测摇摆角并将检测的摇摆角信号发送至所述横摇稳定控制器；

纵倾角测量仪，所述纵倾角测量仪与所述纵倾稳定控制器信号连接，用于检测纵倾角并将检测的纵倾角信号发送至所述纵倾稳定控制器。

Images