CN113296505B - 一种基于速变los的无人船多模式路径跟踪控制方法 - Google Patents

Abstract

本发明公开了一种基于速变LOS的无人船多模式路径跟踪控制方法。所述包括以下步骤：建立惯性坐标系下固定双桨推进无人船的三自由度运动学和动力学模型；构建无人船多模式路径跟踪控制系统；实时对无人船多模式路径跟踪控制系统中的多模式决策模块下达指令，实现无人船多模式路径跟踪控制。本发明采用的路径拓展模块克服了路径信息有限的不良条件，并可以平滑路径信息；本发明采用的严格路径选择模块能够应对严格跟踪任务需求，在跟踪过程中严格跟踪初始路径点；本发明采用的纵向速度制导律有效利用了路径信息并抑制了无人船在路径切向角突变处的过冲现象。

Description

一种基于速变LOS的无人船多模式路径跟踪控制方法

技术领域

本发明涉及无人船航行控制领域，具体为一种基于速变LOS的无人船多模式路径跟踪控制方法。

背景技术

目前，无人船在海洋测绘、水质采样、环境监测、水文探测、海上搜救等诸多领域发挥了越来越大的作用，其具有广阔的发展空间与应用前景，涵盖的技术领域非常广泛。具备自动化功能的无人船为相关团队的实验提供了便利，但在执行任务过程中，作为实验平台的无人船有着更多功能和模式的需求。很多现有的无人船跟踪方法通常只能一直正向跟踪路径，对路径参数更新具有严格的要求，不能在跟踪过程中实时干预并调整已经设定好的跟踪方向和路径信息来实现反向跟踪和改变路径形状。

中国专利CN 11506086 A公开了一种改进LOS引导律结合模糊PID的无人船路径跟踪控制方法，该方法通过增加变速控制律到LOS引导律以及采用模糊PID设计控制器，在一定程度上降低了航速对无人船路径跟踪控制的影响。该方法采取了有限路径点来进行路径的跟踪，通过判断与下一个点的距离来跟新跟踪的路径点，这种方法没有考虑到切换路径点数目过少时切换路径点会给控制效果带来不好的影响，并且没有严格跟踪设置好的路径，即不能有效的跟踪第一个路径点。

在传统的LOS路径跟踪方法中，由于设定期望纵向速度为定值且没有充分利用到路径信息。这使得在无人船经过路径切向角突变的路径点时，原有的纵向速度太大导致控制算法不能及时调整姿态来很好的跟踪路径，在路径切向角突变的路径点附近产生了过冲现象，导致跟踪效果不良。

发明内容

为了解决现有技术的上述问题，本发明目的在于提供一种基于速变LOS的无人船多模式路径跟踪控制方法，能够在跟踪过程中实时干预并调整跟踪模式、应对严格跟踪需求和克服路径信息有限并且在路径切向角突变点处有效减轻过冲现象的路径跟踪控制方法。

本发明的目的至少通过如下技术方案之一实现。

一种基于速变LOS的无人船多模式路径跟踪控制方法，包括以下步骤：

S1、建立惯性坐标系下固定双桨推进无人船的三自由度运动学和动力学模型；

S2、构建无人船多模式路径跟踪控制系统；

S3、实时对无人船多模式路径跟踪控制系统中的多模式决策模块下达指令，实现无人船多模式路径跟踪控制。

进一步地，步骤S1中，运动学与动力学模型的建立过程如下：

建立惯性坐标系下固定双桨推进无人船的三自由度运动学和动力学模型，其中，O_ex_ey_e是建立在地球上的惯性坐标系，船体坐标系是以船体中心为原点，纵向指向船艏并平行水面，横向垂直于船艏并平行于水面，在地球上的惯性坐标系下无人船的运动学和动力学模型为：

其中，ψ是无人船当前的艏摇角，u,v,r分别表示船体坐标系中的纵向速度、横向速度和艏摇角速度；τ_u和τ_r分别表示无人船动力装置产生的纵向推力和横向力矩，τ₁,τ₂,τ₃分别表示无人船在纵向、横向、艏向上受到的干扰力，m_ii和d_ii分别为实验无人船的给定惯性系数和阻尼系数；由于不考虑外界干扰，τ₁,τ₂,τ₃设置为0；

为无人船当前的位置(x,y)的对应导数，

和

为对应的导数；

其中τ_u＝f₁+f₂，τ_r＝(f₁-f₂)·B/2；B表示无人船双桨的横向距离，f₁和f₂分别表示两个螺旋桨的推力；

由于固定双桨推进无人船以两个直流电机为推进系统，两个螺旋桨分别对应两个直流电机，螺旋桨推力和电调输出电压有如下线性关系：f＝kV；

结合上述方程最终得到实际的控制输入：左电机控制电压V_L和右电机控制电压V_R分别为：

V_L＝(2·τ_r/B+τ_u)/2k，V_R＝(-2·τ_r/B+τ_u)/2k；

则动力学方程进一步改写为：

进一步地，步骤S3中所述无人船多模式路径跟踪控制系统包括路径拓展模块、严格路径选择模块和多模式决策模块；

路径拓展模块根据拓展后的路径点数目和输入路径点数目计算得到插值步长，接收多模式决策模块传递的信号，对输入路径信息分别进行分段线性插值、球面插值得到拓展后的路径信息；

严格路径选择模块根据拓展后路径信息设置第一虚拟任务点，根据拓展后的路径信息和第一虚拟任务点通过路径参数更新律、艏摇角制导律和纵向速度制导律得到当前跟踪路径参数、路径切向角矩阵、期望艏摇角和纵向速度；

多模式决策模块根据第一虚拟任务点、期望艏摇角和纵向速度，通过第一PID控制器、第二PID控制器控制无人船的运动；

无人船在跟踪路径时，多模式决策模块实时接收指令使无人船多模式路径跟踪控制系统实现不同的功能。

进一步地，路径拓展模块中，给定拓展后的路径点数目N_e，路径信息输入点的个数N_n，设计插值步长：

根据多模式决策模块的信号，由插值步长step对输入的路径信息

进行分段线性插值或球面插值得到拓展后的路径信息为：

其中，(x_i,y_i)是path_in插值前的第i个路径点坐标，(x_θ,y_θ)为path_in经过插值后的第θ个路径点的坐标，θ表示一个与时间无关的路径参数，拓展后的路径信息为最终无人船的跟踪路径信息。

进一步地，严格路径选择模块中，第一虚拟任务点设置为：

其中(x_θ＝1,y_θ＝1)^Τ为拓展后的路径信息的第1个点，

为拓展后的路径信息的第k_vis个点，k_vis为严格路径选择尺度参数。

进一步地，路径参数更新律和艏摇角制导律具体如下：

给定一条期望路径的路径信息和第一虚拟任务点，通过位置误差计算得到路径参数更新律和三种期望艏摇角；

给定拓展后的路径信息矩阵path_interp和无人船当前的位置(x,y)，由此得出x_e轴路径信息差分矩阵x_diff(θ)和y_e轴路径信息差分矩阵y_diff(θ)为：

得到路径切向角矩阵ψ_d(θ)为：

ψ_d(θ)＝atan2(y_diff(θ),x_diff(θ))；

定义横向跟踪误差矩阵x_e(θ)和纵向跟踪误差矩阵y_e(θ)为：

设计跟踪误差矩阵d_err(θ)为：

d_err(θ)＝x_e(θ)·x_e(θ)+y_e(θ)·y_e(θ)；

则当前跟踪路径参数θ_now更新律具体如下：

艏摇角制导律如下：

其中ψ_r1为正向跟踪期望艏摇角，ψ_r2为反向跟踪期望艏摇角，ψ_r3为第一虚拟任务点期望艏摇角，n×l为无人船的可视距离，n为整数，l为无人船的长度，ψ_r1中θ_now取argmind_err(θ)+1，ψ_r2中θ_now取argmind_err(θ)-1。

进一步地，纵向速度制导律具体如下：

由路径切向角计算得到路径切向角导数，选取路径切向角导数大于定值的点为路径切向角突变点；当无人船进入路径切向角突变点一定范围内时，根据相对路径切向角突变点的距离设置期望纵向速度，越接近路径切向角突变点期望纵向速度越低；

给定当前的跟踪路径参数θ_now、路径切向角差分矩阵ψ_diff(θ)和初始设定的期望纵向速度u_p；由|ψ_diff(θ)|＞ψ_change得到切向角突变参数矩阵θ_change，其中ψ_change为设定的突变角；

纵向速度响应距离为：

d_res＝min|θ_change-θ_now|；

根据纵向速度响应距离得到纵向速度制导律为：

其中，u_min为设定好的最低期望纵向速度，d_max为响应限制距离且有

其中k_res为响应限制参数，d_mid为响应中间距离，u_res为路径响应后的期望纵向速度，最后由第二PID控制器根据误差u_res-u得到输出τ_u。

进一步地，多模式决策模块通过接收指令使无人船多模式路径跟踪控制系统进行严格路径选择、反向跟踪返航或改变路径拓展模式功能；

多模式决策模块接收严格路径选择模块、艏摇角制导律传递过来的第一虚拟任务点(x₀,y₀)^Τ以及三种期望艏摇角ψ_r1、ψ_r2和ψ_r3，根据接收指令选择是否使能严格路径选择功能、反向跟踪返航功能以及改变路径拓展模式功能，即从ψ_r1、ψ_r2和ψ_r3中选择传递到第一PID控制器的多模式决策后的期望艏摇角ψ_r以及传递信号给路径拓展模块选择插值算法得到不同的path_interp，最后由第一PID控制器根据误差ψ_r-ψ得到输出ψ_r。

进一步地，严格路径选择功能如下：

当无人船执行严格路径选择功能时，无人船跟踪受到第一虚拟任务点(x₀,y₀)^Τ的约束，跟踪目标锁定第一虚拟任务点，此时ψ_r＝ψ_r3；当无人船经过第一虚拟任务点一定范围内时再从ψ_r1、ψ_r2中进行选择；

其中反向跟踪返航功能如下：

无人船正常正向跟踪时，选择ψ_r＝ψ_r1；

当无人船执行反向跟踪返航功能时，选择ψ_r＝ψ_r2。

其中改变路径拓展模式功能如下：

无人船在路径跟踪过程中可响应指令并传递信号给路径拓展模块，曲线跟踪时路径拓展模块切换为球面插值算法，直线跟踪时路径拓展模块切换为分段线性插值算法对路径信息path_in进行实时处理，得到不同的拓展后的路径信息path_interp。

进一步地，通过LOS制导算出的期望艏摇角和真实艏摇角的取值区间不同，会引发误差计算错误的问题，为了传递正确的艏摇角误差ψ_r-ψ给第一PID控制器，设置一个误差纠正模块，具体如下：

若-π＜ψ_r-ψ或ψ_r-ψ＜＝π成立，则传递ψ_r-ψ给第一PID控制器；

若-π＜ψ_r-ψ或ψ_r-ψ＜＝π不成立，且ψ_r-ψ＝-π成立，则传递ψ_r-ψ＝π给第一PID控制器；

若-π＜ψ_r-ψ、ψ_r-ψ＜＝π或ψ_r-ψ＝-π不成立，且-π＞ψ_r-ψ成立，则传递以下参数给第一PID控制器：

若-π＜ψ_r-ψ、ψ_r-ψ＜＝π、ψ_r-ψ＝-π或-π＞ψ_r-ψ不成立，则传递以下参数给第一PID控制器：

本发明与现有技术比，其优点如下：

1)本发明采用的路径拓展模块克服了路径信息有限的不良条件，并可以平滑路径信息；

2)本发明采用的严格路径选择模块能够应对严格跟踪任务需求，在跟踪过程中严格跟踪初始路径点；

3)本发明采用的纵向速度制导律有效利用了路径信息并抑制了无人船在路径切向角突变处的过冲现象；

4)本发明采用的多模式决策模块可以对路径信息和跟踪顺序进行实时干预，实现了多模式路径跟踪控制。

附图说明

图1为本发明实施例坐标系示意图；

图2为本发明实施例方法流程图；

图3为本发明实施例多模式决策流程图；

图4和图5均为本发明实施例中的仿真对比图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，下面结合附图并举实施例，对本发明的具体实施进行详细说明。

实施例：

一种基于速变LOS的无人船多模式路径跟踪控制方法，包括以下步骤：

S1、建立惯性坐标系下固定双桨推进无人船的三自由度运动学和动力学模型；

如图1所示，运动学与动力学模型的建立过程如下：

建立惯性坐标系下固定双桨推进无人船的三自由度运动学和动力学模型，其中，O_ex_ey_e是建立在地球上的惯性坐标系，船体坐标系是以船体中心为原点，纵向指向船艏并平行水面，横向垂直于船艏并平行于水面，在地球上的惯性坐标系下无人船的运动学和动力学模型为：

上述方程中，ψ是无人船当前的艏摇角，u,v,r分别表示船体坐标系中纵向、横向速度和艏摇角速度；τ_u和τ_r分别表示无人船动力装置产生的纵向推力和横向力矩，τ₁,τ₂,τ₃分别表示无人船在纵向、横向、艏向上受到的干扰力，m_ii和d_ii分别为实验无人船的给定惯性系数和阻尼系数；由于不考虑外界干扰，τ₁,τ₂,τ₃设置为0；

为无人船当前的位置(x,y)的对应导数，

和

为对应的导数；本实施例中，m₁₁＝25.8、m₂₂＝33.8、m₃₃＝6.2、d₁₁＝12、d₂₂＝17、d₃₃＝0.5

其中τ_u＝f₁+f₂，τ_r＝(f₁-f₂)·B/2；B表示无人船双桨的横向距离，f₁和f₂分别表示两个螺旋桨的推力；

由于固定双桨推进无人船以两个直流电机为推进系统，两个螺旋桨分别对应两个直流电机，螺旋桨推力和电调输出电压有如下线性关系：f＝kV；

结合上述方程最终得到实际的控制输入：左电机控制电压V_L和右电机控制电压V_R分别为：

V_L＝(2·τ_r/B+τ_u)/2k，V_R＝(-2·τ_r/B+τ_u)/2k；

则动力学方程可以进一步改写为：

S2、构建无人船多模式路径跟踪控制系统；

所述无人船多模式路径跟踪控制系统包括路径拓展模块、严格路径选择模块和多模式决策模块；

路径拓展模块根据拓展后的路径点数目和输入路径点数目计算得到插值步长，接收多模式决策模块传递的信号，对输入路径信息分别进行分段线性插值、球面插值得到拓展后的路径信息；

严格路径选择模块根据拓展后路径信息设置第一虚拟任务点，根据拓展后的路径信息和第一虚拟任务点通过路径参数更新律、艏摇角制导律和纵向速度制导律得到当前跟踪路径参数、路径切向角矩阵、期望艏摇角和纵向速度；

多模式决策模块根据第一虚拟任务点、期望艏摇角和纵向速度，通过第一PID控制器、第二PID控制器控制无人船的运动

无人船在跟踪路径时，多模式决策模块实时接收指令使无人船多模式路径跟踪控制系统实现不同的功能。

路径拓展模块中，给定拓展后的路径点数目N_e，路径信息输入点的个数N_n，设计插值步长：

根据多模式决策模块的信号，由插值步长step对输入的路径信息

进行分段线性插值或球面插值得到拓展后的路径信息为：

其中，(x_i,y_i)是path_in插值前的第i个路径点坐标，(x_θ,y_θ)为path_in经过插值后的第θ个路径点的坐标，θ表示一个与时间无关的路径参数，拓展后的路径信息为最终无人船的跟踪路径信息。

严格路径选择模块中，第一虚拟任务点设置为：

其中(x_θ＝1,y_θ＝1)^Τ为拓展后的路径信息的第1个点，

为拓展后的路径信息的第k_vis个点，k_vis为严格路径选择尺度参数，该参数根据N_n和N_e设置，本实施例中，设置为

路径参数更新律和艏摇角制导律具体如下：

给定一条期望路径的路径信息和第一虚拟任务点，通过位置误差计算得到路径参数更新律和三种期望艏摇角；

给定拓展后的路径信息矩阵path_interp和无人船当前的位置(x,y)，由此得出x_e轴路径信息差分矩阵x_diff(θ)和y_e轴路径信息差分矩阵y_diff(θ)为：

得到路径切向角矩阵ψ_d(θ)为：

ψ_d(θ)＝atan2(y_diff(θ),x_diff(θ))；

定义横向跟踪误差矩阵x_e(θ)和纵向跟踪误差矩阵y_e(θ)为：

设计跟踪误差矩阵d_err(θ)为：

d_err(θ)＝x_e(θ)·x_e(θ)+y_e(θ)·y_e(θ)

则当前跟踪路径参数θ_now更新律具体如下：

艏摇角制导律如下：

其中ψ_r1为正向跟踪期望艏摇角，ψ_r2为反向跟踪期望艏摇角，ψ_r3为第一虚拟任务点期望艏摇角，n×l为无人船的可视距离，n为整数，l为无人船的长度，ψ_r1中θ_now取argmind_err(θ)+1，ψ_r2中θ_now取argmind_err(θ)-1。

如图1所示为无人船与path_interp上某一路径点的坐标示意图，图中具体标注了路径参数为的θ时无人船与该点的纵向误差y_e(θ)、横向误差x_e(θ)，无人船的可视距离n×l，θ＝θ_now成立时的正向跟踪期望艏摇角ψ_r1、路径参数为θ处的路径切向角ψ_d(θ)。

纵向速度制导律具体如下：

由路径切向角计算得到路径切向角导数，选取路径切向角导数大于定值的点为路径切向角突变点；当无人船进入路径切向角突变点一定范围内时，根据相对路径切向角突变点的距离设置期望纵向速度，越接近路径切向角突变点期望纵向速度越低；

给定当前的跟踪路径参数θ_now、路径切向角差分矩阵ψ_diff(θ)和初始设定的期望纵向速度u_p；由|ψ_diff(θ)|＞ψ_change得到切向角突变参数矩阵θ_change，其中ψ_change为设定的突变角；本实施例中，ψ_change设置为π/6，即θ_change得到了切向角差值大于π/6的路径参数信息。

纵向速度响应距离为：

d_res＝min|θ_change-θ_now|；

根据纵向速度响应距离得到纵向速度制导律为：

其中，u_min为设定好的最低期望纵向速度，d_max为响应限制距离且有

其中k_res为响应限制参数，d_mid为响应中间距离，u_res为路径响应后的期望纵向速度，最后由第二PID控制器根据误差u_res-u得到输出τ_u。本实施例中，k_res设置为0.2，d_mid设置为

多模式决策模块通过接收指令使无人船多模式路径跟踪控制系统进行严格路径选择、反向跟踪返航或改变路径拓展模式功能；

多模式决策模块接收严格路径选择模块、艏摇角制导律传递过来的第一虚拟任务点(x₀,y₀)^Τ以及三种期望艏摇角ψ_r1、ψ_r2和ψ_r3，根据接收指令选择是否使能严格路径选择功能、反向跟踪返航功能以及改变路径拓展模式功能，即从ψ_r1、ψ_r2和ψ_r3中选择传递到第一PID控制器的多模式决策后的期望艏摇角ψ_r以及传递信号给路径拓展模块选择插值算法得到不同的path_interp，最后由第一PID控制器根据误差ψ_r-ψ得到输出ψ_r。

严格路径选择功能如下：

当无人船执行严格路径选择功能时，无人船跟踪受到第一虚拟任务点(x₀,y₀)^Τ的约束，跟踪目标锁定第一虚拟任务点，此时ψ_r＝ψ_r3；本实施例中，当无人船经过第一虚拟任务点4m圆形范围内时再从ψ_r1、ψ_r2中进行选择；

其中反向跟踪返航功能如下：

无人船正常正向跟踪时，选择ψ_r＝ψ_r1；

当无人船执行反向跟踪返航功能时，选择ψ_r＝ψ_r2。

其中改变路径拓展模式功能设计如下：

无人船在路径跟踪过程中可响应指令并传递信号给路径拓展模块，曲线跟踪时路径拓展模块切换为球面插值算法，直线跟踪时路径拓展模块切换为分段线性插值算法对路径信息path_in进行实时处理，得到不同的拓展后的路径信息path_interp。

通过LOS制导算出的期望艏摇角和真实艏摇角的取值区间不同，会引发误差计算错误的问题，为了传递正确的艏摇角误差ψ_r-ψ给第一PID控制器，设置一个误差纠正模块，具体如下：

若-π＜ψ_r-ψ或ψ_r-ψ＜＝π成立，则传递ψ_r-ψ给第一PID控制器；

若-π＜ψ_r-ψ或ψ_r-ψ＜＝π不成立，且ψ_r-ψ＝-π成立，则传递ψ_r-ψ＝π给第一PID控制器。

若-π＜ψ_r-ψ、ψ_r-ψ＜＝π或ψ_r-ψ＝-π不成立，且-π＞ψ_r-ψ成立，则传递以下参数给第一PID控制器：

若-π＜ψ_r-ψ、ψ_r-ψ＜＝π、ψ_r-ψ＝-π或-π＞ψ_r-ψ不成立，则传递以下参数给第一PID控制器：

S3、实时对无人船多模式路径跟踪控制系统中的多模式决策模块下达指令，实现无人船多模式路径跟踪控制。

根据图4、图5可以看出，本发明在跟踪效果上优于传统LOS：

1、本发明设置的第一虚拟任务点可以使无人船在跟踪过程中严格跟踪初始路径点(图4、图5中初始路径点均为(20,0))。

2、本发明采用的路径拓展模块和纵向速度制导律可以平滑路径信息，有效利用了路径信息并抑制了无人船在路径切向角突变点处的过冲现象(如图4中的(20,20)、(40,20)、(40,40)，图5中(20,20)、(30,30)、(30,10))。

3、此外本发明采用的多模式决策模块可以对路径信息和跟踪顺序进行实时干预，实现了多模式路径跟踪控制，满足不同任务需求。

Claims (9)

1.一种基于速变LOS的无人船多模式路径跟踪控制方法，其特征在于，包括以下步骤：

S1、建立惯性坐标系下固定双桨推进无人船的三自由度运动学和动力学模型；

S2、构建无人船多模式路径跟踪控制系统；

所述无人船多模式路径跟踪控制系统包括路径拓展模块、严格路径选择模块和多模式决策模块；

路径拓展模块根据拓展后的路径点数目和输入路径点数目计算得到插值步长，接收多模式决策模块传递的信号，对输入路径信息分别进行分段线性插值、球面插值得到拓展后的路径信息；

严格路径选择模块根据拓展后路径信息设置第一虚拟任务点，根据拓展后的路径信息和第一虚拟任务点通过路径参数更新律、艏摇角制导律和纵向速度制导律得到当前跟踪路径参数、路径切向角矩阵、期望艏摇角和纵向速度；

多模式决策模块根据第一虚拟任务点、期望艏摇角和纵向速度，通过第一PID控制器、第二PID控制器控制无人船的运动；

无人船在跟踪路径时，多模式决策模块实时接收指令使无人船多模式路径跟踪控制系统实现不同的功能；

S3、实时对无人船多模式路径跟踪控制系统中的多模式决策模块下达指令，实现无人船多模式路径跟踪控制。

2.根据权利要求1所述的一种基于速变LOS的无人船多模式路径跟踪控制方法，其特征在于，步骤S1中，运动学与动力学模型的建立过程如下：

建立惯性坐标系下固定双桨推进无人船的三自由度运动学和动力学模型，其中，O_ex_ey_e是建立在地球上的惯性坐标系，船体坐标系是以船体中心为原点，纵向指向船艏并平行水面，横向垂直于船艏并平行于水面，在地球上的惯性坐标系下无人船的运动学和动力学模型为：

其中，ψ是无人船当前的艏摇角，u,v,r分别表示船体坐标系中的纵向速度、横向速度和艏摇角速度；τ_u和τ_r分别表示无人船动力装置产生的纵向推力和横向力矩，τ₁,τ₂,τ₃分别表示无人船在纵向、横向、艏向上受到的干扰力，m_ii和d_ii分别为实验无人船的给定惯性系数和阻尼系数；由于不考虑外界干扰，τ₁,τ₂,τ₃设置为0；

为无人船当前的位置(x,y)的对应导数，

和

为对应的导数；

其中τ_u＝f₁+f₂，τ_r＝(f₁-f₂)·B/2；B表示无人船双桨的横向距离，f₁和f₂分别表示两个螺旋桨的推力；

由于固定双桨推进无人船以两个直流电机为推进系统，两个螺旋桨分别对应两个直流电机，螺旋桨推力和电调输出电压有如下线性关系：f＝kV；

结合上述方程最终得到实际的控制输入：左电机控制电压V_L和右电机控制电压V_R分别为：

V_L＝(2·τ_r/B+τ_u)/2k，V_R＝(-2·τ_r/B+τ_u)/2k；

则动力学方程进一步改写为：

3.根据权利要求1所述的一种基于速变LOS的无人船多模式路径跟踪控制方法，其特征在于，路径拓展模块中，给定拓展后的路径点数目N_e，路径信息输入点的个数N_n，设计插值步长：

根据多模式决策模块的信号，由插值步长step对输入的路径信息

进行分段线性插值或球面插值得到拓展后的路径信息为：

其中，(x_i,y_i)是path_in插值前的第i个路径点坐标，(x_θ,y_θ)为path_in经过插值后的第θ个路径点的坐标，θ表示一个与时间无关的路径参数，拓展后的路径信息为最终无人船的跟踪路径信息。

4.根据权利要求3所述的一种基于速变LOS的无人船多模式路径跟踪控制方法，其特征在于，严格路径选择模块中，第一虚拟任务点设置为：

其中(x_θ＝1,y_θ＝1)^T为拓展后的路径信息的第1个点，

为拓展后的路径信息的第k_vis个点，k_vis为严格路径选择尺度参数。

5.根据权利要求4所述的一种基于速变LOS的无人船多模式路径跟踪控制方法，其特征在于，路径参数更新律和艏摇角制导律具体如下：

给定一条期望路径的路径信息和第一虚拟任务点，通过位置误差计算得到路径参数更新律和三种期望艏摇角；

给定拓展后的路径信息矩阵path_interp和无人船当前的位置(x,y)，由此得出x_e轴路径信息差分矩阵x_diff(θ)和y_e轴路径信息差分矩阵y_diff(θ)为：

得到路径切向角矩阵ψ_d(θ)为：

ψ_d(θ)＝atan2(y_diff(θ),x_diff(θ))；

定义横向跟踪误差矩阵x_e(θ)和纵向跟踪误差矩阵y_e(θ)为：

x_e(θ)＝cosψ_d(θ)·(x-x_θ)+sinψ_d(θ)·(y-y_θ)

y_e(θ)＝-sinψ_d(θ)·(x-x_θ)+cosψ_d(θ)·(y-y_θ)；

设计跟踪误差矩阵d_err(θ)为：

d_err(θ)＝x_e(θ)·x_e(θ)+y_e(θ)·y_e(θ)；

则当前跟踪路径参数θ_now更新律具体如下：

艏摇角制导律如下：

其中ψ_r1为正向跟踪期望艏摇角，ψ_r2为反向跟踪期望艏摇角，ψ_r3为第一虚拟任务点期望艏摇角，n×l为无人船的可视距离，n为整数，l为无人船的长度，ψ_r1中θ_now取argmind_err(θ)+1，ψ_r2中θ_now取argmind_err(θ)-1。

6.根据权利要求5所述的一种基于速变LOS的无人船多模式路径跟踪控制方法，其特征在于，纵向速度制导律具体如下：

由路径切向角计算得到路径切向角导数，选取路径切向角导数大于定值的点为路径切向角突变点；当无人船进入路径切向角突变点一定范围内时，根据相对路径切向角突变点的距离设置期望纵向速度，越接近路径切向角突变点期望纵向速度越低；

给定当前的跟踪路径参数θ_now、路径切向角差分矩阵ψ_diff(θ)和初始设定的期望纵向速度u_p；由|ψ_diff(θ)|>ψ_change得到切向角突变参数矩阵θ_change，其中ψ_change为设定的突变角；

纵向速度响应距离为：

d_res＝min|θ_change-θ_now|；

根据纵向速度响应距离得到纵向速度制导律为：

其中，u_min为设定好的最低期望纵向速度，d_max为响应限制距离且有

其中k_res为响应限制参数，d_mid为响应中间距离，u_res为路径响应后的期望纵向速度，最后由第二PID控制器根据误差u_res-u得到输出τ_u。

7.根据权利要求6所述的一种基于速变LOS的无人船多模式路径跟踪控制方法，其特征在于，多模式决策模块通过接收指令使无人船多模式路径跟踪控制系统进行严格路径选择、反向跟踪返航或改变路径拓展模式功能；

多模式决策模块接收严格路径选择模块、艏摇角制导律传递过来的第一虚拟任务点(x₀,y₀)^T以及三种期望艏摇角ψ_r1、ψ_r2和ψ_r3，根据接收指令选择是否使能严格路径选择功能、反向跟踪返航功能以及改变路径拓展模式功能，即从ψ_r1、ψ_r2和ψ_r3中选择传递到第一PID控制器的多模式决策后的期望艏摇角ψ_r以及传递信号给路径拓展模块选择插值算法得到不同的path_interp，最后由第一PID控制器根据误差ψ_r-ψ得到输出ψ_r。

8.根据权利要求7所述的一种基于速变LOS的无人船多模式路径跟踪控制方法，其特征在于，严格路径选择功能如下：

当无人船执行严格路径选择功能时，无人船跟踪受到第一虚拟任务点(x₀,y₀)^T的约束，跟踪目标锁定第一虚拟任务点，此时ψ_r＝ψ_r3；当无人船经过第一虚拟任务点一定范围内时再从ψ_r1、ψ_r2中进行选择；

其中反向跟踪返航功能如下：

无人船正常正向跟踪时，选择ψ_r＝ψ_r1；

当无人船执行反向跟踪返航功能时，选择ψ_r＝ψ_r2；

其中改变路径拓展模式功能如下：

无人船在路径跟踪过程中可响应指令并传递信号给路径拓展模块，曲线跟踪时路径拓展模块切换为球面插值算法，直线跟踪时路径拓展模块切换为分段线性插值算法对路径信息path_in进行实时处理，得到不同的拓展后的路径信息path_interp。

9.根据权利要求7所述的一种基于速变LOS的无人船多模式路径跟踪控制方法，其特征在于，通过LOS制导算出的期望艏摇角和真实艏摇角的取值区间不同，会引发误差计算错误的问题，为了传递正确的艏摇角误差ψ_r-ψ给第一PID控制器，设置一个误差纠正模块，具体如下：

若-π<ψ_r-ψ或ψ_r-ψ<＝π成立，则传递ψ_r-ψ给第一PID控制器；

若-π<ψ_r-ψ或ψ_r-ψ<＝π不成立，且ψ_r-ψ＝-π成立，则传递ψ_r-ψ＝π给第一PID控制器；

若-π<ψ_r-ψ、ψ_r-ψ<＝π或ψ_r-ψ＝-π不成立，且-π>ψ_r-ψ成立，则传递以下参数给第一PID控制器：

若-π<ψ_r-ψ、ψ_r-ψ<＝π、ψ_r-ψ＝-π或-π>ψ_r-ψ不成立，则传递以下参数给第一PID控制器：

Images