本发明涉及水下无人航行器(uuv，unmannedunderwatervehicle)控制领域，具体涉及一种具有输出约束的uuv轨迹跟踪控制方法和系统。
背景技术：
uuv由于体积小、可控性强等优势，故而成为一种广泛应用于军事领域、海洋开发以及科学研究等领域的航行器。运动控制是保证uuv在复杂、未知海域成功完成军事、科学勘测等任务的核心技术。
目前对于uuv轨迹跟踪问题的控制方法基本都未考虑跟踪误差的界限，但是尤其在狭窄等复杂海域进行轨迹跟踪控制时，就会对uuv的跟踪误差要求变得很苛刻，如果跟踪误差过大，极有可能发生安全事故。因此急需一种具有输出约束的轨迹跟踪控制方法。由于预设性能控制方法在动态性能及控制精度方面明显优于传统控制方法，因此本发明采用预设性能控制方法进行控制系统的设计。
技术实现要素：
有鉴于此，本发明提供了一种具有输出约束的uuv轨迹跟踪控制方法和系统，通过对控制率的设计限制了uuv轨迹跟踪误差，确保其在狭窄等复杂海域也可以安全航行。
为了解决上述技术问题，本发明是这样实现的：
一种具有输出约束的水下无人航行器轨迹跟踪控制方法，包括：
步骤1、获取水下无人航行器当前状态信息，包括纵向和横向的实际位置[x,y]、纵向和横向的实际速度[u,v]；
步骤2、给定平滑有界的期望轨迹[xd,yd]，计算期望轨迹[xd,yd]和实际位置[x,y]之间的位置误差[ex,ey]；
步骤3、对位置误差[ex,ey]进行等效误差转换，获得位置等效误差[εx,εy]；
步骤4、以实现等效误差收敛为原则设计虚拟控制率表达式，将位置等效误差[εx,εy]代入虚拟控制率表达式，获得期望速度[ud,vd]：
步骤5、计算期望速度[ud,vd]和实际速度[u,v]之间的速度误差[ue,ve]，对速度误差[ue,ve]进行等效误差转换，获得速度等效误差[εu,εv]；
步骤6、将位置等效误差[εx,εy]代入速度控制率表达式和回转控制律表达式，获得控制率[τu,τr]；
步骤7、将控制率[τu,τr]施加到水下无人航行器上。
优选地，步骤3对位置误差[ex,ey]进行等效误差转换，获得位置等效误差[εx,εy]的公式为：
其中，
步骤5对速度误差[ue,ve]进行等效误差转换，获得速度等效误差[εu,εv]的公式为：
其中，连续函数ρu、ρv为定义的正且严格递减的连续函数；δu、δv为取值范围为[0,1]的设定值；
优选地，步骤4的虚拟控制率表达式为：
其中，[ud,vd]为期望纵向速度和期望横向速度；
优选地，步骤6采用的速度控制率表达式和回转控制律表达式分别为：
其中，τu，τr分别为纵向力和回转力矩，su、sv分别为纵向和横向的滑模渐进稳态平面，且
本发明还提供了一种具有输出约束的水下无人航行器轨迹跟踪控制系统，包括位置传感模块、位置误差计算模块、第一性能误差转换模块、期望速度计算模块、速度误差计算模块、第二性能误差转换模块和轨迹跟踪控制模块；
位置传感模块，用于获取水下无人航行器当前状态信息，包括纵向和横向的实际位置[x,y]、纵向和横向的实际速度[u,v]；将获取的纵向和横向的实际位置[x,y]发送给位置误差计算模块，将获取的纵向和横向的实际速度[u,v]发送给速度误差计算模块；
位置误差计算模块，用于根据给定的平滑有界的期望轨迹[xd,yd]，计算期望轨迹[xd,yd]和实际位置[x,y]之间的位置误差[ex,ey]，发送给第一性能误差转换模块；
第一性能误差转换模块，用于采用式(1)对位置误差[ex,ey]进行等效误差转换，获得位置等效误差[εx,εy]，发送给期望速度计算模块；
其中，
期望速度计算模块，用于将位置等效误差[εx,εy]代入为实现等效误差而设计的虚拟控制率表达式(2)，获得期望速度[ud,vd]，发送给速度误差计算模块；
其中，[ud,vd]为期望纵向速度和期望横向速度；
速度误差计算模块，用于计算期望速度[ud,vd]和实际速度[u,v]之间的速度误差[ue,ve]，发送给第二性能误差转换模块；
第二性能误差转换模块，用于采用式(3)对速度误差[ue,ve]进行等效误差转换，获得速度等效误差[εu,εv]，发给轨迹跟踪控制模块；
其中，连续函数ρu、ρv为定义的正且严格递减的连续函数；δu、δv为取值范围为[0,1]的设定值；
轨迹跟踪控制模块，用于将位置等效误差[εx,εy]代入速度控制率表达式(4)和回转控制律表达式(5)，获得控制率[τu,τr]，将控制率[τu,τr]施加到水下无人航行器上；
其中，τu，τr分别为纵向力和回转力矩，su、sv分别为纵向和横向的滑模渐进稳态平面，且
有益效果：
本发明为一种具有输出约束的uuv轨迹跟踪控制方法，通过预设性能控制基本原理，将位置跟踪误差转换为速度跟踪误差，能够成功地控制uuv跟踪上期望轨迹，且跟踪误差始终在预先设定的界限内，可以极大提高航行安全性。本发明考虑环境干扰、模型参数不确定性及测量误差的干扰，采用滑模进行逼近uuv运动学和动力学，能够有效补偿干扰，提高控制精度。
附图说明
图1为本发明具有输出约束的uuv轨迹跟踪控制方法流程图。
图2为本发明具有输出约束的uuv轨迹跟踪控制系统的原理图。
具体实施方式
下面结合附图并举实施例，对本发明进行详细描述。
本发明提供了一种具有输出约束的uuv轨迹跟踪控制方法，以位置跟踪误差控制为出发点，通过预设性能控制基本原理，将位置跟踪误差转换为速度跟踪误差，能够成功地控制uuv跟踪上期望轨迹，且跟踪误差始终在预先设定的界限内，可以极大提高航行安全性。本发明考虑环境干扰、模型参数不确定性及测量误差的干扰，采用滑模进行逼近uuv运动学和动力学，能够有效补偿干扰，提高控制精度。
下面结合图1和图2对本发明做详细说明。
本发明为一种具有输出约束的uuv轨迹跟踪控制方法的推导过程，包括以下步骤：
步骤1：通过导航设备、传感器及采集uuv当前状态信息，包括纵向位置x、横向位置y、纵向速度u、横向速度v、艏向角ψ、回转角速度r。
欠驱动uuv三自由度运动学与动力学数学模型为：
运动学模型：
动力学模型：
其中，x为北向位置(即纵向位置)，y为东向位置(即横向位置)，ψ为艏向角，u为纵向速度，v为横向速度，r为回转角速度，
步骤2：给定平滑有界期望轨迹[xd,yd]。
本发明的控制目标是对于欠驱动uuv，设计一种跟踪控制律，使得跟踪误差是一致最终有界且在预设边界内。
步骤3：利用步骤1和步骤2的信息建立轨迹跟踪位置误差和速度误差。
定义纵向位置跟踪误差ex和横向位置跟踪误差ey为：
定义纵向速度跟踪误差ue和横向速度跟踪误差ve为：
式中，ud为期望纵向速度，vd为期望横向速度。
步骤4：利用预设性能函数进行等效误差转换；
预设性能函数的误差进行等效误差转换公式为：
其中，
性能函数定义为：
连续函数ρ：r →r 为性能函数，且满足：ρ是正且严格递减；
在初始误差e(0)已知的前提下，可以通过如下不等式来实现跟踪误差e(t)满足预先设定的范围之内]：
-δρ(t)<e(t)<ρ(t),e(0)>0
-ρ(t)<e(t)<δρ(t),e(0)<0
其中，t∈[0,∞)，δ∈[0,1]。
本文选取如下函数作为预设性能函数:
ρ(t)＝(ρ0-ρ∞)e-lt ρ∞
其中，ρ0、ρ∞、l>0为预先设定的正常数，ρ0和ρ∞分别表示性能函数的初值和稳态值，l为性能函数的衰减速度，因此可以通过选择合适的参数便可对输出误差进行限制。
那么，对位置误差模型进行等效转换可得：
其中，
则，有
其中，
步骤5：为实现等效误差收敛，设计虚拟控制率；
其中，kx、ky为常数，kx，ky>0。ξx、ξy为定义变量，其中，
当ue＝u-ud，ve＝v-vd趋近于0时，式(3-20)为
对虚拟控制率ud，vd求导可得：
步骤6：构造李雅普诺夫函数证明速度跟踪误差渐进收敛等效位置跟踪误差渐进收敛；
选取如下李雅普诺夫函数：
将上式(3-24)对时间微分(3-22)可以得到：
由于kx,ky>0，显然，对任意(εx,εy)≠(0,0)，都有
步骤7：设计速度控制律和回转控制律。
假设：初始位置跟踪误差满足
(1)速度控制律
纵向轨迹跟踪误差ue为：
ue＝u-ud
对上式进行等效误差转换可得：
上式两边进行微分可得：
其中，
取纵向滑模渐进稳态平面为su为
ku为设定常数。
上式两边进行微分可得：
可以取趋近律为等速趋近律：
其中，ηu为趋近参数。
令
(2)回转控制律
选取横向轨迹跟踪误差为
ve＝v-vd
对上式进行等效误差转换可得：
上式两边进行一次和二次微分可得：
其中，
取滑模渐进稳态平面为sv：
其中，kv为设定常数。
上式两边进行微分可得：
其中，ηv为趋近参数。
令
步骤8、稳定性证明：
选取李雅普诺夫函数vuv为：
则
由于ηu>0,ηv>0，因此对于su、sv不同时为0时，恒有
由此可见，系统渐进稳定，该控制律设计合理。
基于上述方案，本发明所提供的具有输出约束的水下无人航行器轨迹跟踪控制系统，如图2所示，包括位置传感模块、位置误差计算模块、第一性能误差转换模块、期望速度计算模块、速度误差计算模块、第二性能误差转换模块和轨迹跟踪控制模块；
位置传感模块，用于获取水下无人航行器当前状态信息，包括纵向和横向的实际位置[x,y]、纵向和横向的实际速度[u,v]；将获取的纵向和横向的实际位置[x,y]发送给位置误差计算模块，将获取的纵向和横向的实际速度[u,v]发送给速度误差计算模块；
位置误差计算模块，用于根据给定的平滑有界的期望轨迹[xd,yd]，计算期望轨迹[xd,yd]和实际位置[x,y]之间的位置误差[ex,ey]，发送给第一性能误差转换模块；
第一性能误差转换模块，用于采用式(1)对位置误差[ex,ey]进行等效误差转换，获得位置等效误差[εx,εy]，发送给期望速度计算模块；
其中，
期望速度计算模块，用于将位置等效误差[εx,εy]代入为实现等效误差而设计的虚拟控制率表达式(2)，获得期望速度[ud,vd]，发送给速度误差计算模块；
其中，[ud,vd]为期望纵向速度和期望横向速度；
速度误差计算模块，用于计算期望速度[ud,vd]和实际速度[u,v]之间的速度误差[ue,ve]，发送给第二性能误差转换模块；
第二性能误差转换模块，用于采用式(3)对速度误差[ue,ve]进行等效误差转换，获得速度等效误差[εu,εv]，发给轨迹跟踪控制模块；
其中，连续函数ρu、ρv为定义的正且严格递减的连续函数；δu、δv为取值范围为[0,1]的设定值；
轨迹跟踪控制模块，用于将位置等效误差[εx,εy]代入速度控制率表达式(4)和回转控制律表达式(5)，获得控制率[τu,τr]，将控制率[τu,τr]施加到水下无人航行器上；
其中，τu，τr分别为纵向力和回转力矩，su、sv分别为纵向和横向的滑模渐进稳态平面，且
以上的具体实施例仅描述了本发明的设计原理，该描述中的部件形状，名称可以不同，不受限制。所以，本发明领域的技术人员可以对前述实施例记载的技术方案进行修改或等同替换；而这些修改和替换未脱离本发明创造宗旨和技术方案，均应属于本发明的保护范围。
1.一种具有输出约束的水下无人航行器轨迹跟踪控制方法，其特征在于，包括：
步骤1、获取水下无人航行器当前状态信息，包括纵向和横向的实际位置[x,y]、纵向和横向的实际速度[u,v]；
步骤2、给定平滑有界的期望轨迹[xd,yd]，计算期望轨迹[xd,yd]和实际位置[x,y]之间的位置误差[ex,ey]；
步骤3、对位置误差[ex,ey]进行等效误差转换，获得位置等效误差[εx,εy]；
步骤4、以实现等效误差收敛为原则设计虚拟控制率表达式，将位置等效误差[εx,εy]代入虚拟控制率表达式，获得期望速度[ud,vd]：
步骤5、计算期望速度[ud,vd]和实际速度[u,v]之间的速度误差[ue,ve]，对速度误差[ue,ve]进行等效误差转换，获得速度等效误差[εu,εv]；
步骤6、将位置等效误差[εx,εy]代入速度控制率表达式和回转控制律表达式，获得控制率[τu,τr]；
步骤7、将控制率[τu,τr]施加到水下无人航行器上。
2.如权利要求1所述的方法，其特征在于，步骤3对位置误差[ex,ey]进行等效误差转换，获得位置等效误差[εx,εy]的公式为：
其中，
步骤5对速度误差[ue,ve]进行等效误差转换，获得速度等效误差[εu,εv]的公式为：
其中，连续函数ρu、ρv为定义的正且严格递减的连续函数；δu、δv为取值范围为[0,1]的设定值。
3.如权利要求2所述的方法，其特征在于，步骤4的虚拟控制率表达式为：
其中，[ud,vd]为期望纵向速度和期望横向速度；
4.如权利要求3所述的方法，其特征在于，步骤6采用的速度控制率表达式和回转控制律表达式分别为：
其中，τu，τr分别为纵向力和回转力矩，su、sv分别为纵向和横向的滑模渐进稳态平面，且
5.一种具有输出约束的水下无人航行器轨迹跟踪控制系统，其特征在于，包括位置传感模块、位置误差计算模块、第一性能误差转换模块、期望速度计算模块、速度误差计算模块、第二性能误差转换模块和轨迹跟踪控制模块；
位置传感模块，用于获取水下无人航行器当前状态信息，包括纵向和横向的实际位置[x,y]、纵向和横向的实际速度[u,v]；将获取的纵向和横向的实际位置[x,y]发送给位置误差计算模块，将获取的纵向和横向的实际速度[u,v]发送给速度误差计算模块；
位置误差计算模块，用于根据给定的平滑有界的期望轨迹[xd,yd]，计算期望轨迹[xd,yd]和实际位置[x,y]之间的位置误差[ex,ey]，发送给第一性能误差转换模块；
第一性能误差转换模块，用于采用式(1)对位置误差[ex,ey]进行等效误差转换，获得位置等效误差[εx,εy]，发送给期望速度计算模块；
其中，
期望速度计算模块，用于将位置等效误差[εx,εy]代入为实现等效误差而设计的虚拟控制率表达式(2)，获得期望速度[ud,vd]，发送给速度误差计算模块；
其中，[ud,vd]为期望纵向速度和期望横向速度；
速度误差计算模块，用于计算期望速度[ud,vd]和实际速度[u,v]之间的速度误差[ue,ve]，发送给第二性能误差转换模块；
第二性能误差转换模块，用于采用式(3)对速度误差[ue,ve]进行等效误差转换，获得速度等效误差[εu,εv]，发给轨迹跟踪控制模块；
其中，连续函数ρu、ρv为定义的正且严格递减的连续函数；δu、δv为取值范围为[0,1]的设定值；
轨迹跟踪控制模块，用于将位置等效误差[εx,εy]代入速度控制率表达式(4)和回转控制律表达式(5)，获得控制率[τu,τr]，将控制率[τu,τr]施加到水下无人航行器上；
其中，τu，τr分别为纵向力和回转力矩，su、sv分别为纵向和横向的滑模渐进稳态平面，且
