一种UUV轨迹跟踪控制系统的制作方法

一种uuv轨迹跟踪控制系统
技术领域
1.本发明涉及船舶控制技术领域，尤其涉及一种uuv轨迹跟踪控制系统。


背景技术：

2.无人水下航行器(unmanned underwater vehicle ,uuv)是探索典型的电能和机械能变换的系统，可以将其看成由机械端口和电气端口结合组成的二端口能量变换的装置：机械能由机械端口输出，电能由电气端口输入。为提高uuv的电能利用效率，提高控制精度，有效利用各种传感器的测量信息，从能量角度研究uuv的运动控制，有着重要的理论意义和工程实际价值。
3.与此同时，uuv在科学研究、商业开发和军事方面都有着广泛用途，如海底沉船搜索、救援，水下未知地形探索、资源勘探，海底石油管道检查，水下焊接，水文测绘等方面的应用。在uuv完成各种水下作业任务的时候，能对任意设定轨迹的进行精确跟踪是uuv必不可少的能力之一，尤其是在执行水下区域搜索和海底地形测绘任务的时候，轨迹跟踪的精确性将会影响到搜索覆盖的区域和测绘数据的准确性。
4.通常情况下，uuv在航行过程中会受到海洋环境的干扰，使得uuv系统不稳定，无法按照预定轨迹航行，为提高uuv系统的鲁棒性，需要在uuv控制系统设计鲁棒镇定控制器。
5.现有设计的鲁棒镇定控制器结构简单，易于实现，在工程中得到广泛应用，但是现有鲁棒镇定控制器都是基于线性模型或近似线性模型设计的，不符合uuv控制箱实际物理状况，使得所设计的鲁棒镇定控制器鲁棒性较差。
6.此外，在uuv推进器系统中，相对于控制器快速的改变控制指令，推进器的反应速度则显得过慢过长，存在滞后特性，由于存在时滞，响应时间会增加，超调量也会增加。
7.现有所设计的控制器模型太过理想化，没有综合考虑影响uuv推进器系统的时滞问题。
8.鉴于以上实际问题，对uuv轨迹跟踪控制系统设计问题进行研究显得格外重要。


技术实现要素：

9.本发明的目的在于提供一种uuv轨迹跟踪控制系统，基于能量方法，从非线性和uuv推进器时滞角度设计轨迹跟踪控制器，提高uuv控制精度，准确控制uuv路劲跟踪。
10.为实现上述发明目的，本发明采用下述技术方案予以实现：本技术涉及一种uuv轨迹跟踪控制系统，包括uuv、传感器模块、推力分配模块和推进器，其特征在于，还包括：轨迹跟踪控制器，其接收所述传感器模块反馈的状态量和预定轨迹输入模块提供的状态量的设定值，且所述轨迹跟踪控制器与所述推力分配模块连接；执行信号检测模块，其用于检测所述轨迹跟踪控制器输出至所述推力分配模块的执行信号；响应信号检测模块，其用于检测所述推进器做出响应的响应信号；
时间间隔测量模块，其分别与所述执行信号检测模块和响应信号检测模块连接，用于检测所述响应信号和执行信号之间的时间差；时滞补偿器，其接收所述时间差并将具有时滞项的状态量传递至所述轨迹跟踪控制器；其中，基于能量方法设计非线性且具有时滞特性的轨迹跟踪控制器。
11.本技术提供的uuv轨迹跟踪控制系统，从非线性和时滞角度出发，基于能量方法建立轨迹跟踪控制器，避免现有设计中理想线性化建模所带来的控制精度差的问题，且充分考虑了uuv海洋实际运动环境，更符合uuv实际运行控制，对uuv轨迹跟踪起到良好的效果，提高uuv运行稳定性及可靠性。
12.在本技术的一些实施例中，所述传感器模块包括：水声定位系统，其用于采集uuv的位置信息；测速仪，其用于采集uuv的速度信息；磁罗盘，其用于采集uuv的艏摇角度信息。
13.在本技术的一些实施例中，所述轨迹跟踪控制器的具体设计过程如下：s1：获取uuv状态量的信息；s2：建立uuv的非线性运动学模型和动力学方程模型：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（1）其中，式(1)中η是uuv在北东坐标系下的位置向量，分别表示为纵荡位移、横荡位移、艏摇角度，是随船坐标系下的速度向量，分别表示纵荡速度、横荡速度和艏摇角速度，表示为控制量。是船体惯性矩阵，是线性水阻尼矩阵，并且为海洋环境干扰，海洋环境变化缓慢，视为定常干扰，是坐标转换矩阵，满足：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（2）根据公式（1）和（2），获取uuv非线性运动学模型：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（3）获取uuv动力学模型：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)其中，m=diag(m
11
,m
22
,m
33
)，d=diag(d
11
,d
22
,d
33
)，m
11
、m
22
、m
33
表示附加质量在内的惯性系数，d
11
、d
22
、d
33
为水动力阻尼系数；s3：建立uuv轨迹跟踪状态误差模型：定义系统状态量；
ꢀꢀꢀꢀꢀ
（5）根据式（1）和式（5），获取如下状态误差模型：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（6）s4：建立uuv时滞系统运动模型为：（7）其中，h为时滞时间常数，，；s5：基于能量方法将模型（7）等价为pch模型：设计阻尼注入控制器：
ꢀꢀꢀꢀꢀ
（8）其中，k2=diag(k1,k2,k3)，，为轨迹跟踪控制器；选取闭环时uuv时滞系统的hamilton函数为：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（9）根据（8）和（9），将模型（7）等价为
ꢀꢀ
（10）其中，，，；考虑uuv时滞系统运动模型运行过程中产生的参数摄动等不确定因素，则uuv的
pch模型变为：
ꢀꢀꢀꢀ
（11）其中，p为uuv时滞系统运动模型中结构不确定性的参数向量；s6：构造罚函数，并设计所述轨迹跟踪控制器：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（12）将式（12）带入（11），获取到:
ꢀꢀꢀꢀ
（13）其中，是权重矩阵，，, ，，，，。
14.在本技术的一些实施例中，所述uuv轨迹跟踪控制系统还包括验证模块，其利用李雅普诺夫理论验证所述轨迹跟踪控制器的的稳定性。
15.在本技术的一些实施例中，所述验证模块在验证时分别选取时滞无关的李雅普诺夫函数：
ꢀꢀꢀ
（14）；以及时滞相关的李雅普诺夫函数：时滞相关的李雅普诺夫函数：时滞相关的李雅普诺夫函数：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（15）；其中，。
16.结合附图阅读本发明的具体实施方式后，本发明的其他特点和优点将变得更加清楚。
附图说明
17.为了更清楚地说明本发明实施例中的技术方案，下面将对实施例中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
18.图1是本发明提出的uuv轨迹跟踪控制系统一实施例的原理框图；图2是本发明提出的uuv轨迹跟踪控制系统实施例中设计轨迹跟踪控制器的流程图；图3是本发明提出的uuv轨迹跟踪控制系统实施例的轨迹跟踪仿真结果；图4是对本发明提出的uuv轨迹跟踪控制系统实施例中纵荡位移误差变化仿真图；图5是对本发明提出的uuv轨迹跟踪控制系统实施例中横荡位移误差变化仿真图；图6是对本发明提出的uuv轨迹跟踪控制系统实施例中艏摇角度误差变化仿真图。
具体实施方式
19.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。
20.基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。在本发明的描述中，需要理解的是，术语“中心”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
21.在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。在上述实施方式的描述中，具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
22.术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明的描述中，除非另有说明，“多个”的含义是两个或两个以上。
23.本技术涉及一种uuv轨迹跟踪控制系统，基于能量方法设计非线性且具有时滞特性的轨迹跟踪系统，能够同时考虑uuv推进器时滞和海洋干扰问题，提高控制精度，使设计的轨迹跟踪控制系统更加符合uuv海洋实际运动环境。
24.参见图1，本技术uuv轨迹跟踪控制系统，包括uuv 10、传感器模块20、推力分配模块40、信号处理模块90、轨迹跟踪控制器30、推进器50、执行信号检测模块70、响应信号检测模块80、时间间隔测量模块60、时间补偿器60'和预定轨迹输入模块100。
25.本技术所提到的环境干扰为uuv所受环境干扰的合力，该合力由海洋环境干扰力和推进器50所提供的推力两部分组成，所提到的时滞问题指轨迹跟踪控制器30发出控制指令，推进器50响应时间存在延时，导致uuv执行机构时滞现象。
26.传感器模块20可以包括水声定位系统、测速仪和磁罗盘。
27.水声定位系统在航海领域内时比较常用的系统，其可以依赖于gps完成，用于采集uuv位置信息；测速仪可以采用速度传感器或多普勒测速仪完成、用于实际采集uuv的速度信息；磁罗盘用于采集uuv的艏摇角度信息。
28.传感器模块20将采集到的位置信息、速度信息和艏摇角度信息发送至信号处理模块90，进行去噪处理，之后传递至轨迹跟踪控制器30，即，传感器模块20输出的信息可以记为状态量。
29.轨迹跟踪控制器30还接收预定轨迹输入100模块提供的状态量的设定值，轨迹跟踪控制器30根据所反馈的状态量和状态量的设定值，对推力分配模块40进行控制，从而实现uuv 10按照预定轨迹运行，实现轨迹跟踪控制，其中需要考虑时滞问题和海洋环境干扰问题，设计轨迹跟踪控制器30，且在设计过程中，主要关心对uuv位置信息（例如，纵荡位移、横荡位移）的控制。
30.执行信号检测模块70用于检测轨迹跟踪控制器30输出至推力分配模块40的执行信号；响应信号检测模块80用于检测推进器50做出响应的响应信号。
31.当时间间隔测量模块60检测响应信号和执行信号之间存在时间差时，说明推进器50发生时滞问题，此时时间间隔测量模块60测出时滞时间常数h。
32.根据时滞时间常数h，经过时滞补偿器60'将输出带有时滞项的位置、速度和艏摇角度信息，并传递至轨迹跟踪控制器30，以消除因时滞对象信息反馈不及时而出现超调和振荡现象。
33.在设计轨迹跟踪控制器30时，考虑海洋环境对uuv 10的海洋干扰力w，且基于能量方法设计非线性且具有时滞特性的轨迹跟踪控制器30。
34.如下将参考图2，给出具体设计轨迹跟踪控制器30的步骤。
35.s1：获取uuv状态量的信息。
36.基于图1中给出的原理框图，获取uuv状态量的信息，此处的状态量的信息包括位置、速度和艏摇角度。
37.s2：建立uuv的非线性运动学模型和动力学方程模型。
38.首先，建立如下模型：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（1）其中，式(1)中η是uuv在北东坐标系下的位置向量，分别表示为纵荡位移、横荡位移、艏摇角度，是随船坐标系下的速度向量，分别表示纵荡速度、横荡速度和艏摇角速度，表示为控制量。是船体惯性矩阵，是线性水阻尼矩阵，并且为海洋环境干扰，海洋环境变化缓慢，视为
定常干扰，是坐标转换矩阵，满足：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（2）且式（2）具有和的特征。
39.由于矩阵m=diag(m11,m22,m33)，矩阵d=diag(d11,d22,d33)，将公式（1）展开得到，获取uuv非线性运动学模型：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（3）获取uuv动力学模型：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（4）其中m11、m22、m33表示附加质量在内的惯性系数，其可以通过已知数获取，即，m11、m22、m33也为已知数。
40.，，，表示uuv关于z轴的转动惯量，m为uuv质量，其他参数例如为一阶水动力系数。
41.，，，且d11、d22、d33均为水动力阻尼系数。
42.、、、、、、、、及为uv的各项水动力参数。
43.以上符合是由美国海军建筑师和轮机工程师学会（sname）在1950年制定的，现在仍然使用。
44.需要说明的是，在已知如如上所述的uuv的模型参数时，可以易于计算出上述参数m11、m22、m33、d11、d22和d33，即获取到m和d。
45.s3：建立uuv轨迹跟踪状态误差模型。
46.轨迹跟踪控制器输出的状态量为x1和x2。
47.重新定义系统状态量：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（5）即，状态量的偏差为：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（6）其中，和为预设的期望状态量。
48.根据式（1）、式（5）和式（6），获取如下状态误差模型：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（7）需要说明的是，对于uuv动态系统来说，给定时变参考轨迹，其一阶导数和二阶导数均可以由其导航系统计算得到，则期望状态量为：。
49.s4：建立uuv时滞系统运动模型。
50.根据时间间隔测量模块输出的时滞时间常数h，以及时间补偿器的补偿输出，从而建立uuv时滞系统运动模型：（8）其中，时间补偿器的补偿输出为a1和a2：，。
51.其中，控制量，为阻尼注入控制器，为轨迹跟踪控制器。
52.s5：基于能量方法将模型（8）等价为pch（port-controlled hamilton，端口受控哈密顿）模型。
53.为了将模型（8）等价为pch模型，首先设计阻尼注入控制器：
ꢀꢀꢀꢀꢀꢀꢀꢀ
（9）其中，k2=diag(k1,k2,k3)且为正常数。
54.其次，选取闭环时uuv时滞系统的hamilton函数为：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（10）根据（9）和（10），将模型（8）等价为如下：
ꢀꢀ
（11）其中，。
55.因此，式（11）可以简写为如下：
ꢀꢀꢀꢀꢀꢀꢀ
（12）其中，，，。
56.考虑uuv时滞系统运动模型运行过程中产生的参数摄动等不确定因素，此时uuv时滞系统运动模型参数不再精确，因此，可以将uuv的pch模型变为如下：
ꢀꢀꢀꢀꢀ
（13）其中，p为uuv时滞系统运动模型中结构不确定性的参数向量，y(t)表示uuv系统输出。
57.出于简化，令，因此，式（13）可以是简化为如下：
ꢀꢀꢀ
（14）其中，。
58.s6：设计轨迹跟踪控制器。
59.研究uuv轨迹跟踪控制系统在受海洋环境干扰时的稳定性，为了满足系统基于状态观测器的鲁棒控制要求，构造罚函数，其中，是权重矩阵，并设计所述轨迹跟踪控制器：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（15）
其中，，。
60.将式（15）带入（14），获取到：
ꢀꢀꢀꢀ
（16）其中，，，，。
61.如上，完成对轨迹跟踪控制器的设计，其考虑时滞问题和海洋环境干扰问题，用于实现uuv轨迹跟踪控制。
62.如下，将采用李雅普诺夫理论验证该控制器的稳定性。
63.此验证部分由该验证模块执行完成。
64.具体的验证过程及仿真结果参照图3至图5来说明。
65.首先，基于李雅普诺夫理论，分从时滞无关和时滞相关两个方面对uuv轨迹跟踪控制系统的稳定特性进行验证。
66.选取时滞无关的李雅普诺夫函数如下：
ꢀꢀꢀꢀꢀ
（17）选取时滞相关的李雅普诺夫函数如下：选取时滞相关的李雅普诺夫函数如下：选取时滞相关的李雅普诺夫函数如下：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（18）；其中，。
67.其次，根据式（17）和式（18），利用matlab软件设计仿真测试，以验证式（16）的稳定性及有效性。
68.在本技术中，通过uuv在水平面做回转运动验证所建立的uuv轨迹跟踪控制系统的准确性。
69.在进行仿真测试之前，首先，预先设计uuv已知模型参数，参见表1示出的。
70.实际uuv在海洋环境中，uuv的参数是不确定的，为了验证本发明uuv轨迹跟踪控制
系统，假设，如上所述的船体惯性矩阵m和线性水阻尼矩阵包括两部分：一部分是确定的、由表1中给出的参数m0和d0，另一部分为不确定的参数和。
71.表1
ꢀꢀ
uuv模型参数因此，如上所述的可以定义为，即，。
72.其次，对仿真参数进行设置，具体地，时滞时间常数h=5秒。
73.且由于本技术是对uuv位置进行控制，以实现轨迹跟踪的目的，因此，主要关心位置向量，设定uuv系统的（即，由预设轨迹输入模块提供的）期望运动轨迹为（即，纵荡位移），（即，横荡位移），（即，艏摇角度）。
74.控制参数k1=500，k2=5000，k3=100，海洋环境干扰，，。
75.干扰衰减水平设为和，初始纵荡位移、横荡位移及艏摇角度误差分别1m、0.5m和；初始纵荡速度、横荡速度和艏摇角速度误差分别为1.2m/s、1.6m/s和。
76.仿真结果分别参见图3至图6。
77.参见图3，uuv的期望运动轨迹为椭圆，uuv能够按照预定的轨迹进行航行，但在外部海洋环境干扰下，uvv起始运动轨迹与期望运动轨迹存在较大误差，在轨迹跟踪控制器30的作用下，uuv的实际运动轨迹逐渐收敛至期望运动轨迹，定位精度误差在0.05m以内，运动轨迹光滑平缓。
78.参见图4至图6，纵荡位移和横荡位移误差随时间逐渐收敛在0m附近，且艏摇角度误差也随时间逐渐收敛在0rad附近。
79.从图3至图6的仿真结果可以看出，uuv能在水平面上能够在轨迹跟踪控制器30的控制下能够逐渐趋于做回转运动，且各项位置信息（包括纵荡位移、横荡位移和艏摇角度）的误差也逐渐收敛至0附近，说明该控制器操纵性能良好，在考虑到时滞问题和环境干扰问
题的情况下，仍能够使uuv满足精准的轨迹跟踪，这也间接表明了本技术所设计的轨迹跟踪控制器30的合理性及有效性。
80.以上实施例仅用以说明本发明的技术方案，而非对其进行限制；尽管参照前述实施例对本发明进行了详细的说明，对于本领域的普通技术人员来说，依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或替换，并不使相应技术方案的本质脱离本发明所要求保护的技术方案的精神和范围。