基于四元数的全驱动碟形AUV单矢量反馈控制

doi:10.11959/j.issn.2096-6652.202251

基于四元数的全驱动碟形AUV单矢量反馈控制

许一航¹, 刘剑¹, 孙长银¹^,²

1 东南大学自动化学院，江苏南京 210096

2 安徽大学人工智能学院，安徽合肥 230031

Quaternion-based single-vector feedback control for fully-actuated dish-shaped AUV

XU Yihang¹, LIU Jian¹, SUN Changyin¹^,²

1 School of Automation, Southeast University, Nanjing 210096, China

2 School of Artificial Intelligence, Anhui University, Hefei 230031, China

通讯作者: 孙长银，cysun@seu.edu.cn

修回日期: 2022-11-13 网络出版日期: 2022-12-15

基金资助:

国家自然科学基金资助项目.  61921004
国家自然科学基金资助项目.  62103099
江苏省前沿引领技术基础研究专项.  BK20202006
中央高校基本科研业务费.  2242022R42003

Revised: 2022-11-13 Online: 2022-12-15

Fund supported:

The National Natural Science Foundation of China.  61921004
The National Natural Science Foundation of China.  62103099
Natural Science Foundation of Jiangsu Province of China.  BK20202006
The Fundamental Research Funds for the Central Universities.  2242022R42003

作者简介 About authors

许一航（1997-），男，东南大学自动化学院博士生，主要研究方向为水下机器人控制技术、水下伺服驱动技术。

刘剑（1992-），男，博士，东南大学自动化学院副研究员，主要研究方向为自主无人系统协同控制、固定时间控制。

孙长银（1975-），男，博士，安徽大学党委常委、副校长，二级教授、博士生导师，自主无人系统技术教育部工程中心主任。中国高等教育学会科技服务专家指导委员会副主任，中国自动化学会会士、常务理事、副秘书长、人工智能与机器人教育专委会主任，《智能科学与技术学报》副主编。主要研究方向为自主无人系统协同控制、智能控制理论及其应用、非线性系统分析与优化、模式识别等。

摘要

提出了一种基于四元数的单向量反馈自治式潜水器（AUV）位姿控制算法，使用向量表示控制量和姿态或位置的误差。此算法突破了欧拉角姿态下存在姿态死区的局限，解决了 AUV 任意方向多圈旋转问题。使用该算法实现了全驱动碟形AUV的位置和姿态控制。利用MATLAB的SIMSCAPE工具箱建立碟形AUV模型，并使用离散数字控制器进行仿真，验证了算法的稳定性。实验结果表明，在碟形 AUV 的位姿控制任务中，四元数反馈控制算法可以使碟形 AUV 系统在外部控制的作用下，从一个任意状态到达状态空间中的另一个任意状态。本算法将机体姿态视为单个闭环，相比基于欧拉角的控制方案，收敛域更大、参数更少，降低了实际设备的调试难度，在工程应用中更加便利。

关键词： 四元数反馈 ; 全驱动系统 ; 碟形AUV

Abstract

The single vector quaternion feedback control algorithm for the Special Topic: Autonomous Underwater Vehicle (AUV) was proposed, which used vectors to represent control input and attitude errors.This algorithm solved the attitude dead zone and multi-turn rotation problem in Euler angle attitude.The above algorithm could realize the attitude and position control in a fully-actuated dish-shaped AUV.In order to demonstrate the efficacy of the algorithm mentioned, the physical model of the dish-shaped AUV was built in the simulation and a digital controller was designed based on quaternion feedback.The experimental results showed that the quaternion feedback control algorithm could make the dish-shaped AUV system move from one arbitrary state to another arbitrary state in the state space.The algorithm regarded the body attitude or position as a single closed loop, thus having larger convergence domain and requiring fewer parameters to tuning than the scheme using Euler angles, which made the debugging of dish-shaped AUV control parameters easier and more convenient in engineering application.

Keywords： quaternion feedback ; fully-actuated system ; dish-shaped AUV

PDF (6006KB) 元数据多维度评价相关文章导出 EndNote| Ris| Bibtex 收藏本文

本文引用格式

许一航, 刘剑, 孙长银. 基于四元数的全驱动碟形AUV单矢量反馈控制. 智能科学与技术学报[J], 2022, 4(4): 513-521 doi:10.11959/j.issn.2096-6652.202251

XU Yihang. Quaternion-based single-vector feedback control for fully-actuated dish-shaped AUV. Chinese Journal of Intelligent Science and Technology[J], 2022, 4(4): 513-521 doi:10.11959/j.issn.2096-6652.202251

0 引言

水下机器人在控制方法上与航天器类似，将水下机器人整体视为一个刚体，需要进行6个自由度的控制，但是由于水下机器人作业时同时受重力、浮力的作用以及水流扰动，因此对姿态控制的动态性能要求较高。大量研究者在改善水下机器人动态响应效果、简化调试难度方面提出了众多控制算法，并在水下机器人机械结构、执行器等方面做出了改进与创新^[1,2,3,4]。

由于流体自身存在的黏滞阻力等特性和机器人复杂外形产生的扰流，水下机器人的水动力参数往往难以准确测得，在工程实践中仍以手动调节参数为主。Lyu C 等人^[5]提出了一种基于改进粒子群（modified particle swarm optimization，MPSO）算法的优化4自由度PD控制参数的算法，在已知水动力参数的情况下减少了手动调节参数的时间。此外，借助强化学习的自适应神经网络方法也被应用于调节水下机器人轨迹反馈控制律中的参数，这种方法甚至可以应用于水动力参数未知的控制模型参数调节^[6]。对于模型完全未知的潜水器，王日中等人^[7]提出了一种基于深度强化学习的控制思路，让潜水器自主学习控制律。另一些研究者在机械结构方面做出了改进，Song S等人^[8]设计了一种浮力引擎用于调节机器人浮心和重心，借助重力矩的变化实现对俯仰与横滚的控制，这一过程无须推进器提供动力。

目前，以 4 旋翼为代表的欠驱动飞行器在悬停或巡航时，为了保证垂直方向的高度稳定，除航向角以外不会有超过90°的机动倾角，因此，现有研究大多使用基于欧拉角的误差反馈实现姿态控制^[9,10,11,12]。但水下机器人在工作过程中，经常遇到需要大幅度变换姿态的情况，如果直接使用欧拉角来表示姿态，在俯仰角接近±90°时，容易触发万向节死锁，导致姿态控制失效^[13]。

针对以上问题，有一些研究者采用改进欧拉角表达方式的方法来规避万向节死锁问题。Gavrilina E等人^[14]提出了一种基于欧拉角的顺规可切换表示算法，该算法在系统逼近奇点时，通过切换顺规的方式避开奇点，从而防止系统在大倾角机动时的通道干扰。此外Brezov D^[15]提出在广义欧拉分解中引入第 4 次旋转，并在临近奇点时借助附加参数来避免万向节死锁问题。这些方法在保留欧拉角容易理解的优势的同时，有效降低了控制过程中发生万向节死锁的可能性，但同时也增加了算法的复杂度，不利于工程应用。因此，基于四元数的姿态表示方法由于其导数平滑、易于插值等特点逐渐在计算机立体视觉和机械臂控制中得到普及和应用^{[16,17,18,19]}。吴琪等人^[20]使用四元数方法设计了一种对模型已知的欠驱动自治式潜水器（Special Topic: Autonomous Underwater Vehicle，AUV）的6自由度控制律，在仿真中实现了镇定控制。

水下机器人与飞行器或航天器的工作环境不同。在水下工作时，AUV 同时受到重力和浮力的作用，当两种作用力不共线时，将对AUV机体产生一个扭矩，影响AUV的姿态控制，同时海浪也会对AUV的位置和姿态产生周期性的扰动^{[21,22,23,24,25]}。为了抑制扰动，Shi K等人^[26]在研究AUV的对接任务时提出了一种基于非线性模型预测控制（nonlinear model predictive control，NMPC）的策略，可以有效地抑制海流扰动和模型误差导致的偏移。但是在姿态已经偏移较大时，外扰对AUV姿态产生的影响会因为欧拉角描述的特性而不能被及时修正，为此本文提出了一种针对全驱动碟形水下机器人的控制策略，并使用四元数反馈的方式实现了碟形AUV的6自由度控制，从而避免在使用欧拉角表示姿态时出现的死锁问题和姿态角突变问题。通过设置积分观测器的方式，对上述扰动进行辨识并抑制和补偿了这些扰动对AUV控制的影响。最后使用MATLAB的SIMSCAPE工具箱对碟形AUV使用本文提出的算法进行了仿真，仿真结果显示该算法具有较好的鲁棒性，相较于欧拉角定义的反馈控制算法具有参数少、易整定、无状态死区等优势。

1 四元数简介

四元数最早由爱尔兰数学家哈密顿提出，是一种由1个实部和3个虚部i、j、k组成的超复数^[13]，如式（1）所示：

Q = q_{0} + q_{1} i + q_{2} j + q_{3} k (1)

四元数Q的模长如式（2）所示：

‖ Q ‖ = \sqrt{q_{0}^{2} + q_{1}^{2} + q_{2}^{2} + q_{3}^{2}} (2)

当 $‖ Q ‖ = 1$ 时，称这个四元数为单位四元数，下文中没有特殊指出的四元数，都是单位四元数。单位四元数可以用来对三维向量进行旋转操作^[26]。定义一个旋转变换，以单位矢量 $n = {[x_{n} y_{n} z_{n}]}^{⊤}$ 所在直线为轴旋转大小为θ的角度。当使用四元数表示该旋转变换时，将这个变换记为一个四元数，如式（3）所示：

Q_{θ}^{n} = \cos \frac{θ}{2} + \sin \frac{θ}{2} x_{n} i + \sin \frac{θ}{2} y_{n} j + \sin \frac{θ}{2} z_{n} k (3)

为了方便理解，定义如式（4）所示的映射：

T : v = {[x y z]}^{⊤} \to Q = 0 + x i + y j + z k (4)

本文可以通过式（5）将任意的矢量 $n$ 绕任意轴l旋转任意角度θ得到 $n^{'}$ ：

n^{'} = T^{- 1} (Q_{θ}^{l} T (n) Q_{θ}^{l *}) (5)

其中，T^-1(·)表示T(·)的逆映射， $Q^{*}$ 表示 $Q$ 的共轭，即对四元数的3个虚部分别取相反数。四元数的乘法遵循式（6），对于 $Q_{1} = w_{1} + x_{1} i + y_{1} j + z_{1} k$ 、 $Q_{2} = w_{2} + x_{2} i + y_{2} j + z_{2} k$ ，有：

Q_{1} Q_{2} = w_{1} w_{2} - x_{1} x_{2} - y_{1} y_{2} - z_{1} z_{2} +

(x_{1} w_{2} + w_{1} x_{2} - z_{1} y_{2} + y_{1} z_{2}) i +

(y_{1} w_{2} + z_{1} x_{2} + w_{1} y_{2} - x_{1} z_{2}) j +

(z_{1} w_{2} - y_{1} x_{2} + x_{1} y_{2} + w_{1} z_{2}) k (6)

四元数 $Q_{θ}^{l} T (n) Q_{θ}^{l *}$ 的实部为0，它的虚部是原来三维向量经过式（5）所代表的旋转变换后的新的三维向量。对于四元数的其他计算性质，本文不再赘述。

通过四元数的以上特性，在使用四元数描述姿态的同时，可以提取到完成旋转操作需要的转轴和绕此轴的旋转角度。当得到转轴后，选取合适大小的与转轴共线的力矩即可控制 AUV 旋转到给定姿态。

2 碟形AUV模型

本文设计了一种全驱动的碟形AUV，使用6个推进器实现对碟形 AUV 3 轴旋转和 3 轴平动的控制，6自由度的碟形AUV保证了挂载的机械臂与摄像头可以从任何方向指向目标物体，每个推进器都为双向推进器。其中3个垂直于碟面放置，记为P1、P2和P3；另外3个平行于碟面放置，记为V1、V2和V3。碟形AUV推进器布局如图1所示。

建立如图2所示的坐标系，大地坐标系N为与地表固连的东北天坐标系，机载坐标系B为原点在碟形AUV质心且朝向与图1一致的坐标系，为简化描述，下文将大地坐标系N、机载坐标系B分别简称为N系、B系。

图1

新窗口打开| 下载原图ZIP| 生成PPT

图1 碟形AUV推进器布局

图2

新窗口打开| 下载原图ZIP| 生成PPT

图2 坐标系参考

根据碟形 AUV 结构，可以推导出 x、y、z 轴旋转力矩和x、y、z轴平动力分配到各推进器的推力分配解耦矩阵 $M_{P}^{B}$ ，进而将B系下的控制量 $U^{B}$ 分配到碟形AUV的6个推进器输出 $U^{P}$ 上。

U^{P} = M_{P}^{B} U^{B} (7)

M_{P}^{B} = [\begin{matrix} \frac{1}{3} & - \frac{\sqrt{3}}{3} & 0 & 0 & 0 & \frac{1}{3 d_{P}} \\ - \frac{2}{3} & 0 & 0 & 0 & 0 & \frac{1}{3 d_{P}} \\ \frac{1}{3} & \frac{\sqrt{3}}{3} & 0 & 0 & 0 & \frac{1}{3 d_{P}} \\ 0 & 0 & \frac{1}{3} & \frac{2}{3 d_{V}} & 0 & 0 \\ 0 & 0 & \frac{1}{3} & - \frac{1}{3 d_{V}} & - \frac{\sqrt{3}}{3 d_{V}} & 0 \\ 0 & 0 & \frac{1}{3} & - \frac{1}{3 d_{V}} & - \frac{\sqrt{3}}{3 d_{V}} & 0 \end{matrix}] (8)

其中，d_V、d _P分别是碟形AUV几何中心到垂直推进器和水平推进器的水平距离。通过式（3），可以得到B系下6自由度控制量 $U^{B} = {[F_{x}^{B} F_{y}^{B} F_{z}^{B} T_{x}^{B} T_{y}^{B} T_{z}^{B}]}^{⊤}$ ，经过解耦分配到 6 个推进器的力 $U^{P} = {[F_{P 1} F_{P 2} F_{P 3} F_{V 1} F_{V 2} F_{V 3}]}^{⊤}$ 。

碟形 AUV 在水下工作时可视为 6 自由度刚体，本文将使用四元数描述碟形AUV的姿态，故建立无扰动情况下的位姿分离的动力学模型，如式（9）所示：

{\begin{matrix} m \ddot{X} = U_{F}^{N} + T^{- 1} (Q_{B}^{N} T (Z \dot{X}) Q_{B}^{N *}) + B + G \\ I_{O_{B}} \dot{w} = U_{T}^{N} + T^{- 1} (Q_{B}^{N} T (H w) Q_{B}^{N *}) + T_{BG} \end{matrix} (9)

其中，m是碟形AUV的整机质量， $\ddot{X} = {[\ddot{x} \ddot{y} \ddot{z}]}^{⊤}$ 是N系下碟形AUV的加速度， $U_{F}^{N}$ 是N系下由推进器产生的推力， $T^{- 1} (Q_{B}^{N} T (Z \dot{X}) Q_{B}^{N *})$ 是 N 系下的一阶平动水阻尼， $Z = diag (ζ_{x}, ζ_{y}, ζ_{z})$ 是B系下的平动阻尼系数矩阵，B、G分别是N系下碟形AUV所受浮力和重力， $I_{O_{B}}$ 是N系下相对B系原点O_B的惯量矩阵， $U_{F}^{N}$ 是 N 系下由推进器产生的力矩， $\dot{w} = {[{\dot{w}}_{x} {\dot{y}}_{x} {\dot{w}}_{z}]}^{⊤}$ 是 N 系下碟形 AUV 的角加速度， $T^{- 1} (Q_{B}^{N} T (H w) Q_{B}^{N *})$ 是 N 系下的一阶转动水阻尼， $H = diag (η_{x}, η_{y}, η_{z})$ 是B系下的阻尼系数矩阵， $T_{BG}$ 是由重力和浮力不共线产生的重力矩（浮力矩）。

3 算法

通过读取惯导的数据，可以得到B系相对N系的旋转四元数 $Q_{B}^{N}$ ，期望的N系姿态用四元数表示为 $Q_{\exp}$ 。根据之前的定义，碟形AUV从当前姿态旋转到期望姿态所需的四元数 $Q_{fb}$ fb可由式（10）得到：

Q_{fb} = Q_{\exp} Q_{now}^{*} (10)

其中， $Q_{now}^{*}$ 为碟形AUV当前四元数姿态 $Q_{now}^{}$ 的共轭。此时的Q 称为反馈四元数，可以看作当前姿态和期望姿态的误差，其虚部 $img (Q_{fb})$ 中蕴含了从当前姿态单次旋转到期望姿态的转轴 $l_{θ}$ ，其实部 $real (Q_{fb})$ 中可以提取出需要绕轴 $l_{θ}$ 旋转的角度θ_fb，如式（11）所示：

{\begin{array}{l} θ_{fb} = 2 \arccos (q_{{fb}_{0}}) \\ l_{θ} = θ_{fb}^{- 1} {[q_{{fb}_{1}} q_{{fb}_{2}} q_{{fb}_{3}}]}^{⊤} \end{array} (11)

经陀螺仪提取的3轴角速度 $w = {[w_{x}^{B} w_{y}^{B} w_{z}^{B}]}^{⊤}$ ，通过归一化和取模，分别得到碟形AUV当前转速的瞬时转轴 $l_{w}^{B}$ 和绕其旋转的角速度大小w_fb，如式（12）～式（13）所示：

l_{w}^{B} = {| w_{fb} |}^{- 1} {[w_{x} w_{y} w_{z}]}^{⊤} (12)

w_{fb} = \sqrt{w_{x}^{2} + w_{y}^{2} + w_{z}^{2}} (13)

对于碟形 AUV 的姿态控制，本文提出的算法所期望的是其从初始状态快速地转动到期望姿态，并保持姿态的静止。考虑到碟形 AUV 在水中时，质心与浮心不重合导致在一些姿态下浮力与重力作用会产生一个额外的力矩，影响姿态闭环的稳态性能，故在此加入一个类似误差积分的参数，用于抑制包括重力矩在内的各种低频扰动。

为此我们可以设置针对姿态的力矩控制律如式（14）所示：

U_{T}^{N} = [\begin{matrix} t_{x}^{N} \\ t_{y}^{N} \\ t_{z}^{N} \end{matrix}] = (k_{t 1} θ_{fb} + k_{t 2} \int^{​} θ_{fb} d t) l_{θ}^{N} - k_{t 3} w_{fb} l_{w}^{N} (14)

其中，k_t1、k_t2、k_t3为实际模型中姿态环相关的可调参数。对于碟形 AUV 空间位置的闭环，类比姿态环所使用的单矢量控制，也可将3轴的控制量、误差等视为矢量来计算，位置闭环的力控制率设计如式（15）所示：

U_{F}^{N} = [\begin{matrix} f_{x}^{N} \\ f_{y}^{N} \\ f_{z}^{N} \end{matrix}] = k_{f 1} d_{fb}^{N} + k_{f 2} \int^{​} d_{fb}^{N} d t + k_{f 3} v_{fb}^{N} (15)

其中，k_f1、k_{f 2}、k_f3是实际模型中位置环相关的可调参数， $d_{fb}^{N}$ 为碟形AUV在N系下的位置误差矢量， $v_{fb}^{N}$ 为碟形AUV在N系下的速度误差矢量。

d_{fb}^{N} = d_{exp}^{N} - d_{now}^{N} = [\begin{matrix} e_{x}^{N} \\ e_{y}^{N} \\ e_{z}^{N} \end{matrix}] (16)

v_{fb}^{N} = 0 - v_{now}^{N} = - [\begin{matrix} v_{x}^{N} \\ v_{y}^{N} \\ v_{z}^{N} \end{matrix}] (17)

上述的力矩控制量 $U_{T}^{N}$ 和力控制量 $U_{F}^{N}$ 都是在N系下的，为了方便处理，需要将 $U^{N} = {[U_{T}^{N} U_{F}^{N}]}^{⊤}$ 转换为B系下的 $U^{B} = {[U_{T}^{B} U_{F}^{B}]}^{⊤}$ ，可以通过四元数Q_now完成这个变换，如式（18）～式（19）所示：

U_{T}^{B} = T^{- 1} [Q_{now} T (U_{T}^{N}) Q_{now}^{*}] (18)

U_{F}^{B} = T^{- 1} [Q_{now} T (U_{F}^{N}) Q_{now}^{*}] (19)

4 仿真

为了验证算法的可行性，本文使用 MATLAB的SIMSCAPE工具箱搭建了仿真，并根据实物的参数完成了碟形AUV三维模型的搭建，如图3所示。

图3

新窗口打开| 下载原图ZIP| 生成PPT

图3 碟形AUV三维模型

图3中 A为碟形 AUV主体，在水中呈正浮力；B为可调节的配重，用于调节碟形AUV的重心并将机器人在水中整体配置为中性浮力，使机器人整体浮力与所受重力大小近似相等；C为6个双向旋桨式水下推进器。参照现有实物，模型各模块采用的物理参数见表1。

表1 碟形AUV物理参数

物理量	数值	单位
m	9.4	kg
g	9.8	N/m ²
d_V	0.25	m
d_P	0.25	m
$ζ_{x}, ζ_{y}, ζ_{z}$	0.5	N · m/(deg/s)
$η_{x}, η_{y}, η_{z}$	0.6	N/(m/s)
I_x,I_y	0.1847	kg · m ²
I_z	0.2749	kg · m ²

新窗口打开| 下载CSV

其中，m为碟形AUV的整机质量，g为仿真环境重力加速度，d_V、d_P分别为垂直推进器和水平推进器到碟形 AUV 中心的水平距离，ζ_x、ζ_y、ζ_z为碟形AUV在B系下绕x、y、z轴旋转时受到的一阶转动阻尼，η_x、η_y、η_z为碟形AUV在B系下沿x、y、z 轴平移时受到的一阶平动阻尼，I_x、I_y、I_z为碟形AUV在B系下在x、y、z轴分布的转动惯量。

实际推进器的推力是有限的，故需要对各推进器的直接控制量输出添加双向限幅。基于该模型尺寸下常见的推进器参数，本文在仿真中设置单推进器的最大推力输出为±60 N。

为了验证算法在实物上运行的可行性，本文将控制率进行了离散化，并使离散数字控制器以1 000 Hz的频率工作。

基于上文提出的算法，得到的离散控制律如式（20）～式（21）所示：

U_{T}^{N} = [\begin{matrix} t_{x}^{N} \\ t_{y}^{N} \\ t_{z}^{N} \end{matrix}] = (k_{t 1} θ_{fb} + k_{t 2} \sum^{​} θ_{fb} Δ t) l_{θ}^{N} - k_{t 3} w_{fb} l_{w}^{N} (20)

U_{F}^{N} = [\begin{matrix} f_{x}^{N} \\ f_{y}^{N} \\ f_{z}^{N} \end{matrix}] = k_{f 1} d_{fb}^{N} + k_{f 2} \sum^{​} d_{fb}^{N} Δ t + k_{t 3} v_{fb}^{N} (21)

其中，Δt为控制器的更新周期，在本文中，控制器更新频率为1 000 Hz，故取Δt＝0.001 s。

将式（20）～式（21）中的控制量 $U^{N} = {[U_{T}^{N} U_{F}^{N}]}^{⊤}$ 变换到 B 系下，再通过式（7）将 $U^{B} = {[U_{T}^{B} U_{F}^{B}]}^{⊤}$ 分配到6个推进器上。

4.1 位姿阶跃响应

在阶跃响应仿真实验中，碟形 AUV 在一个非零的初始状态下切换到一个俯仰角达到 90°的期望姿态（这是一个在经典欧拉角定义下无法收敛的状态）。

在 t＝1s 时，姿态四元数期望从（0.966,0,0,0.259）切换到（0.5,0.866,0,0），在t＝2s时，位置期望从 ${[\begin{matrix} 0 & 0 & 0 \end{matrix}]}^{⊤}$ 切换到 ${[\begin{matrix} 1 & 0.6 & - 0.4 \end{matrix}]}^{⊤}$ 。

图4（a）和图4（b）分别展示了在阶跃响应下碟形 AUV 的位置控制和姿态控制的效果，由于四元数曲线不能直观反应期望姿态与当前姿态之间的误差，故本文采用期望姿态与当前姿态之间的轴角偏移来体现姿态的收敛性能，图4（c）展示了在控制下轴角误差的收敛曲线，图4（d）给出了碟形AUV的6个推进器推力的变化曲线。

图4

新窗口打开| 下载原图ZIP| 生成PPT

图4 碟形AUV 位姿阶跃响应

4.2 连续绕轴旋转

在连续绕轴旋转实验中，碟形 AUV 绕任意轴实现连续的超过360°的旋转，在不使用过渡坐标系的欧拉角模式下，这种旋转是不能保证实现的。

图5（a）展示了碟形 AUV 绕轴 $l = [\frac{\sqrt{14}}{14} {\frac{\sqrt{14}}{7} \frac{3 \sqrt{14}}{14}]}^{⊤}$ 实现连续转动的控制效果，图5（b）给出了碟形AUV的6个推进器推力的变化曲线。

从仿真结果可见，对于超过360°的连续旋转，四元数姿态期望和碟形 AUV 的实时四元数姿态始终保持连续且平滑，不存在数值突变的情况。

4.3 轨迹跟踪实验

在轨迹跟踪实验中，设计了如式（22）所示的一条深度存在阶跃的环形轨迹，并保持碟形 AUV的姿态四元数期望控制在(1,0,0,0)的状态。

图5

新窗口打开| 下载原图ZIP| 生成PPT

图5 碟形AUV 连续绕轴旋转

{\begin{cases} x = \sin (0.4 t) \\ y = \cos (0.4 t) \\ z = {\begin{array}{l} 0, 0 \leq t ＜ 2 \\ - 1, 2 \leq t ＜ 10 \\ 0, 10 \leq t \end{array} \end{cases} (22)

图6（a）给出了碟形AUV轨迹跟踪的效果，图6（b）展示了控制下轴角误差的收敛曲线，图6（c）给出了碟形 AUV的 6个推进器推力的变化曲线。

图6

新窗口打开| 下载原图ZIP| 生成PPT

图6 碟形AUV 轨迹跟踪

5 结束语

本文推导并获得了一种碟形 AUV 推进器推力到各坐标系的动力的解耦，使用四元数表示 AUV的姿态并直接使用四元数完成姿态的反馈控制，给出了该算法在几何意义上的描述。

通过仿真实验，可以看到基于四元数反馈的单矢量控制方法只需要 3 个参数即可完成姿态环或者位置环的系数整定。相比于x、y、z 3轴分别闭环调试，本文算法具有参数少、调试直观的优点，此外由于使用四元数来表示姿态，碟形AUV的给定姿态不再受限于欧拉角姿态表示方法的死锁局限，使系统得以实现在状态空间内的任意两个状态间的切换过渡和插值。对于有连续旋转、大角度机动等需求的潜水器，使用四元数反馈具有极大的优势。

参考文献

原文顺序

文献年度倒序

文中引用次数倒序

被引期刊影响因子

[1]

ALI

F A

, ARAS

M S M

, AZIS

F A

,et al.

Design and development of auto depth control of remotely operated vehicle using thruster system

[J]. Journal of Mechanical Engineering and Sciences, 2014,7: 1141-1149.