[发明专利]一种作业型ROV六自由度运动控制方法

权利要求书

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

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

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

Mυ·+C(υ)υ+D(υ)υ+g(η)=τ]]>

式中：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的线速度向量为：

p·=R(Θ)v]]>

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

R(Θ)=cψcθ-sψcφ+cψsθsφsψsφ+cψcφsθsψcθcψcφ+sφsθsψ-cψsφ+sθsψcφ-sθcθsφcθcφ]]>

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

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

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

p＝∫R(Θ)vdv

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

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

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

Θ·=T(Θ)ω]]>

式中：Θ·=φ·θ·ψ·T]]>为大地坐标系中水下机器人的角速度向量；T(Θ)为本体坐标系到 大地坐标系的角速度转换矩阵；

T(Θ)=1sφtθsφtθ0cφ-sφ0sφ/cθcφ/cθ]]>

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

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

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

Θ＝∫T(Θ)ωdω

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

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

v=R-1(Θ)p·]]>

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

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

P=∫0tvdt=∫0tR-1(Θ)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三个方向的角速度为：

ω=T-1(Θ)Θ·]]>

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

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

Q=∫0tωdt=∫0tT-1(Θ)Θ·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按照期望控制命令运动、定向或悬停定位。