基于速度与航向双重制导的无人船精确路径跟踪控制方法与流程

本发明属于无人船领域，尤其涉及一种无人船精确路径跟踪控制方法。

背景技术：

随着自动化理论及实践在海洋工程的广泛应用，欠驱动水面无人船作为一种高度自主的无人交通工具，可以灵活便捷地完成系列高风险海洋任务。其中，高精度的路径跟踪控制技术在发展欠驱动水面无人船自主性方面起着至关重要的作用。实际上，通过将制导与控制相结合，可以很好地解决路径跟踪问题。在制导子系统设计中，视线制导的有效性已得到理论和实验的广泛认可，比例视线制导通过设定一个与横向误差相关的反正切函数来计算期望航向角。积分视线制导在比例视线制导的基础上，通过一个积分项来抵消恒定或缓慢时变的侧滑角的干扰。自适应视线制导，通过自适应补偿方法来解决侧滑问题。相对速度视线制导，在视线制导的基础上考虑了洋流影响。在控制系统的设计中，反步法、奇异摄动法、模糊划分法、神经网络法以及各种自适应控制方法已被有效运用于船舶运动控制中，但是这些控制方法难以实现对外界复杂扰动的高效估计与补偿，从而降低了控制系统的精度。

在已有的制导方法中，通常要求无人船预置恒定或严格为正的速度，这样即使得无人船的速度不受制导控制，仅由方向舵控制，从而降低了操纵的整体性与灵活性。在控制子系统中，未能实现对外部扰动及内部参数摄动引起不确定的精准估计，因此无法从跟本上排除这些复杂不确定性对系统的影响，降低了无人船路径跟踪控制系统的精度。

本发明采用速度和航向双重制导方法，减轻方向舵的操纵负担，提高了制导系统的操纵灵活性与决策效率。利用有限时间不确定观测器对复杂的外界扰动与内部不确定进行精确观测，并在控制器设计时进行补偿，从而极大地增强了路径跟踪控制系统的工作效率。从而极大地提高了路径跟踪控制系统的跟踪精度。

技术实现要素：

为解决现有技术存在的上述问题，本发明要设计一种可以提高路径跟踪控制系统精度的基于速度与航向双重制导的无人船精确路径跟踪控制方法。

为了实现上述目的，本发明的技术方案如下：

基于速度与航向双重制导的无人船精确路径跟踪控制方法，所述的无人船的运动学模型如下：

式中，x代表惯性坐标系下无人船运动位置的横坐标，y代表惯性坐标系下无人船运动位置的纵坐标，ψ代表惯性坐标系下无人船的航向；为x的导数，是y的导数，是ψ的导数；u代表主体固定坐标系下无人船的前向速度，v代表主体固定坐标系下无人船的横向速度，r代表主体固定坐标系下无人船的航向角速度。

所述的无人船的动力学模型如下：

其中，τu为可利用的控制输入前向推力，τr为可利用的控制输入转向力矩，

式中，d11为无人船前向速度维上的水动力阻尼参数，d22为无人船横向速度维上的水动力阻尼参数，d33为无人船航向角速度维度上的水动力阻尼参数；m11为无人船前向速度维度上的质量参数，m22为无人船横向速度维度上的质量参数，m33为无人船航向角速度维度上的质量参数；τu为控制输入中的前向推力，τr为控制输入中的转向力矩；τδu为无人船在前向速度上受到的外界扰动，τδv为无人船在横向速度上受到的外界扰动，τδr为无人船在航向角速度上受到的外界扰动。

所述的控制方法包括以下步骤：

a、计算路径跟踪误差动态

在无人船参数化路径跟踪曲线上定义一个移动虚拟船舶，在惯性坐标系下，该船舶位置横坐标为xp、纵坐标为yp，xp、yp是关于一个时间变量的点，以该点为跟踪目标并建立路径正切坐标系，该路径正切坐标系相对于惯性坐标系的旋转角度为φp。无人船实际位置相对于移动虚拟船舶位置在路径正切坐标横坐标方向上的误差为xe、纵坐标方向上的误差为ye，则跟踪误差表达式为：

路径跟踪误差的动表达式态为：

us是路径上移动虚拟船舶的速度，表达成下列形式：

式中，为一个与时间相关的路径变量，

b、设计速度与航向双重制导律

根据路径跟踪误差动态表达式，设计如下速度与航向制导律，使路径跟踪误差能渐进稳定到零：

βd＝arctan(v/ud)

utar＝k2xe+udcos(ψ-φp+βd)

式中，为无人船理想航速，参数k1＞0为前向速度制导律中的常数值，k2＞0为航向制导律中的常数值，ud为无人船前向速度参考值，ψd为无人船航向角参考值，βd为理想侧滑角。利用所设计的制导律，将无人船实际运动轨迹与设计路径之间的误差渐进收敛为零。

定义第一个李雅普诺夫方程：

求取该李雅普诺夫方程的导数：

将设计的双重制导律代入到上述方程中得到：

其中，k＝2min{k1,k2}，则代入双重制导律的李雅普诺夫导数是负定的，满足渐进稳定性条件，因此制导系统的全局渐近稳定性得到了保证。因此，方程中的路径跟踪误差xe、ye渐进稳定到零，从而用所设计的双重制导律可以使速度与航向都得到制导。

c、设计有限时间未知观测器

将无人船的动力学模型整理成如下形式：

其中：m＝diag(m11,m22,m33)

f(ν)＝[fu,fv,fr]

τ＝[τu,0,τr]

fu＝m22vr-d11u

fv＝-m11ur-d22v

fr＝-(m22-m11)uv-d33r

有限时间未知观测器设计成如下形式：

速度维度上外界干扰的估计量，是航向角速度维度上外界干扰的估计量。

为了证明复杂未知量在该观测器下被精准确定，定义下列观测误差：

基于设计的有限时间未知观测器，对其观测误差进行微分：

根据levant引理，保证观测误差在有限时间稳定，即存在一个时间0＜tδ＜∞，以至于

d、基于有限时间未知观测器设计非光滑控制器

d1、设计基于有限时间未知观测器的非光滑速度控制器

基于有限时间未知观测器和非光滑控制理论，将非光滑速度控制器设计成如下形式：

式中，前向速度误差ue＝u-ud，参数ku＞0，0＜p1/q1＜1。

定义第二个李雅普诺夫方程：

求取该李雅普诺夫方程的导数，并将所设计的非光滑速度控制器与有限时间观测器代入方程得到：

由于求得的李雅普诺夫导数是负定的，根据全局有限时间稳定引理，前向速度误差ue可以在有限时间稳定到零，该有限时间为：

由此，得出速度可以在有限时间内精确跟踪的结论。

d2、设计基于有限时间未知观测器的非光滑航向控制器

基于有限时间未知观测器和非光滑控制理论，将非光滑航向控制器设计成如下形式：

ψe＝ψ-ψd代表航向角跟踪误差，re＝r-rd代表航向角速度跟踪误差。sψ是非奇异终端滑模表达式，sψ表示形式如下：

式中，σ1＞0，q2＜p2＜2q2，p3＜q3。

定义第三个李雅普诺夫方程：

代入所设计的速度控制器与有限时间观测器，对上述李雅普诺夫方程求导，并将所设计的非光滑航向控制器与有限时间观测器代入方程得到：

其中：

当re≠0时，李雅普诺夫导数是负定的，根据全局有限时间稳定引理，得到sψ在有限时间内稳定；当re＝0时，有根据levant引理，非奇异终端滑模sψ在有限时间能收敛到零，该有限时间表示为：

当到达滑动面时，航向角跟踪误差ψe是有限时间稳定到零的，该时间为：

使得ψe(t)≡0

由此，得出航向角在有限时间内精确跟踪的结论。

综上所述，所设计的基于有限时间未知观测器的非光滑速度与航向控制器，精确跟踪速度与航向角的制导信号。

与现有技术相比，本发明具有以下有益效果：

1.本发明提出了能根据跟踪误差同时制导速度和航向角的双重制导律，使无人船的位置误差能在有限时间内稳定到零，减轻了方向舵的操作负担，提高了制导系统的操纵灵活性与整体性。

2.本发明通过构造有限时间未知观测器，在复杂未知的环境下，精确估计未知扰动，并根据该估计量在设计的速度与航向控制器中实施有效补偿，从而快速稳定地使跟踪误差收敛到零，弥补了有界观测与渐进观测的局限性。

3.本发明在有限时间未知观测器的基础上设计了非光滑速度与航向控制器，使得控制系统在存在复杂干扰的情况下，精确跟踪制导信号。

4.综上所述，本发明设计速度与航向双重制导律来提高制导系统的操纵灵活性与整体性，有限时间未知观测器对复杂的外界扰动与内部不确定进行精确观测，并在非光滑速度与航向控制器中进行补偿，从而极大地提高了路径跟踪控制系统的跟踪精度。

附图说明

图1是无人船路径跟踪控制几何图。

图2是基于速度与航向双重制导的无人船有限时间路径跟踪控制系统框图。

图3是本发明的设计流程图。

图4是大连临海港实景模拟跟踪图。

图5是路径横向跟踪误差曲线图。

图6是路径纵向跟踪误差曲线图。

图7是路径航向跟踪误差曲线图。

图8是速度跟踪曲线图。

图9是速度跟踪误差曲线图。

图10是前向速度外界扰动观测曲线图。

图11是横向速度外界扰动观测曲线图。

图12是航向角速度外界扰动观测曲线图。

图13是前向速度外界扰动观测误差曲线图。

图14是横向速度外界扰动观测误差曲线图。

图15是航向角速度外界扰动观测误差曲线图。

图16是前向推力示意图。

图17是转向力矩示意图。

具体实施方式

下面结合附图对本发明进行进一步地描述。

图1所示为惯性坐标系和主体固定坐标系的关系以及步骤a中移动虚拟船舶和无人船位置示意图。

图2是路径跟踪控制系统框图，由欠驱动无人船动力学模型和虚拟船舶与无人船实际位置误差得到跟踪误差动力学方程，并以此方程为基础，设计速度与航向双重制导律。根据未知的不确定项，设计有限时间观测器，并设计相应的非光滑控制器，以控制欠驱动无人船达到精确的路径跟踪要求。

为了检验本发明所提出方法的有效性，按照图3所示的流程、利用cybershipi船舶在中国辽宁省大连市临海港进行了实景模拟路径跟踪，并且用该发明提出的制导控制方法(简称：shg-fpc)与不加有限时间干扰观测器的算法框架进行对比，进一步说明了观测器的观测效果。该船舶模型参数为：m11＝19kg，m22＝35.2kg，m33＝4.2kg，d11＝4kg/s，d22＝1kg/s，d33＝10kg/s。复杂的未知干扰假设如下：

λd＝1，δd＝[δdu,δdv,δdr]^t∈[-6,6]³为随机噪声，船舶的初始状态设定为：[x(0),y(0),ψ(0)]^t＝[345,20,0]^t,[u(0),v(0),r(0)]^t＝[0,0,0]^t，路径变量设定如下：

算法中的设定参数选择为：

k1＝0.4，k2＝1，δ＝3，l＝diag(300,300,300)，λ1＝0.4，λ2＝0.01，ku＝1.2，kψ＝1.2，σ1＝1，p1＝5，q1＝7，p2＝5，q2＝5，p3＝5，p3＝5，q3＝7。

图4为本发明的实景模拟训练图，对比无人船在本发明方案与不带有限时间不确定观测器(简称：shg-fpc/fuo)控制方案下实际和期望路径，由此可以看出，用本发明的方法可以同时实现更高的路径跟随精度和更强的抗干扰能力。图5展示了shg-fpc方案与shg-fpc/fuo方案下路径横向跟踪误差情况，从图中可以看出shg-fpc算法可以使横向误差更加有效收敛到零。图6展示了shg-fpc方案与shg-fpc/fuo方案下路径纵向跟踪误差情况，从图中可以看出shg-fpc算法可以使纵向误差收敛到零，而shg-fpc/fuo算法无法使纵向误差收敛到零且存在不稳定误差。图7展示了shg-fpc方案与shg-fpc/fuo方案下路径航向跟踪误差情况，从图中可以看出shg-fpc算法可以快速使航向跟踪误差收敛到零，而shg-fpc/fuo算法跟踪效果较差。图8展示了速度跟踪曲线图，从图中可以看出shg-fpc算法可以使无人船前向速度很快跟踪上参考速度。图9展示了速度跟踪误差曲线，从图中可以看出shg-fpc算法可以更稳定地使速度跟踪误差收敛为零。图10-12是前向、横向和航向角三个方向的外界扰动观测曲线，从图中可以看出，本发明提出的有限时间未知观测器可以实现精确观测到扰动量。图13-15是前向、横向和航向角三个方向的外界扰动误差观测曲线，从图中可以看出，通过观测器在短时间内使观测误差收敛到零点。图16是无人船前向推力示意图，作为前向速度控制输入，图17是无人船转动力矩示意图，作为航向控制输入。通过以上实施例，可以得到结论：本发明能精确跟踪制导系统产生的参考信号，具有显著的有效性和优越性。

本发明不局限于本实施例，任何在本发明披露的技术范围内的等同构思或者改变，均列为本发明的保护范围。