CN105607476A - 一种作业型rov六自由度运动控制方法 - Google Patents

Abstract

本发明属于无人水下机器人运动控制技术领域，具体涉及一种过驱动作业型遥控水下机器人的作业型ROV六自由度运动控制方法。本发明包括：建立ROV水动力学模型；建立作业型ROV在六个自由度方向的控制力和力矩向量；计算本体坐标系{b}中作业型ROV的运动状态；建立ROV的运动学方程；将ROV本体坐标系中的角速度运动状态转化为大地坐标系中来表示；将ROV本体坐标系中的角位移状态转化为大地坐标系中来表示；根据输入的大地坐标系中的ROV期望航向角，计算在本体坐标系中ROV的期望航向角。本发明中可以逼真的模拟通过外部控制输入来操纵作业型ROV运动及姿态变化的过程，具有能够逼真的模拟水下作业过程中ROV的各种运动及控制参数变化过程的优点。

Description

一种作业型ROV六自由度运动控制方法

技术领域

本发明属于无人水下机器人运动控制技术领域，具体涉及一种过驱动作业型遥控水下机器人(ROV—RemotelyOperatedVehicle)的作业型ROV六自由度运动控制方法。

背景技术

作业型ROV的最大特点是能在危险深海环境中完成高强度、大负荷和高精度作业，它是我国实施深海资源开发战略不可或缺的重大技术装备之一。作业型ROV装备的核心技术之一是设计一种具有六自由度操纵控制能力的运动控制系统，这对保证ROV水下作业能力，提高作业效率，保证水下作业的安全性，具有重要的理论意义和工程价值。

ROV六自由度运动控制系统是开发作业型ROV装备的关键技术之一。作业型ROV运动控制系统的设计难点主要包括以下四个方面：(1)一般水下机器人只需要控制三个(进退、纵倾、转艏)自由度运动，而为了保证机械手的水下作业能力，作业型ROV需要同时控制六个自由度运动(进退、横移、升沉、横倾、纵倾、转艏)。(2)作业型ROV需要控制变量的种类很多，如：操纵手柄需要控制四个方向(进退、横移、升沉、转艏)的运动速度；速度微调旋钮需要控制三个方向的速度(进退、横移、升沉)；同时具有自动保持航向、保持深度、保持高程控制功能；还要具有通过软件如数控制改变ROV当前位置及姿态的能力等。(3)作业型ROV引导律算法比较复杂，需要实现大地坐标系和本体坐标系之间ROV六自由度运动及姿态值转换的引导律算法。目前，由于技术比较敏感，国内外很少有公开关于作业型ROV运动控制系统设计方法的文献资料。

本发明在已知作业型ROV水动力学模型的基础上，设计一种作业型ROV六自由度运动控制系统，能够模拟操纵手柄、速度微调旋钮、自动按钮控制、软件输入控制等功能，来实时控制作业型ROV的六自由度运动及姿态变化。本发明最终可应用于作业型ROV仿真训练模拟器的设计中，也可以应用于实际作业型ROV运动控制系统设计中。

发明内容

本发明目的在于提供一种作业型ROV六自由度运动控制方法。

本发明的目的是这样实现的：

(1)建立ROV水动力学模型：

建立作业型ROV在本体坐标系{b}中的六自由度的水动力学方程：

$M\stackrel{\·}{\mathrm{\υ}}+C\left(\mathrm{\υ}\right)\mathrm{\υ}+D\left(\mathrm{\υ}\right)\mathrm{\υ}+g\left(\mathrm{\η}\right)=\mathrm{\τ}$

式中：M∈(6×6)为系统惯性矩阵；C(υ)∈(6×6)为科氏向心力矩阵；D(υ)∈(6×6)为由水动力引起阻尼力矩阵；g(η)∈(6×1)为重力和浮力引起的力和力矩向量；υ∈(6×1)为ROV的速度和角速度向量；为ROV的加速度和角加速度向量；τ∈(6×1)为ROV的推力及推力矩向量；

(2)建立作业型ROV在六个自由度方向的控制力和力矩向量：

作业型ROV总共安装8台呈矢量布置的液压螺旋桨推进器，将在ROV本体坐标系{b}中的六个方向产生控制力及力矩向量：

τ＝[X_TY_TZ_TK_TM_TN_T]^T

(3)计算本体坐标系{b}中作业型ROV的运动状态：

根据作业型ROV模型试验得到的水动力参数结果，计算出式中ROV水动力学模型中的系数矩阵M、C(υ)、D(υ)、g(η)，将六自由度的控制力和力矩向量τ输入到ROV水动力学模型，即可得到本体坐标系{b}中的运动状态，包括速度[uvwpqr]^T和位移[XYZαβγ]^T；

(4)建立ROV的运动学方程，将ROV本体坐标系{b}中的线速度状态转化为大地坐标系{n}中来表示：

ROV在本体坐标系{b}中的速度向量为：

υ＝[vω]^T

式中：v＝[uvw]^T为ROV在本体坐标系中三个线速度向量；ω＝[pqr]^T为ROV在本体坐标系中的三个角速度向量；

大地坐标系中ROV的线速度向量为：

$\stackrel{\·}{p}=R\left(\mathrm{\Θ}\right)v$

式中： $\stackrel{\·}{p}={\left[\begin{array}{ccc}\stackrel{\·}{N}& \stackrel{\·}{E}& \stackrel{\·}{D}\end{array}\right]}^T$ 为大地坐标系的线速度向量；Θ＝[φθψ]^T为大地坐标系{n}和本体坐标系{b}之间的欧拉角向量；R(Θ)为本体坐标系{b}到大地坐标系{n}的线速度转换矩阵；

$R\left(\mathrm{\Θ}\right)=\left[\begin{array}{ccc}c\mathrm{\ψ}c\mathrm{\θ}& -s\mathrm{\ψ}c\mathrm{\φ}+c\mathrm{\ψ}s\mathrm{\θ}s\mathrm{\φ}& s\mathrm{\ψ}s\mathrm{\φ}+c\mathrm{\ψ}c\mathrm{\φ}s\mathrm{\θ}\\ s\mathrm{\ψ}c\mathrm{\θ}& c\mathrm{\ψ}c\mathrm{\φ}+s\mathrm{\φ}s\mathrm{\θ}s\mathrm{\ψ}& -c\mathrm{\ψ}s\mathrm{\φ}+s\mathrm{\θ}s\mathrm{\ψ}c\mathrm{\φ}\\ -s\mathrm{\θ}& c\mathrm{\θ}s\mathrm{\φ}& c\mathrm{\θ}c\mathrm{\φ}\end{array}\right]$

式中：c表示为cos(·)，s表示为sin(·)；

(5)建立ROV的运动学方程，将ROV本体坐标系{b}中的位移状态转化为大地坐标系{n}中来表示，

大地坐标系{n}中ROV的位移向量：

p＝∫R(Θ)vdv

式中：p＝[NED]^T为大地坐标系{n}中的位移向量

(6)将ROV本体坐标系中的角速度运动状态转化为大地坐标系中来表示：

在大地坐标系中ROV的角速度向量可由下式计算：

$\stackrel{\·}{\mathrm{\Θ}}=T\left(\mathrm{\Θ}\right)\mathrm{\ω}$

式中： $\stackrel{\·}{\mathrm{\Θ}}={\left[\begin{array}{ccc}\stackrel{\·}{\mathrm{\φ}}& \stackrel{\·}{\mathrm{\θ}}& \stackrel{\·}{\mathrm{\ψ}}\end{array}\right]}^T$ 为大地坐标系中水下机器人的角速度向量；T(Θ)为本体坐标系到大地坐标系的角速度转换矩阵；

$T\left(\mathrm{\Θ}\right)=\left[\begin{array}{ccc}1& s\mathrm{\φ}t\mathrm{\θ}& s\mathrm{\φ}t\mathrm{\θ}\\ 0& c\mathrm{\φ}& -s\mathrm{\φ}\\ 0& s\mathrm{\φ}/c\mathrm{\θ}& c\mathrm{\φ}/c\mathrm{\θ}\end{array}\right]$

式中：t表示为tan(·)；

(7)将ROV本体坐标系中的角位移状态转化为大地坐标系中来表示：

大地坐标系中ROV的角位移向量：

Θ＝∫T(Θ)ωdω

(8)计算本体坐标系ROV期望位移值和大地坐标系中ROV期望位移值之间的关系：

本体坐标系中ROV三个方向的线速度：

$v=R^{-1}\left(\mathrm{\Θ}\right)\stackrel{\·}{p}$

式中：R^-1(Θ)——R(Θ)的逆矩阵；

本体坐标系中ROV三个方向的位移为：

$P=\underset{0}{\overset{i}{\∫}}vdt=\underset{0}{\overset{i}{\∫}}{R}^{-1}\left(\mathrm{\Θ}\right)\stackrel{\·}{p}dt$

式中：P＝[XYZ]^T为本体坐标系中的ROV的三个方向位移向量；

(9)根据控制输入的大地坐标系中的ROV期望位移值[N₀E₀D₀]^T，计算在本体坐标系中ROV的期望位移值[X₀Y₀Z₀]^T

[X₀Y₀Z₀]^T＝R^-1(Θ)[N₀E₀D₀]^T

(10)计算本体坐标系ROV期望姿态角和大地坐标系中ROV期望姿态角之间的关系：

本体坐标系中ROV三个方向的角速度为：

$\mathrm{\ω}=T^{-1}\left(\mathrm{\Θ}\right)\stackrel{\·}{\mathrm{\Θ}}$

式中：T^-1(Θ)为T(Θ)的逆矩阵；

本体坐标系中ROV三个方向的角位移为：

$Q=\underset{0}{\overset{t}{\∫}}\mathrm{\ω}dt=\underset{0}{\overset{t}{\∫}}{T}^{-1}\left(\mathrm{\Θ}\right)\stackrel{\·}{\mathrm{\Θ}}dt$

式中：Q＝[αβγ]^T为本体坐标系中的ROV的角位移向量；

(11)根据输入的大地坐标系中的ROV期望航向角，计算在本体坐标系中ROV的期望航向角：

在大地坐标系中ROV的期望姿态角为[φ₀θ₀ψ₀]^T，则本体坐标系中的期望姿态角值为[α₀β₀γ₀]^T为：

[α₀β₀γ₀]^T＝T^-1(Θ)[φ₀θ₀ψ₀]^T

式中：α₀为本体坐标系中的期望横倾角，对作业型ROV来说α₀＝0；β₀为本体坐标系中的期望纵倾角，对作业型ROV来说β₀＝0；γ₀为本体坐标系中的期望航向角；

对作业型ROV来说，本体坐标系中的期望航向角为：

γ₀＝ψ₀；

(12)建立作业型ROV外部控制输入的功能框图：ROV外部控制输入包括：ROV操纵面板输入控制和ROV软件输入控制，ROV操纵面板输入控制包括：操纵手柄输入、微调旋钮、自动按钮控制、ROV软件输入控制包括：键盘输入相对位置和软件自动控制；

ROV外部控制命令的优先级是：ROV操纵面板输入控制和ROV软件输入控制优先级相同；对于ROV操纵面板输入控制来说：操纵手柄输入命令优先级高于自动按钮控制，保持高程和保持深度命令优先级相同，操纵手柄优先级高于微调旋钮命令；微调旋钮优先级高于自动按钮控制命令；对于ROV软件输入控制命令来说，其命令优先级的顺序与ROV操纵面板输入控制命令优先级顺序一致；

(13)构建ROV六自由度运动控制器；

(14)通过ROV外部控制输入来实现ROV的六自由度运动控制：

ROV的各种外部控制命令经过控制命令逻辑判断模块，输出有效控制命令至ROV运动引导律计算模块和ROV运动控制器；ROV运动控制器输出六个方向的推力和推力矩，操纵ROV按照期望控制命令运动、定向或悬停定位。

本发明的有益效果在于：

本发明中的作业型ROV六自由度运动控制系统具有功能齐全、层次结构清晰、组成简单的特点，可以比较真实的描述作业型ROV的内部控制体系结构组成和引导控制过程，可以逼真的模拟通过外部控制输入来操纵作业型ROV运动及姿态变化的过程，具有能够逼真的模拟水下作业过程中ROV的各种运动及控制参数变化过程的优点。

附图说明

图1作业型ROV六自由度运动控制系统框图；

图2作业型ROV外部输入的功能框图；

图3作业型ROV纵向运动控制器；

图4作业型ROV横向运动控制器；

图5作业型ROV垂向运动控制器；

图6作业型ROV转艏运动控制器；

图7作业型ROV横倾运动控制器；

图8作业型ROV纵倾运动控制器。

具体实施方式

下面结合附图对本发明做进一步描述。

基于某作业型ROV水动力学模型，设计了一种作业型ROV六自由度运动控制系统，包含：ROV本体水动力学模型、ROV运动学方程模块、ROV六自由度运动控制器、ROV运动引导律计算模块、ROV操纵面板输入控制模块和ROV软件控制输入模块。

ROV六个自由度的运动控制器都是在ROV本体坐标系中采用双闭环PID控制器，其中内环是速度环，采用PI控制算法；外环是位置环，采用PD控制算法。纵向、横向、垂向和转艏的速度控制命令可由内环输入，实现ROV的进退、横向、升沉和转艏方向的速度控制。纵向、横向、垂向位置和航向角的位置控制命令可由外环输入，实现ROV的纵向、横向、垂向位置和航向角的稳定控制。横倾和纵倾控制器都具有自动姿态保持功能，没有外部控制输入信号，能够实现水下作业时ROV姿态自动保持。

作业型ROV的外部控制可输入四个方向(纵向、横向、垂向和转艏方向)的控制命令，可按照公式(12)和(16)所示的位置和姿态引导律算法，将大地坐标系中的ROV期望位置和期望航向角，分别转化成本体坐标系中的ROV期望位置和期望航向角，并发送给本体坐标系中ROV六自由度运动控制器，进而在本体坐标系中实现ROV的位置和姿态控制。这样设计控制器的优点：一是可实现在大地坐标系中ROV的相对位置和航向角准确控制；二是可以使操纵手柄的速度控制命令输入到ROV内环控制器，从而实现本体坐标系中的ROV四自由度(纵向、横向、垂向和转艏方向)的运动速度控制。

本发明包括：ROV本体水动力学模型、ROV运动学方程模块、ROV六自由度运动控制器、ROV运动引导律计算模块、ROV操纵面板输入控制模块和ROV软件输入控制模块。如图1所示，本发明可实现以下功能：可通过模拟ROV操纵面板输入控制和软件输入控制命令来分别操纵控制作业型ROV的六自由度运动及姿态变化。如图2所示，ROV操纵面板输入控制模块包括操纵手柄、微调旋钮、自动按钮控制；软件输入控制模块包括键盘输入相对位置命令和软件自动控制命令。

如图1所示，本发明的目的是这样实现的：公式(1)所示的是作业型ROV本体水动力学模型，可输出本体坐标系{b}中的ROV六自由度运动状态，包括：速度向量[uvwpqr]^T和加速度向量 ${\left[\begin{array}{cccccc}\stackrel{\·}{u}& \stackrel{\·}{v}& \stackrel{\·}{w}& \stackrel{\·}{p}& \stackrel{\·}{q}& \stackrel{\·}{r}\end{array}\right]}^T.$ 然后，将坐标系{b}中的ROV六自由度运动状态输入到ROV运动学方程模块，分别按照公式(4)和公式(7)、公式(6)和公式(9)的算法，并经过积分运算，将其转化为大地坐标系{n}中的ROV六自由度运动状态，包括：速度向量 ${\left[\begin{array}{cccccc}\stackrel{\·}{N}& \stackrel{\·}{E}& \stackrel{\·}{D}& \stackrel{\·}{\mathrm{\φ}}& \stackrel{\·}{\mathrm{\θ}}& \stackrel{\·}{\mathrm{\ψ}}\end{array}\right]}^T$ 和位移向量[NEDφθψ]^T。然后，将坐标系{b}和{n}中ROV的运动状态都输入到ROV六自由度运动控制器中。同时，将ROV操纵面板输入控制或ROV软件输入控制命令发送到控制命令逻辑判断模块，经过优先级及逻辑判断，输出需要执行的控制命令。然后将需执行控制命令输入到ROV运动引导律计算模块，将计算出的引导律再输入到ROV运动控制器中；同时，需要执行的控制命令也输入到ROV运动控制器中，并在运动引导律的导引下，通过ROV六自由度运动控制算法，输出六个方向的推力和推力矩，如公式(2)所示。推力和推力矩最后作用到ROV水动力学模型上，从而实现控制ROV运动和姿态按照期望命令进行变化。

图2所示的是ROV外部输入的控制功能框图。其中，ROV操纵面板输入控制模块包括：操纵手柄、微调旋钮、自动按钮控制；ROV软件输入控制包括：键盘输入相对位置和软件自动控制命令。操纵手柄可控制ROV四个自由度的运动速度：进退、横移、升沉和转艏运动。微调旋钮可精确控制ROV三个自由度的运动速度：进退、横移和升沉运动。自动按钮控制可实现三种位置自动控制功能：保持高程、保持航向和保持深度。键盘输入相对位置命令可实现操纵控制ROV相对当前位置移动一段距离，包括：纵向和横向相对位置。软件自动控制包括：保持高程、保持航向、保持深度。

ROV六自由度运动控制算法是这样实现的：首先ROV六自由度运动控制器采集ROV在坐标系{b}和{n}中的速度、角速度、位移和角位移信息，在ROV引导律作用下，再经过作业型ROV六自由度运动控制算法，最终分别输出六自由度的推力和推力矩，来实现作业型ROV六自由度运动和姿态的控制。ROV六个自由度运动控制器都是采用双闭环PID控制器，其中内环是速度环，采用PI控制算法；外环是位置环，采用PD控制算法。图3是作业型ROV的纵向运动控制器，操纵手柄的纵向速度控制命令(u₀)可由“纵向速度控制”端输入，实现ROV的进退速度控制；ROV的纵向位置控制命令(X₀)可由位置环的“纵向位置控制”端输入。图4是作业型ROV的横向运动控制器，操纵手柄的横向速度控制命令(v₀)可由“横向速度控制”端输入，实现ROV的左右横移速度控制；ROV的横向位置控制命令(Y₀)可由位置环的“横向位置控制”端输入。图5是作业型ROV的垂向运动控制器，操纵手柄的垂向速度控制命令(w₀)可由“垂向速度控制”端输入，实现ROV的升沉速度控制；ROV的垂向位置控制命令(Z₀)可由位置环的“垂向位置控制”端输入。图6是作业型ROV的转艏运动控制器，操纵手柄的转艏速度控制命令(r₀)可由“转艏角速度控制”端输入，实现ROV的左右转艏速度控制；ROV的航向角控制命令(γ₀)可由位置环的“航向角控制”端输入。图7是作业型ROV的横倾运动控制器，图8是作业型ROV的纵倾运动控制器，横倾和纵倾控制器都具有自动姿态保持功能，没有外部控制输入信号，能够实现水下作业时ROV横倾和纵倾姿态自动保持。

ROV引导律是这样实现的：作业型ROV的外部控制可输入四个方向(纵向、横向、垂向和转艏方向)的控制命令，可按照公式(12)和(16)所示的位置和航向角引导律算法，将大地坐标系{n}中的ROV期望位置和期望航向角(N₀、E₀、D₀、ψ₀)，分别转化成本体坐标系{b}中的ROV期望位置和期望航向角(X₀、Y₀、Z₀、γ₀)，并输出给本体坐标系{b}中ROV六自由度运动控制器，进而在本体坐标系{b}中实现ROV的位置和姿态控制。这样设计控制器的优点：一是可实现在大地坐标系{n}中ROV的位置和航向角准确控制；二是可以使操纵手柄的速度控制命令输入到ROV内环控制器，从而实现本体坐标系{b}中的ROV四自由度的运动速度控制。

ROV六自由度运动控制器具有如下特点：首先，为了实现ROV在大地坐标系下的精确位置和航向控制，外部输入的期望位置和姿态是大地坐标系{n}中的期望控制量(N₀、E₀、D₀、ψ₀)，需要将其转化为本体坐标系{b}中的期望控制量(X₀、Y₀、Z₀、γ₀)，再将它们输入到六自由度运动控制器。其次，六个运动控制器(纵向、横向、垂向、转艏、横倾和纵倾)的输入量都是ROV本体坐标系中的运动状态，如：速度(u、v、w、p、q、r)和位移值(X、Y、Z、α、β、γ)，然后在本体坐标系{b}中采用双闭环PID控制算法实现作业型ROV的运动及姿态控制。再者，由于ROV在横倾及纵倾方向没有外部控制输入，故在这两个方向无需引入引导律算法，只需要在本体坐标系{b}中实现ROV横倾及纵倾方向姿态控制即可。

下面给出本发明专利的具体实施方式，并结合附图1-8说明，具体步骤如下：

第一步，建立如图1中所示的ROV水动力学模型。

建立作业型ROV在本体坐标系{b}中的六自由度的水动力学方程：

$M\stackrel{\·}{\mathrm{\υ}}+C\left(\mathrm{\υ}\right)\mathrm{\υ}+D\left(\mathrm{\υ}\right)\mathrm{\υ}+g\left(\mathrm{\η}\right)=\mathrm{\τ}---\left(1\right)$

式中：M∈(6×6)——系统惯性矩阵；C(υ)∈(6×6)——科氏向心力矩阵；D(υ)∈(6×6)——由水动力引起阻尼力矩阵；g(η)∈(6×1)——重力和浮力引起的力和力矩向量；υ∈(6×1)——ROV的速度和角速度向量；——ROV的加速度和角加速度向量；τ∈(6×1)——ROV的推力及推力矩向量。

第二步，建立作业型ROV在六个自由度方向的控制力和力矩向量。

本发明所研究的作业型ROV总共安装8台呈矢量布置的液压螺旋桨推进器，将在ROV本体坐标系{b}中的六个方向产生控制力及力矩向量：

τ＝[X_TY_TZ_TK_TM_TN_T]^T(2)

第三步，计算本体坐标系{b}中作业型ROV的运动状态。

可根据作业型ROV模型试验得到的水动力参数结果，计算出式中ROV水动力学模型中的系数矩阵M、C(υ)、D(υ)、g(η)。将六自由度的控制力和力矩向量τ输入到ROV水动力学模型，即可得到本体坐标系{b}中的运动状态，如：速度[uvwpqr]^T和位移[XYZαβγ]^T。

第四步，建立ROV的运动学方程，将ROV本体坐标系{b}中的线速度状态转化为大地坐标系{n}中来表示。

ROV在本体坐标系{b}中的速度向量如下所示：

υ＝[vω]^T(3)

式中：v＝[uvw]^T——ROV在本体坐标系中三个线速度向量；ω＝[pqr]^T——ROV在本体坐标系中的三个角速度向量。

大地坐标系中ROV的线速度向量可由下式计算：

$\stackrel{\·}{p}=R\left(\mathrm{\Θ}\right)v---\left(4\right)$

式中： $\stackrel{\·}{p}={\left[\begin{array}{ccc}\stackrel{\·}{N}& \stackrel{\·}{E}& \stackrel{\·}{D}\end{array}\right]}^T$ ——大地坐标系的线速度向量；Θ＝[φθψ]^T——大地坐标系{n}和本体坐标系{b}之间的欧拉角向量；R(Θ)——本体坐标系{b}到大地坐标系{n}的线速度转换矩阵。

$R\left(\mathrm{\Θ}\right)=\left[\begin{array}{ccc}c\mathrm{\ψ}c\mathrm{\θ}& -s\mathrm{\ψ}c\mathrm{\φ}+c\mathrm{\ψ}s\mathrm{\θ}s\mathrm{\φ}& s\mathrm{\ψ}s\mathrm{\φ}+c\mathrm{\ψ}c\mathrm{\φ}s\mathrm{\θ}\\ s\mathrm{\ψ}c\mathrm{\θ}& c\mathrm{\ψ}c\mathrm{\φ}+s\mathrm{\φ}s\mathrm{\θ}s\mathrm{\ψ}& -c\mathrm{\ψ}s\mathrm{\φ}+s\mathrm{\θ}s\mathrm{\ψ}c\mathrm{\φ}\\ -s\mathrm{\θ}& c\mathrm{\θ}s\mathrm{\φ}& c\mathrm{\θ}c\mathrm{\φ}\end{array}\right]---\left(5\right)$

式中：c表示为cos(·)，s表示为sin(·)。

第五步，建立ROV的运动学方程，将ROV本体坐标系{b}中的位移状态转化为大地坐标系{n}中来表示。

大地坐标系{n}中ROV的位移向量：

p＝∫R(Θ)vdv(6)

式中：p＝[NED]^T——大地坐标系{n}中的位移向量

第六步，将ROV本体坐标系中的角速度运动状态转化为大地坐标系中来表示。

在大地坐标系中ROV的角速度向量可由下式计算：

$\stackrel{\·}{\mathrm{\Θ}}=T\left(\mathrm{\Θ}\right)\mathrm{\ω}---\left(7\right)$

式中： $\stackrel{\·}{\mathrm{\Θ}}={\left[\begin{array}{ccc}\stackrel{\·}{\mathrm{\φ}}& \stackrel{\·}{\mathrm{\θ}}& \stackrel{\·}{\mathrm{\ψ}}\end{array}\right]}^T$ ——大地坐标系中水下机器人的角速度向量；T(Θ)——本体坐标系到大地坐标系的角速度转换矩阵。

$T\left(\mathrm{\Θ}\right)=\left[\begin{array}{ccc}1& s\mathrm{\φ}t\mathrm{\θ}& s\mathrm{\φ}t\mathrm{\θ}\\ 0& c\mathrm{\φ}& -s\mathrm{\φ}\\ 0& s\mathrm{\φ}/c\mathrm{\θ}& c\mathrm{\φ}/c\mathrm{\θ}\end{array}\right]---\left(8\right)$

式中：t表示为tan(·)。

第七步，将ROV本体坐标系中的角位移状态转化为大地坐标系中来表示。

大地坐标系中ROV的角位移向量：

Θ＝∫T(Θ)ωdω(9)

第八步，计算本体坐标系ROV期望位移值和大地坐标系中ROV期望位移值之间的关系。

本体坐标系中ROV三个方向的线速度按下式计算：

$v=R^{-1}\left(\mathrm{\Θ}\right)\stackrel{\·}{p}---\left(10\right)$

式中：R^-1(Θ)——R(Θ)的逆矩阵。

本体坐标系中ROV三个方向的位移可按下式计算：

$P=\underset{0}{\overset{t}{\∫}}vdt=\underset{0}{\overset{t}{\∫}}{R}^{-1}\left(\mathrm{\Θ}\right)\stackrel{\·}{p}dt---\left(11\right)$

式中：P＝[XYZ]^T——本体坐标系中的ROV的三个方向位移向量。

第九步，根据控制输入的大地坐标系中的ROV期望位移值[N₀E₀D₀]^T，计算在本体坐标系中ROV的期望位移值[X₀Y₀Z₀]^T。

[X₀Y₀Z₀]^T＝R^-1(Θ)[N₀E₀D₀]^T(12)

第十步，计算本体坐标系ROV期望姿态角和大地坐标系中ROV期望姿态角之间的关系。

本体坐标系中ROV三个方向的角速度可按下式计算：

$\mathrm{\ω}=T^{-1}\left(\mathrm{\Θ}\right)\stackrel{\·}{\mathrm{\Θ}}---\left(13\right)$

式中：T^-1(Θ)——T(Θ)的逆矩阵。

本体坐标系中ROV三个方向的角位移可按下式计算：

$Q=\underset{0}{\overset{t}{\∫}}\mathrm{\ω}dt=\underset{0}{\overset{t}{\∫}}{T}^{-1}\left(\mathrm{\Θ}\right)\stackrel{\·}{\mathrm{\Θ}}dt---\left(14\right)$

式中：Q＝[αβγ]^T——本体坐标系中的ROV的角位移向量。

第十一步，根据输入的大地坐标系中的ROV期望航向角，计算在本体坐标系中ROV的期望航向角。

假如在大地坐标系中ROV的期望姿态角为[φ₀θ₀ψ₀]^T，则本体坐标系中的期望姿态角值为[α₀β₀γ₀]^T可由下式计算：

[α₀β₀γ₀]^T＝T^-1(Θ)[φ₀θ₀ψ₀]^T(15)

式中：α₀——本体坐标系中的期望横倾角，对作业型ROV来说α₀＝0；β₀——本体坐标系中的期望纵倾角，对作业型ROV来说β₀＝0；γ₀——本体坐标系中的期望航向角。

对作业型ROV来说，本体坐标系中的期望航向角可由下式计算：

γ₀＝ψ₀(16)

第十二步，按照图2，建立作业型ROV外部控制输入的功能框图。ROV外部控制输入包括：ROV操纵面板输入控制和ROV软件输入控制。其中，ROV操纵面板输入控制包括：操纵手柄输入(纵向速度、横向速度、垂向速度和转艏速度)、微调旋钮(纵向速度、横向速度和垂向速度)、自动按钮控制(保持航向、保持高程和保持深度)。ROV软件输入控制包括：键盘输入相对位置(纵向位置、横向位置)和软件自动控制(保持航向、保持高程、保持深度)。

ROV外部控制命令的优先级是这样实现的：1)ROV操纵面板输入控制和ROV软件输入控制优先级相同，可通过切换功能按钮来转移两者对ROV的控制权。2)对于ROV操纵面板输入控制来说：操纵手柄输入命令优先级高于自动按钮控制，例如：操纵手柄的转艏速度命令优先级高于保持航向命令，操纵手柄的垂向速度命令优先级高于保持高程和保持深度命令。保持高程和保持深度命令优先级相同，当其中一个命令有效时，另一个命令自动失效。操纵手柄优先级高于微调旋钮命令。微调旋钮优先级高于自动按钮控制命令。3)对于ROV软件输入控制命令来说，其命令优先级的顺序与ROV操纵面板输入控制命令优先级顺序一致。

第十三步，构建ROV六自由度运动控制器。按照图3-8分别建立ROV六自由度运动控制器，如：纵向、横向、垂向、转艏、横倾和纵倾运动控制器。ROV四自由度操纵手柄可分别通过对应控制器的速度环对ROV进行纵向、横向、垂向和转艏速度进行控制。当软件输入控制功能和自动按钮控制功能有效时，对应的位置环控制可通过引导律算法实现，引导律算法如公式(12)和(16)所示。图7是ROV横倾运动控制器，图8是ROV纵倾运动控制器，它们没有外部控制输入，具有自动保持ROV横倾和纵倾姿态的功能。由于ROV在水下经常使用两个机械手进行各种水下作业任务，在横倾和纵倾方向经常处于较大的力矩不平衡状态，横倾和纵倾姿态控制对作业型ROV的稳定运动控制是非常重要的。

第十四步，通过ROV外部控制输入来实现ROV的六自由度运动控制。首先ROV的各种外部控制命令经过控制命令逻辑判断模块，输出有效控制命令至ROV运动引导律计算模块和ROV运动控制器。ROV运动控制器输出六个方向的推力和推力矩，操纵ROV按照期望控制命令运动、定向或悬停定位。

第十五步，实现作业型ROV六自由度的操纵运动控制仿真。按照以上步骤，在Simulink中建立ROV六自由度运动仿真控制系统。然后输入ROV的外部控制输入命令，通过控制命令逻辑判断模块输出有效控制命令，通过引导律算法模块输出引导命令，最后通过ROV运动控制系统实现了ROV的六自由度的运动和姿态控制。本发明最终在半物理仿真系统——作业型ROV仿真训练模拟器上实现。

Claims (1)

1.一种作业型ROV六自由度运动控制方法，其特征在于：

(1)建立ROV水动力学模型：

建立作业型ROV在本体坐标系{b}中的六自由度的水动力学方程：

$M\stackrel{\·}{\mathrm{\υ}}+C\left(\mathrm{\υ}\right)\mathrm{\υ}+D\left(\mathrm{\υ}\right)\mathrm{\υ}+g\left(\mathrm{\η}\right)=\mathrm{\τ}$

式中：M∈(6×6)为系统惯性矩阵；C(υ)∈(6×6)为科氏向心力矩阵；D(υ)∈(6×6)为由水动力引起阻尼力矩阵；g(η)∈(6×1)为重力和浮力引起的力和力矩向量；υ∈(6×1)为ROV的速度和角速度向量；为ROV的加速度和角加速度向量；τ∈(6×1)为ROV的推力及推力矩向量；

(2)建立作业型ROV在六个自由度方向的控制力和力矩向量：

作业型ROV总共安装8台呈矢量布置的液压螺旋桨推进器，将在ROV本体坐标系{b}中的六个方向产生控制力及力矩向量：

τ＝[X_TY_TZ_TK_TM_TN_T]^T

(3)计算本体坐标系{b}中作业型ROV的运动状态：

根据作业型ROV模型试验得到的水动力参数结果，计算出式中ROV水动力学模型中的系数矩阵M、C(υ)、D(υ)、g(η)，将六自由度的控制力和力矩向量τ输入到ROV水动力学模型，即可得到本体坐标系{b}中的运动状态，包括速度[uvwpqr]^T和位移[XYZαβγ]^T；

(4)建立ROV的运动学方程，将ROV本体坐标系{b}中的线速度状态转化为大地坐标系{n}中来表示：

ROV在本体坐标系{b}中的速度向量为：

υ＝[vω]^T

式中：v＝[uvw]^T为ROV在本体坐标系中三个线速度向量；ω＝[pqr]^T为ROV在本体坐标系中的三个角速度向量；

大地坐标系中ROV的线速度向量为：

$\stackrel{\·}{p}=R\left(\mathrm{\Θ}\right)v$

式中： $\stackrel{\·}{p}={\left[\begin{array}{ccc}\stackrel{\·}{N}& \stackrel{\·}{E}& \stackrel{\·}{D}\end{array}\right]}^T$ 为大地坐标系的线速度向量；Θ＝[φθψ]^T为大地坐标系{n}和本体坐标系{b}之间的欧拉角向量；R(Θ)为本体坐标系{b}到大地坐标系{n}的线速度转换矩阵；

$R\left(\mathrm{\Θ}\right)=\left[\begin{array}{ccc}c\mathrm{\ψ}c\mathrm{\θ}& -s\mathrm{\ψ}c\mathrm{\φ}+c\mathrm{\ψ}s\mathrm{\θ}s\mathrm{\φ}& s\mathrm{\ψ}s\mathrm{\φ}+c\mathrm{\ψ}c\mathrm{\φ}s\mathrm{\θ}\\ s\mathrm{\ψ}c\mathrm{\θ}& c\mathrm{\ψ}c\mathrm{\φ}+s\mathrm{\φ}s\mathrm{\θ}s\mathrm{\ψ}& -c\mathrm{\ψ}s\mathrm{\φ}+s\mathrm{\θ}s\mathrm{\ψ}c\mathrm{\φ}\\ -s\mathrm{\θ}& c\mathrm{\θ}s\mathrm{\φ}& c\mathrm{\θ}c\mathrm{\φ}\end{array}\right]$

式中：c表示为cos(·)，s表示为sin(·)；

(5)建立ROV的运动学方程，将ROV本体坐标系{b}中的位移状态转化为大地坐标系{n}中来表示，

大地坐标系{n}中ROV的位移向量：

p＝∫R(Θ)vdv

式中：p＝[NED]^T为大地坐标系{n}中的位移向量

(6)将ROV本体坐标系中的角速度运动状态转化为大地坐标系中来表示：

在大地坐标系中ROV的角速度向量可由下式计算：

$\stackrel{\·}{\mathrm{\Θ}}=T\left(\mathrm{\Θ}\right)\mathrm{\ω}$

式中： $\stackrel{\·}{\mathrm{\Θ}}={\left[\begin{array}{ccc}\stackrel{\·}{\mathrm{\φ}}& \stackrel{\·}{\mathrm{\θ}}& \stackrel{\·}{\mathrm{\ψ}}\end{array}\right]}^T$ 为大地坐标系中水下机器人的角速度向量；T(Θ)为本体坐标系到大地坐标系的角速度转换矩阵；

$T\left(\mathrm{\Θ}\right)=\left[\begin{array}{ccc}1& s\mathrm{\φ}t\mathrm{\θ}& s\mathrm{\φ}t\mathrm{\θ}\\ 0& c\mathrm{\φ}& -s\mathrm{\φ}\\ 0& s\mathrm{\φ}/c\mathrm{\θ}& c\mathrm{\φ}/c\mathrm{\θ}\end{array}\right]$

式中：t表示为tan(·)；

(7)将ROV本体坐标系中的角位移状态转化为大地坐标系中来表示：

大地坐标系中ROV的角位移向量：

Θ＝∫T(Θ)ωdω

(8)计算本体坐标系ROV期望位移值和大地坐标系中ROV期望位移值之间的关系：

本体坐标系中ROV三个方向的线速度：

$v=R^{-1}\left(\mathrm{\Θ}\right)\stackrel{\·}{p}$

式中：R^-1(Θ)——R(Θ)的逆矩阵；

本体坐标系中ROV三个方向的位移为：

$P=\underset{0}{\overset{t}{\∫}}vdt=\underset{0}{\overset{t}{\∫}}{R}^{-1}\left(\mathrm{\Θ}\right)\stackrel{\·}{p}dt$

式中：P＝[XYZ]^T为本体坐标系中的ROV的三个方向位移向量；

(9)根据控制输入的大地坐标系中的ROV期望位移值[N₀E₀D₀]^T，计算在本体坐标系中ROV的期望位移值[X₀Y₀Z₀]^T

[X₀Y₀Z₀]^T＝R^-1(Θ)[N₀E₀D₀]^T

(10)计算本体坐标系ROV期望姿态角和大地坐标系中ROV期望姿态角之间的关系：

本体坐标系中ROV三个方向的角速度为：

$\mathrm{\ω}=T^{-1}\left(\mathrm{\Θ}\right)\stackrel{\·}{\mathrm{\Θ}}$

式中：T^-1(Θ)为T(Θ)的逆矩阵；

本体坐标系中ROV三个方向的角位移为：

$Q=\underset{0}{\overset{t}{\∫}}\mathrm{\ω}dt=\underset{0}{\overset{t}{\∫}}{T}^{-1}\left(\mathrm{\Θ}\right)\stackrel{\·}{\mathrm{\Θ}}dt$

式中：Q＝[αβγ]^T为本体坐标系中的ROV的角位移向量；

(11)根据输入的大地坐标系中的ROV期望航向角，计算在本体坐标系中ROV的期望航向角：

在大地坐标系中ROV的期望姿态角为[φ₀θ₀ψ₀]^T，则本体坐标系中的期望姿态角值为[α₀β₀γ₀]^T为：

[α₀β₀γ₀]^T＝T^-1(Θ)[φ₀θ₀ψ₀]^T

式中：α₀为本体坐标系中的期望横倾角，对作业型ROV来说α₀＝0；β₀为本体坐标系中的期望纵倾角，对作业型ROV来说β₀＝0；γ₀为本体坐标系中的期望航向角；

对作业型ROV来说，本体坐标系中的期望航向角为：

γ₀＝ψ₀；

(12)建立作业型ROV外部控制输入的功能框图：ROV外部控制输入包括：ROV操纵面板输入控制和ROV软件输入控制，ROV操纵面板输入控制包括：操纵手柄输入、微调旋钮、自动按钮控制、ROV软件输入控制包括：键盘输入相对位置和软件自动控制；

ROV外部控制命令的优先级是：ROV操纵面板输入控制和ROV软件输入控制优先级相同；对于ROV操纵面板输入控制来说：操纵手柄输入命令优先级高于自动按钮控制，保持高程和保持深度命令优先级相同，操纵手柄优先级高于微调旋钮命令；微调旋钮优先级高于自动按钮控制命令；对于ROV软件输入控制命令来说，其命令优先级的顺序与ROV操纵面板输入控制命令优先级顺序一致；

(13)构建ROV六自由度运动控制器；

(14)通过ROV外部控制输入来实现ROV的六自由度运动控制：

ROV的各种外部控制命令经过控制命令逻辑判断模块，输出有效控制命令至ROV运动引导律计算模块和ROV运动控制器；ROV运动控制器输出六个方向的推力和推力矩，操纵ROV按照期望控制命令运动、定向或悬停定位。