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

Abstract

本发明公开了一种基于速度与航向双重制导的无人船精确路径跟踪控制方法，包括以下步骤：建立无人船运动学和动力学模型；构造路径跟踪误差动态；设计速度与航向双重制导律；设计有限时间干扰观测器。本发明提出的速度与航向双重制导律可以根据路径跟踪误差同时制导速度和航向角，提高了路径跟踪控制系统的稳定性和灵活性。本发明构造的有限时间扰动观测器能精确观测复杂的外界干扰，并且在有限时间内确保观测误差为零，可以避免有界观测和渐近观测的局限性。本发明设计了结合有限时间观测器设计和反步控制技术的速度和航向跟踪控制器，使得在存在复杂干扰的情况下可以精确跟踪制导信号，从而实现精确的无人船路径跟踪控制。

Description

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

技术领域

本发明属于无人船控制技术，尤其涉及一种基于速度与航向双重制导的无人船精确路径跟踪控制方法。

背景技术

在海洋军事和商业蓬勃发展的驱动下，无人船凭借其高度的自主性、自动性得到了高度的关注。无人船可以在无人参与的情况下自主执行各种高风险的海事任务，路径跟踪控制技术在各种海事任务中起着基础性作用。通常无人船路径跟踪控制系统可分为制导子系统和控制子系统。在制导子系统设计中，主要是采用视线制导方法，比例视线制导最先被提出，通过横向误差与一个设定参数的反正切值计算期望舵角。积分视线制导在比例视线制导的基础上，通过一个积分项补偿侧滑角的影响。自适应视线制导，通过自适应方法估计未知的侧滑角。在控制系统的设计中，反步控制、滑模控制、模糊及神经网络控制方法，被广泛采用于无人船运动控制中，但是都无法做到对外界扰动和内部未知动态进行精确快速的估计和补偿，导致控制系统的控制精度大大降低。

在已有的制导方法中，无人船的速度通常预定义为常量。在这种情况下，无人船实际上仅由方向舵控制，从而不仅降低了总体操纵性与灵活性，而且增加了方向舵的操纵负担。在控制子系统中，快速精确的扰动估计和补偿没有实现，极大地降低了路径跟踪控制系统的精度。

发明内容

为解决现有技术存在的上述问题，本发明要设计一种不仅能提高总体操纵性与灵活性，而且能减轻方向舵的操纵负担，并能极大地提高路径跟踪控制系统精度的基于速度与航向双重制导的无人船精确路径跟踪控制方法。

为了实现上述目的，本发明的技术方案如下：基于速度与航向双重制导的无人船精确路径跟踪控制方法，利用路径跟踪控制系统进行跟踪控制，所述的路径跟踪控制系统包括制导子系统和控制子系统；所述的制导子系统是一个决策系统，结合无人船与参考路径之间的误差产生速度和航向的参考指令，以此参考指令动作，跟踪误差将渐近收敛到零；所述的控制子系统是一个执行系统，使无人船实际姿态和参考指令达到一致，通过设计基于有限时间观测器的鲁棒控制器，实现精确的扰动观测以及精确跟踪控制；

所述的路径跟踪控制方法，包括以下步骤：

A、建立无人船运动学和动力学模型

基于无人船运动学模型导出路径跟踪误差系统，进而设计制导律稳定制导子系统；基于无人船动力学模型，设计观测器及控制器稳定控制子系统，进而实现整个路径跟踪控制系统的稳定，实现精确的路径跟踪控制；

无人船运动学模型如下：

式中，x、y分别是惯性坐标系下无人船的横坐标、纵坐标，ψ是无人船在惯性坐标系下的航向，u是浮体坐标系下无人船的前向速度，v是浮体坐标系下无人船的横向速度、r是浮体坐标系下无人船的航向角速度；

无人船动力学模型如下：

式中：d₁₁是无人船在前向速度维度上的阻尼参数，d₂₂是无人船在横向速度维度上的阻尼参数，d₃₃是无人船在航向角速度维度上的阻尼参数，m₁₁是无人船在前向速度维度上的质量参数，m₂₂是无人船在横向速度维度上的质量参数，m₃₃是无人船在航向角速度维度上的质量参数，τ_u和τ_r分别为可利用的控制输入前向推力和转向力矩，是无人船在前向速度上的外界干扰，是无人船在横向速度上的外界干扰，是无人船在航向角速度速度上的外界干扰；

B、构造路径跟踪误差动态

在无人船跟踪参数路径上选取一个移动虚拟点，以这个移动虚拟点为原点，其在惯性坐标系下的横纵坐标分别为x_p和y_p，建立一个相对于惯性坐标系旋转角度φ_p的坐标系，称之为路径正切参考坐标系，无人船相对于移动虚拟点在路径参考坐标系横坐标方向上的位置误差为x_e，在纵坐标方向上的位置误差为y_e，x_e和y_e的计算公式如下：

则路径跟踪误差的动态为：

式中，是无人船在路径正切参考坐标系下横坐标方向上的路径跟踪误差的导数，是无人船在路径正切参考坐标系下纵坐标方向上的路径跟踪误差的导数，是侧滑角，u_tar是路径上移动虚拟点的速度，表达成下列形式：

式中：是路径参数，是路径参数的导数，是x_p相对于的偏导数，是y_p相对于的偏导数。

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

基于推导出的路径跟踪误差动态(4)，设计制导律如下，使路径跟踪误差渐近稳定到零，制导子系统达到渐近稳定：

其中，u_d是无人船前向速度u的参考值，ψ_d是航向ψ的参考值，β_d是侧滑角β的理想信号，k₁＞0是前向速度制导律中的设定常数，Δ>0是前视距离，k₂＞0是航向制导律中的设定常数，是理想航速值。

运用以上设计的制导律，路径跟踪误差将渐近收敛到零点，无人船速度和航向得到双重制导，制导子系统的决策水平和灵活性都得到了极大的提高。

为了证明制导子系统在设计的制导律的作用下的稳定性，定义第一个李亚诺夫方程如下：

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

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

由于设计的李亚诺夫方程的导数是负定的，满足渐近稳定特性的充分条件，因此制导子系统的渐近稳定性得到了证明和保证。

D、设计有限时间干扰观测器

为了方便有限时间干扰观测器的设计，于是将前述无人船的动力学模型进一步整理成矩阵形式：

v＝[u,v,r]^T(10)

τ＝[τ_u 0 τ_r]^T (12)

f(v)＝[f_u,f_v,f_r]^T (13)

式中，v是无人船速度矩阵，是无人船速度导数矩阵，M是无人船质量矩阵，τ是无人船控制输入矩阵，其中：

有限时间干扰观测器被设计成如下形式：

是速度导数矩阵的估计值，是速度矩阵的估计值，ζ是在有限时间干扰观测器设计过程中定义的辅助变量，L是有限时间干扰观测器增益矩阵，λ₁是有限时间干扰观测器调节参数，λ₂是有限时间干扰观测器另一个调节参数，是外界干扰的估计值。

为了证明该观测器的观测效果，定义下列观测误差：

基于设计的有限时间干扰观测器，对其进行微分

根据Levant引理，证明观测误差在有限时间内能够到达零，存在一个时间0＜T_obs＜∞，以至于

E、基于有限时间观测器的控制器设计

k_u是速度控制器中的前向速度设定参数，要求是一个正值，k_r是航向控制器中的航向角速度设定参数，要求是一个正值，是前向速度制导律的导数，是前向速度维度上的干扰的估计值，是航向角速度方向上的干扰的估计值，r_d是虚拟控制律：

是虚拟控制律的导数。

定义如下李亚诺夫方程：

u_e＝u-u_d (21)

ψ_e＝ψ-ψ_d (22)

r_e＝r-r_d (23)

u_e是前向速度追踪误差，ψ_e是航向追踪误差，r_e＝r-r_d是定义的虚拟控制律的追踪误差。

对上述李亚诺夫方程求导，并代入上述设计的前向速度和航向控制器(18)和有限时间干扰观测器(15)，得到：

其中，控制子系统的渐近稳定性得到了证明。

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

1、本发明提出的速度与航向双重制导律可以根据路径跟踪误差同时制导速度和航向角，提高了路径跟踪控制系统的稳定性和灵活性。

2、本发明构造的有限时间扰动观测器能精确观测复杂的外界干扰，并且在有限时间内确保观测误差为零，可以避免有界观测和渐近观测的局限性。

3、本发明设计了结合有限时间观测器设计和反步控制技术的速度和航向跟踪控制器，使得在存在复杂干扰的情况下可以精确跟踪制导信号，从而实现精确的无人船路径跟踪控制。

4、本发明通过设计制导律和控制器分别使制导子系统和控制子系统达到渐近稳定，通过李亚诺夫方程证明所提出的算法可以使整个路径跟踪控制系统具有全局渐近稳定特性。

附图说明

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

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

图3是参考路径与实际路径示意图。

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

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

图6是速度跟踪曲线图。

图7是航向跟踪曲线图。

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

图9是航向跟踪误差曲线图。

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

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

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

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

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

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

图16是前向推力示意图

图17是转向力矩示意图。

具体实施方式

为了说明方法的有效性，在CybershipI无人船模型上进行仿真，并且用本发明提出的控制方法(简称：FDO-SVLOS)与没有有限时间干扰观测器的算法(简称：SVLOS)进行对比，以进一步说明有限时间干扰观测器的观测效果。该无人船模型参数为：

m₁₁＝19kg,m₂₂＝35.2kg,m₃₃＝4.2kg,d₁₁＝4kg/s,d₂₂＝1kg/s,d₃₃＝10kg/s。

复杂的未知干扰假设如下：

τ_δ＝[sin(t+0.3π),cos(t+0.1π),sin(t+0.2π)]^T

无人船的初始状态设定为：

[x,y,ψ]＝[10,0,0],[u,v,r]＝[0,0,0]，

参考路径的设定如下：

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

k₁＝1,k₂＝1,Δ＝1.2,L＝diag(400,400,400),λ₁＝0.2,λ₂＝0.01,k_u＝1.2,k_ψ＝1.2,k_r＝1.

图1为无人船路径跟踪控制几何图，无人船精确路径跟踪控制就是使无人船精确地跟踪一条设定好的几何路径，同时图中也表达了三个坐标系的几何位置关系，即惯性坐标系、浮体坐标系以及路径正切参考坐标系的位置关系。图2为本发明提出的基于速度与航向双重制导的无人船精确路径跟踪控制系统框图。图3-17为所提出算法的仿真验证图，无人船实际和期望的路径如图3所示，从中可以看出，本发明所提出的FDO-SVLOS算法可以同时实现更高的路径跟随精度和更强的抗干扰能力，而SVLOS算法无法实现精确的路径跟踪控制，无人船的实际路径无法精确的收敛到参考路径。图4展现的是路径跟踪横向误差，从图中可以看出FDO-SVLOS算法可以使路径跟踪横向误差收敛到零，而SVLOS算法则无法使路径跟踪横向误差收敛到零。图5展现的是路径跟踪纵向误差，从图中可以看出FDO-SVLOS算法可以使路径跟踪纵向误差收敛到零，而SVLOS算法则无法使路径跟踪纵向误差收敛到零却存在较大误差。图6展现的是速度跟踪曲线图，从图中可以看出FDO-SVLOS算法可以使前向速度精确地追踪上参考速度，而SVLOS算法则无法实现精确的速度追踪。图7展现的是航向跟踪曲线图，从图中可以看出FDO-SVLOS算法可以使航向精确地跟踪上参考航向，而SVLOS算法则无法实现精确的航向追踪。图8展现的是速度跟踪误差，从图中可以看出FDO-SVLOS算法可以使速度跟踪误差收敛到零，而SVLOS算法则无法使速度跟踪误差收敛到零。图9展现的是航向跟踪误差，从图中可以看出FDO-SVLOS算法可以使航向跟踪误差收敛到零，而SVLOS算法则无法使航向跟踪误差收敛到零却存在较大误差。图10-12是三个方向上的外界干扰观测，从图中可以看出，本发明提出的扰动观测器可以实现精确的扰动观测。图13-15为扰动观测误差曲线图，从图中可以看出观测误差可以在短时间内收敛到零，实现了精确的外界扰动观测。图16为前向推力曲线图，控制前向速度的大小。图17为转向力矩曲线图，控制无人船航向。可以得出这样的结论：验证了所提出的算法方案显著的有效性和优越性。

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

Claims (1)

1.基于速度与航向双重制导的无人船精确路径跟踪控制方法，其特征在于：利用路径跟踪控制系统进行跟踪控制，所述的路径跟踪控制系统包括制导子系统和控制子系统；所述的制导子系统是一个决策系统，结合无人船与参考路径之间的误差产生速度和航向的参考指令，以此参考指令动作，跟踪误差将渐近收敛到零；所述的控制子系统是一个执行系统，使无人船实际姿态和参考指令达到一致，通过设计基于有限时间观测器的鲁棒控制器，实现精确的扰动观测以及精确跟踪控制；

所述的路径跟踪控制方法，包括以下步骤：

A、建立无人船运动学和动力学模型

基于无人船运动学模型导出路径跟踪误差系统，进而设计制导律稳定制导子系统；基于无人船动力学模型，设计观测器及控制器稳定控制子系统，进而实现整个路径跟踪控制系统的稳定，实现精确的路径跟踪控制；

无人船运动学模型如下：

式中，x、y分别是惯性坐标系下无人船的横坐标、纵坐标，ψ是无人船在惯性坐标系下的航向，u是浮体坐标系下无人船的前向速度，v是浮体坐标系下无人船的横向速度、r是浮体坐标系下无人船的航向角速度；

无人船动力学模型如下：

式中：d₁₁是无人船在前向速度维度上的阻尼参数，d₂₂是无人船在横向速度维度上的阻尼参数，d₃₃是无人船在航向角速度维度上的阻尼参数，m₁₁是无人船在前向速度维度上的质量参数，m₂₂是无人船在横向速度维度上的质量参数，m₃₃是无人船在航向角速度维度上的质量参数，τ_u和τ_r分别为可利用的控制输入前向推力和转向力矩，是无人船在前向速度上的外界干扰，是无人船在横向速度上的外界干扰，是无人船在航向角速度速度上的外界干扰；

B、构造路径跟踪误差动态

在无人船跟踪参数路径上选取一个移动虚拟点，以这个移动虚拟点为原点，其在惯性坐标系下的横纵坐标分别为x_p和y_p，建立一个相对于惯性坐标系旋转角度φ_p的坐标系，称之为路径正切参考坐标系，无人船相对于移动虚拟点在路径参考坐标系横坐标方向上的位置误差为x_e，在纵坐标方向上的位置误差为y_e，x_e和y_e的计算公式如下：

则路径跟踪误差的动态为：

式中，是无人船在路径正切参考坐标系下横坐标方向上的路径跟踪误差的导数，是无人船在路径正切参考坐标系下纵坐标方向上的路径跟踪误差的导数，是侧滑角，u_tar是路径上移动虚拟点的速度，表达成下列形式：

式中：是路径参数，是路径参数的导数，是x_p相对于的偏导数，是y_p相对于的偏导数。

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

基于推导出的路径跟踪误差动态(4)，设计制导律如下，使路径跟踪误差渐近稳定到零，制导子系统达到渐近稳定：

其中，u_d是无人船前向速度u的参考值，ψ_d是航向ψ的参考值，β_d是侧滑角β的理想信号，k₁＞0是前向速度制导律中的设定常数，Δ>0是前视距离，k₂＞0是航向制导律中的设定常数，是理想航速值。

D、设计有限时间干扰观测器

为了方便有限时间干扰观测器的设计，于是将前述无人船的动力学模型进一步整理成矩阵形式：

v＝[u,v,r]^T (7)

τ＝[τ_u 0 τ_r]^T (9)

f(v)＝[f_u,f_v,f_r]^T (10)

式中，v是无人船速度矩阵，是无人船速度导数矩阵，M是无人船质量矩阵，τ是无人船控制输入矩阵，其中：

有限时间干扰观测器被设计成如下形式：

是速度导数矩阵的估计值，是速度矩阵的估计值，ζ是在有限时间干扰观测器设计过程中定义的辅助变量，L是有限时间干扰观测器增益矩阵，λ₁是有限时间干扰观测器调节参数，λ₂是有限时间干扰观测器另一个调节参数，是外界干扰的估计值。

E、基于有限时间观测器的控制器设计

k_u是速度控制器中的前向速度设定参数，要求是一个正值，k_r是航向控制器中的航向角速度设定参数，要求是一个正值，是前向速度制导律的导数，是前向速度维度上的干扰的估计值，是航向角速度方向上的干扰的估计值，r_d是虚拟控制律：

是虚拟控制律的导数。