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

摘要

本发明属于无人水下机器人运动控制技术领域，具体涉及一种过驱动作业型遥控水下机器人的作业型ROV六自由度运动控制方法。本发明包括：建立ROV水动力学模型；建立作业型ROV在六个自由度方向的控制力和力矩向量；计算本体坐标系{b}中作业型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_T Y_T Z_T K_T M_T N_T]^T(3)计算本体坐标系{b}中作业型ROV的运动状态：根据作业型ROV模型试验得到的水动力参数结果，计算出式中ROV水动力学模型中的系数矩阵M、C(υ)、D(υ)、g(η)，将六自由度的控制力和力矩向量τ输入到ROV水动力学模型，即可得到本体坐标系{b}中的运动状态，包括速度[u v w p q r]^T和位移[X Y Z α β γ]^T；(4)建立ROV的运动学方程，将ROV本体坐标系{b}中的线速度状态转化为大地坐标系{n}中来表示：ROV在本体坐标系{b}中的速度向量为：υ＝[v ω]^T式中：v＝[u v w]^T为ROV在本体坐标系中三个线速度向量；ω＝[p q r]^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＝[N E D]^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＝[X Y Z]^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按照期望控制命令运动、定向或悬停定位。