CN114610023A - 一种无人船路径跟踪控制方法 - Google Patents

Abstract

本发明公开了一种无人船路径跟踪控制方法，包括以下步骤：首先，将一条给定的待跟踪路径离散为位点集合，并裁剪该集合；其次，使用一种新的制导算法，根据模型预测窗口大小和当前无人船纵向速度确定下一个时间窗口内所跟踪的参考点集合；然后，将参考点作为模型预测控制下个预测窗口的参考量，构建优化目标函数，计算满足船舶动力学模型约束、舵角约束、舵角增量约束的最优舵角序列；最后，将计算得到的舵角序列第一个值施加于船舶运动系统，实现路径跟踪。本发明通过跟踪离散位点集合的形式可以实现对任意路径的跟踪，且能够保证舵角变化平稳。

Description

一种无人船路径跟踪控制方法

技术领域

本发明涉及无人船路径跟踪技术领域，具体涉及一种无人船路径跟踪控制方法。

背景技术

水上无人船的研制和智能化的进程不断加快。相比较普通船舶，无人船具备高度自动化、智能化特点，是智能船舶和水上交通运输的一个重要研究方向，未来在军事领域、民用领域都具有巨大的发展空间。

路径跟踪是无人船的核心任务之一。但是由于船舶操纵性能存在限制，现有的控制方法，比如反步法、滑模控制、神经网络控制，难以直接处理约束问题。

模型预测控制(model predictive control，MPC)不仅可以处理带约束条件的控制问题，还由于其具备预测模型的多样性、滚动优化的时变性、反馈校正的鲁棒性等特征，使得其在不确定性和干扰等环境下对惯性大、时滞性强的无人船运动能够有较好的控制性能。

但是MPC需要在每个时刻根据预测窗口内的参考点构建最优化问题并在线求解，相关技术中，将LOS制导与MPC相结合，随MPC滚动优化不断获得新的参考点，但是传统LOS制导是一种直线制导算法，对曲线路径的跟踪精度较低。

发明内容

根据现有技术的不足，本发明的目的是提供一种无人船路径跟踪控制方法，能够实现对任意类型路径的跟踪，且满足船舶操纵特性约束。

为了解决上述技术问题，本发明采用的技术方案为：

一种无人船路径跟踪控制方法，包括：

步骤1、在无人船需要跟踪的路径中获得第一密集离散位点集合，所述第一密集离散位点集合包括多个位点；

步骤2、裁剪第一密集离散位点集合，删掉第一密集离散位点集合中违背船舶运动规律的前列位点，得到第二密集离散位点集合；

步骤3、结合MPC控制器，使用ALOS法，根据模型预测控制的预测窗口大小和当前无人船纵向速度确定下个时间窗口内所跟踪的参考点集合；

步骤4、根据参考点集合计算满足船舶动力学模型约束、舵角约束、舵角增量约束的最优舵角序列；

步骤5、在最优舵角序列中选取舵角控制量施加于船舶运动系统，无人船产生运动响应；

步骤6、判断无人船是否到达路径终点，若达到则跟踪结束，否则回到步骤3，继续跟踪。

进一步地，所述步骤1中，第一密集离散位点集合在路径跟踪问题中由对路径采样获得，记第一密集离散位点集合为D＝{P_i(x_i,y_i),i＝1,2,3…,N}，以i为索引，P_N是密集离散位点集合中最后一个位点。

进一步地，所述步骤2包括：

步骤2.1、在NED坐标系下，设当前船位为O_s，初始位置坐标为(x₀,y₀)，船长为L_PP，设定变化阈值角

船长倍数n′，若船舶初始艏向为Ψ，则切向角搜索范围为

对于初始艏向Ψ，若-45°<Ψ≤45°，则东向坐标搜索范围为[-∞,∞]，北向坐标搜索范围为[y₀,y₀+n′L_PPcosΨ]；

若-135°<Ψ≤-45°，则东向坐标搜索范围为[x₀+n′L_PP sinΨ,x₀]，北向坐标搜索范围为[-∞,∞]；

若-180°<Ψ≤-135°或135°<Ψ≤180°，则东向坐标搜索范围为[-∞,∞]，北向坐标搜索范围为[y₀+n′L_PPcosΨ,y₀]；

若45°<Ψ≤135°，则东向坐标搜索范围为[x₀,x₀+n′L_PP sinΨ]，北向坐标搜索范围为[-∞,∞]；

根据艏向Ψ确定东向坐标搜索范围和北向坐标搜索范围，从第一密集离散位点集合中第一个元素开始依次搜索，直到搜索出位点S，使得在位点S处切向角、东向坐标和北向坐标分别满足上述范围，停止搜索，并将位点S之前的位点从第一密集离散位点集合中删除，得到裁剪后的第二密集离散位点D′；

步骤2.2、设定距离阈值d，截止搜索点索引M，创建索引标识变量H，H∈N，对第二密集离散位点D′进行循环搜索，找到距离当前船位艏向延长线所在的直线距离小于d的位点，直到P_M，当有一位点(x_i,y_i)到当前船位艏向延长线所在的直线距离小于d，则跳出循环，将该位点索引i赋值给变量H，若直到P_M都无法搜索到一点满足上述要求，则将索引M赋值给变量H，此时，已完成对变量H的初始化，称初始化的P_H(x_H,y_H)为该路径跟踪任务的切入位点。

进一步地，

取45～70°，n′取2～6。

进一步地，所述步骤3包括：

步骤3.1、设模型预测控制的预测窗口大小为N_P，跟踪开始时的初始速度为u₀，当前纵向速度为u，定义循环变量j，j的初始赋值为0；

步骤3.2、以

为半径作包络圆；

步骤3.3、记当前时刻为k，从P_H开始依次搜索第二密集离散位点集合D′中的点，直到寻得第一个在包络圆外的位点，该位点被认为是第k+j个时刻的参考点，即路径跟踪中期望第k+j个时刻到达的坐标位置，当且仅当j＝0时，该位点被认为是当前时刻的参考点P_LOS，将该位点索引赋值于H，使该位点成为新的P_H；

步骤3.4、令j＝j+1，返回步骤3.2，直到得到j＝N_P，完成搜索寻得预测窗口中下一个时刻到未来第N_P个时刻期望到达的坐标位置。

进一步地，在步骤4中，最优舵角序列使得在预测窗口内各时刻无人船到达的坐标位置与各参考点距离、艏向角差值、舵角增量三者比例平方加和最小。

进一步地，记当前时刻为k，将步骤3中所得到的参考点视为模型预测控制中的参考量，即期望到达的期望点，将优化目标函数表示为预测未来多个时刻的艏向角平方误差、位置平方误差和相邻时刻舵角变化量的权重加和：

其中，w₁,w₂,w₃分别是各目标函数项的权重系数，(x^k+j _ref,y^k+j _ref)和Ψ^k+j _ref分别是上一步骤搜索得到的未来第j个参考点坐标及其参考切向角，δ是舵角。

进一步地，模型预测控制中采用非线性的Abkowitz模型，满足船舶动力学模型约束、舵角约束和舵角增量约束的非线性模型预测控制的优化计算公式为：

其中，{u,v,r,x,y,Ψ}分别是无人船的纵向速度、横向速度、转艏角速度、北向坐标、东向坐标、艏向角，Δδ是相邻两时刻的舵角增量，F₁,F₂,F₃分别是Abkowitz模型中关于运动变量{u,v,r,δ}的函数，

是加速度水动力导数。

进一步地，w₁,w₂和w₃分别取值为50、1和1000。

进一步地，在步骤6中，判断当前船位与所设置终点距离是否小于阈值ζ，若小于ζ则表示跟踪结束，否则回到步骤3，继续跟踪流程。

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

本发明所述的一种无人船路径跟踪控制方法，无需路径的函数表达式，通过跟踪离散位点集合的形式，可以快速地、唯一地选取参考点，实现对任意路径的制导，避免了制导过程中出现解不唯一的情况，且在跟踪过程中考虑了船舶操纵特性约束，所得参考点不用于计算横向误差，而是作为目标函数中动态变化的位置参考量，计算预测模型与各参考点的位置误差和艏向角误差，求解满足船舶动力学模型约束、舵角约束、舵角增量约束的最优舵角问题，使得船舶快速地、更准确地跟踪期望路径。

附图说明

图1为本发明一种无人船路径跟踪控制方法架构图。

图2为本发明步骤3即制导算法示意图。

图3为本发明实施例第一工况路径跟踪控制方法仿真实验结果。

图4为本发明实施例第二工况路径跟踪控制方法仿真实验结果。

图5为本发明实施例中NMPC施加的最优舵角时历图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。

一种无人船路径跟踪控制方法，如图1所示，包括以下步骤：

步骤1、在无人船需要跟踪的路径中获得第一密集离散位点集合，所述第一密集离散位点集合包括多个位点；

步骤2、裁剪第一密集离散位点集合，删掉第一密集离散位点集合中违背船舶运动规律的前列位点，得到第二密集离散位点集合；

步骤3、结合MPC控制器，使用ALOS法，根据模型预测控制的预测窗口大小和当前无人船纵向速度确定下个时间窗口内所跟踪的参考点集合；

步骤4、根据参考点集合计算满足船舶动力学模型约束、舵角约束、舵角增量约束的最优舵角序列；

步骤5、将计算得到的最优舵角序列中的第一个值施加于船舶运动系统，无人船产生运动响应；

步骤6、判断无人船是否到达路径终点，若达到则跟踪结束，否则回到步骤3，继续跟踪。

本发明所指ALOS法，指的是一种新的制导方法。

传统的LOS制导是一种直线制导算法，对曲线路径的跟踪精度较低，且每一步的参考点均由方程联立求解所得，因此存在计算速度慢、需要路径函数表达式已知以及对带有小圆环的路径制导时会出现方程解不唯一的问题。

本发明的ALOS法，无需路径的函数表达式，通过对离散位点序列搜索的形式，可以快速地、唯一地选取参考点，实现对任意路径的制导，根据模型预测控制的预测窗口大小和当前无人船纵向速度确定下个时间窗口内所跟踪的参考点集合，在每个预测时间步搜索得到多个参考点分别对应当前与未来多个时刻，形成一段局部参考轨迹，所得参考点不用于计算横向误差，而是作为目标函数中动态变化的位置参考量和艏向参考量，计算预测模型与各参考点的位置误差和艏向角误差，求解满足船舶动力学模型约束、舵角约束、舵角增量约束的最优舵角问题。

本发明将无人船路径跟踪转化为参考量随时间变化的多段轨迹跟踪，使得船舶快速地、更准确地跟踪期望路径

通过对离散位点集合循环搜索得到下个时间窗口内的多个参考点并构建目标函数，该目标函数中参考量是时变的，将路径跟踪转化为在每个时间窗口内的轨迹跟踪，比结合传统LOS法的MPC控制器有更好跟踪效果。搜索过程中，视线距离随无人船纵向速度变化而成比例变化，本发明方法对路径拐角和曲率较大的场景同样适用，且有较好效果。

具体地，所述步骤1中，第一密集离散位点集合在路径跟踪问题中由对路径采样获得，记第一密集离散位点集合为D＝{P_i(x_i,y_i),i＝1,2,3…,N}，以i为索引，P_N是密集离散位点集合中最后一个位点。

为避免在跟踪任务开始时跟踪一个违背船舶运动规律的点，且初始化制导算法当前搜索点的索引，需裁剪第一密集离散位点集合D，所述步骤2包括：

步骤2.1、在NED坐标系下，设当前船位为O_s，初始位置坐标为(x₀,y₀)，船长为L_PP，设定变化阈值角

船长倍数n′，若船舶初始艏向为Ψ，则切向角搜索范围为

对于初始艏向Ψ，若-45°<Ψ≤45°，则东向坐标搜索范围为[-∞,∞]，北向坐标搜索范围为[y₀,y₀+n′L_PPcosΨ]；

若-135°<Ψ≤-45°，则东向坐标搜索范围为[x₀+n′L_PP sinΨ,x₀]，北向坐标搜索范围为[-∞,∞]；

若-180°<ψ≤-135°或135°<Ψ≤180°，则东向坐标搜索范围为[-∞,∞]，北向坐标搜索范围为[y₀+n′L_PPcosΨ,y₀]；

若45°<Ψ≤135°，则东向坐标搜索范围为[x₀,x₀+n′L_PP sinψ]，北向坐标搜索范围为[-∞,∞]；

记第二密集离散位点集合为D′，根据艏向Ψ确定东向坐标搜索范围和北向坐标搜索范围，从第一密集离散位点集合D中第一个元素(x₁,y₁)开始依次搜索，直到搜索出位点S，使得在位点S处切向角、东向坐标和北向坐标分别满足上述范围，停止搜索，并将位点S之前的位点从第一密集离散位点集合D中删除，得到裁剪后的第二密集离散位点D′；

步骤2.2、设定距离阈值d，截止搜索点索引M，创建索引标识变量H，H∈N，对第二密集离散位点D′进行循环搜索，找到距离当前船位艏向延长线所在的直线距离小于d的位点，直到P_M，当有一位点(x_i,y_i)到当前船位艏向延长线所在的直线距离小于d，则跳出循环，将该位点索引i赋值给变量H，若直到P_M都无法搜索到一点满足上述要求，则将索引M赋值给变量H，此时，已完成对变量H的初始化，称初始化的P_H(x_H,y_H)为该路径跟踪任务的切入位点(approach waypoint)。

其中，当前船位艏向延长线所在的直线设定为O_sΨ。

在步骤2中，通过大量的仿真实验发现，变化阈值角

取45～70°，船长倍数n′取2～6倍为宜，可以有效避免开始跟踪时搜索到违背船舶运动规律的参考点，同时也避免裁剪掉过多的位点。

具体地，如图2所示，所述步骤3包括：

步骤3.1、设模型预测控制的预测窗口大小为N_P，跟踪开始时的初始速度为u₀，当前纵向速度为u，定义循环变量j，j的初始赋值为0；

步骤3.2、以

为半径作包络圆；

步骤3.3、记当前时刻为k，从P_H开始依次搜索第二密集离散位点集合D′中的点，直到寻得第一个在圆外的位点，该位点被认为是第k+j个时刻的参考点，即路径跟踪中期望第k+j个时刻到达的坐标位置，当且仅当j＝0时，该位点被认为是当前时刻的参考点P_LOS，将该位点索引赋值于H，使该位点成为新的P_H；

步骤3.4、令j＝j+1，返回步骤3.2，直到得到j＝N_P，完成搜索寻得预测窗口中下一个时刻到未来第N_P个时刻期望到达的坐标位置。

在步骤4中，最优舵角序列使得在预测窗口内各时刻无人船到达的坐标位置与各参考点距离、艏向角差值、舵角增量三者比例平方加和最小。

在步骤4中，记当前时刻为k，将步骤3中所得到的参考点视为模型预测控制中的参考量，即期望到达的期望点，将优化目标函数表示为预测未来N_P个时刻的艏向角平方误差、位置平方误差和相邻时刻舵角变化量的权重加和：

其中，{w₁,w₂,w₃}分别是各目标函数项的权重系数，(x^k+j _ref,y^k+j _ref)和Ψ^k+j _ref分别是上一步骤搜索得到的未来第j个参考点坐标及其参考切向角，δ是舵角；

计算满足船舶动力学模型约束、舵角约束、舵角增量约束的最优舵角序列，使在预测窗口内各时刻无人船到达的坐标位置与各参考点距离、艏向角差值、舵角增量三者比例平方加和最小。

由于线性化的模型精度较低，在激烈的操纵运动情况下对船舶运动预报不准确，故本实施例采用Abkowitz非线性模型，则将无人船的三自由度操纵运动方程描述为：

其中，m为船舶质量；I_z为船体绕Z轴的转动惯量；x_G为船舶重心纵向坐标；

u、v、r、

和

分别是纵向速度分量、横向速度分量、转首角速度、加速度和角加速度；

均为水动力导数；Δu为关于初始纵向速度的变化量；

将上式中所有加速度项移至左边，写成以下形式：

其中，F₁(u,v,r,δ),F₂(u,v,r,δ),F₃(u,v,r,δ)分别是Abkowitz模型中关于运动变量{u,v,r,δ}的函数：

将加速度离散化，得到差分形式方程：

对于某一时刻船舶当前横向坐标、纵向坐标、艏向角{x,y,Ψ}则有：

构建非线性优化问题，并加入舵角约束、舵角增量约束，则有：

其中，k表示当前时刻，Δδ是相邻两时刻的舵角增量，(x^k+j _ref,y^k+j _ref)和Ψ^k+j _ref分别是步骤3中搜索得到的未来第j个参考点坐标及其参考切向角，{w₁,w₂,w₃}分别是各目标函数项的权重系数，在本实施例中，{w₁,w₂,w₃}分别取值为50、1、1000，求解该非线性优化问题可以得到N_P个舵角控制量，分别对应从当前时刻到未来第N_P-1个时刻的最优舵角。

上述步骤4通过对离散位点集合循环搜索得到下个时间窗口内的多个参考点并构建目标函数，该目标函数中参考量是时变的，比传统LOS法有更好跟踪效果，搜索过程中，视线距离随无人船纵向速度变化而成比例变化，使得本发明对路径拐角和曲率较大的场景同样适用，且有较好效果。

具体地，传统的LOS法参考点计算方式采用包络圆法或投影点预瞄法。使用包络圆法的LOS制导算法，联立方程组，取直线与包络圆上的交点作为参考点，通常只适用于直径路径，且每个时间步参考点个数为1，视线距离恒定不变，通过接纳圆切换目标点，而本发明给出的ALOS法，参考点计算方式采用包络圆法，顺序搜索离散点，取包络圆外点，不仅适用于直径路径，也适用于曲线路径，且每个时间步参考点个数为N，视线距离随纵向速度改变，不需要目标点。

在步骤5中，将计算得到的最优舵角序列中的第一个值施加于船舶运动系统，即从当前时刻到未来第N_P-1个时刻的最优舵角中选择第一个值施加于船舶运动系统，无人船产生运动响应，k自增1。

在步骤6中，判断当前船位与所设置终点距离是否小于阈值ζ，该阈值应与船长有关，若小于ζ则表示跟踪结束，否则回到步骤3，继续跟踪流程。

在步骤5和步骤6中，对步骤4的N_P个舵角控制量，仅取第一个舵角控制量作用于船舶并产生运动响应(即取模型预测控制的控制窗口为1)，k自增1。判断当前船位与所设置终点距离是否小于阈值ζ。由于在船舶操纵运动建模时将船舶质心视为当前船位，出于安全考虑，该阈值应与船长有关，若小于ζ则表示跟踪结束，否则回到步骤3，继续跟踪流程。

本发明以一艘特大型油轮KVLCC2为例，该船的具体参数见表2，待跟踪的路径是该船10°/10°Z形试验仿真获得离散位点集合，第一工况和第二工况起始点坐标分别设为(0,0)、(-200,-200)，终点设置为该集合中的最后一个位点即坐标点(203.72,7969.23)，初始艏向皆为0，初始速度{u₀,v₀,r₀}皆为{7.98m/s,0,0}。

表2 KVLCC2船型主要参数

本实施例中ζ取0.2倍船长，即64米。

对于本发明实施例，变化阈值角

取60°，船长倍数n′取5倍，距离阈值d设为30米，截止搜索点索引M设为200，经过步骤2裁剪确定的两个跟踪工况切入位点坐标分别为(0,0)、(-184.41，1259.88)，该点是裁剪后第二密集离散位点集合D′中的第一个P_H点，也是被视为当前出发时刻无人船应该到达的期望位置。

在步骤3中，视线距离R_LOS是船长的n倍，n取1～3为宜，在本实施例中n取值为1倍；N_P是模型预测控制的预测窗口大小，即在每个时间步对未来N_P个时刻的船舶运动状态进行预测。考虑到大型船舶操纵强时滞性、大惯性和计算时间复杂性，N_P一般取8～20为宜。在本实施例中N_P取值为10，即每一个时间步预测未来10个时刻的无人船状态。由于船舶运动惯性大、时滞性强，故N_P取值不宜过小。

在本实施例中，Δt取10s，即每隔10s对以上非线性最优化问题进行一次在线求解，各预测时刻的间隔也为10s；预测窗口N_P为10，控制窗口为1，最大舵角约束为±35°，舵角增量限制Δδ＝[-1°/s,1°/s]；其他所设置参数均已在上述步骤中说明。

如图3和图4，在第一工况和第二工况下，分别从起点坐标在(0,0)和(-200,-200)处的第一工况和第二工况切入，将本发明方法(图例名称为ALOS-NMPC，长虚线)与结合传统LOS法的NMPC方法(图例名称为LOS-NMPC，点虚线)作对比，本发明方法进行路径跟踪时与原路径偏差更小，尤其在转角处、曲率较大处明显性能更佳。

图5为本发明方法每个时间步求解得到的最优舵角序列第一个值的时历曲线，即每个时刻控制过程的舵角输入时历，实线和虚线分别是起点坐标在(0,0)和(-200,-200)处的两个工况的舵角时历曲线。可以看出由于考虑了最大舵角约束-35°≤δ≤+35°和舵角增量约束-1°/s≤Δδ≤1°/s，仿真航行中船舶舵角均在[-35°,+35°]内，且舵角变化缓慢，符合实际航行的要求。

综上所述，本发明提出一种无人船路径跟踪控制方法。结合MPC控制器，使用一种新的制导方法，即ALOS法，使用ALOS法，即一种新的制导方法，ALOS法，在制导环节的视线距离随船舶当前时刻纵向速度变化而成比例变化，无需路径的函数表达式，通过对离散位点序列搜索的形式，可以快速地、唯一地选取参考点，实现对任意路径的制导；根据模型预测控制的预测窗口大小和当前无人船纵向速度确定下个时间窗口内所跟踪的参考点集合，所得参考点不用于计算横向误差，而是作为目标函数中动态变化的位置参考量，求解满足船舶动力学模型约束、舵角约束、舵角增量约束的最优舵角问题，使得船舶快速地、更准确地跟踪期望路径。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (10)

1.一种无人船路径跟踪控制方法，其特征在于，包括：

步骤1、在无人船需要跟踪的路径中获得第一密集离散位点集合，所述第一密集离散位点集合包括多个位点；

步骤2、裁剪第一密集离散位点集合，删掉第一密集离散位点集合中违背船舶运动规律的前列位点，得到第二密集离散位点集合；

步骤3、结合MPC控制器，使用ALOS法，根据模型预测控制的预测窗口大小和当前无人船纵向速度确定下个时间窗口内所跟踪的参考点集合；

步骤4、根据参考点集合计算满足船舶动力学模型约束、舵角约束、舵角增量约束的最优舵角序列；

步骤5、在最优舵角序列中选取舵角控制量施加于船舶运动系统，无人船产生运动响应；

步骤6、判断无人船是否到达路径终点，若达到则跟踪结束，否则回到步骤3，继续跟踪。

2.根据权利要求1所述的无人船路径跟踪控制方法，其特征在于：所述步骤1中，第一密集离散位点集合在路径跟踪问题中由对路径采样获得，记第一密集离散位点集合为D＝{P_i(x_i,y_i),i＝1,2,3…,N}，以i为索引，P_N是密集离散位点集合中最后一个位点。

3.根据权利要求1所述的无人船路径跟踪控制方法，其特征在于，所述步骤2包括：

步骤2.1、在NED坐标系下，设当前船位为O_s，初始位置坐标为(x₀,y₀)，船长为L_PP，设定变化阈值角

船长倍数n′，若船舶初始艏向为Ψ，则切向角搜索范围为

对于初始艏向Ψ，若-45°<Ψ≤45°，则东向坐标搜索范围为[-∞,∞]，北向坐标搜索范围为[y₀,y₀+n′L_PPcosΨ]；

若-135°<Ψ≤-45°，则东向坐标搜索范围为[x₀+n′L_PPsinΨ,x₀]，北向坐标搜索范围为[-∞,∞]；

若-180°<Ψ≤-135°或135°<Ψ≤180°，则东向坐标搜索范围为[-∞,∞]，北向坐标搜索范围为[y₀+n′L_PPcosΨ,y₀]；

若45°<Ψ≤135°，则东向坐标搜索范围为[x₀,x₀+n′L_PPsinΨ]，北向坐标搜索范围为[-∞,∞]；

根据艏向Ψ确定东向坐标搜索范围和北向坐标搜索范围，从第一密集离散位点集合中第一个元素开始依次搜索，直到搜索出位点S，使得在位点S处切向角、东向坐标和北向坐标分别满足上述范围，停止搜索，并将位点S之前的位点从第一密集离散位点集合中删除，得到裁剪后的第二密集离散位点D′；

步骤2.2、设定距离阈值d，截止搜索点索引M，创建索引标识变量H，H∈N，对第二密集离散位点D′进行循环搜索，找到距离当前船位艏向延长线所在的直线距离小于d的位点，直到P_M，当有一位点(x_i,y_i)到当前船位艏向延长线所在的直线距离小于d，则跳出循环，将该位点索引i赋值给变量H，若直到P_M都无法搜索到一点满足上述要求，则将索引M赋值给变量H，此时，已完成对变量H的初始化，称初始化的P_H(x_H,y_H)为该路径跟踪任务的切入位点。

4.根据权利要求3所述的无人船路径跟踪控制方法，其特征在于：

取45～70°，n′取2～6。

5.根据权利要求1所述的无人船路径跟踪控制方法，其特征在于：所述步骤3包括：

步骤3.1、设模型预测控制的预测窗口大小为N_P，跟踪开始时的初始速度为u₀，当前纵向速度为u，定义循环变量j，j的初始赋值为0；

步骤3.2、以

为半径作包络圆；

步骤3.3、记当前时刻为k，从P_H开始依次搜索第二密集离散位点集合中的点，直到寻得第一个在包络圆外的位点，该位点被认为是第k+j个时刻的参考点，即路径跟踪中期望第k+j个时刻到达的坐标位置，当且仅当j＝0时，该位点被认为是当前时刻的参考点P_LOS，将该位点索引赋值于H，使该位点成为新的P_H；

步骤3.4、令j＝j+1，返回步骤3.2，直到得到j＝N_P，完成搜索寻得预测窗口中下一个时刻到未来第N_P个时刻期望到达的坐标位置。

6.根据权利要求1所述的无人船路径跟踪控制方法，其特征在于：在步骤4中，最优舵角序列使得在预测窗口内各时刻无人船到达的坐标位置与各参考点距离、艏向角差值、舵角增量三者比例平方加和最小。

7.根据权利要求6所述的无人船路径跟踪控制方法，其特征在于：记当前时刻为k，将步骤3中所得到的参考点视为模型预测控制中的参考量，即期望到达的期望点，将优化目标函数表示为预测未来多个时刻的艏向角平方误差、位置平方误差和相邻时刻舵角变化量的权重加和：

其中，w₁,w₂,w₃分别是各目标函数项的权重系数，(x^k+j _ref,y^k+j _ref)和Ψ^k+j _ref分别是上一步骤搜索得到的未来第j个参考点坐标及其参考切向角，δ是舵角。

8.根据权利要求7所述的无人船路径跟踪控制方法，其特征在于：模型预测控制中采用非线性的Abkowitz模型，满足船舶动力学模型约束、舵角约束和舵角增量约束的非线性模型预测控制的优化计算公式为：

其中，{u,v,r,x,y,Ψ}分别是无人船的纵向速度、横向速度、转艏角速度、北向坐标、东向坐标、艏向角，Δδ是相邻两时刻的舵角增量，F₁,F₂,F₃分别是Abkowitz模型中关于运动变量{u,v,r,δ}的函数，

是加速度水动力导数。

9.根据权利要求7所述的无人船路径跟踪控制方法，其特征在于：w₁,w₂和w₃分别取值为50、1和1000。

10.根据权利要求1所述的无人船路径跟踪控制方法，其特征在于：在步骤6中，判断当前船位与所设置终点距离是否小于阈值ζ，若小于ζ则表示跟踪结束，否则回到步骤3，继续跟踪流程。

Images