CN100476669C - 浮游移动体的控制*** - Google Patents

Abstract

本发明的目的是提供一种浮游移动体的控制***，即便是在波浪、潮流等的干扰下，也能够使浮游移动体高精度地静止在规定位置，或者高精度地追踪目标轨道。可以达到上述目的的本发明的浮游移动体的控制***是浮游移动体(10)的控制***(1)，上述浮游移动体包括：主体部(B)，占浮游移动体的一部分，并可视为单一性刚体；执行器部(E)，对浮游移动体产生推力；以及推力传输门(G)，是对主体部和执行器部进行力学结合的部分，构成为能够实际测定从执行器部作用于主体部的推力；使用来自推力传输门(G)的推力测定值来得到对执行器部(E)的推力指令。

Description

浮游移动体的控制***

技术领域

本发明涉及水下、空中、宇宙空间等中的浮游移动体、或在平面上滑行的移动体的控制***。

背景技术

首先，参照图11～图14，对已知的浮游移动体的控制方法进行说明。图11是已知的浮游移动体的概略图，图11～图14是表示已知的浮游移动体的控制方法的概略图。

如图11所示，为了控制以水下机器人为代表的浮游移动体10主体的位置和速度，在该主体上设有传感器13，由此来检测出浮游移动体10主体的位置和速度，并提供给计算机P内的减法运算电路11。另外，还由操作者H向减法运算电路11提供表示成为目标的位置和速度的信号。而减法运算电路11的输出提供给推进器控制电路12，由此控制浮游移动体10的位置和速度(所谓的位置/速度反馈控制)，该推进器控制电路12用于控制设置在浮游移动体10上的作为推进力产生装置(执行器)的推进器T。

图12是对该图11所示的浮游移动体10进行控制的框图。作为推进器控制电路12的输出的执行器(effector)推力指令信号，被提供给具有规定的推力特性14的推进器T，由此把执行器推力赋予浮游移动体10，如参照符号15所示，获得浮游移动体10的动力特性(dynamics)、即动力学输入输出特性，并利用传感器13如上述所述来检测出其主体的位置和速度，并提供给减法运算电路11。

这样，在以往的浮游移动体的控制中，在通过反馈成为控制对象的浮游移动体的位置和速度的信息来直接对执行器的推力指令进行操作时，在一般的执行器的响应速度比较慢的浮游移动体中，从向执行器发出指令，到浮游移动体的位置和速度反映出其应变，并被传感器得到为止，需要一定的时间，因而存在着对控制性能产生不良影响这样的问题(所谓的传感延迟的问题)。而且浮游移动体的质量越大、或者执行器的推力越小该倾向越明显。

另外，在水下、空中等流体中的浮游移动体，除了经常受波浪、潮流、风等流体力学上的干扰的影响外，还如图11所示，由于水下的浮游移动体10连接有来自水上的控制线缆C(供电线缆和通信线缆等)等，所以经常出现该线缆抻拽浮游移动体10的情况，按上述的控制方法，对未知的干扰作出响应耗费时间，因而也存在着对控制性能产生不良影响这样的问题。

接下来，当如图11所示，在浮游移动体10上还设置有具有多个连杆的机械臂A时，则构成图13所图示的框图。参照符号16所示的机械臂A的反作用力和扭矩，如作为加法运算电路17所举例说明的那样，被加到推进器T的执行器推力上。因此，来自机械臂A的反作用力和扭矩使得浮游移动体10的动作，即位置和速度发生紊乱，需要控制推进器T来修正因来自该机械臂A的反作用力和扭矩而产生的浮游移动体10的位置和速度的误差。这样，由于来自机械臂A的反作用力和扭矩使得浮游移动体10的动作(位置和速度)发生紊乱，所以，在以往的姿势控制中，通过使推进器T动作来修正所产生的位置和速度的误差。

即，在已知的控制方法中，在浮游移动体10因机械臂A的反作用力和扭矩而动作后，使推进器T动作，因而，依然没有解决对干扰的响应速度慢的所谓的传感延迟的问题。

另外，图14是表示其它现有技术的框图。该控制方法考虑到机械臂A的运动与浮游移动体10主体的运动之间的相互干扰，同时确定执行器推力和机械臂关节扭矩，使得各自的动作与目的一致。在该现有技术中，把具备了机械臂A的浮游移动体10作为多体连杆***，根据其运动方程式，把各个机械臂的各个关节扭矩对浮游移动体10主体的运动所产生的影响利用算式来描述，通过求解该算式来确定浮游移动体10主体的推进力(所谓的基于模型的控制)。

在该图14的现有技术中，把来自于推进器/机械臂控制电路18的执行器推力指令信号提供给推进器T，另外，把机械臂A的扭矩指令信号提供给关节致动器19。这样，由从推进器T输出的执行器推力和关节致动器19的关节扭矩形成了浮游移动体10主体和机械臂A的复合***动力特性20，传感器13检测浮游移动体10主体的位置和速度，并提供给减法器11，该减法器11还被提供了表示浮游移动体10主体的成为目标的位置和姿势的信号。另外，来自复合***动力特性20的表示机械臂控制变量、即位置、姿势、关节角度、速度、指尖反作用力等的信号被提供给减法运算电路21，该减法运算电路21还被提供了机械臂A的控制变量的目标值。这些减法器11、21的输出，被提供给推进器/机械臂控制电路18。

即，在该控制方法中，考虑机械臂A的运动和浮游移动体10主体的运动之间的相互干扰来同时确定执行器推力和机械臂关节扭矩，使得各自的动作与目的一致。由此，具有可准确地控制浮游移动体10主体的动作这样的优点，然而还存在以下的(1)～(3)的问题。

(1)对象的动力特性一般都很复杂，如果机械臂A为6轴，则加上浮游移动体10主体，将成为7个刚体连接在一起的连杆的动力特性。如果机械臂A有多个，则将进一步变成非常复杂的动力特性。对于这样的控制***的运算需要花费大量的工夫，给计算机带来沉重的负荷。因此，在实际中使用小型计算机来构成上述的控制***是非常困难的。

(2)另外，在上述的控制***中，需要机械臂A的各个连杆和主体的质量、惯性力矩、重心位置等多种力学参数，对这些力学参数而言，如果不是全部使用正确的值，则不能实现正确的控制。因此，在用机械臂A抓取物体等情况下，必须正确地提供被抓取的物体的质量、惯性力矩、和重心位置等。因此，需要具备可抓取的物体的数据库，或测量对象的力学特性，在实际中存在其用途受到限制的问题。

(3)另外，在水下、空中等流体中的浮游移动体，不仅经常受到波浪、潮流、风等流体力学上未知的干扰的影响，而且如图11所示，水下的浮游移动体10还连接有来自水上的控制线缆C(供电线缆和通信线缆等)等，因而时常会发生该线缆拖拽浮游移动体10的情况，这样的未知的干扰不仅不能模型化，而且一般也很难对该干扰的影响进行预测，所以这样的基于模型的控制方法存在着根本不能适应浮游移动体的控制的问题。

另外，作为已知的浮游移动体的控制方法，还有专利文献1所公开的机械手根部的力反馈控制，或非专利文献1所提出的推力局部反馈控制等。

根据专利文献1的机械手根部的力反馈控制，在浮游移动体主体因机械手的干扰而动作之前，先使推进器动作来消除对浮游移动体主体的影响，从而可防止浮游移动体主体因机械手的干扰而动作。

另外，作为用于控制浮游移动体的位置/速度的执行器而使用的推进器，由于利用流体来产生推力，所以在对推进器的输入指令与实际输出的推力之间存在起因于流体的较强的非线性特性，而且构成这样的非线性特性的要素涉及多方面，以往难以进行将这些全盘考虑了的控制(所谓的执行器动态特性的非线性特性的问题)，但根据非专利文献1涉及的推力局部反馈控制，可防止因上述执行器动态特性的非线性特性而导致的浮游移动体的控制性能的恶化。

然而，上述专利文献1和非专利文献1涉及的控制方法，虽然都是测量一部分的力并进行反馈，但都不能消除作为对浮游移动体的最大干扰的波浪、潮流、风等的影响。

总之，在上述任意一种现有技术中都还未实现以下这样的浮游移动体的控制***：即使有机械臂的反作用等的干扰，以及处于水下或空中等流体中的浮游移动体经常受到的波浪、潮流、风等流体力学上的未知的干扰，也能够自动地对干扰的影响进行补偿，从而可使浮游移动体高精度地静止，或高精度地追踪目标轨道。

另一方面，除了上述的问题外，作为使浮游移动体航行的已知方法，在多数情况下使用惯性导航，然而该惯性导航存在着以下的问题。即，惯性导航使用根据浮游移动体求出的加速度，在浮游移动体自身很重的情况下，暂时把加速度转换为位移、然后把加速度的值作为电信号输出的已知的加速度传感器，从其构造上讲，不能高精度地检测出微小的加速度的变化，存在着从传感器获得的加速度值与实际的加速度值之间产生明显误差这样的问题。这样的加速度值的误差，对惯性导航自身的精度也产生明显的影响，由此产生了不能沿着所要的轨道准确地引导浮游移动体的不良情况。

专利文献1：JP特开平5-119837号公报

非专利文献1：金冈、中山、林、川村著“用于实现水下高精度运动的推进器推力局部反馈控制”，日本机械学会机器人学及机械电子讲演会′03讲演论文集2P1-2F-A6，2003年

发明内容

因此，本发明的目的是提供一种浮游移动体的控制***，该控制***即使在受到例如波浪、潮流、风以及安装在浮游移动体上的机械臂的反作用等的干扰的情况下，也能够使浮游移动体高精度地静止在规定位置，或者高精度地追踪目标轨道。

本发明的另一个目的是提供一种能够以比较简单的结构，准确地控制浮游移动体的位置和姿势等的浮游移动体的控制***。

另外，本发明的再一个目的是提供一种浮游移动体的加速度检测***，该加速度检测***能够高精度检测出浮游移动体的加速度，甚至是微小的加速度变化，而不依赖于浮游移动体的大小、重量，或加速度传感器的性能。

本发明的发明人发现如果把浮游移动体分成占浮游移动体的一部分、并可视为单一刚体的主体部、和对浮游移动体产生推力的执行器部，并在对该主体部和执行器部进行力学结合的唯一部分(推力传输门)上设置力/扭矩传感器，则由于该力/扭矩传感器所检测出的值是从执行器部施加给主体部的推力，所以可将施加在主体部与执行器部之间的力和扭矩全部检测出来，如果反馈该值，则可直接控制对浮游移动体施加的推力，由此，即使在受到波浪、潮流等流体力学上的干扰下，也能够高精度地使浮游移动体静止在规定位置，或高精度地追踪目标轨道，并据此完成了本发明。

能够解决上述问题的本发明的浮游移动体的控制***的特征在于：(1)在浮游移动体的控制***中，上述浮游移动体包括：主体部，占上述浮游移动体的一部分、并可视为单一性刚体；执行器部，对上述浮游移动体产生推力；以及推力传输门，是对上述主体部和上述执行器部进行力学结合的部分，构成为可实际测定从上述执行器部作用于上述主体部的推力；并且，上述主体部具有可测量上述浮游移动体的加速度的加速度测定装置，使用来自上述主体部的上述加速度测定装置的输出、和来自上述推力传输门的推力测定值，来得到对上述执行器部的推力指令。

另外，本发明的特征还在于，在上述的浮游移动体的控制***中，(3)上述推力传输门具有可测量施加在上述主体部与上述执行器部之间的力或扭矩的力或扭矩测定装置；使用来自上述推力传输门的上述力或扭矩测定装置的输出、来自上述浮游移动体的上述加速度测定装置的输出、以及转换来自操作者的目标位置速度轨道指令而得到的目标加速度轨道指令，来得到对上述执行器部的推力指令。

另外，除了后面的实施方式和实施例所说明的在三维空间内浮游的浮游移动体以外，在把本发明的控制***和加速度检测***应用于在斜面上滑行的浮游移动体的情况下，由于需要对重力加速度的影响进行补偿，所以，需要如后面的(7)的发明所规定的那样，具有可测量主体部的倾斜的倾斜角测定装置。

但是，在将本发明的控制***或加速度检测***应用于在根本没有重力作用的宇宙空间中浮游的浮游移动体、和在不需要考虑倾斜或可忽略倾斜的水平面上滑行的浮游移动体时，如上述(1)或(3)、或者下面所说明的(2)或(4)的发明所规定的那样，不需要倾斜角测定装置。

这在应用了虚拟推力传输门***的构思的下述(5)或(6)的发明的情况下也一样，在将本发明的使用了虚拟推力传输门***的构思的控制***应用于在三维空间内浮游的浮游移动体等时，如下述(7)的发明所规定的那样，需要具备可测量浮游移动体的倾斜的倾斜角测定装置。

同样，能解决上述问题的本发明的浮游移动体的控制***的特征在于，(2)在浮游移动体的控制***中，上述浮游移动体包括：主体部，占上述浮游移动体的一部分，并可视为单一性刚体；执行器部，对上述浮游移动体产生推力；以及推力传输门，是对上述主体部和上述执行器部进行力学结合的部分，构成为可实际测定从上述执行器部作用于上述主体部的推力；上述主体部构成为：只与上述推力传输门进行力学结合，而且作用于上述主体部的推力实质上全部通过上述执行器部和上述推力传输门来输入，使用来自上述推力传输门的推力测定值来得到对上述执行器部的推力指令。

另外，作为上述主体部构成为只与上述推力传输门进行力学结合、而且作用于上述主体部的力实质上全部通过上述执行器部和上述推力传输门来输入的典型例子，可列举出上述执行器部呈包围主体部的壳状体。

但是，这样的构造是极端的例子，由呈壳状体的执行器部来密封主体部不是必要的条件。总之，在由该(2)所规定的发明中，只要构成为对主体部不直接施加水压、较强的风压等干扰以及其它外力的构造即可，例如，空气与被收纳于在水上或地上滑行的浮游移动体内部的主体部接触这样的情况是足以忽略的程度，因此在该(2)涉及的发明的应用上不会出现问题。

因此，可理解为由该(2)所规定的发明不限于图3所示的构造。

另外，本发明的特征还在于，在上述(2)的浮游移动体的控制***中，(4)上述推力传输门具有可测量施加在上述主体部与上述执行器部之间的力或扭矩的力或扭矩测定装置，使用来自上述推力传输门的上述力或扭矩测定装置的输出、以及转换来自操作者的目标位置速度轨道指令而得到的目标加速度轨道指令，来得到对上述执行器部的推力指令。

另外，本发明的特征还在于，(5)在浮游移动体的控制***中，上述浮游移动体具有可测量上述浮游移动体的加速度的加速度测定装置，上述浮游移动体在计算上，包括：虚拟主体部，占上述浮游移动体的一部分，并可视为单一性刚体；虚拟执行器部，对上述浮游移动体产生推力；以及虚拟推力传输门，是对上述虚拟主体部和上述虚拟执行器部进行虚拟结合的部分，构成为可使用来自上述加速度测定装置的输出，来推定从上述虚拟执行器部作用于上述虚拟主体部的推力；使用来自上述虚拟推力传输门的推力推定值，来得到对上述虚拟执行器部的推力指令。

另外，本发明的特征还在于，在上述(5)的浮游移动体的控制***中，(6)上述虚拟推力传输门具有可推定施加在上述虚拟主体部与上述虚拟执行器部之间的力或扭矩的力或扭矩推定装置，使用来自上述虚拟推力传输门的上述力或扭矩推定装置的输出、以及转换来自操作者的目标位置速度轨道指令而得到的目标加速度轨道指令，来得到对上述虚拟执行器部的推力指令。

本发明的特征还在于，上述(1)～(6)的浮游移动体的控制***，如上所述，是构成为可排除重力的影响的浮游移动体的控制***，(7)上述主体部或上述虚拟主体部还具有可测量上述浮游移动体的倾斜的倾斜角测定装置，

还利用来自上述倾斜角测定装置的输出，来得到对上述执行器部或上述虚拟执行器部的推力指令。

另外，如下所述，在上述本发明的浮游移动体的控制***中，浮游移动体的控制通过“推力计划部”和“推力控制部”2级结构来进行，“推力计划部”为了实现目标轨道而计算出主体部应从推力传输门接受的推力并发出指令，“推力控制部”把推力传输门推力的当前值反馈到执行器部来直接驱动执行器部，使得在推力传输门实现指令推力。

在本发明中，把上述的机构设计和控制方法统称为“推力传输门***”。作为推力传输门***的实施方式，主要有以下3种：

i)使主体部露出在外部的状态，即、采用使主体部可直接受到水压、较强的风压等干扰、以及其它外力的影响的结构，并且具有推力传输门的方式(参照下述的第1实施方式的第1例)；

ii)具有推力传输门，并且主体部只与推力传输门进行力学结合，且作用于主体部的力实质上全部通过执行器部和推力传输门输入，以便使水压、较强的风压等干扰以及其它外力不直接施加于主体部(例如利用执行器部覆盖、密封主体部的状态等)的方式(参照下述的第1实施方式的第2例)；

iii)不改变浮游移动体的硬件，但在计算上将浮游移动体分为主体部和执行器部，并在计算机内预先准备主体部的惯性矩阵，使用该惯性矩阵、设在浮游移动体上的加速度传感器的输出、以及来自根据需要而设置在浮游移动体上的倾斜角传感器的输出，来得到对执行器部的推力指令的方式(虚拟推力传输门、参照下述的第1实施方式的第3例)。

对于i)和ii)而言，虽然为了实现推力传输门***，需要变更浮游移动体的硬件设计，并且在推力传输门中需要力/扭矩传感器，但所实现的控制性能较高。

另一方面，对于iii)而言，虽然与i)和ii)的方法相比，控制性能降低，但不需要改变浮游移动体的硬件设计，只要在现有的浮游移动体上附加加速度传感器、另外根据需要而附加倾斜角传感器，即可使用推力传输门***，从而能提高控制性能。

特别是，目前在航空器等中装备的公知的惯性导航装置，也可以作为在本发明中所使用的加速度传感器、倾斜角传感器、主体部位置/速度传感器来使用，如果使用这些，则与现有的控制相比，可实现高速化、高精度化，而无须进行硬件的改变。

另外，本发明的特征还在于，在用于检测浮游移动体的加速度***中，上述浮游移动体包括：主体部，占上述浮游移动体的一部分、且可视为单一性刚体；执行器部，对上述浮游移动体产生推力；和推力传输门，是对上述主体部和上述执行器部进行力学结合的部分，构成为可实际测定从上述执行器部作用于上述主体部的推力；上述推力传输门具有可测量施加在上述主体部与上述执行器部之间的力或扭矩的力或扭矩测定装置，并且，上述主体部只与上述推力传输门进行力学结合，而且构成为作用于上述主体部的力实质上全部通过上述执行器部和上述推力传输门来输入；使用来自上述推力传输门的上述力或扭矩测定装置的输出来得到上述主体部的加速度的推定值。

另外，本发明的特征还在于，在上述的浮游移动体的加速度检测***中，上述主体部还具有可测量上述主体部的倾斜的倾斜角测定装置；还利用来自上述主体部的上述倾斜角测定装置的输出，生成上述主体部的加速度的推定值。

根据本发明，可提供一种能够高速且高精度地控制浮游移动体的***结构，即使在例如波浪、潮流、风或安装在浮游移动体上的机械臂的反作用等的干扰下，也能够使浮游移动体高精度地静止在规定位置，或者高精度地追踪目标轨道。

另外，以下对在本发明的说明中使用的技术用语进行定义。

所谓“浮游移动体”是指在水下、空中或宇宙空间等中浮游的各种移动体。而且，浮游移动体的概念还包括在平面上、或在轨道上滑行的移动体。

本发明所述的“浮游移动体”的概念，根据不同的环境，包括如下列举的具体的例子：

(1)宇宙：宇宙机器人、宇宙飞船、人造卫星等

(2)空中：航空机器人、航空器、直升飞机、飞船等

(3)水下：水下机器人、潜水艇等

(4)水上：船舶、气垫船等

(5)地上：汽车、火车、气垫船、或雪橇、摩托雪橇以及其它在冰上或雪上滑行的移动体等。

关于汽车等地上车辆，可理解为在例如车轮打滑的状态下，能成为“浮游移动体”。因此，通过应用本发明，在汽车的情况下，无论车轮打滑还是不打滑，都可以同样进行车辆控制。

不过，在车轮打滑的状态下，由现有的发动机和轮胎构成的推力传输机构不能发挥作用。如果考虑到这一点，在该状况下应用本发明时，最好使上述地上车辆具备另外不同的推力传输机构(各种形式的推进器等)，对此，通过以下的各个说明可以理解。

所谓“主体部”是指，占浮游移动体的一部分、且可视为单一刚体的物理构造。原则上，在主体部中不包括可机械性活动的部分。而且，即使有可机械性活动的部分，如果其影响小到可以被忽略的程度也可以，除此以外，即使不能忽略，只要能够预测其影响，就能将该影响消除也可以。

另外，在应用虚拟推力传输门***的构思的本发明中的、与下述第1实施方式的第3例相当的发明中，只不过是单纯为了便于说明而进行了主体部和执行器的区别，将哪个部分规定为主体部，也可以由用户来判断。因此，在实际实施与下述第1实施方式的第3例相当的发明时，除可以把主体部规定为占浮游移动体的质量的主要部分的部分以外，也可以相反地把追加设置在浮游移动体中的一部分上的物体(例如，数千克左右的锤等)规定为主体部。

另外，即使在不应用虚拟推力传输门***的构思的、与下述第1实施方式的第1例和第2例相当的发明中，区别主体部和执行器部的判断基准也只是两者是否由下面说明的推力传输门进行结合，只要满足这一点，对浮游移动体中的主体部所占的比例就没有特殊的限定。

因此，在本发明中，主体部不限于由占浮游移动体的质量的主要部分的单一刚体所构成的物理构造。

所谓“执行器部”是指，对浮游移动体产生推力的物理构造。其包括所有的用于控制浮游移动体的位置、速度等的推进力产生装置(执行器)。

所谓“力学结合”是指，在作为对象的物体之间产生了某种力的相互作用的状态。虽然几乎都是机械性结合的状态，但也包括非机械性结合，例如由电磁力等形成非接触性的力的相互作用的状态。

另一方面，所谓“力学分离”是指，在作为对象的物体之间不产生力的相互作用的状态。

所谓“推力传输门”是指，是对主体部和执行器部进行力学结合的唯一部分，具备能够检测施加在主体部与执行器部之间的所有的力和扭矩的传感器，从而可检测出从执行器部作用于主体部的所有的推力。

所谓“虚拟推力传输门”是指，假设设置在浮游移动体内而在物理上不存在的推力传输门，在为了便于计算而分成主体部和执行器部的现有的浮游移动体中，构成为根据主体部加速度测定值，能够推定施加在主体部和执行器部之间的力和扭矩、即执行器部作用于主体部的所有的推力。

另外，在本发明中，只称“推力传输门”时，如果没有特殊的指定，则包括“虚拟推力传输门”。

所谓“虚拟推力传输门的推力推定值”是指，不是在物理上存在的推力传输门处实际测量出的值，而是在虚拟推力传输门处根据加速度传感器的测定值(主体部加速度测定值)通过计算而求出的虚拟值。

所谓“推力计划部”是指，为了实现目标轨道而计算出主体部在推力传输门处应从执行器部受到的目标推力，并对推力控制部发出指令的计算机上的一部分或者功能。

所谓“推力控制部”是指，通过反馈推力传输门处的测定值或推力推定值而向执行器输出推力指令，以便在推力传输门实现所希望的目标推力的计算机上的一部分或功能。

所谓“目标推力”是指，为了实现目标轨道而计算出主体部在推力传输门处应从执行器部受到的推力。

所谓“目标加速度”是指，通过对所希望的目标轨道或目标速度进行微分运算而求出的加速度。

所谓“推力传输门***(TTGS：Thrust Transfer Gate System)”是指，利用包括推力计划部和推力控制部的控制***来控制具备推力传输门的浮游移动体的控制***。

所谓“执行器推力特性”是指，输入到推进器等执行器的执行器推力指令与实际输出的执行器推力之间的关系。另外，由于推进器等执行器一般是通过流体来产生推力，所以，在执行器推力特性中，除了包含执行器自身的机械性动力特性之外，还包含受流体力学中的阻力/升力影响等的复杂的动力特性。

以往，反馈浮游移动体的位置/速度的传感器信息来控制推进器的推力，但在本发明中，直接反馈所产生的推力来控制执行器的推力，所以从原理上实现了高速的响应。因此，可缓解上述的传感延迟的问题。特别是，只要采用具有作为硬件的推力传输门的结构(参照第1实施方式的第1例和第2例等)，则无论是浮游移动体的质量大，还是执行器的推力小，都不会增加传感延迟的影响。

在本发明的浮游移动体的控制***中，如下述的那样，推力控制部通过反馈推力传输门的推力来控制执行器的推力。由此，在实现了上述的高速响应的同时，如果针对非线性特性对推力控制部应用鲁棒(robust)的现有控制方法(滑模(sliding mode)控制等)，则可抑制执行器动态特性的非线性特性，从而可防止对控制性能的不良影响。以往，通过基于执行器的动态特性模型的前馈补偿来进行执行器的动态特性的线性化，但是需要大量的工夫和计算工作。然而，在本发明的浮游移动体的控制***中，由于通过不必使用动态特性模型的推力反馈来实现线性化，所以能够通过少量的计算工作来构筑控制***。

另外，在以往的技术中，如上述的那样，从进行传感到实际向推进器发出推力指令为止的期间将产生不能忽略的时间延迟，所以，例如在把浮游移动体维持在规定轨道上时，必须反复地进行很多次过冲(overshoot)和下冲(undershoot)的修正等，结果经常使浮游移动体进行无用的动作。确切地进行控制***的增益调节，除了费时费力，还需要具备经验，因而负担非常大。

而根据本发明，由于能够适时地提供推进器真正需要的推力，所以不会使推进器和浮游移动体进行无用的动作。而且不会使浮游移动体消耗不必要的推进器驱动能量，结果还可获得能够减少浮游移动体的耗能的效果。

另外，在本发明的浮游移动体的控制***中，未知的干扰的影响，可作为推力传输门的推力误差被立刻检测出来，并且通过推力控制部的局部反馈控制得到补偿，所以在推力计划部以上的级中，不必考虑干扰。即，在推力计划部中，只需进行“静止”、“直进”等指示，就可在推力控制部中自动地对干扰的影响进行补偿，能够实现预先计划的所希望的动作。另外，与以往那样的根据位置/速度误差来检测干扰的影响的情况相比，可实现高速的响应。

另一方面，根据本发明的浮游移动体的加速度检测***，由于使用预先确认了的主体部的惯性矩阵、来自推力传输门的力/扭矩传感器的输出、以及根据需要从主体部的倾斜角传感器进行的输出，可求出主体部的加速度推定值，所以可高精度地检测出浮游移动体的加速度，甚至是微小的加速度变化，而与浮游移动体的重量和大小无关，或者不依赖于加速度传感器的性能，在使用惯性导航引导浮游移动体时，能够把浮游移动体准确地引导到所希望的轨道上。

下面，对本发明进行进一步详细的说明。

附图说明

图1是表示应用了本发明的控制***的浮游移动体的一个例子的概略图。

图2是表示本发明的控制***的一个结构例的方框图。

图3是表示应用了本发明的控制***的浮游移动体的另一例子的概略图。

图4是表示本发明的控制***的另一结构例的方框图。

图5是表示应用了本发明的控制***的浮游移动体的另一例子的概略图。

图6是表示本发明的控制***的另一结构例的方框图。

图7是表示本发明的控制***的第2实施方式的方框图。

图8是表示本发明的控制***的一个实施例的方框图。

图9是表示本发明的控制***的另一实施例的方框图。

图10是表示本发明的控制***的另一实施例的方框图。

图11是表示已知的浮游移动体的概要的图。

图12是表示已知的浮游移动体的控制***的方框图。

图13是表示已知的浮游移动体的控制***的其它的方框图。

图14是表示已知的浮游移动体的控制***的其它的方框图。

图中符号：

A、机械臂部；B、主体部；B1、主体部静力学；B2、主体部惯性；B2’、主体部惯性；B3、主体部惯性；B4、积分；C、线缆；E、执行器部；E1、执行器推力特性；E2、执行器部静力学；G、推力传输门；G’、虚拟推力传输门；H、操作者；P、计算机；P1、微分；P2、推力计划部；P3、推力控制部；P4、位置控制/速度控制；S、耐压壳；T、推进器；1、控制***；2、加速度传感器；3、倾斜角传感器；4、位置/速度传感器；5、空洞；10、浮游移动体；11、减法运算电路；12、推进器控制电路；13、传感器；14、执行器推力特性；15、浮游移动体动力特性；16、机械臂的反作用力/扭矩；17、加法运算电路；18、推进器/机械臂控制电路；19、关节致动器；20、浮游移动体主体/机械臂复合***动力特性；21、减法运算电路。

具体实施方式

(第1实施方式)

下面，基于图1～图7对作为实施本发明的对象的代表例的水下机器人的本发明一个实施方式及其作用进行说明。水下机器人是在三维空间浮游的浮游移动体，在对该水下机器人应用本发明的控制***的情况下，由于需要对重力加速度的影响进行补偿，所以作为如上述(7)涉及的发明所规定的那样，需要具备可测量主体部的倾斜的倾斜角测定装置。这与应用虚拟推力传输门***的构思的上述(5)的发明的情况一样，在对浮游于三维空间内的浮游移动体等应用使用了虚拟推力传输门***的构思的本发明的控制***时，如作为上述(7)涉及的发明所规定的那样，需要具有可测量浮游移动体的倾斜的倾斜角测定装置。

作为上述(7)涉及的发明、即还具备倾斜角测定装置的上述(1)和(3)或(2)和(4)涉及的发明，分别相当于本第1实施方式的第1例或第2例、以及下述的实施例1或实施例2。而且，上述(5)和(6)涉及的发明相当于第1实施方式的第3例和下述的实施例3。

另外，根据上述定义部分和下述的实施例中的描述，可充分得知使用对象不限于水下机器人。而且，即使是水下机器人，也不限于图中所示的用于深海勘探等的典型的示例，可用于水下营救、油田勘探等的比较小型的机器人也属于本水下机器人的范畴。

这里，图1是表示应用了本发明的控制***的浮游移动体的一个例子的概略图，而图2是表示本发明的控制***的一个结构例的方框图。另外，图3和图5是表示应用了本发明的控制***的浮游移动体的其它例子的概略图，图4和图6是表示本发明的控制***的其它结构例的方框图。图7是表示本发明的控制***的第2实施方式的方框图。

在上述图1～图7中，对于与之前所示的图11～图14中相同的部分，使用相同的符号进行说明。

在图2、4、6以及7中，连接在各方框之间的细实线表示信号。另一方面，连接在浮游移动体10内的各方框之间的双线，表示在浮游移动体10上作为物理作用而产生的现象(力学上的转换作用等)。

在描述以下内容的本发明的说明中，把固定在惯性***的绝对位置上的基准坐标系设为∑_R，而把以浮游移动体的重心位置为原点、固定在浮游移动体主体部上的主体坐标系设为∑_B。另外，只要没有特别说明，则矢量和矩阵是从主体坐标系∑_B看到的表现。并且，浮游移动体存在于m维空间(m≤6)，浮游移动体的执行器能够产生n级自由度(m≤n)的推力。

第1例

本实施方式涉及的推力传输门***的第1例的结构如图1和2所示。图1是浮游移动体的概略图，图2是控制***的方框图。

在本例中，浮游移动体10被力学分离成主体部B和执行器部E，在主体部B与执行器部E之间，作为硬件具有推力传输门G。另外，主体部B具有加速度传感器2和倾斜角传感器3，而推力传输门G具有力/扭矩传感器。在本例中，主体部B和推力传输门G与执行器部E一样，构成为露出在外部。

另外，表现下述的主体部B的惯性矩阵M_B和浮游移动体10的几何学构造的J_GB、J_EG，采用预先测定的已知的矩阵。

以下，对本例的结构进行详细说明。

参照图2的主体部B和执行器部E的内部，则表示浮游移动体的物理动力特性的运动方程式一般可写成下式。

(算式1)

J_GB ^Tu_G(t)＝M_B(a_B(t)-g_B(t))+M_Aa_B(t)-f_B(t)    (1)

u_G(t)＝J_EG ^T(t)u_E(t)+f_E(t)                   (2)

式(1)是表示主体部的式子，而式(2)是表示执行器部的式子。各个变量的定义如下：

a_B(t)∈R^m浮游移动体主体部(重心)的加速度

g_B(t)∈R^m施加于浮游移动体主体部(重心)的重力加速度

u_G(t)∈R^m在推力传输门处从执行器部作用于主体部的推力

J_GB ∈R^m×m进行从主体部速度向推力传输门速度的转换的雅可比矩阵

M_B∈R^m×m浮游移动体主体部的惯性矩阵

M_A∈R^m×m因流体的影响而对浮游移动体主体部附加的附加惯性矩阵

f_B(t)∈R^m不能用式(1)的其它项来表现的作用于浮游移动体主体部的推力(基于潮流、波浪、风的外力等)

u_E(t)∈Rⁿ各执行器所产生的推力

J_EG(t)∈R^n×m进行从推力传输门速度向各执行器速度的转换的雅可比矩阵

f_G(t)∈R^m从推力传输门观察到的不能用式(2)的其它项来表现的从浮游移动体执行器作用于推力传输门的推力(执行器部的惯性力；基于潮流、波浪、风的外力等)的表现。

另外，在本例的推力传输门***1中，采用了在主体部B的单一刚体构造上固定推力传输门G的构造。由此，雅可比矩阵J_GB和惯性矩阵M_B为常数矩阵。

下面，对本例的推力计划部进行说明。

如图2的计算机P内的推力计划部P2的方框部分所示，把实现主体部B的目标加速度a_Bd(t)所需的推力传输门G上的目标推力u_Gd(t)定义为下式。

(算式2)

$u_{\mathrm{Gd}}\left(t\right)=u_{\mathrm{Gs}}\left(t\right)+{J_{\mathrm{GB}}}^{-T}{M}_B(a_{\mathrm{Bd}}\left(t\right)-{\hat{a}}_B\left(t\right))---\left(3\right)$

其中，a_Bd(t)、

(m维矢量)分别是主体加速度a_B(t)的目标值和推定值，而u_Gd(t)(m维矢量)是门推力u_G(t)的目标值，u_Gs(t)(m维矢量)是由推力传输门所具有的力/扭矩传感器测量的u_G(t)的测定值。

这里，在图2的计算机P内，在由参照符号P3所示的推力控制部中，通过控制u_G(t)，使得利用推力反馈实现该u_Gd(t)，可以构成利用推力反馈来控制推力的、延迟少的反馈控制***。

在推力控制部P3中，为了抑制在图2的执行器部E内由参照符号E1所表示的执行器推力特性、和对执行器部的干扰或动态力f_E(t)等执行器部动态特性的未知的非线性特性，应使用对非线性特性***可发挥良好的鲁棒性的控制方法。例如，如果应用现有的滑模控制，则推力控制部P3可如下构成。

在本例的推力控制部P3中，把实现目标推力u_Gd(t)所需的对执行器的推力指令u_Ed(t)定义为下式。

(算式3)

$s_G\left(t\right)={J_{\mathrm{EG}}}^{+T}\left(t\right)\{(u_{\mathrm{Gs}}\left(t\right)-u_{\mathrm{Gd}}\left(t\right))+T_G\frac{d}{\mathrm{dt}}{u}_{\mathrm{Gs}}\left(t\right)\}---\left(4\right)$

${\left[u_{\mathrm{Ed}}\left(t\right)\right]}_i=\left\{\begin{array}{cc}{u}_{\mathrm{Edi}\max }& ({\left[s_G\left(t\right)\right]}_i<0)\\ 0& ({\left[s_G\left(t\right)\right]}_i=0)\\ u_{\mathrm{Edi}\min }& ({\left[s_G\left(t\right)\right]}_i>0)\end{array}\right.---\left(5\right)$

各变量的定义如下。

s_G(t)∈Rⁿ滑模控制的切换函数

T_G∈R由切换函数限定的一次延迟***的时间常数

u_Ed(t)∈R对各执行器的指令推力

u_Edimax∈R执行器推力的第i分量中可指令的最大推力

u_Edimin∈R执行器推力的第i分量中可指令的最小推力

(*)_i∈R矢量*的第i分量

*^+T矩阵*的虚拟逆矩阵的转置

在本例中，对推力计划部、推力控制部的反馈所需的值是u_Gs(t)和

。由推力传输门所具备的力/扭矩传感器测量的测定值直接用于门推力测定值u_Gs(t)。

下面，对主体部加速度a_B(t)的推定方法进行说明。为了考察传感器特性、推力控制特性，以下主要是在频率范围内展开论述。

首先，在图2的主体部B内，利用由参照符号2所示的主体部加速度传感器所获得的加速度的测定值a_Bs(t)(m维矢量)表示为下式。由于在该测定值中包含重力加速度，所以，不能直接用作主体加速度a_B(t)的推定值。

(算式4)

a_Bs(s)＝G_aBs(s)(a_B(s)-g_B(s))    (6)

另外，*(s)是矢量或矩阵*的拉普拉斯变换式*(s)＝L(*(t))，G_aBs(s)是表示主体部加速度传感器特性的m行m列的传输函数矩阵。

同样，由主体部倾斜角传感器所获得的重力加速度的测定值_gBs(t)(m维矢量)表示为下式。

(算式5)

g_Bs(s)＝G_gBs(s)g_B(s)    (7)

其中，G_gBs(s)是表示主体部倾斜角传感器特性的m行m列的传输函数矩阵。

同样，由推力传输门所获得的门推力测定值u_Gs(t)(m维矢量)表示为下式。

(算式6)

u_Gs(s)＝G_uGs(s)u_G(s)    (8)

其中，G_uGs(s)是表示推力传输门的力/扭矩传感器特性的m行m列的传输函数矩阵。

下面，说明本例中的主体加速度a_B(t)的推定。

把主体加速度a_B(t)的推定值

定义为下式。

(算式7)

${\hat{a}}_B\left(t\right)=a_{\mathrm{Bs}}\left(t\right)+g_{\mathrm{Bs}}\left(t\right)---\left(9\right)$

这里，考察上述主体加速度推定的合理性。把该式(9)进行拉普拉斯变换，并代入上面的式(6)、式(7)中，则成为，

${\hat{a}}_B\left(s\right)=G_{\mathrm{gBs}}\left(s\right)(a_B\left(s\right)-g_B\left(s\right))+G_{\mathrm{gBs}}\left(s\right)g_B\left(s\right)---{\left(9\right)}^{,}$

在满足上述“所有种类、所有自由度的传感器特性相同，各传感器、各自由度之间不存在干扰(传感器特性一致条件)”的条件的情况下，关于主体部加速度传感器特性、主体部倾斜角传感器特性、和推力传输门的力/扭矩传感器特性，可以表示为：

G_aBs(s)＝G_gBs(s)＝G_uGs(s)＝G_s(s)I    (10)

其中，G_s(s)是表示同一传感器特性的函数，I是单位矩阵。即，

a_Bs(s)＝G_s(s)(a_B(s)-g_B(s))    (11)

g_Bs(s)＝G_s(s)g_B(s)            (12)

u_Gs(s)＝G_s(s)u_G(s)            (13)

此时，式(9)’成为：

(算式8)

${\hat{a}}_B\left(s\right)=G_s\left(s\right)a_B\left(s\right)---\left(14\right)$

利用式(8)可进行主体部加速度a_B(t)的推定。由于在式(14)中未明示出重力加速度g_Bs(t)，所以，只要满足上述传感器特性一致条件，就可视为直接测定了主体部加速度a_B(t)。

另外，通过选择例如与浮游移动体的动力特性相比具有足够高速的响应、且高精度的传感器，可实现上述传感器特性的一致条件。

第2例

下面，对本实施方式的推力传输门***的第2例进行说明。

推力传输门***的第2例的结构如图3、4所示。图3是浮游移动体的概略图。图4是控制***的方框图。

在本例中，也是把浮游移动体10力学分离成主体部B和执行器部E，并且在主体部B与执行器部E之间，作为硬件具有推力传输门G。而且在主体部B上具有倾斜角传感器3，在推力传输门G上具有力/扭矩传感器。

与第1例大的不同之处是：在本例中执行器部E形成覆盖主体部B的壳(图3所示的耐压壳S)，由该壳S将主体部B和推力传输门G与外界完全隔离。因为这样的构造而不需要在第1例中所需的主体部B的加速度传感器2。

与第1例一样，主体部的惯性矩阵M_B和表现浮游移动体的几何学构造的J_GB、J_EG采用预先测定的已知的矩阵。

下面，对本例的结构进行详细说明。

参照图4的主体部B和执行器部E的内部，则表示浮游移动体的物理动力特性的运动方程式一般可写成下式。

(算式9)

J_GB ^Tu_G(t)＝M_B(a_B(t)-g_B(t))    (15)

u_G(t)＝J_EG ^T(t)u_E(t)+f_E(t)     (16)

式(15)是表示主体部的式子，式(16)是表示执行器部的式子。这里，如下式所示，重新定义U_G2(t)(m维矢量)。

(算式10)

u_G2(t)＝u_G(t)+J_GB ^-TM_Bg_B(t)  (17)

下面，对本例的推力计划部进行说明。

如图4的计算机P内的推力计划部P2的方框部分所示，把实现主体部B的目标加速度a_Bd(t)所需的推力传输门G的目标推力u_G2d(t)定义为下式。

(算式11)

u_G2d(t)＝J_GB ^-TM_Ba_Bd(t)    (18)

在图4的计算机P内，在由参照符号P3表示的推力控制部中，通过控制u_G2(t)，使得利用推力反馈实现该u_G2d(t)，可以构成利用推力反馈来控制推力的、延迟少的反馈控制***。

这里，关于本例的推力控制，如果应用例如滑模控制，则可构成如下的推力控制部P3。

在本例的推力控制部P3中，把实现目标推力u_G2d(t)所需的对执行器的推力指令u_Ed(t)定义为下式。

(算式12)

$s_G\left(t\right)={J_{\mathrm{EG}}}^{+T}\left(t\right)\{({\hat{u}}_{G2}\left(t\right)-u_{G2d}\left(t\right))+T_G\frac{d}{\mathrm{dt}}{\hat{u}}_{G2}\left(t\right)\}---\left(19\right)$

${\left[u_{\mathrm{Ed}}\left(t\right)\right]}_i=\left\{\begin{array}{cc}{u}_{\mathrm{Edi}\max }& ({\left[s_G\left(t\right)\right]}_i<0)\\ 0& ({\left[s_G\left(t\right)\right]}_i=0)\\ u_{\mathrm{Edi}\min }& ({\left[s_G\left(t\right)\right]}_i>0)\end{array}\right.---\left(20\right)$

其中，

(m维矢量)是u_G2(t)的推定值。

在本例中，不需要对推力计划部P2的反馈，对推力控制部P3的反馈所需的值只是式(17)的u_G2(t)的推定值

。这里，说明该u_G2(t)的推定方法。

下面，对本例中的门推力u_G2(t)的推定进行说明。

把门推力u_G2(t)的推定值

定义为下式：

(算式13)

${\hat{u}}_{G2}\left(t\right)=u_{\mathrm{Gs}}\left(t\right)+{J_{\mathrm{GB}}}^{-T}{M}_B{g}_{\mathrm{Bs}}\left(t\right)---\left(21\right)$

这里，考察上述门推力推定的合理性。对式(21)进行拉普拉斯变换，成为：

${\hat{U}}_{G2}\left(s\right)=u_{\mathrm{Gs}}\left(s\right)+{J_{\mathrm{GB}}}^{-T}{M}_B{g}_{\mathrm{Bs}}\left(s\right)---{\left(21\right)}^{,}$

这里，如果满足上述的传感器特性一致条件，则式(12)、(13)也同样成立。将它们代入式(17)，则成为：

(算式14)

${\hat{u}}_{G2}\left(s\right)=G_s\left(s\right)u_{G2}\left(s\right)---\left(22\right)$

根据式(21)可进行门推力u_G2(t)的推定。由于在式(22)中未明示出重力加速度g_Bs(t)，所以，只要满足传感器特性一致条件，即可视为直接测定了门推力u_G2(t)。

第3例

下面，对本实施方式的推力传输门***的第3例进行说明。

推力传输门***的第3例的结构如图5、6所示。图5是浮游移动体的概略图，图6是控制***的方框图。

在该第3例中，浮游移动体10未被力学分离成主体部B和执行器部E，并且也没有硬件的推力传输门G。但本例的浮游移动体10具有加速度传感器2和倾斜角传感器3。

在本例中，通过适当地确定主体部B的惯性矩阵M_B，并且根据该M_B，从传感器的测定值计算在图6的计算机P内所示出的虚拟推力传输门G’的推力推定值，视为存在推力传输门来构成推力传输门***。关于表现浮游移动体10的几何学构造的J_GB、J_EG，与上述第1例子、第2例子一样，采用预先测定的已知的矩阵。

另外，作为主体部B的惯性矩阵M_B的确定方法，例如，可列举出如下的方法等，即，利用计算机根据设计浮游移动体10时的设计图、主要规格等预先求出主体部B的惯性矩阵M_B，并把其存储在未图示的存储器等中。

下面，对本例的构造进行详细说明。

参照图6的主体部B和执行器部E的内部，则表示浮游移动体的物理动力特性的运动方程式一般可写成下式。

(算式15)

J_GB ^Tu_Gv(t)＝M_Ba_B(t)(23)

u_Gv(t)＝J_EG ^T(t)u_E(t)+f_E(t)+J_GB ^-TM_Bg_B(t) (24)

式(23)是表示主体部的式子，式(24)是表示执行器部的式子。虽然这些与第2例的情况大致相同，但在该第3例中，由于不存在物理上的推力传输门，所以需要注意推力传输门的推力u_Gv(m维矢量)是虚拟值。

下面，对本例的推力计划部进行说明。

如图6的计算机P内的推力计划部P2的方框部分所示，把实现主体部B的目标加速度aBd(t)所需的虚拟推力传输门G’的目标推力u_Gvd(t)定义为下式。

(算式16)

u_Gvd(t)＝J_GB ^-TM_Ba_Bd(t)     (25)

在图6的计算机P内，在由参照符号P3表示的推力控制部中，通过控制u_Gv(t)，使得利用反馈实现该u_Gvd(t)，可以构成利用推力反馈来控制推力的、延迟少的反馈控制***。

这里，如果在本例的推力控制中应用例如滑模控制，则可构成如下的推力控制部P3。

在本例的推力控制部P3中，把实现目标推力u_Gvd(t)所需的对执行器的推力指令u_Ed定义为下式。

(算式17)

$s_G\left(t\right)={J_{\mathrm{EG}}}^{+T}\left(t\right)\{({\hat{u}}_{\mathrm{Gv}}\left(t\right)-u_{\mathrm{Gvd}}\left(t\right))+T_G\frac{d}{\mathrm{dt}}{\hat{u}}_{\mathrm{Gv}}\left(t\right)\}---\left(26\right)$

${\left[u_{\mathrm{Ed}}\left(t\right)\right]}_i=\left\{\begin{array}{cc}{u}_{\mathrm{Edi}\max }& ({\left[s_G\left(t\right)\right]}_i<0)\\ 0& ({\left[s_G\left(t\right)\right]}_i=0)\\ u_{\mathrm{Edi}\min }& ({\left[s_G\left(t\right)\right]}_i>0)\end{array}\right.---\left(27\right)$

其中，

(m维矢量)是u_Gv(t)的推定值。

另外，在本例中与上述第2例一样，不需要对推力计划部P2的反馈，对推力控制部P3的反馈所需的值只是u_Gv(t)的推定值

。这里，对该u_Gv(t)的推定方法进行说明。

下面，对本例中的门推力u_Gv(t)的推定进行说明。

将门推力u_Gv(t)的推定值

定义为下式。

(算式18)

${\hat{u}}_{\mathrm{Gv}}\left(t\right)={J_{\mathrm{GB}}}^{-T}{M}_B(a_{\mathrm{Bs}}\left(t\right)+g_{\mathrm{Bs}}\left(t\right))---\left(28\right)$

这里，考察上述门推力推定的合理性。对式(28)进行拉普拉斯变换，而成为

${\hat{u}}_{\mathrm{Gv}}\left(s\right)={J_{\mathrm{GB}}}^{-T}{M}_B(a_{\mathrm{Bs}}\left(s\right)+g_{\mathrm{Bs}}\left(s\right))---{\left(28\right)}^{,}$

这里也是如果满足上述的传感器特性一致条件，则式(11)、(12)同样成立。如果把它们代入式(23)，则成为：

(算式19)

${\hat{u}}_{\mathrm{Gv}}\left(s\right)=G_s\left(s\right)u_{\mathrm{Gv}}\left(s\right)---\left(29\right)$

利用式(28)可进行门推力u_Gv(t)的推定。由于在式(29)中未明示出重力加速度g_Bs(t)，所以只要满足传感器特性一致条件，就可视为直接测定了门推力u_Gv(t)。

(第2实施方式)

下面，对本发明的浮游移动体的加速度检测***的一个实施方式进行说明。与第1实施方式一样，本实施方式也是把水下机器人作为适用对象来进行以下说明。另外，水下机器人是在三维空间中浮游的浮游移动体，在对该水下机器人应用本发明的加速度检测***的情况下，与第1实施方式一样，需要对重力加速度的影响进行补偿，因此，需要具备可测量主体部的倾斜的倾斜角测定装置。

在本实施方式中，利用推力传输门进行主体部加速度的推定。本实施方式涉及的加速度检测***的结构如图3和图7所示。图3是浮游移动体的概略图，图7是加速度检测***的方框图。

在此前的第1实施方式的各例中，作为浮游移动体10的控制***1构筑了推力传输门***，而在第2实施方式中，作为浮游移动体10的高精度的加速度传感***，将利用推力传输门G。

本实施方式的硬件结构与前面的第1实施方式涉及的第2例(图3)完全相同。即，将浮游移动体10力学分离成主体部B和执行器部E，在主体部B中具有倾斜角传感器3，在推力传输门G中具有力/扭矩传感器。

不过，由于在本实施方式中的目的是高精度地推定主体部加速度a_B(t)，而不是进行控制，所以在本实施例中不包含推力计划部和推力控制部。

另外，主体部的惯性矩阵M_B和表现浮游移动体的几何学构造的J_GB，采用预先测定的已知的常数矩阵。通过考虑推力传输门的力/扭矩传感器的测定范围、和在本实施方式中应检测的加速度范围来适当地设计该主体部惯性矩阵M_B，可实现高精度的加速度测定。

下面，对本实施方式的结构进行详细说明。

在图3和图7所示的本实施方式的硬件结构中，对式(15)进行变形而将主体部加速度a_B(t)表示为下式。

(算式20)

a_B(t)＝M_B ^-1J_GB ^Tu_G(t)+g_B(t)    (30)

接着，对本实施方式的主体部加速度a_B(t)的推定进行说明。

把主体部加速度a_B(t)的推定值

定义为下式。

(算式21)

${\hat{a}}_B\left(t\right)={M_B}^{-1}{J_{\mathrm{GB}}}^T{u}_{\mathrm{Gs}}\left(t\right)+g_{\mathrm{Gs}}\left(t\right)---\left(31\right)$

这里，考察上述主体部加速度推定的合理性。对式(31)进行拉普拉斯变换，成为下式。

(算式22)

${\hat{a}}_B\left(s\right)={M_B}^{-1}{J_{\mathrm{GB}}}^T{u}_{\mathrm{Gs}}\left(s\right)+g_{\mathrm{Bs}}\left(s\right)---\left(32\right)$

这里，如果假设满足上述的传感器特性一致条件，则与第1实施方式一样，式(12)、(13)成立。如果把它们代入式(30)，则成为下式。

(算式23)

${\hat{a}}_B\left(s\right)=G_s\left(s\right)a_B\left(s\right)---\left(33\right)$

利用式(31)可进行主体加速度a_B(t)的推定。

实施例1

如在上述第1实施方式中说明的那样，在本发明的浮游移动体的控制***中，为了进行推力控制，严格地讲，所实现的不是位置/速度轨道，而是加速度的轨道。这里，因加速度的积分误差而产生的漂移的影响不可避免，因此，本发明可对比较高的频带的运动单独发挥显著的效果。而对低频的缓慢运动需要并用以往的反馈位置/速度的控制方法，但是，即便是在这种情况下，与单独使用以往的方法相比，也能提高精度。

下面，作为本发明的一个实施例，对在上述第1实施方式中所说明的本发明的浮游移动体的控制***的各例中并用位置/速度反馈控制的控制***进行说明。

这里，图8是表示本发明的控制***的一个实施例的方框图，其相当于上述第1实施方式的第1例，图9是表示本发明的控制***的另一实施例的方框图，其相当于上述第1实施方式的第2例，图10是表示本发明的控制***的另一实施例的方框图，其相当于上述第1实施方式的第3例。在上述图8～10中，对与上述的图1～7以及图11～14相同的部分赋予相同的符号来进行说明。

另外，与图2、4、6和7一样，在图8～10中连接在各个方框间的细实线表示信号。另一方面，在浮游移动体10内的连接在各方框之间的双线，表示作为物理作用而在浮游移动体10上产生的现象(力学转换作用等)。

实施例1相当于上述第1实施方式的第1例。图8表示本实施例涉及的控制***1的方框图。本实施例的浮游移动体10的概略结构如先前的图1所示。

根据图8的方框图可知，本实施例是把在上述第1实施方式的第1例中所说明的图2所示的结构，应用在已知的位置/速度反馈控制***中的实施例。因此，基于前面的图1和图2来说明本发明的推力传输门***部分的结构以及控制***的动作等。

本实施例涉及的控制***1、即推力传输门***的主要特征是，把由参照符号10所示的浮游移动体的机构力学分离成接受推力的主体部B和产生推力的执行器部E，并在其结合部分附加了推力传输门G。这里，推力传输门G是将主体部B和执行器部E结合起来的唯一部分，并具有能够测量施加在主体部B与执行器部E之间的所有力和扭矩的传感器(力/扭矩传感器)。另外，对于力/扭矩传感器，可使用市场销售的通用产品(可输出多维的力和扭矩分量的多通道产品等)。

这样，在本实施例中，通过利用推力传输门G来结合主体部B和执行器部E，构成了能够实际检测出从执行器部E作用于主体部B的所有的推力。如图1所示，应用了推力传输门***的本实施例的浮游移动体10，在主体部B中具有加速度传感器2和倾斜角传感器3。

具有上述结构的本实施例涉及的控制***1，在进行已知的位置/速度反馈控制的同时，还进行基于本发明的推力传输门***的推力反馈控制。

根据本实施例，可并用以往的反馈位置/速度的控制方法来避免因加速度的积分误差而引起的漂移的影响，并且通过使用本发明的控制***、即推力传输门***，即使在例如波浪、潮流、风、或者安装在浮游移动体上的机械臂的反作用等的干扰下，也能够使浮游移动体高精度地静止在规定位置，或者高精度地追踪目标轨道，因此，能够对浮游移动体进行更高速且更高精度的控制。

实施例2

本实施例相当于上述第1实施方式的第2例。图9是表示本实施例涉及的控制***1的方框图。本实施例的浮游移动体10的概略结构如前面的图3所示。

与实施例1一样，本实施例涉及的控制***1的主要特征也是，把水下机器人10的机构力学分离成接受推力的主体部B、和产生推力的执行器部E，并在其结合部分附加了推力传输门G。

但是本实施例与上述实施例1的不同之处在于：主体部B被收纳在执行器部E的内部，构成为来自外部的干扰和机械臂A的反作用等全都对作为耐压壳S的执行器部E产生影响。这里，主体部B在执行器部E的内部，只通过推力传输门G来承担。因此，在推力传输门G是将主体部B和执行器部E结合起来的唯一部分这一点，与上述实施例1相同。

因此，本实施例也构成为通过利用推力传输门G将主体部B与执行器部E结合起来，而能够实际检测出执行器部E作用于主体部B的所有推力的结构。

根据本实施例2，也可并用以往的反馈位置/速度的控制方法来避免因加速度的积分误差而引起的漂移的影响，并且通过使用本发明的控制***、即推力传输门***，即使是在例如波浪、潮流、风、或者安装在浮游移动体上的机械臂的反作用等的干扰下，也能够使浮游移动体高精度地静止在规定位置，或者高精度地追踪目标轨道，因此，能够对浮游移动体进行更高速且更高精度的控制。

实施例3

本实施例相当于上述第1实施方式的第3例。图10是表示本实施例涉及的控制***1的方框图。本实施例的浮游移动体10的概略结构如前面的图5所示。

本实施例涉及的控制***1的主要的特征是，把水下机器人的机构在计算上分成接受推力的主体部B、和产生推力的执行器部E，并在两者之间虚拟地设置了虚拟推力传输门G’。

另外，虚拟推力传输门G’能够推定出施加在主体部B和执行器部E之间的全部的力和扭矩。

根据本实施例3，也能够并用以往的反馈位置/速度的控制方法来避免因加速度的积分误差所引起的漂移的影响，并且通过使用本发明的控制***、即推力传输门***，即使是在例如波浪、潮流、风、或者安装在浮游移动体上的机械臂的反作用等的干扰下，也能够使浮游移动体高精度地静止在规定位置，或者高精度地追踪目标轨道，因此，能够对浮游移动体进行更高速且更高精度的控制。

(利用了惯性导航装置的安装)

被广泛用于航空器等浮游移动体的作为现有技术的惯性导航装置可作为本发明中所使用的加速度传感器、倾斜角传感器、主体部位置/速度传感器使用。

即，只要是装备了惯性导航装置的浮游移动体，无须任何硬件的追加、变更，而只需改***件，即可安装例如本实施例3或上述第1实施方式的第3例涉及的本发明的控制***，因此，可更容易地实现现有的浮游移动体的高速、高精度化。

更详细地讲，在现有的惯性导航装置中，现状是无论是否以足够高的精度测量加速度，在控制中都是只使用通过对其积分而获得的位置/速度信息。然而，在本发明的推力传输门***的构成中，通过在推力测定中使用至今为止还未被加以利用的加速度信息，在硬件与以往相同的情况下，与现有的控制相比，能实现更高的速度、更高的精度。

特别是，对于上述惯性导航装置而言，最近把加速度传感器、陀螺仪、GPS(Global Positioning System)以及其它的传感器组件化来获得加速度、速度和位移信息的混合型惯性导航装置已成为主流，从整体上提高了测量速度。因此，可以说上述惯性导航装置是可在安装本发明的控制***时加以利用的优选的传感器组件。

另外，本发明并不限于上述各例所记载的结构，能够进行各种设计变化以及其它的变形。

首先，在上述各例中，针对把本发明的控制***应用于水下机器人的示例进行了说明，但作为适用本发明的控制***的对象不限于水下机器人。本发明的控制***除了水下机器人以外，如上述定义部分中所列举的示例所示，可适用于潜水艇、直升飞机、飞船、航空器、宇宙飞船或宇宙机器人等水下、空中或宇宙空间等中的浮游移动体。另外，本发明的控制***也可适用于在水上或地上滑行的气垫式交通工具、和在车轮处于打滑状态下的汽车等在平面上滑行的各种移动体。

另外，在上述实施方式的各例以及上述各实施例中，对把本发明的控制***或加速度检测***应用于在三维空间中浮游的浮游移动体的情况进行了说明。但是，在把本发明的控制***或加速度检测***应用于在根本没有重力作用的宇宙空间浮游的浮游移动体、可以不考虑倾斜或者可忽略倾斜的在水平面上滑行的浮游移动体时，如作为上述(1)～(6)的发明所规定的那样可不需要倾斜角测定装置，此时，在推力计划部、推力控制部中通过使施加于浮游移动体主体部(重心)的重力加速度g_B＝0，可采用更简单的控制法则。

另外，在上述各例中，作为推力控制部P3的控制法则的一个例子，对执行器动态特性的非线性，使用了作为鲁棒的现有控制方法的滑模控制，但推力控制部P3的控制法则不限于此。因此，例如作为推力控制部P3的控制法则，也可以对推力传输门G的目标推力与推力传输门G的推力测定或推定值的误差使用推进器T输入电压的PID控制。

并且，在上述各例中，作为推进器T使用了基于与作为驱动源的马达机械连接的螺旋桨的推力发生设备，但推进器T的结构不限于此。因此，推进器T也可以是喷气喷射式推进器，而且该驱动源也不限于马达，可采用内燃机、蒸汽涡轮机、原子能涡轮机等各种形式的推进器。

如上所述，本发明是一种新型且非常有用的发明，其提供了一种可高速且高精度地控制浮游移动体的***结构，即使在例如波浪、潮流、风、或者安装在浮游移动体上的机械臂的反作用等的干扰下，也能够使浮游移动体高精度地静止在规定位置，或者高精度地追踪目标轨道。

Claims (7)

1.一种浮游移动体的控制***，其特征在于，上述浮游移动体包括：

主体部，占上述浮游移动体的一部分，并可视为单一性刚体；

执行器部，对上述浮游移动体产生推力；以及

推力传输门，是对上述主体部和上述执行器部进行力学结合的部分，构成为可实际测定从上述执行器部作用于上述主体部的推力，并且，

上述主体部具有可测量上述浮游移动体的加速度的加速度测定装置，

使用来自上述主体部的上述加速度测定装置的输出、和来自上述推力传输门的推力测定值，来得到对上述执行器部的推力指令。

2.一种浮游移动体的控制***，其特征在于，上述浮游移动体包括：

主体部，占上述浮游移动体的一部分，并可视为单一性刚体；

执行器部，对上述浮游移动体产生推力；以及

推力传输门，是对上述主体部和上述执行器部进行力学结合的部分，构成为可实际测定从上述执行器部作用于上述主体部的推力，

上述主体部构成为：只与上述推力传输门进行力学结合，而且作用于上述主体部的推力实质上全部通过上述执行器部和上述推力传输门来输入，

使用来自上述推力传输门的推力测定值来得到对上述执行器部的推力指令。

3.根据权利要求1所述的浮游移动体的控制***，其特征在于，

上述推力传输门具有可测量施加在上述主体部与上述执行器部之间的力或扭矩的力或扭矩测定装置，

使用来自上述推力传输门的上述力或扭矩测定装置的输出、来自上述浮游移动体的上述加速度测定装置的输出、以及转换来自操作者的目标位置速度轨道指令而得到的目标加速度轨道指令，来得到对上述执行器部的推力指令。

4.根据权利要求2所述的浮游移动体的控制***，其特征在于，

上述推力传输门具有可测量施加在上述主体部与上述执行器部之间的力或扭矩的力或扭矩测定装置，

使用来自上述推力传输门的上述力或扭矩测定装置的输出、以及转换来自操作者的目标位置速度轨道指令而得到的目标加速度轨道指令，来得到对上述执行器部的推力指令。

5.一种浮游移动体的控制***，其特征在于，上述浮游移动体具有可测量上述浮游移动体的加速度的加速度测定装置，

上述浮游移动体在计算上，包括：

虚拟主体部，占上述浮游移动体的一部分，并可视为单一性刚体；

虚拟执行器部，对上述浮游移动体产生推力；以及

虚拟推力传输门，是对上述虚拟主体部和上述虚拟执行器部进行虚拟结合的部分，构成为可使用来自上述加速度测定装置的输出，来推定从上述虚拟执行器部作用于上述虚拟主体部的推力，

使用来自上述虚拟推力传输门的推力推定值，来得到对上述虚拟执行器部的推力指令。

6.根据权利要求5所述的浮游移动体的控制***，其特征在于，

上述虚拟推力传输门具有可推定施加在上述虚拟主体部与上述虚拟执行器部之间的力或扭矩的力或扭矩推定装置，

使用来自上述虚拟推力传输门的上述力或扭矩推定装置的输出、以及转换来自操作者的目标位置速度轨道指令而得到的目标加速度轨道指令，来得到对上述虚拟执行器部的推力指令。

7.根据权利要求1～6的任意一项所述的浮游移动体的控制***，其特征在于，

上述主体部或上述虚拟主体部还具有可测量上述浮游移动体的倾斜的倾斜角测定装置，

还利用来自上述倾斜角测定装置的输出，来得到对上述执行器部或上述虚拟执行器部的推力指令。

Images