CN113132905B - 一种具有动态节点的自主水下机器人无线传感器网络 - Google Patents

Abstract

本发明提供一种具有动态节点的自主水下机器人无线传感器网络，包括作为水面网关的主节点、作为信息中继传输中介的汇聚节点和传感器节点，所述主节点包括水面部分的无线电调制解调器和水下部分的声学调制解调器；所述汇聚节点包括分别与其进行数据传输和通信的功率放大器、信号处理单元和接口增益控制模块；所述传感器节点包括装载各种传感器的自主水下机器人中央控制模块和传感器模块，所述传感器模块包括导航模块和运动规划模块；本发明提供的具有动态节点的自主水下机器人无线传感器网络无动力驱使的、多动态节点相互通信传输信息，保证自主水下机器人能够不断调整速度跟踪偏差而使水下机器人的实际位置进行不断调整以跟踪时变的期望轨迹。

Description

一种具有动态节点的自主水下机器人无线传感器网络

技术领域

本发明属于水下通信技术领域，具体涉及一种具有动态节点的自主水下机器人无线传感器网络。

背景技术

海洋拥有丰富的资源和广阔的空间。水下传感器网络部署在极其复杂可变的水下环境中，主要利用水声进行通信。在军事应用方面，水下武器系统的日益智能化，要求对其进行指挥控制通信，如：潜艇之间、母舰与潜艇或其它水下无人作战平台之间的通信，对水下航行器实施监测和导航，以及对水雷的远程声遥控等。在民用方面，如水下语音通信、工业用海岸遥测、水下机器人和海上平台的遥控指令传送、海底勘探数据与图像的传输，环境系统中的污染监测数据，水文站的采集数据等等，无不使其对水下通信的需求大为增加。

水下无线传感器网络是使用飞行器、潜艇或水面舰将大量的微型传感器节点随机布放到感兴趣水域，节点通过水声无线通信方式形成的一个多跳的自组织的网络系统，协作地感知、采集和处理网络覆盖区域中感知对象的信息，并发送给接收者。

但是，现有技术中的浮标均是静态节点，不能够适应海洋浮动动态的环境信息采集需求。

发明内容

本发明针对上述缺陷，提供一种无动力驱使的、多动态节点相互通信传输信息，保证自主水下机器人能够不断调整速度跟踪偏差而使水下机器人的实际位置进行不断调整以跟踪时变的期望轨迹的具有动态节点的自主水下机器人无线传感器网络。

本发明提供如下技术方案：一种具有动态节点的自主水下机器人无线传感器网络，包括作为水面网关的主节点、作为信息中继传输中介的汇聚节点和传感器节点，

所述主节点包括水面部分的无线电调制解调器和水下部分的声学调制解调器；

所述汇聚节点包括分别与其进行数据传输和通信的功率放大器、信号处理单元和接口增益控制模块；

所述传感器节点包括装载各种传感器的自主水下机器人中央控制模块和传感器模块，所述传感器模块包括环境探测模块、电源监控和管理模块、通信模块、动力模块、障碍探测模块、导航模块和运动规划模块。

进一步地，所述功率放大器与声学换能器通信连接，所述信号处理单元与存储单元通信连接，所述接口增益控制模块与传感器模块接口通信连接。

进一步地，所述环境探测模块包括水压探测传感器、盐度探测传感器和水温探测传感器。

进一步地，所述主节点的水下部分与有限网络互连时，所述主节点作为岸基网关。

进一步地，所述主节点由浮标或舰船搭建，水上部分为无线电调制解调器，水下部分为声学调制解调器时，所述主节点作为水面网关。

进一步地，根据整个水下或水上网络系统数据传输量的需求，所述主节点的数量不唯一。

进一步地，所述障碍探测模块内部设置有GPS定位系统，用于实时定位所述水下无线传感器网络所在位置坐标。

进一步地，所述导航模块和运动规划模块的导航方法，包括以下步骤：

1)、构建所述自主水下机器人的地心坐标系水平运动模型：

其中所述u为所述自主水下机器人的纵荡线速度，所述v为所述自主水下机器人的和横荡线速度，所述r是所述自主水下机器人的偏航角速度；所述x为所述自主水下机器人的质心纵荡方向坐标，所述y为所述自主水下机器人的质心横荡方向坐标，所述ψ为所述自主水下机器人的目标方向；

2)、在忽略所述垂荡、横摇和纵摇运动，构建所述自主水下机器人的自主水下机器人坐标系横向动力学模型：

其中，所述

所述

所述

所述

所述

所述

所述m是所述自主水下机器人的质量，I_z是所述自主水下机器人绕垂直方向z轴的惯性矩，X_u、Y_v和N_r为所述位置坐标包含线性阻尼影响的负项，

和

分别是纵荡、横摇和偏航运动方向的水动力附加质量项；所述τ_u为激励器产生的喘振力，所述τ_r为激励器产生的横摆力矩，所述τ_u和τ_r为控制输入；

3)、接收所述自主水下机器人中央控制模块(4)传输的期望位置信号[x_d,y_d]^T，根据所述期望位置信号的时间导数选择目标根据期望位置坐标进行运动时的时变位置期望纵荡速度u_d和时变位置期望横荡速度v_d，得到期望速度[u_d,v_d]^T：

4)、根据所述步骤3)得到的期望速度[u_d,v_d]^T和所述动力模块(5-4)得到的所述自主水下机器人的实际运动的纵荡速度u和横档速度v，计算速度跟踪偏差[e_u,e_v]^T：

e_u＝u-u_d；

e_v＝v-v_d；

所述e_u纵荡方向速度误差值，所述e_v为横荡方向速度误差值；

5)、根据所述步骤1)和所述步骤2)得到的所述自主水下机器人的动力学模型，计算所述自主水下机器人的位置跟踪误差[x_e,y_e]^T和位置误差动力学模型

结合不断调整的所述位置误差动力学模型

与所述步骤4)得到的速度跟踪偏差[e_u,e_v]^T，可以得到根据不断调整的位置误差而得到的速度误差动力学模型

进而得到导航和运动规划控制律，所述控制律计算所施加的纵荡力τ_u和横摆力矩τ_r，从而调整所述期望速度[u_d,v_d]^T，得到期望速度[u′_d,v′_d]^T，使水下机器人的实际位置

进行不断调整以跟踪时变的期望轨迹

其中，所述

为时变位置纵荡方向速度的导数，所述横荡方向速度误差值的导数，所述

为时变位置横荡方向速度的导数；所述

所述

进一步地，所述步骤5)中包括以下步骤：

501)、结合所述步骤1)和所述步骤2)构建得到的未施加驱动力的自主动态漂浮横向动力学模型，构建所述自主水下机器人的位置跟踪误差[x_e,y_e]^T；

x_e＝x-x_d；y_e＝y-y_d；

其中，所述x_d和所述y_d分别为目标根据期望位置坐标进行运动时的时变位置纵荡方向坐标和时变位置横荡方向坐标；即跟随时间变化的坐标；所述x_e为纵荡方向坐标误差值，所述y_e为横荡方向坐标误差值；

502)、根据所述步501)得到的位置误差[x_e,y_e]^T，分别计算所述x_e和所述y_e的导数的同时结合所述步骤1)得到的水平运动模型，构建所述自主水下机器人的位置误差动力学模型

503)、根据所述步骤502)得到的位置误差动力学模型

进而得到需要实时调整期望的时变轨迹

根据所述

和位置跟踪误差[x_e,y_e]^T，在控制律的作用下，构建调整后的期望纵荡速度u′_d和横荡速度v′_d的矩阵[u′_d,v′_d]^T：

其中，所述k_x＞0，所述k_y＞0，所述l_x≠0，所述l_y≠0，当所述步骤4)中的速度跟踪偏差差收敛到零时，保证所述步骤501)中定义的位置跟踪误差[x_e,y_e]^T渐近收敛到零。

本发明的有益效果为：

1、本发明水下无线传感器网络节点增加AUV新节点形式。具有布防更灵活、适应环境的能力更强、搜集信息更快捷的特点。相比普通静态水下无线传感器节点，AUV由多套智能系统组成，结构复杂、性能更强、功能更丰富。

2、水下无线传感器网络是由布放在海底、海中的传感器节点(包括固定的传感器节点和装载传感器的AUV平台)和海面浮标节点以及它们之间的双向声链路组成的分布式、多节点、大面积覆盖水下三维区域，可以对信息进行采集、处理、分类和压缩，普通节点用于获取环境数据，AUV平台的任务在于更加机动的执行任务，汇聚节点用于收集普通节点数据并与岸边基站控制中心进行连接。这些设备都通过水声信道相互连接与通信。这样就构成了一个可交互的网络环境，岸上用户可以实时有效的获取感兴趣的水下传感器数据，并可以随时将控制信息传递给水下节点完成对水下传感器网络的重新配置和修改。

3、本发明提供的自主水下机器人无线传感器网络中的导航模块和运动规划模块通过构建无动力驱动的包括定义在地心坐标系中的位置和方位(x，y，ψ)、定义在自主水下机器人坐标系中线速度和角速度(u，v，r)自主水下机器人的运动学模型，从运动学模型的角度设计了期望的纵荡速度和横荡速度[u_d,v_d]^T，以保证位置跟踪误差收敛到零；

4、通过结合不断调整的所述位置误差动力学模型

与所述步骤4)得到的速度跟踪偏差[e_u,e_v]^T，可以得到根据不断调整的位置误差而得到的速度误差动力学模型

进而得到导航和运动规划控制律，所述控制律计算所施加的纵荡力τ_u和横摆力矩τ_r，从而调整所述期望速度[u_d,v_d]^T，得到期望速度[u′_d,v′_d]^T，使水下机器人的实际位置

进行不断调整以跟踪时变的期望轨迹

以保证AUV的速度收敛到不断调整而得到的期望速度[u′_d,v′_d]^T，该方法具有有限时间收敛性和稳态误差改善等优点。

附图说明

在下文中将基于实施例并参考附图来对本发明进行更详细的描述。

其中：

图1为本发明提供的具有动态节点的自主水下机器人无线传感器网络整体示意图；

图2为本发明提供的汇聚节点结构示意图；

图3为本发明提供的传感器节点结构示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例1

如图1所示，为本实施例提供的一种具有动态节点的自主水下机器人(AUV)无线传感器网络，包括作为水面网关的主节点1、作为信息中继传输中介的汇聚节点2和传感器节点3，

主节点1包括水面部分的无线电调制解调器和水下部分的声学调制解调器；把水下无线传感器网络与常规通信网络相连的中转枢纽节点。其中主节点用于接收水下无线传感器网络传输的数据信息，再将上传的水下信息按照无线电或以太网协议进行变换，再转发出去。

如图2所示，汇聚节点2包括分别与其进行数据传输和通信的功率放大器2-1、信号处理单元2-2和接口增益控制模块2-3；汇聚节点2主要用于中继来自其它节点(传感器节点3、主节点1、别的汇聚节点2)的数据或指令，它本身可能并不具备探测采集功能，而是将接收的数据按照网络路由协议进行处理再转发出去。相对于传感器节点，它可以不带有传感器，而使用一个或两个声学换能器，实现半双工或全双工通信。按布放空间分析，汇聚节点填充在主节点和传感器节点中间，其作用是扩大整个网络的作用范围，增加探测数据传输距离。

如图3所示，传感器节点3包括装载各种传感器的自主水下机器人中央控制模块4和传感器模块5，传感器模块5包括环境探测模块5-1、电源监控和管理模块5-2、通信模块5-3、动力模块5-4、障碍探测模块5-5、导航模块和运动规划模块5-6；通过特定传感器获取水下环境参数信息，并通过设备接口将包含信息的电信号送给节点信号处理单元，再通过水声信道将数据转发出去。传感器节点的主要功能是用于收集和存储水下的信息。

如图2所示，功率放大器2-1与声学换能器通信连接，信号处理单元2-2与存储单元通信连接，接口增益控制模块2-3与传感器模块3接口通信连接。

环境探测模块5-1包括水压探测传感器、盐度探测传感器和水温探测传感器，用于探测所示自主水下机器人所在水域的水压、盐度和水温信息，感知周围水域的环境参数，帮助AUV采集应用所需的兴趣数据。

电源监控和管理模块5-2为AUV提供电能供应，电源管理在这里进行；通信模块5-3使得AUV具备在水下与其他节点或基站交互信息的能力；动力模块5-4是AUV区别于普通静态水下无线传感器节点最重要的特征，它保证了AUV具备在水下自由移动的能力，从而帮助AUV完成许多普通静态节点无法完成的任务。

障碍探测模块5-5内部设置有GPS定位系统，用于实时定位水下无线传感器网络所在位置坐标；障碍探测模块5-5够探测AUV附近存在的障碍物，内部配备有GPS定位系统，它能够实时定位AUV当前所在的位置坐标，这对AUV来说至关重要；导航模块和运动规划模块5-6系统帮助AUV在移动过程中保持正确的路径和姿态。所有这些模块由自主水下机器人中央控制模块4统一管理，自主水下机器人中央控制模块4作为AUV的逻辑中枢，所有的数据处理、运算、指令逻辑等在这里完成，它负责控制和协调各个系统有序工作，是整个系统的核心。

主节点1的水下部分与有限网络互连，主节点1作为岸基网关。根据整个水下或水上网络系统数据传输量的需求，主节点1的数量不唯一。

水下无线传感器网络是由布放在海底、海中的传感器节点(包括固定的传感器节点和装载传感器的AUV平台)和海面浮标节点以及它们之间的双向声链路组成的分布式、多节点、大面积覆盖水下三维区域，可以对信息进行采集、处理、分类和压缩，普通节点用于获取环境数据，AUV平台的任务在于更加机动的执行任务，汇聚节点用于收集普通节点数据并与岸边基站控制中心进行连接。这些设备都通过水声信道相互连接与通信。这样就构成了一个可交互的网络环境，岸上用户可以实时有效的获取感兴趣的水下传感器数据，并可以随时将控制信息传递给水下节点完成对水下传感器网络的重新配置和修改。

传感器节点监测的数据沿着传感器骨干节点(簇首)多跳地进行传输，在传输过程中监测数据可能被多个节点处理，经过多跳后路由到汇聚节点，最后通过互联网或卫星到达管理节点。用户通过管理节点对传感器网络进行配置和管理，发布监测任务以及收集监测数据。

实施例2

本实施例提供实施例1中的导航模块和运动规划模块5-6的导航方法，包括以下步骤：

1)、构建自主水下机器人的地心坐标系水平运动模型：

其中u为自主水下机器人的纵荡线速度，v为自主水下机器人的和横荡线速度，r是自主水下机器人的偏航角速度；x为自主水下机器人的质心纵荡方向坐标，y为自主水下机器人的质心横荡方向坐标，ψ为自主水下机器人的目标方向；

2)、在忽略垂荡、横摇和纵摇运动，构建自主水下机器人的自主水下机器人坐标系横向动力学模型：

其中，

m是自主水下机器人的质量，I_z是自主水下机器人绕垂直方向z轴的惯性矩，X_u、Y_v和N_r为位置坐标包含线性阻尼影响的负项，

和

分别是纵荡、横摇和偏航运动方向的水动力附加质量项；τ_u为激励器产生的喘振力，τ_r为激励器产生的横摆力矩，τ_u和τ_r为控制输入；

AUV的位置和方位，即(x，y，ψ)定义在地心坐标系中，而线速度和角速度，即(u，v，r)定义在自主水下机器人坐标系中；

由步骤1)和步骤2)构建得到的包括定义在地心坐标系的自主水下机器人的具有位置和方位(x，y，ψ)信息的水平运动模型和定义在自主水下机器人坐标系中具有线速度和角速度(u，v，r)的横向动力学模型的AUV模型的是未施加外部驱动力的动力学模型，因为驱动力和力矩仅在摇摆运动未被驱动的情况下在纵荡和横摆方向产生。未施加驱动力情况下，自主水下机器人自动动态漂浮的一个运动模型，保证了水下自主机器人在未施加外部驱动力时自动漂浮的导航和运动规划的一个基准；

3)、接收自主水下机器人中央控制模块(4)传输的期望位置信号[x_d,y_d]^T，根据期望位置信号的时间导数选择目标根据期望位置坐标进行运动时的时变位置期望纵荡速度u_d和时变位置期望横荡速度v_d，得到期望速度[u_d,v_d]^T：

4)、根据步骤3)得到的期望速度[u_d,v_d]^T和动力模块(5-4)得到的自主水下机器人的实际运动的纵荡速度u和横档速度v，计算速度跟踪偏差[e_u,e_v]^T：

e_u＝u-u_d；

e_v＝v-v_d；

e_u纵荡方向速度误差值，e_v为横荡方向速度误差值；

5)、根据步骤1)和步骤2)得到的自主水下机器人的动力学模型，计算自主水下机器人的位置跟踪误差[x_e,y_e]^T和位置误差动力学模型

结合不断调整的位置误差动力学模型

与步骤4)得到的速度跟踪偏差[e_u,e_v]^T，可以得到根据不断调整的位置误差而得到的速度误差动力学模型

进而得到导航和运动规划控制律，控制律计算所施加的纵荡力τ_u和横摆力矩τ_r，从而调整期望速度[u_d,v_d]^T，得到期望速度[u′_d,v′_d]^T，使水下机器人的实际位置

进行不断调整以跟踪时变的期望轨迹

其中，

为时变位置纵荡方向速度的导数，横荡方向速度误差值的导数，

为时变位置横荡方向速度的导数；

具体地，步骤5)中包括以下步骤：

501)、结合步骤1)和步骤2)构建得到的未施加驱动力的自主动态漂浮横向动力学模型，构建自主水下机器人的位置跟踪误差[x_e,y_e]^T；

x_e＝x-x_d；y_e＝y-y_d；

其中，x_d和y_d分别为目标根据期望位置坐标进行运动时的时变位置纵荡方向坐标和时变位置横荡方向坐标；即跟随时间变化的坐标；x_e为纵荡方向坐标误差值，y_e为横荡方向坐标误差值；

502)、根据步501)得到的位置误差[x_e,y_e]^T，分别计算x_e和y_e的导数的同时结合步骤1)得到的水平运动模型，构建自主水下机器人的位置误差动力学模型

503)、根据步骤502)得到的位置误差动力学模型

进而得到需要实时调整期望的时变轨迹

根据

和位置跟踪误差[x_e,y_e]^T，在控制律的作用下，构建调整后的期望纵荡速度u′_d和横荡速度v′_d的矩阵[u′_d,v′_d]^T：

其中，k_x＞0，k_y＞0，l_x≠0，l_y≠0，当步骤4)中的速度跟踪偏差差收敛到零时，保证步骤501)中定义的位置跟踪误差[x_e,y_e]^T渐近收敛到零。

实施例3

本实施例与实施例1的区别仅在于，主节点1由浮标或舰船搭建，水上部分为无线电调制解调器，水下部分为声学调制解调器时，主节点1作为水面网关。

虽然已经参考优选实施例对本发明进行了描述，但在不脱离本发明的范围的情况下，可以对其进行各种改进并且可以用等效物替换其中的部件。尤其是，只要不存在结构冲突，各个实施例中所提到的各项技术特征均可以任意方式组合起来。本发明并不局限于文中公开的特定实施例，而是包括落入权利要求的范围内的所有技术方案。

Claims (8)

1.一种具有动态节点的自主水下机器人无线传感器网络，包括作为水面网关的主节点(1)、作为信息中继传输中介的汇聚节点(2)和传感器节点(3)，其特征在于，

所述主节点(1)包括水面部分的无线电调制解调器(1-1)和水下部分的声学调制解调器(1-2)；

所述汇聚节点(2)包括分别与其进行数据传输和通信的功率放大器(2-1)、信号处理单元(2-2)和接口增益控制模块(2-3)；

所述传感器节点(3)包括装载各种传感器的自主水下机器人中央控制模块(4)和传感器模块(5)，所述传感器模块(5)包括环境探测模块(5-1)、电源监控和管理模块(5-2)、通信模块(5-3)、动力模块(5-4)、障碍探测模块(5-5)、导航模块和运动规划模块(5-6)；

所述导航模块和运动规划模块(5-6)的导航方法，包括以下步骤：

1)、构建所述自主水下机器人的地心坐标系水平运动模型：

其中所述u为所述自主水下机器人的纵荡线速度，所述v为所述自主水下机器人的横荡线速度，所述r是所述自主水下机器人的偏航角速度；x为所述自主水下机器人的质心纵荡方向坐标，y为所述自主水下机器人的质心横荡方向坐标，所述ψ为所述自主水下机器人的目标方向；

2)、在忽略所述垂荡、横摇和纵摇运动，构建所述自主水下机器人的自主水下机器人坐标系横向动力学模型：

其中，所述

所述

所述

所述

所述

所述

所述m是所述自主水下机器人的质量，I_z是所述自主水下机器人绕垂直方向z轴的惯性矩，X_u、Y_v和N_r为所述位置坐标包含线性阻尼影响的负项，

和

分别是纵荡、横摇和偏航运动方向的水动力附加质量项；所述τ_u为激励器产生的喘振力，所述τ_r为激励器产生的横摆力矩，所述τ_u和τ_r为控制输入；

3)、接收所述自主水下机器人中央控制模块(4)传输的期望位置信号[x_d，y_d]^T，根据所述期望位置信号的时间导数选择目标根据期望位置坐标进行运动时的时变位置期望纵荡速度u_d和时变位置期望横荡速度v_d，得到期望速度[u_d，v_d]^T：

4)、根据所述步骤3)得到的期望速度[u_d，v_d]^T和所述动力模块(5-4)得到的所述自主水下机器人的实际运动的纵荡速度u和横档速度v，计算速度跟踪偏差[e_u，e_v]^T：

e_u＝u-u_d；

e_v＝v-v_d；

所述e_u纵荡方向速度误差值，所述e_v为横荡方向速度误差值；

5)、根据所述步骤1)和所述步骤2)得到的所述自主水下机器人的动力学模型，计算所述自主水下机器人的位置跟踪误差[x_e，y_e]^T和位置误差动力学模型

结合不断调整的所述位置误差动力学模型

与所述步骤4)得到的速度跟踪偏差[e_u，e_v]^T，可以得到根据不断调整的位置误差而得到的速度误差动力学模型

进而得到导航和运动规划控制律，所述控制律计算所施加的纵荡力τ_u和横摆力矩τ_r，从而调整所述期望速度[u_d，v_d]^T，得到期望速度[u′_d，v′_d]^T，使水下机器人的实际位置

进行不断调整以跟踪时变的期望轨迹

其中，所述

为时变位置纵荡方向速度的导数，所述

为时变位置横荡方向速度的导数；所述

所述

2.根据权利要求1所述的一种具有动态节点的自主水下机器人无线传感器网络，其特征在于，所述功率放大器(2-1)与声学换能器通信连接，所述信号处理单元(2-2)与存储单元通信连接，所述接口增益控制模块(2-3)与传感器模块(3)接口通信连接。

3.根据权利要求1所述的一种具有动态节点的自主水下机器人无线传感器网络，其特征在于，所述环境探测模块(5-1)包括水压探测传感器、盐度探测传感器和水温探测传感器。

4.根据权利要求1所述的一种具有动态节点的自主水下机器人无线传感器网络，其特征在于，所述主节点(1)的水下部分与有限网络互连时，所述主节点(1)作为岸基网关。

5.根据权利要求1所述的一种具有动态节点的自主水下机器人无线传感器网络，其特征在于，所述主节点(1)由浮标或舰船搭建，水上部分为无线电调制解调器，水下部分为声学调制解调器时，所述主节点(1)作为水面网关。

6.根据权利要求4或5所述的一种具有动态节点的自主水下机器人无线传感器网络，其特征在于，根据整个水下或水上网络系统数据传输量的需求，所述主节点(1)的数量不唯一。

7.根据权利要求1所述的一种具有动态节点的自主水下机器人无线传感器网络，其特征在于，所述障碍探测模块(5-5)内部设置有GPS定位系统，用于实时定位所述水下无线传感器网络所在位置坐标。

8.根据权利要求1所述的一种具有动态节点的自主水下机器人无线传感器网络，其特征在于，所述步骤5)中包括以下步骤：

501)、结合所述步骤1)和所述步骤2)构建得到的未施加驱动力的自主动态漂浮横向动力学模型，构建所述自主水下机器人的位置跟踪误差[x_e，y_e]^T；

x_e＝x-x_d；y_e＝y-y_d；

其中，所述x_d和所述y_d分别为目标根据期望位置坐标进行运动时的时变位置纵荡方向坐标和时变位置横荡方向坐标；即跟随时间变化的坐标；所述x_e为纵荡方向坐标误差值，所述y_e为横荡方向坐标误差值；

502)、根据所述步501)得到的位置误差[x_e，y_e]^T，分别计算所述x_e和所述y_e的导数的同时结合所述步骤1)得到的水平运动模型，构建所述自主水下机器人的位置误差动力学模型

503)、根据所述步骤502)得到的位置误差动力学模型

进而得到需要实时调整时变的期望轨迹

根据所述

和位置跟踪误差[x_e，y_e]^T，在控制律的作用下，构建调整后的期望纵荡速度u′_d和横荡速度v′_d的矩阵[u′_d，v′_d]^T：

其中，所述k_x＞0，所述k_y＞0，所述l_x≠0，所述l_y≠0，当所述步骤4)中的速度跟踪偏差收敛到零时，保证所述步骤501)中定义的位置跟踪误差[x_e，y_e]^T渐近收敛到零。

Images