一种ROV悬停定位控制方法

一种rov悬停定位控制方法
技术领域
1.本发明涉及水下机器人的运动控制技术领域，具体地说是一种基于rov导航定位系统采集的定位信息以及rov的运动能力，实现rov悬停于水中，近似一动不动的控制方法。


背景技术：

2.海洋自然资源的挖掘、开采必将成为未来人类的重点开采方向。随着人类需求的不断提升，陆地自然资源的逐年开采，陆地上自然资源必将无法满足人类未来的需求，而除了陆地自然资源以外，还有大量自然矿物资源蕴藏于占地球表面积71％的海洋之中，这些自然资源将是人类未来生存能源供给的不二之选。我国是陆地大国，同时也是海洋大国，但我国对海洋资源的开发利用还处于初级阶段，面对日益严峻的资源紧缺和海洋主权维护，对海洋装备的发展就显得尤为迫切。
3.随着人类对海洋资源开发的不断深入以及各类水下机器人装备的开发应用，有缆遥控水下机器人(rov)因其活动范围广、电源充足、负载量大、可操作强等自身特点，能够在水下完成观察、摄像、打捞和施工等作业，在海洋资源开发中得到了广泛的应用，其开发技术受到了各国的极大重视。目前国内的作业型rov大多通过外国进口，不但价格昂贵，而且配套服务难，有些rov并不适合我国海洋环境使用，其抗流能力和作业能力都明显不足。
4.目前，随着rov的发展与应用，rov的功能已经不仅仅局限于在水底低速航行或坐底作业了，而是需要具有如悬停定位、轨迹跟踪等更高的运动控制性能来满足水下作业的各种需求。目前，有许多rov的执行任务需要利用rov对目标物进行更细致的观察和作业，这就需要rov相对于目标物的位置保持不变，但是海洋环境是比较复杂的，经常有洋流、暗涌等外力干扰，严重影响了rov位置的稳定。因此，为了让rov在海底具备更多精确复杂的作业能力，研究实现rov的悬停定位控制有非常重要的实际意义。


技术实现要素：

5.针对rov需要具备相对于目标物的位置保持不变、可提供更细致的观察和作业能力，本发明提供了一种即简单又实用的rov海底悬停定位控制方法。其是以rov的深度或高度、航向、北向位置和东向位置等的当前值为设定值进行闭环控制，获取前进/后退、左移/右移、左转/右转、上浮/下潜等四个方向上的运动控制量，并将各运动控制量输入rov推力分配矩阵进行推力分配，得到各个推进器的推力控制量，控制推进器旋转，从而使rov具有了相对于目标物位置保持不变的能力，同时又具有较强的抗扰动能力。
6.本发明为实现上述目的所采用的技术方案是：一种rov悬停定位控制方法，通过推进器的旋转实现rov悬停于水中，包括以下步骤：
7.首先实时获取包含rov的深度或高度、航向位置、北向位置和东向位置的导航定位信息；
8.当rov处于海底能够连续获得导航定位信息时，进入悬停定位控制；否则，放弃悬停定位控制；
9.进入悬停定位控制后，以潜水器的北向位置、东向位置、高度或深度、航向位置的当前值为设定值，对潜水器的北向位置、东向位置、用于表示高度或深度的z向位置和用于表示航向角的ψ向角度四个自由度进行闭环控制，得到四个运动方向的闭环控制量；
10.将北向位置和东向位置的闭环输出推力控制量进行坐标变换，得到以载体为参考的运动推力控制量，和其余两个闭环输出推力控制量一起进行运动推力分配，从而获取每个推进器的推力控制量；
11.输出推力控制量给各推进器，控制各推进器旋转，从而完成前进/后退、左移/右移、左转/右转、上浮/下潜四个自由度的闭环运动，使rov悬停于水中。
12.rov的导航定位系统实时连续采集rov的深度或高度、航向位置、东向位置和北向位置。
13.rov的前进/后退、左移/右移、左转/右转、上浮/下潜、左摇/右摇、俯艏/抬艏方向上的运动推力和各个推进器的输出推力对应关系如公式1所示：
[0014][0015]
其中，t1、t2、t3、t4、t5、t6、t7分别对应7个推进器的输出推力，f
x
、fy、fz分别对应前进/后退即沿x向、左移/右移即沿y向和上浮/下潜即沿z向三个方向上的运动推力，m
x
、my、mz分别对应左摇/右摇即绕x向、俯艏/抬艏即绕y向和左转/右转即绕z向三个方向上的运动推力，α为水平布置各推进器的桨轴与潜水器纵向轴线即x向的夹角，采用45
°
，β为前部垂向布置的、左右对称的2台推进器相对于它们中心线的倾斜角度，采用20
°
，a、b、c、d分别为水平布置的各推进器参考潜水器重心得出的力臂。
[0016]
对航向进行闭环控制，获得左转/右转的运动推力控制值。
[0017]
对深度或高度进行闭环控制，获得上浮/下潜的运动推力控制值。
[0018]
对北向位置进行闭环控制，获得北向/南向移动的运动推力控制值。
[0019]
对东向位置进行闭环控制，获得东向/西向移动的运动推力控制值。
[0020]
对北向和东向的运动推力控制值采用公式2进行坐标变换，转换为前进/后退、左移/右移两个方向上的运动推力控制量f
x
、fy；
[0021][0022]
其中，fn为北向或南向移动的运动推力控制值，fe为东向或西向移动的运动推力控制值，ψ为实时检测的rov航向角。
[0023]
将获得的前进/后退、左移/右移、上浮/下潜、左转/右转的闭环运动推力控制量带入公式1进行各个推进器输出推力的求解，得到rov悬停时各个推进器的输出推力控制量，然后输出给各推进器，控制推进器旋转，以实现rov悬停定位。
[0024]
在潜水器上设置的7台推进器布置如下：
[0025]
水平方向的4台推进器采用矢量布置形式，分别安装在潜水器上框架下部的左前、右前、左后、右后4个位置上，呈平行四边形结构，与潜水器的纵向轴线夹角各为45
°
，用于潜水器的前进/后退、左移/右移、左转/右转运动；
[0026]
垂向方向的3台推进器中，前部两台以潜水器的纵向轴线左右对称布置，并分别向外侧下方倾斜20
°
，后面一台布置在潜水器艉部中央，并垂直向下，用于潜水器的上浮/下潜、左摇/右摇、俯艏/抬艏运动。
[0027]
本发明具有以下有益效果和优点：
[0028]
(1)本发明的rov悬停定位控制方法，具有较强的抗扰动能力，可以使rov在近似静止状态下对目标物进行更细致的观察和作业；
[0029]
(2)本发明的rov悬停定位控制方法，具有较高的位置控制精度，在很多作业场合可以代替手操控制，有利于减轻驾驶人员的疲劳感；
[0030]
(3)本发明的rov悬停定位控制方法，实现简单，适用于大部分rov的升级改造。
附图说明
[0031]
图1是本发明的导航定位系统示意图；
[0032]
图2是本发明的推进器布置示意图；
[0033]
图3是本发明的控制原理示意图。
具体实施方式
[0034]
下面结合附图及实施例对本发明做进一步的详细说明。
[0035]
一种rov悬停定位控制方法，可以通过控制推进器的旋转实现rov相对于目标物的位置保持不变，其特征在于包括以下步骤：
[0036]
悬停定位时，以进入悬停定位一瞬间的深度或高度、航向、北向位置和东向位置的当前值为设定值，分别进行闭环控制，得到四个运动方向的闭环推力控制量；
[0037]
将北向位置和东向位置的闭环推力控制量转换为载体坐标系下的前进/后退和左移/右移运动控制量；
[0038]
将以上最终得到的四个闭环推力控制量，代入rov运动推力与推进器推力对应关系方程，求解获取各个推进器的推力控制量；
[0039]
将各推进器的推力控制量输出给各推进器，控制各推进器旋转，使rov悬停于水中。
[0040]
本发明的悬停定位功能需要实时获取潜水器的深度或高度(z向位置)、航向角(ψ向)、北向位置和东向位置，这四个值由rov携带的导航定位传感器来采集，主要包括深度计、高度计、多普勒计程仪(dvl)和光纤惯导，如图1所示。深度计可以测得rov与水面的垂直距离，高度计可以测得rov与海底的垂直距离，多普勒计程仪则可以在海底航行时、测得rov的前向与右向速度信息，光纤惯导则可以测得rov的航向角度。与此同时，光纤惯导的测量信息和dvl的测量信息进行数据融合，还可以计算出rov的北向位置和东向位置信息。
[0041]
本发明悬停定位功能主要由布置在潜水器上的7台推进器来完成的，其布置情况如图2(俯视图)所示。水平方向的4台推进器采用矢量布置形式，分别安装在上框架下部的左前、右前、左后、右后4个位置上，呈平行四边形结构，与潜水器的纵向轴线夹角各为45
°
；
垂向方向的3台推进器中，前部两台以潜水器的纵向轴线左右对称布置，并分别向外侧下方倾斜20
°
，后面一台布置在潜水器艉部中央，并垂直向下。推进器的该种布置方式可以使潜水器产生6个自由度的运动，即前进/后退(沿x向)、左移/右移(沿y向)、上浮/下潜(沿z向)、左转/右转(ψ向)、左摇/右摇、俯艏/抬艏，各方向运动推力与各个推进器的推力对应关系如下公式1所示：
[0042][0043]
其中，t1、t2、t3、t4、t5、t6、t7分别对应7个推进器的输出推力，f
x
、fy、fz分别对应前进/后退(沿x向)、左移/右移(沿y向)和上浮/下潜(沿z向)三个方向上的运动推力，m
x
、my、mz分别对应左摇/右摇(绕x向)、俯艏/抬艏(绕y向)和左转/右转(绕z向)三个方向上的运动推力，α为水平布置各推进器的桨轴与潜水器纵向轴线(x向)的夹角，采用45
°
，β为前部垂向布置的、左右对称的2台推进器相对于它们中心线的倾斜角度，采用20
°
，a、b、c、d分别为水平布置的各推进器参考潜水器重心得出的力臂。
[0044]
本发明悬停定位功能方法原理图如图3所示。其中，η为东向位移，ξ为北向位移，z为深度或高度，ηi为东向位移闭环控制设定值，ξi为北向位移闭环控制设定值，zi为深度或高度闭环控制设定值，ψi为艏向角闭环控制设定值，f
*
为剩余自由度上的运动推力输入。在悬停定位时，需要以当前值为设定值对潜水器的北向位置、东向位置、z向位置(高度或深度)和ψ向角度(航向)四个自由度进行闭环控制，得到四个控制分量，分别为fn、fe、fz、mz；然后采用公式2对fn、fe进行坐标变换，转变为载体坐标系下相应的x向和y向下的控制量，最终得到x向、y向、z向、ψ向四个运动方向上的控制量，分别为f
x
、fy、fz、mz，最后代入公式1，求解得到每个推进器的推力控制量并输出，控制各个推进器旋转，进而使rov保持在悬停点上。
[0045][0046]
其中，fn为北向或南向移动的运动推力控制值，fe为东向或西向移动的运动推力控制值，ψ为实时检测的rov航向角。
[0047]
当rov在近海低航行、多普勒计程仪(dvl)可以测得rov的运动速度时，方可进入悬停定位控制；当离海底距离较远或者dvl无法测得运动速度时，需及时退出悬停定位控制。悬停定位的具体实施步骤如下：
[0048]
第一步，首先判断是否满足悬停定位条件，满足进入下一步，否则退出悬停定位功能；
[0049]
第二步，对航向进行闭环控制，获得左转/右转的推力控制值；
[0050]
第三步，对深度或高度进行闭环控制，获得上浮/下潜的推力控制值；
[0051]
第四步，对北向位置进行闭环控制，获得北向/南向移动的推力控制值；
[0052]
第五步，对东向位置进行闭环控制，获得东向/西向移动的推力控制值；
[0053]
第六步，对第四步和第五步获取的推力控制值，采用公式2进行坐标变换，获得前进/后退和左移/右移两个方向上的推力控制值；
[0054]
第七步，将获得的前进/后退、左移/右移、左转/右转和上浮/下潜4个推力控制值和其余自由度上的运动推力输入值代入公式1，求解得到各个推进器的推力控制值，并输出给各个推进器，控制各个推进器旋转；
[0055]
以上步骤，循环执行，即可完成rov悬停定位功能。
[0056]
由于以上所述rov导航定位系统、推进器器布置方式及具体的实施步骤仅为本发明的具体实施对象和方法而已，并不用于限制本发明，因此，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

技术特征：
1.一种rov悬停定位控制方法，其特征在于，通过推进器的旋转实现rov悬停于水中，包括以下步骤：首先实时获取包含rov的深度或高度、航向位置、北向位置和东向位置的导航定位信息；当rov处于海底能够连续获得导航定位信息时，进入悬停定位控制；否则，放弃悬停定位控制；进入悬停定位控制后，以潜水器的北向位置、东向位置、高度或深度、航向位置的当前值为设定值，对潜水器的北向位置、东向位置、用于表示高度或深度的z向位置和用于表示航向角的ψ向角度四个自由度进行闭环控制，得到四个运动方向的闭环控制量；将北向位置和东向位置的闭环输出推力控制量进行坐标变换，得到以载体为参考的运动推力控制量，和其余两个闭环输出推力控制量一起进行运动推力分配，从而获取每个推进器的推力控制量；输出推力控制量给各推进器，控制各推进器旋转，从而完成前进/后退、左移/右移、左转/右转、上浮/下潜四个自由度的闭环运动，使rov悬停于水中。2.根据权利要求1所述的一种rov悬停定位控制方法，其特征在于，rov的导航定位系统实时连续采集rov的深度或高度、航向位置、东向位置和北向位置。3.根据权利要求1所述的一种rov悬停定位控制方法，其特征在于rov的前进/后退、左移/右移、左转/右转、上浮/下潜、左摇/右摇、俯艏/抬艏方向上的运动推力和各个推进器的输出推力对应关系如公式1所示：其中，t1、t2、t3、t4、t5、t6、t7分别对应7个推进器的输出推力，f
x
、f
y
、f
z
分别对应前进/后退即沿x向、左移/右移即沿y向和上浮/下潜即沿z向三个方向上的运动推力，m
x
、m
y
、m
z
分别对应左摇/右摇即绕x向、俯艏/抬艏即绕y向和左转/右转即绕z向三个方向上的运动推力，α为水平布置各推进器的桨轴与潜水器纵向轴线即x向的夹角，采用45
°
，β为前部垂向布置的、左右对称的2台推进器相对于它们中心线的倾斜角度，采用20
°
，a、b、c、d分别为水平布置的各推进器参考潜水器重心得出的力臂。4.根据权利要求1所述的一种rov悬停定位控制方法，其特征在于，对航向进行闭环控制，获得左转/右转的运动推力控制值。5.根据权利要求1所述的一种rov悬停定位控制方法，其特征在于，对深度或高度进行闭环控制，获得上浮/下潜的运动推力控制值。6.根据权利要求1所述的一种rov悬停定位控制方法，其特征在于，对北向位置进行闭环控制，获得北向/南向移动的运动推力控制值。7.根据权利要求1所述的一种rov悬停定位控制方法，其特征在于，对东向位置进行闭
环控制，获得东向/西向移动的运动推力控制值。8.根据权利要求1所述的一种rov悬停定位控制方法，其特征在于，对北向和东向的运动推力控制值采用公式2进行坐标变换，转换为前进/后退、左移/右移两个方向上的运动推力控制量f
x
、f
y
；其中，f
n
为北向或南向移动的运动推力控制值，f
e
为东向或西向移动的运动推力控制值，ψ为实时检测的rov航向角。9.根据权利要求1所述的一种rov悬停定位控制方法，其特征在于将获得的前进/后退、左移/右移、上浮/下潜、左转/右转的闭环运动推力控制量带入公式1进行各个推进器输出推力的求解，得到rov悬停时各个推进器的输出推力控制量，然后输出给各推进器，控制推进器旋转，以实现rov悬停定位。10.根据权利要求1所述的一种rov悬停定位控制方法，其特征在于，在潜水器上设置的7台推进器布置如下：水平方向的4台推进器采用矢量布置形式，分别安装在潜水器上框架下部的左前、右前、左后、右后4个位置上，呈平行四边形结构，与潜水器的纵向轴线夹角各为45
°
，用于潜水器的前进/后退、左移/右移、左转/右转运动；垂向方向的3台推进器中，前部两台以潜水器的纵向轴线左右对称布置，并分别向外侧下方倾斜20
°
，后面一台布置在潜水器艉部中央，并垂直向下，用于潜水器的上浮/下潜、左摇/右摇、俯艏/抬艏运动。

技术总结
本发明涉及水下机器人技术领域，具体来说是一种ROV悬停定位控制方法。本发明依托的ROV可以实时获得深度或高度、航向、北向位置和东向位置等信息，并具备前进/后退、左移/右移、左转/右转、上浮/下潜等运动能力。ROV进入悬停定位功能时，通过对深度或高度、航向、北向位置和东向位置等进行闭环算法获取4个控制量，并将获取的后2个控制量进行坐标变换，转换为ROV的前进/后退与左移/右移2个控制量，然后输入推力分配矩阵进行推力分配，从而获得各个推进器的控制量。本发明简单实用，使ROV具有较好的抗流能力，满足ROV海底作业时对悬停定位的需求。满足ROV海底作业时对悬停定位的需求。满足ROV海底作业时对悬停定位的需求。


技术研发人员：崔胜国 张奇峰 冀萌凡 孔范东 张斌 齐心
受保护的技术使用者：中国科学院沈阳自动化研究所
技术研发日：2020.11.23
技术公布日：2022/5/25