CN109131801A - 全海深无人潜水器的自主回收装置及控制方法 - Google Patents

Abstract

本发明涉及一种全海深无人潜水器的自主回收装置及控制方法，针对全海深无人潜水器海试作业后回收困难的问题，通过姿态传感器采集的航向角和各轴加速度信息，结合PID航线控制算法可以使无人潜水器回收航行时具有自主的航线及航行纠错能力，能够自行调整航向及各推进器运动状态，确保其始终在目标航线上运行且航线路径偏移量始终在可控范围内。实现无人潜水器利用北斗定位与通信功能控制推进器自主的向母船航行，实现了无人潜水器回收的智能化，提升了无人潜水器海上实验回收的快速性、稳定性和准确性；本发明通用性好,可广泛用于深海作业型ROV、AUV、ARV等海洋装备的控制系统中。

Description

全海深无人潜水器的自主回收装置及控制方法

技术领域

本发明涉及一种无人潜水器控制技术，特别涉及一种全海深无人潜水器的自主回收装置及控制方法。

背景技术

全海深无人潜水器是一种用微细光缆代替传统电缆，通过母船上遥控装置控制运行的水下智能化装置，它即能进行近距离的成像和采样，又能够进行海底探索和测绘。未来无人潜水器在人类对海洋的各式各样的开发中将发挥巨大的作用。微细光缆的使用虽然降低了传统脐带缆缠绕、控制范围小及对母船要求高的缺点，但是其在复杂海况下易出现通信中断甚至光缆断裂现象，这对于无人潜水器的水面搜寻与回收提出了更高的要求。同时无人潜水器在水下作业的活动时间主要受自身携带的能源限制，当无人潜水器完成某一任务后，通常需要回收到母船，以便补充能源、下载数据和卸载样品等。而当无人潜水器在大深度海底工作时，由于水流等因素会使其偏离预先的布放点，浮出水面后的无人潜水器可能远离出发地点，若作业海域海况较差，回收过程更会增加一定的风险甚至于丢失。但无人潜水器往往集成很多昂贵测量与探测仪器、存储大量宝贵数据，可见其安全、高效回收一直备受重视。

目前国内外主流回收方式是建造专用回收母船或以船员冒险作业回收，但由于受到风速及海浪的影响使无人潜水器与母船回收装置对接困难，且船员下水回收人身安全也无法得到保障，因此研制一种智能的无人潜水器远程自主回收装置及控制系统显得尤为重要。

发明内容

本发明是针对全海深无人潜水器回收技术中存在的问题，提出了一种全海深无人潜水器的自主回收装置及控制方法，结合姿态传感器所采集的无人潜水器姿态数据及双闭环控制方法，设计了全海深无人潜水器自主回收装置及控制系统，能够实现无人潜水器水面位置、姿态及运动位置监测，实现了无人潜水器回收的智能化，大大提高了无人潜水器水面回收的安全性、稳定性、快速性和准确性。

本发明的技术方案为：一种全海深无人潜水器的自主回收装置，包括安装在无人潜水器本体顶部玻璃浮球控制舱、无人潜水器本体运动控制系统、北斗卫星和科考母船上的水面控制系统，玻璃浮球控制舱内有STM32微处理器、北斗定位与通信模块BDM910、温度传感器模块、光敏传感器、姿态传感器和电源模块，运动控制系统包括两个水平推进器、一个转向推进器及各推进器的ELMO驱动器；玻璃浮球控制舱和运动控制系统之间利用CAN总线通过水密电缆连接通信；姿态传感器、北斗定位与通信模块、温度传感器、光敏传感器与STM32微控制器之间分别通过IIC、USART、单总线、IO口进行通信；当无人潜水器浮出水面时，温度传感器、光敏传感器检测到周围环境变化启动北斗定位与通信模块BDM910，北斗定位与通信模块BDM910发送位置及姿态传感器采集的姿态数据通过北斗卫星至母船的水面控制系统，通过母船水面控制系统将母船位置信息发送至无人潜水器，无人潜水器顶端的玻璃浮球控制舱内的北斗定位与通信模块BDM910接收到母船位置信息后，将其作为目标点信息，结合姿态传感器采集的无人潜水器航向和速度信息，利用PID航线控制算法控制无人潜水器上三台推进器自主的向母船靠拢完成回收。

所述科考母船上的水面控制系统包括北斗发送接收终端、水面电源和计算机，监控无人潜水器回收状态与传输数据和命令。

所述全海深无人潜水器的自主回收装置的控制方法，无人潜水器接收到北斗发送的母船位置信息P_ship(k)后，航线控制器结合无人潜水器本体位置信息P_ARV(k)进行航线解算，然后航向PID控制器结合姿态传感器的参数实时对无人潜水器回收的航向进行监测，若偏离了航线则通过姿态传感器反馈的速度值V_G(k)作为变量来调节各推进器的推力，同时修正无人潜水器的航向，缩小航线路径偏移量ε(k)实现航向的闭环控制；修正后的航向值Ψ(k)经过坐标变换和航线解算后，作为航线控制器的反馈变量实现对航线的闭环控制；无人潜水器在航向和航线的双闭环回收控制作用下维持在预定的航线上运动，完成无人潜水器自主的向母船航行，实现回收。

所述全海深无人潜水器的自主回收装置的控制方法，无人潜水器双闭环回收控制系统中，定义无人潜水器中心点指向正北的方向为X轴，在水平方向上与X轴垂直指向正东方向为Y轴，外环的航线控制器通过母船位置P_ship(k)＝[X_ship(k)Y_ship(k)]^T和无人潜水器位置P_ARV(k)＝[X_ARV(k)Y_ARV(k)]^T的距离偏差来实现无人潜水器航线偏移量ε(k)的计算，具体为：

ε(k)＝L_k×sinδ(k) (1)

式中，

其中，L_k为回收过程中k时刻无人潜水器与母船位置P_ship之间的直线距离，δ(k)为回收过程中无人潜水器中心点与预定航线间的夹角。

所述全海深无人潜水器的自主回收装置的控制方法，无人潜水器双闭环回收控制系统中，无人潜水器航线偏移量ε(k)与无人潜水器航向角的偏差作为内环航向PID控制器的输入量，控制无人潜水器艏部和水平推进器；姿态传感器采集测得的数据转换成无人潜水器平面运动的速度向量V(k),然后计算无人潜水器相对大地坐标系下的速度向量V_G(k),通过姿态传感器反馈的速度值V_G(k)作为变量来调节各推进器的推力，进而修正无人潜水器的航向：

V_G(k)＝R×V(k) (4)

式中，R欧拉角坐标转换矩阵为：

其中：θ为无人潜水器绕Y轴旋转的俯仰角，Ψ为无人潜水器绕X轴旋转的横滚角。

本发明的有益效果在于：本发明全海深无人潜水器的自主回收装置及控制方法，通过姿态传感器采集的航向角和各轴加速度信息，结合PID航线控制算法可以使无人潜水器回收航行时具有自主的航线及航行纠错能力，能够自行调整航向及各推进器运动状态，确保其始终在目标航线上运行且航线路径偏移量始终在可控范围内。实现无人潜水器利用北斗导航定位通信功能控制推进器自主的向母船航行，提升了无人潜水器海上实验回收的快速性、稳定性和准确性；本发明通用性好,可广泛用于深海作业型ROV、AUV、ARV等海洋装备的控制系统中。

附图说明

图1为本发明全海深无人潜水器的自主回收装置控制系统结构框图；

图2为本发明全海深无人潜水器的自主回收装置整体结构示意图；

图3为本发明中全海深无人潜水器回收装置硬件组成及接口示意图；

图4为本发明中全海深无人潜水器本体与回收控制装置的CAN总线接线图；

图5为本发明中全海深无人潜水器预定航线控制示意图；

图6为本发明中全海深无人潜水器运动航向控制原理示意图；

图7为本发明中全海深无人潜水器回收装置软件运行整体框图。

具体实施方式

本发明能够供全海深作业型和观察型ARV、AUV、ROV等深海潜水器及水下机器人推广使用，尤其是具有运动控制能力的全海深无人潜水器。该系统能够根据母船所处的位置和无人潜水器浮出水面时经纬度信息，来制定无人潜水器回收路线且母船的上位机系统通过北斗实时的监控无人潜水器的航线信息完成回收。

全海深无人潜水器新型自主回收装置控制原理为：当无人潜水器浮出水面时，温度、光敏传感器检测到周围环境变化启动北斗定位与通信模块BDM910，发送位置及姿态传感器采集的姿态数据至母船的北斗收发装置，工作人员根据实际情况制定回收策略，并操作位于母船上基于北斗的水面控制系统完成无人潜水器回收。通常策略为：若回收时海况恶劣或光纤通信中断，则采用母船向无人潜水器靠近的回收方案，即母船接收到无人潜水器浮出水面时发出的位置信息后，迅速向其靠拢完成水面回收；若海况好但光纤通信差或中断，则可以结合无人潜水器与母船交换的位置信息及姿态传感器的数据，利用水面控制系统通过北斗通信控制推进器运动，以使无人潜水器自主的向母船航行完成水面回收；否则，通过光纤通信控制无人潜水器向母船靠拢，实现无人潜水器高效、快速的水面回收。

如图1所示全海深无人潜水器的自主回收装置控制系统结构框图，包括上位机系统、通信系统、唤醒休眠系统和运动控制系统。上位机系统监测无人潜水器的姿态数据以及负责通信的北斗定位与通信模块BDM910发送命令控制无人潜水器完成自主回收，玻璃浮球控制舱中的姿态传感器所采集的姿态数据包括无人潜水器姿态信息(即航向角、横滚角和俯仰角)和无人潜水器所处的经纬度信息。通信系统利用北斗定位与通信模块BDM910完成无人潜水器的导航定位以及姿态传感器所采集姿态信息的传输。唤醒休眠系统通过STM32微处理器对传感器信号进行采集，负责STM32微处理器唤醒作用的传感器由光敏传感器和温度传感器组成，STM32微处理器通过温度传感器的设定值及光敏传感器的开关状态来判断无人潜水器处于水下作业还是水面漂浮，进而让北斗定位与通信模块BDM910进入相应的低功耗休眠或唤醒状态。无人潜水器本体运动控制系统包括两个水平推进器、一个转向推进器和ELMO驱动器，推进器通过CAN通信接口的ELMO驱动器控制，可实现无人潜水器自主回收过程实时的运动控制。无人潜水器自主回收工作原理为：当无人潜水器浮出水面，唤醒休眠系统唤醒北斗定位与通信模块BDM910发送经纬度信息至母船，母船在接收到无人潜水器的位置信息后，通过母船接收发送终端将无人潜水器回收的目标点即母船位置信息发送至无人潜水器，无人潜水器接收到目标点信息后结合姿态传感器采集的无人潜水器航向和速度信息，利用PID航线控制算法控制三台推进器自主的向母船靠拢完成回收。

如图2所示全海深无人潜水器的自主回收装置整体结构示意图，无人潜水器本体11、科考母船7、水面控制系统12和北斗卫星6。其中，无人潜水器本体主要设备包括运动控制系统和安装于无人潜水器顶部的玻璃浮球控制舱5，它们构成了整个自主回收控制系统的灵魂，是无人潜水器能否完成自主回收的关键所在；玻璃浮球控制舱5包括STM32微处理器、北斗定位与通信模块BDM910、温度传感器、光敏传感器、姿态传感器和电源模块。无人潜水器本体的运动控制系统由尾部水平推进器左1、水平推进器右2、转向推进器4及各推进器的ELMO驱动器和电池罐3组成。位于科考母船7上的水面控制系统12由北斗发送接收终端8、水面电源9和计算机10组成，它们构成了整个自主回收控制系统的大脑与眼睛，是监控无人潜水器回收状态与传输数据和命令的运载机构。

无人潜水器在完成海底作业任务浮出水面时，位于无人潜水器顶部的玻璃浮球控制舱5检测到外界环境变化并启动向科考母船7发送位置信息及姿态数据，位于科考母船7上的北斗接收发送终端8接收到信息，通过计算机10发送科考母船7位置信息至位于无人潜水器顶部的玻璃浮球控制舱5，最后结合无人潜水器本体11和科考母船8所处的位置信息制定回收航线，并通过PID航线控制算法控制各推进器使无人潜水器向母船运动完成回收。

如图3所示全海深无人潜水器回收装置硬件组成及接口示意图，六轴加速度传感器(姿态传感器)、北斗定位与通信模块BDM910、温度传感器、光敏传感器与STM32微控制器之间分别通过IIC、USART、单总线、IO口进行通信，而玻璃浮球控制舱5利用CAN总线通过水面电缆将STM32微处理器与无人潜水器本体中ELMO驱动器连接，然后利用玻璃浮球控制舱5中北斗定位与通信模块BDM910，将科考母船的回收控制控制命令经过STM32微处理器的CAN总线下发到EMLO驱动器控制推进器运动，从而实现了无人潜水器自主回收过程中的远程控制。由于融合了多传感器数据通信接口电路设计与优化显得尤为重要，玻璃浮球控制舱5内部的北斗定位与通信模块BDM910是整套全海深无人潜水器自主回收控制系统的核心，是无人潜水器能否完成远程自主控制回收的关键，整个玻璃浮球控制舱5安装在无人潜水器本体顶部，以便更好的进行无人潜水器自身定位和与母船进行远程通信。

如图4所示全海深无人潜水器本体与回收控制装置的CAN总线接线图，无人潜水器本体中各推进器及其ELMO驱动器的控制是实现无人潜水器水下作业和复杂情况下自主完成水面运动与回收的关键。由电源、两个尾部水平推进器、一个艏部转向推进器及其Elmo驱动器和通信系统组成无人潜水器本体的运动控制系统。各推进器的驱动器采用Elmo伺服驱动器，此款Elmo驱动器能够对电机实现位置、速度和力的闭环控制，CAN总线接口通信方式，接线数量少、全数字通信、抗干扰、通信速率高，STM32微处理器自带CAN通信接口,两个输出端CANH和CANL可通过水密电缆与无人潜水器本体中ELMO驱动器的CAN接口完成总线连接，从而利用玻璃浮球控制舱中北斗定位与通信模块BDM910和科考母船中的北斗收发装置实现无人潜水器回收命令与信息的远程传输。

如图5所示全海深无人潜水器预定航线控制示意图，全海深无人潜水器起始位置P_ARV和母船位置P_ship的连线即为预定航线。但是，在海流的作用下，难于保证无人潜水器与母船按照预定航线运行，导致回收困难甚至丢失。为此，无人潜水器采用双闭环算法控制以确保航向角自动调整指向母船位置，其中外环控制无人潜水器的航线，内环控制无人潜水器的航向角，从而通过艏部转向推进器来调整航向角，克服海流对无人潜水器航线的影响。无人潜水器双闭环回收控制系统中，定义无人潜水器中心点指向正北的方向为X轴，在水平方向上与X轴垂直指向正东方向为Y轴，与XY所构成的水平面垂直向上的方向为Z轴；外环的航线控制器通过母船位置P_ship(k)＝[X_ship(k)Y_ship(k)]^T和无人潜水器位置P_ARV(k)＝[X_ARV(k)Y_ARV(k)]^T的距离偏差来实现无人潜水器航线偏移量ε(k)的计算，具体为：

ε(k)＝L_k×sinδ(k) (1)

式中，

其中，L_k为回收过程中k时刻无人潜水器与母船位置P_ship之间的直线距离，δ(k)为回收过程中k时刻无人潜水器中心点与预定航线间的夹角。

无人潜水器回收过程中航线偏移量ε(k)与无人潜水器航向角的偏差作为内环航向PID控制器的输入量，进而控制无人潜水器艏部和水平推进器。其中，姿态传感器采集测得的数据转换成无人潜水器平面运动的速度向量V(k),然后计算无人潜水器相对大地坐标系下的速度向量V_G(k)：

V_G(k)＝R×V(k) (4)

式中，R欧拉角坐标转换矩阵为：

其中：θ为无人潜水器绕Y轴旋转的俯仰角，Ψ为无人潜水器绕X轴旋转的横滚角。

对式(4)进行积分可以得到无人潜水器的位置坐标P_ARV(k)，从而确保航线有效控制。

其中P_ARV(k₀)为无人潜水器起始位置，V_G(ζ)为回收过程中ζ时刻无人潜水器相对大地坐标系下的速度向量(ζ表示为起始时刻k₀到回收结束K时刻之间的任一时刻)。这样，实际回收过程中，无人潜水器根据其姿态数据及运动控制系统的性能，有效实现无人潜水器回收运动轨迹控制。

如图6所示全海深无人潜水器航向控制原理为：无人潜水器接收到北斗发送的母船位置信息(P_ship(k))后，航线控制器结合无人潜水器本体位置信息(P_ARV(k))进行航线解算，然后航向PID控制器结合姿态传感器的参数实时对无人潜水器回收的航向进行监测，若偏离了航线则通过姿态传感器反馈的速度值(V_G(k))作为变量来调节各推进器的推力，同时修正无人潜水器的航向，缩小航线路径偏移量ε(k)实现航向的闭环控制；而修正后的航向值Ψ(k)经过坐标变换和航线解算后，作为航线控制器的反馈变量P_ARV(k1)实现对航线的闭环控制；从而保证无人潜水器在航向和航线的双闭环回收控制作用下能够稳定的维持在预定的航线上运动来完成无人潜水器自主的向母船航行实现快速稳定的回收。

如图7所示全海深无人潜水器自主回收控制方法整体框图，包括以下步骤：(1)上电运行程序首先进行系统初始化，包括各传感器设置、BDM910初始化参数配置；(2)为了节省电池能量，无人潜水器下潜时玻璃浮球控制舱内的STM32微处理器进入休眠状态，无人潜水器浮出海面时玻璃浮球控制舱内的STM32微处理器唤醒工作，原理是检测光敏传感器是否打开、温度传感器采集的数据是否高于初始值，只有在同时满足上述传感器要求的前提下才能唤醒STM32微处理器进行工作，否则进入低功耗休眠模式以便节约电能供无人潜水器在海底作业。(3)若检测到光敏传感器打开且温度传感器采集的数据高于初始值，则STM32微处理器被唤醒启动北斗定位与通信模块BDM910工作将无人潜水器所处的经纬度及俯仰角、横滚角、航向角和各轴加速度值发送至母船。(4)母船根据实际回收海况制定策略是否利用北斗自主回收，若采用北斗通信控制回收，则通过母船上的北斗监控系统发送母船位置信息至无人潜水器上北斗装置，无人潜水器结合传感器采集的航向角、横滚角、俯仰角及各轴加速度参数，由北斗卫星无线电测定业务(RDSS)接收母船控制指令，结合姿态数据通过CAN总线控制无人潜水器运动，并向母船发送位置信息及相关传感器信息，利用双闭环PID控制算法控制无人潜水器自主的向母船靠拢完成回收。若海况条件不满足，则采用光纤通信控制无人潜水器完成回收。(5)利用上位机监控系统时刻监测无人潜水器是否回收成功，以便结合实际海况和上位机采集的信息通过北斗通信功能实时的调整无人潜水器回收的状态直至成功完成回收。

本发明全海深无人潜水器的自主回收装置及控制方法，立足实践，全面认真分析了现有全海深无人潜水器海上实验回收方法的不足，以需求为导向，结合北斗独有的双向通信优势对无人潜水器回收的方式进行改进：

(1)本发明当全海深无人潜水器在海底工作时安装于无人潜水器顶部玻璃浮球内部的北斗定位与通信模块BDM910、温度传感器和光敏传感器检测到外界环境变化使STM32微处理器进入低功耗状态，当潜水器浮出水面时光敏及温度传感器检测到变化STM32微处理器被唤醒，并通过安装于潜水器顶部的玻璃浮球控制舱中的北斗定位与通信模块BDM910发送经纬度信息至母船；

(2)本发明中全海深无人潜水器本体上的运动控制系统即推进器通过CAN总线将ELMO驱动器和安装于潜水器顶部的玻璃浮球控制舱中的STM32微处理器连接，从而可以实现通过母船的北斗发送接收终端发送运动控制命令至无人潜水器的玻璃浮球控制舱内的北斗定位与通信模块BDM910，再通过CAN总线将命令传达至相应的ELMO驱动器完成推进器的运动操作；

(3)本发明中当全海深无人潜水器水面回收时母船通过北斗将其经纬度信息发送至安装于无人潜水器顶部的玻璃浮球控制舱中的北斗定位与通信模块BDM910，然后经过STM32微处理器处理结合PID航线控制算法可以控制无人潜水器自主的向母船运动完成回收，其中姿态传感器确保无人潜水器在航行过程中航向始终正确；

(4)本发明解决现有无人潜水器回收技术中存在的不足与缺陷，能够实现无人潜水器利用北斗定位与通信功能控制推进器自主的向母船航行，提升了无人潜水器海上实验回收的快速性、稳定性和准确性；

(5)本发明通用性好,可广泛用于深海作业型ROV、AUV、ARV等海洋装备的控制系统中；

(6)通过姿态传感器采集的航向角和各轴加速度信息，结合PID航线控制算法可以使无人潜水器回收航行时具有自主的航线及航行纠错能力，能够自行调整航向及各推进器运动状态，确保其始终在目标航线上运行且航线路径偏移量始终在可控范围内。

Claims (5)

1.一种全海深无人潜水器的自主回收装置，其特征在于，包括安装在无人潜水器本体顶部玻璃浮球控制舱、无人潜水器本体运动控制系统、北斗卫星和科考母船上的水面控制系统，玻璃浮球控制舱内有STM32微处理器、北斗定位与通信模块BDM910、温度传感器模块、光敏传感器、姿态传感器和电源模块，运动控制系统包括两个水平推进器、一个转向推进器及各推进器的ELMO驱动器；玻璃浮球控制舱和运动控制系统之间利用CAN总线通过水密电缆连接通信；姿态传感器、北斗定位与通信模块、温度传感器、光敏传感器与STM32微控制器之间分别通过IIC、USART、单总线、IO口进行通信；当无人潜水器浮出水面时，温度传感器、光敏传感器检测到周围环境变化启动北斗定位与通信模块BDM910，北斗定位与通信模块BDM910发送位置及姿态传感器采集的姿态数据通过北斗卫星至母船的水面控制系统，通过母船水面控制系统将母船位置信息发送至无人潜水器，无人潜水器顶端的玻璃浮球控制舱内的北斗定位与通信模块BDM910接收到母船位置信息后，将其作为目标点信息，结合姿态传感器采集的无人潜水器航向和速度信息，利用PID航线控制算法控制无人潜水器上三台推进器自主的向母船靠拢完成回收。

2.根据权利要求1所述全海深无人潜水器的自主回收装置，其特征在于，所述科考母船上的水面控制系统包括北斗发送接收终端、水面电源和计算机，监控无人潜水器回收状态与传输数据和命令。

3.根据权利要求1或2所述全海深无人潜水器的自主回收装置的控制方法，其特征在于，无人潜水器接收到北斗发送的母船位置信息P_ship(k)后，航线控制器结合无人潜水器本体位置信息P_ARV(k)进行航线解算，然后航向PID控制器结合姿态传感器的参数实时对无人潜水器回收的航向进行监测，若偏离了航线则通过姿态传感器反馈的速度值V_G(k)作为变量来调节各推进器的推力，同时修正无人潜水器的航向，缩小航线路径偏移量ε(k)实现航向的闭环控制；修正后的航向值Ψ(k)经过坐标变换和航线解算后，作为航线控制器的反馈变量实现对航线的闭环控制；无人潜水器在航向和航线的双闭环回收控制作用下维持在预定的航线上运动，完成无人潜水器自主的向母船航行，实现回收。

4.根据权利要求3所述全海深无人潜水器的自主回收装置的控制方法，其特征在于，无人潜水器双闭环回收控制系统中，定义无人潜水器中心点指向正北的方向为X轴，在水平方向上与X轴垂直指向正东方向为Y轴，外环的航线控制器通过母船位置P_ship(k)＝[X_ship(k)Y_ship(k)]^T和无人潜水器位置P_ARV(k)＝[X_ARV(k)Y_ARV(k)]^T的距离偏差来实现无人潜水器航线偏移量ε(k)的计算，具体为：

ε(k)＝L_k×sinδ(k)(1)

式中，

其中，L_k为回收过程中k时刻无人潜水器与母船位置P_ship之间的直线距离，δ(k)为回收过程中无人潜水器中心点与预定航线间的夹角。

5.根据权利要求4所述全海深无人潜水器的自主回收装置的控制方法，其特征在于，无人潜水器双闭环回收控制系统中，无人潜水器航线偏移量ε(k)与无人潜水器航向角的偏差作为内环航向PID控制器的输入量，控制无人潜水器艏部和水平推进器；姿态传感器采集测得的数据转换成无人潜水器平面运动的速度向量V(k),然后计算无人潜水器相对大地坐标系下的速度向量V_G(k),通过姿态传感器反馈的速度值V_G(k)作为变量来调节各推进器的推力，进而修正无人潜水器的航向：

V_G(k)＝R×V(k)(4)

式中，R欧拉角坐标转换矩阵为：

其中：θ为无人潜水器绕Y轴旋转的俯仰角，Ψ为无人潜水器绕X轴旋转的横滚角。